41 #define PRINT_CONDITION_NUMBER
44 #include <visp3/core/vpCameraParameters.h>
45 #include <visp3/core/vpHomogeneousMatrix.h>
46 #include <visp3/core/vpIoTools.h>
47 #include <visp3/core/vpMath.h>
48 #include <visp3/core/vpMomentCommon.h>
49 #include <visp3/core/vpMomentDatabase.h>
50 #include <visp3/core/vpMomentObject.h>
51 #include <visp3/core/vpPlane.h>
52 #include <visp3/core/vpPoseVector.h>
53 #include <visp3/gui/vpDisplayGDI.h>
54 #include <visp3/gui/vpDisplayGTK.h>
55 #include <visp3/gui/vpDisplayOpenCV.h>
56 #include <visp3/gui/vpDisplayX.h>
57 #include <visp3/gui/vpPlot.h>
58 #include <visp3/robot/vpImageSimulator.h>
59 #include <visp3/robot/vpSimulatorCamera.h>
60 #include <visp3/visual_features/vpFeatureBuilder.h>
61 #include <visp3/visual_features/vpFeatureMomentCommon.h>
62 #include <visp3/visual_features/vpFeaturePoint.h>
63 #include <visp3/vs/vpServo.h>
65 #if !defined(VISP_HAVE_DISPLAY)
68 std::cout <<
"Can't run this example since no display capability is available." << std::endl;
69 std::cout <<
"You should install one of the following third-party library: X11, OpenCV, GDI, GTK." << std::endl;
72 #elif !defined(VISP_HAVE_THREADS)
75 std::cout <<
"Can't run this example since multi-threading capability is not available." << std::endl;
76 std::cout <<
"You should maybe enable cxx11 standard." << std::endl;
81 #ifndef DOXYGEN_SHOULD_SKIP_THIS
86 : m_width(640), m_height(480), m_cMo(), m_cdMo(), m_robot(), m_Iint(m_height, m_width, 0), m_task(), m_cam(),
87 m_error(0), m_imsim(), m_cur_img(m_height, m_width, 0), m_src_img(m_height, m_width, 0),
88 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),
89 m_moments(nullptr), m_momentsDes(nullptr), m_featureMoments(nullptr), m_featureMomentsDes(nullptr), m_displayInt(nullptr)
93 #ifdef VISP_HAVE_DISPLAY
100 delete m_featureMoments;
101 delete m_featureMomentsDes;
105 void paramRobot() { m_cam =
vpCameraParameters(600, 600, m_width / 2., m_height / 2.); }
111 m_imsim.setCameraPosition(m_cMo);
112 m_imsim.getImage(m_cur_img, m_cam);
120 for (
int i = 0; i < 4; i++)
143 imsim_start.
init(tmp_start_img, X);
145 imsim_start.
getImage(m_start_img, m_cam);
148 m_imsim.init(tmp_img, X);
150 m_imsim.setCameraPosition(m_cMo);
151 m_imsim.getImage(m_src_img, m_cam);
154 m_src.fromImage(m_src_img, 128, m_cam);
157 m_imsim.setCameraPosition(m_cdMo);
158 m_imsim.getImage(m_dst_img, m_cam);
159 m_dst.fromImage(m_dst_img, 128, m_cam);
178 planeToABC(pl, A, B, C);
182 planeToABC(pl, Ad, Bd, Cd);
200 m_moments->updateAll(m_src);
201 m_momentsDes->updateAll(m_dst);
203 m_featureMoments->updateAll(A, B, C);
204 m_featureMomentsDes->updateAll(Ad, Bd, Cd);
207 m_task.setInteractionMatrixType(m_interaction_type);
210 m_task.addFeature(m_featureMoments->getFeatureGravityNormalized(),
211 m_featureMomentsDes->getFeatureGravityNormalized());
212 m_task.addFeature(m_featureMoments->getFeatureAn(), m_featureMomentsDes->getFeatureAn());
214 m_task.addFeature(m_featureMoments->getFeatureCInvariant(), m_featureMomentsDes->getFeatureCInvariant(),
215 (1 << 10) | (1 << 11));
216 m_task.addFeature(m_featureMoments->getFeatureAlpha(), m_featureMomentsDes->getFeatureAlpha());
218 m_task.setLambda(1.);
228 #ifdef VISP_HAVE_DISPLAY
230 #if defined(VISP_HAVE_X11)
232 #elif defined(HAVE_OPENCV_HIGHGUI)
234 #elif defined(VISP_HAVE_GDI)
236 #elif defined(VISP_HAVE_D3D9)
238 #elif defined(VISP_HAVE_GTK)
241 m_displayInt->
init(m_Iint, 50, 50,
"Visual servoing with moments");
252 void execute(
unsigned int nbIter)
255 init_visp_plot(ViSP_plot);
262 std::cout <<
"Display task information " << std::endl;
267 unsigned int iter = 0;
272 m_robot.setPosition(wMc);
273 double sampling_time = 0.010;
274 m_robot.setSamplingTime(sampling_time);
277 while (iter++ < nbIter) {
282 wMc = m_robot.getPosition();
289 planeToABC(pl, A, B, C);
294 m_moments->updateAll(obj);
297 m_featureMoments->updateAll(A, B, C);
299 m_imsim.setCameraPosition(m_cMo);
301 m_Iint = m_start_img;
303 m_imsim.getImage(m_Iint, m_cam);
311 v = m_task.computeControlLaw();
313 std::cout <<
" || s - s* || = " << m_task.error.
sumSquare() << std::endl;
317 ViSP_plot.
plot(0, iter, v);
319 ViSP_plot.
plot(2, iter, m_task.getError());
321 m_error = (m_task.getError()).sumSquare();
323 #if defined(PRINT_CONDITION_NUMBER)
330 Linteraction.
svd(singularvals, tmpry);
332 std::cout <<
"Condition Number: " << condno << std::endl;
342 m_imsim.getImage(m_Iint, m_cam);
351 double error() {
return m_error; }
353 void planeToABC(
vpPlane &pl,
double &A,
double &B,
double &C)
355 if (fabs(pl.
getD()) < std::numeric_limits<double>::epsilon()) {
356 std::cout <<
"Invalid position:" << std::endl;
357 std::cout << m_cMo << std::endl;
358 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
366 void init_visp_plot(
vpPlot &ViSP_plot)
372 const unsigned int NbGraphs = 3;
373 const unsigned int NbCurves_in_graph[NbGraphs] = { 6, 6, 6 };
375 ViSP_plot.
init(NbGraphs, 800, 800, 100 +
static_cast<int>(m_width), 50,
"Visual Servoing results...");
380 for (
unsigned int p = 0; p < NbGraphs; p++) {
381 ViSP_plot.
initGraph(p, NbCurves_in_graph[p]);
382 for (
unsigned int c = 0; c < NbCurves_in_graph[p]; c++)
383 ViSP_plot.
setColor(p, c, Colors[c]);
386 ViSP_plot.
setTitle(0,
"Robot velocities");
394 ViSP_plot.
setTitle(1,
"Camera pose cMo");
402 ViSP_plot.
setTitle(2,
"Error in visual features: ");
413 unsigned int m_width;
414 unsigned int m_height;
457 servo.init(cMo, cdMo);
463 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="") 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...
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)
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.
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()