视觉伺服控制工具Visual Servoing Platform---VISP（7）----vpServo这个看懂了就会用VISP了，很简单
看好了，基本上我们实现真实的机械臂控制的所有方法需要用到的类都可以在这个博客中找到。
这个类是整个VISP的核心部分了，因为其他的部分可以用现有的所有算法替换，但是这个vpServo和另一个姿态估计类是真的好用，是无法替换的。
在vpServo中有好几个方法：两个大的方向是eye in hand和eye to hand。基于这两个大类，开发人员开发了另外一些方法
- NONE
- EYEINHAND_CAMERA
- EYEINHAND_L_cVe_eJe
- EYETOHAND_L_cVe_eJe
- EYETOHAND_L_cVf_fVe_eJe
- EYETOHAND_L_cVf_fJe
具体每个方法的意义可以参考这个连接，简洁明了
一. vpServo的调用方法
首先实例化这个类
vpServo task;
然后具体设置这个类中的每个方法
task.setServo(vpServo::EYEINHAND_CAMERA);//设置为eye in hand的视觉伺服模式
task.setInteractionMatrixType(vpServo::CURRENT);//交互矩阵的计算方法是根据当前图像进行计算
task.setLambda(0.5);//增益参数lambda设为0.5
关于 task.setServo(）这个方法，我们还有很多种调用模式，例如：基于图像的视觉伺服也分为eye in hand和eye to hand.也就是摄像头在机械臂上和摄像头固定在空间中。
task.setServo(vpServo::EYEINHAND_CAMERA);//设置为eye in hand的视觉伺服模式
task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
//具体每一种方法的作用可以参考上面的链接
task.setServo(vpServo::EYETOHAND_L_cVe_eJe);
task.setServo(vpServo::EYETOHAND_L_cVf_fVe_eJe);
task.setServo(vpServo::EYETOHAND_L_cVf_fJe);
关于vpServo::vpServoIteractionMatrixType()这个方法也有很多调用方法
这个很关键阿，就是计算雅可比矩阵的，新手一般都不知道怎么搞雅可比矩阵，看了这个就知道了
task.setInteractionMatrixType(vpServo::CURRENT);//交互矩阵的计算方法是根据当前图像进行计算
task.setInteractionMatrixType(vpServo::DESIRED);
task.setInteractionMatrixType(vpServo::MEAN);
task.setInteractionMatrixType(vpServo::USER_DEFINED);
一些相关的方法
void vpServo::addFeature(vpBasicFeature & s_cur,vpBasicFeature & s_star,const unsigned int select = vpBasicFeature::FEATURE_ALL )
在任务中添加一组新特征s\bf ss和s∗{\bf s}^*s∗。
参数
s_cur：表示为s\bf ss的当前视觉特征。
s_star：表示为s∗{\bf s}^*s∗的所需视觉特征。
select：特征选择器。默认情况下，使用s和s_star中的所有特征，但可以指定在多个特征的情况下使用哪一个。
以下示例代码解释了如何使用此方法添加视觉要素点（x，y）（x，y）（x，y）：
vpFeaturePoint s, s_star;
...
vpServo task;
task.addFeature(s, s_star);
例如，要仅使用xxx视觉特征，前面的代码将变为：
vpFeaturePoint s, s_star;
...
vpServo task;
task.addFeature(s, s_star, vpFeaturePoint::selectX());
void vpServo::addFeature(vpBasicFeature & s_cur,const unsigned int select = vpBasicFeature::FEATURE_ALL )
在任务中添加新功能s\bf ss。表示为s∗{\bf s}^*s∗的所需视觉功能等于零。
参数
s_cur：表示为s\bf ss的当前视觉特征。
select：特征选择器。默认情况下，将使用s中的所有特征，但可以指定在多个特征的情况下使用哪个特征。
以下示例代码解释了如何使用此方法添加θu=(θux,θuy,θuz)\theta {\bf u} =(\theta u_x, \theta u_y, \theta u_z)θu=(θux,θuy,θuz) 特征：
vpFeatureThetaU s(vpFeatureThetaU::cRcd);
...
vpServo task;
task.addFeature(s);
例如，要仅使用θux\theta u_xθux特征，前面的代码变为：
vpFeatureThetaU s(vpFeatureThetaU::cRcd);
...
vpServo task;
task.addFeature(s, vpFeatureThetaU::selectTUx);
计算控制率是该类的一个主要方法
vpColVector vpServo::computeControlLaw()
使用setServo()计算指定的控制律。
控制法的一般形式如下：
其中：
q˙{\bf\dot q}q˙是应用于机器人的结果速度命令。控制律的符号取决于eye in hand or eye to hand的配置。
J\bf JJ是任务的雅可比矩阵。它是交互矩阵和机器人雅可比矩阵的函数。
e=（s−s∗）\bf e=（s-s^*）e=（s−s∗）是要调节的误差。
为确保连续排序，可使用computeControlLaw（双精度）功能。它将确保计算的速度是连续的。
vpColVector vpServo::computeControlLaw(double t)
使用setServo（）计算指定的控制律。
对于computeControlLaw（）中给出的控制律的一般形式，我们在这里添加了一个附加项。这个附加项允许通过避免命令中的突然变化来计算连续速度。
此处考虑的控制法形式如下：
其中：
q˙{\bf\dot q}q˙是应用于机器人的连续速度命令。控制律的符号取决于手对眼或手对眼的配置。
J\bf JJ是任务的雅可比矩阵。它是交互矩阵和机器人雅可比矩阵的函数。
e=（s−s∗）\bf e=（s-s^*）e=（s−s∗）是要调节的误差。
ttt是作为此方法参数给定的时间。
μ\muμ是默认设置为4的增益，可以使用setMu（）进行修改。
J^e(0)+e(0){\bf \widehat J}_{e(0)}^+ {\bf e}(0)Je(0)+e(0) 是t=0t=0t=0时 J^e+e{\bf \widehat J}_e^+ {\bf e}Je+e 的值。此值在第一次调用此方法时，或当t参数设置为0时，在内部存储。
参数
t:时间是秒。设置为零时， J^e(0)+e(0){{\bf \widehat J}_{e(0)}}^+ {{\bf e}(0)}Je(0)+e(0)在内部刷新。
vpColVector vpServo::computeControlLaw(double t,const vpColVector & e_dot_init)
参数
t：时间是秒。设置为零时，J^e(0)+e(0){{\bf \widehat J}_{e(0)}}^+ {{\bf e}(0)}Je(0)+e(0)在内部刷新。
e_dot_init：e˙(0){\bf \dot e}(0)e˙(0)的初始值。.
二. 一个基于位置的视觉伺服的例子
为了避免潜在的内存泄漏，可以显式调用kill（）函数来销毁任务。否则，析构函数~vpServo（）将启动异常vpServoException:：notkilled。
下面的示例显示了如何从3D视觉特征s=(c∗tc,θu)s=(_{}^{c^*}\textrm{t}_{c}, \theta u)s=(c∗tc,θu)构建基于位置的视觉伺服。在这种情况下，我们有s∗=0s^*=0s∗=0。
让我们用θu\theta uθu表示旋转分量c∗Rc_{}^{c^*}\textrm{R}_{c}c∗Rc的角度/轴参数化。此外，c∗tc_{}^{c^*}\textrm{t}_{c}c∗tc和c∗Rc_{}^{c^*}\textrm{R}_{c}c∗Rc分别表示期望相机坐标系和通过姿势估计(参考这个连接)获得的当前坐标系之间的平移和旋转（请参见vpPose类）。
#include <visp/vpColVector.h>
#include <visp/vpFeatureThetaU.h>
#include <visp/vpFeatureTranslation.h>
#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpMatrix.h>
#include <visp/vpServo.h>
int main()
{// 创建齐次矩阵，该矩阵表示相机从期望相机坐标系和当前相机坐标系移动期望的位移 vpHomogeneousMatrix cdMc;// ... 这里的cdMc是姿势估计的结果// 创建当前视觉特征s=（c*\u t\u c，ThetaU）vpFeatureTranslation s_t(vpFeatureTranslation::cdMc);vpFeatureThetaU s_tu(vpFeatureThetaU::cdRc);// Set the initial values of the current visual feature s = (c*_t_c, ThetaU)s_t.buildFrom(cdMc);s_tu.buildFrom(cdMc);// 设置当前视觉特征的初始值s=（c*\u t\u c，ThetaU）vpFeatureTranslation s_star_t(vpFeatureTranslation::cdMc); // 默认初始化为零 vpFeatureThetaU s_star_tu(vpFeatureThetaU::cdRc);// 默认初始化为零 vpColVector v; // Camera velocity相机速度double error; // Task error 人物误差//创建一个视觉伺服任务，vpServo task;//实例化视觉任务为task// Visual servo task initialization初始化视觉伺服任务// -摄像机安装在机器人末端执行器上，并在摄像机坐标系中计算速度task.setServo(vpServo::EYEINHAND_CAMERA); // - 交互矩阵由当前视觉特征s计算得出，当然你也可以设置为其他的方法，下面有讲task.setInteractionMatrixType(vpServo::CURRENT); // - 设置增益为1task.setLambda(1);// - 添加当前和期望的平移特征task.addFeature(s_t, s_star_t); // - 添加当前和期望的旋转特征task.addFeature(s_tu, s_star_tu); // 视觉伺服回路。这里的目标是更新视觉特征s=（c*\u t\u c，ThetaU），计算控制律并将其应用于机器人do {// ... 这里的cdMc是姿势估计的结果// 更新当前视觉特征ss_t.buildFrom(cdMc); // 更新平移视觉特征s_tu.buildFrom(cdMc); // 更新旋转视觉特征v = task.computeControlLaw(); // Compute camera velocity skew计算相机速度螺杆error = ( task.getError() ).sumSquare(); // error = s^2 - s_star^2} while (error > 0.0001); // 当当前和期望的视觉特征关闭时停止任务// A call to kill() is requested here to destroy properly the current// and desired feature lists.task.kill();
}
三. 一个基于图像的视觉伺服的例子
上面的例子不是一个完整的例子，这个例子是比较真实的例子。在下一章，我们还会更新一个控制实际机械比的例子。总是，看懂代码才行。
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/robot/vpImageSimulator.h>
#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
void display_trajectory(const vpImage<unsigned char> &I, const std::vector<vpDot2> &dot);
class vpVirtualGrabber//用于计算在不同位置处目标物体在相机坐标系下的成像，
//具体过程可以参见博客：[ViSP学习笔记（七）：平面图像投影](https://blog.csdn.net/qq_36104364/article/details/113881985)
{public:vpVirtualGrabber(const std::string &filename, const vpCameraParameters &cam) : sim_(), target_(), cam_(){// 目标是一个20厘米乘20厘米的正方形 // 初始化目标角点的三维坐标for (int i = 0; i < 4; i++)X_[i].resize(3);// Top left Top right Bottom right Bottom leftX_[0][0] = -0.1;X_[1][0] = 0.1;X_[2][0] = 0.1;X_[3][0] = -0.1;X_[0][1] = -0.1;X_[1][1] = -0.1;X_[2][1] = 0.1;X_[3][1] = 0.1;X_[0][2] = 0;X_[1][2] = 0;X_[2][2] = 0;X_[3][2] = 0;vpImageIo::read(target_, filename);// 初始化相机模拟器cam_ = cam;sim_.setInterpolationType(vpImageSimulator::BILINEAR_INTERPOLATION);sim_.init(target_, X_);}void acquire(vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo){sim_.setCleanPreviousImage(true);sim_.setCameraPosition(cMo);sim_.getImage(I, cam_);}
private:vpColVector X_[4]; //目标角点的三维坐标vpImageSimulator sim_; //vpImage<unsigned char> target_; // image of the targetvpCameraParameters cam_;
};
//用于显示目标物体的运动轨迹
void display_trajectory(const vpImage<unsigned char> &I, const std::vector<vpDot2> &dot)
{static std::vector<vpImagePoint> traj[4];for (unsigned int i = 0; i < 4; i++) {traj[i].push_back(dot[i].getCog());}for (unsigned int i = 0; i < 4; i++) {for (unsigned int j = 1; j < traj[i].size(); j++) {vpDisplay::displayLine(I, traj[i][j - 1], traj[i][j], vpColor::green);}}
}
int main()
{#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)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));//相机的初始位置和物体之间的位置关系vpImage<unsigned char> I(480, 640, 255);vpCameraParameters cam(840, 840, I.getWidth() / 2, I.getHeight() / 2);//设置相机内部参数std::vector<vpPoint> point;//目标物体特征点的空间坐标容器point.push_back(vpPoint(-0.1, -0.1, 0));point.push_back(vpPoint(0.1, -0.1, 0));point.push_back(vpPoint(0.1, 0.1, 0));point.push_back(vpPoint(-0.1, 0.1, 0));vpServo task;task.setServo(vpServo::EYEINHAND_CAMERA);//设置为eye in hand的视觉伺服模式task.setInteractionMatrixType(vpServo::CURRENT);//交互矩阵的计算方法是根据当前图像进行计算task.setLambda(0.5);//增益参数lambda设为0.5vpVirtualGrabber g("./target_square.pgm", cam);//获取目标图像g.acquire(I, cMo);//根据相机当前位置将物体的成像保存到I中
#if defined(VISP_HAVE_X11)vpDisplayX d(I, 0, 0, "Current camera view");
#elif defined(VISP_HAVE_GDI)vpDisplayGDI d(I, 0, 0, "Current camera view");
#elif defined(VISP_HAVE_OPENCV)vpDisplayOpenCV d(I, 0, 0, "Current camera view");
#elsestd::cout << "No image viewer is available..." << std::endl;
#endifvpDisplay::display(I);vpDisplay::displayText(I, 10, 10, "Click in the 4 dots to initialise the tracking and start the servo",vpColor::red);vpDisplay::flush(I);//此函数是特定的，必须调用以显示覆盖。因为这是在浪费时间，所以要尽量少用它。vpFeaturePoint p[4], pd[4];//新建特征点容器，p表示当前位置，pd表示期望位置std::vector<vpDot2> dot(4);for (unsigned int i = 0; i < 4; i++) {point[i].track(cdMo);//将目标特征点在期望位置处的空间坐标投影到相机坐标系中vpFeatureBuilder::create(pd[i], point[i]);//生成期望位置处的特征dot[i].setGraphics(true);dot[i].initTracking(I);//根据鼠标点击获取连通区域blob（四个圆点）作为跟踪的目标vpDisplay::flush(I);vpFeatureBuilder::create(p[i], cam, dot[i].getCog());//计算连通区域blob的重心作为当前位置处的特征task.addFeature(p[i], pd[i]);//把期望位置特征和当前位置特征添加到视觉伺服任务中}vpHomogeneousMatrix wMc, wMo;//新建相机和目标物体在世界坐标系下的坐标容器vpSimulatorCamera robot;//新建仿真相机robot.setSamplingTime(0.040);//设置采样时间robot.getPosition(wMc);//获取相机初始位置wMo = wMc * cMo;//根据相机初始位置和目标物体在相机坐标系下的位置计算目标物体在世界坐标系下的坐标for (;;) {robot.getPosition(wMc);//更新相机当前位置cMo = wMc.inverse() * wMo;//更新目标物体在相机坐标系下的坐标g.acquire(I, cMo);//更新图像vpDisplay::display(I);for (unsigned int i = 0; i < 4; i++) {dot[i].track(I);vpFeatureBuilder::create(p[i], cam, dot[i].getCog());vpColVector cP;point[i].changeFrame(cMo, cP);//根据当前目标物体在相机坐标系下的坐标和初始位置坐标计算当前目标物体在世界坐标系下的坐标p[i].set_Z(cP[2]);//获取深度距离信息用于视觉伺服计算}vpColVector v = task.computeControlLaw();//视觉伺服控制律计算display_trajectory(I, dot);//显示运动轨迹vpServoDisplay::display(task, cam, I, vpColor::green, vpColor::red);robot.setVelocity(vpRobot::CAMERA_FRAME, v);//设置相机运动速度vpDisplay::flush(I);if (vpDisplay::getClick(I, false))break;vpTime::wait(robot.getSamplingTime() * 1000);}} catch (const vpException &e) {std::cout << "Catch an exception: " << e << std::endl;}
#endif
}
-四. 关于一些容器类的实例化方法
关于第四节中的一些“容器”，例如：vpFeatureTranslation ，vpFeaturePoint3D，pFeatureThetaU，vpImagePoint，vpPose等。这些其实都是VISP中的都是一个类，调用方法都是实例化一个类，例如：
我建议详细了解C++的类Class的实现和使用之后再来学习这些东西之后就很容易看懂VISP的每个方法的调用了。具体可以看这个博客。
首先创建一个类，如下：
class Coordinate
{public:int m_iX;int m_iY;
};
下面就是实例化一个对象。
void main()
{Coordinate coord[3]; //实例化一个容器，可以包含三个坐标coord[1].m_iX=10;//定义第2个坐标的x轴的直Coordinate *p=new Coordinate[3]; //堆中p[0].m_iX=10;p->m_iX=10;delete []p;p=NULL;
}
四. 需要具体知道的类
一些还需要了解的类
下面四个类是关于视觉特征的类，可以直接参考
关于查看VISP的所有类（方法）的的调用方法和作用，可以参考这个链接。
4.1 vpFeatureTranslation
Class that defines the translation visual feature s=(tx,ty,tz)s=(t_x,t_y,t_z)s=(tx,ty,tz).
该类定义平移视觉特征s=（tx，ty，tz）s=（t_x，t_y，t_z）s=（tx，ty，tz）。
为了方便，这里考虑两个坐标系，F1F_{1}F1和F2F_{2}F2。
设F2MF1^{F_{2}}M_{F_{1}}F2MF1为齐次矩阵，给出坐标系F1F_{1}F1相对于坐标系F2F_{2}F2的方向和平移。
与 F2MF1^{F_{2}}M_{F_{1}}F2MF1 的旋转矩阵，该矩阵给出坐标系的方向 F1F_{1}F1 相对于坐标系F2F_{2}F2和F2MF1^{F_{2}}M_{F_{1}}F2MF1的平移向量，该向量给出坐标系F1F_{1}F1 相对于坐标系 F2F_{2}F2的位置。
这个类可用于操纵三种视觉特征：
#include <visp/vpFeatureTranslation.h>
#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpServo.h>
int main()
{vpServo task; // Visual servoing taskvpHomogeneousMatrix cdMc;// ... 这里cdMc需要从例如姿势估计初始化。// 创建当前视觉特征svpFeatureTranslation s(vpFeatureTranslation::cdMc);s.buildFrom(cdMc); // 初始化当前的特征s=（tx，ty，tz）// Set eye-in-hand control law. // 计算出的速度将在相机坐标系中表示task.setServo(vpServo::EYEINHAND_CAMERA);// 交互矩阵由当前视觉特征s计算得出task.setInteractionMatrixType(vpServo::CURRENT); // Set the constant gaindouble lambda = 0.8;task.setLambda(lambda); // 将三维平移特征添加到任务中task.addFeature(s); // s* is here considered as zero// Control loopfor ( ; ; ) {// ... 这里cdMc需要从例如姿势估计初始化。 // 更新当前三维平移视觉特征s.buildFrom(cdMc);// compute the control law 计算控制律vpColVector v = task.computeControlLaw(); // camera velocity摄像机速度}
}
下面来解释以下这些代码
如果只想处理3D转换中的(tx，ty)(t_x，t_y)(tx，ty)子集(平移)功能，只需通过以下行修改前面示例中的addFeature()调用。在这种情况下，sss的维度是2。
// Add the (tx,ty) subset features from 3D translation to the task
task.addFeature(s, vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy());
如果您想要构建自己的控制律，则另一个示例显示如何创建当前（sss）和所需（s∗s^*s∗）三维平移视觉特征，计算相应的误差向量(s−s∗)(s-s^*)(s−s∗)，最后构建交互矩阵LsL_{s}Ls。
#include <visp/vpFeatureTranslation.h>
#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpMatrix.h>
int main()
{vpHomogeneousMatrix cdMc;// ... cdMc need here to be initialized from for example a pose estimation.// Creation of the current feature svpFeatureTranslation s(vpFeatureTranslation::cdMc);s.buildFrom(cdMc); // Initialization of the feature// 创建所需的特征s*。默认情况下，此特征初始化为零vpFeatureTranslation s_star(vpFeatureTranslation::cdMc); // 计算平移特征的交互矩阵vpMatrix L = s.interaction();//计算平移特征的误差向量（s-s*）vpColVector e = s.error(s_star); // e = (s-s*)
}
下面的代码显示了如何使用3D平移特征(tx，ty，tz)(t_x，t_y，t_z)(tx，ty，tz)创建eye-in hand视觉伺服任务，该特征对应于当前相机坐标系和对象坐标系之间的3D平移。与前面的示例一样，要控制六个自由度，必须将至少三个其他特征视为vpFeatureThetaU 视觉特征。初始化视觉特征的方法与以前完全相同。不同之处在于cMo方法必须精确，期望特征必须等于零。
#include <visp/vpFeatureTranslation.h>
#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpServo.h>
int main()
{vpServo task; // 实例化一个视觉伺服任务vpHomogeneousMatrix cdMo;// ... cdMo在这里需要从例如姿势估计初始化。// 创建所需的视觉特征s*vpFeatureTranslation s_star(vpFeatureTranslation::cMo);//实例化这个方法，期望的视觉特征s_star.buildFrom(cdMo); // 初始化期望的视觉特征 s*=(tx*,ty*,tz*)vpHomogeneousMatrix cMo;// ... 正常的算法里，在代码中的这部分，这里需要计算cMo。// 创建当前视觉特征 svpFeatureTranslation s(vpFeatureTranslation::cMo);//实例化这个方法，当前的视觉特征s.buildFrom(cMo); // 创建当前视觉特征 s=(tx,ty,tz)// Set eye-in-hand control law. // 计算出的速度将在相机坐标系中表示task.setServo(vpServo::EYEINHAND_CAMERA);// 交互矩阵(雅可比矩阵)由当前视觉特征s计算得出task.setInteractionMatrixType(vpServo::CURRENT); // Set the constant gaindouble lambda = 0.8;task.setLambda(lambda); // 将三维平移特征添加到任务中 Add the 3D translation feature to the tasktask.addFeature(s, s_star); // s* 一般考虑为0// Control loopfor ( ; ; ) {// ... 这里需要根据例如姿势估计来计算cMo.//..这里需要添加实际的代码// 更新当前三维平移视觉特征s.buildFrom(cMo);// compute the control lawvpColVector v = task.computeControlLaw(); // camera velocity}
}
一种实现的三维平移特征。
- cdMc：用于操纵视觉特征s=c∗tcs=^{c^*}t_cs=c∗tc的选择器，该视觉特征给出当前相机坐标系相对于期望相机坐标系的位置。
- cMcd：用于操纵视觉功能s=ctc∗s=^{c}t{c^*}s=ctc∗的选择器，该选择器提供期望相机坐标系相对于当前相机坐标系的位置。
- cMo：用于操纵视觉特征s=ctos=^{c}t_os=cto的选择器，该特征给出对象坐标系相对于当前相机坐标系的位置。
解释一些类
vpFeatureTranslation::vpFeatureTranslation(vpFeatureTranslationRepresentationType r)
这个类就是构建视觉特征，并将其初始化为零的默认构造函数。
参数
r：考虑的三维平移特征的类型。
一般来讲，我们实例化一个视觉特征类时，需要这样
vpFeatureTranslation s_t(vpFeatureTranslation::cdMc);
vpFeatureTranslation::vpFeatureTranslation(vpHomogeneousMatrix & f2Mf1_,vpFeatureTranslationRepresentationType r)
从表示两个坐标系F1{{F}}_1F1和F2{{F}}_2F2之间的3D转换的齐次矩阵F2MF1^{{{F}}_2}M_{{{F}}_1}F2MF1 构建3D视觉特征的构造函数。
参数
f2Mf1_u[in]：相机从坐标系F2{{F}}_2F2移动到坐标系F1{{F}}_1F1(F2MF1^{{{F}}_2}M_{{{F}}_1}F2MF1)时必须实现的三维位移。
r：特征类型。它可以是vpFeature::cdMc或vpFeature::cMo。
void vpFeatureTranslation::buildFrom (const vpHomogeneousMatrix & f2Mf1_ )
从表示两个帧F1{{F}}_1F1 和F2{{F}}_2F2之间的3D转换的齐次矩阵F2MF1^{{{F}}_2}M_{{{F}}_1}F2MF1 构建3D转换视觉特征。
参数
f2Mf1_u[in]：相机从坐标系F2F_2F2移动到坐标系 F1F_1F1 (F2MF1^{{{F}}_2}M_{{{F}}_1}F2MF1)时必须实现的三维位移。
4.2 vpFeatureThetaU and vpRxyzVector
此类从表示坐标系之间旋转的θu\theta uθu轴/角度参数化定义三维视觉特征sss。
我们定义 θu=(θux,θuy,θuz)\theta u = (\theta u_{x}, \theta u_{y}, \theta u_{z})θu=(θux,θuy,θuz) .
为了便于表示，只考虑两个坐标系之间的转换：当前相机坐标系FcF_{c}Fc和期望的相机坐标系Fc∗F_{c^*}Fc∗。
设c∗Rc^{c^*}R_{c}c∗Rc为旋转矩阵，给出当前相机坐标系相对于期望相机坐标系的方向。让θuc∗Rc\theta u_{{{c^*}R_{c}}}θuc∗Rc对应于此旋转的轴/角度表示。
此外，让cRc∗^{c}R{c^*}cRc∗旋转矩阵给出期望相机坐标系相对于当前相机坐标系的方向。设θucRc\theta u_{{{c}R_{c}}}θucRc为该旋转的对应轴/角度表示。
θuc∗Rc\theta u_{{{c^*}R_{c}}}θuc∗Rc，如果必须考虑当前相机坐标系相对于期望相机坐标系的方向。期望的视觉特征s∗s^*s∗等于零。相应的误差不等于e=(s−s∗)=θuc∗Rce=(s-s^*)=\theta u_{{{c^*}R_{c}}}e=(s−s∗)=θuc∗Rc。在这种情况下，与sss相关的交互矩阵如下所示：
with
其中，030_303是3∗33*33∗3的nul矩阵，I3I_3I3是3∗33*33∗3单位矩阵，为了更易于阅读，θ\thetaθ和uuu分别是θuc∗Rc\theta u_{{{c^*}R_{c}}}θuc∗Rc表示的角度和轴坐标。
下面的代码显示了如何使用3Dθu\theta uθu特征(θux,θuy,θuz)(\theta u_x, \theta u_y, \theta u_z)(θux,θuy,θuz)创建eye in hand视觉伺服任务，该特征对应于当前相机坐标系和期望相机坐标系之间的3D旋转。要控制六个自由度，必须考虑至少三个其他特征，如vpFeatureTranslation视觉特征。首先，我们创建一个当前（sss）3Dθu\theta uθu特征，然后将任务设置为使用与当前特征LsL_sLs关联的交互矩阵，然后计算相机速度v=−λLs+(s−s∗)v=-\lambda \; L_s^+ \; (s-s^*)v=−λLs+(s−s∗)。当前特征sss在while()循环中更新，而s∗s^*s∗被视为零。
#include <visp/vpFeatureThetaU.h>
#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpServo.h>
int main()
{vpServo task; // Visual servoing taskvpHomogeneousMatrix cMcd;//d表示desire期望的，c表示camera相机// ... cMcd need here to be initialized from for example a pose estimation.// 创建与当前相机坐标系和期望相机坐标系之间的角度/轴参数化旋转相对应的当前特征vpFeatureThetaU s(vpFeatureThetaU::cRcd);s.buildFrom(cMcd); // 初始化视觉特征// Set eye-in-hand control law. // 计算出的速度将在相机坐标系中表示task.setServo(vpServo::EYEINHAND_CAMERA);// 交互矩阵由当前视觉特征s计算得出task.setInteractionMatrixType(vpServo::CURRENT); // 将3D ThetaU 特征添加到任务中 task.addFeature(s); // s* is here considered as zero// Control loopfor ( ; ; ) {// ... cMcd need here to be initialized from for example a pose estimation.// Update the current ThetaU visual features.buildFrom(cMcd);// compute the control lawvpColVector v = task.computeControlLaw(); // camera velocity}
}
如果只想处理3Dθu\theta uθu中的 (θux,θuy)(\theta u_x,\theta u_y)(θux,θuy) 子集功能，只需通过以下行修改前面示例中的addFeature()调用。在这种情况下，sss的维度是2。
// Add the (ThetaU_x, ThetaU_y) subset features from the 3D ThetaU
// rotation to the task
task.addFeature(s, vpFeatureThetaU::selectTUx() | vpFeatureThetaU::selectTUy());
继续推断以下，如果你只想控制一个自由度的旋转，那么…
// Add the (ThetaU_x, ThetaU_y) subset features from the 3D ThetaU
// rotation to the task
task.addFeature(s, vpFeatureThetaU::selectTUx());
如果您想构建自己的控制律，这个示例演示如何创建当前（sss）和期望（s∗s^*s∗）3Dθu\theta uθu视觉特征，计算相应的误差向量（s−s∗）（s-s^*）（s−s∗），最后构建交互矩阵LsL_sLs。
#include <visp/vpFeatureThetaU.h>
#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpMatrix.h>
int main()
{vpHomogeneousMatrix cdMc;//d表示desire期望的，c表示camera相机// ... cdMc need here to be initialized from for example a pose estimation.// Creation of the current feature svpFeatureThetaU s(vpFeatureThetaU::cdRc);//R表示旋转，d表示desire期望的，c表示camera相机s.buildFrom(cdMc); // Initialization of the feature// Creation of the desired feature s*. By default this feature is // initialized to zerovpFeatureThetaU s_star(vpFeatureThetaU::cdRc); // Compute the interaction matrix L_s for the current ThetaU featurevpMatrix L = s.interaction();// Compute the error vector (s-s*) for the ThetaU features.error(s_star);
}
- cdRc：用于操纵可视特征 s=θuc∗Rcs = \theta u_{c^*R_{c}}s=θuc∗Rc的选择器。此视觉特征表示当前相机坐标系相对于期望相机坐标系的方向。
- cRcd：用于操纵可视特征 s=θucRc∗s = \theta u_{{c}R_{c^*}}s=θucRc∗的选择器。此视觉特征表示期望相机坐标系相对于当前相机坐标系的方向。
- TUx：从θu\theta uθu角度/轴表示中选择子集θux\theta u_xθux视觉特征。
- TUy：从θu\theta uθu角度/轴表示中选择子集θuy\theta u_yθuy视觉特征。
TUz：从θu\theta uθu角度/轴表示中选择子集θuz\theta u_zθuz视觉特征。
下面介绍vpRxyzVector
具体介绍参考在源码网站
考虑使用X-Y-Z约定的Euler(φ,θ,ψ)(\varphi,\theta,\psi)(φ,θ,ψ) 角度的类，其中(φ,θ,ψ)(\varphi,\theta,\psi)(φ,θ,ψ) 分别是xxx，yyy和zzz轴附近的旋转角。
对应于x-y-z约定的旋转矩阵由下式给出：
下面的代码首先显示如何初始化此Euler角度表示，而不是如何从vpRxyzVector构造旋转矩阵，最后显示如何从构建旋转矩阵提取vpRxyzVector Euler角度。
#include <iostream>
#include <visp/vpMath.h>
#include <visp/vpRotationMatrix.h>
#include <visp/vpRxyzVector.h>
int main()
{vpRxyzVector rxyz;// Initialise the Euler anglesrxyz[0] = vpMath::rad( 45.f); // phi angle in rad around x axis rxyz[1] = vpMath::rad(-30.f); // theta angle in rad around y axisrxyz[2] = vpMath::rad( 90.f); // psi angle in rad around z axis// 从欧拉角构造旋转矩阵vpRotationMatrix R(rxyz);// 从旋转矩阵中提取围绕x、y、z轴的Euler角度rxyz.buildFrom(R);// 打印提取的Euler角度。值与用于初始化的值相同 std::cout << rxyz; // 因为旋转向量是3个值的列向量，所以转置操作生成一个行向量。vpRowVector rxyz_t = rxyz.t();// Print the transpose row vectorstd::cout << rxyz_t << std::endl;
}
下面是关于这个类内的一些方法
vpRxyzVector::vpRxyzVector()
将所有角度初始化为零的默认构造函数。
vpRxyzVector::vpRxyzVector(const vpRxyzVector & rxyz)
复制另一个vpRxyzVector的构造函数
vpRxyzVector::vpRxyzVector(const double phi,const double theta,const double psi )
从3个角度构造（弧度）。
参数
phi : φ\varphiφ angle around the xxx axis.
theta : θ\thetaθ angle around the yyy axis.
psi : ψ\psiψ angle around the zzz axis.
vpRxyzVector::vpRxyzVector ( const vpRotationMatrix & R )
从旋转矩阵初始化Rxyz=（φ，θ，ψ）R_{xyz}=（\varphi，\theta，\psi）Rxyz=（φ，θ，ψ）Euler角度的构造函数。
参数
R：用于初始化Euler角度的旋转矩阵。
vpRxyzVector::vpRxyzVector(const vpThetaUVector & tu)
初始化Rxyz=（φ，θ，ψ）R_{xyz}=（\varphi，\theta，\psi）Rxyz=（φ，θ，ψ）Euler角度向量的构造函数，该向量来自θu\theta uθu向量。
参数
tu：θu\theta uθu表示此处用作初始化Euler角度输入的旋转。
下面是成员函数
void vpRxyzVector::buildFrom (const double phi,const double theta,const double psi )
Construction from 3 angles (in radian).
Parameters
phi : φ\varphiφ angle around the xxx axis.
theta : θ\thetaθ angle around the yyy axis.
psi : ψ\psiψ angle around the zzz axis.
vpRxyzVector vpRxyzVector::buildFrom (const vpRotationMatrix & R )
Convert a rotation matrix into a Rxyz=(φ,θ,ψ)R_{xyz}=(\varphi,\theta,\psi)Rxyz=(φ,θ,ψ) Euler angles vector.
参数
R : Rotation matrix used as input.
返回
Rxyz=(φ,θ,ψ)R_{xyz}=(\varphi,\theta,\psi)Rxyz=(φ,θ,ψ) Euler angles vector.
vpRxyzVector vpRxyzVector::buildFrom ( const vpThetaUVector & tu )
Convert a θu\theta uθu vector into a Rxyz=(φ,θ,ψ)R_{xyz}=(\varphi,\theta,\psi)Rxyz=(φ,θ,ψ) Euler angles vector.
参数
tu : θu\theta uθu representation of a rotation used here as input.
返回
Rxyz=(φ,θ,ψ)R_{xyz}=(\varphi,\theta,\psi)Rxyz=(φ,θ,ψ) Euler angles vector.
4.3 vpFeaturePoint3D
定义三维点视觉特征的类。
三维点视觉特征对应于相机帧中坐标为X=（X，Y，Z）{\bf X}=（X，Y，Z）X=（X，Y，Z）的三维点。
此类用于操作三维点视觉要素s=（X，Y，Z）s=（X，Y，Z）s=（X，Y，Z）。与sss相关的交互矩阵如下所示：
有两种方法可以初始化特征。
第一种方法是使用vpFeaturePoint3D成员函数（如set_X（）、set_Y（）、set_Z（）或buildFrom（））设置要素值（X、Y、Z）（X、Y、Z）（X、Y、Z）。
第二种方法是使用feature builder功能从点结构
（如vpFeatureBuilder:：create（vpFeaturePoint3D&，const vpPoint&））初始化特征。
interaction（）方法允许计算与三维点视觉特征相关联的交互矩阵LLL，而error（）方法计算当前视觉特征和所需特征之间的误差向量（s−s∗）（s-s^*）（s−s∗）。
下面的代码显示了如何使用3D点特征（X，Y，Z）（X，Y，Z）（X，Y，Z）创建手眼视觉伺服任务，该特征对应于相机帧中的3D点坐标。要控制六个自由度，必须考虑至少三个其他功能，如VPFEATURATETAU视觉功能。首先，我们创建当前（sss）和所需（s∗s^*s∗）三维点特征，设置任务以使用与所需特征Ls∗L_{s^*}Ls∗关联的交互矩阵，然后计算相机速度 v=−λLs∗+(s−s∗)v=-\lambda \; {L_{s^*}}^+ \; (s-s^*)v=−λLs∗+(s−s∗)。当前功能sss在while（）循环中更新，而s∗s^*s∗设置为Z∗=1Z^*=1Z∗=1。
#include <iostream>
#include <visp/vpFeaturePoint3D.h>
#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpServo.h>
int main()
{vpServo task; // Visual servoing task// Set the 3D point coordinates in the object frame: oPvpPoint point;point.setWorldCoordinates(0.1, -0.1, 0);vpHomogeneousMatrix cMo; // Pose between the camera and the object framecMo.buildFrom(0, 0, 1.2, 0, 0, 0); // ... cMo need here to be computed from a pose estimationpoint.changeFrame(cMo); // Compute the 3D point coordinates in the camera frame cP = cMo * oP// Creation of the current feature svpFeaturePoint3D s;s.buildFrom(point); // Initialize the feature from the 3D point coordinates in the camera frame: s=(X,Y,Z)s.print();// Creation of the desired feature s*. vpFeaturePoint3D s_star;s_star.buildFrom(0, 0, 1); // Z*=1 meters_star.print();// Set eye-in-hand control law. // The computed velocities will be expressed in the camera frametask.setServo(vpServo::EYEINHAND_CAMERA);// Interaction matrix is computed with the desired visual features s*task.setInteractionMatrixType(vpServo::DESIRED); // Set the constant gaindouble lambda = 0.8;task.setLambda(lambda); // Add the 3D point feature to the tasktask.addFeature(s, s_star); // Control loopfor ( ; ; ) {// ... cMo need here to be estimated from for example a pose estimation.point.changeFrame(cMo); // Compute the 3D point coordinates in the camera frame cP = cMo * oP// Update the current 3D point visual features.buildFrom(point);// compute the control lawvpColVector v = task.computeControlLaw(); // camera velocity}
}
如果只想处理三维点特征中的（X，Y）（X，Y）（X，Y）子集特征，只需通过以下行修改上一示例中的addFeature（）调用。在这种情况下，sss的维度是2。
// Add the (X,Y) subset feature from the 3D point visual feature to the task
task.addFeature(s, s_star, vpFeaturePoint3D::selectX() | vpFeaturePoint3D::selectY());
如果您想要构建自己的控制律，则另一个示例显示如何创建当前（sss）和所需（s∗s^*s∗）三维点视觉特征，计算相应的错误向量（（s−s∗）），最后构建交互矩阵（s-s^*）），最后构建交互矩阵（s−s∗）），最后构建交互矩阵L_{s}$。
#include <iostream>
#include <visp/vpFeaturePoint3D.h>
#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpMatrix.h>
int main()
{// Set the 3D point coordinates in the object frame: oPvpPoint point;point.setWorldCoordinates(0.1, -0.1, 0);vpHomogeneousMatrix cMo; // Pose between the camera and the object framecMo.buildFrom(0, 0, 1.2, 0, 0, 0); // ... cMo need here to be computed from a pose estimationpoint.changeFrame(cMo); // Compute the 3D point coordinates in the camera frame cP = cMo * oP// Creation of the current feature svpFeaturePoint3D s;s.buildFrom(point); // Initialize the feature from the 3D point coordinates in the camera frame s.print();// Creation of the desired feature s*. vpFeaturePoint3D s_star;s_star.buildFrom(0, 0, 1); // Z*=1 meters_star.print();// Compute the L_s interaction matrix associated to the current featurevpMatrix L = s.interaction();std::cout << "L: " << L << std::endl;// Compute the error vector (s-s*) for the 3D point featurevpColVector e = s.error(s_star); // e = (s-s*)std::cout << "e: " << e << std::endl;
}4.4 vpFeatureDepth
这个大概率用不到，我就不介绍了，连接大家自己看
#include <visp/vpFeatureDepth.h>
#include <visp/vpServo.h>
int main()
{vpServo task; // Visual servoing taskvpFeatureDepth s; //The current point feature.//Set the current parameters x, y, Z and the desired depth Zsdouble x; //You have to compute the value of x.double y; //You have to compute the value of y.double Z; //You have to compute the value of Z.double Zs; //You have to define the desired depth Zs.//Set the point feature thanks to the current parameters.s.buildfrom(x, y, Z, log(Z/Zs));// Set eye-in-hand control law. // The computed velocities will be expressed in the camera frametask.setServo(vpServo::EYEINHAND_CAMERA);// Interaction matrix is computed with the desired visual features sdtask.setInteractionMatrixType(vpServo::CURRENT);// Add the 3D depth feature to the tasktask.addFeature(s); // s* is here considered as zero// Control loopfor ( ; ; ) {// The new parameters x, y and Z must be computed here.// Update the current point visual features.buildfrom(x, y, Z, log(Z/Zs));// compute the control lawvpColVector v = task.computeControlLaw(); // camera velocity}return 0;
}4.5(-0.5) vpSimulatorCamera
定义最简单的机器人的类：自由飞行的照相机。
这种自由飞行的相机有6个自由度；3个平移，3个旋转。它在世界坐标系中进化成一个一般性的机器人。
该类与vpRobotCamera类类似，只是此处提供了机器人的位置作为从世界坐标系到相机坐标系的转换；wMc。
这种表示法比vpRobotCamera中实现的表示法更直观，vpRobotCamera中考虑了从相机到世界坐标系的转换；cMw。
对于这个特殊的模拟机器人，末端执行器和相机坐标系是混淆的。这意味着cMe转换等于单位矩阵。末端效应器坐标系 eJe{^e}{\bf J}_eeJe 中表示的机器人雅可比矩阵也设置为标识（请参见get_eJe（）。
下面的代码显示了如何控制这个机器人的位置和速度。
#include <visp/vpSimulatorCamera.h>
int main()
{vpHomogeneousMatrix wMc;vpSimulatorCamera robot;robot.getPosition(wMc); // 相机在世界坐标系中的位置std::cout << "Default position of the camera in the world frame wMc:\n" << wMc << std::endl;wMc[2][3] = 1.; //相机坐标系在世界坐标系前面沿z轴1米robot.setPosition(wMc); //设置相机在世界坐标系中的新位置std::cout << "New position of the camera in the world frame wMc:\n" << wMc << std::endl;robot.setSamplingTime(0.100); //将默认采样时间修改为0.1秒robot.setMaxTranslationVelocity(1.); // vx, vy and vz max set to 1 m/srobot.setMaxRotationVelocity(vpMath::rad(90)); // wx, wy and wz max set to 90 deg/svpColVector v(6);v = 0;v[2] = 1.; // set v_z to 1 m/srobot.setVelocity(vpRobot::CAMERA_FRAME, v);//机器人已沿z轴从0.1米处移动robot.getPosition(wMc); //摄影机在世界在坐标系中的位置std::cout << "New position of the camera wMc:\n" << wMc << std::endl;
}
4.5 vpImageSimulator
这个类是相机仿真类，需要了解这个类，从而了代码中那些代码是用来仿真相机的。这样才能知道如何将仿真的部分替换成真实的相机。
该类允许在三维空间中投影图像并获取虚拟摄影机的视图。
该图像由矩形图像表示，其角点坐标在链接到三维矩形的三维坐标中已知。
三维矩形相对于虚拟摄影机（由其内部参数表示）进行定位。实际上，pose cMtcMtcMt必须由类的用户给出。
最后，通过geImage（）方法给出虚拟相机的视图。
您可以使用彩色或灰度图像。
为了避免混叠，特别是当相机离图像平面很近时，可以对必须填充的每个像素进行双线性插值。默认情况下不使用此功能，因为它会占用大量时间。
下面的示例解释如何使用该类。
#include <visp/vpImage.h>
#include <visp/vpImageSimulator.h>
int main()
{vpImage<vpRGBa> Icamera(480,640,0);vpImage<vpRGBa> Iimage(60,60);// Initialise the image which will be projected into the image IcameravpRGBa colorb(0,0,255);vpRGBa colorw(255,255,255);vpRGBa colorr(255,0,0);for(int i = 0; i < 60; i++){for(int j = 0; j < 20; j++)Iimage[i][j] = colorb;for(int j = 20; j < 40; j++)Iimage[i][j] = colorw;for(int j = 40; j < 60; j++)Iimage[i][j] = colorr;}// Initialise the 3D coordinates of the Iimage cornersvpColVector X[4];for (int i = 0; i < 4; i++) X[i].resize(3);// Top left cornerX[0][0] = -1;X[0][1] = -1;X[0][2] = 0;// Top right cornerX[1][0] = 1;X[1][1] = -1;X[1][2] = 0;// Bottom right cornerX[2][0] = 1;X[2][1] = 1;X[2][2] = 0;//Bottom left cornerX[3][0] = -1;X[3][1] = 1;X[3][2] = 0;vpImageSimulator sim;sim.init(Iimage, X);sim.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(60),vpMath::rad(0),0));vpCameraParameters cam(868.0, 869.0, 320, 240);sim.getImage(Icamera,cam);return 0;
}
4.5(0.5) vpMatrix
vpMatrix类提供矩阵的数据结构以及对这些矩阵的一组操作.
vpMatrix::vpMatrix ()
基本构造函数。
矩阵的基本构造函数类对象矩阵的构造。列数和行数为零。
vpMatrix::vpMatrix (unsigned int r,unsigned int c )
构造器。将A初始化为具有0的r x c矩阵。
构造器。
用0初始化矩阵。
参数
r：矩阵行数。
c：矩阵列数。
4.6 vpHomogeneousMatrix 和 vpRotationMatrix
该类提供齐次矩阵的数据结构以及对这些矩阵的一组操作。
关于vpHomogeneousMatrix.cpp的源码可以在该链接。
关于vpRotationMatrix的源码可以在该链接。
vpHomogeneousMatrix由vpMatrix派生而来。
齐次矩阵是4x4矩阵，定义为：
它定义了坐标系b在坐标系a中的位置
aRb^a{\bf R}_baRb 是旋转矩阵，atb^a{\bf t}_batb 是平移向量。
下面看一下关于vpHomogeneousMatrix这个类的一些其他构造和实例化的方法。
vpHomogeneousMatrix::vpHomogeneousMatrix ()
基本构造函数。
将齐次矩阵初始化为单位矩阵。
vpHomogeneousMatrix::vpHomogeneousMatrix (const vpHomogeneousMatrix & M )
复制构造函数。
从另一个齐次矩阵初始化齐次矩阵。
vpHomogeneousMatrix::vpHomogeneousMatrix (const vpTranslationVector & t,const vpRotationMatrix & R )
由平移向量和旋转矩阵构造。
vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector & t,const vpThetaUVector & tu )
从平移向量和θu旋转向量构造。
下面再看vpHomogeneousMatrix一个更重要且常用的方法（这个方法指的是类里面的一些函数）。
void vpHomogeneousMatrix::buildFrom (const vpTranslationVector & t,const vpRotationMatrix & R )
由平移向量和旋转矩阵构造。
void vpHomogeneousMatrix::buildFrom(const vpTranslationVector & t,const vpThetaUVector & tu )
从平移向量和θu旋转向量构造。
void vpHomogeneousMatrix::buildFrom(const vpTranslationVector & t,const vpQuaternionVector & q )
从平移向量和四元数旋转向量构造。
void vpHomogeneousMatrix::buildFrom(const vpPoseVector & p)
从定义为姿势向量的平移向量和θu旋转向量构造。
void vpHomogeneousMatrix::buildFrom(const double tx,const double ty,const double tz,const double tux,const double tuy,const double tuz )
从定义为θu向量的平移和旋转构造。
void vpHomogeneousMatrix::extract(vpRotationMatrix & R) const
从齐次矩阵中提取旋转矩阵。
参数
R：作为旋转矩阵的旋转分量。
void vpHomogeneousMatrix::extract(vpTranslationVector & tv) const
从齐次矩阵中提取平移向量。
void vpHomogeneousMatrix::extract(vpThetaUVector & tu) const
将旋转提取为θU向量。
void vpHomogeneousMatrix::extract(vpQuaternionVector & q) const
将旋转提取为四元数。
下面开始介绍vpRotationMatrix
具体参考这个链接。
vpRotationMatrix考虑旋转矩阵的特殊情况。
它是由vpMatrix导出的。
vpRotationMatrix::vpRotationMatrix ()
基本构造
将旋转矩阵初始化为单位矩阵。
vpRotationMatrix::vpRotationMatrix(const vpRotationMatrix & R)
复制构造函数
从另一个旋转矩阵初始化旋转矩阵
vpRotationMatrix::vpRotationMatrix (const vpThetaUVector & r)
从旋转构造（θU参数化）
vpRotationMatrix::vpRotationMatrix(const vpRzyzVector & r)
从旋转构造（Euler参数化）
旋转构造（欧拉参数化，即Rzyz参数化）
vpRotationMatrix::vpRotationMatrix(const vpRxyzVector & r)
从旋转Rxyz构造。
vpRotationMatrix::vpRotationMatrix(const vpRzyxVector & r)
从旋转Rzyx构造。
vpRotationMatrix::vpRotationMatrix(const double tux,const double tuy,const double tuz )
从旋转构造（θU参数化）
vpRotationMatrix::vpRotationMatrix(const vpQuaternionVector & q)
从四元数表示的旋转构造。
vpRotationMatrix vpRotationMatrix::buildFrom(const vpThetaUVector & v)
将矢量vpThetaUVector转换为旋转矩阵。
vpRotationMatrix vpRotationMatrix::buildFrom(const vpRzyzVector & v)
将表示欧拉（Rzyz）角度的向量转换为旋转矩阵
将表示euler角度的向量转换为旋转矩阵。Rzyz=Rot（z,ϕz, \phiz,ϕ）Rot（y,θy, \thetay,θ）Rot（z,ψz, \psiz,ψ）
vpRotationMatrix vpRotationMatrix::buildFrom(const vpRxyzVector & v)
将表示Rxyz角度的向量转换为旋转矩阵。
Rxyz（ϕθψ\phi\theta\psiϕθψ）=Rot（xψx\psixψ）Rot（yθy\thetayθ）Rot（zϕz\phizϕ）
vpRotationMatrix vpRotationMatrix::buildFrom(const vpRzyxVector & v)
将表示Rzyx角度的向量转换为旋转矩阵。
Rxyz（ϕ、θ、ψ\phi、\theta、\psiϕ、θ、ψ）Rot（z、ψz、\psiz、ψ）Rot（y、θy、\thetay、θ）Rot（x、ϕx、\phix、ϕ）
vpRotationMatrix vpRotationMatrix::buildFrom(const double tux,const double tuy,const double tuz )
从旋转构造（θU参数化）
vpRotationMatrix vpRotationMatrix::buildFrom(const vpQuaternionVector & q)
从旋转构造（作为四元数）
关于一系列的类，可以看下面的例子
try {vpHomogeneousMatrix cdMo;cdMo[1][3] = 1.2;cdMo[2][3] = 0.5;vpHomogeneousMatrix cMo;cMo[0][3] = 0.3;cMo[1][3] = cdMo[1][3];cMo[2][3] = 1.;vpRotationMatrix cRo(0, atan2( cMo[0][3], cMo[1][3]), 0);cMo.insert(cRo);vpSimulatorPioneer robot ;robot.setSamplingTime(0.04);vpHomogeneousMatrix wMc, wMo;robot.getPosition(wMc);wMo = wMc * cMo;vpPoint point;point.setWorldCoordinates(0,0,0);point.track(cMo);vpServo task;task.setServo(vpServo::EYEINHAND_L_cVe_eJe);task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);task.setLambda(0.2);vpVelocityTwistMatrix cVe;cVe = robot.get_cVe();task.set_cVe(cVe);vpMatrix eJe;robot.get_eJe(eJe);task.set_eJe(eJe);vpFeaturePoint s_x, s_xd;vpFeatureBuilder::create(s_x, point);s_xd.buildFrom(0, 0, cdMo[2][3]);task.addFeature(s_x, s_xd, vpFeaturePoint::selectX());vpFeatureDepth s_Z, s_Zd;double Z = point.get_Z();double Zd = cdMo[2][3];s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd));s_Zd.buildFrom(0, 0, Zd, 0);task.addFeature(s_Z, s_Zd);
#ifdef VISP_HAVE_DISPLAY// Create a window (800 by 500) at position (400, 10) with 3 graphicsvpPlot graph(3, 800, 500, 400, 10, "Curves...");// Init the curve plottergraph.initGraph(0,2);graph.initGraph(1,2);graph.initGraph(2,1);graph.setTitle(0, "Velocities");graph.setTitle(1, "Error s-s*");graph.setTitle(2, "Depth");graph.setLegend(0, 0, "vx");graph.setLegend(0, 1, "wz");graph.setLegend(1, 0, "x");graph.setLegend(1, 1, "log(Z/Z*)");graph.setLegend(2, 0, "Z");
#endifint iter = 0;for (; ;){robot.getPosition(wMc) ;cMo = wMc.inverse() * wMo;point.track(cMo);vpFeatureBuilder::create(s_x, point);Z = point.get_Z() ;s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd)) ;robot.get_cVe(cVe);task.set_cVe(cVe);robot.get_eJe(eJe);task.set_eJe(eJe);vpColVector v = task.computeControlLaw(iter*robot.getSamplingTime());robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
#ifdef VISP_HAVE_DISPLAYgraph.plot(0, iter, v); // plot velocities applied to the robotgraph.plot(1, iter, task.getError()); // plot error vectorgraph.plot(2, 0, iter, Z); // plot the depth
#endifiter ++;if (task.getError().sumSquare() < 0.0001) {std::cout << "Reached a small error. We stop the loop... " << std::endl;break;}}
#ifdef VISP_HAVE_DISPLAYgraph.saveData(0, "./v2.dat");graph.saveData(1, "./error2.dat");const char *legend = "Click to quit...";vpDisplay::displayCharString(graph.I, (int)graph.I.getHeight()-60, (int)graph.I.getWidth()-150, legend, vpColor::red);vpDisplay::flush(graph.I);vpDisplay::getClick(graph.I);
#endif// Kill the servo tasktask.print() ;task.kill();}catch(vpException e) {std::cout << "Catch an exception: " << e << std::endl;}
}
4.7 vpImagePoint
定义图像中二维点的类。此类对于图像处理非常有用，并且仅存储以亚像素为单位给出的二维坐标。
如果要根据对象坐标系、相机坐标系或图像平面中以米为单位给出的坐标定义点，则必须使用vpPoint类。
在此类中，二维坐标不是必需得是整数值。在ViSP中使用的两个框架中，很容易操作给定的坐标：（i，j）坐标和（u，v）坐标。以下两幅图说明了这两个坐标系。
vpImagePoint类的实例对应于特定点。因此，如果使用 set_i(const double i)更改点坐标，则产生的效果与使用 set_v(const double v)时相同。这两个方法更改相同的私有属性。对于set_j（const double j）和set_（const double u）这两种方法也是如此。
由于参数iiiiii和jjjjjj，初始化图像坐标的默认构造函数。
vpImagePoint::vpImagePoint ( double ii,double jj )
。
vpImagePoint::vpImagePoint( const vpImagePoint & ip)
复制构造函数。
使用ip初始化图像点的坐标。
4.8 vpPose
用于从N个点计算姿势的类（仅从点计算姿势）。
也可以使用vpPoseFeatures类从其他特征估计姿势。
位姿估计的方法有很多中：
- LAGRANGE
- DEMENTHON
- LOWE
- RANSAC
- LAGRANGE_LOWE
- DEMENTHON_LOWE
- VIRTUAL_VS
- DEMENTHON_VIRTUAL_VS
- LAGRANGE_VIRTUAL_VS
void vpPose::addPoint(const vpPoint &newP)
在此array中添加一个新点。
在点array中添加新点。
参数
newP：要添加到点数组中的新点。
从vpPoint类中考虑一个点，X、Y和Z将表示3D信息，X和Y将表示其2D信息。这5个字段必须初始化才能在此库中使用
void vpPose::clearPoint()
抑制点array中的所有点
删除点的array
void vpPose::computePose (vpPoseMethodType methode,vpHomogeneousMatrix & cMo )
计算给定方法的姿势
根据所需的方法计算姿势。
不同的方法是
- LAGRANGE 拉格朗日方法（在平面和非平面算法之间切换进行测试）
- DEMENTHON DEMENTHON方法（测试在平面和非平面算法之间切换）
- VIRTUAL_VS 虚拟视觉伺服方法
- DEMENTHON_VIRTUAL_VS 虚拟视觉伺服方法使用DEMENTHON方法初始化
- LAGRANGE_VIRTUAL_VS 用拉格朗日方法初始化虚拟视觉伺服
double vpPose::computeResidual(const vpHomogeneousMatrix & cMo) const
计算并返回姿势矩阵“cMo”的残差（以米为单位）。
计算残差（即结果的质量）计算残差（姿势M以米为单位）
Parameters
cMo：输入姿势。定义要测试的姿势的矩阵。
Return
以米为单位的残差值。。
double vpPose::computeResidual(const vpColVector & x,const vpColVector & M,vpColVector & d )
4.8 (0.5) vpPoseVector
姿势是欧几里得空间中每个刚体运动的完整表示。
它由平移和最小旋转组成，最小旋转由6维姿势向量表示，如下所示：
其中arb^{a}{\bf r}_barb 是从坐标系aaa到坐标系bbb的姿势，atb^{a}{\bf t}{b}atb是这些坐标系之间沿x、y、z轴和Θu\Theta\bf uΘu的平移向量，Θu\Theta\bf uΘu表示这些坐标系之间的旋转aRb^{a}\bf{R}_{b}aRb 。
要了解有关Θu\Theta\bf uΘu旋转表示法的更多信息，请参阅vpThetaUVector文档。
vpPoseVector::vpPoseVector()
默认构造函数。
构造一个6维**(x1,x2,x3,x4,x5,x6)**姿势向量 [t,Θu]⊤[\bf t, \Theta \bf u]^\top[t,Θu]⊤ ，其中Θu\Theta\bf uΘu是一个旋转向量[Θux,Θuy,Θuz]⊤[\Theta u_x, \Theta u_y, \Theta u_z]^\top[Θux,Θuy,Θuz]⊤，而t\bf tt是一个平移向量 [tx,ty,tz]⊤[t_x, t_y, t_z]^\top[tx,ty,tz]⊤。
姿势向量初始化为零。
vpPoseVector::vpPoseVector(const double tx,const double ty,const double tz,const double tux,const double tuy,const double tuz )
从3个平移和3个Θu\Theta\bf{u}Θu角度构造6维姿势向量[t、Θu]⊤[\bf{t}、\Theta\bf{u}]^\top[t、Θu]⊤。
平移以米表示，旋转以弧度表示。
参数
tx，ty，tz：Translations[tx，ty，tz]⊤[t_x，t_y，t_z]^\top[tx，ty，tz]⊤分别沿x、y和z轴（以米为单位）。
tux、tuy、tuz：分别围绕x、y和z轴旋转 [Θux,Θuy,Θuz]⊤[\Theta u_x, \Theta u_y, \Theta u_z]^\top[Θux,Θuy,Θuz]⊤ （弧度）。
vpPoseVector::vpPoseVector(const vpHomogeneousMatrix & M )
从齐次矩阵M\bf MM构造6维姿势向量[tΘ,u]⊤[\bf t\Theta, \bf u]^\top[tΘ,u]⊤。
参数
M：齐次矩阵M\bf MM，从中提取平移t\bf tt和Θu\Theta\bf uΘu向量以初始化姿势向量。
vpPoseVector::vpPoseVector(const vpTranslationVector & tv,const vpThetaUVector & tu )
从平移向量t\bf tt和Θu\Theta\bf uΘu向量构造一个6维姿势向量[t,Θu]⊤[\bf t, \Theta\bf u]^\top[t,Θu]⊤。
参数
tv：平移向量t\bf tt。
tu：Θu\Theta\bf uΘu旋转向量。
vpPoseVector::vpPoseVector(const vpTranslationVector & tv,const vpRotationMatrix & R )
从平移向量t\bf tt和旋转矩阵R\bf RR构造一个6维姿势向量[tΘu]⊤[\bf t\Theta\bf u]^\top[tΘu]⊤。
参数
tv：平移向量t\bf tt。
R：旋转矩阵R\bf RR，从中提取Θu\Theta\bf uΘu向量以初始化姿势向量。
4.9 vpPoseFeatures
用于从任何特征计算姿势的工具。
该类允许从视觉特征通过虚拟视觉伺服来估计姿势。所考虑的特征包括点、线段、直线和椭圆。如果编译器与C++ 11兼容，则可以引入在菲斯普中未直接实现的特定特征。
位姿估计器
- VIRTUAL_VS 虚拟视觉伺服方法。
- ROBUST_VIRTUAL_VS 鲁棒虚拟视觉伺服方法。
4.10 vpPoint
源码地址直接参考这个链接
vpPoint构建一个结构，既包含对象坐标系中点的三维坐标，也包含以米表示的点的二维坐标。即，（X，Y，Z，x，y）。其中前三个是该点的三维坐标，后两个是2维坐标
Public Member FunctionsvpPoint ()virtual ~vpPoint ()void changeFrame (const vpHomogeneousMatrix &cMo, vpColVector &_cP)void changeFrame (const vpHomogeneousMatrix &cMo)void display (const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, const unsigned int thickness=1)void display (const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color=vpColor::green, const unsigned int thickness=1)void display (const vpImage< vpRGBa > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color=vpColor::green, const unsigned int thickness=1)vpPoint * duplicate () constdouble get_X () constdouble get_Y () constdouble get_Z () constdouble get_W () constdouble get_oX () constdouble get_oY () constdouble get_oZ () constdouble get_oW () constdouble get_x () constdouble get_y () constdouble get_w () constvoid getWorldCoordinates (double &ox, double &oy, double &oz)void getWorldCoordinates (vpColVector &_oP)vpColVector getWorldCoordinates (void)void init ()vpPoint & operator= (const vpPoint &vpp)void projection (const vpColVector &_cP, vpColVector &_p)void projection ()void set_X (const double X)void set_Y (const double Y)void set_Z (const double Z)void set_W (const double W)void set_oX (const double X)void set_oY (const double Y)void set_oZ (const double Z)void set_oW (const double W)void set_x (const double x)void set_y (const double y)void set_w (const double w)void setWorldCoordinates (const double ox, const double oy, const double oz)void setWorldCoordinates (const vpColVector &_oP)void project ()void project (const vpHomogeneousMatrix &cMo)void track (const vpHomogeneousMatrix &cMo)virtual void print () constvoid setDeallocate (vpForwardProjectionDeallocatorType d)vpForwardProjectionDeallocatorType getDeallocate ()
下面这个函数是主要用的
void vpPoint::changeFrame(const vpHomogeneousMatrix & cMo,vpColVector & _cP)
从对象坐标系集中的点的三维坐标（例如使用setWorldCoordinates（）或set_oX（）、set_oY（）、set_oZ（））计算相机坐标系中点的三维坐标。
参数
cMo：从摄影机到对象坐标系的变换。
_cP：摄影机坐标系中点的三维坐标。
void vpPoint::changeFrame (const vpHomogeneousMatrix & cMo)
从对象坐标系集中的点的三维坐标（例如使用setWorldCoordinates（）或set_oX（）、set_oY（）、set_oZ（））计算相机坐标系中点的三维坐标。
参数
cMo：从摄影机到对象坐标系的变换。
double vpPoint::get_oW () const
获取对象坐标系中的点W坐标。
double vpPoint::get_oX () const
获取对象坐标系中的点X坐标。
double vpPoint::get_oY () const
获取对象坐标系中的点Y坐标。
double vpPoint::get_oZ () const
获取对象坐标系中的点Z坐标。
double vpPoint::get_W () const
获取相机坐标系中的点W坐标。
double vpPoint::get_w () const
获取图像平面中的点w坐标。
double vpPoint::get_X () const
获取相机坐标系中的点X坐标。
double vpPoint::get_x () const
获取图像平面中的点x坐标。
double vpPoint::get_Y () const
获取相机坐标系中的点Y坐标。
double vpPoint::get_y () const
获取图像平面中的点y坐标。
double vpPoint::get_Z () const
获取摄影机帧中的点Z坐标。
void vpPoint::getWorldCoordinates (double &ox,double &oy,double &oz )
获取点的世界坐标。我们这里指的是目标坐标系中点的坐标。
void vpPoint::getWorldCoordinates (vpColVector &_oP)
获取点的世界坐标。我们这里指的是目标坐标系中点的坐标。
4.11 vpFeatureBuilder
定义跟踪器和视觉特征之间转换的类，详细参考。
void vpFeatureBuilder::create(vpFeaturePoint & s,const vpCameraParameters &cam,const vpDot &d )
使用vpDot和相机参数创建vpFeaturePoint。vpDot仅包含图像中该点的像素坐标。因此，该方法使用相机参数计算图像平面中的仪表坐标xxx和yyy。这些坐标存储在vpFeaturePoint中。
由于vpDot，无法计算点ZZZ在相机坐标系中的深度。vpFeaturePoint中需要此坐标来计算交互矩阵。所以这个值必须在这个函数之外计算。
参数
s：要初始化的视觉特征（x，y）（x，y）（x，y）。请注意，用于计算交互矩阵的3D深度ZZZ不是由该函数初始化的。
cam：用于获取包含vpDot的图像的摄像机参数。
d：用于创建vpFeaturePoint的vpDot。
下面的代码显示了如何初始化vpFeaturePoint视觉特征。首先，我们初始化x，yx，yx，y，最后我们设置点的3D深度ZZZ，这通常是姿势估计的结果。
vpImage<unsigned char> I; // Image container
vpCameraParameters cam; // Default intrinsic camera parameters
vpDot dot; // Dot tracker
vpFeaturePoint s; // Point feature
...
// Tracking on the dot
dot.track(I);
// Initialize rho,theta visual feature
vpFeatureBuilder::create(s, cam, dot);
// 请求姿势估计以初始化Z，即相机坐标系中点的深度。
double Z = 1; // Depth of the point in meters
....
s.set_Z(Z);
void vpFeatureBuilder::create ( vpFeaturePoint & s,const vpCameraParameters & cam,const vpDot2 & d )
借助vpDot2和相机参数创建vpFeaturePoint。vpDot2仅包含图像中点的像素坐标。因此，该方法使用相机参数计算图像平面中的仪表坐标xxx和yyy。这些坐标存储在vpFeaturePoint中。
由于vpDot2，无法计算相机坐标系中点ZZZ的深度。vpFeaturePoint中需要此坐标来计算交互矩阵。所以这个值必须在这个函数之外计算。
参数
s：特征点。
cam：用于获取包含vpDot2的图像的摄像机参数。
d：用于创建vpFeaturePoint的vpDot2。
下面的代码显示了如何初始化vpFeaturePoint视觉特征。首先，我们初始化x，yx，yx，y，最后我们设置点的3D深度ZZZ，这通常是姿势估计的结果。
vpImage<unsigned char> I; // Image container
vpCameraParameters cam; // Default intrinsic camera parameters
vpDot2 dot; // Dot tracker
vpFeaturePoint s; // Point feature
...
// Tracking on the dot
dot.track(I);
// Initialize rho,theta visual feature
vpFeatureBuilder::create(s, cam, dot);
// A pose estimation is requested to initialize Z, the depth of the
// point in the camera frame.
double Z = 1; // Depth of the point in meters
....
s.set_Z(Z);
另一个create
void vpFeatureBuilder::create(vpFeaturePoint & s,const vpPoint & p)
使用vpPoint创建vpFeaturePoint。此方法使用图像平面中的点坐标xxx和yyy来设置视觉特征参数。相机坐标系中深度ZZZ的值也会根据存储在vpPoint中的相机坐标系的坐标进行计算。
为确保vpFeaturePoint已正确初始化，必须确保至少计算并存储在vpPoint中的图像平面和相机坐标系中的点坐标。
参数
s : The feature point.
p : The vpPoint used to create the vpFeaturePoint.
4.12 vpDot
详细介绍参考
此跟踪器用于跟踪 vpImage上的点（具有相同灰度的连接像素）。
该算法基于图像二值化和connex组件分割来确定点特征（位置、矩、大小…）。
下面的示例代码演示如何从firewire摄影机获取图像、跟踪水滴并显示跟踪结果。
#include <visp/vpConfig.h>
#include <visp/vp1394TwoGrabber.h>
#include <visp/vpDisplayX.h>
#include <visp/vpDot.h>
#include <visp/vpImage.h>
int main()
{#if defined(VISP_HAVE_DC1394_2)vpImage<unsigned char> I; // Create a gray level image containervp1394TwoGrabber g(false); //基于libdc1394-2.x第三方库创建抓取器g.acquire(I); // Acquire an image 采集图像
#if defined(VISP_HAVE_X11)vpDisplayX d(I, 0, 0, "Camera view");
#endifvpDisplay::display(I);vpDisplay::flush(I);vpDot blob;blob.initTracking(I);blob.setGraphics(true);while(1) {g.acquire(I); // Acquire an imagevpDisplay::display(I);blob.track(I);vpDisplay::flush(I);}
#endif
}
vpImagePoint vpDot::getCog () const
返回点重心的位置。
const vpDot &d；
vpImagePoint cog;
cog = d.getCog();
4.13 vpDot2
详细介绍参考。
此跟踪器用于跟踪 vpImage上的blob（具有相同灰度的connex像素）。
算法基于图像的二值化，然后基于轮廓检测，使用Freeman链编码确定水滴特征（位置、力矩、大小…）。
二值化使用定义blob允许灰度的灰度最小值和最大值来完成。可以通过setGrayLevelMin（）和setGrayLevelMax（）指定这些级别。这些级别也由setGrayLevelPrecision（）自动设置。该算法允许在黑色背景上跟踪白色对象，反之亦然。
当发现blob时，将执行一些测试以查看其是否有效：
- 默认情况下，blob被视为椭球体。如果形状不是椭球体，则发现的blob可能会被拒绝。为了确定形状是否为椭球，该算法考虑内椭圆和外椭圆。这两个椭圆上的采样点应具有正确的灰度。沿内椭圆，采样点的灰度应在灰度最小和最大范围内，而在外椭圆上，灰度应在灰度范围外。要设置应具有正确级别的采样点的百分比，请使用setEllipIDPadPointsPercentage（）。内部ellpsoid和blob轮廓之间的距离，以及blob轮廓和外部椭圆之间的距离由setEllploidShapePrecision（）固定。如果要跟踪非椭球形状，并关闭此验证测试，则必须调用SetEllilidShapePrecision（0）。
- 将blob的宽度、高度和表面与前一个blob的相应值进行比较。如果它们相差太大，则可能会被拒绝。要设置允许的距离，可以使用setSizePrecision（）。
请注意，track（）和searchDotsInArea（）是此类最重要的特征。
- track（）使用点的前一个位置估计点的当前位置，然后尝试计算点的新参数。如果一切正常，则跟踪成功，否则我们将在窗口中围绕点的最后位置搜索该点。
- searchDotsInArea（）允许在窗口中查找与此点类似的点。当执行点的基本跟踪出现问题时使用，但也可用于在完整图像中查找特定类型的点。
tutorial-blob-tracker.cpp中提供的以下示例代码显示了如何从firewire摄影机抓取图像、跟踪blob并显示跟踪结果。
#include <visp/vp1394CMUGrabber.h>
#include <visp/vp1394TwoGrabber.h>
#include <visp/vpDisplayGDI.h>
#include <visp/vpDisplayX.h>
#include <visp/vpDot2.h>
int main()
{#if (defined(VISP_HAVE_DC1394_2) || defined(VISP_HAVE_CMU1394))try {vpImage<unsigned char> I; // Create a gray level image container
#if defined(VISP_HAVE_DC1394_2)vp1394TwoGrabber g(false);
#elif defined(VISP_HAVE_CMU1394)vp1394CMUGrabber g;
#endifg.open(I);g.acquire(I);
#if defined(VISP_HAVE_X11)vpDisplayX d(I, 0, 0, "Camera view");
#elif defined(VISP_HAVE_GDI)vpDisplayGDI d(I, 0, 0, "Camera view");
#elsestd::cout << "No image viewer is available..." << std::endl;
#endifvpDisplay::display(I);vpDisplay::flush(I);vpDot2 blob;blob.setGraphics(true);blob.setGraphicsThickness(2);blob.initTracking(I);while(1) {g.acquire(I); // Acquire an imagevpDisplay::display(I);blob.track(I);vpDisplay::flush(I);if (vpDisplay::getClick(I, false))break;}}catch(vpException e) {std::cout << "Catch an exception: " << e << std::endl;}
#endif
}
tutorial-blob-auto-tracker.cpp中提供的另一个示例首先显示了如何在第一张图像中检测所有在大小、面积和灰度方面与某些特征匹配的blob。其次，它显示了如何跟踪检测到的所有点。
#include <visp/vpDisplayGDI.h>
#include <visp/vpDisplayX.h>
#include <visp/vpDot2.h>
#include <visp/vpImageIo.h>
int main()
{try {bool learn = false;vpImage<unsigned char> I; // Create a gray level image containervpImageIo::read(I, "./target.pgm");
#if defined(VISP_HAVE_X11)vpDisplayX d(I, 0, 0, "Camera view");
#elif defined(VISP_HAVE_GDI)vpDisplayGDI d(I, 0, 0, "Camera view");
#elsestd::cout << "No image viewer is available..." << std::endl;
#endifvpDisplay::display(I);vpDisplay::flush(I);vpDot2 blob;if (learn) {// Learn the characteristics of the blob to auto detectblob.setGraphics(true);blob.setGraphicsThickness(1);blob.initTracking(I);blob.track(I);std::cout << "Blob characteristics: " << std::endl;std::cout << " width : " << blob.getWidth() << std::endl;std::cout << " height: " << blob.getHeight() << std::endl;
#if VISP_VERSION_INT > VP_VERSION_INT(2,7,0)std::cout << " area: " << blob.getArea() << std::endl;
#endifstd::cout << " gray level min: " << blob.getGrayLevelMin() << std::endl;std::cout << " gray level max: " << blob.getGrayLevelMax() << std::endl;std::cout << " grayLevelPrecision: " << blob.getGrayLevelPrecision() << std::endl;std::cout << " sizePrecision: " << blob.getSizePrecision() << std::endl;std::cout << " ellipsoidShapePrecision: " << blob.getEllipsoidShapePrecision() << std::endl;}else {// Set blob characteristics for the auto detectionblob.setWidth(50);blob.setHeight(50);
#if VISP_VERSION_INT > VP_VERSION_INT(2,7,0)blob.setArea(1700);
#endifblob.setGrayLevelMin(0);blob.setGrayLevelMax(30);blob.setGrayLevelPrecision(0.8);blob.setSizePrecision(0.65);blob.setEllipsoidShapePrecision(0.65);}std::list<vpDot2> blob_list;blob.searchDotsInArea(I, 0, 0, I.getWidth(), I.getHeight(), blob_list);if (learn) {// The blob that is tracked by initTracking() is not in the list of auto detected blobs// We add it:blob_list.push_back(blob);}std::cout << "Number of auto detected blob: " << blob_list.size() << std::endl;std::cout << "A click to exit..." << std::endl;while(1) {vpDisplay::display(I);for(std::list<vpDot2>::iterator it=blob_list.begin(); it != blob_list.end(); ++it) {(*it).setGraphics(true);(*it).setGraphicsThickness(3);(*it).track(I);}vpDisplay::flush(I);if (vpDisplay::getClick(I, false))break;vpTime::wait(40);}}catch(vpException e) {std::cout << "Catch an exception: " << e << std::endl;}
}
cog 一般表示中心点
4.14 vpFeaturePoint
详细介绍参考该链接
Public Member Functions
void init ()vpFeaturePoint ()virtual ~vpFeaturePoint ()void buildFrom (const double x, const double y, const double Z)void set_x (const double x)void set_y (const double y)void set_Z (const double Z)void set_xyZ (const double x, const double y, const double Z)double get_x () constdouble get_y () constdouble get_Z () constvpMatrix interaction (const unsigned int select=FEATURE_ALL)vpColVector error (const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)void print (const unsigned int select=FEATURE_ALL) constvpFeaturePoint * duplicate () constvoid display (const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) constvoid display (const vpCameraParameters &cam, const vpImage< vpRGBa > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) constunsigned int dimension_s ()virtual double operator[] (const unsigned int i) constvpColVector get_s (unsigned int select=FEATURE_ALL) constunsigned int getDimension (const unsigned int select=FEATURE_ALL) constvoid setFlags ()void setDeallocate (vpBasicFeatureDeallocatorType d)vpBasicFeatureDeallocatorType getDeallocate ()定义二维点视觉特征sss的类，该特征由两个参数组成，即笛卡尔坐标xxx和yyy。
在该类中，xxx和yyy是图像平面中的二维坐标，以米为单位ZZZ是表示深度的三维坐标，也是点的一个参数。在计算交互矩阵LLL时需要它。
可以通过vpPoint、vpDot或vpDot2类的实例轻松设置视觉特征。有关更高精度的信息，请参见vpFeatureBuilder类。
设置视觉特征的值后，interaction（）方法允许计算与视觉特征关联的交互矩阵LLL，而error（）方法计算当前视觉特征和所需视觉特征之间的错误向量(s−s∗)(s-s^*)(s−s∗)。
下面的代码显示了如何使用2D点特征（x，y）（x，y）（x，y）创建eye-in hand视觉伺服任务，该特征对应于图像平面中点的2D坐标。要控制六个自由度，必须将至少四个其他特征视为与其他两个点特征相同，例如。首先，我们创建一个当前（sss）二维点特征。然后，我们将任务设置为使用与当前特征LsL_ sLs关联的交互矩阵。最后我们计算摄像机的速度v=−λLs+(s−s∗)v=-\lambda \; L_s^+ \; (s-s^*)v=−λLs+(s−s∗)。当前特征sss在while（）循环中更新。
#include <visp/vpFeaturePoint.h>
#include <visp/vpServo.h>
int main()
{vpServo task; // Visual servoing taskvpFeaturePoint sd; //The desired point feature.//Set the desired features x and ydouble xd = 0;double yd = 0;//Set the depth of the point in the camera frame.double Zd = 1;//Set the point feature thanks to the desired parameters.sd.buildFrom(xd, yd, Zd);vpFeaturePoint s; //The current point feature.//Set the current features x and ydouble x; //You have to compute the value of x.double y; //You have to compute the value of y.double Z; //You have to compute the value of Z.//Set the point feature thanks to the current parameters.s.buildFrom(x, y, Z);//In this case the parameter Z is not necessary because the interaction matrix is computed//with the desired visual feature.// Set eye-in-hand control law. // The computed velocities will be expressed in the camera frametask.setServo(vpServo::EYEINHAND_CAMERA);// Interaction matrix is computed with the desired visual features sdtask.setInteractionMatrixType(vpServo::DESIRED);// Add the 2D point feature to the tasktask.addFeature(s, sd);// Control loopfor ( ; ; ) {// The new parameters x and y must be computed here.// Update the current point visual features.buildFrom(x, y, Z);// compute the control lawvpColVector v = task.computeControlLaw(); // camera velocity}return 0;
}
如果您想构建自己的控制律，另一个示例显示如何创建当前（sss）和所需（s∗s^*s∗）二维点视觉特征，计算相应的错误向量（（s−s∗）），最后构建交互矩阵（s-s^*）），最后构建交互矩阵（s−s∗）），最后构建交互矩阵L_s$。
#include <visp/vpFeaturePoint.h>
#include <visp/vpMatrix.h>
int main()
{vpFeaturePoint sd; //The desired point feature.//Set the desired features x and ydouble xd = 0;double yd = 0;//Set the depth of the point in the camera frame.double Zd = 1;//Set the point feature thanks to the desired parameters.sd.buildFrom(xd, yd, Zd);vpFeaturePoint s; //The current point feature.//Set the current features x and ydouble x; //You have to compute the value of x.double y; //You have to compute the value of y.double Z; //You have to compute the value of Z.//Set the point feature thanks to the current parameters.s.buildFrom(x, y, Z);// Compute the interaction matrix L_s for the current point featurevpMatrix L = s.interaction();// You can also compute the interaction matrix L_s for the desired point feature// The corresponding line of code is : vpMatrix L = sd.interaction();// Compute the error vector (s-sd) for the point features.error(s_star);
}
4.15 vpPixelMeterConversion
从像素坐标（u，v）（u，v）（u，v）到标准化坐标（x，y）（x，y）（x，y）的转换，单位为米。
此类与vpCameraParameters相关。
void vpPixelMeterConversion::convertLine(const vpCameraParameters & cam,const double & rho_p,const double & theta_p,double & rho_m,double & theta_m )
void vpPixelMeterConversion::convertMoment ( const vpCameraParameters & cam,unsigned int order,const vpMatrix & moment_pixel,vpMatrix & moment_meter ) staticstatic void vpPixelMeterConversion::convertPoint ( const vpCameraParameters & cam,const double & u,const double & v,double & x,double & y )
点坐标从像素坐标（u，v）（u，v）（u，v）转换为标准化坐标（x，y）（x，y）（x，y），单位为米。
使用的公式取决于相机的投影模型。要了解当前使用的投影模型，请使用vpCameraParameter:：get_projModel（）
参数
摄像机参数。
u、 v：以像素为单位输入坐标。
x、 y：以米为单位的输出坐标。
在透视投影没有失真的情况下，x=（u−u0）/pxx=（u-u_0）/p_xx=（u−u0）/px和y=（v−v0）/pyy=（v-v_0）/p_yy=（v−v0）/py。
在具有失真的透视投影的情况下，使用r2=((u−u0)/px)2+((v−v0)/py)2r^2=((u - u_0)/p_x)^2+((v-v_0)/p_y)^2r2=((u−u0)/px)2+((v−v0)/py)2 的x=(u−u0)∗(1+kdu∗r2)/pxx = (u-u_0)*(1+k_{du}*r^2)/p_xx=(u−u0)∗(1+kdu∗r2)/px and y=(v−v0)∗(1+kdu∗r2)/pyy = (v-v_0)*(1+k_{du}*r^2)/p_yy=(v−v0)∗(1+kdu∗r2)/py 。
4.16 vpColVector
为列向量以及这些向量上的一组操作提供数据结构的类。
4.17 vpImageConvert
vpImageConvert.h是一个图像格式转换器，他几乎把所有图像格式转换格式都收录进来。
基本的成员函数如下：
Static Public Member Functions
static void convert (const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
static void convert (const vpImage< vpRGBa > &src, vpImage< unsigned char > &dest)
static void convert (const vpImage< float > &src, vpImage< unsigned char > &dest)
static void convert (const vpImage< unsigned char > &src, vpImage< float > &dest)
static void convert (const vpImage< double > &src, vpImage< unsigned char > &dest)
static void convert (const vpImage< unsigned char > &src, vpImage< double > &dest)
static void convert (const IplImage *src, vpImage< vpRGBa > &dest, bool flip=false)
static void convert (const IplImage *src, vpImage< unsigned char > &dest, bool flip=false)
static void convert (const vpImage< vpRGBa > &src, IplImage *&dest)
static void convert (const vpImage< unsigned char > &src, IplImage *&dest)
static void convert (const cv::Mat &src, vpImage< vpRGBa > &dest, const bool flip=false)
static void convert (const cv::Mat &src, vpImage< unsigned char > &dest, const bool flip=false)
static void convert (const vpImage< vpRGBa > &src, cv::Mat &dest)
static void convert (const vpImage< unsigned char > &src, cv::Mat &dest, const bool copyData=true)
static void convert (const vpImage< unsigned char > &src, yarp::sig::ImageOf< yarp::sig::PixelMono > *dest, const bool copyData=true)
static void convert (const yarp::sig::ImageOf< yarp::sig::PixelMono > *src, vpImage< unsigned char > &dest, const bool copyData=true)
static void convert (const vpImage< vpRGBa > &src, yarp::sig::ImageOf< yarp::sig::PixelRgba > *dest, const bool copyData=true)
static void convert (const yarp::sig::ImageOf< yarp::sig::PixelRgba > *src, vpImage< vpRGBa > &dest, const bool copyData=true)
static void convert (const vpImage< vpRGBa > &src, yarp::sig::ImageOf< yarp::sig::PixelRgb > *dest)
static void convert (const yarp::sig::ImageOf< yarp::sig::PixelRgb > *src, vpImage< vpRGBa > &dest)
static void split (const vpImage< vpRGBa > &src, vpImage< unsigned char > *pR, vpImage< unsigned char > *pG, vpImage< unsigned char > *pB, vpImage< unsigned char > *pa=NULL)
static void YUVToRGB (unsigned char y, unsigned char u, unsigned char v, unsigned char &r, unsigned char &g, unsigned char &b)
static void YUYVToRGBa (unsigned char *yuyv, unsigned char *rgba, unsigned int width, unsigned int height)
static void YUYVToRGB (unsigned char *yuyv, unsigned char *rgb, unsigned int width, unsigned int height)
static void YUYVToGrey (unsigned char *yuyv, unsigned char *grey, unsigned int size)
static void YUV411ToRGBa (unsigned char *yuv, unsigned char *rgba, unsigned int size)
static void YUV411ToRGB (unsigned char *yuv, unsigned char *rgb, unsigned int size)
static void YUV411ToGrey (unsigned char *yuv, unsigned char *grey, unsigned int size)
static void YUV422ToRGBa (unsigned char *yuv, unsigned char *rgba, unsigned int size)
static void YUV422ToRGB (unsigned char *yuv, unsigned char *rgb, unsigned int size)
static void YUV422ToGrey (unsigned char *yuv, unsigned char *grey, unsigned int size)
static void YUV420ToRGBa (unsigned char *yuv, unsigned char *rgba, unsigned int width, unsigned int height)
static void YUV420ToRGB (unsigned char *yuv, unsigned char *rgb, unsigned int width, unsigned int height)
static void YUV420ToGrey (unsigned char *yuv, unsigned char *grey, unsigned int size)
static void YUV444ToRGBa (unsigned char *yuv, unsigned char *rgba, unsigned int size)
static void YUV444ToRGB (unsigned char *yuv, unsigned char *rgb, unsigned int size)
static void YUV444ToGrey (unsigned char *yuv, unsigned char *grey, unsigned int size)
static void YV12ToRGBa (unsigned char *yuv, unsigned char *rgba, unsigned int width, unsigned int height)
static void YV12ToRGB (unsigned char *yuv, unsigned char *rgb, unsigned int width, unsigned int height)
static void YVU9ToRGBa (unsigned char *yuv, unsigned char *rgba, unsigned int width, unsigned int height)
static void YVU9ToRGB (unsigned char *yuv, unsigned char *rgb, unsigned int width, unsigned int height)
static void RGBToRGBa (unsigned char *rgb, unsigned char *rgba, unsigned int size)
static void RGBaToRGB (unsigned char *rgba, unsigned char *rgb, unsigned int size)
static void RGBToGrey (unsigned char *rgb, unsigned char *grey, unsigned int size)
static void RGBaToGrey (unsigned char *rgba, unsigned char *grey, unsigned int size)
static void RGBToRGBa (unsigned char *bgr, unsigned char *rgba, unsigned int width, unsigned int height, bool flip=false)
static void RGBToGrey (unsigned char *bgr, unsigned char *grey, unsigned int width, unsigned int height, bool flip=false)
static void GreyToRGBa (unsigned char *grey, unsigned char *rgba, unsigned int size)
static void GreyToRGB (unsigned char *grey, unsigned char *rgb, unsigned int size)
static void BGRToRGBa (unsigned char *bgr, unsigned char *rgba, unsigned int width, unsigned int height, bool flip)
static void BGRToGrey (unsigned char *bgr, unsigned char *grey, unsigned int width, unsigned int height, bool flip)
static void YCbCrToRGB (unsigned char *ycbcr, unsigned char *rgb, unsigned int size)
static void YCbCrToRGBa (unsigned char *ycbcr, unsigned char *rgb, unsigned int size)
static void YCrCbToRGB (unsigned char *ycbcr, unsigned char *rgb, unsigned int size)
static void YCrCbToRGBa (unsigned char *ycbcr, unsigned char *rgb, unsigned int size)
static void YCbCrToGrey (unsigned char *ycbcr, unsigned char *grey, unsigned int size)
static void MONO16ToGrey (unsigned char *grey16, unsigned char *grey, unsigned int size)
static void MONO16ToRGBa (unsigned char *grey16, unsigned char *rgba, unsigned int size)
例如，下面一个例子：
我们需要将visp和opencv集和使用
void vpImageConvert::convert(const cv::Mat & src,vpImage< unsigned char > & dest,const bool flip = false )
将cv:：Mat转换为vpImage
cv::Mat是一个OpenCV图像类。看见http://opencv.willowgarage.com对于常规OpenCV文档，或http://opencv.willowgarage.com/documentation/cpp/core_basic_structures.html对于特定的垫结构文档。
与 convert(const IplImage*src，vpImage&dest，bool flip)方法类似，仅转换深度等于8且通道介于1和3之间的Mat。
视觉伺服控制工具Visual Servoing Platform---VISP（7）----vpServo这个看懂了就会用VISP了，很简单相关推荐
- 视觉伺服控制工具Visual Servoing Platform---VISP（6）----基于4个平面点的姿态估计
本教程重点介绍平面或非平面点的姿势估计.从它们在图像平面中的二维坐标以及在对象坐标系中指定的相应三维坐标,ViSP能够估计相机和对象坐标系之间的相对姿势.此姿势作为齐次矩阵cMo返回.请注意,要估计姿 ...
- 视觉伺服控制工具Visual Servoing Platform---VISP（2）----使用ViSP滤波图像。
在本教程中,您将学习如何使用vpImageFilter类中实现的ViSP过滤函数. #include <visp/vpDisplayD3D.h> #include <visp/vpD ...
- 视觉伺服控制工具Visual Servoing Platform---VISP（4）----目标检测与跟踪
使用ViSP,您可以使用vpDot或vpDot2类跟踪blob. #include <visp/vp1394CMUGrabber.h> #include <visp/vp1394Tw ...
- 视觉伺服控制完整解析
视觉伺服控制完整解析 视觉伺服控制简介 相关符号及概念的说明 坐标变换 刚体运动 相机模型 视觉伺服控制理论 基于位置的视觉伺服控制 基于图像的视觉伺服控制 参考文献 视觉伺服控制简介 视觉伺服控制( ...
- Visp_ros学习笔记（二）：在Gazebo环境下实现Pionner3dx移动机器人视觉伺服仿真
开发环境:Unbuntu 18.04 LTS + ROS Melodic + ViSP 3.3.1 本文主要介绍了如何实现Pionner3dx移动机器人视觉伺服仿真,仿真环境是ROS+Gazebo ...
- matlab相机标定_【显微视界】基于视觉伺服的工业机器人系统研究（摄像机标定、手眼标定、目标单目定位）...
今日光电 有人说,20世纪是电的世纪,21世纪是光的世纪:知光解电,再小的个体都可以被赋能.欢迎来到今日光电! ----与智者为伍 为创新赋能---- 标定技术 常见的机器人视觉伺服中要 ...
- scare机器人如何手眼标定_基于视觉伺服的工业机器人系统研究（摄像机标定、手眼标定、目标单目定位）...
击上方"新机器视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达 标定技术 常见的机器人视觉伺服中要实现像素坐标与实际坐标的转换,首先 ...
- 视觉伺服研究学习——2021年10月
一.基础学习课程 深度学习 计算机视觉 机器学习 约定: 黄色高亮表示关键词,不认识的知识点: 绿色下划线:重要的思想观点,精髓的理解. 二.论文学习 1.室内动态视觉SLAM算法研究 ...
- 机械手基础知识（3）之基于图像的视觉伺服与基于位置的视觉伺服
** 基于图像的视觉伺服与基于位置的视觉伺服 ** 有关于机械手位姿问题的理解,可以去看我关于机械手正逆运动学分析的文章 首先说,基于图像的视觉伺服与基于位置的视觉伺服主要表现形式在于图像雅可比矩阵的 ...
最新文章
- tidb mysql hbase_HBase/TiDB都在用的数据结构：LSM Tree，不得了解一下？
- Flutter 基础布局Widgets之Baseline、AspectRatio详解
- 整体管理6个过程及相关重点
- Head First Python学习笔记4——处理数据
- 谷歌地图添加点击事件 Google Maps API V3: Add click event listener to all (multiple) marker
- 在想的事情......
- 项目移植过程中报：“Project facet Java version 1.7 is not supported.” 错误
- Swiper 触屏滑动切换
- 从抓包的角度分析connect()函数的连接过程
- oracle数据库报01033,oracle数据库报ORA-01033错误
- CodeSmith链接Oracle、MySQL数据库
- LANP 配置文件
- ab 与 abs 测试详解
- 计算机系统关机后自动重启,深度技术win7系统电脑关机后又自动开机如何解决【图文】...
- LaTeX小白必备技能--用.bib文件自动生成参考文献
- JSP页面请求和响应
- 鸿蒙系统宣传标语,有多项新功能加持，搭载鸿蒙系统的华为手表WATCH 3来了！...
- 一个渗透自学者的自述
- 药品名自动归类机器人
- 适合初学Altium Designer的教学视频