├── .gitignore
├── ISMAR_Demo
    ├── main.cpp
    └── parameter
    │   ├── parametersDescriptorFieldsOptimization.yml
    │   ├── parametersGradientMagnitudeOptimization.yml
    │   └── parametersSSDOptimization.yml
├── LICENSE
├── Makefile
├── Makefile.inc
├── README
├── include
    ├── Homography.hpp
    ├── HomographyEstimation.hpp
    ├── IterativeOptimization.hpp
    ├── Typedefs.hpp
    └── Utilities.hpp
├── src
    ├── Homography.cpp
    ├── IterativeOptimization.cpp
    └── Utilities.cpp
└── unit
    ├── UnitTests.cpp
    ├── UnitTests.hpp
    ├── data
        ├── SDImage.txt
        ├── imageCropped.jpg
        ├── imageDx.txt
        ├── imageDy.txt
        ├── jacobian.txt
        ├── sample1.mov
        ├── template.png
        └── warpedIntensities.txt
    └── parameter
        └── parametersForTest.yml


/.gitignore:
--------------------------------------------------------------------------------
1 | ISMAR_Demo/homographyDemo
2 | unit/unittests
3 | *~
4 | *.so
5 | 


--------------------------------------------------------------------------------
/ISMAR_Demo/main.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | Copyright 2014 Alberto Crivellaro, Ecole Polytechnique Federale de Lausanne (EPFL), Switzerland.
  3 | alberto.crivellaro@epfl.ch
  4 | 
  5 | terms of the GNU General Public License as published by the Free Software
  6 | Foundation; either version 2 of the License, or (at your option) any later
  7 | version.
  8 | 
  9 | This program is distributed in the hope that it will be useful, but WITHOUT ANY
 10 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
 11 | PARTICULAR PURPOSE. See the GNU General Public License for more details.
 12 | 
 13 | You should have received a copy of the GNU General Public License along with
 14 | this program; if not, write to the Free Software Foundation, Inc., 51 Franklin
 15 | Street, Fifth Floor, Boston, MA 02110-1301, USA
 16 |  */
 17 | 
 18 | 
 19 | 
 20 | #if defined(_OPENMP)
 21 | #include <omp.h>
 22 | #endif
 23 | 
 24 | #include <Eigen/Core>
 25 | #include <opencv2/calib3d/calib3d.hpp>
 26 | #include <opencv2/highgui/highgui.hpp>
 27 | #include <opencv2/imgproc/imgproc.hpp>
 28 | #include <opencv2/features2d/features2d.hpp>
 29 | #include <iostream>
 30 | #include <list>
 31 | #include <vector>
 32 | #include<stdio.h>
 33 | #include <chrono>
 34 | 
 35 | #include "HomographyEstimation.hpp"
 36 | 
 37 | using namespace std;
 38 | using namespace cv;
 39 | using namespace std::chrono;
 40 | 
 41 | 
 42 | #define B_VERBOSE_TRACKING 1
 43 | #define CAM_HEIGHT (480)
 44 | #define CAM_WIDTH (640)
 45 | 
 46 | void CalibrateVideoStream(VideoCapture capture);
 47 | 
 48 | int main(int ac, char ** av)
 49 | {
 50 | 	if (ac != 2)
 51 | 	{
 52 | 		cerr <<" Usage : ./homographyDemo <videoName>"<<endl;
 53 | 		cerr <<" <videoName> can be the video file name or a camera ID (0, 1, etc.) for live demo. "<<endl;
 54 | 		return 1;
 55 | 	}
 56 | 
 57 | 	cout<< " ***********************   Starting demo ... ************************************"<<endl;
 58 | 	cout<< "press 1 : intensities "<<endl;
 59 | 	cout<< "press 2 : gradient magnitude"<<endl;
 60 | 	cout<< "press 3 : descriptor fields"<<endl;
 61 | 	cout<< "press g : resets tracking"<<endl;
 62 | 	cout<< "press space : pause tracking"<<endl;
 63 | 	cout<< " *********************************************************************************"<<endl;
 64 | 
 65 | 	std::string arg = av[1];
 66 | 	VideoCapture capture(arg);
 67 | 	if (!capture.isOpened())
 68 | 		capture.open(atoi(arg.c_str()));
 69 | 	if (!capture.isOpened()) {
 70 | 		cerr << "Failed to open a video device or video file!\n" << endl;
 71 | 		return 1;
 72 | 	}
 73 | 	CalibrateVideoStream(capture);
 74 | 
 75 | 	return 0;
 76 | }
 77 | 
 78 | 
 79 | 
 80 | void CalibrateVideoStream(VideoCapture capture)
 81 | {
 82 | 
 83 | 	string window_name = "ISMAR Demo 2014";
 84 | 
 85 | 	namedWindow(window_name, CV_WINDOW_KEEPRATIO);
 86 | 
 87 | 	OptimizationType optimizationType = descriptorFields;
 88 | 	string method = string("Using Our DESCRIPTOR FIELDS!");
 89 | 
 90 | 	bool bResetOptimization(false);
 91 | 	OptimizationParameters optimizationParametersDescrFields = ReadOptimizationParametersFromXML("parameter/parametersDescriptorFieldsOptimization.yml");
 92 | 	OptimizationParameters optimizationParametersSSD = ReadOptimizationParametersFromXML("parameter/parametersSSDOptimization.yml");
 93 | 	OptimizationParameters optimizationParametersGradientModule = ReadOptimizationParametersFromXML("parameter/parametersGradientMagnitudeOptimization.yml");
 94 | 
 95 | 	OptimizationParameters* optimizationParameters(&optimizationParametersDescrFields);
 96 | 
 97 | 	cout<<endl<<endl<<*optimizationParameters<<endl;
 98 | 
 99 | 	LucasKanade optimization;
100 | 	AlignmentResults results;
101 | 	
102 | 	Mat templ, image, firstFrame, currentFrameRGB;
103 | 
104 | 
105 | //--------------------init traking of libalbe start here
106 | 	AcquireVGAGrayscaleImage(capture, templ);
107 | 
108 | 
109 | 	firstFrame = templ.clone();
110 | 	StructOfArray2di pixelsOnTemplate;
111 | 	if(optimizationParameters->bAdaptativeChoiceOfPoints)
112 | 		pixelsOnTemplate = CreateAnisotropicGridOfControlPoints(templ, optimizationParameters->nControlPointsOnEdge, optimizationParameters->borderThicknessHorizontal, optimizationParameters->borderThicknessVertical);
113 | 	else
114 | 		pixelsOnTemplate = CreateGridOfControlPoints(templ, optimizationParameters->nControlPointsOnEdge, optimizationParameters->borderThicknessHorizontal, optimizationParameters->borderThicknessVertical);
115 | 
116 | 	StructOfArray2di pixelsOnTemplateWarped = pixelsOnTemplate;
117 | 
118 | 	vector<float> parameters(8,0);
119 | 	vector<float> parametersInitialGuess(8,0);
120 | 
121 | 	StructOfArray2di warpedPixels(4);
122 | 	StructOfArray2di  panelCorners = GetRectangleCornersForAugmentation(optimizationParameters, templ.cols, templ.rows);
123 | //--------------------init traking of libalbe stop here
124 | 
125 | 	Matx33f homographyMatrix;
126 | 	high_resolution_clock::time_point startTime, endTime;
127 | 
128 | 	float elapsedTime(0.), tim;
129 | 	bool bKeepTracking(true);
130 | 
131 | 	int nFrames = 0;
132 | 
133 | 	while(bKeepTracking)
134 | 	{
135 | 		AcquireVGAGrayscaleImage(capture, image, currentFrameRGB);
136 | 
137 | 		if (image.empty())
138 | 			break;
139 | 
140 | 		startTime = high_resolution_clock::now();
141 | 
142 | 		switch (optimizationType)
143 | 		{
144 | 		case descriptorFields:
145 | 			results = optimization.DescriptorFieldsCalibration(pixelsOnTemplateWarped, templ, image, parametersInitialGuess, *optimizationParameters); //parameters are updated with the current guess!
146 | 			break;
147 | 		case gradientModule:
148 | 			results = optimization.GradientModuleCalibration(pixelsOnTemplateWarped, templ, image, parametersInitialGuess, *optimizationParameters); //parameters are updated with the current guess!
149 | 			break;
150 | 		case intensity:
151 | 			results = optimization.SSDCalibration(pixelsOnTemplateWarped, templ, image, parametersInitialGuess, *optimizationParameters);//parameters are updated with the current guess!
152 | 			break;
153 | 		default:
154 | 			break;
155 | 		}
156 | 
157 | 		Homography::CompositionalParametersUpdateWithCheck(parameters, parametersInitialGuess);
158 | 
159 | 		for (int iPoint = 0; iPoint< panelCorners.size(); ++iPoint)
160 | 			Homography::ComputeWarpedPixels(panelCorners[iPoint].x,panelCorners[iPoint].y, warpedPixels.x[iPoint], warpedPixels.y[iPoint], parameters);
161 | 
162 | 		Mat ctrlPtImg;
163 | 		if (B_VERBOSE_TRACKING)
164 | 		{
165 | 			WriteResultsOnImage(image, results, pixelsOnTemplateWarped.size(), optimizationType);
166 | 			// Draw the control points
167 | 			ctrlPtImg = Mat(templ.rows, templ.cols,CV_8UC3,Scalar(0,0,0));
168 | 			for (int idxPt = 0; idxPt < pixelsOnTemplateWarped.size(); ++idxPt)
169 | 				circle(ctrlPtImg, pixelsOnTemplateWarped[idxPt], 3, Scalar(255,255,255) );
170 | 		}
171 | 
172 | 		AugmentFrameWithQuadrilater(warpedPixels, currentFrameRGB);
173 | 
174 | 		Mat frameToDisplay;
175 | 		if (ctrlPtImg.rows > 0)
176 | 		{
177 | 			hconcat(currentFrameRGB, ctrlPtImg, frameToDisplay);
178 | 		}else
179 | 			frameToDisplay = currentFrameRGB;//quick copy
180 | 
181 | 		imshow(window_name, frameToDisplay);
182 | 
183 | 		endTime = high_resolution_clock::now();
184 | 		tim = duration_cast<duration<double> >(endTime - startTime).count();
185 | 		elapsedTime +=tim;
186 | 		nFrames++;
187 | 
188 | 		char key = (char)waitKey(1);
189 | 		switch (key)
190 | 		{
191 | 		case 27:
192 | 			bKeepTracking = false;
193 | 			break;
194 | 		case ' ':
195 | 			key = (char)waitKey(0);
196 | 			break;
197 | 		case '1':
198 | 			optimizationType = intensity;
199 | 			optimizationParameters = &optimizationParametersSSD;
200 | 			method = string("Using intensity!");
201 | 			cout<<method<<endl;
202 | 			bResetOptimization = true;
203 | 			break;
204 | 		case '2':
205 | 			optimizationType = gradientModule;
206 | 			optimizationParameters = &optimizationParametersGradientModule;
207 | 			method = string("Using gradient magnitude!");
208 | 			cout<< method <<endl;
209 | 			bResetOptimization = true;
210 | 			break;
211 | 		case '3':
212 | 			optimizationType = descriptorFields;
213 | 			optimizationParameters = &optimizationParametersDescrFields;
214 | 			method = string("Using Our DESCRIPTOR FIELDS!");
215 | 			cout<< method <<endl;
216 | 			bResetOptimization = true;
217 | 			break;
218 | 		case 'g':
219 | 			bResetOptimization = true;
220 | 			break;
221 | 		default:
222 | 			;
223 | 		}
224 | 
225 | 		if (bResetOptimization)
226 | 		{
227 | 			bResetOptimization = false;
228 | 			cout<<"reset template!"<<endl;
229 | 			fill(parameters.begin(), parameters.end(), 0.);
230 | 			fill(parametersInitialGuess.begin(), parametersInitialGuess.end(), 0.);
231 | 
232 | 			AcquireVGAGrayscaleImage(capture, templ);
233 | 
234 | 			firstFrame = templ.clone();
235 | 			if(optimizationParameters->bAdaptativeChoiceOfPoints)
236 | 				pixelsOnTemplate = CreateAnisotropicGridOfControlPoints(firstFrame, optimizationParameters->nControlPointsOnEdge, optimizationParameters->borderThicknessHorizontal, optimizationParameters->borderThicknessVertical);
237 | 			else
238 | 				pixelsOnTemplate = CreateGridOfControlPoints(firstFrame, optimizationParameters->nControlPointsOnEdge, optimizationParameters->borderThicknessHorizontal, optimizationParameters->borderThicknessVertical);
239 | 
240 | 			pixelsOnTemplateWarped = pixelsOnTemplate;
241 | 		}
242 | 
243 | 		homographyMatrix = Homography::GetUnscaledMatrix(parameters);
244 | 		warpPerspective(firstFrame, templ, homographyMatrix, templ.size(), INTER_LINEAR, BORDER_CONSTANT);
245 | 		WarpGridOfControlPoints(pixelsOnTemplate, pixelsOnTemplateWarped, parameters, templ.cols, templ.rows);
246 | 		fill(parametersInitialGuess.begin(), parametersInitialGuess.end(), 0.);
247 | 	}
248 | 
249 | 	cout << "Calibrated "<<nFrames<< " frames"<<endl;
250 | 	cout << "average elapsedTime = "<< elapsedTime/nFrames<<endl;
251 | }
252 | 


--------------------------------------------------------------------------------
/ISMAR_Demo/parameter/parametersDescriptorFieldsOptimization.yml:
--------------------------------------------------------------------------------
 1 | %YAML:1.0
 2 | resTol: 1e-10
 3 | pTol: 5e-5
 4 | maxIter: 50
 5 | maxIterSingleLevel: 10
 6 | pyramidSmoothingVariance: [7]
 7 | presmoothingVariance: 1
 8 | nControlPointsOnEdge: 25
 9 | borderThicknessHorizontal: 150
10 | borderThicknessVertical: 100
11 | bAdaptativeChoiceOfPoints: 0
12 | bNormalizeDescriptors: 0
13 | 


--------------------------------------------------------------------------------
/ISMAR_Demo/parameter/parametersGradientMagnitudeOptimization.yml:
--------------------------------------------------------------------------------
 1 | %YAML:1.0
 2 | resTol: 1e-10
 3 | pTol: 5e-5
 4 | maxIter: 50
 5 | maxIterSingleLevel: 10
 6 | pyramidSmoothingVariance: [7]
 7 | presmoothingVariance: 1
 8 | nControlPointsOnEdge: 25
 9 | borderThicknessHorizontal: 150
10 | borderThicknessVertical: 100
11 | bAdaptativeChoiceOfPoints: 0
12 | bNormalizeDescriptors: 1
13 | 


--------------------------------------------------------------------------------
/ISMAR_Demo/parameter/parametersSSDOptimization.yml:
--------------------------------------------------------------------------------
 1 | %YAML:1.0
 2 | resTol: 1e-10
 3 | pTol: 5e-5
 4 | maxIter: 50
 5 | maxIterSingleLevel: 10
 6 | pyramidSmoothingVariance: [7]
 7 | presmoothingVariance: 1
 8 | nControlPointsOnEdge: 25
 9 | borderThicknessHorizontal: 150
10 | borderThicknessVertical: 100
11 | bAdaptativeChoiceOfPoints: 0
12 | bNormalizeDescriptors: 1
13 | 
14 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
  1 | 		    GNU GENERAL PUBLIC LICENSE
  2 | 		       Version 2, June 1991
  3 | 
  4 |  Copyright (C) 1989, 1991 Free Software Foundation, Inc.,
  5 |  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
  6 |  Everyone is permitted to copy and distribute verbatim copies
  7 |  of this license document, but changing it is not allowed.
  8 | 
  9 | 			    Preamble
 10 | 
 11 |   The licenses for most software are designed to take away your
 12 | freedom to share and change it.  By contrast, the GNU General Public
 13 | License is intended to guarantee your freedom to share and change free
 14 | software--to make sure the software is free for all its users.  This
 15 | General Public License applies to most of the Free Software
 16 | Foundation's software and to any other program whose authors commit to
 17 | using it.  (Some other Free Software Foundation software is covered by
 18 | the GNU Lesser General Public License instead.)  You can apply it to
 19 | your programs, too.
 20 | 
 21 |   When we speak of free software, we are referring to freedom, not
 22 | price.  Our General Public Licenses are designed to make sure that you
 23 | have the freedom to distribute copies of free software (and charge for
 24 | this service if you wish), that you receive source code or can get it
 25 | if you want it, that you can change the software or use pieces of it
 26 | in new free programs; and that you know you can do these things.
 27 | 
 28 |   To protect your rights, we need to make restrictions that forbid
 29 | anyone to deny you these rights or to ask you to surrender the rights.
 30 | These restrictions translate to certain responsibilities for you if you
 31 | distribute copies of the software, or if you modify it.
 32 | 
 33 |   For example, if you distribute copies of such a program, whether
 34 | gratis or for a fee, you must give the recipients all the rights that
 35 | you have.  You must make sure that they, too, receive or can get the
 36 | source code.  And you must show them these terms so they know their
 37 | rights.
 38 | 
 39 |   We protect your rights with two steps: (1) copyright the software, and
 40 | (2) offer you this license which gives you legal permission to copy,
 41 | distribute and/or modify the software.
 42 | 
 43 |   Also, for each author's protection and ours, we want to make certain
 44 | that everyone understands that there is no warranty for this free
 45 | software.  If the software is modified by someone else and passed on, we
 46 | want its recipients to know that what they have is not the original, so
 47 | that any problems introduced by others will not reflect on the original
 48 | authors' reputations.
 49 | 
 50 |   Finally, any free program is threatened constantly by software
 51 | patents.  We wish to avoid the danger that redistributors of a free
 52 | program will individually obtain patent licenses, in effect making the
 53 | program proprietary.  To prevent this, we have made it clear that any
 54 | patent must be licensed for everyone's free use or not licensed at all.
 55 | 
 56 |   The precise terms and conditions for copying, distribution and
 57 | modification follow.
 58 | 
 59 | 		    GNU GENERAL PUBLIC LICENSE
 60 |    TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
 61 | 
 62 |   0. This License applies to any program or other work which contains
 63 | a notice placed by the copyright holder saying it may be distributed
 64 | under the terms of this General Public License.  The "Program", below,
 65 | refers to any such program or work, and a "work based on the Program"
 66 | means either the Program or any derivative work under copyright law:
 67 | that is to say, a work containing the Program or a portion of it,
 68 | either verbatim or with modifications and/or translated into another
 69 | language.  (Hereinafter, translation is included without limitation in
 70 | the term "modification".)  Each licensee is addressed as "you".
 71 | 
 72 | Activities other than copying, distribution and modification are not
 73 | covered by this License; they are outside its scope.  The act of
 74 | running the Program is not restricted, and the output from the Program
 75 | is covered only if its contents constitute a work based on the
 76 | Program (independent of having been made by running the Program).
 77 | Whether that is true depends on what the Program does.
 78 | 
 79 |   1. You may copy and distribute verbatim copies of the Program's
 80 | source code as you receive it, in any medium, provided that you
 81 | conspicuously and appropriately publish on each copy an appropriate
 82 | copyright notice and disclaimer of warranty; keep intact all the
 83 | notices that refer to this License and to the absence of any warranty;
 84 | and give any other recipients of the Program a copy of this License
 85 | along with the Program.
 86 | 
 87 | You may charge a fee for the physical act of transferring a copy, and
 88 | you may at your option offer warranty protection in exchange for a fee.
 89 | 
 90 |   2. You may modify your copy or copies of the Program or any portion
 91 | of it, thus forming a work based on the Program, and copy and
 92 | distribute such modifications or work under the terms of Section 1
 93 | above, provided that you also meet all of these conditions:
 94 | 
 95 |     a) You must cause the modified files to carry prominent notices
 96 |     stating that you changed the files and the date of any change.
 97 | 
 98 |     b) You must cause any work that you distribute or publish, that in
 99 |     whole or in part contains or is derived from the Program or any
