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;
457 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS 469 servo.init(cMo, cdMo);
474 std::cout <<
"Catch an exception: " << e << std::endl;
void svd(vpColVector &w, vpMatrix &V)
Implementation of a matrix and operations on matrices.
VISP_EXPORT int wait(double t0, double t)
void init(const vpImage< unsigned char > &I, vpColVector *X)
Class that defines generic functionnalities for display.
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines the simplest robot: a free flying camera.
void getImage(vpImage< unsigned char > &I, const vpCameraParameters &cam)
void init(unsigned int nbGraph, unsigned int height=700, unsigned int width=700, int x=-1, int y=-1, const std::string &title="")
Display for windows using GDI (available on any windows 32 platform).
Class to define RGB colors available for display functionnalities.
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
This class allows to access common vpFeatureMoments in a pre-filled database.
error that can be emited by ViSP classes.
Class for generic objects.
static const vpColor green
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
static void flush(const vpImage< unsigned char > &I)
VISP_EXPORT double measureTimeMs()
void fromImage(const vpImage< unsigned char > &image, unsigned char threshold, const vpCameraParameters &cam)
static const vpColor orange
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed...
void setCameraPosition(const vpHomogeneousMatrix &cMt)
static const vpColor cyan
void setInterpolationType(const vpInterpolationType interplt)
void changeFrame(const vpHomogeneousMatrix &cMo)
static std::vector< double > getMu3(vpMomentObject &object)
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Generic class defining intrinsic camera parameters.
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
Class which enables to project an image in the 3D space and get the view of a virtual camera...
void setTitle(unsigned int graphNum, const std::string &title)
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
vpServoIteractionMatrixType
void setABCD(double a, double b, double c, double d)
static double getSurface(vpMomentObject &object)
static double rad(double deg)
void initGraph(unsigned int graphNum, unsigned int curveNbr)
This class initializes and allows access to commonly used moments.
static double getAlpha(vpMomentObject &object)
Implementation of column vector and the associated operations.
Implementation of a pose vector and operations on poses.
vpHomogeneousMatrix inverse() const
void setType(vpObjectType input_type)
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
This class defines the container for a plane geometrical structure.
void setColor(unsigned int graphNum, unsigned int curveNum, vpColor color)
static const vpColor purple
Class that consider the case of a translation vector.
static const vpColor blue