├── .gitignore
├── MotionDetection
    ├── CMakeLists.txt
    ├── MotionDetector.cpp
    ├── MotionDetector.h
    ├── README.md
    ├── data
    │   └── VOT3.mp4
    └── main.cpp
├── PrimeSenseCalib
    ├── .gitignore
    ├── CMakeLists.txt
    ├── CalibTest.cpp
    ├── OpenNICapture.cpp
    ├── OpenNIEngine.cpp
    ├── OpenNIEngine.h
    ├── README.md
    ├── StereoCalib.cpp
    ├── chess.pdf
    └── cmake
    │   ├── FindOpenNI.cmake
    │   └── FindOpenNI.cmake~
├── README.md
├── RelativePoseSolver
    ├── CMakeLists.txt
    ├── CalibStereo.cpp
    ├── CalibStereo.h
    ├── absolute_orientation_horn.h
    ├── data
    │   ├── aligned.txt
    │   ├── itm.txt
    │   ├── output.txt
    │   ├── output.txt~
    │   ├── output2.txt
    │   ├── pose1.txt
    │   ├── pose2.txt
    │   └── vicon.txt
    ├── relative_pose_solver.h
    ├── solve_relative_pose.cpp
    ├── test_absolute_orientation.cpp
    └── test_relative_pose.cpp
└── pwp
    ├── CMakeLists.txt
    ├── README.md
    ├── data
        └── hand.avi
    ├── main.cpp
    └── pwp
        ├── Histogram.hpp
        ├── PWPTracker.cpp
        └── PWPTracker.hpp


/.gitignore:
--------------------------------------------------------------------------------
1 | *.user
2 | *-build/
3 | */build/


--------------------------------------------------------------------------------
/MotionDetection/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8)
 2 | project(MotionDetection)
 3 | 
 4 | 
 5 | find_package(OpenCV REQUIRED)
 6 | INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS})
 7 | 
 8 | IF(MSVC_IDE)
 9 |   add_definitions(-D_CRT_SECURE_NO_WARNINGS)
10 |   add_definitions(-DUSING_CMAKE=1)
11 | ELSE(MSVC_IDE)
12 |   set(CFLAGS_WARN "-Wall -Wextra -Wno-unused-parameter -Wno-strict-aliasing")
13 |   set(CMAKE_CXX_FLAGS "-fPIC -O3 -march=native ${CFLAGS_WARN} ${CMAKE_CXX_FLAGS}")
14 | ENDIF(MSVC_IDE)
15 | 
16 | add_executable(MotionDetection
17 | MotionDetector.h
18 | MotionDetector.cpp
19 | main.cpp)
20 | 
21 | target_link_libraries(MotionDetection ${OpenCV_LIBS})
22 | 
23 | 
24 | 


--------------------------------------------------------------------------------
/MotionDetection/MotionDetector.cpp:
--------------------------------------------------------------------------------
  1 | #include "MotionDetector.h"
  2 | #include <iostream>
  3 | #include<math.h>
  4 | 
  5 | using namespace  std;
  6 | using namespace cv;
  7 | 
  8 | 
  9 | MotionDetector::~MotionDetector(){}
 10 | 
 11 | MotionDetector::MotionDetector(bool need_rotate){
 12 |     
 13 |     target_size_.width = 320;
 14 |     target_size_.height = 180;
 15 |     
 16 |     patch_size_.width = 1;
 17 |     patch_size_.height = 1;
 18 |     
 19 |     model_initialized_ = false;
 20 |     pre_image_loaded = false;
 21 |     motion_compensated_ = false;
 22 |     
 23 |     max_age_ = 30;
 24 |     sigma_blend_ = 2.0f;
 25 |     sigma_classify_ = 4.0f;
 26 |     sigma_decay_ = 50*50;
 27 |     lambda_decay_ = 0.001;
 28 |     
 29 |     
 30 |     if(need_rotate) swap(target_size_.width, target_size_.height);
 31 | 
 32 |     grid_layout_.width = target_size_.width / patch_size_.width;
 33 |     grid_layout_.height = target_size_.height / patch_size_.height;
 34 |     
 35 |     total_grid_no_ = grid_layout_.width * grid_layout_.height;
 36 |     
 37 |     background_models_ = new DualSGM[total_grid_no_];
 38 |     warped_background_models_ = new DualSGM[total_grid_no_];
 39 |     
 40 |     for (int i=0;i<total_grid_no_;i++){
 41 |         background_models_[i].clear();
 42 |         warped_background_models_[i].clear();
 43 |     }
 44 |     
 45 |     small_motion_mask_.create(target_size_,CV_8UC1);
 46 | 
 47 | }
 48 | 
 49 | // done
 50 | void MotionDetector::prepareData(const Mat &in_img){
 51 |     
 52 |     resize(in_img,small_rgb_image_,target_size_);
 53 |     cvtColor(small_rgb_image_,cur_small_gray_image_,CV_BGR2GRAY);
 54 |     GaussianBlur(small_rgb_image_, small_blur_image_,Size(7,7),0);
 55 |     medianBlur(small_blur_image_,small_blur_image_,5);
 56 |     
 57 |       if (!pre_image_loaded){
 58 |         original_size_.width = in_img.cols;
 59 |         original_size_.height = in_img.rows;
 60 |         cur_small_gray_image_.copyTo(pre_small_gray_image_);
 61 |         pre_image_loaded = true;
 62 |     }
 63 | }
 64 | 
 65 | // done
 66 | //#define DEBUG_BG_MOTION
 67 | Matx33f MotionDetector::computeBackgroundMotion(const Mat &pre_img, const Mat &cur_img){
 68 | 
 69 |         cv::Mat n;
 70 |      
 71 |         std::vector<cv::Point2f>   pre_points;
 72 |         std::vector<cv::Point2f>   cur_points;
 73 |         std::vector<uchar> point_status;
 74 |         std::vector<float>         point_error;
 75 |         
 76 |         const int maxCorners = 300;
 77 |         const float qualityLevel = 0.01;
 78 |         const float minDistance = 5;
 79 |         const int blockSize = 4;
 80 |         const bool useHarrisDetector = false;
 81 |         const double k = 0.04;
 82 |         
 83 |         cv::goodFeaturesToTrack(
 84 |                     pre_img,
 85 |                     pre_points, 
 86 |                     maxCorners, 
 87 |                     qualityLevel, 
 88 |                     minDistance, 
 89 |                     noArray(),
 90 |                     blockSize, 
 91 |                     useHarrisDetector, 
 92 |                     k);
 93 |     
 94 |         if(pre_points.size() >= 1) {
 95 |             
 96 |             cv::calcOpticalFlowPyrLK(
 97 |                         pre_img, 
 98 |                         cur_img, 
 99 |                         pre_points, 
100 |                         cur_points, 
101 |                         point_status, 
102 |                         point_error, 
103 |                         Size(20,20), 
104 |                         5);
105 |     
106 |             vector <Point2f> prev_corner2, cur_corner2;
107 |             n = cur_img.clone();
108 |     
109 |             // weed out bad matches
110 |             for(size_t i=0; i < point_status.size(); i++) {
111 |                 if(point_status[i]) {
112 |                     prev_corner2.push_back(pre_points[i]);
113 |                     cur_corner2.push_back(cur_points[i]);
114 |                 }
115 |             }
116 |             
117 |             cv::Mat H = cv::findHomography(
118 |                         prev_corner2,
119 |                         cur_corner2, 
120 |                         CV_RANSAC,
121 |                         1);
122 |             
123 | #ifdef DEBUG_BG_MOTION            
124 |             warpPerspective(cur_img, n, H, target_size_, INTER_LINEAR | WARP_INVERSE_MAP);
125 |             
126 |             Mat show_frame;
127 |             cvtColor(n,show_frame,CV_GRAY2BGR);
128 |             for (size_t i=0;i<cur_corner2.size();i++){
129 |                 cv::arrowedLine(show_frame,cur_corner2[i], prev_corner2[i],Scalar(0,255,0));
130 |             }
131 |             
132 |             Mat err_before;//(n.rows, n.cols,CV_8UC1);
133 |             Mat err_after;//(n.rows,n.cols,CV_8UC1);
134 |             
135 |             absdiff(pre_img,cur_img,err_before);
136 |             absdiff(pre_img,n,err_after);
137 |            
138 |             imshow("err_before",err_before);
139 |             imshow("err_after",err_after);
140 |             
141 | //            moveWindow("err_before",100,200);
142 | //            moveWindow("err_after",400,200);
143 |             
144 |             imshow("tracking", show_frame);
145 | #endif                        
146 |             
147 |             return Matx33f(H);
148 |         } else {
149 |             printf("NO matching points");
150 |             return Matx33f::eye();
151 |         }
152 |  
153 |         
154 | }
155 | 
156 | #define MIX_DSGM_WARP
157 | void MotionDetector::compensateMotion(const DualSGM *in_dsgm, DualSGM *out_dsgm, Matx33f pose){
158 | 
159 |     for (int i=0; i<total_grid_no_; i++)  out_dsgm[i].clear();
160 |     
161 |     for (int r=0, count = 0;r<grid_layout_.height;r++){
162 |         for (int c=0;c<grid_layout_.width;c++, count++){
163 |             
164 |             DualSGM& current_dsgm_out = out_dsgm[count];
165 |             
166 |             float X = patch_size_.width * c + patch_size_.width * 0.5;
167 |             float Y = patch_size_.height * r + patch_size_.height * 0.5;
168 |             
169 |             float normalizer = pose(2,0) * X + pose(2,1) * Y + pose(2,2);
170 |             float X_new = (pose(0,0) * X + pose(0,1) * Y + pose(0,2)) / normalizer;
171 |             float Y_new = (pose(1,0) * X + pose(1,1) * Y + pose(1,2)) / normalizer;
172 |             
173 |             float c_new = X_new / patch_size_.width;
174 |             float r_new = Y_new / patch_size_.height;
175 |             
176 |             int c_idx = floor(c_new);
177 |             int r_idx = floor(r_new);
178 |             
179 |             float dr = r_new - ((float)r_idx + 0.5);
180 |             float dc = c_new - ((float)c_idx + 0.5);
181 |             
182 |             float area_h;
183 |             float area_v;
184 |             float area_hv;
185 |             float area_self;
186 |             
187 |             Vec3f tmp_var[2] = {Vec3f(0,0,0),Vec3f(0,0,0)};
188 |             Vec3f tmp_mean[2] = {Vec3f(0,0,0),Vec3f(0,0,0)};
189 |             float tmp_age[2] = {0,0};
190 |             float total_area = 0;      
191 |             
192 |             
193 |             ///////////////////////////////////////////////////////////
194 |             //
195 |             //
196 |             // first we warp the mean
197 |             //
198 |             //
199 |             ///////////////////////////////////////////////////////////            
200 |             {
201 |       
202 | #ifdef MIX_DSGM_WARP
203 |                 ///////////////////////////////////////////////////////////
204 |                 // horizontal neighbor
205 |                 ///////////////////////////////////////////////////////////
206 |                 if (dc!=0){
207 |                     
208 |                     int c_idx_new = c_idx;
209 |                     int r_idx_new = r_idx;
210 |                     c_idx_new += dc > 0 ? 1 : -1;
211 |                     
212 |                     if (c_idx_new >=0 && c_idx_new < grid_layout_.width && r_idx_new >=0 && r_idx_new < grid_layout_.height){ 
213 |                         const DualSGM& selected_dsgm = in_dsgm[r_idx_new * grid_layout_.width + c_idx_new];
214 |                         area_h = abs(dc) * (1.0 - abs(dr));
215 |                         tmp_mean[0] += area_h * selected_dsgm.currentModel().mean;
216 |                         tmp_mean[1] += area_h * selected_dsgm.inactiveModel().mean;
217 |                         tmp_age[0] += area_h * selected_dsgm.currentModel().age;
218 |                         tmp_age[1] += area_h * selected_dsgm.inactiveModel().age;
219 |                         total_area += area_h;
220 |                     }    
221 |                 }
222 |                 
223 |                 ///////////////////////////////////////////////////////////
224 |                 // vertical neighbor
225 |                 ///////////////////////////////////////////////////////////
226 |                 if (dr!=0){
227 |                     
228 |                     int c_idx_new = c_idx;
229 |                     int r_idx_new = r_idx;
230 |                     r_idx_new += dr > 0 ? 1 : -1;
231 |                     
232 |                     if (c_idx_new >=0 && c_idx_new < grid_layout_.width && r_idx_new >=0 && r_idx_new < grid_layout_.height){ 
233 |                         const DualSGM& selected_dsgm = in_dsgm[r_idx_new * grid_layout_.width + c_idx_new];
234 |                         area_v = abs(dr) * (1.0 - abs(dc));
235 |                         tmp_mean[0] += area_v * selected_dsgm.currentModel().mean;
236 |                         tmp_mean[1] += area_v * selected_dsgm.inactiveModel().mean;
237 |                         tmp_age[0] += area_v * selected_dsgm.currentModel().age;
238 |                         tmp_age[1] += area_v * selected_dsgm.inactiveModel().age;
239 |                         total_area += area_v;
240 |                     }
241 |                 }
242 |                 
243 |                 ///////////////////////////////////////////////////////////
244 |                 // vertical+horizontal neighbor
245 |                 ///////////////////////////////////////////////////////////
246 |                 if (dr!=0 && dc!=0){
247 |                     
248 |                     int c_idx_new = c_idx;
249 |                     int r_idx_new = r_idx;
250 |                     c_idx_new += dc > 0 ? 1 : -1;
251 |                     r_idx_new += dr > 0 ? 1 : -1;
252 |                     
253 |                     if (c_idx_new >=0 && c_idx_new < grid_layout_.width && r_idx_new >=0 && r_idx_new < grid_layout_.height){ 
254 |                         const DualSGM& selected_dsgm = in_dsgm[r_idx_new * grid_layout_.width + c_idx_new];
255 |                         area_hv = abs(dr) * abs(dc);
256 |                         tmp_mean[0] += area_hv * selected_dsgm.currentModel().mean;
257 |                         tmp_mean[1] += area_hv * selected_dsgm.inactiveModel().mean;
258 |                         tmp_age[0] += area_hv * selected_dsgm.currentModel().age;
259 |                         tmp_age[1] += area_hv * selected_dsgm.inactiveModel().age;
260 |                         total_area += area_hv;
261 |                     }
262 |                 }
263 | #endif                
264 |                 ///////////////////////////////////////////////////////////
265 |                 // self
266 |                 ///////////////////////////////////////////////////////////
267 |                 
268 |                 if (c_idx >=0 && c_idx < grid_layout_.width && r_idx >=0 && r_idx < grid_layout_.height){ 
269 |                     const DualSGM& selected_dsgm = in_dsgm[r_idx * grid_layout_.width + c_idx];
270 |                     area_self = (1.0 - abs(dr)) * (1.0 - abs(dc));
271 |                     tmp_mean[0] += area_self * selected_dsgm.currentModel().mean;
272 |                     tmp_mean[1] += area_self * selected_dsgm.inactiveModel().mean;
273 |                     tmp_age[0] += area_self * selected_dsgm.currentModel().age;
274 |                     tmp_age[1] += area_self * selected_dsgm.inactiveModel().age;
275 |                     total_area += area_self;
276 |                     
277 |                 }
278 |                 
279 |                 if (total_area>0)
280 |                 {
281 |                     tmp_mean[0] /= total_area;
282 |                     tmp_mean[1] /= total_area;
283 |                     tmp_age[0] /= total_area;
284 |                     tmp_age[1] /= total_area;
285 |                 }
286 |                 
287 |                 
288 |             }
289 |             
290 |             
291 |             ///////////////////////////////////////////////////////////
292 |             //
293 |             //
294 |             // then we warp the variance
295 |             //
296 |             //
297 |             ///////////////////////////////////////////////////////////
298 |             {
299 |                 
300 | #ifdef MIX_DSGM_WARP                
301 |                 ///////////////////////////////////////////////////////////
302 |                 // horizontal neighbor
303 |                 ///////////////////////////////////////////////////////////
304 |                 if (dc!=0){
305 |                     
306 |                     int c_idx_new = c_idx;
307 |                     int r_idx_new = r_idx;
308 |                     c_idx_new += dc > 0 ? 1 : -1;
309 |                     
310 |                     if (c_idx_new >=0 && c_idx_new < grid_layout_.width && r_idx_new >=0 && r_idx_new < grid_layout_.height){ 
311 | 
312 |                         const SGM& selected_active = in_dsgm[r_idx_new * grid_layout_.width + c_idx_new].currentModel();
313 |                         const Vec3f diff_active = selected_active.mean - tmp_mean[0];                        
314 |                         tmp_var[0][0] +=area_h * (selected_active.var[0] + pow(diff_active[0],(int)2));
315 |                         tmp_var[0][1] +=area_h * (selected_active.var[1] + pow(diff_active[1],(int)2));
316 |                         tmp_var[0][2] +=area_h * (selected_active.var[2] + pow(diff_active[2],(int)2));
317 |                         
318 |                         const SGM& selected_inactive = in_dsgm[r_idx_new * grid_layout_.width + c_idx_new].inactiveModel();
319 |                         const Vec3f diff_inactive = selected_inactive.mean - tmp_mean[1]; 
320 |                         tmp_var[1][0] +=area_h * (selected_inactive.var[0] + pow(diff_inactive[0],(int)2));
321 |                         tmp_var[1][1] +=area_h * (selected_inactive.var[1] + pow(diff_inactive[1],(int)2));
322 |                         tmp_var[1][2] +=area_h * (selected_inactive.var[2] + pow(diff_inactive[2],(int)2));
323 |                         
324 |                     }    
325 |                 }
326 |                 
327 |                 ///////////////////////////////////////////////////////////
328 |                 // vertical neighbor
329 |                 ///////////////////////////////////////////////////////////
330 |                 if (dr!=0){
331 |                     
332 |                     int c_idx_new = c_idx;
333 |                     int r_idx_new = r_idx;
334 |                     r_idx_new += dr > 0 ? 1 : -1;
335 |                     
336 |                     if (c_idx_new >=0 && c_idx_new < grid_layout_.width && r_idx_new >=0 && r_idx_new < grid_layout_.height){ 
337 | 
338 |                         const SGM& selected_active = in_dsgm[r_idx_new * grid_layout_.width + c_idx_new].currentModel();
339 |                         const Vec3f diff_active = selected_active.mean - tmp_mean[0];                        
340 |                         tmp_var[0][0] +=area_v * (selected_active.var[0] + pow(diff_active[0],(int)2));
341 |                         tmp_var[0][1] +=area_v * (selected_active.var[1] + pow(diff_active[1],(int)2));
342 |                         tmp_var[0][2] +=area_v * (selected_active.var[2] + pow(diff_active[2],(int)2));
343 |                         
344 |                         const SGM& selected_inactive = in_dsgm[r_idx_new * grid_layout_.width + c_idx_new].inactiveModel();
345 |                         const Vec3f diff_inactive = selected_inactive.mean - tmp_mean[1]; 
346 |                         tmp_var[1][0] +=area_v * (selected_inactive.var[0] + pow(diff_inactive[0],(int)2));
347 |                         tmp_var[1][1] +=area_v * (selected_inactive.var[1] + pow(diff_inactive[1],(int)2));
348 |                         tmp_var[1][2] +=area_v * (selected_inactive.var[2] + pow(diff_inactive[2],(int)2));
349 |                         
350 |                     }
351 |                 }
352 |                 
353 |                 ///////////////////////////////////////////////////////////
354 |                 // vertical+horizontal neighbor
355 |                 ///////////////////////////////////////////////////////////
356 |                 if (dr!=0 && dc!=0){
357 |                     
358 |                     int c_idx_new = c_idx;
359 |                     int r_idx_new = r_idx;
360 |                     c_idx_new += dc > 0 ? 1 : -1;
361 |                     r_idx_new += dr > 0 ? 1 : -1;
362 |                     
363 |                     if (c_idx_new >=0 && c_idx_new < grid_layout_.width && r_idx_new >=0 && r_idx_new < grid_layout_.height){ 
364 |                         
365 |                         const SGM& selected_active = in_dsgm[r_idx_new * grid_layout_.width + c_idx_new].currentModel();
366 |                         const Vec3f diff_active = selected_active.mean - tmp_mean[0];                        
367 |                         tmp_var[0][0] +=area_hv * (selected_active.var[0] + pow(diff_active[0],(int)2));
368 |                         tmp_var[0][1] +=area_hv * (selected_active.var[1] + pow(diff_active[1],(int)2));
369 |                         tmp_var[0][2] +=area_hv * (selected_active.var[2] + pow(diff_active[2],(int)2));
370 |                         
371 |                         const SGM& selected_inactive = in_dsgm[r_idx_new * grid_layout_.width + c_idx_new].inactiveModel();
372 |                         const Vec3f diff_inactive = selected_inactive.mean - tmp_mean[1]; 
373 |                         tmp_var[1][0] +=area_hv * (selected_inactive.var[0] + pow(diff_inactive[0],(int)2));
374 |                         tmp_var[1][1] +=area_hv * (selected_inactive.var[1] + pow(diff_inactive[1],(int)2));
375 |                         tmp_var[1][2] +=area_hv * (selected_inactive.var[2] + pow(diff_inactive[2],(int)2));
376 | 
377 |                     }
378 |                 }
379 | #endif                
380 |                 ///////////////////////////////////////////////////////////
381 |                 // self
382 |                 ///////////////////////////////////////////////////////////
383 |                 
384 |                 if (c_idx >=0 && c_idx < grid_layout_.width && r_idx >=0 && r_idx < grid_layout_.height){ 
385 | 
386 |                     const SGM& selected_active = in_dsgm[r_idx * grid_layout_.width + c_idx].currentModel();
387 |                     const Vec3f diff_active = selected_active.mean - tmp_mean[0];                        
388 |                     tmp_var[0][0] +=area_self * (selected_active.var[0] + pow(diff_active[0],(int)2));
389 |                     tmp_var[0][1] +=area_self * (selected_active.var[1] + pow(diff_active[1],(int)2));
390 |                     tmp_var[0][2] +=area_self * (selected_active.var[2] + pow(diff_active[2],(int)2));
391 |                     
392 |                     const SGM& selected_inactive = in_dsgm[r_idx * grid_layout_.width + c_idx].inactiveModel();
393 |                     const Vec3f diff_inactive = selected_inactive.mean - tmp_mean[1]; 
394 |                     tmp_var[1][0] +=area_self * (selected_inactive.var[0] + pow(diff_inactive[0],(int)2));
395 |                     tmp_var[1][1] +=area_self * (selected_inactive.var[1] + pow(diff_inactive[1],(int)2));
396 |                     tmp_var[1][2] +=area_self * (selected_inactive.var[2] + pow(diff_inactive[2],(int)2));
397 |                     
398 |                 }
399 |                 
400 |                 if (total_area>0)
401 |                 {
402 |                     tmp_var[0] /= total_area;
403 |                     tmp_var[1] /= total_area;
404 |                 }
405 |                 
406 |             }
407 |             
408 |             tmp_var[0][0] = MAX(tmp_var[0][0],DSGM_VAR_INIT);
409 |             tmp_var[0][1] = MAX(tmp_var[0][1],DSGM_VAR_INIT);
410 |             tmp_var[0][2] = MAX(tmp_var[0][2],DSGM_VAR_INIT);
411 |             
412 |             tmp_var[1][0] = MAX(tmp_var[1][0],DSGM_VAR_INIT);
413 |             tmp_var[1][1] = MAX(tmp_var[1][1],DSGM_VAR_INIT);
414 |             tmp_var[1][2] = MAX(tmp_var[1][2],DSGM_VAR_INIT);
415 |             
416 |             // writing tmp stuffs back to the model
417 |             
418 |             current_dsgm_out.currentModel().mean = tmp_mean[0];
419 |             current_dsgm_out.inactiveModel().mean = tmp_mean[1];
420 |             
421 |             current_dsgm_out.currentModel().age = tmp_age[0];
422 |             current_dsgm_out.inactiveModel().age = tmp_age[1];
423 |                         
424 |             
425 |             if (c_idx <1 || c_idx >= grid_layout_.width-1 || r_idx <1 || r_idx >= grid_layout_.height-1){
426 |                 
427 |                 current_dsgm_out.currentModel().var = Vec3f(DSGM_VAR_INIT,DSGM_VAR_INIT,DSGM_VAR_INIT);
428 |                 current_dsgm_out.inactiveModel().var = Vec3f(DSGM_VAR_INIT,DSGM_VAR_INIT,DSGM_VAR_INIT);
429 |                                 
430 |                 current_dsgm_out.currentModel().age = 0;
431 |                 current_dsgm_out.inactiveModel().age = 0;                
432 |                 
433 |             }else{
434 |                 
435 |                 current_dsgm_out.currentModel().var = tmp_var[0];
436 |                 current_dsgm_out.inactiveModel().var = tmp_var[1];
437 | 
438 | //                float decay_factor = exp(-lambda_decay_ * MAX(0,tmp_var[0]-sigma_decay_));
439 | //                current_dsgm_out.currentModel().age =  MIN(decay_factor * current_dsgm_out.currentModel().age, max_age_) ;
440 |                 
441 | //                decay_factor = exp(-lambda_decay_ * MAX(0,tmp_var[1]-sigma_decay_));
442 | //                current_dsgm_out.inactiveModel().age =  MIN(decay_factor * current_dsgm_out.inactiveModel().age, max_age_) ;
443 |             }
444 |             
445 |             if (current_dsgm_out.inactiveModel().age>current_dsgm_out.currentModel().age)
446 |                 current_dsgm_out.swapModel();
447 |         }
448 |     }
449 |     
450 | }
451 | 
452 | 
453 | void MotionDetector::updateDualSGM(const Mat &in_img, const DualSGM *in_dsgm, DualSGM *out_dsgm, Mat *out_motion_mask){
454 |     
455 |     for (int r=0, count = 0;r<grid_layout_.height;r++){
456 |         for (int c=0;c<grid_layout_.width;c++, count++){
457 |             
458 |             const DualSGM& current_dsgm_in = in_dsgm[count];
459 |             DualSGM& current_dsgm_out = out_dsgm[count];
460 |             
461 |             
462 |             const int y_base = r * patch_size_.height;
463 |             const int x_base = c * patch_size_.width;
464 |         
465 |             Vec3f mean_tmp = 0;
466 |             float no_points = 0;
467 |             
468 |             // compute mean of current image block
469 |             for (int i = 0; i<patch_size_.height; i++){
470 |                 for(int j = 0; j<patch_size_.width; j++){
471 |                     mean_tmp += in_img.at<Vec3b>(y_base+i,x_base+j);
472 |                     no_points++;
473 |                 }
474 |             }
475 |             mean_tmp /= no_points;
476 |             
477 |             current_dsgm_out.copyFrom(current_dsgm_in);
478 |             
479 |             const SGM& active_sgm_in = current_dsgm_in.currentModel();
480 |             const SGM& inactive_sgm_in = current_dsgm_in.inactiveModel();
481 |                         
482 |             SGM& active_sgm_out = current_dsgm_out.currentModel();
483 |             SGM& inactive_sgm_out = current_dsgm_out.inactiveModel();
484 |                     
485 |             if (active_sgm_in.age == 0 && inactive_sgm_in.age == 0){
486 |                 active_sgm_out.mean = mean_tmp;
487 |                 active_sgm_out.var = Vec3f(DSGM_VAR_INIT,DSGM_VAR_INIT,DSGM_VAR_INIT);
488 |                 active_sgm_out.age = 1;
489 |                 continue;
490 |             }
491 |             
492 |             SGM* sgm_to_update = &active_sgm_out;
493 |             
494 |             /////////////////////////////////////////////
495 |             //
496 |             // update mean fist here
497 |             // with the current mean, decide with model to update
498 |             //
499 |             /////////////////////////////////////////////
500 |             
501 |             // current mean fits active model
502 |             if (active_sgm_in.classify(mean_tmp,sigma_blend_))
503 |             {
504 |                 active_sgm_out.age = active_sgm_in.age + 1;
505 |                 float weight = 1.0 / active_sgm_out.age;
506 |                 active_sgm_out.mean = (1-weight) * active_sgm_in.mean + weight * mean_tmp;
507 |                 sgm_to_update = &active_sgm_out;
508 |             } 
509 |             // current mean fits inactive model
510 |             else if (inactive_sgm_in.classify(mean_tmp,sigma_blend_))
511 |             {
512 |                 inactive_sgm_out.age = inactive_sgm_in.age + 1;
513 |                 float weight = 1.0 / inactive_sgm_out.age;
514 |                 inactive_sgm_out.mean = (1-weight) * inactive_sgm_in.mean + weight * mean_tmp;
515 |                 sgm_to_update = &inactive_sgm_out;
516 |             }
517 |             // current mean doesn't fit anything, re-initialzie inactive model
518 |             else 
519 |             {
520 |                 inactive_sgm_out.mean = mean_tmp;
521 |                 inactive_sgm_out.age = 1;
522 |                 sgm_to_update = &inactive_sgm_out;
523 |             }
524 | 
525 |             
526 |             /////////////////////////////////////////////
527 |             //
528 |             // then use the updated mean to update variance
529 |             //
530 |             /////////////////////////////////////////////
531 |             
532 |             Vec3f var_tmp = Vec3f(DSGM_VAR_INIT,DSGM_VAR_INIT,DSGM_VAR_INIT);      
533 |             for (int i = 0; i<patch_size_.height; i++){
534 |                 for(int j = 0; j<patch_size_.width; j++){
535 |                     Vec3f diff =  Vec3f(in_img.at<Vec3b>(y_base+i,x_base+j)) - sgm_to_update->mean;
536 |                     var_tmp[0] = MAX(pow(diff[0], (int)2),var_tmp[0]);
537 |                     var_tmp[1] = MAX(pow(diff[1], (int)2),var_tmp[1]);
538 |                     var_tmp[2] = MAX(pow(diff[2], (int)2),var_tmp[2]);
539 |                 }
540 |             }
541 |             
542 |             if(sgm_to_update->age == 1){
543 |                 sgm_to_update->var = var_tmp;
544 |             }else{
545 |                 float weight = 1.0f /  sgm_to_update->age;
546 |                 sgm_to_update->var = (1-weight) * sgm_to_update->var + weight * var_tmp;
547 |                 sgm_to_update->age = MIN(sgm_to_update->age, max_age_);    
548 |             }
549 |             
550 |             if (active_sgm_out.age < inactive_sgm_out.age) {
551 |                 current_dsgm_out.swapModel();
552 |                 //cout<<"swap!"<<endl;
553 |             }
554 |             
555 |         }    
556 |     }
557 |     
558 | }
559 | 
560 | #define SHOW_DSGM_MODEL
561 | void MotionDetector::updateBackgroundModel(const Mat &in_img, const Hom3x3 pose, Mat* out_motion_mask)
562 | {
563 |     // preprocess image by smoothing
564 |     prepareData(in_img);
565 |     Matx33f warp = computeBackgroundMotion(pre_small_gray_image_,cur_small_gray_image_); 
566 |     compensateMotion(background_models_, warped_background_models_,warp.inv());
567 |     updateDualSGM(small_blur_image_,warped_background_models_,background_models_, out_motion_mask);
568 |     
569 |     cur_small_gray_image_.copyTo(pre_small_gray_image_);
570 |     
571 | #ifdef SHOW_DSGM_MODEL    
572 | //// debug   gaussian means
573 |     Mat sgm_image(grid_layout_,CV_8UC3);
574 |     for (int r=0, count = 0;r<grid_layout_.height;r++){
575 |         for (int c=0;c<grid_layout_.width;c++, count++){
576 |             DualSGM& bm = background_models_[count];
577 |             SGM& current_model = bm.currentModel();
578 |             sgm_image.at<Vec3b>(r,c) = current_model.mean;
579 |         }
580 |     }
581 |     imshow("sgm_mean", sgm_image);
582 |     
583 |     Mat sgm_warp_image(grid_layout_,CV_8UC3);
584 |     for (int r=0, count = 0;r<grid_layout_.height;r++){
585 |         for (int c=0;c<grid_layout_.width;c++, count++){
586 |             DualSGM& bm = background_models_[count];
587 |             SGM& current_model = bm.inactiveModel();
588 |             sgm_warp_image.at<Vec3b>(r,c) = current_model.mean;
589 |         }
590 |     }
591 |     imshow("warped_mean", sgm_warp_image);
592 | #endif
593 |     
594 | }
595 | 
596 | 
597 | 
598 | void MotionDetector::detectMotion(const Mat &in_img, Mat &out_motion_mask, Hom3x3 pose){
599 |     
600 |     updateBackgroundModel(in_img,pose);
601 | 
602 |     resize(in_img,out_motion_mask,target_size_);
603 |     
604 |     small_motion_mask_ = Scalar(0);
605 |     for (int r=0, idx=0;r<grid_layout_.height;r++){
606 |         for (int c=0;c<grid_layout_.width;c++,idx++){
607 |         
608 |             const SGM& current_sgm = background_models_[idx].currentModel();
609 |             
610 |             const int y_base = r * patch_size_.height;
611 |             const int x_base = c * patch_size_.width;
612 |             
613 |             for (int i = 0; i<patch_size_.height; i++){
614 |                 for(int j = 0; j<patch_size_.width; j++){
615 |                     const Vec3b val = small_blur_image_.at<Vec3b>(y_base+i,x_base+j);
616 |                     
617 |                     if (!current_sgm.classify(val,sigma_classify_))
618 |                         out_motion_mask.at<Vec3b>(y_base+i,x_base+j) = Vec3b(0,0,255);
619 |                     
620 |                 }
621 |             }
622 |             
623 |         }
624 |     }
625 |     
626 | }
627 | 


