42#define PRINT_CONDITION_NUMBER
45#include <visp3/core/vpCameraParameters.h>
46#include <visp3/core/vpHomogeneousMatrix.h>
47#include <visp3/core/vpIoTools.h>
48#include <visp3/core/vpMath.h>
49#include <visp3/core/vpMomentCommon.h>
50#include <visp3/core/vpMomentDatabase.h>
51#include <visp3/core/vpMomentObject.h>
52#include <visp3/core/vpPlane.h>
53#include <visp3/core/vpPoseVector.h>
54#include <visp3/gui/vpDisplayGDI.h>
55#include <visp3/gui/vpDisplayGTK.h>
56#include <visp3/gui/vpDisplayOpenCV.h>
57#include <visp3/gui/vpDisplayX.h>
58#include <visp3/gui/vpPlot.h>
59#include <visp3/robot/vpImageSimulator.h>
60#include <visp3/robot/vpSimulatorCamera.h>
61#include <visp3/visual_features/vpFeatureBuilder.h>
62#include <visp3/visual_features/vpFeatureMomentCommon.h>
63#include <visp3/visual_features/vpFeaturePoint.h>
64#include <visp3/vs/vpServo.h>
66#if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD)
70 std::cout <<
"Can't run this example since vpSimulatorAfma6 capability is "
73 std::cout <<
"You should install pthread third-party library." << std::endl;
77#elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && \
78 !defined(VISP_HAVE_GTK)
81 std::cout <<
"Can't run this example since no display capability is available." << std::endl;
82 std::cout <<
"You should install one of the following third-party library: "
83 "X11, OpenCV, GDI, GTK."
89#ifndef DOXYGEN_SHOULD_SKIP_THIS
94 : m_width(640), m_height(480), m_cMo(), m_cdMo(), m_robot(), m_Iint(m_height, m_width, 0), m_task(), m_cam(),
95 m_error(0), m_imsim(), m_cur_img(m_height, m_width, 0), m_src_img(m_height, m_width, 0),
96 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),
97 m_moments(NULL), m_momentsDes(NULL), m_featureMoments(NULL), m_featureMomentsDes(NULL), m_displayInt(NULL)
101#ifdef VISP_HAVE_DISPLAY
108 delete m_featureMoments;
109 delete m_featureMomentsDes;
113 void paramRobot() { m_cam =
vpCameraParameters(600, 600, m_width / 2., m_height / 2.); }
119 m_imsim.setCameraPosition(m_cMo);
120 m_imsim.getImage(m_cur_img, m_cam);
128 for (
int i = 0; i < 4; i++)
151 imsim_start.
init(tmp_start_img, X);
153 imsim_start.
getImage(m_start_img, m_cam);
156 m_imsim.init(tmp_img, X);
158 m_imsim.setCameraPosition(m_cMo);
159 m_imsim.getImage(m_src_img, m_cam);
162 m_src.fromImage(m_src_img, 128, m_cam);
165 m_imsim.setCameraPosition(m_cdMo);
166 m_imsim.getImage(m_dst_img, m_cam);
167 m_dst.fromImage(m_dst_img, 128, m_cam);
186 planeToABC(pl, A, B, C);
190 planeToABC(pl, Ad, Bd, Cd);
208 m_moments->updateAll(m_src);
209 m_momentsDes->updateAll(m_dst);
211 m_featureMoments->updateAll(A, B, C);
212 m_featureMomentsDes->updateAll(Ad, Bd, Cd);
215 m_task.setInteractionMatrixType(m_interaction_type);
218 m_task.addFeature(m_featureMoments->getFeatureGravityNormalized(),
219 m_featureMomentsDes->getFeatureGravityNormalized());
220 m_task.addFeature(m_featureMoments->getFeatureAn(), m_featureMomentsDes->getFeatureAn());
222 m_task.addFeature(m_featureMoments->getFeatureCInvariant(), m_featureMomentsDes->getFeatureCInvariant(),
223 (1 << 10) | (1 << 11));
224 m_task.addFeature(m_featureMoments->getFeatureAlpha(), m_featureMomentsDes->getFeatureAlpha());
226 m_task.setLambda(1.);
236#ifdef VISP_HAVE_DISPLAY
238#if defined(VISP_HAVE_X11)
240#elif defined(HAVE_OPENCV_HIGHGUI)
242#elif defined(VISP_HAVE_GDI)
244#elif defined(VISP_HAVE_D3D9)
246#elif defined(VISP_HAVE_GTK)
249 m_displayInt->
init(m_Iint, 50, 50,
"Visual servoing with moments");
260 void execute(
unsigned int nbIter)
263 init_visp_plot(ViSP_plot);
270 std::cout <<
"Display task information " << std::endl;
275 unsigned int iter = 0;
280 m_robot.setPosition(wMc);
281 double sampling_time = 0.010;
282 m_robot.setSamplingTime(sampling_time);
285 while (iter++ < nbIter) {
290 wMc = m_robot.getPosition();
297 planeToABC(pl, A, B, C);
302 m_moments->updateAll(obj);
305 m_featureMoments->updateAll(A, B, C);
307 m_imsim.setCameraPosition(m_cMo);
309 m_Iint = m_start_img;
311 m_imsim.getImage(m_Iint, m_cam);
319 v = m_task.computeControlLaw();
321 std::cout <<
" || s - s* || = " << m_task.error.
sumSquare() << std::endl;
325 ViSP_plot.
plot(0, iter, v);
327 ViSP_plot.
plot(2, iter, m_task.getError());
329 m_error = (m_task.getError()).sumSquare();
331#if defined(PRINT_CONDITION_NUMBER)
338 Linteraction.
svd(singularvals, tmpry);
340 std::cout <<
"Condition Number: " << condno << std::endl;
350 m_imsim.getImage(m_Iint, m_cam);
359 double error() {
return m_error; }
361 void planeToABC(
vpPlane &pl,
double &A,
double &B,
double &C)
363 if (fabs(pl.
getD()) < std::numeric_limits<double>::epsilon()) {
364 std::cout <<
"Invalid position:" << std::endl;
365 std::cout << m_cMo << std::endl;
366 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
374 void init_visp_plot(
vpPlot &ViSP_plot)
380 const unsigned int NbGraphs = 3;
381 const unsigned int NbCurves_in_graph[NbGraphs] = { 6, 6, 6 };
383 ViSP_plot.
init(NbGraphs, 800, 800, 100 +
static_cast<int>(m_width), 50,
"Visual Servoing results...");
388 for (
unsigned int p = 0; p < NbGraphs; p++) {
389 ViSP_plot.
initGraph(p, NbCurves_in_graph[p]);
390 for (
unsigned int c = 0; c < NbCurves_in_graph[p]; c++)
391 ViSP_plot.
setColor(p, c, Colors[c]);
394 ViSP_plot.
setTitle(0,
"Robot velocities");
402 ViSP_plot.
setTitle(1,
"Camera pose cMo");
410 ViSP_plot.
setTitle(2,
"Error in visual features: ");
421 unsigned int m_width;
422 unsigned int m_height;
465 servo.init(cMo, cdMo);
471 std::cout <<
"Catch an exception: " << e << std::endl;
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Class to define RGB colors available for display functionalities.
static const vpColor cyan
static const vpColor orange
static const vpColor blue
static const vpColor purple
static const vpColor green
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...
Class that defines generic functionalities for display.
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.
@ divideByZeroError
Division by zero.
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.
static double rad(double deg)
Implementation of a matrix and operations on matrices.
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.
void changeFrame(const vpHomogeneousMatrix &cMo)
void setABCD(double a, double b, double c, double d)
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
void initGraph(unsigned int graphNum, unsigned int curveNbr)
void init(unsigned int nbGraph, unsigned int height=700, unsigned int width=700, int x=-1, int y=-1, const std::string &title="")
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
void setColor(unsigned int graphNum, unsigned int curveNum, vpColor color)
void setTitle(unsigned int graphNum, const std::string &title)
Implementation of a pose vector and operations on poses.
vpServoIteractionMatrixType
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)