void SE_average<3>::append(const mrpt::poses::CPose3D &p, const double weight) {
m_count += weight;
m_accum_x += weight * p.x();
m_accum_y += weight * p.y();
m_accum_z += weight * p.z();
m_rot_part.append(p.getRotationMatrix(), weight);
}
// Transform all 6 components for a change of reference frame from "A" to
// another frame "B" whose rotation with respect to "A" is given by `rot`. The translational part of the pose is ignored
void TTwist3D::rotate(const mrpt::poses::CPose3D & rot)
{
const TTwist3D t = *this;
const mrpt::math::CMatrixDouble33 & R = rot.getRotationMatrix();
vx=R(0,0)*t.vx+R(0,1)*t.vy+R(0,2)*t.vz;
vy=R(1,0)*t.vx+R(1,1)*t.vy+R(1,2)*t.vz;
vz=R(2,0)*t.vx+R(2,1)*t.vy+R(2,2)*t.vz;
wx=R(0,0)*t.wx+R(0,1)*t.wy+R(0,2)*t.wz;
wy=R(1,0)*t.wx+R(1,1)*t.wy+R(1,2)*t.wz;
wz=R(2,0)*t.wx+R(2,1)*t.wy+R(2,2)*t.wz;
}
//
// Given a list of chessboard images, the number of corners (nx, ny)
// on the chessboards, and a flag: useCalibrated for calibrated (0) or
// uncalibrated (1: use cvStereoCalibrate(), 2: compute fundamental
// matrix separately) stereo. Calibrate the cameras and display the
// rectified results along with the computed disparity images.
//
void StereoCalib(
const char* imageList,
const char* sOutFile,
int nx, int ny,
int useUncalibrated,
const float squareSize,
const double alpha,
const bool flag_fix_aspect_ratio,
const bool flag_zero_tangent_dist,
const bool flag_same_focal_len
)
{
int displayCorners = 1;
bool isVerticalStereo = false;//OpenCV can handle left-right
//or up-down camera arrangements
const int maxScale = 1;
FILE* f = fopen(imageList, "rt");
int i, j, lr, nframes, n = nx*ny, N = 0;
vector<string> imageNames[2];
vector<CvPoint3D32f> objectPoints;
vector<CvPoint2D32f> points[2];
vector<int> npoints;
vector<uchar> active[2];
vector<CvPoint2D32f> temp(n);
CvSize imageSize = {0,0};
// ARRAY AND VECTOR STORAGE:
double M1[3][3], M2[3][3], D1[5], D2[5];
double R[3][3], T[3], E[3][3], F[3][3];
CvMat _M1 = cvMat(3, 3, CV_64F, M1 );
CvMat _M2 = cvMat(3, 3, CV_64F, M2 );
CvMat _D1 = cvMat(1, 5, CV_64F, D1 );
CvMat _D2 = cvMat(1, 5, CV_64F, D2 );
CvMat _R = cvMat(3, 3, CV_64F, R );
CvMat _T = cvMat(3, 1, CV_64F, T );
CvMat _E = cvMat(3, 3, CV_64F, E );
CvMat _F = cvMat(3, 3, CV_64F, F );
if( displayCorners )
cvNamedWindow( "corners", 1 );
// READ IN THE LIST OF CHESSBOARDS:
if( !f )
{
fprintf(stderr, "can not open file %s\n", imageList );
return;
}
std::vector<std::string> lst_image_files;
printf("Starting main loop\n");
for(i=0;; i++)
{
printf("Iteration %d\n", i);
char buf[1024];
int count = 0, result=0;
lr = i % 2;
vector<CvPoint2D32f>& pts = points[lr];
if( !fgets( buf, sizeof(buf)-3, f ))
break;
size_t len = strlen(buf);
while( len > 0 && isspace(buf[len-1]))
buf[--len] = '\0';
if( buf[0] == '#')
continue;
IplImage* img = cvLoadImage( buf, 0 );
if( !img )
break;
lst_image_files.push_back(string(buf));
imageSize = cvGetSize(img);
imageNames[lr].push_back(buf);
//FIND CHESSBOARDS AND CORNERS THEREIN:
for( int s = 1; s <= maxScale; s++ )
{
IplImage* timg = img;
if( s > 1 )
{
timg = cvCreateImage(cvSize(img->width*s,img->height*s),
img->depth, img->nChannels );
cvResize( img, timg, CV_INTER_CUBIC );
}
result = cvFindChessboardCorners( timg, cvSize(nx, ny),
&temp[0], &count,
CV_CALIB_CB_ADAPTIVE_THRESH |
CV_CALIB_CB_NORMALIZE_IMAGE);
if( timg != img )
cvReleaseImage( &timg );
if( result || s == maxScale )
for( j = 0; j < count; j++ )
{
temp[j].x /= s;
temp[j].y /= s;
}
if( result )
break;
}
if( displayCorners )
{
printf("%s\n", buf);
IplImage* cimg = cvCreateImage( imageSize, 8, 3 );
cvCvtColor( img, cimg, CV_GRAY2BGR );
cvDrawChessboardCorners( cimg, cvSize(nx, ny), &temp[0],
count, result );
cvShowImage( "corners", cimg );
cvReleaseImage( &cimg );
int c = cvWaitKey(100);
if( c == 27 || c == 'q' || c == 'Q' ) //Allow ESC to quit
exit(-1);
}
else
putchar('.');
N = pts.size();
pts.resize(N + n, cvPoint2D32f(0,0));
active[lr].push_back((uchar)result);
//assert( result != 0 );
if( result )
{
//Calibration will suffer without subpixel interpolation
cvFindCornerSubPix( img, &temp[0], count,
cvSize(11, 11), cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,
30, 0.01) );
copy( temp.begin(), temp.end(), pts.begin() + N );
}
cvReleaseImage( &img );
}
fclose(f);
printf("\n");
// HARVEST CHESSBOARD 3D OBJECT POINT LIST:
nframes = active[0].size();//Number of good chessboads found
objectPoints.resize(nframes*n);
for( i = 0; i < ny; i++ )
for( j = 0; j < nx; j++ )
objectPoints[i*nx + j] =
cvPoint3D32f(i*squareSize, j*squareSize, 0);
for( i = 1; i < nframes; i++ )
copy( objectPoints.begin(), objectPoints.begin() + n,
objectPoints.begin() + i*n );
npoints.resize(nframes,n);
N = nframes*n;
CvMat _objectPoints = cvMat(1, N, CV_32FC3, &objectPoints[0] );
CvMat _imagePoints1 = cvMat(1, N, CV_32FC2, &points[0][0] );
CvMat _imagePoints2 = cvMat(1, N, CV_32FC2, &points[1][0] );
CvMat _npoints = cvMat(1, npoints.size(), CV_32S, &npoints[0] );
cvSetIdentity(&_M1);
cvSetIdentity(&_M2);
cvZero(&_D1);
cvZero(&_D2);
// CALIBRATE THE STEREO CAMERAS
// ======================================================
printf("Running stereo calibration ...");
fflush(stdout);
cvStereoCalibrate( &_objectPoints, &_imagePoints1,
&_imagePoints2, &_npoints,
&_M1, &_D1, &_M2, &_D2,
imageSize, &_R, &_T, &_E, &_F,
cvTermCriteria(CV_TERMCRIT_ITER+
CV_TERMCRIT_EPS, 150, 1e-6),
(flag_fix_aspect_ratio ? CV_CALIB_FIX_ASPECT_RATIO:0)
+
(flag_zero_tangent_dist ? CV_CALIB_ZERO_TANGENT_DIST:0)
+
(flag_same_focal_len ? CV_CALIB_SAME_FOCAL_LENGTH:0) );
printf(" done\n");
// CALIBRATION QUALITY CHECK
// ======================================================
// because the output fundamental matrix implicitly
// includes all the output information,
// we can check the quality of calibration using the
// epipolar geometry constraint: m2^t*F*m1=0
vector<CvPoint3D32f> lines[2];
points[0].resize(N);
points[1].resize(N);
_imagePoints1 = cvMat(1, N, CV_32FC2, &points[0][0] );
_imagePoints2 = cvMat(1, N, CV_32FC2, &points[1][0] );
lines[0].resize(N);
lines[1].resize(N);
CvMat _L1 = cvMat(1, N, CV_32FC3, &lines[0][0]);
CvMat _L2 = cvMat(1, N, CV_32FC3, &lines[1][0]);
//Always work in undistorted space
cvUndistortPoints( &_imagePoints1, &_imagePoints1,
&_M1, &_D1, 0, &_M1 );
cvUndistortPoints( &_imagePoints2, &_imagePoints2,
&_M2, &_D2, 0, &_M2 );
cvComputeCorrespondEpilines( &_imagePoints1, 1, &_F, &_L1 );
cvComputeCorrespondEpilines( &_imagePoints2, 2, &_F, &_L2 );
double avgErr = 0;
for( i = 0; i < N; i++ )
{
double err = fabs(points[0][i].x*lines[1][i].x +
points[0][i].y*lines[1][i].y + lines[1][i].z)
+ fabs(points[1][i].x*lines[0][i].x +
points[1][i].y*lines[0][i].y + lines[0][i].z);
avgErr += err;
}
printf( "avg err = %g\n", avgErr/(nframes*n) );
//COMPUTE RECTIFICATION
// ========================================================
CvMat* mx1 = cvCreateMat( imageSize.height,
imageSize.width, CV_32F );
CvMat* my1 = cvCreateMat( imageSize.height,
imageSize.width, CV_32F );
CvMat* mx2 = cvCreateMat( imageSize.height,
imageSize.width, CV_32F );
CvMat* my2 = cvCreateMat( imageSize.height,
imageSize.width, CV_32F );
CvMat* img1r = cvCreateMat( imageSize.height,
imageSize.width, CV_8U );
CvMat* img2r = cvCreateMat( imageSize.height,
imageSize.width, CV_8U );
CvMat* disp = cvCreateMat( imageSize.height,
imageSize.width, CV_16S );
CvMat* vdisp = cvCreateMat( imageSize.height,
imageSize.width, CV_8U );
CvMat* pair;
double R1[3][3], R2[3][3], P1[3][4], P2[3][4], Q[4][4];
CvMat _R1 = cvMat(3, 3, CV_64F, R1);
CvMat _R2 = cvMat(3, 3, CV_64F, R2);
CvMat _Q = cvMat(4, 4, CV_64F, Q);
// IF BY CALIBRATED (BOUGUET'S METHOD)
if( useUncalibrated == 0 )
{
CvMat _P1 = cvMat(3, 4, CV_64F, P1);
CvMat _P2 = cvMat(3, 4, CV_64F, P2);
#if MRPT_OPENCV_VERSION_NUM<0x210
// OpenCV 2.0.X
cvStereoRectify( &_M1, &_M2, &_D1, &_D2, imageSize,
&_R, &_T,
&_R1, &_R2, &_P1, &_P2, &_Q,
0/*CV_CALIB_ZERO_DISPARITY*/
);
#else
// OpenCV 2.1.X - 2.2.X - 2.3.X
cvStereoRectify( &_M1, &_M2, &_D1, &_D2, imageSize,
&_R, &_T,
&_R1, &_R2, &_P1, &_P2, &_Q,
0 /* CV_CALIB_ZERO_DISPARITY */,
0 /* alpha */
);
#endif
isVerticalStereo = fabs(P2[1][3]) > fabs(P2[0][3]);
//Precompute maps for cvRemap()
cvInitUndistortRectifyMap(&_M1,&_D1,&_R1,&_P1,mx1,my1);
cvInitUndistortRectifyMap(&_M2,&_D2,&_R2,&_P2,mx2,my2);
}
//OR ELSE HARTLEY'S METHOD
else if( useUncalibrated == 1 || useUncalibrated == 2 )
// use intrinsic parameters of each camera, but
// compute the rectification transformation directly
// from the fundamental matrix
{
double H1[3][3], H2[3][3], iM[3][3];
CvMat _H1 = cvMat(3, 3, CV_64F, H1);
CvMat _H2 = cvMat(3, 3, CV_64F, H2);
CvMat _iM = cvMat(3, 3, CV_64F, iM);
//Just to show you could have independently used F
if( useUncalibrated == 2 )
cvFindFundamentalMat( &_imagePoints1,
&_imagePoints2, &_F);
cvStereoRectifyUncalibrated( &_imagePoints1,
&_imagePoints2, &_F,
imageSize,
&_H1, &_H2, 3);
cvInvert(&_M1, &_iM);
cvMatMul(&_H1, &_M1, &_R1);
cvMatMul(&_iM, &_R1, &_R1);
cvInvert(&_M2, &_iM);
cvMatMul(&_H2, &_M2, &_R2);
cvMatMul(&_iM, &_R2, &_R2);
//Precompute map for cvRemap()
cvInitUndistortRectifyMap(&_M1,&_D1,&_R1,&_M1,mx1,my1);
cvInitUndistortRectifyMap(&_M2,&_D1,&_R2,&_M2,mx2,my2);
}
else
assert(0);
// SAVE CALIBRATION REPORT FILE
// ==============================================================================
cout << "Writing report file: " << sOutFile<< endl;
FILE *f_out = fopen(sOutFile,"wt");
time_t systime;
time(&systime);
struct tm * timeinfo=localtime(&systime);
fprintf( f_out,
"# Stereo camera calibration report\n"
"# Generated by camera-calib-gui - MRPT at %s"
"# (This file is loadable from rawlog-edit and other MRPT tools)\n"
"# ---------------------------------------------------------------------\n\n",
asctime(timeinfo)
);
fprintf( f_out,
"# Left camera calibration parameters:\n"
"[CAMERA_PARAMS_LEFT]\n"
"resolution = [%u %u]\n"
"cx = %f\n"
"cy = %f\n"
"fx = %f\n"
"fy = %f\n"
"dist = [%e %e %e %e %e] // The order is: [K1 K2 T1 T2 K3]\n\n",
imageSize.width,imageSize.height,
cvGet2D(&_M1,0,2).val[0],
cvGet2D(&_M1,1,2).val[0],
cvGet2D(&_M1,0,0).val[0],
cvGet2D(&_M1,1,1).val[0],
cvGet2D(&_D1,0,0).val[0], cvGet2D(&_D1,0,1).val[0], cvGet2D(&_D1,0,2).val[0],
cvGet2D(&_D1,0,3).val[0], cvGet2D(&_D1,0,4).val[0] );
fprintf( f_out,
"# Right camera calibration parameters:\n"
"[CAMERA_PARAMS_RIGHT]\n"
"resolution = [%u %u]\n"
"cx = %f\n"
"cy = %f\n"
"fx = %f\n"
"fy = %f\n"
"dist = [%e %e %e %e %e] // The order is: [K1 K2 T1 T2 K3]\n\n",
imageSize.width,imageSize.height,
cvGet2D(&_M2,0,2).val[0],
cvGet2D(&_M2,1,2).val[0],
cvGet2D(&_M2,0,0).val[0],
cvGet2D(&_M2,1,1).val[0],
cvGet2D(&_D2,0,0).val[0], cvGet2D(&_D2,0,1).val[0], cvGet2D(&_D2,0,2).val[0],
cvGet2D(&_D2,0,3).val[0], cvGet2D(&_D2,0,4).val[0] );
// Convert RT to MRPT classes:
mrpt::math::CMatrixFixedNumeric<double,3,3> mROT;
for (int i=0; i<3; i++)
for (int j=0; j<3; j++)
mROT(i,j)=cvGet2D(&_R,i,j).val[0];
mrpt::math::TPoint3D mT;
mT.x = cvGet2D(&_T,0,0).val[0];
mT.y = cvGet2D(&_T,1,0).val[0];
mT.z = cvGet2D(&_T,2,0).val[0];
// NOTE: OpenCV seems to return the inverse of what we want, so invert the pose:
const mrpt::poses::CPose3D RT_YPR(-mrpt::poses::CPose3D(mROT,mT));
const mrpt::poses::CPose3DQuat RT_quat(RT_YPR);
fprintf( f_out,
"# Relative pose of the right camera wrt to the left camera:\n"
"[CAMERA_PARAMS_LEFT2RIGHT_POSE]\n"
"translation_only = [%e %e %e]\n"
"rotation_matrix_only = %s\n"
"pose_yaw_pitch_roll = %s\n"
"pose_quaternion = %s\n\n"
,
RT_YPR.x(),RT_YPR.y(),RT_YPR.z(),
RT_YPR.getRotationMatrix().inMatlabFormat(13).c_str(),
RT_YPR.asString().c_str(),
RT_quat.asString().c_str()
);
// Convert RT to MRPT classes:
mrpt::math::CMatrixFixedNumeric<double,3,3> mR1, mR2;
mrpt::math::CMatrixFixedNumeric<double,3,4> mP1, mP2;
mrpt::math::CMatrixFixedNumeric<double,4,4> mQ;
for (int i=0; i<3; i++)
for (int j=0; j<3; j++)
{
mR1(i,j)=R1[i][j];
mR2(i,j)=R2[i][j];
}
for (int i=0; i<3; i++)
for (int j=0; j<4; j++)
{
mP1(i,j)=P1[i][j];
mP2(i,j)=P2[i][j];
}
for (int i=0; i<4; i++)
for (int j=0; j<4; j++)
mQ(i,j)=Q[i][j];
fprintf( f_out,
"# Stereo rectify matrices (see http://opencv.willowgarage.com/documentation/camera_calibration_and_3d_reconstruction.html ):\n"
"# R1, R2: The output 3x3 rectification transforms (rotation matrices) for the first and the second cameras, respectively.\n"
"# P1, P2: The output 3x4 projection matrices in the new (rectified) coordinate systems.\n"
"[STEREO_RECTIFY_MATRICES]\n"
"R1 = %s\n"
"R2 = %s\n"
"P1 = %s\n"
"P2 = %s\n\n"
"Q = %s\n\n"
,
mR1.inMatlabFormat(13).c_str(),
mR2.inMatlabFormat(13).c_str(),
mP1.inMatlabFormat(13).c_str(),
mP2.inMatlabFormat(13).c_str(),
mQ.inMatlabFormat(13).c_str()
);
fprintf( f_out,
"# Info about calibration parameters:\n"
"[CALIB_METAINFO]\n"
"number_good_cheesboards = %i\n"
"average_reprojection_error = %f // pixels\n"
"cheesboard_nx = %i\n"
"cheesboard_ny = %i\n"
"cheesboard_square_size = %f\n"
"alpha = %f // Parameter for zoom in/out\n"
"flag_fix_aspect_ratio = %s\n"
"flag_zero_tangent_dist = %s\n"
"flag_same_focal_len = %s\n\n"
,
nframes,
avgErr/(nframes*n),
nx,ny,
(double)squareSize,
alpha,
flag_fix_aspect_ratio ? "true":"false",
flag_zero_tangent_dist ? "true":"false",
flag_same_focal_len ? "true":"false"
);
fprintf( f_out,
"# List of files used in the optimization:\n"
"[CALIB_FILE_LIST]\n");
for (unsigned int i=0; i<lst_image_files.size(); i++)
fprintf( f_out, "img_%04u = %s\n", i, lst_image_files[i].c_str() );
fprintf( f_out, "\n");
fclose(f_out);
// DISPLAY RECTIFICATION
// ========================================================
cvNamedWindow( "rectified", 1 );
// RECTIFY THE IMAGES AND FIND DISPARITY MAPS
if( !isVerticalStereo )
pair = cvCreateMat( imageSize.height, imageSize.width*2,
CV_8UC3 );
else
pair = cvCreateMat( imageSize.height*2, imageSize.width,
CV_8UC3 );
//Setup for finding stereo corrrespondences
CvStereoBMState *BMState = cvCreateStereoBMState();
assert(BMState != 0);
BMState->preFilterSize=41;
BMState->preFilterCap=31;
BMState->SADWindowSize=41;
BMState->minDisparity=-64;
BMState->numberOfDisparities=128;
BMState->textureThreshold=10;
BMState->uniquenessRatio=15;
for( i = 0; i < nframes; i++ )
{
IplImage* img1=cvLoadImage(imageNames[0][i].c_str(),0);
IplImage* img2=cvLoadImage(imageNames[1][i].c_str(),0);
if( img1 && img2 )
{
CvMat part;
cvRemap( img1, img1r, mx1, my1 );
cvRemap( img2, img2r, mx2, my2 );
if( !isVerticalStereo || useUncalibrated != 0 )
{
// When the stereo camera is oriented vertically,
// useUncalibrated==0 does not transpose the
// image, so the epipolar lines in the rectified
// images are vertical. Stereo correspondence
// function does not support such a case.
cvFindStereoCorrespondenceBM( img1r, img2r, disp,
BMState);
cvNormalize( disp, vdisp, 0, 256, CV_MINMAX );
//cvNamedWindow( "disparity" );
//cvShowImage( "disparity", vdisp );
}
if( !isVerticalStereo )
{
cvGetCols( pair, &part, 0, imageSize.width );
cvCvtColor( img1r, &part, CV_GRAY2BGR );
cvGetCols( pair, &part, imageSize.width,
imageSize.width*2 );
cvCvtColor( img2r, &part, CV_GRAY2BGR );
for( j = 0; j < imageSize.height; j += 16 )
cvLine( pair, cvPoint(0,j),
cvPoint(imageSize.width*2,j),
CV_RGB(0,255,0));
}
else
{
cvGetRows( pair, &part, 0, imageSize.height );
cvCvtColor( img1r, &part, CV_GRAY2BGR );
cvGetRows( pair, &part, imageSize.height,
imageSize.height*2 );
cvCvtColor( img2r, &part, CV_GRAY2BGR );
for( j = 0; j < imageSize.width; j += 16 )
cvLine( pair, cvPoint(j,0),
cvPoint(j,imageSize.height*2),
CV_RGB(0,255,0));
}
cvShowImage( "rectified", pair );
if( cvWaitKey() == 27 )
break;
}
cvReleaseImage( &img1 );
cvReleaseImage( &img2 );
}
cvReleaseStereoBMState(&BMState);
cvReleaseMat( &mx1 );
cvReleaseMat( &my1 );
cvReleaseMat( &mx2 );
cvReleaseMat( &my2 );
cvReleaseMat( &img1r );
cvReleaseMat( &img2r );
cvReleaseMat( &disp );
}