Newer
Older
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
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
#include "overlay.hpp"
#include <opencv2/imgproc.hpp>
void ftl::overlay::draw3DLine(
const ftl::rgbd::Camera &cam,
cv::Mat &colour,
cv::Mat &depth,
const Eigen::Vector4d &begin,
const Eigen::Vector4d &end,
const cv::Scalar &linecolour) {
auto begin_pos = cam.camToScreen<int2>(make_float3(begin[0], begin[1], begin[2]));
auto end_pos = cam.camToScreen<int2>(make_float3(end[0], end[1], end[2]));
cv::LineIterator lineit(colour, cv::Point(begin_pos.x, colour.rows-begin_pos.y), cv::Point(end_pos.x, colour.rows-end_pos.y));
double z_grad = (end[2] - begin[2]) / lineit.count;
double current_z = begin[2];
for(int i = 0; i < lineit.count; i++, ++lineit) {
colour.at<cv::Vec4b>(lineit.pos()) = linecolour;
depth.at<float>(lineit.pos()) = current_z;
current_z += z_grad;
}
}
void ftl::overlay::drawPoseBox(
const ftl::rgbd::Camera &cam,
cv::Mat &colour,
cv::Mat &depth,
const Eigen::Matrix4d &pose,
const cv::Scalar &linecolour,
double size) {
double size2 = size/2.0;
Eigen::Vector4d p001 = pose.inverse() * Eigen::Vector4d(size2,size2,-size2,1);
Eigen::Vector4d p011 = pose.inverse() * Eigen::Vector4d(size2,-size2,-size2,1);
Eigen::Vector4d p111 = pose.inverse() * Eigen::Vector4d(-size2,-size2,-size2,1);
Eigen::Vector4d p101 = pose.inverse() * Eigen::Vector4d(-size2,size2,-size2,1);
Eigen::Vector4d p110 = pose.inverse() * Eigen::Vector4d(-size2,-size2,size2,1);
Eigen::Vector4d p100 = pose.inverse() * Eigen::Vector4d(-size2,size2,size2,1);
Eigen::Vector4d p010 = pose.inverse() * Eigen::Vector4d(size2,-size2,size2,1);
Eigen::Vector4d p000 = pose.inverse() * Eigen::Vector4d(size2,size2,size2,1);
p001 /= p001[3];
p011 /= p011[3];
p111 /= p111[3];
p101 /= p101[3];
p110 /= p110[3];
p100 /= p100[3];
p010 /= p010[3];
p000 /= p000[3];
if (p001[2] < 0.1 || p011[2] < 0.1 || p111[2] < 0.1 || p101[2] < 0.1 || p110[2] < 0.1 || p100[2] < 0.1 || p010[2] < 0.1 || p000[2] < 0.1) return;
draw3DLine(cam, colour, depth, p000, p001, linecolour);
draw3DLine(cam, colour, depth, p000, p010, linecolour);
draw3DLine(cam, colour, depth, p000, p100, linecolour);
draw3DLine(cam, colour, depth, p001, p011, linecolour);
draw3DLine(cam, colour, depth, p001, p101, linecolour);
draw3DLine(cam, colour, depth, p010, p011, linecolour);
draw3DLine(cam, colour, depth, p010, p110, linecolour);
draw3DLine(cam, colour, depth, p100, p101, linecolour);
draw3DLine(cam, colour, depth, p100, p110, linecolour);
draw3DLine(cam, colour, depth, p101, p111, linecolour);
draw3DLine(cam, colour, depth, p110, p111, linecolour);
draw3DLine(cam, colour, depth, p011, p111, linecolour);
}
void ftl::overlay::drawPoseCone(
const ftl::rgbd::Camera &cam,
cv::Mat &colour,
cv::Mat &depth,
const Eigen::Matrix4d &pose,
const cv::Scalar &linecolour,
double size) {
double size2 = size;
Eigen::Vector4d p110 = pose.inverse() * Eigen::Vector4d(-size2,-size2,size2,1);
Eigen::Vector4d p100 = pose.inverse() * Eigen::Vector4d(-size2,size2,size2,1);
Eigen::Vector4d p010 = pose.inverse() * Eigen::Vector4d(size2,-size2,size2,1);
Eigen::Vector4d p000 = pose.inverse() * Eigen::Vector4d(size2,size2,size2,1);
Eigen::Vector4d origin = pose.inverse() * Eigen::Vector4d(0,0,0,1);
p110 /= p110[3];
p100 /= p100[3];
p010 /= p010[3];
p000 /= p000[3];
origin /= origin[3];
if (origin[2] < 0.1 || p110[2] < 0.1 || p100[2] < 0.1 || p010[2] < 0.1 || p000[2] < 0.1) return;
draw3DLine(cam, colour, depth, p000, origin, linecolour);
draw3DLine(cam, colour, depth, p000, p010, linecolour);
draw3DLine(cam, colour, depth, p000, p100, linecolour);
draw3DLine(cam, colour, depth, p010, origin, linecolour);
draw3DLine(cam, colour, depth, p010, p110, linecolour);
draw3DLine(cam, colour, depth, p100, origin, linecolour);
draw3DLine(cam, colour, depth, p100, p110, linecolour);
draw3DLine(cam, colour, depth, p110, origin, linecolour);
}
void ftl::overlay::drawText(
const ftl::rgbd::Camera &cam,
cv::Mat &colour,
cv::Mat &depth,
const std::string &text,
const Eigen::Vector4d &pos,
double size,
const cv::Scalar &textcolour) {
auto pt = cam.camToScreen<int2>(make_float3(pos[0], pos[1], pos[2]));
if (pos[2] < 0.1) return;
cv::putText(colour, text, cv::Point(pt.x, colour.rows-pt.y), 0, size, textcolour, 1, cv::LINE_8, true);
}