josch
/
3dpcp
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You cannot select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
main
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '8b4640b056'
${ noResults }
3dpcp/.svn/pristine/12/12492f5775ced91e4bbe8a6700e...

649 lines
22 KiB
Plaintext
Raw Blame History

/*
* 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.3d, 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\nConfig 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!\nConfig 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;
}
Reference in New Issue View Git Blame Copy Permalink