位姿估计_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 
#include 
#include 
#include 

#include 
#include 

using namespace std;
using namespace cv;

const Mat  intrinsic_matrix = (Mat_(3, 3)
                               << 803.9233,  0,         286.5234,
                                  0,         807.6013,  245.9685,
                                  0,         0,         1);

const Mat  distCoeffs = (Mat_(5, 1) << 0.1431, -0.4943, 0, 0, 0);


const Mat  arucodistCoeffs = (Mat_(1, 5) << 0, 0, 0, 0, 0);//矫正后的照片用于检测

int main(int args, char *argv[])
{
        VideoCapture cap(0);

        Mat frame,framecopy;
        Ptr dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);

        while (true)
        {
            cap>>frame;
            frame.copyTo(framecopy);

            vector ids;
            vector > 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 :"<

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 

#include 

#include 
#include 

#include "getPose.h"

using namespace std;
using namespace cv;


vector get3DPoints(targetInfo &strTargetInfo)
{
    vector 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 corners, Mat cameraMatrix, Mat distCoeffs, targetInfo &strTargetInfo)
{
    if(corners.size()!=4)
        return;

    vector 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(0,0), rMatrix.at(0,1), rMatrix.at(0,2),
            rMatrix.at(1,0), rMatrix.at(1,1), rMatrix.at(1,2),
            rMatrix.at(2,0), rMatrix.at(2,1), rMatrix.at(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 :"<

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 get3DPoints(targetInfo &strTargetInfo);

// the input corners'order shoule be
//LefeUp -> RightUp -> RightDown -> LeftDown
void estimatePose(std::vector corners, cv::Mat cameraMatrix, cv::Mat distCoeffs, targetInfo &strTargetInfo);

#endif