├── 6.png
├── 8.png
├── CMakeLists.txt
├── README.txt
├── include
    ├── CPM_include
    │   ├── CPM.h
    │   ├── Image.h
    │   ├── ImageFeature.h
    │   ├── ImageIO.h
    │   ├── ImageProcessing.h
    │   ├── ImagePyramid.h
    │   ├── OpticFlowIO.h
    │   ├── Stochastic.h
    │   ├── Util.h
    │   ├── Vector.h
    │   └── project.h
    ├── ColorModel.h
    ├── Minimization.h
    ├── Pixel.h
    ├── ThreadPool.h
    ├── Unmixing.h
    ├── constants.h
    └── guidedfilter.h
├── lib
    ├── libCPM_lib.a
    └── libCPM_lib.so
├── sphere01.txt
└── src
    ├── ColorModel.cpp
    ├── Minimization.cpp
    ├── Pixel.cpp
    ├── guidedfilter.cpp
    └── main.cpp


/6.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/V-Sense/soft_segmentation/a0e2717199787633eb849b5e66a59e9fd7f53b31/6.png


--------------------------------------------------------------------------------
/8.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/V-Sense/soft_segmentation/a0e2717199787633eb849b5e66a59e9fd7f53b31/8.png


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8)
 2 | 
 3 | set(PROJECTNAME "SoftSegmentation")
 4 | project(${PROJECTNAME} C CXX)
 5 | 
 6 | # for gdb debug -> run cmake -DCMAKE_BUILD_TYPE=Debug <other params>
 7 | set(CMAKE_BUILD_TYPE Debug) #Debug or Release
 8 | 
 9 | # Enable C++11 features
