42 #include <visp3/core/vpCameraParameters.h>
43 #include <visp3/core/vpConfig.h>
44 #include <visp3/core/vpDebug.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/gui/vpDisplayD3D.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/vpSimulatorAfma6.h>
59 #include <visp3/visual_features/vpFeatureBuilder.h>
60 #include <visp3/visual_features/vpFeatureMomentCommon.h>
61 #include <visp3/visual_features/vpFeaturePoint.h>
62 #include <visp3/vs/vpServo.h>
64 #if !defined(VISP_HAVE_DISPLAY)
67 std::cout <<
"Can't run this example since no display capability is available." << std::endl;
68 std::cout <<
"You should install one of the following third-party library: X11, OpenCV, GDI, GTK." << std::endl;
71 #elif !defined(VISP_HAVE_THREADS)
74 std::cout <<
"Can't run this example since multi-threading capability is not available." << std::endl;
75 std::cout <<
"You should maybe enable cxx11 standard." << std::endl;
80 #ifndef DOXYGEN_SHOULD_SKIP_THIS
85 : m_width(640), m_height(480), m_cMo(), m_cdMo(), m_robot(false), m_Iint(m_height, m_width, 255), m_task(), m_cam(),
86 m_error(0), m_imsim(), m_interaction_type(), m_src(6), m_dst(6), m_moments(nullptr), m_momentsDes(nullptr),
87 m_featureMoments(nullptr), m_featureMomentsDes(nullptr), m_displayInt(nullptr)
91 #ifdef VISP_HAVE_DISPLAY
98 delete m_featureMoments;
99 delete m_featureMomentsDes;
105 std::vector<vpPoint> src_pts;
106 std::vector<vpPoint> dst_pts;
108 double x[8] = { 1, 3, 4, -1, -3, -2, -1, 1 };
109 double y[8] = { 0, 1, 4, 4, -2, -2, 1, 0 };
112 for (
int i = 0; i < nbpoints; i++) {
113 vpPoint p(x[i] / 20, y[i] / 20, 0.0);
115 src_pts.push_back(p);
119 m_src.fromVector(src_pts);
120 for (
int i = 0; i < nbpoints; i++) {
121 vpPoint p(x[i] / 20, y[i] / 20, 0.0);
123 dst_pts.push_back(p);
126 m_dst.fromVector(dst_pts);
145 planeToABC(pl, A, B, C);
149 planeToABC(pl, Ad, Bd, Cd);
167 m_moments->updateAll(m_src);
168 m_momentsDes->updateAll(m_dst);
170 m_featureMoments->updateAll(A, B, C);
171 m_featureMomentsDes->updateAll(Ad, Bd, Cd);
174 m_task.setInteractionMatrixType(m_interaction_type);
177 m_task.addFeature(m_featureMoments->getFeatureGravityNormalized(),
178 m_featureMomentsDes->getFeatureGravityNormalized());
179 m_task.addFeature(m_featureMoments->getFeatureAn(), m_featureMomentsDes->getFeatureAn());
180 m_task.addFeature(m_featureMoments->getFeatureCInvariant(), m_featureMomentsDes->getFeatureCInvariant(),
181 (1 << 3) | (1 << 5));
182 m_task.addFeature(m_featureMoments->getFeatureAlpha(), m_featureMomentsDes->getFeatureAlpha());
184 m_task.setLambda(1.);
192 double x[8] = { 1, 3, 4, -1, -3, -2, -1, 1 };
193 double y[8] = { 0, 1, 4, 4, -2, -2, 1, 0 };
195 std::vector<vpPoint> cur_pts;
197 for (
int i = 0; i < nbpoints; i++) {
198 vpPoint p(x[i] / 20, y[i] / 20, 0.0);
200 cur_pts.push_back(p);
212 #ifdef VISP_HAVE_DISPLAY
214 #if defined(VISP_HAVE_X11)
216 #elif defined(HAVE_OPENCV_HIGHGUI)
218 #elif defined(VISP_HAVE_GDI)
220 #elif defined(VISP_HAVE_D3D9)
222 #elif defined(VISP_HAVE_GTK)
225 m_displayInt->
init(m_Iint, 50, 50,
"Visual servoing with moments");
236 void execute(
unsigned int nbIter)
239 init_visp_plot(ViSP_plot);
246 std::cout <<
"Display task information " << std::endl;
250 m_robot.getInternalView(m_Iint);
252 unsigned int iter = 0;
255 while (iter++ < nbIter) {
258 m_cMo = m_robot.get_cMo();
264 planeToABC(pl, A, B, C);
269 m_moments->updateAll(obj);
272 m_featureMoments->updateAll(A, B, C);
275 m_robot.getInternalView(m_Iint);
282 v = m_task.computeControlLaw();
288 ViSP_plot.
plot(0, iter, v);
290 ViSP_plot.
plot(2, iter, m_task.getError());
292 m_error = (m_task.getError()).sumSquare();
302 m_robot.getInternalView(m_Iint);
326 robot.setJointLimit(limMin, limMax);
331 void planeToABC(
vpPlane &pl,
double &A,
double &B,
double &C)
333 if (fabs(pl.
getD()) < std::numeric_limits<double>::epsilon()) {
334 std::cout <<
"Invalid position:" << std::endl;
335 std::cout << m_cMo << std::endl;
336 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
349 m_robot.setCurrentViewColor(
vpColor(150, 150, 150));
350 m_robot.setDesiredViewColor(
vpColor(200, 200, 200));
352 removeJointLimits(m_robot);
354 m_robot.setConstantSamplingTimeMode(
true);
357 m_robot.initialiseObjectRelativeToCamera(m_cMo);
360 m_robot.setDesiredCameraPosition(m_cdMo);
361 m_robot.getCameraParameters(m_cam, m_Iint);
366 double error() {
return m_error; }
368 void init_visp_plot(
vpPlot &ViSP_plot)
374 const unsigned int NbGraphs = 3;
375 const unsigned int NbCurves_in_graph[NbGraphs] = { 6, 6, 6 };
377 ViSP_plot.
init(NbGraphs, 800, 800, 100 +
static_cast<int>(m_width), 50,
"Visual Servoing results...");
382 for (
unsigned int p = 0; p < NbGraphs; p++) {
383 ViSP_plot.
initGraph(p, NbCurves_in_graph[p]);
384 for (
unsigned int c = 0; c < NbCurves_in_graph[p]; c++)
385 ViSP_plot.
setColor(p, c, Colors[c]);
388 ViSP_plot.
setTitle(0,
"Robot velocities");
396 ViSP_plot.
setTitle(1,
"Camera pose cMo");
404 ViSP_plot.
setTitle(2,
"Error in visual features: ");
415 unsigned int m_width;
416 unsigned int m_height;
453 servo.init(cMo, cdMo);
458 std::cout <<
"Catch an exception: " << e << std::endl;
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
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.
Class which enables to project an image in the 3D space and get the view of a virtual camera.
static double rad(double deg)
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 fromVector(std::vector< vpPoint > &points)
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)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Implementation of a pose vector and operations on poses.
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
void setMaxRotationVelocity(double maxVr)
void setMaxTranslationVelocity(double maxVt)
vpServoIteractionMatrixType
Simulator of Irisa's gantry robot named Afma6.
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.