--------------------------------------------------------------------------------
/MotionDetection/MotionDetector.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | #include <opencv2/opencv.hpp>
  4 | 
  5 | #define DSGM_VAR_INIT (20.0*20.0)
  6 | 
  7 | 
  8 | class MotionDetector{
  9 |     
 10 |     struct SGM{
 11 |         
 12 |         cv::Vec3f  mean;
 13 |         cv::Vec3f var;
 14 |         float age;
 15 |         
 16 |         SGM():mean(cv::Vec3f(0,0,0)),var(cv::Vec3f(DSGM_VAR_INIT,DSGM_VAR_INIT,DSGM_VAR_INIT)),age(0){}
 17 |         
 18 |         bool classify(cv::Vec3b val, float theta = 1.0f) const {
 19 |             
 20 |             return
 21 |             (pow(val[0] - mean[0],(int)2) <= theta * var[0]) &&
 22 |             (pow(val[1] - mean[1],(int)2) <= theta * var[1]) &&
 23 |             (pow(val[2] - mean[2],(int)2) <= theta * var[2]);        
 24 |             
 25 |         }
 26 |         
 27 |         bool classify(cv::Vec3f val, float theta = 1.0f) const {
 28 |             
 29 |             return
 30 |             (pow(val[0] - mean[0],(int)2) <= theta * var[0]) &&
 31 |             (pow(val[1] - mean[1],(int)2) <= theta * var[1]) &&
 32 |             (pow(val[2] - mean[2],(int)2) <= theta * var[2]);        
 33 |             
 34 |         }
 35 |         
 36 |         void clear()
 37 |         {
 38 |             mean = cv::Vec3f(0,0,0);
 39 |             var = cv::Vec3f(DSGM_VAR_INIT,DSGM_VAR_INIT,DSGM_VAR_INIT);
 40 |             age = 0;
 41 |         }
 42 |     };
 43 |     
 44 |     struct DualSGM{
 45 |         
 46 |         SGM models[2];
 47 |         int active_idx;
 48 |         
 49 |         SGM& currentModel(){
 50 |             return models[active_idx];
 51 |         }
 52 |         
 53 |         const SGM& currentModel() const{
 54 |             return models[active_idx];
 55 |         }
 56 |         
 57 |         SGM& inactiveModel(){
 58 |             return models[!active_idx];
 59 |         }
 60 |         
 61 |         const SGM& inactiveModel() const{
 62 |             return models[!active_idx];
 63 |         }
 64 |         
 65 |         void copyFrom(const DualSGM& rhs){
 66 |             models[0] = rhs.models[0];
 67 |             models[1] = rhs.models[1];
 68 |             active_idx = rhs.active_idx;
 69 |         }
 70 |         
 71 |         void swapModel(){
 72 |             active_idx = !active_idx;
 73 |         }
 74 |         
 75 |         DualSGM():active_idx(0){}
 76 |         
 77 |         void clear()
 78 |         {
 79 |             models[0].clear();
 80 |             models[1].clear();
 81 |             active_idx = 0;
 82 |         }
 83 |     };
 84 |         
 85 |     typedef cv::Matx33f Hom3x3;
 86 |     
 87 | private:
 88 |     
 89 |     bool model_initialized_;
 90 |     bool pre_image_loaded;
 91 |     bool motion_compensated_;
 92 |     
 93 |     float sigma_blend_;
 94 |     float sigma_classify_;
 95 |     float sigma_decay_;
 96 |     float lambda_decay_;
 97 |     
 98 |     float max_age_;
 99 |     
100 |     cv::Size original_size_;
101 |     cv::Size target_size_;
102 |     cv::Size grid_layout_;
103 |     cv::Size patch_size_;
104 |     
105 |     int total_grid_no_;
106 |     
107 |     cv::Mat gray_image_;
108 |     cv::Mat small_rgb_image_;
109 |     cv::Mat small_blur_image_;
110 |     cv::Mat small_motion_mask_;
111 |     
112 |     cv::Mat pre_small_gray_image_;
113 |     cv::Mat cur_small_gray_image_;
114 |     
115 |     DualSGM *background_models_;
116 |     DualSGM *warped_background_models_;
117 |     
118 |     
119 |     void prepareData(const cv::Mat& in_img);
120 |     
121 |     // this homography directly warp prev_frame to cur_frame, with target_size
122 |     cv::Matx33f computeBackgroundMotion(const cv::Mat& pre_img, const cv::Mat& cur_img);
123 |     
124 |     void updateDualSGM(const cv::Mat& in_img, const DualSGM* in_dsgm, DualSGM *out_dsgm, cv::Mat* out_motion_mask = NULL);
125 |    
126 |     void compensateMotion(const DualSGM* in_dsgm, DualSGM* out_dsgm, cv::Matx33f pose);
127 |        
128 |    void updateBackgroundModel(const cv::Mat& in_img, const Hom3x3 pose, cv::Mat* out_motion_mask = NULL);
129 |    
130 | public:
131 |     MotionDetector(bool need_rotate=false);
132 |     ~MotionDetector();
133 |     
134 |     void detectMotion(const cv::Mat& in_img, cv::Mat& out_motion_mask, Hom3x3 pose);
135 |     
136 |     
137 | };
138 | 
139 | 
140 | 


--------------------------------------------------------------------------------
/MotionDetection/README.md:
--------------------------------------------------------------------------------
 1 | Build:
 2 | ```
 3 | mkdir build
 4 | cd build
 5 | cmake ..
 6 | make -j$(nproc)
 7 | ```
 8 | 
 9 | Run demo
10 | ```
11 | ./MotionDetection ../data/VOT3.mp4 
12 | ```
13 | 
14 | Original paper:
15 | Detection of Moving Objects with Non-Stationary Cameras in 5.8ms: Bringing Motion Detection to your Mobile Device
16 | 


--------------------------------------------------------------------------------
/MotionDetection/data/VOT3.mp4:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/carlren/VisionTools/d14d4049989b1c1f99ab3d1b8c85a20ae06c16e1/MotionDetection/data/VOT3.mp4


--------------------------------------------------------------------------------
/MotionDetection/main.cpp:
--------------------------------------------------------------------------------
  1 | #include <string>
  2 | #include<stdexcept>
  3 | #include <opencv2/opencv.hpp>
  4 | 
  5 | #include"MotionDetector.h"
  6 | 
  7 | using namespace cv;
  8 | using namespace std;
  9 | 
 10 | ////////////////////////////////////////////////////////////////////////////////
 11 | std::vector <cv::Mat3b>
 12 | loadVideo (const std::string& input_filename, bool need_rotate=false)
 13 | {
 14 |     std::cerr << "Loading video '" << input_filename << "'\n";
 15 |     cv::VideoCapture video_capture (input_filename);
 16 |     std::vector <cv::Mat3b> frames;
 17 |     
 18 |     //size_t i = 0;
 19 |     while (video_capture.grab ())
 20 |     {
 21 |         cv::Mat3b frame;
 22 |         video_capture.retrieve (frame);
 23 |         
 24 |         if(need_rotate)
 25 |         {
 26 |             cv::transpose(frame.clone(),frame);
 27 |         }
 28 |             frames.push_back (frame.clone ());
 29 | 
 30 |     }
 31 |     if (frames.empty ())
 32 |     {
 33 |         throw std::runtime_error ("The input frames are empty");
 34 |     }
 35 |     std::cerr << "Loaded " << frames.size () << " frames." << std::endl;
 36 |     
 37 |     return (frames);
 38 | }
 39 | ////////////////////////////////////////////////////////////////////////////////
 40 | 
 41 | void RunMotionDetection(std::string& video_name, bool need_rotate){
 42 |     
 43 |     std::vector<cv::Mat3b> frames = loadVideo(video_name, need_rotate);
 44 |     int w = frames[0].cols/3;
 45 |     int h = frames[0].rows/3;    
 46 |     Size tSize(w,h);
 47 |   
 48 |     Mat smallFrame(tSize.height,tSize.width,CV_8UC3);
 49 |     Mat motionFrame(tSize.height,tSize.width,CV_8UC3);
 50 |     
 51 |     bool show_frames = true;
 52 |     bool need_refresh =true;
 53 |     int frame_id = 1;
 54 |     
 55 |     MotionDetector* motion_detector = new MotionDetector(need_rotate);
 56 |         
 57 |     namedWindow("origin",WINDOW_AUTOSIZE|WINDOW_OPENGL);
 58 |     namedWindow("motion",WINDOW_AUTOSIZE|WINDOW_OPENGL);
 59 |     namedWindow("sgm_mean",WINDOW_AUTOSIZE|WINDOW_OPENGL);
 60 |     namedWindow("warped_mean",WINDOW_AUTOSIZE|WINDOW_OPENGL);
 61 |     
 62 |     
 63 |      moveWindow("origin",100,100);
 64 |      moveWindow("motion",100,1000);
 65 |      moveWindow("sgm_mean", 500,100);
 66 |      moveWindow("warped_mean", 500,300);
 67 |      
 68 |      
 69 |     while(show_frames){
 70 |         
 71 |         if(need_refresh)
 72 |         {
 73 |             resize(frames[frame_id], smallFrame, tSize);
 74 |                         
 75 |             size_t tic = getTickCount();
 76 |             motion_detector->detectMotion(smallFrame,motionFrame,Matx33f::eye());
 77 |             size_t toc = getTickCount() - tic;
 78 |             cout<<frame_id<<"-detection time:" << (double) toc * 1000 / getTickFrequency() << " ms" <<endl;
 79 |                         
 80 |             need_refresh = false;
 81 |         }
 82 |         
 83 |         imshow("origin", smallFrame);
 84 |         imshow("motion",motionFrame);
 85 |         
 86 |         
 87 |         char key = cv::waitKey(1);
 88 |         
 89 |         switch (key) {
 90 |             case 27: case 'q':
 91 |                 show_frames = false;
 92 |                 break;
 93 |             case 'x':
 94 |                 ++frame_id;
 95 |                 if (frame_id>=frames.size()-1) {
 96 |                     frame_id = frames.size()-2;
 97 |                 }
 98 |                 else need_refresh = true;
 99 |                 break;
100 |             case 'z':
101 |                 --frame_id;
102 |                 if (frame_id<=1) {
103 |                     frame_id=1;
104 |                 }
105 |                 else need_refresh = true;
106 |                 break;
107 |             default:
108 |                 break;
109 |         }
110 |     }
111 |     
112 | }
113 | /////////////////////////////////////////////////////////////////////////////
114 | 
115 | int main(int argc, char** argv)
116 | {
117 |     
118 |     if (argc!=2) {
119 |         cerr<<"Useage: ./MotionDetection <input video name>"<<endl;
120 |         return -1;
121 |     }
122 |     else
123 |     {
124 |         string video_name = argv[1];
125 |         RunMotionDetection(video_name,false);
126 |         return 0;
127 |     }
128 | }
129 | 


--------------------------------------------------------------------------------
/PrimeSenseCalib/.gitignore:
--------------------------------------------------------------------------------
1 | build/
2 | *.user
3 | 


--------------------------------------------------------------------------------
/PrimeSenseCalib/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8)
 2 | project(RGBDCalib)
 3 | 
 4 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/cmake/")
 5 | 
 6 | #set(OPEN_NI_ROOT /home/carl/Work/3rdparty/OpenNI2/)
 7 | 
 8 | find_package(OpenCV REQUIRED)
 9 | find_package(OpenNI REQUIRED)
10 | 
11 | 
12 | IF(MSVC_IDE)
13 |   add_definitions(-D_CRT_SECURE_NO_WARNINGS)
14 |   add_definitions(-DUSING_CMAKE=1)
15 | ELSE(MSVC_IDE)
16 |   set(CFLAGS_WARN "-Wall -Wextra -Wno-unused-parameter -Wno-strict-aliasing")
17 |   set(CMAKE_CXX_FLAGS "-fPIC -O3 -march=native ${CFLAGS_WARN} ${CMAKE_CXX_FLAGS}")
18 | ENDIF(MSVC_IDE)
19 | 
20 | 
21 | include_directories("/home/carl/Work/3rdparty")
22 | include_directories("/home/carl/Work/3rdparty/Eigen3")
23 | include_directories(${OPENCV_INCLUDE_DIRS})
24 | include_directories(${OpenNI_INCLUDE_DIR})
25 | 
26 | add_library(OpenNIEngine
27 | OpenNIEngine.h
28 | OpenNIEngine.cpp
29 | )
30 | 
31 | add_executable(CalibCapture 
32 | OpenNICapture.cpp 
33 | )
34 | 
35 | add_executable(StereoCalib
36 | StereoCalib.cpp)
37 | 
38 | add_executable(TestCalibResult
39 | OpenNIEngine.h
40 | OpenNIEngine.cpp
41 | CalibTest.cpp)
42 | 
43 | target_link_libraries(OpenNIEngine
44 | ${OpenCV_LIBS} 
45 | ${OpenNI_LIBRARY}
46 | )
47 | 
48 | target_link_libraries(CalibCapture 
49 | OpenNIEngine
50 | ${OpenCV_LIBS} 
51 | ${OpenNI_LIBRARY}
52 | )
53 | 
54 | 
55 | target_link_libraries(StereoCalib 
56 | ${OpenCV_LIBS} 
57 | )
58 | 
59 | target_link_libraries(TestCalibResult 
60 | OpenNIEngine
61 | ${OpenCV_LIBS} 
62 | ${OpenNI_LIBRARY})
63 | 
64 | 


--------------------------------------------------------------------------------
/PrimeSenseCalib/CalibTest.cpp:
--------------------------------------------------------------------------------
  1 | 
  2 | #include<stdio.h>
  3 | #include<opencv2/opencv.hpp>
  4 | #include<fstream>
  5 | #include"OpenNIEngine.h"
  6 | 
  7 | using namespace std;
  8 | using namespace cv;
  9 | 
 10 | void 
 11 | mapDepthToColor(
 12 |         const Mat& rgb, 
 13 |         const Mat& raw_depth, 
 14 |         Matx44f M_d2rgb, 
 15 |         Matx33f K_depth,
 16 |         Matx33f K_rgb,
 17 |         Mat& out_rgb)
 18 | {
 19 |     cvtColor(rgb,out_rgb,CV_BGR2RGB);
 20 |     float max_depth=4000;
 21 |     
 22 | //    for (int i=0;i<rgb.rows;i++)
 23 | //        for (int j=0;j<rgb.cols;j++)
 24 | //        {
 25 | //            max_depth = max(max_depth,(float)raw_depth.at<unsigned short>(i,j));
 26 | //        }
 27 |     
 28 |     for (int i=0;i<rgb.rows;i++)
 29 |         for (int j=0;j<rgb.cols;j++)
 30 |         {
 31 |             unsigned short d = raw_depth.at<unsigned short>(i,j);
 32 |             
 33 |             if (d>=4000||d==0){
 34 |                 continue;
 35 |             }
 36 |             else
 37 |             {
 38 |                 unsigned char dColor =(1-(float)d/max_depth)*255;
 39 |                 float fd = (float)d/1000;
 40 |                 Vec3f pt_w = K_depth.inv()*Vec3f(j*fd, i*fd, fd);
 41 |                 Vec4f pt_cw = M_d2rgb * Vec4f(pt_w[0],pt_w[1],pt_w[2], 1);
 42 |                 Vec3f pt_color = K_rgb * Vec3f(pt_cw[0],pt_cw[1],pt_cw[2]);
 43 |                 
 44 |                 int x_w = pt_color[0] / pt_color[2];
 45 |                 int y_w = pt_color[1] / pt_color[2];
 46 |                 
 47 |                 if(x_w>=0 && x_w < rgb.cols && y_w>=0 && y_w < rgb.rows)
 48 |                 {
 49 |                     out_rgb.at<Vec3b>(y_w,x_w)[3] = dColor;   
 50 |                     out_rgb.at<Vec3b>(y_w,x_w)[2] = dColor;   
 51 |                 }
 52 |             }
 53 |         }
 54 | }
 55 | 
 56 | int main(int argc, char** argv){
 57 | 
 58 |     Mat R_d2rgb,T_d2rgb;
 59 |     Matx44d M_d2rgb = Matx44d::eye();
 60 |     Matx44d M_rgb2d = Matx44d::eye();
 61 |     Mat K_depth, K_rgb;
 62 |     
 63 |     FileStorage fs("intrinsics.yml",FileStorage::READ);
 64 |     fs["M1"]>>K_depth;
 65 |     fs["M2"]>>K_rgb;
 66 |     fs.release();
 67 |     
 68 |     fs.open("extrinsics.yml", FileStorage::READ);
 69 |     fs["R"]>>R_d2rgb;
 70 |     fs["T"]>>T_d2rgb;
 71 |     fs.release();
 72 |     
 73 |     Matx33d Kd(K_depth);
 74 |     Matx33d Kc(K_rgb);
 75 |     
 76 |     cout << Kd << endl;
 77 |     cout << Kc << endl;
 78 |     
 79 |     cout << R_d2rgb << endl<<endl;
 80 |     cout << T_d2rgb.t() << endl<<endl;
 81 | 
 82 |     for (int r=0;r<3;r++) {
 83 |         for (int c=0;c<3;c++){
 84 |             M_d2rgb(r,c) = R_d2rgb.at<double>(r,c);
 85 |         }
 86 |         M_d2rgb(r,3) = T_d2rgb.at<double>(r,0);
 87 |     }
 88 |     
 89 |     cout<<M_d2rgb<<endl;
 90 |     M_rgb2d = M_d2rgb.inv();    
 91 |     
 92 |     Mat rgb(480,640,CV_8UC3);
 93 |     Mat depth(480,640,CV_16UC1);
 94 |     Mat depth_show(480,640,CV_8UC1);
 95 |     Mat rgb_aligned(480,640,CV_8UC3);
 96 |     Size size(640,480);
 97 |     
 98 |     OpenNIEngine* openni_engine = new OpenNIEngine();
 99 | 
100 | 
101 | 
102 |     while(true)
103 |     {
104 |         openni_engine->getRGBDImages(rgb,depth);       
105 |         mapDepthToColor(rgb,depth,M_d2rgb,Kd,Kc,rgb_aligned);
106 |         imshow("aligned",rgb_aligned);
107 |         
108 | //        depth.convertTo(depth_show,CV_8UC1);
109 | //        imshow("depth",depth_show);
110 |         
111 |         char key = waitKey(1);        
112 |         if(key=='q') break;
113 |     }
114 |     
115 |     ofstream ofs("Calib_ITM.txt");
116 |     ofs<<640<<" "<<480<<endl;
117 |     ofs<<Kc(0,0)<<" "<<Kc(1,1)<<endl;
118 |     ofs<<Kc(0,2)<<" "<<Kc(1,2)<<endl;
119 | 
120 |     ofs<<endl;
121 |     
122 |     ofs<<640<<" "<<480<<endl;
123 |     ofs<<Kd(0,0)<<" "<<Kd(1,1)<<endl;
124 |     ofs<<Kd(0,2)<<" "<<Kd(1,2)<<endl;
125 |     
126 |     ofs<<endl;
127 |     
128 |     for (int r=0;r<3;r++) {
129 |         for (int c=0;c<4;c++){
130 |             ofs<<M_rgb2d(r,c)<<" ";
131 |         }
132 |         ofs<<endl;
133 |     }
134 |     
135 |     ofs<<endl;
136 | 
137 |     ofs<<0<<" "<<0;
138 |     
139 |     ofs.close();
140 |     
141 |     imwrite("testdepth.pgm",depth);
142 |     
143 |     delete openni_engine;
144 | 
145 |     return 0;
146 | }
147 | 


--------------------------------------------------------------------------------
/PrimeSenseCalib/OpenNICapture.cpp:
--------------------------------------------------------------------------------
 1 | #include<stdio.h>
 2 | #include<opencv2/opencv.hpp>
 3 | #include<fstream>
 4 | #include"OpenNIEngine.h"
 5 | 
 6 | using namespace std;
 7 | using namespace cv;
 8 | 
 9 | int main(int argc, char** argv){
10 | 
11 |     Mat rgb(480,640,CV_8UC3);
12 |     Mat depth(480,640,CV_16UC1);
13 |     Mat IR,gray;
14 |     
15 |     Size size(640,480);
16 |     
17 |     OpenNIEngine* openni_engine = new OpenNIEngine();
18 |     
19 |     ofstream ofs("image_lists.txt");
20 |     
21 |     int count = 0;
22 |     char out_name[200];
23 |     
24 |     while(true)
25 |     {
26 |         openni_engine->getRGBDImages(rgb,depth);
27 |         imshow("rgb",rgb);
28 |         
29 |         char key = waitKey(1);
30 |         
31 |         if(key == 's'){
32 |             openni_engine->shotGrayAndIRImages(gray,IR);
33 |             imshow("IR",IR);
34 |             imshow("gray",gray);
35 |             
36 |             sprintf(out_name,"%04d_left.jpg",count);
37 |             ofs<<out_name<<endl;
38 |             cout<<out_name<<"\t";
39 |            imwrite(out_name,IR);
40 |            
41 |             sprintf(out_name,"%04d_rignt.jpg",count);
42 |             ofs<<out_name<<endl;
43 |            cout<<out_name<<endl;
44 |            imwrite(out_name,gray);
45 | 
46 |             
47 |             count++;
48 |         }
49 |         
50 |         if(key=='q') break;
51 |     }
52 |     ofs.close();
53 |     
54 |     delete openni_engine;
55 | 
56 |     return 0;
57 | }
58 | 


