c++/opencv利用相机位姿估计实现2D图像像素坐标到3D世界坐标的转换

最近在做自动泊车项目中的车位线检测，用到了将图像像素坐标转换为真实世界坐标的过程，该过程可以通过世界坐标到图像像素坐标之间的关系进行求解，在我的一篇博文中已经详细讲解了它们之间的数学关系，不清楚的童鞋们可参考:相机标定：从世界坐标系到图像像素坐标系转换过程解析
推导过程如下：

一般情况下，摄像头的内参是我们事先标定好的，具体标定方法可以参考这篇博文：张正友相机标定Opencv实现以及标定流程&&标定结果评价&&图像矫正流程解析（附标定程序和棋盘图）
其次，用到了相机位姿估计中求解PNP的问题，相机位姿估计就是通过几个已知坐标的特征点，以及他们在相机照片中的成像，从而得到相机坐标系关于世界坐标系的旋转矩阵R与平移矩阵T，可以直接调用opencv::solvePnP函数求解，详细讲解可以参考这篇博文：相机位姿估计
具体标定方法如下：
1.以车身中心投影到地面的位置为世界坐标系的原点，垂直于地面朝上为z轴正向，车头方向为x轴的正向，y轴方向满足右手法则。
2.车位线识别中只用到了右侧摄像头，在水平地面上放置标定板，不要共线，记录出标定板的世界坐标（至少需要4个点）以及在图像中的像素坐标。

3.调用opencv::solvePnP求解相机坐标系关于世界坐标系的旋转矩阵R与平移矩阵T。
4.根据上面公式的推倒求解出图像中车位线在世界坐标中的真实位置。
下面是实现该过程的c++/opencv的代码：

#include <opencv2/opencv.hpp>
#include <iostream>
#include <ctype.h>
using namespace cv;
using namespace std;
int main()
{// 首先通过标定板的图像像素坐标以及对应的世界坐标，通过PnP求解相机的R&T//Point2f point;vector<Point2f> boxPoints; //存入像素坐标// Loading imageMat sourceImage = imread("2.bmp");namedWindow("Source", 1);/ Setting box corners in image//one Pointpoint = Point2f((float) 558, (float) 259); //640X480boxPoints.push_back(point);circle(sourceImage, boxPoints[0], 3, Scalar(0, 255, 0), -1, 8);two Pointpoint = Point2f((float) 629, (float) 212); //640X480boxPoints.push_back(point);circle(sourceImage, boxPoints[1], 3, Scalar(0, 255, 0), -1, 8);three Pointpoint = Point2f((float) 744, (float) 260); //640X480boxPoints.push_back(point);circle(sourceImage, boxPoints[2], 3, Scalar(0, 255, 0), -1, 8);four Pointpoint = Point2f((float) 693, (float) 209); //640X480boxPoints.push_back(point);circle(sourceImage, boxPoints[3], 3, Scalar(0, 255, 0), -1, 8);//Setting box corners in real worldvector<Point3f> worldBoxPoints;  //存入世界坐标Point3f tmpPoint;tmpPoint = Point3f((float) 2750, (float) 890, (float) 0);worldBoxPoints.push_back(tmpPoint);tmpPoint = Point3f((float) 3500, (float) 450, (float) 0);worldBoxPoints.push_back(tmpPoint);tmpPoint = Point3f((float) 2790, (float) -240, (float) 0);worldBoxPoints.push_back(tmpPoint);tmpPoint = Point3f((float) 3620, (float) -50, (float) 0);worldBoxPoints.push_back(tmpPoint);//camera  intristic///cv::Mat cameraMatrix1=Mat::eye(3, 3, cv::DataType<double>::type);  //相机内参矩阵cv::Mat distCoeffs1(5, 1, cv::DataType<double>::type);  //畸变参数distCoeffs1.at<double>(0,0) = 0.061439051;distCoeffs1.at<double>(1,0) = 0.03187556;distCoeffs1.at<double>(2,0) = -0.00726151;distCoeffs1.at<double>(3,0) = -0.00111799;distCoeffs1.at<double>(4,0) = -0.00678974;//Taken from Mastring OpenCV ddouble fx = 328.61652824;double fy = 328.56512516;double cx = 629.80551148;double cy = 340.5442837;cameraMatrix1.at<double>(0, 0) = fx;cameraMatrix1.at<double>(1, 1) = fy;cameraMatrix1.at<double>(0, 2) = cx;cameraMatrix1.at<double>(1, 2) = cy;//PnP solve R&T///cv::Mat rvec1(3, 1, cv::DataType<double>::type);  //旋转向量cv::Mat tvec1(3, 1, cv::DataType<double>::type);  //平移向量cv::solvePnP(worldBoxPoints, boxPoints, cameraMatrix1, distCoeffs1, rvec1, tvec1, false,CV_ITERATIVE);cv::Mat rvecM1(3, 3, cv::DataType<double>::type);  //旋转矩阵cv::Rodrigues(rvec1, rvecM1);/此处用于求相机位于坐标系内的旋转角度,2D-3D的转换并不用求const double PI=3.1415926;double thetaZ=atan2(rvecM1.at<double>(1,0),rvecM1.at<double>(0,0))/PI*180;double thetaY=atan2(-1*rvecM1.at<double>(2,0),sqrt(rvecM1.at<double>(2,1)*rvecM1.at<double>(2,1)+rvecM1.at<double>(2,2)*rvecM1.at<double>(2,2)))/PI*180;double thetaX=atan2(rvecM1.at<double>(2,1),rvecM1.at<double>(2,2))/PI*180;cout<<"theta x  "<<thetaX<<endl<<"theta Y: "<<thetaY<<endl<<"theta Z: "<<thetaZ<<endl;///根据公式求Zc，即scv::Mat imagePoint = cv::Mat::ones(3, 1, cv::DataType<double>::type); cv::Mat tempMat, tempMat2;//输入一个2D坐标点，便可以求出相应的simagePoint.at<double>(0,0)=558;imagePoint.at<double>(1,0)=259;double zConst = 0;//实际坐标系的距离//计算参数sdouble s;tempMat = rvecM1.inv() * cameraMatrix1.inv() * imagePoint;tempMat2 = rvecM1.inv() * tvec1;s = zConst + tempMat2.at<double>(2, 0);s /= tempMat.at<double>(2, 0);cout<<"s : "<<s<<endl;///3D to 2Dcv::Mat worldPoints=Mat::ones(4,1,cv::DataType<double>::type);worldPoints.at<double>(0,0)=3620;worldPoints.at<double>(1,0)=-590;worldPoints.at<double>(2,0)=0;cout<<"world Points :  "<<worldPoints<<endl;Mat image_points=Mat::ones(3,1,cv::DataType<double>::type);//setIdentity(image_points);Mat RT_;hconcat(rvecM1,tvec1,RT_);cout<<"RT_"<<RT_<<endl;image_points=cameraMatrix1*RT_*worldPoints;Mat D_Points=Mat::ones(3,1,cv::DataType<double>::type);D_Points.at<double>(0,0)=image_points.at<double>(0,0)/image_points.at<double>(2,0);D_Points.at<double>(1,0)=image_points.at<double>(1,0)/image_points.at<double>(2,0);//cv::projectPoints(worldPoints, rvec1, tvec1, cameraMatrix1, distCoeffs1, imagePoints);cout<<"3D to 2D:   "<<D_Points<<endl;//camera_coordinatesMat camera_cordinates=-rvecM1.inv()*tvec1;/2D to 3D///cv::Mat imagePoint_your_know = cv::Mat::ones(3, 1, cv::DataType<double>::type); //u,v,1imagePoint_your_know.at<double>(0, 0) = 558;imagePoint_your_know.at<double>(1, 0) = 259;Mat wcPoint = rvecM1.inv() * (cameraMatrix1.inv() *s*imagePoint_your_know - tvec1);Point3f worldPoint(wcPoint.at<double>(0, 0), wcPoint.at<double>(1, 0), wcPoint.at<double>(2, 0));cout <<"2D to 3D :"<< worldPoint << endl;imshow("Source",sourceImage);waitKey(0);return 0;
}

