1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
|
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/vision/vpPose.h>
#include "pose_helper.h"
//! [Compute pose]
void computePose(std::vector<vpPoint> &point, const std::vector<vpImagePoint> &ip, const vpCameraParameters &cam,
bool init, vpHomogeneousMatrix &cMo)
{
vpPose pose;
double x = 0, y = 0;
for (unsigned int i = 0; i < point.size(); i++) {
vpPixelMeterConversion::convertPoint(cam, ip[i], x, y);
point[i].set_x(x);
point[i].set_y(y);
pose.addPoint(point[i]);
}
if (init == true) {
pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo);
}
else {
pose.computePose(vpPose::VIRTUAL_VS, cMo);
}
}
//! [Compute pose]
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
std::vector<vpImagePoint> track(vpImage<unsigned char> &I, std::vector<vpDot2> &dot, bool init)
{
try {
double distance_same_blob = 10.; // 2 blobs are declared same if their distance is less than this value
std::vector<vpImagePoint> ip(dot.size());
if (init) {
vpDisplay::flush(I);
for (unsigned int i = 0; i < dot.size(); i++) {
dot[i].setGraphics(true);
dot[i].setGraphicsThickness(2);
std::stringstream ss;
ss << "Click on point " << i + 1;
vpDisplay::displayText(I, 20, 20, "Status: initialize blob tracker", vpColor::red);
vpDisplay::displayText(I, 40 + i * 20, 20, ss.str(), vpColor::red);
vpDisplay::flush(I);
dot[i].initTracking(I);
vpDisplay::flush(I);
}
} else {
for (unsigned int i = 0; i < dot.size(); i++) {
dot[i].track(I);
}
}
for (unsigned int i = 0; i < dot.size(); i++) {
ip[i] = dot[i].getCog();
// Compare distances between all the dots to check if some of them are not the same
}
for (unsigned int i = 0; i < ip.size(); i++) {
for (unsigned int j = i + 1; j < ip.size(); j++) {
if (vpImagePoint::distance(ip[i], ip[j]) < distance_same_blob) {
std::cout << "Traking lost: 2 blobs are the same" << std::endl;
throw(vpException(vpException::fatalError, "Tracking lost: 2 blobs are the same"));
}
}
}
return ip;
} catch (...) {
std::cout << "Traking lost" << std::endl;
throw(vpException(vpException::fatalError, "Tracking lost"));
}
}
#endif
|
