VISP库IBVS仿真

示例程序1–tutorial-ibvs-4pts.cpp

/*! \example tutorial-ibvs-4pts.cpp */
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/vs/vpServo.h> //实现控制律的头文件
#include <visp3/robot/vpSimulatorCamera.h> //包含六自由度的虚拟相机

int main()
{
  try {
    vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0); //相机目标位置
    vpHomogeneousMatrix cMo(0.15, -0.1, 1.,        //相机初始位置
                            vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));

    vpPoint point[4] ;                             //四个角点
    point[0].setWorldCoordinates(-0.1,-0.1, 0);
    point[1].setWorldCoordinates( 0.1,-0.1, 0);
    point[2].setWorldCoordinates( 0.1, 0.1, 0);
    point[3].setWorldCoordinates(-0.1, 0.1, 0);

    vpServo task ;
    task.setServo(vpServo::EYEINHAND_CAMERA);     //初始化任务，手眼模型
    task.setInteractionMatrixType(vpServo::CURRENT);//设着交互矩阵类型（current, desired, mean or user defined）
    task.setLambda(0.5);                          //收敛速度

    vpFeaturePoint p[4], pd[4] ;                  //初始化特征点
    for (unsigned int i = 0 ; i < 4 ; i++) {
      point[i].track(cdMo);                      //相机坐标系下 跟踪特征点，并计算二位图像平面内的投影
      vpFeatureBuilder::create(pd[i], point[i]); //根据point[i]和相机参数，创建一个特征点，
      point[i].track(cMo);
      vpFeatureBuilder::create(p[i], point[i]);
      task.addFeature(p[i], pd[i]);
    }

    vpHomogeneousMatrix wMc, wMo;   //定义相机、目标位置
    vpSimulatorCamera robot;
    robot.setSamplingTime(0.040);
    robot.getPosition(wMc);
    wMo = wMc * cMo;

    for (unsigned int iter=0; iter < 150; iter ++) {
      robot.getPosition(wMc);
      cMo = wMc.inverse() * wMo;
      for (unsigned int i = 0 ; i < 4 ; i++) {   //通过将3维点投影到图像平面，来更新特征点
        point[i].track(cMo);
        vpFeatureBuilder::create(p[i], point[i]);
      }
      vpColVector v = task.computeControlLaw();
      robot.setVelocity(vpRobot::CAMERA_FRAME, v);
    }

    task.kill();
  }
  catch(vpException &e) {
    std::cout << "Catch an exception: " << e << std::endl;
  }
}

http://visp-doc.inria.fr/doxygen/visp-daily/tutorial-ibvs.html

---

vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));

用两个齐次矩阵定义相机的目标位置与初始位置。前三个值表示平移分量，后三个表示旋转分量，组合成其次坐标表示，参考：http://blog.csdn.net/yjy728/article/details/78335758

---

vpPoint point[4] ;
point[0].setWorldCoordinates(-0.1,-0.1, 0);
point[1].setWorldCoordinates( 0.1,-0.1, 0);
point[2].setWorldCoordinates( 0.1, 0.1, 0);
point[3].setWorldCoordinates(-0.1, 0.1, 0);

然后定义边长20cm的正方形的四个角点；vpPoint这个数据类型定义如下。

   51 void
   52 vpPoint::init()
   53 {
   54   p.resize(3) ; p = 0 ; p[2] = 1 ;
   55   oP.resize(4) ; oP = 0 ; oP[3] = 1 ;
   56   cP.resize(4) ; cP = 0 ; cP[3] = 1 ;
   57 
   58   //default value Z (1 meters)
   59   set_Z(1) ;
   60 }

vpPoint这个类型里面包含了
1. p–图像平面p内的表示的特征坐标，与2维正规化坐标对应，单位m。
2. oP–目标坐标系表示的特征，也叫世界坐标系
3. cP–相机坐标系下的特征表示
setWorldCoordinates是设置了oP；

---

vpServo task ;
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT);
task.setLambda(0.5);

将任务初始化为手眼相机的视觉伺服，控制器计算出来的是相机坐标系下的速度值。交互矩阵有当前特征值计算出来，需要在每个循环内更新。常数 λ $\lambda$ 是收敛的速度，设为0.5。
其中：