求出各种参数以后可以将其封装为一个函数，实现图像坐标到世界坐标的转换：

Point3f getWorldPoints(Point2f inPoints)
{cv::Mat imagePoint = cv::Mat::ones(3, 1, cv::DataType<double>::type); //u,v,1imagePoint.at<double>(0, 0) = inPoints.x;imagePoint.at<double>(1, 0) = inPoints.y;Mat wcPoint = rotationMatrix.inv() * (s * cameraMatrix.inv() * imagePoint - tvec);Point3f worldPoint(wcPoint.at<double>(0, 0), wcPoint.at<double>(1, 0), wcPoint.at<double>(2, 0));return worldPoint;
}

c++/opencv利用相机位姿估计实现2D图像像素坐标到3D世界坐标的转换相关推荐

相机位姿估计2：[应用]实时位姿估计与三维重建相机姿态
关键词:相机位姿估计 OpenCV::solvePnP labview三维图片文章类型:应用展示+Demo演示 @Author:VShawn(singlex@foxmail.com) @Date:2 ...
相机计算坐标公式_相机位姿估计3：根据两幅图像的位姿估计结果求某点的世界坐标...
关键词:相机位姿估计,单目尺寸测量,环境探知用途:基于相机的环境测量,SLAM,单目尺寸测量文章类型:原理说明.Demo展示 @Author:VShawn @Date:2016-11-28 @La ...
《增强现实：原理、算法与应用》读书笔记（5）运动恢复结构（上）初始化、相机位姿估计、集束调整
<增强现实:原理.算法与应用>读书笔记(5)运动恢复结构(上)初始化.相机位姿估计.集束调整运动恢复结构(SfM)是一种从运动的相机拍摄的图像或视频序列中自动地恢复出相机运动轨迹以及场景 ...
【SLAM文献】2017-2018 CVPR ICCV ECCV 相机位姿估计、视觉定位、SLAM相关论文综述
作者:变胖是梦想2014 来源链接:https://www.jianshu.com/p/22151f39b50c 目录 CVPR-2018 references CVPR-2017 reference ...
OpenCV入门基础操作（二）----图像像素的处理
OpenCV入门基础操作(二)----图像像素的处理像素处理读取一个图像像素修改像素值代码案例像素处理读取一个图像像素在读取图像的时候一般要用到如下的命令: 返回值=图像(位置参数), ...
密集人体姿态估计：2D图像帧可实时生成UV贴图（附论文）
Root 编译整理量子位出品 | 公众号 QbitAI Facebook人工智能研究院和法国国立计算机及自动化研究院最近提出了一种密集人体姿态估计新方法:DensePose-RCNN,同时宣布即将 ...
相机标定 2D图像到3D坐标转换像素坐标转到世界坐标时相机坐标系中的Zc值求解：线结构光平面标定法
线激光平面拟合图像在单目视觉中会丢失掉焊缝的深度信息,为了后续求解焊缝的世界坐标则需要先求解出焊缝的深度信息才可以进行坐标计算. 在α和β平面分别对标定板进行拍照多幅图片(我采集了13幅)进行相机标 ...
英伟达这篇CVPR 2022 Oral火了！2D图像秒变逼真3D物体！虚拟爵士乐队来了！
点击下方卡片,关注"CVer"公众号 AI/CV重磅干货,第一时间送达你见过乐器自己演奏么?看看这个: 图1. "活灵活现"的虚拟乐器还是在 NVIDIA 服 ...
opencv C++ 三重for循环遍历RGB图像像素（逐像素操作、操作像素值、遍历像素遍历）at＜typename＞(i,j)、灰度图at＜uchar＞、at＜Vec3b＞、at＜Vec3F＞
示例:数组遍历法at<typename>(i,j) -- 其中一种方法(速度可能并不是很快),如果对速度有要求可查看参考文章2里的其他方法 #include <opencv2/ope ...

c++/opencv利用相机位姿估计实现2D图像像素坐标到3D世界坐标的转换

c++/opencv利用相机位姿估计实现2D图像像素坐标到3D世界坐标的转换相关推荐

最新文章

热门文章