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
|
/*
* =====================================================================================
*
* Filename: test_feature.cpp
*
* Description: Test the feature class.
*
* Version: 1.0
* Created: 03/25/2017 12:43:53 PM
* Revision: none
* Compiler: gcc
*
* Author: Martin Miller (MHM), miller7@illinois.edu
* Organization: Aerospace Robotics and Controls Lab (ARC)
*
* =====================================================================================
*/
#include "test_feature.h"
/*
* === FUNCTION ======================================================================
* Name: test_findDepth
* Description: Test the Feature::findDepth() method.
* =====================================================================================
*/
bool
test_findDepth ( Camera &cam )
{
bool success;
UTM pos;
pos.northing = 4451490.899494356;
pos.easting = 381887.311030777;
pos.up = 206.256890383;
pos.zone_i = 16;
pos.zone_c = 'T';
// Quat for 16.bmp
attitude_t att{ -1.073641623878,2.685395530695,16.357370508605};
double roll, pitch, yaw;
roll = M_PI*att.roll/180.;
pitch = M_PI*att.pitch/180.;
yaw = M_PI*(att.yaw)/180.;
Matrix3d R;
Quaterniond q;
R = AngleAxisd(yaw, Vector3d::UnitZ())
* AngleAxisd(roll, Vector3d::UnitX())
* AngleAxisd(pitch, Vector3d::UnitY());
q = Quaterniond(R);
Matrix<double,26,5> meas;
meas << 162,493,260,493,331,
66,370,229,369,352,
268,763,235,764,364,
414,554,250,554,337,
452,365,210,365,370,
580,353,239,353,344,
606,455,253,455,333,
680,748,237,747,357,
706,650,233,651,358,
724,671,199,668,400,
736,768,283,768,318,
774,368,159,369,370,
842,281,221,282,352,
882,748,249,748,349,
910,386,217,387,369,
916,713,210,713,383,
966,519,218,519,368,
1026,567,264,567,325,
1050,686,227,686,361,
1070,650,220,650,370,
1084,531,267,531,323,
1160,331,222,332,353,
1184,388,273,388,314,
1262,547,219,546,360,
1274,314,218,313,354,
1278,469,217,469,365;
double z;
z = -0.34;
z -= 0.6*3.43*sin(pitch);
Vector3d xbw;
xbw << pos.northing, pos.easting, -pos.up;
printf("id,utm_z,utm_c,utm-northing,utm-easting,altitude\n");
for (int i=0; i<meas.rows(); ++i) {
Matrix<double,1,5> row;
row = meas.block<1,5>(i,0);
int id = row[0];
Vector3d xsi,xri,xs,xr;
xsi << row[1], row[2], 1;
xri << row[3], row[4], 1;
xs = cam.img2body(xsi);
xr = cam.img2body(xri);
Feature f(id,xs,xr,xbw,q,z);
if (f.sane())
{
UTM utmi;
utmi = f.utm(xbw,q);
printf("%d,%d,%c,%f,%f,%f\n", id,utmi.zone_i, utmi.zone_c, utmi.northing, utmi.easting, utmi.up);
}
}
success = true;
return success;
} /* ----- end of function test_findDepth ----- */
int main(int argc, char **argv)
{
Camera cam(argv[1]);
if (test_findDepth(cam)) {
;//printf("PASSED\n");
} else {
printf("FAILED\n");
}
return 0;
}
|
