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.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