位姿估计_1

说在前面的话

位姿估计（Pose estimation）在计算机视觉领域扮演着十分重要的角色。在使用视觉传感器估计机器人位姿进行控制、机器人导航、增强现实以及其它方面都有着极大的应用。位姿估计这一过程的基础是找到现实世界和图像投影之间的对应点。然后根据这些点对的类型，如2D-2D, 2D-3D, 3D-3D，采取相应的位姿估计方法。当然同一类型的点对也有基于代数和非线性优化的方法之分，如直接线性变（Direct Linear Transform, DLT）换和光束平差法（Bundle Adjustment，BA）。我们通常称把根据已知点对估计位姿的过程成为求解PnP。

我打算分三次来写位姿估计，第一篇是简单的应用，主要是求相机和目标之间的相对位姿，第二篇是从两张图片中找出若干匹配点对，使用单应矩阵的方法来求解两张图片间的位姿变换，第三篇是使用灭点（vanishing point）来估计相机位姿，这是第一篇，写的不好的请留言指出，谢谢～

1. 准备

1. 安装opencv及opencv-contrib的arcuo module
   在这里我们使用opencv-contrib的aruco的Marker作为位姿估计的对象。aruco模块基于ArUco库，这是一个检测二进制marker的非常流行的库，它的函数包含c++ 头文件aruco.hpp，详见opencv官网和中文翻译 。

2. 一张打印Marker的纸，如图
   

2. 代码步骤

1. 使用getPredefinedDictionary创建一个字典
2. 读取每一帧，使用detectMarkers检测当前帧中的Markers
3. 使用estimatePoseSingleMarkers估计Marker的位姿，得到的即是即是R和t

#include <ctime>
#include <string>
#include <cstdio>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
using namespace std;
using namespace cv;
const Mat
intrinsic_matrix = (Mat_<float>(3, 3)
<< 803.9233,
0,
286.5234,
0,
807.6013,
245.9685,
0,
0,
1);
const Mat
distCoeffs = (Mat_<float>(5, 1) << 0.1431, -0.4943, 0, 0, 0);
const Mat
arucodistCoeffs = (Mat_<float>(1, 5) << 0, 0, 0, 0, 0);//矫正后的照片用于检测
int main(int args, char *argv[])
{
VideoCapture cap(0);
Mat frame,framecopy;
Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
while (true)
{
cap>>frame;
frame.copyTo(framecopy);
vector<int> ids;
vector<vector<Point2f> > corners;
cv::aruco::detectMarkers(framecopy, dictionary, corners, ids);
black.setTo(Scalar(255,255,255,0));
if (ids.size() > 0)
{
cv::aruco::drawDetectedMarkers(frame, corners, ids);
vector< Vec3d > rvecs, tvecs;
cv::aruco::estimatePoseSingleMarkers(corners, 0.10, intrinsic_matrix, arucodistCoeffs, rvecs, tvecs); // draw axis for each marker
cout<<"R :"<<rvecs[0]<<endl;
cout<<"T :"<<tvecs[0]<<endl;
for(int i=0; i<ids.size(); i++)
{
cv::aruco::drawAxis(frameCalibration, intrinsic_matrix, arucodistCoeffs, rvecs[i], tvecs[i], 0.1);
}
}
imshow("frame", frame);
if( char(waitKey(30))==' ')
break;
}
return 0;
}

3. 运行结果（arcuo库）

4. 思考

这里虽然得到了Marker的位姿，相对还是比较准确的，但是当我们检测的不是Marker而是其他物体时如何将估计位姿的方法应用起来呐？

这里有几个条件：

1. 已知检测目标的像素坐标。在Marker中，在detectMarker函数中，我们已经获得了其四个顶点的像素坐标，这是2D点
2. 已知检测目标的真实物理尺寸。在estimatePoseSingleMarkers函数中，第二个参数float markerLength 是Markers的真实尺寸，（单位是米或者其它）。注意Pose检测的平移矩阵t单位都是相同的。在这里，我们人为的把Markers的初始位置放在了相机坐标系z=0的平面上，由于Markers是正方形，所以根据边长也就得到了其四个定点在相机坐标系中的坐标，这是3D点
3. 得到2D点对和3D点对之后，就可以使用Opencv中的SlovePnP函数求解，点击查看函数详情,注意点对的匹配顺序！该函数原型是

bool cv::solvePnP	(	InputArray
objectPoints,
InputArray
imagePoints,
InputArray
cameraMatrix,
InputArray
distCoeffs,
OutputArray
rvec,
OutputArray
tvec,
bool
useExtrinsicGuess = false,
int
flags = SOLVEPNP_ITERATIVE //注意该参数的选择即可
)

5. 扩展

