void ImageOverlayer::MakeGroundPlaneZ(CvScalar color, IplImage* baseImage)
{
int linePoints = 500;
vector<CvPoint3D32f> Robot_PositionsToReProject;
Robot_PositionsToReProject.resize(linePoints);
CvMat _Robot_PositionsToReProject = cvMat(1, linePoints, CV_32FC3, &Robot_PositionsToReProject[0]);
for(int pts = 0; pts < linePoints; pts++) //circlePointsPerPosition points out of linePoints for circle
{
float zValue = 0 + ((float)(5.0/linePoints))*(float)pts;
//std::cout<<"xvalue = "<<xValue<<std::endl;
Robot_PositionsToReProject[pts] = cvPoint3D32f(0, 0, zValue);
}
vector<CvPoint2D32f> reprojectedPoints_Robot;
reprojectedPoints_Robot.resize(linePoints);
CvMat _imageReprojectedPoints_RobotRight = cvMat(1, linePoints, CV_32FC2, &reprojectedPoints_Robot[0]);
cvProjectPoints2(&_Robot_PositionsToReProject, Rvec_right_n, Tvec_right, &_M2, &_D2, &_imageReprojectedPoints_RobotRight, NULL, NULL, NULL, NULL, NULL);
for(int pts = 0; pts < linePoints; pts++)
{
CvPoint robot_PointToBeShownRight = cvPoint(reprojectedPoints_Robot[pts].x,reprojectedPoints_Robot[pts].y);
cvCircle(baseImage, robot_PointToBeShownRight, 0, color, 2, 8, 0);
}
}
/* Does reprojection of 3d object points to the view plane */
void cvProjectPoints( int point_count, CvPoint3D64f* _object_points,
double* _rotation_vector, double* _translation_vector,
double* focal_length, CvPoint2D64f principal_point,
double* _distortion, CvPoint2D64f* _image_points,
double* _deriv_points_rotation_matrix,
double* _deriv_points_translation_vect,
double* _deriv_points_focal,
double* _deriv_points_principal_point,
double* _deriv_points_distortion_coeffs )
{
CvMat object_points = cvMat( point_count, 1, CV_64FC3, _object_points );
CvMat image_points = cvMat( point_count, 1, CV_64FC2, _image_points );
CvMat rotation_vector = cvMat( 3, 1, CV_64FC1, _rotation_vector );
CvMat translation_vector = cvMat( 3, 1, CV_64FC1, _translation_vector );
double a[9];
CvMat camera_matrix = cvMat( 3, 3, CV_64FC1, a );
CvMat dist_coeffs = cvMat( 4, 1, CV_64FC1, _distortion );
CvMat dpdr = cvMat( 2*point_count, 3, CV_64FC1, _deriv_points_rotation_matrix );
CvMat dpdt = cvMat( 2*point_count, 3, CV_64FC1, _deriv_points_translation_vect );
CvMat dpdf = cvMat( 2*point_count, 2, CV_64FC1, _deriv_points_focal );
CvMat dpdc = cvMat( 2*point_count, 2, CV_64FC1, _deriv_points_principal_point );
CvMat dpdk = cvMat( 2*point_count, 4, CV_64FC1, _deriv_points_distortion_coeffs );
a[0] = focal_length[0]; a[4] = focal_length[1];
a[2] = principal_point.x; a[5] = principal_point.y;
a[1] = a[3] = a[6] = a[7] = 0.;
a[8] = 1.;
cvProjectPoints2( &object_points, &rotation_vector, &translation_vector,
&camera_matrix, &dist_coeffs, &image_points,
&dpdr, &dpdt, &dpdf, &dpdc, &dpdk, 0 );
}
/* Simpler version of the previous function */
void cvProjectPointsSimple( int point_count, CvPoint3D64f* _object_points,
double* _rotation_matrix, double* _translation_vector,
double* _camera_matrix, double* _distortion, CvPoint2D64f* _image_points )
{
CvMat object_points = cvMat( point_count, 1, CV_64FC3, _object_points );
CvMat image_points = cvMat( point_count, 1, CV_64FC2, _image_points );
CvMat rotation_matrix = cvMat( 3, 3, CV_64FC1, _rotation_matrix );
CvMat translation_vector = cvMat( 3, 1, CV_64FC1, _translation_vector );
CvMat camera_matrix = cvMat( 3, 3, CV_64FC1, _camera_matrix );
CvMat dist_coeffs = cvMat( 4, 1, CV_64FC1, _distortion );
cvProjectPoints2( &object_points, &rotation_matrix, &translation_vector,
&camera_matrix, &dist_coeffs, &image_points,
0, 0, 0, 0, 0, 0 );
}
double compute_reprojection_error( const CvMat* object_points,
const CvMat* rot_vects, const CvMat* trans_vects,
const CvMat* camera_matrix, const CvMat* dist_coeffs,
const CvMat* image_points, const CvMat* point_counts,
CvMat* per_view_errors )
{
CvMat* image_points2 = cvCreateMat( image_points->rows,
image_points->cols, image_points->type );
int i, image_count = rot_vects->rows, points_so_far = 0;
double total_err = 0, err;
for( i = 0; i < image_count; i++ )
{
CvMat object_points_i, image_points_i, image_points2_i;
int point_count = point_counts->data.i[i];
CvMat rot_vect, trans_vect;
cvGetCols( object_points, &object_points_i,
points_so_far, points_so_far + point_count );
cvGetCols( image_points, &image_points_i,
points_so_far, points_so_far + point_count );
cvGetCols( image_points2, &image_points2_i,
points_so_far, points_so_far + point_count );
points_so_far += point_count;
cvGetRow( rot_vects, &rot_vect, i );
cvGetRow( trans_vects, &trans_vect, i );
cvProjectPoints2( &object_points_i, &rot_vect, &trans_vect,
camera_matrix, dist_coeffs, &image_points2_i,
0, 0, 0, 0, 0 );
err = cvNorm( &image_points_i, &image_points2_i, CV_L1 );
if( per_view_errors )
per_view_errors->data.db[i] = err/point_count;
total_err += err;
}
cvReleaseMat( &image_points2 );
return total_err/points_so_far;
}
void ImageOverlayer::OverlayEstimatedRobotPose(double x, double y, double z, double thetaRob, CvScalar color, IplImage* baseImage)
{
vector<CvPoint3D32f> Robot_PositionsToReProject;
Robot_PositionsToReProject.resize(totPntsPerPos);
CvMat _Robot_PositionsToReProject = cvMat(1, totPntsPerPos, CV_32FC3, &Robot_PositionsToReProject[0]);
for(float j=0; j<=robHeight; j+=0.01)
{
Robot_PositionsToReProject[0] = cvPoint3D32f(x,y,j);
for(int pts = 0; pts < circlePointsPerPosition; pts++) //circlePointsPerPosition points out of totPntsPerPos for circle
{
float theta = -M_PI + (float)pts*2*M_PI/(circlePointsPerPosition);
Robot_PositionsToReProject[1 + pts] = cvPoint3D32f( x + robRadius*cosf(theta), y + robRadius*sinf(theta), j);
}
for(int pts = 0; pts < arrowPointsPerPosition /*&& j==robHeight*/; pts++) //arrowPointsPerPosition points out of totPntsPerPos for th arrow
{
Robot_PositionsToReProject[1 + circlePointsPerPosition + pts] = cvPoint3D32f( x + (float)pts*(robRadius/(float)arrowPointsPerPosition)*cosf(thetaRob), y + (float)pts*(robRadius/(float)arrowPointsPerPosition)*sinf(thetaRob), robHeight);
}
vector<CvPoint2D32f> reprojectedPoints_Robot;
reprojectedPoints_Robot.resize(totPntsPerPos);
CvMat _imageReprojectedPoints_RobotRight = cvMat(1, totPntsPerPos, CV_32FC2, &reprojectedPoints_Robot[0]);
cvProjectPoints2(&_Robot_PositionsToReProject, Rvec_right_n, Tvec_right, &_M2, &_D2, &_imageReprojectedPoints_RobotRight, NULL, NULL, NULL, NULL, NULL);
for(int pts = 0; pts < totPntsPerPos; pts++)
{
CvPoint robot_PointToBeShownRight = cvPoint(reprojectedPoints_Robot[pts].x,reprojectedPoints_Robot[pts].y);
cvCircle(baseImage, robot_PointToBeShownRight, 0, color, 2, 8, 0);
}
}
}
void projectRect(Image<PixRGB<byte> > &img, float width, float height)
{
//draw center point
drawCircle(img, Point2D<int>(img.getWidth()/2, img.getHeight()/2), 3, PixRGB<byte>(255,0,0));
CvMat *my_3d_point = cvCreateMat( 3, 5, CV_64FC1);
CvMat *my_image_point = cvCreateMat( 2, 5, CV_64FC2);
cvSetReal2D( my_3d_point, 0, 0, -width/2);
cvSetReal2D( my_3d_point, 1, 0, -height/2);
cvSetReal2D( my_3d_point, 2, 0, 0.0);
cvSetReal2D( my_3d_point, 0, 1, width/2);
cvSetReal2D( my_3d_point, 1, 1, -height/2);
cvSetReal2D( my_3d_point, 2, 1, 0.0);
cvSetReal2D( my_3d_point, 0, 2, width/2);
cvSetReal2D( my_3d_point, 1, 2, height/2);
cvSetReal2D( my_3d_point, 2, 2, 0.0);
cvSetReal2D( my_3d_point, 0, 3, -width/2);
cvSetReal2D( my_3d_point, 1, 3, height/2);
cvSetReal2D( my_3d_point, 2, 3, 0.0);
cvSetReal2D( my_3d_point, 0, 4, 0);
cvSetReal2D( my_3d_point, 1, 4, 0);
cvSetReal2D( my_3d_point, 2, 4, 0.0);
cvProjectPoints2( my_3d_point,
itsCameraRotation,
itsCameraTranslation,
itsIntrinsicMatrix,
itsDistortionCoeffs,
my_image_point);
int x1 = (int)cvGetReal2D( my_image_point, 0, 0);
int y1 = (int)cvGetReal2D( my_image_point, 1, 0);
int x2 = (int)cvGetReal2D( my_image_point, 0, 1);
int y2 = (int)cvGetReal2D( my_image_point, 1, 1);
int x3 = (int)cvGetReal2D( my_image_point, 0, 2);
int y3 = (int)cvGetReal2D( my_image_point, 1, 2);
int x4 = (int)cvGetReal2D( my_image_point, 0, 3);
int y4 = (int)cvGetReal2D( my_image_point, 1, 3);
int cx = (int)cvGetReal2D( my_image_point, 0, 4);
int cy = (int)cvGetReal2D( my_image_point, 1, 4);
drawLine(img, Point2D<int>(x1,y1), Point2D<int>(x2,y2), PixRGB<byte>(0, 255,0));
drawLine(img, Point2D<int>(x2,y2), Point2D<int>(x3,y3), PixRGB<byte>(0, 255,0));
drawLine(img, Point2D<int>(x3,y3), Point2D<int>(x4,y4), PixRGB<byte>(0, 255,0));
drawLine(img, Point2D<int>(x4,y4), Point2D<int>(x1,y1), PixRGB<byte>(0, 255,0));
drawCircle(img, Point2D<int>(cx,cy), 3, PixRGB<byte>(0,255,0));
cvReleaseMat( &my_3d_point);
cvReleaseMat( &my_image_point);
}
void projectGrid(Image<PixRGB<byte> > &img)
{
//draw center point
drawCircle(img, Point2D<int>(img.getWidth()/2, img.getHeight()/2), 3, PixRGB<byte>(255,0,0));
CvMat *rot_mat = cvCreateMat( 3, 3, CV_64FC1);
cvRodrigues2( itsCameraRotation, rot_mat, 0);
int NUM_GRID = 9; //21
CvMat *my_3d_point = cvCreateMat( 3, NUM_GRID * NUM_GRID + 2, CV_64FC1);
CvMat *my_image_point = cvCreateMat( 2, NUM_GRID * NUM_GRID + 2, CV_64FC2);
for ( int i = 0; i < NUM_GRID; i++){
for ( int j = 0; j < NUM_GRID; j++){
cvSetReal2D( my_3d_point, 0, i * NUM_GRID + j,(i * GRID_SIZE));
cvSetReal2D( my_3d_point, 1, i * NUM_GRID + j,(j * GRID_SIZE));
cvSetReal2D( my_3d_point, 2, i * NUM_GRID + j, 0.0);
}
}
cvSetReal2D( my_3d_point, 0, NUM_GRID*NUM_GRID, 0);
cvSetReal2D( my_3d_point, 1, NUM_GRID*NUM_GRID, 0);
cvSetReal2D( my_3d_point, 2, NUM_GRID*NUM_GRID, 0);
cvSetReal2D( my_3d_point, 0, NUM_GRID*NUM_GRID+1, 0);
cvSetReal2D( my_3d_point, 1, NUM_GRID*NUM_GRID+1, 0);
cvSetReal2D( my_3d_point, 2, NUM_GRID*NUM_GRID+1, 30);
cvProjectPoints2( my_3d_point,
itsCameraRotation,
itsCameraTranslation,
itsIntrinsicMatrix,
itsDistortionCoeffs,
my_image_point);
for ( int i = 0; i < NUM_GRID; i++){
for ( int j = 0; j < NUM_GRID-1; j++){
int im_x1 = (int)cvGetReal2D( my_image_point, 0, i * NUM_GRID + j);
int im_y1 = (int)cvGetReal2D( my_image_point, 1, i * NUM_GRID + j);
int im_x2 = (int)cvGetReal2D( my_image_point, 0, i * NUM_GRID + j+1);
int im_y2 = (int)cvGetReal2D( my_image_point, 1, i * NUM_GRID + j+1);
cvLine( img2ipl(img), cvPoint( im_x1, im_y1), cvPoint( im_x2, im_y2), CV_RGB( 0, 255, 0), 1);
}
}
for ( int j = 0; j < NUM_GRID; j++){
for ( int i = 0; i < NUM_GRID-1; i++){
int im_x1 = (int)cvGetReal2D( my_image_point, 0, i * NUM_GRID + j);
int im_y1 = (int)cvGetReal2D( my_image_point, 1, i * NUM_GRID + j);
int im_x2 = (int)cvGetReal2D( my_image_point, 0, (i+1) * NUM_GRID + j);
int im_y2 = (int)cvGetReal2D( my_image_point, 1, (i+1) * NUM_GRID + j);
cvLine( img2ipl(img), cvPoint( im_x1, im_y1), cvPoint( im_x2, im_y2), CV_RGB( 0, 255, 0), 1);
}
}
int im_x0 = (int)cvGetReal2D( my_image_point, 0, NUM_GRID*NUM_GRID);
int im_y0 = (int)cvGetReal2D( my_image_point, 1, NUM_GRID*NUM_GRID);
int im_x = (int)cvGetReal2D( my_image_point, 0, NUM_GRID*NUM_GRID+1);
int im_y = (int)cvGetReal2D( my_image_point, 1, NUM_GRID*NUM_GRID+1);
cvLine( img2ipl(img), cvPoint( im_x0, im_y0), cvPoint( im_x, im_y), CV_RGB( 255, 0, 0), 2); //Z axis
cvReleaseMat( &my_3d_point);
cvReleaseMat( &my_image_point);
cvReleaseMat( &rot_mat);
}
bool
cvFindExtrinsicCameraParams3( const CvMat* obj_points,
const CvMat* img_points, const CvMat* A,
const CvMat* dist_coeffs,
CvMat* r_vec, CvMat* t_vec )
{
bool fGood = true;
const int max_iter = 20;
CvMat *_M = 0, *_Mxy = 0, *_m = 0, *_mn = 0, *_L = 0, *_J = 0;
CV_FUNCNAME( "cvFindExtrinsicCameraParams3" );
__BEGIN__;
int i, j, count;
double a[9], k[4] = { 0, 0, 0, 0 }, R[9], ifx, ify, cx, cy;
double Mc[3] = {0, 0, 0}, MM[9], U[9], V[9], W[3];
double JtJ[6*6], JtErr[6], JtJW[6], JtJV[6*6], delta[6], param[6];
CvPoint3D64f* M = 0;
CvPoint2D64f *m = 0, *mn = 0;
CvMat _a = cvMat( 3, 3, CV_64F, a );
CvMat _R = cvMat( 3, 3, CV_64F, R );
CvMat _r = cvMat( 3, 1, CV_64F, param );
CvMat _t = cvMat( 3, 1, CV_64F, param + 3 );
CvMat _Mc = cvMat( 1, 3, CV_64F, Mc );
CvMat _MM = cvMat( 3, 3, CV_64F, MM );
CvMat _U = cvMat( 3, 3, CV_64F, U );
CvMat _V = cvMat( 3, 3, CV_64F, V );
CvMat _W = cvMat( 3, 1, CV_64F, W );
CvMat _JtJ = cvMat( 6, 6, CV_64F, JtJ );
CvMat _JtErr = cvMat( 6, 1, CV_64F, JtErr );
CvMat _JtJW = cvMat( 6, 1, CV_64F, JtJW );
CvMat _JtJV = cvMat( 6, 6, CV_64F, JtJV );
CvMat _delta = cvMat( 6, 1, CV_64F, delta );
CvMat _param = cvMat( 6, 1, CV_64F, param );
CvMat _dpdr, _dpdt;
if( !CV_IS_MAT(obj_points) || !CV_IS_MAT(img_points) ||
!CV_IS_MAT(A) || !CV_IS_MAT(r_vec) || !CV_IS_MAT(t_vec) )
CV_ERROR( CV_StsBadArg, "One of required arguments is not a valid matrix" );
count = MAX(obj_points->cols, obj_points->rows);
CV_CALL( _M = cvCreateMat( 1, count, CV_64FC3 ));
CV_CALL( _Mxy = cvCreateMat( 1, count, CV_64FC2 ));
CV_CALL( _m = cvCreateMat( 1, count, CV_64FC2 ));
CV_CALL( _mn = cvCreateMat( 1, count, CV_64FC2 ));
M = (CvPoint3D64f*)_M->data.db;
m = (CvPoint2D64f*)_m->data.db;
mn = (CvPoint2D64f*)_mn->data.db;
CV_CALL( cvConvertPointsHomogenious( obj_points, _M ));
CV_CALL( cvConvertPointsHomogenious( img_points, _m ));
CV_CALL( cvConvert( A, &_a ));
if( dist_coeffs )
{
CvMat _k;
if( !CV_IS_MAT(dist_coeffs) ||
CV_MAT_DEPTH(dist_coeffs->type) != CV_64F &&
CV_MAT_DEPTH(dist_coeffs->type) != CV_32F ||
dist_coeffs->rows != 1 && dist_coeffs->cols != 1 ||
dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 )
CV_ERROR( CV_StsBadArg,
"Distortion coefficients must be 1x4 or 4x1 floating-point vector" );
_k = cvMat( dist_coeffs->rows, dist_coeffs->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(dist_coeffs->type)), k );
CV_CALL( cvConvert( dist_coeffs, &_k ));
}
if( CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F ||
r_vec->rows != 1 && r_vec->cols != 1 ||
r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3 )
CV_ERROR( CV_StsBadArg, "Rotation vector must be 1x3 or 3x1 floating-point vector" );
if( CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F ||
t_vec->rows != 1 && t_vec->cols != 1 ||
t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
CV_ERROR( CV_StsBadArg,
"Translation vector must be 1x3 or 3x1 floating-point vector" );
ifx = 1./a[0]; ify = 1./a[4];
cx = a[2]; cy = a[5];
// normalize image points
// (unapply the intrinsic matrix transformation and distortion)
for( i = 0; i < count; i++ )
{
double x = (m[i].x - cx)*ifx, y = (m[i].y - cy)*ify, x0 = x, y0 = y;
// compensate distortion iteratively
if( dist_coeffs )
for( j = 0; j < 5; j++ )
{
double r2 = x*x + y*y;
double icdist = 1./(1 + k[0]*r2 + k[1]*r2*r2);
double delta_x = 2*k[2]*x*y + k[3]*(r2 + 2*x*x);
double delta_y = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y;
x = (x0 - delta_x)*icdist;
y = (y0 - delta_y)*icdist;
}
mn[i].x = x; mn[i].y = y;
// calc mean(M)
Mc[0] += M[i].x;
Mc[1] += M[i].y;
Mc[2] += M[i].z;
}
Mc[0] /= count;
Mc[1] /= count;
Mc[2] /= count;
cvReshape( _M, _M, 1, count );
cvMulTransposed( _M, &_MM, 1, &_Mc );
cvSVD( &_MM, &_W, 0, &_V, CV_SVD_MODIFY_A + CV_SVD_V_T );
// initialize extrinsic parameters
if( W[2]/W[1] < 1e-3 || count < 4 )
{
// a planar structure case (all M's lie in the same plane)
double tt[3], h[9], h1_norm, h2_norm;
CvMat* R_transform = &_V;
CvMat T_transform = cvMat( 3, 1, CV_64F, tt );
CvMat _H = cvMat( 3, 3, CV_64F, h );
CvMat _h1, _h2, _h3;
if( V[2]*V[2] + V[5]*V[5] < 1e-10 )
cvSetIdentity( R_transform );
if( cvDet(R_transform) < 0 )
cvScale( R_transform, R_transform, -1 );
cvGEMM( R_transform, &_Mc, -1, 0, 0, &T_transform, CV_GEMM_B_T );
for( i = 0; i < count; i++ )
{
const double* Rp = R_transform->data.db;
const double* Tp = T_transform.data.db;
const double* src = _M->data.db + i*3;
double* dst = _Mxy->data.db + i*2;
dst[0] = Rp[0]*src[0] + Rp[1]*src[1] + Rp[2]*src[2] + Tp[0];
dst[1] = Rp[3]*src[0] + Rp[4]*src[1] + Rp[5]*src[2] + Tp[1];
}
cvFindHomography( _Mxy, _mn, &_H );
cvGetCol( &_H, &_h1, 0 );
_h2 = _h1; _h2.data.db++;
_h3 = _h2; _h3.data.db++;
h1_norm = sqrt(h[0]*h[0] + h[3]*h[3] + h[6]*h[6]);
h2_norm = sqrt(h[1]*h[1] + h[4]*h[4] + h[7]*h[7]);
cvScale( &_h1, &_h1, 1./h1_norm );
cvScale( &_h2, &_h2, 1./h2_norm );
cvScale( &_h3, &_t, 2./(h1_norm + h2_norm));
cvCrossProduct( &_h1, &_h2, &_h3 );
cvRodrigues2( &_H, &_r );
cvRodrigues2( &_r, &_H );
cvMatMulAdd( &_H, &T_transform, &_t, &_t );
cvMatMul( &_H, R_transform, &_R );
cvRodrigues2( &_R, &_r );
}
else
{
// non-planar structure. Use DLT method
double* L;
double LL[12*12], LW[12], LV[12*12], sc;
CvMat _LL = cvMat( 12, 12, CV_64F, LL );
CvMat _LW = cvMat( 12, 1, CV_64F, LW );
CvMat _LV = cvMat( 12, 12, CV_64F, LV );
CvMat _RRt, _RR, _tt;
CV_CALL( _L = cvCreateMat( 2*count, 12, CV_64F ));
L = _L->data.db;
for( i = 0; i < count; i++, L += 24 )
{
double x = -mn[i].x, y = -mn[i].y;
L[0] = L[16] = M[i].x;
L[1] = L[17] = M[i].y;
L[2] = L[18] = M[i].z;
L[3] = L[19] = 1.;
L[4] = L[5] = L[6] = L[7] = 0.;
L[12] = L[13] = L[14] = L[15] = 0.;
L[8] = x*M[i].x;
L[9] = x*M[i].y;
L[10] = x*M[i].z;
L[11] = x;
L[20] = y*M[i].x;
L[21] = y*M[i].y;
L[22] = y*M[i].z;
L[23] = y;
}
cvMulTransposed( _L, &_LL, 1 );
cvSVD( &_LL, &_LW, 0, &_LV, CV_SVD_MODIFY_A + CV_SVD_V_T );
_RRt = cvMat( 3, 4, CV_64F, LV + 11*12 );
cvGetCols( &_RRt, &_RR, 0, 3 );
cvGetCol( &_RRt, &_tt, 3 );
if( cvDet(&_RR) < 0 )
cvScale( &_RRt, &_RRt, -1 );
sc = cvNorm(&_RR);
cvSVD( &_RR, &_W, &_U, &_V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
cvGEMM( &_U, &_V, 1, 0, 0, &_R, CV_GEMM_A_T );
cvScale( &_tt, &_t, cvNorm(&_R)/sc );
cvRodrigues2( &_R, &_r );
cvReleaseMat( &_L );
}
//
// Check if new r and t are good
//
if ( cvGetReal1D( r_vec, 0 ) ||
cvGetReal1D( r_vec, 1 ) ||
cvGetReal1D( r_vec, 2 ) ||
cvGetReal1D( t_vec, 0 ) ||
cvGetReal1D( t_vec, 1 ) ||
cvGetReal1D( t_vec, 2 ) )
{
//
// perfom this only when the old r and t exist.
//
CvMat * R_inv = cvCreateMat( 3, 3, CV_64FC1 );
CvMat * r_old = cvCreateMat( 3, 1, CV_64FC1 );
CvMat * R_old = cvCreateMat( 3, 3, CV_64FC1 );
CvMat * t_old = cvCreateMat( 3, 1, CV_64FC1 );
// get new center
cvInvert( &_R, R_inv );
double new_center[3];
CvMat newCenter = cvMat( 3, 1, CV_64FC1, new_center );
cvMatMul( R_inv, &_t, &newCenter );
cvScale( &newCenter, &newCenter, -1 );
// get old center
cvConvert( r_vec, r_old );
cvConvert( t_vec, t_old );
cvRodrigues2( r_old, R_old );
cvInvert( R_old, R_inv );
double old_center[3];
CvMat oldCenter = cvMat( 3, 1, CV_64FC1, old_center );
cvMatMul( R_inv, t_old, &oldCenter );
cvScale( &oldCenter, &oldCenter, -1 );
// get difference
double diff_center = 0;
for ( int i = 0; i < 3 ; i ++ )
diff_center += pow( new_center[i] - old_center[i], 2 );
diff_center = sqrt( diff_center );
if ( diff_center > 300 )
{
printf("diff_center = %.2f --> set initial r and t as same as the previous\n", diff_center);
cvConvert(r_vec, &_r);
cvConvert(t_vec, &_t);
fGood = false;
}
// else printf("diff_center = %.2f\n", diff_center );
cvReleaseMat( &R_inv );
cvReleaseMat( &r_old );
cvReleaseMat( &R_old );
cvReleaseMat( &t_old );
}
if ( fGood )
{
CV_CALL( _J = cvCreateMat( 2*count, 6, CV_64FC1 ));
cvGetCols( _J, &_dpdr, 0, 3 );
cvGetCols( _J, &_dpdt, 3, 6 );
// refine extrinsic parameters using iterative algorithm
for( i = 0; i < max_iter; i++ )
{
double n1, n2;
cvReshape( _mn, _mn, 2, 1 );
cvProjectPoints2( _M, &_r, &_t, &_a, dist_coeffs,
_mn, &_dpdr, &_dpdt, 0, 0, 0 );
cvSub( _m, _mn, _mn );
cvReshape( _mn, _mn, 1, 2*count );
cvMulTransposed( _J, &_JtJ, 1 );
cvGEMM( _J, _mn, 1, 0, 0, &_JtErr, CV_GEMM_A_T );
cvSVD( &_JtJ, &_JtJW, 0, &_JtJV, CV_SVD_MODIFY_A + CV_SVD_V_T );
if( JtJW[5]/JtJW[0] < 1e-12 )
break;
cvSVBkSb( &_JtJW, &_JtJV, &_JtJV, &_JtErr,
&_delta, CV_SVD_U_T + CV_SVD_V_T );
cvAdd( &_delta, &_param, &_param );
n1 = cvNorm( &_delta );
n2 = cvNorm( &_param );
if( n1/n2 < 1e-10 )
break;
}
_r = cvMat( r_vec->rows, r_vec->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), param );
_t = cvMat( t_vec->rows, t_vec->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), param + 3 );
cvConvert( &_r, r_vec );
cvConvert( &_t, t_vec );
}
__END__;
cvReleaseMat( &_M );
cvReleaseMat( &_Mxy );
cvReleaseMat( &_m );
cvReleaseMat( &_mn );
cvReleaseMat( &_L );
cvReleaseMat( &_J );
return fGood;
}
int main(int argc, char* argv[]) {
if(argc != 6){
printf("too few args\n");
return -1;
}
// INPUT PARAMETERS:
//
int board_w = atoi(argv[1]);
int board_h = atoi(argv[2]);
int board_n = board_w * board_h;
CvSize board_sz = cvSize( board_w, board_h );
CvMat* intrinsic = (CvMat*)cvLoad(argv[3]);
CvMat* distortion = (CvMat*)cvLoad(argv[4]);
IplImage* image = 0;
IplImage* gray_image = 0;
if( (image = cvLoadImage(argv[5])) == 0 ) {
printf("Error: Couldn't load %s\n",argv[5]);
return -1;
}
CvMat* image_points = cvCreateMat(1*board_n,2,CV_32FC1);
CvMat* object_points = cvCreateMat(1*board_n,3,CV_32FC1);
CvMat* objdrawpoints = cvCreateMat(1,1,CV_32FC3);
CvMat* imgdrawpoints = cvCreateMat(1,1,CV_32FC2);
float x=0;
float y=0;
float z=0;
double grid_width=2.85;
gray_image = cvCreateImage( cvGetSize(image), 8, 1 );
cvCvtColor(image, gray_image, CV_BGR2GRAY );
CvPoint2D32f* corners = new CvPoint2D32f[ board_n ];
int corner_count = 0;
int found = cvFindChessboardCorners(
gray_image,
board_sz,
corners,
&corner_count,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS
);
if(!found){
printf("Couldn't aquire chessboard on %s, "
"only found %d of %d corners\n",
argv[5],corner_count,board_n
);
return -1;
}
//Get Subpixel accuracy on those corners:
cvFindCornerSubPix(
gray_image,
corners,
corner_count,
cvSize(11,11),
cvSize(-1,-1),
cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1 )
);
// If we got a good board, add it to our data
for( int i=0, j=0; j<board_n; ++i,++j ) {
CV_MAT_ELEM(*image_points, float,i,0) = corners[j].x;
CV_MAT_ELEM(*image_points, float,i,1) = corners[j].y;
CV_MAT_ELEM(*object_points,float,i,0) =grid_width*( j/board_w);
// cout<<j/board_w<<" "<<j%board_w<<endl;
CV_MAT_ELEM(*object_points,float,i,1) = grid_width*(j%board_w);
CV_MAT_ELEM(*object_points,float,i,2) = 0.0f;
}
// DRAW THE FOUND CHESSBOARD
//
cvDrawChessboardCorners(
image,
board_sz,
corners,
corner_count,
found
);
// FIND THE HOMOGRAPHY
//
CvMat *trans = cvCreateMat( 1, 3, CV_32F);
CvMat *rot = cvCreateMat( 1, 3, CV_32F);
// LET THE USER ADJUST THE Z HEIGHT OF THE VIEW
//
cvFindExtrinsicCameraParams2(object_points,image_points,intrinsic,distortion,rot,trans);
// cvSave("trans.xml",trans);
// cvSave("rot.xml",rot);
int key = 0;
IplImage *drawn_image = cvCloneImage(image);
cvNamedWindow("translation");
// LOOP TO ALLOW USER TO PLAY WITH HEIGHT:
//
// escape key stops
//
// cvSetZero(trans);
// cvSetZero(rot);
while(key != 27) {
cvCopy(image,drawn_image);
if(key==97)x--;
else if(key==113)x++;
else if(key==115)y--;
else if(key==119)y++;
else if(key==100)z--;
else if(key==101)z++;
((float*)(objdrawpoints->data.ptr))[0]=x;
((float*)(objdrawpoints->data.ptr))[1]=y;
((float*)(objdrawpoints->data.ptr))[2]=z;
printf("%f %f %f\n",x,y,z);
cvProjectPoints2(objdrawpoints,rot,trans,intrinsic,distortion,imgdrawpoints);
cvCircle(drawn_image,cvPoint(((float*)(imgdrawpoints->data.ptr))[0],((float*)(imgdrawpoints->data.ptr))[1]),5,cvScalar(255,0,0),-1);
printf("%f %f\n",((float*)(imgdrawpoints->data.ptr))[0],((float*)(imgdrawpoints->data.ptr))[1]);
cvShowImage( "translation", drawn_image );
key = cvWaitKey(3);
}
cvDestroyWindow( "translation" );
//must add a lot of memory releasing here
return 0;
}