--------------------------------------------------------------------------------
/PrimeSenseCalib/OpenNIEngine.cpp:
--------------------------------------------------------------------------------
  1 | #include "OpenNIEngine.h"
  2 | #include <cstdio>
  3 | #include <stdexcept>
  4 | 
  5 | using namespace cv;
  6 | using namespace std;
  7 | 
  8 | static openni::VideoMode 
  9 | findBestMode(
 10 |         const openni::SensorInfo *sensorInfo, 
 11 |         int requiredResolutionX = -1, 
 12 |         int requiredResolutionY = -1, 
 13 |         openni::PixelFormat requiredPixelFormat = (openni::PixelFormat)-1)
 14 | {
 15 | 	const openni::Array<openni::VideoMode> & modes = sensorInfo->getSupportedVideoModes();
 16 | 	openni::VideoMode bestMode = modes[0];
 17 | 	for (int m = 0; m < modes.getSize(); ++m) {
 18 | 		//fprintf(stderr, "mode %i: %ix%i, %i %i\n", m, modes[m].getResolutionX(), modes[m].getResolutionY(), modes[m].getFps(), modes[m].getPixelFormat());
 19 | 		const openni::VideoMode & curMode = modes[m];
 20 | 		if ((requiredPixelFormat != (openni::PixelFormat)-1)&&(curMode.getPixelFormat() != requiredPixelFormat)) continue;
 21 | 
 22 | 		bool acceptAsBest = false;
 23 | 		if ((curMode.getResolutionX() == bestMode.getResolutionX())&&
 24 | 		     (curMode.getFps() > bestMode.getFps())) {
 25 | 			acceptAsBest = true;
 26 | 		} else if ((requiredResolutionX <= 0)&&(requiredResolutionY <= 0)) {
 27 | 			if (curMode.getResolutionX() > bestMode.getResolutionX()) {
 28 | 				acceptAsBest = true;
 29 | 			}
 30 | 		} else {
 31 | 			int diffX_cur = abs(curMode.getResolutionX()-requiredResolutionX);
 32 | 			int diffX_best = abs(bestMode.getResolutionX()-requiredResolutionX);
 33 | 			int diffY_cur = abs(curMode.getResolutionY()-requiredResolutionY);
 34 | 			int diffY_best = abs(bestMode.getResolutionY()-requiredResolutionY);
 35 | 			if (requiredResolutionX > 0) {
 36 | 				if (diffX_cur < diffX_best) {
 37 | 					acceptAsBest = true;
 38 | 				}
 39 | 				if ((requiredResolutionY > 0)&&(diffX_cur == diffX_best)&&(diffY_cur < diffY_best)) {
 40 | 					acceptAsBest = true;
 41 | 				}
 42 | 			} else if (requiredResolutionY > 0) {
 43 | 				if (diffY_cur < diffY_best) {
 44 | 					acceptAsBest = true;
 45 | 				}
 46 | 			}
 47 | 		}
 48 | 		if (acceptAsBest) bestMode = curMode;
 49 | 	}
 50 | 	//fprintf(stderr, "=> best mode: %ix%i, %i %i\n", bestMode.getResolutionX(), bestMode.getResolutionY(), bestMode.getFps(), bestMode.getPixelFormat());
 51 | 	return bestMode;
 52 | }
 53 | 
 54 | 
 55 | OpenNIEngine::OpenNIEngine(const char *device_URI, 
 56 |         const bool use_internal_calibration, 
 57 |         Size requested_size_rgb, 
 58 |         Size requested_size_d)
 59 | {
 60 | 	if (device_URI==NULL) device_URI = openni::ANY_DEVICE;
 61 | 	openni::Status rc = openni::STATUS_OK;
 62 | 
 63 | 	rc = openni::OpenNI::initialize();
 64 | 	printf("OpenNI: Initialization ... \n%s\n", openni::OpenNI::getExtendedError());
 65 | 
 66 |     rc = openni_device_data.device.open(device_URI);
 67 | 	if (rc != openni::STATUS_OK)
 68 | 	{
 69 | 		std::string message("OpenNI: Device open failed!\n");
 70 | 		message += openni::OpenNI::getExtendedError();
 71 | 		openni::OpenNI::shutdown();
 72 | 		std::cout << message;
 73 | 		return;
 74 | 	}
 75 |     
 76 |     openni::PlaybackControl *control = openni_device_data.device.getPlaybackControl();
 77 | 	if (control != NULL) {
 78 | 		// this is a file! make sure we get every frame
 79 | 		control->setSpeed(-1.0f);
 80 | 		control->setRepeatEnabled(false);
 81 | 	}
 82 | 
 83 |     initRGBDStreams(use_internal_calibration, requested_size_d,requested_size_d);
 84 |     
 85 |     tmp_ir_image.create(image_size_depth,CV_16UC1);
 86 |     tmp_RGB_image.create(image_size_rgb,CV_8UC3);
 87 | }
 88 | 
 89 | 
 90 | void OpenNIEngine::initRGBDStreams(const bool use_internal_calibration, Size requested_size_rgb, Size requested_size_d)
 91 | {
 92 |     // create depth stream
 93 |     openni::Status rc = openni_device_data.depthStream.create(openni_device_data.device,openni::SENSOR_DEPTH);
 94 | 	if (rc == openni::STATUS_OK)
 95 | 	{
 96 | 		openni::VideoMode depthMode = findBestMode( openni_device_data.device.getSensorInfo(openni::SENSOR_DEPTH), requested_size_d.width, requested_size_d.height, openni::PIXEL_FORMAT_DEPTH_1_MM);
 97 | 		image_size_depth.width = depthMode.getResolutionX();
 98 | 		image_size_depth.height = depthMode.getResolutionY();
 99 | 		rc = openni_device_data.depthStream.setVideoMode(depthMode);
100 | 		if (rc != openni::STATUS_OK)
101 | 		{
102 | 			printf("OpenNI: Failed to set depth mode\n");
103 | 		}
104 |         openni_device_data.depthStream.setMirroringEnabled(false);
105 |         rc = openni_device_data.depthStream.start();
106 | 
107 | 
108 | 		if (use_internal_calibration) 
109 |             openni_device_data.device.setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR);
110 |         
111 | 		if (rc != openni::STATUS_OK)
112 | 		{
113 | 			printf("OpenNI: Couldn't start depthStream stream:\n%s\n", openni::OpenNI::getExtendedError());
114 |             openni_device_data.depthStream.destroy();
115 | 		}
116 | 		printf("Initialised OpenNI depth camera with resolution: %d x %d\n", image_size_depth.width,image_size_depth.height);
117 | 
118 | 		depth_available = true;
119 | 	}
120 | 	else
121 | 	{
122 | 		printf("OpenNI: Couldn't find depthStream stream:\n%s\n", openni::OpenNI::getExtendedError());
123 | 		depth_available = false;
124 | 	}
125 | 
126 |     
127 |     // create color stream
128 | 	rc = openni_device_data.colorStream.create(openni_device_data.device, openni::SENSOR_COLOR);
129 | 	if (rc == openni::STATUS_OK)
130 | 	{
131 | 		openni::VideoMode colourMode = findBestMode(openni_device_data.device.getSensorInfo(openni::SENSOR_COLOR), requested_size_rgb.width, requested_size_rgb.height);
132 | 		image_size_rgb.width = colourMode.getResolutionX();
133 | 		image_size_rgb.height = colourMode.getResolutionY();
134 | 		rc = openni_device_data.colorStream.setVideoMode(colourMode);
135 | 		if (rc != openni::STATUS_OK)
136 | 		{
137 | 			printf("OpenNI: Failed to set color mode\n");
138 | 		}
139 | 		openni_device_data.colorStream.setMirroringEnabled(false);
140 | 
141 | 		rc = openni_device_data.colorStream.start();
142 | 		if (rc != openni::STATUS_OK)
143 | 		{
144 | 			printf("OpenNI: Couldn't start colorStream stream:\n%s\n", openni::OpenNI::getExtendedError());
145 | 			openni_device_data.colorStream.destroy();
146 | 		}
147 | 
148 | 		printf("Initialised OpenNI color camera with resolution: %d x %d\n", image_size_rgb.width, image_size_rgb.height);
149 | 
150 | 		color_available = true;
151 | 	}
152 | 	else
153 | 	{
154 | 		printf("OpenNI: Couldn't find colorStream stream:\n%s\n", openni::OpenNI::getExtendedError());
155 | 		color_available = false;
156 | 	}
157 | 
158 | 	if (!depth_available)
159 | 	{
160 | 		openni::OpenNI::shutdown();
161 | 		std::cout << "OpenNI: No valid streams. Exiting." << std::endl;
162 | 		return;
163 | 	}
164 | 	
165 | 	openni_device_data.streams = new openni::VideoStream*[3];
166 | 	if (depth_available) openni_device_data.streams[0] = &openni_device_data.depthStream;
167 | 	if (color_available) openni_device_data.streams[1] = &openni_device_data.colorStream;
168 |      
169 |     rc  = openni_device_data.IRStream.create(openni_device_data.device,openni::SENSOR_IR);
170 |     openni::VideoMode IRMode = findBestMode(openni_device_data.device.getSensorInfo(openni::SENSOR_IR), requested_size_rgb.width, requested_size_rgb.height);
171 |     rc = openni_device_data.IRStream.setVideoMode(IRMode);
172 |     openni_device_data.IRStream.setMirroringEnabled(false);
173 |     
174 |     // rc = openni_device_data.IRStream.start();
175 |     
176 |     if(rc!=openni::STATUS_OK)
177 |     {
178 |         cout<<"can not create IR stream!"<<endl;
179 |     }
180 |    
181 |     
182 |     std::cout<<flush;
183 | }
184 | 
185 | OpenNIEngine::~OpenNIEngine()
186 | {
187 | 
188 | 		if (depth_available)
189 | 		{
190 | 			openni_device_data.depthStream.stop();
191 | 			openni_device_data.depthStream.destroy();
192 | 		}
193 | 		if (color_available)
194 | 		{
195 | 			openni_device_data.colorStream.stop();
196 | 			openni_device_data.colorStream.destroy();
197 | 		}
198 | 		openni_device_data.device.close();
199 | 
200 | 		delete[] openni_device_data.streams;
201 | 
202 | 	openni::OpenNI::shutdown();
203 | }
204 | 
205 | 
206 | void OpenNIEngine::getRGBDImages(cv::Mat& rgb, cv::Mat& raw_depth)
207 | {
208 | 	int changedIndex, waitStreamCount;
209 | 	if (depth_available && color_available) waitStreamCount = 2;
210 | 	else waitStreamCount = 1;
211 | 
212 | 	openni::Status rc = openni::OpenNI::waitForAnyStream(openni_device_data.streams, waitStreamCount, &changedIndex);
213 | 	if (rc != openni::STATUS_OK) { printf("OpenNI: Wait failed\n"); return /*false*/; }
214 | 
215 | 	if(depth_available) openni_device_data.depthStream.readFrame(&openni_device_data.depthFrame);
216 | 	if(color_available) openni_device_data.colorStream.readFrame(&openni_device_data.colorFrame);
217 | 
218 | 	if (depth_available && !openni_device_data.depthFrame.isValid()) return;
219 | 	if (color_available && !openni_device_data.colorFrame.isValid()) return;
220 | 
221 | 	if (color_available)
222 | 	{
223 | 		const openni::RGB888Pixel* colorImagePix = (const openni::RGB888Pixel*)openni_device_data.colorFrame.getData();
224 |         memcpy(rgb.data,colorImagePix,rgb.cols*rgb.rows*3*sizeof(unsigned char));        
225 | 	}
226 | 	else memset(rgb.data, 0, rgb.cols*rgb.rows*3*sizeof(unsigned char));
227 | 
228 | 	if (depth_available)
229 | 	{
230 | 		const openni::DepthPixel* depthImagePix = (const openni::DepthPixel*)openni_device_data.depthFrame.getData();
231 | 		memcpy(raw_depth.data, depthImagePix, raw_depth.cols*raw_depth.rows * sizeof(short));
232 | 	}
233 | 	else memset(raw_depth.data, 0, raw_depth.cols*raw_depth.rows * sizeof(short));
234 | 
235 | 	return /*true*/;
236 | }
237 | 
238 | 
239 | 
240 | void OpenNIEngine::shotGrayAndIRImages(Mat &gray, Mat &ir){
241 |     
242 |     openni_device_data.colorStream.readFrame(&openni_device_data.colorFrame);
243 |     if (openni_device_data.colorFrame.isValid())
244 |     {
245 |         const openni::RGB888Pixel* colorImagePix = (const openni::RGB888Pixel*)openni_device_data.colorFrame.getData();
246 |         memcpy(tmp_RGB_image.data,colorImagePix,tmp_RGB_image.cols*tmp_RGB_image.rows*3*sizeof(unsigned char));     
247 |         cvtColor(tmp_RGB_image,gray,CV_RGB2GRAY);
248 |     }
249 |     else
250 |     {
251 |         cerr<<"rgb frame wrong!"<<endl;
252 |     }
253 |     
254 |     openni_device_data.depthStream.stop();
255 |     openni_device_data.colorStream.stop();
256 |     openni_device_data.IRStream.start();
257 |     
258 |     openni_device_data.IRStream.readFrame(&openni_device_data.IRFrame);
259 |     if (openni_device_data.IRFrame.isValid())
260 |     {
261 |         const openni::Grayscale16Pixel* irImagePix = (const openni::Grayscale16Pixel*)openni_device_data.IRFrame.getData();
262 |         memcpy(tmp_ir_image.data, irImagePix, tmp_ir_image.cols*tmp_ir_image.rows * sizeof(openni::Grayscale16Pixel));
263 |         tmp_ir_image.convertTo(ir,CV_8UC1);
264 |     }
265 |     else
266 |     {
267 |         cerr<<"IR frame wrong!"<<endl;
268 |     }
269 |     
270 |     openni_device_data.IRStream.stop();
271 |     openni_device_data.depthStream.start();
272 |     openni_device_data.colorStream.start();
273 | 
274 | }
275 | 
276 | cv::Size OpenNIEngine::getDepthImageSize(void) {return image_size_depth;}
277 | cv::Size OpenNIEngine::getRGBImageSize(void){return image_size_rgb;}
278 | 
279 | 


--------------------------------------------------------------------------------
/PrimeSenseCalib/OpenNIEngine.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | #include<opencv2/opencv.hpp>
 3 | #include <OpenNI.h>
 4 | 
 5 | class OpenNIEngine
 6 | {
 7 |     struct DeviceData{
 8 |         openni::Device device;
 9 |         
10 |         openni::VideoStream depthStream;
11 |         openni::VideoStream colorStream;
12 |         openni::VideoStream IRStream;
13 |         
14 |         openni::VideoFrameRef depthFrame;
15 |         openni::VideoFrameRef colorFrame;
16 |         openni::VideoFrameRef IRFrame;
17 |         
18 |         openni::VideoStream **streams;
19 |     };
20 |     
21 | private:
22 |     
23 |     DeviceData openni_device_data;
24 |     
25 |     cv::Size image_size_rgb;
26 |     cv::Size image_size_depth;    
27 |     
28 |     bool color_available;
29 |     bool depth_available;
30 |     bool ir_available;    
31 |     
32 |     cv::Mat tmp_ir_image;
33 |     cv::Mat tmp_RGB_image;
34 |     
35 |     void initRGBDStreams( const bool use_internal_calibration, 
36 |                           cv::Size requested_size_rgb, 
37 |                           cv::Size requested_size_d);
38 |     
39 | public:
40 |     OpenNIEngine(
41 |             const char *device_URI = NULL, 
42 |             const bool use_internal_calibration = false,
43 |             cv::Size requested_size_rgb = cv::Size(640,480), 
44 |             cv::Size requested_size_d = cv::Size(640,480));
45 |     
46 |     ~OpenNIEngine();
47 | 
48 |     void shotGrayAndIRImages(cv::Mat& gray, cv::Mat& ir);
49 |     void getRGBDImages(cv::Mat& rgb, cv::Mat& raw_depth);
50 |     
51 |     cv::Size getDepthImageSize(void);
52 |     cv::Size getRGBImageSize(void);
53 | };
54 | 
55 | 


--------------------------------------------------------------------------------
/PrimeSenseCalib/README.md:
--------------------------------------------------------------------------------
 1 | # Calibration Tool for OpenNI based RGB-D sensors
 2 | 
 3 | ## Build:
 4 | 
 5 | ```
 6 | mkdir build
 7 | cd build
 8 | cmake .. -DOPEN_NI_ROOT=/your-path-to-OpenNI2/ 
 9 | make -j$(nproc)
10 | ```
11 | 
12 | ## Dependency
13 | 
14 | - OpenCV3.0 <http://opencv.org/> -- REQUIRED  
15 | - OpenNI2 <http://structure.io/openni>  -- REQUIRED  
16 | 
17 | ## How to Calibrate a OpenNI sensor
18 | 
19 | - Print the pattern in chess.pdf and stick the two page together side by side, making a 7x5 pattern.  
20 |   Don't scale the print, since each rectango should be 38mmx38mm.
21 | - Plug in your OpenNI sensor, run `./CalibCapture` program. When you see the color frame window, use a small piece of glue tape to cover the infrad projector of your sensor, so that you will have less noise on the IR image. Point the sensor to the printed pattern, leave the sensor still, then press `s` on the keyboard to save a capture. Captured frame will be shown on separte windows. 
22 | - **DO NOT MOVE THE SENSOR WHEN YOU CAPTURE THE FRAMES!** (this is because OpenNI2 don't give you access to the color and the IR stream of the sensor at the same time, the program is switching the color stream and IR stream on and off, and this takes time.)
23 | - Take around 40 captures from different viewport. **press `q` to exit**
24 | - Run `./StereoCalib`, the program will autometically calibrate the camera and save calibration results to  
25 |   `intrinsics.yml` and `extrinsics.yml`
26 | 
27 | ## Verify result and generate [InfiniTAM](https://github.com/victorprad/InfiniTAM) readable calib file
28 | 
29 | Run `./TestCalibResult` to visualize a overlay between the color and the depth frame using the calibration result. press `q` to quite the program and a `Calib_ITM.txt` file will be written to the `build/` folder. This file can be directly read from [InfiniTAM](https://github.com/victorprad/InfiniTAM) 
30 |   
31 | 


--------------------------------------------------------------------------------
/PrimeSenseCalib/StereoCalib.cpp:
--------------------------------------------------------------------------------
  1 | /* This is sample from the OpenCV book. The copyright notice is below */
  2 | 
  3 | /* *************** License:**************************
  4 |    Oct. 3, 2008
  5 |    Right to use this code in any way you want without warranty, support or any guarantee of it working.
  6 | 
  7 |    BOOK: It would be nice if you cited it:
  8 |    Learning OpenCV: Computer Vision with the OpenCV Library
  9 |      by Gary Bradski and Adrian Kaehler
 10 |      Published by O'Reilly Media, October 3, 2008
 11 | 
 12 |    AVAILABLE AT:
 13 |      http://www.amazon.com/Learning-OpenCV-Computer-Vision-Library/dp/0596516134
 14 |      Or: http://oreilly.com/catalog/9780596516130/
 15 |      ISBN-10: 0596516134 or: ISBN-13: 978-0596516130
 16 | 
 17 |    OPENCV WEBSITES:
 18 |      Homepage:      http://opencv.org
 19 |      Online docs:   http://docs.opencv.org
 20 |      Q&A forum:     http://answers.opencv.org
 21 |      Issue tracker: http://code.opencv.org
 22 |      GitHub:        https://github.com/Itseez/opencv/
 23 |    ************************************************** */
 24 | 
 25 | #include "opencv2/calib3d/calib3d.hpp"
 26 | #include "opencv2/imgcodecs.hpp"
 27 | #include "opencv2/highgui/highgui.hpp"
 28 | #include "opencv2/imgproc/imgproc.hpp"
 29 | 
 30 | #include <vector>
 31 | #include <string>
 32 | #include <algorithm>
 33 | #include <iostream>
 34 | #include <iterator>
 35 | #include <stdio.h>
 36 | #include <stdlib.h>
 37 | #include <ctype.h>
 38 | #include <fstream>
 39 | 
 40 | using namespace cv;
 41 | using namespace std;
 42 | 
 43 | static int print_help()
 44 | {
 45 |     cout <<
 46 |             " Given a list of chessboard images, the number of corners (nx, ny)\n"
 47 |             " on the chessboards, and a flag: useCalibrated for \n"
 48 |             "   calibrated (0) or\n"
 49 |             "   uncalibrated \n"
 50 |             "     (1: use cvStereoCalibrate(), 2: compute fundamental\n"
 51 |             "         matrix separately) stereo. \n"
 52 |             " Calibrate the cameras and display the\n"
 53 |             " rectified results along with the computed disparity images.   \n" << endl;
 54 |     cout << "Usage:\n ./stereo_calib -w board_width -h board_height [-nr /*dot not view results*/] <image list XML/YML file>\n" << endl;
 55 |     return 0;
 56 | }
 57 | 
 58 | 
 59 | static void
 60 | StereoCalib(const vector<string>& imagelist, Size boardSize, bool useCalibrated=true, bool showRectified=true)
 61 | {
 62 |     if( imagelist.size() % 2 != 0 )
 63 |     {
 64 |         cout << "Error: the image list contains odd (non-even) number of elements\n";
 65 |         return;
 66 |     }
 67 | 
 68 |     bool displayCorners = true;//true;
 69 |     const int maxScale = 2;
 70 |     const float squareSize = 0.038f;  // Set this to your actual square size
 71 |     // ARRAY AND VECTOR STORAGE:
 72 | 
 73 |     vector<vector<Point2f> > imagePoints[2];
 74 |     vector<vector<Point3f> > objectPoints;
 75 |     Size imageSize;
 76 | 
 77 |     int i, j, k, nimages = (int)imagelist.size()/2;
 78 | 
 79 |     imagePoints[0].resize(nimages);
 80 |     imagePoints[1].resize(nimages);
 81 |     vector<string> goodImageList;
 82 | 
 83 |     for( i = j = 0; i < nimages; i++ )
 84 |     {
 85 |         for( k = 0; k < 2; k++ )
 86 |         {
 87 |             const string& filename = imagelist[i*2+k];
 88 |             Mat img = imread(filename, 0);
 89 |             if(img.empty())
 90 |                 break;
 91 |             if( imageSize == Size() )
 92 |                 imageSize = img.size();
 93 |             else if( img.size() != imageSize )
 94 |             {
 95 |                 cout << "The image " << filename << " has the size different from the first image size. Skipping the pair\n";
 96 |                 break;
 97 |             }
 98 |             bool found = false;
 99 |             vector<Point2f>& corners = imagePoints[k][j];
100 |             for( int scale = 1; scale <= maxScale; scale++ )
101 |             {
102 |                 Mat timg;
103 |                 if( scale == 1 )
104 |                     timg = img;
105 |                 else
106 |                     resize(img, timg, Size(), scale, scale);
107 |                 found = findChessboardCorners(timg, boardSize, corners,
108 |                     CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE);
109 |                 if( found )
110 |                 {
111 |                     if( scale > 1 )
112 |                     {
113 |                         Mat cornersMat(corners);
114 |                         cornersMat *= 1./scale;
115 |                     }
116 |                     break;
117 |                 }
118 |             }
119 |             if( displayCorners )
120 |             {
121 |                 cout << filename << endl;
122 |                 Mat cimg, cimg1;
123 |                 cvtColor(img, cimg, COLOR_GRAY2BGR);
124 |                 drawChessboardCorners(cimg, boardSize, corners, found);
125 |                 double sf = 640./MAX(img.rows, img.cols);
126 |                 resize(cimg, cimg1, Size(), sf, sf);
127 |                 imshow("corners", cimg1);
128 |                 char c = (char)waitKey(500);
129 |                 if( c == 27 || c == 'q' || c == 'Q' ) //Allow ESC to quit
130 |                     exit(-1);
131 |             }
132 |             else
133 |                 putchar('.');
134 |             if( !found )
135 |                 break;
136 |             cornerSubPix(img, corners, Size(11,11), Size(-1,-1),
137 |                          TermCriteria(TermCriteria::COUNT+TermCriteria::EPS,
138 |                                       30, 0.01));
139 |         }
140 |         if( k == 2 )
141 |         {
142 |             goodImageList.push_back(imagelist[i*2]);
143 |             goodImageList.push_back(imagelist[i*2+1]);
144 |             j++;
145 |         }
146 |     }
147 |     cout << j << " pairs have been successfully detected.\n";
148 |     nimages = j;
149 |     if( nimages < 2 )
150 |     {
151 |         cout << "Error: too little pairs to run the calibration\n";
152 |         return;
153 |     }
154 | 
155 |     imagePoints[0].resize(nimages);
156 |     imagePoints[1].resize(nimages);
157 |     objectPoints.resize(nimages);
158 | 
159 |     cout<<"board size:"<<boardSize<<endl;
160 |     cout << "square size:"<<squareSize<<endl;
161 |     
162 |     for( i = 0; i < nimages; i++ )
163 |     {
164 |         for( j = 0; j < boardSize.height; j++ )
165 |             for( k = 0; k < boardSize.width; k++ )
166 |                 objectPoints[i].push_back(Point3f(k*squareSize, j*squareSize, 0));
167 |     }
168 | 
169 |     cout << "Running stereo calibration ..."<<endl;
170 | 
171 |     Mat cameraMatrix[2], distCoeffs[2];
172 |     cameraMatrix[0] = Mat::eye(3, 3, CV_64F);
173 |     cameraMatrix[1] = Mat::eye(3, 3, CV_64F);
174 |     Mat R, T, E, F;
175 | 
176 |     double rms = stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1],
177 |                     cameraMatrix[0], distCoeffs[0],
178 |                     cameraMatrix[1], distCoeffs[1],
179 |                     imageSize, R, T, E, F,
180 |                     CALIB_ZERO_TANGENT_DIST +
181 |                     CALIB_FIX_K3 + CALIB_FIX_K4 + CALIB_FIX_K5,
182 |                     TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-5) );
183 |     cout << "done with RMS error=" << rms << endl;
184 | 
185 | // CALIBRATION QUALITY CHECK
186 | // because the output fundamental matrix implicitly
187 | // includes all the output information,
188 | // we can check the quality of calibration using the
189 | // epipolar geometry constraint: m2^t*F*m1=0
190 |     double err = 0;
191 |     int npoints = 0;
192 |     vector<Vec3f> lines[2];
193 |     for( i = 0; i < nimages; i++ )
194 |     {
195 |         int npt = (int)imagePoints[0][i].size();
196 |         Mat imgpt[2];
197 |         for( k = 0; k < 2; k++ )
198 |         {
199 |             imgpt[k] = Mat(imagePoints[k][i]);
200 |             undistortPoints(imgpt[k], imgpt[k], cameraMatrix[k], distCoeffs[k], Mat(), cameraMatrix[k]);
201 |             computeCorrespondEpilines(imgpt[k], k+1, F, lines[k]);
202 |         }
203 |         for( j = 0; j < npt; j++ )
204 |         {
205 |             double errij = fabs(imagePoints[0][i][j].x*lines[1][j][0] +
206 |                                 imagePoints[0][i][j].y*lines[1][j][1] + lines[1][j][2]) +
207 |                            fabs(imagePoints[1][i][j].x*lines[0][j][0] +
208 |                                 imagePoints[1][i][j].y*lines[0][j][1] + lines[0][j][2]);
209 |             err += errij;
210 |         }
211 |         npoints += npt;
212 |     }
213 |     cout << "average reprojection err = " <<  err/npoints << endl;
214 | 
215 |     // save intrinsic parameters
216 |     FileStorage fs("intrinsics.yml", FileStorage::WRITE);
217 |     if( fs.isOpened() )
218 |     {
219 |         fs << "M1" << cameraMatrix[0] << "D1" << distCoeffs[0] <<
220 |             "M2" << cameraMatrix[1] << "D2" << distCoeffs[1];
221 |         fs.release();
222 |     }
223 |     else
224 |         cout << "Error: can not save the intrinsic parameters\n";
225 | 
226 |     Mat R1, R2, P1, P2, Q;
227 |     Rect validRoi[2];
228 | 
229 |     stereoRectify(cameraMatrix[0], distCoeffs[0],
230 |                   cameraMatrix[1], distCoeffs[1],
231 |                   imageSize, R, T, R1, R2, P1, P2, Q,
232 |                   CALIB_ZERO_DISPARITY, 1, imageSize, &validRoi[0], &validRoi[1]);
233 | 
234 |     fs.open("extrinsics.yml", FileStorage::WRITE);
235 |     if( fs.isOpened() )
236 |     {
237 |         fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1" << P1 << "P2" << P2 << "Q" << Q;
238 |         fs.release();
239 |     }
240 |     else
241 |         cout << "Error: can not save the extrinsic parameters\n";
242 | 
243 |     // OpenCV can handle left-right
244 |     // or up-down camera arrangements
245 |     bool isVerticalStereo = fabs(P2.at<double>(1, 3)) > fabs(P2.at<double>(0, 3));
246 | 
247 | // COMPUTE AND DISPLAY RECTIFICATION
248 |     if( !showRectified )
249 |         return;
250 | 
251 |     Mat rmap[2][2];
252 | // IF BY CALIBRATED (BOUGUET'S METHOD)
253 |     if( useCalibrated )
254 |     {
255 |         // we already computed everything
256 |     }
257 | // OR ELSE HARTLEY'S METHOD
258 |     else
259 |  // use intrinsic parameters of each camera, but
260 |  // compute the rectification transformation directly
261 |  // from the fundamental matrix
262 |     {
263 |         vector<Point2f> allimgpt[2];
264 |         for( k = 0; k < 2; k++ )
265 |         {
266 |             for( i = 0; i < nimages; i++ )
267 |                 std::copy(imagePoints[k][i].begin(), imagePoints[k][i].end(), back_inserter(allimgpt[k]));
268 |         }
269 |         F = findFundamentalMat(Mat(allimgpt[0]), Mat(allimgpt[1]), FM_8POINT, 0, 0);
270 |         Mat H1, H2;
271 |         stereoRectifyUncalibrated(Mat(allimgpt[0]), Mat(allimgpt[1]), F, imageSize, H1, H2, 3);
272 | 
273 |         R1 = cameraMatrix[0].inv()*H1*cameraMatrix[0];
274 |         R2 = cameraMatrix[1].inv()*H2*cameraMatrix[1];
275 |         P1 = cameraMatrix[0];
276 |         P2 = cameraMatrix[1];
277 |     }
278 | 
279 |     //Precompute maps for cv::remap()
280 |     initUndistortRectifyMap(cameraMatrix[0], distCoeffs[0], R1, P1, imageSize, CV_16SC2, rmap[0][0], rmap[0][1]);
281 |     initUndistortRectifyMap(cameraMatrix[1], distCoeffs[1], R2, P2, imageSize, CV_16SC2, rmap[1][0], rmap[1][1]);
282 | 
283 |     Mat canvas;
284 |     double sf;
285 |     int w, h;
286 |     if( !isVerticalStereo )
287 |     {
288 |         sf = 600./MAX(imageSize.width, imageSize.height);
289 |         w = cvRound(imageSize.width*sf);
290 |         h = cvRound(imageSize.height*sf);
291 |         canvas.create(h, w*2, CV_8UC3);
292 |     }
293 |     else
294 |     {
295 |         sf = 300./MAX(imageSize.width, imageSize.height);
296 |         w = cvRound(imageSize.width*sf);
297 |         h = cvRound(imageSize.height*sf);
298 |         canvas.create(h*2, w, CV_8UC3);
299 |     }
300 | 
301 |     for( i = 0; i < nimages; i++ )
302 |     {
303 |         for( k = 0; k < 2; k++ )
304 |         {
305 |             Mat img = imread(goodImageList[i*2+k], 0), rimg, cimg;
306 |             remap(img, rimg, rmap[k][0], rmap[k][1], INTER_LINEAR);
307 |             cvtColor(rimg, cimg, COLOR_GRAY2BGR);
308 |             Mat canvasPart = !isVerticalStereo ? canvas(Rect(w*k, 0, w, h)) : canvas(Rect(0, h*k, w, h));
309 |             resize(cimg, canvasPart, canvasPart.size(), 0, 0, INTER_AREA);
310 |             if( useCalibrated )
311 |             {
312 |                 Rect vroi(cvRound(validRoi[k].x*sf), cvRound(validRoi[k].y*sf),
313 |                           cvRound(validRoi[k].width*sf), cvRound(validRoi[k].height*sf));
314 |                 rectangle(canvasPart, vroi, Scalar(0,0,255), 3, 8);
315 |             }
316 |         }
317 | 
318 |         if( !isVerticalStereo )
319 |             for( j = 0; j < canvas.rows; j += 16 )
320 |                 line(canvas, Point(0, j), Point(canvas.cols, j), Scalar(0, 255, 0), 1, 8);
321 |         else
322 |             for( j = 0; j < canvas.cols; j += 16 )
323 |                 line(canvas, Point(j, 0), Point(j, canvas.rows), Scalar(0, 255, 0), 1, 8);
324 |         imshow("rectified", canvas);
325 |         char c = (char)waitKey();
326 |         if( c == 27 || c == 'q' || c == 'Q' )
327 |             break;
328 |     }
329 | }
330 | 
331 | 
332 | static bool readStringList( const string& filename, vector<string>& l )
333 | {
334 |     ifstream ifs(filename.c_str());
335 |     string tmp_str;
336 |     while(ifs>>tmp_str)  l.push_back(tmp_str);
337 |     return true;
338 | }
339 | 
340 | int main(int argc, char** argv)
341 | {
342 |     Size boardSize;
343 |     string imagelistfn;
344 |     bool showRectified = true;
345 | 
346 |     for( int i = 1; i < argc; i++ )
347 |     {
348 |         if( string(argv[i]) == "-w" )
349 |         {
350 |             if( sscanf(argv[++i], "%d", &boardSize.width) != 1 || boardSize.width <= 0 )
351 |             {
352 |                 cout << "invalid board width" << endl;
353 |                 return print_help();
354 |             }
355 |         }
356 |         else if( string(argv[i]) == "-h" )
357 |         {
358 |             if( sscanf(argv[++i], "%d", &boardSize.height) != 1 || boardSize.height <= 0 )
359 |             {
360 |                 cout << "invalid board height" << endl;
361 |                 return print_help();
362 |             }
363 |         }
364 |         else if( string(argv[i]) == "-nr" )
365 |             showRectified = false;
366 |         else if( string(argv[i]) == "--help" )
367 |             return print_help();
368 |         else if( argv[i][0] == '-' )
369 |         {
370 |             cout << "invalid option " << argv[i] << endl;
371 |             return 0;
372 |         }
373 |         else
374 |             imagelistfn = argv[i];
375 |     }
376 | 
377 |     if( imagelistfn == "" )
378 |     {
379 |         imagelistfn = "image_lists.txt";
380 |         boardSize = Size(7, 5);
381 |     }
382 |     else if( boardSize.width <= 0 || boardSize.height <= 0 )
383 |     {
384 |         cout << "if you specified XML file with chessboards, you should also specify the board width and height (-w and -h options)" << endl;
385 |         return 0;
386 |     }
387 | 
388 |     vector<string> imagelist;
389 |     bool ok = readStringList(imagelistfn, imagelist);
390 |     if(!ok || imagelist.empty())
391 |     {
392 |         cout << "can not open " << imagelistfn << " or the string list is empty" << endl;
393 |         return print_help();
394 |     }
395 | 
396 |     StereoCalib(imagelist, boardSize, true, showRectified);
397 |     return 0;
398 | }
399 | 


