Visual Servoing Platform version 3.5.0
pose_helper.cpp
1#include <visp3/vision/vpPose.h>
2#include <visp3/core/vpDisplay.h>
3#include <visp3/core/vpPixelMeterConversion.h>
4
5#include "pose_helper.h"
6
8void computePose(std::vector<vpPoint> &point, const std::vector<vpImagePoint> &ip,
9 const vpCameraParameters &cam, bool init, vpHomogeneousMatrix &cMo)
10{
11 vpPose pose;
12 double x = 0, y = 0;
13 for (unsigned int i = 0; i < point.size(); i++) {
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)
37std::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 }
75 catch(...) {
76 std::cout << "Traking lost" << std::endl;
77 throw(vpException(vpException::fatalError, "Tracking lost"));
78 }
79}
80#endif
81
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:81
void addPoint(const vpPoint &P)
Definition: vpPose.cpp:149
@ DEMENTHON
Definition: vpPose.h:86
@ VIRTUAL_VS
Definition: vpPose.h:95
@ LAGRANGE
Definition: vpPose.h:85
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:336
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:374