Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
8void 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++) {
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)
30std::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)
Definition of the vpImage class member functions.
Definition vpImage.h:135
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:140
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Definition vpPose.h:102
@ VIRTUAL_VS
Definition vpPose.h:96
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition vpPose.cpp:469