--------------------------------------------------------------------------------
/PrimeSenseCalib/chess.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/carlren/VisionTools/d14d4049989b1c1f99ab3d1b8c85a20ae06c16e1/PrimeSenseCalib/chess.pdf


--------------------------------------------------------------------------------
/PrimeSenseCalib/cmake/FindOpenNI.cmake:
--------------------------------------------------------------------------------
 1 | # - Find OpenNI2
 2 | # This module defines
 3 | #  OpenNI_INCLUDE_DIR, where to find OpenNI include files
 4 | #  OpenNI_LIBRARIES, the libraries needed to use OpenNI
 5 | #  OpenNI_FOUND, If false, do not try to use OpenNI.
 6 | # also defined, but not for general use are
 7 | #  OpenNI_LIBRARY, where to find the OpenNI library.
 8 | 
 9 | message(STATUS ${OPEN_NI_ROOT})
10 | 
11 | find_path(OpenNI_INCLUDE_DIR OpenNI.h PATH "${OPEN_NI_ROOT}/Include")
12 | find_library(OpenNI_LIBRARY OpenNI2 PATH "${OPEN_NI_ROOT}/Bin/x64-Release/")
13 | 
14 | # handle the QUIETLY and REQUIRED arguments and set JPEG_FOUND to TRUE if
15 | # all listed variables are TRUE
16 | #include(${CMAKE_CURRENT_LIST_DIR}/FindPackageHandleStandardArgs.cmake)
17 | #include(${CMAKE_MODULE_PATH}/FindPackageHandleStandardArgs.cmake)
18 | find_package_handle_standard_args(OpenNI DEFAULT_MSG OpenNI_LIBRARY OpenNI_INCLUDE_DIR)
19 | 
20 | if(OPENNI_FOUND)
21 |   set(OpenNI_LIBRARIES ${OpenNI_LIBRARY})
22 | endif()
23 | 
24 | mark_as_advanced(OpenNI_LIBRARY OpenNI_INCLUDE_DIR)
25 | 
26 | 


--------------------------------------------------------------------------------
/PrimeSenseCalib/cmake/FindOpenNI.cmake~:
--------------------------------------------------------------------------------
 1 | # - Find OpenNI2
 2 | # This module defines
 3 | #  OpenNI_INCLUDE_DIR, where to find OpenNI include files
 4 | #  OpenNI_LIBRARIES, the libraries needed to use OpenNI
 5 | #  OpenNI_FOUND, If false, do not try to use OpenNI.
 6 | # also defined, but not for general use are
 7 | #  OpenNI_LIBRARY, where to find the OpenNI library.
 8 | 
 9 | message(STATUS ${OPEN_NI_ROOT})
10 | 
11 | find_path(OpenNI_INCLUDE_DIR OpenNI.h PATH "${OPEN_NI_ROOT}/Include")
12 | 
13 | find_library(OpenNI_LIBRARY OpenNI2 PATH "${OPEN_NI_ROOT}/Bin/x64-Release/")
14 | 
15 | # handle the QUIETLY and REQUIRED arguments and set JPEG_FOUND to TRUE if
16 | # all listed variables are TRUE
17 | #include(${CMAKE_CURRENT_LIST_DIR}/FindPackageHandleStandardArgs.cmake)
18 | #include(${CMAKE_MODULE_PATH}/FindPackageHandleStandardArgs.cmake)
19 | find_package_handle_standard_args(OpenNI DEFAULT_MSG OpenNI_LIBRARY OpenNI_INCLUDE_DIR)
20 | 
21 | if(OPENNI_FOUND)
22 |   set(OpenNI_LIBRARIES ${OpenNI_LIBRARY})
23 | endif()
24 | 
25 | mark_as_advanced(OpenNI_LIBRARY OpenNI_INCLUDE_DIR)
26 | 
27 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # VisionTools
2 | A set of computer vision tools
3 | 
4 | Feel free to add stuff :)
5 | 


--------------------------------------------------------------------------------
/RelativePoseSolver/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8)
 2 | project(absolute_orientation)
 3 | 
 4 | #FIND_PACKAGE(LAPACK)
 5 | 
 6 | IF(MSVC_IDE)
 7 |   add_definitions(-D_CRT_SECURE_NO_WARNINGS)
 8 |   add_definitions(-DUSING_CMAKE=1)
 9 | ELSE(MSVC_IDE)
10 |   set(CFLAGS_WARN "-Wall -Wextra -Wno-unused-parameter -Wno-strict-aliasing")
11 |   set(CMAKE_CXX_FLAGS "-fPIC -O3 -march=native ${CFLAGS_WARN} ${CMAKE_CXX_FLAGS}")
12 | ENDIF(MSVC_IDE)
13 | 
14 | INCLUDE_DIRECTORIES("/home/carl/Work/3rdparty")
15 | INCLUDE_DIRECTORIES("/home/carl/Work/3rdparty/Eigen3")
16 | #INCLUDE_DIRECTORIES("/Users/carlren/Work/VisLib/VisLib/Frameworks/gtsam/gtsam/3rdparty/Eigen")
17 | #INCLUDE_DIRECTORIES("/Users/carlren/Work/3rdparty")
18 | 
19 | add_executable(test_absolute_orientation test_absolute_orientation.cpp absolute_orientation_horn.h)
20 | target_link_libraries(test_absolute_orientation)
21 | 
22 | #add_executable(test_relative_pose test_relative_pose.cpp relative_pose_solver.h CalibStereo.h CalibStereo.cpp)
23 | add_executable(test_relative_pose test_relative_pose.cpp relative_pose_solver.h )
24 | target_link_libraries(test_relative_pose ${LAPACK_LIBRARIES})
25 | 
26 | add_executable(SRP solve_relative_pose.cpp relative_pose_solver.h )
27 | target_link_libraries(SRP)
28 | 


--------------------------------------------------------------------------------
/RelativePoseSolver/CalibStereo.cpp:
--------------------------------------------------------------------------------
  1 | #include "CalibStereo.h"
  2 | #include <TooN/SVD.h>
  3 | #include <TooN/SymEigen.h>
  4 | #include <assert.h>
  5 | 
  6 | //#define DEBUG_STEREO
  7 | 
  8 | static TooN::Matrix<3,3> orthonormalize(const TooN::Matrix<3,3> & A)
  9 | {
 10 | 	TooN::SVD<3> svdA(A);
 11 | 	return svdA.get_U()*svdA.get_VT();
 12 | }
 13 | 
 14 | TooN::SE3<double> CalibStereo::getRelativePose_commonFrame(const std::vector<TooN::SE3<double> > & pose1, const std::vector<TooN::SE3<double> > & pose2)
 15 | {
 16 | 	assert(pose1.size()==pose2.size());
 17 | 
 18 | 	TooN::Matrix<> A(pose1.size()*12, 12);
 19 | 	TooN::Vector<> b(pose1.size()*12);
 20 | 
 21 | 	for (int r = 0; r < A.num_rows(); r++) 
 22 | 		for (int c = 0; c < A.num_cols(); c++) A(r,c) = 0.0;
 23 | 
 24 | 	std::vector<TooN::SE3<double> >::const_iterator it1 = pose1.begin();
 25 | 	std::vector<TooN::SE3<double> >::const_iterator it2 = pose2.begin();
 26 | 	int counter = 0;
 27 | 	for (; it1 != pose1.end(); ++it1, ++it2, ++counter) {
 28 | 		A.slice(counter*12, 0, 3,3) = it1->get_rotation().get_matrix().T();
 29 | 		A.slice(counter*12+3, 3, 3,3) = it1->get_rotation().get_matrix().T();
 30 | 		A.slice(counter*12+6, 6, 3,3) = it1->get_rotation().get_matrix().T();
 31 | 		A.slice(counter*12+9, 0, 1,3) = it1->get_translation().as_row();
 32 | 		A(counter*12+9, 9) = 1.0;
 33 | 		A.slice(counter*12+10, 3, 1,3) = it1->get_translation().as_row();
 34 | 		A(counter*12+10, 10) = 1.0;
 35 | 		A.slice(counter*12+11, 6, 1,3) = it1->get_translation().as_row();
 36 | 		A(counter*12+11, 11) = 1.0;
 37 | 		b.slice(counter*12, 3) = it2->get_rotation().get_matrix()[0];
 38 | 		b.slice(counter*12+3, 3) = it2->get_rotation().get_matrix()[1];
 39 | 		b.slice(counter*12+6, 3) = it2->get_rotation().get_matrix()[2];
 40 | 		b.slice(counter*12+9, 3) = it2->get_translation();
 41 | 	} 
 42 | 
 43 | 	#ifdef DEBUG_STEREO
 44 | 	std::cerr << "getRelativePose_commonFrame Matrix A:" << std::endl << A;
 45 | 	std::cerr << "Vector b:" << std::endl << b << std::endl;
 46 | 	#endif
 47 | 	TooN::SVD<> svdA(A);
 48 | 	TooN::Vector<12> v = svdA.backsub(b);
 49 | 
 50 | 	TooN::Matrix<3,3> Rrel;
 51 | 	TooN::Vector<3> trel;
 52 | 	Rrel.slice<0,0,1,3>() = v.slice<0,3>().as_row();
 53 | 	Rrel.slice<1,0,1,3>() = v.slice<3,3>().as_row();
 54 | 	Rrel.slice<2,0,1,3>() = v.slice<6,3>().as_row();
 55 | 	trel = v.slice<9,3>();
 56 | 
 57 | 	return TooN::SE3<double>(orthonormalize(Rrel), trel);
 58 | }
 59 | 
 60 | TooN::SE3<double> CalibStereo::getRelativePose_commonFrame(const std::vector<TooN::SE3<double> > & pose1, const std::vector<TooN::SE3<double> > & pose2, const std::map<int,int> & synclist)
 61 | {
 62 | 	std::vector<TooN::SE3<double> > poses1;
 63 | 	std::vector<TooN::SE3<double> > poses2;
 64 | 	for (std::map<int,int>::const_iterator it = synclist.begin();
 65 | 	     it != synclist.end(); ++it) {
 66 | 		poses1.push_back(pose1[it->first]);
 67 | 		poses2.push_back(pose2[it->second]);
 68 | 	}
 69 | 	if (poses1.size()>0) return CalibStereo::getRelativePose_commonFrame(poses1, poses2);
 70 | 	return TooN::SE3<double>();
 71 | }
 72 | 
 73 | TooN::SO3<double> CalibStereo::getRelativePose_differentFrames(
 74 | 		const std::vector<TooN::SO3<double> > & pose1,
 75 | 		const std::vector<TooN::SO3<double> > & pose2)
 76 | {
 77 | 	assert(pose1.size()==pose2.size());
 78 | 	assert(pose1.size()>0);
 79 | 	TooN::Matrix<> A(9*(pose1.size()*(pose1.size()-1) / 2), 9);
 80 | 	for (int r=0; r<A.num_rows(); r++) for (int c=0; c<A.num_cols(); c++) A(r,c) = 0.0;
 81 | 	int counter = 0;
 82 | 	for (unsigned int i=0; i<pose1.size(); i++) for (unsigned int j=i+1; j<pose1.size(); j++) {
 83 | 		const TooN::Matrix< 3, 3 > & A1 = pose2[i].inverse().get_matrix();
 84 | 		const TooN::Matrix< 3, 3 > & C1 = pose1[i].get_matrix();
 85 | 		const TooN::Matrix< 3, 3 > & A2 = pose2[j].inverse().get_matrix();
 86 | 		const TooN::Matrix< 3, 3 > & C2 = pose1[j].get_matrix();
 87 | 
 88 | 		for (int ro=0; ro<3; ro++) for (int co=0; co<3; co++) {
 89 | 			for (int r=0; r<3; r++) for (int c=0; c<3; c++) 
 90 | 				A(counter,c+3*r) = A1(ro,r) * C1(c,co) - A2(ro,r) * C2(c,co);
 91 | 			counter++;
 92 | 		}
 93 | 	}
 94 | 
 95 | 	TooN::SVD<> svdA(A);
 96 | 	TooN::Vector<9> diag = svdA.get_diagonal();
 97 | 	int min = 0;
 98 | 	for (int i=1; i<9; i++) if (diag[i]<diag[min]) min=i;
 99 | 	TooN::Vector<9> v = svdA.get_VT()[min];
100 | 
101 | 	TooN::Matrix<3,3> Rrel;
102 | 	Rrel.slice<0,0,1,3>() = v.slice<0,3>().as_row();
103 | 	Rrel.slice<1,0,1,3>() = v.slice<3,3>().as_row();
104 | 	Rrel.slice<2,0,1,3>() = v.slice<6,3>().as_row();
105 | 
106 | 	Rrel = orthonormalize(Rrel);
107 | 	if ((Rrel[0] ^ Rrel[1])*Rrel[2] < 0.0) {
108 | 		Rrel = -Rrel;
109 | 	}
110 | 	return TooN::SO3<double>(Rrel);
111 | }
112 | 
113 | TooN::SO3<double> CalibStereo::getRelativePose_differentFrames(const std::vector<TooN::SO3<double> > & pose1, const std::vector<TooN::SO3<double> > & pose2, const std::map<int,int> & synclist)
114 | {
115 | 	std::vector<TooN::SO3<double> > poses1;
116 | 	std::vector<TooN::SO3<double> > poses2;
117 | 	for (std::map<int,int>::const_iterator it = synclist.begin();
118 | 	     it != synclist.end(); ++it) {
119 | 		poses1.push_back(pose1[it->first]);
120 | 		poses2.push_back(pose2[it->second]);
121 | 	}
122 | 	if (poses1.size()>0) return CalibStereo::getRelativePose_differentFrames(poses1, poses2);
123 | 	return TooN::SO3<double>();
124 | }
125 | 
126 | static TooN::Matrix<4,4> SE3_get_matrix(const TooN::SE3<double> & se3)
127 | {
128 | 	TooN::Matrix<4,4> ret;
129 | 	ret.slice<0,0,3,3>() = se3.get_rotation().get_matrix();
130 | 	ret.slice<0,3,3,1>() = se3.get_translation().as_col();
131 | 	ret.slice<3,0,1,4>() = TooN::makeVector(0,0,0,1).as_row();
132 | 	return ret;
133 | }
134 | 
135 | TooN::SE3<double> CalibStereo::getRelativePose_differentFrames(
136 | 		const std::vector<TooN::SE3<double> > & pose1,
137 | 		const std::vector<TooN::SE3<double> > & pose2)
138 | {
139 | 	assert(pose1.size()==pose2.size());
140 | 	assert(pose1.size()>0);
141 | 	TooN::Matrix<> A(16*(pose1.size()*(pose1.size()-1) / 2), 16);
142 | 	for (int r=0; r<A.num_rows(); r++) for (int c=0; c<A.num_cols(); c++) A(r,c) = 0.0;
143 | 	int counter = 0;
144 | 	for (unsigned int i=0; i<pose1.size(); i++) for (unsigned int j=i+1; j<pose1.size(); j++) {
145 | 		TooN::Matrix< 4, 4 > A1 = SE3_get_matrix(pose2[i].inverse());
146 | 		TooN::Matrix< 4, 4 > C1 = SE3_get_matrix(pose1[i]);
147 | 		TooN::Matrix< 4, 4 > A2 = SE3_get_matrix(pose2[j].inverse());
148 | 		TooN::Matrix< 4, 4 > C2 = SE3_get_matrix(pose1[j]);
149 | 
150 | 		for (int ro=0; ro<4; ro++) for (int co=0; co<4; co++) {
151 | 			for (int r=0; r<4; r++) for (int c=0; c<4; c++) 
152 | 				A(counter,c+4*r) = A1(ro,r) * C1(c,co) - A2(ro,r) * C2(c,co);
153 | 			counter++;
154 | 		}
155 | 	}
156 | 
157 | 	// find column of V with smalest singular value
158 | 	TooN::SVD<> svdA(A);
159 | 	TooN::Vector<16> diag = svdA.get_diagonal();
160 | 	int min = 0;
161 | 	for (int i=1; i<16; i++) if (diag[i]<diag[min]) min=i;
162 | 	TooN::Vector<16> v = svdA.get_VT()[min];
163 | 
164 | 	// extract R ant t from vector v
165 | 	TooN::Matrix<3,3> Rrel;
166 | 	Rrel.slice<0,0,1,3>() = v.slice<0,3>().as_row();
167 | 	Rrel.slice<1,0,1,3>() = v.slice<4,3>().as_row();
168 | 	Rrel.slice<2,0,1,3>() = v.slice<8,3>().as_row();
169 | 	TooN::Vector<3> trel;
170 | 	trel[0] = v[3];
171 | 	trel[1] = v[7];
172 | 	trel[2] = v[11];
173 | 
174 | 	// fix the scaling
175 | 	double scale = (sqrt(Rrel[0]*Rrel[0]) + sqrt(Rrel[1]*Rrel[1]) + sqrt(Rrel[2]*Rrel[2]) + fabs(v[15]))/4;
176 | 	Rrel *= 1.0/scale;
177 | 	trel *= 1.0/scale;
178 | 
179 | 	// orthonormalize and take care of negative scaling
180 | 	Rrel = orthonormalize(Rrel);
181 | 	if ((Rrel[0] ^ Rrel[1])*Rrel[2] < 0.0) {
182 | 		Rrel = -Rrel;
183 | 		trel = -trel;
184 | 	}
185 | 
186 | 	// create and return a SE3
187 | 	TooN::SO3<double> Rrel_so3(Rrel);
188 | 	return TooN::SE3<double>(Rrel_so3, trel);
189 | }
190 | 
191 | TooN::SE3<double> CalibStereo::getRelativePose_differentFrames(const std::vector<TooN::SE3<double> > & pose1, const std::vector<TooN::SE3<double> > & pose2, const std::map<int,int> & synclist)
192 | {
193 | 	std::vector<TooN::SE3<double> > poses1;
194 | 	std::vector<TooN::SE3<double> > poses2;
195 | 	for (std::map<int,int>::const_iterator it = synclist.begin();
196 | 	     it != synclist.end(); ++it) {
197 | 		poses1.push_back(pose1[it->first]);
198 | 		poses2.push_back(pose2[it->second]);
199 | 	}
200 | 	if (poses1.size()>0) return CalibStereo::getRelativePose_differentFrames(poses1, poses2);
201 | 	return TooN::SE3<double>();
202 | }
203 | 
204 | TooN::SE3<double> CalibStereo::getRelativePose(const std::vector<TooN::Vector<3> > & pts1, const std::vector<TooN::Vector<3> > & pts2)
205 | {
206 | 	assert(pts1.size()==pts2.size());
207 | 	assert(pts1.size()>0);
208 | 
209 | 	// align center of mass
210 | 	TooN::Vector<3> center1 = TooN::Zeros, center2 = TooN::Zeros;
211 | 	for (unsigned int pt=0; pt<pts1.size(); pt++) {
212 | 		center1 += pts1[pt];
213 | 		center2 += pts2[pt];
214 | 	}
215 | 	center1 /= pts1.size();
216 | 	center2 /= pts1.size();
217 | 
218 | 	// apply the translation
219 | 	// find the rotation
220 | 	TooN::Matrix<3,3> M = TooN::Zeros;
221 | 	for (unsigned int pt=0; pt<pts1.size(); pt++) {
222 | 		M += (pts1[pt] - center1).as_col()*(pts2[pt] - center2).as_row();
223 | 	}
224 | 	TooN::Matrix<4,4> N;
225 | 	N[0][0] = M[0][0] + M[1][1] + M[2][2];
226 | 	N[0][1] = M[1][2] - M[2][1];
227 | 	N[0][2] = M[2][0] - M[0][2];
228 | 	N[0][3] = M[0][1] - M[1][0];
229 | 
230 | 	N[1][0] = N[0][1];
231 | 	N[1][1] = M[0][0] - M[1][1] - M[2][2];
232 | 	N[1][2] = M[0][1] + M[1][0];
233 | 	N[1][3] = M[0][2] + M[2][0];
234 | 
235 | 	N[2][0] = N[0][2];
236 | 	N[2][1] = N[1][2];
237 | 	N[2][2] =-M[0][0] + M[1][1] - M[2][2];
238 | 	N[2][3] = M[1][2] + M[2][1];
239 | 
240 | 	N[3][0] = N[0][3];
241 | 	N[3][1] = N[1][3];
242 | 	N[3][2] = N[2][3];
243 | 	N[3][3] =-M[0][0] - M[1][1] + M[2][2];
244 | 
245 | 	TooN::SymEigen<4> eigN(N);
246 | 	int max_eig = 0;
247 | 	for (int i = 1; i < 4; ++i) if (eigN.get_evalues()[i]>eigN.get_evalues()[max_eig]) max_eig = i;
248 | 	TooN::Vector<4> q = eigN.get_evectors()[max_eig];
249 | 
250 | 	TooN::SE3<double> ret;
251 | 	double fact = 2.0 * acos(q[0]) / sqrt(1.0-q[0]*q[0]);
252 | 	ret.get_rotation() = TooN::SO3<double>::exp(TooN::makeVector(q[1]*fact, q[2]*fact, q[3]*fact));
253 | 	ret.get_translation() = center2 - ret.get_rotation()*center1;
254 | 	return ret;
255 | }
256 | 
257 | TooN::SE3<double> CalibStereo::getRelativePose(const std::vector<TooN::Vector<3> > & points1, const std::vector<TooN::Vector<3> > & points2, const std::map<int,int> & synclist)
258 | {
259 | 	std::vector<TooN::Vector<3> > pts1;
260 | 	std::vector<TooN::Vector<3> > pts2;
261 | 	for (std::map<int,int>::const_iterator it = synclist.begin();
262 | 	     it != synclist.end(); ++it) {
263 | 		pts1.push_back(points1[it->first]);
264 | 		pts2.push_back(points2[it->second]);
265 | 	}
266 | 	if (pts1.size()>0) return CalibStereo::getRelativePose(pts1, pts2);
267 | 	return TooN::SE3<double>();
268 | }
269 | 
270 | 


