Created
November 24, 2020 07:06
-
-
Save ixtiyoruz/b109ce262823a899b1e5d474135e6edc to your computer and use it in GitHub Desktop.
calibration
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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; | |
} | |
} |
Author
ixtiyoruz
commented
Mar 18, 2021
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment