calibration

calibration

void calibrate_lidarwithradar(std::vector<cv::Point3f> world_points,std::vector<cv::Point2f> rad_points ){
    cv::Mat cameraMatrix(3,3,cv::DataType<double>::type);
    cv::setIdentity(cameraMatrix);

    cv::Mat distCoeffs(4,1,cv::DataType<double>::type);
    distCoeffs.at<double>(0) = 0;
    distCoeffs.at<double>(1) = 0;
    distCoeffs.at<double>(2) = 0;
    distCoeffs.at<double>(3) = 0;

    cv::Mat rvec(3,1,cv::DataType<double>::type);
    cv::Mat tvec(3,1,cv::DataType<double>::type);

    cv::solvePnP(world_points, rad_points, cameraMatrix, distCoeffs, rvec, tvec);

    std::cout << "rvec: " << rvec << std::endl;
    std::cout << "tvec: " << tvec << std::endl;

    std::vector<cv::Point2f> projectedPoints;
    cv::projectPoints(world_points, rvec, tvec, cameraMatrix, distCoeffs, projectedPoints);
    
    for(unsigned int i = 0; i < projectedPoints.size(); ++i)
    {
        std::cout << "Image point: " << rad_points[i] << " Projected to " << projectedPoints[i] << std::endl;
    }
    cv::Mat R = cv::Mat::eye(3,3,CV_32F);
    cv::Rodrigues(rvec,R);
    // std::cout << "rotation matrix" << R << std::endl;
    cv::Mat projection_matrix = computeProjMat(R, tvec);
    std::cout<< "proj matrix\n" << projection_matrix << std::endl;
    cv::Mat inv_proj_mtx = projection_matrix.inv();
    std::cout<< "inverse proj matrix\n" << inv_proj_mtx << std::endl;

    std::cout <<"starting to convert back the points from radar 2 lidar" << std::endl;
    cv::Mat_<double> point(4,1);
    for(int i =0;i < rad_points.size();i++){
        point(0, 0) = rad_points[i].x;
        point(1, 0) = rad_points[i].y;
        point(2, 0) = 0.0; //+ point(0, 0) * sin(tilt_angle * PI/180);
        point(3, 0) = 1.0;

        point = inv_proj_mtx*point;

        std::cout << "radar point " << world_points[i] << " new lidar point " << point << std::endl;
    }
}

-0.00286325     -0.00043507      3.05429435 
-0.00122530      0.00669575     -8.06684494 
-0.00011358     -0.00366075      1.00000000 

p1p2
p1 -1.58 12.54
p2 1.41 12.53

p3p4
p3 -1.56 19.00
p4 1.45 18.99

p5p6
p5 -1.56 29.54
p6 1.46 29.63

after centering the image:

 0.00000000      0.00000000 

-0.00334384      0.00030351      3.15223289 
 0.00041359      0.00820327    -11.84533882 
 0.00004424     -0.00425690      1.00000000 

-0.00334384      0.00030351      3.15223289 
 0.00041359      0.00820327    -11.84533882 
 0.00004424     -0.00425690      1.00000000 

inverse:

  [[-3.1461e+02 -1.0225e+02 -2.1947e+02]
   [-6.9867e+00 -2.5956e+01 -2.8543e+02]
   [-1.5823e-02 -1.0597e-01 -2.0533e-01]]

last points, for the calibration above

p1  -2.99  	7.17
p2  2.90  	7.07
p3  -6.00  	20.91
p4  6.67  	21.20

 0.00000000      0.00000000 

-0.00349348      0.00046639      3.28094697 
 0.00052752      0.00837598    -12.38357639 
 0.00006577     -0.00436319      1.00000000 

-0.00349348      0.00046639      3.28094697 
 0.00052752      0.00837598    -12.38357639 
 0.00006577     -0.00436319      1.00000000 

-0.00369643     -0.00039033      3.87230778 
 0.00022367      0.01109517    -13.05994225 
 0.00001367     -0.00433823      1.00000000 

 0.00000000      0.00000000 

-0.00081330      0.00100924      0.59634918 
-0.00006421     -0.06464103     21.67111397 
-0.00000128     -0.00311338      1.00000000 

-0.00081330      0.00100924      0.59634918 
-0.00006421     -0.06464103     21.67111397 
-0.00000128     -0.00311338      1.00000000