--------------------------------------------------------------------------------
/RelativePoseSolver/CalibStereo.h:
--------------------------------------------------------------------------------
  1 | #ifndef _INCLUDED_calib_compute_CalibStereo_h_
  2 | #define _INCLUDED_calib_compute_CalibStereo_h_
  3 | 
  4 | #include <TooN/TooN.h>
  5 | #include <TooN/se3.h>
  6 | 
  7 | #include <vector>
  8 | #include <map>
  9 | 
 10 | /** This class is used as a namespace to contain various stereo/hand-eye/multi-
 11 |     camera calibration algorithms. The main goal in all cases is to compute a
 12 |     rigid transformation between the coordinate frames of different sensors.
 13 | */
 14 | class CalibStereo {
 15 | 	public:
 16 | 	/** Given two sets of corresponding points expressed in two different
 17 | 	    coordinate frames, calculate the relative pose between them.
 18 | 	    In this variant, a general translation and rotation is considered.
 19 | 	    The algorithm is in fact an implementation of Horn's "absolute
 20 | 	    orientation" paper.
 21 | 
 22 | 	    At least two points and their corresponding partners are needed as
 23 | 	    input.
 24 | 	*/
 25 | 	static TooN::SE3<double> getRelativePose(
 26 | 		const std::vector<TooN::Vector<3> > & points1,
 27 | 		const std::vector<TooN::Vector<3> > & points2);
 28 | 
 29 | 	/** Just like the above method, but instead of two sets of
 30 | 	    corresponding points rather two general sets with a list of
 31 | 	    "synchronized points" is passed as arguments.
 32 | 	*/
 33 | 	static TooN::SE3<double> getRelativePose(
 34 | 		const std::vector<TooN::Vector<3> > & points1,
 35 | 		const std::vector<TooN::Vector<3> > & points2,
 36 | 		const std::map<int,int> & synclist);
 37 | 
 38 | 	/** Given two sets of corresponding poses relative to the same
 39 | 	    coordinate frame, calculate the relative rotation and translation
 40 | 	    between them. As a possible application scenario, consider a set
 41 | 	    of calibrated images with a common calibration pattern observed by
 42 | 	    two rigidly aligned cameras. This method then allows to compute the
 43 | 	    relative pose between the two cameras. In terms of SE3s, the poses
 44 | 	    will be:
 45 | 		pose2 = relative * pose1
 46 | 
 47 | 	    A single pair of poses is sufficient to compute the relative pose.
 48 | 	    This is a linear least squares problem, solved internally with SVD.
 49 | 	*/
 50 | 	static TooN::SE3<double> getRelativePose_commonFrame(
 51 | 		const std::vector<TooN::SE3<double> > & pose1,
 52 | 		const std::vector<TooN::SE3<double> > & pose2);
 53 | 
 54 | 	/** Just like the above method, but instead of two sets of
 55 | 	    corresponding poses rather two general sets with a list of
 56 | 	    synchronized frames is passed as arguments.
 57 | 	*/
 58 | 	static TooN::SE3<double> getRelativePose_commonFrame(
 59 | 		const std::vector<TooN::SE3<double> > & pose1,
 60 | 		const std::vector<TooN::SE3<double> > & pose2,
 61 | 		const std::map<int,int> & synclist);
 62 | 
 63 | 	/** Given two sets of corresponding poses relative to two different
 64 | 	    coordinate frames, calculate the relative rotation between them.
 65 | 	    In this variant, only pure rotations are considered. Possible
 66 | 	    application scenarios are hand-eye-calibration or camera rig
 67 | 	    calibration.
 68 | 
 69 | 	    In terms of SO3s, the overall transformation from the coordinate
 70 | 	    frame of pose1 to the coordinate frame of pose2 will be:
 71 | 		pose1 * relative * pose2^T
 72 | 
 73 | 	    At least two pairs of corresponding poses (i.e. three different
 74 | 	    poses) are needed as input and the axes of these relative rotations
 75 | 	    must not be identical (ideally they are orthogonal). The solution
 76 | 	    is then determined by solving a linear least squares problem with
 77 | 	    SVD. Details on the algorithm are given in the SE3 variant of this
 78 | 	    method.
 79 | 	*/
 80 | 	static TooN::SO3<double> getRelativePose_differentFrames(
 81 | 		const std::vector<TooN::SO3<double> > & pose1,
 82 | 		const std::vector<TooN::SO3<double> > & pose2);
 83 | 
 84 | 	/** Just like the above method, but instead of two sets of
 85 | 	    corresponding poses rather two general sets with a list of
 86 | 	    synchronized frames is passed as arguments.
 87 | 	*/
 88 | 	static TooN::SO3<double> getRelativePose_differentFrames(
 89 | 		const std::vector<TooN::SO3<double> > & pose1,
 90 | 		const std::vector<TooN::SO3<double> > & pose2,
 91 | 		const std::map<int,int> & synclist);
 92 | 
 93 | 	/** Given two sets of corresponding poses relative to two different
 94 | 	    coordinate frames, calculate the relative rotation between them.
 95 | 	    Possible application scenarios are hand-eye-calibration or camera
 96 | 	    rig calibration.
 97 | 
 98 | 	    In terms of SE3s, the overall transformation from the coordinate
 99 | 	    frame of pose1 to the coordinate frame of pose2 will be:
100 | 		pose1 * relative * pose2^-1
101 | 
102 | 	    At least two pairs of corresponding poses (i.e. three different
103 | 	    poses) are needed as input and the axes of the relative rotations
104 | 	    of these poses must not be identical (ideally they are orthogonal).
105 | 	    The solution is then determined by solving a linear least squares
106 | 	    problem with SVD, as is explained below.
107 | 
108 | 	    Consider the transformations \f$\mathbf{A}_i, \mathbf{C}_i\f$
109 | 	    from the coordinate frames of sensors 1 and 2 to the \f$i\f$-th
110 | 	    observed pose of each sensor. The concatenated transformation
111 | 	    \f$ \mathbf{A}_i \mathbf{T} \mathbf{C}_i^{-1} \f$ with the unknown
112 | 	    relative transformation \f$ \mathbf{T} \f$ then transforms the
113 | 	    coordinate frame of sensor 1 to the coordinate frame of sensor 2.
114 | 	    For each i and j we have:
115 | 	    \f[
116 | 		\mathbf{A}_i \mathbf{T} \mathbf{C}_i^{-1} = \mathbf{A}_j \mathbf{T} \mathbf{C}_j^{-1}
117 | 	    \f] \f[
118 | 		\mathbf{A}_i \mathbf{T} \mathbf{C}_i^{-1} - \mathbf{A}_j \mathbf{T} \mathbf{C}_j^{-1} = 0
119 | 	    \f]
120 | 	    This provides a linear equation system in the entries of
121 | 	    \f$ \mathbf{T} \f$. Reinterpreting \f$ \mathbf{T} \f$ as a vector,
122 | 	    we arrive at a homogeneous linear equation system with a system
123 | 	    matrix defined by the entries of \f$ \mathbf{A}_i, \mathbf{A}_j, \mathbf{C}_i, \mathbf{C}_j \f$.
124 | 	    We solve this equation system using SVD. That's it. The only
125 | 	    problem is to get the indices right in your implementation...
126 | 	*/
127 | 	static TooN::SE3<double> getRelativePose_differentFrames(
128 | 		const std::vector<TooN::SE3<double> > & pose1,
129 | 		const std::vector<TooN::SE3<double> > & pose2);
130 | 
131 | 	/** Just like the above method, but instead of two sets of
132 | 	    corresponding poses rather two general sets with a list of
133 | 	    synchronized frames is passed as arguments.
134 | 	*/
135 | 	static TooN::SE3<double> getRelativePose_differentFrames(
136 | 		const std::vector<TooN::SE3<double> > & pose1,
137 | 		const std::vector<TooN::SE3<double> > & pose2,
138 | 		const std::map<int,int> & synclist);
139 | };
140 | 
141 | #endif
142 | 
143 | 


--------------------------------------------------------------------------------
/RelativePoseSolver/absolute_orientation_horn.h:
--------------------------------------------------------------------------------
  1 | // Albert Huang <albert@csail.mit.edu>
  2 | //
  3 | // Implementation of 
  4 | //
  5 | //    Berthold K. P. Horn, 
  6 | //    "Closed-form solution of absolute orientation using unit quaternions",
  7 | //    Journal of the Optical society of America A, Vol. 4, April 1987
  8 | 
  9 | #ifndef __absolute_orientation_horn__
 10 | #define __absolute_orientation_horn__
 11 | 
 12 | #include <assert.h>
 13 | #include <Eigen/Geometry>
 14 | #include <Eigen/Eigenvalues>
 15 | 
 16 | /**
 17 |  * absolute_orientation_horn:
 18 |  * @P1: a matrix of dimension [3 x num_points]
 19 |  * @P2: a matrix of dimension [3 x num_points]
 20 |  * @result: output parameter
 21 |  *
 22 |  * Given two point sets P1 and P2, with each point p1_i in set P1 matched with
 23 |  * a point p2_i in P2, compute the rigid body transformation (isometry) M that
 24 |  * minimizes
 25 |  *
 26 |  * \sum ||p2_i - M p1_i||
 27 |  *
 28 |  * returns: 0 on success, -1 on failure
 29 |  */
 30 | template <typename DerivedA, typename DerivedB>
 31 | int absolute_orientation_horn(const Eigen::MatrixBase<DerivedA>& P1, 
 32 |         const Eigen::MatrixBase<DerivedB>& P2,
 33 |         Eigen::Isometry3d* result)
 34 | {
 35 |     int num_points = P1.cols();
 36 |     assert(P1.cols() == P2.cols());
 37 |     assert(P1.rows() == 3 && P2.rows() == 3);
 38 |     if(num_points < 3)
 39 |         return -1;
 40 | 
 41 |     // compute centroids of point sets
 42 |     Eigen::Vector3d P1_centroid = P1.rowwise().sum() / num_points;
 43 |     Eigen::Vector3d P2_centroid = P2.rowwise().sum() / num_points;
 44 | 
 45 |     Eigen::MatrixXd R1 = P1;
 46 |     R1.colwise() -= P1_centroid;
 47 |     Eigen::MatrixXd R2 = P2;
 48 |     R2.colwise() -= P2_centroid;
 49 | 
 50 |     // compute matrix M
 51 |     double Sxx = R1.row(0).dot(R2.row(0));
 52 |     double Sxy = R1.row(0).dot(R2.row(1));
 53 |     double Sxz = R1.row(0).dot(R2.row(2));
 54 |     double Syx = R1.row(1).dot(R2.row(0));
 55 |     double Syy = R1.row(1).dot(R2.row(1));
 56 |     double Syz = R1.row(1).dot(R2.row(2));
 57 |     double Szx = R1.row(2).dot(R2.row(0));
 58 |     double Szy = R1.row(2).dot(R2.row(1));
 59 |     double Szz = R1.row(2).dot(R2.row(2));
 60 | 
 61 |     double A00 = Sxx + Syy + Szz;
 62 |     double A01 = Syz - Szy;
 63 |     double A02 = Szx - Sxz;
 64 |     double A03 = Sxy - Syx;
 65 |     double A11 = Sxx - Syy - Szz;
 66 |     double A12 = Sxy + Syx;
 67 |     double A13 = Szx + Sxz;
 68 |     double A22 = -Sxx + Syy - Szz;
 69 |     double A23 = Syz + Szy;
 70 |     double A33 = -Sxx - Syy + Szz;
 71 | 
 72 |     // prepare matrix for eigen analysis
 73 |     Eigen::Matrix4d N;
 74 |     N << A00, A01, A02, A03,
 75 |         A01, A11, A12, A13,
 76 |         A02, A12, A22, A23,
 77 |         A03, A13, A23, A33;
 78 | 
 79 |     Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> eigensolver(N);
 80 | 
 81 |     // rotation quaternion is the eigenvector with greatest eigenvalue
 82 |     Eigen::Vector4d eigvals = eigensolver.eigenvalues();
 83 |     int max_eigen_ind = 0;
 84 |     double max_eigen_val = eigvals(0);
 85 |     for(int i=1; i<4; i++) {
 86 |         if(eigvals(i) > max_eigen_val) {
 87 |             max_eigen_val = eigvals(i);
 88 |             max_eigen_ind = i;
 89 |         }
 90 |     }
 91 |     Eigen::Vector4d quat = eigensolver.eigenvectors().col(max_eigen_ind);
 92 |     Eigen::Quaterniond rotation(quat[0], quat[1], quat[2], quat[3]);
 93 |  
 94 |     // now compute the resulting isometry
 95 |     result->setIdentity();
 96 |     result->translate(P2_centroid - rotation * P1_centroid);
 97 |     result->rotate(rotation);
 98 | 
 99 |     return 0;
100 | }
101 | #endif
102 | 


--------------------------------------------------------------------------------
/RelativePoseSolver/data/aligned.txt:
--------------------------------------------------------------------------------
  1 | -0.0613958  0.0421279  -0.347368          1
  2 | -0.0619917  0.0430739  -0.347456          1
  3 | -0.0623931  0.0440369  -0.347465          1
  4 | -0.0625638  0.0449608  -0.347441          1
  5 | -0.0629908  0.0441336   -0.34753          1
  6 | -0.0635276  0.0438042  -0.347542          1
  7 | -0.0641204  0.0459259  -0.347252          1
  8 | -0.0650719  0.0474147  -0.346928          1
  9 | -0.0654161  0.0475028  -0.346854          1
 10 | -0.0658203  0.0460454   -0.34703          1
 11 | -0.0662689  0.0456158  -0.347183          1
 12 | -0.0660768  0.0454372  -0.347406          1
 13 | -0.0667852  0.0458392  -0.347507          1
 14 | -0.0680596  0.0484125  -0.347446          1
 15 | -0.0693172  0.0489778  -0.347637          1
 16 | -0.0709201  0.0456506  -0.348206          1
 17 | -0.0721378  0.0491222  -0.348383          1
 18 | -0.0732117   0.053405  -0.348249          1
 19 | -0.0749988  0.0532568  -0.348467          1
 20 | -0.0775814  0.0520489   -0.34882          1
 21 | -0.079855 0.0538645 -0.348852         1
 22 | -0.0815202   0.055227  -0.348947          1
 23 | -0.083702 0.0598244  -0.34858         1
 24 | -0.0854701  0.0625849  -0.348094          1
 25 | -0.0875952  0.0614247  -0.347454          1
 26 | -0.0891122  0.0661141  -0.346262          1
 27 | -0.0905591  0.0679732  -0.345174          1
 28 | -0.0930817  0.0715534  -0.343334          1
 29 | -0.0945623  0.0733614  -0.342127          1
 30 | -0.095725 0.0755097 -0.340927         1
 31 | -0.0970707  0.0810846  -0.339003          1
 32 | -0.0982774  0.0833308   -0.33768          1
 33 | -0.099369 0.0822801  -0.33673         1
 34 | -0.101239  0.085578 -0.334668         1
 35 | -0.102522 0.0919007 -0.332603         1
 36 | -0.103368  0.096305 -0.330784         1
 37 | -0.104382 0.0954897  -0.32924         1
 38 | -0.104634 0.0974572 -0.327829         1
 39 | -0.104659 0.0995095 -0.326507         1
 40 | -0.103577  0.107685 -0.324374         1
 41 | -0.103245  0.106792 -0.323653         1
 42 | -0.102081  0.113226 -0.322246         1
 43 | -0.101389  0.114989 -0.321131         1
 44 | -0.101366  0.113563 -0.320588         1
 45 | -0.0998497    0.11663  -0.319922          1
 46 | -0.0977811   0.119244  -0.319665          1
 47 | -0.096145  0.118997 -0.319871         1
 48 | -0.0937685   0.122694  -0.319801          1
 49 | -0.0924436   0.123857  -0.319935          1
 50 | -0.0913686    0.12188  -0.320473          1
 51 | -0.0894388   0.121768  -0.321215          1
 52 | -0.0889008   0.120057  -0.321868          1
 53 | -0.0870503   0.121294  -0.322689          1
 54 | -0.0846967   0.122715  -0.323828          1
 55 | -0.082897   0.12368 -0.324759         1
 56 | -0.082003  0.120576 -0.325957         1
 57 | -0.0791986   0.121095  -0.327453          1
 58 | -0.0774835   0.119818  -0.328617          1
 59 | -0.0755924   0.117678  -0.329991          1
 60 | -0.0729416   0.116344  -0.331611          1
 61 | -0.0699058     0.1166   -0.33308          1
 62 | -0.0684344    0.11163  -0.334811          1
 63 | -0.0653168   0.112358  -0.336177          1
 64 | -0.0635729   0.109955  -0.337354          1
 65 | -0.0610879   0.107433  -0.338716          1
 66 | -0.058527  0.107141 -0.339552         1
 67 | -0.0564552   0.104613  -0.340583          1
 68 | -0.053051  0.103829 -0.341547         1
 69 | -0.0506039   0.103018  -0.342231          1
 70 | -0.0488553    0.10051  -0.342802          1
 71 | -0.045589 0.0987427 -0.343632         1
 72 | -0.0426838  0.0981189  -0.344253          1
 73 | -0.0404805  0.0959643   -0.34477          1
 74 | -0.0376242  0.0942321  -0.345197          1
 75 | -0.035114 0.0941216 -0.345414         1
 76 | -0.0324759  0.0930752  -0.345752          1
 77 | -0.0289122  0.0920006  -0.346093          1
 78 | -0.0265805  0.0905714  -0.346245          1
 79 | -0.0243926  0.0894115  -0.346388          1
 80 | -0.0218328  0.0868357  -0.346532          1
 81 | -0.0187208  0.0866584  -0.346877          1
 82 | -0.0162294  0.0842736  -0.346911          1
 83 | -0.0136541  0.0828552  -0.347175          1
 84 | -0.0116202  0.0811106  -0.347066          1
 85 | -0.00800019   0.0803202   -0.347115           1
 86 | -0.00584509   0.0787022   -0.347041           1
 87 | -0.00271904   0.0785208   -0.347036           1
 88 | 8.62944e-05   0.0756889   -0.346743           1
 89 | 0.00231393  0.0734641  -0.346669          1
 90 | 0.00598543  0.0733923  -0.346607          1
 91 | 0.00922875  0.0706124  -0.346308          1
 92 |  0.012041 0.0688329 -0.346132         1
 93 | 0.0143706 0.0671233   -0.3456         1
 94 | 0.0177171 0.0656471 -0.344817         1
 95 | 0.0195808 0.0634925 -0.344075         1
 96 | 0.0221025 0.0623926 -0.343346         1
 97 | 0.0249051 0.0607401 -0.342189         1
 98 | 0.0269043 0.0597551 -0.341292         1
 99 | 0.0283971 0.0573059 -0.339886         1
100 | 0.0299285 0.0561941 -0.338903         1
101 | 0.0319439 0.0555052 -0.338068         1
102 | 0.0336697 0.0549587 -0.336704         1
103 | 0.0343624 0.0534797 -0.335873         1
104 |  0.035885 0.0539816  -0.33502         1
105 | 0.0366069 0.0534359 -0.333974         1
106 | 0.0363458 0.0525705 -0.333067         1
107 |  0.036297 0.0527608 -0.332214         1
108 | 0.0354005  0.053169 -0.331053         1
109 | 0.0345655 0.0539684 -0.330302         1
110 | 0.0334631 0.0541812 -0.329775         1
111 | 0.0315221 0.0556133 -0.329163         1
112 | 0.0296682 0.0560753 -0.328782         1
113 | 0.0283645 0.0578969  -0.32852         1
114 | 0.0248756 0.0583588 -0.328024         1
115 |  0.022563 0.0600967 -0.327568         1
116 | 0.0202953 0.0616417 -0.327306         1
117 | 0.0171226 0.0636936 -0.327033         1
118 |  0.013357 0.0631455 -0.326938         1
119 | 0.00920272  0.0641031  -0.326946          1
120 | 0.00692198  0.0670102  -0.326912          1
121 | 0.00354805  0.0669836  -0.327045          1
122 | -9.69267e-05     0.068273    -0.327288            1
123 | -0.00271526   0.0702389   -0.327334           1
124 | -0.00587861   0.0704256   -0.327515           1
125 | -0.010211   0.06988 -0.327898         1
126 | -0.0128368  0.0714136  -0.328091          1
127 | -0.0152107  0.0745917  -0.328087          1
128 | -0.0196973  0.0726693  -0.328399          1
129 | -0.0221944  0.0749056  -0.328333          1
130 | -0.0251752  0.0744757  -0.328434          1
131 | -0.028405  0.076689 -0.328416         1
132 | -0.0318081  0.0754634  -0.328474          1
133 | -0.0343036  0.0756985  -0.328647          1
134 | -0.0370743  0.0789814  -0.328832          1
135 | -0.0401502   0.078513  -0.329123          1
136 | -0.0446994   0.076834  -0.329508          1
137 | -0.0477194  0.0769581  -0.329811          1
138 | -0.0508077  0.0762922  -0.330254          1
139 | -0.055813 0.0745281 -0.330852         1
140 | -0.0588038  0.0773783  -0.331082          1
141 | -0.0618923  0.0780669  -0.331567          1
142 | -0.066189 0.0788535   -0.3322         1
143 | -0.0690939   0.077705  -0.332842          1
144 | -0.0724661  0.0759148   -0.33353          1
145 | -0.0759595  0.0761976  -0.334434          1
146 | -0.079438  0.072076 -0.335306         1
147 | -0.082217 0.0753828 -0.335796         1
148 | -0.0865902  0.0720135  -0.336901          1
149 | -0.0890768  0.0741503  -0.337506          1
150 | -0.091864 0.0756247 -0.338036         1
151 | -0.095973 0.0714844 -0.339047         1
152 | -0.0984511  0.0703814  -0.339634          1
153 | -0.102077 0.0703785  -0.34032         1
154 | -0.104534 0.0685557 -0.340905         1
155 | -0.106123 0.0685671 -0.341646         1
156 | -0.107555  0.066792 -0.342856         1
157 | -0.109281  0.065304 -0.343683         1
158 | -0.110762 0.0668369 -0.344508         1
159 | -0.112453 0.0655383 -0.345958         1
160 | -0.113877 0.0634551 -0.347244         1
161 | -0.115266 0.0571037 -0.348885         1
162 | -0.117006 0.0545832 -0.350939         1
163 | -0.117885 0.0550691 -0.352659         1
164 | -0.118708 0.0554331 -0.354481         1
165 | -0.120596 0.0505485 -0.357185         1
166 | -0.121528 0.0490838 -0.359277         1
167 | -0.123029 0.0440083  -0.36151         1
168 | -0.124744  0.040782 -0.364363         1
169 | -0.126426 0.0364107 -0.366444         1
170 | -0.127416 0.0337683 -0.368651         1
171 | -0.128558 0.0311144 -0.371576         1
172 | -0.129884  0.028043  -0.37377         1
173 | -0.132001 0.0223491 -0.376727         1
174 | -0.133307 0.0203693  -0.37904         1
175 | -0.134795  0.016898 -0.381436         1
176 |  -0.13699 0.0123166 -0.384647         1
177 |  -0.138611 0.00980016  -0.387036          1
178 |  -0.140272 0.00587111  -0.389426          1
179 |  -0.141964 0.00279841  -0.392646          1
180 |  -0.143149 0.00112009  -0.395037          1
181 |   -0.144454 -0.00246832   -0.397359           1
182 |  -0.145896 -0.0109022   -0.40066          1
183 |  -0.146573 -0.0128401  -0.403357          1
184 |  -0.147611 -0.0157847  -0.405966          1
185 |  -0.148556 -0.0207272  -0.409594          1
186 |  -0.149766 -0.0247795  -0.412303          1
187 |  -0.150254 -0.0281751  -0.415186          1
188 |  -0.150606 -0.0348856  -0.419062          1
189 |  -0.150825 -0.0372792   -0.42211          1
190 |   -0.15162 -0.0439434  -0.425878          1
191 |   -0.15195 -0.0463265  -0.428823          1
192 |  -0.151743 -0.0477897  -0.431854          1
193 |  -0.152116 -0.0529938  -0.435719          1
194 |  -0.152842 -0.0600366  -0.438315          1
195 |  -0.152354 -0.0610806  -0.441334          1
196 |  -0.152758 -0.0659875  -0.444815          1
197 |  -0.153328 -0.0713812  -0.447131          1
198 |  -0.152847 -0.0724959   -0.44985          1
199 |  -0.152773 -0.0793055  -0.453094          1
200 | -0.152773 -0.082658  -0.45544         1
201 | 


--------------------------------------------------------------------------------
/RelativePoseSolver/data/itm.txt:
--------------------------------------------------------------------------------
  1 | -0.000368523 -0.000223139    0.0003863            1
  2 |  -0.00108742 -0.000204059  0.000802004            1
  3 | -0.00169299 9.38658e-05  0.00118851           1
  4 | -0.00193933 0.000269214  0.00144209           1
  5 |  -0.00223674 -0.000127545   0.00176715            1
  6 | -0.00285563 0.000207362  0.00209659           1
  7 |  -0.00291993 -0.000285545   0.00238106            1
  8 | -0.00268659 -0.00111096  0.00246634           1
  9 | -0.00288526 -0.00167591  0.00259285           1
 10 | -0.00322343 -0.00160388  0.00268962           1
 11 | -0.00353915   -0.001739  0.00285661           1
 12 | -0.00414385 -0.00156665  0.00319859           1
 13 |  -0.0047571 -0.00138773  0.00353129           1
 14 | -0.00516442 -0.00134275  0.00380001           1
 15 | -0.00472218 -0.00247107  0.00411762           1
 16 | -0.00521375 -0.00224641   0.0047129           1
 17 | -0.00610028 -0.00165146  0.00563377           1
 18 | -0.00686523 -0.00162369  0.00673067           1
 19 | -0.00770908 -0.00193692  0.00794518           1
 20 |  -0.0084165 -0.00256909  0.00929419           1
 21 | -0.00950199 -0.00264558   0.0108575           1
 22 |  -0.0106247 -0.00192609   0.0125911           1
 23 |  -0.0115998 -0.00210213   0.0146605           1
 24 |  -0.0121787 -0.00236414   0.0166745           1
 25 |  -0.0126354 -0.00282221   0.0187432           1
 26 |  -0.0128997 -0.00300599   0.0205504           1
 27 |  -0.0136737 -0.00349665    0.022442           1
 28 |    -0.01393 -0.00381478   0.0240533           1
 29 |  -0.0140825 -0.00425379   0.0255645           1
 30 | -0.0149468 -0.0041168  0.0272298          1
 31 |  -0.0156866 -0.00391924    0.028987           1
 32 |  -0.0161602 -0.00396207   0.0309055           1
 33 |  -0.0166964 -0.00419221   0.0329542           1
 34 |  -0.0169424 -0.00415886   0.0350734           1
 35 |  -0.0176575 -0.00401145   0.0373399           1
 36 |  -0.0181858 -0.00463307   0.0395864           1
 37 |  -0.0186982 -0.00508448   0.0420271           1
 38 |  -0.0196138 -0.00435054   0.0444822           1
 39 |  -0.0209475 -0.00383686   0.0473344           1
 40 |  -0.0213185 -0.00377099   0.0501284           1
 41 |  -0.0214901 -0.00357364   0.0527425           1
 42 | -0.0215929 -0.0027617  0.0554838          1
 43 |  -0.0219123 -0.00133336   0.0582797           1
 44 |   -0.0217237 -0.000597145     0.060916            1
 45 |  -0.022579 0.00177514  0.0635819          1
 46 | -0.0242838 0.00390741  0.0664522          1
 47 | -0.0253298 0.00627066  0.0690788          1
 48 | -0.0240164 0.00695505  0.0710488          1
 49 | -0.0237253 0.00822649  0.0734311          1
 50 |  -0.022478 0.00949647   0.074882          1
 51 | -0.0219716  0.0110083  0.0766273          1
 52 | -0.0213309  0.0119827  0.0779857          1
 53 | -0.0210888  0.0137351  0.0793538          1
 54 | -0.0205086  0.0151886  0.0804951          1
 55 | -0.0196907  0.0173108  0.0811615          1
 56 | -0.0208369  0.0200605   0.082227          1
 57 | -0.0188467  0.0217474  0.0820893          1
 58 | -0.0174105  0.0238507  0.0819254          1
 59 | -0.0146441  0.0248578  0.0813373          1
 60 | -0.0118046  0.0252622  0.0804453          1
 61 | -0.010471 0.0260638 0.0798758         1
 62 | -0.00934906   0.0263679   0.0793802           1
 63 | -0.00872512   0.0269036    0.078913           1
 64 | -0.00785115   0.0272348   0.0783115           1
 65 | -0.00596226   0.0273782   0.0772902           1
 66 | -0.00424125   0.0277746   0.0762556           1
 67 | -0.00206808    0.028402   0.0749882           1
 68 | 0.000226399   0.0289078   0.0736459           1
 69 | 0.00195069  0.0294074  0.0725579          1
 70 | 0.00340188  0.0306135  0.0713888          1
 71 | 0.00523212  0.0308852  0.0700582          1
 72 | 0.00773686  0.0315555  0.0683182          1
 73 | 0.0101976 0.0326851 0.0666086         1
 74 | 0.0119737 0.0333772 0.0652124         1
 75 | 0.0140597 0.0340165 0.0638352         1
 76 | 0.0162222 0.0343019 0.0624224         1
 77 | 0.0139192 0.0327515 0.0628506         1
 78 | 0.0168235  0.033035 0.0610059         1
 79 | 0.0196281 0.0331664 0.0593591         1
 80 | 0.0216122 0.0331601 0.0581169         1
 81 |  0.023632  0.033115 0.0569055         1
 82 | 0.0253662 0.0325855 0.0559907         1
 83 | 0.0269455 0.0321768 0.0550937         1
 84 |  0.028245 0.0310985  0.054427         1
 85 | 0.0292382 0.0294111 0.0540004         1
 86 | 0.0300389 0.0284101  0.053457         1
 87 | 0.0307831 0.0273682 0.0529381         1
 88 | 0.0316816  0.025981 0.0523185         1
 89 | 0.0328526 0.0251468 0.0514316         1
 90 | 0.0341099 0.0237749 0.0505503         1
 91 | 0.0356438 0.0222877 0.0493836         1
 92 | 0.0367849 0.0218341 0.0481771         1
 93 | 0.0379859 0.0203314 0.0470109         1
 94 | 0.0394229 0.0188196 0.0458062         1
 95 | 0.0403725 0.0186699 0.0444352         1
 96 | 0.0416025 0.0174029 0.0431182         1
 97 | 0.0430278 0.0163102 0.0416306         1
 98 | 0.0443811 0.0147737 0.0402249         1
 99 | 0.0453035  0.014248 0.0386477         1
