root / rgbdslam / gicp / transform.cpp @ 9240aaa3
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 