Visual Servoing Platform  version 3.5.1 under development (2022-08-10)
pose_helper.cpp
1 #include <visp3/core/vpDisplay.h>
2 #include <visp3/core/vpPixelMeterConversion.h>
3 #include <visp3/vision/vpPose.h>
4 
5 #include "pose_helper.h"
6 
8 void computePose(std::vector<vpPoint> &point, const std::vector<vpImagePoint> &ip, const vpCameraParameters &cam,
9  bool init, vpHomogeneousMatrix &cMo)
10 {
11  vpPose pose;
12  double x = 0, y = 0;
13  for (unsigned int i = 0; i < point.size(); i++) {
14  vpPixelMeterConversion::convertPoint(cam, ip[i], x, y);
15  point[i].set_x(x);
16  point[i].set_y(y);
17  pose.addPoint(point[i]);
18  }
19 
20  if (init == true) {
21  vpHomogeneousMatrix cMo_dem;
22  vpHomogeneousMatrix cMo_lag;
23  pose.computePose(vpPose::DEMENTHON, cMo_dem);
24  pose.computePose(vpPose::LAGRANGE, cMo_lag);
25  double residual_dem = pose.computeResidual(cMo_dem);
26  double residual_lag = pose.computeResidual(cMo_lag);
27  if (residual_dem < residual_lag)
28  cMo = cMo_dem;
29  else
30  cMo = cMo_lag;
31  }
33 }
35 
36 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
37 std::vector<vpImagePoint> track(vpImage<unsigned char> &I, std::vector<vpDot2> &dot, bool init)
38 {
39  try {
40  double distance_same_blob = 10.; // 2 blobs are declared same if their distance is less than this value
41  std::vector<vpImagePoint> ip(dot.size());
42  if (init) {
44  for (unsigned int i = 0; i < dot.size(); i++) {
45  dot[i].setGraphics(true);
46  dot[i].setGraphicsThickness(2);
47  std::stringstream ss;
48  ss << "Click on point " << i + 1;
49  vpDisplay::displayText(I, 20, 20, "Status: initialize blob tracker", vpColor::red);
50  vpDisplay::displayText(I, 40 + i * 20, 20, ss.str(), vpColor::red);
52  dot[i].initTracking(I);
54  }
55  } else {
56  for (unsigned int i = 0; i < dot.size(); i++) {
57  dot[i].track(I);
58  }
59  }
60  for (unsigned int i = 0; i < dot.size(); i++) {
61  ip[i] = dot[i].getCog();
62  // Compare distances between all the dots to check if some of them are not the same
63  }
64  for (unsigned int i = 0; i < ip.size(); i++) {
65  for (unsigned int j = i + 1; j < ip.size(); j++) {
66  if (vpImagePoint::distance(ip[i], ip[j]) < distance_same_blob) {
67  std::cout << "Traking lost: 2 blobs are the same" << std::endl;
68  throw(vpException(vpException::fatalError, "Tracking lost: 2 blobs are the same"));
69  }
70  }
71  }
72 
73  return ip;
74  } catch (...) {
75  std::cout << "Traking lost" << std::endl;
76  throw(vpException(vpException::fatalError, "Tracking lost"));
77  }
78 }
79 #endif
Generic class defining intrinsic camera parameters.
static const vpColor red
Definition: vpColor.h:217
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.
Definition: vpException.h:72
@ fatalError
Fatal error.
Definition: vpException.h:96
Implementation of an homogeneous matrix and operations on such kind of matrices.
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
void init(unsigned int h, unsigned int w, Type value)
Definition: vpImage.h:631
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:90
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:148
@ DEMENTHON
Definition: vpPose.h:95
@ VIRTUAL_VS
Definition: vpPose.h:104
@ LAGRANGE
Definition: vpPose.h:94
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo.
Definition: vpPose.cpp:335
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:373