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/da/da4d3508c0d27871a3aaa14a21c...

2467 lines
83 KiB
Plaintext
Raw Blame History

#include <errno.h>
#include "thermo/thermo.h"
#include "newmat/newmatap.h"
using namespace NEWMAT;
#include "cvblob.h"
using namespace cvb;
#include <slam6d/globals.icc>
#ifndef _MSC_VER
#include <getopt.h>
#include <sys/stat.h>
#else
#include "XGetopt.h"
#define strcasecmp _stricmp
#define strncasecmp _strnicmp
#include <windows.h>
#include <direct.h>
#endif
#ifdef _EiC
#define WIN32
#endif
Float2D data1;
Float2D data2;
unsigned int BLOB_SIZE = 65;
double AVG_THRES = 0.8;
unsigned int GRAY_TH = 65;
/**
* Calculates the PCA of a two-dimensional point cloud
* @param x x coordinate of the axis
* @param y y coordinate of the axis
* @param pc true if the principal axis is wanted, false for the least dominant
* @param cx center x of the point cloud
* @param cy center y of the point cloud
*/
void calcBoard(double point_array[][2], int board_n, double &x, double &y, double &cx, double &cy, bool pc) {
cx = cy = 0;
for (int a = 0; a < board_n; a++) {
cx += point_array[a][0];
cy += point_array[a][1];
}
cx /= board_n;
cy /= board_n;
SymmetricMatrix A(2);
A = 0;
for(int a = 0; a < board_n; a++) {
A(1,1) += (point_array[a][0] - cx)*(point_array[a][0] - cx);
A(2,2) += (point_array[a][1] - cy)*(point_array[a][1] - cy);
A(1,2) += (point_array[a][0] - cx)*(point_array[a][1] - cy);
}
DiagonalMatrix D;
Matrix V;
try {
Jacobi(A,D,V);
} catch (ConvergenceException) {
cout << "couldn't find board..." << endl;
}
int min, max;
D.MaximumAbsoluteValue1(max);
D.MinimumAbsoluteValue1(min);
// return eigenvector with highest eigenvalue
if(pc) {
x = V(1,max);
y = V(2,max);
// return eigenvector with lowest eigenvalue
} else {
x = V(1,min);
y = V(2,min);
}
}
/**
* Sorts the detected light bulbs on the board.
* @param point_array list of detected points
* @param board_n number of lightbulbs
* @param board_h number of rows
* @param board_w number of columns
* @param quiet if true, debug information is printed
*/
void sortBlobs(double point_array[][2], int board_n, int board_h, int board_w, bool quiet) {
double x, y, cx, cy;
// align board using PCA
calcBoard(point_array, board_n, x, y, cx, cy, board_h <= board_w);
double point_array2[board_n][2];
double angle = -atan2(y,x);
for(int i = 0; i < board_n; i++) {
double tmpx = point_array[i][0] - cx;
double tmpy = point_array[i][1] - cy;
point_array2[i][0] = tmpx * cos(angle) - tmpy * sin(angle);
point_array2[i][1] = tmpx * sin(angle) + tmpy * cos(angle);
}
// sorting the points on the basis of y coordinate//////
int swapped1 = 0;
do {
swapped1 = 0;
for (int a = 1; a <= board_n - 1; a++) {
if (point_array2[a][1] < point_array2[a - 1][1]) {
//rotated points
double tempx = point_array2[a][0];
double tempy = point_array2[a][1];
point_array2[a][0] = point_array2[a - 1][0];
point_array2[a][1] = point_array2[a - 1][1];
point_array2[a - 1][0] = tempx;
point_array2[a - 1][1] = tempy;
//original points
double tmpx = point_array[a][0];
double tmpy = point_array[a][1];
point_array[a][0] = point_array[a - 1][0];
point_array[a][1] = point_array[a - 1][1];
point_array[a - 1][0] = tmpx;
point_array[a - 1][1] = tmpy;
swapped1 = 1;
}
}
} while (swapped1 == 1);
if(!quiet) {
cout << "sorted array:" << endl;
for (int f = 0; f < board_n; f++) {
cout << point_array2[f][0] << " " << point_array2[f][1] << endl;
}
}
// sorting the array rows now
for (int x = 0; x < board_h; x++) {
double row_points[board_w][2];
double row_points2[board_w][2];
for (int y = 0; y < board_w; y++) {
row_points[y][0] = point_array[x * board_w + y][0];
row_points[y][1] = point_array[x * board_w + y][1];
row_points2[y][0] = point_array2[x * board_w + y][0];
row_points2[y][1] = point_array2[x * board_w + y][1];
if(!quiet) cout << row_points[y][0] << " " << row_points[y][1] << " ";
}
if(!quiet) cout << endl;
int swapped = 0;
do {
swapped = 0;
for (int a = 1; a <= board_w - 1; a++) {
if (row_points2[a][0] < row_points2[a - 1][0]) {
// original points
double tempx = row_points[a][0];
double tempy = row_points[a][1];
row_points[a][0] = row_points[a - 1][0];
row_points[a][1] = row_points[a - 1][1];
row_points[a - 1][0] = tempx;
row_points[a - 1][1] = tempy;
// rotated points
double tmpx = row_points2[a][0];
double tmpy = row_points2[a][1];
row_points2[a][0] = row_points2[a - 1][0];
row_points2[a][1] = row_points2[a - 1][1];
row_points2[a - 1][0] = tmpx;
row_points2[a - 1][1] = tmpy;
swapped = 1;
}
}
} while (swapped == 1);
if(!quiet) cout << "sorted:" << endl;
for (int z = 0; z < board_w; z++) {
point_array2[x * board_w + z][0] = row_points2[z][0];
point_array2[x * board_w + z][1] = row_points2[z][1];
if(!quiet) cout << point_array[x * board_w + z][0] << " " << point_array[x * board_w + z][1] << " ";
point_array[x * board_w + z][0] = row_points[z][0];
point_array[x * board_w + z][1] = row_points[z][1];
}
if(!quiet) cout << endl;
}
}
/**
* Detects the blobs.
*/
IplImage* detectBlobs(IplImage *org_image, int &corner_exp, int board_h, int board_w, bool quiet, double point_array2[][2]) {
IplImage *gray_image = cvCloneImage(org_image);
cvThreshold(gray_image, gray_image, GRAY_TH, 255, CV_THRESH_BINARY);
IplImage *labelImg = cvCreateImage(cvGetSize(gray_image), IPL_DEPTH_LABEL, 1);
// detect blobs
CvBlobs blobs;
cvLabel(gray_image, labelImg, blobs);
double average_size = 0;
int count = 0;
for (CvBlobs::const_iterator it = blobs.begin(); it != blobs.end(); ++it) {
if (it->second->area < BLOB_SIZE) {
count++;
average_size += it->second->area;
}
}
if(!quiet) cout << "centroid:" << average_size << endl;
// refine blobs
average_size = average_size / count;
double average_size_min = average_size * (1.0 - AVG_THRES);
double average_size_max = average_size * (1.0 + AVG_THRES);
int blob_count = 0;
for (CvBlobs::const_iterator it2 = blobs.begin(); it2 != blobs.end(); ++it2) {
if (it2->second->area >= average_size_min &&
it2->second->area <= average_size_max &&
blob_count < corner_exp) {
point_array2[blob_count][0] = it2->second->centroid.x;
point_array2[blob_count][1] = it2->second->centroid.y;
double sumx = 0.0;
double sumy = 0.0;
double sum = 0.0;
/*
int step = 5;
int minx = ((int)it2->second->minx - step) > -1 ? (it2->second->minx - step) : 0;
int maxx = ((it2->second->maxx + step) < gray_image->width) ? (it2->second->maxx + step) : (gray_image->width - 1);
int miny = ((int)it2->second->miny - step) > -1 ? (it2->second->miny - step) : 0;
int maxy = ((it2->second->maxy + step) < gray_image->height) ? (it2->second->maxy + step) : (gray_image->height - 1);
*/
int minx = it2->second->minx;
int miny = it2->second->miny;
int maxx = it2->second->maxx;
int maxy = it2->second->maxy;
for(int x = minx; x <= maxx; x++) {
for(int y = miny; y <= maxy; y++) {
if(cvGet2D(gray_image, y, x).val[0] > 0) {
CvScalar c;
c = cvGet2D(org_image, y, x);
sum += c.val[0];
sumx += c.val[0]*x;
sumy += c.val[0]*y;
}
}
}
sumx /= sum;
sumy /= sum;
point_array2[blob_count][0] = sumx;
point_array2[blob_count][1] = sumy;
blob_count++;
}
}
if(!quiet) cout << "Refined number of blobs=" << blob_count << endl;
// sorting the points
sortBlobs(point_array2, corner_exp, board_h, board_w, true);
cvReleaseImage(&labelImg);
corner_exp = blob_count;
return gray_image;
}
/**
* Connects the detected calibration features in the image with lines.
*/
void drawLines(double point_array2[][2], int corner_exp, IplImage *image, bool color) {
for (int i = 0; i <= corner_exp - 2; i++) {
CvPoint pt1;
CvPoint pt2;
CvScalar s;
if(color) {
s = CV_RGB(255,0,0);
} else {
s.val[0] = 100;
}
double temp1 = point_array2[i][0] - floor(point_array2[i][0]);
if (temp1 < .5) {
pt1.x = floor(point_array2[i][0]);
} else {
pt1.x = floor(point_array2[i][0]) + 1;
}
double temp2 = point_array2[i][1] - floor(point_array2[i][1]);
if (temp2 < .5) {
pt1.y = floor(point_array2[i][1]);
} else {
pt1.y = floor(point_array2[i][1]) + 1;
}
double temp3 = point_array2[i + 1][0] - floor(
point_array2[i + 1][0]);
if (temp3 < .5) {
pt2.x = floor(point_array2[i + 1][0]);
} else {
pt2.x = floor(point_array2[i + 1][0]) + 1;
}
double temp4 = point_array2[i + 1][1] - floor(
point_array2[i + 1][1]);
if (temp4 < .5) {
pt2.y = floor(point_array2[i + 1][1]);
} else {
pt2.y = floor(point_array2[i + 1][1]) + 1;
}
cvLine(image, pt1, pt2, s, 3, 8);
}
cvShowImage("Final Result", image);
}
/**
* Resizes the image
*/
IplImage* resizeImage(IplImage *source, int scale) {
int width, height;
IplImage *image;
switch(scale) {
case 2:
width = 1200;
height = 900;
break;
case 3:
width = 800;
height = 600;
break;
case 4:
width = 400;
height = 300;
break;
case 5:
width = 160;
height = 120;
break;
case 1:
default:
return cvCloneImage(source);
}
image = cvCreateImage(cvSize(width,height),source->depth,source->nChannels);
cvResize(source,image);
return image;
}
/**
* Detects the corners of the chessboard pattern.
*/
IplImage* detectCorners(IplImage *orgimage, int &corner_exp, int board_h, int board_w, bool quiet, double point_array2[][2], int scale) {
cout << "Scale: " << scale << endl;
IplImage *image = resizeImage(orgimage, scale);
CvSize size = cvGetSize(image);
CvPoint2D32f* corners = new CvPoint2D32f[corner_exp];
CvSize board_sz = cvSize(board_w, board_h);
IplImage *gray_image = cvCreateImage(size,8,1);
if (image->nChannels == 3) {
cvCvtColor(image, gray_image, CV_BGR2GRAY);
} else {
gray_image = image;
}
int found = cvFindChessboardCorners(image, board_sz, corners, &corner_exp,
CV_CALIB_CB_ADAPTIVE_THRESH
| CV_CALIB_CB_NORMALIZE_IMAGE | CV_CALIB_CB_FILTER_QUADS);
cout << "found corners:" << corner_exp << endl;
if (found != 0) {//if all corners found successfully
//Get Subpixel accuracy on those corners
if(size.width > 400) {
cvFindCornerSubPix(gray_image, corners, corner_exp, cvSize(11, 11), cvSize(-1, -1),
cvTermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
}
else {
cvFindCornerSubPix(gray_image, corners, corner_exp, cvSize(3, 3), cvSize(-1, -1),
cvTermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
}
}
for (int i = 0; i < corner_exp; i++) {
point_array2[i][0] = corners[i].x;
point_array2[i][1] = corners[i].y;
}
return gray_image;
}
/**
* Writes the intrinsic calibration parameters to files.
*/
void writeCalibParam(int images, int corner_exp, int board_w, CvMat*
image_points, CvSize size, string dir, string substring) {
CvMat* intrinsic_matrix = cvCreateMat(3, 3, CV_32FC1);
CvMat* distortion_coeffs = cvCreateMat(5, 1, CV_32FC1);
//ALLOCATE MATRICES ACCORDING TO HOW MANY CHESSBOARDS FOUND
CvMat* object_points2 = cvCreateMat(images * corner_exp, 3, CV_32FC1);
CvMat* image_points2 = cvCreateMat(images * corner_exp, 2, CV_32FC1);
CvMat* point_counts2 = cvCreateMat(images, 1, CV_32SC1);
CvMat* Rotation = cvCreateMat(images, 3, CV_32FC1);
CvMat* Translation = cvCreateMat(images, 3, CV_32FC1);
//TRANSFER THE POINTS INTO THE CORRECT SIZE MATRICES
int j;
for (int i = 0; i < images * corner_exp; ++i) {
j = i % corner_exp;
CV_MAT_ELEM( *image_points2, float, i, 0) = CV_MAT_ELEM( *image_points, float, i, 0);
CV_MAT_ELEM( *image_points2, float,i,1) = CV_MAT_ELEM( *image_points, float, i, 1);
CV_MAT_ELEM(*object_points2, float, i, 0) = (j / board_w) * 4;
CV_MAT_ELEM( *object_points2, float, i, 1) = (j % board_w) * 4;
CV_MAT_ELEM( *object_points2, float, i, 2) = 0.0f;
}
for (int i = 0; i < images; ++i) { //These are all the same number
CV_MAT_ELEM( *point_counts2, int, i, 0) = corner_exp;
}
// Initialize the intrinsic matrix with focal length = 1.0
CV_MAT_ELEM( *intrinsic_matrix, float, 0, 0 ) = 1.0f;
CV_MAT_ELEM( *intrinsic_matrix, float, 1, 1 ) = 1.0f;
//CALIBRATE THE CAMERA!
cvCalibrateCamera2(object_points2, image_points2, point_counts2, size,
intrinsic_matrix, distortion_coeffs, Rotation, Translation, 0
);
// SAVE AND PRINT THE INTRINSICS AND DISTORTIONS
string file = dir + "Intrinsics" + substring + ".xml";
cvSave(file.c_str(), intrinsic_matrix);
file = dir + "Distortion" + substring + ".xml";
cvSave(file.c_str(), distortion_coeffs);
cout << "Camera Intrinsic Matrix is:" << endl;
for (int row = 0; row < 3; row++) {
for (int col = 0; col < 3; col++) {
cout << CV_MAT_ELEM( *intrinsic_matrix, float, row, col ) << "\t";
}
cout << endl;
}
cout << "Distortion Coefficients are:" << endl;
for (int row = 0; row < 5; row++) {
for (int col = 0; col < 1; col++) {
cout << CV_MAT_ELEM( *distortion_coeffs, float, row, col ) << "\t";
}
}
cout << endl;
CvMat *intrinsic = intrinsic_matrix;
CvMat *distortion = distortion_coeffs;
// CLEANUP
cvReleaseMat(&object_points2);
cvReleaseMat(&image_points2);
cvReleaseMat(&point_counts2);
cvReleaseMat(&Rotation);
cvReleaseMat(&Translation);
cvReleaseMat(&intrinsic);
cvReleaseMat(&distortion);
}
/**
* Main function for intrinsic calibration
*/
void CalibFunc(int board_w, int board_h, int start, int end, bool optical, bool
chess, bool quiet, string dir, int scale) {
cvNamedWindow("Original Image", 0);
cvNamedWindow("Final Result", 0);
cvResizeWindow( "Original Image", 480, 640 );
cvResizeWindow( "Final Result", 480, 640 );
int nr_img = end - start + 1;
if (nr_img == 0) {
cout << "ImageCount is zero!" << endl;
return;
}
int corner_exp = board_w * board_h;
CvSize board_sz = cvSize(board_w, board_h);
CvSize size;
//ALLOCATE STORAGE(depending upon the number of images in(in case if command line arguments are given )
//not on the basis of number of images in which all corner extracted/while in the other case the number is the same )
CvMat* image_points = cvCreateMat(nr_img * corner_exp, 2, CV_32FC1);
//TODO CvPoint2D32f* corners = new CvPoint2D32f[ board_n ];
int successes = 0;
int step = 0;
for (int count = start; count <= end; count++) {
cout << "count : " << count << endl;
string t;
string t1;
if(optical) {
//TODO t = dir + "/photo" + to_string(count, 3) + ".ppm";
t = dir + "/photo" + to_string(count, 3) + ".jpg";
//t = dir + to_string(count, 3) + "/photo" + to_string(count, 3) + ".ppm";
} else {
//t = dir + to_string(count, 3) + "/image" + to_string(count, 3) + ".ppm";
t = dir + "/image" + to_string(count, 3) + ".ppm";
}
cout << t << endl;
//loading images and finding corners
IplImage* image1 = cvLoadImage(t.c_str(), -1);
if (!image1) {
cout << "image cannot be loaded" << endl;
return;
}
cvShowImage("Original Image", image1);
/////////////////////////////////////////////////////////////
double point_array2[corner_exp][2];
IplImage *image;
int tmp_corners = corner_exp;
if(chess) {
cout << "detect corners" << endl;
image = detectCorners(image1, tmp_corners, board_h, board_w, quiet, point_array2, scale);
} else {
cout << "detect blob" << endl;
image = detectBlobs(image1, tmp_corners, board_h, board_w, quiet, point_array2);
}
/*
for(int i = 0; i < corner_exp; i++) {
cout << (float) point_array2[i][0] << " " << (float) point_array2[i][1] <<
endl;
}
//drawing the lines on the image now
*/
drawLines(point_array2, corner_exp, image);
//cvDrawChessboardCorners(image, size, point_array2, tmp_corners, true);
//cvShowImage("Final Result", image);
char in;
if(tmp_corners == corner_exp) {
cout << "\nDo you want to use the data from this image ('y' or 'n'). 'x' aborts the calibration? : ";
in = 'y';
/*
int c = cvWaitKey(100);
if (c == 27) {
break;
}
cin >> in;
*/
if (in == 'y') {
//cvSaveImage(t1.c_str(), image);
size = cvGetSize(image);
step = successes * corner_exp;
//appending corner data to a generic data structure for all images
for (int i = step, j = 0; j < corner_exp; ++i, ++j) {
CV_MAT_ELEM(*image_points, float,i,0) = (float) point_array2[j][0];
CV_MAT_ELEM(*image_points, float,i,1) = (float) point_array2[j][1];
}
successes++;
} else if(in == 'x') {
break;
}
}
cvReleaseImage(&image);
cvReleaseImage(&image1);
}
cout << "Images for which all corners were found successfully="
<< successes << endl;
if (successes == 0) {
cout << "No successful corners found from any image" << endl;
return;
}
string substring = optical? "Optical" : "";
writeCalibParam(successes, corner_exp, board_w, image_points, size, dir, substring);
cvReleaseMat(&image_points);
}
/**
* Reads the 3D information of the features from a file.
*/
bool readPoints(string filename, CvPoint3D32f *corners, int size) {
ifstream infile(filename.c_str(), ios::in);
if (!infile) {
cout << "3Ddata file cannot be loaded" << endl;
return false;
}
string verify;
infile >> verify;
if(strcmp(verify.c_str(), "failed") == 0) return false;
for(int l = 0; l < size; l++) {
infile >> corners[l].y;
infile >> corners[l].z;
infile >> corners[l].x;
corners[l].y = -corners[l].y;
}
return true;
}
/**
* Calculates the median of a set of translation vectors, i.e., the translation
* that has the smallest distance to all other translation.
*/
int realMedian(CvMat * vectors, int nr_vectors) {
double distances[nr_vectors];
for(int i = 0; i < nr_vectors; i++) {
double sum = 0;
double x1 = (CV_MAT_ELEM(*vectors,float,i,0));
double y1 = (CV_MAT_ELEM(*vectors,float,i,1));
double z1 = (CV_MAT_ELEM(*vectors,float,i,2));
for(int j = 0; j < nr_vectors; j++) {
double x2 = (CV_MAT_ELEM(*vectors,float,j,0));
double y2 = (CV_MAT_ELEM(*vectors,float,j,1));
double z2 = (CV_MAT_ELEM(*vectors,float,j,2));
double tmp = (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)+(z2-z1)*(z2-z1);
sum += sqrt(tmp);
}
distances[i] = sum;
}
int min_pos = -1;
double min_dist = DBL_MAX;
for(int i = 0; i < nr_vectors; i++) {
if(distances[i] < min_dist) {
min_pos = i;
min_dist = distances[i];
}
}
return min_pos;
}
/*
* Calculates the median of a set of vectors by iteratively calculating the
* median and cropping outliers.
*/
void filterMedian(CvMat * vectors, int nr_vectors, int thres, CvMat * mean) {
int threshold = thres;
// calculate Median
int min_pos = realMedian(vectors, nr_vectors);
if(thres == 4) {
(CV_MAT_ELEM(*mean,float,0,0)) = (CV_MAT_ELEM(*vectors,float,min_pos,0));
(CV_MAT_ELEM(*mean,float,0,1)) = (CV_MAT_ELEM(*vectors,float,min_pos,1));
(CV_MAT_ELEM(*mean,float,0,2)) = (CV_MAT_ELEM(*vectors,float,min_pos,2));
(CV_MAT_ELEM(*mean,float,0,3)) = (CV_MAT_ELEM(*vectors,float,min_pos,3));
(CV_MAT_ELEM(*mean,float,0,4)) = (CV_MAT_ELEM(*vectors,float,min_pos,4));
(CV_MAT_ELEM(*mean,float,0,5)) = (CV_MAT_ELEM(*vectors,float,min_pos,5));
return;
}
// crop outliers
double x1 = (CV_MAT_ELEM(*vectors,float,min_pos,0));
double y1 = (CV_MAT_ELEM(*vectors,float,min_pos,1));
double z1 = (CV_MAT_ELEM(*vectors,float,min_pos,2));
int count = 0;
for(int i = 0; i < nr_vectors; i++) {
double x2 = (CV_MAT_ELEM(*vectors,float,i,0));
double y2 = (CV_MAT_ELEM(*vectors,float,i,1));
double z2 = (CV_MAT_ELEM(*vectors,float,i,2));
double tmp = (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)+(z2-z1)*(z2-z1);
if(sqrt(tmp) < 1.0/threshold) count++;
}
CvMat* some_vectors = cvCreateMat(count, 6, CV_32FC1);
count = 0;
for(int i = 0; i < nr_vectors; i++) {
double x2 = (CV_MAT_ELEM(*vectors,float,i,0));
double y2 = (CV_MAT_ELEM(*vectors,float,i,1));
double z2 = (CV_MAT_ELEM(*vectors,float,i,2));
double tmp = (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)+(z2-z1)*(z2-z1);
if(sqrt(tmp) < 1.0/threshold) {
for(int j = 0; j < 6; j++) {
(CV_MAT_ELEM(*some_vectors,float,count,j)) = (CV_MAT_ELEM(*vectors,float,i,j));
cout << (CV_MAT_ELEM(*some_vectors,float,count,j)) << " ";
}
cout << endl;
count++;
}
}
cout << "min_pos " << min_pos << endl;
// recurse
if(thres < 3) {
filterMedian(some_vectors, count, ++threshold, mean);
cvReleaseMat(&some_vectors);
// determine result
} else {
/*
x1 = (CV_MAT_ELEM(*some_vectors,float,min_pos,0));
y1 = (CV_MAT_ELEM(*some_vectors,float,min_pos,1));
z1 = (CV_MAT_ELEM(*some_vectors,float,min_pos,2));
*/
double x2 = 0;
double y2 = 0;
double z2 = 0;
double r1 = 0;
double r2 = 0;
double r3 = 0;
for(int i = 0; i < count; i++) {
x2 += (CV_MAT_ELEM(*some_vectors,float,i,0));
y2 += (CV_MAT_ELEM(*some_vectors,float,i,1));
z2 += (CV_MAT_ELEM(*some_vectors,float,i,2));
r1 += (CV_MAT_ELEM(*some_vectors,float,i,3));
r2 += (CV_MAT_ELEM(*some_vectors,float,i,4));
r3 += (CV_MAT_ELEM(*some_vectors,float,i,5));
}
(CV_MAT_ELEM(*mean,float,0,0)) = x2/count;
(CV_MAT_ELEM(*mean,float,0,1)) = y2/count;
(CV_MAT_ELEM(*mean,float,0,2)) = z2/count;
(CV_MAT_ELEM(*mean,float,0,3)) = r1/count;
(CV_MAT_ELEM(*mean,float,0,4)) = r2/count;
(CV_MAT_ELEM(*mean,float,0,5)) = r3/count;
}
}
/**
* Sorts vectors element by element, enables one to calculate the median of
* each element separately.
*/
void sortElementByElement(CvMat * vectors, int nr_elems, int nr_vectors) {
bool swapped;
for (int i = 0; i < nr_elems; i++) {
do {
swapped = false;
for (int j = 1; j <= nr_vectors - 1; j++) {
if (CV_MAT_ELEM(*vectors,float,j,i) < CV_MAT_ELEM(*vectors,float,j-1,i)) {
float temp = CV_MAT_ELEM(*vectors,float,j,i);
CV_MAT_ELEM(*vectors,float,j,i) = CV_MAT_ELEM(*vectors,float,j-1,i);
CV_MAT_ELEM(*vectors,float,j-1,i) = temp;
swapped = true;
}
}
} while (swapped);
}
}
/**
* Calculates the extrinsic parameters of a set of matches. Find the best match
* by calculating the reprojection error of each set of calibration parameters.
*/
void calculateExtrinsicsWithReprojectionCheck(CvMat * points2D, CvMat *
points3D, CvMat * rotation_vectors_temp, CvMat * translation_vectors_temp, CvMat
* distortion, CvMat * intrinsics, int corners, int successes, string dir, bool quiet, string substring) {
double reprojectionError[successes];
for(int i = 0; i < successes; i++) {
reprojectionError[i] = 0.0;
}
cout << "reprojectionError" << endl;
for(int i = 0; i < successes + 4; i++) {
reprojectionError[i] = 0.0;
CvMat * rotation = cvCreateMat(1, 3, CV_32FC1);
CvMat * translation = cvCreateMat(1, 3, CV_32FC1);
if(i==successes) cout << "now other stuff" << endl;
for(int k = 0; k < 3; k++) {
CV_MAT_ELEM(*rotation, float, 0, k) = CV_MAT_ELEM(*rotation_vectors_temp, float, i, k);
CV_MAT_ELEM(*translation, float, 0, k) = CV_MAT_ELEM(*translation_vectors_temp, float, i, k);
cerr << CV_MAT_ELEM(*translation, float, 0, k)<< " ";
}
for(int k = 0; k < 3; k++) {
cerr << CV_MAT_ELEM(*rotation, float, 0, k)<< " ";
}
cerr << endl;
for(int j = 0; j < successes; j++) {
double tmp = 0;
//calculate reprojection error
CvMat * point_3Dcloud = cvCreateMat(corners, 3, CV_32FC1);
CvMat * point_2Dcloud = cvCreateMat(corners, 2, CV_32FC1);
for(int l = 0; l < corners; l++) {
CV_MAT_ELEM(*point_2Dcloud,float,l,0) = 0.0;
CV_MAT_ELEM(*point_2Dcloud,float,l,1) = 1.0;
CV_MAT_ELEM(*point_3Dcloud,float,l,0) = CV_MAT_ELEM(*points3D,CvPoint3D32f,j,l).x;
CV_MAT_ELEM(*point_3Dcloud,float,l,1) = CV_MAT_ELEM(*points3D,CvPoint3D32f,j,l).y;
CV_MAT_ELEM(*point_3Dcloud,float,l,2) = CV_MAT_ELEM(*points3D,CvPoint3D32f,j,l).z;
}
cvProjectPoints2(point_3Dcloud, rotation, translation, intrinsics,
distortion, point_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0);
for(int l = 0; l < corners; l++) {
double x = CV_MAT_ELEM(*point_2Dcloud,float,l,0) - CV_MAT_ELEM(*points2D,CvPoint2D32f,j,l).x;
double y = CV_MAT_ELEM(*point_2Dcloud,float,l,1) - CV_MAT_ELEM(*points2D,CvPoint2D32f,j,l).y;
tmp += sqrt(x*x + y*y);
}
cvReleaseMat(&point_2Dcloud);
reprojectionError[i] += tmp;
cvReleaseMat(&point_3Dcloud);
}
cout << reprojectionError[i]/successes/30 << endl;
cvReleaseMat(&rotation);
cvReleaseMat(&translation);
}
int maxindex = -1;
double max = DBL_MAX;
for(int i = 0; i < successes + 4; i++) {
if(reprojectionError[i] < max) {
maxindex = i;
max = reprojectionError[i];
}
}
cerr << "Reprojection error: " << max/successes << endl;
CvMat * rotation = cvCreateMat(1, 3, CV_32FC1);
CvMat * translation = cvCreateMat(1, 3, CV_32FC1);
for(int i = 0; i < 3; i++) {
CV_MAT_ELEM(*rotation, float, 0, i) = CV_MAT_ELEM(*rotation_vectors_temp, float, maxindex, i);
CV_MAT_ELEM(*translation, float, 0, i) = CV_MAT_ELEM(*translation_vectors_temp, float, maxindex, i);
}
string file = dir + "Rotation" + substring + ".xml";
cvSave(file.c_str(), rotation);
file = dir + "Translation" + substring + ".xml";
cvSave(file.c_str(), translation);
}
/**
* Calculates the extrinsic parameters given a set of feature matches using the
* mean and median method.
*/
void calculateExtrinsics(CvMat * rotation_vectors_temp, CvMat * translation_vectors_temp, int successes, string dir, bool quiet, string substring) {
CvMat* rotation_vectors = cvCreateMat(successes, 3, CV_32FC1);
CvMat* translation_vectors = cvCreateMat(successes, 3, CV_32FC1);
CvMat* vectors = cvCreateMat(successes, 6, CV_32FC1);
CvMat* rotation_vector_mean = cvCreateMat(1, 3, CV_32FC1);
CvMat* translation_vector_mean = cvCreateMat(1, 3, CV_32FC1);
CvMat* rotation_vector_median = cvCreateMat(1, 3, CV_32FC1);
CvMat* translation_vector_median = cvCreateMat(1, 3, CV_32FC1);
CvMat* median = cvCreateMat(1, 6, CV_32FC1);
for (int t = 0; t < 3; t++) {
CV_MAT_ELEM(*rotation_vector_mean,float,0,t) = 0;
CV_MAT_ELEM(*translation_vector_mean,float,0,t) = 0;
CV_MAT_ELEM(*rotation_vector_median,float,0,t) = 0;
CV_MAT_ELEM(*translation_vector_median,float,0,t) = 0;
CV_MAT_ELEM(*median,float,0,t) = 0;
CV_MAT_ELEM(*median,float,0,t + 3) = 0;
}
for (int h = 0; h < successes; h++) {
for(int t = 0; t < 3; t++) {
CV_MAT_ELEM(*rotation_vectors,float,h,t) = CV_MAT_ELEM(*rotation_vectors_temp,float,h,t);
CV_MAT_ELEM(*rotation_vector_mean,float,0,t) += CV_MAT_ELEM(*rotation_vectors,float,h,t);
CV_MAT_ELEM(*translation_vectors,float,h,t) = CV_MAT_ELEM(*translation_vectors_temp,float,h,t);
CV_MAT_ELEM(*translation_vector_mean,float,0,t) += CV_MAT_ELEM(*translation_vectors,float,h,t);
CV_MAT_ELEM(*vectors,float,h,t) = CV_MAT_ELEM(*translation_vectors_temp,float,h,t);
CV_MAT_ELEM(*vectors,float,h,t + 3) = CV_MAT_ELEM(*rotation_vectors_temp,float,h,t);
cout << CV_MAT_ELEM(*translation_vectors,float,h,t) << " ";
}
cout << endl;
}
// mean
cout << "Getting mean vector" << endl;
for(int t = 0; t < 3; t++) {
CV_MAT_ELEM(*rotation_vector_mean,float,0,t) /= successes;
CV_MAT_ELEM(*rotation_vectors_temp,float,successes,t) = CV_MAT_ELEM(*rotation_vector_mean,float,0,t);
CV_MAT_ELEM(*translation_vector_mean,float,0,t) /= successes;
CV_MAT_ELEM(*translation_vectors_temp,float,successes,t) = CV_MAT_ELEM(*translation_vector_mean,float,0,t);
cout << CV_MAT_ELEM(*translation_vectors_temp,float,successes,t) << " ";
}
cout << endl;
// getting the median vectors
// median
cout << "Getting median vector" << endl;
filterMedian(vectors, successes, 1, median);
cout << "Got median vector" << endl;
for(int t = 0; t < 3; t++) {
CV_MAT_ELEM(*translation_vector_median,float,0,t) = CV_MAT_ELEM(*median,float,0,t);
CV_MAT_ELEM(*rotation_vector_median,float,0,t) = CV_MAT_ELEM(*median,float,0,t+3);
CV_MAT_ELEM(*rotation_vectors_temp,float,successes + 1,t) = CV_MAT_ELEM(*rotation_vector_median,float,0,t);
CV_MAT_ELEM(*translation_vectors_temp,float,successes + 1,t) = CV_MAT_ELEM(*translation_vector_median,float,0,t);
}
// filtered median
cout << "Getting filtered median vector" << endl;
filterMedian(vectors, successes, 4, median);
cout << "Got filtered median vector" << endl;
for(int t = 0; t < 3; t++) {
CV_MAT_ELEM(*rotation_vectors_temp,float,successes + 2,t) = CV_MAT_ELEM(*median,float,0,t+3);
CV_MAT_ELEM(*translation_vectors_temp,float,successes + 2,t) = CV_MAT_ELEM(*median,float,0,t);
}
// elementwise median
// finding the median of rotation and translation
// sorting the rotation vectors element by element
sortElementByElement(vectors, 6, successes);
/*
sortElementByElement(translation_vectors, 3, successes);
if(!quiet) {
cout << "number of successes : " << successes << endl;
cout << "rotation vectors are" << endl;
for (int i = 0; i < successes; i++) {
cout << CV_MAT_ELEM(*rotation_vectors,float,i,0) << " "
<<CV_MAT_ELEM(*rotation_vectors,float,i,1) << " "
<<CV_MAT_ELEM(*rotation_vectors,float,i,2) << endl;
}
cout << "translation vectors are" << endl;
for (int i = 0; i < successes; i++) {
cout << CV_MAT_ELEM(*translation_vectors,float,i,0) << " "
<<CV_MAT_ELEM(*translation_vectors,float,i,1) << " "
<<CV_MAT_ELEM(*translation_vectors,float,i,2) << endl;
}
}
*/
int index = successes / 2;
for(int t = 0; t < 3; t++) {
CV_MAT_ELEM(*translation_vector_median,float,0,t) = CV_MAT_ELEM(*translation_vectors,float,index,t);
CV_MAT_ELEM(*rotation_vector_median,float,0,t) = CV_MAT_ELEM(*rotation_vectors,float,index,t);
}
for(int t = 0; t < 3; t++) {
CV_MAT_ELEM(*translation_vectors_temp,float,successes + 3,t) = CV_MAT_ELEM(*vectors,float,index,t);
CV_MAT_ELEM(*rotation_vectors_temp,float,successes + 3,t) = CV_MAT_ELEM(*vectors,float,index,t+3);
}
cout << "mean rotation vector is :" << endl;
for (int f = 0; f < 3; f++) {
cout << CV_MAT_ELEM(*rotation_vector_mean,float,0,f) << " ";
}
cout << endl;
cout << "mean translation vector is :" << endl;
for (int g = 0; g < 3; g++) {
cout << CV_MAT_ELEM(*translation_vector_mean,float,0,g) << " ";
}
cout << endl;
cout << "median rotation vector is :" << endl;
for (int ff = 0; ff < 3; ff++) {
cout << CV_MAT_ELEM(*rotation_vector_median,float,0,ff) << " ";
}
cout << endl;
cout << "median translation vector is :" << endl;
for (int gg = 0; gg < 3; gg++) {
cout << CV_MAT_ELEM(*translation_vector_median,float,0,gg) << " ";
}
cout << endl;
// write extrinsic parameters
string file = dir + "RotationMean" + substring + ".xml";
cvSave(file.c_str(), rotation_vector_mean);
file = dir + "TranslationMean" + substring + ".xml";
cvSave(file.c_str(), translation_vector_mean);
file = dir + "RotationMedian" + substring + ".xml";
cvSave(file.c_str(), rotation_vector_median);
file = dir + "TranslationMedian" + substring + ".xml";
cvSave(file.c_str(), translation_vector_median);
// cleanup
cvReleaseMat(&rotation_vector_mean);
cvReleaseMat(&translation_vector_mean);
cvReleaseMat(&rotation_vector_median);
cvReleaseMat(&translation_vector_median);
cout << "End calculateExtrinsics" << endl;
}
/**
* Main function for extrinsic calibration of laser scanner and camera.
*/
void ExtrCalibFunc(int board_w, int board_h, int start, int end, bool optical, bool chess, bool quiet, string dir, int scale) {
int nr_img = end - start + 1;
if (nr_img == 0) {
cout << "ImageCount is zero!" << endl;
return;
}
cvNamedWindow("Original Image", 0);
cvResizeWindow( "Original Image", 480, 640 );
cvNamedWindow("Final Result", 0);
cvResizeWindow( "Final Result", 480, 640 );
int corner_exp = board_w * board_h;
CvSize board_sz = cvSize(board_w, board_h);
CvSize size;
CvPoint3D32f* corners = new CvPoint3D32f[corner_exp];
//ALLOCATE STORAGE(depending upon the number of images in(in case if command line arguments are given )
//not on the basis of number of images in which all corner extracted/while in the other case the number is the same )
CvMat *intrinsic;
CvMat *distortion;
loadIntrinsicCalibration(intrinsic, distortion, dir, optical);
//for storing the rotations and translation vectors
CvMat* rotation_vectors_temp = cvCreateMat(nr_img + 4, 3, CV_32FC1);
CvMat* translation_vectors_temp = cvCreateMat(nr_img + 4, 3, CV_32FC1);
CvMat* points3D = cvCreateMat(nr_img, corner_exp, CV_32FC3);
CvMat* points2D = cvCreateMat(nr_img, corner_exp, CV_32FC2);
int successes = 0;
int tmp_corners = corner_exp;
for (int count = start; count <= end; count++) {
corner_exp = tmp_corners;
string i;
string p;
cout << "Reading data " << to_string(count, 3) << endl;
if(optical) {
//TODO i = dir + "/photo" + to_string(count, 3) + ".ppm";
i = dir + "/photo" + to_string(count, 3) + ".jpg";
} else {
i = dir + "/image" + to_string(count, 3) + ".ppm";
}
p = dir + "cali/scan" + to_string(count,3) + ".3d";
cout << p << endl;
// Load points from scan
bool scan_cali = readPoints(p, corners, corner_exp);
if(!scan_cali) continue;
// Load image and detect corners
IplImage* image1 = cvLoadImage(i.c_str(), -1);
if (!image1) {
cout << "image cannot be loaded" << endl;
return;
}
cvShowImage("Original Image", image1);
IplImage* image2 = cvCloneImage(image1);
cvUndistort2(image1, image2, intrinsic, distortion);
cvShowImage("Final Result", image2);
double point_array2[corner_exp][2];
IplImage *image;
if(chess) {
image = detectCorners(image1, corner_exp, board_h, board_w, quiet, point_array2, scale);
} else {
image = detectBlobs(image1, tmp_corners, board_h, board_w, quiet, point_array2);
}
if (!image) {
cout << "image cannot be loaded" << endl;
return;
}
//drawing the lines on the image now
if(tmp_corners == corner_exp) {
cout << tmp_corners << " " << corner_exp << endl;
drawLines(point_array2, corner_exp, image);
CvMat* image_points = cvCreateMat(corner_exp, 2, CV_32FC1);
CvMat* object_points = cvCreateMat(corner_exp, 3, CV_32FC1);
CvMat* Rotation = cvCreateMat(1, 3, CV_32FC1);
CvMat* Translation = cvCreateMat(1, 3, CV_32FC1);
char in;// = 'y';
size = cvGetSize(image);
//appending corner data to a generic data structure for all images
for (int j = 0; j < corner_exp; ++j) {
CV_MAT_ELEM(*image_points, float,j,0) = (float) point_array2[j][0];
CV_MAT_ELEM(*image_points, float,j,1) = (float) point_array2[j][1];
CV_MAT_ELEM(*object_points,float,j,0) = corners[j].x;
CV_MAT_ELEM(*object_points,float,j,1) = corners[j].y;
CV_MAT_ELEM(*object_points,float,j,2) = corners[j].z;
/*
cout << (float)point_array2[j][0] << " ";
cout << (float)point_array2[j][1] << " ";
cout << corners[j].x << " ";
cout << corners[j].y << " ";
cout << corners[j].z << endl;
*/
}
cvFindExtrinsicCameraParams2(object_points, image_points, intrinsic, distortion, Rotation, Translation);
// append data to vectors
/*
if(CV_MAT_ELEM(*Translation, float, 0, 1) > -30)
if(CV_MAT_ELEM(*Translation, float, 0, 0) < -20)
if(CV_MAT_ELEM(*Translation, float, 0, 1) > -4 )
if(CV_MAT_ELEM(*Translation, float, 0, 1) < -00)
if(CV_MAT_ELEM(*Translation, float, 0, 2) > -12)
if(CV_MAT_ELEM(*Translation, float, 0, 2) < -8 ) {
*/
if(!quiet) cout << "Rotation is:" << endl;
for (int row = 0; row < 1; row++) {
for (int col = 0; col < 3; col++) {
if(!quiet) cout << CV_MAT_ELEM( *Rotation, float, row, col ) << " ";
CV_MAT_ELEM( *rotation_vectors_temp, float, successes, col ) = CV_MAT_ELEM( *Rotation, float, row, col );
}
if(!quiet) cout << endl;
}
if(!quiet) cout << "Translation is:" << endl;
for (int row = 0; row < 1; row++) {
for (int col = 0; col < 3; col++) {
if(!quiet) cout << CV_MAT_ELEM( *Translation, float, row, col ) << " ";
CV_MAT_ELEM( *translation_vectors_temp, float, successes, col ) = CV_MAT_ELEM( *Translation, float, row, col );
}
if(!quiet) cout << endl;
}
cout << "\nDo you want to use the data from this image ('y' or 'n'). 'x' aborts the calibration? : ";
int c = cvWaitKey(100);
cin >> in;
if (c == 27) {
break;
}
if (in == 'y') {
for (int j = 0; j < corner_exp; ++j) {
CV_MAT_ELEM(*points2D, CvPoint2D32f, successes, j).x = (float)point_array2[j][0];
CV_MAT_ELEM(*points2D, CvPoint2D32f, successes, j).y = (float)point_array2[j][1];
CV_MAT_ELEM(*points3D, CvPoint3D32f, successes, j).x = corners[j].x;
CV_MAT_ELEM(*points3D, CvPoint3D32f, successes, j).y = corners[j].y;
CV_MAT_ELEM(*points3D, CvPoint3D32f, successes, j).z = corners[j].z;
}
successes++;
// }
} else if(in == 'x') {
break;
}
//cvReleaseImage(&image);
cvReleaseMat(&image_points);
cvReleaseMat(&object_points);
cvReleaseMat(&Rotation);
cvReleaseMat(&Translation);
}
cvReleaseImage(&image);
cvReleaseImage(&image1);
cvReleaseImage(&image2);
}//for loop for imagecount
cvDestroyWindow("Original Image");
cvDestroyWindow("Final Result");
cout << "Number of successes: " << successes << endl;
// Now calculating mean and median rotation and trans
string substring = optical? "Optical" : "";
calculateExtrinsics(rotation_vectors_temp, translation_vectors_temp, successes, dir, quiet, substring);
calculateExtrinsicsWithReprojectionCheck(points2D, points3D, rotation_vectors_temp, translation_vectors_temp, distortion, intrinsic, corner_exp, successes, dir, quiet, substring);
cvReleaseMat(&intrinsic);
cvReleaseMat(&distortion);
cvReleaseMat(&translation_vectors_temp);
cvReleaseMat(&rotation_vectors_temp);
cvReleaseMat(&points2D);
cvReleaseMat(&points3D);
}
//bool readFrames(char * dir, int index, double * rPos, rPosTheta) {
//bool readFrames(char * dir, int index, double * inMatrix, double * rPos) {
bool readFrames(const char * dir, int index, double * tMatrix, CvMat * inMatrix, CvMat * rPos) {
char frameFileName[255];
snprintf(frameFileName,255,"%sscan%.3d.frames",dir,index);
ifstream pose_in;
pose_in.open(frameFileName);
if (!pose_in.good()) return false; // no more files in the directory
cout << "Reading frame " << frameFileName << "..." << endl;
double tmpMatrix[17];
while(pose_in.good()) {
for (unsigned int i = 0; i < 17; pose_in >> tMatrix[i++]);
}
M4inv(tMatrix, tmpMatrix);
CV_MAT_ELEM(*inMatrix, float,0,0) = tmpMatrix[10];
CV_MAT_ELEM(*inMatrix, float,0,1) = -tmpMatrix[2];
CV_MAT_ELEM(*inMatrix, float,0,2) = tmpMatrix[6];
CV_MAT_ELEM(*inMatrix, float,1,0) = -tmpMatrix[8];
CV_MAT_ELEM(*inMatrix, float,1,1) = tmpMatrix[0];
CV_MAT_ELEM(*inMatrix, float,1,2) = -tmpMatrix[4];
CV_MAT_ELEM(*inMatrix, float,2,0) = tmpMatrix[9];
CV_MAT_ELEM(*inMatrix, float,2,1) = -tmpMatrix[1];
CV_MAT_ELEM(*inMatrix, float,2,2) = tmpMatrix[5];
CV_MAT_ELEM(*rPos, float,0,0) = tmpMatrix[14];
CV_MAT_ELEM(*rPos, float,1,0) = -tmpMatrix[12];
CV_MAT_ELEM(*rPos, float,2,0) = tmpMatrix[13];
return true;
}
void writeGlobalCameras(int start, int end, bool optical, bool quiet, string dir,
IOType type, int scale, double rot_angle, double minDist, double maxDist,
bool correction, int neighborhood, int method) {
string file;
int nr_img = end - start + 1;
if (nr_img < 1) {
cout << "ImageCount is zero!" << endl;
return;
}
CvMat *Rotation;
CvMat *Translation;
loadExtrinsicCalibration(Rotation, Translation, dir, method, optical);
double starttime = GetCurrentTimeInMilliSec();
stringstream outdat;
string outdir = dir + "/labscan-map";
#ifdef _MSC_VER
int success = mkdir(outdir.c_str());
#else
int success = mkdir(outdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO);
#endif
if(success == 0) {
cout << "Writing scans to " << outdir << endl;
} else if(errno == EEXIST) {
cout << "Directory " << outdir << " exists already. CONTINUE" << endl;
} else {
cerr << "Creating directory " << outdir << " failed" << endl;
exit(1);
}
for (int count = start; count <= end; count++) {
// filling the rotation matrix
// reading the frame files 3D points and projecting them back to 2d
CvMat* inMatrix = cvCreateMat(3,3,CV_32FC1);
CvMat* rPos = cvCreateMat(3,1,CV_32FC1);
double * tMatrix = new double[17];
readFrames(dir.c_str(), count, tMatrix, inMatrix, rPos);
// TODO make sure correct points are printed
/*
CV_MAT_ELEM(*point_3Dcloud, float,j,0) = p.z;
CV_MAT_ELEM(*point_3Dcloud, float,j,1) = -p.x;
CV_MAT_ELEM(*point_3Dcloud, float,j,2) = p.y;
*/
// write colored data
int nrP360 = 9;
for(int p = 0; p < nrP360; p++) {
double angle = rot_angle * (p%nrP360);
int count0 = count * nrP360 + p;
string outname = outdir + "/image" + to_string(count0, 3) + ".mat";
fstream outfile;
outfile.open(outname.c_str(), ios::out);
CvMat* RotationI = cvCreateMat(3,1,CV_32FC1);
CvMat* TranslationI = cvCreateMat(3,1,CV_32FC1);
CvMat* rod40 = cvCreateMat(3,1,CV_32FC1);
cout << "Angle: " << angle << " " << rad(angle) << endl;
CV_MAT_ELEM(*rod40,float,0,0) = 0.0;
CV_MAT_ELEM(*rod40,float,1,0) = 0.0;
CV_MAT_ELEM(*rod40,float,2,0) = 1.0 * rad(angle);
CvMat* t40 = cvCreateMat(3,1,CV_32FC1);
CV_MAT_ELEM(*t40,float,0,0) = 0.0;
CV_MAT_ELEM(*t40,float,1,0) = 0.0;
CV_MAT_ELEM(*t40,float,2,0) = 0.0;
CvMat* rot_tmp = cvCreateMat(3,3,CV_32FC1);
CvMat* rod_comI = cvCreateMat(3,1,CV_32FC1);
CvMat* t_comI = cvCreateMat(3,1,CV_32FC1);
CvMat* rod_com = cvCreateMat(1,3,CV_32FC1);
CvMat* t_com = cvCreateMat(1,3,CV_32FC1);
for(int w = 0; w < 3; w++) {
CV_MAT_ELEM(*RotationI,float,w,0) = CV_MAT_ELEM(*Rotation,float,0,w);
CV_MAT_ELEM(*TranslationI,float,w,0) = CV_MAT_ELEM(*Translation,float,0,w);
}
cout << "Final Rotation" << endl;
cvComposeRT(rod40, t40, RotationI, TranslationI, rod_comI, t_comI);
for(int w = 0; w < 3; w++) {
CV_MAT_ELEM(*rod_com,float,0,w) = CV_MAT_ELEM(*rod_comI,float,w,0);
CV_MAT_ELEM(*t_com,float,0,w) = CV_MAT_ELEM(*t_comI,float,w,0);
/*
cout << CV_MAT_ELEM(*RotationI,float,w,0) << " ";
cout << CV_MAT_ELEM(*TranslationI,float,w,0) << " ";
cout << CV_MAT_ELEM(*rod40,float,w,0) << " ";
cout << CV_MAT_ELEM(*t40,float,w,0) << " ";
cout << CV_MAT_ELEM(*rod_comI,float,w,0) << " ";
cout << CV_MAT_ELEM(*t_comI,float,w,0) << endl;
*/
}
cout << endl;
// transform into global coordinate system (inMatrix, rPos)
CvMat* t_globalI = cvCreateMat(3,1,CV_32FC1);
CvMat* r_globalI = cvCreateMat(3,1,CV_32FC1);
CvMat* rodglob = cvCreateMat(3,1,CV_32FC1);
cvRodrigues2(inMatrix, rodglob);
cvComposeRT(rodglob, rPos, rod_comI, t_comI, r_globalI, t_globalI);
//cvRodrigues2(r_globalI, rot_tmp);
cvReleaseMat(&rod40);
cvReleaseMat(&RotationI);
cvReleaseMat(&TranslationI);
cvReleaseMat(&t40);
cvReleaseMat(&rod_comI);
cvReleaseMat(&rod_com);
cvReleaseMat(&t_com);
cvReleaseMat(&t_comI);
cout << "Done projecting points" << endl;
CvMat* rotmatrix = cvCreateMat(3,3,CV_32FC1);
cvRodrigues2(r_globalI, rotmatrix);
//cvSave(outname.c_str(), rotmatrix);
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 3; j++) {
outfile << CV_MAT_ELEM(*rotmatrix,float,i,j) << " ";
}
outfile << CV_MAT_ELEM(*t_globalI,float,i,0) << endl;
}
outfile << "0 0 0 1" << endl;
// checking whether projection lies within the image boundaries
cvReleaseMat(&rot_tmp);
outfile.close();
outfile.flush();
}
}
cvReleaseMat(&Rotation);
cvReleaseMat(&Translation);
}
/**
* Read scans
* Read frames
*/
void calculateGlobalCameras(int start, int end, bool optical, bool quiet, string dir,
IOType type, int scale, double rot_angle, double minDist, double maxDist,
bool correction, int neighborhood, int method) {
int nr_img = end - start + 1;
if (nr_img < 1) {
cout << "ImageCount is zero!" << endl;
return;
}
CvMat *intrinsic;
CvMat *distortion;
loadIntrinsicCalibration(intrinsic, distortion, dir, optical);
CvMat *Rotation;
CvMat *Translation;
loadExtrinsicCalibration(Translation, Rotation, dir, method, optical);
CvMat* undistort = cvCreateMat(5,1,CV_32FC1);
for (int hh = 0; hh < 5; hh++) {
CV_MAT_ELEM(*undistort, float,hh,0) = 0;
}
double starttime = GetCurrentTimeInMilliSec();
stringstream outdat;
int pointcnt = 0;
string outdir = dir + "/labscan-map";
#ifdef _MSC_VER
int success = mkdir(outdir.c_str());
#else
int success = mkdir(outdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO);
#endif
if(success == 0) {
cout << "Writing scans to " << outdir << endl;
} else if(errno == EEXIST) {
cout << "Directory " << outdir << " exists already. CONTINUE" << endl;
} else {
cerr << "Creating directory " << outdir << " failed" << endl;
exit(1);
}
for (int count = start; count <= end; count++) {
// filling the rotation matrix
CvMat* point_3Dcloud;
CvMat* point_2Dcloud;
CvMat* undistort_2Dcloud;
// reading the 3D points and projecting them back to 2d
Scan::openDirectory(false, dir, type, count, count);
Scan::allScans[0]->setRangeFilter(-1, -1);
Scan::allScans[0]->setSearchTreeParameter(simpleKD, false);
Scan::allScans[0]->setReductionParameter(-1, 0);
//Scan::readScans(type, count, count, dir, maxDist, minDist, 0);
DataXYZ reduced = Scan::allScans[0]->get("xyz reduced");
int red_size = reduced.size();
point_3Dcloud = cvCreateMat(red_size, 3, CV_32FC1);
cout << "Points: " << red_size << endl;
point_2Dcloud = cvCreateMat(red_size, 2, CV_32FC1);
undistort_2Dcloud = cvCreateMat(red_size, 2, CV_32FC1);
cout << "readScans done" << endl;
// read frames
//double * rPos = new double[3];
//double * rPosTheta= new double[3];
//readFrames(dir, count, rPos, rPosTheta);
CvMat* inMatrix = cvCreateMat(3,3,CV_32FC1);
CvMat* rPos = cvCreateMat(3,1,CV_32FC1);
double * tMatrix = new double[17];
readFrames(dir.c_str(), count, tMatrix, inMatrix, rPos);
Scan::allScans[0]->transform(tMatrix, Scan::INVALID, 0);
for (int j = 0; j < red_size; j++) {
Point p(reduced[j]);
// TODO make sure correct points are printed
CV_MAT_ELEM(*point_3Dcloud, float,j,0) = p.z;
CV_MAT_ELEM(*point_3Dcloud, float,j,1) = -p.x;
CV_MAT_ELEM(*point_3Dcloud, float,j,2) = p.y;
}
int nr_points = red_size;
cout << "Number of points read: " << red_size << endl;
delete Scan::allScans[0];
Scan::allScans.clear();
// write colored data
string outname = outdir + "/scan" + to_string(count, 3) + ".3d";
fstream outfile;
outfile.open(outname.c_str(), ios::out);
int nrP360 = 9;
for(int p = 0; p < nrP360; p++) {
//for(int p = 4; p < 5; p++) {
//double angle = rot_angle * (p%nrP360) + 2.0;
double angle = rot_angle * (p%nrP360);
cout << angle << endl;
// loading images
int count0 = count * nrP360 + p;
string t, t0;
if(optical) {
//TODO t = dir + "/photo" + to_string(count, 3) + ".ppm";
//t = dir + "/photo" + to_string(count0, 3) + "_2.jpg";
//t = dir + "/photo" + to_string(count0, 3) + "_90.jpg";
t = dir + "/photo" + to_string(count0, 3) + ".jpg";
//t = dir + "/photo" + to_string(count0, 3) + "_1.jpg";
} else {
t = dir + "/image" + to_string(count0, 3) + ".ppm";
}
IplImage* image = cvLoadImage(t.c_str(), -1);
if (!image) {
cout << "first image " << t << " cannot be loaded" << endl;
return;
}
CvSize size = cvGetSize(image);
image = resizeImage(image, scale);
/** TODO!!!
* Transform rPos and rPosTheta into OpenCV (Rodrigues)
* cvComposeRT with rod_comI and t_comI
*/
// rotate Rotation and Translation
CvMat* RotationI = cvCreateMat(3,1,CV_32FC1);
CvMat* TranslationI = cvCreateMat(3,1,CV_32FC1);
CvMat* rod40 = cvCreateMat(3,1,CV_32FC1);
cout << "Angle: " << angle << " " << rad(angle) << endl;
CV_MAT_ELEM(*rod40,float,0,0) = 0.0;
CV_MAT_ELEM(*rod40,float,1,0) = 0.0;
CV_MAT_ELEM(*rod40,float,2,0) = 1.0 * rad(angle);
cout << "tmp" << endl;
CvMat* t40 = cvCreateMat(3,1,CV_32FC1);
CV_MAT_ELEM(*t40,float,0,0) = 0.0;
CV_MAT_ELEM(*t40,float,1,0) = 0.0;
CV_MAT_ELEM(*t40,float,2,0) = 0.0;
cout << "tmp2" << endl;
CvMat* rot_tmp = cvCreateMat(3,3,CV_32FC1);
CvMat* rod_comI = cvCreateMat(3,1,CV_32FC1);
CvMat* t_comI = cvCreateMat(3,1,CV_32FC1);
CvMat* rod_com = cvCreateMat(1,3,CV_32FC1);
CvMat* t_com = cvCreateMat(1,3,CV_32FC1);
cout << "tmp3" << endl;
for(int w = 0; w < 3; w++) {
CV_MAT_ELEM(*RotationI,float,w,0) = CV_MAT_ELEM(*Rotation,float,0,w);
CV_MAT_ELEM(*TranslationI,float,w,0) = CV_MAT_ELEM(*Translation,float,0,w);
}
cout << endl;
cout << "Final Rotation" << endl;
cvComposeRT(rod40, t40, RotationI, TranslationI, rod_comI, t_comI);
for(int w = 0; w < 3; w++) {
CV_MAT_ELEM(*rod_com,float,0,w) = CV_MAT_ELEM(*rod_comI,float,w,0);
CV_MAT_ELEM(*t_com,float,0,w) = CV_MAT_ELEM(*t_comI,float,w,0);
cout << CV_MAT_ELEM(*RotationI,float,w,0) << " ";
cout << CV_MAT_ELEM(*TranslationI,float,w,0) << " ";
cout << CV_MAT_ELEM(*rod40,float,w,0) << " ";
cout << CV_MAT_ELEM(*t40,float,w,0) << " ";
cout << CV_MAT_ELEM(*rod_comI,float,w,0) << " ";
cout << CV_MAT_ELEM(*t_comI,float,w,0) << endl;
}
cout << endl;
cvRodrigues2(rod_comI, rot_tmp);
// transform into global coordinate system (inMatrix, rPos)
CvMat* t_globalI = cvCreateMat(3,1,CV_32FC1);
CvMat* r_globalI = cvCreateMat(3,1,CV_32FC1);
CvMat* rodglob = cvCreateMat(3,1,CV_32FC1);
cvRodrigues2(inMatrix, rodglob);
cvComposeRT(rodglob, rPos, rod_comI, t_comI, r_globalI, t_globalI);
//TODO verify order
cvRodrigues2(r_globalI, rot_tmp);
//cvComposeRT(rod_comI, t_comI, rodglob, rPos, r_globalI, t_globalI);
cvProjectPoints2(point_3Dcloud, r_globalI, t_globalI, intrinsic, distortion, point_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0);
cvProjectPoints2(point_3Dcloud, r_globalI, t_globalI, intrinsic, undistort, undistort_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0);
// END TODO
// Project Points
/*
cvProjectPoints2(point_3Dcloud, rod_comI, t_comI, intrinsic, distortion, point_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0);
cvProjectPoints2(point_3Dcloud, rod_comI, t_comI, intrinsic, undistort, undistort_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0);
*/
cvReleaseMat(&rod40);
cvReleaseMat(&RotationI);
cvReleaseMat(&TranslationI);
cvReleaseMat(&t40);
cvReleaseMat(&rod_comI);
cvReleaseMat(&rod_com);
cvReleaseMat(&t_com);
cvReleaseMat(&t_comI);
cout << "Done projecting points" << endl;
//for counting how many points get mapped to first and second image file
int point_map1 = 0; // #points mapped to first image
int point_map2 = 0; // " " " second image
int point_map3 = 0; // " " " second image
// checking whether projection lies within the image boundaries
cout << "Now project points" << endl;
for (int k = 0; k < nr_points; k++) {
float px = CV_MAT_ELEM(*undistort_2Dcloud,float,k,0);
float py = CV_MAT_ELEM(*undistort_2Dcloud,float,k,1);
if (px < image->width - .5 && px >= 0 && py >= 0 && py < image->height - .5 ) {
point_map1++;
px = CV_MAT_ELEM(*point_2Dcloud,float,k,0);
py = CV_MAT_ELEM(*point_2Dcloud,float,k,1);
if (px < image->width - .5 && px >= 0 && py >= 0 && py < image->height - .5) {
point_map2++;
CvMat* tmp1 = cvCreateMat(1, 1, CV_32FC3);
CvMat* tmp2 = cvCreateMat(1, 1, CV_32FC3);
CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).x = CV_MAT_ELEM(*point_3Dcloud,float,k,0);
CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).y = CV_MAT_ELEM(*point_3Dcloud,float,k,1);
CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).z = CV_MAT_ELEM(*point_3Dcloud,float,k,2);
cvTransform(tmp1, tmp2, rot_tmp);
cvReleaseMat(&tmp1);
//double tmpz = CV_MAT_ELEM(*t_globalI, CV_32FC1, 2,0);
double tmpz = -CV_MAT_ELEM(*t_globalI, float, 2,0);
//double tmpz = CV_MAT_ELEM(*TranslationI, float, 2,0);
if(CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).z < tmpz) {
cvReleaseMat(&tmp2);
continue;
}
//cout << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0, 0).x << " "
// << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0, 0).y << " "
// << CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0, 0).z << endl;
cvReleaseMat(&tmp2);
/*
outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,0) << " "
<< CV_MAT_ELEM(*point_3Dcloud,float,k,1) << " "
<< CV_MAT_ELEM(*point_3Dcloud,float,k,2) << " "
<< CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).x << " "
<< CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).y << " "
<< CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).z << endl;
*/
point_map3++;
int ppx = 0;
int ppy = 0;
if (px - int(px) < .5) {
ppx = int(px);
} else {
ppx = int(px) + 1;
}
if (py - int(py) < .5) {
ppy = int(py);
} else {
ppy = int(py) + 1;
}
CvScalar c;
c = cvGet2D(image, ppy, ppx);
// check for overlap
/*
if(correction) {
vector<float> temp_vec;
float p_id = 1; // 1 for pixel, 0 for neighboring pixel
temp_vec.push_back(-(CV_MAT_ELEM(*point_3Dcloud,float,k,1)));
temp_vec.push_back((CV_MAT_ELEM(*point_3Dcloud,float,k,2)));
temp_vec.push_back((CV_MAT_ELEM(*point_3Dcloud,float,k,0)));
temp_vec.push_back(c.val[2]);
temp_vec.push_back(c.val[1]);
temp_vec.push_back(c.val[0]);
temp_vec.push_back(p_id);
if(neighborhood > 1) {
int limit = neighborhood / 2;
int lower_y = ppy - limit > 0 ? ppy - limit : 0;
int upper_y = ppy + limit < size.height ? ppy + limit : size.height - 1;
int lower_x = ppx - limit > 0 ? ppx - limit : 0;
int upper_x = ppx + limit < size.width ? ppx + limit : size.width - 1;
for (int y = lower_y; y < upper_y; y++) {
for (int x = lower_x; x < upper_x; x++) {
if(x == ppx && y == ppy) {
temp_vec[6] = 1;
} else {
temp_vec[6] = 0;
}
data1[y][x].push_back(temp_vec);
}
}
} else {
data1[ppy][ppx].push_back(temp_vec);
}
temp_vec.clear();
} else {
*/
// write all the data
outdat << -(CV_MAT_ELEM(*point_3Dcloud,float,k,1))<<" ";
outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,2)<<" ";
outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,0)<<" ";
if(optical) {
outdat << c.val[2] <<" "<< c.val[1]<<" "<<c.val[0]<<endl;
} else {
outdat << (c.val[0] - 1000.0)/10.0 << endl;
}
pointcnt++;
if(pointcnt > 100) {
outfile.write(outdat.str().c_str(), outdat.str().size());
pointcnt = 0;
outdat.clear();
outdat.str("");
}
/*
outfile << -(CV_MAT_ELEM(*point_3Dcloud,float,k,1))<<" ";
outfile << CV_MAT_ELEM(*point_3Dcloud,float,k,2)<<" ";
outfile << CV_MAT_ELEM(*point_3Dcloud,float,k,0)<<" ";
if(optical) {
outfile << c.val[2] <<" "<< c.val[1]<<" "<<c.val[0]<<endl;
} else {
outfile << (c.val[0] - 1000.0)/10.0 << endl;
}
//outfile << c.val[2] <<" "<< c.val[1]<<" "<<c.val[0]<<endl;
*/
//}
}
// second image
}
}
cout << "Done sorting points" << endl;
// write data with overlap correction
if(correction) {
CorrectErrorAndWrite(data1, outfile, size, optical);
cout << "Done first image" << endl;
if(fabs(rot_angle) > 1) {
if(size.width > 0 && size.height > 0) {
CorrectErrorAndWrite(data2, outfile, size, optical);
}
}
}
cout << "Done correction" << endl;
// clean up
outfile.flush();
double endtime = GetCurrentTimeInMilliSec();
double time = endtime - starttime;
time = time/1000.0;
cout<<"runtime for scan " << count0 << " in seconds is: " << time << endl;
cout << point_map1 << " " << point_map2 << " " << point_map3 << endl;
cvReleaseImage(&image);
if(correction) {
for (int i = 0; i < size.height; i++) {
for (int j = 0; j < size.width; j++) {
data1[i][j].clear();
if(fabs(rot_angle) > 1) {
data2[i][j].clear();
}
}
}
}
cvReleaseMat(&rot_tmp);
}
outfile.close();
cvReleaseMat(&point_2Dcloud);
cvReleaseMat(&point_3Dcloud);
cvReleaseMat(&undistort_2Dcloud);
}
cvReleaseMat(&intrinsic);
cvReleaseMat(&distortion);
cvReleaseMat(&Rotation);
cvReleaseMat(&Translation);
cvReleaseMat(&undistort);
}
void loadIntrinsicCalibration(CvMat * intrinsic, CvMat * distortion, string dir, bool optical) {
string substring = optical? "Optical" : "";
string file = dir + "Intrinsics" + substring + ".xml";
intrinsic = (CvMat*) cvLoad(file.c_str());
file = dir + "Distortion" + substring + ".xml";
distortion = (CvMat*) cvLoad(file.c_str());
}
void loadExtrinsicCalibration(CvMat * Translation, CvMat * Rotation, string dir, int method, bool optical) {
string substring = optical? "Optical" : "";
string file;
switch(method) {
case 0:
file = dir + "Rotation" + substring + ".xml";
break;
case 1:
file = dir + "RotationMedian" + substring + ".xml";
break;
case 2:
file = dir + "RotationMean" + substring + ".xml";
break;
}
Rotation = (CvMat*) cvLoad(file.c_str());
switch(method) {
case 0:
file = dir + "Translation" + substring + ".xml";
break;
case 1:
file = dir + "TranslationMedian" + substring + ".xml";
break;
case 2:
file = dir + "TranslationMean" + substring + ".xml";
break;
}
Translation = (CvMat*) cvLoad(file.c_str());
}
void openOutputDirectory(string outdir) {
#ifdef _MSC_VER
int success = mkdir(outdir.c_str());
#else
int success = mkdir(outdir.c_str(), S_IRWXU|S_IRWXG|S_IRWXO);
#endif
if(success == 0) {
cout << "Writing scans to " << outdir << endl;
} else if(errno == EEXIST) {
cout << "Directory " << outdir << " exists already. CONTINUE" << endl;
} else {
cerr << "Creating directory " << outdir << " failed" << endl;
exit(1);
}
}
int openDirectory(CvMat* point_3Dcloud, string dir, IOType type, int count) {
// reading the 3D points and projecting them back to 2d
Scan::openDirectory(false, dir, type, count, count);
Scan::allScans[0]->setRangeFilter(-1, -1);
Scan::allScans[0]->setSearchTreeParameter(simpleKD, false);
Scan::allScans[0]->setReductionParameter(-1, 0);
//Scan::readScans(type, count, count, dir, maxDist, minDist, 0);
DataXYZ reduced = Scan::allScans[0]->get("xyz reduced");
int red_size = reduced.size();
point_3Dcloud = cvCreateMat(red_size, 3, CV_32FC1);
for (int j = 0; j < red_size; j++) {
Point p(reduced[j]);
CV_MAT_ELEM(*point_3Dcloud, float,j,0) = p.z;
CV_MAT_ELEM(*point_3Dcloud, float,j,1) = -p.x;
CV_MAT_ELEM(*point_3Dcloud, float,j,2) = p.y;
}
return red_size;
}
void loadImage(IplImage *image, string dir, int count0, bool optical, int scale){
string t, t0;
if(optical) {
t = dir + "/photo" + to_string(count0, 3) + ".jpg";
} else {
t = dir + "/image" + to_string(count0, 3) + ".ppm";
}
image = cvLoadImage(t.c_str(), -1);
if (!image) {
cout << "first image " << t << " cannot be loaded" << endl;
exit(0);
}
image = resizeImage(image, scale);
}
void calculateGlobalPoses(CvMat *Translation, CvMat *Rotation, CvMat *t_comI,
CvMat *rod_comI, double angle, CvMat *rot_tmp) {
CvMat* RotationI = cvCreateMat(3,1,CV_32FC1);
CvMat* TranslationI = cvCreateMat(3,1,CV_32FC1);
CvMat* rod40 = cvCreateMat(3,1,CV_32FC1);
//cout << "Angle: " << angle << " " << rad(angle) << endl;
CV_MAT_ELEM(*rod40,float,0,0) = 0.0;
CV_MAT_ELEM(*rod40,float,1,0) = 0.0;
CV_MAT_ELEM(*rod40,float,2,0) = 1.0 * rad(angle);
//cout << "tmp" << endl;
CvMat* t40 = cvCreateMat(3,1,CV_32FC1);
CV_MAT_ELEM(*t40,float,0,0) = 0.0;
CV_MAT_ELEM(*t40,float,1,0) = 0.0;
CV_MAT_ELEM(*t40,float,2,0) = 0.0;
rot_tmp = cvCreateMat(3,3,CV_32FC1);
//CvMat* rot_tmp = cvCreateMat(3,3,CV_32FC1);
rod_comI = cvCreateMat(3,1,CV_32FC1);
t_comI = cvCreateMat(3,1,CV_32FC1);
CvMat* rod_com = cvCreateMat(1,3,CV_32FC1);
CvMat* t_com = cvCreateMat(1,3,CV_32FC1);
for(int w = 0; w < 3; w++) {
CV_MAT_ELEM(*RotationI,float,w,0) = CV_MAT_ELEM(*Rotation,float,0,w);
CV_MAT_ELEM(*TranslationI,float,w,0) = CV_MAT_ELEM(*Translation,float,0,w);
}
//cout << endl;
//cout << "Final Rotation" << endl;
cvComposeRT(rod40, t40, RotationI, TranslationI, rod_comI, t_comI);
for(int w = 0; w < 3; w++) {
CV_MAT_ELEM(*rod_com,float,0,w) = CV_MAT_ELEM(*rod_comI,float,w,0);
CV_MAT_ELEM(*t_com,float,0,w) = CV_MAT_ELEM(*t_comI,float,w,0);
/*
cout << CV_MAT_ELEM(*RotationI,float,w,0) << " ";
cout << CV_MAT_ELEM(*TranslationI,float,w,0) << " ";
cout << CV_MAT_ELEM(*rod40,float,w,0) << " ";
cout << CV_MAT_ELEM(*t40,float,w,0) << " ";
cout << CV_MAT_ELEM(*rod_comI,float,w,0) << " ";
cout << CV_MAT_ELEM(*t_comI,float,w,0) << endl;
*/
}
//cout << endl;
cvRodrigues2(rod_comI, rot_tmp);
cvReleaseMat(&rod40);
cvReleaseMat(&RotationI);
cvReleaseMat(&TranslationI);
cvReleaseMat(&t40);
cvReleaseMat(&rod_com);
cvReleaseMat(&t_com);
}
/**
* Main function for projecting the 3D points onto the corresponding image and
* associating temperature values to the data points.
*/
void ProjectAndMap(int start, int end, bool optical, bool quiet, string dir,
IOType type, int scale, double rot_angle, double minDist, double maxDist,
bool correction, int neighborhood, int method) {
int nr_img = end - start + 1;
if (nr_img < 1) {
cout << "ImageCount is zero!" << endl;
return;
}
CvMat *distortion;
CvMat *intrinsic;
CvMat *Translation;
CvMat *Rotation;
loadExtrinsicCalibration(Translation, Rotation,dir,method,optical);
loadIntrinsicCalibration(intrinsic,distortion,dir,optical);
CvMat* undistort = cvCreateMat(5,1,CV_32FC1);
for (int hh = 0; hh < 5; hh++) {
CV_MAT_ELEM(*undistort, float,hh,0) = 0;
}
double starttime = GetCurrentTimeInMilliSec();
stringstream outdat;
int pointcnt = 0;
string outdir = dir + "/labscan-map";
openOutputDirectory(outdir);
for (int count = start; count <= end; count++) {
// filling the rotation matrix
CvMat *point_3Dcloud;
int nr_points = openDirectory(point_3Dcloud, dir, type, count);
CvMat* point_2Dcloud = cvCreateMat(nr_points, 2, CV_32FC1);
CvMat* undistort_2Dcloud = cvCreateMat(nr_points, 2, CV_32FC1);
cout << "Number of points read: " << nr_points << endl;
delete Scan::allScans[0];
Scan::allScans.clear();
// write colored data
string outname = outdir + "/scan" + to_string(count, 3) + ".3d";
fstream outfile;
outfile.open(outname.c_str(), ios::out);
int nrP360 = 9;
for(int p = 0; p < nrP360; p++) {
//for(int p = 0; p < 9; p++) {
//double angle = rot_angle * (p%nrP360) + 2.0;
double angle = rot_angle * (p%nrP360);
cout << angle << endl;
// loading images
int count0 = count * nrP360 + p;
IplImage *image;
loadImage(image, dir, count0, optical, scale);
CvSize size = cvGetSize(image);
// rotate Rotation and Translation
CvMat* rod_comI;
CvMat* t_comI;
CvMat* rot_tmp = cvCreateMat(3,3,CV_32FC1);
calculateGlobalPoses(Translation, Rotation, t_comI, rod_comI, angle, rot_tmp);
// Project Points
cvProjectPoints2(point_3Dcloud, rod_comI, t_comI, intrinsic, distortion, point_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0);
cvProjectPoints2(point_3Dcloud, rod_comI, t_comI, intrinsic, undistort, undistort_2Dcloud, NULL, NULL, NULL, NULL, NULL, 0);
cvReleaseMat(&t_comI);
cvReleaseMat(&rod_comI);
cout << "Done projecting points" << endl;
//for counting how many points get mapped to first and second image file
int point_map1 = 0; // #points mapped to first image
int point_map2 = 0; // " " " second image
int point_map3 = 0; // " " " second image
// checking whether projection lies within the image boundaries
cout << "Now project points" << endl;
for (int k = 0; k < nr_points; k++) {
float px = CV_MAT_ELEM(*undistort_2Dcloud,float,k,0);
float py = CV_MAT_ELEM(*undistort_2Dcloud,float,k,1);
if (px < image->width - .5 && px >= 0 && py >= 0 && py < image->height - .5 ) {
point_map1++;
px = CV_MAT_ELEM(*point_2Dcloud,float,k,0);
py = CV_MAT_ELEM(*point_2Dcloud,float,k,1);
if (px < image->width - .5 && px >= 0 && py >= 0 && py < image->height - .5) {
point_map2++;
CvMat* tmp1 = cvCreateMat(1, 1, CV_32FC3);
CvMat* tmp2 = cvCreateMat(1, 1, CV_32FC3);
CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).x = CV_MAT_ELEM(*point_3Dcloud,float,k,0);
CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).y = CV_MAT_ELEM(*point_3Dcloud,float,k,1);
CV_MAT_ELEM(*tmp1, CvPoint3D32f, 0,0).z = CV_MAT_ELEM(*point_3Dcloud,float,k,2);
cvTransform(tmp1, tmp2, rot_tmp);
cvReleaseMat(&tmp1);
if(CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).z < 0) {
cvReleaseMat(&tmp2);
continue;
}
cvReleaseMat(&tmp2);
/*
outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,0) << " "
<< CV_MAT_ELEM(*point_3Dcloud,float,k,1) << " "
<< CV_MAT_ELEM(*point_3Dcloud,float,k,2) << " "
<< CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).x << " "
<< CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).y << " "
<< CV_MAT_ELEM(*tmp2, CvPoint3D32f, 0,0).z << endl;
*/
point_map3++;
int ppx = 0;
int ppy = 0;
if (px - int(px) < .5) {
ppx = int(px);
} else {
ppx = int(px) + 1;
}
if (py - int(py) < .5) {
ppy = int(py);
} else {
ppy = int(py) + 1;
}
CvScalar c;
c = cvGet2D(image, ppy, ppx);
// check for overlap
/*
if(correction) {
vector<float> temp_vec;
float p_id = 1; // 1 for pixel, 0 for neighboring pixel
temp_vec.push_back(-(CV_MAT_ELEM(*point_3Dcloud,float,k,1)));
temp_vec.push_back((CV_MAT_ELEM(*point_3Dcloud,float,k,2)));
temp_vec.push_back((CV_MAT_ELEM(*point_3Dcloud,float,k,0)));
temp_vec.push_back(c.val[2]);
temp_vec.push_back(c.val[1]);
temp_vec.push_back(c.val[0]);
temp_vec.push_back(p_id);
if(neighborhood > 1) {
int limit = neighborhood / 2;
int lower_y = ppy - limit > 0 ? ppy - limit : 0;
int upper_y = ppy + limit < size.height ? ppy + limit : size.height - 1;
int lower_x = ppx - limit > 0 ? ppx - limit : 0;
int upper_x = ppx + limit < size.width ? ppx + limit : size.width - 1;
for (int y = lower_y; y < upper_y; y++) {
for (int x = lower_x; x < upper_x; x++) {
if(x == ppx && y == ppy) {
temp_vec[6] = 1;
} else {
temp_vec[6] = 0;
}
data1[y][x].push_back(temp_vec);
}
}
} else {
data1[ppy][ppx].push_back(temp_vec);
}
temp_vec.clear();
} else {
*/
// write all the data
outdat << -(CV_MAT_ELEM(*point_3Dcloud,float,k,1))<<" ";
outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,2)<<" ";
outdat << CV_MAT_ELEM(*point_3Dcloud,float,k,0)<<" ";
if(optical) {
outdat << c.val[2] <<" "<< c.val[1]<<" "<<c.val[0]<<endl;
} else {
outdat << (c.val[0] - 1000.0)/10.0 << endl;
}
pointcnt++;
if(pointcnt > 100) {
outfile.write(outdat.str().c_str(), outdat.str().size());
pointcnt = 0;
outdat.clear();
outdat.str("");
}
/*
outfile << -(CV_MAT_ELEM(*point_3Dcloud,float,k,1))<<" ";
outfile << CV_MAT_ELEM(*point_3Dcloud,float,k,2)<<" ";
outfile << CV_MAT_ELEM(*point_3Dcloud,float,k,0)<<" ";
if(optical) {
outfile << c.val[2] <<" "<< c.val[1]<<" "<<c.val[0]<<endl;
} else {
outfile << (c.val[0] - 1000.0)/10.0 << endl;
}
//outfile << c.val[2] <<" "<< c.val[1]<<" "<<c.val[0]<<endl;
*/
//}
}
// second image
}
}
cout << "Done sorting points" << endl;
// write data with overlap correction
if(correction) {
CorrectErrorAndWrite(data1, outfile, size, optical);
cout << "Done first image" << endl;
if(fabs(rot_angle) > 1) {
if(size.width > 0 && size.height > 0) {
CorrectErrorAndWrite(data2, outfile, size, optical);
}
}
}
cout << "Done correction" << endl;
// clean up
outfile.flush();
double endtime = GetCurrentTimeInMilliSec();
double time = endtime - starttime;
time = time/1000.0;
cout<<"runtime for scan " << count0 << " in seconds is: " << time << endl;
cout << point_map1 << " " << point_map2 << " " << point_map3 << endl;
cvReleaseImage(&image);
if(correction) {
for (int i = 0; i < size.height; i++) {
for (int j = 0; j < size.width; j++) {
data1[i][j].clear();
if(fabs(rot_angle) > 1) {
data2[i][j].clear();
}
}
}
}
cvReleaseMat(&rot_tmp);
}
outfile.close();
cvReleaseMat(&point_2Dcloud);
cvReleaseMat(&point_3Dcloud);
cvReleaseMat(&undistort_2Dcloud);
}
// Final cleanup
cvReleaseMat(&intrinsic);
cvReleaseMat(&distortion);
cvReleaseMat(&Rotation);
cvReleaseMat(&Translation);
cvReleaseMat(&undistort);
}
/**
* Sorts a number of float array according to their distance to the origin.
*/
void sortDistances(float ** points, int size) {
int swapped1 = 0;
do {
swapped1 = 0;
for(int a = 1; a <= size - 1; a++) {
if(Len(points[a]) < Len(points[a - 1])) {
float * tmp = points[a-1];
points[a-1] = points[a];
points[a] = tmp;
swapped1 = 1;
}
}
} while (swapped1 == 1);
}
/**
* Performs clustering on all points that are projected onto one pixel.
* Writes only the points from the largest closest cluster.
*/
void clusterSearch(float ** points, int size, double thresh1, double thres2, fstream &outfile, bool optical) {
int position = 0;
int cluster_count = 0;
double max_cluster = 0;
int max_position = 0;
vector<double*> clusters;
while (position < size) {
double sum = 0.0;
int j = position + 1;
while(j < size && (Len(points[j]) < (Len(points[j-1]) + thresh1))) {
j++;
cluster_count++;
sum+=Len(points[j-1]);
}
double * tmp = new double[4];
tmp[0] = position;
tmp[1] = j - 1;
// tmp[2] = sum / (j - position);
// weird heuristic ;-) (clustersize/(rank of the cluster))
tmp[2] = (double)(j - position) / (clusters.size() + 1.0);
tmp[3] = (double)(j - position);
if(tmp[3] > max_cluster) {
max_position = clusters.size();
max_cluster = tmp[3];
}
clusters.push_back(tmp);
position = j;
}
/*
max_position = 0;
for(int p = clusters.size() - 1; p > -1; p--) {
max_position = p;
break;
}
*/
for(int p = clusters[max_position][0]; p <= clusters[max_position][1]; p++) {
if(points[p][6] == 1) {
outfile << points[p][0] << " " << points[p][1] << " " << points[p][2] << " ";
if(optical) {
outfile << points[p][3] << " " << points[p][4] << " " << points[p][5] << endl;
} else {
outfile << (points[p][5] - 1000.0)/10.0 << endl;
}
}
}
for(unsigned int i = 0; i < clusters.size(); i++) {
delete[] clusters[i];
}
}
void CorrectErrorAndWrite(Float2D &data, fstream &outfile, CvSize size, bool optical) {
double thresh1 = 4;
double thresh2 = 5;
cout << size.height << " " << size.width << endl;
// getting points mapping to one pixel
for (int i = 0; i < size.height; i++) {
for (int j = 0; j < size.width; j++) {
int tmp_size = data[i][j].size();
if (tmp_size > 0) {
float ** points = new float*[tmp_size];
for (int k = 0; k < tmp_size; k++) {
points[k] = new float[7];
for(int l = 0; l < 7; l++) {
points[k][l] = data[i][j][k][l];
}
}
//sorting the points now in ascending order wrt distance
sortDistances(points, tmp_size);
//look for clusters
clusterSearch(points, tmp_size, thresh1, thresh2, outfile, optical);
for (int k = 0; k < tmp_size; k++) {
delete[] points[k];
}
delete[] points;
}
}
}
}
/**
* Prints out usage message
*/
void usage(char* prog) {
#ifndef _MSC_VER
const string bold("\033[1m");
const string normal("\033[m");
#else
const string bold("");
const string normal("");
#endif
cout << endl
<< bold << "USAGE " << normal << endl
<< " " << prog << " [options] directory" << endl << endl;
cout << bold << "OPTIONS" << normal << endl
<< bold << " -f" << normal << " F, " << bold << "--format=" << normal << "F" << endl
<< " using shared library F for input" << endl
<< " (chose F from {uos, uos_map, uos_rgb, uos_frames, uos_map_frames, old, rts, rts_map, ifp, riegl_txt, riegl_rgb, riegl_bin, zahn, ply})" << endl
<< endl
<< bold << " -s" << normal << " NR, " << bold << "--start=" << normal << "NR" << endl
<< " start at scan NR (i.e., neglects the first NR scans)" << endl
<< " [ATTENTION: counting naturally starts with 0]" << endl
<< endl
<< bold << " -e" << normal << " NR, " << bold << "--end=" << normal << "NR" << endl
<< " end at scan NR" << endl
<< endl
<< bold << " -x" << normal << " NR, " << bold << "--width=" << normal << "NR" << endl
<< " NR of lamps/corners in x direction" << endl
<< endl
<< bold << " -y" << normal << " NR, " << bold << "--height=" << normal << "NR" << endl
<< " NR of lamps/corners in y direction" << endl
<< endl
<< bold << " -o --=optical" << normal << endl
<< " use optical camera instead of thermal camera" << endl
<< endl
<< bold << " -c --=chess" << normal << endl
<< " use chessboard pattern for calibration instead of lightbulb pattern" << endl
<< endl
<< bold << " -I --=intrinsic" << normal << endl
<< " perform intrinsic calibration" << endl
<< endl
<< bold << " -E --=extrinsic" << normal << endl
<< " perform extrinsic calibration" << endl
<< endl
<< bold << " -P --=mapping" << normal << endl
<< " perform mapping of image data to point cloud" << endl
<< endl
<< bold << " -q --=quiet" << normal << endl
<< " " << endl
<< endl << endl;
cout << bold << "EXAMPLES " << normal << endl
<< " " << prog << " -s 2 -e 10 dat" << endl << endl;
exit(1);
}
/**
* Parses command line arguments needed for plane detection. For details about
* the argument see usage().
*/
int parseArgs(int argc, char **argv, string &dir, int &start, int &end, double
&maxDist, double &minDist, IOType &type, bool &optical, bool &chess, int
&width, int &height, bool &intrinsic, bool &extrinsic, bool &mapping, bool
&correction, int &scale, int &neighborhood, double &angle, bool &quiet ) {
// from unistd.h:
int c;
extern char *optarg;
extern int optind;
/* options descriptor */
// 0: no arguments, 1: required argument, 2: optional argument
static struct option longopts[] = {
{ "correction", no_argument, 0, 'C' },
{ "scale", required_argument, 0, 'S' },
{ "neighborhood", required_argument, 0, 'n' },
{ "angle", required_argument, 0, 'a' },
{ "format", required_argument, 0, 'f' },
{ "max", required_argument, 0, 'm' },
{ "min", required_argument, 0, 'M' },
{ "start", required_argument, 0, 's' },
{ "end", required_argument, 0, 'e' },
{ "width", required_argument, 0, 'x' },
{ "height", required_argument, 0, 'y' },
{ "quiet", no_argument, 0, 'q' },
{ "optical", no_argument, 0, 'o' },
{ "intrinsic", no_argument, 0, 'I' },
{ "extrinsic", no_argument, 0, 'E' },
{ "mapping", no_argument, 0, 'P' },
{ "chess", no_argument, 0, 'c' },
{ 0, 0, 0, 0} // needed, cf. getopt.h
};
cout << endl;
while ((c = getopt_long(argc, argv, "f:s:e:x:y:m:M:qoIEPcCS:n:a:", longopts, NULL)) != -1) {
switch (c)
{
case 's':
start = atoi(optarg);
if (start < 0) { cerr << "Error: Cannot start at a negative scan number.\n"; exit(1); }
break;
case 'e':
end = atoi(optarg);
if (end < 0) { cerr << "Error: Cannot end at a negative scan number.\n"; exit(1); }
if (end < start) { cerr << "Error: <end> cannot be smaller than <start>.\n"; exit(1); }
break;
case 'f':
try {
type = formatname_to_io_type(optarg);
} catch (...) { // runtime_error
cerr << "Format " << optarg << " unknown." << endl;
abort();
}
break;
case 'q':
quiet = true;
break;
case 'm':
maxDist = atoi(optarg);
break;
case 'M':
minDist = atoi(optarg);
break;
case 'o':
optical = true;
break;
case 'I':
intrinsic = true;
break;
case 'E':
extrinsic = true;
break;
case 'P':
mapping = true;
break;
case 'c':
chess = true;
break;
case 'x':
width = atoi(optarg);
break;
case 'y':
height = atoi(optarg);
break;
case 'S':
scale = atoi(optarg);
break;
case 'a':
angle = atof(optarg);
break;
case 'n':
neighborhood = atoi(optarg);
break;
case 'C':
correction = true;
break;
case '?':
usage(argv[0]);
return 1;
default:
cout << "Abort" << endl;
abort ();
}
}
if (optind != argc-1) {
cerr << "\n*** Directory missing ***" << endl;
usage(argv[0]);
}
dir = argv[optind];
#ifndef _MSC_VER
if (dir[dir.length()-1] != '/') dir = dir + "/";
#else
if (dir[dir.length()-1] != '\\') dir = dir + "\\";
#endif
if(!(intrinsic || extrinsic || mapping)) {
cerr << "\n*** Please choose at least one method (intrinsic calibration, "
<< "extrinsic calibration, mapping of image data to point data! ***\n" <<
endl;
usage(argv[0]);
}
return 0;
}
/**
* Main function. Calls either function for color mapping or function for
* intrinsic and/or extrinsic calibration.
*/
int main(int argc, char** argv) {
string dir;
int start = 0;
int end = -1;
int width = 5;
int height = 6;
double maxDist = -1;
double minDist = -1;
IOType type = UOS;
bool optical = false;
bool chess = false;
bool intrinsic = false;
bool extrinsic = false;
bool mapping = false;
bool quiet = false;
int scale = 1;
//double rot_angle = -40;
double rot_angle = 0;
bool correction = false;
int neighborhood = 1;
parseArgs(argc, argv, dir, start, end, maxDist, minDist, type, optical, chess,
width, height, intrinsic, extrinsic, mapping, correction, scale, neighborhood,
rot_angle, quiet);
// either mapping
if(mapping) {
if(!quiet) cout << "Starting projecting and mapping image data to point cloud..." << endl;
//TODO ProjectAndMap(start, end, optical, quiet, dir, type, scale, rot_angle, minDist, maxDist, correction, neighborhood);
//calculateGlobalCameras(start, end, optical, quiet, dir, type, scale,
writeGlobalCameras(start, end, optical, quiet, dir, type, scale,
rot_angle, minDist, maxDist, correction, neighborhood, 0);
if(!quiet) cout << "\nDONE" << endl;
return 0;
}
// or calibration
if(intrinsic) {
if(!quiet) {
cout << "Starting intrinsic calibration using ";
if(chess) cout << "chessboard pattern..." << endl;
else cout << "lightbulb pattern..." << endl;
}
CalibFunc(width, height, start, end, optical, chess, quiet, dir, scale);
if(!quiet) cout << "\nDONE" << endl;
}
if(extrinsic) {
if(!quiet) {
cout << "Starting extrinsic calibration using ";
if(chess) cout << "chessboard pattern..." << endl;
else cout << "lightbulb pattern..." << endl;
}
ExtrCalibFunc(width, height, start, end, optical, chess, quiet, dir, scale);
if(!quiet) cout << "\nDONE" << endl;
}
return 0;
}
Reference in New Issue View Git Blame Copy Permalink