Statistics
| Branch: | Revision:

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