10 | if(CMAKE_COMPILER_IS_GNUCXX)   
11 | 	set(CMAKE_CXX_FLAGS "-std=gnu++11") 
12 | elseif(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")   
13 |        set(CMAKE_CXX_FLAGS "-std=c++11") 
14 | endif()
15 | 
16 | find_package(OpenCV REQUIRED)
17 | 
18 | set(CMAKE_THREAD_PREFER_PTHREAD TRUE)
19 | set(THREADS_PREFER_PTHREAD_FLAG TRUE)
20 | 
21 | find_package(Threads REQUIRED)
22 | 
23 | include_directories(src)
24 | include_directories(include)
25 | include_directories(include/CPM_include)
26 | 
27 | #file(GLOB HEADERS include/*.h)
28 | file(GLOB SOURCES src/*.cpp)
29 | 
30 | #message(STATUS ${CMAKE_SOURCE_DIR})
31 | 
32 | #create a library out of this program
33 | #add_library(${PROJECTNAME}_lib ${SOURCES})
34 | 
35 | add_executable(${PROJECTNAME} src/main.cpp ${SOURCES})
36 | target_link_libraries(${PROJECTNAME}
37 |     ${CMAKE_THREAD_LIBS_INIT}
38 |     ${CMAKE_SOURCE_DIR}/lib/libCPM_lib.so
39 |     ${OpenCV_LIBS}
40 |     )
41 | 


--------------------------------------------------------------------------------
/README.txt:
--------------------------------------------------------------------------------
 1 | This is a C++ implementation of the paper: 
 2 | 
 3 | Unmixing-Based Soft Color Segmentation for Image Manipulation
 4 | 
 5 | Yagiz Aksoy, Tunc Ozan Aydin, Aljosa Smolic and Marc Pollefeys, ACM Trans. Graph., 2017
 6 | 
 7 | Dependencies
 8 | 
 9 | 
10 | Ubuntu 14.04 or 16.04 
11 | OpenCV 3.x
12 | Install the following dependencies from terminal:
13 | 
14 | 
15 | sudo apt-get install libpthread-stubs0-dev libboost-all-dev
16 | 
17 | 
18 | Installation:
19 | 
20 | 1. Clone the code.
21 | 
22 | 2. Build the project using the following commands:
23 | 
24 | 	mkdir build
25 | 	cd build
26 | 	cmake ..
27 | 	make
28 | 
29 | 3. Run the code as follow:
30 | 
31 | ./SoftSegmentation <path/to/image/> <tau_parameterer(optional)> <results directory>
32 | 
33 | For example:
34 | ./SoftSegmentation ../4_small.png 15 results
35 | 
36 | Results are saved in the folder <results directory>.
37 | 
38 | A tau parameter of 11 gives good results. 
39 | 


--------------------------------------------------------------------------------
/include/CPM_include/CPM.h:
--------------------------------------------------------------------------------
 1 | /*
 2 | 
 3 | Code of the Coarse-to-Fine PatchMatch, published at CVPR 2016 in
 4 | "Efficient Coarse-to-Fine PatchMatch for Large Displacement Optical Flow"
 5 | by Yinlin.Hu, Rui Song and Yunsong Li.
 6 | 
 7 | Email: huyinlin@gmail.com
 8 | 
 9 | Version 1.2
10 | 
11 | Copyright (C) 2016 Yinlin.Hu
12 | 
13 | Usages:
14 | 
15 | The program "cpm.exe" has been built and tested on Windows 7.
16 | 
17 | USAGE: cpm.exe img1Name img2Name outMatchName
18 | 
19 | Explanations:
20 | 
21 | The output of the program is a text file, which is in the format of "x1,y1,x2,y2"
22 | corresponding to one match per line.
23 | 
24 | */
25 | 
26 | #ifndef _CPM_H_
27 | #define _CPM_H_
28 | 
29 | #include "ImagePyramid.h"
30 | 
31 | class CPM
32 | {
33 | public:
34 | 	CPM();
35 | 	~CPM();
36 | 
37 | 	int Matching(FImage& img1, FImage& img2, FImage& outMatches);
38 | 	void SetStereoFlag(int needStereo);
39 | 	void SetStep(int step);
40 | 
41 | private:
42 | 	void imDaisy(FImage& img, UCImage& outFtImg);
43 | 	void CrossCheck(IntImage& seeds, FImage& seedsFlow, FImage& seedsFlow2, IntImage& kLabel2, int* valid, float th);
44 | 	float MatchCost(FImage& img1, FImage& img2, UCImage* im1f, UCImage* im2f, int x1, int y1, int x2, int y2);
45 | 
46 | 	// a good initialization is already stored in bestU & bestV
47 | 	int Propogate(FImagePyramid& pyd1, FImagePyramid& pyd2, UCImage* pyd1f, UCImage* pyd2f, int level, float* radius, int iterCnt, IntImage* pydSeeds, IntImage& neighbors, FImage* pydSeedsFlow, float* bestCosts);
48 | 	void PyramidRandomSearch(FImagePyramid& pyd1, FImagePyramid& pyd2, UCImage* im1f, UCImage* im2f, IntImage* pydSeeds, IntImage& neighbors, FImage* pydSeedsFlow);
49 | 	void OnePass(FImagePyramid& pyd1, FImagePyramid& pyd2, UCImage* im1f, UCImage* im2f, IntImage& seeds, IntImage& neighbors, FImage* pydSeedsFlow);
50 | 	void UpdateSearchRadius(IntImage& neighbors, FImage* pydSeedsFlow, int level, float* outRadius);
51 | 
52 | 	// minimum circle
53 | 	struct Point{
54 | 		double x, y;
55 | 	};
56 | 	double dist(Point a, Point b);
57 | 	Point intersection(Point u1, Point u2, Point v1, Point v2);
58 | 	Point circumcenter(Point a, Point b, Point c);
59 | 	// return the radius of the minimal circle
60 | 	float MinimalCircle(float* x, float*y, int n, float* centerX = NULL, float* centerY = NULL);
61 | 
62 | 	//
63 | 	int _step;
64 | 	int _maxIters;
65 | 	float _stopIterRatio;
66 | 	float _pydRatio;
67 | 
68 | 	int _isStereo;
69 | 	int _maxDisplacement;
70 | 	float _checkThreshold;
71 | 	int _borderWidth;
72 | 
73 | 	IntImage _kLabels, _kLabels2;
74 | 
75 | 	FImagePyramid _pyd1;
76 | 	FImagePyramid _pyd2;
77 | 
78 | 	UCImage* _im1f;
79 | 	UCImage* _im2f;
80 | 
81 | 	FImage* _pydSeedsFlow;
82 | 	FImage* _pydSeedsFlow2;
83 | 
84 | 	IntImage _seeds;
85 | 	IntImage _seeds2;
86 | 	IntImage _neighbors;
87 | 	IntImage _neighbors2;
88 | 
89 | };
90 | 
91 | #endif // _CPM_H_


--------------------------------------------------------------------------------
/include/CPM_include/ImageFeature.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | #include "Image.h"
  3 | #include "math.h"
  4 | #include "memory.h"
  5 | #include <vector>
  6 | 
  7 | #ifndef M_PI
  8 | #define M_PI 3.1415926535897932384626433
  9 | #endif
 10 | 
 11 | 
 12 | class ImageFeature
 13 | {
 14 | public:
 15 | 	ImageFeature(void);
 16 | 	~ImageFeature(void);
 17 | 
 18 | 	template <class T>
 19 | 	static void imSIFT(const Image<T>& imsrc, UCImage& imsift, int cellSize = 2, int stepSize = 1, bool IsBoundaryIncluded = false, int nBins = 8);
 20 | 
 21 | 	template <class T>
 22 | 	static void imSIFT(const Image<T>& imsrc, UCImage& imsift, const vector<int> cellSizeVect, int stepSize = 1, bool IsBoundaryIncluded = false, int nBins = 8);
 23 | 
 24 | };
 25 | 
 26 | template <class T>
 27 | void ImageFeature::imSIFT(const Image<T>& imsrc, UCImage &imsift, int cellSize, int stepSize, bool IsBoundaryIncluded, int nBins)
 28 | {
 29 | 	if(cellSize<=0)
 30 | 	{
 31 | 		cout<<"The cell size must be positive!"<<endl;
 32 | 		return;
 33 | 	}
 34 | 
 35 | 	// this parameter controls decay of the gradient energy falls into a bin
 36 | 	// run SIFT_weightFunc.m to see why alpha = 9 is the best value
 37 | 	int alpha = 9;
 38 | 
 39 | 	int width = imsrc.width(), height = imsrc.height(), nchannels = imsrc.nchannels();
 40 | 	int nPixels = width*height;
 41 | 	FImage imdx(width, height, nchannels), imdy(width, height, nchannels);
 42 | 	// compute the derivatives;
 43 | #if 0
 44 | 	imsrc.dx(imdx, true);
 45 | 	imsrc.dy(imdy, true);
 46 | #else
 47 | 	// sobel, more robust
 48 | 	float xKernel[9] = { -1, 0, 1, -2, 0, 2, -1, 0, 1 };
 49 | 	float yKernel[9] = { -1, -2, -1, 0, 0, 0, 1, 2, 1 };
 50 | 	ImageProcessing::filtering(imsrc.pData, imdx.pData, width, height, nchannels, xKernel, 1);
 51 | 	ImageProcessing::filtering(imsrc.pData, imdy.pData, width, height, nchannels, yKernel, 1);
 52 | #endif
 53 | 
 54 | 	// get the maximum gradient over the channels and estimate the normalized gradient
 55 | 	FImage magsrc(width,height,nchannels),mag(width,height),gradient(width,height,2);
 56 | 	float Max;
 57 | 	for(int i=0;i<nPixels;i++)
 58 | 	{
 59 | 		int offset = i*nchannels;
 60 | 		for(int j = 0;j<nchannels;j++)
 61 | 			magsrc.pData[offset+j] = sqrt(imdx.pData[offset+j]*imdx.pData[offset+j]+imdy.pData[offset+j]*imdy.pData[offset+j]);
 62 | 		Max = magsrc.pData[offset];
 63 | 		if(Max!=0)
 64 | 		{
 65 | 			gradient.pData[i*2] = imdx.pData[offset]/Max;
 66 | 			gradient.pData[i*2+1] = imdy.pData[offset]/Max;
 67 | 		}
 68 | 		for(int j = 1;j<nchannels;j++)
 69 | 		{
 70 | 			if(magsrc.pData[offset+j]>Max)
 71 | 			{
 72 | 				Max = magsrc.pData[offset+j];
 73 | 				gradient.pData[i*2] = imdx.pData[offset+j]/Max;
 74 | 				gradient.pData[i*2+1] = imdy.pData[offset+j]/Max;
 75 | 			}
 76 | 		}
 77 | 		mag.pData[i] = Max;
 78 | 	}
 79 | 
 80 | 	// get the pixel-wise energy for each orientation band
 81 | 	FImage imband(width,height,nBins);
 82 | 	float theta = M_PI*2/nBins;
 83 | 	float _cos,_sin,temp;
 84 | 	for(int k = 0;k<nBins;k++)
 85 | 	{
 86 | 		_sin    = sin(theta*k);
 87 | 		_cos   = cos(theta*k);
 88 | 		for(int i = 0;i<nPixels; i++)
 89 | 		{
 90 | 			temp = __max(gradient.pData[i*2]*_cos + gradient.pData[i*2+1]*_sin,0);
 91 | 			if(alpha>1)
 92 | 				temp = pow(temp,alpha);
 93 | 			imband.pData[i*nBins+k] = temp*mag.pData[i];
 94 | 		}
 95 | 	}
 96 | 
 97 | 	// filter out the SIFT feature
 98 | 	FImage filter(cellSize*2+1,1);
 99 | 	filter[0] = filter[cellSize+1] = 0.25;
100 | 	for(int i = 1;i<cellSize+1;i++)
101 | 		filter[i+1] = 1;
102 | 	for(int i = cellSize+2;i<cellSize*2+1;i++)
103 | 		filter[i] = 0;
104 | 
105 |     FImage imband_cell;
106 | 	imband.imfilter_hv(imband_cell,filter.data(),cellSize,filter.data(),cellSize);
107 | 
108 | 	// allocate buffer for the sift image
109 | 	int siftdim = nBins*16;
110 | 	int sift_width,sift_height,x_shift=0,y_shift=0;
111 | 
112 | 	sift_width = width/stepSize;
113 | 	sift_height = height/stepSize;
114 | 
115 | 	if(IsBoundaryIncluded == false)
116 | 	{
117 | 		sift_width = (width-4*cellSize)/stepSize;
118 | 		sift_height= (height-4*cellSize)/stepSize;
119 | 		x_shift = 2*cellSize;
120 | 		y_shift = 2*cellSize;
121 | 	}
122 | 
123 | 	if(!imsift.matchDimension(sift_width,sift_height,siftdim))
124 | 		imsift.allocate(sift_width,sift_height,siftdim);
125 | 
126 | #ifdef WITH_SSE
127 | 	hu_m128 _v1, _v5;
128 | 	float v1 = 0.01, v5 = 255.;
129 | 	_v1.m = _mm_set_ps1(v1);
130 | 	_v5.m = _mm_set_ps1(v5);
131 | #endif
132 | 	// now sample to get SIFT image
133 | 	FImage sift_cell(siftdim,1);
134 | 	for (int i = 0; i < sift_height; i++){
135 | 		for (int j = 0; j < sift_width; j++){
136 | 			int count = 0;
137 | 			for (int ii = -1; ii <= 2; ii++){
138 | 				for (int jj = -1; jj <= 2; jj++){
139 | 					int y = __min(__max(y_shift + i*stepSize + ii*cellSize, 0), height - 1);
140 | 					int x = __min(__max(x_shift + j*stepSize + jj*cellSize, 0), width - 1);
141 | 
142 | 					// the following code is the same as the above two for debugging purposes
143 | 					//int y = y_shift+i*stepSize+ii*cellSize;
144 | 					//int x = x_shift+j*stepSize+jj*cellSize;
145 | 					//if (x<0 || x>=width)
146 | 					//	x = __min(__max(x,0),width-1);
147 | 					//if (y<0 || y>=height)
148 | 					//	y= __min(__max(y,0),height-1);
149 | 
150 | 					memcpy(sift_cell.pData + count*nBins, imband_cell.pData + (y*width + x)*nBins, sizeof(float)*nBins);
151 | 					count++;
152 | 				}
153 | 			}
154 | 			// normalize the SIFT descriptor
155 | #ifdef WITH_SSE
156 | 			int offset = (i*sift_width + j)*siftdim;
157 | 			//memcpy(imsift.pData+offset,sift_cell.pData,sizeof(float)*siftdim);
158 | 			unsigned char* dst = imsift.pData + offset;
159 | 			//
160 | 			hu_m128* src = (hu_m128*)sift_cell.pData;
161 | 			hu_m128 r0, r1, r2, r3, r4;
162 | 			r0.m = _mm_set_ps1(0);
163 | 			for (int ii = 0; ii < siftdim / 4; ii++){
164 | 				r1.m = _mm_mul_ps(src->m, src->m);
165 | 				r0.m = _mm_add_ps(r0.m, r1.m);
166 | 				src++;
167 | 			}
168 | 			float mag = sqrt(r0.m128_f32[0] + r0.m128_f32[1] + r0.m128_f32[2] + r0.m128_f32[3]);
169 | 			//
170 | 			hu_m128 _vmag;
171 | 			_vmag.m= _mm_set_ps1(mag);
172 | 			src = (hu_m128*)sift_cell.pData;
173 | 			for (int ii = 0; ii < siftdim / 4;ii++){
174 | 				r1.m = _mm_add_ps(_vmag.m, _v1.m);
175 | 				r2.m = _mm_div_ps(src->m, r1.m);
176 | 				r3.m = _mm_mul_ps(r2.m, _v5.m);
177 | 				r4.m = _mm_min_ps(r3.m, _v5.m);
178 | 				dst[0] = r4.m128_f32[0];
179 | 				dst[1] = r4.m128_f32[1];
180 | 				dst[2] = r4.m128_f32[2];
181 | 				dst[3] = r4.m128_f32[3];
182 | 				dst += 4;
183 | 				src++;
184 | 			}
185 | #else
186 | 			float mag = sqrt(sift_cell.norm2());
187 | 			int offset = (i*sift_width + j)*siftdim;
188 | 			//memcpy(imsift.pData+offset,sift_cell.pData,sizeof(float)*siftdim);
189 | 			for (int k = 0; k < siftdim; k++)
190 | 				imsift.pData[offset + k] = (unsigned char)__min(sift_cell.pData[k] / (mag + 0.01) * 255, 255);//(unsigned char) __min(sift_cell.pData[k]/mag*512,255);
191 | #endif
192 | 		}//*/
193 | 	}
194 | }
195 | 
196 | 
197 | //--------------------------------------------------------------------------------------------------
198 | // multi-scale SIFT features
199 | //--------------------------------------------------------------------------------------------------
200 | template <class T>
201 | void ImageFeature::imSIFT(const Image<T>& imsrc, UCImage &imsift, const vector<int> cellSizeVect, int stepSize, bool IsBoundaryIncluded, int nBins)
202 | {
203 | 	// this parameter controls decay of the gradient energy falls into a bin
204 | 	// run SIFT_weightFunc.m to see why alpha = 9 is the best value
205 | 	int alpha = 9;
206 | 
207 | 	int width = imsrc.width(), height = imsrc.height(), nchannels = imsrc.nchannels();
208 | 	int nPixels = width*height;
209 | 	FImage imdx(width, height, nchannels), imdy(width, height, nchannels);
210 | 	// compute the derivatives;
211 | #if 0
212 | 	imsrc.dx(imdx, true);
213 | 	imsrc.dy(imdy, true);
214 | #else
215 | 	// sobel, more robust
216 | 	float xKernel[9] = { -1, 0, 1, -2, 0, 2, -1, 0, 1 };
217 | 	float yKernel[9] = { -1, -2, -1, 0, 0, 0, 1, 2, 1 };
218 | 	ImageProcessing::filtering(imsrc.pData, imdx.pData, width, height, nchannels, xKernel, 1);
219 | 	ImageProcessing::filtering(imsrc.pData, imdy.pData, width, height, nchannels, yKernel, 1);
220 | #endif
221 | 
222 | 	// get the maximum gradient over the channels and estimate the normalized gradient
223 | 	FImage magsrc(width,height,nchannels),mag(width,height),gradient(width,height,2);
224 | 	float Max;
225 | 	for(int i=0;i<nPixels;i++)
226 | 	{
227 | 		int offset = i*nchannels;
228 | 		for(int j = 0;j<nchannels;j++)
229 | 			magsrc.pData[offset+j] = sqrt(imdx.pData[offset+j]*imdx.pData[offset+j]+imdy.pData[offset+j]*imdy.pData[offset+j]);
230 | 		Max = magsrc.pData[offset];
231 | 		if(Max!=0)
232 | 		{
233 | 			gradient.pData[i*2] = imdx.pData[offset]/Max;
234 | 			gradient.pData[i*2+1] = imdy.pData[offset]/Max;
235 | 		}
236 | 		for(int j = 1;j<nchannels;j++)
237 | 		{
238 | 			if(magsrc.pData[offset+j]>Max)
239 | 			{
240 | 				Max = magsrc.pData[offset+j];
241 | 				gradient.pData[i*2] = imdx.pData[offset+j]/Max;
242 | 				gradient.pData[i*2+1] = imdy.pData[offset+j]/Max;
243 | 			}
244 | 		}
245 | 		mag.pData[i] = Max;
246 | 	}
247 | 
248 | 	// get the pixel-wise energy for each orientation band
249 | 	FImage imband(width,height,nBins);
250 | 	float theta = M_PI*2/nBins;
251 | 	float _cos,_sin,temp;
252 | 	for(int k = 0;k<nBins;k++)
253 | 	{
254 | 		_sin    = sin(theta*k);
255 | 		_cos   = cos(theta*k);
256 | 		for(int i = 0;i<nPixels; i++)
257 | 		{
258 | 			temp = __max(gradient.pData[i*2]*_cos + gradient.pData[i*2+1]*_sin,0);
259 | 			if(alpha>1)
260 | 				temp = pow(temp,alpha);
261 | 			imband.pData[i*nBins+k] = temp*mag.pData[i];
262 | 		}
263 | 	}
264 | 	// get the maximum cell size
265 | 	int maxCellSize = cellSizeVect[0];
266 | 	int nScales = cellSizeVect.size();
267 | 	for(int h=1;h<nScales;h++)
268 | 		maxCellSize = __max(maxCellSize,cellSizeVect[h]);
269 | 
270 | 	// allocate buffer for the sift image
271 | 	int siftdim = nBins*16;
272 | 	int sift_width,sift_height,x_shift=0,y_shift=0;
273 | 
274 | 	sift_width = width/stepSize;
275 | 	sift_height = height/stepSize;
276 | 
277 | 	if(IsBoundaryIncluded == false)
278 | 	{
279 | 		sift_width = (width-4*maxCellSize)/stepSize;
280 | 		sift_height= (height-4*maxCellSize)/stepSize;
281 | 		x_shift = 2*maxCellSize;
282 | 		y_shift = 2*maxCellSize;
283 | 	}
284 | 
285 | 	if(!imsift.matchDimension(sift_width,sift_height,siftdim*nScales))
286 | 		imsift.allocate(sift_width,sift_height,siftdim*nScales);
287 | 
288 | 	for(int h=0;h<nScales;h++)
289 | 	{
290 | 		int cellSize = cellSizeVect[h];
291 | 		if(cellSize<=0)
292 | 		{
293 | 			cout<<"The cell size must be positive!"<<endl;
294 | 			return;
295 | 		}
296 | 		// filter out the SIFT feature
297 | 		FImage filter(cellSize*2+1,1);
298 | 		filter[0] = filter[cellSize+1] = 0.25;
299 | 		for(int i = 1;i<cellSize+1;i++)
300 | 			filter[i+1] = 1;
301 | 		for(int i = cellSize+2;i<cellSize*2+1;i++)
302 | 			filter[i] = 0;
303 | 
304 | 		FImage imband_cell;
305 | 		imband.imfilter_hv(imband_cell,filter.data(),cellSize,filter.data(),cellSize);
306 | 
307 | 		// now sample to get SIFT image
308 | 		FImage sift_cell(siftdim,1);
309 | 		for(int i=0;i<sift_height;i++)
310 | 			for(int j =0;j<sift_width;j++)
311 | 			{
312 | 				int count = 0;
313 | 				for(int ii = -1;ii<=2;ii++)
314 | 					for(int jj=-1;jj<=2;jj++)
315 | 					{
316 | 						int y = __min(__max(y_shift+i*stepSize+ii*cellSize,0),height-1);
317 | 						int x = __min(__max(x_shift+j*stepSize+jj*cellSize,0),width-1);
318 | 
319 | 						// the following code is the same as the above two for debugging purposes
320 | 						//int y = y_shift+i*stepSize+ii*cellSize;
321 | 						//int x = x_shift+j*stepSize+jj*cellSize;
322 | 						//if (x<0 || x>=width)
323 | 						//	x = __min(__max(x,0),width-1);
324 | 						//if (y<0 || y>=height)
325 | 						//	y= __min(__max(y,0),height-1);
326 | 
327 | 						memcpy(sift_cell.pData+count*nBins,imband_cell.pData+(y*width+x)*nBins,sizeof(float)*nBins);
328 | 						count++;
329 | 					}
330 | 				// normalize the SIFT descriptor
331 | 				float mag = sqrt(sift_cell.norm2());
332 | 				int offset = (i*sift_width+j)*siftdim*nScales+h*siftdim;
333 | 				//memcpy(imsift.pData+offset,sift_cell.pData,sizeof(float)*siftdim);
334 | 				for(int k = 0;k<siftdim;k++)
335 | 					imsift.pData[offset+k] = (unsigned char)__min(sift_cell.pData[k]/(mag+0.01)*255,255);//(unsigned char) __min(sift_cell.pData[k]/mag*512,255);
336 | 			}//*/
337 | 	}
338 | }
339 | 


--------------------------------------------------------------------------------
/include/CPM_include/ImageIO.h:
--------------------------------------------------------------------------------
  1 | #ifndef _ImageIO_h
  2 | #define _ImageIO_h
  3 | 
  4 | #include "Util.h"
  5 | #include "project.h"
  6 | #include "malloc.h"
  7 | #include "opencv2/opencv.hpp"
  8 | #include <typeinfo>
  9 | 
 10 | #ifdef WITH_SSE
 11 | #include <xmmintrin.h>
 12 | #endif
 13 | 
 14 | template <class T>
 15 | inline void* xmalloc(T size){
 16 | #ifdef WITH_SSE
 17 | #ifdef WIN32
 18 | 	return _aligned_malloc(size, 32);
 19 | #else
 20 | 	return memalign(32, size);
 21 | #endif
 22 | #else
 23 | 	return malloc(size);
 24 | #endif
 25 | }
 26 | 
 27 | template <class T>
 28 | inline void xfree(T* ptr){
 29 | #if defined(WITH_SSE) && defined(WIN32)
 30 | 	_aligned_free(ptr);
 31 | #else
 32 | 	free(ptr);
 33 | #endif
 34 | }
 35 | 
 36 | #ifdef WITH_SSE
 37 | 
 38 | // for windows and linux
 39 | typedef union _m128{
 40 | 	__m128 m;
 41 | 	__m128i mi;
 42 | 	float m128_f32[4];
 43 | 	unsigned short m128i_u16[8];
 44 | }hu_m128;
 45 | 
 46 | #endif
 47 | 
 48 | class ImageIO
 49 | {
 50 | public:
 51 | 	enum ImageType{standard, derivative, normalized};
 52 | 	ImageIO(void);
 53 | 	~ImageIO(void);
 54 | public:
 55 | 	template <class T>
 56 | 	static bool loadImage(const char* filename,T*& pImagePlane,int& width,int& height, int& nchannels);
 57 | 	template <class T>
 58 | 	static bool saveImage(const char* filename,const T* pImagePlane,int width,int height, int nchannels,ImageType imtype = standard);
 59 | 	template <class T>
 60 | 	static void showImage(const char* winname, const T* pImagePlane, int width, int height, int nchannels, ImageType imtype = standard, int waittime = 1);
 61 | 	template <class T>
 62 | 	static void showGrayImageAsColor(const char* winname, const unsigned char* pImagePlane, int width, int height, T minV, T maxV, int waittime = 1);
 63 | 	template <class T>
 64 | 	static cv::Mat CvmatFromPixels(const T* pImagePlane, int width, int height, int nchannels, ImageType imtype = standard);
 65 | 	template <class T>
 66 | 	static void CvmatToPixels(const cv::Mat& cvInImg, T*& pOutImagePlane, int& width, int& height, int& nchannels);
 67 | private:
 68 | };
 69 | 
 70 | template <class T>
 71 | bool ImageIO::loadImage(const char *filename, T *&pImagePlane, int &width, int &height, int &nchannels)
 72 | {
 73 | 	cv::Mat im = cv::imread(filename);
 74 | 	if (im.data == NULL){
 75 | 		return false;
 76 | 	}
 77 | 	pImagePlane = (T*)xmalloc(sizeof(T) * im.total() * im.elemSize());
 78 | 	CvmatToPixels(im, pImagePlane, width, height, nchannels);
 79 | 	return true;
 80 | }
 81 | 
 82 | template <class T>
 83 | bool ImageIO::saveImage(const char* filename,const T* pImagePlane,int width,int height, int nchannels,ImageType imtype)
 84 | {
 85 | 	cv::Mat img = CvmatFromPixels(pImagePlane, width, height, nchannels, imtype);
 86 | 	return cv::imwrite(filename, img);
 87 | }
 88 | 
 89 | template <class T>
 90 | void ImageIO::showImage(const char* winname, const T* pImagePlane, int width, int height, int nchannels, 
 91 | 	ImageType imtype /*= standard*/, int waittime /*= 1*/)
 92 | {
 93 | 	cv::Mat img = CvmatFromPixels(pImagePlane, width, height, nchannels, imtype);
 94 | 	cv::imshow(winname, img);
 95 | 	cv::waitKey(waittime);
 96 | }
 97 | 
 98 | template <class T>
 99 | void ImageIO::showGrayImageAsColor(const char* winname, const unsigned char* pImagePlane, int width, int height, 
100 | 	T minV, T maxV, int waittime /*= 1*/)
101 | {
102 | 	CColorTable colorTbl;
103 | 
104 | 	// check whether the type is float point
105 | 	bool IsFloat = false;
106 | 	if (typeid(T) == typeid(double) || typeid(T) == typeid(float) || typeid(T) == typeid(long double))
107 | 		IsFloat = true;
108 | 
109 | 	cv::Mat im;
110 | 	im.create(height, width, CV_8UC3);
111 | 	for (int i = 0; i < height; i++){
112 | 		for (int j = 0; j < width; j++){
113 | 			unsigned char grayVal = pImagePlane[i*width + j];
114 | 			if (IsFloat){
115 | 				grayVal = pImagePlane[i*width + j] * 255;
116 | 			}
117 | 			memcpy(im.data + i*im.step + j * 3, colorTbl[grayVal], 3);
118 | 		}
119 | 	}
120 | 
121 | 	// show range
122 | 	char info[256];
123 | 	if (IsFloat)
124 | 		sprintf(info, "[%.3f, %.3f]", (float)minV, (float)maxV);
125 | 	else
126 | 		sprintf(info, "[%d, %d]", (int)minV, (int)maxV);
127 | 	cv::putText(im, info, cvPoint(10, 20), CV_FONT_HERSHEY_SIMPLEX, 0.5, cvScalar(255, 255, 255));
128 | 
129 | 	//
130 | 	cv::imshow(winname, im);
131 | 	cv::waitKey(waittime);
132 | }
133 | 
134 | template <class T>
135 | cv::Mat ImageIO::CvmatFromPixels(const T* pImagePlane, int width, int height, int nchannels, ImageType imtype /*= standard*/)
136 | {
137 | 	cv::Mat im;
138 | 	switch (nchannels){
139 | 	case 1:
140 | 		im.create(height, width, CV_8UC1);
141 | 		break;
142 | 	case 3:
143 | 		im.create(height, width, CV_8UC3);
144 | 		break;
145 | 	case 4:
146 | 		im.create(height, width, CV_8UC4);
147 | 		break;
148 | 	default:
149 | 		return im;
150 | 	}
151 | 	// check whether the type is float point
152 | 	bool IsFloat = false;
153 | 	if (typeid(T) == typeid(double) || typeid(T) == typeid(float) || typeid(T) == typeid(long double))
154 | 		IsFloat = true;
155 | 
156 | 	double Max, Min;
157 | 	int nElements = width*height*nchannels;
158 | 	switch (imtype){
159 | 	case standard:
160 | 		break;
161 | 	case derivative:
162 | 		// find the max of the absolute value
163 | 		Max = pImagePlane[0];
164 | 		for (int i = 0; i < nElements; i++)
165 | 			Max = __max(Max, fabs((double)pImagePlane[i]));
166 | 		Min = -Max;
167 | 		break;
168 | 	case normalized:
169 | 		Max = Min = pImagePlane[0];
170 | 		for (int i = 0; i < nElements; i++)
171 | 		{
172 | 			Max = __max(Max, pImagePlane[i]);
173 | 			Min = __min(Min, pImagePlane[i]);
174 | 		}
175 | 		break;
176 | 	}
177 | 	if (typeid(T) == typeid(unsigned char) && imtype == standard)
178 | 	{
179 | 		for (int i = 0; i < height; i++)
180 | 			memcpy(im.data + i*im.step, pImagePlane + i*im.step, width*nchannels);
181 | 	}
182 | 	else
183 | 	{
184 | 		for (int i = 0; i < height; i++)
185 | 		{
186 | 			int offset1 = i*width*nchannels;
187 | 			int offset2 = i*im.step;
188 | 			for (int j = 0; j < im.step; j++)
189 | 			{
190 | 				switch (imtype){
191 | 				case standard:
192 | 					if (IsFloat)
193 | 						im.data[offset2 + j] = pImagePlane[offset1 + j] * 255;
194 | 					else
195 | 						im.data[offset2 + j] = __max(__min(pImagePlane[offset1 + j], 255), 0);
196 | 					break;
197 | 				case derivative:
198 | 				case normalized:
199 | 					im.data[offset2 + j] = ((double)pImagePlane[offset1 + j] - Min) / (Max - Min) * 255;
200 | 					break;
201 | 				}
202 | 			}
203 | 		}
204 | 	}
205 | 
206 | 	// show range
207 | 	if (imtype == derivative || imtype == normalized){
208 | 		char info[256];
209 | 		if (IsFloat)
210 | 			sprintf(info, "[%.3f, %.3f]", Min, Max);
211 | 		else
212 | 			sprintf(info, "[%d, %d]", (int)Min, (int)Max);
213 | 		cv::putText(im, info, cvPoint(10, 20), CV_FONT_HERSHEY_SIMPLEX, 0.5, cvScalar(255, 255, 255));
214 | 	}
215 | 
216 | 	return im;
217 | }
218 | 
219 | template <class T>
220 | void ImageIO::CvmatToPixels(const cv::Mat& cvInImg, T*& pOutImagePlane, int& width, int& height, int& nchannels)
221 | {
222 | 	if (cvInImg.data == NULL) // if allocation fails
223 | 		return;
224 | 	if (cvInImg.type() != CV_8UC1 && cvInImg.type() != CV_8UC3 && cvInImg.type() != CV_8UC4) // we only support three types of image information for now
225 | 		return;
226 | 	width = cvInImg.size().width;
227 | 	height = cvInImg.size().height;
228 | 	nchannels = cvInImg.channels();
229 | 
230 | 	if (typeid(T) == typeid(unsigned char))
231 | 	{
232 | 		for (int i = 0; i < height; i++)
233 | 			memcpy(pOutImagePlane + i*cvInImg.step, cvInImg.data + i*cvInImg.step, width*nchannels);
234 | 		return;
235 | 	}
236 | 
237 | 	// check whether the type is float point
238 | 	bool IsFloat = false;
239 | 	if (typeid(T) == typeid(double) || typeid(T) == typeid(float) || typeid(T) == typeid(long double))
240 | 		IsFloat = true;
241 | 
242 | 	for (int i = 0; i < height; i++)
243 | 	{
244 | 		int offset1 = i*width*nchannels;
245 | 		int offset2 = i*cvInImg.step;
246 | 		for (int j = 0; j < width*nchannels; j++)
247 | 		{
248 | 			if (IsFloat)
249 | 				pOutImagePlane[offset1 + j] = (T)cvInImg.data[offset2 + j] / 255;
250 | 			else
251 | 				pOutImagePlane[offset1 + j] = cvInImg.data[offset2 + j];
252 | 		}
253 | 	}
254 | 	return;
255 | }
256 | 
257 | /*
258 | #include <QVector>
259 | #include <QImage>
260 | #include <QString>
261 | #include "math.h"
262 | //-----------------------------------------------------------------------------------------
263 | // this class is a wrapper to use QImage to load image into image planes
264 | //-----------------------------------------------------------------------------------------
265 | 
266 | class ImageIO
267 | {
268 | public:
269 | 	enum ImageType{standard, derivative, normalized};
270 | 	ImageIO(void);
271 | 	~ImageIO(void);
272 | public:
273 | 	template <class T>
274 | 	static void loadImage(const QImage& image,T*& pImagePlane,int& width,int& height,int& nchannels);
275 | 	template <class T>
276 | 	static bool loadImage(const QString& filename,T*& pImagePlane,int& width,int& height,int& nchannels);
277 | 
278 | 	template <class T>
279 | 	static unsigned char convertPixel(const T& value,bool IsFloat,ImageType type,T& _Max,T& _Min);
280 | 
281 | 	template <class T>
282 | 	static bool writeImage(const QString& filename, const T*& pImagePlane,int width,int height,int nchannels,ImageType type=standard,int quality=-1);
283 | 
284 | 	template <class T>
285 | 	static bool writeImage(const QString& filename,const T* pImagePlane,int width,int height,int nchannels,T min, T max,int quality=-1);
286 | 
287 | };
288 | 
289 | template <class T>
290 | void ImageIO::loadImage(const QImage& image, T*& pImagePlane,int& width,int& height,int& nchannels)
291 | {
292 | 	// get the image information
293 | 	width=image.width();
294 | 	height=image.height();
295 | 	nchannels=3;
296 | 	pImagePlane=new T[width*height*nchannels];
297 | 
298 | 	// check whether the type is float point
299 | 	bool IsFloat=false;
300 | 	if(typeid(T)==typeid(double) || typeid(T)==typeid(float) || typeid(T)==typeid(long double))
301 | 		IsFloat=true;
302 | 
303 | 	const unsigned char* plinebuffer;
304 | 	for(int i=0;i<height;i++)
305 | 	{
306 | 		plinebuffer=image.scanLine(i);
307 | 		for(int j=0;j<width;j++)
308 | 		{
309 | 			if(IsFloat)
310 | 			{
311 | 				pImagePlane[(i*width+j)*3]=(T)plinebuffer[j*4]/255;
312 | 				pImagePlane[(i*width+j)*3+1]=(T)plinebuffer[j*4+1]/255;
313 | 				pImagePlane[(i*width+j)*3+2]=(T)plinebuffer[j*4+2]/255;
314 | 			}
315 | 			else
316 | 			{
317 | 				pImagePlane[(i*width+j)*3]=plinebuffer[j*4];
318 | 				pImagePlane[(i*width+j)*3+1]=plinebuffer[j*4+1];
319 | 				pImagePlane[(i*width+j)*3+2]=plinebuffer[j*4+2];
320 | 			}
321 | 		}
322 | 	}
323 | }
324 | 
325 | template <class T>
326 | bool ImageIO::loadImage(const QString&filename, T*& pImagePlane,int& width,int& height,int& nchannels)
327 | {
328 | 	QImage image;
329 | 	if(image.load(filename)==false)
330 | 		return false;
331 | 	if(image.format()!=QImage::Format_RGB32)
332 | 	{
333 | 		QImage temp=image.convertToFormat(QImage::Format_RGB32);
334 | 		image=temp;
335 | 	}
336 | 	loadImage(image,pImagePlane,width,height,nchannels);
337 | 	return true;
338 | }
339 | 
340 | template <class T>
341 | bool ImageIO::writeImage(const QString& filename, const T*& pImagePlane,int width,int height,int nchannels,ImageType type,int quality)
342 | {
343 | 	int nPixels=width*height,nElements;
344 | 	nElements=nPixels*nchannels;
345 | 	unsigned char* pTempBuffer;
346 | 	pTempBuffer=new unsigned char[nPixels*4];
347 | 	memset(pTempBuffer,0,nPixels*4);
348 | 
349 | 	// check whether the type is float point
350 | 	bool IsFloat=false;
351 | 	if(typeid(T)==typeid(double) || typeid(T)==typeid(float) || typeid(T)==typeid(long double))
352 | 		IsFloat=true;
353 | 
354 | 	T _Max=0,_Min=0;
355 | 	switch(type){
356 | 		case standard:
357 | 			break;
358 | 		case derivative:
359 | 			_Max=0;
360 | 			for(int i=0;i<nPixels;i++)
361 | 			{
362 | 				if(IsFloat)
363 | 					_Max=__max(_Max,fabs((double)pImagePlane[i]));
364 | 				else
365 | 					_Max=__max(_Max,abs(pImagePlane[i]));
366 | 			}
367 | 			break;
368 | 		case normalized:
369 | 			_Min=_Max=pImagePlane[0];
370 | 			for(int i=1;i<nElements;i++)
371 | 			{
372 | 				_Min=__min(_Min,pImagePlane[i]);
373 | 				_Max=__max(_Max,pImagePlane[i]);
374 | 			}
375 | 			break;
376 | 	}
377 | 
378 | 	for(int i=0;i<nPixels;i++)
379 | 	{
380 | 		if(nchannels>=3)
381 | 		{
382 | 			pTempBuffer[i*4]=convertPixel(pImagePlane[i*nchannels],IsFloat,type,_Max,_Min);
383 | 			pTempBuffer[i*4+1]=convertPixel(pImagePlane[i*nchannels+1],IsFloat,type,_Max,_Min);
384 | 			pTempBuffer[i*4+2]=convertPixel(pImagePlane[i*nchannels+2],IsFloat,type,_Max,_Min);
385 | 		}
386 | 		else 
387 | 			for (int j=0;j<3;j++)
388 | 				pTempBuffer[i*4+j]=convertPixel(pImagePlane[i*nchannels],IsFloat,type,_Max,_Min);
389 | 		pTempBuffer[i*4+3]=255;
390 | 	}
391 | 	QImage *pQImage=new QImage(pTempBuffer,width,height,QImage::Format_RGB32);
392 | 	bool result= pQImage->save(filename,0,quality);
393 | 	delete pQImage;
394 | 	delete pTempBuffer;
395 | 	return result;
396 | }
397 | 
398 | template <class T>
399 | bool ImageIO::writeImage(const QString& filename, const T* pImagePlane,int width,int height,int nchannels,T min,T max,int quality)
400 | {
401 | 	int nPixels=width*height,nElements;
402 | 	nElements=nPixels*nchannels;
403 | 	unsigned char* pTempBuffer;
404 | 	pTempBuffer=new unsigned char[nPixels*4];
405 | 	memset(pTempBuffer,0,nPixels*4);
406 | 
407 | 	// check whether the type is float point
408 | 	bool IsFloat=false;
409 | 	if(typeid(T)==typeid(double) || typeid(T)==typeid(float) || typeid(T)==typeid(long double))
410 | 		IsFloat=true;
411 | 
412 | 	T _Max=max,_Min=min;
413 | 
414 | 	for(int i=0;i<nPixels;i++)
415 | 	{
416 | 		if(nchannels>=3)
417 | 		{
418 | 			pTempBuffer[i*4]=convertPixel(pImagePlane[i*nchannels],IsFloat,normalized,_Max,_Min);
419 | 			pTempBuffer[i*4+1]=convertPixel(pImagePlane[i*nchannels+1],IsFloat,normalized,_Max,_Min);
420 | 			pTempBuffer[i*4+2]=convertPixel(pImagePlane[i*nchannels+2],IsFloat,normalized,_Max,_Min);
421 | 		}
422 | 		else 
423 | 			for (int j=0;j<3;j++)
424 | 				pTempBuffer[i*4+j]=convertPixel(pImagePlane[i*nchannels],IsFloat,normalized,_Max,_Min);
425 | 		pTempBuffer[i*4+3]=255;
426 | 	}
427 | 	QImage *pQImage=new QImage(pTempBuffer,width,height,QImage::Format_RGB32);
428 | 	bool result= pQImage->save(filename,0,quality);
429 | 	delete pQImage;
430 | 	delete pTempBuffer;
431 | 	return result;
432 | }
433 | 
434 | template <class T>
435 | unsigned char ImageIO::convertPixel(const T& value,bool IsFloat,ImageType type,T& _Max,T& _Min)
436 | {
437 | 	switch(type){
438 | 		case standard:
439 | 			if(IsFloat)
440 | 				return __max(__min(value*255,255),0);
441 | 			else
442 | 				return __max(__min(value,255),0);
443 | 			break;
444 | 		case derivative:
445 | 			return (double)((double)value/_Max+1)/2*255;
446 | 			break;
447 | 		case normalized:
448 | 			return (double)(value-_Min)/(_Max-_Min)*255;
449 | 			break;
450 | 	}
451 | 	return 0;
452 | }
453 | //*/
454 | #endif


--------------------------------------------------------------------------------
/include/CPM_include/ImageProcessing.h:
--------------------------------------------------------------------------------
  1 | #ifndef _ImageProcessing_h
  2 | #define _ImageProcessing_h
  3 | 
  4 | #include "math.h"
  5 | #include "stdio.h"
  6 | #include "stdlib.h"
  7 | #include "project.h"
  8 | #include <typeinfo>
  9 | 
 10 | //----------------------------------------------------------------------------------
 11 | // class to handle basic image processing functions
 12 | // this is a collection of template functions. These template functions are
 13 | // used in other image classes such as BiImage, IntImage and FImage
 14 | //----------------------------------------------------------------------------------
 15 | enum InterType{ INTER_NN, INTER_LINEAR };
 16 | 
 17 | class ImageProcessing
 18 | {
 19 | public:
 20 | 	ImageProcessing(void);
 21 | 	~ImageProcessing(void);
 22 | public:
 23 | 
 24 | 	// basic functions
 25 | 	template <class T>
 26 | 	static inline T EnforceRange(const T& x,const int& MaxValue) {return __min(__max(x,0),MaxValue-1);};
 27 | 
 28 | 	// Values for L are in the range[0, 100] while a and b are roughly in the range[-110, 110].
 29 | 	template <class T1, class T2>
 30 | 	static void BGR2Lab(T1* pSrcImage, T2* pDstImage, int width, int height);
 31 | 
 32 | 	//---------------------------------------------------------------------------------
 33 | 	// function to interpolate the image plane
 34 | 	//---------------------------------------------------------------------------------
 35 | 	template <class T1,class T2>
 36 | 	static inline void BilinearInterpolate(const T1* pImage,int width,int height,int nChannels,float x,float y,T2* result);
 37 | 
 38 | 	template <class T1>
 39 | 	static inline T1 BilinearInterpolate(const T1* pImage,int width,int height,float x,float y);
 40 | 
 41 | 	// the transpose of bilinear interpolation
 42 | 	template <class T1,class T2>
 43 | 	static inline void BilinearInterpolate_transpose(const T1* pImage,int width,int height,int nChannels,float x,float y,T2* result);
 44 | 
 45 | 	template <class T1>
 46 | 	static inline T1 BilinearInterpolate_transpose(const T1* pImage,int width,int height,float x,float y);
 47 | 
 48 | 	template <class T1,class T2>
 49 | 	static void ResizeImage(const T1* pSrcImage,T2* pDstImage,int SrcWidth,int SrcHeight,int nChannels,float Ratio, InterType type = INTER_LINEAR);
 50 | 
 51 | 	template <class T1,class T2>
 52 | 	static void ResizeImage(const T1* pSrcImage, T2* pDstImage, int SrcWidth, int SrcHeight, int nChannels, int DstWidth, int DstHeight, InterType type = INTER_LINEAR);
 53 | 
 54 | 	//---------------------------------------------------------------------------------
 55 | 	// functions for 1D filtering
 56 | 	//---------------------------------------------------------------------------------
 57 | 	template <class T1,class T2>
 58 | 	static void hfiltering(const T1* pSrcImage,T2* pDstImage,int width,int height,int nChannels,const float* pfilter1D,int fsize);
 59 | 
 60 | 	template <class T1,class T2>
 61 | 	static void vfiltering(const T1* pSrcImage,T2* pDstImage,int width,int height,int nChannels,const float* pfilter1D,int fsize);
 62 | 
 63 | 	template <class T1,class T2>
 64 | 	static void hfiltering_transpose(const T1* pSrcImage,T2* pDstImage,int width,int height,int nChannels,const float* pfilter1D,int fsize);
 65 | 
 66 | 	template <class T1,class T2>
 67 | 	static void vfiltering_transpose(const T1* pSrcImage,T2* pDstImage,int width,int height,int nChannels,const float* pfilter1D,int fsize);
 68 | 
 69 | 	//---------------------------------------------------------------------------------
 70 | 	// functions for 2D filtering
 71 | 	//---------------------------------------------------------------------------------
 72 | 	template <class T1,class T2>
 73 | 	static void filtering(const T1* pSrcImage,T2* pDstImage,int width,int height,int nChannels,const float* pfilter2D,int fsize);
 74 | 
 75 | 	template <class T1,class T2>
 76 | 	static void filtering_transpose(const T1* pSrcImage,T2* pDstImage,int width,int height,int nChannels,const float* pfilter2D,int fsize);
 77 | 
 78 | 	template <class T1,class T2>
 79 | 	static void Laplacian(const T1* pSrcImage,T2* pDstImage,int width,int height,int nChannels);
 80 | 
 81 | 	template <class T1, class T2>
 82 | 	static void Medianfiltering(const T1* pSrcImage, T2* pDstImage, int width, int height, int nChannels, int fsize);
 83 | 
 84 | 	template <class T1, class T2>
 85 | 	static void Integral(const T1* pSrcImage, T2* pDstImage, int width, int height, int nChannels);
 86 | 
 87 | 	template <class T1, class T2> // O(1) time box filtering using cumulative sum
 88 | 	static void BoxFilter(const T1* pSrcImage, T2* pDstImage, int width, int height, int nChannels, int r, bool norm = true);
 89 | 
 90 | 	//---------------------------------------------------------------------------------
 91 | 	// functions for sample a patch from the image
 92 | 	//---------------------------------------------------------------------------------
 93 | 	template <class T1,class T2>
 94 | 	static void getPatch(const T1* pSrcImgae,T2* pPatch,int width,int height,int nChannels,float x,float y,int wsize);
 95 | 
 96 | 	//---------------------------------------------------------------------------------
 97 | 	// function to warp image
 98 | 	//---------------------------------------------------------------------------------
 99 | 	template <class T1,class T2>
100 | 	static void warpImage(T1* pWarpIm2,const T1* pIm1,const T1* pIm2,const T2* pVx,const T2* pVy,int width,int height,int nChannels);
101 | 
102 | 	template <class T1,class T2>
103 | 	static void warpImageFlow(T1* pWarpIm2,const T1* pIm1,const T1* pIm2,const T2* pFlow,int width,int height,int nChannels);
104 | 
105 | 	template <class T1,class T2>
106 | 	static void warpImage(T1* pWarpIm2,const T1* pIm2,const T2* pVx,const T2* pVy,int width,int height,int nChannels);
107 | 
108 | 	template <class T1,class T2>
109 | 	static void warpImage_transpose(T1* pWarpIm2,const T1* pIm2,const T2* pVx,const T2* pVy,int width,int height,int nChannels);
110 | 
111 | 	template <class T1,class T2>
112 | 	static void warpImage(T1* pWarpIm2,const T1* pIm2,const T2*flow,int width,int height,int nChannels);
113 | 
114 | 	template <class T1,class T2>
115 | 	static void warpImage_transpose(T1* pWarpIm2,const T1* pIm2,const T2* flow,int width,int height,int nChannels);
116 | 
117 | 	template <class T1,class T2,class T3>
118 | 	static void warpImage(T1 *pWarpIm2, T3* pMask,const T1 *pIm1, const T1 *pIm2, const T2 *pVx, const T2 *pVy, int width, int height, int nChannels);
119 | 
120 | 
121 | 	//---------------------------------------------------------------------------------
122 | 	// function to crop an image
123 | 	//---------------------------------------------------------------------------------
124 | 	template <class T1,class T2>
125 | 	static void cropImage(const T1* pSrcImage,int SrcWidth,int SrcHeight,int nChannels,T2* pDstImage,int Left,int Top,int DstWidth,int DstHeight);
126 | 	//---------------------------------------------------------------------------------
127 | 
128 | 	//---------------------------------------------------------------------------------
129 | 	// function to generate a 2D Gaussian
130 | 	//---------------------------------------------------------------------------------
131 | 	template <class T>
132 | 	static void generate2DGaussian(T*& pImage,int wsize,float sigma=-1);
133 | 
134 | 	template <class T>
135 | 	static void generate1DGaussian(T*& pImage,int wsize,float sigma=-1);
136 | 
137 | };
138 | 
139 | template <class T1, class T2>
140 | void ImageProcessing::BGR2Lab(T1* pSrcImage, T2* pDstImage, int width, int height)
141 | {
142 | 	float normFactor = 1.f;
143 | 	if (typeid(T1) == typeid(unsigned char)){
144 | 		normFactor = 255.f;
145 | 	}
146 | 	T1* pBGR = pSrcImage;
147 | 	T2* pLab = pDstImage;
148 | 	const int npix = width*height;
149 | 
150 | 	const float T = 0.008856;
151 | 	const float color_attenuation = 1.5f;
152 | 	int i;
153 | 	for (i = 0; i < npix; i++){
154 | 		const float b = pBGR[0] / normFactor;
155 | 		const float g = pBGR[1] / normFactor;
156 | 		const float r = pBGR[2] / normFactor;
157 | 		float X = 0.412453 * r + 0.357580 * g + 0.180423 * b;
158 | 		float Y = 0.212671 * r + 0.715160 * g + 0.072169 * b;
159 | 		float Z = 0.019334 * r + 0.119193 * g + 0.950227 * b;
160 | 		X /= 0.950456;
161 | 		Z /= 1.088754;
162 | 		float Y3 = pow(Y, 1. / 3);
163 | 		float fX = X > T ? pow(X, 1. / 3) : 7.787 * X + 16 / 116.;
164 | 		float fY = Y > T ? Y3 : 7.787 * Y + 16 / 116.;
165 | 		float fZ = Z > T ? pow(Z, 1. / 3) : 7.787 * Z + 16 / 116.;
166 | 		float L = Y > T ? 116 * Y3 - 16.0 : 903.3 * Y;
167 | 		float A = 500 * (fX - fY);
168 | 		float B = 200 * (fY - fZ);
169 | 		// correct L*a*b*: dark area or light area have less reliable colors
170 | 		float correct_lab = exp(-color_attenuation*pow(pow(L / 100, 2) - 0.6, 2));
171 | 		pLab[0] = L;
172 | 		pLab[1] = A*correct_lab;
173 | 		pLab[2] = B*correct_lab;
174 | #if 0
175 | 		// considering the Values for L are in the range[0, 100] while a and b are roughly in the range[-110, 110],
176 | 		// normalize to [0,1]
177 | 		pLab[0] /= 220.;
178 | 		pLab[1] = (pLab[1] + 110) / 220.;
179 | 		pLab[2] = (pLab[2] + 110) / 220.;
180 | #endif
181 | 		//
182 | 		pBGR += 3;
183 | 		pLab += 3;
184 | 	}
185 | }
186 | 
187 | //--------------------------------------------------------------------------------------------------
188 | // function to interpolate multi-channel image plane for (x,y)
189 | // --------------------------------------------------------------------------------------------------
190 | template <class T1,class T2>
191 | inline void ImageProcessing::BilinearInterpolate(const T1* pImage,int width,int height,int nChannels,float x,float y,T2* result)
192 | {
193 | 	int xx,yy,m,n,u,v,l,offset;
194 | 	xx=x;
195 | 	yy=y;
196 | 	float dx,dy,s;
197 | 	dx=__max(__min(x-xx,1),0);
198 | 	dy=__max(__min(y-yy,1),0);
199 | 
200 | 	memset(result, 0, sizeof(T2)*nChannels);
201 | 	for(m=0;m<=1;m++)
202 | 		for(n=0;n<=1;n++)
203 | 		{
204 | 			u=EnforceRange(xx+m,width);
205 | 			v=EnforceRange(yy+n,height);
206 | 			offset=(v*width+u)*nChannels;
207 | 			s=fabs(1-m-dx)*fabs(1-n-dy);
208 | 			for(l=0;l<nChannels;l++)
209 | 				result[l]+=pImage[offset+l]*s;
210 | 		}
211 | }
212 | 
213 | template <class T1>
214 | inline T1 ImageProcessing::BilinearInterpolate(const T1* pImage,int width,int height,float x,float y)
215 | {
216 | 	int xx,yy,m,n,u,v,l,offset;
217 | 	xx=x;
218 | 	yy=y;
219 | 	float dx,dy,s;
220 | 	dx=__max(__min(x-xx,1),0);
221 | 	dy=__max(__min(y-yy,1),0);
222 | 
223 | 	T1 result=0;
224 | 	for(m=0;m<=1;m++)
225 | 		for(n=0;n<=1;n++)
226 | 		{
227 | 			u=EnforceRange(xx+m,width);
228 | 			v=EnforceRange(yy+n,height);
229 | 			offset=v*width+u;
230 | 			s=fabs(1-m-dx)*fabs(1-n-dy);
231 | 			result+=pImage[offset]*s;
232 | 		}
233 | 	return result;
234 | }
235 | 
236 | 
237 | //--------------------------------------------------------------------------------------------------
238 | // function to interpolate multi-channel image plane for (x,y)
239 | // --------------------------------------------------------------------------------------------------
240 | template <class T1,class T2>
241 | inline void ImageProcessing::BilinearInterpolate_transpose(const T1* pInput,int width,int height,int nChannels,float x,float y,T2* pDstImage)
242 | {
243 | 	int xx,yy,m,n,u,v,l,offset;
244 | 	xx=x;
245 | 	yy=y;
246 | 	float dx,dy,s;
247 | 	dx=__max(__min(x-xx,1),0);
248 | 	dy=__max(__min(y-yy,1),0);
249 | 
250 | 	for(m=0;m<=1;m++)
251 | 		for(n=0;n<=1;n++)
252 | 		{
253 | 			u=EnforceRange(xx+m,width);
254 | 			v=EnforceRange(yy+n,height);
255 | 			offset=(v*width+u)*nChannels;
256 | 			s=fabs(1-m-dx)*fabs(1-n-dy);
257 | 			for(l=0;l<nChannels;l++)
258 | 				pDstImage[offset+l] += pInput[l]*s;
259 | 		}
260 | }
261 | 
262 | //------------------------------------------------------------------------------------------------------------
263 | // this is the most general function for resizing an image with a varying nChannels
264 | // bilinear interpolation is used for now. It might be replaced by other (bicubic) interpolation methods
265 | //------------------------------------------------------------------------------------------------------------
266 | template <class T1,class T2>
267 | void ImageProcessing::ResizeImage(const T1* pSrcImage, T2* pDstImage, int SrcWidth, int SrcHeight, int nChannels, float Ratio, InterType type/* = INTER_LINEAR*/)
268 | {
269 | 	int DstWidth,DstHeight;
270 | 	DstWidth=(float)SrcWidth*Ratio;
271 | 	DstHeight=(float)SrcHeight*Ratio;
272 | 	memset(pDstImage,0,sizeof(T2)*DstWidth*DstHeight*nChannels);
273 | 
274 | 	float x,y;
275 | 
276 | 	if (type == INTER_LINEAR){
277 | 		for (int i = 0; i < DstHeight; i++)
278 | 			for (int j = 0; j < DstWidth; j++)
279 | 			{
280 | 				x = (float)(j + 1) / Ratio - 1;
281 | 				y = (float)(i + 1) / Ratio - 1;
282 | 
283 | 				// bilinear interpolation
284 | 				BilinearInterpolate(pSrcImage, SrcWidth, SrcHeight, nChannels, x, y, pDstImage + (i*DstWidth + j)*nChannels);
285 | 			}
286 | 	}else if (type == INTER_NN){
287 | 		int ix, iy;
288 | 		for (int i = 0; i < DstHeight; i++)
289 | 			for (int j = 0; j < DstWidth; j++)
290 | 			{
291 | 				x = (float)(j + 1) / Ratio - 1;
292 | 				y = (float)(i + 1) / Ratio - 1;
293 | 				ix = EnforceRange(x + 0.5, SrcWidth);
294 | 				iy = EnforceRange(y + 0.5, SrcHeight);
295 | 				// nearest neighbor interpolation
296 | 				for (int c = 0; c < nChannels; c++){
297 | 					pDstImage[(i*DstWidth + j)*nChannels + c] = pSrcImage[(iy*SrcWidth + ix)*nChannels + c];
298 | 				}
299 | 			}
300 | 	}
301 | }
302 | 
303 | template <class T1,class T2>
304 | void ImageProcessing::ResizeImage(const T1 *pSrcImage, T2 *pDstImage, int SrcWidth, int SrcHeight, int nChannels, int DstWidth, int DstHeight, InterType type/* = INTER_LINEAR*/)
305 | {
306 | 	float xRatio=(float)DstWidth/SrcWidth;
307 | 	float yRatio=(float)DstHeight/SrcHeight;
308 | 	memset(pDstImage, 0, sizeof(T2)*DstWidth*DstHeight*nChannels);
309 | 
310 | 	float x,y;
311 | 
312 | 	if (type == INTER_LINEAR){
313 | 		for (int i = 0; i < DstHeight; i++)
314 | 			for (int j = 0; j < DstWidth; j++)
315 | 			{
316 | 				x = (float)(j + 1) / xRatio - 1;
317 | 				y = (float)(i + 1) / yRatio - 1;
318 | 
319 | 				// bilinear interpolation
320 | 				BilinearInterpolate(pSrcImage, SrcWidth, SrcHeight, nChannels, x, y, pDstImage + (i*DstWidth + j)*nChannels);
321 | 			}
322 | 	}else if (type == INTER_NN){
323 | 		int ix, iy;
324 | 		for (int i = 0; i < DstHeight; i++)
325 | 			for (int j = 0; j < DstWidth; j++)
326 | 			{
327 | 				x = (float)(j + 1) / xRatio - 1;
328 | 				y = (float)(i + 1) / yRatio - 1;
329 | 				ix = EnforceRange(x + 0.5, SrcWidth);
330 | 				iy = EnforceRange(y + 0.5, SrcHeight);
331 | 
332 | 				// nearest neighbor interpolation
333 | 				for (int c = 0; c < nChannels; c++){
334 | 					pDstImage[(i*DstWidth + j)*nChannels + c] = pSrcImage[(iy*SrcWidth + ix)*nChannels + c];
335 | 				}
336 | 			}
337 | 	}
338 | }
339 | 
340 | //------------------------------------------------------------------------------------------------------------
341 | //  horizontal direction filtering
342 | //------------------------------------------------------------------------------------------------------------
343 | template <class T1,class T2>
344 | void ImageProcessing::hfiltering(const T1* pSrcImage,T2* pDstImage,int width,int height,int nChannels,const float* pfilter1D,int fsize)
345 | {
346 | 	memset(pDstImage,0,sizeof(T2)*width*height*nChannels);
347 | 	T2* pBuffer;
348 | 	float w;
349 | 	int i,j,l,k,offset,jj;
350 | 	for(i=0;i<height;i++)
351 | 		for(j=0;j<width;j++)
352 | 		{
353 | 			offset=i*width*nChannels;
354 | 			pBuffer=pDstImage+offset+j*nChannels;
355 | 			for(l=-fsize;l<=fsize;l++)
356 | 			{
357 | 				w=pfilter1D[l+fsize];
358 | 				jj=EnforceRange(j+l,width);
359 | 				for(k=0;k<nChannels;k++)
360 | 					pBuffer[k]+=pSrcImage[offset+jj*nChannels+k]*w;
361 | 			}
362 | 		}
363 | }
364 | 
365 | //------------------------------------------------------------------------------------------------------------
366 | //  horizontal direction filtering transpose
367 | //------------------------------------------------------------------------------------------------------------
368 | template <class T1,class T2>
369 | void ImageProcessing::hfiltering_transpose(const T1* pSrcImage,T2* pDstImage,int width,int height,int nChannels,const float* pfilter1D,int fsize)
370 | {
371 | 	memset(pDstImage,0,sizeof(T2)*width*height*nChannels);
372 | 	const T1* pBuffer;
373 | 	float w;
374 | 	int i,j,l,k,offset,jj;
375 | 	for(i=0;i<height;i++)
376 | 		for(j=0;j<width;j++)
377 | 		{
378 | 			int offset0=i*width*nChannels;
379 | 			pBuffer=pSrcImage+(i*width+j)*nChannels;
380 | 			for(l=-fsize;l<=fsize;l++)
381 | 			{
382 | 				w=pfilter1D[l+fsize];
383 | 				jj=EnforceRange(j+l,width);
384 | 				offset = offset0 + jj*nChannels;
385 | 				for(k=0;k<nChannels;k++)
386 | 					pDstImage[offset+k] += pBuffer[k]*w;
387 | 			}
388 | 		}
389 | }
390 | //------------------------------------------------------------------------------------------------------------
391 | // fast filtering algorithm for laplacian
392 | //------------------------------------------------------------------------------------------------------------
393 | template <class T1,class T2>
394 | void ImageProcessing::Laplacian(const T1 *pSrcImage, T2 *pDstImage, int width, int height, int nChannels)
395 | {
396 | 	int LineWidth=width*nChannels;
397 | 	int nElements=width*height*nChannels;
398 | 	// first treat the corners
399 | 	for(int k=0;k<nChannels;k++)
400 | 	{
401 | 		pDstImage[k]=pSrcImage[k]*2-pSrcImage[nChannels+k]-pSrcImage[LineWidth+k];
402 | 		pDstImage[LineWidth-nChannels+k]=pSrcImage[LineWidth-nChannels+k]*2-pSrcImage[LineWidth-2*nChannels+k]-pSrcImage[2*LineWidth-nChannels+k];
403 | 		pDstImage[nElements-LineWidth+k]=pSrcImage[nElements-LineWidth+k]*2-pSrcImage[nElements-LineWidth+nChannels+k]-pSrcImage[nElements-2*LineWidth+k];
404 | 		pDstImage[nElements-nChannels+k]=pSrcImage[nElements-nChannels+k]*2-pSrcImage[nElements-2*nChannels+k]-pSrcImage[nElements-LineWidth-nChannels+k];
405 | 	}
406 | 	// then treat the borders
407 | 	for(int i=1;i<width-1;i++)
408 | 		for(int k=0;k<nChannels;k++)
409 | 		{
410 | 			pDstImage[i*nChannels+k]=pSrcImage[i*nChannels+k]*3-pSrcImage[(i-1)*nChannels+k]-pSrcImage[(i+1)*nChannels+k]-pSrcImage[i*nChannels+LineWidth+k];
411 | 			pDstImage[nElements-LineWidth+i*nChannels+k]=pSrcImage[nElements-LineWidth+i*nChannels+k]*3-pSrcImage[nElements-LineWidth+(i-1)*nChannels+k]-pSrcImage[nElements-LineWidth+(i+1)*nChannels+k]-pSrcImage[nElements-2*LineWidth+i*nChannels+k];
412 | 		}
413 | 	for(int i=1;i<height-1;i++)
414 | 		for(int k=0;k<nChannels;k++)
415 | 		{
416 | 			pDstImage[i*LineWidth+k]=pSrcImage[i*LineWidth+k]*3-pSrcImage[i*LineWidth+nChannels+k]-pSrcImage[(i-1)*LineWidth+k]-pSrcImage[(i+1)*LineWidth+k];
417 | 			pDstImage[(i+1)*LineWidth-nChannels+k]=pSrcImage[(i+1)*LineWidth-nChannels+k]*3-pSrcImage[(i+1)*LineWidth-2*nChannels+k]-pSrcImage[i*LineWidth-nChannels+k]-pSrcImage[(i+2)*LineWidth-nChannels+k];
418 | 		}
419 | 	// now the interior
420 | 	for(int i=1;i<height-1;i++)
421 | 		for(int j=1;j<width-1;j++)
422 | 		{
423 | 			int offset=(i*width+j)*nChannels;
424 | 			for(int k=0;k<nChannels;k++)
425 | 				pDstImage[offset+k]=pSrcImage[offset+k]*4-pSrcImage[offset+nChannels+k]-pSrcImage[offset-nChannels+k]-pSrcImage[offset-LineWidth+k]-pSrcImage[offset+LineWidth+k];
426 | 		}
427 | }
428 | 
429 | template <class T1, class T2>
430 | void ImageProcessing::Medianfiltering(const T1* pSrcImage, T2* pDstImage, int width, int height, int nChannels, int fsize)
431 | {
432 | 	T2* tmpImg = new T2[width*height*nChannels];
433 | 
434 | 	int regionSize = (2 * fsize + 1) * (2 * fsize + 1);
435 | 	T1* pTmpSrc = (T1*)malloc(regionSize * sizeof(T1));
436 | 	T2* pBuffer = NULL;
437 | 	float w;
438 | 	int i, j, l, k, c, offset, ii, jj;
439 | 	for (i = 0; i < height; i++){
440 | 		for (j = 0; j < width; j++){
441 | 			pBuffer = tmpImg + (i*width + j)*nChannels;
442 | 			for (c = 0; c < nChannels; c++){
443 | 				int idx = 0;
444 | 				for (l = -fsize; l <= fsize; l++){
445 | 					for (k = -fsize; k <= fsize; k++){
446 | 						ii = EnforceRange(i + l, height);
447 | 						jj = EnforceRange(j + k, width);
448 | 						pTmpSrc[idx++] = pSrcImage[(ii*width + jj)*nChannels + c];
449 | 					}
450 | 				}
451 | 				// debug
452 | 				// 				printf("\n");
453 | 				// 				for(int kk=0; kk<regionSize; kk++){
454 | 				// 					printf("%f ", pTmpSrc[kk]);
455 | 				// 				}
456 | 				sort(pTmpSrc, pTmpSrc + regionSize);
457 | 				// debug
458 | 				// 				printf("\n");
459 | 				// 				for(int kk=0; kk<regionSize; kk++){
460 | 				// 					printf("%f ", pTmpSrc[kk]);
461 | 				// 				}
462 | 				pBuffer[c] = pTmpSrc[regionSize / 2];
463 | 			}
464 | 		}
465 | 	}
466 | 	free(pTmpSrc);
467 | 
468 | 	memcpy(pDstImage, tmpImg, width*height*nChannels*sizeof(T2));
469 | 	delete[] tmpImg;
470 | }
471 | 
472 | //------------------------------------------------------------------------------------------------------------
473 | // vertical direction filtering
474 | //------------------------------------------------------------------------------------------------------------
475 | template <class T1,class T2>
476 | void ImageProcessing::vfiltering(const T1* pSrcImage,T2* pDstImage,int width,int height,int nChannels,const float* pfilter1D,int fsize)
477 | {
478 | 	memset(pDstImage,0,sizeof(T2)*width*height*nChannels);
479 | 	T2* pBuffer;
480 | 	float w;
481 | 	int i,j,l,k,offset,ii;
482 | 	for(i=0;i<height;i++)
483 | 		for(j=0;j<width;j++)
484 | 		{
485 | 			pBuffer=pDstImage+(i*width+j)*nChannels;
486 | 			for(l=-fsize;l<=fsize;l++)
487 | 			{
488 | 				w=pfilter1D[l+fsize];
489 | 				ii=EnforceRange(i+l,height);
490 | 				for(k=0;k<nChannels;k++)
491 | 					pBuffer[k]+=pSrcImage[(ii*width+j)*nChannels+k]*w;
492 | 			}
493 | 		}
494 | }
495 | 
496 | //------------------------------------------------------------------------------------------------------------
497 | // vertical direction filtering transpose
498 | //------------------------------------------------------------------------------------------------------------
499 | template <class T1,class T2>
500 | void ImageProcessing::vfiltering_transpose(const T1* pSrcImage,T2* pDstImage,int width,int height,int nChannels,const float* pfilter1D,int fsize)
501 | {
502 | 	memset(pDstImage,0,sizeof(T2)*width*height*nChannels);
503 | 	const T1* pBuffer;
504 | 	float w;
505 | 	int i,j,l,k,offset,ii;
506 | 	for(i=0;i<height;i++)
507 | 		for(j=0;j<width;j++)
508 | 		{
509 | 			pBuffer=pSrcImage+(i*width+j)*nChannels;
510 | 			for(l=-fsize;l<=fsize;l++)
511 | 			{
512 | 				w=pfilter1D[l+fsize];
513 | 				ii=EnforceRange(i+l,height);
514 | 				offset = (ii*width+j)*nChannels;
515 | 				for(k=0;k<nChannels;k++)
516 | 					//pBuffer[k]+=pSrcImage[(ii*width+j)*nChannels+k]*w;
517 | 					pDstImage[offset+k] += pBuffer[k]*w;
518 | 			}
519 | 		}
520 | }
521 | 
522 | 
523 | //------------------------------------------------------------------------------------------------------------
524 | // 2d filtering
525 | //------------------------------------------------------------------------------------------------------------
526 | template <class T1,class T2>
527 | void ImageProcessing::filtering(const T1* pSrcImage,T2* pDstImage,int width,int height,int nChannels,const float* pfilter2D,int fsize)
528 | {
529 | 	float w;
530 | 	int i,j,u,v,k,ii,jj,wsize,offset;
531 | 	wsize=fsize*2+1;
532 | 	float* pBuffer=new float[nChannels];
533 | 	for(i=0;i<height;i++)
534 | 		for(j=0;j<width;j++)
535 | 		{
536 | 			for(k=0;k<nChannels;k++)
537 | 				pBuffer[k]=0;
538 | 			for(u=-fsize;u<=fsize;u++)
539 | 				for(v=-fsize;v<=fsize;v++)
540 | 				{
541 | 					w=pfilter2D[(u+fsize)*wsize+v+fsize];
542 | 					ii=EnforceRange(i+u,height);
543 | 					jj=EnforceRange(j+v,width);
544 | 					offset=(ii*width+jj)*nChannels;
545 | 					for(k=0;k<nChannels;k++)
546 | 						pBuffer[k]+=pSrcImage[offset+k]*w;
547 | 				}
548 | 			offset=(i*width+j)*nChannels;
549 | 			for(k=0;k<nChannels;k++)
550 | 				pDstImage[offset+k]=pBuffer[k];
551 | 		}
552 | 	delete []pBuffer;
553 | }
554 | 
555 | //------------------------------------------------------------------------------------------------------------
556 | // 2d filtering transpose
557 | //------------------------------------------------------------------------------------------------------------
558 | template <class T1,class T2>
559 | void ImageProcessing::filtering_transpose(const T1* pSrcImage,T2* pDstImage,int width,int height,int nChannels,const float* pfilter2D,int fsize)
560 | {
561 | 	float w;
562 | 	int i,j,u,v,k,ii,jj,wsize,offset;
563 | 	wsize=fsize*2+1;
564 | 	memset(pDstImage,0,sizeof(T2)*width*height*nChannels);
565 | 	for(i=0;i<height;i++)
566 | 		for(j=0;j<width;j++)
567 | 		{
568 | 			int offset0 = (i*width+j)*nChannels;
569 | 			for(u=-fsize;u<=fsize;u++)
570 | 				for(v=-fsize;v<=fsize;v++)
571 | 				{
572 | 					w=pfilter2D[(u+fsize)*wsize+v+fsize];
573 | 					ii=EnforceRange(i+u,height);
574 | 					jj=EnforceRange(j+v,width);
575 | 					int offset=(ii*width+jj)*nChannels;
576 | 					for(k=0;k<nChannels;k++)
577 | 						pDstImage[offset+k]+=pSrcImage[offset0+k]*w;
578 | 				}
579 | 		}
580 | }
581 | 
582 | 
583 | template <class T1, class T2>
584 | void ImageProcessing::Integral(const T1* pSrcImage, T2* pDstImage, int width, int height, int nChannels)
585 | {
586 | #if 0
587 | 	for (int i = 0; i < 10; i++){
588 | 		for (int j = 0; j < 10; j++){
589 | 			printf("%.2f ", pSrcImage[(i*width + j)*nChannels]);
590 | 		}
591 | 		printf("\n");
592 | 	}
593 | #endif
594 | 
595 | 	int ti, tj;
596 | 	double sum;
597 | 	for (int k = 0; k < nChannels; k++)
598 | 	{
599 | 		for (int i = 0; i < height; i++){
600 | 			for (int j = 0; j < width; j++){
601 | 				sum = pSrcImage[(i*width + j)*nChannels + k];
602 | 
603 | 				ti = i - 1;
604 | 				tj = j - 1;
605 | 				if (tj >= 0){
606 | 					sum += pDstImage[(i*width + tj)*nChannels + k];
607 | 				}
608 | 				if (ti >= 0){
609 | 					sum += pDstImage[(ti*width + j)*nChannels + k];
610 | 				}
611 | 				if (ti >= 0 && tj >= 0){
612 | 					sum -= pDstImage[(ti*width + tj)*nChannels + k];
613 | 				}
614 | 
615 | 				pDstImage[(i*width + j)*nChannels + k] = sum;
616 | 			}
617 | 		}
618 | 	}
619 | 
620 | #if 0
621 | 	printf("\n");
622 | 	for (int i = 0; i < 10; i++){
623 | 		for (int j = 0; j < 10; j++){
624 | 			printf("%.2f ", pDstImage[(i*width + j)*nChannels]);
625 | 		}
626 | 		printf("\n");
627 | 	}
628 | #endif
629 | }
630 | 
631 | template <class T1, class T2>
632 | void ImageProcessing::BoxFilter(const T1* pSrcImage, T2* pDstImage, int width, int height, int nChannels, int r, bool norm)
633 | {
634 | 	double* pBuffer = new double[width*height*nChannels];
635 | 	Integral(pSrcImage, pBuffer, width, height, nChannels);
636 | 
637 | 	int ti, tj;
638 | 	double sum;
639 | 	int starti, startj, endi, endj;
640 | 	for (int k = 0; k < nChannels; k++)
641 | 	{
642 | 		for (int i = 0; i < height; i++){
643 | 			for (int j = 0; j < width; j++){
644 | 				ti = EnforceRange(i + r, height);
645 | 				tj = EnforceRange(j + r, width);
646 | 				sum = pBuffer[(ti*width + tj)*nChannels + k];
647 | 
648 | 				starti = 0;
649 | 				startj = 0;
650 | 				endi = ti;
651 | 				endj = tj;
652 | 
653 | 				ti = i - r - 1;
654 | 				tj = j - r - 1;
655 | 
656 | 				if (ti >= 0){
657 | 					sum -= pBuffer[(ti*width + endj)*nChannels + k];
658 | 					starti = ti + 1;
659 | 				}
660 | 				if (tj >= 0){
661 | 					sum -= pBuffer[(endi*width + tj)*nChannels + k];
662 | 					startj = tj + 1;
663 | 				}
664 | 				if (ti >= 0 && tj >= 0){
665 | 					sum += pBuffer[(ti*width + tj)*nChannels + k];
666 | 				}
667 | 
668 | 				int cnt = 1;
669 | 				if (norm){	// normalize
670 | 					cnt = (endi - starti + 1)*(endj - startj + 1);
671 | 				}
672 | 
673 | 				pDstImage[(i*width + j)*nChannels + k] = sum / cnt;
674 | 			}
675 | 		}
676 | 	}
677 | 	delete []pBuffer;
678 | }
679 | 
680 | //------------------------------------------------------------------------------------------------------------
681 | // function to sample a patch from the source image
682 | //------------------------------------------------------------------------------------------------------------
683 | template <class T1,class T2>
684 | void ImageProcessing::getPatch(const T1* pSrcImage,T2* pPatch,int width,int height,int nChannels,float x0,float y0,int wsize)
685 | {
686 | 	// suppose pPatch has been allocated and cleared before calling the function
687 | 	int wlength=wsize*2+1;
688 | 	float x,y;
689 | 	for(int i=-wsize;i<=wsize;i++)
690 | 		for(int j=-wsize;j<=wsize;j++)
691 | 		{
692 | 			y=y0+i;
693 | 			x=x0+j;
694 | 			if(x<0 || x>width-1 || y<0 || y>height-1)
695 | 				continue;
696 | 			BilinearInterpolate(pSrcImage,width,height,nChannels,x,y,pPatch+((i+wsize)*wlength+j+wsize)*nChannels);
697 | 		}
698 | }
699 | 
700 | //------------------------------------------------------------------------------------------------------------
701 | // function to warp an image with respect to flow field
702 | // pWarpIm2 has to be allocated before hands
703 | //------------------------------------------------------------------------------------------------------------
704 | template <class T1,class T2>
705 | void ImageProcessing::warpImage(T1 *pWarpIm2, const T1 *pIm1, const T1 *pIm2, const T2 *pVx, const T2 *pVy, int width, int height, int nChannels)
706 | {
707 | 	memset(pWarpIm2,0,sizeof(T1)*width*height*nChannels);
708 | 	for(int i=0;i<height;i++)
709 | 		for(int j=0;j<width;j++)
710 | 		{
711 | 			int offset=i*width+j;
712 | 			float x,y;
713 | 			y=i+pVy[offset];
714 | 			x=j+pVx[offset];
715 | 			offset*=nChannels;
716 | 			if(x<0 || x>width-1 || y<0 || y>height-1)
717 | 			{
718 | 				for(int k=0;k<nChannels;k++)
719 | 					pWarpIm2[offset+k]=pIm1[offset+k];
720 | 				continue;
721 | 			}
722 | 			BilinearInterpolate(pIm2,width,height,nChannels,x,y,pWarpIm2+offset);
723 | 		}
724 | }
725 | 
726 | template <class T1,class T2>
727 | void ImageProcessing::warpImageFlow(T1 *pWarpIm2, const T1 *pIm1, const T1 *pIm2, const T2 *pFlow, int width, int height, int nChannels)
728 | {
729 | 	memset(pWarpIm2,0,sizeof(T1)*width*height*nChannels);
730 | 	for(int i=0;i<height;i++)
731 | 		for(int j=0;j<width;j++)
732 | 		{
733 | 			int offset=i*width+j;
734 | 			float x,y;
735 | 			y=i+pFlow[offset*2+1];
736 | 			x=j+pFlow[offset*2];
737 | 			offset*=nChannels;
738 | 			if(x<0 || x>width-1 || y<0 || y>height-1)
739 | 			{
740 | 				for(int k=0;k<nChannels;k++)
741 | 					pWarpIm2[offset+k]=pIm1[offset+k];
742 | 				continue;
743 | 			}
744 | 			BilinearInterpolate(pIm2,width,height,nChannels,x,y,pWarpIm2+offset);
745 | 		}
746 | }
747 | 
748 | template <class T1,class T2>
749 | void ImageProcessing::warpImage(T1 *pWarpIm2,const T1 *pIm2, const T2 *pVx, const T2 *pVy, int width, int height, int nChannels)
750 | {
751 | 	memset(pWarpIm2,0,sizeof(T1)*width*height*nChannels);
752 | 	for(int i=0;i<height;i++)
753 | 		for(int j=0;j<width;j++)
754 | 		{
755 | 			int offset=i*width+j;
756 | 			float x,y;
757 | 			y=i+pVy[offset];
758 | 			x=j+pVx[offset];
759 | 			offset*=nChannels;
760 | 			if(x<0 || x>width-1 || y<0 || y>height-1)
761 | 				continue;
762 | 			BilinearInterpolate(pIm2,width,height,nChannels,x,y,pWarpIm2+offset);
763 | 		}
764 | }
765 | 
766 | template <class T1,class T2>
767 | void ImageProcessing::warpImage_transpose(T1 *pWarpIm2,const T1 *pIm2, const T2 *pVx, const T2 *pVy, int width, int height, int nChannels)
768 | {
769 | 	memset(pWarpIm2,0,sizeof(T1)*width*height*nChannels);
770 | 	for(int i=0;i<height;i++)
771 | 		for(int j=0;j<width;j++)
772 | 		{
773 | 			int offset=i*width+j;
774 | 			float x,y;
775 | 			y=i+pVy[offset];
776 | 			x=j+pVx[offset];
777 | 			offset*=nChannels;
778 | 			if(x<0 || x>width-1 || y<0 || y>height-1)
779 | 				continue;
780 | 			//BilinearInterpolate(pIm2,width,height,nChannels,x,y,pWarpIm2+offset);
781 | 			BilinearInterpolate_transpose(pIm2+offset,width,height,nChannels,x,y,pWarpIm2);
782 | 		}
783 | }
784 | 
785 | //////////////////////////////////////////////////////////////////////////////////////
786 | // different format
787 | //////////////////////////////////////////////////////////////////////////////////////
788 | template <class T1,class T2>
789 | void ImageProcessing::warpImage(T1 *pWarpIm2,const T1 *pIm2, const T2 *flow, int width, int height, int nChannels)
790 | {
791 | 	memset(pWarpIm2,0,sizeof(T1)*width*height*nChannels);
792 | 	for(int i=0;i<height;i++)
793 | 		for(int j=0;j<width;j++)
794 | 		{
795 | 			int offset=i*width+j;
796 | 			float x,y;
797 | 			y=i+flow[offset*2+1];
798 | 			x=j+flow[offset*2];
799 | 			offset*=nChannels;
800 | 			if(x<0 || x>width-1 || y<0 || y>height-1)
801 | 				continue;
802 | 			BilinearInterpolate(pIm2,width,height,nChannels,x,y,pWarpIm2+offset);
803 | 		}
804 | }
805 | 
806 | template <class T1,class T2>
807 | void ImageProcessing::warpImage_transpose(T1 *pWarpIm2,const T1 *pIm2, const T2 *flow, int width, int height, int nChannels)
808 | {
809 | 	memset(pWarpIm2,0,sizeof(T1)*width*height*nChannels);
810 | 	for(int i=0;i<height;i++)
811 | 		for(int j=0;j<width;j++)
812 | 		{
813 | 			int offset=i*width+j;
814 | 			float x,y;
815 | 			y=i+flow[offset*2+1];
816 | 			x=j+flow[offset*2];
817 | 			offset*=nChannels;
818 | 			if(x<0 || x>width-1 || y<0 || y>height-1)
819 | 				continue;
820 | 			//BilinearInterpolate(pIm2,width,height,nChannels,x,y,pWarpIm2+offset);
821 | 			BilinearInterpolate_transpose(pIm2+offset,width,height,nChannels,x,y,pWarpIm2);
822 | 		}
823 | }
824 | 
825 | 
826 | template <class T1,class T2,class T3>
827 | void ImageProcessing::warpImage(T1 *pWarpIm2, T3* pMask,const T1 *pIm1, const T1 *pIm2, const T2 *pVx, const T2 *pVy, int width, int height, int nChannels)
828 | {
829 | 	memset(pWarpIm2,0,sizeof(T1)*width*height*nChannels);
830 | 	for(int i=0;i<height;i++)
831 | 		for(int j=0;j<width;j++)
832 | 		{
833 | 			int offset=i*width+j;
834 | 			float x,y;
835 | 			y=i+pVy[offset];
836 | 			x=j+pVx[offset];
837 | 			offset*=nChannels;
838 | 			if(x<0 || x>width-1 || y<0 || y>height-1)
839 | 			{
840 | 				for(int k=0;k<nChannels;k++)
841 | 					pWarpIm2[offset+k]=pIm1[offset+k];
842 | 				pMask[i*width+j]=0;
843 | 				continue;
844 | 			}
845 | 			pMask[i*width+j]=1;
846 | 			BilinearInterpolate(pIm2,width,height,nChannels,x,y,pWarpIm2+offset);
847 | 		}
848 | }
849 | 
850 | //------------------------------------------------------------------------------------------------------------
851 | // function to crop an image from the source
852 | // assume that pDstImage has been allocated
853 | // also Left and Top must be valid, DstWidth and DstHeight should ensure that the image lies
854 | // inside the image boundary
855 | //------------------------------------------------------------------------------------------------------------
856 | template <class T1,class T2>
857 | void ImageProcessing::cropImage(const T1 *pSrcImage, int SrcWidth, int SrcHeight, int nChannels, T2 *pDstImage, int Left, int Top, int DstWidth, int DstHeight)
858 | {
859 | 	if(typeid(T1)==typeid(T2))
860 | 	{
861 | 		for(int i=0;i<DstHeight;i++)
862 | 			memcpy(pDstImage+i*DstWidth*nChannels,pSrcImage+((i+Top)*SrcWidth+Left)*nChannels,sizeof(T1)*DstWidth*nChannels);
863 | 		return;
864 | 	}
865 | 	int offsetSrc,offsetDst;
866 | 	for(int i=0;i<DstHeight;i++)
867 | 		for(int j=0;j<DstWidth;j++)
868 | 		{
869 | 			offsetSrc=((i+Top)*SrcWidth+Left+j)*nChannels;
870 | 			offsetDst=(i*DstWidth+j)*nChannels;
871 | 			for(int k=0;k<nChannels;k++)
872 | 				pDstImage[offsetDst+k]=pSrcImage[offsetSrc+k];
873 | 		}
874 | }
875 | 
876 | //------------------------------------------------------------------------------------------------------------
877 | // function to generate a 2D Gaussian image
878 | // pImage must be allocated before calling the function
879 | //------------------------------------------------------------------------------------------------------------
880 | template <class T>
881 | void ImageProcessing::generate2DGaussian(T*& pImage, int wsize, float sigma)
882 | {
883 | 	if(sigma==-1)
884 | 		sigma=wsize/2;
885 | 	float alpha=1/(2*sigma*sigma);
886 | 	int winlength=wsize*2+1;
887 | 	if(pImage==NULL)
888 | 		pImage=new T[winlength*winlength];
889 | 	float total = 0;
890 | 	for(int i=-wsize;i<=wsize;i++)
891 | 		for(int j=-wsize;j<=wsize;j++)
892 | 		{
893 | 			pImage[(i+wsize)*winlength+j+wsize]=exp(-(float)(i*i+j*j)*alpha);
894 | 			total += pImage[(i+wsize)*winlength+j+wsize];
895 | 		}
896 | 	for(int i = 0;i<winlength*winlength;i++)
897 | 		pImage[i]/=total;
898 | }
899 | 
900 | //------------------------------------------------------------------------------------------------------------
901 | // function to generate a 1D Gaussian image
902 | // pImage must be allocated before calling the function
903 | //------------------------------------------------------------------------------------------------------------
904 | template <class T>
905 | void ImageProcessing::generate1DGaussian(T*& pImage, int wsize, float sigma)
906 | {
907 | 	if(sigma==-1)
908 | 		sigma=wsize/2;
909 | 	float alpha=1/(2*sigma*sigma);
910 | 	int winlength=wsize*2+1;
911 | 	if(pImage==NULL)
912 | 		pImage=new T[winlength];
913 | 	float total = 0;
914 | 	for(int i=-wsize;i<=wsize;i++)
915 | 	{
916 | 		pImage[i+wsize]=exp(-(float)(i*i)*alpha);
917 | 		total += pImage[i+wsize];
918 | 	}
919 | 	for(int i = 0;i<winlength;i++)
920 | 		pImage[i]/=total;
921 | }
922 | 
923 | #endif
924 | 


--------------------------------------------------------------------------------
/include/CPM_include/ImagePyramid.h:
--------------------------------------------------------------------------------
  1 | #ifndef _GaussianPyramid_h
  2 | #define _GaussianPyramid_h
  3 | 
  4 | #include "Image.h"
  5 | 
  6 | template <class T>
  7 | class ImagePyramid
  8 | {
  9 | private:
 10 | 	Image<T>* ImPyramid;
 11 | 	int nLevels;
 12 | 	float fRatio;
 13 | public:
 14 | 	ImagePyramid(void){ ImPyramid = NULL; };
 15 | 	~ImagePyramid(void){if(ImPyramid != NULL) delete[]ImPyramid;};
 16 | 	inline Image<T>& operator[](int level) { return ImPyramid[level]; };
 17 | 	void ConstructPyramid(const FImage& image, float ratio = 0.8, int minWidth = 30);
 18 | 	void ConstructPyramidLevels(const FImage& image, float ratio = 0.8, int _nLevels = 2);
 19 | 	void displayTop(const char* filename){ ImPyramid[nLevels - 1].imwrite(filename); };
 20 | 	inline int nlevels() const {return nLevels;};
 21 | 	inline float ratio() const { return fRatio; };
 22 | };
 23 | 
 24 | typedef ImagePyramid<float> FImagePyramid;
 25 | 
 26 | //---------------------------------------------------------------------------------------
 27 | // function to construct the pyramid
 28 | // this is the slow way
 29 | //---------------------------------------------------------------------------------------
 30 | /*void GaussianPyramid::ConstructPyramid(const DImage &image, float ratio, int minWidth)
 31 | {
 32 | // the ratio cannot be arbitrary numbers
 33 | if(ratio>0.98 || ratio<0.4)
 34 | ratio=0.75;
 35 | // first decide how many levels
 36 | nLevels=log((float)minWidth/image.width())/log(ratio);
 37 | if(ImPyramid!=NULL)
 38 | delete []ImPyramid;
 39 | ImPyramid=new DImage[nLevels];
 40 | ImPyramid[0].copyData(image);
 41 | float baseSigma=(1/ratio-1);
 42 | for(int i=1;i<nLevels;i++)
 43 | {
 44 | DImage foo;
 45 | float sigma=baseSigma*i;
 46 | image.GaussianSmoothing(foo,sigma,sigma*2.5);
 47 | foo.imresize(ImPyramid[i],pow(ratio,i));
 48 | }
 49 | }//*/
 50 | 
 51 | //---------------------------------------------------------------------------------------
 52 | // function to construct the pyramid
 53 | // this is the fast way
 54 | //---------------------------------------------------------------------------------------
 55 | template <class T>
 56 | void ImagePyramid<T>::ConstructPyramid(const FImage& image, float ratio /*= 0.8*/, int minWidth /*= 30*/)
 57 | {
 58 | 	// the ratio cannot be arbitrary numbers
 59 | 	if (ratio>0.98 || ratio<0.4)
 60 | 		ratio = 0.75;
 61 | 	// first decide how many levels
 62 | 	nLevels = log((float)minWidth / image.width()) / log(ratio);
 63 | 	fRatio = ratio;
 64 | 	if (ImPyramid != NULL)
 65 | 		delete[]ImPyramid;
 66 | 	ImPyramid = new FImage[nLevels];
 67 | 	ImPyramid[0].copyData(image);
 68 | 	float baseSigma = (1 / ratio - 1);
 69 | 	int n = log(0.25) / log(ratio);
 70 | 	float nSigma = baseSigma*n;
 71 | 	for (int i = 1; i<nLevels; i++)
 72 | 	{
 73 | 		FImage foo;
 74 | 		if (i <= n)
 75 | 		{
 76 | 			float sigma = baseSigma*i;
 77 | 			image.GaussianSmoothing(foo, sigma, sigma * 3);
 78 | 			foo.imresize(ImPyramid[i], pow(ratio, i));
 79 | 		}
 80 | 		else
 81 | 		{
 82 | 			ImPyramid[i - n].GaussianSmoothing(foo, nSigma, nSigma * 3);
 83 | 			float rate = (float)pow(ratio, i)*image.width() / foo.width();
 84 | 			foo.imresize(ImPyramid[i], rate);
 85 | 		}
 86 | 	}
 87 | }
 88 | 
 89 | template <class T>
 90 | void ImagePyramid<T>::ConstructPyramidLevels(const FImage& image, float ratio /*= 0.8*/, int _nLevels /*= 2*/)
 91 | {
 92 | 	// the ratio cannot be arbitrary numbers
 93 | 	if (ratio>0.98 || ratio<0.4)
 94 | 		ratio = 0.75;
 95 | 	nLevels = _nLevels;
 96 | 	fRatio = ratio;
 97 | 	if (ImPyramid != NULL)
 98 | 		delete[]ImPyramid;
 99 | 	ImPyramid = new FImage[nLevels];
100 | 	ImPyramid[0].copyData(image);
101 | 	float baseSigma = (1 / ratio - 1);
102 | 	int n = log(0.25) / log(ratio);
103 | 	float nSigma = baseSigma*n;
104 | 	for (int i = 1; i<nLevels; i++)
105 | 	{
106 | 		FImage foo;
107 | 		if (i <= n)
108 | 		{
109 | 			float sigma = baseSigma*i;
110 | 			image.GaussianSmoothing(foo, sigma, sigma * 3);
111 | 			foo.imresize(ImPyramid[i], pow(ratio, i));
112 | 		}
113 | 		else
114 | 		{
115 | 			ImPyramid[i - n].GaussianSmoothing(foo, nSigma, nSigma * 3);
116 | 			float rate = (float)pow(ratio, i)*image.width() / foo.width();
117 | 			foo.imresize(ImPyramid[i], rate);
118 | 		}
119 | 	}
120 | }
121 | 
122 | #endif


--------------------------------------------------------------------------------
/include/CPM_include/OpticFlowIO.h:
--------------------------------------------------------------------------------
  1 | // flowIO.h
  2 | 
  3 | #ifndef _OpticFlowIO_H
  4 | #define _OpticFlowIO_H
  5 | 
  6 | #include <math.h>
  7 | #include <memory.h>
  8 | #include <string.h>
  9 | 
 10 | #include <stdint.h>
 11 | #include "opencv2/opencv.hpp" // for KITTI
 12 | 
 13 | // read and write our simple .flo flow file format
 14 | 
 15 | // ".flo" file format used for optical flow evaluation
 16 | //
 17 | // Stores 2-band float image for horizontal (u) and vertical (v) flow components.
 18 | // Floats are stored in little-endian order.
 19 | // A flow value is considered "unknown" if either |u| or |v| is greater than 1e9.
 20 | //
 21 | //  bytes  contents
 22 | //
 23 | //  0-3     tag: "PIEH" in ASCII, which in little endian happens to be the float 202021.25
 24 | //          (just a sanity check that floats are represented correctly)
 25 | //  4-7     width as an integer
 26 | //  8-11    height as an integer
 27 | //  12-end  data (width*height*2*4 bytes total)
 28 | //          the float values for u and v, interleaved, in row order, i.e.,
 29 | //          u[row0,col0], v[row0,col0], u[row0,col1], v[row0,col1], ...
 30 | //
 31 | 
 32 | // value to use to represent unknown flow
 33 | #define UNKNOWN_FLOW 1e10
 34 | 
 35 | typedef struct 
 36 | {
 37 | 	double aee; // Average Endpoint Error
 38 | 	double aae; // Average Angular Error
 39 | }FlowErr;
 40 | 
 41 | class OpticFlowIO
 42 | {
 43 | public:
 44 | 	// return whether flow vector is unknown
 45 | 	template <class T>
 46 | 	static bool unknown_flow(T u, T v);
 47 | 	template <class T>
 48 | 	static bool unknown_flow(T *f);
 49 | 
 50 | 	// read a flow file into 2-band image
 51 | 	template <class T>
 52 | 	static int ReadFlowFile(T* U, T* V, int* w, int* h, const char* filename);
 53 | 
 54 | 	// write a 2-band image into flow file 
 55 | 	template <class T>
 56 | 	static int WriteFlowFile(T* U, T* V, int w, int h, const char* filename);
 57 | 
 58 | 	// read a KITTI flow file into 2-band image
 59 | 	template <class T>
 60 | 	static int ReadKittiFlowFile(T* U, T* V, int* w, int* h, const char* filename);
 61 | 
 62 | 	// write a 2-band image into KITTI flow file 
 63 | 	template <class T>
 64 | 	static int WriteKittiFlowFile(T* U, T* V, int w, int h, const char* filename);
 65 | 
 66 | 	// render the motion to a 4-band BGRA color image
 67 | 	template <class T>
 68 | 	static double MotionToColor(unsigned char* fillPix, T* U, T* V, int w, int h, float range = -1);
 69 | 
 70 | 	template <class T>
 71 | 	static float ShowFlow(const char* winname, T* U, T* V, int w, int h, float range = -1, int waittime = 1);
 72 | 	template <class T>
 73 | 	static void SaveFlowAsImage(const char* imgName, T* U, T* V, int w, int h, float range = -1);
 74 | 	static void SaveFlowMatrixAsImage(const char* imgName, cv::Mat2f flowMat);
 75 | 
 76 | 	static void Match2Flow(FImage& inMat, FImage& ou, FImage& ov, int w, int h);
 77 | 	static void FlowMatrix2Flow(cv::Mat2f inMat, FImage& ou, FImage& ov);
 78 | 
 79 | 	template <class T>
 80 | 	static float ErrorImage(unsigned char* fillPix, T* u1, T* v1, T* u2, T* v2, int w, int h);
 81 | 	template <class T>
 82 | 	static float ErrorImage(unsigned char* fillPix, T* u1, T* v1, char* gtName, int w, int h);
 83 | 	template <class T>
 84 | 	static float ShowErrorImage(const char* winname, T* U, T* V, char* gtName, int w, int h, int waittime = 1);
 85 | 	template <class T>
 86 | 	static float SaveErrorImage(const char* imgName, T* U, T* V, char* gtName, int w, int h);
 87 | 
 88 | 	template <class T1, class T2>
 89 | 	static FlowErr CalcFlowError(T1* u1, T1* v1, T2* u2, T2*v2, int w, int h);
 90 | 
 91 | private:
 92 | 	// first four bytes, should be the same in little endian
 93 | 	#define TAG_FLOAT 202021.25  // check for this when READING the file
 94 | 	#define TAG_STRING "PIEH"    // use this when WRITING the file
 95 | 
 96 | 	#define M_PI       3.14159265358979323846
 97 | 
 98 | 	// the "official" threshold - if the absolute value of either 
 99 | 	// flow component is greater, it's considered unknown
100 | 	#define UNKNOWN_FLOW_THRESH 1e9
101 | 
102 | 	#define NUM_BANDS 2
103 | 
104 | 	// Color encoding of flow vectors
105 | 	// adapted from the color circle idea described at
106 | 	//   http://members.shaw.ca/quadibloc/other/colint.htm
107 | 	//
108 | 	// Daniel Scharstein, 4/2007
109 | 	// added tick marks and out-of-range coding 6/05/07
110 | 
111 | 	#define MAXWHEELCOLS 60
112 | 	template <class T>
113 | 	static void setcols(T* colorwheel, int r, int g, int b, int k);
114 | 	template <class T>
115 | 	static int makecolorwheel(T* colorwheel);
116 | 	template <class T>
117 | 	static void computeColor(double fx, double fy, unsigned char *pix, T* colorwheel, int ncols);
118 | };
119 | 
120 | template <class T>
121 | int OpticFlowIO::ReadKittiFlowFile(T* U, T* V, int* w, int* h, const char* filename)
122 | {
123 | 	if (filename == NULL){
124 | 		printf("ReadKittiFlowFile: empty filename\n");
125 | 		return -1;
126 | 	}
127 | 
128 | 	const char *dot = strrchr(filename, '.');
129 | 	if (strcmp(dot, ".png") != 0){
130 | 		printf("ReadKittiFlowFile (%s): extension .png expected\n", filename);
131 | 		return -1;
132 | 	}
133 | 
134 | 	IplImage* img = cvLoadImage(filename, CV_LOAD_IMAGE_COLOR | CV_LOAD_IMAGE_ANYDEPTH);
135 | 	if (img == NULL){
136 | 		printf("ReadKittiFlowFile: could not open %s\n", filename);
137 | 		return -1;
138 | 	}
139 | 
140 | 	int width = img->width;
141 | 	int height = img->height;
142 | 	for(int i=0; i<height; i++){
143 | 		for(int j=0; j<width; j++){
144 | 			uint16_t* rowImgData = (uint16_t*)(img->imageData + i*img->widthStep);
145 | 			uint16_t validFlag = rowImgData[j*img->nChannels];
146 | 			if(validFlag > 0){
147 | 				U[i*width+j] = (rowImgData[j*img->nChannels + 2] - 32768.0f)/64.0f;
148 | 				V[i*width+j] = (rowImgData[j*img->nChannels + 1] - 32768.0f)/64.0f;
149 | 			}else{
150 | 				U[i*width+j] = UNKNOWN_FLOW;
151 | 				V[i*width+j] = UNKNOWN_FLOW;
152 | 			}
153 | 		}
154 | 	}
155 | 
156 | 	*w = width;
157 | 	*h = height;
158 | 	cvReleaseImage(&img);
159 | 	return 0;
160 | }
161 | 
162 | template <class T>
163 | int OpticFlowIO::WriteKittiFlowFile(T* U, T* V, int w, int h, const char* filename)
164 | {
165 | 	if (filename == NULL){
166 | 		printf("WriteKittiFlowFile: empty filename\n");
167 | 		return -1;
168 | 	}
169 | 
170 | 	const char *dot = strrchr(filename, '.');
171 | 	if (dot == NULL){
172 | 		printf("WriteKittiFlowFile: extension required in filename '%s'\n", filename);
173 | 		return -1;
174 | 	}
175 | 
176 | 	if (strcmp(dot, ".png") != 0){
177 | 		printf("WriteKittiFlowFile: filename '%s' should have extension '.png'\n", filename);
178 | 		return -1;
179 | 	}
180 | 
181 | 	int width = w, height = h;
182 | 
183 | 	IplImage* img = cvCreateImage(cvSize(w,h), IPL_DEPTH_16U, 3);
184 | 	for(int i=0; i<height; i++){
185 | 		for(int j=0; j<width; j++){
186 | 			double u,v;
187 | 			u = U[i*width+j];
188 | 			v = V[i*width+j];
189 | 			uint16_t* rowImgData = (uint16_t*)(img->imageData + i*img->widthStep);
190 | 			if(!unknown_flow(u,v)){
191 | 				rowImgData[j*img->nChannels + 2] = __max(__min(U[i*width+j]*64.0f+32768.0f, 65535), 0);
192 | 				rowImgData[j*img->nChannels + 1] = __max(__min(V[i*width+j]*64.0f+32768.0f, 65535), 0);
193 | 				rowImgData[j*img->nChannels] = 1;
194 | 			}else{
195 | 				rowImgData[j*img->nChannels + 2] = 0;
196 | 				rowImgData[j*img->nChannels + 1] = 0;
197 | 				rowImgData[j*img->nChannels] = 0;
198 | 			}
199 | 		}
200 | 	}
201 | 
202 | 	const int params[2]={CV_IMWRITE_PNG_COMPRESSION, 1};
203 | 	cvSaveImage(filename, img, params); // slight lossy PNG
204 | 	cvReleaseImage(&img);
205 | 	return 0;
206 | }
207 | 
208 | template <class T>
209 | bool OpticFlowIO::unknown_flow(T u, T v)
210 | {
211 | 	return (abs(u) > UNKNOWN_FLOW_THRESH) 
212 | 		|| (abs(v) > UNKNOWN_FLOW_THRESH)
213 | 		|| u != u || v != v;	// isnan()
214 | }
215 | 
216 | template <class T>
217 | bool OpticFlowIO::unknown_flow(T *f)
218 | {
219 | 	return unknown_flow(f[0], f[1]);
220 | }
221 | 
222 | template <class T>
223 | int OpticFlowIO::ReadFlowFile(T* U, T* V, int* w, int* h, const char* filename)
224 | {
225 | 	if (filename == NULL){
226 | 		printf("ReadFlowFile: empty filename\n");
227 | 		return -1;
228 | 	}
229 | 
230 | 	const char *dot = strrchr(filename, '.');
231 | 	if (strcmp(dot, ".flo") != 0){
232 | 		printf("ReadFlowFile (%s): extension .flo expected\n", filename);
233 | 		return -1;
234 | 	}
235 | 
236 | 	FILE *stream = fopen(filename, "rb");
237 | 	if (stream == 0){
238 | 		printf("ReadFlowFile: could not open %s\n", filename);
239 | 		return -1;
240 | 	}
241 | 
242 | 	int width, height;
243 | 	float tag;
244 | 
245 | 	if ((int)fread(&tag,    sizeof(float), 1, stream) != 1 
246 | 		||(int)fread(&width,  sizeof(int),   1, stream) != 1 
247 | 		||(int)fread(&height, sizeof(int),   1, stream) != 1)
248 | 	{
249 | 		printf("ReadFlowFile: problem reading file %s\n", filename);
250 | 		return -1;
251 | 	}
252 | 
253 | 	if (tag != TAG_FLOAT) // simple test for correct endian-ness
254 | 	{
255 | 		printf("ReadFlowFile(%s): wrong tag (possibly due to big-endian machine?)\n", filename);
256 | 		return -1;
257 | 	}
258 | 
259 | 	// another sanity check to see that integers were read correctly (99999 should do the trick...)
260 | 	if (width < 1 || width > 99999){
261 | 		printf("ReadFlowFile(%s): illegal width %d\n", filename, width);
262 | 		return -1;
263 | 	}
264 | 
265 | 	if (height < 1 || height > 99999){
266 | 		printf("ReadFlowFile(%s): illegal height %d\n", filename, height);
267 | 		return -1;
268 | 	}
269 | 
270 | 	for(int i=0; i<height; i++){
271 | 		for(int j=0; j<width; j++){
272 | 			float tmp[NUM_BANDS];
273 | 			if ((int)fread(tmp, sizeof(float), NUM_BANDS, stream) != NUM_BANDS){
274 | 				printf("ReadFlowFile(%s): file is too short\n", filename);
275 | 				return -1;
276 | 			}
277 | 			U[i*width+j] = tmp[0];
278 | 			V[i*width+j] = tmp[1];
279 | 		}
280 | 	}
281 | 
282 | 	if (fgetc(stream) != EOF){
283 | 		printf("ReadFlowFile(%s): file is too long\n", filename);
284 | 		return -1;
285 | 	}
286 | 
287 | 	*w = width;
288 | 	*h = height;
289 | 
290 | 	fclose(stream);
291 | 	return 0;
292 | }
293 | 
294 | template <class T>
295 | int OpticFlowIO::WriteFlowFile(T* U, T* V, int w, int h, const char* filename)
296 | {
297 | 	if (filename == NULL){
298 | 		printf("WriteFlowFile: empty filename\n");
299 | 		return -1;
300 | 	}
301 | 
302 | 	const char *dot = strrchr(filename, '.');
303 | 	if (dot == NULL){
304 | 		printf("WriteFlowFile: extension required in filename '%s'\n", filename);
305 | 		return -1;
306 | 	}
307 | 
308 | 	if (strcmp(dot, ".flo") != 0){
309 | 		printf("WriteFlowFile: filename '%s' should have extension '.flo'\n", filename);
310 | 		return -1;
311 | 	}
312 | 
313 | 	int width = w, height = h;
314 | 
315 | 	FILE *stream = fopen(filename, "wb");
316 | 	if (stream == 0){
317 | 		printf("WriteFlowFile: could not open %s\n", filename);
318 | 		return -1;
319 | 	}
320 | 
321 | 	// write the header
322 | 	fprintf(stream, TAG_STRING);
323 | 	if ((int)fwrite(&width,  sizeof(int),   1, stream) != 1 
324 | 		||(int)fwrite(&height, sizeof(int),   1, stream) != 1)
325 | 	{
326 | 		printf("WriteFlowFile(%s): problem writing header\n", filename);
327 | 		return -1;
328 | 	}
329 | 
330 | 	// write the rows
331 | 	for(int i=0; i<height; i++){
332 | 		for(int j=0; j<width; j++){
333 | 			float tmp[NUM_BANDS];
334 | 			tmp[0] = U[i*width+j];
335 | 			tmp[1] = V[i*width+j];
336 | 			if ((int)fwrite(tmp, sizeof(float), NUM_BANDS, stream) != NUM_BANDS){
337 | 				printf("WriteFlowFile(%s): problem writing data\n", filename);
338 | 				return -1;
339 | 			}
340 | 		}
341 | 	}
342 | 
343 | 	fclose(stream);
344 | 	return 0;
345 | }
346 | 
347 | template <class T>
348 | double OpticFlowIO::MotionToColor(unsigned char* fillPix, T* U, T* V, int w, int h, float range /*= -1*/)
349 | {
350 | 	// determine motion range:
351 | 	double maxrad;
352 | 
353 | 	if (range > 0) {
354 | 		maxrad = range;
355 | 	}else{	// obtain the motion range according to the max flow
356 | 		double maxu = -999, maxv = -999;
357 | 		double minu = 999, minv = 999;
358 | 		maxrad = -1;
359 | 		for (int i = 0; i < h; i++){
360 | 			for (int j = 0; j < w; j++){
361 | 				double u = U[i*w + j];
362 | 				double v = V[i*w + j];
363 | 				if (unknown_flow(u, v))
364 | 					continue;
365 | 				maxu = __max(maxu, u);
366 | 				maxv = __max(maxv, v);
367 | 				minu = __min(minu, u);
368 | 				minv = __min(minv, v);
369 | 				double rad = sqrt(u * u + v * v);
370 | 				maxrad = __max(maxrad, rad);
371 | 			}
372 | 		}
373 | 		if (maxrad == 0) // if flow == 0 everywhere
374 | 			maxrad = 1;
375 | 	}
376 | 
377 | 	//printf("max motion: %.2f  motion range: u = [%.2f,%.2f];  v = [%.2f,%.2f]\n",
378 | 	//	maxrad, minu, maxu, minv, maxv);
379 | 
380 | 	int colorwheel[MAXWHEELCOLS*3];
381 | 	int ncols = makecolorwheel(colorwheel);
382 | 
383 | 	for(int i=0; i<h; i++){
384 | 		for(int j=0; j<w; j++){
385 | 			int idx = i*w+j;
386 | 			double u = U[idx];
387 | 			double v = V[idx];
388 | 			if (unknown_flow(u, v)){
389 | 				memset(fillPix+idx*4, 0, 4);
390 | 				fillPix[idx*4 + 3] = 0xff; // alpha channel, only for alignment
391 | 			}else{
392 | 				double dx = __min(__max(u / maxrad, -1), 1);
393 | 				double dy = __min(__max(v / maxrad, -1), 1);
394 | 				computeColor(dx, dy, (unsigned char*)(fillPix + idx * 4), colorwheel, ncols);
395 | 			}
396 | 		}
397 | 	}
398 | 
399 | 	return maxrad;
400 | }
401 | 
402 | template <class T>
403 | float OpticFlowIO::ShowFlow(const char* winname, T* U, T* V, int w, int h, 
404 | 	float range /*= -1*/, int waittime /*= 1*/)
405 | {
406 | 	cv::Mat img(h, w, CV_8UC4);
407 | 	float maxFlow = OpticFlowIO::MotionToColor(img.data, U, V, w, h, range);
408 | 
409 | #if 0
410 | 	// get corner color
411 | 	int x = 10, y = 20;
412 | 	unsigned char color[4];
413 | 	unsigned char* pSrc = img.data + y*img.step + x * 4;
414 | 	color[0] = 255 - pSrc[0];
415 | 	color[1] = 255 - pSrc[1];
416 | 	color[2] = 255 - pSrc[2];
417 | 	char info[256];
418 | 	sprintf(info, "max: %.1f", maxFlow);
419 | 	cv::putText(img, info, cvPoint(x, y), CV_FONT_HERSHEY_SIMPLEX, 0.5, cvScalar(color[0], color[1], color[2]));
420 | #endif
421 | 
422 | 	cv::imshow(winname, img);
423 | 	cv::waitKey(waittime);
424 | 
425 | 	return maxFlow;
426 | }
427 | 
428 | template <class T>
429 | void OpticFlowIO::SaveFlowAsImage(const char* imgName, T* U, T* V, int w, int h, float range /*= -1*/)
430 | {
431 | 	cv::Mat img(h, w, CV_8UC4);
432 | 	float maxFlow = OpticFlowIO::MotionToColor(img.data, U, V, w, h, range);
433 | 
434 | #if 1
435 | 	// get corner color
436 | 	int x = 10, y = 20;
437 | 	unsigned char color[3];
438 | 	unsigned char* pSrc = img.data + y*img.step + x * 4;
439 | 	color[0] = 255 - pSrc[0];
440 | 	color[1] = 255 - pSrc[1];
441 | 	color[2] = 255 - pSrc[2];
442 | 	char info[256];
443 | 	sprintf(info, "max: %.1f", maxFlow);
444 | 	cv::putText(img, info, cvPoint(x, y), CV_FONT_HERSHEY_SIMPLEX, 0.5, cvScalar(color[0], color[1], color[2]));
445 | #endif
446 | 
447 | 	cv::imwrite(imgName, img);
448 | }
449 | 
450 | inline void OpticFlowIO::SaveFlowMatrixAsImage(const char* imgName, cv::Mat2f flowMat)
451 | {
452 | 	int rows = flowMat.rows; int cols = flowMat.cols;
453 | 	FImage tmp_u2 = FImage(cols, rows, 1);
454 |     FImage tmp_v2 = FImage(cols, rows, 1);
455 |     OpticFlowIO::FlowMatrix2Flow(flowMat, tmp_u2, tmp_v2);        
456 |     OpticFlowIO::SaveFlowAsImage(imgName, tmp_u2.pData, tmp_v2.pData, cols, rows);
457 | }
458 | 
459 | // draw each match as a 3x3 color block
460 | inline void OpticFlowIO::Match2Flow(FImage& inMat, FImage& ou, FImage& ov, int w, int h)
461 | {
462 | 	if (!ou.matchDimension(w, h, 1)){
463 | 		ou.allocate(w, h, 1);
464 | 	}
465 | 	if (!ov.matchDimension(w, h, 1)){
466 | 		ov.allocate(w, h, 1);
467 | 	}
468 | 	ou.setValue(UNKNOWN_FLOW);
469 | 	ov.setValue(UNKNOWN_FLOW);
470 | 	int cnt = inMat.height();
471 | 	for (int i = 0; i < cnt; i++){
472 | 		float* p = inMat.rowPtr(i);
473 | 		float x = p[0];
474 | 		float y = p[1];
475 | 		float u = p[2] - p[0]; //x component of flow velocity
476 | 		float v = p[3] - p[1]; //y component of flow velocity
477 | 		for (int di = -1; di <= 1; di++){
478 | 			for (int dj = -1; dj <= 1; dj++){
479 | 				int tx = ImageProcessing::EnforceRange(x + dj, w);
480 | 				int ty = ImageProcessing::EnforceRange(y + di, h);
481 | 				ou[ty*w + tx] = u;
482 | 				ov[ty*w + tx] = v;
483 | 			}
484 | 		}
485 | 	}
486 | }
487 | 
488 | 
489 | // draw each match from a dense flow matrix inMat to a 1x1 color block
490 | inline void OpticFlowIO::FlowMatrix2Flow(cv::Mat2f inMat, FImage& ou, FImage& ov)
491 | {
492 |     int rows = inMat.rows;
493 |     int cols = inMat.cols;
494 | 	if (!ou.matchDimension(cols, rows, 1)){
495 | 		ou.allocate(cols, rows, 1);
496 | 	}
497 | 	if (!ov.matchDimension(cols, rows, 1)){
498 | 		ov.allocate(cols, rows, 1);
499 | 	}
500 | 	for (int j = 0; j < rows; j++){
501 |         for(int i = 0; i < cols; i++){
502 |             float u = inMat.at<cv::Vec2f>(j,i)[0];
503 |             float v = inMat.at<cv::Vec2f>(j,i)[1];
504 |             ou[j*cols + i] = u;
505 |             ov[j*cols + i] = v;
506 |         }
507 | 	}
508 | }
509 | 
510 | template <class T>
511 | float OpticFlowIO::ErrorImage(unsigned char* fillPix, T* u1, T* v1, T* u2, T* v2, int w, int h)
512 | {
513 | 	unsigned char pix[4];
514 | 
515 | //#define LOG_COLOR
516 | #ifdef LOG_COLOR
517 | 	float LC[10][5] =
518 | 	{ { 0, 0.0625, 49, 54, 149 },
519 | 	{ 0.0625, 0.125, 69, 117, 180 },
520 | 	{ 0.125, 0.25, 116, 173, 209 },
521 | 	{ 0.25, 0.5, 171, 217, 233 },
522 | 	{ 0.5, 1, 224, 243, 248 },
523 | 	{ 1, 2, 254, 224, 144 },
524 | 	{ 2, 4, 253, 174, 97 },
525 | 	{ 4, 8, 244, 109, 67 },
526 | 	{ 8, 16, 215, 48, 39 },
527 | 	{ 16, 1000000000.0, 165, 0, 38 } };
528 | #endif
529 | 
530 | 	int totalCnt = 0;
531 | 	int validCnt = 0;
532 | 	for (int i = 0; i < h; i++){
533 | 		for (int j = 0; j < w; j++){
534 | 			int idx = i*w + j;
535 | 
536 | 			if (unknown_flow(u1[idx], v1[idx]) || unknown_flow(u2[idx], v2[idx])){
537 | 				// red: occlusion
538 | 				pix[0] = 0; pix[1] = 0; pix[2] = 255;
539 | 				pix[2] = 0; // TODO
540 | 				pix[3] = 0xFF; // only for alignment
541 | 				memcpy(fillPix + idx * 4, pix, 4);
542 | 				continue;
543 | 			}
544 | 
545 | 			float endPtErr = sqrt(pow(u1[idx] - u2[idx], 2) + pow(v1[idx] - v2[idx], 2));
546 | 
547 | #ifdef LOG_COLOR
548 | 			float f_err = endPtErr;
549 | 			float f_mag = sqrt(u2[idx] * u2[idx] + v2[idx] * v2[idx]);
550 | 			float n_err = std::min(f_err / 3.0, 20.0*f_err / f_mag);
551 | 			for (int i = 0; i < 10; i++) {
552 | 				if (n_err >= LC[i][0] && n_err < LC[i][1]) {
553 | 					pix[3] = 0xFF; // only for alignment
554 | 					pix[2] = (uint8_t)LC[i][2];
555 | 					pix[1] = (uint8_t)LC[i][3];
556 | 					pix[0] = (uint8_t)LC[i][4];
557 | 				}
558 | 			}
559 | 			if (unknown_flow(u1[idx], v1[idx]) || unknown_flow(u2[idx], v2[idx])) {
560 | 				pix[2] *= 0.5;
561 | 				pix[1] *= 0.5;
562 | 				pix[0] *= 0.5;
563 | 			}
564 | #else
565 | 			float v = __min(endPtErr, 5.0) / 5.0;
566 | 			//float v = __min(endPtErr, 3.0) / 3.0;
567 | 			pix[0] = v * 255; pix[1] = v * 255; pix[2] = v * 255;
568 | 			pix[3] = 0xFF; // only for alignment
569 | 
570 | #if 1
571 | 			if (endPtErr > 3.0){ // red
572 | 				pix[0] = pix[1] = 0;
573 | 				pix[2] = 255;
574 | 			}
575 | #endif
576 | #endif
577 | 			if (endPtErr < 3.0){
578 | 				validCnt++;
579 | 			}
580 | 			memcpy(fillPix + idx * 4, pix, 4);
581 | 			totalCnt++;
582 | 		}
583 | 	}
584 | 	return 1. - (float)validCnt / totalCnt;
585 | }
586 | 
587 | template <class T>
588 | float OpticFlowIO::ErrorImage(unsigned char* fillPix, T* u1, T* v1, char* gtName, int w, int h)
589 | {
590 | 	int gtw, gth;
591 | 	T* u2 = new T[w*h];
592 | 	T* v2 = new T[w*h];
593 | 	ReadFlowFile(u2, v2, &gtw, &gth, gtName);
594 | 	assert(w == gtw&&h == gth);
595 | 
596 | 	float r = ErrorImage(fillPix, u1, v1, u2, v2, w, h);
597 | 	delete[] u2;
598 | 	delete[] v2;
599 | 	return r;
600 | }
601 | 
602 | template <class T>
603 | float OpticFlowIO::ShowErrorImage(const char* winname, T* U, T* V, char* gtName, int w, int h, int waittime /*= 1*/)
604 | {
605 | 	cv::Mat img(h, w, CV_8UC4);
606 | 	float r = OpticFlowIO::ErrorImage(img.data, U, V, gtName, w, h);
607 | 
608 | 	cv::imshow(winname, img);
609 | 	cv::waitKey(waittime);
610 | 
611 | 	return r;
612 | }
613 | 
614 | template <class T>
615 | float OpticFlowIO::SaveErrorImage(const char* imgName, T* U, T* V, char* gtName, int w, int h)
616 | {
617 | 	cv::Mat img(h, w, CV_8UC4);
618 | 	float r = OpticFlowIO::ErrorImage(img.data, U, V, gtName, w, h);
619 | 
620 | 	cv::imwrite(imgName, img);
621 | 	return r;
622 | }
623 | 
624 | template <class T1, class T2>
625 | FlowErr OpticFlowIO::CalcFlowError(T1* u1, T1* v1, T2* u2, T2*v2, int w, int h)
626 | {
627 | 	FlowErr stat;
628 | 	memset(&stat, 0, sizeof(stat));
629 | 
630 | 	double endPtErr = 0;
631 | 	double angErr = 0;
632 | 	int n = 0;
633 | 	for(int i=0; i<h; i++){
634 | 		for(int j=0; j<w; j++){
635 | 			int idx = i*w+j;
636 | 			if(unknown_flow(u1[idx], v1[idx]) || unknown_flow(u2[idx], v2[idx])){
637 | 				continue;
638 | 			}
639 | 			endPtErr += sqrt(pow(u1[idx]-u2[idx],2)+pow(v1[idx]-v2[idx],2));
640 | 
641 | 			double tmp = (1.0+u1[idx]*u2[idx]+v1[idx]*v2[idx])
642 | 				/(sqrt(1.0+pow(u1[idx],2)+pow(v1[idx],2))*sqrt(1.0+pow(u2[idx],2)+pow(v2[idx],2)));
643 | 			
644 | 			if(tmp < -1.0)	tmp = -1.0;
645 | 			if(tmp > 1.0)	tmp = 1.0;
646 | 
647 | 			angErr += acos(tmp);
648 | 			n++;
649 | 		}
650 | 	}
651 | 
652 | 	stat.aae = (angErr/n) * 180/M_PI;
653 | 	stat.aee = endPtErr/n;
654 | 
655 | 	return stat;
656 | }
657 | 
658 | template <class T>
659 | void OpticFlowIO::setcols(T* colorwheel, int r, int g, int b, int k)
660 | {
661 | 	colorwheel[k*3+0] = r;
662 | 	colorwheel[k*3+1] = g;
663 | 	colorwheel[k*3+2] = b;
664 | }
665 | 
666 | template <class T>
667 | int OpticFlowIO::makecolorwheel(T* colorwheel)
668 | {
669 | 	// relative lengths of color transitions:
670 | 	// these are chosen based on perceptual similarity
671 | 	// (e.g. one can distinguish more shades between red and yellow 
672 | 	//  than between yellow and green)
673 | 	int RY = 15;
674 | 	int YG = 6;
675 | 	int GC = 4;
676 | 	int CB = 11;
677 | 	int BM = 13;
678 | 	int MR = 6;
679 | 	int ncols = RY + YG + GC + CB + BM + MR;
680 | 	//printf("ncols = %d\n", ncols);
681 | 	if (ncols > MAXWHEELCOLS){
682 | 		printf("Too Many Columns in ColorWheel!\n");
683 | 		//exit(1);
684 | 	}
685 | 	int i;
686 | 	int k = 0;
687 | 	for (i = 0; i < RY; i++) setcols(colorwheel, 255,	   255*i/RY,	 0,	       k++);
688 | 	for (i = 0; i < YG; i++) setcols(colorwheel, 255-255*i/YG, 255,		 0,	       k++);
689 | 	for (i = 0; i < GC; i++) setcols(colorwheel, 0,		   255,		 255*i/GC,     k++);
690 | 	for (i = 0; i < CB; i++) setcols(colorwheel, 0,		   255-255*i/CB, 255,	       k++);
691 | 	for (i = 0; i < BM; i++) setcols(colorwheel, 255*i/BM,	   0,		 255,	       k++);
692 | 	for (i = 0; i < MR; i++) setcols(colorwheel, 255,	   0,		 255-255*i/MR, k++);
693 | 
694 | 	return ncols;
695 | }
696 | 
697 | template <class T>
698 | void OpticFlowIO::computeColor(double fx, double fy, unsigned char *pix, T* colorwheel, int ncols)
699 | {
700 | 	double rad = sqrt(fx * fx + fy * fy);
701 | 	double a = atan2(-fy, -fx) / M_PI;
702 | 	double fk = (a + 1.0) / 2.0 * (ncols-1);
703 | 	int k0 = (int)fk;
704 | 	int k1 = (k0 + 1) % ncols;
705 | 	double f = fk - k0;
706 | 	//f = 0; // uncomment to see original color wheel
707 | 	for (int b = 0; b < 3; b++) {
708 | 		double col0 = colorwheel[k0*3+b] / 255.0;
709 | 		double col1 = colorwheel[k1*3+b] / 255.0;
710 | 		double col = (1 - f) * col0 + f * col1;
711 | 		if (rad <= 1)
712 | 			col = 1 - rad * (1 - col); // increase saturation with radius
713 | 		else
714 | 			col *= .75; // out of range
715 | 		pix[2 - b] = (int)(255.0 * col);
716 | 	}
717 | 	pix[3] = 0xff; // alpha channel, only for alignment
718 | }
719 | 
720 | #endif //_OpticFlowIO_H


--------------------------------------------------------------------------------
/include/CPM_include/Stochastic.h:
--------------------------------------------------------------------------------
  1 | #ifndef STOCHASTIC_H
  2 | #define STOCHASTIC_H
  3 | 
  4 | #include "math.h"
  5 | #include "stdlib.h"
  6 | #include "project.h"
  7 | #include "memory.h"
  8 | 
  9 | #define _Release_2DArray(X,i,length) for(i=0;i<length;i++) if(X[i]!=NULL) delete X[i]; delete []X
 10 | 
 11 | template <typename T>
 12 | T _abs(T x)
 13 | {
 14 | 	return x >= 0 ? x:-x;
 15 | }
 16 | 
 17 | #ifndef PI
 18 | #define PI 3.1415927
 19 | #endif
 20 | 
 21 | enum SortType{SortAscending,SortDescending};
 22 | 
 23 | class CStochastic
 24 | {
 25 | public:
 26 | 	CStochastic(void);
 27 | 	~CStochastic(void);
 28 | 	static void ConvertInt2String(int x,char* string,int BitNumber=3);
 29 | 	static double UniformSampling();
 30 | 	static int UniformSampling(int R);
 31 | 	static double GaussianSampling();
 32 | 	template <class T> static void GetMeanVar(T* signal,int length,double* mean,double* var);
 33 | 	static int Sampling(double* Density,int NumSamples);
 34 | 	static double GetMean(double *signal,int length);
 35 | 	static void Generate1DGaussian(double* pGaussian,int size,double sigma=0);
 36 | 	static void Generate2DGaussian(double* pGaussian,int size,double sigma=0);
 37 | 	static double entropy(double* pDensity,int n);
 38 | 
 39 | 	template <class T> static T sum(int NumData,T* pData);
 40 | 	template <class T> static void Normalize(int NumData,T* pData);
 41 | 	template <class T> static T mean(int NumData, T* pData);
 42 | 	template <class T> static void sort(int number, T* pData,int *pIndex,SortType m_SortType=SortDescending);
 43 | 	template <class T> static T Min(int NumData, T* pData);
 44 | 	template <class T> static T Min(int NumData, T* pData1,T* pData2);
 45 | 	template <class T> static T Max(int NumData ,T* pData);
 46 | 	template <class T> static int FindMax(int NumData,T* pData);
 47 | 	template <class T1,class T2> static void ComputeVectorMean(int Dim,int NumData,T1* pData,T2* pMean,double* pWeight=NULL);
 48 | 	template <class T1,class T2> static void ComputeMeanCovariance(int Dim,int NumData,T1* pData,T2* pMean,T2* pCovarance,double* pWeight=NULL);
 49 | 	template <class T1,class T2> static double VectorSquareDistance(int Dim,T1* pVector1,T2* pVector2);
 50 | 	template <class T1> static void KMeanClustering(int Dim,int NumData,int NumClusters,T1* pData,int *pPartition,double** pClusterMean=NULL,int MaxIterationNum=10,int MinClusterSampleNumber=2);
 51 | 	template <class T> static double norm(T* X,int Dim);
 52 | 	template <class T1,class T2> static int FindClosestPoint(T1* pPointSet,int NumPoints,int nDim,T2* QueryPoint);
 53 | 	template <class T1,class T2> static void GaussianFiltering(T1* pSrcArray,T2* pDstArray,int NumPoints,int nChannels,int size,double sigma);
 54 | };
 55 | 
 56 | template <class T>
 57 | void CStochastic::GetMeanVar(T* signal,int length,double* mean,double* var)
 58 | {
 59 | 	double m_mean=0,m_var=0;
 60 | 
 61 | 	int i;
 62 | 	for (i=0;i<length;i++)
 63 | 		m_mean+=signal[i];
 64 | 	m_mean/=length;
 65 | 	for (i=0;i<length;i++)
 66 | 		m_var+=(signal[i]-m_mean)*(signal[i]-m_mean);
 67 | 	m_var/=length-1;
 68 | 	*mean=m_mean;
 69 | 	*var=m_var;
 70 | }
 71 | 
 72 | template <class T>
 73 | T CStochastic::sum(int NumData, T* pData)
 74 | {
 75 | 	T sum=0;
 76 | 	int i;
 77 | 	for(i=0;i<NumData;i++)
 78 | 		sum+=pData[i];
 79 | 	return sum;
 80 | }
 81 | 
 82 | template <class T>
 83 | void CStochastic::Normalize(int NumData,T* pData)
 84 | {
 85 | 	int i;
 86 | 	T Sum;
 87 | 	Sum=sum(NumData,pData);
 88 | 	for(i=0;i<NumData;i++)
 89 | 		pData[i]/=Sum;
 90 | }
 91 | 
 92 | template <class T>
 93 | T CStochastic::mean(int NumData,T* pData)
 94 | {
 95 | 	return sum(NumData,pData)/NumData;
 96 | }
 97 | 
 98 | ////////////////////////////////////////////////////////////
 99 | // sort data in descending order
100 | template <class T>
101 | void CStochastic::sort(int Number,T* pData,int *pIndex,SortType m_SortType)
102 | {
103 | 	int i,j,offset_extreme,*flag;
104 | 	double extreme;
105 | 	flag=new int[Number];
106 | 	memset(flag,0,sizeof(int)*Number);
107 | 	for(i=0;i<Number;i++)
108 | 	{
109 | 		if(m_SortType==SortDescending)
110 | 			extreme=-1E100;
111 | 		else
112 | 			extreme=1E100;
113 | 		offset_extreme=0;
114 | 		for(j=0;j<Number;j++)
115 | 		{
116 | 			if(flag[j]==1)
117 | 				continue;
118 | 			if( (m_SortType==SortDescending && extreme<pData[j]) || (m_SortType==SortAscending && extreme>pData[j]))
119 | 			{
120 | 				extreme=pData[j];
121 | 				offset_extreme=j;
122 | 			}
123 | 		}
124 | 		pIndex[i]=offset_extreme;
125 | 		flag[offset_extreme]=1;
126 | 	}
127 | 	delete flag;
128 | }
129 | 
130 | template <class T>
131 | T CStochastic::Min(int NumData,T* pData)
132 | {
133 | 	int i;
134 | 	T result=pData[0];
135 | 	for(i=1;i<NumData;i++)
136 | 		result=__min(result,pData[i]);
137 | 	return result;
138 | }
139 | 
140 | template <class T>
141 | T CStochastic::Min(int NumData,T* pData1,T* pData2)
142 | {
143 | 	int i;
144 | 	T result=pData1[0]+pData2[0];
145 | 	for(i=1;i<NumData;i++)
146 | 		result=__min(result,pData1[i]+pData2[i]);
147 | 	return result;
148 | }
149 | 
150 | template <class T>
151 | T CStochastic::Max(int NumData,T* pData)
152 | {
153 | 	int i;
154 | 	T result=pData[0];
155 | 	for(i=1;i<NumData;i++)
156 | 		result=__max(result,pData[i]);
157 | 	return result;
158 | }
159 | 
160 | template <class T>
161 | int CStochastic::FindMax(int NumData,T* pData)
162 | {
163 | 	int i,index;
164 | 	T result=pData[0];
165 | 	index=0;
166 | 	for(i=1;i<NumData;i++)
167 | 		if(pData[i]>result)
168 | 		{
169 | 			index=i;
170 | 			result=pData[i];
171 | 		}
172 | 	return index;
173 | }
174 | 
175 | 
176 | template <class T1,class T2>
177 | void CStochastic::ComputeMeanCovariance(int Dim,int NumData,T1* pData,T2* pMean,T2* pCovariance,double* pWeight)
178 | {
179 | 	int i,j,k;
180 | 	memset(pMean,0,sizeof(T2)*Dim);
181 | 	memset(pCovariance,0,sizeof(T2)*Dim*Dim);
182 | 	
183 | 	bool IsWeightLoaded=false;
184 | 	double Sum;
185 | 	if(pWeight!=NULL)
186 | 		IsWeightLoaded=true;
187 | 
188 | 	// compute mean first
189 | 	Sum=0;
190 | 	if(IsWeightLoaded)
191 | 		for(i=0;i<NumData;i++)
192 | 		{
193 | 			if(pWeight[i]==0)
194 | 				continue;
195 | 			for(j=0;j<Dim;j++)
196 | 				pMean[j]+=pData[i*Dim+j]*pWeight[i];
197 | 			Sum+=pWeight[i];
198 | 		}
199 | 	else
200 | 	{
201 | 		for(i=0;i<NumData;i++)
202 | 			for(j=0;j<Dim;j++)
203 | 				pMean[j]+=pData[i*Dim+j];
204 | 		Sum=NumData;
205 | 	}
206 | 	for(j=0;j<Dim;j++)
207 | 		pMean[j]/=Sum;
208 | 
209 | 	//compute covariance;
210 | 	T2* pTempVector;
211 | 	pTempVector=new T2[Dim];
212 | 
213 | 	for(i=0;i<NumData;i++)
214 | 	{
215 | 		for(j=0;j<Dim;j++)
216 | 			pTempVector[j]=pData[i*Dim+j]-pMean[j];
217 | 		if(IsWeightLoaded)
218 | 		{
219 | 			if(pWeight[i]==0)
220 | 				continue;
221 | 			for(j=0;j<Dim;j++)
222 | 				for(k=0;k<=j;k++)
223 | 					pCovariance[j*Dim+k]+=pTempVector[j]*pTempVector[k]*pWeight[i];
224 | 		}
225 | 		else
226 | 			for(j=0;j<Dim;j++)
227 | 				for(k=0;k<=j;k++)
228 | 					pCovariance[j*Dim+k]+=pTempVector[j]*pTempVector[k];
229 | 	}
230 | 	for(j=0;j<Dim;j++)
231 | 		for(k=j+1;k<Dim;k++)
232 | 			pCovariance[j*Dim+k]=pCovariance[k*Dim+j];
233 | 
234 | 	for(j=0;j<Dim*Dim;j++)
235 | 		pCovariance[j]/=Sum;
236 | 
237 | 	delete []pTempVector;
238 | }
239 | 
240 | template <class T1,class T2>
241 | void CStochastic::ComputeVectorMean(int Dim,int NumData,T1* pData,T2* pMean,double* pWeight)
242 | {
243 | 	int i,j;
244 | 	memset(pMean,0,sizeof(T2)*Dim);
245 | 	bool IsWeightLoaded;
246 | 	double Sum;
247 | 	if(pWeight=NULL)
248 | 		IsWeightLoaded=false;
249 | 	else
250 | 		IsWeightLoaded=true;
251 | 
252 | 	Sum=0;
253 | 	if(IsWeightLoaded)
254 | 		for(i=0;i<NumData;i++)
255 | 		{
256 | 			if(pWeight[i]==0)
257 | 				continue;
258 | 			for(j=0;j<Dim;j++)
259 | 				pMean[j]+=pData[i*Dim+j]*pWeight[i];
260 | 			Sum+=pWeight[i];
261 | 		}
262 | 	else
263 | 	{
264 | 		for(i=0;i<NumData;i++)
265 | 			for(j=0;j<Dim;j++)
266 | 				pMean[j]+=pData[i*Dim+j];
267 | 		Sum=NumData;
268 | 	}
269 | 	for(j=0;j<Dim;j++)
270 | 		pMean[j]/=Sum;
271 | }
272 | 
273 | template <class T1,class T2>
274 | double CStochastic::VectorSquareDistance(int Dim,T1* pVector1,T2* pVector2)
275 | {
276 | 	double result=0,temp;
277 | 	int i;
278 | 	for(i=0;i<Dim;i++)
279 | 	{
280 | 		temp=pVector1[i]-pVector2[i];
281 | 		result+=temp*temp;
282 | 	}
283 | 	return result;
284 | }
285 | 
286 | template <class T1>
287 | void CStochastic::KMeanClustering(int Dim,int NumData,int NumClusters,T1* pData,int *pPartition,double** pClusterMean,int MaxIterationNum, int MinClusterSampleNumber)
288 | {
289 | 	int i,j,k,l,Index,ClusterSampleNumber;
290 | 	double MinDistance,Distance;
291 | 	double** pCenters;
292 | 	pCenters=new double*[NumClusters];
293 | 	for(i=0;i<NumClusters;i++)
294 | 		pCenters[i]=new double[Dim];
295 | 	
296 | 	// generate randome guess of the partition
297 | _CStochastic_KMeanClustering_InitializePartition:
298 | 	for(i=0;i<NumClusters;i++)
299 | 	{
300 | 		Index=UniformSampling(NumData);
301 | 		for(j=0;j<Dim;j++)
302 | 			pCenters[i][j]=pData[Index*Dim+j];
303 | 	}
304 | 
305 | 	for(k=0;k<MaxIterationNum;k++)
306 | 	{
307 | 		// step 1. do partition
308 | 		for(i=0;i<NumData;i++)
309 | 		{
310 | 			MinDistance=1E100;
311 | 			for(j=0;j<NumClusters;j++)
312 | 			{
313 | 				Distance=VectorSquareDistance(Dim,pData+i*Dim,pCenters[j]);
314 | 				if(Distance<MinDistance)
315 | 				{
316 | 					MinDistance=Distance;
317 | 					Index=j;
318 | 				}
319 | 			}
320 | 			pPartition[i]=Index;
321 | 		}
322 | 		// step 2. compute mean
323 | 		for(i=0;i<NumClusters;i++)
324 | 		{
325 | 			memset(pCenters[i],0,sizeof(double)*Dim);
326 | 			ClusterSampleNumber=0;
327 | 			for(j=0;j<NumData;j++)
328 | 				if(pPartition[j]==i)
329 | 				{
330 | 					for(l=0;l<Dim;l++)
331 | 						pCenters[i][l]+=pData[j*Dim+l];
332 | 					ClusterSampleNumber++;
333 | 				}
334 | 			// maybe the initial partition is bad
335 | 			// if so just do initial partition again
336 | 			if(ClusterSampleNumber<MinClusterSampleNumber)
337 | 				goto _CStochastic_KMeanClustering_InitializePartition;
338 | 			for(l=0;l<Dim;l++)
339 | 				pCenters[i][l]/=ClusterSampleNumber;
340 | 		}
341 | 	}
342 | 	// output the final partition if necessary
343 | 	if(pClusterMean!=NULL)
344 | 		for(i=0;i<NumClusters;i++)
345 | 			for(l=0;l<Dim;l++)
346 | 				pClusterMean[i][l]=pCenters[i][l];
347 | 	// free buffer
348 | 	for(i=0;i<NumClusters;i++)
349 | 		delete pCenters[i];
350 | 	delete []pCenters;
351 | }
352 | 
353 | template <class T>
354 | double CStochastic::norm(T* X,int Dim)
355 | {
356 | 	double result=0;
357 | 	int i;
358 | 	for(i=0;i<Dim;i++)
359 | 		result+=X[i]*X[i];
360 | 	result=sqrt(result);
361 | 	return result;
362 | }
363 | 
364 | template <class T1,class T2>
365 | int CStochastic::FindClosestPoint(T1* pPointSet,int NumPoints,int nDim,T2* QueryPoint)
366 | {
367 | 	int i,j,Index=0,offset;
368 | 	T1 MinDistance,Distance,x;
369 | 	MinDistance=0;
370 | 	for(j=0;j<nDim;j++)
371 | 		MinDistance+=_abs(pPointSet[j]-QueryPoint[j]);
372 | 	for(i=1;i<NumPoints;i++)
373 | 	{
374 | 		Distance=0;
375 | 		offset=i*nDim;
376 | 		for(j=0;j<nDim;j++)
377 | 		{
378 | 			x=pPointSet[offset+j]-QueryPoint[j];
379 | 			Distance+=_abs(x);
380 | 		}
381 | 		if(Distance<MinDistance)
382 | 		{
383 | 			MinDistance=Distance;
384 | 			Index=i;
385 | 		}
386 | 	}
387 | 	return Index;
388 | }
389 | 
390 | template <class T1,class T2>
391 | void CStochastic::GaussianFiltering(T1* pSrcArray,T2* pDstArray,int NumPoints,int nChannels,int size,double sigma)
392 | {
393 | 	int i,j,u,l;
394 | 	double *pGaussian,temp;
395 | 	pGaussian=new double[2*size+1];
396 | 	Generate1DGaussian(pGaussian,size,sigma);
397 | 	for(i=0;i<NumPoints;i++)
398 | 		for(l=0;l<nChannels;l++)
399 | 		{
400 | 			temp=0;
401 | 			for(j=-size;j<=size;j++)
402 | 			{
403 | 				u=i+j;
404 | 				u=__max(__min(u,NumPoints-1),0);
405 | 				temp+=pSrcArray[u*nChannels+l]*pGaussian[j+size];
406 | 			}
407 | 			pDstArray[i*nChannels+l]=temp;
408 | 		}
409 | 	delete pGaussian;
410 | }
411 | 
412 | #endif


--------------------------------------------------------------------------------
/include/CPM_include/Util.h:
--------------------------------------------------------------------------------
  1 | #ifndef _UTIL_H_
  2 | #define _UTIL_H_
  3 | 
  4 | #include <stdio.h>
  5 | 
  6 | /************************************************************************/
  7 | /*                   CTimer                                             */
  8 | /************************************************************************/
  9 | #ifdef WIN32
 10 | #include <windows.h>
 11 | #else
 12 | #include <sys/time.h>
 13 | #endif
 14 | 
 15 | template <class T>
 16 | class _CTimer
 17 | {
 18 | public:
 19 | 	_CTimer();
 20 | 	~_CTimer(){};
 21 | 
 22 | 	void tic();	// time in clock
 23 | 	double toc(const char* msg = NULL); // time out clock
 24 | private:
 25 | 	T inT, f;
 26 | };
 27 | 
 28 | template <class T>
 29 | _CTimer<T>::_CTimer()
 30 | {
 31 | #ifdef WIN32
 32 | 	QueryPerformanceFrequency(&f);
 33 | #endif
 34 | 	tic();
 35 | }
 36 | 
 37 | #ifdef WIN32
 38 | typedef _CTimer<LARGE_INTEGER> CTimer; // only CTimer makes sense
 39 | #else
 40 | typedef _CTimer<struct timeval> CTimer;
 41 | #endif
 42 | 
 43 | template <class T>
 44 | void _CTimer<T>::tic()
 45 | {
 46 | #ifdef WIN32
 47 | 	QueryPerformanceCounter(&inT);
 48 | #else
 49 | 	gettimeofday(&inT,NULL);
 50 | #endif
 51 | }
 52 | 
 53 | template <class T>
 54 | double _CTimer<T>::toc(const char* msg)
 55 | {
 56 | #ifdef WIN32
 57 | 	LARGE_INTEGER outT;
 58 | 	QueryPerformanceCounter(&outT);
 59 | 
 60 | 	double dt = (double)(outT.QuadPart-inT.QuadPart)/f.QuadPart;
 61 | #else
 62 | 	struct timeval outT;
 63 | 	gettimeofday(&outT,NULL);
 64 | 	double dt = 1000000 * (outT.tv_sec - inT.tv_sec) + outT.tv_usec - inT.tv_usec;
 65 | 	dt /= 1000000;
 66 | #endif
 67 | 	if(msg){
 68 | 		printf("%s %f [s]\n", msg, dt);
 69 | 	}
 70 | 	
 71 | 	tic();
 72 | 	return dt;
 73 | }
 74 | 
 75 | /************************************************************************/
 76 | /*                      Gray2ColorTable                                 */
 77 | /************************************************************************/
 78 | template<class T>
 79 | class _CColorTable
 80 | {
 81 | public:
 82 | 	_CColorTable();
 83 | 	~_CColorTable(){};
 84 | 
 85 | 	// function to access the member variables
 86 | 	inline T* operator [] (int index) { return m_colorTbl[index]; };
 87 | 
 88 | private:
 89 | 	T m_colorTbl[256][3];
 90 | };
 91 | 
 92 | typedef _CColorTable<unsigned char> CColorTable;
 93 | 
 94 | template<class T>
 95 | _CColorTable<T>::_CColorTable()
 96 | {
 97 | 	// BGR table
 98 | static unsigned char gray2ColorTb[256][3] = {
 99 | {143,  0,  0},{147,  0,  0},{151,  0,  0},{155,  0,  0},{159,  0,  0},{163,  0,  0},{167,  0,  0},{171,  0,  0},{175,  0,  0},{179,  0,  0},{183,  0,  0},{187,  0,  0},{191,  0,  0},{195,  0,  0},{199,  0,  0},{203,  0,  0},{206,  0,  0},{210,  0,  0},{214,  0,  0},{218,  0,  0},{222,  0,  0},{226,  0,  0},{230,  0,  0},{234,  0,  0},{238,  0,  0},{242,  0,  0},{246,  0,  0},{250,  0,  0},{254,  0,  0},{255,  3,  0},{255,  7,  0},{255, 11,  0},
100 | {255, 14,  0},{255, 18,  0},{255, 22,  0},{255, 26,  0},{255, 30,  0},{255, 34,  0},{255, 38,  0},{255, 42,  0},{255, 46,  0},{255, 50,  0},{255, 54,  0},{255, 58,  0},{255, 62,  0},{255, 66,  0},{255, 70,  0},{255, 74,  0},{255, 77,  0},{255, 81,  0},{255, 85,  0},{255, 89,  0},{255, 93,  0},{255, 97,  0},{255,101,  0},{255,105,  0},{255,109,  0},{255,113,  0},{255,117,  0},{255,121,  0},{255,125,  0},{255,129,  0},{255,133,  0},{255,137,  0},
101 | {255,140,  0},{255,144,  0},{255,148,  0},{255,152,  0},{255,156,  0},{255,160,  0},{255,164,  0},{255,168,  0},{255,172,  0},{255,176,  0},{255,180,  0},{255,184,  0},{255,188,  0},{255,192,  0},{255,196,  0},{255,200,  0},{255,203,  0},{255,207,  0},{255,211,  0},{255,215,  0},{255,219,  0},{255,223,  0},{255,227,  0},{255,231,  0},{255,235,  0},{255,239,  0},{255,243,  0},{255,247,  0},{255,251,  0},{255,255,  0},{251,255,  4},{247,255,  8},
102 | {244,255, 11},{240,255, 15},{236,255, 19},{232,255, 23},{228,255, 27},{224,255, 31},{220,255, 35},{216,255, 39},{212,255, 43},{208,255, 47},{204,255, 51},{200,255, 55},{196,255, 59},{192,255, 63},{188,255, 67},{184,255, 71},{181,255, 74},{177,255, 78},{173,255, 82},{169,255, 86},{165,255, 90},{161,255, 94},{157,255, 98},{153,255,102},{149,255,106},{145,255,110},{141,255,114},{137,255,118},{133,255,122},{129,255,126},{125,255,130},{122,255,133},
103 | {118,255,137},{114,255,141},{110,255,145},{106,255,149},{102,255,153},{ 98,255,157},{ 94,255,161},{ 90,255,165},{ 86,255,169},{ 82,255,173},{ 78,255,177},{ 74,255,181},{ 70,255,185},{ 66,255,189},{ 62,255,193},{ 59,255,196},{ 55,255,200},{ 51,255,204},{ 47,255,208},{ 43,255,212},{ 39,255,216},{ 35,255,220},{ 31,255,224},{ 27,255,228},{ 23,255,232},{ 19,255,236},{ 15,255,240},{ 11,255,244},{  7,255,248},{  3,255,252},{  0,254,255},{  0,251,255},
104 | {  0,247,255},{  0,243,255},{  0,239,255},{  0,235,255},{  0,231,255},{  0,227,255},{  0,223,255},{  0,219,255},{  0,215,255},{  0,211,255},{  0,207,255},{  0,203,255},{  0,199,255},{  0,195,255},{  0,191,255},{  0,188,255},{  0,184,255},{  0,180,255},{  0,176,255},{  0,172,255},{  0,168,255},{  0,164,255},{  0,160,255},{  0,156,255},{  0,152,255},{  0,148,255},{  0,144,255},{  0,140,255},{  0,136,255},{  0,132,255},{  0,128,255},{  0,125,255},
105 | {  0,121,255},{  0,117,255},{  0,113,255},{  0,109,255},{  0,105,255},{  0,101,255},{  0, 97,255},{  0, 93,255},{  0, 89,255},{  0, 85,255},{  0, 81,255},{  0, 77,255},{  0, 73,255},{  0, 69,255},{  0, 65,255},{  0, 62,255},{  0, 58,255},{  0, 54,255},{  0, 50,255},{  0, 46,255},{  0, 42,255},{  0, 38,255},{  0, 34,255},{  0, 30,255},{  0, 26,255},{  0, 22,255},{  0, 18,255},{  0, 14,255},{  0, 10,255},{  0,  6,255},{  0,  2,255},{  0,  0,254},
106 | {  0,  0,250},{  0,  0,246},{  0,  0,242},{  0,  0,238},{  0,  0,234},{  0,  0,230},{  0,  0,226},{  0,  0,222},{  0,  0,218},{  0,  0,214},{  0,  0,210},{  0,  0,206},{  0,  0,202},{  0,  0,198},{  0,  0,194},{  0,  0,191},{  0,  0,187},{  0,  0,183},{  0,  0,179},{  0,  0,175},{  0,  0,171},{  0,  0,167},{  0,  0,163},{  0,  0,159},{  0,  0,155},{  0,  0,151},{  0,  0,147},{  0,  0,143},{  0,  0,139},{  0,  0,135},{  0,  0,131},{  0,  0,128}
107 | };
108 | 	for (int i = 0; i < 256; i++){
109 | 		memcpy(m_colorTbl[i], gray2ColorTb[i], 3);
110 | 	}
111 | }
112 | 
113 | #endif // _UTIL_H_
114 | 
115 | 


--------------------------------------------------------------------------------
/include/CPM_include/Vector.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | #include <vector>
  4 | #include <iostream>
  5 | #include <fstream>
  6 | #include "project.h"
  7 | 
  8 | using namespace std;
  9 | 
 10 | template <class T>
 11 | class Vector
 12 | {
 13 | protected:
 14 | 	int nDim;
 15 | 	T* pData;
 16 | public:
 17 | 	Vector(void);
 18 | 	Vector(int ndim,const T *data=NULL);
 19 | 	Vector(const Vector<T>& vect);
 20 | 	~Vector(void);
 21 | 	void releaseData();
 22 | 	void allocate(int ndim);
 23 | 	void allocate(const Vector<T>& vect){allocate(vect.nDim);};
 24 | 	void copyData(const Vector<T>& vect);
 25 | 	void dimcheck(const Vector<T>& vect) const;
 26 | 	void reset();
 27 | 	double norm2() const;
 28 | 
 29 | 	T sum() const;
 30 | 
 31 | 	void printVector();
 32 | 
 33 | 	// access the members
 34 | 	const T* data() const{return (const T*)pData;};
 35 | 	T* data() {return pData;};
 36 | 	int dim() const {return nDim;};
 37 | 	inline bool matchDimension(int _ndim) const {if(nDim==_ndim) return true;else return false;};
 38 | 	inline bool matchDimension(const Vector<T>& vect) const {return matchDimension(vect.nDim);};
 39 | 
 40 | 	// operators
 41 | 	inline T operator[](int index) const {return pData[index];};
 42 | 	inline T& operator[](int index){return *(pData+index);};
 43 | 	Vector<T>& operator=(const Vector<T>& vect);
 44 | 
 45 | 	//const Vector<T>& operator/(double val) const
 46 | 	//{
 47 | 	//	Vector<T> result(nDim);
 48 | 	//	for(int i =0;i<nDim;i++)
 49 | 	//		result.pData[i] = pData[i]/val;
 50 | 	//	return result;
 51 | 	//}
 52 | 	
 53 | 	Vector<T>& operator+=(const Vector<T>& vect);
 54 | 	Vector<T>& operator*=(const Vector<T>& vect);
 55 | 	Vector<T>& operator-=(const Vector<T>& vect);
 56 | 	Vector<T>& operator/=(const Vector<T>& vect);
 57 | 
 58 | 	Vector<T>& operator+=(double val);
 59 | 	Vector<T>& operator*=(double val);
 60 | 	Vector<T>& operator-=(double val);
 61 | 	Vector<T>& operator/=(double val);
 62 | 	
 63 | 	//friend const Vector<T> operator+(const Vector<T>& vect1,const Vector<T>& vect2);
 64 | 	//friend const Vector<T> operator*(const Vector<T>& vect1,const Vector<T>& vect2);
 65 | 	//friend const Vector<T> operator-(const Vector<T>& vect1,const Vector<T>& vect2);
 66 | 	//friend const Vector<T> operator/(const Vector<T>& vect1,const Vector<T>& vect2);
 67 | 
 68 | 	//friend const Vector<T> operator+(const Vector<T>& vect1,double val);
 69 | 	//friend const Vector<T> operator*(const Vector<T>& vect1,double val);
 70 | 	//friend const Vector<T> operator-(const Vector<T>& vect1,double val);
 71 | 	//friend Vector<T> operator/(const Vector<T>& vect,double val);
 72 | 	
 73 | 	friend double innerproduct(const Vector<T>& vect1,const Vector<T>& vect2)
 74 | 	{
 75 | 		double result = 0;
 76 | 		for(int i = 0;i<vect1.dim();i++)
 77 | 			result += vect1[i]*vect2[i];
 78 | 		return result;
 79 | 	}
 80 | 	
 81 | 	void concatenate(const vector< Vector<T> >& vect);
 82 | 
 83 | 	//friend const Vector<T> concatenate(const vector<Vector<T>>& vect){Vector<T> result; result.concatenate(vect); return result;};
 84 | 	bool write(ofstream& myfile)
 85 | 	{
 86 | 		myfile.write((char *)&nDim,sizeof(int));
 87 | 		myfile.write((char *)pData,sizeof(T)*nDim);
 88 | 		return true;
 89 | 	}
 90 | 	bool read(ifstream& myfile)
 91 | 	{
 92 | 		myfile.read((char *)&nDim,sizeof(int));
 93 | 		allocate(nDim);
 94 | 		myfile.read((char *)pData,sizeof(T)*nDim);
 95 | 		return true;
 96 | 	}
 97 | 	T mean(int N=-1) const
 98 | 	{
 99 | 		if(N==-1)
100 | 			N = nDim;
101 | 		T result = 0;
102 | 		for(int i = 0;i<N;i++)
103 | 			result += pData[i];
104 | 		return result/N;
105 | 	}
106 | #ifdef _MATLAB
107 | 	void readVector(const mxArray* prhs);
108 | 	void writeVector(mxArray*& prhs) const;
109 | #endif
110 | };
111 | 
112 | //template <class T>
113 | //double innerproduct(const Vector<T>& vect1,const Vector<T>& vect2)
114 | //{
115 | //	double result = 0;
116 | //	for(int i = 0;i<vect1.dim();i++)
117 | //		result += vect1[i]*vect2[i];
118 | //	return result;
119 | //}
120 | 
121 | template <class T>
122 | Vector<T>::Vector(void)
123 | {
124 | 	nDim=0;
125 | 	pData=NULL;
126 | }
127 | 
128 | template <class T>
129 | Vector<T>::Vector(int ndim, const T *data)
130 | {
131 | 	nDim=ndim;
132 | 	pData=new T[nDim];
133 | 	if(data!=NULL)
134 | 		memcpy(pData,data,sizeof(T)*nDim);
135 | 	else
136 | 		memset(pData,0,sizeof(T)*nDim);
137 | }
138 | 
139 | template <class T>
140 | Vector<T>::Vector(const Vector& vect)
141 | {
142 | 	nDim=0;
143 | 	pData=NULL;
144 | 	copyData(vect);
145 | }
146 | 
147 | template <class T>
148 | Vector<T>::~Vector(void)
149 | {
150 | 	releaseData();
151 | }
152 | 
153 | template <class T>
154 | void Vector<T>::releaseData()
155 | {
156 | 	if(pData!=NULL)
157 | 		delete []pData;
158 | 	pData=NULL;
159 | 	nDim=0;
160 | }
161 | 
162 | template <class T>
163 | void Vector<T>::allocate(int ndim)
164 | {
165 | 	releaseData();
166 | 	nDim=ndim;
167 | 	if(nDim>0)
168 | 	{
169 | 		pData=new T[nDim];
170 | 		reset();
171 | 	}
172 | }
173 | 
174 | 
175 | template <class T>
176 | void Vector<T>::copyData(const Vector &vect)
177 | {
178 | 	if(nDim!=vect.nDim)
179 | 	{
180 | 		releaseData();
181 | 		nDim=vect.nDim;
182 | 		pData=new T[nDim];
183 | 	}
184 | 	memcpy(pData,vect.pData,sizeof(T)*nDim);
185 | }
186 | 
187 | template <class T>
188 | void Vector<T>::dimcheck(const Vector &vect) const
189 | {
190 | 	if(nDim!=vect.nDim)
191 | 		cout<<"The dimensions of the vectors don't match!"<<endl;
192 | }
193 | 
194 | template <class T>
195 | void Vector<T>::reset()
196 | {
197 | 	if(pData!=NULL)
198 | 		memset(pData,0,sizeof(T)*nDim);
199 | }
200 | 
201 | 
202 | template <class T>
203 | T Vector<T>::sum() const
204 | {
205 | 	T total = 0;
206 | 	for(int i=0;i<nDim;i++)
207 | 		total += pData[i];
208 | 	return total;
209 | }
210 | 
211 | template <class T>
212 | double Vector<T>::norm2() const
213 | {
214 | 	double temp=0;
215 | 	for(int i=0;i<nDim;i++)
216 | 		temp+=pData[i]*pData[i];
217 | 	return temp;
218 | }
219 | 
220 | template <class T>
221 | void Vector<T>::printVector()
222 | {
223 | 	for(int i=0;i<nDim;i++)
224 | 		cout<<pData[i]<<' ';
225 | 	cout<<endl;
226 | }
227 | 
228 | 
229 | //----------------------------------------------------------------------------------
230 | // operators
231 | //----------------------------------------------------------------------------------
232 | template <class T>
233 | Vector<T>& Vector<T>::operator =(const Vector<T> &vect)
234 | {
235 | 	copyData(vect);
236 | 	return *this;
237 | }
238 | 
239 | template <class T>
240 | Vector<T>& Vector<T>::operator +=(const Vector<T> &vect)
241 | {
242 | 	dimcheck(vect);
243 | 	for(int i=0;i<nDim;i++)
244 | 		pData[i]+=vect.data()[i];
245 | 	return *this;
246 | }
247 | 
248 | template <class T>
249 | Vector<T>& Vector<T>::operator *=(const Vector<T> &vect)
250 | {
251 | 	dimcheck(vect);
252 | 	for(int i=0;i<nDim;i++)
253 | 		pData[i]*=vect.data()[i];
254 | 	return *this;
255 | }
256 | 
257 | template <class T>
258 | Vector<T>& Vector<T>::operator -=(const Vector<T> &vect)
259 | {
260 | 	dimcheck(vect);
261 | 	for(int i=0;i<nDim;i++)
262 | 		pData[i]-=vect.data()[i];
263 | 	return *this;
264 | }
265 | 
266 | template <class T>
267 | Vector<T>& Vector<T>::operator /=(const Vector<T> &vect)
268 | {
269 | 	dimcheck(vect);
270 | 	for(int i=0;i<nDim;i++)
271 | 		pData[i]/=vect.data()[i];
272 | 	return *this;
273 | }
274 | 
275 | template <class T>
276 | Vector<T>& Vector<T>::operator +=(double val)
277 | {
278 | 	for(int i=0;i<nDim;i++)
279 | 		pData[i]+=val;
280 | 	return *this;
281 | }
282 | 
283 | template <class T>
284 | Vector<T>& Vector<T>::operator *=(double val)
285 | {
286 | 	for(int i=0;i<nDim;i++)
287 | 		pData[i]*=val;
288 | 	return *this;
289 | }
290 | 
291 | template <class T>
292 | Vector<T>& Vector<T>::operator -=(double val)
293 | {
294 | 	for(int i=0;i<nDim;i++)
295 | 		pData[i]-=val;
296 | 	return *this;
297 | }
298 | 
299 | template <class T>
300 | Vector<T>& Vector<T>::operator /=(double val)
301 | {
302 | 	for(int i=0;i<nDim;i++)
303 | 		pData[i]/=val;
304 | 	return *this;
305 | }
306 | 
307 | 
308 | template<class T>
309 | const Vector<T> operator+(const Vector<T>& vect1,const Vector<T>& vect2)
310 | {
311 | 	vect1.dimcheck(vect2);
312 | 	Vector<T> result(vect1);
313 | 	result+=vect2;
314 | 	return result;
315 | }
316 | 
317 | template<class T>
318 | const Vector<T> operator-(const Vector<T>& vect1,const Vector<T>& vect2)
319 | {
320 | 	vect1.dimcheck(vect2);
321 | 	Vector<T> result(vect1);
322 | 	result-=vect2;
323 | 	return result;
324 | }
325 | 
326 | template<class T>
327 | const Vector<T> operator*(const Vector<T>& vect1,const Vector<T>& vect2)
328 | {
329 | 	vect1.dimcheck(vect2);
330 | 	Vector<T> result(vect1);
331 | 	result*=vect2;
332 | 	return result;
333 | }
334 | 
335 | template<class T>
336 | const Vector<T> operator/(const Vector<T>& vect1,const Vector<T>& vect2)
337 | {
338 | 	vect1.dimcheck(vect2);
339 | 	Vector<T> result(vect1);
340 | 	result/=vect2;
341 | 	return result;
342 | }
343 | 
344 | template <class T>
345 | Vector<T> operator+(const Vector<T>& vect,double val)
346 | {
347 | 	Vector<T> result(vect);
348 | 	result+=val;
349 | 	return result;
350 | }
351 | 
352 | template <class T>
353 | Vector<T> operator-(const Vector<T>& vect,double val)
354 | {
355 | 	Vector<T> result(vect);
356 | 	result-=val;
357 | 	return result;
358 | }
359 | 
360 | template <class T>
361 | Vector<T> operator*(const Vector<T>& vect,double val)
362 | {
363 | 	Vector<T> result(vect);
364 | 	result*=val;
365 | 	return result;
366 | }
367 | 
368 | template <class T>
369 | Vector<T> operator/(const Vector<T>& vect,double val)
370 | {
371 | 	Vector<T> result(vect);
372 | 	result/=val;
373 | 	return result;
374 | }
375 | 
376 | 
377 | template <class T>
378 | double innerproduct(const Vector<T>& vect1,const Vector<T>& vect2)
379 | {
380 | 	vect1.dimcheck(vect2);
381 | 	double result=0;
382 | 	for(int i=0;i<vect1.nDim;i++)
383 | 		result+=vect1.pData[i]*vect2.pData[i];
384 | 	return result;
385 | }
386 | 
387 | template <class T>
388 | void Vector<T>::concatenate(const vector< Vector<T> >& vect)
389 | {
390 | 	releaseData();
391 | 	nDim = 0;
392 | 	for(int i = 0;i<vect.size();i++)
393 | 		nDim += vect[i].dim();
394 | 	allocate(nDim);
395 | 	int dim = 0;
396 | 	for(int i = 0;i<vect.size(); i++)
397 | 	{
398 | 		for(int j = 0;j<vect[i].dim();j++)
399 | 			pData[dim+j] = vect[i][j];
400 | 		dim += vect[i].dim();
401 | 	}
402 | }
403 | 
404 | #ifdef _QT
405 | 
406 | bool Vector::writeVector(QFile& file) const
407 | {
408 | 	file.write((char *)&nDim,sizeof(int));
409 | 	if(file.write((char *)pData,sizeof(double)*nDim)!=sizeof(double)*nDim)
410 | 		return false;
411 | 	return true;
412 | }
413 | 
414 | bool Vector::readVector(QFile &file)
415 | {
416 | 	releaseData();
417 | 	file.read((char *)&nDim,sizeof(int));
418 | 	if(nDim<0)
419 | 		return false;
420 | 	if(nDim>0)
421 | 	{
422 | 		allocate(nDim);
423 | 		if(file.read((char *)pData,sizeof(double)*nDim)!=sizeof(double)*nDim)
424 | 			return false;
425 | 	}
426 | 	return true;
427 | }
428 | 
429 | #endif
430 | 
431 | 
432 | #ifdef _MATLAB
433 | 
434 | template <class T>
435 | void Vector<T>::readVector(const mxArray* prhs)
436 | {
437 | 	if(pData!=NULL)
438 | 		delete pData;
439 | 	int nElements = mxGetNumberOfDimensions(prhs);
440 | 	if(nElements>2)
441 | 		mexErrMsgTxt("A vector is expected to be loaded!");
442 | 	const int* dims = mxGetDimensions(prhs);
443 | 	nDim = dims[0]*dims[1];
444 | 	pData = new T[nDim];
445 | 	double* ptr = (double*)mxGetData(prhs);
446 | 	for(int i =0;i<nDim;i++)
447 | 		pData[i] = ptr[i];
448 | }
449 | 
450 | template <class T>
451 | void Vector<T>::writeVector(mxArray*& plhs) const
452 | {
453 | 	int dims[2];
454 | 	dims[0]=nDim;dims[1]=1;
455 | 	plhs=mxCreateNumericArray(2, dims,mxDOUBLE_CLASS, mxREAL);
456 | 	double *ptr = (double*)mxGetData(plhs);
457 | 	for(int i =0;i<nDim;i++)
458 | 		ptr[i] = pData[i];
459 | }
460 | #endif
461 | 
462 | //*/


--------------------------------------------------------------------------------
/include/CPM_include/project.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | #include "stdio.h"
 3 | #include <vector>
 4 | 
 5 | template <class T>
 6 | void _Release1DBuffer(T* pBuffer)
 7 | {
 8 | 	if(pBuffer!=NULL)
 9 | 		delete []pBuffer;
10 | 	pBuffer=NULL;
11 | }
12 | 
13 | template <class T>
14 | void _Rlease2DBuffer(T** pBuffer,size_t nElements)
15 | {
16 | 	for(size_t i=0;i<nElements;i++)
17 | 		delete [](pBuffer[i]);
18 | 	delete []pBuffer;
19 | 	pBuffer=NULL;
20 | }
21 | 
22 | #ifdef _MATLAB
23 | #include "mex.h"
24 | #endif
25 | 
26 | 
27 | #ifndef WIN32
28 | 
29 | #define strcmpi strcasecmp
30 | 
31 | template <class T1,class T2>
32 | T1 __min(T1 a, T2 b)
33 | {
34 |   return (a>b)?b:a;
35 | }
36 | 
37 | template <class T1,class T2>
38 | T1 __max(T1 a, T2 b)
39 | {
40 |   return (a<b)?b:a;
41 | }
42 | 
43 | #endif
44 | 


--------------------------------------------------------------------------------
/include/ColorModel.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * ColorModel.h
 3 |  *
 4 |  *      Author: Sebastian Lutz & Mairéad Grogan & Johanna Barbier
 5 |  *  University: Trinity College Dublin
 6 |  *      School: Computer Science and Statistics
 7 |  *     Project: V-SENSE
 8 |  */
 9 | 
10 | #ifndef COLORMODEL_H_
11 | #define COLORMODEL_H_
12 | 
13 | #include <opencv2/opencv.hpp>
14 | #include <vector>
15 | #include "constants.h"
16 | #include <tuple>
17 | #include "guidedfilter.h"
18 | #include "Pixel.h"
19 | #include "Minimization.h"
20 | 
21 | void getGlobalColorModel(cv::Mat &image, std::vector<cv::Vec3d> &means, std::vector<cv::Matx33d> &covs, double tau);
22 | bool getGlobalColorModelVideo(std::vector<cv::Vec3d> &means,
23 | 	std::vector<cv::Matx33d> &covs, std::vector<cv::Mat> frames, int frames_to_stack, double tau);
24 | 	
25 | void saveColorModelAsImage(const char * filename, std::vector<cv::Vec3d> means, std::vector<cv::Matx33d> covs);
26 | 
27 | float distHistogram(cv::Mat img1, cv::Mat img2);
28 | bool isCMRepresentative(std::vector<cv::Vec3d> means, std::vector<cv::Matx33d> covs, cv::Mat frame);
29 | 
30 | 
31 | #endif // COLORMODEL_H_
32 | 


--------------------------------------------------------------------------------
/include/Minimization.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Minimization.h
 3 |  *
 4 |  *      Author: Sebastian Lutz
 5 |  *  University: Trinity College Dublin
 6 |  *      School: Computer Science and Statistics
 7 |  *     Project: V-SENSE
 8 |  */
 9 | 
10 | #ifndef MINIMIZATION_H_
11 | #define MINIMIZATION_H_
12 | 
13 | #include <vector>
14 | #include <tuple>
15 | #include <opencv2/opencv.hpp>
16 | #include <numeric>
17 | #include "constants.h"
18 | 
19 | typedef double (* vFunctionCall)(std::vector<double> v, void* params); // pointer to function returning double
20 | typedef std::vector<double> (* vFunctionCall2)(std::vector<double> v, void* params);
21 | 
22 | //static variables for debugging (which is reached: maxIterLineSeach, cg_max_iter or isMin)
23 | extern int reach_ls_iter;
24 | extern int total_line_search;
25 | extern int reach_cg_iter;
26 | extern int reach_isMin_iter;
27 | 
28 | double energy(std::vector<double> v, std::vector<cv::Vec3d> &means, std::vector<cv::Matx33d> &covs, bool sparse);
29 | double min_f(std::vector<double> &v, void *params);
30 | std::vector<double> min_df(std::vector<double> &v, void* params);
31 | double min_refine_f(std::vector<double> &v, void *params);
32 | std::vector<double> min_refine_df(std::vector<double> &v, void* params);
33 | cv::Vec4d g(std::vector<double> &v, int n, cv::Vec3d color);
34 | std::vector<double> minimizeCG(std::vector<double> x_0, vFunctionCall f, vFunctionCall2 df,
35 |     std::vector<cv::Vec3d> &means, std::vector<cv::Matx33d> &covs, cv::Vec3d color);
36 | std::vector<double> minimizeFCG(std::vector<double> x_0, vFunctionCall f, vFunctionCall2 df,
37 |     std::vector<cv::Vec3d> &means, std::vector<cv::Matx33d> &covs, cv::Vec3d color,  double p, cv::Vec4d lambda, std::vector<double> gt_alpha);
38 | std::vector<double> minimizeMofM(std::vector<double> x_0, vFunctionCall f, vFunctionCall2 df,
39 |     std::vector<cv::Vec3d> &means, std::vector<cv::Matx33d> &covs, cv::Vec3d color, std::vector<double> gt_alpha);
40 | 
41 | void print_v(std::vector<double> v);
42 | 
43 | #endif // MINIMIZATION_H_
44 | 


--------------------------------------------------------------------------------
/include/Pixel.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Pixel.h
 3 |  *
 4 |  *      Author: Sebastian Lutz
 5 |  *  University: Trinity College Dublin
 6 |  *      School: Computer Science and Statistics
 7 |  *     Project: V-SENSE
 8 |  */
 9 | 
10 | #ifndef PIXEL_H_
11 | #define PIXEL_H_
12 | 
13 | #include <opencv2/opencv.hpp>
14 | #include <vector>
15 | #include <tuple>
16 | #include "Unmixing.h"
17 | #include "Minimization.h"
18 | 
19 | class Pixel
20 | {
21 | public:
22 | 	Pixel(cv::Vec3d color, cv::Point coord);
23 | 	Unmixing unmix(std::vector<cv::Vec3d> &means, std::vector<cv::Matx33d> &covs, std::vector<double> x_init);
24 | 	Unmixing refine(std::vector<cv::Vec3d> &means, std::vector<cv::Matx33d> &covs, std::vector<double> x_init);
25 | private:
26 | 	int minIndex(cv::Vec3d color, std::vector<cv::Vec3d> &means, std::vector<cv::Matx33d> &covs);
27 | 	cv::Vec3d color;
28 | 	cv::Point coord;
29 | };
30 | 
31 | 
32 | #endif // PIXEL_H_
33 | 


--------------------------------------------------------------------------------
/include/ThreadPool.h:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * ThreadPool.h
  3 |  *
  4 |  *      Author: Sebastian Lutz
  5 |  *  University: Trinity College Dublin
  6 |  *      School: Computer Science and Statistics
  7 |  *     Project: V-SENSE
  8 |  */
  9 | 
 10 | #ifndef THREAD_POOL_H
 11 | #define THREAD_POOL_H
 12 | 
 13 | #include <vector>
 14 | #include <queue>
 15 | #include <memory>
 16 | #include <thread>
 17 | #include <mutex>
 18 | #include <condition_variable>
 19 | #include <future>
 20 | #include <functional>
 21 | #include <stdexcept>
 22 | 
 23 | class ThreadPool {
 24 | public:
 25 |     ThreadPool(size_t);
 26 |     template<class F, class... Args>
 27 |     auto enqueue(F&& f, Args&&... args) 
 28 |         -> std::future<typename std::result_of<F(Args...)>::type>;
 29 |     ~ThreadPool();
 30 | private:
 31 |     // need to keep track of threads so we can join them
 32 |     std::vector< std::thread > workers;
 33 |     // the task queue
 34 |     std::queue< std::function<void()> > tasks;
 35 |     
 36 |     // synchronization
 37 |     std::mutex queue_mutex;
 38 |     std::condition_variable condition;
 39 |     bool stop;
 40 | };
 41 |  
 42 | // the constructor just launches some amount of workers
 43 | inline ThreadPool::ThreadPool(size_t threads)
 44 |     :   stop(false)
 45 | {
 46 |     for(size_t i = 0;i<threads;++i)
 47 |         workers.emplace_back(
 48 |             [this]
 49 |             {
 50 |                 for(;;)
 51 |                 {
 52 |                     std::function<void()> task;
 53 | 
 54 |                     {
 55 |                         std::unique_lock<std::mutex> lock(this->queue_mutex);
 56 |                         this->condition.wait(lock,
 57 |                             [this]{ return this->stop || !this->tasks.empty(); });
 58 |                         if(this->stop && this->tasks.empty())
 59 |                             return;
 60 |                         task = std::move(this->tasks.front());
 61 |                         this->tasks.pop();
 62 |                     }
 63 | 
 64 |                     task();
 65 |                 }
 66 |             }
 67 |         );
 68 | }
 69 | 
 70 | // add new work item to the pool
 71 | template<class F, class... Args>
 72 | auto ThreadPool::enqueue(F&& f, Args&&... args) 
 73 |     -> std::future<typename std::result_of<F(Args...)>::type>
 74 | {
 75 |     using return_type = typename std::result_of<F(Args...)>::type;
 76 | 
 77 |     auto task = std::make_shared< std::packaged_task<return_type()> >(
 78 |             std::bind(std::forward<F>(f), std::forward<Args>(args)...)
 79 |         );
 80 |         
 81 |     std::future<return_type> res = task->get_future();
 82 |     {
 83 |         std::unique_lock<std::mutex> lock(queue_mutex);
 84 | 
 85 |         // don't allow enqueueing after stopping the pool
 86 |         if(stop)
 87 |             throw std::runtime_error("enqueue on stopped ThreadPool");
 88 | 
 89 |         tasks.emplace([task](){ (*task)(); });
 90 |     }
 91 |     condition.notify_one();
 92 |     return res;
 93 | }
 94 | 
 95 | // the destructor joins all threads
 96 | inline ThreadPool::~ThreadPool()
 97 | {
 98 |     {
 99 |         std::unique_lock<std::mutex> lock(queue_mutex);
100 |         stop = true;
101 |     }
102 |     condition.notify_all();
103 |     for(std::thread &worker: workers)
104 |         worker.join();
105 | }
106 | 
107 | #endif // THREAD_POOL_H
108 | 


--------------------------------------------------------------------------------
/include/Unmixing.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Unmixing.h
 3 |  *
 4 |  *  Created on: 9 Mar 2017
 5 |  *      Author: Sebastian Lutz
 6 |  *  University: Trinity College Dublin
 7 |  *      School: Computer Science and Statistics
 8 |  *     Project: V-SENSE
 9 |  */
10 | 
11 | #ifndef UNMIXING_H_
12 | #define UNMIXING_H_
13 | 
14 | #include <vector>
15 | 
16 | struct Unmixing
17 | {
18 | 	std::vector<double> alphas;
19 | 	std::vector<cv::Vec3d> colors;
20 | 	cv::Point coords;
21 | 	Unmixing() : alphas(0), colors(0), coords(0,0) {}
22 | };
23 | 
24 | #endif // UNMIXING_H_
25 | 


--------------------------------------------------------------------------------
/include/constants.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * constants.h
 3 |  *
 4 |  *      Author: Sebastian Lutz
 5 |  *  University: Trinity College Dublin
 6 |  *      School: Computer Science and Statistics
 7 |  *     Project: V-SENSE
 8 |  */
 9 | 
10 | #ifndef CONSTANTS_H_
11 | #define CONSTANTS_H_
12 | 
13 | namespace constants {
14 | constexpr double beta = 10;
15 | constexpr double gamma = 0.25;
16 | constexpr double sigma = 100; //set to 10 in paper - mg increased to 100
17 | constexpr double eps = 0.0001;
18 | constexpr double step_size = 0.01;
19 | constexpr double tol = 0.000001;
20 | constexpr double ls_max_iter = 10; //line_search max iteration
21 | constexpr double cg_max_iter = 30; //conjugate gradient max iteration
22 | constexpr double isMin_max_iter = 2;
23 | constexpr double ls_tau = 0.25;
24 | constexpr double ls_c = 0.05; //
25 | constexpr double tau = 13; //mg found 13 was best with this method
26 | }
27 | 
28 | 
29 | #endif // CONSTANTS_H_
30 | 


--------------------------------------------------------------------------------
/include/guidedfilter.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * guidedfilter.h
 3 |  *
 4 |  *  Created on: 9 Mar 2017
 5 |  *      Author: https://github.com/atilimcetin/guided-filter
 6 |  */
 7 | 
 8 | #ifndef GUIDED_FILTER_H
 9 | #define GUIDED_FILTER_H
10 | 
11 | #include <opencv2/opencv.hpp>
12 | #include <opencv2/ximgproc/edge_filter.hpp>
13 | 
14 | class GuidedFilterImpl;
15 | 
16 | class GuidedFilter
17 | {
18 | public:
19 |     GuidedFilter(const cv::Mat &I, int r, double eps);
20 |     ~GuidedFilter();
21 | 
22 |     cv::Mat filter(const cv::Mat &p, int depth = -1) const;
23 | 
24 | private:
25 |     GuidedFilterImpl *impl_;
26 | };
27 | 
28 | cv::Mat guidedFilter(const cv::Mat &I, const cv::Mat &p, int r, double eps, int depth = -1);
29 | std::vector<cv::Mat> MatteRegularisation(int radius, cv::Mat frame, std::vector<cv::Mat> layers);
30 | 
31 | #endif // GUIDED_FILTER_H
32 | 


--------------------------------------------------------------------------------
/lib/libCPM_lib.a:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/V-Sense/soft_segmentation/a0e2717199787633eb849b5e66a59e9fd7f53b31/lib/libCPM_lib.a


--------------------------------------------------------------------------------
/lib/libCPM_lib.so:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/V-Sense/soft_segmentation/a0e2717199787633eb849b5e66a59e9fd7f53b31/lib/libCPM_lib.so


--------------------------------------------------------------------------------
/sphere01.txt:
--------------------------------------------------------------------------------
  1 | -0.000346410770796031	-0.00991991226090840	0.000605795643510027
  2 | 0.00268875135569942	-0.00427533199438197	-0.00821303171939867
  3 | -0.000689021396815425	0.00880298747953641	-0.00358694223678493
  4 | -0.00566701531588124	-0.000508330137014683	0.00733981766824431
  5 | -0.00576587961277150	0.00200352752827952	0.00550215915835332
  6 | -0.000392784321414518	0.000608641478146654	-0.00744701538541392
  7 | 0.00256410326963282	-0.00408894961713651	0.00140998986522865
  8 | 0.00686276656887821	-0.000757983760820438	0.00526498923131010
  9 | -0.00574048436874321	-0.00584711334258481	-0.00485489076551649
 10 | 0.00133277060716982	0.00289220590517101	-0.00566622827267021
 11 | 0.00594891213145767	0.00580248855880727	-0.00446986573919050
 12 | -0.00220095637282734	-0.00120942257170801	-0.00328634016337061
 13 | -0.00342542049973282	0.00636891390138187	-0.000235601131002549
 14 | -0.000725535305767255	0.00632173933629186	-0.00104617371334173
 15 | -0.00493799585743880	-0.00161265783612940	-0.00126962892699989
 16 | 0.00229848596596587	0.00492599475185195	0.00663468508966543
 17 | -0.00292194737866990	-0.00605973547621982	-0.00108263846362756
 18 | 0.00835382733191465	0.00304844880609641	-0.00426330041517505
 19 | 0.00331892770576681	0.000721252008818821	-0.00716583140371230
 20 | -0.00170101746894195	0.00728282459416352	-0.00507712448750829
 21 | 0.00176235942929337	0.000931562994856469	-0.00121883598811372
 22 | 0.00231276745027008	-0.00166765084547378	-0.00796432130702514
 23 | 0.00434120282745502	-0.00625125997701136	0.00336746840899558
 24 | -0.00128527395043839	0.000987956840942914	-0.00916943262870319
 25 | 0.00411603485432771	0.00740037187075786	0.00192656009763142
 26 | -0.00852869250692418	0.00398373214432487	-0.00176034357290511
 27 | -0.00722551701066825	0.00419884907330671	-0.00485204320155524
 28 | -0.000676528168883442	-0.00504004628989312	0.00162812567234348
 29 | -0.00448486739046868	0.00298758392607041	-0.00207572859207374
 30 | 0.00538179056829148	-0.00168132403585133	-0.00733059094438642
 31 | 0.00333586514776798	-0.00205909760402081	0.000279550274850011
 32 | 0.00517063080783865	-0.00162938545912096	-0.00489628885932731
 33 | -0.00243713606732973	0.00353805462366970	0.00513353525423037
 34 | -0.00212816238775931	0.00823615939296002	0.00300330603250425
 35 | 0.00256011865773797	-0.00220887821849559	0.00327282803183328
 36 | 0.00195685141449269	0.00762756013951757	0.00486152555767620
 37 | 0.00495008764145538	0.00532983014241912	0.00565880837746597
 38 | -0.00774825351658527	-0.00100435872135781	0.00215512124451070
 39 | 0.00475678533178413	-0.00321252952005549	0.00266301575395090
 40 | -0.00313713190662616	0.00220392299079052	0.00156210945420954
 41 | 0.00170485497137111	0.00780219222060401	-0.00306305973054751
 42 | 0.00255729787605129	0.00137962489082975	0.000184292103133459
 43 | 0.00240500264256855	-0.00107573158030582	0.00295479719522861
 44 | -0.00183475730362208	-0.00138428461501332	-0.00450872842565611
 45 | -0.00550530744221865	0.00533553906942542	6.14515384693971e-06
 46 | -0.00442216976628491	-0.00747013462615853	-0.00117805534150162
 47 | 0.00146379889581746	-0.00539195385810767	0.00767759019025177
 48 | -0.00383426588688712	0.00662586719403366	0.00206438794631277
 49 | -0.000332694962239512	-0.000883116591189718	0.00352996440419309
 50 | 0.00191600299267566	0.00641632218528475	0.00115798756367457
 51 | 0.00610254274662561	-0.00161955397010945	0.00587601664320117
 52 | -0.00218810890538352	-0.00894861404823490	-0.000575711319736619
 53 | 0.00328416586581960	0.00570819746765080	0.000598077650141374
 54 | -0.00568941676959433	0.00213853584070495	-0.00508665414485020
 55 | -0.00604271601485119	-0.00668237243668922	0.00251549377086587
 56 | 0.00287455169021628	-0.00135338563712854	0.00778829719140404
 57 | -0.00796914603877676	-0.00148550505015122	0.000552739829986141
 58 | -0.00484116698359083	-0.00493170624224248	-0.000281996561041041
 59 | -0.00253548753167612	-0.00546094550898460	0.00436925412716349
 60 | 0.00475796998206675	-0.00249257862226405	-0.00754288956635110
 61 | 0.00260114733838592	0.00371762486582006	0.00533227216108758
 62 | -0.000797118695139246	0.000536460084830983	-0.00767408090715778
 63 | -0.00274018101013022	0.00661820489314206	0.000165210760946795
 64 | -0.00283534659888801	-0.00770775119326335	0.00313803603728731
 65 | 0.00634743640087924	-0.00520501141383116	0.00108885485701917
 66 | -0.00864735004017978	0.000507454792372716	-0.000373539002362329
 67 | 0.00253022636640164	-0.00691549114116078	-0.00283220098047867
 68 | -0.00332896525592955	0.00638997861434284	0.00149496701822525
 69 | -0.00418773741498638	-0.00135716354323644	0.00645843024513384
 70 | -0.00288907567998560	0.00242745835623416	0.00147956175916046
 71 | 0.00266194870992240	-0.00201625276004204	0.00664307459806941
 72 | -0.00264748924020772	-0.00528765816892206	-0.00793474465190644
 73 | 0.00435343107349473	-0.00811132799293284	0.000203108658973412
 74 | 0.00433979004001365	0.00359426913789520	-0.00701774727780806
 75 | -0.00166372713741915	0.00895761099404485	0.000828144466525234
 76 | 0.000933071818800646	-0.00838587404690006	0.00344047168008163
 77 | 0.00141785655148700	0.00654698851728822	-0.00666213839032254
 78 | 0.00745028098876408	0.00161929844678937	0.00508840368943987
 79 | -0.00896138510561712	0.00377462581319280	-0.00200451093955259
 80 | 0.00268247513641644	-0.00112370283098531	0.00383250380753599
 81 | -0.000514115648101117	0.00675546995213059	-0.00283409945659419
 82 | -0.00342317507280625	-0.00160414294665662	-0.00693080149681453
 83 | -0.00334803051990475	0.00407029325778571	-0.00357218425499297
 84 | 0.00181031012149713	0.000308203349931697	-0.00277934010285077
 85 | -0.00430511622704525	-1.13391109887115e-05	0.00893031674675153
 86 | 0.00106140403412229	-0.00201950384142423	-0.00869507016124307
 87 | -0.000190122600437710	0.00336510822386691	-0.00407615557051391
 88 | 0.00112257628757960	0.000332918733297048	-0.00515706055723516
 89 | 0.000162428485773624	0.00732544243162358	-0.00505080744006532
 90 | -0.000819740756565707	-0.00130493496295852	-0.00708377653497038
 91 | -0.00194634480228235	0.00357170438335017	0.00120895372980799
 92 | -0.00400088791632147	-0.00626050261825919	0.000252622338483913
 93 | 0.00448636892282674	0.000367387126086369	-0.00266122528794947
 94 | -0.00146175591836459	-0.00718760338387702	-0.00568343614215758
 95 | -0.00434885742694054	0.00354649118521918	-0.00798773499772761
 96 | 0.00158254776540120	0.000338168099520990	-0.000441346494067485
 97 | -0.00126545232626731	0.000848248962669164	-0.00852755803414164
 98 | -0.00180700425234852	-0.00840797672500618	0.00180494051807410
 99 | 0.00556734928578910	-0.00300007691012031	-0.000267694262778189
100 | 0.00867995310743906	-0.000874688314945102	-0.00362553296868265
101 | 


--------------------------------------------------------------------------------
/src/ColorModel.cpp:
--------------------------------------------------------------------------------
  1 | #include "ColorModel.h"
  2 | #include <fstream>
  3 | #include <iostream>
  4 | 
  5 | 
  6 | //this function reads a mat from a .txt file and reshapes for a given number of rows
  7 | cv::Mat ReadMatFromTxt(std::string filename, int rows, int cols) 
  8 | {
  9 | 	std::ifstream in(filename);
 10 | 	std::vector<double> nums;
 11 | 	while (in.good()){
 12 | 		double n;
 13 | 		in >> n;
 14 | 		if (in.eof()) break;
 15 | 		nums.push_back(n);
 16 | 	}
 17 | 	// now make a Mat from the vector:
 18 | 	cv::Mat mat = cv::Mat(nums);
 19 | 	cv::Mat mat_re = mat.reshape(1, rows); //reshape it so it has the correct number of rows/cols
 20 | 	cv::Mat mat_clone = mat_re.clone(); // must clone it to return it, other wise it return a pointer to the values nums
 21 | 	return mat_clone;
 22 | }
 23 | 
 24 | //this function writes a mat to file and is used for debugging
 25 | void writeMatToFile(cv::Mat& m, const char* filename) 
 26 | {
 27 | 	std::ofstream fout(filename);
 28 | 
 29 | 	if (!fout)
 30 | 	{
 31 | 		std::cout << "File Not Opened" << std::endl;  return;
 32 | 	}
 33 | 
 34 | 	for (int i = 0; i < m.rows; i++)
 35 | 	{
 36 | 		for (int j = 0; j < m.cols; j++)
 37 | 		{
 38 | 			fout << m.at<double>(i, j) << "\t";
 39 | 		}
 40 | 		fout << std::endl;
 41 | 	}
 42 | 
 43 | 	fout.close();
 44 | }
 45 | 
 46 | 
 47 | /**
 48 | *Compute 2D mahalanobis distance when color and distributions are projected to plane. See Eq 16 of "Unmixing-Based Soft Color Segmentation for Image Manipulation"
 49 | */
 50 | double mg_computeMahalProj(cv::Vec3d p, cv::Vec3d normal, cv::Matx33d cov, cv::Vec3d color){
 51 | 	// We need to project 'covariance' and 'color' to the plane defined by 'p' and 'normal'. To do this, we transform everything so that 'p' is at the origin and 'normal' is aligned with the z-axis.
 52 | 	// Then, instead of projecting to the plane defined by 'p' and 'normal', we project everything onto the plane created by the x and y axis. (This makes the covariance projection easier).    
 53 | 
 54 | 	//create rotation matrix to align normal to the z-axis. See https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d
 55 | 	cv::Mat nn, F_1;
 56 | 	cv::Matx33d F_2, U_trans;
 57 | 
 58 | 	//set up the variables needed to create G, F and U (see link)
 59 | 	cv::normalize(cv::Mat(normal), nn);
 60 | 	cv::Mat z = cv::Mat(cv::Vec3d(0,0,1)); //z axis
 61 | 	cv::Mat v = nn.cross(z);
 62 | 	double s = cv::norm(v);
 63 | 	double c = nn.dot(z);
 64 | 	cv::Mat v_mat = (z - c*nn)/cv::norm(z - c*nn);
 65 | 	cv::Mat w_mat = z.cross(nn);
 66 | 	
 67 | 	//create G, F and U matrices as defined in link above.
 68 | 	cv::Matx33d G = cv::Matx33d(c, -s, 0, s, c, 0 , 0, 0, 1);
 69 | 	cv::hconcat(nn, v_mat, F_1);
 70 | 	cv::hconcat(F_1, w_mat, F_2);
 71 | 	cv::Matx33d F = F_2.inv();
 72 | 	cv::Matx33d U = ((F.inv())*G*F);
 73 | 	cv::transpose(cv::Mat(U), U_trans);
 74 | 
 75 | 	//Transform color - first translate by p (as p has been moved to the origin) and rotate by U (because we aligned 'normal' to the z-axis).
 76 | 	cv::Vec3d c_rot = U*(color - p);
 77 | 	cv::Vec2d c_proj = cv::Vec2d(c_rot.val[0], c_rot.val[1]); //project transformed color onto xy plane
 78 | 
 79 | 	cv::Mat cov1_rot = cv::Mat(U*cov*U_trans); //transform the covariance using the rotation U (see https://robotics.stackexchange.com/questions/2556/how-to-rotate-covariance)
 80 | 	cv::Matx22d cov1_proj = cv::Matx22d(cov1_rot.at<double>(0,0),cov1_rot.at<double>(0,1),cov1_rot.at<double>(1,0),cov1_rot.at<double>(1,1)); //project to xy plane by removing 3rd dimension
 81 | 	double mahal = std::pow(cv::Mahalanobis(c_proj, cv::Vec2d(0,0),(cov1_proj).inv()),2); // compute mahalanobis distance
 82 | 
 83 | 	return mahal;
 84 | }
 85 | 
 86 | /**
 87 | * Projected Color Unmixing Energy. See Eq. 16 of "Unmixing-Based Soft Color Segmentation for Image Manipulation"
 88 | */
 89 | double mg_projectedCUEnergyAlt(cv::Vec3d m1, cv::Vec3d m2, cv::Matx33d c1, cv::Matx33d c2, cv::Vec3d color){
 90 | 	
 91 | 	double cost = 0;
 92 | 
 93 | 	//Check to make sure 'color' lies between the two planes defined by m1 and m2 
 94 | 	cv::Vec3d n = (m1 - m2)/cv::norm(cv::Mat(m1 - m2));
 95 | 	double dist = cv::norm(cv::Mat(m1 - m2)); // d(plane1, plane2)
 96 | 	double d1 = cv::norm((color-m1).dot(n)); //d(color, plane1)
 97 |  	double d2 = cv::norm((color-m2).dot(n)); //d(color, plane2)
 98 | 	if(d1 > dist || d2 > dist){
 99 | 
100 | 		cost = 100000; //if not between planes, set very high cost.
101 | 
102 | 	}
103 | 	else{
104 | 
105 | 		//Compute F (Eq 16 in unmixing paper)
106 | 		cv::Vec3d u1 = color - (((color - m1).dot(n))/(n.dot(n)))*n; //Eq 14 in unmixing paper
107 | 		cv::Vec3d u2 = color - (((color - m2).dot(n))/(n.dot(n)))*n;
108 | 		double alpha_1 = cv::norm(color - u2)/cv::norm(u1 - u2);//Eq 15 in unmixing paper
109 | 		double alpha_2 = 1 - alpha_1;
110 | 		double mahal1 = mg_computeMahalProj(m1, cv::Vec3d(m2-m1), c1, color); //computed projected mahalanobis distance
111 | 		double mahal2 = mg_computeMahalProj(m2, cv::Vec3d(m1-m2), c2, color);
112 | 		cost = alpha_1*(mahal1) + alpha_2*(mahal2); 
113 | 	}
114 | 	return cost;
115 | }
116 | 
117 | 
118 | /**
119 |  * Representation score of input color. If the score is lower, the color is better represented with the current color model.
120 |  */
121 | double representationScoreAlt(cv::Vec3d color, int n, std::vector<cv::Vec3d> &means, std::vector<cv::Matx33d> &covs, double tau, int &proj){
122 | 	if(n == 0){ 
123 | 		// No colors yet in color model, all colors not well represented
124 | 		return std::pow(tau,2) + 1;
125 | 
126 | 	} else if(n == 1){
127 | 
128 | 		return std::pow(cv::Mahalanobis(color, means.at(0), (covs.at(0)).inv()),2);
129 | 
130 | 	} else {
131 | 
132 | 		std::vector<double> repList;
133 | 		std::vector<int> indList; //use this to check if a given color uses the projected color unmixing or the original color unmixing cost 
134 | 		double proj_cost, norm_cost;
135 | 		for(unsigned int i = 0; i < n; i++){
136 | 
137 | 			norm_cost = (std::pow(cv::Mahalanobis(color, means.at(i), (covs.at(i)).inv()),2));
138 | 			repList.push_back(norm_cost);
139 | 			indList.push_back(0);
140 | 			for(unsigned int j = i+1; j < n; j++){ 
141 | 
142 | 				proj_cost = mg_projectedCUEnergyAlt(means.at(i), means.at(j), covs.at(i), covs.at(j), color); //compute projected unmixing energy
143 | 				repList.push_back(proj_cost);
144 | 				indList.push_back(1);
145 | 			}
146 | 		}
147 | 		//use proj to see what colors are using projected unmixing - for debugging
148 | 		double min_cost = *min_element(repList.begin(), repList.end());
149 | 		for(unsigned int j = 0; j < repList.size(); j++){
150 | 			if(min_cost == repList.at(j)){
151 | 				proj = indList.at(j);
152 | 			}
153 | 		}
154 | 		return min_cost;
155 | 	}
156 | }
157 | 
158 | /**
159 |  * Get the bin that pixel c is voting for
160 |  */
161 | std::tuple<int, int, int> getVotingBinAlt(cv::Vec3d c){
162 | 	std::tuple<int, int, int> bin;
163 | 	bin = std::make_tuple(std::floor(c.val[0]/0.1),std::floor(c.val[1]/0.1),std::floor(c.val[2]/0.1));
164 | 	if(std::get<0>(bin) == 10){
165 | 			std::get<0>(bin) = 9;
166 | 		}
167 | 	if(std::get<1>(bin) == 10){
168 | 			std::get<1>(bin) = 9;
169 | 		}
170 | 	if(std::get<2>(bin) == 10){
171 | 			std::get<2>(bin) = 9;
172 | 		}
173 | 
174 | 	return bin;
175 | }
176 | 
177 | /**
178 |  * Create guided filter kernel around the seed pixel at coordinates m,n
179 |  * See Eq. 11 of "Guided Image Filtering" by He et. al.
180 |  */
181 | cv::Mat kernelValuesAlt(cv::Rect roi, cv::Mat &img, int m, int n){
182 | 	// Neighbourhood around seed pixel, each pixel gets a kernel value
183 | 	int count3;
184 | 	cv::Mat neighbourhood = img(roi).clone();
185 | 	//imwrite("kernel_neigh.png", neighbourhood*255);
186 | 	cv::Mat kernel(neighbourhood.size(), CV_64FC3, cv::Scalar(0.0));
187 | 	cv::Mat kernel_weights(neighbourhood.size(), CV_64FC1, cv::Scalar(0.0));
188 | 	cv::Vec3d S = img.at<cv::Vec3d>(m,n); // Seed pixel
189 | 	double eps = 0.01;
190 | 	double weight = 0;
191 | 	// Create filter window for every pixel in neighbourhood
192 | 	for(int i = roi.y; i < neighbourhood.rows+roi.y; i++){
193 | 		for(int j = roi.x; j < neighbourhood.cols+roi.x; j++){
194 | 			cv::Rect roiN(std::max(j-10,0),std::max(i-10,0),std::min(20,img.cols-j),std::min(20,img.rows-i));
195 | 			cv::Mat patch = img(roiN).clone();
196 | 			cv::Scalar nMean;
197 | 			cv::Scalar nStddev;
198 | 			cv::meanStdDev(patch, nMean, nStddev);
199 | 			// For every pixel in window, calculate value to add to kernel value
200 | 			for(int k = roiN.y; k < patch.rows+roiN.y; k++){ // k start patch corner (in image coord), goes to patch corner + 20
201 | 				for(int l = roiN.x; l < patch.cols+roiN.x; l++){ // l start patch corner (in image coord), goes to patch corner + 20
202 | 					int x = k - (m-10); //m-10 is where original patch start, if k is smaller, its outside original patch //coordinates in original patch
203 | 					int y = l - (n-10);//n-10 is where original patch start, if l is smaller, its outside original patch //coordinates in original patch
204 | 					if(x >= 0 && y >= 0 && x < 20 && y < 20){ // But only if that pixel was in the original neighbourhood
205 | 						cv::Vec3d I = img.at<cv::Vec3d>(k,l);
206 | 						double v1 = 1 + (((I.val[0] - nMean.val[0]) * (S.val[0] - nMean.val[0])) / (std::pow(nStddev.val[0],2) + eps));
207 | 						double v2 = 1 + (((I.val[1] - nMean.val[1]) * (S.val[1] - nMean.val[1])) / (std::pow(nStddev.val[1],2) + eps));
208 | 						double v3 = 1 + (((I.val[2] - nMean.val[2]) * (S.val[2] - nMean.val[2])) / (std::pow(nStddev.val[2],2) + eps));
209 | 						kernel.at<cv::Vec3d>(x,y) = kernel.at<cv::Vec3d>(x,y) + cv::Vec3d(v1,v2,v3);
210 | 					}
211 | 				}
212 | 			}
213 | 			//writeMatToFile(kernel,"kernel.txt");
214 | 			//std::string var_ker_s = std::string("kernel_") + std::to_string(i) + std::to_string(j) + std::string(".txt");
215 | 			//const char* cvec = var_ker_s.c_str();
216 | 			//writeMatToFile(kernel,cvec);
217 | 
218 | 		}
219 | 	}
220 | 	for(size_t i = 0; i < kernel.rows; i++){
221 | 		for(size_t j = 0; j < kernel.cols; j++){
222 | 			weight = sqrt(kernel.at<cv::Vec3d>(i,j)[0]*kernel.at<cv::Vec3d>(i,j)[0] + kernel.at<cv::Vec3d>(i,j)[1]*kernel.at<cv::Vec3d>(i,j)[1] + kernel.at<cv::Vec3d>(i,j)[2]*kernel.at<cv::Vec3d>(i,j)[2]);
223 | 			kernel_weights.at<double>(i,j) = weight;
224 | 		}
225 | 	}
226 | 	return kernel_weights;
227 | }
228 | 
229 | 
230 | /**
231 |  * Voting energy of pixel - eq 10 unmixing paper
232 | */
233 | double getVoteAlt(cv::Vec3d gradient, double repScore){
234 | 	return std::pow(std::exp(-cv::norm(gradient, cv::NORM_L2)),2) * (1-std::exp(-repScore)); //mg added the square term 
235 | }
236 | 
237 | /**
238 |  * Calculate from which bin the next seed pixel should come from. See Section 5 of
239 |  * "Unmixing-Based Soft Color Segmentation for Image Manipulation"
240 |  */
241 | std::tuple<int, int, int> nextBin(cv::Mat &img, cv::Mat &gradient, std::vector<cv::Vec3d> &means,
242 | 	std::vector<cv::Matx33d> &covs, cv::Mat &votemask, double tau, double &vote){
243 | 	
244 | 	//mg used for debugging to save image output
245 | 	//cv::Mat check_rep = cv::Mat(img.rows, img.cols,CV_64FC3, cv::Scalar(0.0,0.0,0.0)); //use this to check whether color uses proj unmixing or not
246 | 	//cv::Mat store_score = cv::Mat(img.rows, img.cols,CV_64FC3, cv::Scalar(0.0,0.0,0.0)); //use this to store rep score as image
247 | 	//cv::Mat store_grad =  cv::Mat(img.rows, img.cols,CV_64FC1, cv::Scalar(0.0)); //use this to store rep score as txt file (can open in Matlab)
248 | 	//cv::Mat store_score2 = cv::Mat(img.rows, img.cols,CV_64FC1, cv::Scalar(0.0)); //use this to store rep score as txt file (can open in Matlab)
249 | 	//cv::Mat store_vote = cv::Mat(img.rows, img.cols,CV_64FC1, cv::Scalar(0.0)); //use this to store rep score as txt file (can open in Matlab)
250 | 	
251 | 	double votes[10][10][10] = {0};
252 | 	std::vector<double> scoreList;
253 | 	std::vector<double> voteList;
254 | 	std::tuple<int, int, int> bin;
255 | 	cv::Vec3d c, g;
256 | 	int n = means.size();
257 | 	double score;
258 | 	int scorecounter = 0;
259 | 
260 |     for(size_t i = 0; i < img.rows; i++){
261 | 		for(size_t j = 0; j < img.cols; j++){
262 | 		
263 | 			//mg used for debugging 
264 | 			//c = img.at<cv::Vec3d>(i,j);//mg if this section added, remove from lower loop
265 | 			//g = gradient.at<cv::Vec3d>(i,j);
266 | 			//bin = getVotingBinAlt(c);
267 | 			//int proj = 0;
268 | 			//score = representationScoreAlt(c,n,means,covs, tau, proj);
269 | 			//if(proj == 1){
270 | 			//	check_rep.at<cv::Vec3d>(i,j) = cv::Vec3d(1, 0, 0);
271 | 			//}
272 | 			//store_score.at<cv::Vec3d>(i,j) = cv::Vec3d(score, 0, 0);
273 | 			//store_score2.at<double>(i,j) = score;
274 | 			//store_grad.at<double>(i,j) = cv::norm(g, cv::NORM_L2);
275 | 			//store_vote.at<double>(i,j) =  std::exp(-cv::norm(g, cv::NORM_L2))*(1-std::exp(-score));//std::exp(-cv::norm(g, cv::NORM_L2));//getVoteAlt(g,score);
276 | 			//uchar temp = votemask.at<uchar>(i,j);		
277 | 
278 | 			if(votemask.at<uchar>(i,j) == 0){
279 | 				c = img.at<cv::Vec3d>(i,j);
280 | 				g = gradient.at<cv::Vec3d>(i,j);
281 | 				bin = getVotingBinAlt(c);
282 | 				int proj = 0;
283 | 				score = representationScoreAlt(c,n,means,covs, tau, proj);
284 | 				// Each pixel votes for its own bin with a certain energy
285 | 				scoreList.push_back(score);
286 | 				voteList.push_back(getVoteAlt(g,score));
287 | 				if(score > pow(tau,2)){
288 | 					scorecounter++;
289 | 					votes[std::get<0>(bin)][std::get<1>(bin)][std::get<2>(bin)]
290 | 						= votes[std::get<0>(bin)][std::get<1>(bin)][std::get<2>(bin)] + getVoteAlt(g,score);
291 | 				} else {
292 | 					votemask.at<uchar>(i,j) = 255;
293 | 				}
294 | 			}
295 | 		}
296 | 	}
297 | 
298 | 	//mg used for debugging 
299 | 	//std::string var_rep_score_check = std::string("rep_score_blue") + std::to_string(means.size())+ std::string(".png");	
300 | 	//imwrite( var_rep_score_check, 255*check_rep);
301 | 	//std::string var_rep_s = std::string("rep_score") + std::to_string(means.size())+ std::string(".txt");
302 | 	//const char* cvec = var_rep_s.c_str();
303 | 	//writeMatToFile(store_score2,cvec);
304 | 	//std::string var_vote_s = std::string("vote_") + std::to_string(means.size())+ std::string(".txt");
305 | 	//const char* cvecscore = var_vote_s.c_str();
306 | 	//writeMatToFile(store_vote,cvecscore);
307 | 	//cv::normalize(store_score,store_score, 255, 0, CV_MINMAX);
308 | 	//std::string var_rep = std::string("rep_score") + std::to_string(means.size())+ std::string(".png");
309 |     //imwrite(var_rep, store_score);
310 | 	//std::string var_grad_s = std::string("grad_") + std::to_string(means.size())+ std::string(".txt");
311 | 	//const char* cvecgrad = var_grad_s.c_str();
312 | 	//writeMatToFile(store_grad,cvecgrad);
313 | 
314 | 
315 | 	// Find bin with most votes
316 | 	std::tuple<int, int, int> ret;
317 | 	for(size_t i = 0; i < 10; i++){
318 | 		for(size_t j = 0; j < 10; j++){
319 | 			for(size_t k = 0; k < 10; k++){
320 | 				if(votes[i][j][k] > vote){ //make sure it has enough votes
321 | 					vote = votes[i][j][k];
322 | 					ret = std::make_tuple(i,j,k);
323 | 				}
324 | 			}
325 | 		}
326 | 	}
327 | 
328 | 	return ret;
329 | }
330 | 
331 | 
332 | 
333 | 
334 | 
335 | 
336 | /**
337 |  * Calculate the next seed pixel for the color model. See Section 5 of
338 |  * "Unmixing-Based Soft Color Segmentation for Image Manipulation"
339 |  */
340 | void getNextSeedPixelAlt(cv::Mat &img, cv::Mat &gradient, std::vector<cv::Vec3d> &means, std::vector<cv::Matx33d> &covs,
341 | 	cv::Mat &votemask, double tau, std::tuple<int, int, int> t){
342 | 	// Given the next bin, find the next seed pixel
343 | 	//std::cout << "The next seed pixel comes from bin: " << std::get<0>(t) << " " << std::get<1>(t);
344 | 	cv::Mat mask, pixelsInBin;
345 | 	double low_b = std::get<0>(t)*0.1;
346 | 	double low_g = std::get<1>(t)*0.1;
347 | 	double low_r = std::get<2>(t)*0.1;
348 | 	double high_b = (std::get<0>(t)+1)*0.1;
349 | 	double high_g = (std::get<1>(t)+1)*0.1;
350 | 	double high_r = (std::get<2>(t)+1)*0.1;
351 | 	int s = 0;
352 | 	// Create a mask for all pixels in the same bin
353 | 	cv::inRange(img,cv::Scalar(low_b,low_g,low_r),cv::Scalar(high_b,high_g,high_r),mask);
354 | 	cv::bitwise_and(img,img,pixelsInBin,mask);
355 | 	std::tuple<int, int> p;
356 |     cv::Mat neighbourhood;
357 |     /* Count for each pixel in that bin how many pixels in the surrounding 20x20 neighbourhood are also in that bin
358 |        Seed pixel is the pixel with the highest count */
359 | 	for(int i = 0; i < pixelsInBin.rows; i++){
360 | 		for(int j = 0; j < pixelsInBin.cols; j++){
361 | 			if(mask.at<uchar>(i,j) != 0){
362 | 				if(votemask.at<uchar>(i,j) != 255){//make sure pixels are unrepresented
363 | 					cv::Vec3d grad = gradient.at<cv::Vec3d>(i,j);
364 | 					cv::Rect roi(std::max(j-10,0),std::max(i-10,0),std::min(20,pixelsInBin.cols-j),std::min(20,pixelsInBin.rows-i));
365 | 					neighbourhood = mask(roi).clone();
366 | 					int sp = 0;
367 | 					for(size_t k = 0; k < neighbourhood.rows; k++){
368 | 						for(size_t l = 0; l < neighbourhood.cols; l++){
369 | 							if(neighbourhood.at<uchar>(k,l) == 255){
370 | 								sp++;
371 | 							}
372 | 						}
373 | 					}
374 | 					double si = sp*exp(-cv::norm(grad, cv::NORM_L2));
375 | 					if(si >= s){
376 | 						s = si;
377 | 						p = std::make_tuple(i,j);
378 | 					}
379 | 				}
380 | 			}
381 | 		}
382 | 	}
383 | 	// i, j are coordinates of next seed pixel
384 | 	int i = std::get<0>(p);
385 | 	int j = std::get<1>(p);
386 | 	std::cout << "The color of the next seed pixel is: "  << img.at<cv::Vec3d>(i,j)  <<  std::endl;
387 | 
388 | 	cv::Rect roi(std::max(j-10,0),std::max(i-10,0),std::min(20,pixelsInBin.cols-j),std::min(20,pixelsInBin.rows-i));
389 | 	//use filter to find pixels that are similar in colour
390 | 	cv::Mat kernel;
391 | 	kernel =  kernelValuesAlt(roi, img, i, j);
392 | 	double min_k, max_k; 
393 | 	minMaxLoc(kernel, &min_k, &max_k); //store the min/max of the kernel 
394 | 	neighbourhood = img(roi).clone(); //neighbourhood around seed pixel
395 | 	cv::Mat neigh_mask = mask(roi).clone(); //this mask indicates pixels in the same bin as the seed pixel
396 | 	cv::Mat samples_new = cv::Mat(neigh_mask.rows*neigh_mask.cols, 3, CV_64F);
397 | 	int num_samples = 0;
398 | 	//std::string var_neigh1 = std::string("orig_bin") + std::to_string(means.size())+ std::string(".png");
399 | 	//imwrite( var_neigh1, 255*neighbourhood);
400 | 	for(size_t m = 0; m < neigh_mask.rows; m++){
401 | 		for(size_t n = 0; n < neigh_mask.cols; n++){
402 | 			if(kernel.at<double>(m,n) > (0.7*max_k)){ //if kernel weight is >.7*max, it's close enough in colour to be used to compute mean/cov
403 | 				num_samples ++;
404 | 				cv::Vec3d v = neighbourhood.at<cv::Vec3d>(m,n);
405 | 				samples_new.at<double>(num_samples-1,0) = v.val[0];
406 | 				samples_new.at<double>(num_samples-1,1) = v.val[1];
407 | 				samples_new.at<double>(num_samples-1,2) = v.val[2];
408 | 			}
409 | 		}
410 | 	}
411 | 	//std::string var_neigh = std::string("red_bin") + std::to_string(means.size())+ std::string(".png");
412 | 	//imwrite( var_neigh, 255*neighbourhood);
413 | 	//remove any zeros rows at the end of samples_new
414 | 	cv::Rect mySamples(0, 0, 3, num_samples);
415 | 	cv::Mat samples_new2 = samples_new(mySamples).clone();
416 | 	//std::cout << samples_new << std::endl;
417 | 	//make sure it's not empty
418 | 	if(samples_new2.empty()){ //if there are no points in the neighbourhood, fill the votemask so the algorithm will end
419 | 		votemask = cv::Mat(img.rows, img.cols, CV_8U, cvScalar(255));
420 | 		return;
421 | 	}else{
422 | 
423 | 		//use samples_new2 to compute the covariance matrix
424 | 		cv::Mat mean_samp, cov_samp, cov, cov2, cov3, full_samp;
425 | 		cv::Mat sphere_color = cv::Mat(100, 3, CV_64FC1);
426 | 		cv::Mat sphere001;
427 | 		std::string sphere_filename = "../sphere01.txt";
428 | 
429 | 		//I found that if the 20x20 neighbourhood has a very small variation in colours, the covariance can be too small/narrow along certain direction.
430 | 		//So I add a sample of colours to the colours found in the image patch to prevent any covariance problems.
431 | 		sphere001 = ReadMatFromTxt(sphere_filename, 100, 3); //this file contains a sample of 3D points with mean 0 and std dev .01
432 | 		cv::Vec3d v = img.at<cv::Vec3d>(i,j);
433 | 		for(int i = 0; i < sphere001.rows; i++){
434 | 			sphere_color.at<double>(i,0) = (3*sphere001.at<double>(i,0)) + v.val[0]; //so points now centred at seed pixel colour with std dev .03
435 | 			sphere_color.at<double>(i,1) = (3*sphere001.at<double>(i,1)) + v.val[1];
436 | 			sphere_color.at<double>(i,2) = (3*sphere001.at<double>(i,2)) + v.val[2];
437 | 		}
438 | 		cv::vconcat(sphere_color, samples_new2, full_samp); //add sample from sphere about seed pixel colour to the sample from the patch
439 | 		cv::calcCovarMatrix(full_samp,cov3,mean_samp,CV_COVAR_NORMAL | CV_COVAR_ROWS); //compute covariance
440 | 		cov3 = (cov3 / (full_samp.rows - 1)); //normalise covariance
441 | 		cov = cov3;
442 | 
443 | 
444 | 		//check to make sure covariance is a correct covariance matrix/not close to singular
445 | 		cv::Mat mat_zeros = cv::Mat(3,3,CV_64FC1, cv::Scalar(0.0)); 
446 | 		cv::Scalar sum_mat = (cv::sum(cov - mat_zeros)); 
447 | 		cv::Scalar sum_inv_mat = (cv::sum(cov.inv() - mat_zeros));
448 | 		//1. check diagonal does not contain elements close to 0
449 | 		if(!(cov.at<double>(0,0) > 0.0000001 && cov.at<double>(1,1) > 0.0000001 && cov.at<double>(2,2) > 0.0000001)){ 
450 | 			cov = (cv::Mat_<float>(3, 3) <<0.0001, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0001); //if it does, replace cov with .00001*I
451 | 		}
452 | 		//2. check if cov or cov.inv() are close to zero matrix
453 | 		if((sum_mat.val[0] < .0000000001)||(sum_inv_mat.val[0] < .0000000001)){ //check if cov or cov.inv() are close to zero matrix
454 | 			cov = (cv::Mat_<float>(3, 3) <<0.0001, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0001); //if they are, replace cov with .00001*I
455 | 		}
456 | 
457 | 		//3. make sure the eigenvalues are positive
458 | 		cv::Mat eigenval;
459 | 		cv::eigen(cov, eigenval);
460 | 		for(int eig = 0; eig < eigenval.rows; eig++){
461 | 			if(eigenval.at<double>(eig, 0) < 0){
462 | 				cov = (cv::Mat_<float>(3, 3) <<0.0001, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0001); //if they are, replace cov with .00001*I
463 | 			}
464 | 		}
465 | 
466 | 		//then pass out the new mean and covariance
467 | 		cv::Vec3d mean = img.at<cv::Vec3d>(i,j); //New mean is the color of the seed pixel (not the sample mean)
468 | 		means.push_back(mean);
469 | 		covs.push_back(cov);
470 | 	}	
471 | }
472 | 
473 | /**
474 |  * Calculate the Global Color Model for image
475 |  */
476 | void getGlobalColorModel(cv::Mat &image, std::vector<cv::Vec3d> &means, std::vector<cv::Matx33d> &covs, double tau){
477 | 	cv::Mat gradient;
478 | 	cv::Mat img_small;
479 | 	cv::Laplacian(image, gradient, image.type());
480 | 	int count = 0;
481 | 
482 | 	cv::Mat votemask(image.rows, image.cols, CV_8U, cvScalar(0));
483 | 	double vote = 0.0;
484 | 	std::tuple<int, int, int> t = nextBin(image ,gradient, means, covs, votemask, tau, vote);//compute the bin that the next seed pixel will come from & update votemask
485 | 	
486 | 	// While more than 0.8% of pixels are not yet well represented, and the next bin has enough votes, add more colors to color model
487 | 	do{
488 | 		count = 0;
489 | 		getNextSeedPixelAlt(image, gradient, means, covs, votemask, tau, t); // given the next bin is t, find the seed pixel in t and add new mean and cov
490 | 		vote = 0.0;
491 | 		t = nextBin(image ,gradient, means, covs, votemask, tau, vote); //compute  the next bin to see if it has enough votes. Update votemask to compute unrepresented pixels.
492 | 	
493 | 		// Check which pixels are not yet represented by the color model
494 | 		for(size_t i = 0; i < image.rows; i++){
495 | 			for(size_t j = 0; j < image.cols; j++){
496 | 				if(votemask.at<uchar>(i,j) == 0){ //
497 | 						count++;
498 | 				}
499 | 			}
500 | 		}
501 | 	
502 | 		//std::string var_mask = std::string("mask") + std::to_string(means.size())+ std::string(".png");
503 | 		//imwrite( var_mask, votemask);
504 | 		std::cout << means.size() << " colors in model so far. Still " << count << " pixels unrepresented." << std::endl;
505 |     }while((count >= (image.rows*image.cols)/800) && (vote > 100.0) );
506 | 
507 | }
508 | 
509 | 
510 | /**
511 |  * Save colour mode as an image with name filename
512 |  * Start by simply saving square with means values
513 |  */
514 | void saveColorModelAsImage(const char * filename, std::vector<cv::Vec3d> means, std::vector<cv::Matx33d> covs) 
515 | {
516 | 	int nb_colours = means.size();
517 | 	int size_box = 100;
518 | 	cv::Mat model = cv::Mat(size_box, size_box * nb_colours, CV_64FC3); //rows, cols, type
519 | 	for (size_t b=0; b<nb_colours; b++){ //looping on colour boxes
520 | 		for(size_t i=0; i< size_box; i++){ //rows
521 | 			for (size_t j=size_box * b; j<size_box * (b+1); j++){ //cols
522 | 				model.at<cv::Vec3d>(i,j) = means[b] * 255.0;
523 | 			}
524 | 		}
525 | 	}
526 | 	cv::imwrite(filename, model);
527 | 	std::cout << "Colour model mean colours saved as image. "<< std::endl;
528 | }
529 | 
530 | 
531 | 


--------------------------------------------------------------------------------
/src/Minimization.cpp:
--------------------------------------------------------------------------------
  1 | #include "Minimization.h"
  2 | 
  3 | using InputParams = std::tuple<std::vector<cv::Vec3d>, std::vector<cv::Matx33d>, cv::Vec4d, cv::Vec3d, double,
  4 | 	bool, std::vector<double>, std::vector<double>>; //mg added this last term to store gt alphas for refinement
  5 | 
  6 | 
  7 | /**
  8 |  * Print vector v in cout
  9 |  */
 10 | void print_v(std::vector<double> v){
 11 | 	for(double d : v){
 12 | 		std::cout << d << " ";
 13 | 	}
 14 | 	std::cout << std::endl;
 15 | }
 16 | 
 17 | /**
 18 |  * Energy function of vector v. See Eq. 4 in "Unmixing-Based Soft Color Segmentation for Image Manipulation"
 19 |  * v: Input vector
 20 |  * means: mean values of color model
 21 |  * covs: inverse covariances of color model
 22 |  * sparse: true if the sparsity part should be included
 23 |  */
 24 | double energy(std::vector<double> v, std::vector<cv::Vec3d> &means, std::vector<cv::Matx33d> &covs, bool sparse){
 25 | 	int n = means.size();
 26 | 	double energy = 0;
 27 | 	double dist, c1, c2, c3, alpha, sparsity;
 28 | 	cv::Vec3d color;
 29 | 	std::vector<double> alphas;
 30 | 	for(size_t i = 0; i < n; i++){
 31 | 		alpha = v.at(i);
 32 | 		alphas.push_back(alpha);
 33 | 		c1 = v.at(3*i+n);
 34 | 		c2 = v.at(3*i+n+1);
 35 | 		c3 = v.at(3*i+n+2);
 36 | 		color = cv::Vec3d(c1,c2,c3);
 37 | 		dist = pow(cv::Mahalanobis(color, means.at(i), (covs.at(i)).inv()),2);
 38 | 		energy += alpha * dist;
 39 | 	}
 40 | 	if(sparse){
 41 | 		double sum_alpha = 0.0;
 42 | 		double sum_squared = 0.0;
 43 | 		for(auto& n : alphas){
 44 | 			sum_alpha += n;
 45 | 			sum_squared += pow(n,2);
 46 | 		}
 47 | 		if(sum_squared == 0)
 48 | 		{
 49 | 			sparsity = 500;
 50 | 		}
 51 | 		else{
 52 | 			sparsity = constants::sigma * (((sum_alpha)/(sum_squared)) - 1);
 53 | 		}
 54 | 	} else {
 55 | 		sparsity = 0;
 56 | 	}
 57 | 	return energy + sparsity;
 58 | }
 59 | 
 60 | /**
 61 |  * Constraint vector g in the unmixing step. See Eq. 4 in "Interactive High-Quality Green-Screen Keying via Color Unmixing"
 62 |  * v: Input vector
 63 |  * n: number of colors in color model
 64 |  * color: Color of pixel in the input image
 65 |  */
 66 | cv::Vec4d g(std::vector<double> &v, int n, cv::Vec3d color){
 67 | 	double g1 = 0, g2 = 0, g3 = 0, g4 = 0;
 68 | 	for(size_t i = 0; i < n; i++){
 69 | 		g1 += v.at(i)*v.at(3*i+n);
 70 | 		g2 += v.at(i)*v.at(3*i+n+1);
 71 | 		g3 += v.at(i)*v.at(3*i+n+2);
 72 | 		g4 += v.at(i);
 73 | 	}
 74 | 	return cv::Vec4d(pow(g1-color.val[0],2),pow(g2-color.val[1],2),pow(g3-color.val[2],2),pow(g4-1,2));
 75 | }
 76 | 
 77 | /**
 78 |  * Partial derivative of g in respect to alpha
 79 |  * v: minimization vector
 80 |  * n: number of layers
 81 |  * color: color of pixel
 82 |  * k: index of alpha value for this partial derivative
 83 |  */
 84 | cv::Vec4d dg_a(std::vector<double> &v, int n, cv::Vec3d color, int k){//mg checked - seems correct
 85 | 	double g1 = 0, g2 = 0, g3 = 0, g4 = 0;
 86 | 	for(size_t i = 0; i < n; i++){
 87 | 		g1 += v.at(i)*v.at(3*i+n);//r
 88 | 		g2 += v.at(i)*v.at(3*i+n+1);//g
 89 | 		g3 += v.at(i)*v.at(3*i+n+2);//b
 90 | 		g4 += v.at(i);//alpha
 91 | 	}
 92 | 	g1 = 2*(g1-color.val[0])*v.at(3*k+n);
 93 | 	g2 = 2*(g2-color.val[1])*v.at(3*k+n+1);
 94 | 	g3 = 2*(g3-color.val[2])*v.at(3*k+n+2);
 95 | 	g4 = 2*(g4-1);
 96 | 	return cv::Vec4d(g1,g2,g3,g4);
 97 | }
 98 | 
 99 | /**
100 |  * Partial derivative of g in respect to u. Note that this function calculates the partial derivatives for all 3 RGB values
101 |  * of u. In the gradient of the minimization function, only the partial derivative of one of those values at a time is needed.
102 |  */
103 | cv::Vec4d dg_u(std::vector<double> &v, int n, cv::Vec3d color, int k){
104 | 	double g1 = 0, g2 = 0, g3 = 0, g4 = 0;
105 | 	for(size_t i = 0; i < n; i++){
106 | 		g1 += v.at(i)*v.at(3*i+n);
107 | 		g2 += v.at(i)*v.at(3*i+n+1);
108 | 		g3 += v.at(i)*v.at(3*i+n+2);
109 | 	}
110 | 	g1 = 2*(g1-color.val[0])*v.at(k);
111 | 	g2 = 2*(g2-color.val[1])*v.at(k);
112 | 	g3 = 2*(g3-color.val[2])*v.at(k);
113 | 	return cv::Vec4d(g1,g2,g3,g4);
114 | }
115 | 
116 | /**
117 |  * Constraint vector g in the refinement step. See Eq. 6 in "Unmixing based soft color segmentation for image manipulation"
118 |  * v: Input vector
119 |  * n: number of colors in color model
120 |  * color: Color of pixel in the input image
121 |  * gt_alpha : the alpha values estimated using filtering step
122 |  */
123 | cv::Vec4d g_refine(std::vector<double> &v, int n, cv::Vec3d color, std::vector<double> gt_alphas){
124 | 	double g1 = 0, g2 = 0, g3 = 0, g4 = 0;
125 | 	for(size_t i = 0; i < n; i++){
126 | 		g1 += v.at(i)*v.at(3*i+n);
127 | 		g2 += v.at(i)*v.at(3*i+n+1);
128 | 		g3 += v.at(i)*v.at(3*i+n+2);
129 | 		g4 += (v.at(i) - gt_alphas.at(i))*(v.at(i) - gt_alphas.at(i));
130 | 	}
131 | 	return cv::Vec4d(pow(g1-color.val[0],2),pow(g2-color.val[1],2),pow(g3-color.val[2],2),pow(g4,2));
132 | }
133 | 
134 | /**
135 |  * Partial derivative of g_refine in respect to alpha
136 |  * v: minimization vector
137 |  * n: number of layers
138 |  * color: color of pixel
139 |  * k: index of alpha value for this partial derivative
140 |  * gt_alpha : the alpha values estimated using filtering step
141 |  */
142 | cv::Vec4d dg_refine_a(std::vector<double> &v, int n, cv::Vec3d color, int k, std::vector<double> gt_alphas){
143 | 	double g1 = 0, g2 = 0, g3 = 0, g4 = 0;
144 | 	for(size_t i = 0; i < n; i++){
145 | 		g1 += v.at(i)*v.at(3*i+n);//r
146 | 		g2 += v.at(i)*v.at(3*i+n+1);//g
147 | 		g3 += v.at(i)*v.at(3*i+n+2);//b
148 | 		//g4 += v.at(i);//alpha
149 | 	}
150 | 	g1 = 2*(g1-color.val[0])*v.at(3*k+n);
151 | 	g2 = 2*(g2-color.val[1])*v.at(3*k+n+1);
152 | 	g3 = 2*(g3-color.val[2])*v.at(3*k+n+2);
153 | 	g4 = 2*( v.at(k) - gt_alphas.at(k)); 
154 | 	return cv::Vec4d(g1,g2,g3,g4);
155 | }
156 | 
157 | 
158 | 
159 | /**
160 |  * Partial derivative of the Mahalanobis distance in respect to u
161 |  */
162 | double d_maha(cv::Vec3d mean, cv::Matx33d cov, cv::Vec3d u, int i){ //the square has been removed from the mahalanobis distance so we don't need to deal with sqrt in derivative
163 |     cv::Matx33d cov_inv = cov.inv();
164 | 	cv::Vec3d df_m;
165 | 	double df_i;
166 | 	df_m = cv::Vec3d(2*cov_inv*(cv::Mat(u - mean)));
167 | 	df_i = df_m.val[i];
168 |     //return 2*(u.val[i]-mean.val[i])*(cov_inv.col(i).val[0]+cov_inv.col(i).val[1]+cov_inv.col(i).val[2]);//mg checked, 
169 | 	return df_i;
170 | 	}
171 | 
172 | /**
173 |  * Minimization function for the unmixing step. See Line 1 in Algorithm 1 in
174 |  * "Interactive High-Quality Green-Screen Keying via Color Unmixing"
175 |  */
176 | double min_f(std::vector<double> &v, void* params){
177 | 	InputParams* param = static_cast<InputParams*>(params);
178 | 	std::vector<cv::Vec3d> means = std::get<0>((*param));
179 | 	std::vector<cv::Matx33d> covs = std::get<1>((*param));
180 | 	cv::Vec4d lambda = std::get<2>((*param));
181 | 	cv::Vec3d color = std::get<3>((*param));
182 | 	double p = std::get<4>((*param));
183 | 	bool s =std:: get<5>((*param));
184 | 	cv::Vec4d g_vec = g(v, means.size(), color); //constraint vector 
185 | 	return energy(v, means, covs, s) + g_vec.dot(lambda) + 0.5*p*pow(cv::norm(g_vec, cv::NORM_L2),2);
186 | }
187 | 
188 | /**
189 |  * Gradient of the minimization function for the unmixing step. Returns a vector where each element is the partial
190 |  * derivative of input vector v in respect to that element (#Jo: rather of energy(v) in repect to that element)
191 |  */
192 | std::vector<double> min_df(std::vector<double> &v, void* params){
193 | 	InputParams* param = static_cast<InputParams*>(params);
194 | 	std::vector<cv::Vec3d> means = std::get<0>((*param));
195 | 	std::vector<cv::Matx33d> covs = std::get<1>((*param));
196 | 	cv::Vec4d lambda = std::get<2>((*param));
197 | 	cv::Vec3d color = std::get<3>((*param));
198 | 	double p = std::get<4>((*param));
199 | 	bool s = std::get<5>((*param));
200 | 	int n = means.size();
201 | 	std::vector<double> df(4*n);
202 | 	double dist, sparse, alpha, dot_prod, g_norm, u1, u2, u3, grad_a, grad_u1, grad_u2, grad_u3;
203 | 	double sum_alpha = 0.0, sum_squared = 0.0, sum_u1 = 0.0, sum_u2 = 0.0, sum_u3 = 0.0;
204 | 	cv::Vec3d u;
205 | 	for(size_t i = 0; i < n; i++){
206 | 		alpha = v.at(i);
207 | 		u1 = v.at(3*i+n);
208 | 		u2 = v.at(3*i+n+1);
209 | 		u3 = v.at(3*i+n+2);
210 | 		sum_alpha += alpha;
211 | 		sum_squared += pow(alpha,2);
212 | 		sum_u1 += alpha*u1; //B_i * alpha_i
213 | 		sum_u2 += alpha*u2; //G_i * alpha_i
214 | 		sum_u3 += alpha*u3; //R_i * alpha_i
215 | 	}
216 | 	for(size_t i = 0; i < n; i++){
217 | 		alpha = v.at(i);
218 | 		u1 = v.at(3*i+n);
219 | 		u2 = v.at(3*i+n+1);
220 | 		u3 = v.at(3*i+n+2);
221 | 		u = cv::Vec3d(u1,u2,u3);
222 | 		//Alphas
223 | 		dist = pow(cv::Mahalanobis(u, means.at(i), (covs.at(i)).inv()),2);
224 | 		if(s){
225 | 			if(sum_squared < 0.0000000001){
226 | 				sparse = 0;
227 | 			}else{
228 | 				sparse = (constants::sigma*sum_squared-(constants::sigma*sum_alpha*2*alpha))/(pow(sum_squared,2));
229 | 			}
230 | 		} else {
231 | 			sparse = 0;
232 | 		}
233 | 		dot_prod = lambda.dot(dg_a(v, means.size(), color, i));
234 | 		g_norm = 0.5*p* (4*pow(sum_u1-color.val[0],3)*u1
235 | 					+ 4*pow(sum_u2-color.val[1],3)*u2 
236 | 					+ 4*pow(sum_u3-color.val[2],3)*u3
237 | 					+ 4*pow(sum_alpha-1,3));
238 | 		grad_a = dist+sparse+dot_prod+g_norm;
239 | 		df.at(i) = grad_a;
240 | 		//Colors
241 | 		//u1
242 | 		dist = alpha*d_maha(means.at(i),covs.at(i),u,0);
243 | 		dot_prod = lambda.dot(cv::Vec4d(dg_u(v, means.size(), color, i).val[0],0,0,0));
244 | 		g_norm = 0.5*p*4*pow(sum_u1-color.val[0],3)*alpha;
245 | 		grad_u1 = dist+dot_prod+g_norm;
246 | 		df.at(3*i+n) = grad_u1;
247 | 		//u2
248 | 		dist = alpha*d_maha(means.at(i),covs.at(i),u,1);
249 | 		dot_prod = lambda.dot(cv::Vec4d(0,dg_u(v, means.size(), color, i).val[1],0,0));
250 | 		g_norm = 0.5*p*4*pow(sum_u2-color.val[1],3)*alpha;
251 | 		grad_u2 = dist+dot_prod+g_norm;
252 | 		df.at(3*i+n+1) = grad_u2;
253 | 		//u3
254 | 		dist = alpha*d_maha(means.at(i),covs.at(i),u,2);
255 | 		dot_prod = lambda.dot(cv::Vec4d(0,0,dg_u(v, means.size(), color, i).val[2],0));
256 | 		g_norm = 0.5*p*4*pow(sum_u3-color.val[2],3)*alpha;
257 | 		grad_u3 = dist+dot_prod+g_norm;
258 | 		df.at(3*i+n+2) = grad_u3;
259 | 	}
260 | 	// Apply box constraints
261 | 	for(size_t i = 0; i < 4*n; i++){
262 | 		if((df.at(i) < 0 && v.at(i) >= 1) || (df.at(i) > 0 && v.at(i) <= 0)){
263 | 			df.at(i) = 0;
264 | 		}
265 | 	}
266 | 	return df;
267 | }
268 | 
269 | 
270 | 
271 | /**
272 |  * mg - Minimization function for the color refinement step. See equation (4) and(6) of unmixing paper
273 |  * 
274 |  */
275 | double min_refine_f(std::vector<double> &v, void* params){
276 | 	InputParams* param = static_cast<InputParams*>(params);
277 | 	std::vector<cv::Vec3d> means = std::get<0>((*param));
278 | 	std::vector<cv::Matx33d> covs = std::get<1>((*param));
279 | 	cv::Vec4d lambda = std::get<2>((*param));
280 | 	cv::Vec3d color = std::get<3>((*param));
281 | 	double p = std::get<4>((*param));
282 | 	bool s = 0;
283 | 	std::vector<double> gt_alpha = std::get<7>((*param));
284 | 	cv::Vec4d g_vec = g_refine(v, means.size(), color, gt_alpha); //constraint vector
285 | 	return energy(v, means, covs, s) + g_vec.dot(lambda) + 0.5*p*pow(cv::norm(g_vec, cv::NORM_L2),2);
286 | }
287 | 
288 | /**
289 |  * Gradient of the minimization function for the color refinement step. Returns a vector where each element is the partial
290 |  * derivative of input vector v in respect to that element.
291 |  */
292 | std::vector<double> min_refine_df(std::vector<double> &v, void* params){
293 | 	InputParams* param = static_cast<InputParams*>(params);
294 | 	std::vector<cv::Vec3d> means = std::get<0>((*param));
295 | 	std::vector<cv::Matx33d> covs = std::get<1>((*param));
296 | 	cv::Vec4d lambda = std::get<2>((*param));
297 | 	cv::Vec3d color = std::get<3>((*param));
298 | 	double p = std::get<4>((*param));
299 | 	bool s = std::get<5>((*param));
300 | 	std::vector<double> gt_alpha = std::get<7>((*param));
301 | 	int n = means.size();
302 | 	std::vector<double> df(4*n);
303 | 	double dist, sparse, alpha, alphai_gt, dot_prod, g_norm, u1, u2, u3, grad_a, grad_u1, grad_u2, grad_u3;
304 | 	double diff_gt_alpha = 0.0, sum_squared = 0.0, sum_u1 = 0.0, sum_u2 = 0.0, sum_u3 = 0.0;
305 | 	cv::Vec3d u;
306 | 	for(size_t i = 0; i < n; i++){
307 | 		alpha = v.at(i);
308 | 		alphai_gt = gt_alpha.at(i);
309 | 		u1 = v.at(3*i+n);
310 | 		u2 = v.at(3*i+n+1);
311 | 		u3 = v.at(3*i+n+2);
312 | 		diff_gt_alpha += pow((alpha - alphai_gt),2);
313 | 		sum_squared += pow(alpha,2);
314 | 		sum_u1 += alpha*u1; //B_i * alpha_i
315 | 		sum_u2 += alpha*u2; //G_i * alpha_i
316 | 		sum_u3 += alpha*u3; //R_i * alpha_i
317 | 	}
318 | 	for(size_t i = 0; i < n; i++){
319 | 		alpha = v.at(i);
320 | 		alphai_gt = gt_alpha.at(i);
321 | 		u1 = v.at(3*i+n);
322 | 		u2 = v.at(3*i+n+1);
323 | 		u3 = v.at(3*i+n+2);
324 | 		u = cv::Vec3d(u1,u2,u3);
325 | 		//Alphas - mg changed this so that the alpha values are not estimated in the final step - the filtered alphas are used instead - if they change, they may not add to 1 after optimisation.
326 | 		//these are the rows that were removed:
327 | 		/*************************
328 | 		//dist = pow(cv::Mahalanobis(u, means.at(i), (covs.at(i)).inv()),2);
329 | 		//sparse = 0;
330 | 		//dot_prod = lambda.dot(dg_refine_a(v, means.size(), color, i, gt_alpha));
331 | 		//g_norm = 0.5*p* (4*pow(sum_u1-color.val[0],3)*u1
332 | 		//			+ 4*pow(sum_u2-color.val[1],3)*u2 
333 | 		//			+ 4*pow(sum_u3-color.val[2],3)*u3
334 | 		//			+ 4*(diff_gt_alpha)*(alpha - alphai_gt)); //mg checked
335 | 		//grad_a = dist+sparse+dot_prod+g_norm;
336 | 		//df.at(i) = grad_a;
337 | 		/****************************/
338 | 		df.at(i) = 0;
339 | 		//Colors
340 | 		//u1
341 | 		dist = alpha*d_maha(means.at(i),covs.at(i),u,0);
342 | 		dot_prod = lambda.dot(cv::Vec4d(dg_u(v, means.size(), color, i).val[0],0,0,0));
343 | 		g_norm = 0.5*p*4*pow(sum_u1-color.val[0],3)*alpha;
344 | 		grad_u1 = dist+dot_prod+g_norm;
345 | 		df.at(3*i+n) = grad_u1;
346 | 		//u2
347 | 		dist = alpha*d_maha(means.at(i),covs.at(i),u,1);
348 | 		dot_prod = lambda.dot(cv::Vec4d(0,dg_u(v, means.size(), color, i).val[1],0,0));
349 | 		g_norm = 0.5*p*4*pow(sum_u2-color.val[1],3)*alpha;
350 | 		grad_u2 = dist+dot_prod+g_norm;
351 | 		df.at(3*i+n+1) = grad_u2;
352 | 		//u3
353 | 		dist = alpha*d_maha(means.at(i),covs.at(i),u,2);
354 | 		dot_prod = lambda.dot(cv::Vec4d(0,0,dg_u(v, means.size(), color, i).val[2],0));
355 | 		g_norm = 0.5*p*4*pow(sum_u3-color.val[2],3)*alpha;
356 | 		grad_u3 = dist+dot_prod+g_norm;
357 | 		df.at(3*i+n+2) = grad_u3;
358 | 	}
359 | 	// Apply box constraints
360 | 	for(size_t i = 0; i < 4*n; i++){
361 | 		if((df.at(i) < 0 && v.at(i) >= 1) || (df.at(i) > 0 && v.at(i) <= 0)){
362 | 			df.at(i) = 0;
363 | 		}
364 | 	}
365 | 	//mg added this to make sure that the alpha values don't change in the final refinement step - can rejig this code in future to make better
366 | 	//for(size_t i = 0; i < n; i++){
367 | 	//	df.at(i) = 0;
368 | 	//}
369 | 	return df;
370 | }
371 | 
372 | 
373 | 
374 | 
375 | /**
376 |  * Returns -v for input vector v
377 |  */
378 | std::vector<double> negate_v(std::vector<double> v){
379 | 	std::vector<double> r(v.size());
380 | 	for(unsigned int i = 0; i < v.size(); i++){
381 | 		r.at(i) = -v.at(i);
382 | 	}
383 | 	return r;
384 | }
385 | 
386 | /**
387 |  * The beta value for the polak-ribiere conjugate gradient descent
388 |  */
389 | double b_pr(std::vector<double> &a, std::vector<double> &b){
390 | 	double r1 = 0.0, r2 = 0.0;
391 | 	for(unsigned int i = 0; i < a.size(); i++){
392 | 		r1 += a.at(i)*(a.at(i)-b.at(i));
393 | 		r2 += b.at(i)*b.at(i);
394 | 	}
395 | 	if(r2 != 0){
396 | 		return r1/r2;
397 | 	} else {
398 | 		return 0;
399 | 	}
400 | }
401 | 
402 | /**
403 |  * Make Input vector v a unit vector
404 |  */
405 | std::vector<double> make_unit(std::vector<double> v){
406 | 	double l = 0.0;
407 | 	std::vector<double> r(v.size());
408 | 	for(double d : v){
409 | 		l += pow(d,2);
410 | 	}
411 | 	double k = sqrt(l);
412 | 	for(unsigned int i = 0; i < v.size(); i++){
413 | 		if(k != 0){
414 | 			r.at(i) = v.at(i)/k;
415 | 		}
416 | 	}
417 | 	return r;
418 | }
419 | 
420 | /**
421 |  * Clip a double value into range [0,1]
422 |  */
423 | double clip(double x){
424 | 	if(x > 1){
425 | 		return 1;
426 | 	} else if(x < 0){
427 | 		return 0;
428 | 	} else {
429 | 		return x;
430 | 	}
431 | }
432 | 
433 | /**
434 |  * Clip a vector into range [0,1]
435 |  */
436 | std::vector<double> clip(std::vector<double> &v){
437 | 	for(unsigned int i = 0; i < v.size(); i++){
438 | 		v.at(i) = (v.at(i)>1.0)?1.0:v.at(i);
439 | 		v.at(i) = (v.at(i)<0.0)?0.0:v.at(i);
440 | 	}
441 | 	return v;
442 | }
443 | 
444 | /**
445 |  * Linear transformation of two vectors and a scalar: a+c*b
446 |  */
447 | std::vector<double> lin_transform(std::vector<double> &a, std::vector<double> &b, double c){
448 | 	std::vector<double> r(a.size());
449 | 	for(unsigned int i = 0; i < a.size(); i++){
450 | 		r.at(i) = a.at(i)+c*b.at(i);
451 | 	}
452 | 	return r;
453 | }
454 | 
455 | /**
456 |  * Backtracking line-search
457 |  * Start direction x, search direction d
458 |  */
459 | double line_search(vFunctionCall f, std::vector<double> &x, std::vector<double> d, void* params)
460 | {
461 | 
462 | 	double in = f(x,params);
463 | 	double a_k = .5;
464 | 	int iter = 0;
465 | 	double rhok  = 1e-8;
466 | 	std::vector<double> x1;
467 | 	x1 = lin_transform(x,d,a_k);
468 | 	x1 = clip(x1); // Clip to box constraints
469 | 	while(!(f(x1,params) <= f(x,params) - (a_k*a_k)*(1e-4)*cv::norm(d,cv::NORM_L2)*cv::norm(d,cv::NORM_L2)) && (iter < 7)){ //mg was 5
470 | 		iter++;
471 | 		if(a_k*cv::norm(d,cv::NORM_L2) < rhok){   
472 |             a_k = 0;
473 | 			x1 = lin_transform(x,d,a_k);
474 | 			x1 = clip(x1);  
475 | 		}else{
476 | 			a_k = a_k*0.5;
477 | 			x1 = lin_transform(x,d,a_k);
478 | 			x1 = clip(x1);
479 | 		}
480 | 	};
481 | 	double out = f(x1,params);
482 | 	if(in < out){
483 | 		a_k = 0;
484 | 	}
485 | 	return a_k;
486 | }
487 | 
488 | //mg_changed line_search for testing
489 | // double mg_line_search(vFunctionCall f, std::vector<double> &x, std::vector<double> d, void* params)
490 | // {
491 | // 	//std::cout << f(x,params) << "just inside line search " << std::endl;
492 | // 	double in = f(x,params);
493 | // 	int iter = 0;
494 | // 	std::vector<double> x1;
495 | // 	double min_cost = 1000000;
496 | // 	double cost_ai;
497 | // 	double a_k = 0.0;
498 | // 	double a_ik = 0.0;
499 | // 	std::vector<double> x_k;
500 | // 	for(int a_i = 0; a_i < 501 ; a_i++){
501 | // 		a_ik = double(a_i)/1000;
502 | // 		x1 = lin_transform(x,d,a_ik);
503 | // 		x1 = clip(x1);
504 | // 		cost_ai = f(x1,params);
505 | // 		if(cost_ai < min_cost){
506 | // 			min_cost = cost_ai;
507 | // 			a_k = a_ik;
508 | // 			x_k = x1;
509 | // 		}
510 | // 	}
511 | 
512 | // 	double out = f(x_k,params);
513 | // 	if(in < out){
514 | // 		a_k = 0;
515 | // 		//std::cout << "line search wrong" << std::endl;
516 | // 	}
517 | // 	return a_k;
518 | // }
519 | 
520 | /**
521 |  * L2-Normed difference between vectors x1 and x2: ||x1-x2||
522 |  */
523 | double normed_diff(std::vector<double> &x1, std::vector<double> &x2){
524 | 	double ret = 0;
525 | 	for(unsigned int i = 0; i < x1.size(); i++){
526 | 		ret += pow(x1.at(i) - x2.at(i),2);
527 | 	}
528 | 	return sqrt(ret);
529 | }
530 | 
531 | /**
532 |  * Given a in_vect vector of values with alphas and B G R of each layer
533 |  * return the corresponding pixel value when summed together
534 |  */
535 | cv::Vec4i getPixelBGRA(std::vector<double> in_vect)
536 | {
537 | 	int n = in_vect.size() / 4;
538 | 	double alpha_unit;
539 | 	double sum_u1 = 0.0, sum_u2 = 0.0, sum_u3 = 0.0, sum_alpha = 0.0;
540 | 	cv::Vec4i out_pix;
541 | 	// for each layer
542 | 	for(size_t l = 0; l < n; l++){
543 | 		alpha_unit = in_vect.at(l);
544 | 		sum_u1 += alpha_unit*in_vect.at(3*l+n) * 255; //B_i * alpha_i
545 | 		sum_u2 += alpha_unit*in_vect.at(3*l+n+1) * 255; //G_i * alpha_i
546 | 		sum_u3 += alpha_unit*in_vect.at(3*l+n+2) * 255; //R_i * alpha_i
547 | 		sum_alpha += in_vect.at(l) * 255;
548 | 	}
549 | 	out_pix[0] = sum_u1;
550 | 	out_pix[1] = sum_u2;
551 | 	out_pix[2] = sum_u3;
552 | 	out_pix[3] = sum_alpha;
553 | 	return out_pix;
554 | }
555 | 
556 | /**
557 |  * This is the conjugate gradient minimisation step - used to minimise the cost function F
558 |  */
559 | 
560 | std::vector<double> minimizeFCG(std::vector<double> x_0, vFunctionCall f, vFunctionCall2 df, std::vector<cv::Vec3d> &means,
561 | 	std::vector<cv::Matx33d> &covs, cv::Vec3d color, double p, cv::Vec4d lambda, std::vector<double> gt_alpha){
562 | 	// Initialize variables
563 | 	int n = means.size();
564 | 	InputParams par[1] = {std::make_tuple(means, covs, lambda, color, p, true, std::vector<double> {0}, gt_alpha)};
565 | 	void* params = (void *)par;
566 | 
567 | 	// First search direction is gradient of f. Direction is always made a unit vector
568 | 	std::vector<double> dx0 = negate_v(df(x_0,params)); 
569 | 	dx0 = make_unit(dx0);
570 | 	double a0 = line_search(f, x_0, dx0, params);// arg min alpha
571 | 
572 | 	std::vector<double> x_n1, x_n, s_n, s_n1, dx_n, dx_n1;
573 | 	// Add direction*step_size to x_0 to get x_1, always clip to box constraints
574 | 	x_n = lin_transform(x_0, dx0, a0);
575 | 	x_n = clip(x_n); 
576 | 	s_n = dx0;
577 | 	dx_n = dx0;
578 | 	double b, a_n, tol;
579 | 	int iter = 0, isMin = 0;
580 | 
581 | 	do{ // Iterate until no progress is made
582 | 		iter++;		
583 | 		dx_n1 = negate_v(df(x_n,params)); //go in direction of gradient of the minimisation function
584 | 		dx_n1 = make_unit(dx_n1);
585 | 		b = b_pr(dx_n1, dx_n);
586 | 		s_n = lin_transform(dx_n1, s_n, b);
587 | 		s_n = make_unit(s_n); // New search direction
588 | 
589 | 		a_n = line_search(f, x_n, s_n, params);
590 | 		x_n1 = lin_transform(x_n, s_n, a_n);
591 | 		x_n1 = clip(x_n1);
592 | 
593 | 
594 | 		tol = normed_diff(x_n1, x_n);
595 | 		x_n = x_n1;
596 | 		dx_n = dx_n1;
597 | 		if(tol < constants::tol){
598 | 			isMin++;
599 | 			s_n = negate_v(df(x_n,params));
600 | 			s_n = make_unit(s_n);
601 | 		}
602 | 		if(iter % (4*n) == 0){ //Periodical reset of direction
603 | 			s_n = negate_v(df(x_n,params));
604 | 			s_n = make_unit(s_n);
605 | 		}
606 | 
607 | 	}while((isMin < constants::isMin_max_iter) && (iter < constants::cg_max_iter));
608 | 
609 | 	//  std::cout << "end: " << std::endl; 
610 | 	//  for(int i = 0; i < x_n.size(); i++){
611 | 	//  	std::cout << "i " << i << " " << x_n.at(i) << std::endl;
612 | 	//  }
613 | 	//  std::cout << "f x_n: " << f(x_n,params) << std::endl;
614 | 
615 | 	return x_n;
616 | }
617 | 
618 | 
619 | /* 
620 | *
621 | *This is the method of multipliers - mg
622 | *
623 | */
624 | 
625 | std::vector<double> minimizeMofM(std::vector<double> x_0, vFunctionCall f, vFunctionCall2 df, std::vector<cv::Vec3d> &means,
626 | 	std::vector<cv::Matx33d> &covs, cv::Vec3d color, std::vector<double>gt_alpha){
627 | 	// Initialize variables
628 | 	double p = .1; //initial value 0.1 
629 | 	cv::Vec4d lambda = cv::Vec4d(.1,.1,.1,.1); //initial value (0.1,0.1,0.1,0.1) 
630 | 	int n = means.size();
631 | 	InputParams par[1] = {std::make_tuple(means, covs, lambda, color, p, true, std::vector<double> {0}, gt_alpha)};
632 | 	void* params = (void *)par;
633 | 	std::vector<double> x_n, x_n1;
634 | 	double diff;
635 | 	x_n = x_0;
636 | 	int count = 0;
637 | 	int iter = 0;
638 | 	do{
639 | 		//std::cout << "lambda: " << lambda.val[0] << std::endl;
640 | 		//std::cout << "p: " << p << std::endl;
641 | 		if(count== 0){
642 | 			x_n1 = minimizeFCG( x_n, f, df,  means,covs, color, p, lambda, gt_alpha);
643 | 			count++;
644 | 		}else{
645 | 
646 | 			lambda += p*g(x_n1,n,color); 
647 | 			if(cv::norm(g(x_n1,n,color), cv::NORM_L2) > constants::gamma*cv::norm(g(x_n,n,color), cv::NORM_L2)){
648 | 				p = constants::beta * p;
649 | 			}
650 | 			par[0] = std::make_tuple(means, covs, lambda, color, p, true, std::vector<double> {0}, gt_alpha);
651 | 			params = (void *)par;
652 | 			x_n1 = minimizeFCG( x_n, f, df,  means,covs, color, p, lambda, gt_alpha);
653 | 		}
654 | 		iter ++;
655 | 		diff = cv::norm(x_n,x_n1,cv::NORM_L2);
656 | 		x_n = x_n1;
657 | 		
658 | 	}while((diff < 0.0001 && iter < 11) ||(diff > 0.0001));	//mg found 11 was a good number for the minimum number of iterations needed to make sure constraints were imposed
659 | 	std::cout << "MoM iter: " << iter << std::endl;
660 | 	return x_n;
661 | }
662 | 
663 | 
664 | 
665 | 


--------------------------------------------------------------------------------
/src/Pixel.cpp:
--------------------------------------------------------------------------------
  1 | #include "Pixel.h"
  2 | 
  3 | Pixel::Pixel(cv::Vec3d color, cv::Point coord){
  4 | 	this->color = color;
  5 | 	this->coord = coord;
  6 | }
  7 | 
  8 | // Get the index of the mean that represents the pixel the closest
  9 | int Pixel::minIndex(cv::Vec3d color, std::vector<cv::Vec3d> &means, std::vector<cv::Matx33d> &covs){
 10 | 	std::vector<double> distances;
 11 | 	double dist;
 12 | 	int n = means.size();
 13 | 	for(size_t i = 0; i < n; i++){
 14 | 		dist = pow(cv::Mahalanobis(color, means.at(i), (covs.at(i)).inv()),2);
 15 | 		distances.push_back(dist);
 16 | 	}
 17 | 	return min_element(distances.begin(), distances.end()) - distances.begin();
 18 | }
 19 | 
 20 | /** Unmix the color of the pixel based on the means and covs
 21 |  *  means: vector of Vec3d of mean values for the colors in the model
 22 |  *  covs: covs of the Matx33d of the inverse covariances of the colors in the model
 23 |  * 	x_init layers initialisation thanks to layer n-1 and optical flow, null if first frame
 24 |  */
 25 | Unmixing Pixel::unmix(std::vector<cv::Vec3d> &means, std::vector<cv::Matx33d> &covs,
 26 | 	std::vector<double> x_init)
 27 | {
 28 | 	int n = means.size(); //number of layers
 29 | 	std::vector<double> x_min(4*n);
 30 | 
 31 | 
 32 | 	int mI = minIndex(this->color, means, covs); //idx of mean that represents the pixel the closest
 33 | 	std::vector<double> x(4*n); //create vector of zeros of size 4*n
 34 | 	// Create initial x-vector for minimization
 35 | 	for(size_t i = 0; i < n; i++){
 36 | 		x.at(i) = 0;
 37 | 		x.at(3*i+n) =  means.at(i).val[0];
 38 | 		x.at(3*i+n+1) = means.at(i).val[1];
 39 | 		x.at(3*i+n+2) = means.at(i).val[2];
 40 | 	}
 41 | 
 42 | 	//intialise layer most representing the pixel to opaque and to pixel colour
 43 | 	x.at(mI) = 1;
 44 | 	x.at(3*mI+n) = this->color.val[0];
 45 | 	x.at(3*mI+n+1) = this->color.val[1];
 46 | 	x.at(3*mI+n+2) = this->color.val[2];
 47 | 	
 48 | 	x_min = minimizeMofM(x, (vFunctionCall)min_f, (vFunctionCall2)min_df, means, covs, this->color, std::vector<double> {0} );
 49 | 
 50 | 	// Format results
 51 | 	std::vector<double> alphas;
 52 | 	std::vector<cv::Vec3d> colors;
 53 | 	double u1, u2, u3;
 54 | 	for(size_t i = 0; i < n; i++){
 55 | 		alphas.push_back(x_min.at(i));
 56 | 		u1 = x_min.at(3*i+n);
 57 | 		u2 = x_min.at(3*i+n+1);
 58 | 		u3 = x_min.at(3*i+n+2);
 59 | 		colors.push_back(cv::Vec3d(u1,u2,u3));
 60 | 	}
 61 | 
 62 | 	Unmixing res;
 63 | 	res.alphas = alphas;
 64 | 	res.colors = colors;
 65 | 	res.coords = this->coord;
 66 | 
 67 | 
 68 | 	return res;
 69 | }
 70 | 
 71 | 
 72 | /***********************************************
 73 |  * This is the final color refinement step, with the new alpha constraint (Equation 6 of unmixing paper)
 74 |  * ********************************************/
 75 | Unmixing Pixel::refine(std::vector<cv::Vec3d> &means, std::vector<cv::Matx33d> &covs,
 76 | 	std::vector<double> x_init)
 77 | {
 78 | 	int n = means.size(); //number of layers
 79 | 	std::vector<double> x_min(4*n);
 80 | 
 81 | 	std::vector<double> gt_alphas(n);// save ground truth alphas to variable
 82 | 	for(int i = 0; i < n; i++){
 83 | 		gt_alphas.at(i) = x_init.at(i);
 84 | 	}
 85 | 
 86 | 		
 87 | 	x_min = minimizeMofM(x_init, (vFunctionCall)min_refine_f, (vFunctionCall2)min_refine_df, means, covs,this->color, gt_alphas);
 88 | 
 89 | 	// Format results
 90 | 	std::vector<double> alphas;
 91 | 	std::vector<cv::Vec3d> colors;
 92 | 	double u1, u2, u3;
 93 | 	for(size_t i = 0; i < n; i++){
 94 | 		alphas.push_back(x_min.at(i));
 95 | 		u1 = x_min.at(3*i+n);
 96 | 		u2 = x_min.at(3*i+n+1);
 97 | 		u3 = x_min.at(3*i+n+2);
 98 | 		colors.push_back(cv::Vec3d(u1,u2,u3));
 99 | 	}
100 | 
101 | 	Unmixing res;
102 | 	res.alphas = alphas;
103 | 	res.colors = colors;
104 | 	res.coords = this->coord;
105 | 
106 | 	return res;
107 | }
108 | 
109 | 


--------------------------------------------------------------------------------
/src/guidedfilter.cpp:
--------------------------------------------------------------------------------
  1 | #include "guidedfilter.h"
  2 | 
  3 | static cv::Mat boxfilter(const cv::Mat &I, int r)
  4 | {
  5 |     cv::Mat result;
  6 |     cv::blur(I, result, cv::Size(r, r));
  7 |     return result;
  8 | }
  9 | 
 10 | static cv::Mat convertTo(const cv::Mat &mat, int depth)
 11 | {
 12 |     if (mat.depth() == depth)
 13 |         return mat;
 14 | 
 15 |     cv::Mat result;
 16 |     mat.convertTo(result, depth);
 17 |     return result;
 18 | }
 19 | 
 20 | class GuidedFilterImpl
 21 | {
 22 | public:
 23 |     virtual ~GuidedFilterImpl() {}
 24 | 
 25 |     cv::Mat filter(const cv::Mat &p, int depth);
 26 | 
 27 | protected:
 28 |     int Idepth;
 29 | 
 30 | private:
 31 |     virtual cv::Mat filterSingleChannel(const cv::Mat &p) const = 0;
 32 | };
 33 | 
 34 | class GuidedFilterMono : public GuidedFilterImpl
 35 | {
 36 | public:
 37 |     GuidedFilterMono(const cv::Mat &I, int r, double eps);
 38 | 
 39 | private:
 40 |     virtual cv::Mat filterSingleChannel(const cv::Mat &p) const;
 41 | 
 42 | private:
 43 |     int r;
 44 |     double eps;
 45 |     cv::Mat I, mean_I, var_I;
 46 | };
 47 | 
 48 | class GuidedFilterColor : public GuidedFilterImpl
 49 | {
 50 | public:
 51 |     GuidedFilterColor(const cv::Mat &I, int r, double eps);
 52 | 
 53 | private:
 54 |     virtual cv::Mat filterSingleChannel(const cv::Mat &p) const;
 55 | 
 56 | private:
 57 |     std::vector<cv::Mat> Ichannels;
 58 |     int r;
 59 |     double eps;
 60 |     cv::Mat mean_I_r, mean_I_g, mean_I_b;
 61 |     cv::Mat invrr, invrg, invrb, invgg, invgb, invbb;
 62 | };
 63 | 
 64 | 
 65 | cv::Mat GuidedFilterImpl::filter(const cv::Mat &p, int depth)
 66 | {
 67 |     cv::Mat p2 = convertTo(p, Idepth);
 68 | 
 69 |     cv::Mat result;
 70 |     if (p.channels() == 1)
 71 |     {
 72 |         result = filterSingleChannel(p2);
 73 |     }
 74 |     else
 75 |     {
 76 |         std::vector<cv::Mat> pc;
 77 |         cv::split(p2, pc);
 78 | 
 79 |         for (std::size_t i = 0; i < pc.size(); ++i)
 80 |             pc[i] = filterSingleChannel(pc[i]);
 81 | 
 82 |         cv::merge(pc, result);
 83 |     }
 84 | 
 85 |     return convertTo(result, depth == -1 ? p.depth() : depth);
 86 | }
 87 | 
 88 | GuidedFilterMono::GuidedFilterMono(const cv::Mat &origI, int r, double eps) : r(r), eps(eps)
 89 | {
 90 |     if (origI.depth() == CV_32F || origI.depth() == CV_64F)
 91 |         I = origI.clone();
 92 |     else
 93 |         I = convertTo(origI, CV_32F);
 94 | 
 95 |     Idepth = I.depth();
 96 | 
 97 |     mean_I = boxfilter(I, r);
 98 |     cv::Mat mean_II = boxfilter(I.mul(I), r);
 99 |     var_I = mean_II - mean_I.mul(mean_I);
100 | }
101 | 
102 | cv::Mat GuidedFilterMono::filterSingleChannel(const cv::Mat &p) const
103 | {
104 |     cv::Mat mean_p = boxfilter(p, r);
105 |     cv::Mat mean_Ip = boxfilter(I.mul(p), r);
106 |     cv::Mat cov_Ip = mean_Ip - mean_I.mul(mean_p); // this is the covariance of (I, p) in each local patch.
107 | 
108 |     cv::Mat a = cov_Ip / (var_I + eps); // Eqn. (5) in the paper;
109 |     cv::Mat b = mean_p - a.mul(mean_I); // Eqn. (6) in the paper;
110 | 
111 |     cv::Mat mean_a = boxfilter(a, r);
112 |     cv::Mat mean_b = boxfilter(b, r);
113 | 
114 |     return mean_a.mul(I) + mean_b;
115 | }
116 | 
117 | GuidedFilterColor::GuidedFilterColor(const cv::Mat &origI, int r, double eps) : r(r), eps(eps)
118 | {
119 |     cv::Mat I;
120 |     if (origI.depth() == CV_32F || origI.depth() == CV_64F)
121 |         I = origI.clone();
122 |     else
123 |         I = convertTo(origI, CV_32F);
124 | 
125 |     Idepth = I.depth();
126 | 
127 |     cv::split(I, Ichannels);
128 | 
129 |     mean_I_r = boxfilter(Ichannels[0], r);
130 |     mean_I_g = boxfilter(Ichannels[1], r);
131 |     mean_I_b = boxfilter(Ichannels[2], r);
132 | 
133 |     // variance of I in each local patch: the matrix Sigma in Eqn (14).
134 |     // Note the variance in each local patch is a 3x3 symmetric matrix:
135 |     //           rr, rg, rb
136 |     //   Sigma = rg, gg, gb
137 |     //           rb, gb, bb
138 |     cv::Mat var_I_rr = boxfilter(Ichannels[0].mul(Ichannels[0]), r) - mean_I_r.mul(mean_I_r) + eps;
139 |     cv::Mat var_I_rg = boxfilter(Ichannels[0].mul(Ichannels[1]), r) - mean_I_r.mul(mean_I_g);
140 |     cv::Mat var_I_rb = boxfilter(Ichannels[0].mul(Ichannels[2]), r) - mean_I_r.mul(mean_I_b);
141 |     cv::Mat var_I_gg = boxfilter(Ichannels[1].mul(Ichannels[1]), r) - mean_I_g.mul(mean_I_g) + eps;
142 |     cv::Mat var_I_gb = boxfilter(Ichannels[1].mul(Ichannels[2]), r) - mean_I_g.mul(mean_I_b);
143 |     cv::Mat var_I_bb = boxfilter(Ichannels[2].mul(Ichannels[2]), r) - mean_I_b.mul(mean_I_b) + eps;
144 | 
145 |     // Inverse of Sigma + eps * I
146 |     invrr = var_I_gg.mul(var_I_bb) - var_I_gb.mul(var_I_gb);
147 |     invrg = var_I_gb.mul(var_I_rb) - var_I_rg.mul(var_I_bb);
148 |     invrb = var_I_rg.mul(var_I_gb) - var_I_gg.mul(var_I_rb);
149 |     invgg = var_I_rr.mul(var_I_bb) - var_I_rb.mul(var_I_rb);
150 |     invgb = var_I_rb.mul(var_I_rg) - var_I_rr.mul(var_I_gb);
151 |     invbb = var_I_rr.mul(var_I_gg) - var_I_rg.mul(var_I_rg);
152 | 
153 |     cv::Mat covDet = invrr.mul(var_I_rr) + invrg.mul(var_I_rg) + invrb.mul(var_I_rb);
154 | 
155 |     invrr /= covDet;
156 |     invrg /= covDet;
157 |     invrb /= covDet;
158 |     invgg /= covDet;
159 |     invgb /= covDet;
160 |     invbb /= covDet;
161 | }
162 | 
163 | cv::Mat GuidedFilterColor::filterSingleChannel(const cv::Mat &p) const
164 | {
165 |     cv::Mat mean_p = boxfilter(p, r);
166 | 
167 |     cv::Mat mean_Ip_r = boxfilter(Ichannels[0].mul(p), r);
168 |     cv::Mat mean_Ip_g = boxfilter(Ichannels[1].mul(p), r);
169 |     cv::Mat mean_Ip_b = boxfilter(Ichannels[2].mul(p), r);
170 | 
171 |     // covariance of (I, p) in each local patch.
172 |     cv::Mat cov_Ip_r = mean_Ip_r - mean_I_r.mul(mean_p);
173 |     cv::Mat cov_Ip_g = mean_Ip_g - mean_I_g.mul(mean_p);
174 |     cv::Mat cov_Ip_b = mean_Ip_b - mean_I_b.mul(mean_p);
175 | 
176 |     cv::Mat a_r = invrr.mul(cov_Ip_r) + invrg.mul(cov_Ip_g) + invrb.mul(cov_Ip_b);
177 |     cv::Mat a_g = invrg.mul(cov_Ip_r) + invgg.mul(cov_Ip_g) + invgb.mul(cov_Ip_b);
178 |     cv::Mat a_b = invrb.mul(cov_Ip_r) + invgb.mul(cov_Ip_g) + invbb.mul(cov_Ip_b);
179 | 
180 |     cv::Mat b = mean_p - a_r.mul(mean_I_r) - a_g.mul(mean_I_g) - a_b.mul(mean_I_b); // Eqn. (15) in the paper;
181 | 
182 |     return (boxfilter(a_r, r).mul(Ichannels[0])
183 |           + boxfilter(a_g, r).mul(Ichannels[1])
184 |           + boxfilter(a_b, r).mul(Ichannels[2])
185 |           + boxfilter(b, r));  // Eqn. (16) in the paper;
186 | }
187 | 
188 | 
189 | GuidedFilter::GuidedFilter(const cv::Mat &I, int r, double eps)
190 | {
191 |     CV_Assert(I.channels() == 1 || I.channels() == 3);
192 | 
193 |     if (I.channels() == 1)
194 |         impl_ = new GuidedFilterMono(I, 2 * r + 1, eps);
195 |     else
196 |         impl_ = new GuidedFilterColor(I, 2 * r + 1, eps);
197 | }
198 | 
199 | GuidedFilter::~GuidedFilter()
200 | {
201 |     delete impl_;
202 | }
203 | 
204 | cv::Mat GuidedFilter::filter(const cv::Mat &p, int depth) const
205 | {
206 |     return impl_->filter(p, depth);
207 | }
208 | 
209 | cv::Mat guidedFilter(const cv::Mat &I, const cv::Mat &p, int r, double eps, int depth)
210 | {
211 |     return GuidedFilter(I, r, eps).filter(p, depth);
212 | }
213 | 
214 | /**
215 |  * Do the matte regularisation step from Aksoy et al. paper, section 3
216 |  * "Unmixing-Based Soft Color Segmentation for Image Manipulation" [2016]
217 |  * 1. Apply the guided filter to the current given a frame set of layers
218 |  *    following the guide image guide_img, (use current frame as guide image)
219 |  * 2. Regularise resulting alpha values so that they add up to one.
220 |  * Return the regularised layers
221 |  * */
222 | std::vector<cv::Mat> MatteRegularisation(int radius, cv::Mat guide_img, std::vector<cv::Mat> layers)
223 | {
224 |     int n = layers.size(); //number of colours in CM
225 |     std::vector<cv::Mat> filtered_layers; //filtered using guided filter
226 |     std::vector<cv::Mat> regularised_layers; //regularised using matte regularisation
227 | 
228 |     cv::Mat sum_filtered_alphas(guide_img.rows, guide_img.cols, CV_32FC1, cvScalar(0.0));
229 |     int r = radius;
230 |     double eps = .001; 
231 |     eps *= 255 * 255;
232 |     for(size_t i = 0; i < n; i++){
233 |         //initialise
234 |         cv::Mat layer = layers.at(i);
235 |         cv::Mat layer_split[4];
236 |         cv::Mat ALayer8U, fLayer, guide_img8U;
237 |         cv::Mat ALayer(guide_img.size(),CV_32FC1); //for alpha layer
238 |         cv::Mat fLayer8U(guide_img.size(),CV_8UC1); 
239 | 
240 |         cv::split(layer, layer_split); //split RGBA channels in layers
241 |         layer_split[3].convertTo(ALayer, CV_32FC1); //convert to float 
242 |         ALayer.convertTo(ALayer8U, CV_8UC1);//convert to uint
243 |         guide_img.convertTo(guide_img8U, CV_8UC3); //convert imput image to uint
244 |         cv::ximgproc::guidedFilter(guide_img8U,ALayer8U,fLayer8U,r,eps,-1); //filter uint alpha layer with uint input image (filtering uint to prevent negative alpha values/decimals)
245 |         fLayer8U.convertTo(fLayer, CV_32FC1);//convert filtered layer back to float so we can manipulate values so they add to 1 (or 255).
246 |         cv::addWeighted(sum_filtered_alphas,1.0,fLayer,1.0/255.0,0,sum_filtered_alphas); //sum all of the filtered alpha layers - this will be used to normalise 
247 |         filtered_layers.push_back(fLayer);
248 |     }	
249 | 
250 |     // Regularize alpha values so that they add up to one
251 |     for(size_t i = 0; i < n; i++){
252 |         cv::Mat orig_layer = layers.at(i); //these is the original 4D layers - with colour and alpha info
253 |         cv::Mat fillayer = filtered_layers.at(i); // these layers are the new filtered 1D alpha layers
254 |         cv::Mat reg_alpha(guide_img.rows, guide_img.cols, CV_32FC1, cvScalar(0.0));
255 |         //regularise the alpha values so they all add to 1 (or 255, depending on scaling)
256 |         for(size_t i = 0; i < guide_img.rows; i++){
257 |             for(size_t j = 0; j < guide_img.cols; j++){
258 |                 reg_alpha.at<float>(i,j) = (1.0/sum_filtered_alphas.at<float>(i,j))*(fillayer.at<float>(i,j));
259 |             }
260 |         }
261 |         
262 |         //transfer these regularized alpha layers to our full colour+alpha layers
263 |         cv::Mat bgra[4];
264 |         cv::Mat bgra2[4];
265 |         cv::Mat reg_layer(guide_img.rows, guide_img.cols, CV_32FC4);
266 |         cv::split(reg_layer,bgra);
267 |         cv::split(orig_layer,bgra2); //split original layers so we can access colour layers
268 |         
269 |         //create new layers with colour and filtered regularised alphas
270 |         bgra2[0].convertTo(bgra[0],bgra[0].type()); //add b to bgra
271 |         bgra2[1].convertTo(bgra[1],bgra[0].type());//add g to bgra
272 |         bgra2[2].convertTo(bgra[2],bgra[0].type());//add r to bgra
273 |         reg_alpha.convertTo(bgra[3],bgra[0].type()); //add a to bgra
274 |         cv::merge(bgra,4,reg_layer);
275 | 
276 |         reg_layer.convertTo(reg_layer, CV_8UC4);
277 |         regularised_layers.push_back(reg_layer);
278 |     }
279 | 
280 |     return regularised_layers;
281 | }


--------------------------------------------------------------------------------
/src/main.cpp:
--------------------------------------------------------------------------------
  1 | #include <opencv2/opencv.hpp>
  2 | #include "Pixel.h"
  3 | #include "Unmixing.h"
  4 | #include "ThreadPool.h"
  5 | #include <chrono>
  6 | #include <vector>
  7 | #include <string>
  8 | #include "guidedfilter.h"
  9 | #include "ColorModel.h"
 10 | #include <sys/stat.h> 
 11 | 
 12 | 
 13 | int makeDirectories(std::string result_folder_name)
 14 | {
 15 |     //create folder to save output layers and videos cleanly
 16 |     int status;
 17 |     status = mkdir(result_folder_name.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
 18 | 	if( status != 0){
 19 | 		std::cout << "Error mkdir: " <<strerror(errno)<<std::endl;
 20 | 	    return EXIT_FAILURE;
 21 | 	}
 22 |     result_folder_name += "/";
 23 | 	status = mkdir((result_folder_name +"output_layers").c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
 24 | 	if( status != 0){
 25 | 		std::cout << "Error mkdir: " <<strerror(errno)<<std::endl;
 26 | 	    return EXIT_FAILURE;
 27 | 	}
 28 | 	status = mkdir((result_folder_name +"sum_frames").c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
 29 | 	if( status != 0){
 30 | 		std::cout << "Error mkdir: " <<strerror(errno)<<std::endl;
 31 | 	    return EXIT_FAILURE;
 32 | 	}
 33 |     return EXIT_SUCCESS;
 34 | }
 35 | 
 36 | void saveLayers(std::string dir, std::vector<cv::Mat> layers, std::string step)
 37 | {
 38 |     for (int l = 0; l<layers.size(); l++)
 39 |         {
 40 |             std::string l_str = std::string(2 - std::to_string(l).length(), '0') + std::to_string(l);
 41 |             imwrite(dir + "/output_layers/" + step + l_str + ".png",
 42 |                 layers[l]);
 43 |         }
 44 | }
 45 | 
 46 | void sumLayers(cv::Mat sum, std::vector<cv::Mat> layers)
 47 | {
 48 | 	int n = layers.size();
 49 | 
 50 | 	//for every pixel
 51 | 	for(size_t i = 0; i < sum.rows; i++)
 52 | 	{
 53 | 		for(size_t j = 0; j < sum.cols; j++)
 54 | 		{
 55 | 			double alpha_unit;
 56 | 			double sum_u1 = 0.0, sum_u2 = 0.0, sum_u3 = 0.0, sum_alpha = 0.0;
 57 | 			cv::Vec4b& pix = sum.at<cv::Vec4b>(i,j);
 58 | 			//for every layer
 59 | 			for(size_t l = 0; l < n; l++){
 60 | 				cv::Mat layer = layers.at(l);
 61 | 				cv::Vec4b v = layer.at<cv::Vec4b>(i,j);
 62 | 
 63 | 				alpha_unit = v[3] / 255.0;
 64 | 				sum_u1 += alpha_unit*v[0]; //B_i * alpha_i
 65 | 				sum_u2 += alpha_unit*v[1]; //G_i * alpha_i
 66 | 				sum_u3 += alpha_unit*v[2]; //R_i * alpha_i
 67 | 				sum_alpha += v[3];
 68 | 			}
 69 | 
 70 | 			// clip because 256 becomes 1 in uchar
 71 | 			pix[0] = (sum_u1>255)?255:sum_u1;
 72 | 			pix[1] = (sum_u2>255)?255:sum_u2;
 73 | 			pix[2] = (sum_u3>255)?255:sum_u3;
 74 | 			pix[3] = (sum_alpha>255)?255:sum_alpha;
 75 | 		}
 76 | 	}
 77 | }
 78 | 
 79 | 
 80 | 
 81 | int main( int argc, char** argv )
 82 | {
 83 | 	double tau;
 84 | 	if(argc == 1){
 85 | 		std::cout << "Usage: ./SoftSegmentation <path/to/image/> <tau_parameterer(optional)> <output_dir>" << std::endl; 
 86 | 		return -1;
 87 | 	}else if(argc == 2){
 88 | 		tau = constants::tau;
 89 | 	}else {
 90 | 		tau = std::stod(argv[2]);
 91 | 	}
 92 | 
 93 |     std::vector<cv::Vec3d> means;
 94 |     std::vector<cv::Matx33d> covs;
 95 | 
 96 |     // Load image
 97 | 	char* imageName = argv[1];
 98 |     char* output_dir_name = argv[3];
 99 | 	std::string imageName_short(imageName);//mg_added
100 |   	std::string imageName_s(imageName_short.begin()+3, imageName_short.end()-4);
101 | 	std::string dir_name(output_dir_name);
102 | 
103 | 	//create directories that we'll save images into
104 | 	if(makeDirectories(std::string("../") + dir_name) != 0){
105 |         std::cout <<"Error making directories"<<std::endl;
106 |         return false;
107 |     }
108 | 
109 | 	  
110 | 	//Read input image and write it to output directory
111 | 	cv::Mat image;
112 | 	image = cv::imread(imageName, 1);
113 | 	cv::imwrite(std::string("../") + dir_name + "/input_image.png", image); //save input image for reference
114 | 	if( !image.data ){
115 | 	   printf( " No image data \n " );
116 | 	   return -1;
117 | 	}
118 | 
119 | 
120 | 	/*************************************************************
121 | 	 *            To Begin: Compute Global Color Model             *
122 | 	 *************************************************************/
123 | 	//create terminal output
124 | 	std::cout << "" << std::endl;
125 | 	std::cout << "Colour Model Estimation Step..." << std::endl;
126 | 	std::cout << "" << std::endl;
127 | 
128 | 
129 |     auto t_start_CM = std::chrono::high_resolution_clock::now();
130 | 
131 |     means.clear();
132 |     covs.clear();
133 | 
134 |     std::cout << "Calculating Global Color Model with tau = " << tau<<"."<< std::endl;
135 |             
136 |     //convert video_frames from CV_U8C3 to CV_64FC3 
137 |     cv::Mat img;
138 | 	image.convertTo(img,CV_64F,1.0/255.0);
139 | 
140 | 	//compute colour model
141 |     getGlobalColorModel(img, means, covs, tau);
142 | 
143 | 	//output to terminal and save image
144 |     std::cout << "Found " << means.size() << " colors." << std::endl;
145 |     saveColorModelAsImage((std::string("../") + dir_name + "/CM" + ".png").c_str(), means, covs);
146 | 
147 | 	std::cout << "" << std::endl;
148 | 	std::cout << "Colour Model Estimation Step: Done. " << std::endl;
149 |     std::cout << "" << std::endl;
150 | 	//output mean and covariances for debugging
151 |     //std::cout << "Means:"<<std::endl;
152 |     //for (size_t i=0; i< means.size(); i++){
153 |     //   std::cout << "mean :" << means[i] <<std::endl;
154 |     //    std::cout << "cov : " << covs[i] <<std::endl;
155 |     //}
156 | 
157 | 	///////////////////////////////////////////////// end of step one
158 | 
159 | 
160 | 
161 | 	/*************************************************************
162 | 	*               Step 1. Sparse Colour Unmixing Step              *
163 |     *************************************************************/
164 | 
165 |     std::cout << "Step 1/3: Colour Unmixing..." << std::endl;
166 | 	std::cout << "" << std::endl;
167 |    	//initialise variables
168 | 	int n = means.size(); //nb of layers
169 | 	int rows = image.rows;
170 | 	int cols = image.cols;
171 |     cv::Vec3d color;
172 | 	std::vector<cv::Mat> layers;
173 | 	for(size_t i = 0; i < n; i++){
174 | 		cv::Mat layer(image.rows, image.cols, CV_8UC4);
175 | 		layers.push_back(layer);
176 | 	}
177 | 
178 | 
179 | 
180 |     // Create Thread pool
181 | 	int num_thread = 8;
182 |     //std::cout << "Using Thread Pool with " << num_thread << " threads." << std::endl;
183 |     ThreadPool pool(num_thread); 
184 |     std::vector<std::future<Unmixing>> results; //where we will store unmixing results
185 | 
186 | 
187 |     //for each frame unmixing duration
188 | 	//auto t_start_unmix = std::chrono::high_resolution_clock::now(); //which is also t_end_flow
189 | 
190 |     // Parse image and add one task per pixel to thread pool
191 |     for(size_t i = 0; i < image.rows; i++){
192 |         for(size_t j = 0; j < image.cols; j++){
193 | 
194 |             color = image.at<cv::Vec3b>(i,j);
195 |             color = color/255;
196 |             Pixel p = Pixel(color, cv::Point(j,i));
197 |                 
198 |             std::vector<double> x_init(0); //create vector of zeros of size 0
199 |             results.emplace_back(pool.enqueue( 
200 |             [](Pixel p, std::vector<cv::Vec3d> means, std::vector<cv::Matx33d> covs,std::vector<double> x_init)
201 |             {return p.unmix(means, covs, x_init);}, p, means, covs, x_init));
202 |         }
203 |     }
204 | 
205 | 	// Get results from thread pool and create layers
206 | 	std::cout << "Percentage complete:"  << std::endl;
207 |     int num = 0;
208 |     int transp_pix_sup = 0, transp_pix_inf = 0;
209 |     float progress = 0.0;
210 |     int barWidth = 70;
211 |     for(auto && result : results)
212 |     {
213 |         num++;
214 |         //show progress percentage
215 |         progress = float(num)/float(cols*rows);
216 |         std::cout<< int(progress * 100.0) << " %\r";
217 |         std::cout.flush();
218 | 
219 |         // Create layers from results
220 |         int sum_alpha = 0; //verify alpha constraint
221 |         Unmixing u = result.get(); 
222 |             
223 |         for(size_t i = 0; i < n; i++){
224 |             cv::Mat layer = layers.at(i);
225 |             cv::Vec4b& v = layer.at<cv::Vec4b>(u.coords);
226 | 
227 |             v[0] = u.colors.at(i).val[0]*255;
228 |             v[1] = u.colors.at(i).val[1]*255;
229 |             v[2] = u.colors.at(i).val[2]*255;
230 |             v[3] = u.alphas.at(i)*255; 
231 |             sum_alpha += floor(u.alphas.at(i)*255);
232 |         }
233 |     }
234 |     std::cout<< "100 %" <<std::endl;
235 | 	std::cout << "" << std::endl;
236 | 	std::cout<< "Step 1/3: Done." <<std::endl;
237 | 	std::cout << "" << std::endl;
238 | 
239 | 
240 |     //duration of layer filtering
241 |     auto t_start_filters = std::chrono::high_resolution_clock::now();
242 | 
243 | 
244 |     // save images
245 |     saveLayers((std::string("../") + dir_name).c_str(),  layers, "step1_");
246 | 
247 | 
248 | 
249 | 	/*************************************************************
250 |     *               Step 2. Matte Regularization Step                *
251 |     *************************************************************/
252 |     std::cout << "Step 2/3: Matte Regularisation..."   << std::endl;
253 | 	//Matte regularisation with guided filter
254 | 	std::vector<cv::Mat> output_layers;
255 | 	int r = ((60.0/(sqrt(1000000.0/(cols*rows))))+1.0); //radius for filter based on one used in unmixing paper
256 |     std::cout << "Filter radius has size: " << r <<std::endl; 
257 | 
258 |     output_layers = MatteRegularisation(r, image, layers); //filter and regularise the alpha values
259 | 
260 |     saveLayers((std::string("../") + dir_name).c_str(),  output_layers, "step2_");
261 | 	
262 | 	std::cout << "" << std::endl;
263 | 	std::cout << "Step 2/3: Done."  << std::endl;
264 | 	std::cout << "" << std::endl;
265 | 
266 | 
267 | 
268 | 	/*************************************************************
269 |     *               3. Colour Refinement Step         	         *
270 |     *************************************************************/
271 |     std::cout << "Step 3/3: Colour Refinement..."  << std::endl;
272 | 	std::cout << "" << std::endl;
273 | 
274 | 
275 |     //std::cout << "Using Thread Pool with " << num_thread << " threads." << std::endl;
276 |     ThreadPool pool_r(num_thread);
277 |     std::vector<std::future<Unmixing>> refine_results;
278 | 
279 |     // Parse image and add one task per pixel to thread pool
280 |     for(size_t i = 0; i < rows; i++){
281 |        for(size_t j = 0; j < cols; j++){
282 | 
283 |             color = image.at<cv::Vec3b>(i,j);
284 |             color = color/255;
285 |             Pixel p = Pixel(color, cv::Point(j,i));
286 |                 
287 |             std::vector<double> x_init_r(4*n);
288 |             for(size_t lay = 0; lay < n; lay++){ //for each layer of this frame, initialise colours 
289 |                 x_init_r.at(lay) = output_layers[lay].at<cv::Vec4b>(i,j)[3] / 255.0f;
290 |                 x_init_r.at(3*lay+n) =  output_layers[lay].at<cv::Vec4b>(i,j)[0] / 255.0f;
291 |                 x_init_r.at(3*lay+n+1) = output_layers[lay].at<cv::Vec4b>(i,j)[1] / 255.0f;
292 |                 x_init_r.at(3*lay+n+2) = output_layers[lay].at<cv::Vec4b>(i,j)[2] / 255.0f;
293 |             }
294 |             refine_results.emplace_back(pool_r.enqueue( 
295 |             [](Pixel p, std::vector<cv::Vec3d> means, std::vector<cv::Matx33d> covs,std::vector<double> x_init_r)
296 |             {return p.refine(means, covs, x_init_r);}, p, means, covs, x_init_r));
297 |         }
298 |     }
299 |         
300 |     // Get results from thread pool and create layers
301 |     num = 0;
302 |     transp_pix_sup = 0;
303 | 	transp_pix_inf = 0;
304 |     progress = 0.0;
305 |     barWidth = 70;
306 | 	std::cout << "Percentage complete:"  << std::endl;
307 |     for(auto && result : refine_results)
308 |     {
309 |         num++;
310 |         //show progress percentage
311 |         progress = float(num)/float(cols*rows);
312 |         std::cout<< int(progress * 100.0) << " %\r";
313 |         std::cout.flush();
314 | 
315 |         // Create layers from results
316 |         int sum_alpha = 0; //verify alpha constraint
317 |         Unmixing u = result.get();
318 |             
319 |         for(size_t i = 0; i < n; i++){
320 |     		cv::Mat layer = layers.at(i);
321 |         	cv::Vec4b& v = layer.at<cv::Vec4b>(u.coords);
322 |             v[0] = u.colors.at(i).val[0]*255;
323 |             v[1] = u.colors.at(i).val[1]*255;
324 |             v[2] = u.colors.at(i).val[2]*255;
325 |             v[3] = u.alphas.at(i)*255; 
326 |             sum_alpha += floor(u.alphas.at(i)*255);
327 |         }
328 |         //std::cout <<   "sum_alpha" << sum_alpha << std::endl;
329 |     }
330 |     std::cout<< "100 %" <<std::endl;
331 | 	std::cout << "" << std::endl;
332 | 	std::cout<< "Step 3/3: Done. " <<std::endl;
333 | 	std::cout << "" << std::endl;
334 | 	/*************************************************************
335 |     *                  Save Final Layers          	         *
336 |     *************************************************************/
337 |     //compute sum of layers
338 | 	cv::Mat sum_layers(image.rows, image.cols, CV_8UC4, 0.0);
339 |     sumLayers(sum_layers, layers);
340 | 
341 |     //save all layers as images
342 |     saveLayers((std::string("../") + dir_name).c_str(),  layers, "FinalLayers_" ); 
343 | 
344 | 	//save the sum of the layers as an image
345 |     cv::imwrite((std::string("../") + dir_name).c_str() + std::string("/sum_frames/sum_frame.png"), sum_layers);
346 | 
347 | 	std::cout<< "Final layers saved as images. " <<std::endl;
348 | 
349 | 	/************************************************************/
350 | 
351 | 
352 | 	return 0;
353 | }
354 | 


--------------------------------------------------------------------------------