100 |     part thereof, to be licensed as a whole at no charge to all third
101 |     parties under the terms of this License.
102 | 
103 |     c) If the modified program normally reads commands interactively
104 |     when run, you must cause it, when started running for such
105 |     interactive use in the most ordinary way, to print or display an
106 |     announcement including an appropriate copyright notice and a
107 |     notice that there is no warranty (or else, saying that you provide
108 |     a warranty) and that users may redistribute the program under
109 |     these conditions, and telling the user how to view a copy of this
110 |     License.  (Exception: if the Program itself is interactive but
111 |     does not normally print such an announcement, your work based on
112 |     the Program is not required to print an announcement.)
113 | 
114 | These requirements apply to the modified work as a whole.  If
115 | identifiable sections of that work are not derived from the Program,
116 | and can be reasonably considered independent and separate works in
117 | themselves, then this License, and its terms, do not apply to those
118 | sections when you distribute them as separate works.  But when you
119 | distribute the same sections as part of a whole which is a work based
120 | on the Program, the distribution of the whole must be on the terms of
121 | this License, whose permissions for other licensees extend to the
122 | entire whole, and thus to each and every part regardless of who wrote it.
123 | 
124 | Thus, it is not the intent of this section to claim rights or contest
125 | your rights to work written entirely by you; rather, the intent is to
126 | exercise the right to control the distribution of derivative or
127 | collective works based on the Program.
128 | 
129 | In addition, mere aggregation of another work not based on the Program
130 | with the Program (or with a work based on the Program) on a volume of
131 | a storage or distribution medium does not bring the other work under
132 | the scope of this License.
133 | 
134 |   3. You may copy and distribute the Program (or a work based on it,
135 | under Section 2) in object code or executable form under the terms of
136 | Sections 1 and 2 above provided that you also do one of the following:
137 | 
138 |     a) Accompany it with the complete corresponding machine-readable
139 |     source code, which must be distributed under the terms of Sections
140 |     1 and 2 above on a medium customarily used for software interchange; or,
141 | 
142 |     b) Accompany it with a written offer, valid for at least three
143 |     years, to give any third party, for a charge no more than your
144 |     cost of physically performing source distribution, a complete
145 |     machine-readable copy of the corresponding source code, to be
146 |     distributed under the terms of Sections 1 and 2 above on a medium
147 |     customarily used for software interchange; or,
148 | 
149 |     c) Accompany it with the information you received as to the offer
150 |     to distribute corresponding source code.  (This alternative is
151 |     allowed only for noncommercial distribution and only if you
152 |     received the program in object code or executable form with such
153 |     an offer, in accord with Subsection b above.)
154 | 
155 | The source code for a work means the preferred form of the work for
156 | making modifications to it.  For an executable work, complete source
157 | code means all the source code for all modules it contains, plus any
158 | associated interface definition files, plus the scripts used to
159 | control compilation and installation of the executable.  However, as a
160 | special exception, the source code distributed need not include
161 | anything that is normally distributed (in either source or binary
162 | form) with the major components (compiler, kernel, and so on) of the
163 | operating system on which the executable runs, unless that component
164 | itself accompanies the executable.
165 | 
166 | If distribution of executable or object code is made by offering
167 | access to copy from a designated place, then offering equivalent
168 | access to copy the source code from the same place counts as
169 | distribution of the source code, even though third parties are not
170 | compelled to copy the source along with the object code.
171 | 
172 |   4. You may not copy, modify, sublicense, or distribute the Program
173 | except as expressly provided under this License.  Any attempt
174 | otherwise to copy, modify, sublicense or distribute the Program is
175 | void, and will automatically terminate your rights under this License.
176 | However, parties who have received copies, or rights, from you under
177 | this License will not have their licenses terminated so long as such
178 | parties remain in full compliance.
179 | 
180 |   5. You are not required to accept this License, since you have not
181 | signed it.  However, nothing else grants you permission to modify or
182 | distribute the Program or its derivative works.  These actions are
183 | prohibited by law if you do not accept this License.  Therefore, by
184 | modifying or distributing the Program (or any work based on the
185 | Program), you indicate your acceptance of this License to do so, and
186 | all its terms and conditions for copying, distributing or modifying
187 | the Program or works based on it.
188 | 
189 |   6. Each time you redistribute the Program (or any work based on the
190 | Program), the recipient automatically receives a license from the
191 | original licensor to copy, distribute or modify the Program subject to
192 | these terms and conditions.  You may not impose any further
193 | restrictions on the recipients' exercise of the rights granted herein.
194 | You are not responsible for enforcing compliance by third parties to
195 | this License.
196 | 
197 |   7. If, as a consequence of a court judgment or allegation of patent
198 | infringement or for any other reason (not limited to patent issues),
199 | conditions are imposed on you (whether by court order, agreement or
200 | otherwise) that contradict the conditions of this License, they do not
201 | excuse you from the conditions of this License.  If you cannot
202 | distribute so as to satisfy simultaneously your obligations under this
203 | License and any other pertinent obligations, then as a consequence you
204 | may not distribute the Program at all.  For example, if a patent
205 | license would not permit royalty-free redistribution of the Program by
206 | all those who receive copies directly or indirectly through you, then
207 | the only way you could satisfy both it and this License would be to
208 | refrain entirely from distribution of the Program.
209 | 
210 | If any portion of this section is held invalid or unenforceable under
211 | any particular circumstance, the balance of the section is intended to
212 | apply and the section as a whole is intended to apply in other
213 | circumstances.
214 | 
215 | It is not the purpose of this section to induce you to infringe any
216 | patents or other property right claims or to contest validity of any
217 | such claims; this section has the sole purpose of protecting the
218 | integrity of the free software distribution system, which is
219 | implemented by public license practices.  Many people have made
220 | generous contributions to the wide range of software distributed
221 | through that system in reliance on consistent application of that
222 | system; it is up to the author/donor to decide if he or she is willing
223 | to distribute software through any other system and a licensee cannot
224 | impose that choice.
225 | 
226 | This section is intended to make thoroughly clear what is believed to
227 | be a consequence of the rest of this License.
228 | 
229 |   8. If the distribution and/or use of the Program is restricted in
230 | certain countries either by patents or by copyrighted interfaces, the
231 | original copyright holder who places the Program under this License
232 | may add an explicit geographical distribution limitation excluding
233 | those countries, so that distribution is permitted only in or among
234 | countries not thus excluded.  In such case, this License incorporates
235 | the limitation as if written in the body of this License.
236 | 
237 |   9. The Free Software Foundation may publish revised and/or new versions
238 | of the General Public License from time to time.  Such new versions will
239 | be similar in spirit to the present version, but may differ in detail to
240 | address new problems or concerns.
241 | 
242 | Each version is given a distinguishing version number.  If the Program
243 | specifies a version number of this License which applies to it and "any
244 | later version", you have the option of following the terms and conditions
245 | either of that version or of any later version published by the Free
246 | Software Foundation.  If the Program does not specify a version number of
247 | this License, you may choose any version ever published by the Free Software
248 | Foundation.
249 | 
250 |   10. If you wish to incorporate parts of the Program into other free
251 | programs whose distribution conditions are different, write to the author
252 | to ask for permission.  For software which is copyrighted by the Free
253 | Software Foundation, write to the Free Software Foundation; we sometimes
254 | make exceptions for this.  Our decision will be guided by the two goals
255 | of preserving the free status of all derivatives of our free software and
256 | of promoting the sharing and reuse of software generally.
257 | 
258 | 			    NO WARRANTY
259 | 
260 |   11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
261 | FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW.  EXCEPT WHEN
262 | OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
263 | PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
264 | OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
265 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE ENTIRE RISK AS
266 | TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU.  SHOULD THE
267 | PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
268 | REPAIR OR CORRECTION.
269 | 
270 |   12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
271 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
272 | REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
273 | INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
274 | OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
275 | TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
276 | YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
277 | PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
278 | POSSIBILITY OF SUCH DAMAGES.
279 | 
280 | 		     END OF TERMS AND CONDITIONS
281 | 
282 | 	    How to Apply These Terms to Your New Programs
283 | 
284 |   If you develop a new program, and you want it to be of the greatest
285 | possible use to the public, the best way to achieve this is to make it
286 | free software which everyone can redistribute and change under these terms.
287 | 
288 |   To do so, attach the following notices to the program.  It is safest
289 | to attach them to the start of each source file to most effectively
290 | convey the exclusion of warranty; and each file should have at least
291 | the "copyright" line and a pointer to where the full notice is found.
292 | 
293 |     <one line to give the program's name and a brief idea of what it does.>
294 |     Copyright (C) <year>  <name of author>
295 | 
296 |     This program is free software; you can redistribute it and/or modify
297 |     it under the terms of the GNU General Public License as published by
298 |     the Free Software Foundation; either version 2 of the License, or
299 |     (at your option) any later version.
300 | 
301 |     This program is distributed in the hope that it will be useful,
302 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
303 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
304 |     GNU General Public License for more details.
305 | 
306 |     You should have received a copy of the GNU General Public License along
307 |     with this program; if not, write to the Free Software Foundation, Inc.,
308 |     51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
309 | 
310 | Also add information on how to contact you by electronic and paper mail.
311 | 
312 | If the program is interactive, make it output a short notice like this
313 | when it starts in an interactive mode:
314 | 
315 |     Gnomovision version 69, Copyright (C) year name of author
316 |     Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
317 |     This is free software, and you are welcome to redistribute it
318 |     under certain conditions; type `show c' for details.
319 | 
320 | The hypothetical commands `show w' and `show c' should show the appropriate
321 | parts of the General Public License.  Of course, the commands you use may
322 | be called something other than `show w' and `show c'; they could even be
323 | mouse-clicks or menu items--whatever suits your program.
324 | 
325 | You should also get your employer (if you work as a programmer) or your
326 | school, if any, to sign a "copyright disclaimer" for the program, if
327 | necessary.  Here is a sample; alter the names:
328 | 
329 |   Yoyodyne, Inc., hereby disclaims all copyright interest in the program
330 |   `Gnomovision' (which makes passes at compilers) written by James Hacker.
331 | 
332 |   <signature of Ty Coon>, 1 April 1989
333 |   Ty Coon, President of Vice
334 | 
335 | This General Public License does not permit incorporating your program into
336 | proprietary programs.  If your program is a subroutine library, you may
337 | consider it more useful to permit linking proprietary applications with the
338 | library.  If this is what you want to do, use the GNU Lesser General
339 | Public License instead of this License.
340 | 


--------------------------------------------------------------------------------
/Makefile:
--------------------------------------------------------------------------------
 1 | SOURCES=$(wildcard src/*.cpp)
 2 | OBJECTS=$(SOURCES:.cpp=.o)
 3 | EXECUTABLE=homographyDemo
 4 | LIB_CPP = libHomographyEstimation.so
 5 | 
 6 | include Makefile.inc
 7 | 
 8 | help:           ## Show this help.
 9 | 	@echo "——————— Available Targets  ——————— \n\n"
10 | 	@fgrep -h "##" $(MAKEFILE_LIST) | fgrep -v fgrep | sed -e 's/\\$$//' | sed -e 's/##//'
11 | 	@echo "\n\n"
12 | 
13 | 
14 | all: ## 		build all in release mode
15 | all: libCppRelease demoRelease unittestsRelease cleanObj
16 | 
17 | release: ## 	build all in release mode
18 | release: libCppRelease demoRelease unittestsRelease cleanObj
19 | 
20 | debug: ## 	build all in debug mode
21 | debug: libCppDebug demoDebug unittestsDebug cleanObj 
22 | 
23 | 
24 | libCppRelease: ## 	C++ lib - release mode
25 | libCppRelease: CFLAGS += $(CFLAGS_RELEASE)
26 | libCppRelease:	$(LIB_CPP)
27 | 
28 | libCppDebug: ##		C++ lib - release mode
29 | libCppDebug: CFLAGS += $(CFLAGS_DEBUG)
30 | libCppDebug:	$(LIB_CPP)
31 | 
32 | $(LIB_CPP): $(SOURCES) $(OBJECTS)
33 | 	@echo ——————— compiling the Homography library  ——————— 
34 | 	$(CC) -shared $(CFLAGS) $(OBJECTS) $(LDFLAGS) -o $@
35 | 
36 | demoRelease: ##		executable - release mode 
37 | demoRelease: CFLAGS += $(CFLAGS_RELEASE)
38 | demoRelease: libCppRelease $(EXECUTABLE)
39 | 
40 | demoDebug: ##		executable - debug mode 
41 | demoDebug: CFLAGS += $(CFLAGS_DEBUG)
42 | demoDebug: libCppDebug $(EXECUTABLE)
43 | 
44 | $(EXECUTABLE): $(SOURCES) $(OBJECTS)
45 | 	@echo ——————— compiling the demo  ——————— 
46 | 	$(CC) ISMAR_Demo/main.cpp $(CFLAGS) $(LDFLAGS) -L. -lHomographyEstimation -o ISMAR_Demo/$@
47 | 	cd ISMAR_Demo && ln -sf ../libHomographyEstimation.so libHomographyEstimation.so
48 | 
49 | unittestsRelease: ##		unit tests - release mode 
50 | unittestsRelease: CFLAGS += $(CFLAGS_RELEASE)
51 | unittestsRelease: libCppRelease unittests
52 | 
53 | unittestsDebug: ##		unit tests - debug mode 
54 | unittestsDebug: CFLAGS += $(CFLAGS_DEBUG)
55 | unittestsDebug: libCppDebug unittests
56 | 
57 | unittests: $(SOURCES) $(OBJECTS)
58 | 	@echo ——————— compiling the test  ——————— 
59 | 	$(CC) unit/UnitTests.cpp $(CFLAGS) $(LDFLAGS) -L. -lHomographyEstimation -o unit/$@ 
60 | 	cd unit && ln -sf ../libHomographyEstimation.so libHomographyEstimation.so
61 | .cpp.o:
62 | 	$(CC) -c $(CFLAGS) $< -o $@
63 | 
64 | 
65 | .PHONY: clean all help
66 | 
67 | cleanObj:
68 | 	@echo ——————— Cleaning the objects .o  ——————— 
69 | 	rm -rf ISMAR_Demo/main.o unit/UnitTests.o $(OBJECTS)
70 | 
71 | clean:
72 | 	@echo ——————— Cleaning the project   ——————— 
73 | 	rm -rf ISMAR_Demo/main.o ISMAR_Demo/libHomographyEstimation.so unit/UnitTests.o unit/libHomographyEstimation.so $(OBJECTS) libHomographyEstimation.so unit/unittests ISMAR_Demo/$(EXECUTABLE)
74 | 
75 | 


--------------------------------------------------------------------------------
/Makefile.inc:
--------------------------------------------------------------------------------
 1 | UNAME=$(shell uname)
 2 | 
 3 | CC=g++
 4 | LDFLAGS = `pkg-config opencv --libs`
 5 | CFLAGS = `pkg-config --cflags eigen3` `pkg-config opencv --cflags` -Iinclude -std=c++11
 6 | CFLAGS_RELEASE= -O3 -DNDEBUG -DEIGEN_NO_DEBUG -ffast-math
 7 | CFLAGS_DEBUG= -g -O0
 8 | ifeq ($(UNAME),Linux)
 9 | 	CFLAGS +=  -fopenmp -fPIC
10 | 	LDFLAGS += -fopenmp 
11 | endif
12 | 


--------------------------------------------------------------------------------
/README:
--------------------------------------------------------------------------------
  1 | 
  2 | This  code has  been  presented for  the demo  :  "Tracking texture-less,  shiny
  3 | objects with descriptor fields." Mixed  and Augmented Reality (ISMAR), 2014 IEEE
  4 | International Symposium on. IEEE, 2014.
  5 | 
  6 | It allows  to track homographies using  the descriptor fields introduced  in our
  7 | CVPR paper:
  8 | 
  9 | [1] Crivellaro Alberto and Vincent  Lepetit. "Robust 3D Tracking with Descriptor
 10 |     Fields.", CVPR 2014
 11 | 
 12 | Please cite [1] if you make use of this code.
 13 | 
 14 | Authors: 
 15 | 	 Alberto Crivellaro 
 16 | 	 Yannick Verdie
 17 | 	 Kwang Moo Yi
 18 | 	 Pascal Fua
 19 | 	 Vincent Lepetit
 20 | 
 21 | For questions or comments please write to :
 22 |     	      
 23 | 	 alberto<dot>crivellaro<at>epfl<dot>ch
 24 | 
 25 | 
 26 | ================================================================================
 27 | BUILDING: 
 28 | 
 29 | The software  has been tested and  successfully run in  real time (> 30  fps) on
 30 | linux Ubuntu  14.04 on a  MacBookPro i7 2.30GHz. It  was built with  gcc-4.9 and
 31 | openmp 4.0. We also tested on MacOS  with clang (no openmp) and we also obtained
 32 | good performance  (~20fps).  Older  versions of gcc/openmp  could lead  to worse
 33 | performances.
 34 | 
 35 | 
 36 | Prior  to  build the  code  you  should install  the  (latest  version of  the)
 37 | following dependencies:
 38 | 
 39 |    - opencv (we tested with 2.4.9 and 3.0)
 40 |    - eigen 3 or more
 41 | 
 42 | Moreover,  you   should  correctly  configure  your   LD_LIBRARY_PATH  and  your
 43 | PKG_CONFIG_PATH. For building just 'make all’.  The build produces :
 44 | 
 45 |    - libC++/libHomographyEstimation.so  : a  C++ dynamic  library for  computing
 46 |      descriptor fields  out of a  grayscale image, for computing  the homography
 47 |      between 2 images and several utilities for display, I/O, etc.
 48 | 
 49 |    - ISMAR_Demo/homographyDemo  :  an executable  for  performing  real time  3D
 50 |      tracking of planar surfaces., it links to the c++ library and can be use as
 51 |      example to understand how to link to the library.
 52 | 
 53 |    - unit/unittests : some tests
 54 | 
 55 | 
 56 | ================================================================================
 57 | EXECUTING THE DEMO:
 58 | 
 59 | ./homographyDemo  <camera_device_number> 
 60 | 
 61 | you should  initialize the template  putting a planar  surface in front  of the
 62 | camera (covering at least  all the region in the black  rectangle on the image),
 63 | and then press:
 64 | 
 65 |     1 for tracking using image intensities
 66 |     2 for tracking using gradient magnitude
 67 |     3 for tracking using descriptor fields
 68 | 
 69 | The             optimization            parameters             are            in
 70 | ISMAR_Demo/parameter/parameters*****Optimization.yml files. You  can change them
 71 | for attempting to achieve a better stability.
 72 | 
 73 | 
 74 | 
 75 | ================================================================================
 76 | USING THE LIBRARY FOR YOUR PROJECTS: 
 77 | 
 78 | include   libC++/HomographyEstimation.hpp   in   your  code,   and   link   with
 79 | libHomographyEstimation.so  .   For  examples  on   how  to  use  the  different
 80 | utilities, you can take a look at the unit tests, more in particular :
 81 | 
 82 |     void LucaskanadeSSDTest();
 83 |     void LucaskanadeDescriptorFieldsTest();
 84 |     void LucaskanadeVideoSSDSpeedTest();
 85 |     void LucaskanadeVideoDesciptorFieldsSpeedTest();
 86 |     void LucaskanadeVideoGradientMagnitudeSpeedTest();
 87 | 
 88 | 
 89 | 
 90 | ================================================================================
 91 | USEFUL LINKS:
 92 | 
 93 | homepage of the project: http://cvlab.epfl.ch/page-107683-en.html 
 94 | demonstration video: 	 https://www.youtube.com/watch?v=r-WJDM6HyAs
 95 | 
 96 | 
 97 | 
 98 | 
 99 | 
100 | 
101 | 
102 | 


--------------------------------------------------------------------------------
/include/Homography.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 | Copyright 2014 Alberto Crivellaro, Ecole Polytechnique Federale de Lausanne (EPFL), Switzerland.
 3 | alberto.crivellaro@epfl.ch
 4 | 
 5 | terms of the GNU General Public License as published by the Free Software
 6 | Foundation; either version 2 of the License, or (at your option) any later
 7 | version.
 8 | 
 9 | This program is distributed in the hope that it will be useful, but WITHOUT ANY
10 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
11 | PARTICULAR PURPOSE. See the GNU General Public License for more details.
12 | 
13 | You should have received a copy of the GNU General Public License along with
14 | this program; if not, write to the Free Software Foundation, Inc., 51 Franklin
15 | Street, Fifth Floor, Boston, MA 02110-1301, USA
16 |  */
17 | 
18 | 
19 | #ifndef HOMOGRAPHY_H_
20 | #define HOMOGRAPHY_H_
21 | 
22 | #include "Typedefs.hpp"
23 | #include <vector>
24 | 
25 | using namespace cv;
26 | using namespace std;
27 | 
28 | 
29 | namespace Homography
30 | {
31 | 
32 | void ComputeWarpJacobian(const int inputPixelx, const int inputPixely, const vector<float>& p, Eigen::Matrix<float, 2, N_PARAM>& warpJacobian);
33 | void ComputeWarpedHomoPoints(const Point3f & inputPixel, Point3f & outputPixel,const vector<float> & p);
34 | Matx33f GetMatrix(const vector<float> & p);
35 | Matx33f GetUnscaledMatrix(const vector<float> &p);
36 | vector<float> GetScaledParam(const vector<float> &unscaled_p);
37 | 
38 | void InverseWarpParameters(const vector<float>& directP, vector<float>& invP);
39 | void InverseWarpParameters(const Eigen::Matrix<float, N_PARAM, 1>& directP, vector<float>& invP);
40 | 
41 | void CompositionalParametersUpdateWithCheck(vector<float> &p, const vector<float>& deltap);
42 | vector<float> ParametersUpdateCompositional(const vector<float> &p,const vector<float>& deltap);
43 | 
44 | void DisplayParameters(const vector<float> &parameters);
45 | bool CheckHomography(const vector<float> &parameters);
46 | 
47 | inline void ComputeWarpedPixels(const int inputPixelx, const int inputPixely, int & outputPixelx, int & outputPixely, const vector<float> & p)
48 | {
49 | 	float scalex = (MAX_SCALING - MIN_SCALING)/(PICTURE_WIDTH-1);
50 | 	float scaley = (MAX_SCALING - MIN_SCALING)/(PICTURE_HEIGHT-1);
51 | 	cv::Matx33f scalingMatrix(scalex, 0, MIN_SCALING, 0, scaley, MIN_SCALING, 0, 0, 1);
52 | 	cv::Matx33f homographyMatrix = Homography::GetMatrix(p);
53 | 	cv::Matx33f unNormalizedHomography  = scalingMatrix.inv() * homographyMatrix * scalingMatrix;
54 | 	vector<float> pUnscaled(8);
55 | 	pUnscaled[0] = unNormalizedHomography(0,0)-1;
56 | 	pUnscaled[1] = unNormalizedHomography(1,0);
57 | 	pUnscaled[2] = unNormalizedHomography(0,1);
58 | 	pUnscaled[3] = unNormalizedHomography(1,1)-1;
59 | 	pUnscaled[4] = unNormalizedHomography(0,2);
60 | 	pUnscaled[5] = unNormalizedHomography(1,2);
61 | 	pUnscaled[6] = unNormalizedHomography(2,0);
62 | 	pUnscaled[7] = unNormalizedHomography(2,1);
63 | 
64 | 
65 | 	float denom = 1./(1 + pUnscaled[6]*inputPixelx + pUnscaled[7]*inputPixely);
66 | 	outputPixelx = (((1+pUnscaled[0])*inputPixelx + pUnscaled[2]*inputPixely + pUnscaled[4]) * denom + 0.5f);
67 | 	outputPixely = ((pUnscaled[1]*inputPixelx + (1+pUnscaled[3])*inputPixely + pUnscaled[5]) * denom + 0.5f);
68 | }
69 | 
70 | }
71 | 
72 | #endif /* HOMOGRAPHY_H_ */
73 | 


--------------------------------------------------------------------------------
/include/HomographyEstimation.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 | Copyright 2014 Alberto Crivellaro, Ecole Polytechnique Federale de Lausanne (EPFL), Switzerland.
 3 | alberto.crivellaro@epfl.ch
 4 | 
 5 | terms of the GNU General Public License as published by the Free Software
 6 | Foundation; either version 2 of the License, or (at your option) any later
 7 | version.
 8 | 
 9 | This program is distributed in the hope that it will be useful, but WITHOUT ANY
10 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
11 | PARTICULAR PURPOSE. See the GNU General Public License for more details.
12 | 
13 | You should have received a copy of the GNU General Public License along with
14 | this program; if not, write to the Free Software Foundation, Inc., 51 Franklin
15 | Street, Fifth Floor, Boston, MA 02110-1301, USA
16 |  */
17 | 
18 | #ifndef HOMOGRAPHY_ESTIMATION_HPP_
19 | #define HOMOGRAPHY_ESTIMATION_HPP_
20 | 
21 | #include "Utilities.hpp"
22 | #include "Typedefs.hpp"
23 | #include "Homography.hpp"
24 | #include "IterativeOptimization.hpp"
25 | 
26 | #endif


--------------------------------------------------------------------------------
/include/IterativeOptimization.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 | Copyright 2014 Alberto Crivellaro, Ecole Polytechnique Federale de Lausanne (EPFL), Switzerland.
 3 | alberto.crivellaro@epfl.ch
 4 | 
 5 | terms of the GNU General Public License as published by the Free Software
 6 | Foundation; either version 2 of the License, or (at your option) any later
 7 | version.
 8 | 
 9 | This program is distributed in the hope that it will be useful, but WITHOUT ANY
10 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
11 | PARTICULAR PURPOSE. See the GNU General Public License for more details.
12 | 
13 | You should have received a copy of the GNU General Public License along with
14 | this program; if not, write to the Free Software Foundation, Inc., 51 Franklin
15 | Street, Fifth Floor, Boston, MA 02110-1301, USA
16 |  */
17 | 
18 | 
19 | #ifndef ITERATIVEOPTIMIZATION_HPP_
20 | #define ITERATIVEOPTIMIZATION_HPP_
21 | 
22 | #include "Typedefs.hpp"
23 | #include "Utilities.hpp"
24 | #include "Homography.hpp"
25 | 
26 | #include <Eigen/LU>
27 | 
28 | #include <math.h>
29 | #include <vector>
30 | using namespace std;
31 | using namespace cv;
32 | 
33 | class IterativeOptimization{
34 | public:
35 | 	AlignmentResults SSDCalibration(const StructOfArray2di & pixelsOnTemplate, Mat &grayscaleFloatTemplate, Mat &grayscaleFloatImage, vector<float> &parameters, OptimizationParameters & optimizationParameters);
36 | 	AlignmentResults DescriptorFieldsCalibration(const StructOfArray2di & pixelsOnTemplate, Mat &grayscaleFloatTemplate, Mat &grayscaleFloatImage, vector<float> &paramters, OptimizationParameters & optimizationParameters);
37 | 	AlignmentResults GradientModuleCalibration(const StructOfArray2di & pixelsOnTemplate, Mat &grayscaleFloatTemplate, Mat &grayscaleFloatImage, vector<float> &paramters, OptimizationParameters & optimizationParameters);
38 | 
39 | 	//these should be protected but there are tests on them.
40 | 	void ComputeWarpedPixels(const StructOfArray2di & pixelsOnTemplate, const vector<float>&  parameters, StructOfArray2di & warpedPixels);
41 |     void AssembleSDImages(const vector<float>&  parameters, const Mat &imageDx, const Mat &imageDy, const StructOfArray2di & warpedPixels, const vector<Eigen::Matrix<float, 2, N_PARAM>, Eigen::aligned_allocator<Eigen::Matrix<float, 2, N_PARAM> > > & warpJacobians,  Eigen::MatrixXf & sdImages);
42 | protected:
43 | 	virtual AlignmentResults GaussNewtonMinimization(const StructOfArray2di & pixelsOnTemplate, const vector<Mat> & images, const vector<Mat> & templates, const OptimizationParameters optParam, vector<float> & parameters) = 0;
44 | 	AlignmentResults PyramidMultiLevelCalibration(const StructOfArray2di & pixelsOnTemplate, vector<Mat> &templateDescriptorFields, vector<Mat> &imageDescriptorFields, vector<float> & parameters, OptimizationParameters & optimizationParameters);
45 | 	void ComputeResiduals(const Mat &image,  vector<float> & templatePixelIntensities, const StructOfArray2di & warpedPixels, vector<float> & errorImage);
46 | 	float ComputeResidualNorm(vector<float> &errorImage);
47 | 	int CheckConvergenceOptimization(float deltaPoseNorm, int nIter, float residualNormIncrement, OptimizationParameters optParam);
48 | };
49 | 
50 | 
51 | class LucasKanade: public IterativeOptimization{
52 | private:
53 | 	AlignmentResults GaussNewtonMinimization(const StructOfArray2di & pixelsOnTemplate, const vector<Mat> & images, const vector<Mat> & templates, const OptimizationParameters optParam, vector<float> & parameters);
54 | };
55 | 
56 | // TODO: implement this
57 | //class ICA: public IterativeOptimization{
58 | //private:
59 | //	AlignmentResults GaussNewtonMinimization(const StructOfArray2di & pixelsOnTemplate, const vector<Mat> & images, const vector<Mat> & templates, const OptimizationParameters optParam, vector<float> & parameters);
60 | //};
61 | 
62 | #endif /* ITERATIVEOPTIMIZATION_HPP_*/
63 | 


--------------------------------------------------------------------------------
/include/Typedefs.hpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | Copyright 2014 Alberto Crivellaro, Ecole Polytechnique Federale de Lausanne (EPFL), Switzerland.
  3 | alberto.crivellaro@epfl.ch
  4 | 
  5 | terms of the GNU General Public License as published by the Free Software
  6 | Foundation; either version 2 of the License, or (at your option) any later
  7 | version.
  8 | 
  9 | This program is distributed in the hope that it will be useful, but WITHOUT ANY
 10 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
 11 | PARTICULAR PURPOSE. See the GNU General Public License for more details.
 12 | 
 13 | You should have received a copy of the GNU General Public License along with
 14 | this program; if not, write to the Free Software Foundation, Inc., 51 Franklin
 15 | Street, Fifth Floor, Boston, MA 02110-1301, USA
 16 |  */
 17 | 
 18 | #ifndef TYPEDEFS_HPP_
 19 | #define TYPEDEFS_HPP_
 20 | #include "opencv2/opencv.hpp"
 21 | #include <Eigen/Dense>
 22 | #include <Eigen/StdVector>
 23 | #ifndef uint
 24 | typedef unsigned int uint;
 25 | #endif
 26 | 
 27 | // TODO make optimization generic wrt type of warp
 28 | #define N_PARAM 8
 29 | #define MIN_SCALING (0.0) // like thi sYanick can play around ^^
 30 | #define MAX_SCALING  (1.0)
 31 | #define PICTURE_WIDTH (2.0) // like thi sYanick can play around ^^
 32 | #define PICTURE_HEIGHT (2.0)
 33 | 
 34 | using namespace cv;
 35 | using namespace std;
 36 | 
 37 | 
 38 | typedef enum {intensity, gradientModule, descriptorFields} OptimizationType;
 39 | 
 40 | template<typename T = int >
 41 | class StructOfArray2d
 42 | {
 43 | public:
 44 | 	StructOfArray2d(int size = 0)
 45 | {
 46 | 		x = vector<T>(size);
 47 | 		y = vector<T>(size);
 48 | };
 49 | 	int size() const
 50 | 	{
 51 | 		return x.size();
 52 | 	}
 53 | 
 54 | 	void reserve(const int s)
 55 | 	{
 56 | 		  x.reserve(s);
 57 | 		  y.reserve(s);
 58 | 	}
 59 | 
 60 | 	void resize(const int s)
 61 | 	{
 62 | 		x.resize(s);
 63 | 		y.resize(s);
 64 | 	}
 65 | 
 66 | 	void push_back(cv::Point_<T> p)
 67 | 	{
 68 | 		x.push_back(p.x);
 69 | 		y.push_back(p.y);
 70 | 	}
 71 | 
 72 | 	void push_back(T xx, T yy)
 73 | 	{
 74 | 		x.push_back(xx);
 75 | 		y.push_back(yy);
 76 | 	}
 77 | 
 78 | 	void clear()
 79 | 		{
 80 | 			x.clear();
 81 | 			y.clear();
 82 | 		}
 83 | 
 84 | 	void info() const
 85 | 	{
 86 | 		cout<<"size: "<<x.size()<<endl;
 87 | 		for(int i=0;i<x.size();i++)
 88 | 			cout<<"("<<x[i]<<","<<y[i]<<")"<<endl;
 89 | 	}
 90 | 
 91 | 	const cv::Point_<T> operator[](int n) const
 92 | 	{
 93 | 		return cv::Point_<T>(x[n],y[n]) ;
 94 | 	}
 95 | 
 96 | 	StructOfArray2d& operator=(const StructOfArray2d &anotherStruct)
 97 | 	{
 98 | 			this->x = anotherStruct.x;
 99 | 			this->y = anotherStruct.y;
100 | 			return *this;
101 | 	  }
102 | 
103 | 	vector<T> x;
104 | 	vector<T> y;
105 | };
106 | 
107 | typedef StructOfArray2d<int> StructOfArray2di;
108 | typedef StructOfArray2d<float> StructOfArray2df;
109 | 
110 | template<typename T = int >
111 | class StructOfArray3d
112 | {
113 | 	StructOfArray3d<T>(int size)
114 | 			{
115 | 		x = vector<T>(size);
116 | 		y = vector<T>(size);
117 | 		z = vector<T>(size);
118 | 			};
119 | public:
120 | 	vector<T> x;
121 | 	vector<T> y;
122 | 	vector<T> z;
123 | };
124 | 
125 | 
126 | struct AlignmentResults
127 | {
128 | 	vector<float> residualNorm;
129 | 	vector<vector<float> > poseIntermediateGuess;
130 | 	int exitFlag;
131 | 	int nIter;
132 | 	AlignmentResults(): exitFlag(1e6), nIter(0){};
133 | };
134 | 
135 | 
136 | struct OptimizationParameters
137 | {
138 | 	float resTol, pTol;
139 | 	int maxIter;
140 | 	int maxIterSingleLevel;
141 | 	vector<float> pyramidSmoothingVariance;
142 | 	float presmoothingVariance;
143 | 	int nControlPointsOnEdge;
144 | 	float borderThicknessHorizontal;
145 | 	float borderThicknessVertical;
146 | 	bool bAdaptativeChoiceOfPoints;
147 | 	bool bNormalizeDescriptors;
148 | 
149 | 	friend ostream &operator<<(ostream &out, OptimizationParameters optParam)     //output
150 | 	{
151 | 		out<<" ***** optimization parameters ************" << endl;
152 | 		out<<"res tol : "<<optParam.resTol<<"\n";
153 | 		out<<"param tol : "<<optParam.pTol<<"\n";
154 | 		out<<"max iter : "<<optParam.maxIter<<"\n";
155 | 		out<<"max iter single level: "<<optParam.maxIterSingleLevel<<"\n";
156 | 		out<<"presmoothing variance : "<<optParam.presmoothingVariance<<"\n";
157 | 		out<<"pyramid smoothing var : [";
158 | 		for (uint i(0); i < optParam.pyramidSmoothingVariance.size(); ++i)
159 | 			out<<optParam.pyramidSmoothingVariance[i]<<",  ";
160 | 		out<<"]\n";
161 | 		out<<"n control points on edge : "<<optParam.nControlPointsOnEdge<<"\n";
162 | 		out<<"border thickness horizontal: "<<optParam.borderThicknessHorizontal<<"\n";
163 | 		out<<"border thickness vertical: "<<optParam.borderThicknessVertical<<"\n";
164 | 		out<<"adaptative choice of points : "<<optParam.bAdaptativeChoiceOfPoints<<"\n";
165 | 		out<<"normalization of intensity : "<<optParam.bNormalizeDescriptors<<"\n";
166 | 
167 | 		out<<" *****************" << endl;
168 | 		return out;
169 | 	}
170 | 
171 | };
172 | 
173 | #endif /* TYPEDEFS_HPP_ */
174 | 


--------------------------------------------------------------------------------
/include/Utilities.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 | Copyright 2014 Alberto Crivellaro, Ecole Polytechnique Federale de Lausanne (EPFL), Switzerland.
 3 | alberto.crivellaro@epfl.ch
 4 | 
 5 | terms of the GNU General Public License as published by the Free Software
 6 | Foundation; either version 2 of the License, or (at your option) any later
 7 | version.
 8 | 
 9 | This program is distributed in the hope that it will be useful, but WITHOUT ANY
10 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
11 | PARTICULAR PURPOSE. See the GNU General Public License for more details.
12 | 
13 | You should have received a copy of the GNU General Public License along with
14 | this program; if not, write to the Free Software Foundation, Inc., 51 Franklin
15 | Street, Fifth Floor, Boston, MA 02110-1301, USA
16 |  */
17 | 
18 | #ifndef UTLITIES_HPP_
19 | #define UTLITIES_HPP_
20 | 
21 | #include <vector>
22 | #include <string>
23 | 
24 | #include "Typedefs.hpp"
25 | #include <fstream>
26 | #include <iostream>
27 | #include "Homography.hpp"
28 | #include <sstream>
29 | #include <vector>
30 | 
31 | using namespace cv;
32 | using namespace std;
33 | 
34 | //dense descriptors utilities
35 | void ComputeGradientBasedDescriptorFields(Mat &grayscaleImage, vector<Mat> &outDescriptorFields);
36 | void ComputeGradientMagnitudeDescriptorFields(Mat &grayscaleImage, vector<Mat> &outDescriptorFields);
37 | void ComputeImageIntensityDescriptorFields(Mat &grayscaleImage, vector<Mat> &outDescriptorFields);
38 | vector<Mat> SmoothDescriptorFields(const float sigma,const vector<Mat> & descriptorFields);
39 | 
40 | 
41 | // control points grids utilities 
42 | StructOfArray2di CreateGridOfControlPoints(Mat & image,uint nPoints, float widthBorderThickness, float heightBorderThickness);
43 | StructOfArray2di CreateAnisotropicGridOfControlPoints(Mat & image, uint nPoints, float widthBorderThickness = 0., float heightBorderThickness = 0.);
44 | StructOfArray2di CreateDenseGridOfControlPoints(uint width, uint height);
45 | StructOfArray2di GetRectangleCornersForAugmentation(OptimizationParameters* optimizationParameters, int width, int height);
46 | 
47 | void WarpGridOfControlPoints(const StructOfArray2di & pixelsOnTemplate, StructOfArray2di & pixelsOnTemplateWarped, const vector<float> & parameters, const int width, const int height);
48 | inline cv::Matx33f GetScalingMatrix()
49 | {
50 | 	float scalex = (MAX_SCALING - MIN_SCALING)/(PICTURE_WIDTH);
51 | 	float scaley = (MAX_SCALING - MIN_SCALING)/(PICTURE_HEIGHT);
52 | 	return cv::Matx33f(scalex, 0, MIN_SCALING, 0, scaley, MIN_SCALING, 0, 0, 1);
53 | }
54 | 
55 | 
56 | //image acquisition and filtering utilities
57 | void AcquireVGAGrayscaleImage(VideoCapture &capture, Mat &outGrayImage, Mat &outRGBImage);
58 | void AcquireVGAGrayscaleImage(VideoCapture &capture, Mat &outGrayImage);
59 | void ConvertImageToFloat(Mat & image);
60 | Mat SmoothImage(const float sigma, const Mat & im);
61 | void ComputeImageDerivatives(const Mat & image, Mat & imageDx, Mat &imageDy);
62 | void NormalizeImage(Mat &image);
63 | 
64 | 
65 | //IO files utilities
66 | vector<Point2f> ReadArrayOf2dPoints(const char* fileName);
67 | vector<float> ReadArrayOfFloats(const char* fileName);
68 | vector<vector<float> > ReadMatrixOfFloats(const char* fileName);
69 | Mat ReadGrayscaleImageFile(const char* fileName, uint nRows, uint nCols);
70 | void LoadImage(const char* fileName, Mat &image);
71 | OptimizationParameters ReadOptimizationParametersFromXML(const char* fileName);
72 | void WritePixelsOnTxtFile(const StructOfArray2di & pixels, const char* fileName);
73 | 
74 | 
75 | //Visualization utilities
76 | void ShowConvergenceResults(Mat & templ, Mat &image,  vector<vector<float> > &intermediateGuess);
77 | void ShowDetailedOptimizationResults(const AlignmentResults & results, vector<float> parametersBaseline);
78 | void ShowConvergenceResults(Mat & templ, Mat &image, vector<vector<float> > &intermediateGuess, StructOfArray2di panelCorners);
79 | void AugmentFrameWithQuadrilater(string windowName, const StructOfArray2di & panelCorners, Mat& frame);
80 | void AugmentFrameWithQuadrilater(const StructOfArray2di & warpedPixels, Mat& frame);
81 | void WriteResultsOnImage(Mat & image, const AlignmentResults & results, int pixelsNumber, OptimizationType optimizationType);
82 | void operator<<( std::ostream& os, const OptimizationType& optimizationType );
83 | void CheckMatrixForNans(Eigen::MatrixXf & aMatrix);
84 | 
85 | #endif /* UTLITIES_HPP_*/
86 | 


--------------------------------------------------------------------------------
/src/Homography.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | Copyright 2014 Alberto Crivellaro, Ecole Polytechnique Federale de Lausanne (EPFL), Switzerland.
  3 | alberto.crivellaro@epfl.ch
  4 | 
  5 | terms of the GNU General Public License as published by the Free Software
  6 | Foundation; either version 2 of the License, or (at your option) any later
  7 | version.
  8 | 
  9 | This program is distributed in the hope that it will be useful, but WITHOUT ANY
 10 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
 11 | PARTICULAR PURPOSE. See the GNU General Public License for more details.
 12 | 
 13 | You should have received a copy of the GNU General Public License along with
 14 | this program; if not, write to the Free Software Foundation, Inc., 51 Franklin
 15 | Street, Fifth Floor, Boston, MA 02110-1301, USA
 16 | */
 17 | 
 18 | #include "Homography.hpp"
 19 | 
 20 | void Homography::ComputeWarpedHomoPoints(const Point3f & inputPixel, Point3f & outputPixel,const vector<float> & p)
 21 | {
 22 | 	outputPixel.x =((1+p[0])*inputPixel.x + p[2]*inputPixel.y + p[4]*inputPixel.z);
 23 | 	outputPixel.y = (p[1]*inputPixel.x + (1+p[3])*inputPixel.y + p[5]*inputPixel.z);
 24 | 	outputPixel.z = (p[6]*inputPixel.x + p[7]*inputPixel.y + inputPixel.z);
 25 | 	outputPixel.x /= outputPixel.z;
 26 | 	outputPixel.y /= outputPixel.z;
 27 | 	outputPixel.z /= outputPixel.z;
 28 | }
 29 | 
 30 | Matx33f Homography::GetMatrix(const vector<float>& p)
 31 | {
 32 | 	return Matx33f(1+p[0], p[2], p[4], p[1], 1+p[3], p[5], p[6],p[7], 1);
 33 | }
 34 | 
 35 | Matx33f Homography::GetUnscaledMatrix(const vector<float> &p)
 36 | {
 37 | 	float scalex = (MAX_SCALING - MIN_SCALING)/(PICTURE_WIDTH-1);
 38 | 	float scaley = (MAX_SCALING - MIN_SCALING)/(PICTURE_HEIGHT-1);
 39 | 	cv::Matx33f scalingMatrix(scalex, 0, MIN_SCALING, 0, scaley, MIN_SCALING, 0, 0, 1);
 40 | 	cv::Matx33f homographyMatrix = Homography::GetMatrix(p);
 41 | 	return scalingMatrix.inv() * homographyMatrix * scalingMatrix;
 42 | }
 43 | 
 44 | vector<float> Homography::GetScaledParam(const vector<float> &unscaled_p)
 45 | {
 46 | 	float scalex = (MAX_SCALING - MIN_SCALING)/(PICTURE_WIDTH-1);
 47 | 	float scaley = (MAX_SCALING - MIN_SCALING)/(PICTURE_HEIGHT-1);
 48 | 	cv::Matx33f scalingMatrix(scalex, 0, MIN_SCALING, 0, scaley, MIN_SCALING, 0, 0, 1);
 49 | 	cv::Matx33f unscaledHomographyMatrix = Homography::GetMatrix(unscaled_p);
 50 | 	cv::Matx33f scaledHomographyMatrix = scalingMatrix * unscaledHomographyMatrix * scalingMatrix.inv();
 51 | 
 52 | 	vector<float> scaled_p(8);
 53 |     scaled_p[0] = scaledHomographyMatrix(0,0) - 1;
 54 |     scaled_p[2] = scaledHomographyMatrix(0,1);
 55 |     scaled_p[4] = scaledHomographyMatrix(0,2);
 56 |     scaled_p[1] = scaledHomographyMatrix(1,0);
 57 |     scaled_p[3] = scaledHomographyMatrix(1,1) - 1;
 58 |     scaled_p[5] = scaledHomographyMatrix(1,2);
 59 |     scaled_p[6] = scaledHomographyMatrix(2,0);
 60 |     scaled_p[7] = scaledHomographyMatrix(2,1);
 61 | 
 62 | 	return scaled_p;
 63 | }
 64 | 
 65 | void Homography::ComputeWarpJacobian(const int inputPixelx, const int inputPixely, const vector<float>& p, Eigen::Matrix<float, 2, N_PARAM> &warpJacobian)
 66 | {
 67 | 
 68 | 	float scalex = (MAX_SCALING - MIN_SCALING)/(PICTURE_WIDTH-1);
 69 | 	float scaley = (MAX_SCALING - MIN_SCALING)/(PICTURE_HEIGHT-1);
 70 | 
 71 | 	float xScaled = MIN_SCALING + scalex * inputPixelx;
 72 | 	float yScaled = MIN_SCALING + scaley * inputPixely;
 73 | 
 74 | 	float denom = 1./(1 + p[6]*xScaled + p[7]*yScaled);
 75 | 	// first row
 76 | 	warpJacobian(0,0) = xScaled*denom;
 77 | 	warpJacobian(0,1) = 0.;
 78 | 	warpJacobian(0,2) = yScaled*denom;
 79 | 	warpJacobian(0,3) = 0.;
 80 | 	warpJacobian(0,4) = 1*denom;
 81 | 	warpJacobian(0,5) = 0.;
 82 | 	warpJacobian(0,6) =-xScaled*((1+p[0])*xScaled + p[2]*yScaled + p[4])*(denom*denom);
 83 | 	warpJacobian(0,7) = -yScaled*((1+p[0])*xScaled + p[2]*yScaled + p[4])*(denom*denom);
 84 | 	// second row
 85 | 	warpJacobian(1,0) = 0.;
 86 | 	warpJacobian(1,1) = xScaled*denom;
 87 | 	warpJacobian(1,2) = 0.;
 88 | 	warpJacobian(1,3) = yScaled*denom;
 89 | 	warpJacobian(1,4) = 0.;
 90 | 	warpJacobian(1,5) = 1*denom;
 91 | 	warpJacobian(1,6) =-xScaled*(p[1]*xScaled +(1+ p[3])*yScaled + p[5])*(denom*denom);
 92 | 	warpJacobian(1,7) = -yScaled*(p[1]*xScaled +(1+ p[3])*yScaled + p[5])*(denom*denom);
 93 | 
 94 | 	////
 95 | 	Eigen::Matrix<float, 2, 2> inverseScaleJacobian;
 96 | 	inverseScaleJacobian << 1/scalex, 0, 0, 1/scaley;
 97 | 	warpJacobian = inverseScaleJacobian * warpJacobian;
 98 | }
 99 | 
100 | void Homography::InverseWarpParameters(const vector<float>& directP, vector<float>& invP)
101 | {
102 | 	float det = (1+directP[0])*((1+directP[3] - directP[5]*directP[7]))
103 | 			- directP[2]*(directP[1] - directP[5]*directP[6]) +
104 | 			directP[4]*(directP[1]*directP[7]- directP[6]*(1+directP[3]));
105 | 
106 | 	float denom = det * ((1+directP[0]) * (1+directP[3]) - directP[1]*directP[2]);
107 | 
108 | 	invP[0] = (1 + directP[3] - directP[5]*directP[7]) / denom - 1;
109 | 	invP[1] =(-directP[1] + directP[5]*directP[6]) /denom;
110 | 	invP[2] = (-directP[2] + directP[4]*directP[7])/denom;
111 | 	invP[3] = (1 + directP[0] - directP[4]*directP[6])/denom - 1;
112 | 	invP[4] = (-directP[4] -directP[3]*directP[4] + directP[2]*directP[5])/denom;
113 | 	invP[5] = (-directP[5] -directP[0]*directP[5] + directP[1]*directP[4])/denom;
114 | 	invP[6] = (-directP[6] -directP[3]*directP[6] + directP[1]*directP[7])/denom;
115 | 	invP[7] = (-directP[7] -directP[0]*directP[7] + directP[2]*directP[6])/denom;
116 | 
117 | }
118 | 
119 | void Homography::InverseWarpParameters(const Eigen::Matrix<float, N_PARAM, 1> &directP, vector<float>& invP)
120 | {
121 | 	//  TODO shoot me in the face if I leave this as it is!!!
122 | 	float det = (1+directP(0,0))*((1+directP(3,0) - directP(5,0)*directP(7,0)))
123 | 			- directP(2,0)*(directP(1,0) - directP(5,0)*directP(6,0)) +
124 | 			directP(4,0)*(directP(1,0)*directP(7,0)- directP(6,0)*(1+directP(3,0)));
125 | 
126 | 	float denom = det * ((1+directP(0,0)) * (1+directP(3,0)) - directP(1,0)*directP(2,0));
127 | 
128 | 	invP[0] = (1 + directP(3,0) - directP(5,0)*directP(7,0)) / denom - 1;
129 | 	invP[1] =(-directP(1,0) + directP(5,0)*directP(6,0)) /denom;
130 | 	invP[2] = (-directP(2,0) + directP(4,0)*directP(7,0))/denom;
131 | 	invP[3] = (1 + directP(0,0) - directP(4,0)*directP(6,0))/denom - 1;
132 | 	invP[4] = (-directP(4,0) -directP(3,0)*directP(4,0) + directP(2,0)*directP(5,0))/denom;
133 | 	invP[5] = (-directP(5,0) -directP(0,0)*directP(5,0) + directP(1,0)*directP(4,0))/denom;
134 | 	invP[6] = (-directP(6,0) -directP(3,0)*directP(6,0) + directP(1,0)*directP(7,0))/denom;
135 | 	invP[7] = (-directP(7,0) -directP(0,0)*directP(7,0) + directP(2,0)*directP(6,0))/denom;
136 | 
137 | }
138 | 
139 | vector<float> Homography::ParametersUpdateCompositional(const vector<float> &p,const vector<float>& deltap)
140 | {
141 | 	vector<float> newP(8);
142 | 	float denom = 1 + deltap[6]*p[4]  + deltap[7] * p[5];
143 | 
144 | 	newP[0] = (deltap[0] + p[0] + deltap[0]*p[0] + deltap[2]*p[1] + deltap[4]*p[6] - deltap[6]*p[4] - deltap[7]*p[5])/denom;
145 | 	newP[1] = (deltap[1] + p[1] + deltap[1]*p[0] + deltap[3]*p[1] + deltap[5]*p[6])/denom;
146 | 	newP[2] = (deltap[2] + p[2] + deltap[0]*p[2] + deltap[2]*p[3] + deltap[4]*p[7])/denom;
147 | 	newP[3] = (deltap[3] + p[3] + deltap[1]*p[2] + deltap[3]*p[3] + deltap[5]*p[7] - deltap[6]*p[4] - deltap[7]*p[5])/denom;
148 | 	newP[4] = (deltap[4] + p[4] + deltap[0]*p[4] + deltap[2]*p[5])/denom;
149 | 	newP[5] = (deltap[5] + p[5] + deltap[1]*p[4] + deltap[3]*p[5])/denom;
150 | 	newP[6] = (deltap[6] + p[6] + deltap[6]*p[0] + deltap[7]*p[1])/denom;
151 | 	newP[7] = (deltap[7] + p[7] + deltap[6]*p[2] + deltap[7]*p[3])/denom;
152 | 	return newP;
153 | }
154 | 
155 | void Homography::CompositionalParametersUpdateWithCheck(vector<float> &p,const vector<float>& deltap)
156 | {
157 |   vector<float> newP = Homography::ParametersUpdateCompositional(p, deltap);
158 |   // Check if the result is valid and update
159 |   // TODO re-enable check with less restrictive parameters
160 |   // if (CheckHomography(newP))
161 |     p = newP;
162 |   // else
163 |   //   cout << "ATTENTION! NOT UPDATING PARAMETERS"<<endl;
164 | }
165 | 
166 | // Check if the Homography is a good one (from BRIEF demo, which is from MVG book)
167 | bool Homography::CheckHomography(const vector<float>& p)
168 | {
169 | //	return true;
170 | //	return Matx33f(1+p[0], p[2], p[4], p[1], 1+p[3], p[5], p[6],p[7], 1);
171 | // 	The homography matrix looks like this
172 | //	[1+p[0], p[2]  , p[4]]
173 | //	[p[1]  , 1+p[3], p[5]]
174 | //	[p[6]  , p[7]  , 1   ]
175 | 
176 | //	// just to not keep adding the same thing ??
177 | //	float p0_1 = (1.0+p[0]);
178 | //	float p3_1 = (1.0+p[3]);
179 | 
180 |   const float det = (1.0+p[0]) * (1.0+p[3]) - p[1] * p[2];
181 |   if (det < 0){
182 |     //		cout << "failed condition 1" << endl;
183 |     return false;
184 |   }
185 | 
186 |   const float N1 = sqrt((1.0+p[0]) * (1.0+p[0]) + p[1] * p[1]);
187 |   if (N1 > 4 || N1 < 0.1){
188 |     //		cout << "failed condition 2" << endl;
189 |     return false;
190 |   }
191 | 
192 |   const float N2 = sqrt(p[2] * p[2] + (1.0+p[3]) * (1.0+p[3]));
193 |   if (N2 > 4 || N2 < 0.1){
194 |     //		cout << "failed condition 3" << endl;
195 |     return false;
196 |   }
197 | 
198 |   const float N3 = sqrt(p[6] * p[6] + p[7] * p[7]);
199 |   if (N3 > 0.002){
200 |     //		cout << "failed condition 4 - N3 = " << N3 << "p6=" << p[6]<< "p7=" << p[7]<< endl;
201 |     return false;
202 |   }
203 | 
204 |   //	cout << "succeeded!" << endl;
205 |   return true;
206 | }
207 | 
208 | void Homography::DisplayParameters(const vector<float>& parameters)
209 | {
210 | 	cout<<" homography: ";
211 | 	for(uint i(0); i< parameters.size();++i)
212 | 		cout<< parameters[i]<< "  ";
213 | 	cout<<endl;
214 | }
215 | 
216 | 
217 | 
218 | 
219 | 
220 | 


--------------------------------------------------------------------------------
/src/IterativeOptimization.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | Copyright 2014 Alberto Crivellaro, Ecole Polytechnique Federale de Lausanne (EPFL), Switzerland.
  3 | alberto.crivellaro@epfl.ch
  4 | 
  5 | terms of the GNU General Public License as published by the Free Software
  6 | Foundation; either version 2 of the License, or (at your option) any later
  7 | version.
  8 | 
  9 | This program is distributed in the hope that it will be useful, but WITHOUT ANY
 10 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
 11 | PARTICULAR PURPOSE. See the GNU General Public License for more details.
 12 | 
 13 | You should have received a copy of the GNU General Public License along with
 14 | this program; if not, write to the Free Software Foundation, Inc., 51 Franklin
 15 | Street, Fifth Floor, Boston, MA 02110-1301, USA
 16 |  */
 17 | 
 18 | #include "IterativeOptimization.hpp"
 19 | 
 20 | using namespace std;
 21 | using namespace cv;
 22 | 
 23 | AlignmentResults IterativeOptimization::SSDCalibration(const StructOfArray2di & pixelsOnTemplate, Mat &grayscaleFloatTemplate, Mat &grayscaleFloatImage, vector<float> & parameters, OptimizationParameters & optimizationParameters)
 24 | {
 25 | 	vector<Mat> templateDescriptorFields, imageDescriptorFields;
 26 | 
 27 | 	templateDescriptorFields.push_back(grayscaleFloatTemplate.clone());
 28 | 	imageDescriptorFields.push_back(grayscaleFloatImage.clone());
 29 | 	if (optimizationParameters.bNormalizeDescriptors)
 30 | 	{
 31 | 		NormalizeImage(templateDescriptorFields[0]);
 32 | 		NormalizeImage(imageDescriptorFields[0]);
 33 | 	}
 34 | 	return PyramidMultiLevelCalibration(pixelsOnTemplate, templateDescriptorFields, imageDescriptorFields, parameters, optimizationParameters);
 35 | }
 36 | 
 37 | AlignmentResults IterativeOptimization::DescriptorFieldsCalibration(const StructOfArray2di & pixelsOnTemplate, Mat &grayscaleFloatTemplate, Mat &grayscaleFloatImage, vector<float> &parameters, OptimizationParameters & optimizationParameters)
 38 | {
 39 | 	vector<Mat> templateDescriptorFields, imageDescriptorFields;
 40 | 	
 41 | 	ComputeGradientBasedDescriptorFields(grayscaleFloatTemplate, templateDescriptorFields);
 42 | 	ComputeGradientBasedDescriptorFields(grayscaleFloatImage, imageDescriptorFields);
 43 | 
 44 | 	if (optimizationParameters.bNormalizeDescriptors)
 45 | 	{
 46 | 		for(uint i=0; i < templateDescriptorFields.size(); ++i)
 47 | 		{
 48 | 			NormalizeImage(templateDescriptorFields[i]);
 49 | 			NormalizeImage(imageDescriptorFields[i]);
 50 | 		}
 51 | 	}
 52 | 
 53 | 	return PyramidMultiLevelCalibration(pixelsOnTemplate, templateDescriptorFields, imageDescriptorFields, parameters, optimizationParameters);
 54 | }
 55 | 
 56 | AlignmentResults IterativeOptimization::GradientModuleCalibration(const StructOfArray2di & pixelsOnTemplate, Mat &grayscaleFloatTemplate, Mat &grayscaleFloatImage, vector<float> &parameters, OptimizationParameters & optimizationParameters)
 57 | {
 58 | 	vector<Mat> templateDescriptorFields, imageDescriptorFields;
 59 | 	
 60 | 	ComputeGradientMagnitudeDescriptorFields(grayscaleFloatTemplate, templateDescriptorFields);
 61 | 	ComputeGradientMagnitudeDescriptorFields(grayscaleFloatImage, imageDescriptorFields);
 62 | 
 63 | 	if (optimizationParameters.bNormalizeDescriptors)
 64 | 	{
 65 | 		NormalizeImage(templateDescriptorFields[0]);
 66 | 		NormalizeImage(imageDescriptorFields[0]);
 67 | 	}
 68 | 
 69 | 	return PyramidMultiLevelCalibration(pixelsOnTemplate, templateDescriptorFields, imageDescriptorFields, parameters, optimizationParameters);
 70 | }
 71 | 
 72 | 
 73 | AlignmentResults IterativeOptimization::PyramidMultiLevelCalibration(const StructOfArray2di & pixelsOnTemplate, vector<Mat> &templateDescriptorFields, vector<Mat> &imageDescriptorFields, vector<float> & parameters, OptimizationParameters & optimizationParameters)
 74 | {
 75 | 	//TODO: information in pixelsOnTemplate is redundant, find a better way (compute it inside the function ?)
 76 | 	AlignmentResults alignmentResults, tempResults;
 77 | 	alignmentResults.nIter = 0;
 78 | 
 79 | 	if(optimizationParameters.pyramidSmoothingVariance.empty())
 80 | 	{
 81 | 		optimizationParameters.maxIterSingleLevel = max(optimizationParameters.maxIter, optimizationParameters.maxIterSingleLevel);
 82 | 		alignmentResults = GaussNewtonMinimization(pixelsOnTemplate, imageDescriptorFields, templateDescriptorFields, optimizationParameters, parameters);
 83 | 		return alignmentResults;
 84 | 	}
 85 | 
 86 | 	float originalPTol = optimizationParameters.pTol;
 87 | 	float originalResTol = optimizationParameters.resTol;
 88 | 	optimizationParameters.pTol = optimizationParameters.pTol * 10;
 89 | 	optimizationParameters.resTol = optimizationParameters.resTol * 10;
 90 | 	Mat smoothedImage, smoothedTemplate;
 91 | 
 92 | 	vector< vector<Mat> > smoothedImages( optimizationParameters.pyramidSmoothingVariance.size());
 93 | 	vector< vector<Mat> > smoothedTemplates( optimizationParameters.pyramidSmoothingVariance.size());
 94 | 
 95 | #pragma omp parallel for
 96 | 	for (int i=0;i<optimizationParameters.pyramidSmoothingVariance.size();i++)
 97 | 	{
 98 | 		smoothedImages[i] = SmoothDescriptorFields(optimizationParameters.pyramidSmoothingVariance[i], imageDescriptorFields);
 99 | 		smoothedTemplates[i] = SmoothDescriptorFields(optimizationParameters.pyramidSmoothingVariance[i], templateDescriptorFields);
100 | 	}
101 | 	for (int iLevel = 0;iLevel < optimizationParameters.pyramidSmoothingVariance.size();iLevel++)
102 | 	{
103 | 		if (iLevel == optimizationParameters.pyramidSmoothingVariance.size()-1)
104 | 		{
105 | 			optimizationParameters.maxIterSingleLevel = optimizationParameters.maxIter - alignmentResults.nIter;
106 | 			optimizationParameters.pTol = originalPTol;
107 | 			optimizationParameters.resTol = originalResTol;
108 | 		}
109 | 
110 | #ifdef VERBOSE_OPT
111 | 		cout<<"Start using pyramid level no."<< iLevel + 1<<endl;
112 | #endif
113 | 		tempResults = GaussNewtonMinimization(pixelsOnTemplate, smoothedImages[iLevel], smoothedTemplates[iLevel], optimizationParameters, parameters);
114 | 
115 | 		alignmentResults.poseIntermediateGuess.insert(alignmentResults.poseIntermediateGuess.end(), tempResults.poseIntermediateGuess.begin(), tempResults.poseIntermediateGuess.end());
116 | 		alignmentResults.residualNorm.insert(alignmentResults.residualNorm.end(), tempResults.residualNorm.begin(), tempResults.residualNorm.end());
117 | 		alignmentResults.nIter +=tempResults.nIter;
118 | 		alignmentResults.exitFlag = tempResults.exitFlag;
119 | 
120 | #ifdef VERBOSE_OPT
121 | 		cout<<"Ended using pyramid level no."<<iLevel+1<<" after "<< tempResults.nIter<<" iterations with local exit flag  "<<tempResults.exitFlag<<endl;
122 | #endif
123 | 		if (alignmentResults.nIter > optimizationParameters.maxIter)
124 | 			break;
125 | 
126 | 		if(alignmentResults.nIter > 10 && tempResults.exitFlag <= 0)
127 | 		{
128 | 			if (iLevel == optimizationParameters.pyramidSmoothingVariance.size()-1)
129 | 				break;
130 | 			else
131 | 				iLevel = optimizationParameters.pyramidSmoothingVariance.size()-1;
132 | 		}
133 | 	}
134 | 	return alignmentResults;
135 | }
136 | 
137 | 
138 | float IterativeOptimization::ComputeResidualNorm(vector<float> &errorImage)
139 | {
140 | 	float residualNorm(0.);
141 | 	for(uint i(0); i<errorImage.size(); ++i)
142 | 		residualNorm+=errorImage[i]*errorImage[i];
143 | 	residualNorm/=errorImage.size();
144 | 	return sqrt(residualNorm);
145 | }
146 | 
147 | 
148 | int IterativeOptimization::CheckConvergenceOptimization(float deltaPoseNorm, int nIter, float residualNormIncrement, OptimizationParameters optParam)
149 | {
150 | 	if (residualNormIncrement < optParam.resTol)
151 | 		return -1;
152 | 	else if (deltaPoseNorm < optParam.pTol)
153 | 		return 0;
154 | 	else if (nIter >= optParam.maxIterSingleLevel)
155 | 		return 1;
156 | 	return 1e6;
157 | }
158 | 
159 | inline float estimatorL1L2(const float res)
160 | {
161 | 	//estimator
162 | 	//http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html
163 | 	return res/sqrt((1+res*res/2));
164 | }
165 | 
166 | inline float estimatorGerman(const float res)
167 | {
168 | 	//estimator
169 | 	//http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html
170 | 	return res/((1+res*res)*(1+res*res));
171 | }
172 | 
173 | inline float estimatorTukey(const float res)
174 | {
175 | 	//estimator
176 | 	//http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html
177 | 	const float thrs = 1;
178 | 	if (abs(res) > thrs)
179 | 	{
180 | 		//cout<<"0 virgule qqxchose "<<res<<endl;
181 | 		return std::numeric_limits<float>::infinity();// // KMYI: Below needs to be removed and everything would be fine?
182 | 	}
183 | 	else
184 | 		return res*(1-(res/thrs)*(res/thrs))*(1-(res/thrs)*(res/thrs));
185 | }
186 | 
187 | enum class estimator_t {L2,L1L2,Turkey,German};
188 | 
189 | float estimator(const float res, const estimator_t e = estimator_t::L1L2)
190 | {
191 | 	switch(e)
192 | 	{
193 | 	case estimator_t::L2:
194 | 		return res;//L2
195 | 	case estimator_t::L1L2:
196 | 		return estimatorL1L2(res);
197 | 	case estimator_t::Turkey:
198 | 		return estimatorTukey(res);
199 | 	case estimator_t::German:
200 | 		return estimatorGerman(res);
201 | 	}
202 | }
203 | 
204 | void IterativeOptimization::ComputeResiduals(const Mat &image,  vector<float> & templatePixelIntensities, const StructOfArray2di & warpedPixels, vector<float> & errorImage)
205 | {
206 | #pragma omp parallel for
207 | 	for(int iPoint = 0; iPoint < warpedPixels.size(); ++iPoint)
208 | 	{
209 | 		float res = 0;
210 | 		if((warpedPixels).x[iPoint] >= 0 && (warpedPixels).x[iPoint] < image.cols && (warpedPixels).y[iPoint] >= 0 && (warpedPixels).y[iPoint] < image.rows)
211 | 		{
212 | 			res = templatePixelIntensities[iPoint] - ((float*)image.data)[image.cols * warpedPixels.y[iPoint] + warpedPixels.x[iPoint]];
213 | 			errorImage[iPoint] = estimator(res);
214 | 		}
215 | 	}
216 | }
217 | 
218 | void IterativeOptimization::ComputeWarpedPixels(const StructOfArray2di & pixelsOnTemplate, const vector<float>&  parameters, StructOfArray2di & warpedPixels)
219 | {
220 | 	if(warpedPixels.size()!= pixelsOnTemplate.size())
221 | 		warpedPixels.resize(pixelsOnTemplate.size());
222 | 
223 | #pragma omp parallel for
224 | 	for(int iPoint = 0; iPoint < pixelsOnTemplate.size(); ++iPoint)
225 | 	{
226 | 		int x, y;
227 | 		Homography::ComputeWarpedPixels(pixelsOnTemplate.x[iPoint],pixelsOnTemplate.y[iPoint], x,y, parameters);
228 | 		warpedPixels.x[iPoint] = x;
229 | 		warpedPixels.y[iPoint] = y;
230 | 	}
231 | }
232 | 
233 | void IterativeOptimization::AssembleSDImages(const vector<float>&  parameters, const Mat &imageDx, const Mat &imageDy, const StructOfArray2di & warpedPixels, const vector<Eigen::Matrix<float, 2, N_PARAM>, Eigen::aligned_allocator<Eigen::Matrix<float, 2, N_PARAM> > > & warpJacobians,  Eigen::MatrixXf & sdImages)
234 | {
235 | 
236 | 	// KMYI: OMP for parallization
237 | 	const int stop = warpedPixels.size();
238 | #pragma omp parallel for
239 | 	for(int iPoint = 0; iPoint < stop; ++iPoint)
240 | 	{
241 | 		float imDx, imDy;//, val;
242 | 		//val = 0;
243 | 		if((warpedPixels).x[iPoint] >= 0 && (warpedPixels).x[iPoint] < imageDx.cols && (warpedPixels).y[iPoint] >= 0 && (warpedPixels).y[iPoint] < imageDx.rows)
244 | 		{
245 | 			// KMYI: let's try to calculate only once assuming same size for imageDx and iamge Dy
246 | 			int warpedIdx = imageDx.cols * warpedPixels.y[iPoint] + warpedPixels.x[iPoint];
247 | 			imDx = ((float*)imageDx.data)[warpedIdx];
248 | 			imDy = ((float*)imageDy.data)[warpedIdx];
249 | 			for(int iParam = 0; iParam < parameters.size(); ++iParam)
250 | 				sdImages(iPoint, iParam) = imDx * (warpJacobians[iPoint])(0, iParam) + imDy * (warpJacobians[iPoint])(1, iParam);
251 | 		}
252 | 		else
253 | 			for(int iParam = 0; iParam < parameters.size(); ++iParam)
254 | 				sdImages(iPoint, iParam) = 0;
255 | 	}
256 | }
257 | 
258 | 
259 | 
260 | 
261 | AlignmentResults LucasKanade::GaussNewtonMinimization(const StructOfArray2di & pixelsOnTemplate, const vector<Mat> & images, const vector<Mat> & templates, const OptimizationParameters optParam, vector<float> & parameters)
262 | {
263 | 
264 |  	AlignmentResults alignmentResults;
265 |  	alignmentResults.nIter = 0;
266 |  	alignmentResults.exitFlag = 1e6;
267 | 
268 | 	//parameters must contain the initial guess. It is updated during optimization
269 | 	uint nChannels(images.size());
270 | 	vector<vector<float> > templatePixelIntensities(nChannels,vector<float>(pixelsOnTemplate.size()));
271 | 	vector<Mat> imageDx(nChannels), imageDy(nChannels);
272 | 	uint nParam = parameters.size();
273 | 
274 |     vector<Eigen::Matrix<float, 2, N_PARAM>, Eigen::aligned_allocator<Eigen::Matrix<float, 2, N_PARAM> > >
275 |             warpJacobians(pixelsOnTemplate.size());
276 | 
277 | 	Eigen::MatrixXf sdImages(pixelsOnTemplate.size(), nParam);
278 | 	vector<float>  errorImage(pixelsOnTemplate.size(), 0.0);
279 | 
280 | 	StructOfArray2di warpedPixels;
281 | 	Eigen::Matrix<float, N_PARAM, N_PARAM> hessian;
282 | 	Eigen::Matrix<float, N_PARAM, 1> rhs;
283 | 	Eigen::Matrix<float, N_PARAM, 1> deltaParam;
284 | 
285 | 	for(int iChannel = 0; iChannel<nChannels; ++iChannel)
286 | 	{
287 | 		ComputeImageDerivatives(images[iChannel], imageDx[iChannel], imageDy[iChannel]);
288 | 
289 | 		for(int iPoint = 0; iPoint < pixelsOnTemplate.size(); ++iPoint)
290 | 		{
291 | 			int pos = templates[iChannel].cols* pixelsOnTemplate.y[iPoint] + pixelsOnTemplate.x[iPoint];
292 | 			//if (pos < templates[iChannel].total())
293 | 			templatePixelIntensities[iChannel][iPoint] = ((float*)templates[iChannel].data)[pos];
294 | 			//else
295 | 			//	cout<<"tooo big "<<pos<<" / "<<templates[iChannel].size()<<endl;
296 | 
297 | 		}
298 | 	}
299 | 
300 | 	while (alignmentResults.exitFlag == 1e6)
301 | 	{
302 | 		hessian.setZero();
303 | 		rhs.setZero();
304 | 
305 | 		ComputeWarpedPixels(pixelsOnTemplate, parameters, warpedPixels);
306 | 
307 | 
308 | #pragma omp parallel for
309 | 		for (int iPoint = 0; iPoint < pixelsOnTemplate.size(); ++iPoint)
310 | 			Homography::ComputeWarpJacobian(pixelsOnTemplate.x[iPoint], pixelsOnTemplate.y[iPoint], parameters, warpJacobians[iPoint]);
311 | 
312 | 
313 | 		//#pragma omp parallel for //reduction(+ : hessian)
314 | 		for(int iChannel = 0; iChannel<images.size(); ++iChannel)
315 | 		{
316 | 			AssembleSDImages(parameters, imageDx[iChannel], imageDy[iChannel], warpedPixels, warpJacobians, sdImages);
317 | 			ComputeResiduals(images[iChannel], templatePixelIntensities[iChannel], warpedPixels, errorImage);// no need to put errorImage(outsidePixels) = 0, since corresponding row os sdImages is already 0.
318 | 
319 | 			//tmpsdImages[iChannel] = sdImages;
320 | 			//tmperrorImage[iChannel] = errorImage;// no need to put errorImage(outsidePixels) = 0, since corresponding row os sdImages is already 0.
321 | 
322 | 			/////////
323 | 			//hessian = hessian + sdImages.transpose() * sdImages;
324 | 			hessian += sdImages.transpose() * sdImages;
325 | 			//tmpHessian[iChannel] = tmpsdImages[iChannel].transpose() * tmpsdImages[iChannel];
326 | 
327 | 			for (int i = 0; i<nParam; ++i)
328 | 			{
329 | 				for(uint iPoint(0); iPoint<pixelsOnTemplate.size(); ++iPoint)
330 | 				{
331 | 					float val =	(errorImage[iPoint] == std::numeric_limits<float>::infinity() ? 0: errorImage[iPoint]); 
332 | 					//rhs(i,0) += tmpsdImages[iChannel](iPoint,i) * tmperrorImage[iChannel][iPoint];
333 | 					rhs(i,0) += sdImages(iPoint,i) * val;
334 | 				}
335 | 			}
336 | 		}
337 | 
338 | 		deltaParam = hessian.fullPivLu().solve(rhs);
339 | 
340 | 		for(int i = 0; i<nParam; ++i)
341 | 			parameters[i] += deltaParam(i,0);
342 | 
343 | 		alignmentResults.poseIntermediateGuess.push_back(parameters);
344 | 		alignmentResults.residualNorm.push_back(ComputeResidualNorm(errorImage));
345 | 		if(alignmentResults.nIter > 0)
346 | 			alignmentResults.exitFlag = CheckConvergenceOptimization(deltaParam.norm(), alignmentResults.nIter, abs(alignmentResults.residualNorm[alignmentResults.nIter] - alignmentResults.residualNorm[alignmentResults.nIter-1]), optParam);
347 | 		alignmentResults.nIter++;
348 | 
349 | #ifdef VERBOSE_OPT
350 | 		cout<< "iteration n. " <<alignmentResults.nIter<<endl;
351 | 		//		cout<< " hessian: "<<endl<<hessian<<endl;
352 | 		cout<<"parameters"<<endl;
353 | 		for(uint i(0); i<nParam; ++i)
354 | 		{
355 | 			cout<<"     ["<< i<<"]-> "<< parameters[i];
356 | 		}
357 | 		cout<<endl;
358 | #endif
359 | 
360 | 	}
361 | 	return alignmentResults;
362 | }
363 | 


--------------------------------------------------------------------------------
/src/Utilities.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | Copyright 2014 Alberto Crivellaro, Ecole Polytechnique Federale de Lausanne (EPFL), Switzerland.
  3 | alberto.crivellaro@epfl.ch
  4 | 
  5 | terms of the GNU General Public License as published by the Free Software
  6 | Foundation; either version 2 of the License, or (at your option) any later
  7 | version.
  8 | 
  9 | This program is distributed in the hope that it will be useful, but WITHOUT ANY
 10 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
 11 | PARTICULAR PURPOSE. See the GNU General Public License for more details.
 12 | 
 13 | You should have received a copy of the GNU General Public License along with
 14 | this program; if not, write to the Free Software Foundation, Inc., 51 Franklin
 15 | Street, Fifth Floor, Boston, MA 02110-1301, USA
 16 |  */
 17 | 
 18 | #include "Utilities.hpp"
 19 | 
 20 | using namespace std;
 21 | using namespace cv;
 22 | 
 23 | 
 24 | 
 25 | void ComputeImageIntensityDescriptorFields(Mat &grayscaleImage, vector<Mat> &outDescriptorFields)
 26 | {
 27 | 	outDescriptorFields.clear();
 28 | 	outDescriptorFields.push_back(grayscaleImage.clone());
 29 | }
 30 |                                                                                                   
 31 | void ComputeGradientBasedDescriptorFields(Mat & grayscaleImage, vector<Mat> &outDescriptorFields)
 32 | {	
 33 | 	outDescriptorFields.clear();
 34 | 	Mat dx, dy;
 35 | 	ComputeImageDerivatives(grayscaleImage, dx, dy);
 36 | 	assert(dx.isContinuous());
 37 | 	assert(dy.isContinuous());
 38 | 
 39 | 	Size imSize = grayscaleImage.size();
 40 | 	Mat dxPos(imSize, CV_32F, Scalar(0));
 41 | 	Mat dxNeg(imSize, CV_32F, Scalar(0));
 42 | 	Mat dyPos(imSize, CV_32F, Scalar(0));
 43 | 	Mat dyNeg(imSize, CV_32F, Scalar(0));
 44 | 
 45 | 	float dxPixel, dyPixel;
 46 | 
 47 | 	//TODO: use cv::threshold instead of !
 48 | 	//TODO: add thresholding for eliminating noise
 49 | 	for (int iRow(0); iRow<grayscaleImage.rows; ++iRow)
 50 | 	{
 51 | 		for (int iCol(0); iCol<grayscaleImage.cols; ++iCol)
 52 | 		{
 53 | 			dxPixel = ((float*)dx.data)[dx.cols * iRow + iCol];//dx.at<float>(iRow, iCol);
 54 | 			dyPixel = ((float*)dy.data)[dx.cols * iRow + iCol];//dy.at<float>(iRow, iCol);
 55 | 
 56 | 			if(dxPixel>0)
 57 | 				((float*)dxPos.data)[dx.cols * iRow + iCol] = 10*dxPixel;//10 is just a factor for numerical stability, with no particular meaning
 58 | 			else
 59 | 				((float*)dxNeg.data)[dx.cols * iRow + iCol] = -10*dxPixel;
 60 | 
 61 | 			if(dyPixel>0)
 62 | 				((float*)dyPos.data)[dx.cols * iRow + iCol] = 10*dyPixel;
 63 | 			else
 64 | 				((float*)dyNeg.data)[dx.cols * iRow + iCol] = -10*dyPixel;
 65 | 		}
 66 | 	}
 67 | 	outDescriptorFields.push_back(dxPos);
 68 | 	outDescriptorFields.push_back(dxNeg);
 69 | 	outDescriptorFields.push_back(dyPos);
 70 | 	outDescriptorFields.push_back(dyNeg);
 71 | 
 72 | }
 73 | 
 74 | void ComputeGradientMagnitudeDescriptorFields(Mat &grayscaleImage, vector<Mat> &outDescriptorFields)
 75 | {
 76 | 	outDescriptorFields.clear();
 77 | 	Mat dx, dy;
 78 | 	ComputeImageDerivatives(grayscaleImage, dx, dy);
 79 | 	assert(dx.isContinuous());
 80 | 	assert(dy.isContinuous());
 81 | 	dx = dx.mul(dx);
 82 | 	dy = dy.mul(dy);
 83 | 	outDescriptorFields.push_back(dx + dy);
 84 | }
 85 | 
 86 | 
 87 | 
 88 | StructOfArray2di CreateGridOfControlPoints(Mat & image,uint nPoints, float widthBorderThickness, float heightBorderThickness)
 89 | {
 90 | 	uint width = image.cols;
 91 | 	uint height = image.rows;
 92 | 	assert(width > 0);
 93 | 	assert(height > 0);
 94 | 
 95 | 	StructOfArray2di controlPoints;
 96 | 	float deltau(((width-1) -2*widthBorderThickness)/nPoints);
 97 | 	float deltav(((height-1) -2*heightBorderThickness)/nPoints);
 98 | 
 99 | 	int nPointsi, nPointsj;
100 | 	nPointsi = nPointsj = nPoints;
101 | 
102 | 	if(deltau < 1) //avoid repeated points
103 | 	{
104 | 		deltau = 1.;
105 | 		nPointsj = ((width-1) -2*widthBorderThickness);
106 | 	}
107 | 
108 | 	if(deltav < 1)
109 | 	{
110 | 		deltav = 1.;
111 | 		nPointsi = ((height-1) -2*heightBorderThickness);
112 | 	}
113 | 
114 | 	for(uint iPoint(0); iPoint < nPointsi; ++iPoint)
115 | 	{
116 | 		for(uint jPoint(0); jPoint < nPointsj; ++jPoint)
117 | 			controlPoints.push_back(Point(widthBorderThickness + jPoint*deltau, heightBorderThickness+ iPoint*deltav));
118 | 	}
119 | 	return controlPoints;
120 | }
121 | 
122 | // KMYI: use GFT to initialize controlpoints
123 | StructOfArray2di CreateAnisotropicGridOfControlPoints(Mat & image, uint nPoints, float widthBorderThickness, float heightBorderThickness)
124 | 
125 | {
126 | 	nPoints = nPoints*nPoints; // square the nPoints for compatibility with grid
127 | 	/// Set Image ROI
128 | 	int width = image.cols;
129 | 	int height = image.rows;
130 | 	//	if (widthBorderThickness == 0.)
131 | 	//		widthBorderThickness = (float)width/10;
132 | 	//	if (heightBorderThickness == 0.)
133 | 	//		heightBorderThickness = (float)height/10;
134 | 	Mat roiImg(image, Rect(widthBorderThickness, heightBorderThickness, width - 2*widthBorderThickness, height - 2*heightBorderThickness));
135 | 
136 | 	/// Get descriptor fields
137 | 	vector<Mat> descImg;
138 | 	ComputeGradientBasedDescriptorFields(roiImg, descImg);
139 | 	Mat magImg = roiImg.clone();
140 | 	Mat magImg2 = roiImg.clone();
141 | 	magImg = descImg[0].mul(descImg[0])+descImg[1].mul(descImg[1])+descImg[2].mul(descImg[2])+descImg[3].mul(descImg[3]);
142 | 	cv::sqrt(magImg,magImg2);
143 | 
144 | 	/// Parameters for Shi-Tomasi algorithm
145 | 	vector<Point2f> corners;
146 | 	double qualityLevel = 0.01;
147 | 	double minDistance = 5;
148 | 	int blockSize = 3;
149 | 	bool useHarrisDetector = false;
150 | 	double k = 0.01;//0.04;
151 | 
152 | 	/// Apply corner detection
153 | 	goodFeaturesToTrack( magImg2,
154 | 			corners,
155 | 			nPoints,
156 | 			qualityLevel,
157 | 			minDistance,
158 | 			Mat(),
159 | 			blockSize,
160 | 			useHarrisDetector,
161 | 			k );
162 | 
163 | 
164 | 	StructOfArray2di controlPoints;
165 | 	nPoints = MIN(nPoints, corners.size());
166 | 	for(uint idxPoint(0); idxPoint < nPoints; ++idxPoint)
167 | 	{
168 | 		int cur_j = round(corners[idxPoint].x) + widthBorderThickness;
169 | 		int cur_i = round(corners[idxPoint].y) + heightBorderThickness;
170 | 		controlPoints.push_back(Point(cur_j, cur_i));
171 | 	}
172 | 	return controlPoints;
173 | }
174 | 
175 | StructOfArray2di CreateDenseGridOfControlPoints(uint width, uint height)
176 | {
177 | 	StructOfArray2di controlPoints;
178 | 	for(uint jPoint(0); jPoint < width; ++jPoint)
179 | 	{
180 | 		for(uint iPoint(0); iPoint < height; ++iPoint)
181 | 			controlPoints.push_back(Point(jPoint, iPoint));
182 | 	}
183 | 	return controlPoints;
184 | }
185 | 
186 | 
187 | void WarpGridOfControlPoints(const StructOfArray2di & pixelsOnTemplate, StructOfArray2di & pixelsOnTemplateWarped, const vector<float> & parameters, const int width, const int height)
188 | {
189 | 	pixelsOnTemplateWarped.clear();
190 | 	pixelsOnTemplateWarped.reserve(pixelsOnTemplate.size());//TODO : useless?
191 | 	//StructOfArray2di tempPixels(pixelsOnTemplate.size());
192 | 
193 | 	vector<vector<bool> > takeMeOn(height,vector<bool>(width,false));
194 | 
195 | #pragma omp parallel for ordered
196 | 	for (int iPoint = 0; iPoint < pixelsOnTemplate.size(); ++iPoint)
197 | 	{
198 | 		int x,y;
199 | 		Homography::ComputeWarpedPixels(pixelsOnTemplate.x[iPoint], pixelsOnTemplate.y[iPoint], x, y, parameters);
200 | 		if (x >=0 && y >= 0 && x < width && y < height)
201 | 		{
202 | #pragma omp ordered
203 | 			if (!takeMeOn[y][x])
204 | 			{
205 | 				takeMeOn[y][x] = true;
206 | 				pixelsOnTemplateWarped.push_back(x, y);
207 | 			}
208 | 
209 | 		}
210 | 	}
211 | }
212 | 
213 | StructOfArray2di GetRectangleCornersForAugmentation(OptimizationParameters* optimizationParameters, int width, int height)
214 | {
215 | 	StructOfArray2di  panelCorners(4);
216 | 	panelCorners.x[0] = optimizationParameters->borderThicknessHorizontal;
217 | 	panelCorners.y[0] = optimizationParameters->borderThicknessVertical;
218 | 	panelCorners.x[1] = width-optimizationParameters->borderThicknessHorizontal;
219 | 	panelCorners.y[1] = optimizationParameters->borderThicknessVertical;
220 | 	panelCorners.x[2] = width-optimizationParameters->borderThicknessHorizontal;
221 | 	panelCorners.y[2] = height-optimizationParameters->borderThicknessVertical;
222 | 	panelCorners.x[3] = optimizationParameters->borderThicknessHorizontal;
223 | 	panelCorners.y[3] = height-optimizationParameters->borderThicknessVertical;
224 | 	return panelCorners;
225 | }
226 | 
227 | 
228 | void AcquireVGAGrayscaleImage(VideoCapture &capture, Mat &outGrayImage, Mat &outRGBImage)
229 | {
230 | 	capture >> outRGBImage;
231 | 	resize(outRGBImage, outRGBImage, Size(640,480));
232 | 	cvtColor(outRGBImage, outGrayImage,CV_RGB2GRAY, 1);
233 | 	ConvertImageToFloat(outGrayImage);
234 | }
235 | 
236 | void AcquireVGAGrayscaleImage(VideoCapture &capture, Mat &outGrayImage)
237 | {
238 | 	Mat tempImg;
239 | 	capture >> tempImg;
240 | 	resize(tempImg, tempImg, Size(640,480));
241 | 	cvtColor(tempImg, outGrayImage,CV_RGB2GRAY, 1);
242 | 	ConvertImageToFloat(outGrayImage);
243 | }
244 | 
245 | void ConvertImageToFloat(Mat & image)
246 | {
247 | 	//image.convertTo(image, CV_32F);
248 | 	double min,max;
249 | 	minMaxLoc(image,&min,&max);
250 | 	const float v = 1.0/(max - min);
251 | 	image.convertTo(image, CV_32F, v, -min * v);
252 | 	assert(image.isContinuous());
253 | }
254 | 
255 | 
256 | void ComputeImageDerivatives(const Mat & image, Mat & imageDx, Mat &imageDy)
257 | {
258 | 	int ddepth = -1; //same image depth as source
259 | 	double scale = 1/32.0;// normalize wrt scharr mask for having exact gradient
260 | 	double delta = 0;
261 | 
262 | 	Scharr(image, imageDx, ddepth, 1, 0, scale, delta, BORDER_REFLECT );
263 | 	Scharr(image, imageDy, ddepth, 0, 1, scale, delta, BORDER_REFLECT );
264 | }
265 | 
266 | Mat SmoothImage(const float sigma, const Mat &im)
267 | {
268 | 	Mat smoothedImage;
269 | 	int s = max(5, 2*int(sigma)+1);
270 | 	Size kernelSize(s, s);
271 | 	GaussianBlur(im, smoothedImage, kernelSize, sigma, sigma,BORDER_REFLECT);
272 | 	return smoothedImage;
273 | }
274 | 
275 | vector<Mat> SmoothDescriptorFields(const float sigma, const vector<Mat> & descriptorFields)
276 | {
277 | 	vector<Mat> smoothedDescriptorFields(descriptorFields.size());
278 | 
279 | #pragma omp parallel for
280 | 	for(int iChannel = 0; iChannel < descriptorFields.size(); ++iChannel){
281 | 		smoothedDescriptorFields[iChannel] = SmoothImage(sigma, descriptorFields[iChannel]);}
282 | 
283 | 	return smoothedDescriptorFields;
284 | }
285 | 
286 | 
287 | 
288 | void NormalizeImage(Mat &image)
289 | {
290 | 	Scalar mean, stddev;
291 | 	meanStdDev(image, mean, stddev);
292 | 	image = (image - mean)/stddev[0];
293 | }
294 | 
295 | vector<Point2f> ReadArrayOf2dPoints(const char* fileName)
296 | {
297 | 	ifstream aFileStream;
298 | 	aFileStream.open(fileName);
299 | 	if(!aFileStream)
300 | 	{
301 | 		cerr << "FILE COULD NOT BE OPENED !!!!!!"<<endl;
302 | 		exit(0);
303 | 	}
304 | 	vector<Point2f> pointsArray;
305 | 	float a, b;
306 | 	while (aFileStream >> a >> b)
307 | 	{
308 | 		Vec2f tempPoint(a,b);
309 | 		pointsArray.push_back(tempPoint);
310 | 	}
311 | 	aFileStream.close();
312 | 	return pointsArray;
313 | }
314 | 
315 | vector<float> ReadArrayOfFloats(const char* fileName)
316 | {
317 | 	ifstream aFileStream;
318 | 	aFileStream.open(fileName);
319 | 	if(!aFileStream)
320 | 	{
321 | 		cerr << "FILE COULD NOT BE OPENED !!!!!!"<<endl;
322 | 		exit(0);
323 | 	}
324 | 	vector<float> data;
325 | 	float a;
326 | 	while (aFileStream >> a)
327 | 	{
328 | 		data.push_back(a);
329 | 	}
330 | 	aFileStream.close();
331 | 	return data;
332 | }
333 | 
334 | vector<vector<float> > ReadMatrixOfFloats(const char* fileName)
335 | {
336 | 	vector<vector<float> > data;
337 | 	string lineString;
338 | 	double a;
339 | 
340 | 	ifstream aFileStream;
341 | 	aFileStream.open(fileName);
342 | 	if(!aFileStream)
343 | 	{
344 | 		cerr << "FILE COULD NOT BE OPENED !!!!!!"<<endl;
345 | 		exit(0);
346 | 	}
347 | 
348 | 	while(getline(aFileStream, lineString))
349 | 	{
350 | 		stringstream is(lineString);
351 | 		vector<float> aDataRow;
352 | 		while (is >> a)
353 | 		{
354 | 			aDataRow.push_back(a);
355 | 		}
356 | 		data.push_back(aDataRow);
357 | 	}
358 | 
359 | 	aFileStream.close();
360 | 	return data;
361 | }
362 | 
363 | 
364 | 
365 | void WritePixelsOnTxtFile(const StructOfArray2di & pixels, const char* fileName)
366 | {
367 | 	ofstream myfile;
368 | 	myfile.open (fileName);
369 | 	if(!myfile)
370 | 	{
371 | 		cerr << "FILE COULD NOT BE OPENED !!!!!!"<<endl;
372 | 		exit(0);
373 | 	}
374 | 	for ( int i(0); i < pixels.size(); ++i)
375 | 		myfile << pixels.x[i] << " " << pixels.y[i]<<endl;;
376 | 	myfile.close();
377 | }
378 | 
379 | 
380 | void LoadImage(const char* fileName, Mat &image)
381 | {
382 | 	image = imread(fileName, CV_LOAD_IMAGE_GRAYSCALE);
383 | 	image.convertTo(image, CV_32F);
384 | 	//	Size res(cameraInternalParameters.width, cameraInternalParameters.height);
385 | 	//	resize(image, image, res);
386 | 	double min,max;
387 | 	minMaxLoc(image,&min,&max);
388 | 	image = (image - min)/(max - min);
389 | }
390 | 
391 | Mat ReadGrayscaleImageFile(const char* fileName, uint nRows, uint nCols)
392 | {
393 | 	Mat image(nRows, nCols, CV_32F);
394 | 
395 | 	ifstream aFileStream;
396 | 	aFileStream.open(fileName);
397 | 	if(!aFileStream)
398 | 	{
399 | 		cerr << "FILE COULD NOT BE OPENED !!!!!!"<<endl;
400 | 		exit(0);
401 | 	}
402 | 	aFileStream.seekg(0, ios::beg);
403 | 	for(uint iRow(0); iRow < nRows; ++iRow)
404 | 	{
405 | 		for(uint iCol(0); iCol < nCols; ++iCol)
406 | 			aFileStream >>image.at<float>(iRow, iCol);
407 | 	}
408 | 	return image;
409 | }
410 | 
411 | OptimizationParameters ReadOptimizationParametersFromXML(const char* fileName)
412 | {
413 | 	FileStorage fs2(fileName, FileStorage::READ);
414 | 	if (!fs2.isOpened())
415 | 	{
416 | 		cerr << "Failed to open: !" <<  fileName<<"  . Aborting" <<endl;
417 | 		exit(0);
418 | 	}
419 | 	OptimizationParameters optParam;
420 | 	fs2["resTol"] >> optParam.resTol;
421 | 	fs2["pTol"] >> optParam.pTol;
422 | 	fs2["maxIter"] >> optParam.maxIter;
423 | 	fs2["maxIterSingleLevel"] >> optParam.maxIterSingleLevel;
424 | 
425 | 	FileNode n = fs2["pyramidSmoothingVariance"];
426 | 	if (n.type() == FileNode::SEQ)
427 | 	{
428 | 		FileNodeIterator it = n.begin(), it_end = n.end(); // Go through the node
429 | 		for (; it != it_end; ++it)
430 | 			optParam.pyramidSmoothingVariance.push_back((float)*it);
431 | 	}
432 | 	fs2["presmoothingVariance"] >> optParam.presmoothingVariance;
433 | 	fs2["nControlPointsOnEdge"] >> optParam.nControlPointsOnEdge;
434 | 	fs2["borderThicknessHorizontal"] >> optParam.borderThicknessHorizontal;
435 | 	fs2["borderThicknessVertical"] >> optParam.borderThicknessVertical;
436 | 	fs2["bAdaptativeChoiceOfPoints"] >> optParam.bAdaptativeChoiceOfPoints;
437 | 	fs2["bNormalizeDescriptors"] >> optParam.bNormalizeDescriptors;
438 | 
439 | 	fs2.release();
440 | 	return optParam;
441 | }
442 | 
443 | void ShowDetailedOptimizationResults(const AlignmentResults & results, vector<float> parametersBaseline)
444 | {
445 | 	float paramNorm2(0);
446 | 	cout<<endl<<endl<< "************  Optimization results  *******************"<<endl;
447 | 	cout<< "exit flag  : "<< results.exitFlag<< "       - (1->exceeded maxIter; 0,-1->converged)"<<endl;
448 | 	cout<< "Number of iterations: "<< results.nIter<<endl;
449 | 	cout<< "estimation of the pose  : "<<endl;
450 | 	for(uint iIter(0); iIter < results.poseIntermediateGuess.size();++iIter)
451 | 	{
452 | 		paramNorm2 = 0;
453 | 		for (uint iParam(0); iParam < 8;++iParam)
454 | 			paramNorm2 += pow((parametersBaseline[iParam] - (results.poseIntermediateGuess[iIter])[iParam]), 2);
455 | 
456 | 		cout<<" iter:"<<iIter<<" --> res. norm = " << results.residualNorm[iIter]<<";param. error norm =     " << paramNorm2<< endl;
457 | 	}
458 | }
459 | 
460 | void ShowConvergenceResults(Mat & templ, Mat &image, vector<vector<float> > &intermediateGuess)
461 | {
462 | 	StructOfArray2di panelCorners(4);
463 | 	panelCorners.x[0] = 1;
464 | 	panelCorners.y[0] = 1;
465 | 	panelCorners.x[1] = templ.cols-1;
466 | 	panelCorners.y[1] = 1;
467 | 	panelCorners.x[2] = templ.cols-1;
468 | 	panelCorners.y[2] = templ.rows-1;
469 | 	panelCorners.x[3] = 1;
470 | 	panelCorners.y[3] = templ.rows-1;
471 | 
472 | 	ShowConvergenceResults(templ, image, intermediateGuess, panelCorners);
473 | }
474 | 
475 | 
476 | void ShowConvergenceResults(Mat & templ, Mat &image, vector<vector<float> > &intermediateGuess, StructOfArray2di panelCorners)
477 | {
478 | 	StructOfArray2di warpedPixels(4);
479 | 	namedWindow("Shot template", CV_WINDOW_AUTOSIZE );
480 | 	AugmentFrameWithQuadrilater("Shot template", panelCorners,templ);
481 | 	waitKey(0);
482 | 	namedWindow("ConvergenceResults", CV_WINDOW_AUTOSIZE );
483 | 	int x,y;
484 | 	for(uint i(0); i < 4; ++i)
485 | 	{
486 | 		Homography::ComputeWarpedPixels(panelCorners[i].x,panelCorners[i].y, x,y, intermediateGuess[intermediateGuess.size()-1]);
487 | 		warpedPixels.x[i] = x;
488 | 		warpedPixels.y[i] = y;
489 | 	}
490 | 
491 | 	//	AugmentFrameWithQuadrilater("ConvergenceResults", warpedPixels, shotImage.image);
492 | 	//	waitKey(0);
493 | 	for (uint iGuess(0); iGuess < intermediateGuess.size(); ++iGuess)
494 | 	{
495 | 		int x,y;
496 | 		for(uint i(0); i < 4; ++i)
497 | 		{
498 | 			Homography::ComputeWarpedPixels(panelCorners[i].x,panelCorners[i].y, x,y, intermediateGuess[iGuess]);
499 | 			warpedPixels.y[i] = y;
500 | 		}
501 | 
502 | 		Mat warpedImage = image.clone();
503 | 		AugmentFrameWithQuadrilater("ConvergenceResults", warpedPixels, warpedImage);
504 | 		for(int i(0); i< 100000; ++i)
505 | 			;
506 | 		if (iGuess == intermediateGuess.size()-1)
507 | 			waitKey(0);
508 | 	}
509 | }
510 | 
511 | void AugmentFrameWithQuadrilater(string windowName, const StructOfArray2di & warpedPixels, Mat& frame)
512 | {
513 | 	Scalar color(1.,1.,1.,0.1);
514 | 	int thickness=3;
515 | 	int lineType=8;
516 | 	int shift=0;
517 | 	line(frame, warpedPixels[0], warpedPixels[1], color, thickness, lineType, shift);
518 | 	line(frame, warpedPixels[1], warpedPixels[2], color, thickness, lineType, shift);
519 | 	line(frame, warpedPixels[2], warpedPixels[3], color, thickness, lineType, shift);
520 | 	line(frame, warpedPixels[0], warpedPixels[3], color, thickness, lineType, shift);
521 | 	//		// 		fillConvexPoly(image, &panelCorners[0], 4, color);
522 | 	imshow(windowName, frame);
523 | }
524 | 
525 | void AugmentFrameWithQuadrilater(const StructOfArray2di & warpedPixels, Mat& frame)
526 | {
527 | 	Scalar color(1.,1.,1.,0.1);
528 | 	int thickness=3;
529 | 	int lineType=8;
530 | 	int shift=0;
531 | 	line(frame, warpedPixels[0], warpedPixels[1], color, thickness, lineType, shift);
532 | 	line(frame, warpedPixels[1], warpedPixels[2], color, thickness, lineType, shift);
533 | 	line(frame, warpedPixels[2], warpedPixels[3], color, thickness, lineType, shift);
534 | 	line(frame, warpedPixels[0], warpedPixels[3], color, thickness, lineType, shift);
535 | 	//		// 		fillConvexPoly(image, &panelCorners[0], 4, color);
536 | 	//imshow(windowName, frame);
537 | }
538 | 
539 | void WriteResultsOnImage(Mat & image, const AlignmentResults & results, int pixelsNumber, OptimizationType optimizationType)
540 | {
541 | 	ostringstream str;
542 | 	str << "Optimization : " << optimizationType;
543 | 	putText(image, str.str(), Point(10,50), FONT_HERSHEY_TRIPLEX, 1.2, CV_RGB(255,255,255));
544 | 
545 | 	str.str("");
546 | 	str << "exit flag:" << results.exitFlag;
547 | 	putText(image, str.str(), Point(10,80), FONT_HERSHEY_DUPLEX, 1, CV_RGB(255,255,255));
548 | 
549 | 	str.str("");
550 | 	str << "n. iterations:" << results.nIter;
551 | 	putText(image, str.str(), Point(10,110), FONT_HERSHEY_DUPLEX,1, CV_RGB(255,255,255));
552 | 
553 | 	str.str("");
554 | 	str << "final residual norm:" << results.residualNorm.back();
555 | 	putText(image, str.str(), Point(10,140), FONT_HERSHEY_DUPLEX, 1, CV_RGB(255,255,255));
556 | 
557 | 	str.str("");
558 | 	str << "number of pixels :" << pixelsNumber;
559 | 	putText(image, str.str(), Point(10,170), FONT_HERSHEY_DUPLEX, 1, CV_RGB(255,255,255));
560 | }
561 | 
562 | void CheckMatrixForNans(Eigen::MatrixXf & aMatrix)
563 | {
564 | 	for(uint i(0); i< aMatrix.rows(); ++i)
565 | 	{
566 | 		for(uint j(0); j< aMatrix.cols(); ++j)
567 | 		{
568 | 			if(aMatrix(i,j) != aMatrix(i,j))
569 | 			{
570 | 				cout<<"found nan at "<<i << " "<<j << endl;
571 | 				break;
572 | 			}
573 | 		}
574 | 	}
575 | }
576 | 
577 | void operator<<( std::ostream& os, const OptimizationType& optimizationType )
578 | {
579 | 	switch( optimizationType )
580 | 	{
581 | 	case intensity: os << "intensity"; break;
582 | 	case gradientModule: os << "gradientModule"; break;
583 | 	case descriptorFields: os << "descriptorFields"; break;
584 | 	}
585 | }
586 | 
587 | 
588 | 


--------------------------------------------------------------------------------
/unit/UnitTests.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 | Copyright 2014 Alberto Crivellaro, Ecole Polytechnique Federale de Lausanne (EPFL), Switzerland.
  3 | alberto.crivellaro@epfl.ch
  4 | 
  5 | terms of the GNU General Public License as published by the Free Software
  6 | Foundation; either version 2 of the License, or (at your option) any later
  7 | version.
  8 | 
  9 | This program is distributed in the hope that it will be useful, but WITHOUT ANY
 10 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
 11 | PARTICULAR PURPOSE. See the GNU General Public License for more details.
 12 | 
 13 | You should have received a copy of the GNU General Public License along with
 14 | this program; if not, write to the Free Software Foundation, Inc., 51 Franklin
 15 | Street, Fifth Floor, Boston, MA 02110-1301, USA
 16 |  */
 17 | 
 18 | 
 19 | 
 20 | #include "UnitTests.hpp"
 21 | 	
 22 | using namespace cv;
 23 | using namespace std;
 24 | using namespace std::chrono;
 25 | 
 26 | //#define B_PLOT
 27 | 
 28 | void ReadDataFromXMLTest()
 29 | {
 30 | 	OptimizationParameters optParam = ReadOptimizationParametersFromXML("parameter/parametersForTest.yml");
 31 | 
 32 | 	float bOk = true;
 33 | 
 34 | 	if(abs(optParam.resTol - 0.005) > 1e-9)	bOk = false;
 35 | 	if(abs(optParam.pTol - 0.000004) > 1e-9)	bOk = false;
 36 | 	if(optParam.maxIter != 100)	bOk = false;
 37 | 	if(optParam.maxIterSingleLevel != 10)	bOk = false;
 38 | 	if(optParam.pyramidSmoothingVariance[0] != 35)	bOk = false;
 39 | 	if(optParam.pyramidSmoothingVariance[1] != 17)	bOk = false;
 40 | 	if(optParam.pyramidSmoothingVariance[2] != 9)	bOk = false;
 41 | 	if(optParam.pyramidSmoothingVariance[3] != 5)	bOk = false;
 42 | 	if(optParam.presmoothingVariance != 2)	bOk = false;
 43 | 	if(optParam.nControlPointsOnEdge != 40)	bOk = false;
 44 | 	if(optParam.borderThicknessHorizontal != 10)	bOk = false;
 45 | 	if(abs(optParam.borderThicknessVertical - 10.4) > 1e-6)	bOk = false;
 46 | 	if(optParam.bAdaptativeChoiceOfPoints != 0)	bOk = false;
 47 | 	if(optParam.bNormalizeDescriptors != 0)	bOk = false;
 48 | 
 49 | 	if (bOk ==false)
 50 | 		throw "ReadDataFromXMLTest failed";
 51 | 
 52 | }
 53 | 
 54 | 
 55 | void InverseHomographyTest()
 56 | {
 57 | 	Mat otherParameters(8, 1, CV_32F);
 58 | 	vector<float> parameters(8,0);
 59 | 	vector<float> parametersInv(8);
 60 | 	float eps(1);
 61 | 
 62 | 	for(uint iIter(0); iIter < 1000; ++iIter)
 63 | 	{
 64 | 		for(uint i(0); i < 8; ++i)
 65 | 		{
 66 | 			parameters[i] = (float)rand()/RAND_MAX;
 67 | 			otherParameters.at<float>(i,0) = parameters[i];
 68 | 		}
 69 | 
 70 | 		Point3f inputPixel(1+(rand()% 1000), 1+(rand()% 1000), 1);
 71 | 		Point3f outputPixel, outputPixel2;
 72 | 
 73 | 		Homography::InverseWarpParameters(parameters, parametersInv);
 74 | 		Homography::ComputeWarpedHomoPoints(inputPixel, outputPixel, parameters);
 75 | 		float denom((outputPixel.x/inputPixel.x));
 76 | 		outputPixel.x /= denom;
 77 | 		outputPixel.y /= denom;
 78 | 		outputPixel.z /= denom;
 79 | 
 80 | 		Homography::ComputeWarpedHomoPoints(outputPixel, outputPixel2, parametersInv);
 81 | 		denom = (outputPixel2.x/outputPixel.x);
 82 | 		outputPixel2.x /= denom;
 83 | 		outputPixel2.y /= denom;
 84 | 
 85 | 		if (((abs(inputPixel.x - outputPixel2.x)) < eps) == 0 || ((abs(inputPixel.y - outputPixel2.y)) < eps) == 0)
 86 | 		{
 87 | 			cout << "point  input"<<  inputPixel.x << " " << inputPixel.y<<endl;
 88 | 			cout << "point output " <<outputPixel2.x << " " << outputPixel2.y<<endl;
 89 | 		}
 90 | 
 91 | 		Homography::InverseWarpParameters(otherParameters, parameters);
 92 | 		for(uint i(0); i < 8; ++i)
 93 | 			if	(abs(parameters[i]- parametersInv[i]) > 1e-10) throw "InverseHomographyTest failed" ;
 94 | 
 95 | 
 96 | 		if(abs(inputPixel.x - outputPixel2.x) > eps) throw "InverseHomographyTest failed" ;
 97 | 		if(abs(inputPixel.y - outputPixel2.y) > eps) throw "InverseHomographyTest failed" ;
 98 | 	}
 99 | }
100 | 
101 | void CompositionHomographyTest()
102 | {
103 | 	vector<float> parameters(8,0);
104 | 	vector<float> parametersNew(8);
105 | 	vector<float> deltap(8);
106 | 
107 | 	float eps(1e-4);
108 | 	for(uint iIter(0); iIter < 1000; ++iIter)
109 | 	{
110 | 		for(uint i(0); i < 8; ++i)
111 | 		{
112 | 			parameters[i] = (float)rand()/ RAND_MAX;
113 | 			deltap[i] = (float)rand()/ RAND_MAX;
114 | 		}
115 | 
116 | 		Point3f inputPixel(1+rand()%1000, 1+rand()%1000, 1);
117 | 		Point3f outputPixel, outputPixel2, outputPixelComp;
118 | 
119 | 		Homography::ComputeWarpedHomoPoints(inputPixel, outputPixel,parameters );
120 | 		Homography::ComputeWarpedHomoPoints(outputPixel, outputPixel2, deltap);
121 | 
122 | 		vector<float> newP = Homography::ParametersUpdateCompositional(parameters, deltap);
123 | 		Homography::ComputeWarpedHomoPoints(inputPixel, outputPixelComp, newP);
124 | 
125 | 		if (((abs(outputPixelComp.x - outputPixel2.x)) < eps) == 0 || ((abs(outputPixelComp.y - outputPixel2.y)) < eps) == 0)
126 | 		{
127 | 			cout << "point numerical composition " <<outputPixel2.x << " " << outputPixel2.y<<endl;
128 | 			cout << "point  composition " <<outputPixelComp.x << " " << outputPixelComp.y<<endl;
129 | 			throw "CompositionHomographyTest failed!";
130 | 		}
131 | 	}
132 | }
133 | 
134 | 
135 | void HomographyJacobianTest2()
136 | {
137 | 	vector<vector<float> > jacobianBaseline = ReadMatrixOfFloats("data/jacobian.txt");
138 | 	Eigen::Matrix<float, 2, 8> warpJacobian;
139 | 	Mat templ = imread("data/template.png", CV_LOAD_IMAGE_GRAYSCALE);
140 | 	ConvertImageToFloat(templ);
141 | 	StructOfArray2di controlPoints = CreateDenseGridOfControlPoints(templ.cols, templ.rows);
142 | 	vector<float> parameters(8,0);
143 | 	parameters[4] = 8.7021963e+01-20;
144 | 	parameters[5] = 3.0940031e+01 - 20;
145 | 
146 | 	for(uint iPoint(0); iPoint <controlPoints.size() ; ++iPoint)
147 | 	{
148 | 		controlPoints.x[iPoint] += 1;//matlab matrices are 1 based
149 | 		controlPoints.y[iPoint] += 1;
150 | 
151 | 		Homography::ComputeWarpJacobian(controlPoints.x[iPoint], controlPoints.y[iPoint], parameters, warpJacobian);
152 | 		for (uint i(0); i < 8; ++i)
153 | 		{
154 | 			if(abs(warpJacobian(0, i) -(jacobianBaseline[2*iPoint])[i]) > 5e-3) throw "HomographyJacobianTest2 failed";
155 | 			if(abs(warpJacobian(1, i) -(jacobianBaseline[2*iPoint+1])[i]) > 5e-3) throw "HomographyJacobianTest2 failed";
156 | 		}
157 | 	}
158 | }
159 | 
160 | 
161 | void ComputeHomographyTest()
162 | {
163 | 	vector<float> parameters(8,0);
164 | 	float eps(1);
165 | 
166 | 	for(uint iIter(0); iIter < 1000; ++iIter)
167 | 	{
168 | 		for(uint i(0); i < 8; ++i)
169 | 			parameters[i] = (float)rand()/RAND_MAX;
170 | 
171 | 		Point3f inputPixel(1+(rand()% 1000), 1+(rand()% 1000), 1);
172 | 		Point3f outputPixel, outputPixel2;
173 | 		Homography::ComputeWarpedHomoPoints(inputPixel, outputPixel, parameters);
174 | 
175 | 		Matx33f aMatrix = Homography::GetMatrix(parameters);
176 | 		outputPixel2 = aMatrix * inputPixel;
177 | 		outputPixel2.x /= outputPixel2.z;
178 | 		outputPixel2.y /= outputPixel2.z;
179 | 		outputPixel2.z /= outputPixel2.z;
180 | 
181 | 		Point inputPixel2d, outputPixel2d;
182 | 		inputPixel2d.x = (int)inputPixel.x;
183 | 		inputPixel2d.y = (int)inputPixel.y;
184 | 
185 | 		Homography::ComputeWarpedPixels(inputPixel2d.x , inputPixel2d.y, outputPixel2d.x, outputPixel2d.y, parameters);
186 | 		if(abs(outputPixel.x - outputPixel2d.x) > eps)throw "ComputeHomographyTest failed";
187 | 		if(abs(outputPixel.y - outputPixel2d.y) > eps)throw "ComputeHomographyTest failed";
188 | 		if(abs(outputPixel.x - outputPixel2.x) > eps)throw "ComputeHomographyTest failed";
189 | 		if(abs(outputPixel.y - outputPixel2.y) > eps)throw "ComputeHomographyTest failed";
190 | 	}
191 | }
192 | 
193 | 
194 | void WarpedIntensitiesTest()
195 | {
196 | 
197 | 	Mat templ = imread("data/template.png", CV_LOAD_IMAGE_GRAYSCALE);
198 | 	ConvertImageToFloat(templ);
199 | 
200 | 	Mat image = imread("data/imageCropped.jpg", CV_LOAD_IMAGE_GRAYSCALE);
201 | 	ConvertImageToFloat(image);
202 | 
203 | 	StructOfArray2di controlPoints = CreateDenseGridOfControlPoints(templ.cols, templ.rows);
204 | 	for (uint iPoint(0); iPoint < controlPoints.size(); ++iPoint)
205 | 	{
206 | 		controlPoints.x[iPoint] += 1;//matlab matrices are 1 based
207 | 		controlPoints.y[iPoint] += 1;
208 | 	}
209 | 	vector<float> parametersInitialGuess(8,0);
210 | 	parametersInitialGuess[4] = 8.7021963e+01-20;
211 | 	parametersInitialGuess[5] = 3.0940031e+01 - 20;
212 | 	vector<float> warpedPixelIntensities(controlPoints.size());
213 | 	LucasKanade optimization;
214 | 	StructOfArray2di warpedPixels;
215 | 	optimization.ComputeWarpedPixels(controlPoints, parametersInitialGuess, warpedPixels);
216 | 
217 | 	for(uint iPoint(0); iPoint < controlPoints.size(); ++iPoint)
218 | 		warpedPixelIntensities[iPoint] = image.at<float>(round((warpedPixels[iPoint]).y)-1, round((warpedPixels[iPoint]).x)-1);
219 | 
220 | 	vector<float> intensitiesBaseline = ReadArrayOfFloats("data/warpedIntensities.txt");
221 | 	float max(0), temp;
222 | 	for(uint iPoint(0); iPoint < warpedPixelIntensities.size(); ++iPoint)
223 | 	{
224 | 		temp = abs(warpedPixelIntensities[iPoint] - intensitiesBaseline[iPoint]);
225 | 		if (temp > 4e-2)
226 | 			cout <<warpedPixelIntensities[iPoint]<< " vs" << intensitiesBaseline[iPoint]<< "  "<<round((warpedPixels[iPoint]).y)-1 << " " << round((warpedPixels[iPoint]).x)-1<< endl;
227 | 
228 | 		if (temp > max)
229 | 			max = temp;
230 | 	}
231 | 	if (max > 4e-2)
232 | 		throw "WarpedIntensitiesTest failed.";
233 | }
234 | 
235 | 
236 | void SDImagesTest()
237 | {
238 | 	Mat templ = imread("data/template.png", CV_LOAD_IMAGE_GRAYSCALE);
239 | 	ConvertImageToFloat(templ);
240 | 
241 | 	Mat image = imread("data/imageCropped.jpg", CV_LOAD_IMAGE_GRAYSCALE);
242 | 	ConvertImageToFloat(image);
243 | 
244 | 	StructOfArray2di controlPoints = CreateDenseGridOfControlPoints(templ.cols, templ.rows);
245 | 
246 | 	for (uint iPoint(0); iPoint < controlPoints.size(); ++iPoint)
247 | 	{
248 | 		controlPoints.x[iPoint] += 1;//matlab matrices are 1 based
249 | 		controlPoints.y[iPoint] += 1;
250 | 	}
251 | 
252 | 	vector<float> parametersInitialGuess(8,0);
253 | 	parametersInitialGuess[4] = 8.7021963e+01-20;
254 | 	parametersInitialGuess[5] = 3.0940031e+01 - 20;
255 | 
256 | 	Mat imageDx = ReadGrayscaleImageFile("data/imageDx.txt", 452, 378);
257 | 	Mat imageDy = ReadGrayscaleImageFile("data/imageDy.txt", 452, 378);
258 | 	StructOfArray2di warpedPixels;
259 | 	Eigen::MatrixXf sdImages(controlPoints.size(), 8);
260 | 	LucasKanade optimization;
261 | 	optimization.ComputeWarpedPixels(controlPoints, parametersInitialGuess, warpedPixels);
262 | 	vector<Eigen::Matrix<float, 2, N_PARAM>, Eigen::aligned_allocator<Eigen::Matrix<float, 2, N_PARAM> > > warpJacobians(controlPoints.size());
263 | 	for (uint iPoint(0); iPoint < controlPoints.size(); ++iPoint)
264 | 		Homography::ComputeWarpJacobian(controlPoints.x[iPoint], controlPoints.y[iPoint], parametersInitialGuess, warpJacobians[iPoint]);
265 | 
266 | 	optimization.AssembleSDImages(parametersInitialGuess, imageDx, imageDy, warpedPixels, warpJacobians, sdImages);
267 | 	vector<vector<float> > sdImageBaseline = ReadMatrixOfFloats("data/SDImage.txt");
268 | 
269 | 	for(uint iPoint(0); iPoint < controlPoints.size(); ++iPoint)
270 | 	{
271 | 		for(uint iParam(0); iParam < 8; ++iParam)
272 | 			if(abs((sdImageBaseline[iPoint])[iParam] - sdImages(iPoint, iParam)) > 1e-2)
273 | 				throw "SDImagesTest failed";
274 | 	}
275 | }
276 | 
277 | void LucaskanadeSSDTest()
278 | {
279 | 	//we set some parameters for the optimization. Don't touch !
280 | 	OptimizationParameters optimizationParameters;
281 | 	optimizationParameters.pTol = 5e-4;
282 | 	optimizationParameters.resTol = 5e-8;
283 | 	optimizationParameters.maxIter = 200;
284 | 	optimizationParameters.maxIterSingleLevel = 10;
285 | 
286 | 	Mat templ = imread("data/template.png", CV_LOAD_IMAGE_GRAYSCALE);
287 | 	ConvertImageToFloat(templ);
288 | 	Mat image = imread("data/imageCropped.jpg", CV_LOAD_IMAGE_GRAYSCALE);
289 | 	ConvertImageToFloat(image);
290 | 
291 | 	Matx33f homography(1+4.2553191e-02, -6.4516129e-02, 8.7021963e+01,
292 | 			4.7872340e-02, 1+1.2096774e-02, 3.0940031e+01,
293 | 			0, 0, 1);
294 | 	vector<float> parametersBaseline(8);
295 | 	parametersBaseline[0] = 4.2553191e-02;
296 | 	parametersBaseline[1] = 4.7872340e-02;
297 | 	parametersBaseline[2] = -6.4516129e-02;
298 | 	parametersBaseline[3] = 1.2096774e-02;
299 | 	parametersBaseline[4] = 8.7021963e+01;
300 | 	parametersBaseline[5] = 3.0940031e+01;
301 | 	parametersBaseline[6] = 0;
302 | 	parametersBaseline[7] = 0;
303 | 
304 | 	vector<float> parametersInitialGuess(8,0);
305 | 	parametersInitialGuess[4] = 8.7021963e+01-20;
306 | 	parametersInitialGuess[5] = 3.0940031e+01 - 20;
307 | 	LucasKanade optimization;
308 | 	StructOfArray2di controlPoints = CreateDenseGridOfControlPoints(templ.cols, templ.rows);
309 | 	clock_t begin = clock();
310 | 	AlignmentResults results = optimization.SSDCalibration(controlPoints, templ, image, parametersInitialGuess, optimizationParameters);
311 | 	clock_t end = clock();
312 | 	cout<< "Elapsed time = "<< (double(end - begin) / CLOCKS_PER_SEC)<<endl;
313 | 
314 | 	float paramNorm2(0);
315 | 	for (uint iParam(0); iParam < 8;++iParam)
316 | 		paramNorm2 += pow((parametersBaseline[iParam] - (results.poseIntermediateGuess[results.poseIntermediateGuess.size()-1])[iParam]), 2);
317 | 
318 | #ifdef B_PLOT
319 | 	ShowDetailedOptimizationResults(results, parametersBaseline);
320 | 	namedWindow("Template", CV_WINDOW_AUTOSIZE );
321 | 	imshow("Template", templ);
322 | 	waitKey(0);
323 | 	namedWindow("image", CV_WINDOW_AUTOSIZE );
324 | 	imshow("image", image );
325 | 	waitKey(0);
326 | 	ShowConvergenceResults(templ, image, results.poseIntermediateGuess);
327 | #endif
328 | 
329 | 	if (paramNorm2 > 2e-2) throw "LucaskanadeSSDTest failed 1 " ;
330 | 	if (results.residualNorm[results.residualNorm.size()-1] > 0.35) throw "LucaskanadeSSDTest failed 2 " ;
331 | 	//check that parametersInitialGuess is updated with the current guess
332 | 	for (uint i(0); i< parametersInitialGuess.size(); ++i)
333 | 		if(abs(parametersInitialGuess[i] - (results.poseIntermediateGuess[results.poseIntermediateGuess.size()-1])[i]) > 1e-12) throw "LucaskanadeSSDTest failed 3" ;
334 | }
335 | 
336 | void LucaskanadeDescriptorFieldsTest()
337 | {
338 | 	//we set some parameters for the optimization. Don't touch !
339 | 	OptimizationParameters optimizationParameters;
340 | 	optimizationParameters.pTol = 5e-8;
341 | 	optimizationParameters.resTol = 5e-8;
342 | 	optimizationParameters.maxIter = 40;
343 | 	optimizationParameters.maxIterSingleLevel = 10;
344 | 	optimizationParameters.pyramidSmoothingVariance.push_back(10);
345 | 	optimizationParameters.pyramidSmoothingVariance.push_back(1);
346 | 
347 | 	Mat templ = imread("data/template.png", CV_LOAD_IMAGE_GRAYSCALE);
348 | 	ConvertImageToFloat(templ);
349 | 	Mat image = imread("data/imageCropped.jpg", CV_LOAD_IMAGE_GRAYSCALE);
350 | 	ConvertImageToFloat(image);
351 | 
352 | 	vector<float> parametersBaseline(8);
353 | 	parametersBaseline[0] = 4.2553191e-02;
354 | 	parametersBaseline[1] = 4.7872340e-02;
355 | 	parametersBaseline[2] = -6.4516129e-02;
356 | 	parametersBaseline[3] = 1.2096774e-02;
357 | 	parametersBaseline[4] = 8.7021963e+01;
358 | 	parametersBaseline[5] = 3.0940031e+01;
359 | 	parametersBaseline[6] = 0;
360 | 	parametersBaseline[7] = 0;
361 | 
362 | 	StructOfArray2di controlPoints = CreateDenseGridOfControlPoints(templ.cols, templ.rows);
363 | 
364 | 	vector<float> parametersInitialGuess(8,0);
365 | 	parametersInitialGuess[4] = 8.7021963e+01-20;
366 | 	parametersInitialGuess[5] = 3.0940031e+01 - 20;
367 | 
368 | 	LucasKanade optimization;
369 | 	clock_t begin = clock();
370 | 	AlignmentResults results = optimization.DescriptorFieldsCalibration(controlPoints, templ, image, parametersInitialGuess, optimizationParameters);
371 | 	clock_t end = clock();
372 | 	cout<< "Elapsed time = "<< (double(end - begin) / CLOCKS_PER_SEC)<<endl;
373 | 
374 | 	float paramNorm2(0);
375 | 	for (uint iParam(0); iParam < 8;++iParam)
376 | 		paramNorm2 += pow((parametersBaseline[iParam] - (results.poseIntermediateGuess[results.poseIntermediateGuess.size()-1])[iParam]), 2);
377 | 
378 | #ifdef B_PLOT
379 | 
380 | 	ShowDetailedOptimizationResults(results, parametersBaseline);
381 | 	namedWindow("Template", CV_WINDOW_AUTOSIZE );
382 | 	imshow("Template", templ);
383 | 	waitKey(0);
384 | 	namedWindow("image", CV_WINDOW_AUTOSIZE );
385 | 	imshow("image", image );
386 | 	waitKey(0);
387 | 	ShowConvergenceResults(templ, image, results.poseIntermediateGuess);
388 | #endif
389 | 
390 | 	if(paramNorm2 > 2e-2) throw "LucaskanadeDescriptorFieldsTest failed 1 " ;
391 | 	if(results.residualNorm[results.residualNorm.size()-1] > 0.3) throw "LucaskanadeDescriptorFieldsTest failed 2 " ;
392 | 	//check that parametersInitialGuess is updated with the current guess
393 | 	for (uint i(0); i< parametersInitialGuess.size(); ++i)
394 | 	{
395 | 		if(abs(parametersInitialGuess[i] - (results.poseIntermediateGuess[results.poseIntermediateGuess.size()-1])[i]) > 1e-12) throw "LucaskanadeDescriptorFieldsTest failed 3 " ;
396 | 	}
397 | 
398 | }
399 | 
400 | 
401 | void LucaskanadeVideoSSDSpeedTest()
402 | {
403 | 
404 | 	VideoCapture capture("data/sample1.mov");
405 | 	OptimizationParameters optimizationParameters;
406 | 	optimizationParameters.resTol = 1e-5;
407 | 	optimizationParameters.pTol = 5e-5;
408 | 	optimizationParameters.maxIter =  50;
409 | 	optimizationParameters.maxIterSingleLevel = 10;
410 | 	optimizationParameters.pyramidSmoothingVariance.push_back(7);
411 | 	optimizationParameters.presmoothingVariance = 1;
412 | 	optimizationParameters.nControlPointsOnEdge = 60;
413 | 	optimizationParameters.borderThicknessHorizontal = 100;
414 | 	optimizationParameters.borderThicknessVertical = 50;
415 | 	optimizationParameters.bAdaptativeChoiceOfPoints = 0;
416 | 	optimizationParameters.bNormalizeDescriptors = 1;
417 | 
418 | 	LucasKanade optimization;
419 | 	AlignmentResults results;
420 | 
421 | 	Mat templ, image, imageRGB;
422 | 	Mat firstFrame;
423 | 	AcquireVGAGrayscaleImage(capture, templ);
424 | 	firstFrame = templ.clone();
425 | 
426 | 	StructOfArray2di pixelsOnTemplate;
427 | 	pixelsOnTemplate = CreateGridOfControlPoints(templ, optimizationParameters.nControlPointsOnEdge, optimizationParameters.borderThicknessHorizontal, optimizationParameters.borderThicknessVertical);
428 | 	StructOfArray2di pixelsOnTemplateWarped = pixelsOnTemplate;
429 | 
430 | 	vector<float> parameters(8,0);
431 | 	vector<float> parametersInitialGuess(8,0);
432 | 
433 | #ifdef B_PLOT
434 | 	string window_name = "SSD CALIBRATION";
435 | 	namedWindow(window_name, CV_WINDOW_KEEPRATIO);
436 | 	StructOfArray2di warpedPixels(4);
437 | 	StructOfArray2di  panelCorners = GetRectangleCornersForAugmentation(&optimizationParameters, templ.cols, templ.rows);
438 | #endif
439 | 
440 | 	Matx33f homographyMatrix;
441 | 	high_resolution_clock::time_point startTime, endTime;
442 | 	float elapsedTime(0.), tim;
443 | 	int nFrames = 0;
444 | 	while(nFrames < 50)
445 | 	{
446 | 		AcquireVGAGrayscaleImage(capture, image, imageRGB);
447 | 
448 | 		if (image.empty())
449 | 			break;
450 | 
451 | 		startTime = high_resolution_clock::now();
452 | 		results = optimization.SSDCalibration(pixelsOnTemplateWarped, templ, image, parametersInitialGuess, optimizationParameters);//parameters are updated with the current guess!
453 | 		Homography::CompositionalParametersUpdateWithCheck(parameters, parametersInitialGuess);
454 | 		homographyMatrix = Homography::GetUnscaledMatrix(parameters);
455 | 		warpPerspective(firstFrame, templ, homographyMatrix, templ.size(), INTER_LINEAR, BORDER_CONSTANT);
456 | 		WarpGridOfControlPoints(pixelsOnTemplate, pixelsOnTemplateWarped, parameters, templ.cols, templ.rows);
457 | 		fill(parametersInitialGuess.begin(), parametersInitialGuess.end(), 0.);
458 | 		nFrames++;
459 | 		endTime = high_resolution_clock::now();
460 | 		tim = duration_cast<duration<double> >(endTime - startTime).count();
461 | 		elapsedTime +=tim;
462 | 
463 | #ifdef B_PLOT
464 | 		for (int iPoint = 0; iPoint< panelCorners.size(); ++iPoint)
465 | 			Homography::ComputeWarpedPixels(panelCorners[iPoint].x,panelCorners[iPoint].y, warpedPixels.x[iPoint], warpedPixels.y[iPoint], parameters);
466 | 		AugmentFrameWithQuadrilater(warpedPixels, imageRGB);
467 | 		Mat frameToDisplay = imageRGB;
468 | 		imshow(window_name, frameToDisplay);
469 | 		waitKey(50);
470 | #endif
471 | 	}
472 | 	cout << "average elapsedTime = "<< elapsedTime/nFrames<<endl;
473 | }
474 | 
475 | 
476 | void LucaskanadeVideoDesciptorFieldsSpeedTest()
477 | {
478 | 	VideoCapture capture("data/sample1.mov");
479 | 
480 | 	OptimizationParameters optimizationParameters;
481 | 	optimizationParameters.resTol = 1e-5;
482 | 	optimizationParameters.pTol = 5e-5;
483 | 	optimizationParameters.maxIter =  50;
484 | 	optimizationParameters.maxIterSingleLevel = 10;
485 | 	optimizationParameters.pyramidSmoothingVariance.push_back(10);
486 | 	optimizationParameters.pyramidSmoothingVariance.push_back(5);
487 | 	optimizationParameters.presmoothingVariance = 0;
488 | 	optimizationParameters.nControlPointsOnEdge = 60;
489 | 	optimizationParameters.borderThicknessHorizontal = 100;
490 | 	optimizationParameters.borderThicknessVertical = 50;
491 | 	optimizationParameters.bAdaptativeChoiceOfPoints = 0;
492 | 	optimizationParameters.bNormalizeDescriptors = 0;
493 | 
494 | 	LucasKanade optimization;
495 | 	AlignmentResults results;
496 | 
497 | 	Mat templ, image, imageRGB, firstFrame;
498 | 
499 | 	AcquireVGAGrayscaleImage(capture, templ);
500 | 	firstFrame = templ.clone();
501 | 
502 | 	StructOfArray2di pixelsOnTemplate;
503 | 	pixelsOnTemplate = CreateGridOfControlPoints(templ, optimizationParameters.nControlPointsOnEdge, optimizationParameters.borderThicknessHorizontal, optimizationParameters.borderThicknessVertical);
504 | 	StructOfArray2di pixelsOnTemplateWarped = pixelsOnTemplate;
505 | 
506 | 	vector<float> parameters(8,0);
507 | 	vector<float> parametersInitialGuess(8,0);
508 | 
509 | #ifdef B_PLOT
510 | 	string window_name = "ISMAR Demo 2014";
511 | 	namedWindow(window_name, CV_WINDOW_KEEPRATIO);
512 | 	StructOfArray2di warpedPixels(4);
513 | 	StructOfArray2di  panelCorners = GetRectangleCornersForAugmentation(&optimizationParameters, templ.cols, templ.rows);
514 | #endif
515 | 
516 | 	Matx33f homographyMatrix;
517 | 	high_resolution_clock::time_point startTime, endTime;
518 | 	float elapsedTime(0.), tim;
519 | 	int nFrames = 0;
520 | 	while(nFrames < 50)
521 | 	{
522 | 		AcquireVGAGrayscaleImage(capture, image, imageRGB);
523 | 		if (image.empty())
524 | 			break;
525 | 
526 | 		startTime = high_resolution_clock::now();
527 | 
528 | 		results = optimization.DescriptorFieldsCalibration(pixelsOnTemplateWarped, templ, image, parametersInitialGuess, optimizationParameters);//parameters are updated with the current guess!
529 | 		Homography::CompositionalParametersUpdateWithCheck(parameters, parametersInitialGuess);
530 | 		homographyMatrix = Homography::GetUnscaledMatrix(parameters);
531 | 		warpPerspective(firstFrame, templ, homographyMatrix, templ.size(), INTER_LINEAR, BORDER_CONSTANT);
532 | 		WarpGridOfControlPoints(pixelsOnTemplate, pixelsOnTemplateWarped, parameters, templ.cols, templ.rows);
533 | 		fill(parametersInitialGuess.begin(), parametersInitialGuess.end(), 0.);
534 | 		nFrames++;
535 | 
536 | 		endTime = high_resolution_clock::now();
537 | 		tim = duration_cast<duration<double> >(endTime - startTime).count();
538 | 		elapsedTime +=tim;
539 | 
540 | #ifdef B_PLOT
541 | 		for (int iPoint = 0; iPoint< panelCorners.size(); ++iPoint)
542 | 			Homography::ComputeWarpedPixels(panelCorners[iPoint].x,panelCorners[iPoint].y, warpedPixels.x[iPoint], warpedPixels.y[iPoint], parameters);
543 | 		AugmentFrameWithQuadrilater(warpedPixels, imageRGB);
544 | 		Mat frameToDisplay = imageRGB;
545 | 		imshow(window_name, frameToDisplay);
546 | 		waitKey(50);
547 | #endif
548 | 	}
549 | 	cout << "average elapsedTime = "<< elapsedTime/nFrames<<endl;
550 | }
551 | 
552 | void LucaskanadeVideoGradientMagnitudeSpeedTest()
553 | {
554 | 	VideoCapture capture("data/sample1.mov");
555 | 	OptimizationParameters optimizationParameters;
556 | 	optimizationParameters.resTol = 1e-5;
557 | 	optimizationParameters.pTol = 5e-5;
558 | 	optimizationParameters.maxIter =  50;
559 | 	optimizationParameters.maxIterSingleLevel = 10;
560 | 	optimizationParameters.pyramidSmoothingVariance.push_back(10);
561 | 	optimizationParameters.pyramidSmoothingVariance.push_back(5);
562 | 	optimizationParameters.presmoothingVariance = 1;
563 | 	optimizationParameters.nControlPointsOnEdge = 60;
564 | 	optimizationParameters.borderThicknessHorizontal = 100;
565 | 	optimizationParameters.borderThicknessVertical = 50;
566 | 	optimizationParameters.bAdaptativeChoiceOfPoints = 0;
567 | 	optimizationParameters.bNormalizeDescriptors = 0;
568 | 
569 | 	LucasKanade optimization;
570 | 	AlignmentResults results;
571 | 
572 | 	Mat templ, image, imageRGB, firstFrame;
573 | 
574 | 	AcquireVGAGrayscaleImage(capture, templ);
575 | 	firstFrame = templ.clone();
576 | 
577 | 	StructOfArray2di pixelsOnTemplate;
578 | 	pixelsOnTemplate = CreateGridOfControlPoints(templ, optimizationParameters.nControlPointsOnEdge, optimizationParameters.borderThicknessHorizontal, optimizationParameters.borderThicknessVertical);
579 | 	StructOfArray2di pixelsOnTemplateWarped = pixelsOnTemplate;
580 | 
581 | 	vector<float> parameters(8,0);
582 | 	vector<float> parametersInitialGuess(8,0);
583 | 
584 | #ifdef B_PLOT
585 | 	string window_name = "ISMAR Demo 2014";
586 | 	namedWindow(window_name, CV_WINDOW_KEEPRATIO);
587 | 	StructOfArray2di warpedPixels(4);
588 | 	StructOfArray2di  panelCorners = GetRectangleCornersForAugmentation(&optimizationParameters, templ.cols, templ.rows);
589 | #endif
590 | 
591 | 	Matx33f homographyMatrix;
592 | 	high_resolution_clock::time_point startTime, endTime;
593 | 	float elapsedTime(0.), tim;
594 | 	int nFrames = 0;
595 | 	while(nFrames < 50)
596 | 	{
597 | 		AcquireVGAGrayscaleImage(capture, image, imageRGB);
598 | 		if (image.empty())
599 | 			break;
600 | 
601 | 		startTime = high_resolution_clock::now();
602 | 
603 | 		results = optimization.GradientModuleCalibration(pixelsOnTemplateWarped, templ, image, parametersInitialGuess, optimizationParameters);//parameters are updated with the current guess!
604 | 		Homography::CompositionalParametersUpdateWithCheck(parameters, parametersInitialGuess);
605 | 		homographyMatrix = Homography::GetUnscaledMatrix(parameters);
606 | 		warpPerspective(firstFrame, templ, homographyMatrix, templ.size(), INTER_LINEAR, BORDER_CONSTANT);
607 | 		WarpGridOfControlPoints(pixelsOnTemplate, pixelsOnTemplateWarped, parameters, templ.cols, templ.rows);
608 | 		fill(parametersInitialGuess.begin(), parametersInitialGuess.end(), 0.);
609 | 		nFrames++;
610 | 
611 | 		endTime = high_resolution_clock::now();
612 | 		tim = duration_cast<duration<double> >(endTime - startTime).count();
613 | 		elapsedTime +=tim;
614 | 
615 | #ifdef B_PLOT
616 | 		for (int iPoint = 0; iPoint< panelCorners.size(); ++iPoint)
617 | 			Homography::ComputeWarpedPixels(panelCorners[iPoint].x,panelCorners[iPoint].y, warpedPixels.x[iPoint], warpedPixels.y[iPoint], parameters);
618 | 		AugmentFrameWithQuadrilater(warpedPixels, imageRGB);
619 | 		Mat frameToDisplay = imageRGB;
620 | 		imshow(window_name, frameToDisplay);
621 | 		waitKey(50);
622 | #endif
623 | 	}
624 | 	cout << "average elapsedTime = "<< elapsedTime/nFrames<<endl;
625 | }
626 | 
627 | 
628 | 
629 | void RunAllTests()
630 | {
631 | 	try
632 | 	{
633 | 		cout<<"ReadDataFromXMLTest()...";
634 | 		ReadDataFromXMLTest();
635 | 		cout<<".................... ok."<<endl;
636 | 
637 | 		cout<<"ComputeHomographyTest...";
638 | 		ComputeHomographyTest();
639 | 		cout<<".................... ok."<<endl;
640 | 
641 | 		cout<<"CompositionHomographyTest()...";
642 | 		CompositionHomographyTest();
643 | 		cout<<".................... ok."<<endl;
644 | 
645 | 		cout<<"HomographyJacobianTest()...";
646 | 		HomographyJacobianTest2();
647 | 		cout<<".................... ok."<<endl;
648 | 
649 | 		cout<<"InverseHomographyTest()...";
650 | 		InverseHomographyTest();
651 | 		cout<<".................... ok."<<endl;
652 | 
653 | 		cout<<"WarpedIntensitiesTest()...";
654 | 		WarpedIntensitiesTest();
655 | 		cout<<".................... ok."<<endl;
656 | 
657 | 		cout<<"SDImagesTest()...";
658 | 		SDImagesTest();
659 | 		cout<<".................... ok."<<endl;
660 | 
661 | 		cout<<"LucaskanadeSSDTest()...";
662 | 		LucaskanadeSSDTest();
663 | 		cout<<".................... ok."<<endl;
664 | 
665 | 		cout<<"LucaskanadeDescriptorFieldsTest()...";
666 | 		LucaskanadeDescriptorFieldsTest();
667 | 		cout<<".................... ok."<<endl;
668 | 
669 | 		cout<<"LucaskanadeVideoSSDSpeedTest()...";
670 | 		LucaskanadeVideoSSDSpeedTest();
671 | 		cout<<".................... ok."<<endl;
672 | 
673 | 		cout<<"LucaskanadeVideoDesciptorFieldsSpeedTest()...";
674 | 		LucaskanadeVideoDesciptorFieldsSpeedTest();
675 | 		cout<<".................... ok."<<endl;
676 | 
677 | 		cout<<"LucaskanadeVideoGradientMagnitudeSpeedTest...";
678 | 		LucaskanadeVideoGradientMagnitudeSpeedTest();
679 | 		cout<<".................... ok."<<endl;
680 | 
681 | 	}
682 | 	catch(char const* message)
683 | 	{
684 | 		cerr<<message<<endl;
685 | 		exit(0);
686 | 	}
687 | }
688 | 
689 | int main(int argc, char **argv)
690 | {
691 | 	RunAllTests();
692 | 	exit(0);
693 | }
694 | 
695 | 


--------------------------------------------------------------------------------
/unit/UnitTests.hpp:
--------------------------------------------------------------------------------
 1 | /*
 2 | Copyright 2014 Alberto Crivellaro, Ecole Polytechnique Federale de Lausanne (EPFL), Switzerland.
 3 | alberto.crivellaro@epfl.ch
 4 | 
 5 | terms of the GNU General Public License as published by the Free Software
 6 | Foundation; either version 2 of the License, or (at your option) any later
 7 | version.
 8 | 
 9 | This program is distributed in the hope that it will be useful, but WITHOUT ANY
10 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
11 | PARTICULAR PURPOSE. See the GNU General Public License for more details.
12 | 
13 | You should have received a copy of the GNU General Public License along with
14 | this program; if not, write to the Free Software Foundation, Inc., 51 Franklin
15 | Street, Fifth Floor, Boston, MA 02110-1301, USA
16 |  */
17 | 
18 | #ifndef TESTS_HPP_
19 | #define TESTS_HPP_
20 | #include <iostream>
21 | #include <list>
22 | #include <vector>
23 | 
24 | #include <algorithm>    // std::max
25 | #include <assert.h>
26 | #include <ctime>
27 | #include <cmath>
28 | #include<stdio.h>
29 | #include <chrono>
30 | 
31 | #include "HomographyEstimation.hpp"
32 | 
33 | 
34 | void RunAllTests();
35 | 
36 | 
37 | void ShotSetImageTest();
38 | void ReadDataFromXMLTest();
39 | void ComputeHomographyTest();
40 | void CompositionHomographyTest();
41 | void HomographyJacobianTest2();
42 | void InverseHomographyTest();
43 | void WarpedIntensitiesTest();
44 | void SDImagesTest();
45 | void LucaskanadeSSDTest();
46 | void LucaskanadeDescriptorFieldsTest();
47 | 
48 | void LucaskanadeVideoSSDSpeedTest();
49 | void LucaskanadeVideoDesciptorFieldsSpeedTest();
50 | void LucaskanadeVideoGradientMagnitudeSpeedTest();
51 | 
52 | #endif /* TESTS_HPP_ */
53 | 


--------------------------------------------------------------------------------
/unit/data/imageCropped.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/albertoCrive/homographyTrackingDemo/b8c4636817abb713e44afcdb1a51ce6ffdd05c99/unit/data/imageCropped.jpg


--------------------------------------------------------------------------------
/unit/data/sample1.mov:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/albertoCrive/homographyTrackingDemo/b8c4636817abb713e44afcdb1a51ce6ffdd05c99/unit/data/sample1.mov


--------------------------------------------------------------------------------
/unit/data/template.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/albertoCrive/homographyTrackingDemo/b8c4636817abb713e44afcdb1a51ce6ffdd05c99/unit/data/template.png


--------------------------------------------------------------------------------
/unit/parameter/parametersForTest.yml:
--------------------------------------------------------------------------------
 1 | %YAML:1.0
 2 | resTol: 0.005
 3 | pTol: 0.000004
 4 | maxIter: 100
 5 | maxIterSingleLevel: 10
 6 | pyramidSmoothingVariance: [ 35, 17, 9, 5]
 7 | presmoothingVariance: 2
 8 | nControlPointsOnEdge: 40
 9 | borderThicknessHorizontal: 10 #this is a comment!
10 | borderThicknessVertical: 10.4
11 | bAdaptativeChoiceOfPoints: 0
12 | bNormalizeDescriptors: 0
13 | 


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