/*
* David Scanner implementation
*
* Copyright ( C ) Vladislav Perelman
*
* Released under the GPL version 3.
*
*/
/*
* david_scanner . cc
* Program takes as an input path to the config file which needs to
* have all the necessary information for the program .
* Config file has to have ( each on a new line , 9 lines in total ) :
*
* Path to the directory where frames from the video are stored
* The first frame that has to be used
* The last frame that has to be used
* The empty frame without the laser
* Path to the file with intrinsics of the camera
* Path to the rotation of the left board
* Path to the rotation of the right board
* Path to the translation of the left board
* Path to the translation of the right board
*
* Program computes the 3 point cloud of the object and stores it in the
* file scan000 .3 d , each point in the cloud is represented by the line
* in the file :
* x y z r g b
*
*
* Created on : Oct 4 , 2010
* Author : Vladislav Perelman v . perelman @ jacobs - university . de
*/
# include <iostream>
# include <string>
# include <fstream>
# if (defined(_WIN32) || defined(__WIN32__) || defined(__TOS_WIN__) || defined(__WINDOWS__) || (defined(__APPLE__) & defined(__MACH__)))
# include <cv.h>
# include <highgui.h>
# include <cvaux.h>
# include <cxcore.h>
# elif (CV_MAJOR_VERSION == 2) && (CV_MINOR_VERSION < 2)
# include <opencv/cv.h>
# else
# include <opencv2/opencv.hpp>
# endif
# include <math.h>
# include <vector>
# define PI 3.14159265
using namespace std ;
int main ( int argc , char * * argv ) {
if ( argc ! = 2 ) {
cout < < " USAGE: david_scanner config_file \n Config file should contain path_to_frames first_valid_frame last_valid_frame empty_frame path_to_intrinsics "
" path_to_rotation_left path_to_rotation_right path_to_translation_left and path_to_translation_right each on a new line! " < < endl ;
return - 1 ;
}
//******Reading Input********
ifstream indata ;
indata . open ( argv [ 1 ] ) ;
if ( ! indata ) {
cout < < " Config file could not be opened " < < endl ;
return - 1 ;
}
string line ;
int numlines = 0 ;
while ( getline ( indata , line ) ) numlines + + ;
if ( numlines ! = 9 ) {
cout < < " Invalid number of lines in a config file! \n Config file should contain path_to_frames first_valid_frame last_valid_frame empty_frame path_to_intrinsics "
" path_to_rotation_left path_to_rotation_right path_to_translation_left and path_to_translation_right each on a new line! " ;
return - 1 ;
}
indata . clear ( ) ;
indata . seekg ( 0 ) ;
char path [ 200 ] ;
indata . getline ( path , 200 ) ;
char first_c [ 10 ] ;
char last_c [ 10 ] ;
char empty_c [ 10 ] ;
indata . getline ( first_c , 10 ) ;
indata . getline ( last_c , 10 ) ;
indata . getline ( empty_c , 10 ) ;
int first = atoi ( first_c ) ;
int last = atoi ( last_c ) ;
int empty = atoi ( empty_c ) ;
char intrinsics_path [ 200 ] ;
char rot_left [ 200 ] ;
char rot_right [ 200 ] ;
char tran_left [ 200 ] ;
char tran_right [ 200 ] ;
indata . getline ( intrinsics_path , 200 ) ;
indata . getline ( rot_left , 200 ) ;
indata . getline ( rot_right , 200 ) ;
indata . getline ( tran_left , 200 ) ;
indata . getline ( tran_right , 200 ) ;
//*********done************
//loading an empty frame
IplImage * image_empty ;
IplImage * image ;
char empty_name [ 100 ] ;
sprintf ( empty_name , " %s/%08d.ppm " , path , empty ) ;
if ( ( image_empty = cvLoadImage ( empty_name , 1 ) ) = = NULL ) {
cout < < " Cannot load empty frame...check input name " < < endl ;
return - 1 ;
}
//*******LOADING CAMERA PARAMETERS + CREATING MATRICES FOR FUTURE USE*********
CvMat * intrinsic = cvCreateMat ( 3 , 3 , CV_32F ) ;
if ( ( intrinsic = ( CvMat * ) cvLoad ( intrinsics_path ) ) = = NULL ) {
cout < < " Cannot load intrinsic parameters...check input path and file name " < < endl ;
return - 1 ;
}
//loading R1
CvMat * rotation_left = cvCreateMat ( 3 , 1 , CV_32F ) ;
if ( ( rotation_left = ( CvMat * ) cvLoad ( rot_left ) ) = = NULL ) {
cout < < " Cannot load rotation of the left board...check input " < < endl ;
return - 1 ;
}
//loading T1
CvMat * translation_left = cvCreateMat ( 3 , 1 , CV_32F ) ;
if ( ( translation_left = ( CvMat * ) cvLoad ( tran_left ) ) = = NULL ) {
cout < < " Cannot load translation of the left board...check input " < < endl ;
return - 1 ;
}
CvMat * rotation_matrix_left = cvCreateMat ( 3 , 3 , CV_32F ) ;
cvRodrigues2 ( rotation_left , rotation_matrix_left ) ;
//loading R2
CvMat * rotation_right = cvCreateMat ( 3 , 1 , CV_32F ) ;
if ( ( rotation_right = ( CvMat * ) cvLoad ( rot_right ) ) = = NULL ) {
cout < < " Cannot load rotation of the right board...check input " < < endl ;
return - 1 ;
}
//loading T2
CvMat * translation_right = cvCreateMat ( 3 , 1 , CV_32F ) ;
if ( ( translation_right = ( CvMat * ) cvLoad ( tran_right ) ) = = NULL ) {
cout < < " Cannot load translation of the right board...check input " < < endl ;
return - 1 ;
}
CvMat * rotation_matrix_right = cvCreateMat ( 3 , 3 , CV_32F ) ;
cvRodrigues2 ( rotation_right , rotation_matrix_right ) ;
//creating [R1|T1]
CvMat * r1t1 = cvCreateMat ( 3 , 4 , CV_32F ) ;
for ( int i = 0 ; i < 3 ; i + + ) {
CV_MAT_ELEM ( * r1t1 , float , i , 0 ) = CV_MAT_ELEM ( * rotation_matrix_left , float , i , 0 ) ;
CV_MAT_ELEM ( * r1t1 , float , i , 1 ) = CV_MAT_ELEM ( * rotation_matrix_left , float , i , 1 ) ;
CV_MAT_ELEM ( * r1t1 , float , i , 2 ) = CV_MAT_ELEM ( * rotation_matrix_left , float , i , 2 ) ;
CV_MAT_ELEM ( * r1t1 , float , i , 3 ) = CV_MAT_ELEM ( * translation_left , float , i , 0 ) ;
}
//creating [R2|T2]
CvMat * r2t2 = cvCreateMat ( 3 , 4 , CV_32F ) ;
for ( int i = 0 ; i < 3 ; i + + ) {
CV_MAT_ELEM ( * r2t2 , float , i , 0 ) = CV_MAT_ELEM ( * rotation_matrix_right , float , i , 0 ) ;
CV_MAT_ELEM ( * r2t2 , float , i , 1 ) = CV_MAT_ELEM ( * rotation_matrix_right , float , i , 1 ) ;
CV_MAT_ELEM ( * r2t2 , float , i , 2 ) = CV_MAT_ELEM ( * rotation_matrix_right , float , i , 2 ) ;
CV_MAT_ELEM ( * r2t2 , float , i , 3 ) = CV_MAT_ELEM ( * translation_right , float , i , 0 ) ;
}
//creating R1.i()
CvMat * r1inv = cvCreateMat ( 3 , 3 , CV_32F ) ;
cvInvert ( rotation_matrix_left , r1inv ) ;
//creating A.i()
CvMat * intrinsicinv = cvCreateMat ( 3 , 3 , CV_32F ) ;
cvInvert ( intrinsic , intrinsicinv ) ;
//creating R1.i()*A.i()
CvMat * R1iAi = cvCreateMat ( 3 , 3 , CV_32F ) ;
cvMatMul ( r1inv , intrinsicinv , R1iAi ) ;
//creating R2.i()
CvMat * r2inv = cvCreateMat ( 3 , 3 , CV_32F ) ;
cvInvert ( rotation_matrix_right , r2inv , CV_LU ) ;
//creating R2.i()*A.i()
CvMat * R2iAi = cvCreateMat ( 3 , 3 , CV_32F ) ;
cvMatMul ( r2inv , intrinsicinv , R2iAi ) ;
//creating R1.i()*T1
CvMat * a1 = cvCreateMat ( 3 , 1 , CV_32F ) ;
cvMatMul ( r1inv , translation_left , a1 ) ;
//creating R2.i()*T2
CvMat * a2 = cvCreateMat ( 3 , 1 , CV_32F ) ;
cvMatMul ( r2inv , translation_right , a2 ) ;
//*****************DONE********************
//open file for writing
ofstream scanfile ;
char scanname [ 20 ] ;
sprintf ( scanname , " scan000.3d " ) ;
scanfile . open ( scanname ) ;
//for loop going through each frame in the provided folder between first_valid_frame and last_valid_frame
for ( int m = first ; m < last ; m + + ) {
char name [ 100 ] ;
sprintf ( name , " %s/%08d.ppm " , path , m ) ;
cout < < name < < endl ;
if ( ( image = cvLoadImage ( name ) ) = = NULL ) {
cout < < " cannot load image: " < < name < < endl ;
continue ;
}
//do difference between current frame and the empty frame with no laser
IplImage * diff = cvCloneImage ( image ) ;
cvAbsDiff ( image_empty , image , diff ) ;
//focus on the red pixels, make others black
unsigned char * pixels = ( unsigned char * ) diff - > imageData ;
for ( int row = 0 ; row < diff - > height ; row + + ) {
for ( int col = 0 ; col < diff - > width ; col + + ) {
int R ;
R = pixels [ row * diff - > widthStep + col * 3 + 2 ] ;
if ( R > 30 ) {
pixels [ row * diff - > widthStep + col * 3 + 0 ] = 0 ;
pixels [ row * diff - > widthStep + col * 3 + 1 ] = 0 ;
pixels [ row * diff - > widthStep + col * 3 + 2 ] = 255 ;
} else {
pixels [ row * diff - > widthStep + col * 3 + 0 ] = 0 ;
pixels [ row * diff - > widthStep + col * 3 + 1 ] = 0 ;
pixels [ row * diff - > widthStep + col * 3 + 2 ] = 0 ;
}
}
}
//remove pixels that don't have at least 2 red neighbors
for ( int row = 1 ; row < diff - > height - 1 ; row + + ) {
for ( int col = 1 ; col < diff - > width - 1 ; col + + ) {
int R = pixels [ row * diff - > widthStep + col * 3 + 2 ] ;
if ( R = = 255 ) {
int r1 = pixels [ ( row - 1 ) * diff - > widthStep + col * 3 + 2 ] ;
int r2 = pixels [ ( row - 1 ) * diff - > widthStep + ( col - 1 ) * 3 + 2 ] ;
int r3 = pixels [ ( row - 1 ) * diff - > widthStep + ( col + 1 ) * 3 + 2 ] ;
int r4 = pixels [ ( row + 1 ) * diff - > widthStep + col * 3 + 2 ] ;
int r5 = pixels [ ( row + 1 ) * diff - > widthStep + ( col - 1 ) * 3 + 2 ] ;
int r6 = pixels [ ( row + 1 ) * diff - > widthStep + ( col + 1 ) * 3 + 2 ] ;
int r7 = pixels [ ( row ) * diff - > widthStep + ( col - 1 ) * 3 + 2 ] ;
int r8 = pixels [ ( row ) * diff - > widthStep + ( col + 1 ) * 3 + 2 ] ;
if ( r1 + r2 + r3 + r4 + r5 + r6 + r7 + r8 < = 255 ) pixels [ row * diff - > widthStep + col * 3 + 2 ] = 0 ;
}
}
}
//*****finding 2 lines on the image*****
bool good = false ;
int threshold = 50 ; //original threshold for Hough transform, incremented if too many groups of lines found
IplImage * color_dst ;
IplImage * tmpImage ;
int minX1 , minX2 , maxX1 , maxX2 ;
CvSeq * lines = 0 ;
CvPoint * line1 ;
CvPoint * line2 ;
int count_groups ;
//incrementing thresholds until only 2 groups of lines can be found
while ( ! good ) {
good = true ;
count_groups = 0 ; //counter for number of line groups. Line group is defined by the slope
int epsilon = 1.5 ; //error margin for the slope
color_dst = cvCreateImage ( cvGetSize ( diff ) , 8 , 3 ) ;
color_dst = cvCloneImage ( diff ) ;
tmpImage = cvCreateImage ( cvGetSize ( diff ) , IPL_DEPTH_8U , 1 ) ;
cvCvtColor ( diff , tmpImage , CV_RGB2GRAY ) ;
IplImage * dst = cvCreateImage ( cvGetSize ( diff ) , 8 , 1 ) ;
cvCanny ( tmpImage , dst , 20 , 60 , 3 ) ;
CvMemStorage * storage = cvCreateMemStorage ( 0 ) ;
//find all lines using Hough transform
lines = cvHoughLines2 ( dst , storage , CV_HOUGH_PROBABILISTIC , 1 , CV_PI / 180 , threshold , 150 , 100 ) ;
double first_group , second_group ;
for ( int i = 0 ; i < lines - > total ; i + + ) {
//get the slope of the line, check if it belongs to an already existing group
CvPoint * line = ( CvPoint * ) cvGetSeqElem ( lines , i ) ;
double angle = atan ( ( double ) ( line [ 1 ] . x - line [ 0 ] . x ) / ( double ) ( line [ 1 ] . y - line [ 0 ] . y ) ) * 180 / PI ;
//starting first group
if ( count_groups = = 0 ) {
first_group = angle ;
line1 = line ;
minX1 = line [ 0 ] . x ;
maxX1 = line [ 1 ] . x ;
count_groups + + ;
} else {
if ( angle - first_group < epsilon & & angle - first_group > ( epsilon * - 1 ) ) {
//line belongs to the first group of line..that's good
if ( line [ 0 ] . x < minX1 ) minX1 = line [ 0 ] . x ;
if ( line [ 1 ] . x > maxX1 ) maxX1 = line [ 1 ] . x ;
} else {
//check if belongs to the second group
if ( count_groups = = 2 ) {
if ( angle - second_group < epsilon & & angle - second_group > ( epsilon * - 1 ) ) {
if ( line [ 0 ] . x < minX2 ) minX2 = line [ 0 ] . x ;
if ( line [ 1 ] . x > maxX2 ) maxX2 = line [ 1 ] . x ;
} else {
//if not then try again with a higher threshold
good = false ;
threshold + = 20 ;
cout < < " Increased threshold: " < < threshold < < " " ;
cvReleaseImage ( & color_dst ) ;
cvReleaseImage ( & tmpImage ) ;
cvReleaseImage ( & dst ) ;
break ; //get out of here and increase the threshold since too many lines were found
}
} else { //starting second group
second_group = angle ;
minX2 = line [ 0 ] . x ;
maxX2 = line [ 1 ] . x ;
line2 = line ;
count_groups + + ;
}
}
}
}
//freeing some memory along the way
cvReleaseMemStorage ( & storage ) ;
cvReleaseImage ( & dst ) ;
}
//at this point we have found at most 2 groups of lines, we need to take only 1 line from each group
//basically finding the left-most and right-most point of each group and draw a line between those points, removing all the other lines.
//starting and ending points of 2 lines
CvPoint point1 ;
CvPoint point2 ;
CvPoint point3 ;
CvPoint point4 ;
if ( count_groups = = 2 ) {
int x1 = line1 [ 0 ] . x ;
int x2 = line1 [ 1 ] . x ;
int y1 = line1 [ 0 ] . y ;
int y2 = line1 [ 1 ] . y ;
double c1 = ( double ) ( x1 - minX1 ) / ( double ) ( x2 - minX1 ) ;
double c2 = ( double ) ( maxX1 - x1 ) / ( double ) ( maxX1 - x2 ) ;
int ymax , ymin ;
ymin = ( c1 * y2 - y1 ) / ( c1 - 1 ) ;
ymax = ( c2 * y2 - y1 ) / ( c2 - 1 ) ;
if ( maxX1 = = x2 ) ymax = y2 ;
if ( minX1 = = x1 ) ymin = y1 ;
//getting start and end of the first line
point1 = cvPoint ( minX1 , ymin ) ;
point2 = cvPoint ( maxX1 , ymax ) ;
//points around all the lines in a group so that a black rectangle can be drawn above them
CvPoint points [ 4 ] ;
points [ 0 ] = cvPoint ( minX1 , max ( 0 , ymin - 10 ) ) ;
points [ 1 ] = cvPoint ( minX1 , min ( color_dst - > height , ymin + 10 ) ) ;
points [ 2 ] = cvPoint ( maxX1 , min ( color_dst - > height , ymax + 10 ) ) ;
points [ 3 ] = cvPoint ( maxX1 , max ( 0 , ymax - 10 ) ) ;
CvPoint * pts [ 1 ] ;
pts [ 0 ] = points ;
int npts [ 1 ] ;
npts [ 0 ] = 4 ;
cvPolyLine ( color_dst , pts , npts , 1 , 1 , CV_RGB ( 0 , 0 , 0 ) , 20 , 8 ) ; //removing the group
x1 = line2 [ 0 ] . x ;
x2 = line2 [ 1 ] . x ;
y1 = line2 [ 0 ] . y ;
y2 = line2 [ 1 ] . y ;
c1 = ( double ) ( x1 - minX2 ) / ( double ) ( x2 - minX2 ) ;
c2 = ( double ) ( maxX2 - x1 ) / ( double ) ( maxX2 - x2 ) ;
ymin = ( c1 * y2 - y1 ) / ( c1 - 1 ) ;
ymax = ( c2 * y2 - y1 ) / ( c2 - 1 ) ;
if ( maxX2 = = x2 ) ymax = y2 ;
if ( minX2 = = x1 ) ymin = y1 ;
//getting start and end of the second line
point3 = cvPoint ( minX2 , ymin ) ;
point4 = cvPoint ( maxX2 , ymax ) ;
points [ 0 ] = cvPoint ( minX2 , max ( 0 , ymin - 10 ) ) ;
points [ 1 ] = cvPoint ( minX2 , min ( color_dst - > height , ymin + 10 ) ) ;
points [ 2 ] = cvPoint ( maxX2 , min ( color_dst - > height , ymax + 10 ) ) ;
points [ 3 ] = cvPoint ( maxX2 , max ( 0 , ymax - 10 ) ) ;
pts [ 0 ] = points ;
cvPolyLine ( color_dst , pts , npts , 1 , 1 , CV_RGB ( 0 , 0 , 0 ) , 20 , 8 ) ; //removing the group
cvLine ( color_dst , point3 , point4 , CV_RGB ( 0 , 255 , 0 ) , 3 , 8 ) ; //draw the second line!
cvLine ( color_dst , point1 , point2 , CV_RGB ( 0 , 255 , 0 ) , 3 , 8 ) ; //draw the first line!
//removing everything to the left of the left line and to the right of the right line
if ( point4 . x > point2 . x ) {
if ( color_dst - > width > point4 . x ) {
cvRectangle ( color_dst , cvPoint ( point4 . x , 0 ) , cvPoint ( color_dst - > width , color_dst - > height ) , CV_RGB ( 0 , 0 , 0 ) , CV_FILLED ) ;
}
if ( point1 . x > 0 ) {
cvRectangle ( color_dst , cvPoint ( point1 . x , 0 ) , cvPoint ( 0 , color_dst - > height ) , CV_RGB ( 0 , 0 , 0 ) , CV_FILLED ) ;
}
}
if ( point4 . x < point2 . x ) {
if ( color_dst - > width > point2 . x ) {
cvRectangle ( color_dst , cvPoint ( point2 . x , 0 ) , cvPoint ( color_dst - > width , color_dst - > height ) , CV_RGB ( 0 , 0 , 0 ) , CV_FILLED ) ;
}
if ( point3 . x > 0 ) {
cvRectangle ( color_dst , cvPoint ( point3 . x , 0 ) , cvPoint ( 0 , color_dst - > height ) , CV_RGB ( 0 , 0 , 0 ) , CV_FILLED ) ;
}
}
//at this point we have to lines which we drew in green...which means all the red pixels that remain on the image
//are supposed to be laying on the object. Make them blue (for no particular reason..just looked nicer :) )
unsigned char * pixels = ( unsigned char * ) color_dst - > imageData ;
for ( int row = 1 ; row < color_dst - > height - 1 ; row + + ) {
for ( int col = 1 ; col < color_dst - > width - 1 ; col + + ) {
int R = pixels [ row * color_dst - > widthStep + col * 3 + 2 ] ;
if ( R = = 255 ) {
pixels [ row * color_dst - > widthStep + col * 3 + 0 ] = 255 ;
pixels [ row * color_dst - > widthStep + col * 3 + 1 ] = 0 ;
pixels [ row * color_dst - > widthStep + col * 3 + 2 ] = 0 ;
}
}
}
} else continue ;
//take points on planes
CvPoint left1 , left2 , right1 ;
if ( point1 . x < point3 . x ) {
left1 = point1 ;
left2 = point2 ;
right1 = point3 ;
} else {
left1 = point3 ;
left2 = point4 ;
right1 = point1 ;
}
//find 3d coordinate of the 2 points on the line on the left plane
//(x,y,z).t() = s*R.i()*A.i()*(u,v,1).t() - R.i()*T
CvMat * imagepoint1 = cvCreateMat ( 3 , 1 , CV_32F ) ;
CV_MAT_ELEM ( * imagepoint1 , float , 0 , 0 ) = left1 . x ;
CV_MAT_ELEM ( * imagepoint1 , float , 1 , 0 ) = left1 . y ;
CV_MAT_ELEM ( * imagepoint1 , float , 2 , 0 ) = 1 ;
CvMat * b1 = cvCreateMat ( 3 , 1 , CV_32F ) ;
cvMatMul ( R1iAi , imagepoint1 , b1 ) ;
//calculate scalar s based on the fact that point we take is on the wall => z coordinate is 0
float s1 = CV_MAT_ELEM ( * a1 , float , 2 , 0 ) / CV_MAT_ELEM ( * b1 , float , 2 , 0 ) ;
CvMat * identity = cvCreateMat ( 3 , 3 , CV_32F ) ;
cvSetIdentity ( identity ) ;
for ( int i = 0 ; i < 3 ; i + + ) {
CV_MAT_ELEM ( * identity , float , i , i ) = s1 ;
}
CvMat * temp = cvCreateMat ( 3 , 1 , CV_32F ) ;
cvMatMul ( identity , b1 , temp ) ;
CvMat * dpoint1 = cvCreateMat ( 3 , 1 , CV_32F ) ;
cvSub ( temp , a1 , dpoint1 ) ; //first 3d point on the left plane
//same thing for the second point
CvMat * imagepoint2 = cvCreateMat ( 3 , 1 , CV_32F ) ;
CV_MAT_ELEM ( * imagepoint2 , float , 0 , 0 ) = left2 . x ;
CV_MAT_ELEM ( * imagepoint2 , float , 1 , 0 ) = left2 . y ;
CV_MAT_ELEM ( * imagepoint2 , float , 2 , 0 ) = 1 ;
CvMat * b2 = cvCreateMat ( 3 , 1 , CV_32F ) ;
cvMatMul ( R1iAi , imagepoint2 , b2 ) ;
float s2 = CV_MAT_ELEM ( * a1 , float , 2 , 0 ) / CV_MAT_ELEM ( * b2 , float , 2 , 0 ) ;
cvSetIdentity ( identity , cvRealScalar ( s2 ) ) ;
cvMatMul ( identity , b2 , b2 ) ;
CvMat * dpoint2 = cvCreateMat ( 3 , 1 , CV_32F ) ;
cvSub ( b2 , a1 , dpoint2 ) ; //second 3d point on the left plane
//same for the point on the right plane
CvMat * imagepoint3 = cvCreateMat ( 3 , 1 , CV_32F ) ;
CV_MAT_ELEM ( * imagepoint3 , float , 0 , 0 ) = right1 . x ;
CV_MAT_ELEM ( * imagepoint3 , float , 1 , 0 ) = right1 . y ;
CV_MAT_ELEM ( * imagepoint3 , float , 2 , 0 ) = 1 ;
CvMat * b3 = cvCreateMat ( 3 , 1 , CV_32F ) ;
cvMatMul ( R2iAi , imagepoint3 , b3 ) ;
float s3 = CV_MAT_ELEM ( * a2 , float , 2 , 0 ) / CV_MAT_ELEM ( * b3 , float , 2 , 0 ) ;
cvSetIdentity ( identity , cvRealScalar ( s3 ) ) ;
cvMatMul ( identity , b3 , b3 ) ;
CvMat * dpoint3 = cvCreateMat ( 3 , 1 , CV_32F ) ;
cvSub ( b3 , a2 , dpoint3 ) ; //point on the right plane
//convert point from the right plane into the coord. system of the left plane
//p1 = R1.i()*[R2|T2]*p2 - R1.i()*T1
CvMat * dpoint3left = cvCreateMat ( 3 , 1 , CV_32F ) ;
CvMat * pw = cvCreateMat ( 4 , 1 , CV_32F ) ;
for ( int i = 0 ; i < 3 ; i + + ) {
CV_MAT_ELEM ( * pw , float , i , 0 ) = CV_MAT_ELEM ( * dpoint3 , float , i , 0 ) ;
}
CV_MAT_ELEM ( * pw , float , 3 , 0 ) = 1.0 ;
CvMat * r2t2pw = cvCreateMat ( 3 , 1 , CV_32F ) ;
cvMatMul ( r2t2 , pw , r2t2pw ) ;
CvMat * r1invr2t2pw = cvCreateMat ( 3 , 1 , CV_32F ) ;
cvMatMul ( r1inv , r2t2pw , r1invr2t2pw ) ;
cvSub ( r1invr2t2pw , a1 , dpoint3left ) ;
//now that we have 3 non-colinear point in the same coordinate system we can find the equation of the plane
/*
A = y1 ( z2 - z3 ) + y2 ( z3 - z1 ) + y3 ( z1 - z2 )
B = z1 ( x2 - x3 ) + z2 ( x3 - x1 ) + z3 ( x1 - x2 )
C = x1 ( y2 - y3 ) + x2 ( y3 - y1 ) + x3 ( y1 - y2 )
- D = x1 ( y2 z3 - y3 z2 ) + x2 ( y3 z1 - y1 z3 ) + x3 ( y1 z2 - y2 z1 )
*/
float x1 = CV_MAT_ELEM ( * dpoint1 , float , 0 , 0 ) ;
float y1 = CV_MAT_ELEM ( * dpoint1 , float , 1 , 0 ) ;
float z1 = CV_MAT_ELEM ( * dpoint1 , float , 2 , 0 ) ;
float x2 = CV_MAT_ELEM ( * dpoint2 , float , 0 , 0 ) ;
float y2 = CV_MAT_ELEM ( * dpoint2 , float , 1 , 0 ) ;
float z2 = CV_MAT_ELEM ( * dpoint2 , float , 2 , 0 ) ;
float x3 = CV_MAT_ELEM ( * dpoint3left , float , 0 , 0 ) ;
float y3 = CV_MAT_ELEM ( * dpoint3left , float , 1 , 0 ) ;
float z3 = CV_MAT_ELEM ( * dpoint3left , float , 2 , 0 ) ;
float planeA = ( y1 * ( z2 - z3 ) ) + ( y2 * ( z3 - z1 ) ) + ( y3 * ( z1 - z2 ) ) ;
float planeB = ( z1 * ( x2 - x3 ) ) + ( z2 * ( x3 - x1 ) ) + ( z3 * ( x1 - x2 ) ) ;
float planeC = ( x1 * ( y2 - y3 ) ) + ( x2 * ( y3 - y1 ) ) + ( x3 * ( y1 - y2 ) ) ;
float planeD = - ( ( x1 * ( y2 * z3 - y3 * z2 ) ) + ( x2 * ( y3 * z1 - y1 * z3 ) ) + ( x3 * ( y1 * z2 - y2 * z1 ) ) ) ;
//calculate normal to the lazer plane
CvMat * planeNormal = cvCreateMat ( 3 , 1 , CV_32F ) ;
CV_MAT_ELEM ( * planeNormal , float , 0 , 0 ) = planeA ;
CV_MAT_ELEM ( * planeNormal , float , 1 , 0 ) = planeB ;
CV_MAT_ELEM ( * planeNormal , float , 2 , 0 ) = planeC ;
pixels = ( unsigned char * ) color_dst - > imageData ;
unsigned char * color_pixels = ( unsigned char * ) image_empty - > imageData ;
//go through all the pixels on the object and calculate the 3d coordinate
for ( int row = 1 ; row < color_dst - > height - 1 ; row + + ) {
for ( int col = 1 ; col < color_dst - > width - 1 ; col + + ) {
int B = pixels [ row * color_dst - > widthStep + col * 3 ] ;
if ( B = = 255 ) {
//get RGB of the pixel on the original image
int realB = color_pixels [ row * color_dst - > widthStep + col * 3 ] ;
int realG = color_pixels [ row * color_dst - > widthStep + col * 3 + 1 ] ;
int realR = color_pixels [ row * color_dst - > widthStep + col * 3 + 2 ] ;
//Used http://www.cs.princeton.edu/courses/archive/fall00/cs426/lectures/raycast/sld017.htm for reference
//on how to find intersection of ray and a plane
float p0dotN = cvDotProduct ( a1 , planeNormal ) ;
CvMat * vtmp = cvCreateMat ( 3 , 1 , CV_32F ) ;
CV_MAT_ELEM ( * vtmp , float , 0 , 0 ) = col ;
CV_MAT_ELEM ( * vtmp , float , 1 , 0 ) = row ;
CV_MAT_ELEM ( * vtmp , float , 2 , 0 ) = 1 ;
CvMat * v = cvCreateMat ( 3 , 1 , CV_32F ) ;
cvMatMul ( R1iAi , vtmp , v ) ;
float vdotN = cvDotProduct ( v , planeNormal ) ;
float t = ( p0dotN - planeD ) / vdotN ;
cvSetIdentity ( identity , cvRealScalar ( t ) ) ;
cvMatMul ( identity , v , v ) ;
CvMat * final = cvCreateMat ( 3 , 1 , CV_32F ) ;
cvSub ( v , a1 , final ) ; //final point is still in the coordinate system of the left plane.
CvMat * final_rotated = cvCreateMat ( 3 , 1 , CV_32F ) ; //translate it into the coordinate system of the camera
cvMatMul ( rotation_matrix_left , final , final_rotated ) ;
cvAdd ( final_rotated , translation_left , final_rotated ) ;
//add point to the file (minus next to the y coordinate is there to compensate for the left-handed coordinate system of slam6d, otherwise
//dwarf is shown upside-down.
scanfile < < CV_MAT_ELEM ( * final_rotated , float , 0 , 0 ) < < " " < < - CV_MAT_ELEM ( * final_rotated , float , 1 , 0 ) < < " " < < CV_MAT_ELEM ( * final_rotated , float , 2 , 0 ) < <
" " < < realR < < " " < < realG < < " " < < realB < < " \n " ;
cvReleaseMat ( & vtmp ) ;
cvReleaseMat ( & v ) ;
cvReleaseMat ( & final ) ;
cvReleaseMat ( & final_rotated ) ;
}
}
}
//save the image of the lines and points of the object
char name2 [ 100 ] ;
sprintf ( name2 , " %s/%08d_diff.ppm " , path , m ) ;
cvSaveImage ( name2 , color_dst ) ;
//free memory
cvReleaseImage ( & image ) ;
cvReleaseImage ( & diff ) ;
cvReleaseImage ( & color_dst ) ;
cvReleaseImage ( & tmpImage ) ;
cvReleaseMat ( & imagepoint1 ) ;
cvReleaseMat ( & imagepoint2 ) ;
cvReleaseMat ( & imagepoint3 ) ;
cvReleaseMat ( & b1 ) ;
cvReleaseMat ( & b2 ) ;
cvReleaseMat ( & b3 ) ;
cvReleaseMat ( & temp ) ;
cvReleaseMat ( & dpoint1 ) ;
cvReleaseMat ( & dpoint2 ) ;
cvReleaseMat ( & dpoint3 ) ;
cvReleaseMat ( & dpoint3left ) ;
cvReleaseMat ( & identity ) ;
cvReleaseMat ( & pw ) ;
cvReleaseMat ( & r2t2pw ) ;
cvReleaseMat ( & r1invr2t2pw ) ;
cvReleaseMat ( & planeNormal ) ;
}
//free more memory
cvReleaseImage ( & image_empty ) ;
cvReleaseMat ( & intrinsic ) ;
cvReleaseMat ( & intrinsicinv ) ;
cvReleaseMat ( & rotation_left ) ;
cvReleaseMat ( & rotation_matrix_left ) ;
cvReleaseMat ( & rotation_right ) ;
cvReleaseMat ( & rotation_matrix_right ) ;
cvReleaseMat ( & translation_left ) ;
cvReleaseMat ( & translation_right ) ;
cvReleaseMat ( & r1inv ) ;
cvReleaseMat ( & r2inv ) ;
cvReleaseMat ( & R1iAi ) ;
cvReleaseMat ( & R2iAi ) ;
cvReleaseMat ( & r1t1 ) ;
cvReleaseMat ( & r2t2 ) ;
cvReleaseMat ( & a1 ) ;
cvReleaseMat ( & a2 ) ;
//close file
scanfile . close ( ) ;
return 0 ;
}