100 | 0.0464006 0.0133147 0.0372289         1
101 | 0.0471427  0.011964 0.0360629         1
102 | 0.0479762 0.0107509 0.0347911         1
103 | 0.0489044  0.010087  0.033342         1
104 |  0.0492528 0.00912278  0.0323709          1
105 |   0.050039 0.00782973  0.0313969          1
106 |  0.0506895 0.00739783  0.0303275          1
107 | 0.0511722 0.0073614 0.0292894         1
108 |  0.0522147 0.00628788  0.0282951          1
109 |  0.0529417 0.00582736  0.0273403          1
110 |  0.0534154 0.00547088   0.026583          1
111 |  0.0534487 0.00587332  0.0260142          1
112 |  0.0532388 0.00577431  0.0259255          1
113 |  0.0529675 0.00609634  0.0258772          1
114 |  0.0520365 0.00548834  0.0265916          1
115 |  0.0510786 0.00516897  0.0273589          1
116 |  0.0502346 0.00489854  0.0280338          1
117 |  0.0499543 0.00447788  0.0286811          1
118 |  0.0494488 0.00410101  0.0293703          1
119 | 0.0485193  0.003374 0.0303684         1
120 |  0.0473381 0.00330044  0.0313536          1
121 | 0.0469423 0.0027874 0.0320896         1
122 | 0.0460921 0.0020418 0.0330486         1
123 |  0.0445657 0.00176474  0.0340689          1
124 |   0.0434554 0.000369522   0.0350445           1
125 |    0.0432139 -0.000903239    0.0356406            1
126 |   0.0431979 -0.00218344   0.0359281           1
127 |   0.0426843 -0.00248065   0.0364021           1
128 |   0.0425441 -0.00301597    0.036603           1
129 |   0.0428729 -0.00420292   0.0367388           1
130 |   0.0432583 -0.00545026   0.0367553           1
131 |   0.0423275 -0.00688947   0.0371599           1
132 |    0.041689 -0.00926732   0.0376346           1
133 |  0.0381219 -0.0094747  0.0384272          1
134 |  0.0339744 -0.0107451  0.0397641          1
135 |  0.0288037 -0.0104664  0.0409837          1
136 |   0.0195669 -0.00797222    0.042797           1
137 |   0.0108754 -0.00553796   0.0443451           1
138 |  0.00584279 -0.00546185   0.0448438           1
139 | -0.00332771 -0.00275954   0.0466682           1
140 | -0.00550791 -0.00364358   0.0466192           1
141 | -0.00933048 -0.00354323   0.0473735           1
142 |  -0.0118189 -0.00348371   0.0473691           1
143 |  -0.0138239 -0.00411784   0.0473556           1
144 |  -0.0157483 -0.00426816   0.0472222           1
145 |  -0.0184188 -0.00404186   0.0471259           1
146 |  -0.0210107 -0.00431442   0.0469886           1
147 |  -0.0228651 -0.00425526   0.0467499           1
148 |  -0.0247876 -0.00385691   0.0463929           1
149 |  -0.0282907 -0.00170288   0.0462618           1
150 |  -0.0292053 -0.00206106    0.045662           1
151 |  -0.0301632 -0.00266717   0.0449758           1
152 |  -0.0334608 -0.00270648   0.0447405           1
153 |   -0.035015 -0.00241631   0.0441401           1
154 |  -0.0357769 -0.00197633   0.0433055           1
155 |  -0.0367259 -0.00232272   0.0425903           1
156 | -0.0375251  -0.001793  0.0416477          1
157 |  -0.0383033 -0.00133874   0.0408252           1
158 |  -0.0387585 -0.00146854   0.0400248           1
159 |   -0.0395321 -0.000983014    0.0390222            1
160 |   -0.0400979 -0.000267756    0.0379091            1
161 |  -0.0406763 0.000512323   0.0368098           1
162 | -0.0416094 0.00147744  0.0356379          1
163 | -0.0425934 0.00263416  0.0344403          1
164 | -0.0429562 0.00386907  0.0328906          1
165 | -0.0434079 0.00490881  0.0313793          1
166 | -0.0440049 0.00646538  0.0297381          1
167 | -0.0446631 0.00886041  0.0279009          1
168 | -0.0452905  0.0107319  0.0261192          1
169 | -0.0460614  0.0125845  0.0243004          1
170 | -0.0466572  0.0142843  0.0221989          1
171 | -0.0474416  0.0162173  0.0200157          1
172 | -0.0479178  0.0184775  0.0175002          1
173 | -0.0483895  0.0204267  0.0149328          1
174 | -0.0498525  0.0247917 0.00949077          1
175 | -0.0511141  0.0274709 0.00681932          1
176 | -0.0516839  0.0297923 0.00396826          1
177 | -0.0531659  0.0322332 0.00120298          1
178 |  -0.0535818   0.0343897 -0.00179821           1
179 |  -0.0543011   0.0367387 -0.00475892           1
180 |  -0.0553372   0.0392908 -0.00776888           1
181 | -0.0561191   0.041198 -0.0107222          1
182 | -0.0573031  0.0439145 -0.0138244          1
183 |  -0.057554  0.0464814 -0.0171476          1
184 | -0.0589513  0.0492389 -0.0200991          1
185 | -0.0592608   0.051388 -0.0233141          1
186 | -0.0594985  0.0539423 -0.0266328          1
187 | -0.0603799  0.0572374 -0.0297682          1
188 | -0.0611206   0.060171 -0.0329091          1
189 | -0.0617399  0.0627399 -0.0359944          1
190 | -0.0621849  0.0659427 -0.0392064          1
191 | -0.0620953  0.0692197 -0.0426425          1
192 | -0.0618652  0.0719169 -0.0458654          1
193 | -0.0613187  0.0750006 -0.0492413          1
194 |  -0.061271  0.0779009 -0.0525248          1
195 | -0.0611271  0.0803769 -0.0557158          1
196 | -0.0604677   0.083439 -0.0591337          1
197 | -0.0599519  0.0859692 -0.0624154          1
198 | -0.0598915  0.0888366 -0.0656589          1
199 | -0.0599391   0.091671 -0.0687742          1
200 | -0.0588577  0.0942395 -0.0720982          1
201 | 


--------------------------------------------------------------------------------
/RelativePoseSolver/data/pose1.txt:
--------------------------------------------------------------------------------
  1 | -0  0 -0  1
  2 | -4 -5 -6  1
  3 | -11.1864 -9.45618 -3.65508        1
  4 |  -10.269 -8.66582 -19.2354        1
  5 | -9.41456  -13.466 -27.2028        1
  6 | -2.50552 -35.3448  3.32294        1
  7 | -18.8874 -37.3207 -6.55871        1
  8 | -16.9463 -40.7135 -24.6956        1
  9 | -50.0265   -14.44  12.6303        1
 10 | -18.6413  24.9166 -47.9681        1
 11 |  47.8096 -28.0488  -13.334        1
 12 | -12.1177 -28.4086  45.3462        1
 13 | -38.2453  29.2603  19.7614        1
 14 |  37.4439  25.8517 0.321178        1
 15 | -16.6469 -19.5324  35.2228        1
 16 | 15.0878 22.2317 22.1669       1
 17 | -2.96809  14.4366  22.8863        1
 18 | -6.27065  21.7103 -1.26588        1
 19 |   14.187 -12.1575  5.41324        1
 20 | -21.8311 -4.45925   1.7549        1
 21 |  6.48536 -3.33481 -25.6684        1
 22 | -7.22029 -31.4287  -7.7897        1
 23 | -27.0666 -16.5205  14.8754        1
 24 |  -6.0395  20.1979 -30.2428        1
 25 |  26.4894 -23.9366  -14.418        1
 26 | -20.8049 -34.7618    11.73        1
 27 | -35.8546  22.4556  -2.7906        1
 28 |  35.3884   8.5585 -13.8937        1
 29 | -13.9909  -24.825  26.0277        1
 30 | -29.2017  22.3902 -16.2039        1
 31 | -4.52568 -11.3533 -45.6987        1
 32 | -11.3408 -20.7892 -49.9837        1
 33 | -35.7923 -48.8995  5.01152        1
 34 | -59.3964  7.36994 -27.8081        1
 35 |   -44.03 -34.0288 -49.7089        1
 36 |  32.9903 -53.1527 -49.3659        1
 37 |  10.6953 -62.3936  47.2972        1
 38 | -69.6987  13.4581  33.8416        1
 39 |    9.726  22.1441 -78.5195        1
 40 | -24.6898  -65.469 -57.6303        1
 41 | -53.5917 -39.3396 -73.6645        1
 42 | -14.5238 -85.9593  50.4002        1
 43 |   12.305  90.9086 -31.9962        1
 44 | -21.6692 -99.5243  7.87445        1
 45 | -3.40949 -37.1762 -102.959        1
 46 | -18.2206 -61.4862  -98.743        1
 47 | -47.8663  -103.26 -53.7859        1
 48 | -126.899 -14.0607   19.698        1
 49 | -0.527587   25.3951  -131.512         1
 50 |  89.4422 -99.6414  9.87728        1
 51 | -110.541  4.58104  75.1366        1
 52 | -11.2159   79.936 -109.051        1
 53 |  89.2674 -80.1218 -69.7623        1
 54 | -64.0774    24.25  115.615        1
 55 |   97.869  67.5024 -55.2627        1
 56 |  126.529  1.69629 -16.3027        1
 57 | 30.6578 -67.451 99.7214       1
 58 | -32.6647  114.374 -25.0292        1
 59 |  103.339 -60.7937  5.84595        1
 60 | -116.919   31.871 -23.2841        1
 61 |  95.0585 -39.6138 -70.9406        1
 62 |  -58.021 -18.6174  105.956        1
 63 |   83.557  31.6882 -82.9906        1
 64 |  24.1658 -95.2847  70.9981        1
 65 | -78.8745   35.941  80.1484        1
 66 |  85.7721  7.08572 -81.7073        1
 67 | -35.2599 -25.8259  106.571        1
 68 | -35.4648  89.1309  52.4549        1
 69 | 0.0304447  -13.3689  -114.937         1
 70 | -56.2119 -107.792  3.84546        1
 71 | -106.741   52.233 -37.0234        1
 72 | -45.5577 -31.7728 -120.231        1
 73 | -2.82739 -41.1851 -133.396        1
 74 |  28.5504 -120.409   65.555        1
 75 | -141.15 19.6487 7.90016       1
 76 | -15.3524   31.755 -143.448        1
 77 |  87.6876 -114.413 -43.9933        1
 78 | -109.954  5.43727  100.676        1
 79 | -58.8802  91.9567  -105.21        1
 80 |  40.7243 -76.8047 -131.841        1
 81 |   -127.8 -77.7748  59.7017        1
 82 | -101.025 -77.5847 -112.131        1
 83 | -143.673 -93.9222   31.189        1
 84 | -7.74429  45.4741 -172.956        1
 85 | 13.0003 -151.35 95.6587       1
 86 | -121.419   95.008 -97.8562        1
 87 | -69.9348  61.7574 -162.656        1
 88 | -34.9144 -189.732  2.94981        1
 89 | -124.502 -68.3846 -142.702        1
 90 | -75.9451 -190.546 -36.6266        1
 91 | -198.228  3.15075 -81.0052        1
 92 |  22.8637  -49.479 -213.765        1
 93 | -44.7397 -169.758  -146.93        1
 94 | -215.616  63.8369 -59.7393        1
 95 | 0.625241  147.276 -181.854        1
 96 |  91.9213 -120.745 -185.343        1
 97 |  -84.913 -164.989 -164.863        1
 98 |  -30.47 -56.209 -247.43       1
 99 |  109.115 -161.948 -172.979        1
100 | 28.7248 155.193 197.278       1
101 | 113.134 205.341 70.5847       1
102 | 74.3112 57.3783 217.434       1
103 | 42.0731 212.102 76.6797       1
104 |  223.306  -36.126 -6.36136        1
105 | 71.2684 27.3916 204.942       1
106 | -37.9865 -6.67816  210.039        1
107 | 139.636 139.993 56.8467       1
108 | 46.7118 139.651 131.231       1
109 |  127.079  117.271 -88.3048        1
110 | 70.8274 56.8729 162.142       1
111 | -114.193  126.724  65.0437        1
112 |  87.1507 -40.5047 -160.146        1
113 |  21.5776 -148.263  111.063        1
114 |  56.5733   170.61 -22.5741        1
115 |  167.733  49.0546 -26.0917        1
116 |  69.9731 -66.5337    142.2        1
117 | -50.0696  160.118 -18.5398        1
118 |  59.9407  13.4536 -161.112        1
119 | -14.1097 -140.137  100.615        1
120 | -150.758  77.8842  -45.407        1
121 |  -134.14  68.1635 -98.5559        1
122 |  144.335 -20.1459   -106.6        1
123 | -44.5208 -147.196 -109.636        1
124 | -107.185 -145.767  66.1548        1
125 | -83.8092  7.59745 -180.885        1
126 | -90.2948 -178.434  41.6985        1
127 | -84.1468  82.4662 -172.303        1
128 |  116.912 -158.291 -79.8356        1
129 | -55.4815 -96.0131  178.837        1
130 | -125.832  103.365 -139.016        1
131 |  68.2381 -186.455 -93.7453        1
132 | -213.113 -57.9691 -46.9412        1
133 | -123.981 -51.0162 -191.655        1
134 |   -7.58735   -238.789 -0.0332278          1
135 | -230.028  12.0732  69.4835        1
136 | -65.2879 -27.7624 -237.543        1
137 |  -106.85 -227.438  31.4582        1
138 | -151.179  196.202  38.6155        1
139 |  60.9295 -101.099  -228.24        1
140 | -250.948 -65.1672 -43.7334        1
141 | -160.258 -179.692 -125.164        1
142 | -50.3285 -255.208 -100.274        1
143 | -227.261 -146.315 -94.9029        1
144 |  29.5953 -285.009  44.2807        1
145 | -285.422  64.0071 -16.3349        1
146 |  165.918 -85.1687 -230.789        1
147 | -50.6739 -293.908  42.9874        1
148 | -304.847   8.6102  4.17953        1
149 | 191.595 142.937 -187.33       1
150 | 188.737 58.3707 219.984       1
151 | 83.8274 228.583 152.578       1
152 | 181.621 210.217 -48.237       1
153 |  86.7321 -51.8995  256.816        1
154 | -31.5136  152.203  219.168        1
155 |  125.53 144.355 175.958       1
156 | 185.525 17.7716 170.406       1
157 | -11.287 244.653 31.3333       1
158 |  119.773 -55.4662   -213.6        1
159 |  92.7791 -231.393  43.2627        1
160 | -233.726  63.1421  75.2706        1
161 |  8.62073   136.36 -216.092        1
162 |  75.7248 -220.695 -117.731        1
163 | -243.339  12.5505   97.488        1
164 | 12.9828  154.25 -213.91       1
165 |  179.369 -61.8727  175.274        1
166 | -127.575  94.5023   197.84        1
167 |  175.404 -4.36966  173.334        1
168 |  46.9655 -69.9593  226.407        1
169 |  202.273  113.552 -47.9202        1
170 |  42.1271 -13.9147  226.158        1
171 | -59.9948   201.23 -92.3117        1
172 |  224.868  9.28606 -11.4436        1
173 | -86.5453  107.329  171.063        1
174 | 106.838 180.297 34.6494       1
175 | 191.738 32.5399 67.4782       1
176 | 20.6923 86.8873 176.545       1
177 |  187.315  43.8289 -18.6496        1
178 |  43.7525 -7.51454  181.267        1
179 | -21.2495   150.65  95.5093        1
180 |  164.911  51.1207 -30.2582        1
181 | -122.427 -17.2797  122.643        1
182 |  24.4123  148.079 -84.0241        1
183 |  150.153 -52.7703  53.4829        1
184 | 83.6749 54.5525 124.221       1
185 |  90.7443 -51.0639  113.625        1
186 | -123.645  83.9371  33.0873        1
187 |  93.4174   -59.24 -110.863        1
188 |   45.642 -110.556  98.5005        1
189 | -116.722 -55.6404  87.4747        1
190 | -117.525  34.8617 -105.473        1
191 |  89.5028 -136.276  7.26209        1
192 | -159.186 -47.9912 -32.5247        1
193 |  3.87011 -1.43154 -175.194        1
194 | -14.7415 -34.8706  166.343        1
195 | -163.527   33.719  40.4267        1
196 |  17.7891 -109.725 -140.425        1
197 | -85.2013  -53.189 -158.182        1
198 | -8.55999  -1.4344 -193.287        1
199 |  21.6678 -196.296  7.50483        1
200 | -117.039  48.2697 -159.109        1
201 | 


--------------------------------------------------------------------------------
/RelativePoseSolver/data/pose2.txt:
--------------------------------------------------------------------------------
  1 |   -1.52197   -3.41689 -0.0921404          1
  2 |   -7.539 -3.81083 -6.24776        1
  3 | -13.4306 -9.58686  -6.6461        1
  4 | -8.96712  -10.673 -22.1123        1
  5 | -11.1897 -16.5678 -28.3107        1
  6 | -4.81535 -38.2648  3.69455        1
  7 | -21.8203 -39.3611 -7.67002        1
  8 | -16.2361 -44.0169 -26.3028        1
  9 |   -52.19 -15.2716  15.5676        1
 10 | -18.4774  28.4173 -49.2788        1
 11 |  51.2838 -26.6681 -13.1803        1
 12 | -10.4366 -27.3735  48.5246        1
 13 | -37.4294  30.1708  23.2977        1
 14 |    36.548   29.3632 -0.609855         1
 15 |  -13.663 -20.8528  37.0539        1
 16 | 12.4323 22.9656 24.6987       1
 17 | -2.58875  17.9423  21.6346        1
 18 | -2.69296  22.8044 -1.32158        1
 19 |  13.2867 -9.99122  8.32814        1
 20 | -20.1737 -1.15381  2.32688        1
 21 |  7.82353 -2.30397 -22.3298        1
 22 | -4.86923 -28.7476 -6.65651        1
 23 | -23.764 -14.927 14.1316       1
 24 | -6.23622  18.1697 -27.1047        1
 25 |  23.2529 -22.0598 -14.3628        1
 26 | -18.9327 -33.0351  8.98901        1
 27 | -34.7715  19.0741 -3.97071        1
 28 |    33.14  5.98326 -15.4145        1
 29 | -16.4322 -23.8167  23.3775        1
 30 | -29.9424  19.0011 -17.6059        1
 31 | -6.40524 -14.3186 -44.4049        1
 32 | -14.9198 -19.9314 -49.3089        1
 33 | -34.4646 -50.4033  1.85306        1
 34 |  -61.026  6.72293 -24.5027        1
 35 | -46.3394 -31.0892 -49.5486        1
 36 |   34.065 -50.2736 -51.5004        1
 37 |   12.377 -59.7965  49.4012        1
 38 | -67.4047  12.9317  36.7503        1
 39 |  8.11728  25.0473 -76.7922        1
 40 | -21.7861 -66.7282 -55.6346        1
 41 | -53.9181 -41.6285 -70.7227        1
 42 | -17.1933  -86.088  47.7815        1
 43 |   9.5508  89.8916 -34.3158        1
 44 | -24.4567 -98.8025  5.48513        1
 45 | -0.00571954    -38.5207     -102.18           1
 46 | -20.2582 -63.7688 -96.5895        1
 47 | -49.5669 -100.967 -56.2043        1
 48 | -127.292 -15.5791  16.3008        1
 49 | 0.594919  21.9267 -132.354        1
 50 |   88.196 -100.533  13.2908        1
 51 | -111.426   7.9271  73.7154        1
 52 | -8.21455  78.3533 -110.628        1
 53 |  90.7279 -81.2332 -66.5017        1
 54 | -61.1471  26.4111  116.476        1
 55 | 99.8699  66.135 -52.412       1
 56 |  126.296  3.34301 -12.9511        1
 57 |   29.804 -64.1209  101.198        1
 58 | -29.8065  114.577 -22.6231        1
 59 |  102.623 -59.1364  9.12342        1
 60 | -114.511  34.5228 -24.3646        1
 61 |  94.8085 -41.4555 -67.6932        1
 62 | -58.6183 -15.2401  104.461        1
 63 |   83.228  27.9715 -82.7108        1
 64 |  20.6647 -95.6033  69.7172        1
 65 | -79.8646  37.4653  76.8779        1
 66 |  84.2803  3.70739 -82.3085        1
 67 | -38.5396 -25.6624  104.778        1
 68 | -38.4222  88.6926  50.2052        1
 69 | -3.17988 -15.2239 -114.434        1
 70 | -56.6642 -107.596 0.136445        1
 71 | -108.646  49.7511 -34.9712        1
 72 | -44.8726 -28.2489 -121.286        1
 73 | 0.739442 -42.2199 -132.942        1
 74 |  28.7674 -118.225  68.5853        1
 75 | -139.996  22.5528  9.95788        1
 76 | -11.6601  31.1809 -143.255        1
 77 |  85.9445 -115.953 -41.0625        1
 78 | -112.022  5.87792  97.5891        1
 79 | -59.5358  88.8028 -107.114        1
 80 |  38.7239 -79.6724 -130.509        1
 81 | -129.597 -74.5091  59.3735        1
 82 | -98.0595 -79.7021 -112.979        1
 83 |  -142.43 -93.9464  34.7182        1
 84 | -9.36533  48.5577 -171.591        1
 85 |  16.3564 -151.533  94.0147        1
 86 | -123.692  94.0693 -95.0365        1
 87 | -72.748 62.8427 -160.44       1
 88 | -32.3412 -189.771 0.233686        1
 89 | -126.222  -69.978 -139.786        1
 90 | -75.9857 -189.439 -40.2005        1
 91 | -198.824  0.22178 -78.7544        1
 92 |  21.5604  -46.016 -214.321        1
 93 | -41.0921 -169.487 -147.718        1
 94 | -216.048  64.0485 -56.0287        1
 95 | 2.46862 149.357 -179.35       1
 96 |  94.3391 -121.657 -182.637        1
 97 | -87.5318 -165.249 -162.203        1
 98 | -27.4997 -53.9353  -247.52        1
 99 |  111.648 -161.798 -170.229        1
100 | 31.4378 156.235 194.922       1
101 | 112.909 203.264 73.6891       1
102 | 71.2521 59.4612 216.883       1
103 | 41.6074 212.555  72.995       1
104 |  222.015 -38.9127 -8.49867        1
105 | 69.5848 24.0625  205.23       1
106 | -37.6137 -3.00673  209.421        1
107 | 141.536  136.79 57.2143       1
108 | 44.0781  137.72 133.057       1
109 |  124.451  119.647 -87.1011        1
110 | 73.8718 56.2096  160.07       1
111 | -112.277  125.521  68.0235        1
112 |  85.7061 -37.0881 -160.635        1
113 |  24.8986 -147.705  109.433        1
114 |  54.9236  170.401 -19.2223        1
115 |   165.57  52.1065 -26.1816        1
116 |  72.2962 -64.0416  140.653        1
117 | -49.0064  159.329 -15.0403        1
118 |  61.7493  15.4915 -158.547        1
119 | -12.0366 -137.273   101.84        1
120 | -149.078  78.6946 -42.1637        1
121 | -132.503  70.0156  -95.747        1
122 |   144.19   -18.16 -103.433        1
123 | -42.5815 -147.277 -106.437        1
124 | -108.437 -142.469  64.9057        1
125 | -80.5745  5.92335 -180.029        1
126 |  -88.085 -176.392  43.9226        1
127 | -81.8758   84.002 -169.756        1
128 |  117.077 -156.444 -76.5859        1
129 | -55.9424 -92.5337   177.54        1
130 | -122.153  103.351 -138.334        1
131 |  68.2828 -185.028 -90.2865        1
132 |  -211.16 -54.7873  -46.689        1
133 | -120.371 -51.0844 -190.673        1
134 | -7.40184 -236.023  2.47943        1
135 | -226.682  13.4662  70.4152        1
136 | -63.0061 -25.7397 -235.374        1
137 | -104.905 -224.797  33.2577        1
138 | -147.863  194.699  39.4773        1
139 |   61.297 -98.1403 -225.979        1
140 | -247.502  -66.502 -43.1458        1
141 | -160.042 -177.159 -122.419        1
142 | -51.5658 -251.678 -100.195        1
143 | -223.527  -146.13 -95.0511        1
144 |  27.4366 -282.053  45.0539        1
145 | -282.583  62.6425 -18.3539        1
146 |  163.477 -86.5304 -228.302        1
147 | -52.1792 -290.979  41.2122        1
148 | -302.125  6.22414  3.23279        1
149 |  188.056  141.727 -187.423        1
150 | 187.564 55.3282 218.149       1
151 | 80.9129 227.726 150.394       1
152 |  181.601  206.523 -48.8299        1
153 |  83.4255 -51.1556  255.231        1
154 | -31.8132  152.771  215.482        1
155 | 125.289 140.694 175.221       1
156 | 181.792 18.0275 170.481       1
157 | -12.4915  242.311  28.6754        1
158 |  118.282 -57.5632 -210.883        1
159 |  90.0746 -230.212  40.9627        1
160 | -232.074  59.7908  75.0766        1
161 |  5.75154  134.453 -214.632        1
162 |  76.4171  -217.15 -118.708        1
163 | -240.152  11.4429  99.1052        1
164 |  14.2568  154.702 -210.421        1
165 |   178.95 -58.9517  172.973        1
166 | -126.188   91.106  197.106        1
167 |   173.59 -1.62516  171.552        1
168 |  48.9913 -68.4984  223.621        1
169 |   200.83  111.715 -44.9969        1
170 |  41.0189 -11.3016   223.72        1
171 | -57.5334  199.851 -89.8543        1
172 |  222.129   8.4301 -9.04218        1
173 | -87.587 106.073 167.696       1
174 | 107.224 177.153 32.6574       1
175 | 188.196 33.2206 68.4744       1
176 | 21.4922 87.4342 172.931       1
177 |  185.089  41.1483 -17.2857        1
178 |  43.4463 -5.14522  178.388        1
179 | -19.2099  147.529  95.1915        1
180 | 162.528 49.6834 -27.757       1
181 | -121.688 -15.9126  119.239        1
182 |  23.7673  144.437 -84.5917        1
183 |  146.635 -53.9847  53.0959        1
184 | 80.1606 54.3625 122.951       1
185 |  90.9186 -49.5817  110.194        1
186 | -121.599  81.0743  34.3589        1
187 |  90.7776 -56.6367 -110.359        1
188 |  45.2764 -110.054  94.8107        1
189 | -113.271 -56.6424  86.4336        1
190 | -116.759  35.1597 -101.823        1
191 |  88.9203 -133.384  4.96042        1
192 | -155.768 -49.5112 -32.6051        1
193 |  1.35325 -2.28392  -172.56        1
194 | -15.3278  -36.867  163.234        1
195 | -162.079  31.3262  37.9412        1
196 |  14.6876  -108.77 -138.563        1
197 |  -81.572 -52.5257 -157.559        1
198 | -8.74991 -4.36825 -190.973        1
199 |  20.8366 -194.316  4.44084        1
200 | -117.036  44.9032 -157.476        1
201 | 


