root / rgbdslam / gicp / transform.cpp @ 9240aaa3
History | View | Annotate | Download (9.48 KB)
| 1 | 9240aaa3 | Alex | #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 | } |