00001 #ifndef Impala_Core_Tracking_KernelSet_h
00002 #define Impala_Core_Tracking_KernelSet_h
00003
00004 #include <vector>
00005 #include <algorithm>
00006 #include <assert.h>
00007 #include "Core/Array/Arrays.h"
00008
00009 namespace Impala
00010 {
00011 namespace Core
00012 {
00013 namespace Tracking
00014 {
00015
00017 class KernelSet
00018 {
00019 public:
00020 KernelSet(int size) : mFilterSize(size)
00021 {
00022 }
00023
00024 virtual ~KernelSet()
00025 {
00026 for(int i=0 ; i<mFilters.size() ; i++)
00027 delete mFilters[i];
00028 }
00029
00031 void
00032 AddKernel(Array::Array2dScalarReal64* kernel)
00033 {
00034 assert(kernel->CW() == mFilterSize && kernel->CH() == mFilterSize);
00035 mFilters.push_back(kernel);
00036 }
00037
00038 int
00039 GetFilterCount()
00040 {
00041 return mFilters.size();
00042 }
00043
00054 void
00055 Apply(double* buffer, Array::Array2dVec3Real64& image, int posX, int posY)
00056 {
00057
00058 int i;
00059 for(i=0 ; i<3*mFilters.size() ; i++)
00060 buffer[i] = 0;
00061
00062
00063 int radius = mFilterSize / 2;
00064 if (posX-radius <= -image.BW() ||
00065 posY-radius <= -image.BH() ||
00066 posX+radius >= image.CW() + image.BW() ||
00067 posY+radius >= image.CH() + image.BH())
00068 {
00069 return;
00070 }
00071
00072 posX -= radius;
00073 posY -= radius;
00074 int y;
00075 for(y=0 ; y<mFilterSize ; y++)
00076 {
00077 for(i=0 ; i<mFilters.size() ; i++)
00078 {
00079 double* ip;
00080 double* fp;
00081 ip = image.CPB(posX, y+posY);
00082 fp = mFilters[i]->CPB(0, y);
00083 int x;
00084 for(x=0 ; x<mFilterSize ; x++)
00085 {
00086 double* tp;
00087 tp = buffer+(i*3);
00088 *tp += *ip * *fp;
00089 tp++;
00090 ip++;
00091 *tp += *ip * *fp;
00092 tp++;
00093 ip++;
00094 *tp += *ip * *fp;
00095 ip++;
00096 fp++;
00097 }
00098 }
00099 }
00100
00101 double total=0;
00102 for(i=0 ; i<3*mFilters.size() ; i++)
00103 {
00104 buffer[i] /= (double)mFilterSize*mFilterSize;
00105 total += buffer[i];
00106 }
00107 }
00108
00109
00116 void
00117 Reconstruct(Array::Array2dVec3Real64& image,
00118 Array::Array2dScalarReal64& texture, int posX, int posY)
00119 {
00120 Array::Array2dVec3Real64 mix(3*mFilterSize, mFilterSize, 0, 0);
00121
00122 int y;
00123 double* ip;
00124 for(y=0 ; y<mFilterSize ; y++)
00125 {
00126 ip = mix.CPB(0,y);
00127 int x;
00128 for(x=0 ; x<3*mFilterSize ; x++)
00129 {
00130 *ip = 0.0;
00131 ip++;
00132 }
00133 }
00134
00135 for(y=0 ; y<mFilterSize ; y++)
00136 {
00137 int i;
00138 for(i=0 ; i<mFilters.size() ; i++)
00139 {
00140 double *fp;
00141 ip = mix.CPB(0, y);
00142 fp = mFilters[i]->CPB(0, y);
00143 int x;
00144 for(x=0 ; x<mFilterSize ; x++)
00145 {
00146 double *tp;
00147 tp = texture.CPB(i*3, 0);
00148
00149 *ip += *tp * *fp;
00150 tp++;
00151 ip++;
00152
00153 *ip += *tp * *fp;
00154 tp++;
00155 ip++;
00156
00157 *ip += *tp * *fp;
00158 ip++;
00159 fp++;
00160 }
00161 }
00162 }
00163
00164 for(y=0 ; y<mFilterSize ; y++)
00165 {
00166 ip = mix.CPB(0, y);
00167 int x;
00168 for(x=0 ; x<3*mFilterSize ; x++)
00169 {
00170 *ip /= (double)mFilters.size();
00171 ip++;
00172 }
00173 }
00174
00175
00176 for(y=0 ; y<mFilterSize ; y++)
00177 {
00178 if(y+posY>=0 && y+posY<image.CH())
00179 {
00180 double *mp = mix.CPB(0, y);
00181 double *ip = image.CPB(posX, y+posY);
00182 for(int x=0 ; x<mFilterSize ; x++)
00183 {
00184 if(x+posX>=0 && x+posX<image.CW())
00185 {
00186 ip[3*x] += mp[3*x];
00187 ip[3*x+1] += mp[3*x+1];
00188 ip[3*x+2] += mp[3*x+2];
00189 }
00190 }
00191 }
00192 }
00193 }
00194
00195
00202 void
00203 ReconstructSeparate(std::vector<Array::Array2dVec3Real64*>* images,
00204 Array::Array2dScalarReal64& texture)
00205 {
00206 int y;
00207 for(y=0 ; y<mFilterSize ; y++)
00208 {
00209 for(int i=0 ; i<mFilters.size() ; i++)
00210 {
00211 double* ip = (*images)[i]->CPB(0,y);
00212 int x;
00213 for(x=0 ; x<3*mFilterSize ; x++)
00214 {
00215 *ip = 0.0;
00216 ip++;
00217 }
00218 }
00219 }
00220
00221 for(y=0 ; y<mFilterSize ; y++)
00222 {
00223 for(int i=0 ; i<mFilters.size() ; i++)
00224 {
00225 double *ip = (*images)[i]->CPB(0, y);
00226 double *fp = mFilters[i]->CPB(0, y);
00227 int x;
00228 for(x=0 ; x<mFilterSize ; x++)
00229 {
00230 double *tp;
00231 tp = texture.CPB(i*3, 0);
00232
00233 *ip += *tp * *fp;
00234 tp++;
00235 ip++;
00236
00237 *ip += *tp * *fp;
00238 tp++;
00239 ip++;
00240
00241 *ip += *tp * *fp;
00242 ip++;
00243 fp++;
00244 }
00245 }
00246 }
00247 }
00248
00249 private:
00250 std::vector<Array::Array2dScalarReal64*> mFilters;
00251 int mFilterSize;
00252
00253 void operator= (const KernelSet&);
00254 };
00255
00256 }
00257 }
00258 }
00259
00260 #endif //Impala_Core_Tracking_KernelSet_h