--------------------------------------------------------------------------------
/RelativePoseSolver/data/vicon.txt:
--------------------------------------------------------------------------------
  1 | -1.49012e-08 -5.96046e-08            0            1
  2 | -0.000370181  0.000397434 -0.000194739            1
  3 |  -0.00068722  0.000717093 -0.000298369            1
  4 | -0.000983777   0.00109616 -0.000320362            1
  5 |  -0.00123761   0.00140451 -0.000298715            1
  6 | -0.00151305    0.001644  -0.0002878           1
  7 |  -0.00175677   0.00179297 -0.000305432            1
  8 |   -0.0022256    0.0019669 -0.000237763            1
  9 |  -0.00266913   0.00213424 -0.000136031            1
 10 |   -0.0031365   0.00230612 -9.10505e-05            1
 11 |  -0.00369898   0.00269528 -0.000121751            1
 12 |  -0.00414958   0.00310498 -0.000156705            1
 13 | -0.00467215  0.00358128 -0.00028166           1
 14 |  -0.00552048   0.00438153 -0.000517339            1
 15 |   -0.0063214   0.00518009 -0.000761694            1
 16 |  -0.00731664   0.00616789 -0.000924383            1
 17 | -0.00892652  0.00799324 -0.00122086           1
 18 |  -0.0102209  0.00942952 -0.00139541           1
 19 |  -0.0114334   0.0109007 -0.00151713           1
 20 |  -0.0129726   0.0130626 -0.00166782           1
 21 |  -0.0139742   0.0150035 -0.00192504           1
 22 |  -0.0147807   0.0171359 -0.00209898           1
 23 | -0.0155469  0.0198692 -0.0022163          1
 24 |  -0.0158297   0.0218013 -0.00212337           1
 25 |  -0.0158097   0.0239452 -0.00154788           1
 26 | -0.0156321  0.0255318 -0.0010623          1
 27 |   -0.0153792     0.026777 -0.000413776            1
 28 |  -0.0148375   0.0285947 0.000528144           1
 29 | -0.0144015   0.029996 0.00124261          1
 30 |  -0.013954  0.0314837 0.00197879          1
 31 | -0.0132413  0.0337234 0.00296009          1
 32 |  -0.012673  0.0354964 0.00377668          1
 33 | -0.0120378  0.0371702 0.00466026          1
 34 | -0.0110795   0.039527 0.00586061          1
 35 | -0.0103442  0.0414495  0.0067595          1
 36 | -0.00946757   0.0433722  0.00772693           1
 37 | -0.00795565   0.0457565  0.00904616           1
 38 | -0.00651309   0.0476909  0.00996425           1
 39 | -0.00481348   0.0495337   0.0107504           1
 40 | -0.00214703   0.0519944   0.0115124           1
 41 | 0.000193682     0.05356    0.012019           1
 42 | 0.00297586  0.0552409  0.0121333          1
 43 | 0.00708203  0.0568015  0.0121213          1
 44 | 0.0104842  0.057778 0.0119573         1
 45 | 0.0152431 0.0588509 0.0111732         1
 46 | 0.0188953 0.0596016 0.0105444         1
 47 |  0.0225738  0.0600288 0.00973119          1
 48 |  0.0274888  0.0604672 0.00827422          1
 49 |  0.0310279  0.0604893 0.00709422          1
 50 | 0.0344837 0.0603246 0.0059948         1
 51 |  0.0389661  0.0600307 0.00422226          1
 52 |  0.0422512  0.0595719 0.00270793          1
 53 |   0.0454517   0.0589956 0.000994653           1
 54 |   0.0495525   0.0579859 -0.00134376           1
 55 |   0.0526256   0.0571434 -0.00317632           1
 56 |    0.055622   0.0560681 -0.00485377           1
 57 |   0.0594077   0.0543858 -0.00722384           1
 58 |   0.0622636   0.0529399 -0.00891871           1
 59 |  0.0649869  0.0513223 -0.0105486          1
 60 |  0.0685985   0.049052 -0.0127896          1
 61 |  0.0713007  0.0472331 -0.0145549          1
 62 |  0.0749822  0.0446883 -0.0167558          1
 63 |  0.0777063  0.0428387 -0.0184974          1
 64 |  0.0805029  0.0408573 -0.0200584          1
 65 |  0.0842453  0.0381953 -0.0220247          1
 66 |  0.0870409  0.0361426 -0.0233585          1
 67 |   0.089854  0.0340229 -0.0246405          1
 68 |  0.0936789  0.0311683 -0.0262691          1
 69 |  0.0965987  0.0289126 -0.0275108          1
 70 |  0.0995261  0.0266561 -0.0285774          1
 71 |   0.103302  0.0237879 -0.0299083          1
 72 |   0.106034  0.0217341 -0.0307923          1
 73 |  0.108793 0.0196488 -0.031557         1
 74 |   0.112375  0.0169748 -0.0325621          1
 75 |    0.11498  0.0149478 -0.0333044          1
 76 |    0.11751  0.0129067 -0.0338444          1
 77 |   0.120721  0.0102926 -0.0344161          1
 78 |   0.123085 0.00840915 -0.0347023          1
 79 |   0.125482 0.00658921 -0.0351207          1
 80 |   0.128541 0.00419672 -0.0354579          1
 81 |   0.130722  0.0024655 -0.0357706          1
 82 |    0.133594 3.83567e-05  -0.0359994           1
 83 |     0.13575 -0.00178325  -0.0361707           1
 84 |    0.137968 -0.00361711   -0.036174           1
 85 |    0.140844 -0.00609132    -0.03633           1
 86 |    0.142932 -0.00801242  -0.0363077           1
 87 |    0.144959 -0.00986337   -0.036249           1
 88 |   0.147578 -0.0122553 -0.0357433          1
 89 |   0.149308 -0.0140614 -0.0353152          1
 90 |   0.150867 -0.0156731 -0.0347007          1
 91 |   0.152745 -0.0177511 -0.0335719          1
 92 |   0.153943  -0.019305 -0.0325964          1
 93 |   0.155119 -0.0207541 -0.0314719          1
 94 |   0.156504 -0.0225774 -0.0299145          1
 95 |     0.1575 -0.0239785 -0.0285818          1
 96 |   0.158274 -0.0253248 -0.0271625          1
 97 |   0.159113 -0.0269462 -0.0251582          1
 98 |   0.159657 -0.0281553 -0.0237118          1
 99 |   0.160098 -0.0294784 -0.0215582          1
100 |   0.160194 -0.0303822 -0.0199292          1
101 |    0.16006 -0.0310687 -0.0182239          1
102 |  0.159714 -0.031699  -0.01606         1
103 |    0.15923 -0.0319599 -0.0144426          1
104 |   0.158544  -0.032068 -0.0129034          1
105 |   0.157234 -0.0319014 -0.0109018          1
106 |    0.155965  -0.0317046 -0.00934224           1
107 |    0.154525    -0.03136 -0.00793803           1
108 |    0.152354  -0.0304991 -0.00619868           1
109 |    0.150537  -0.0295912 -0.00506823           1
110 |    0.148567  -0.0284174 -0.00398774           1
111 |     0.14572  -0.0265788 -0.00295414           1
112 |    0.143456  -0.0249266 -0.00213215           1
113 |    0.140913  -0.0230241 -0.00142124           1
114 |     0.137357   -0.0204734 -0.000392542            1
115 |   0.134558 -0.0183593 0.00031993          1
116 |    0.131563  -0.0162077 0.000974304           1
117 |   0.127461 -0.0132313 0.00177812          1
118 |  0.124356 -0.010958 0.0022988         1
119 |     0.12015 -0.00791493  0.00277105           1
120 |    0.116938 -0.00568834  0.00296738           1
121 |    0.113693 -0.00352774   0.0032655           1
122 |       0.1093 -0.000793181   0.00357576            1
123 |   0.105934 0.00125003 0.00379257          1
124 |   0.102605  0.0032601 0.00404041          1
125 |   0.098212 0.00588557 0.00430428          1
126 |  0.0949229 0.00780046 0.00440019          1
127 |    0.09153 0.00971595 0.00448799          1
128 |  0.0870142  0.0119696 0.00497288          1
129 |  0.0835627  0.0137122 0.00525355          1
130 | 0.0800842 0.0153465 0.0056813         1
131 |  0.0754063  0.0173975 0.00612363          1
132 |  0.0719765  0.0187945 0.00652789          1
133 |  0.0684302  0.0201981 0.00687393          1
134 |  0.0635684  0.0220699 0.00708687          1
135 |  0.0599098  0.0233208 0.00725624          1
136 |  0.0550031  0.0248073 0.00748153          1
137 |  0.0513334  0.0258964 0.00751744          1
138 |  0.0476653  0.0269302 0.00749702          1
139 |  0.0427982  0.0281915 0.00728659          1
140 |  0.0391613  0.0292651 0.00699688          1
141 |  0.0354635  0.0302769 0.00673329          1
142 |  0.0304642  0.0314841 0.00636108          1
143 |  0.0267204  0.0323446 0.00623161          1
144 |  0.0229891  0.0330235 0.00596644          1
145 |   0.018241  0.0340293 0.00550288          1
146 |  0.0147888  0.0344419 0.00515499          1
147 |  0.0114994  0.0351668 0.00451132          1
148 | 0.00737272  0.0356637 0.00377235          1
149 | 0.00442021  0.0359128 0.00308919          1
150 | 0.00162928  0.0361227 0.00241553          1
151 | -0.00169249   0.0362642   0.0016572           1
152 | -0.0040487  0.0361642 0.00113906          1
153 | -0.00693922   0.0360491 0.000236724           1
154 |  -0.00890525     0.035808 -0.000330199            1
155 |  -0.0107831   0.0356451 -0.00100637           1
156 | -0.0129509      0.035 -0.0019043          1
157 |   -0.014282   0.0343755 -0.00276154           1
158 |  -0.0153985   0.0337653 -0.00388942           1
159 | -0.0167213  0.0325211 -0.0054381          1
160 |  -0.0175714   0.0314977 -0.00678364           1
161 |  -0.0183708   0.0302681 -0.00815052           1
162 | -0.0193538  0.0285879 -0.0103551          1
163 | -0.0200874  0.0273473 -0.0122532          1
164 | -0.0208483  0.0259952 -0.0142371          1
165 | -0.0216865  0.0239016 -0.0170146          1
166 | -0.0222929  0.0223286 -0.0192099          1
167 | -0.0227477  0.0204885 -0.0215108          1
168 | -0.0232501  0.0179282  -0.024678          1
169 | -0.0236284   0.015677  -0.027015          1
170 | -0.0241163  0.0133716 -0.0293544          1
171 | -0.0247724  0.0102515  -0.032456          1
172 | -0.0254031 0.00782505 -0.0348244          1
173 | -0.0262428 0.00450602 -0.0380447          1
174 | -0.0270169 0.00205649 -0.0405489          1
175 |   -0.027855 -0.00047174  -0.0430997           1
176 |  -0.0290243 -0.00395784  -0.0465829           1
177 |  -0.0298175 -0.00660999  -0.0492438           1
178 |  -0.0305396 -0.00940444  -0.0518839           1
179 | -0.0313782 -0.0131319 -0.0554129          1
180 | -0.0320878 -0.0160119 -0.0580194          1
181 | -0.0327147 -0.0189706 -0.0605341          1
182 |  -0.033421 -0.0230235 -0.0638942          1
183 | -0.0339515 -0.0259648 -0.0666769          1
184 |  -0.034364 -0.0289229 -0.0694966          1
185 | -0.0348277 -0.0329709 -0.0732536          1
186 |  -0.035076   -0.03602 -0.0762541          1
187 | -0.0353158 -0.0389882 -0.0792078          1
188 | -0.0356139 -0.0430465 -0.0830271          1
189 | -0.0356778 -0.0460423 -0.0861435          1
190 | -0.0354533 -0.0501884  -0.090207          1
191 | -0.0351483 -0.0532716 -0.0933863          1
192 | -0.0347394 -0.0564389 -0.0965271          1
193 | -0.0340482 -0.0607929  -0.100744          1
194 | -0.0333849 -0.0641896  -0.103767          1
195 | -0.0327745 -0.0674502  -0.106865          1
196 | -0.0318042 -0.0719195  -0.110833          1
197 | -0.0310273 -0.0752556   -0.11361          1
198 | -0.0302254 -0.0784471  -0.116477          1
199 | -0.0291225 -0.0826636  -0.120065          1
200 | -0.0281031 -0.0857634  -0.122789          1
201 | 


--------------------------------------------------------------------------------
/RelativePoseSolver/relative_pose_solver.h:
--------------------------------------------------------------------------------
  1 | #ifndef RELATIVE_POSE_SOLVER_H
  2 | #define RELATIVE_POSE_SOLVER_H
  3 | 
  4 | #include <assert.h>
  5 | #include <Eigen/Geometry>
  6 | #include <Eigen/Eigenvalues>
  7 | #include<Eigen/Dense>
  8 | 
  9 | #include <vector>
 10 | #include <map>
 11 | 
 12 | #include <iostream>
 13 | 
 14 | static Eigen::Matrix3d orthonormalize(const Eigen::Matrix3d & A)
 15 | {
 16 |     Eigen::JacobiSVD<Eigen::Matrix3d> svdA(A, Eigen::ComputeFullU|Eigen::ComputeFullV);
 17 |     
 18 | //    std::cout<<"me U"<<std::endl;
 19 | //    std::cout<<-svdA.matrixU()<<std::endl<<std::endl;
 20 | //    std::cout<<"me VT"<<std::endl;
 21 | //    std::cout<<-(svdA.matrixV().transpose())<<std::endl<<std::endl;
 22 | //    std::cout<<"me diag"<<std::endl;
 23 | //    std::cout<<svdA.singularValues().transpose()<<std::endl<<std::endl;
 24 |     
 25 | 	return (svdA.matrixU())*(svdA.matrixV().transpose());
 26 | }
 27 | 
 28 | 
 29 | static Eigen::Isometry3d getRelativeTransformation(
 30 |         const std::vector<Eigen::Matrix4d>& pose1,
 31 |         const std::vector<Eigen::Matrix4d>& pose2)
 32 | {
 33 |     assert(pose1.size()==pose2.size());
 34 | 	assert(pose1.size()>0);
 35 |         
 36 |     Eigen::MatrixXd A(16*(pose1.size()*(pose1.size()-1) / 2), 16);
 37 |     A.setZero();
 38 |     
 39 |     int counter = 0;
 40 |     
 41 |     for (unsigned int i=0; i<pose1.size(); i++) 
 42 |         for (unsigned int j=i+1; j<pose1.size(); j++) {
 43 | 		Eigen::Matrix4d A1 = pose2[i].inverse();
 44 |         Eigen::Matrix4d C1 = pose1[i];
 45 |         Eigen::Matrix4d A2 = pose2[j].inverse();
 46 | 		Eigen::Matrix4d C2 = pose1[j];
 47 |                 
 48 | 		for (int ro=0; ro<4; ro++) for (int co=0; co<4; co++) {
 49 | 			for (int r=0; r<4; r++) for (int c=0; c<4; c++) 
 50 | 				A(counter,c+4*r) = A1(ro,r) * C1(c,co) - A2(ro,r) * C2(c,co);
 51 | 			counter++;
 52 | 		}
 53 | 	}
 54 | 
 55 |     //std::cout<<"me A"<<std::endl<<A<<std::endl<<std::endl;    
 56 |     
 57 |     // find column of V with smalest singular value
 58 |     Eigen::JacobiSVD<Eigen::MatrixXd> svdA(A, Eigen::ComputeThinV);
 59 |     Eigen::VectorXd diag = svdA.singularValues();
 60 |    
 61 |    // std::cout<<"me diag:"<<std::endl<<svdA.singularValues().transpose()<<std::endl<<std::endl;
 62 |     
 63 |     //std::cout<<"me V:"<<std::endl<<svdA.matrixV()<<std::endl<<std::endl;
 64 |     //std::cout<<"me V:"<<std::endl<<svdA.matrixV()<<std::endl<<std::endl;
 65 |     
 66 |     int min = 0;
 67 | 	for (int i=1; i<16; i++) if (diag[i]<diag[min]) min=i;
 68 |     Eigen::VectorXd v = svdA.matrixV().col(min); 
 69 |     
 70 |     //std::cout<<"me v"<<std::endl<<v.transpose()<<std::endl<<std::endl;    
 71 |     
 72 |     
 73 |     // extract R ant t from vector v
 74 |     Eigen::Matrix3d Rrel;
 75 |     
 76 |     Rrel.block<1,3>(0,0) = v.segment<3>(0);
 77 |     Rrel.block<1,3>(1,0) = v.segment<3>(4);
 78 |     Rrel.block<1,3>(2,0) = v.segment<3>(8);
 79 |     Eigen::Vector3d trel;
 80 |     trel[0] = v[3];
 81 | 	trel[1] = v[7];
 82 | 	trel[2] = v[11];
 83 | 
 84 |    // std::cout<<"me R"<<std::endl<<Rrel<<std::endl<<std::endl;    
 85 |     
 86 |     // fix the scaling
 87 | 	double scale = (sqrt(Rrel.row(0).dot(Rrel.row(0))) + sqrt(Rrel.row(1).dot(Rrel.row(1))) + sqrt(Rrel.row(2).dot(Rrel.row(2))) + fabs(v[15]))/4;
 88 | 	Rrel *= 1.0/scale;
 89 | 	trel *= 1.0/scale;
 90 |     
 91 |     // orthonormalize and take care of negative scaling
 92 | 	Rrel = orthonormalize(Rrel);
 93 | 	if ((Rrel.row(0).cross(Rrel.row(1))).dot(Rrel.row(2)) < 0.0) {
 94 | 		Rrel = -Rrel;
 95 | 		trel = -trel;
 96 | 	}
 97 |     
 98 |     //std::cout<<"me T"<<std::endl<<trel.transpose()<<std::endl<<std::endl;    
 99 |     
100 |     Eigen::Isometry3d RSE3;
101 |     RSE3.setIdentity();
102 |     RSE3.translate(trel);
103 |     RSE3.rotate(Rrel);
104 |     
105 |     return RSE3;  
106 | }
107 | 
108 | #endif // RELATIVE_POSE_SOLVER_H
109 | 


--------------------------------------------------------------------------------
/RelativePoseSolver/solve_relative_pose.cpp:
--------------------------------------------------------------------------------
 1 | #include "relative_pose_solver.h"
 2 | 
 3 | #include <vector>
 4 | #include <iostream>
 5 | #include <fstream>
 6 | #include <stdio.h>
 7 | 
 8 | using namespace  std;
 9 | using namespace Eigen;
10 | 
11 | 
12 | 
13 | int main(int argc, char **argv){
14 |         
15 |     char name[200] = "/home/carl/Work/Code/github/absolute_orientation/data/output.txt";
16 |     
17 |     vector<Matrix4d> itm_pose_list;
18 |     vector<Matrix4d> vicon_pose_list;
19 |     
20 |     vector<Matrix4d> warped_pose_list;
21 |     
22 |     Matrix4d fliping; fliping.setZero();
23 |     fliping(0,0) = -1;fliping(1,1) = 1;fliping(2,2) = 1;fliping(3,3) = 1;
24 |     cout<<fliping;
25 |     
26 |     
27 |     ifstream ifs(name); string str;
28 |     
29 |     for (int i=0;i<200;i++)
30 |     {
31 |         if(ifs.eof()) break;
32 |         
33 |         Matrix4d tmpMatrix;        
34 |         for (int i=0;i<4;i++)
35 |         {
36 |             Vector4d tmp_vec;
37 |             ifs>>tmp_vec[0]>>str>>tmp_vec[1]>>str>>tmp_vec[2]>>str>>tmp_vec[3];
38 |             tmpMatrix.row(i) = tmp_vec;
39 |         }
40 |         itm_pose_list.push_back(tmpMatrix);
41 |         
42 |         for (int i=0;i<4;i++)
43 |         {
44 |             Vector4d tmp_vec;
45 |             ifs>>tmp_vec[0]>>str>>tmp_vec[1]>>str>>tmp_vec[2]>>str>>tmp_vec[3];
46 |             tmpMatrix.row(i) = tmp_vec;
47 |         }
48 |         vicon_pose_list.push_back(fliping*tmpMatrix);
49 |         ifs>>str;
50 |     }
51 |     ifs.close();
52 |     
53 |     Eigen::Isometry3d result_relative_pose = getRelativeTransformation(vicon_pose_list,itm_pose_list);
54 |     cout <<"my result solved:"<< endl <<result_relative_pose.matrix()<< endl<<endl;
55 |     
56 |     ofstream ofs1("/home/carl/Work/Code/github/absolute_orientation/data/aligned.txt");
57 |     ofstream ofs2("/home/carl/Work/Code/github/absolute_orientation/data/vicon.txt");
58 |     ofstream ofs3("/home/carl/Work/Code/github/absolute_orientation/data/itm.txt");
59 |     
60 |     for (int i=0;i<200;i++){
61 |         
62 |         warped_pose_list.push_back(result_relative_pose.matrix()*vicon_pose_list[i]);
63 |         
64 |         ofs1<<warped_pose_list[i].inverse().col(3).transpose()<<endl;
65 |         ofs2<<vicon_pose_list[i].inverse().col(3).transpose()<<endl;
66 |         ofs3<<itm_pose_list[i].inverse().col(3).transpose()<<endl;
67 |     }
68 |     
69 |     ofs1.close();
70 |     ofs2.close();
71 |     ofs3.close();
72 |     
73 |     return 0;
74 | }
75 | 


--------------------------------------------------------------------------------
/RelativePoseSolver/test_absolute_orientation.cpp:
--------------------------------------------------------------------------------
 1 | #include "absolute_orientation_horn.h"
 2 | #include "relative_pose_solver.h"
 3 | #include <iostream>
 4 | 
 5 | using namespace std;
 6 | 
 7 | int main()
 8 | {
 9 |     const int no_points=200;
10 |     
11 |     float tx, ty, tz;
12 |     float theta=0, r=2;
13 |     
14 |     Eigen::Quaterniond random_rotation(sqrt(0.1),sqrt(0.2),sqrt(0.3),sqrt(0.4));
15 |     Eigen::Vector3d random_translation(rand(),rand(),rand());
16 |     Eigen::Isometry3d random_pose;
17 |     random_pose.setIdentity();
18 |     random_pose.rotate(random_rotation);
19 |     random_pose.translate(random_translation);
20 |     Eigen::Isometry3d result_pose;
21 |     
22 |     Eigen::MatrixXd pts1(3,no_points);
23 |     Eigen::MatrixXd pts2(3,no_points);
24 |     
25 |     for (int i=0;i<no_points;i++)
26 |     {
27 |             tx = r*sin(theta);
28 |             ty = r*cos(theta);
29 |             tz = r;
30 |             r += 0.1;
31 |             
32 |             Eigen::Vector3d pt(tx,ty,tz);
33 |             pts1.col(i) = pt;
34 |             pts2.col(i) = random_pose.rotation()*pt + random_pose.translation();
35 |     }
36 |     
37 |     absolute_orientation_horn(pts1,pts2,&result_pose);
38 |     
39 |     cout<<"relative matrix:"<< endl << random_pose.matrix() << endl<<endl;
40 |     cout <<"result solved:"<< endl << result_pose.matrix() << endl<<endl;
41 |     
42 |     float error_sum=0;
43 |     for (int i=0;i<no_points;i++)
44 |     {
45 |         Eigen::Vector3d diff_pt = (result_pose.rotation() * pts1.col(i) + result_pose.translation() )- pts2.col(i);
46 |         error_sum += sqrt(diff_pt.dot(diff_pt));
47 |     }
48 |     
49 |     cout<<"average error: "<<error_sum/no_points<<endl;
50 |     
51 |     return 0;
52 | }
53 | 
54 | 


--------------------------------------------------------------------------------
/RelativePoseSolver/test_relative_pose.cpp:
--------------------------------------------------------------------------------
  1 | #include "relative_pose_solver.h"
  2 | 
  3 | #include <vector>
  4 | #include <iostream>
  5 | #include <fstream>
  6 | //#include"CalibStereo.h"
  7 | 
  8 | using namespace std;
  9 | using namespace Eigen;
 10 | 
 11 | Eigen::Quaterniond randQuaternion()
 12 | {
 13 |     Eigen::Vector4d v;
 14 |     float sum=0;
 15 |     for (int i=0;i<4;i++) {
 16 |         v(i) = rand();
 17 |         sum += v(i)*v(i);
 18 |     }
 19 |     
 20 |     for (int i=0;i<4;i++)
 21 |         v(i) /=sqrt(sum);
 22 |     
 23 |     return Eigen::Quaterniond(v);
 24 | }
 25 | 
 26 | 
 27 | int main()
 28 | {
 29 |     const int no_poses=200;
 30 |     
 31 |     Eigen::Quaterniond d_rotation(sqrt(0.1),sqrt(0.2),sqrt(0.3),sqrt(0.4));
 32 |     Eigen::Vector3d d_translation(4,5,6);
 33 |     Eigen::Isometry3d pose;
 34 |     pose.setIdentity();
 35 | 
 36 |     Eigen::Quaterniond relative_rotation(sqrt(0.4),sqrt(0.3),sqrt(0.2),sqrt(0.1));
 37 |     Eigen::Vector3d relative_translation(1,2,3);
 38 |     Eigen::Isometry3d relative_pose;
 39 |     relative_pose.setIdentity();
 40 |     relative_pose.translate(relative_translation);
 41 |     relative_pose.rotate(relative_rotation);
 42 |     
 43 |     vector<Eigen::Matrix4d> pose1,pose2;
 44 |     
 45 | //    vector<TooN::SE3<double> > olaf_pose1, olaf_pose2;
 46 |     
 47 |     for (int i=0;i<no_poses;i++)
 48 |     {
 49 |         Eigen::Matrix4d new_pose = relative_pose.matrix()*pose.matrix();
 50 |        
 51 |         pose1.push_back(pose.matrix());
 52 |         pose2.push_back(new_pose);
 53 |         
 54 | //        TooN::Matrix<3,3> o_R1, o_R2;
 55 |        
 56 | //        for(int r=0;r<3;r++) for(int c=0;c<3;c++) 
 57 | //        {
 58 | //            o_R1[r][c] = pose.matrix()(r,c);
 59 | //            o_R2[r][c] = new_pose(r,c);
 60 | //        }
 61 | //        TooN::SO3<double> o_SO3_1(o_R1);
 62 | //        TooN::SO3<double> o_SO3_2(o_R2);
 63 |         
 64 |         
 65 | //        TooN::Vector<3> o_T1, o_T2;
 66 | //        for (int r=0;r<3;r++)
 67 | //        {
 68 | //            o_T1[r] = pose.matrix()(r,3);
 69 | //            o_T2[r] = new_pose(r,3);
 70 | //        }
 71 |         
 72 |         
 73 | //        TooN::SE3<double> se3_1(o_SO3_1,o_T1);
 74 | //        TooN::SE3<double> se3_2(o_SO3_2,o_T2);
 75 |         
 76 | //        olaf_pose1.push_back(se3_1);
 77 | //        olaf_pose2.push_back(se3_2);
 78 |         
 79 |         pose.rotate(randQuaternion());
 80 |         pose.translate(d_translation);
 81 |         
 82 |     }
 83 |         
 84 |     Eigen::Isometry3d result_relative_pose = getRelativeTransformation(pose1, pose2);
 85 | //    TooN::SE3<double> olaf_result_pose =  CalibStereo::getRelativePose_differentFrames(olaf_pose1,olaf_pose2);
 86 |     
 87 |     cout<<"relative matrix:"<< endl << relative_pose.matrix() << endl<<endl;
 88 |     cout <<"my result solved:"<< endl <<result_relative_pose.matrix()<< endl<<endl;
 89 | //    cout <<"olaf result solved:"<< endl <<olaf_result_pose<< endl<<endl;
 90 |     
 91 |     ofstream ofs1("/home/carl/Work/Code/github/absolute_orientation/data/aligned.txt");
 92 |     ofstream ofs2("/home/carl/Work/Code/github/absolute_orientation/data/pose1.txt");
 93 |     ofstream ofs3("/home/carl/Work/Code/github/absolute_orientation/data/pose2.txt");
 94 |     
 95 |     vector<Matrix4d> warped_pose_list;
 96 |     
 97 |     for (int i=0;i<no_poses;i++){
 98 |         
 99 |         warped_pose_list.push_back(relative_pose.matrix() * pose1[i]);
100 |         
101 |         ofs1<<warped_pose_list[i].inverse().col(3).transpose()<<endl;
102 |         ofs2<<pose1[i].inverse().col(3).transpose()<<endl;
103 |         ofs3<<pose2[i].inverse().col(3).transpose()<<endl;
104 |     }
105 |     
106 |     ofs1.close();
107 |     ofs2.close();
108 |     ofs3.close();
109 |     
110 |     return 0;
111 | }
112 | 
113 | 


--------------------------------------------------------------------------------
/pwp/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8)
 2 | project(PWP)
 3 | 
 4 | find_package(OpenCV REQUIRED)
 5 | include_directories(${OpenCV_INCLUDE_DIRS})
 6 | # include_directories("/home/carl/Work/3rdparty")
 7 | # include_directories("/home/carl/Work/3rdparty/Eigen3")
 8 | 
 9 | find_package( PkgConfig )
10 | pkg_check_modules( EIGEN3 REQUIRED eigen3 )
11 | include_directories( ${EIGEN3_INCLUDE_DIRS} )
12 | 
13 | add_executable(PWP
14 | pwp/Histogram.hpp
15 | pwp/PWPTracker.hpp
16 | pwp/PWPTracker.cpp
17 | main.cpp)
18 | 
19 | target_link_libraries(PWP ${OpenCV_LIBS})
20 | 
21 | 
22 | 


--------------------------------------------------------------------------------
/pwp/README.md:
--------------------------------------------------------------------------------
 1 | # Charles Bibby's PWP Tracker
 2 | 
 3 | ## Build:
 4 | 
 5 | ```
 6 | mkdir build
 7 | cd build
 8 | cmake ..
 9 | make -j$(nproc)
10 | ```
11 | 
12 | ## Run
13 | ```
14 | ./PWP ../data/hand.avi
15 | ```
16 | 
17 | - left click to select top-left corner of bounding box
18 | - right click to select right-bottom corner
19 | - `p` to start tracking


--------------------------------------------------------------------------------
/pwp/data/hand.avi:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/carlren/VisionTools/d14d4049989b1c1f99ab3d1b8c85a20ae06c16e1/pwp/data/hand.avi


