Visual Servoing Platform  version 3.6.1 under development (2024-05-21)

Example of moment-based visual servoing with Images

* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2023 by Inria. All rights reserved.
* This software is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
* For using ViSP with software that can not be combined with the GNU
* GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
* See for more information.
* This software was developed at:
* Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
* If you have questions regarding the use of this file, please contact
* Inria at
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* Description:
* Example of visual servoing with moments using an image as object
* container
#include <iostream>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpMomentCommon.h>
#include <visp3/core/vpMomentDatabase.h>
#include <visp3/core/vpMomentObject.h>
#include <visp3/core/vpPlane.h>
#include <visp3/core/vpPoseVector.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpPlot.h>
#include <visp3/robot/vpImageSimulator.h>
#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureMomentCommon.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
#if !defined(VISP_HAVE_DISPLAY)
int main()
std::cout << "Can't run this example since no display capability is available." << std::endl;
std::cout << "You should install one of the following third-party library: X11, OpenCV, GDI, GTK." << std::endl;
#elif !defined(VISP_HAVE_THREADS)
int main()
std::cout << "Can't run this example since multi-threading capability is not available." << std::endl;
std::cout << "You should maybe enable cxx11 standard." << std::endl;
class servoMoment
: m_width(640), m_height(480), m_cMo(), m_cdMo(), m_robot(), m_Iint(m_height, m_width, 0), m_task(), m_cam(),
m_error(0), m_imsim(), m_cur_img(m_height, m_width, 0), m_src_img(m_height, m_width, 0),
m_dst_img(m_height, m_width, 0), m_start_img(m_height, m_width, 0), m_interaction_type(), m_src(6), m_dst(6),
m_moments(nullptr), m_momentsDes(nullptr), m_featureMoments(nullptr), m_featureMomentsDes(nullptr), m_displayInt(nullptr)
{ }
if (m_displayInt) {
delete m_displayInt;
delete m_moments;
delete m_momentsDes;
delete m_featureMoments;
delete m_featureMomentsDes;
// setup robot parameters
void paramRobot() { m_cam = vpCameraParameters(600, 600, m_width / 2., m_height / 2.); }
// update moment objects and interface
void refreshScene(vpMomentObject &obj)
m_cur_img = 0;
m_imsim.getImage(m_cur_img, m_cam);
obj.fromImage(m_cur_img, 128, m_cam);
// initialize scene in the interface
void initScene()
for (int i = 0; i < 4; i++)
X[0][0] = -0.2;
X[0][1] = -0.1;
X[0][2] = 0;
X[1][0] = 0.2;
X[1][1] = -0.1;
X[1][2] = 0;
X[2][0] = 0.2;
X[2][1] = 0.1;
X[2][2] = 0;
X[3][0] = -0.2;
X[3][1] = 0.1;
X[3][2] = 0;
// init source and destination images
vpImage<unsigned char> tmp_img(m_height, m_width, 255);
vpImage<vpRGBa> tmp_start_img(m_height, m_width, vpRGBa(255, 0, 0));
vpImageSimulator imsim_start;
imsim_start.init(tmp_start_img, X);
imsim_start.getImage(m_start_img, m_cam);
m_imsim.init(tmp_img, X);
m_imsim.getImage(m_src_img, m_cam);
m_src.fromImage(m_src_img, 128, m_cam);
m_imsim.getImage(m_dst_img, m_cam);
m_dst.fromImage(m_dst_img, 128, m_cam);
// initialize the moment features
void initFeatures()
// A,B,C parameters of source and destination plane
double A;
double B;
double C;
double Ad;
double Bd;
double Cd;
// init main object: using moments up to order 5
// Initializing values from regular plane (with ax+by+cz=d convention)
vpPlane pl;
pl.setABCD(0, 0, 1.0, 0);
planeToABC(pl, A, B, C);
pl.setABCD(0, 0, 1.0, 0);
planeToABC(pl, Ad, Bd, Cd);
// extracting initial position (actually we only care about Zdst)
// don't need to be specific, vpMomentCommon automatically loads
// Xg,Yg,An,Ci,Cj,Alpha moments
vpMomentCommon::getAlpha(m_dst), vec[2], true);
vpMomentCommon::getAlpha(m_dst), vec[2], true);
// same thing with common features
m_featureMoments = new vpFeatureMomentCommon(*m_moments);
m_featureMomentsDes = new vpFeatureMomentCommon(*m_momentsDes);
m_featureMoments->updateAll(A, B, C);
m_featureMomentsDes->updateAll(Ad, Bd, Cd);
// setup the interaction type
m_task.addFeature(m_featureMoments->getFeatureAn(), m_featureMomentsDes->getFeatureAn());
// the moments are different in case of a symmetric object
m_task.addFeature(m_featureMoments->getFeatureCInvariant(), m_featureMomentsDes->getFeatureCInvariant(),
(1 << 10) | (1 << 11));
m_task.addFeature(m_featureMoments->getFeatureAlpha(), m_featureMomentsDes->getFeatureAlpha());
m_cMo = cMo; // init source matrix
m_cdMo = cdMo; // init destination matrix
m_interaction_type = vpServo::CURRENT; // use interaction matrix for current position
// init the right display
#if defined(VISP_HAVE_X11)
m_displayInt = new vpDisplayX;
#elif defined(HAVE_OPENCV_HIGHGUI)
m_displayInt = new vpDisplayOpenCV;
#elif defined(VISP_HAVE_GDI)
m_displayInt = new vpDisplayGDI;
#elif defined(VISP_HAVE_D3D9)
m_displayInt = new vpDisplayD3D;
#elif defined(VISP_HAVE_GTK)
m_displayInt = new vpDisplayGTK;
m_displayInt->init(m_Iint, 50, 50, "Visual servoing with moments");
paramRobot(); // set up robot parameters
initScene(); // initialize graphical scene (for interface)
initFeatures(); // initialize moment features
// launch the simulation
void execute(unsigned int nbIter)
vpPlot ViSP_plot;
init_visp_plot(ViSP_plot); // Initialize plot object
// init main object: using moments up to order 6
// setting object type (disrete, continuous[form polygon])
std::cout << "Display task information " << std::endl;
unsigned int iter = 0;
vpHomogeneousMatrix wMo; // Set to identity
vpHomogeneousMatrix wMc; // Camera position in the world frame
wMc = wMo * m_cMo.inverse();
double sampling_time = 0.010; // Sampling period in seconds
while (iter++ < nbIter) {
double t = vpTime::measureTimeMs();
// get the cMo
wMc = m_robot.getPosition();
m_cMo = wMc.inverse() * wMo;
// setup the plane in A,B,C style
vpPlane pl;
double A, B, C;
pl.setABCD(0, 0, 1.0, 0);
planeToABC(pl, A, B, C);
// track points, draw points and add refresh our object
// this is the most important thing to do: update our moments
// and update our features. Do it in that order. Features need to use the
// information computed by moments
m_featureMoments->updateAll(A, B, C);
// some graphics again
m_Iint = m_start_img;
m_imsim.getImage(m_Iint, m_cam);
if (iter == 1) {
vpDisplay::displayText(m_Iint, 20, 20, "Click to start servoing", vpColor::red);
v = m_task.computeControlLaw();
std::cout << " || s - s* || = " << m_task.error.sumSquare() << std::endl;
m_robot.setVelocity(vpRobot::CAMERA_FRAME, v);
ViSP_plot.plot(0, iter, v);
ViSP_plot.plot(1, iter, vpPoseVector(m_cMo)); // Plot the velocities
ViSP_plot.plot(2, iter, m_task.getError()); // cMo as translations and theta_u
m_error = (m_task.getError()).sumSquare();
* Condition number of interaction matrix
vpMatrix Linteraction = m_task.L;
vpMatrix tmpry, U;
vpColVector singularvals;
Linteraction.svd(singularvals, tmpry);
double condno = static_cast<double>(singularvals.getMaxValue() / singularvals.getMinValue());
std::cout << "Condition Number: " << condno << std::endl;
vpDisplay::displayText(m_Iint, 20, 20, "Click to stop visual servo...", vpColor::red);
if (vpDisplay::getClick(m_Iint, false)) {
vpTime::wait(t, sampling_time * 1000); // Wait 10 ms
m_imsim.getImage(m_Iint, m_cam);
vpDisplay::displayText(m_Iint, 20, 20, "Click to quit...", vpColor::red);
void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type) { m_interaction_type = type; }
double error() { return m_error; }
void planeToABC(vpPlane &pl, double &A, double &B, double &C)
if (fabs(pl.getD()) < std::numeric_limits<double>::epsilon()) {
std::cout << "Invalid position:" << std::endl;
std::cout << m_cMo << std::endl;
std::cout << "Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
throw vpException(vpException::divideByZeroError, "invalid position!");
A = -pl.getA() / pl.getD();
B = -pl.getB() / pl.getD();
C = -pl.getC() / pl.getD();
void init_visp_plot(vpPlot &ViSP_plot)
/* -------------------------------------
* Initialize ViSP Plotting
* -------------------------------------
const unsigned int NbGraphs = 3; // No. of graphs
const unsigned int NbCurves_in_graph[NbGraphs] = { 6, 6, 6 }; // Curves in each graph
ViSP_plot.init(NbGraphs, 800, 800, 100 + static_cast<int>(m_width), 50, "Visual Servoing results...");
vpColor Colors[6] = {// Colour for s1, s2, s3, in 1st plot
for (unsigned int p = 0; p < NbGraphs; p++) {
ViSP_plot.initGraph(p, NbCurves_in_graph[p]);
for (unsigned int c = 0; c < NbCurves_in_graph[p]; c++)
ViSP_plot.setColor(p, c, Colors[c]);
ViSP_plot.setTitle(0, "Robot velocities");
ViSP_plot.setLegend(0, 0, "v_x");
ViSP_plot.setLegend(0, 1, "v_y");
ViSP_plot.setLegend(0, 2, "v_z");
ViSP_plot.setLegend(0, 3, "w_x");
ViSP_plot.setLegend(0, 4, "w_y");
ViSP_plot.setLegend(0, 5, "w_z");
ViSP_plot.setTitle(1, "Camera pose cMo");
ViSP_plot.setLegend(1, 0, "tx");
ViSP_plot.setLegend(1, 1, "ty");
ViSP_plot.setLegend(1, 2, "tz");
ViSP_plot.setLegend(1, 3, "tu_x");
ViSP_plot.setLegend(1, 4, "tu_y");
ViSP_plot.setLegend(1, 5, "tu_z");
ViSP_plot.setTitle(2, "Error in visual features: ");
ViSP_plot.setLegend(2, 0, "x_n");
ViSP_plot.setLegend(2, 1, "y_n");
ViSP_plot.setLegend(2, 2, "a_n");
ViSP_plot.setLegend(2, 3, "sx");
ViSP_plot.setLegend(2, 4, "sy");
ViSP_plot.setLegend(2, 5, "alpha");
// start and destination positioning matrices
unsigned int m_width;
unsigned int m_height;
// start and destination positioning matrices
vpSimulatorCamera m_robot; // robot used in this simulation
vpImage<vpRGBa> m_Iint; // internal image used for interface display
vpServo m_task; // servoing task
vpCameraParameters m_cam; // robot camera parameters
double m_error; // current error
vpImageSimulator m_imsim; // image simulator used to simulate the perspective-projection camera
// several images used in the simulation
vpImage<vpRGBa> m_start_img;
vpServo::vpServoIteractionMatrixType m_interaction_type; // current or desired
// source and destination objects for moment manipulation
// moment sets and their corresponding features
vpMomentCommon *m_moments;
vpMomentCommon *m_momentsDes;
vpFeatureMomentCommon *m_featureMoments;
vpFeatureMomentCommon *m_featureMomentsDes;
vpDisplay *m_displayInt;
#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
int main()
try {
// intial pose
vpHomogeneousMatrix cMo(-0.1, -0.1, 1.5, -vpMath::rad(20), -vpMath::rad(20), -vpMath::rad(30));
// Desired pose
servoMoment servo;
// init the simulation
servo.init(cMo, cdMo);
catch (const vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
Type getMinValue() const
Definition: vpArray2D.h:1088
Type getMaxValue() const
Definition: vpArray2D.h:1105
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
double sumSquare() const
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
static const vpColor red
Definition: vpColor.h:211
static const vpColor cyan
Definition: vpColor.h:220
static const vpColor orange
Definition: vpColor.h:221
static const vpColor blue
Definition: vpColor.h:217
static const vpColor purple
Definition: vpColor.h:222
static const vpColor green
Definition: vpColor.h:214
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Definition: vpDisplayD3D.h:101
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:128
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="") vp_override
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:128
Class that defines generic functionalities for display.
Definition: vpDisplay.h:173
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ divideByZeroError
Division by zero.
Definition: vpException.h:82
This class allows to access common vpFeatureMoments in a pre-filled database.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Class which enables to project an image in the 3D space and get the view of a virtual camera.
void getImage(vpImage< unsigned char > &I, const vpCameraParameters &cam)
void init(const vpImage< unsigned char > &I, vpColVector *X)
void setInterpolationType(const vpInterpolationType interplt)
void setCameraPosition(const vpHomogeneousMatrix &cMt)
static double rad(double deg)
Definition: vpMath.h:127
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2132
This class initializes and allows access to commonly used moments.
static std::vector< double > getMu3(vpMomentObject &object)
static double getAlpha(vpMomentObject &object)
static double getSurface(vpMomentObject &object)
Class for generic objects.
void setType(vpObjectType input_type)
void fromImage(const vpImage< unsigned char > &image, unsigned char threshold, const vpCameraParameters &cam)
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:54
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:372
double getD() const
Definition: vpPlane.h:106
double getA() const
Definition: vpPlane.h:100
double getC() const
Definition: vpPlane.h:104
void setABCD(double a, double b, double c, double d)
Definition: vpPlane.h:88
double getB() const
Definition: vpPlane.h:102
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:109
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition: vpPlot.cpp:202
void init(unsigned int nbGraph, unsigned int height=700, unsigned int width=700, int x=-1, int y=-1, const std::string &title="")
Definition: vpPlot.cpp:95
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
Definition: vpPlot.cpp:545
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
Definition: vpPlot.cpp:269
void setColor(unsigned int graphNum, unsigned int curveNum, vpColor color)
Definition: vpPlot.cpp:245
void setTitle(unsigned int graphNum, const std::string &title)
Definition: vpPlot.cpp:503
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:189
Definition: vpRGBa.h:61
Definition: vpRobot.h:82
Definition: vpServo.h:155
Definition: vpServo.h:190
Definition: vpServo.h:196
Class that defines the simplest robot: a free flying camera.
Class that consider the case of a translation vector.
void init(vpImage< unsigned char > &Iinput, vpImage< unsigned char > &IcannyVisp, vpImage< unsigned char > *p_dIx, vpImage< unsigned char > *p_dIy, vpImage< unsigned char > *p_IcannyimgFilter)
Initialize the different displays.
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()