registration.c
Go to the documentation of this file.
1 /*
2  * This file is part of the OpenKinect Project. http://www.openkinect.org
3  *
4  * Copyright (c) 2011 individual OpenKinect contributors. See the CONTRIB file
5  * for details.
6  *
7  * This code is licensed to you under the terms of the Apache License, version
8  * 2.0, or, at your option, the terms of the GNU General Public License,
9  * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses,
10  * or the following URLs:
11  * http://www.apache.org/licenses/LICENSE-2.0
12  * http://www.gnu.org/licenses/gpl-2.0.txt
13  *
14  * If you redistribute this file in source form, modified or unmodified, you
15  * may:
16  * 1) Leave this header intact and distribute it under the same terms,
17  * accompanying it with the APACHE20 and GPL20 files, or
18  * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or
19  * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file
20  * In all cases you must keep the copyright notice intact and include a copy
21  * of the CONTRIB file.
22  *
23  * Binary distributions must follow the binary distribution requirements of
24  * either License.
25  */
26 
27 #include "libfreenect.h"
28 #include "freenect_internal.h"
29 #include "registration.h"
30 #include <stdlib.h>
31 #include <string.h>
32 #include <stdio.h>
33 #include <math.h>
34 
35 
36 #define REG_X_VAL_SCALE 256 // "fixed-point" precision for double -> int32_t conversion
37 
38 #define S2D_PIXEL_CONST 10
39 #define S2D_CONST_OFFSET 0.375
40 
41 #define DEPTH_SENSOR_X_RES 1280
42 #define DEPTH_MIRROR_X 0
43 
44 #define DEPTH_MAX_METRIC_VALUE FREENECT_DEPTH_MM_MAX_VALUE
45 #define DEPTH_NO_MM_VALUE FREENECT_DEPTH_MM_NO_VALUE
46 #define DEPTH_MAX_RAW_VALUE FREENECT_DEPTH_RAW_MAX_VALUE
47 #define DEPTH_NO_RAW_VALUE FREENECT_DEPTH_RAW_NO_VALUE
48 
49 #define DEPTH_X_OFFSET 1
50 #define DEPTH_Y_OFFSET 1
51 #define DEPTH_X_RES 640
52 #define DEPTH_Y_RES 480
53 
54 // try to fill single empty pixels AKA "salt-and-pepper noise"
55 // disabled by default, noise removal better handled in later stages
56 // #define DENSE_REGISTRATION
57 
58 
61 {
63 
64  double pixel_size = 1.0 / (zpi->reference_pixel_size * x_scale * S2D_PIXEL_CONST);
65  double pixels_between_rgb_and_ir_cmos = zpi->dcmos_rcmos_dist * pixel_size * S2D_PIXEL_CONST;
66  double reference_distance_in_pixels = zpi->reference_distance * pixel_size * S2D_PIXEL_CONST;
67 
68  memset(depth_to_rgb, DEPTH_NO_MM_VALUE, DEPTH_MAX_METRIC_VALUE * sizeof(int32_t));
69 
70  for (i = 0; i < DEPTH_MAX_METRIC_VALUE; i++) {
71  double current_depth_in_pixels = i * pixel_size;
72  depth_to_rgb[i] = (( pixels_between_rgb_and_ir_cmos * (current_depth_in_pixels - reference_distance_in_pixels) / current_depth_in_pixels) + S2D_CONST_OFFSET) * REG_X_VAL_SCALE;
73  }
74 }
75 
76 // unrolled inner loop of the 11-bit unpacker
77 static inline void unpack_8_pixels(uint8_t *raw, uint16_t *frame)
78 {
79  uint16_t baseMask = 0x7FF;
80 
81  uint8_t r0 = *(raw+0);
82  uint8_t r1 = *(raw+1);
83  uint8_t r2 = *(raw+2);
84  uint8_t r3 = *(raw+3);
85  uint8_t r4 = *(raw+4);
86  uint8_t r5 = *(raw+5);
87  uint8_t r6 = *(raw+6);
88  uint8_t r7 = *(raw+7);
89  uint8_t r8 = *(raw+8);
90  uint8_t r9 = *(raw+9);
91  uint8_t r10 = *(raw+10);
92 
93  frame[0] = (r0<<3) | (r1>>5);
94  frame[1] = ((r1<<6) | (r2>>2) ) & baseMask;
95  frame[2] = ((r2<<9) | (r3<<1) | (r4>>7) ) & baseMask;
96  frame[3] = ((r4<<4) | (r5>>4) ) & baseMask;
97  frame[4] = ((r5<<7) | (r6>>1) ) & baseMask;
98  frame[5] = ((r6<<10) | (r7<<2) | (r8>>6) ) & baseMask;
99  frame[6] = ((r8<<5) | (r9>>3) ) & baseMask;
100  frame[7] = ((r9<<8) | (r10) ) & baseMask;
101 }
102 
103 // apply registration data to a single packed frame
105 {
106  freenect_registration* reg = &(dev->registration);
107  // set output buffer to zero using pointer-sized memory access (~ 30-40% faster than memset)
108  size_t i, *wipe = (size_t*)output_mm;
109  for (i = 0; i < DEPTH_X_RES * DEPTH_Y_RES * sizeof(uint16_t) / sizeof(size_t); i++) wipe[i] = DEPTH_NO_MM_VALUE;
110 
111  uint16_t unpack[8];
112 
113  uint32_t target_offset = DEPTH_Y_RES * reg->reg_pad_info.start_lines;
114  uint32_t x,y,source_index = 8;
115 
116  for (y = 0; y < DEPTH_Y_RES; y++) {
117  for (x = 0; x < DEPTH_X_RES; x++) {
118 
119  // get 8 pixels from the packed frame
120  if (source_index == 8) {
121  unpack_8_pixels( input_packed, unpack );
122  source_index = 0;
123  input_packed += 11;
124  }
125 
126  // get the value at the current depth pixel, convert to millimeters
127  uint16_t metric_depth = reg->raw_to_mm_shift[ unpack[source_index++] ];
128 
129  // so long as the current pixel has a depth value
130  if (metric_depth == DEPTH_NO_MM_VALUE) continue;
131  if (metric_depth >= DEPTH_MAX_METRIC_VALUE) continue;
132 
133  // calculate the new x and y location for that pixel
134  // using registration_table for the basic rectification
135  // and depth_to_rgb_shift for determining the x shift
136  uint32_t reg_index = DEPTH_MIRROR_X ? ((y + 1) * DEPTH_X_RES - x - 1) : (y * DEPTH_X_RES + x);
137  uint32_t nx = (reg->registration_table[reg_index][0] + reg->depth_to_rgb_shift[metric_depth]) / REG_X_VAL_SCALE;
138  uint32_t ny = reg->registration_table[reg_index][1];
139 
140  // ignore anything outside the image bounds
141  if (nx >= DEPTH_X_RES) continue;
142 
143  // convert nx, ny to an index in the depth image array
144  uint32_t target_index = (DEPTH_MIRROR_X ? ((ny + 1) * DEPTH_X_RES - nx - 1) : (ny * DEPTH_X_RES + nx)) - target_offset;
145 
146  // get the current value at the new location
147  uint16_t current_depth = output_mm[target_index];
148 
149  // make sure the new location is empty, or the new value is closer
150  if ((current_depth == DEPTH_NO_MM_VALUE) || (current_depth > metric_depth)) {
151  output_mm[target_index] = metric_depth; // always save depth at current location
152 
153  #ifdef DENSE_REGISTRATION
154  // if we're not on the first row, or the first column
155  if ((nx > 0) && (ny > 0)) {
156  output_mm[target_index - DEPTH_X_RES ] = metric_depth; // save depth at (x,y-1)
157  output_mm[target_index - DEPTH_X_RES - 1] = metric_depth; // save depth at (x-1,y-1)
158  output_mm[target_index - 1] = metric_depth; // save depth at (x-1,y)
159  } else if (ny > 0) {
160  output_mm[target_index - DEPTH_X_RES] = metric_depth; // save depth at (x,y-1)
161  } else if (nx > 0) {
162  output_mm[target_index - 1] = metric_depth; // save depth at (x-1,y)
163  }
164  #endif
165  }
166  }
167  }
168  return 0;
169 }
170 
171 // Same as freenect_apply_registration, but don't bother aligning to the RGB image
173 {
174  freenect_registration* reg = &(dev->registration);
175  uint16_t unpack[8];
176  uint32_t x,y,source_index = 8;
177  for (y = 0; y < DEPTH_Y_RES; y++) {
178  for (x = 0; x < DEPTH_X_RES; x++) {
179  // get 8 pixels from the packed frame
180  if (source_index == 8) {
181  unpack_8_pixels( input_packed, unpack );
182  source_index = 0;
183  input_packed += 11;
184  }
185  // get the value at the current depth pixel, convert to millimeters
186  uint16_t metric_depth = reg->raw_to_mm_shift[ unpack[source_index++] ];
187  output_mm[y * DEPTH_X_RES + x] = metric_depth < DEPTH_MAX_METRIC_VALUE ? metric_depth : DEPTH_MAX_METRIC_VALUE;
188  }
189  }
190  return 0;
191 }
192 
193 // create temporary x/y shift tables
194 static void freenect_create_dxdy_tables(double* reg_x_table, double* reg_y_table, int32_t resolution_x, int32_t resolution_y, freenect_reg_info* regdata )
195 {
196 
197  int64_t AX6 = regdata->ax;
198  int64_t BX6 = regdata->bx;
199  int64_t CX2 = regdata->cx;
200  int64_t DX2 = regdata->dx;
201 
202  int64_t AY6 = regdata->ay;
203  int64_t BY6 = regdata->by;
204  int64_t CY2 = regdata->cy;
205  int64_t DY2 = regdata->dy;
206 
207  // don't merge the shift operations - necessary for proper 32-bit clamping of extracted values
208  int64_t dX0 = (regdata->dx_start << 13) >> 4;
209  int64_t dY0 = (regdata->dy_start << 13) >> 4;
210 
211  int64_t dXdX0 = (regdata->dxdx_start << 11) >> 3;
212  int64_t dXdY0 = (regdata->dxdy_start << 11) >> 3;
213  int64_t dYdX0 = (regdata->dydx_start << 11) >> 3;
214  int64_t dYdY0 = (regdata->dydy_start << 11) >> 3;
215 
216  int64_t dXdXdX0 = (regdata->dxdxdx_start << 5) << 3;
217  int64_t dYdXdX0 = (regdata->dydxdx_start << 5) << 3;
218  int64_t dYdXdY0 = (regdata->dydxdy_start << 5) << 3;
219  int64_t dXdXdY0 = (regdata->dxdxdy_start << 5) << 3;
220  int64_t dYdYdX0 = (regdata->dydydx_start << 5) << 3;
221  int64_t dYdYdY0 = (regdata->dydydy_start << 5) << 3;
222 
223  int32_t row,col,tOffs = 0;
224 
225  for (row = 0 ; row < resolution_y ; row++) {
226 
227  dXdXdX0 += CX2;
228 
229  dXdX0 += dYdXdX0 >> 8;
230  dYdXdX0 += DX2;
231 
232  dX0 += dYdX0 >> 6;
233  dYdX0 += dYdYdX0 >> 8;
234  dYdYdX0 += BX6;
235 
236  dXdXdY0 += CY2;
237 
238  dXdY0 += dYdXdY0 >> 8;
239  dYdXdY0 += DY2;
240 
241  dY0 += dYdY0 >> 6;
242  dYdY0 += dYdYdY0 >> 8;
243  dYdYdY0 += BY6;
244 
245  int64_t coldXdXdY0 = dXdXdY0, coldXdY0 = dXdY0, coldY0 = dY0;
246 
247  int64_t coldXdXdX0 = dXdXdX0, coldXdX0 = dXdX0, coldX0 = dX0;
248 
249  for (col = 0 ; col < resolution_x ; col++, tOffs++) {
250  reg_x_table[tOffs] = coldX0 * (1.0/(1<<17));
251  reg_y_table[tOffs] = coldY0 * (1.0/(1<<17));
252 
253  coldX0 += coldXdX0 >> 6;
254  coldXdX0 += coldXdXdX0 >> 8;
255  coldXdXdX0 += AX6;
256 
257  coldY0 += coldXdY0 >> 6;
258  coldXdY0 += coldXdXdY0 >> 8;
259  coldXdXdY0 += AY6;
260  }
261  }
262 }
263 
264 static void freenect_init_registration_table(int32_t (*registration_table)[2], freenect_reg_info* reg_info) {
265 
266  double* regtable_dx = (double*)malloc(DEPTH_X_RES*DEPTH_Y_RES*sizeof(double));
267  double* regtable_dy = (double*)malloc(DEPTH_X_RES*DEPTH_Y_RES*sizeof(double));
268  memset(regtable_dx, 0, DEPTH_X_RES*DEPTH_Y_RES * sizeof(double));
269  memset(regtable_dy, 0, DEPTH_X_RES*DEPTH_Y_RES * sizeof(double));
270  int32_t x,y,index = 0;
271 
272  // create temporary dx/dy tables
273  freenect_create_dxdy_tables( regtable_dx, regtable_dy, DEPTH_X_RES, DEPTH_Y_RES, reg_info );
274 
275  for (y = 0; y < DEPTH_Y_RES; y++) {
276  for (x = 0; x < DEPTH_X_RES; x++, index++) {
277  double new_x = x + regtable_dx[index] + DEPTH_X_OFFSET;
278  double new_y = y + regtable_dy[index] + DEPTH_Y_OFFSET;
279 
280  if ((new_x < 0) || (new_y < 0) || (new_x >= DEPTH_X_RES) || (new_y >= DEPTH_Y_RES))
281  new_x = 2 * DEPTH_X_RES; // intentionally set value outside image bounds
282 
283  registration_table[index][0] = new_x * REG_X_VAL_SCALE;
284  registration_table[index][1] = new_y;
285  }
286  }
287  free(regtable_dx);
288  free(regtable_dy);
289 }
290 
291 // These are just constants.
292 static double parameter_coefficient = 4;
293 static double shift_scale = 10;
294 static double pixel_size_factor = 1;
295 
298 {
300  double fixed_ref_x = ((raw - (parameter_coefficient * reg->const_shift / pixel_size_factor)) / parameter_coefficient) - S2D_CONST_OFFSET;
301  double metric = fixed_ref_x * zpi->reference_pixel_size * pixel_size_factor;
302  return shift_scale * ((metric * zpi->reference_distance / (zpi->dcmos_emitter_dist - metric)) + zpi->reference_distance);
303 }
304 
307  uint16_t i;
308  for (i = 0; i < DEPTH_MAX_RAW_VALUE; i++)
309  reg->raw_to_mm_shift[i] = freenect_raw_to_mm( i, reg);
311 
313 
315 }
316 
318 void freenect_camera_to_world(freenect_device* dev, int cx, int cy, int wz, double* wx, double* wy)
319 {
320  double ref_pix_size = dev->registration.zero_plane_info.reference_pixel_size;
321  double ref_distance = dev->registration.zero_plane_info.reference_distance;
322  // We multiply cx and cy by these factors because they come from a 640x480 image,
323  // but the zero plane pixel size is for a 1280x1024 image.
324  // However, the 640x480 image is produced by cropping the 1280x1024 image
325  // to 1280x960 and then scaling by .5, so aspect ratio is maintained, and
326  // we should simply multiply by two in each dimension.
327  double factor = 2 * ref_pix_size * wz / ref_distance;
328  *wx = (double)(cx - DEPTH_X_RES/2) * factor;
329  *wy = (double)(cy - DEPTH_Y_RES/2) * factor;
330 }
331 
336 {
337  freenect_registration* reg = &(dev->registration);
338 
339  // Ensure that we free the previous tables before dropping the pointers, if there were any.
341 
342  // Allocate tables.
343  reg->raw_to_mm_shift = (uint16_t*)malloc( sizeof(uint16_t) * DEPTH_MAX_RAW_VALUE );
344  reg->depth_to_rgb_shift = (int32_t*)malloc( sizeof( int32_t) * DEPTH_MAX_METRIC_VALUE );
345  reg->registration_table = (int32_t (*)[2])malloc( sizeof( int32_t) * DEPTH_X_RES * DEPTH_Y_RES * 2 );
346 
347  // Fill tables.
348  complete_tables(reg);
349 
350  return 0;
351 }
352 
354 {
355  freenect_registration retval;
356  retval.reg_info = dev->registration.reg_info;
357  retval.reg_pad_info = dev->registration.reg_pad_info;
359  retval.const_shift = dev->registration.const_shift;
360  retval.raw_to_mm_shift = (uint16_t*)malloc( sizeof(uint16_t) * DEPTH_MAX_RAW_VALUE );
361  retval.depth_to_rgb_shift = (int32_t*)malloc( sizeof( int32_t) * DEPTH_MAX_METRIC_VALUE );
362  retval.registration_table = (int32_t (*)[2])malloc( sizeof( int32_t) * DEPTH_X_RES * DEPTH_Y_RES * 2 );
363  complete_tables(&retval);
364  return retval;
365 }
366 
368 {
369  if (reg->raw_to_mm_shift) {
370  free(reg->raw_to_mm_shift);
371  reg->raw_to_mm_shift = NULL;
372  }
373  if (reg->depth_to_rgb_shift) {
374  free(reg->depth_to_rgb_shift);
375  reg->depth_to_rgb_shift = NULL;
376  }
377  if (reg->registration_table) {
378  free(reg->registration_table);
379  reg->registration_table = NULL;
380  }
381  return 0;
382 }
#define S2D_CONST_OFFSET
Definition: registration.c:39
#define DEPTH_Y_OFFSET
Definition: registration.c:50
FN_INTERNAL int freenect_init_registration(freenect_device *dev)
Definition: registration.c:335
freenect_zero_plane_info zero_plane_info
freenect_registration registration
void freenect_camera_to_world(freenect_device *dev, int cx, int cy, int wz, double *wx, double *wy)
camera -> world coordinate helper function
Definition: registration.c:318
int freenect_destroy_registration(freenect_registration *reg)
Definition: registration.c:367
unsigned short uint16_t
static void freenect_init_depth_to_rgb(int32_t *depth_to_rgb, freenect_zero_plane_info *zpi)
fill the table of horizontal shift values for metric depth -> RGB conversion
Definition: registration.c:60
#define REG_X_VAL_SCALE
Definition: registration.c:36
static void complete_tables(freenect_registration *reg)
Compute registration tables.
Definition: registration.c:306
#define DEPTH_X_OFFSET
Definition: registration.c:49
static void freenect_init_registration_table(int32_t(*registration_table)[2], freenect_reg_info *reg_info)
Definition: registration.c:264
#define DEPTH_Y_RES
Definition: registration.c:52
unsigned char uint8_t
__int64 int64_t
int frame
Definition: regview.c:72
static double pixel_size_factor
Definition: registration.c:294
internal Kinect zero plane data
static void freenect_create_dxdy_tables(double *reg_x_table, double *reg_y_table, int32_t resolution_x, int32_t resolution_y, freenect_reg_info *regdata)
Definition: registration.c:194
#define DEPTH_MAX_RAW_VALUE
Definition: registration.c:46
static void unpack_8_pixels(uint8_t *raw, uint16_t *frame)
Definition: registration.c:77
int int32_t
#define DEPTH_SENSOR_X_RES
Definition: registration.c:41
all data needed for depth->RGB mapping
#define DEPTH_MAX_METRIC_VALUE
Definition: registration.c:44
#define DEPTH_X_RES
Definition: registration.c:51
static double parameter_coefficient
Definition: registration.c:292
static double shift_scale
Definition: registration.c:293
#define DEPTH_NO_RAW_VALUE
Definition: registration.c:47
#define S2D_PIXEL_CONST
Definition: registration.c:38
static uint16_t freenect_raw_to_mm(uint16_t raw, freenect_registration *reg)
convert raw shift value to metric depth (in mm)
Definition: registration.c:297
#define FN_INTERNAL
unsigned int uint32_t
freenect_reg_pad_info reg_pad_info
#define DEPTH_MIRROR_X
Definition: registration.c:42
FN_INTERNAL int freenect_apply_depth_to_mm(freenect_device *dev, uint8_t *input_packed, uint16_t *output_mm)
Definition: registration.c:172
#define DEPTH_NO_MM_VALUE
Definition: registration.c:45
freenect_registration freenect_copy_registration(freenect_device *dev)
Definition: registration.c:353
FN_INTERNAL int freenect_apply_registration(freenect_device *dev, uint8_t *input_packed, uint16_t *output_mm)
Definition: registration.c:104


libfreenect
Author(s): Hector Martin, Josh Blake, Kyle Machulis, OpenKinect community
autogenerated on Mon Jun 10 2019 13:46:42