视觉伺服控制工具Visual Servoing Platform---VISP（6）----基于4个平面点的姿态估计

本教程重点介绍平面或非平面点的姿势估计。从它们在图像平面中的二维坐标以及在对象坐标系中指定的相应三维坐标，ViSP能够估计相机和对象坐标系之间的相对姿势。此姿势作为齐次矩阵cMo返回。请注意，要估计姿势，至少需要考虑四个点。

在这一节中，我们考虑四个平面点的情况。通过我们的blob跟踪器进行的图像处理，可以提取每个blob重心的像素坐标。然后使用相机的固有参数，我们在图像平面上转换这些像素坐标（以米为单位）。知道点的三维坐标后，我们就可以估计物体的姿态。

#include <visp/vpDisplayGDI.h>
#include <visp/vpDisplayX.h>
#include <visp/vpDot2.h>
#include <visp/vpImageIo.h>
#include <visp/vpPixelMeterConversion.h>
#include <visp/vpPose.h>
void computePose(std::vector<vpPoint> &point, const std::vector<vpDot2> &dot,
                 const vpCameraParameters &cam, bool init, vpHomogeneousMatrix &cMo);
void computePose(std::vector<vpPoint> &point, const std::vector<vpDot2> &dot,
                 const vpCameraParameters &cam, bool init, vpHomogeneousMatrix &cMo)
{
  vpPose pose;     double x=0, y=0;
  for (unsigned int i=0; i < point.size(); i ++) {
    vpPixelMeterConversion::convertPoint(cam, dot[i].getCog(), x, y);
    point[i].set_x(x);
    point[i].set_y(y);
    pose.addPoint(point[i]);
  }
  if (init == true) pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo);
  else              pose.computePose(vpPose::VIRTUAL_VS, cMo) ;
}
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);
#endif
    vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);
    std::vector<vpDot2> dot(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(4);
    point[0].setWorldCoordinates(-0.06, -0.06, 0);
    point[1].setWorldCoordinates( 0.06, -0.06, 0);
    point[2].setWorldCoordinates( 0.06,  0.06, 0);
    point[3].setWorldCoordinates(-0.06,  0.06, 0);
    vpHomogeneousMatrix cMo;
    bool init = 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);
      }
      computePose(point, dot, cam, init, 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(vpException e) {
    std::cout << "Catch an exception: " << e << std::endl;
  }
}

首先，我们包含vpPixelMeterConversion类的标题，该类包含静态函数，能够将图像中以像素表示的点的坐标转换为图像平面中的米，这要归功于相机的固有参数。我们还包括vpPose类的头，该类能够从点估计姿势。

#include <visp/vpPixelMeterConversion.h>
#include <visp/vpPose.h>

在这里，我们介绍执行姿势估计的computePose（）函数。此函数使用两个向量作为输入。第一个是vpPoint，它包含点的三维坐标（以米为单位），而第二个是vpDot2，它包含blob重心的二维坐标（以像素为单位）。需要匹配点的3D和2D坐标。这意味着point[i] and dot[i]应该指向相同的物理点。其他输入包括对应于相机固有参数的cam和指示是否需要第一次初始化姿势的init。cMo参数包含从输入点估计的姿势结果。

void computePose(std::vector<vpPoint> &point, const std::vector<vpDot2> &dot,
                 const vpCameraParameters &cam, bool init, vpHomogeneousMatrix &cMo)
{
  vpPose pose;     double x=0, y=0;
  for (unsigned int i=0; i < point.size(); i ++) {
    vpPixelMeterConversion::convertPoint(cam, dot[i].getCog(), x, y);
    point[i].set_x(x);
    point[i].set_y(y);
    pose.addPoint(point[i]);
  }
  if (init == true) pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo);
  else              pose.computePose(vpPose::VIRTUAL_VS, cMo) ;
}

在computePose（）函数中，我们首先创建vpPose类的一个实例。然后，对于每个点，我们将其二维坐标（以像素为单位）作为blob的重心，使用vpPixelMeterConversion:：convertPoint（）将其转换为图像平面中以米为单位的二维坐标（x，y）。然后使用相应的二维坐标更新点实例，并将该点添加到vpPose类中。添加所有点后，我们可以估计姿势pose。如果init为true，我们将使用vpPose:：DEMENTHON线性方法估计第一个姿势pose。它生成一个姿势pose，用作vpPose:：VIRTUAL_VS（虚拟视觉伺服）非线性方法的初始化，该方法将以比线性方法更低的余数收敛到解决方案。如果init为false，则我们认为先前的姿态cMo接近于该解，因此它可以用作非线性视觉伺服最小化的初始化。
在main（）函数中，我们不考虑使用对象的图像序列，而是始终考虑相同的图像“square.pgm”。在这个图像中，我们考虑一个12cm×12厘米的正方形。角点由一个blob表示。它们的重心与拐角位置相对应。读取此灰度图像的原因是：

vpImage<unsigned char> I;
vpImageIo::read(I, "square.pgm");

在能够显示图像的窗口实例化之后，在估计姿势的结果结束时，我们设置相机参数。在这里，我们考虑下面的相机模型$a=(p_x，p_y，u_y，v_0)$，其中$p_x＝p_y＝800$ 是焦距和像素大小之间的比率，并且其中主点$(u_0，v_0)$位于图像中心位置。

vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);

然后使用四个vpDot2跟踪器的向量跟踪每个blob。为了避免人与人之间的交互，我们在每个blob中使用像素位置初始化跟踪器。

std::vector<vpDot2> dot(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));

我们还定义了12cm×12cm正方形的模型，通过设置在这个例子中的物体框架中的角的三维坐标在正方形的中间。我们认为这里的点位于平面$Z＝0$。

std::vector<vpPoint> point(4);
point[0].setWorldCoordinates(-0.06, -0.06, 0);
point[1].setWorldCoordinates( 0.06, -0.06, 0);
point[2].setWorldCoordinates( 0.06,  0.06, 0);
point[3].setWorldCoordinates(-0.06,  0.06, 0);

接下来，我们创建了一个包含估计姿势pose的齐次矩阵cMo。

vpHomogeneousMatrix cMo;

在无限循环中，在每次迭代中我们读取一个新图像（在本例中，图像保持不变），在窗口中显示其内容，并跟踪每个blob。

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);
}

然后我们调用前面介绍的姿势估计函数computePose（）。它使用对象坐标系中定义为我们模型的点的三维坐标，以及blob跟踪器获得的图像中相应的二维位置来估计姿势cMo。这种齐次变换给出了对象坐标系在摄影机坐标系中的位置。

computePose(point, dot, cam, init, cMo);

生成的姿势pose在窗口覆盖中显示为RGB帧。红色、绿色和蓝色分别代表x、y和z轴。每个轴的长度为0.05米。然后刷新所有图形以更新窗口内容。

vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none);
vpDisplay::flush(I);

在第一次迭代结束时，我们关闭init标志，该标志指示下一个姿势估计可以使用我们的非线性vpPose:：VIRTUAL_VS估计方法，将前一个姿势作为初始值。

if (init) init = false;

最后，我们通过用户在窗口中单击鼠标来中断无限循环。

if (vpDisplay::getClick(I, false)) break;

我们还引入了40毫秒的睡眠，以降低循环速度并放松CPU。

vpTime::wait(40);