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
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
|
/*
* =====================================================================================
*
* Filename: vision.cpp
*
* Description: Class for vision tasks such as opening images, plotting
* points in the image frame, performing measurements.
*
* Version: 1.0
* Created: 04/07/2017 11:34:16 AM
* Revision: none
* Compiler: gcc
*
* Author: Martin Miller (MHM), miller7@illinois.edu
* Organization: Aerospace Robotics and Controls Lab (ARC)
*
* =====================================================================================
*/
#include "vision.h"
/*
*--------------------------------------------------------------------------------------
* Class: Vision
* Method: Vision
* Description: constructor
*--------------------------------------------------------------------------------------
*/
Vision::Vision ()
{
;
} /* ----- end of method Vision::Vision (constructor) ----- */
/*
*--------------------------------------------------------------------------------------
* Class: Vision
* Method: Vision :: open
* Description: Opens the given image, undistorts.
*--------------------------------------------------------------------------------------
*/
void
Vision::open ( const char *fn, const Camera &cam )
{
Mat bayered ;
bayered = cv::imread(fn, -1);
if (!bayered.data) {
fprintf(stderr, "Could not read %s.\n", fn);
exit(1);
}
cv::cvtColor(bayered, original, CV_BayerBG2BGR ,3);
display = original.clone();
return ;
} /* ----- end of method Vision::open ----- */
/*
*--------------------------------------------------------------------------------------
* Class: Vision
* Method: Vision :: drawMeasurements
* Description: Draws the measured points onto the image.
*--------------------------------------------------------------------------------------
*/
void
Vision::drawMeasurements ( const vector<measurement_t> &z, const Camera &cam,
const cv::Scalar &colsrc, const cv::Scalar &colref, bool drawEllipse)
{
cv::Scalar ellcol(0,0,200);
for (auto i=z.begin(); i!=z.end(); ++i) {
if (i->z_type==HEIGHT) continue;
cerr << i->id << endl;
Vector3d xis, xir;
xis = cam.body2img(i->source);
xis /= xis[2];
xir = cam.body2img(i->reflection);
xir /= xir[2];
cv::Point ps(xis[0],xis[1]);
cv::Point pr(xir[0],xir[1]);
cv::circle(display, ps, 3, colsrc,2);
cv::circle(display, pr, 3, colref);
if (drawEllipse) {
double xs, ys, xr, yr;
xs = 2*sqrt(i->Ssrc(0));
ys = 2*sqrt(i->Ssrc(1));
xr = 2*sqrt(i->Sref(0));
yr = 2*sqrt(i->Sref(1));
// Create rotated rect for ellipse
cv::RotatedRect rrs(ps, cv::Size(2*5.9*xs, 2*5.9*ys), 0.);
cv::RotatedRect rrr(pr, cv::Size(2*5.9*xr, 2*5.9*yr), 0.);
cv::ellipse(display, rrs, ellcol );
cv::ellipse(display, rrr, ellcol);
}
}
return ;
} /* ----- end of method Vision::drawMeasurements ----- */
/*
*--------------------------------------------------------------------------------------
* Class: Vision
* Method: Vision :: show
* Description: Show the display image.
*--------------------------------------------------------------------------------------
*/
void
Vision::show ( )
{
cv::imshow("Image view", display);
cv::waitKey(1);
return ;
} /* ----- end of method Vision::show ----- */
/*
*--------------------------------------------------------------------------------------
* Class: Vision
* Method: Vision :: acquireFeatures
* Description: Finds new features, assigns IDs, extracts descriptors.
*--------------------------------------------------------------------------------------
*/
void
Vision::acquireFeatures ( const Camera &cam, vector<measurement_t> &z )
{
vector<cv::Point2f> corners;
Mat gray;
cv::cvtColor(original, gray, CV_BGR2GRAY);
cv::goodFeaturesToTrack(gray, corners, 50, 0.1, 20);
for (auto i=corners.begin(); i!=corners.end(); ++i) {
measurement_t m;
m.id = _id++;
m.z_type = MONO;
Vector3d xi;
xi << i->x, i->y, 1.;
m.source = cam.img2body(xi);
m.reflection << 0,0,1;
z.push_back(m);
}
return ;
} /* ----- end of method Vision::acquireFeatures ----- */
int Vision::_id = 1;
|