我给arcuo中的estimatePoseSingleMarkers函数换了一个外壳，方便大家更好的解决其他方面的PnP问题，这是运行的结果

6. **（不晓得该叫什么了）

其实在给arcuo更换外壳以便更方便的使用之前，自己可以获得需要进行位姿估计的目标的至少四个2D点，但是一直在纠结不能获得其对应的3D点坐标。在使用了arcuo后，发现3D坐标就是将目标物体放在世界坐标系z=0的平面上，想了一想，好有道理啊，豁然开朗！！！
之前也参考过一些博客，他是一个系列，也贴出来方便大家一块学习，点击浏览

7. 疑惑的地方(更新,疑惑是错误的)

我还是一直认为在估计相机和目标物体间的相对位姿时，每时每刻世界坐标系都是和相机坐标系相重合的，因为Markers的3D点坐标就应该是世界坐标系下的坐标啊，而且在获取3D点坐标时是放在相机坐标系z=0的平面上的。所以博友们，我能这样理解么？请赐教一下啊

8. 代码

这段代码可以直接拿来使用，但是有几个需要注意的地方

1. 位姿估计目标的真是尺寸在头文件的结构体中进行相应的修改
2. 2D和3D点一定要相互匹配
3. solvePnP求解方法的选择

/*****
Source file
*****/
#include <iostream>
#include <Eigen/Eigen>
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
#include "getPose.h"
using namespace std;
using namespace cv;
vector<Point3f> get3DPoints(targetInfo &strTargetInfo)
{
vector<Point3f> objPoints;
// set coordinate system in the middle of the marker, with Z pointing out
objPoints.push_back( Point3f(-strTargetInfo.markerLength / 2.f,
strTargetInfo.markerWidth / 2.f, 0));
objPoints.push_back( Point3f( strTargetInfo.markerLength / 2.f,
strTargetInfo.markerWidth / 2.f, 0));
objPoints.push_back( Point3f( strTargetInfo.markerLength / 2.f, -strTargetInfo.markerWidth / 2.f, 0));
objPoints.push_back( Point3f(-strTargetInfo.markerLength / 2.f, -strTargetInfo.markerWidth / 2.f, 0));
return objPoints;
}
// the input corners'order shoule be
//LefeUp -> RightUp -> RightDown -> LeftDown
void estimatePose( vector<Point2f> corners, Mat cameraMatrix, Mat distCoeffs, targetInfo &strTargetInfo)
{
if(corners.size()!=4)
return;
vector<Point3f> odjPoints3d;
odjPoints3d = get3DPoints(strTargetInfo);
Vec3d rvecs, tvecs;
// for each corners, calculate its pose
solvePnP(odjPoints3d, corners, cameraMatrix, distCoeffs, rvecs, tvecs); //only 4 points
Mat rMatrix = Mat(3,3,CV_64F);
cv::Rodrigues(rvecs, rMatrix);
Eigen::Matrix3d R;
R << rMatrix.at<double>(0,0), rMatrix.at<double>(0,1), rMatrix.at<double>(0,2),
rMatrix.at<double>(1,0), rMatrix.at<double>(1,1), rMatrix.at<double>(1,2),
rMatrix.at<double>(2,0), rMatrix.at<double>(2,1), rMatrix.at<double>(2,2);
Eigen::Vector3d eular_angle;
eular_angle = R.eulerAngles(0,1,2);
//radian
eular_angle = R.eulerAngles(0,1,2)*180.f/M_PI; //angle
//cout<<"R :"<<eular_angle<<endl;
//cout<<"t :"<<tvecs<<endl;
strTargetInfo.R.thetaX = eular_angle[0];
strTargetInfo.R.thetaY = eular_angle[1];
strTargetInfo.R.thetaZ = eular_angle[2];
strTargetInfo.t.x = tvecs[0];
strTargetInfo.t.y = tvecs[1];
strTargetInfo.t.z = tvecs[2];
}
/*****
Header file
*****/
#ifndef _GETPOSE_H_
#define _GETPOSE_H_
#include <iostream>
typedef struct _targetInfo
{
struct{
float thetaX;
float thetaY;
float thetaZ;
}R;
struct{
float x;
float y;
float z;
}t;
float markerLength=0.1f;
//meter
float markerWidth =0.1f;
}targetInfo;
std::vector<cv::Point3f> get3DPoints(targetInfo &strTargetInfo);
// the input corners'order shoule be
//LefeUp -> RightUp -> RightDown -> LeftDown
void estimatePose(std::vector<cv::Point2f> corners, cv::Mat cameraMatrix, cv::Mat distCoeffs, targetInfo &strTargetInfo);
#endif

最后

以上就是合适白羊为你收集整理的位姿估计_1的全部内容，希望文章能够帮你解决位姿估计_1所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。