45#define PRINT_CONDITION_NUMBER
48#include <visp3/core/vpCameraParameters.h>
49#include <visp3/core/vpHomogeneousMatrix.h>
50#include <visp3/core/vpIoTools.h>
51#include <visp3/core/vpMath.h>
52#include <visp3/core/vpMomentCommon.h>
53#include <visp3/core/vpMomentDatabase.h>
54#include <visp3/core/vpMomentObject.h>
55#include <visp3/core/vpPlane.h>
56#include <visp3/core/vpPoseVector.h>
57#include <visp3/gui/vpDisplayGDI.h>
58#include <visp3/gui/vpDisplayGTK.h>
59#include <visp3/gui/vpDisplayOpenCV.h>
60#include <visp3/gui/vpDisplayX.h>
61#include <visp3/gui/vpPlot.h>
62#include <visp3/robot/vpImageSimulator.h>
63#include <visp3/robot/vpSimulatorCamera.h>
64#include <visp3/visual_features/vpFeatureBuilder.h>
65#include <visp3/visual_features/vpFeatureMomentCommon.h>
66#include <visp3/visual_features/vpFeaturePoint.h>
67#include <visp3/vs/vpServo.h>
69#if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD)
73 std::cout <<
"Can't run this example since vpSimulatorAfma6 capability is "
76 std::cout <<
"You should install pthread third-party library." << std::endl;
80#elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && \
81 !defined(VISP_HAVE_GTK)
84 std::cout <<
"Can't run this example since no display capability is available." << std::endl;
85 std::cout <<
"You should install one of the following third-party library: "
86 "X11, OpenCV, GDI, GTK."
92#ifndef DOXYGEN_SHOULD_SKIP_THIS
97 : m_width(640), m_height(480), m_cMo(), m_cdMo(), m_robot(), m_Iint(m_height, m_width, 0), m_task(), m_cam(),
98 m_error(0), m_imsim(), m_cur_img(m_height, m_width, 0), m_src_img(m_height, m_width, 0),
99 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),
100 m_moments(NULL), m_momentsDes(NULL), m_featureMoments(NULL), m_featureMomentsDes(NULL), m_displayInt(NULL)
105#ifdef VISP_HAVE_DISPLAY
112 delete m_featureMoments;
113 delete m_featureMomentsDes;
117 void paramRobot() { m_cam =
vpCameraParameters(600, 600, m_width / 2., m_height / 2.); }
123 m_imsim.setCameraPosition(m_cMo);
124 m_imsim.getImage(m_cur_img, m_cam);
132 for (
int i = 0; i < 4; i++)
155 imsim_start.
init(tmp_start_img, X);
157 imsim_start.
getImage(m_start_img, m_cam);
160 m_imsim.init(tmp_img, X);
162 m_imsim.setCameraPosition(m_cMo);
163 m_imsim.getImage(m_src_img, m_cam);
166 m_src.fromImage(m_src_img, 128, m_cam);
169 m_imsim.setCameraPosition(m_cdMo);
170 m_imsim.getImage(m_dst_img, m_cam);
171 m_dst.fromImage(m_dst_img, 128, m_cam);
190 planeToABC(pl, A, B, C);
194 planeToABC(pl, Ad, Bd, Cd);
212 m_moments->updateAll(m_src);
213 m_momentsDes->updateAll(m_dst);
215 m_featureMoments->updateAll(A, B, C);
216 m_featureMomentsDes->updateAll(Ad, Bd, Cd);
219 m_task.setInteractionMatrixType(m_interaction_type);
222 m_task.addFeature(m_featureMoments->getFeatureGravityNormalized(),
223 m_featureMomentsDes->getFeatureGravityNormalized());
224 m_task.addFeature(m_featureMoments->getFeatureAn(), m_featureMomentsDes->getFeatureAn());
226 m_task.addFeature(m_featureMoments->getFeatureCInvariant(), m_featureMomentsDes->getFeatureCInvariant(),
227 (1 << 10) | (1 << 11));
228 m_task.addFeature(m_featureMoments->getFeatureAlpha(), m_featureMomentsDes->getFeatureAlpha());
230 m_task.setLambda(1.);
240#ifdef VISP_HAVE_DISPLAY
242#if defined VISP_HAVE_X11
244#elif defined VISP_HAVE_OPENCV
246#elif defined VISP_HAVE_GDI
248#elif defined VISP_HAVE_D3D9
250#elif defined VISP_HAVE_GTK
253 m_displayInt->
init(m_Iint, 50, 50,
"Visual servoing with moments");
264 void execute(
unsigned int nbIter)
267 init_visp_plot(ViSP_plot);
274 std::cout <<
"Display task information " << std::endl;
279 unsigned int iter = 0;
284 m_robot.setPosition(wMc);
285 double sampling_time = 0.010;
286 m_robot.setSamplingTime(sampling_time);
289 while (iter++ < nbIter) {
294 wMc = m_robot.getPosition();
301 planeToABC(pl, A, B, C);
306 m_moments->updateAll(obj);
309 m_featureMoments->updateAll(A, B, C);
311 m_imsim.setCameraPosition(m_cMo);
313 m_Iint = m_start_img;
315 m_imsim.getImage(m_Iint, m_cam);
323 v = m_task.computeControlLaw();
325 std::cout <<
" || s - s* || = " << m_task.error.
sumSquare() << std::endl;
329 ViSP_plot.
plot(0, iter, v);
331 ViSP_plot.
plot(2, iter, m_task.getError());
333 m_error = (m_task.getError()).sumSquare();
335#if defined(PRINT_CONDITION_NUMBER)
342 Linteraction.
svd(singularvals, tmpry);
344 std::cout <<
"Condition Number: " << condno << std::endl;
354 m_imsim.getImage(m_Iint, m_cam);
363 double error() {
return m_error; }
365 void planeToABC(
vpPlane &pl,
double &A,
double &B,
double &C)
367 if (fabs(pl.
getD()) < std::numeric_limits<double>::epsilon()) {
368 std::cout <<
"Invalid position:" << std::endl;
369 std::cout << m_cMo << std::endl;
370 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
378 void init_visp_plot(
vpPlot &ViSP_plot)
384 const unsigned int NbGraphs = 3;
385 const unsigned int NbCurves_in_graph[NbGraphs] = {6, 6, 6};
387 ViSP_plot.
init(NbGraphs, 800, 800, 100 +
static_cast<int>(m_width), 50,
"Visual Servoing results...");
392 for (
unsigned int p = 0; p < NbGraphs; p++) {
393 ViSP_plot.
initGraph(p, NbCurves_in_graph[p]);
394 for (
unsigned int c = 0; c < NbCurves_in_graph[p]; c++)
395 ViSP_plot.
setColor(p, c, Colors[c]);
398 ViSP_plot.
setTitle(0,
"Robot velocities");
406 ViSP_plot.
setTitle(1,
"Camera pose cMo");
414 ViSP_plot.
setTitle(2,
"Error in visual features: ");
425 unsigned int m_width;
426 unsigned int m_height;
469 servo.init(cMo, cdMo);
474 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 functionnalities.
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 functionnalities 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 emited 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)
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 int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()