1 /**
2 * @file pfsalign.cpp
3 * @brief Image stack alignment using feature matching
4 *
5 * The code relies on OpenCV SURF feature detector / descriptor. The ideas for
6 * prunning false matches are mostly based on the book:
7 *
8 * Lagani, R. (2011). OpenCV 2 Computer Vision Application Programming Cookbook.
9 * Packt Publishing.
10 *
11 *
12 * This file is a part of pfscalibration package.
13 * ----------------------------------------------------------------------
14 * Copyright (C) 2003-2013 Rafal Mantiuk and Grzegorz Krawczyk
15 *
16 * This library is free software; you can redistribute it and/or
17 * modify it under the terms of the GNU Lesser General Public
18 * License as published by the Free Software Foundation; either
19 * version 2.1 of the License, or (at your option) any later version.
20 *
21 * This library is distributed in the hope that it will be useful,
22 * but WITHOUT ANY WARRANTY; without even the implied warranty of
23 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
24 * Lesser General Public License for more details.
25 *
26 * You should have received a copy of the GNU Lesser General Public
27 * License along with this library; if not, write to the Free Software
28 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
29 * ----------------------------------------------------------------------
30 *
31 * @author Rafal Mantiuk, <mantiuk@mpi-sb.mpg.de>
32 *
33 * $Id: pfsalign.cpp,v 1.11 2013/11/14 21:28:28 rafm Exp $
34 */
35
36 #include <iostream>
37 #include <sstream>
38 #include <string>
39 #include <algorithm>
40 #include <getopt.h>
41 #include <string.h>
42
43 #include <opencv2/core/core.hpp>
44 #include <opencv2/highgui/highgui.hpp>
45 #include <opencv2/features2d/features2d.hpp>
46 #include <opencv2/nonfree/features2d.hpp>
47 #include <opencv2/calib3d/calib3d.hpp>
48 #include <opencv2/imgproc/imgproc.hpp>
49 #include <libexif/exif-data.h>
50
51 #include <pfs.h>
52
53 #define PROG_NAME "pfsalign"
54 #define VERBOSE_STR if( verbose ) std::cerr << PROG_NAME ": "
55
56 bool verbose = false;
57
58 class QuietException
59 {
60 };
61
62 using namespace cv;
63 using namespace std;
64
printHelp()65 void printHelp()
66 {
67 fprintf( stderr, PROG_NAME " [-r index|--reference index] [-c (min|max)|--crop (min|max)] [-d|--display-matches] [-f|--fail-no-match] -v -h\n"
68 "See man page for more information.\n" );
69 }
70
71 class Toc
72 {
73 double tickCount;
74 public:
tic(const char * name=NULL)75 void tic( const char *name = NULL )
76 {
77 if( name != NULL ) {
78 VERBOSE_STR << "Starting " << name << "...";
79 } else {
80 VERBOSE_STR << "Starting processing ...";
81 }
82 tickCount = static_cast<double>( getTickCount() );
83 }
84
toc()85 void toc( )
86 {
87 double duration = (static_cast<double>( getTickCount() ) - tickCount) / getTickFrequency();
88 if( verbose )
89 std::cerr << "completed. It took " << duration << " seconds." << std::endl;
90 }
91 };
92
93
94
pruneNNDR(std::vector<std::vector<DMatch>> & matches,float ratio)95 void pruneNNDR( std::vector<std::vector<DMatch> > &matches, float ratio )
96 {
97 for( vector<vector<DMatch> >::iterator it = matches.begin(); it != matches.end(); it++ ) {
98 bool prune = false;
99 if( it->size() < 2 )
100 prune = true;
101 else {
102 float NNDR = (*it)[0].distance / (*it)[1].distance;
103 if( NNDR >= ratio )
104 prune = true;
105 }
106 if( prune )
107 it->clear();
108 }
109
110 }
111
symmetryTest(const vector<vector<DMatch>> & matches_12,const vector<vector<DMatch>> & matches_21,vector<DMatch> & matches_sym)112 void symmetryTest( const vector<vector<DMatch> > &matches_12, const vector<vector<DMatch> > &matches_21, vector<DMatch> &matches_sym )
113 {
114 for( vector< vector<DMatch> >::const_iterator it1 = matches_12.begin(); it1 != matches_12.end(); it1++ ) {
115 if( it1->size() == 0 )
116 continue;
117
118 for( vector< vector<DMatch> >::const_iterator it2 = matches_21.begin(); it2 != matches_21.end(); it2++ ) {
119 if( it2->size() == 0 )
120 continue;
121
122 if( (*it1)[0].queryIdx == (*it2)[0].trainIdx && (*it2)[0].trainIdx == (*it1)[0].queryIdx ) {
123 matches_sym.push_back( DMatch( (*it1)[0].queryIdx, (*it1)[0].trainIdx, (*it1)[0].distance ));
124 break;
125 }
126 }
127
128 }
129 }
130
131
132 /**
133 Match feature points in a pair image and find homography.
134 */
alignImagePair(const Mat & ref_img,const Mat & exp_img,Mat & homography,int sensitivity,bool display_matches)135 bool alignImagePair( const Mat &ref_img, const Mat &exp_img, Mat &homography, int sensitivity, bool display_matches )
136 {
137 Toc toc;
138
139 homography = Mat::eye( 3, 3, CV_64F );
140 // cv::imshow( "Result", ref_img );
141 // cv::imshow( "Result2", exp_img );
142 // cv::waitKey(0);
143
144 Ptr<FeatureDetector> detector(new DynamicAdaptedFeatureDetector( new SurfAdjuster( (11-sensitivity) * 100.f, 2, 1000 ),
145 100, 1000, sensitivity/2+2 ));
146 // Ptr<FeatureDetector> detector;
147 // detector = new GoodFeaturesToTrackDetector();
148 // detector = new SurfFeatureDetector();
149
150 std::vector<KeyPoint> keypoints1, keypoints2;
151
152 toc.tic( "feature detection" );
153 detector->detect( ref_img, keypoints1 );
154 detector->detect( exp_img, keypoints2 );
155 toc.toc( );
156
157 if( keypoints1.size() < 10 || keypoints2.size() < 10 ) {
158 cerr << PROG_NAME ": Could not detect a sufficient number of keypoints (found "
159 << keypoints1.size() << " and " << keypoints2.size() << " keypoints)" << endl;
160
161 if( display_matches ) {
162 Mat vis;
163 vector<char> inliners_c;
164 std::vector<cv::DMatch> matches_sym;
165 drawMatches( ref_img, keypoints1, exp_img, keypoints2, matches_sym, vis, Scalar(0,0,255), Scalar(0,255,255), inliners_c, DrawMatchesFlags::DRAW_RICH_KEYPOINTS );
166 cv::namedWindow( "Image pair matches" );
167 cv::imshow( "Image pair matches", vis );
168 cv::waitKey(0);
169 }
170
171 return false;
172 }
173
174 // SiftDescriptorExtractor surfDesc;
175 SurfDescriptorExtractor surfDesc;
176
177 Mat descr_ref, descr_exp;
178 toc.tic( "descriptor extraction" );
179 surfDesc.compute( ref_img, keypoints1, descr_ref );
180 surfDesc.compute( exp_img, keypoints2, descr_exp );
181 toc.toc( );
182
183 FlannBasedMatcher matcher;
184 // BruteForceMatcher< cv::L2<float> > matcher;
185
186 toc.tic( "matching" );
187 std::vector< std::vector<cv::DMatch> > matches_rt;
188 matcher.knnMatch( descr_ref, descr_exp, matches_rt, 2 );
189
190 std::vector< std::vector<cv::DMatch> > matches_tr;
191 matcher.knnMatch( descr_exp, descr_ref, matches_tr, 2 );
192
193 pruneNNDR( matches_rt, 1 );
194 pruneNNDR( matches_tr, 1 );
195
196 std::vector<cv::DMatch> matches_sym;
197 symmetryTest( matches_rt, matches_tr, matches_sym );
198 toc.toc( );
199
200 std::vector<cv::DMatch>::iterator it;
201 vector<Point3f> p1, p2;
202 Mat_<float> pp1(matches_sym.size(),2);
203 Mat_<float> pp2(matches_sym.size(),2);
204 int kk = 0;
205 for( it = matches_sym.begin(); it != matches_sym.end(); it++, kk++ ) {
206
207 const Point2f from = keypoints1[it->queryIdx].pt;
208 p1.push_back( Point3f( from.x, from.y, 1 ) );
209 pp1(kk,0) = from.x;
210 pp1(kk,1) = from.y;
211 // pp1(kk,2) = 1;
212 const Point2f to = keypoints2[it->trainIdx].pt;
213 p2.push_back( Point3f( to.x, to.y, 1 ) );
214 pp2(kk,0) = to.x;
215 pp2(kk,1) = to.y;
216 //pp2(kk,2) = 1;
217 // std::cout << "Matches: " << from << " -> " << to << std::endl;
218 }
219
220 if( matches_sym.size() < 9 ) {
221 cerr << PROG_NAME ": Not enough matches found between a pair of images (found " << matches_sym.size() << ")" << endl;
222 return false;
223 }
224
225 // Mat affine = estimateRigidTransform( e1, e2, false );
226
227 vector<uchar> inliners(matches_sym.size(), 0);
228 // affine = findHomography( pp1, pp2, inliners, CV_RANSAC, 1. );
229 homography = findHomography( pp2, pp1, CV_RANSAC, 1., inliners );
230
231 // solve( pp1, pp2, affine, DECOMP_SVD );
232 // Mat affine = (Mat_<float>(2,3) << 1, 0, 0, 0, 1, 10);
233 int inliner_count = count( inliners.begin(), inliners.end(), 1 );
234
235 VERBOSE_STR << "Found " << matches_sym.size() << " matching features, reduced to " << inliner_count << " inliners." << endl;
236
237 if( display_matches ) {
238 Mat vis;
239 vector<char> inliners_c(matches_sym.size(), 0);
240 for( size_t i = 0; i < inliners.size(); i++ )
241 inliners_c[i] = (char)inliners[i];
242
243 drawMatches( ref_img, keypoints1, exp_img, keypoints2, matches_sym, vis, Scalar(0,0,255), Scalar(0,255,255), inliners_c, DrawMatchesFlags::DRAW_RICH_KEYPOINTS|DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
244
245 cv::namedWindow( "Image pair matches" );
246 cv::imshow( "Image pair matches", vis );
247
248 cv::waitKey(0);
249 }
250
251 if( inliner_count < 9 ) {
252 homography = Mat::eye( 3, 3, CV_64F );
253 cerr << PROG_NAME ": Not enough inliners to find a reliable transformation." << endl;
254 return false;
255 }
256
257
258 return true;
259
260 }
261
262
263 /**
264 Convert floating point image to 8-bit image.
265 */
convert_to_8bit(Mat & in,bool apply_gamma)266 Mat convert_to_8bit( Mat &in, bool apply_gamma )
267 {
268 Mat out;
269 if( !apply_gamma ) {
270 in.convertTo( out, CV_8UC3, 255 );
271 } else {
272 out.create( in.rows, in.cols, CV_8UC3 );
273 for(int i = 0; i < in.rows; i++)
274 {
275 Vec3f *i_p = in.ptr<Vec3f>(i);
276 Vec3b *o_p = out.ptr<Vec3b>(i);
277 for(int j = 0; j < in.cols; j++) {
278 for( int cc=0; cc < 3; cc++ ) {
279 o_p[j][cc] = saturate_cast<uchar>(powf( i_p[j][cc], 1.f/2.2f ) * 255.f);
280 }
281 }
282 }
283
284
285 }
286
287
288 return out;
289 }
290
291
292 // Find maximum and positive minimum for an image
find_min_max(Mat & in,float & minV,float & maxV)293 void find_min_max( Mat &in, float &minV, float &maxV )
294 {
295 minV = 1e10f;
296 maxV = -1e10f;
297 for(int i = 0; i < in.rows; i++)
298 {
299 Vec3f *i_p = in.ptr<Vec3f>(i);
300 for(int j = 0; j < in.cols; j++) {
301 for( int cc=0; cc < 3; cc++ ) {
302 float v = i_p[j][cc];
303 if( v > 0 ) {
304 if( v < minV )
305 minV = v;
306 if( v > maxV )
307 maxV = v;
308 }
309 }
310 }
311 }
312
313 }
314
convert_and_log_scale(Mat & in,float min,float max,float mult,float gamma)315 Mat convert_and_log_scale( Mat &in, float min, float max, float mult, float gamma )
316 {
317 Mat out;
318 out.create( in.rows, in.cols, CV_8UC3 );
319
320 const float l_min = logf( min );
321 const float l_max = logf( max );
322 const float l_mult = logf( mult );
323
324 for(int i = 0; i < in.rows; i++)
325 {
326 Vec3f *i_p = in.ptr<Vec3f>(i);
327 Vec3b *o_p = out.ptr<Vec3b>(i);
328 for(int j = 0; j < in.cols; j++) {
329 for( int cc=0; cc < 3; cc++ ) {
330 if( i_p[j][cc] > 0.f )
331 o_p[j][cc] = saturate_cast<uchar>( ((gamma*(logf(i_p[j][cc]) - l_max) + l_mult) / (l_max-l_min) + 1.f) * 255.f);
332 else
333 o_p[j][cc] = 0;
334 }
335 }
336 }
337
338 return out;
339 }
340
341 /**
342 Convert a pair of floating point images to 8-bit.
343 */
convert_to_8bit(Mat & in1,Mat & in2,Mat & out1,Mat & out2,float exp_mult1,float exp_mult2,bool is_ldr)344 void convert_to_8bit( Mat &in1, Mat &in2, Mat &out1, Mat &out2, float exp_mult1, float exp_mult2, bool is_ldr )
345 {
346 if( is_ldr && exp_mult1 == 1.f && exp_mult2 == 1.f ) {
347 in1.convertTo( out1, CV_8UC3, 255*exp_mult1);
348 in2.convertTo( out2, CV_8UC3, 255*exp_mult2);
349 } else {
350 float minV1, minV2, maxV1, maxV2;
351 find_min_max( in1, minV1, maxV1 );
352 find_min_max( in2, minV2, maxV2 );
353
354 minV1 *= exp_mult1;
355 maxV1 *= exp_mult1;
356 minV2 *= exp_mult2;
357 maxV2 *= exp_mult2;
358
359 float gamma = is_ldr ? 2.2f : 1.f;
360
361 minV1 = powf( minV1, gamma );
362 minV2 = powf( minV2, gamma );
363
364 float minV, maxV;
365 minV = std::min( minV1, minV2 );
366 maxV = std::max( maxV1, maxV2 );
367
368
369 if( (maxV/minV) > 10000.f ) {
370 // To avoid compressing contrast too much
371 minV = maxV / 10000.f;
372 }
373 if( (maxV/minV) < 100.f ) {
374 // To avoid streching contrast too much
375 minV = maxV / 100.f;
376 }
377
378 VERBOSE_STR << "Using log scaling (min val: " << minV1 << " max value: " << maxV1 << ")" << endl;
379
380
381 out1 = convert_and_log_scale( in1, minV, maxV1, exp_mult1, gamma );
382 out2 = convert_and_log_scale( in2, minV, maxV1, exp_mult2, gamma );
383 }
384 }
385
intersect_lines(Point2f p1,Point2f p2,Point2f p3,Point2f p4)386 Point2f intersect_lines( Point2f p1, Point2f p2, Point2f p3, Point2f p4 )
387 {
388 float s = ((p4.x-p3.x)*(p3.y-p1.y) - (p3.x-p1.x)*(p4.y-p3.y)) / ( (p4.x-p3.x)*(p2.y-p1.y) - (p2.x-p1.x)*(p4.y-p3.y) );
389
390 Point2f p;
391 p.x = p1.x + s * (p2.x-p1.x);
392 p.y = p1.y + s * (p2.y-p1.y);
393
394 return p;
395 }
396
auto_crop(const Size2f & size,const vector<Point2f> & rotated)397 Rect auto_crop( const Size2f &size, const vector<Point2f> &rotated )
398 {
399
400 assert( rotated.size() == 4 ); // 4 corners of the polygon
401
402 Point2f centre;
403 for( int kk=0; kk < 4; kk++ )
404 centre += rotated[kk];
405
406 centre = centre * 0.25;
407
408 //cout << "Float: " << (rot_mat.type() == CV_32F) << endl;
409
410 float best_d = 1e10;
411 Point2d best_p;
412 for( int jj = 0; jj < 2; jj++ ) {
413 for( int ii=0; ii < 2; ii++ ) {
414 Point2f to;
415 if( jj == 0 )
416 to = centre - Point2f( size.width/2, size.height/2 );
417 else
418 to = centre - Point2f( -size.width/2, size.height/2 );
419
420 Point2f p1 = intersect_lines( centre, to, rotated[ii], rotated[ii+1] );
421 float d = powf(p1.x-centre.x,2)+powf(p1.y-centre.y,2);
422 if( d < best_d ) {
423 best_d = d;
424 best_p = p1;
425 }
426 }
427 }
428 Point2f ul( centre.x - fabs(best_p.x-centre.x), centre.y - fabs(best_p.y-centre.y) );
429 Point2f br( centre.x + fabs(best_p.x-centre.x), centre.y + fabs(best_p.y-centre.y) );
430
431 Rect crop( ul, br );
432
433 return crop;
434 }
435
436 struct FrameInfo {
437 Mat image;
438 std::string file_name;
439 pfs::Frame *frame;
440 Size size;
441
FrameInfoFrameInfo442 FrameInfo( Mat &image, const char *file_name, pfs::Frame *frame ) :
443 image( image ), file_name( file_name ), frame( frame )
444 {
445 size = Size( image.cols, image.rows );
446 }
447 };
448
alignFrames(int argc,char * argv[])449 void alignFrames(int argc, char *argv[])
450 {
451 pfs::DOMIO pfsio;
452
453 int reference_index = 0; // Index of the reference image
454 bool display_matches = false;
455 bool crop_min = false;
456 bool fail_on_no_match = false;
457 int sensitivity = 5;
458
459 static struct option cmdLineOptions[] = {
460 { "help", no_argument, NULL, 'h' },
461 { "verbose", no_argument, NULL, 'v' },
462 { "reference", required_argument, NULL, 'r' },
463 { "crop", required_argument, NULL, 'c' },
464 { "sensitivity", required_argument, NULL, 's' },
465 { "fail-no-match", no_argument, NULL, 'f' },
466 { "display-matches", no_argument, NULL, 'd' },
467 { NULL, 0, NULL, 0 }
468 };
469
470 int optionIndex = 0;
471 while( 1 ) {
472 int c = getopt_long (argc, argv, "r:c:s:fhvd", cmdLineOptions, &optionIndex);
473 if( c == -1 ) break;
474 switch( c ) {
475 case 'h':
476 printHelp();
477 throw QuietException();
478 case 'v':
479 verbose = true;
480 break;
481 case 'r':
482 reference_index = strtol( optarg, NULL, 10 ) - 1;
483 break;
484 case 'd':
485 display_matches = true;
486 break;
487 case 'c':
488 if( !strcasecmp( optarg, "min" ) ) {
489 crop_min = true;
490 } else if( !strcasecmp( optarg, "max" ) ) {
491 crop_min = false;
492 } else
493 throw pfs::Exception( "Unrecognized cropping mode" );
494 break;
495 case 'f':
496 fail_on_no_match = true;
497 break;
498 case 's':
499 sensitivity = strtol( optarg, NULL, 10 );
500 if( sensitivity <= 0 || sensitivity > 10 ) {
501 throw pfs::Exception( "Sensitivity parameter must be within 1-10 range.");
502 }
503 break;
504 case '?':
505 throw QuietException();
506 case ':':
507 throw QuietException();
508 }
509 }
510
511 vector<FrameInfo> frames;
512
513 if( crop_min ) {
514 VERBOSE_STR << "Cropping to the size that contains only overlaping pixels (min)" << endl;
515 } else {
516 VERBOSE_STR << "Cropping to the size that contains all pixels (max)" << endl;
517 }
518
519
520 // Load all images
521 // bool first_frame = true;
522 int frame_count = 0;
523 for( ; true; frame_count++ ) {
524
525 pfs::Frame *frame = pfsio.readFrame( stdin );
526 if( frame == NULL ) break; // No more frames
527
528 const char *f_name = frame->getTags()->getString("FILE_NAME");
529 if( f_name == NULL ) {
530 std::stringstream f_name_str;
531 f_name_str << "frame_" << frame_count;
532 f_name = f_name_str.str().c_str();
533 }
534 VERBOSE_STR << "Loading " << f_name << endl;
535
536 pfs::Channel *X, *Y, *Z;
537 frame->getXYZChannels( X, Y, Z );
538
539 if( X != NULL ) { // Color, XYZ
540 pfs::Channel* &R = X;
541 pfs::Channel* &G = Y;
542 pfs::Channel* &B = Z;
543 pfs::transformColorSpace( pfs::CS_XYZ, X, Y, Z, pfs::CS_RGB, R, G, B );
544
545 Mat img;
546 img.create( frame->getHeight(), frame->getWidth(), CV_32FC3 );
547
548 int in_index = 0;
549 for(int i = 0; i < img.rows; i++)
550 {
551 Vec3f *o_p = img.ptr<Vec3f>(i);
552 for(int j = 0; j < img.cols; j++) {
553 o_p[j][0] = (*B)(in_index);
554 o_p[j][1] = (*G)(in_index);
555 o_p[j][2] = (*R)(in_index);
556 in_index++;
557 }
558 }
559
560 frames.push_back( FrameInfo( img, f_name, frame ) );
561
562 } else
563 throw pfs::Exception( "Only color images are supported" );
564
565
566 // Remove all channels from the frame to save memory
567 pfs::ChannelIteratorPtr cit( frame->getChannelIterator() );
568 while( cit->hasNext() ) {
569 pfs::Channel *ch = cit->getNext();
570 frame->removeChannel( ch );
571 }
572
573 }
574
575 if( reference_index < 0 || (size_t)reference_index >= frames.size() )
576 throw pfs::Exception( "Reference image index out of range" );
577
578
579 vector<Mat> homoM;
580 vector<Size> imageSizes;
581 for( int ff=1; ff < (int)frames.size(); ff++ ) {
582 VERBOSE_STR << "Processing " << frames[ff-1].file_name << " and " << frames[ff].file_name << endl;
583
584 cv::Mat ref_img = frames[ff-1].image;
585 cv::Mat exp_img = frames[ff].image;
586
587 imageSizes.push_back( exp_img.size() );
588
589 bool is_ldr = false;
590 const char *lum_type = frames[ff].frame->getTags()->getString("LUMINANCE");
591 if( lum_type ) {
592 if( !strcmp( lum_type, "DISPLAY" ) )
593 is_ldr = true;
594 }
595
596 // Extract exposure values
597 float exp1 = 1.f, exp2 = 1.f;
598 const char* exp_str = frames[ff-1].frame->getTags()->getString("BV");
599 if( exp_str != NULL )
600 exp1 = powf(2.0f,strtof( exp_str, NULL ));
601 exp_str = frames[ff].frame->getTags()->getString("BV");
602 if( exp_str != NULL )
603 exp2 = powf(2.0f,strtof( exp_str, NULL ));
604
605 if( exp1 / exp2 != 1.f ) {
606 VERBOSE_STR << "Exposure pair: " << exp1 << ", " << exp2 << " (" << exp2/exp1 << " ratio)" << endl;
607 }
608
609 const float exp_mult1 = exp1 / std::min( exp1, exp2 );
610 const float exp_mult2 = exp2 / std::min( exp1, exp2 );
611
612 Mat ref_img_8b, exp_img_8b;
613
614 // Mat ref_img_8b = convert_to_8bit( ref_img, apply_gamma );
615 // Mat exp_img_8b = convert_to_8bit( exp_img, apply_gamma );
616 // convert_to_8bit( ref_img, exp_img, ref_img_8b, exp_img_8b, 1, 1, is_ldr );
617 convert_to_8bit( ref_img, exp_img, ref_img_8b, exp_img_8b, exp_mult1, exp_mult2, is_ldr );
618
619 Mat homography;
620 bool success = alignImagePair( ref_img_8b, exp_img_8b, homography, sensitivity, display_matches );
621 if( !success && fail_on_no_match )
622 throw pfs::Exception( "Stopping because could not find a match between image pair");
623
624 VERBOSE_STR << "Homography (for the image pair):" << endl << homography << endl;
625
626 homoM.push_back( homography );
627 }
628
629
630 // Chain all transformations and find the cropping box
631 vector<Mat> homoMC;
632 Rect_<double> pano_rect( 0, 0, frames[0].image.cols, frames[0].image.rows );
633 for( int kk = 0; kk < (int)frames.size(); kk++ )
634 {
635 Mat_<double> trans = Mat::eye(3,3,CV_64F);
636 for( int ll = min( kk, reference_index )+1; ll <= max( kk, reference_index); ll++ )
637 {
638 if( ll > 0 )
639 trans = trans*homoM[ll-1];
640 }
641 if( kk < reference_index )
642 trans = trans.inv();
643
644 homoMC.push_back( trans );
645
646 double data[4][3] = { { 0.0, 0.0, 1.0 }, { static_cast<double>(frames[kk].size.width), 0.0, 1.0 }, { static_cast<double>(frames[kk].size.width), static_cast<double>(frames[kk].size.height), 1.0 }, { 0.0, static_cast<double>(frames[kk].size.height), 1.0 } };
647 Mat corners( 4, 3, CV_64F, data );
648
649 Mat corners_trans = trans * corners.t();
650
651 // VERBOSE_STR << "Image: " << fileNames[kk] << endl;
652 // VERBOSE_STR << " Corners: " << trans * corners.t() << endl;
653
654 Mat p_nh; //( 4, 3, CV_32F );
655 Mat_<float> corners_f;
656 corners_trans.convertTo( corners_f, CV_32F );
657 convertPointsFromHomogeneous( corners_f.t(), p_nh );
658
659
660 if( crop_min ) {
661 vector<Point2f> dest_rect(4);
662 for( int ii=0; ii < 4; ii++ ) {
663 dest_rect[ii].x = p_nh.at<float>(ii,0);
664 dest_rect[ii].y = p_nh.at<float>(ii,1);
665 }
666 Rect_<float> img_rect = auto_crop( frames[kk].size, dest_rect );
667 Rect_<double> img_rect_d = img_rect;
668 pano_rect = pano_rect & img_rect_d;
669 } else {
670 Rect_<double> img_rect = boundingRect( p_nh );
671 pano_rect = pano_rect | img_rect;
672 }
673
674
675 // VERBOSE_STR << "Bounding rect: " << pano_rect.x << ", " << pano_rect.y << " - " << pano_rect.width << "x" << pano_rect.height << endl;
676 }
677
678
679 // Align
680 Size dest_size = Size( ceil( pano_rect.width ), ceil( pano_rect.height) );
681 // Mat pano_img( ceil( pano_rect.height), ceil( pano_rect.width ), CV_8UC3 );
682 // pano_img.setTo( Vec3b(255, 255, 255 ));
683 for( size_t ff=0; ff < frames.size(); ff++ ) {
684 VERBOSE_STR << "Warping: "<< frames[ff].file_name << endl;
685
686 Mat exp_img = frames[ff].image;
687
688 Mat_<double> translate = Mat::eye(3,3,CV_64F);
689 translate(0,2) = -pano_rect.x;
690 translate(1,2) = -pano_rect.y;
691 Mat trans = translate*homoMC[ff];
692 VERBOSE_STR << "Homography (to reference): " << trans << endl;
693
694 Mat res_img;
695 warpPerspective( exp_img, res_img, trans, dest_size, INTER_LINEAR );
696
697 pfs::Frame *alignedFrame = NULL;
698 alignedFrame = pfsio.createFrame( res_img.cols, res_img.rows );
699
700 pfs::Channel *X = alignedFrame->createChannel( "X" );
701 pfs::Channel *Y = alignedFrame->createChannel( "Y" );
702 pfs::Channel *Z = alignedFrame->createChannel( "Z" );
703
704 pfs::Channel* &R = X;
705 pfs::Channel* &G = Y;
706 pfs::Channel* &B = Z;
707
708 int out_index = 0;
709 for(int i = 0; i < res_img.rows; i++)
710 {
711 Vec3f *i_p = res_img.ptr<Vec3f>(i);
712 for(int j = 0; j < res_img.cols; j++) {
713 (*B)(out_index) = i_p[j][0];
714 (*G)(out_index) = i_p[j][1];
715 (*R)(out_index) = i_p[j][2];
716 out_index++;
717 }
718 }
719 pfs::transformColorSpace( pfs::CS_RGB, R, G, B, pfs::CS_XYZ, X, Y, Z );
720 pfs::copyTags( frames[ff].frame, alignedFrame );
721
722 pfsio.writeFrame( alignedFrame, stdout );
723
724 pfsio.freeFrame( alignedFrame );
725 pfsio.freeFrame( frames[ff].frame );
726
727
728 // TODO: Add alpha channel
729 /* Mat in_mask( exp_img.size(), CV_8U ), out_mask;
730 in_mask.setTo( Scalar( 255 ) );
731 warpPerspective( in_mask, out_mask, trans, pano_img.size(), INTER_LINEAR );
732 res_img.copyTo( pano_img, out_mask );
733
734 double data[4][3] = { { 0, 0, 1 }, { exp_img.cols, 0, 1 }, { exp_img.cols, exp_img.rows, 1 }, { 0, exp_img.rows, 1 } };
735 Mat corners( 4, 3, CV_64F, data );
736 Mat_<double> corners_trans = trans * corners.t();
737 Mat p_nh; //( 4, 3, CV_32F );
738 Mat_<float> corners_f;
739 corners_trans.convertTo( corners_f, CV_32F );
740 convertPointsFromHomogeneous( corners_f.t(), p_nh );*/
741 // Scalar borderColor = CV_RGB( 0, 0, 0 );
742 // line( pano_img, Point2f( p_nh(0,0), p_nh(0,1) ), Point2f( p_nh(1,0), p_nh(1,1) ), borderColor, 3 );
743 // line( pano_img, Point2f( p_nh(1,0), p_nh(1,1) ), Point2f( p_nh(2,0), p_nh(2,1) ), borderColor, 3 );
744 // line( pano_img, Point2f( p_nh(2,0), p_nh(2,1) ), Point2f( p_nh(3,0), p_nh(3,1) ), borderColor, 3 );
745 // line( pano_img, Point2f( p_nh(3,0), p_nh(3,1) ), Point2f( p_nh(0,0), p_nh(0,1) ), borderColor, 3 );
746 }
747
748 }
749
main(int argc,char * argv[])750 int main( int argc, char* argv[] )
751 {
752 try {
753 alignFrames( argc, argv );
754 }
755 catch( pfs::Exception ex ) {
756 fprintf( stderr, PROG_NAME " error: %s\n", ex.getMessage() );
757 return EXIT_FAILURE;
758 }
759 catch( QuietException ex ) {
760 return EXIT_FAILURE;
761 }
762 return EXIT_SUCCESS;
763 }
764