Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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/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(_WIN32) && !defined(VISP_HAVE_PTHREAD)
// Robot simulator used in this example is not available
int main()
{
std::cout << "Can't run this example since vpSimulatorAfma6 capability is "
"not available."
<< std::endl;
std::cout << "You should install pthread third-party library." << std::endl;
return EXIT_SUCCESS;
}
// No display available
#elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && \
!defined(VISP_HAVE_GTK)
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;
}
#else
#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, 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(NULL), m_momentsDes(NULL), m_featureMoments(NULL), m_featureMomentsDes(NULL), m_displayInt(NULL)
{ }
~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:1006
Type getMaxValue() const
Definition vpArray2D.h:1023
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
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....
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
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:132
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)
Definition of the vpImage class member functions.
Definition vpImage.h:135
static double rad(double deg)
Definition vpMath.h:116
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
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:54
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition vpPlane.cpp:361
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:113
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.
@ CAMERA_FRAME
Definition vpRobot.h:80
@ EYEINHAND_CAMERA
Definition vpServo.h:151
vpServoIteractionMatrixType
Definition vpServo.h:178
@ CURRENT
Definition vpServo.h:179
Class that defines the simplest robot: a free flying camera.
Class that consider the case of a translation vector.
VISP_EXPORT double measureTimeMs()
VISP_EXPORT int wait(double t0, double t)