Visual Servoing Platform  version 3.6.1 under development (2024-03-28)
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) {
22  }
23  else {
25  }
26 }
28 
29 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
30 std::vector<vpImagePoint> track(vpImage<unsigned char> &I, std::vector<vpDot2> &dot, bool init)
31 {
32  try {
33  double distance_same_blob = 10.; // 2 blobs are declared same if their distance is less than this value
34  std::vector<vpImagePoint> ip(dot.size());
35  if (init) {
37  for (unsigned int i = 0; i < dot.size(); i++) {
38  dot[i].setGraphics(true);
39  dot[i].setGraphicsThickness(2);
40  std::stringstream ss;
41  ss << "Click on point " << i + 1;
42  vpDisplay::displayText(I, 20, 20, "Status: initialize blob tracker", vpColor::red);
43  vpDisplay::displayText(I, 40 + i * 20, 20, ss.str(), vpColor::red);
45  dot[i].initTracking(I);
47  }
48  } else {
49  for (unsigned int i = 0; i < dot.size(); i++) {
50  dot[i].track(I);
51  }
52  }
53  for (unsigned int i = 0; i < dot.size(); i++) {
54  ip[i] = dot[i].getCog();
55  // Compare distances between all the dots to check if some of them are not the same
56  }
57  for (unsigned int i = 0; i < ip.size(); i++) {
58  for (unsigned int j = i + 1; j < ip.size(); j++) {
59  if (vpImagePoint::distance(ip[i], ip[j]) < distance_same_blob) {
60  std::cout << "Traking lost: 2 blobs are the same" << std::endl;
61  throw(vpException(vpException::fatalError, "Tracking lost: 2 blobs are the same"));
62  }
63  }
64  }
65 
66  return ip;
67  } catch (...) {
68  std::cout << "Traking lost" << std::endl;
69  throw(vpException(vpException::fatalError, "Tracking lost"));
70  }
71 }
72 #endif
Generic class defining intrinsic camera parameters.
static const vpColor red
Definition: vpColor.h:211
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.
Definition: vpException.h:59
@ fatalError
Fatal error.
Definition: vpException.h:84
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:619
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:78
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:93
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Definition: vpPose.h:99
@ VIRTUAL_VS
Definition: vpPose.h:93
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=nullptr)
Definition: vpPose.cpp:333