ViSP学习笔记（二十三）：根据特征点进行位姿估计

开发环境：Ubuntu 18.04 LTS + ROS Melodic + ViSP 3.3.1
文章内容主要参考ViSP官方教学文档：https://visp-doc.inria.fr/doxygen/visp-daily/tutorial_mainpage.html

  本文主要介绍了如何使用ViSP实现根据特征点进行位姿估计，在视觉伺服过程中，已知目标物体上的特征点在空间坐标系下的三维坐标和在图像坐标系下的二维坐标，计算当前目标物体相对于相机坐标系的位姿是非常重要的步骤。本文主要参考了computer-vision中的 tutorial-pose-from-points-image.cpp 例程。首先要获取这个例程文件并编译它

svn export https://github.com/lagadic/visp.git/trunk/tutorial/computer-vision
cd computer-vision/
mkdir build
cd build 
cmake .. -DCMAKE_BUILD_TYPE=Release -DVISP_DIR=$VISP_WS/visp-build
make 

  执行例程，查看效果

./tutorial-pose-from-points-image

  下面介绍一下代码实现过程

#include 
#include 
#include 
#include 
#include "pose_helper.h"
int main()
{
  try {
    vpImage<unsigned char> I;
    vpImageIo::read(I, "square.pgm"); //读取图片文件
#if defined(VISP_HAVE_X11)
    vpDisplayX d(I);
#elif defined(VISP_HAVE_GDI)
    vpDisplayGDI d(I);
#elif defined(VISP_HAVE_OPENCV)
    vpDisplayOpenCV d(I);
#endif
    vpCameraParameters cam(840, 840, I.getWidth() / 2, I.getHeight() / 2);//设置相机参数
    std::vector<vpDot2> dot(4);//新建特征点跟踪器容器
    std::vector<vpImagePoint> ip(4);//新建二维图像坐标容器
    //初始化特征点的二维图像坐标，至少需要四个特征点
    dot[0].initTracking(I, vpImagePoint(193, 157));
    dot[1].initTracking(I, vpImagePoint(203, 366));
    dot[2].initTracking(I, vpImagePoint(313, 402));
    dot[3].initTracking(I, vpImagePoint(304, 133));
    std::vector<vpPoint> point;//新建三维空间坐标容器
    //初始化特征点的三维空间坐标，与二维坐标必须一一对应
    point.push_back(vpPoint(-0.06, -0.06, 0));
    point.push_back(vpPoint( 0.06, -0.06, 0));
    point.push_back(vpPoint( 0.06,  0.06, 0));
    point.push_back(vpPoint(-0.06,  0.06, 0));
    vpHomogeneousMatrix cMo;//新建相机坐标系和目标物体坐标系之间的单应性矩阵
    bool init = true;//初始化参数设为true
    while (1) {
      vpImageIo::read(I, "square.pgm");
      vpDisplay::display(I);
      for (unsigned int i = 0; i < dot.size(); i++) {
        dot[i].setGraphics(true);
        dot[i].track(I);//从图像中重新获取特征点坐标
        ip[i] = dot[i].getCog();//对二维图像坐标容器中的坐标值进行更新
      }
      computePose(point, ip, cam, init, cMo);//根据二维坐标和三维坐标以及相机参数进行位姿估计
      //point-输入参数，表示特征点的三维空间坐标
      //ip-输入参数，表示特征点的二维图像坐标
      //cam-输入参数，表示相机的内部参数
      //init-输入参数，true表示需要进行初始化，将使用无需初始化的线性位姿估计方法进行计算，
      //并将结果作为初始化值用于非线性位姿估计方式的初始化使用；
      //false表示无需初始化，直接使用上一帧图像位姿估计结果进行计算
      //cMo-输入/输出参数，作为输入参数可以输入上一帧图像的位姿估计结果，
      //作为输出参数可以输出当前帧的位姿估计结果
      vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none);
      vpDisplay::flush(I);
      if (init)
        init = false; // turn off pose initialisation
      if (vpDisplay::getClick(I, false))
        break;
      vpTime::wait(40);
    }
  } catch (const vpException &e) {
    std::cout << "Catch an exception: " << e.getMessage() << std::endl;
  }
}

如果大家对于深度学习与计算机视觉领域感兴趣，希望获得更多的知识分享与最新的论文解读，欢迎关注我的个人公众号“深视”。