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。
其中：

v