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);       	
      }
   
}
Exemple #2
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 );
}
Exemple #3
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 );
}
Exemple #4
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;
}
Exemple #9
0
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;
}