root / rgbdslam / gicp / transform.cpp @ c1426757
History | View | Annotate | Download (9.48 KB)
1 |
#include <stdio.h> |
---|---|
2 |
#include <stdlib.h> |
3 |
#include <math.h> |
4 |
#include <string.h> |
5 |
#include <stdarg.h> |
6 |
#include "transform.h" |
7 |
|
8 |
static void dgc_warning(const char *fmt, ...) |
9 |
{ |
10 |
char message[1024]; |
11 |
va_list args; |
12 |
|
13 |
va_start(args, fmt); |
14 |
vsnprintf(message, 1024, fmt, args);
|
15 |
va_end(args); |
16 |
message[1023] = '\0'; |
17 |
|
18 |
fprintf(stderr, "%s %s\n", "# WARNING: ", message); |
19 |
} |
20 |
|
21 |
static inline char *dgc_next_word(char *str) |
22 |
{ |
23 |
char *mark = str;
|
24 |
|
25 |
if(str == NULL) |
26 |
return NULL; |
27 |
while(*mark != '\0' && !(*mark == ' ' || *mark == '\t')) { |
28 |
mark++; |
29 |
} |
30 |
while(*mark != '\0' && (*mark == ' ' || *mark == '\t')) { |
31 |
mark++; |
32 |
} |
33 |
return mark;
|
34 |
} |
35 |
|
36 |
static inline double dgc_d2r(double theta) |
37 |
{ |
38 |
return (theta * M_PI / 180.0); |
39 |
} |
40 |
|
41 |
|
42 |
void dgc_transform_print(dgc_transform_t t, const char *str) |
43 |
{ |
44 |
int r, c;
|
45 |
|
46 |
fprintf(stdout, "%s:\n", str);
|
47 |
for(r = 0; r < 4; r++) { |
48 |
for(c = 0; c < 4; c++) |
49 |
fprintf(stdout, "%8.3f ", t[r][c]);
|
50 |
fprintf(stdout, "\n");
|
51 |
} |
52 |
} |
53 |
|
54 |
void dgc_transform_identity(dgc_transform_t t)
|
55 |
{ |
56 |
int r, c;
|
57 |
|
58 |
for(r = 0; r < 4; r++) |
59 |
for(c = 0; c < 4; c++) |
60 |
if(r == c)
|
61 |
t[r][c] = 1;
|
62 |
else
|
63 |
t[r][c] = 0;
|
64 |
} |
65 |
|
66 |
void dgc_transform_left_multiply(dgc_transform_t t1, dgc_transform_t t2)
|
67 |
{ |
68 |
dgc_transform_t result; |
69 |
int i, j, k;
|
70 |
|
71 |
for(i = 0; i < 4; i++) |
72 |
for(j = 0; j < 4; j++) { |
73 |
result[i][j] = 0;
|
74 |
for(k = 0; k < 4; k++) |
75 |
result[i][j] += t2[i][k] * t1[k][j]; |
76 |
} |
77 |
for(i = 0; i < 4; i++) |
78 |
for(j = 0; j < 4; j++) |
79 |
t1[i][j] = result[i][j]; |
80 |
} |
81 |
|
82 |
void dgc_transform_left_multiply_nc(dgc_transform_t dest,
|
83 |
dgc_transform_t src, |
84 |
dgc_transform_t left) |
85 |
{ |
86 |
int i, j, k;
|
87 |
|
88 |
for(i = 0; i < 4; i++) |
89 |
for(j = 0; j < 4; j++) { |
90 |
dest[i][j] = 0;
|
91 |
for(k = 0; k < 4; k++) |
92 |
dest[i][j] += left[i][k] * src[k][j]; |
93 |
} |
94 |
} |
95 |
|
96 |
void dgc_transform_rotate_x(dgc_transform_t t, double theta) |
97 |
{ |
98 |
dgc_transform_t temp; |
99 |
double ctheta = cos(theta), stheta = sin(theta);
|
100 |
|
101 |
dgc_transform_identity(temp); |
102 |
temp[1][1] = ctheta; |
103 |
temp[1][2] = -stheta; |
104 |
temp[2][1] = stheta; |
105 |
temp[2][2] = ctheta; |
106 |
dgc_transform_left_multiply(t, temp); |
107 |
} |
108 |
|
109 |
void dgc_transform_rotate_y(dgc_transform_t t, double theta) |
110 |
{ |
111 |
dgc_transform_t temp; |
112 |
double ctheta = cos(theta), stheta = sin(theta);
|
113 |
|
114 |
dgc_transform_identity(temp); |
115 |
temp[0][0] = ctheta; |
116 |
temp[0][2] = stheta; |
117 |
temp[2][0] = -stheta; |
118 |
temp[2][2] = ctheta; |
119 |
dgc_transform_left_multiply(t, temp); |
120 |
} |
121 |
|
122 |
void dgc_transform_rotate_z(dgc_transform_t t, double theta) |
123 |
{ |
124 |
dgc_transform_t temp; |
125 |
double ctheta = cos(theta), stheta = sin(theta);
|
126 |
|
127 |
dgc_transform_identity(temp); |
128 |
temp[0][0] = ctheta; |
129 |
temp[0][1] = -stheta; |
130 |
temp[1][0] = stheta; |
131 |
temp[1][1] = ctheta; |
132 |
dgc_transform_left_multiply(t, temp); |
133 |
} |
134 |
|
135 |
void dgc_transform_translate(dgc_transform_t t, double x, double y, double z) |
136 |
{ |
137 |
t[0][3] += x; |
138 |
t[1][3] += y; |
139 |
t[2][3] += z; |
140 |
} |
141 |
|
142 |
void dgc_transform_copy(dgc_transform_t dest, dgc_transform_t src)
|
143 |
{ |
144 |
int r, c;
|
145 |
|
146 |
for(r = 0; r < 4; r++) |
147 |
for(c = 0; c < 4; c++) |
148 |
dest[r][c] = src[r][c]; |
149 |
} |
150 |
/*
|
151 |
inline void dgc_transform_point(double *x, double *y, double *z,
|
152 |
dgc_transform_t t)
|
153 |
{
|
154 |
double x2, y2, z2;
|
155 |
|
156 |
x2 = t[0][0] * *x + t[0][1] * *y + t[0][2] * *z + t[0][3];
|
157 |
y2 = t[1][0] * *x + t[1][1] * *y + t[1][2] * *z + t[1][3];
|
158 |
z2 = t[2][0] * *x + t[2][1] * *y + t[2][2] * *z + t[2][3];
|
159 |
*x = x2;
|
160 |
*y = y2;
|
161 |
*z = z2;
|
162 |
}
|
163 |
*/
|
164 |
|
165 |
|
166 |
int dgc_transform_read_string(dgc_transform_t t, char *str) |
167 |
{ |
168 |
int done;
|
169 |
char *end, *mark, *mark2, *unit;
|
170 |
char line[1001]; |
171 |
double arg, x, y, z;
|
172 |
|
173 |
/* start with identity transform */
|
174 |
dgc_transform_identity(t); |
175 |
|
176 |
if(str != NULL) { |
177 |
mark = str; |
178 |
done = 0;
|
179 |
do {
|
180 |
end = strchr(mark, '\n');
|
181 |
if(end == NULL) { |
182 |
strcpy(line, mark); |
183 |
done = 1;
|
184 |
} |
185 |
else {
|
186 |
strncpy(line, mark, end - mark + 1);
|
187 |
line[end - mark + 1] = '\0'; |
188 |
mark = end + 1;
|
189 |
} |
190 |
|
191 |
if(strlen(line) > 0) { |
192 |
unit = dgc_next_word(line); |
193 |
mark2 = dgc_next_word((char *)unit);
|
194 |
|
195 |
if(strncasecmp(line, "rx ", 3) == 0) { |
196 |
arg = strtod(mark2, &mark2); |
197 |
if(strncasecmp(unit, "deg", 3) == 0) |
198 |
arg = dgc_d2r(arg); |
199 |
dgc_transform_rotate_x(t, arg); |
200 |
} |
201 |
else if(strncasecmp(line, "ry ", 3) == 0) { |
202 |
arg = strtod(mark2, &mark2); |
203 |
if(strncasecmp(unit, "deg", 3) == 0) |
204 |
arg = dgc_d2r(arg); |
205 |
dgc_transform_rotate_y(t, arg); |
206 |
} |
207 |
else if(strncasecmp(line, "rz ", 3) == 0) { |
208 |
arg = strtod(mark2, &mark2); |
209 |
if(strncasecmp(unit, "deg", 3) == 0) |
210 |
arg = dgc_d2r(arg); |
211 |
dgc_transform_rotate_z(t, arg); |
212 |
} |
213 |
else if(strncasecmp(line, "t ", 2) == 0) { |
214 |
x = strtod(mark2, &mark2); |
215 |
y = strtod(mark2, &mark2); |
216 |
z = strtod(mark2, &mark2); |
217 |
if(strncasecmp(unit, "in", 2) == 0) { |
218 |
x *= 0.0254; |
219 |
y *= 0.0254; |
220 |
z *= 0.0254; |
221 |
} |
222 |
else if(strncasecmp(unit, "cm", 2) == 0) { |
223 |
x *= 0.01; |
224 |
y *= 0.01; |
225 |
z *= 0.01; |
226 |
} |
227 |
dgc_transform_translate(t, x, y, z); |
228 |
} |
229 |
else {
|
230 |
dgc_warning("Error: could not parse line \"%s\" from transform\n",
|
231 |
line); |
232 |
return -1; |
233 |
} |
234 |
} |
235 |
} while(!done);
|
236 |
} |
237 |
return 0; |
238 |
} |
239 |
|
240 |
int dgc_transform_read(dgc_transform_t t, const char *filename) |
241 |
{ |
242 |
FILE *fp; |
243 |
char *err, *mark, *unit, line[1001]; |
244 |
double arg, x, y, z;
|
245 |
|
246 |
/* start with identity transform */
|
247 |
dgc_transform_identity(t); |
248 |
fp = fopen(filename, "r");
|
249 |
if(fp == NULL) { |
250 |
dgc_warning("Error: could not open transform file %s.\n", filename);
|
251 |
return -1; |
252 |
} |
253 |
do {
|
254 |
err = fgets(line, 1000, fp);
|
255 |
if(err != NULL) { |
256 |
unit = dgc_next_word(line); |
257 |
mark = dgc_next_word(unit); |
258 |
if(strncasecmp(line, "rx ", 3) == 0) { |
259 |
arg = strtod(mark, &mark); |
260 |
if(strncasecmp(unit, "deg", 3) == 0) |
261 |
arg = dgc_d2r(arg); |
262 |
dgc_transform_rotate_x(t, arg); |
263 |
} |
264 |
else if(strncasecmp(line, "ry ", 3) == 0) { |
265 |
arg = strtod(mark, &mark); |
266 |
if(strncasecmp(unit, "deg", 3) == 0) |
267 |
arg = dgc_d2r(arg); |
268 |
dgc_transform_rotate_y(t, arg); |
269 |
} |
270 |
else if(strncasecmp(line, "rz ", 3) == 0) { |
271 |
arg = strtod(mark, &mark); |
272 |
if(strncasecmp(unit, "deg", 3) == 0) |
273 |
arg = dgc_d2r(arg); |
274 |
dgc_transform_rotate_z(t, arg); |
275 |
} |
276 |
else if(strncasecmp(line, "t ", 2) == 0) { |
277 |
char *a = strdup("test"); |
278 |
x = strtod(mark, &mark); |
279 |
y = strtod(mark, &mark); |
280 |
z = strtod(mark, &mark); |
281 |
|
282 |
if(strncasecmp(unit, "in", 2) == 0) { |
283 |
x *= 0.0254; |
284 |
y *= 0.0254; |
285 |
z *= 0.0254; |
286 |
} |
287 |
else if(strncasecmp(unit, "cm", 2) == 0) { |
288 |
x *= 0.01; |
289 |
y *= 0.01; |
290 |
z *= 0.01; |
291 |
} |
292 |
dgc_transform_translate(t, x, y, z); |
293 |
} |
294 |
else {
|
295 |
dgc_warning("Error: could not parse line \"%s\" from %s\n",
|
296 |
line, filename); |
297 |
return -1; |
298 |
} |
299 |
|
300 |
} |
301 |
} while(err != NULL); |
302 |
fclose(fp); |
303 |
return 0; |
304 |
} |
305 |
|
306 |
int dgc_transform_write(dgc_transform_t t, const char *filename) |
307 |
{ |
308 |
FILE *fp; |
309 |
double x, y, z;
|
310 |
double rx, ry, rz;
|
311 |
|
312 |
fp = fopen(filename, "w");
|
313 |
if(fp == NULL) { |
314 |
dgc_warning("Error: could not open transform file %s for writing.\n", filename);
|
315 |
return -1; |
316 |
} |
317 |
|
318 |
dgc_transform_get_rotation(t, &rx, &ry, &rz); |
319 |
dgc_transform_get_translation(t, &x, &y, &z); |
320 |
|
321 |
fprintf(fp,"RX RAD %lf\n", rx);
|
322 |
fprintf(fp,"RY RAD %lf\n", ry);
|
323 |
fprintf(fp,"RZ RAD %lf\n", rz);
|
324 |
fprintf(fp,"T M %lf %lf %lf\n", x, y, z);
|
325 |
|
326 |
fclose(fp); |
327 |
return 0; |
328 |
} |
329 |
|
330 |
void dgc_transform_get_translation(dgc_transform_t t, double *x, double *y, |
331 |
double *z)
|
332 |
{ |
333 |
*x = t[0][3]; |
334 |
*y = t[1][3]; |
335 |
*z = t[2][3]; |
336 |
} |
337 |
|
338 |
void dgc_transform_get_rotation(dgc_transform_t t, double *x, double *y, |
339 |
double *z)
|
340 |
{ |
341 |
*x = atan2(t[2][1], t[2][2]); |
342 |
*y = asin(-t[2][0]); |
343 |
*z = atan2(t[1][0], t[0][0]); |
344 |
} |
345 |
|
346 |
void dgc_transform_rpy(dgc_transform_t dest, dgc_transform_t src, double roll, |
347 |
double pitch, double yaw) |
348 |
{ |
349 |
double sinroll = sin(roll), cosroll = cos(roll);
|
350 |
double sinpitch = sin(pitch), cospitch = cos(pitch);
|
351 |
double sinyaw = sin(yaw), cosyaw = cos(yaw);
|
352 |
dgc_transform_t rot; |
353 |
int i, j, k;
|
354 |
|
355 |
/* construct rotation matrix by hand */
|
356 |
rot[0][0] = cosyaw * cospitch; |
357 |
rot[0][1] = cosyaw * sinpitch * sinroll - sinyaw * cosroll; |
358 |
rot[0][2] = cosyaw * sinpitch * cosroll + sinyaw * sinroll; |
359 |
rot[0][3] = 0; |
360 |
rot[1][0] = sinyaw * cospitch; |
361 |
rot[1][1] = sinyaw * sinpitch * sinroll + cosyaw * cosroll; |
362 |
rot[1][2] = sinyaw * sinpitch * cosroll - cosyaw * sinroll; |
363 |
rot[1][3] = 0; |
364 |
rot[2][0] = -sinpitch; |
365 |
rot[2][1] = cospitch * sinroll; |
366 |
rot[2][2] = cospitch * cosroll; |
367 |
rot[2][3] = 0; |
368 |
rot[3][0] = 0; |
369 |
rot[3][1] = 0; |
370 |
rot[3][2] = 0; |
371 |
rot[3][3] = 1; |
372 |
|
373 |
/* left multiply */
|
374 |
for(i = 0; i < 4; i++) |
375 |
for(j = 0; j < 4; j++) { |
376 |
dest[i][j] = 0;
|
377 |
for(k = 0; k < 4; k++) |
378 |
dest[i][j] += rot[i][k] * src[k][j]; |
379 |
} |
380 |
} |
381 |
|
382 |
void dgc_transform_inverse(dgc_transform_t in, dgc_transform_t out)
|
383 |
{ |
384 |
double temp, t1, t2, t3;
|
385 |
|
386 |
dgc_transform_copy(out, in); |
387 |
|
388 |
temp = out[0][1]; |
389 |
out[0][1] = out[1][0]; |
390 |
out[1][0] = temp; |
391 |
|
392 |
temp = out[0][2]; |
393 |
out[0][2] = out[2][0]; |
394 |
out[2][0] = temp; |
395 |
|
396 |
temp = out[1][2]; |
397 |
out[1][2] = out[2][1]; |
398 |
out[2][1] = temp; |
399 |
|
400 |
t1 = |
401 |
-out[0][0] * out[0][3] |
402 |
-out[0][1] * out[1][3] |
403 |
-out[0][2] * out[2][3]; |
404 |
t2 = |
405 |
-out[1][0] * out[0][3] |
406 |
-out[1][1] * out[1][3] |
407 |
-out[1][2] * out[2][3]; |
408 |
t3 = |
409 |
-out[2][0] * out[0][3] |
410 |
-out[2][1] * out[1][3] |
411 |
-out[2][2] * out[2][3]; |
412 |
|
413 |
out[0][3] = t1; |
414 |
out[1][3] = t2; |
415 |
out[2][3] = t3; |
416 |
} |
417 |
|