opencv图像去畸变,把点云投影的图像上
模块1:当已知相机内参的时候,输入原始图像,输出去除畸变的图像
原始图像和去畸变图像效果对比:左图是去畸变图,右图是原始图像
代码:
#include
#include
#include
#include
#include
#include
#include
int main(int argc, const char** argv) {
cv::Mat source_image=cv::imread(argv[1]);
//cv::imshow("source_image",source_image);
cv::Mat out_image,mapx,mapy,K_Mat,K_Mat_New;
Eigen::Matrix3d K_Eigen,K_Eigen_New;
double fx,fy,cx,cy,ratio;
fx=935.94474;
fy=938.5178;
cx=942.50075;
cy= 553.61195;
ratio=0.8;
K_Eigen<(0)=D_Eigen(0);
D_Mat.at(1)=D_Eigen(1);
D_Mat.at(2)=D_Eigen(2);
D_Mat.at(3)=D_Eigen(3);
cv::eigen2cv(K_Eigen, K_Mat);
cv::eigen2cv(K_Eigen_New, K_Mat_New);
//对于鱼眼相机
cv::fisheye::initUndistortRectifyMap(K_Mat,D_Mat, cv_R, K_Mat_New, cv_size, CV_16SC2, mapx, mapy);
//对于针孔相机
//针孔相机的畸变系数就变成k1 k2 p1 p2 p3
//cv::initUndistortRectifyMap(K_Mat,D_Mat, cv_R, K_Mat_New, cv_size, CV_16SC2, mapx, mapy);
cv::remap(source_image, out_image, mapx, mapy, cv::INTER_LINEAR, cv::BORDER_CONSTANT);
cv::imshow("out_image",out_image);
cv::imwrite("outimage.jpg",out_image);
cv::waitKey(0);
return 0;
} 常见坐标系整理
novatel y轴朝前,x轴朝右
pandar, 后座线的方向是y轴方向
vlp 后座线的反方向是y轴方向
rslidar 后座线的反向方是y轴方向
相机 镜头方向是是z轴方向
注意:符合右手法则
模块二:将点云投影到图像的像素平面上面
投影后的图像和去畸变图像效果对比:左图是去畸变图,右图是投影后的图像
代码:
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
int main(int argc, const char** argv) {
cv::Mat source_image=cv::imread(argv[1]);
pcl::PointCloud::Ptr cloud_in(new pcl::PointCloud);
if (pcl::io::loadPLYFile(argv[2], *cloud_in) == -1) //* load the ply file
{
PCL_ERROR("Couldn't read file test_pcd.pcd \n");
system("PAUSE");
return (-1);
}
//载入相机外参,这个外参相机坐标系下的激光雷达的位移+旋转
//针对这个旋转,相当于吧激光雷达旋转到相机的四元数
Eigen::Vector3d t_vlp_in_camera;
double x,y,z,qw,qx,qy,qz;
x=0.05,y=-0.25,z=-0.45;
qw=0.7967406380680323;
qx=0.5973086920321353;
qy=-0.019489554988954232;
qz=0.048301723565409322;
t_vlp_in_camera<::Ptr transformed_cloud(new pcl::PointCloud);
pcl::transformPointCloud (*cloud_in, *transformed_cloud, d_SE3.matrix());//输入的是变换矩阵
//载入相机内参
cv::Mat out_image,mapx,mapy,K_Mat,K_Mat_New;
Eigen::Matrix3d K_Eigen,K_Eigen_New;
double fx,fy,cx,cy,ratio;
fx=935.94474;
fy=938.5178;
cx=942.50075;
cy= 553.61195;
ratio=0.6;
K_Eigen<(0)=D_Eigen(0);
D_Mat.at(1)=D_Eigen(1);
D_Mat.at(2)=D_Eigen(2);
D_Mat.at(3)=D_Eigen(3);
cv::eigen2cv(K_Eigen, K_Mat);
cv::eigen2cv(K_Eigen_New, K_Mat_New);
//对于鱼眼相机
cv::fisheye::initUndistortRectifyMap(K_Mat,D_Mat, cv_R, K_Mat_New, cv_size, CV_16SC2, mapx, mapy);
//对于针孔相机
//针孔相机的畸变系数就变成k1 k2 p1 p2 p3
//cv::initUndistortRectifyMap(K_Mat,D_Mat, cv_R, K_Mat_New, cv_size, CV_16SC2, mapx, mapy);
cv::remap(source_image, out_image, mapx, mapy, cv::INTER_LINEAR, cv::BORDER_CONSTANT);
std::vector project_p3ds;//每个点在像素平面上的坐标集合
double max_distance=0;
for(int i =0; i< transformed_cloud->size();i++)
{
Eigen::Vector3d p_eigen;//定义一个点的坐标从pcl->eigen
p_eigen<points[i].x,transformed_cloud->points[i].y,transformed_cloud->points[i].z;
cv::Mat transformed_point = cv::Mat::zeros(3, 1, CV_64F);//eigen->mat
//遇到的问题 在pcl->mat过程中遇到了问题
//transformed_point.at(0,0)=transformed_cloud->points[i].x;
cv::eigen2cv(p_eigen,transformed_point);
//point的坐标系上面的单是y轴超前,所以这个mat是(2,0)
if(transformed_point.at(2,0)<=2)
{
continue;
}
cv::Point3d project_p3d;//mat 转成cv::point 3d
// 对这些点进行去畸变
cv::Mat project_p3d_in_outImage=K_Mat_New*transformed_point;//在去畸变图上投影
cv::Mat project_p3d_in_sourceImage=K_Mat*transformed_point;//在原图上投影
project_p3d.x= project_p3d_in_outImage.at(0,0);
project_p3d.y= project_p3d_in_outImage.at(1,0);
project_p3d.z= project_p3d_in_outImage.at(2,0);
project_p3ds.push_back(project_p3d);
//为了找到最远点的距离,然后用不同的颜色表示距离
if(max_distance