Visual Servoing Platform  version 3.6.1 under development (2024-11-11)
servoMomentImage.cpp

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 https://visp.inria.fr 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 visp@inria.fr
*
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
* Example of visual servoing with moments using an image as object
* container
*/
#define PRINT_CONDITION_NUMBER
#include <iostream>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpConfig.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;
return EXIT_SUCCESS;
}
#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;
return EXIT_SUCCESS;
}
#else
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
#ifndef DOXYGEN_SHOULD_SKIP_THIS
class servoMoment
{
public:
servoMoment()
: m_width(640), m_height(480), m_cMo(), m_cdMo(), m_robot(), m_Iint(m_height, m_width, vpRGBa(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, vpRGBa(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)
{ }
~servoMoment()
{
#ifdef VISP_HAVE_DISPLAY
if (m_displayInt) {
delete m_displayInt;
}
#endif
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.setCameraPosition(m_cMo);
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[i].resize(3);
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.setCameraPosition(m_cdMo);
imsim_start.getImage(m_start_img, m_cam);
m_imsim.setInterpolationType(vpImageSimulator::BILINEAR_INTERPOLATION);
m_imsim.init(tmp_img, X);
m_imsim.setCameraPosition(m_cMo);
m_imsim.getImage(m_src_img, m_cam);
m_src.fromImage(m_src_img, 128, m_cam);
m_imsim.setCameraPosition(m_cdMo);
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);
pl.changeFrame(m_cMo);
planeToABC(pl, A, B, C);
pl.setABCD(0, 0, 1.0, 0);
pl.changeFrame(m_cdMo);
planeToABC(pl, Ad, Bd, Cd);
// extracting initial position (actually we only care about Zdst)
m_cdMo.extract(vec);
// 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_moments->updateAll(m_src);
m_momentsDes->updateAll(m_dst);
m_featureMoments->updateAll(A, B, C);
m_featureMomentsDes->updateAll(Ad, Bd, Cd);
// setup the interaction type
m_task.setInteractionMatrixType(m_interaction_type);
m_task.addFeature(m_featureMoments->getFeatureGravityNormalized(),
m_featureMomentsDes->getFeatureGravityNormalized());
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_task.setLambda(1.);
}
{
m_cMo = cMo; // init source matrix
m_cdMo = cdMo; // init destination matrix
m_interaction_type = vpServo::CURRENT; // use interaction matrix for current position
#ifdef VISP_HAVE_DISPLAY
// 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;
#endif
m_displayInt->init(m_Iint, 50, 50, "Visual servoing with moments");
#endif
paramRobot(); // set up robot parameters
m_task.setServo(vpServo::EYEINHAND_CAMERA);
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;
m_task.print();
unsigned int iter = 0;
vpHomogeneousMatrix wMo; // Set to identity
vpHomogeneousMatrix wMc; // Camera position in the world frame
wMc = wMo * m_cMo.inverse();
m_robot.setPosition(wMc);
double sampling_time = 0.010; // Sampling period in seconds
m_robot.setSamplingTime(sampling_time);
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);
pl.changeFrame(m_cMo);
planeToABC(pl, A, B, C);
// track points, draw points and add refresh our object
refreshScene(obj);
// this is the most important thing to do: update our moments
m_moments->updateAll(obj);
// 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_imsim.setCameraPosition(m_cMo);
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();
#if defined(PRINT_CONDITION_NUMBER)
/*
* 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;
#endif
vpDisplay::displayText(m_Iint, 20, 20, "Click to stop visual servo...", vpColor::red);
if (vpDisplay::getClick(m_Iint, false)) {
break;
}
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");
}
protected:
// 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);
servo.execute(1500);
return EXIT_SUCCESS;
}
catch (const vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
}
#endif
Type getMinValue() const
Definition: vpArray2D.h:1113
Type getMaxValue() const
Definition: vpArray2D.h:1130
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
double sumSquare() const
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
static const vpColor red
Definition: vpColor.h:217
static const vpColor cyan
Definition: vpColor.h:226
static const vpColor orange
Definition: vpColor.h:227
static const vpColor blue
Definition: vpColor.h:223
static const vpColor purple
Definition: vpColor.h:228
static const vpColor green
Definition: vpColor.h:220
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Definition: vpDisplayD3D.h:106
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:130
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:133
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...
Class that defines generic functionalities for display.
Definition: vpDisplay.h:178
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:60
@ divideByZeroError
Division by zero.
Definition: vpException.h:70
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:129
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
void svd(vpColVector &w, vpMatrix &V)
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:57
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:391
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:112
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition: vpPlot.cpp:203
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:96
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
Definition: vpPlot.cpp:552
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
Definition: vpPlot.cpp:270
void setColor(unsigned int graphNum, unsigned int curveNum, vpColor color)
Definition: vpPlot.cpp:246
void setTitle(unsigned int graphNum, const std::string &title)
Definition: vpPlot.cpp:510
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:203
Definition: vpRGBa.h:65
@ CAMERA_FRAME
Definition: vpRobot.h:84
@ EYEINHAND_CAMERA
Definition: vpServo.h:161
vpServoIteractionMatrixType
Definition: vpServo.h:196
@ CURRENT
Definition: vpServo.h:202
Class that defines the simplest robot: a free flying camera.
Class that consider the case of a translation vector.
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()