--------------------------------------------------------------------------------
/pwp/main.cpp:
--------------------------------------------------------------------------------
  1 | //
  2 | //  main.cpp
  3 | //  pwpLib
  4 | //
  5 | //  Created by Carl Ren on 03/05/2015.
  6 | //  Copyright (c) 2015 Carl Ren. All rights reserved.
  7 | //
  8 | #include "pwp/PWPTracker.hpp"
  9 | #include <iostream>
 10 | 
 11 | 
 12 | using namespace std;
 13 | using namespace cv;
 14 | 
 15 | ////////////////////////////////////////////////////////////////////////////////
 16 | struct MouseState
 17 | {
 18 |     int x;
 19 |     int y;
 20 |     
 21 |     int event;
 22 | };
 23 | 
 24 | ////////////////////////////////////////////////////////////////////////////////
 25 | static MouseState mouse_state;
 26 | 
 27 | ////////////////////////////////////////////////////////////////////////////////
 28 | void mouseCallback (int event, int x, int y, int flags, void* userdata)
 29 | {
 30 |     mouse_state.x = x;
 31 |     mouse_state.y = y;
 32 |     mouse_state.event = event;
 33 | }
 34 | 
 35 | ////////////////////////////////////////////////////////////////////////////////
 36 | int main(int argc, char** argv)
 37 | {
 38 |     if (argc!=2){
 39 |         
 40 |         cerr<<"useage: ./PWP <video name>"<<endl;
 41 |         return -1;
 42 |     }
 43 |         
 44 |     
 45 |     VideoCapture cap(argv[1]);
 46 |     if (!cap.isOpened())
 47 |     {
 48 |         cerr << "no video cap!\n"<<endl;
 49 |         return -1;
 50 |     }
 51 |     
 52 |     
 53 |     PWPTracker *mytracker = new PWPTracker();
 54 |     namedWindow("frame", WINDOW_AUTOSIZE | CV_GUI_NORMAL);
 55 |     
 56 |     Mat oldFrame;
 57 |     Mat frame;
 58 | 	Mat targetFram;
 59 |     Size s(640, 480);
 60 |     cap.read(oldFrame);
 61 |     
 62 |     if (!cap.read(oldFrame)) {
 63 |         cout<<"cant grab frame"<<endl;
 64 |         return -1;
 65 |     }
 66 |     
 67 |     resize(oldFrame, frame, s);
 68 |     
 69 |     cv::imshow ("frame", frame);
 70 |     cv::setMouseCallback ("frame", mouseCallback, NULL);
 71 | 
 72 |     cv::Point upper_left_point (0,0);
 73 |     cv::Point lower_right_point (0,0);
 74 |     bool bounding_box_set = false;
 75 |     while (!bounding_box_set)
 76 |     {
 77 |         cv::Mat tmp_frame = frame.clone ();
 78 |         
 79 |         cv::circle (tmp_frame, cv::Point (mouse_state.x, mouse_state.y), 5, CV_RGB (0, 0, 255), 1);
 80 |         
 81 |         if (mouse_state.event == cv::EVENT_LBUTTONDOWN)
 82 |         {
 83 |             upper_left_point.x = mouse_state.x;
 84 |             upper_left_point.y = mouse_state.y;
 85 |         }
 86 |         if (mouse_state.event == cv::EVENT_RBUTTONDOWN)
 87 |         {
 88 |             lower_right_point.x = mouse_state.x;
 89 |             lower_right_point.y = mouse_state.y;
 90 |         }
 91 |         
 92 |         cv::line (tmp_frame, upper_left_point, cv::Point (upper_left_point.x, lower_right_point.y), CV_RGB (0, 255, 0), 3);
 93 |         cv::line (tmp_frame, upper_left_point, cv::Point (lower_right_point.x, upper_left_point.y), CV_RGB (0, 255, 0), 3);
 94 |         cv::line (tmp_frame, lower_right_point, cv::Point (upper_left_point.x, lower_right_point.y), CV_RGB (0, 128, 0), 3);
 95 |         cv::line (tmp_frame, lower_right_point, cv::Point (lower_right_point.x, upper_left_point.y), CV_RGB (0, 128, 0), 3);
 96 |         
 97 |         cv::imshow ("frame", tmp_frame);
 98 |         char key = cv::waitKey (10);
 99 |         
100 |         if (key == 'p')
101 |             bounding_box_set = true;
102 |     }
103 |     
104 |     
105 |     int count = 0;
106 |      
107 | 	vector<Eigen::Vector2f> corners(4);
108 |     
109 |     while (cap.read(oldFrame))
110 |     {
111 |         resize(oldFrame, frame, s);
112 |         
113 |         if (!mytracker->HasTarget())
114 |         {
115 |             PWPTracker::PWPBoundingBox bb(upper_left_point.x, upper_left_point.y, lower_right_point.x, lower_right_point.y); // hand
116 |             mytracker->AddTarget(frame, bb);
117 |         }
118 |         
119 | 		mytracker->Process(frame);
120 | 
121 | 		mytracker->DrawTargeLSOverlay(frame);
122 | 
123 | 		mytracker->GetBoundingBox(corners);
124 | 		cv::line(frame, cvPoint(corners[0][0], corners[0][1]), cvPoint(corners[1][0], corners[1][1]), cv::Scalar(0, 0, 255), 2);
125 | 		cv::line(frame, cvPoint(corners[1][0], corners[1][1]), cvPoint(corners[2][0], corners[2][1]), cv::Scalar(0, 0, 255), 2);
126 | 		cv::line(frame, cvPoint(corners[2][0], corners[2][1]), cvPoint(corners[3][0], corners[3][1]), cv::Scalar(0, 0, 255), 2);
127 | 		cv::line(frame, cvPoint(corners[3][0], corners[3][1]), cvPoint(corners[0][0], corners[0][1]), cv::Scalar(0, 0, 255), 2);
128 |         
129 |         imshow("frame", frame);
130 |         
131 |         //char outname[200];
132 |         //sprintf_s(outname, "e:/pwp/movie/%04i.png", count);
133 |         //imwrite(outname, frame);
134 |         
135 |         count++;
136 |         
137 |         if (cvWaitKey(10) == 27)
138 |             break;
139 |     }
140 |     
141 |     cv::destroyAllWindows();
142 |     return 0;
143 | }
144 | 


--------------------------------------------------------------------------------
/pwp/pwp/Histogram.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef _HISTOGRAM_H_
  2 | #define _HISTOGRAM_H_
  3 | 
  4 | #include <math.h>
  5 | #include <algorithm>
  6 | #include <stdlib.h>
  7 | 
  8 | using namespace std;
  9 | 
 10 | class Histogram
 11 | {
 12 | 	public:
 13 | 		Histogram(const int nBitsA,const int nBitsB,const int nBitsC, const double fSigma):	
 14 | 																								m_nBinsA(int(pow(2.0,double(nBitsA)))),
 15 | 																								m_nBinsB(int(pow(2.0,double(nBitsB)))),
 16 | 																								m_nBinsC(int(pow(2.0,double(nBitsC)))),
 17 | 																								m_nShiftA(8-nBitsA),
 18 | 																								m_nShiftB(8-nBitsB),
 19 | 																								m_nShiftC(8-nBitsC),
 20 | 																								m_fSigma(fSigma)
 21 | 		{
 22 | 			double fSum = 0.0;
 23 | 			m_nSigmaLimit = (int)ceil(2.0f*m_fSigma);
 24 | 			double fSig = m_fSigma+1e-30;
 25 | 
 26 | 			for(int i=0;i<=m_nSigmaLimit;i++)
 27 | 			{
 28 | 				for(int j=0;j<=m_nSigmaLimit;j++)
 29 | 				{
 30 | 					for(int k=0;k<=m_nSigmaLimit;k++)
 31 | 					{
 32 | 						m_afWeights[i][j][k] = exp(-double(i*i+j*j+k*k)/fSig/fSig/2.0);
 33 | 						fSum += m_afWeights[i][j][k];
 34 | 					}
 35 | 				}
 36 | 			}
 37 | 
 38 | 			for(int i=0;i<=m_nSigmaLimit;i++)
 39 | 				for(int j=0;j<=m_nSigmaLimit;j++)
 40 | 					for(int k=0;k<=m_nSigmaLimit;k++)
 41 | 						m_afWeights[i][j][k] /= fSum;
 42 | 
 43 | 			Clear();
 44 | 		}
 45 | 
 46 | 		~Histogram() {}
 47 | 
 48 | 		inline double		ComputeBhattacharyyaCoeff(Histogram& cP);
 49 | 		inline double		ComputeKLDiv(Histogram& cP);
 50 | 
 51 | 		inline void			AddSample(int nA, int nB, int nC, double fWeight);
 52 | 		inline void			Normalise();
 53 | 		inline void			SetUniform();
 54 | 		inline double		GetBinVal(int nA, int nB, int nC);
 55 | 		inline double		MaxValue();
 56 | 		inline double		MinValue();
 57 | 		inline void			ScaleBy(double fScale);
 58 | 		inline void         Clear();
 59 | 		inline int			Bins()		{return m_nBinsA*m_nBinsB*m_nBinsC;}
 60 | 		inline int			NonZeroBins();
 61 | 		inline double		BinVolume()	{return 1.0f/double(m_nBinsA*m_nBinsB*m_nBinsC);}
 62 | 		inline double		CheckNorm();
 63 | 
 64 | 		inline void			BeginAddingNewSamples();
 65 | 		inline void			AddNewSample(int nA, int nB, int nC, double fWeight);
 66 | 		inline void			CommitNewSamples(double fPrior);
 67 | 
 68 | 	protected:
 69 | 		static const int MAX_BINS = 32;
 70 | 		
 71 | 		const int m_nBinsA, m_nBinsB, m_nBinsC, m_nShiftA, m_nShiftB, m_nShiftC;
 72 | 		const double m_fSigma;
 73 | 
 74 | 		int	m_nSigmaLimit;
 75 | 
 76 | 		double				m_afWeights[16][16][16];
 77 | 
 78 | 		double				m_afHistogram[MAX_BINS][MAX_BINS][MAX_BINS];
 79 | 		double				m_fHistNorm;
 80 | 		double				m_afNewHistogram[MAX_BINS][MAX_BINS][MAX_BINS];
 81 | 		double				m_fNewHistNorm;
 82 | 		double				m_fNewHistProbWeightNorm;
 83 | };
 84 | 
 85 | //----------------------------------------------------------------------
 86 | 
 87 | void Histogram::AddSample(int nA, int nB, int nC, double fWeight)
 88 | {
 89 | 	//m_afHistogram[nA >> m_nShiftA][nB >> m_nShiftB][nC >> m_nShiftC] += fWeight;
 90 | 	//m_fHistNorm += fWeight;
 91 | 
 92 | 	nA = nA>>m_nShiftA;
 93 | 	nB = nB>>m_nShiftB;
 94 | 	nC = nC>>m_nShiftC;
 95 | 
 96 | 	for(int i=max(0,nA-m_nSigmaLimit); i<=min(m_nBinsA-1,nA+m_nSigmaLimit); i++)
 97 | 	{
 98 | 		for(int j=max(0,nB-m_nSigmaLimit); j<=min(m_nBinsB-1,nB+m_nSigmaLimit); j++)
 99 | 		{
100 | 			for(int k=max(0,nC-m_nSigmaLimit); k<=min(m_nBinsC-1,nC+m_nSigmaLimit); k++)
101 | 			{
102 | 				double fW = m_afWeights[abs(i-nA)][abs(j-nB)][abs(k-nC)]*fWeight;
103 | 				m_afHistogram[i][j][k] += fW;
104 | 				m_fHistNorm += fW;
105 | 			}
106 | 		}
107 | 	}
108 | 
109 | }
110 | 
111 | //----------------------------------------------------------------------
112 | 
113 | void Histogram::Normalise()
114 | {
115 | 	if(m_fHistNorm > 0.0f)
116 | 	{
117 | 		for(int i=0; i<m_nBinsA; i++)
118 | 			for(int j=0; j<m_nBinsB; j++)
119 | 				for(int k=0; k<m_nBinsC; k++)
120 | 					if(m_afHistogram[i][j][k])
121 | 						m_afHistogram[i][j][k] /= m_fHistNorm;
122 | 
123 | 		m_fHistNorm = 1.0;
124 | 	}
125 | }
126 | 
127 | //----------------------------------------------------------------------
128 | 
129 | double Histogram::CheckNorm()
130 | {
131 | 	double fNorm = 0.0f;
132 | 	for(int i=0; i<m_nBinsA; i++)
133 | 		for(int j=0; j<m_nBinsB; j++)
134 | 			for(int k=0; k<m_nBinsC; k++)
135 | 				fNorm+=m_afHistogram[i][j][k];
136 | 	return fNorm;
137 | }
138 | 
139 | //----------------------------------------------------------------------
140 | 
141 | double Histogram::MaxValue()
142 | {
143 | 	double fMaxVal = -1e30f;
144 | 
145 | 	for(int i=0; i<m_nBinsA; i++)
146 | 	{
147 | 		for(int j=0; j<m_nBinsB; j++)
148 | 		{
149 | 			for(int k=0; k<m_nBinsC; k++)
150 | 			{
151 | 				fMaxVal = max(fMaxVal,m_afHistogram[i][j][k]);
152 | 			}
153 | 		}
154 | 	}
155 | 
156 | 	return fMaxVal;
157 | }
158 | 
159 | 
160 | //----------------------------------------------------------------------
161 | 
162 | double Histogram::MinValue()
163 | {
164 | 	double fMinVal = 1e30f;
165 | 
166 | 	for(int i=0; i<m_nBinsA; i++)
167 | 	{
168 | 		for(int j=0; j<m_nBinsB; j++)
169 | 		{
170 | 			for(int k=0; k<m_nBinsC; k++)
171 | 			{
172 | 				if(m_afHistogram[i][j][k]!=0.0f)
173 | 					fMinVal = min(fMinVal,m_afHistogram[i][j][k]);
174 | 			}
175 | 		}
176 | 	}
177 | 
178 | 	return fMinVal;
179 | }
180 | 
181 | //----------------------------------------------------------------------
182 | 
183 | void Histogram::ScaleBy(double fScale)
184 | {
185 | 	for(int i=0; i<m_nBinsA; i++)
186 | 		for(int j=0; j<m_nBinsB; j++)
187 | 			for(int k=0; k<m_nBinsC; k++)
188 | 				m_afHistogram[i][j][k] *= fScale;
189 | }
190 | 
191 | //----------------------------------------------------------------------
192 | 
193 | void Histogram::SetUniform()
194 | {
195 | 	for(int i=0; i<m_nBinsA; i++)
196 | 		for(int j=0; j<m_nBinsB; j++)
197 | 			for(int k=0; k<m_nBinsC; k++)
198 | 				m_afHistogram[i][j][k] = 1.0;
199 | 	
200 | 	m_fHistNorm = double(m_nBinsA*m_nBinsB*m_nBinsC);
201 | }
202 | 
203 | //----------------------------------------------------------------------
204 | 
205 | double Histogram::GetBinVal(int nA, int nB, int nC)
206 | {
207 | 	return m_afHistogram[nA>>m_nShiftA][nB>>m_nShiftB][nC>>m_nShiftC]/*/m_fHistNorm*/;
208 | }
209 | 
210 | //----------------------------------------------------------------------
211 | 
212 | void Histogram::Clear()
213 | {
214 | 	m_fHistNorm=0.0f;
215 | 
216 | 	for(int i=0; i<m_nBinsA; i++)
217 | 		for(int j=0; j<m_nBinsB; j++)
218 | 			for(int k=0; k<m_nBinsC; k++)
219 | 				m_afHistogram[i][j][k] = 0.0;
220 | }
221 | 
222 | //----------------------------------------------------------------------
223 | 
224 | double Histogram::ComputeBhattacharyyaCoeff(Histogram& cP)
225 | {
226 | 	double fBhatt = 0.0f;
227 | 	for(int i=0; i<m_nBinsA; i++)
228 | 		for(int j=0; j<m_nBinsB; j++)
229 | 			for(int k=0; k<m_nBinsC; k++)
230 | 				fBhatt += sqrt(m_afHistogram[i][j][k]*cP.m_afHistogram[i][j][k]);
231 | 	return fBhatt;
232 | }
233 | 
234 | //----------------------------------------------------------------------
235 | 
236 | double Histogram::ComputeKLDiv(Histogram& cP)
237 | {
238 | 	double fKLDiv = 0.0f;
239 | 	for(int i=0; i<m_nBinsA; i++)
240 | 		for(int j=0; j<m_nBinsB; j++)
241 | 			for(int k=0; k<m_nBinsC; k++)
242 | 				fKLDiv += m_afHistogram[i][j][k]*log((m_afHistogram[i][j][k]+1e-200)/(cP.m_afHistogram[i][j][k]+1e-200));
243 | 	return fKLDiv;
244 | }
245 | 
246 | //----------------------------------------------------------------------
247 | 
248 | void Histogram::BeginAddingNewSamples()
249 | {
250 | 	m_fNewHistNorm=0.0;
251 | 	m_fNewHistProbWeightNorm=0.0;
252 | 	
253 | 	for(int i=0; i<m_nBinsA; i++)
254 | 		for(int j=0; j<m_nBinsB; j++)
255 | 			for(int k=0; k<m_nBinsC; k++)
256 | 				m_afNewHistogram[i][j][k] = 0.0;
257 | }
258 | 
259 | //----------------------------------------------------------------------
260 | 
261 | void Histogram::AddNewSample(int nA, int nB, int nC, double fWeight)
262 | {
263 | 	nA = nA>>m_nShiftA;
264 | 	nB = nB>>m_nShiftB;
265 | 	nC = nC>>m_nShiftC;
266 | 
267 | 	double fW = fWeight;
268 | 	m_afNewHistogram[nA][nB][nC] += fW;
269 | 	m_fNewHistProbWeightNorm += fW;
270 | 	m_fNewHistNorm += 1.0;
271 | }
272 | 
273 | //----------------------------------------------------------------------
274 | 
275 | void Histogram::CommitNewSamples(double fPrior)
276 | {
277 | 	//fPrior *= m_fNewHistProbWeightNorm/m_fNewHistNorm;
278 | 
279 | 	// Normalise
280 | 	if(m_fNewHistProbWeightNorm > 0.0f)
281 | 	{
282 | 		for(int i=0; i<m_nBinsA; i++)
283 | 			for(int j=0; j<m_nBinsB; j++)
284 | 				for(int k=0; k<m_nBinsC; k++)
285 | 					if(m_afNewHistogram[i][j][k])
286 | 						m_afNewHistogram[i][j][k] /= m_fNewHistProbWeightNorm;
287 | 
288 | 		m_fNewHistProbWeightNorm = 1.0;
289 | 	}
290 | 
291 | 	// Marginalise over two distributions
292 | 	for(int i=0; i<m_nBinsA; i++)
293 | 		for(int j=0; j<m_nBinsB; j++)
294 | 			for(int k=0; k<m_nBinsC; k++)
295 | 					m_afHistogram[i][j][k] = m_afNewHistogram[i][j][k]*fPrior+m_afHistogram[i][j][k]*(1.0-fPrior);
296 | }
297 | 
298 | //----------------------------------------------------------------------
299 | 
300 | int Histogram::NonZeroBins()
301 | {
302 | 	int nRetVal = 0;
303 | 	for(int i=0; i<m_nBinsA; i++)
304 | 		for(int j=0; j<m_nBinsB; j++)
305 | 			for(int k=0; k<m_nBinsC; k++)
306 | 				if(m_afNewHistogram[i][j][k])
307 | 					nRetVal++;
308 |     return nRetVal;
309 | }
310 | 
311 | //----------------------------------------------------------------------
312 | //----------------------------------------------------------------------
313 | //----------------------------------------------------------------------
314 | //----------------------------------------------------------------------
315 | 
316 | 
317 | class GreyScaleHistogram
318 | {
319 | 	public:
320 | 		GreyScaleHistogram(const int nBitsA,const double fSigma):	
321 | 																								m_nBinsA(int(pow(2.0,double(nBitsA)))),
322 | 																								m_nShiftA(8-nBitsA),
323 | 																								m_fSigma(fSigma)
324 | 		{
325 | 			double fSum = 0.0;
326 | 			m_nSigmaLimit = (int)ceil(2.0f*m_fSigma);
327 | 			double fSig = m_fSigma+1e-30;
328 | 
329 | 			for(int i=0;i<=m_nSigmaLimit;i++)
330 | 			{
331 | 				m_afWeights[i] = exp(-double(i*i)/fSig/fSig/2.0);
332 | 				fSum += m_afWeights[i];
333 | 			}
334 | 
335 | 			for(int i=0;i<=m_nSigmaLimit;i++)
336 | 						m_afWeights[i] /= fSum;
337 | 
338 | 			Clear();
339 | 		}
340 | 
341 | 		~GreyScaleHistogram() {}
342 | 
343 | 		inline double		ComputeBhattacharyyaCoeff(GreyScaleHistogram& cP);
344 | 		inline double		ComputeKLDiv(GreyScaleHistogram& cP);
345 | 
346 | 		inline void			AddSample(int nA, double fWeight);
347 | 		inline void			Normalise();
348 | 		inline void			SetUniform();
349 | 		inline double		GetBinVal(int nA);
350 | 		inline double		MaxValue();
351 | 		inline double		MinValue();
352 | 		inline void			ScaleBy(double fScale);
353 | 		inline void         Clear();
354 | 		inline int			Bins()		{return m_nBinsA;}
355 | 		inline int			NonZeroBins();
356 | 		inline double		BinVolume()	{return 1.0f/double(m_nBinsA);}
357 | 		inline double		CheckNorm();
358 | 
359 | 		inline void			BeginAddingNewSamples();
360 | 		inline void			AddNewSample(int nA, double fWeight);
361 | 		inline void			CommitNewSamples(double fPrior);
362 | 
363 | 	protected:
364 | 		static const int MAX_BINS = 256;
365 | 		
366 | 		const int m_nBinsA, m_nShiftA;
367 | 		const double m_fSigma;
368 | 
369 | 		int	m_nSigmaLimit;
370 | 
371 | 		double				m_afWeights[16];
372 | 
373 | 		double				m_afGreyScaleHistogram[MAX_BINS];
374 | 		double				m_fHistNorm;
375 | 		double				m_afNewGreyScaleHistogram[MAX_BINS];
376 | 		double				m_fNewHistNorm;
377 | 		double				m_fNewHistProbWeightNorm;
378 | };
379 | 
380 | //----------------------------------------------------------------------
381 | 
382 | void GreyScaleHistogram::AddSample(int nA, double fWeight)
383 | {
384 | 	nA = nA>>m_nShiftA;
385 | 
386 | 	for(int i=max(0,nA-m_nSigmaLimit); i<=min(m_nBinsA-1,nA+m_nSigmaLimit); i++)
387 | 	{
388 | 		double fW = m_afWeights[abs(i-nA)]*fWeight;
389 | 		m_afGreyScaleHistogram[i] += fW;
390 | 		m_fHistNorm += fW;
391 | 	}
392 | }
393 | 
394 | //----------------------------------------------------------------------
395 | 
396 | void GreyScaleHistogram::Normalise()
397 | {
398 | 	if(m_fHistNorm > 0.0f)
399 | 	{
400 | 		for(int i=0; i<m_nBinsA; i++)
401 | 			if(m_afGreyScaleHistogram[i])
402 | 				m_afGreyScaleHistogram[i] /= m_fHistNorm;
403 | 		m_fHistNorm = 1.0;
404 | 	}
405 | }
406 | 
407 | //----------------------------------------------------------------------
408 | 
409 | double GreyScaleHistogram::CheckNorm()
410 | {
411 | 	double fNorm = 0.0f;
412 | 	for(int i=0; i<m_nBinsA; i++)
413 | 		fNorm+=m_afGreyScaleHistogram[i];
414 | 	return fNorm;
415 | }
416 | 
417 | //----------------------------------------------------------------------
418 | 
419 | double GreyScaleHistogram::MaxValue()
420 | {
421 | 	double fMaxVal = -1e30f;
422 | 
423 | 	for(int i=0; i<m_nBinsA; i++)
424 | 	{
425 | 		fMaxVal = max(fMaxVal,m_afGreyScaleHistogram[i]);
426 | 	}
427 | 
428 | 	return fMaxVal;
429 | }
430 | 
431 | 
432 | //----------------------------------------------------------------------
433 | 
434 | double GreyScaleHistogram::MinValue()
435 | {
436 | 	double fMinVal = 1e30f;
437 | 
438 | 	for(int i=0; i<m_nBinsA; i++)
439 | 	{
440 | 		if(m_afGreyScaleHistogram[i]!=0.0f)
441 | 			fMinVal = min(fMinVal,m_afGreyScaleHistogram[i]);
442 | 	}
443 | 
444 | 	return fMinVal;
445 | }
446 | 
447 | //----------------------------------------------------------------------
448 | 
449 | void GreyScaleHistogram::ScaleBy(double fScale)
450 | {
451 | 	for(int i=0; i<m_nBinsA; i++)
452 | 		m_afGreyScaleHistogram[i] *= fScale;
453 | }
454 | 
455 | //----------------------------------------------------------------------
456 | 
457 | void GreyScaleHistogram::SetUniform()
458 | {
459 | 	for(int i=0; i<m_nBinsA; i++)
460 | 		m_afGreyScaleHistogram[i] = 1.0;
461 | 	
462 | 	m_fHistNorm = double(m_nBinsA);
463 | }
464 | 
465 | //----------------------------------------------------------------------
466 | 
467 | double GreyScaleHistogram::GetBinVal(int nA)
468 | {
469 | 	return m_afGreyScaleHistogram[nA>>m_nShiftA]/*/m_fHistNorm*/;
470 | }
471 | 
472 | //----------------------------------------------------------------------
473 | 
474 | void GreyScaleHistogram::Clear()
475 | {
476 | 	m_fHistNorm=0.0f;
477 | 
478 | 	for(int i=0; i<m_nBinsA; i++)
479 | 		m_afGreyScaleHistogram[i] = 0.0;
480 | }
481 | 
482 | //----------------------------------------------------------------------
483 | 
484 | double GreyScaleHistogram::ComputeBhattacharyyaCoeff(GreyScaleHistogram& cP)
485 | {
486 | 	double fBhatt = 0.0f;
487 | 	for(int i=0; i<m_nBinsA; i++)
488 | 		fBhatt += sqrt(m_afGreyScaleHistogram[i]*cP.m_afGreyScaleHistogram[i]);
489 | 	return fBhatt;
490 | }
491 | 
492 | //----------------------------------------------------------------------
493 | 
494 | double GreyScaleHistogram::ComputeKLDiv(GreyScaleHistogram& cP)
495 | {
496 | 	double fKLDiv = 0.0f;
497 | 	for(int i=0; i<m_nBinsA; i++)
498 | 		fKLDiv += m_afGreyScaleHistogram[i]*log((m_afGreyScaleHistogram[i]+1e-200)/(cP.m_afGreyScaleHistogram[i]+1e-200));
499 | 	return fKLDiv;
500 | }
501 | 
502 | //----------------------------------------------------------------------
503 | 
504 | void GreyScaleHistogram::BeginAddingNewSamples()
505 | {
506 | 	m_fNewHistNorm=0.0;
507 | 	m_fNewHistProbWeightNorm=0.0;
508 | 	
509 | 	for(int i=0; i<m_nBinsA; i++)
510 | 		m_afNewGreyScaleHistogram[i] = 0.0;
511 | }
512 | 
513 | //----------------------------------------------------------------------
514 | 
515 | void GreyScaleHistogram::AddNewSample(int nA , double fWeight)
516 | {
517 | 	nA = nA>>m_nShiftA;
518 | 
519 | 	double fW = fWeight;
520 | 	m_afNewGreyScaleHistogram[nA] += fW;
521 | 	m_fNewHistProbWeightNorm += fW;
522 | 	m_fNewHistNorm += 1.0;
523 | }
524 | 
525 | //----------------------------------------------------------------------
526 | 
527 | void GreyScaleHistogram::CommitNewSamples(double fPrior)
528 | {
529 | 	//fPrior *= m_fNewHistProbWeightNorm/m_fNewHistNorm;
530 | 
531 | 	// Normalise
532 | 	if(m_fNewHistProbWeightNorm > 0.0f)
533 | 	{
534 | 		for(int i=0; i<m_nBinsA; i++)
535 | 			if(m_afNewGreyScaleHistogram[i])
536 | 				m_afNewGreyScaleHistogram[i] /= m_fNewHistProbWeightNorm;
537 | 
538 | 		m_fNewHistProbWeightNorm = 1.0;
539 | 	}
540 | 
541 | 	// Marginalise over two distributions
542 | 	for(int i=0; i<m_nBinsA; i++)
543 | 		m_afGreyScaleHistogram[i] = m_afNewGreyScaleHistogram[i]*fPrior+m_afGreyScaleHistogram[i]*(1.0-fPrior);
544 | }
545 | 
546 | //----------------------------------------------------------------------
547 | 
548 | int GreyScaleHistogram::NonZeroBins()
549 | {
550 | 	int nRetVal = 0;
551 | 	for(int i=0; i<m_nBinsA; i++)
552 | 		if(m_afNewGreyScaleHistogram[i])
553 | 			nRetVal++;
554 |     return nRetVal;
555 | }
556 | 
557 | #endif


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