vpServo::vpServoIteractionMatrixType	区别
CURRENT	使用当前特征点 s $s$计算出来的交互矩阵L̂ s$\hat{L}_{s}$
DESIRED	使用目标特征点 s⋆ $s^{\star}$计算出来的交互矩阵 L̂ s⋆ $\hat{L}_{s^{\star}}$
MEAN	使用 L̂ =(L̂ s+L̂ s⋆)/2 $\hat{L}=(\hat{L}_{s}+\hat{L}_{s^{\star}})/2$
USER-DEFINED	控制律中，使用用户自定义的交互矩阵

vpFeaturePoint p[4], pd[4] ;

定义图像平面内的视觉特征，这里使用点特征，p[i]代表当前点特征，pd[i]代表目标特征。

---

for (unsigned int i = 0 ; i < 4 ; i++) {
  point[i].track(cdMo);
  vpFeatureBuilder::create(pd[i], point[i]);
  point[i].track(cMo);
  vpFeatureBuilder::create(p[i], point[i]);
  task.addFeature(p[i], pd[i]);
}

track()函数是跟踪特征点，并计算二维图像平面内的投影，内部调用了虚函数project(cMo)；project()定义如下：

   75 void
   76 vpForwardProjection::project()
   77 {
   78   projection(cP, p) ;
   79 }
   80 
   95 void
   96 vpForwardProjection::project(const vpHomogeneousMatrix &cMo)
   97 {
   98   try{
   99     changeFrame(cMo) ;
  100     projection() ;
  101   }
  102   catch(...)
  103   {
  104     vpERROR_TRACE("Error caught") ;
  105     throw ;
  106   }
  107 }

从相机坐标系下的参数 (vpTracker::cP).计算图像平面内的特征参数(vpTracker::p) 。
先通过changeFrame(cMo)这个函数，根据给定目标坐标系下的参数(vpForwardProjection::oP) 和给定的齐次坐标矩阵(cMo) ，计算相机坐标系下的参数 (cP)，同时，更新相机坐标系下的参数 (vpTracker::cP)；
projection() 是一个虚函数，计算图像平面内的特征参数。定义如下

  471 void vpPoint::projection() {
  472   double d = 1/cP[2] ;
  473   p[0] = cP[0]*d ;
  474   p[1] = cP[1]*d ;
  475   p[2] = 1 ;
  476 }

所以point[i].track(cdMo);这一句实际上做了这么几步
1. 根据point[i]中的oP与cMo，计算出cP=cMo*oP；
2. 更新point[i]中的cP；
3. 根据cp计算p。

---

vpFeatureBuilder::create(pd[i], point[i]);

通过point[i]创建一个vpFeaturePoint 类型(表示二维视觉特征s，包含两个参数x，y)的pd[i];其中x，y来自point中的图像平面参数p；深度信息Z来自相机坐标系，即point中的cP；所以，至少要知道point中的p和cP才能正确地设置pd；

---

task.addFeature(p[i], pd[i]);

给任务添加一对特征点，分别为 s $s$与s⋆$s^{\star}$;还有一个可选的参数，默认选择所有的特征点都使用，但是当有很多特征点的时候可以选择使用哪一些。

---

vpHomogeneousMatrix wMc, wMo;

定义wMc：相机在世界坐标系下位置的齐次矩阵；wMo：目标在世界坐标系下的齐次矩阵

---

vpSimulatorCamera robot;
robot.setSamplingTime(0.040);

设置一个虚拟的飞行相机对象，采样时间设置为0.04s；但计算出一个速度时，利用这个采样时间计算出下一时刻相机的位置。

---

robot.getPosition(wMc);
wMo = wMc * cMo;

通过相机初始位置wMc和相机坐标系下的目标位置cMo，计算出目标在世界坐标系位置wMo；因为本任务中目标静止，所以wMo不变。

---

进入真正的视觉伺服环节

robot.getPosition(wMc);
cMo = wMc.inverse() * wMo;

cMo=wMc−1∗wMo $cMo=wMc^{-1}*wMo$计算新的相机坐标系下的目标位置，下为齐次矩阵求逆公式：

$${\begin{bmatrix}R&t\\0_{1\times3}&1\end{bmatrix}}^{-1}= \begin{bmatrix}R^T&-R^Tt\\0_{1\times3}&1\end{bmatrix}$$

---

vpColVector v = task.computeControlLaw();

最后，计算纠正速度 vc $v_c$，是一个6维向量，对应相机的平移和旋转速度；