TYCamport3  3
TYCoordinateMapper.h
Go to the documentation of this file.
1 
6 #ifndef TY_COORDINATE_MAPPER_H_
7 #define TY_COORDINATE_MAPPER_H_
8 
9 #include <stdlib.h>
10 #include "TYApi.h"
11 
12 typedef struct TY_PIXEL_DESC
13 {
14  int16_t x; // x coordinate in pixels
15  int16_t y; // y coordinate in pixels
16  uint16_t depth; // depth value
17  uint16_t rsvd;
19 
20 typedef struct TY_PIXEL_COLOR_DESC
21 {
22  int16_t x; // x coordinate in pixels
23  int16_t y; // y coordinate in pixels
24  uint8_t bgr_ch1; // color info <channel 1>
25  uint8_t bgr_ch2; // color info <channel 2>
26  uint8_t bgr_ch3; // color info <channel 3>
27  uint8_t rsvd;
29 
30 // ------------------------------
31 // base convertion
32 // ------------------------------
33 
39 TY_CAPI TYInvertExtrinsic (const TY_CAMERA_EXTRINSIC* orgExtrinsic,
40  TY_CAMERA_EXTRINSIC* invExtrinsic);
41 
50 TY_CAPI TYMapDepthToPoint3d (const TY_CAMERA_CALIB_INFO* src_calib,
51  uint32_t depthW, uint32_t depthH,
52  const TY_PIXEL_DESC* depthPixels, uint32_t count,
53  TY_VECT_3F* point3d,
54  float f_scale_unit = 1.0f);
55 
64 TY_CAPI TYMapPoint3dToDepth (const TY_CAMERA_CALIB_INFO* dst_calib,
65  const TY_VECT_3F* point3d, uint32_t count,
66  uint32_t depthW, uint32_t depthH,
67  TY_PIXEL_DESC* depth,
68  float f_scale_unit = 1.0f);
69 
77 TY_CAPI TYMapDepthImageToPoint3d (const TY_CAMERA_CALIB_INFO* src_calib,
78  int32_t imageW, int32_t imageH,
79  const uint16_t* depth,
80  TY_VECT_3F* point3d,
81  float f_scale_unit = 1.0f);
82 
91 TY_CAPI TYMapPoint3dToDepthImage (const TY_CAMERA_CALIB_INFO* dst_calib,
92  const TY_VECT_3F* point3d, uint32_t count,
93  uint32_t depthW, uint32_t depthH, uint16_t* depth,
94  float f_target_scale = 1.0f);
95 
102 TY_CAPI TYMapPoint3dToPoint3d (const TY_CAMERA_EXTRINSIC* extrinsic,
103  const TY_VECT_3F* point3dFrom, int32_t count,
104  TY_VECT_3F* point3dTo);
105 
106 // ------------------------------
107 // inlines
108 // ------------------------------
109 
121 static inline TY_STATUS TYMapDepthToColorCoordinate(
122  const TY_CAMERA_CALIB_INFO* depth_calib,
123  uint32_t depthW, uint32_t depthH,
124  const TY_PIXEL_DESC* depth, uint32_t count,
125  const TY_CAMERA_CALIB_INFO* color_calib,
126  uint32_t mappedW, uint32_t mappedH,
127  TY_PIXEL_DESC* mappedDepth,
128  float f_scale_unit = 1.0f);
129 
140 static inline TY_STATUS TYMapDepthImageToColorCoordinate(
141  const TY_CAMERA_CALIB_INFO* depth_calib,
142  uint32_t depthW, uint32_t depthH, const uint16_t* depth,
143  const TY_CAMERA_CALIB_INFO* color_calib,
144  uint32_t mappedW, uint32_t mappedH, uint16_t* mappedDepth,
145  float f_scale_unit = 1.0f);
146 
157 static inline TY_STATUS TYCreateDepthToColorCoordinateLookupTable(
158  const TY_CAMERA_CALIB_INFO* depth_calib,
159  uint32_t depthW, uint32_t depthH, const uint16_t* depth,
160  const TY_CAMERA_CALIB_INFO* color_calib,
161  uint32_t mappedW, uint32_t mappedH,
162  TY_PIXEL_DESC* lut,
163  float f_scale_unit = 1.0f);
164 
179 static inline TY_STATUS TYMapRGBPixelsToDepthCoordinate(
180  const TY_CAMERA_CALIB_INFO* depth_calib,
181  uint32_t depthW, uint32_t depthH, const uint16_t* depth,
182  const TY_CAMERA_CALIB_INFO* color_calib,
183  uint32_t rgbW, uint32_t rgbH,
184  TY_PIXEL_COLOR_DESC* src, uint32_t cnt,
185  uint32_t min_distance,
186  uint32_t max_distance,
187  TY_PIXEL_COLOR_DESC* dst,
188  float f_scale_unit = 1.0f);
189 
201 static inline TY_STATUS TYMapRGBImageToDepthCoordinate(
202  const TY_CAMERA_CALIB_INFO* depth_calib,
203  uint32_t depthW, uint32_t depthH, const uint16_t* depth,
204  const TY_CAMERA_CALIB_INFO* color_calib,
205  uint32_t rgbW, uint32_t rgbH, const uint8_t* inRgb,
206  uint8_t* mappedRgb,
207  float f_scale_unit = 1.0f);
208 
220 static inline TY_STATUS TYMapRGB48ImageToDepthCoordinate(
221  const TY_CAMERA_CALIB_INFO* depth_calib,
222  uint32_t depthW, uint32_t depthH, const uint16_t* depth,
223  const TY_CAMERA_CALIB_INFO* color_calib,
224  uint32_t rgbW, uint32_t rgbH, const uint16_t* inRgb,
225  uint16_t* mappedRgb,
226  float f_scale_unit = 1.0f);
227 
239 static inline TY_STATUS TYMapMono16ImageToDepthCoordinate(
240  const TY_CAMERA_CALIB_INFO* depth_calib,
241  uint32_t depthW, uint32_t depthH, const uint16_t* depth,
242  const TY_CAMERA_CALIB_INFO* color_calib,
243  uint32_t rgbW, uint32_t rgbH, const uint16_t* gray,
244  uint16_t* mappedGray,
245  float f_scale_unit = 1.0f);
246 
247 
259 static inline TY_STATUS TYMapMono8ImageToDepthCoordinate(
260  const TY_CAMERA_CALIB_INFO* depth_calib,
261  uint32_t depthW, uint32_t depthH, const uint16_t* depth,
262  const TY_CAMERA_CALIB_INFO* color_calib,
263  uint32_t monoW, uint32_t monoH, const uint8_t* inMono,
264  uint8_t* mappedMono,
265  float f_scale_unit = 1.0f);
266 
267 
268 #define TYMAP_CHECKRET(f, bufToFree) \
269  do{ \
270  TY_STATUS err = (f); \
271  if(err){ \
272  if(bufToFree) \
273  free(bufToFree); \
274  return err; \
275  } \
276  } while(0)
277 
278 
279 static inline TY_STATUS TYMapDepthToColorCoordinate(
280  const TY_CAMERA_CALIB_INFO* depth_calib,
281  uint32_t depthW, uint32_t depthH,
282  const TY_PIXEL_DESC* depth, uint32_t count,
283  const TY_CAMERA_CALIB_INFO* color_calib,
284  uint32_t mappedW, uint32_t mappedH,
285  TY_PIXEL_DESC* mappedDepth,
286  float f_scale_unit)
287 {
288  TY_VECT_3F* p3d = (TY_VECT_3F*)malloc(sizeof(TY_VECT_3F) * count);
289  TYMAP_CHECKRET(TYMapDepthToPoint3d(depth_calib, depthW, depthH, depth, count, p3d, f_scale_unit), p3d );
290  TY_CAMERA_EXTRINSIC extri_inv;
291  TYMAP_CHECKRET(TYInvertExtrinsic(&color_calib->extrinsic, &extri_inv), p3d);
292  TYMAP_CHECKRET(TYMapPoint3dToPoint3d(&extri_inv, p3d, count, p3d), p3d );
293  TYMAP_CHECKRET(TYMapPoint3dToDepth(color_calib, p3d, count, mappedW, mappedH, mappedDepth, f_scale_unit), p3d );
294  free(p3d);
295  return TY_STATUS_OK;
296 }
297 
298 
299 static inline TY_STATUS TYMapDepthImageToColorCoordinate(
300  const TY_CAMERA_CALIB_INFO* depth_calib,
301  uint32_t depthW, uint32_t depthH, const uint16_t* depth,
302  const TY_CAMERA_CALIB_INFO* color_calib,
303  uint32_t mappedW, uint32_t mappedH, uint16_t* mappedDepth, float f_scale_unit)
304 {
305  TY_VECT_3F* p3d = (TY_VECT_3F*)malloc(sizeof(TY_VECT_3F) * depthW * depthH);
306  TYMAP_CHECKRET(TYMapDepthImageToPoint3d(depth_calib, depthW, depthH, depth, p3d, f_scale_unit), p3d);
307  TY_CAMERA_EXTRINSIC extri_inv;
308  TYMAP_CHECKRET(TYInvertExtrinsic(&color_calib->extrinsic, &extri_inv), p3d);
309  TYMAP_CHECKRET(TYMapPoint3dToPoint3d(&extri_inv, p3d, depthW * depthH, p3d), p3d);
310  TYMAP_CHECKRET(TYMapPoint3dToDepthImage(
311  color_calib, p3d, depthW * depthH, mappedW, mappedH, mappedDepth, f_scale_unit), p3d);
312  free(p3d);
313  return TY_STATUS_OK;
314 }
315 
316 static inline TY_STATUS TYMapRGBPixelsToDepthCoordinate(
317  const TY_CAMERA_CALIB_INFO* depth_calib,
318  uint32_t depthW, uint32_t depthH, const uint16_t* depth,
319  const TY_CAMERA_CALIB_INFO* color_calib,
320  uint32_t rgbW, uint32_t rgbH,
321  TY_PIXEL_COLOR_DESC* src, uint32_t cnt,
322  uint32_t min_distance,
323  uint32_t max_distance,
324  TY_PIXEL_COLOR_DESC* dst,
325  float f_scale_unit)
326 {
327  uint32_t m_distance_range = max_distance - min_distance;
328  TY_CAMERA_EXTRINSIC extri = color_calib->extrinsic;
329 
330  TY_PIXEL_DESC* pixels_array = (TY_PIXEL_DESC*)malloc(sizeof(TY_PIXEL_DESC) * m_distance_range);
331  TY_PIXEL_DESC* pixels_mapped_array = (TY_PIXEL_DESC*)malloc(sizeof(TY_PIXEL_DESC) * m_distance_range);
332  TY_VECT_3F* p3d_array = (TY_VECT_3F*)malloc(sizeof(TY_VECT_3F) * m_distance_range);
333  for (uint32_t i = 0; i < cnt; i++) {
334  for (uint32_t m = 0; m < m_distance_range; m++) {
335  pixels_array[m].x = src[i].x;
336  pixels_array[m].y = src[i].y;
337  pixels_array[m].depth = m + min_distance;
338  }
339 
340  TYMapDepthToPoint3d(color_calib, rgbW, rgbH, pixels_array, m_distance_range, &p3d_array[0], f_scale_unit);
341  TYMapPoint3dToPoint3d(&extri, &p3d_array[0], m_distance_range, &p3d_array[0]);
342 
343  TYMapPoint3dToDepth(depth_calib, p3d_array, m_distance_range, depthW, depthH, pixels_mapped_array, f_scale_unit);
344 
345  uint16_t m_min_delt = 0xffff;
346  dst[i].x = -1;
347  dst[i].y = -1;
348  for (uint32_t m = 0; m < m_distance_range; m++) {
349  int16_t pixel_x = pixels_mapped_array[m].x;
350  int16_t pixel_y = pixels_mapped_array[m].y;
351  uint16_t delt = abs(pixels_mapped_array[m].depth - depth[pixel_y*depthW + pixel_x]);
352  if (delt < m_min_delt) {
353  m_min_delt = delt;
354  if (m_min_delt < 10) {
355  dst[i].x = pixel_x;
356  dst[i].y = pixel_y;
357  dst[i].bgr_ch1 = src[i].bgr_ch1;
358  dst[i].bgr_ch2 = src[i].bgr_ch2;
359  dst[i].bgr_ch3 = src[i].bgr_ch3;
360  }
361  }
362  }
363  }
364 
365  free(pixels_array);
366  free(pixels_mapped_array);
367  free(p3d_array);
368 
369 
370  return TY_STATUS_OK;
371 }
372 
373 static inline TY_STATUS TYCreateDepthToColorCoordinateLookupTable(
374  const TY_CAMERA_CALIB_INFO* depth_calib,
375  uint32_t depthW, uint32_t depthH, const uint16_t* depth,
376  const TY_CAMERA_CALIB_INFO* color_calib,
377  uint32_t mappedW, uint32_t mappedH,
378  TY_PIXEL_DESC* lut,
379  float f_scale_unit)
380 {
381  TY_VECT_3F* p3d = (TY_VECT_3F*)malloc(sizeof(TY_VECT_3F) * depthW * depthH);
382  TYMAP_CHECKRET(TYMapDepthImageToPoint3d(depth_calib, depthW, depthH, depth, p3d, f_scale_unit), p3d);
383  TY_CAMERA_EXTRINSIC extri_inv;
384  TYMAP_CHECKRET(TYInvertExtrinsic(&color_calib->extrinsic, &extri_inv), p3d);
385  TYMAP_CHECKRET(TYMapPoint3dToPoint3d(&extri_inv, p3d, depthW * depthH, p3d), p3d);
386  TYMAP_CHECKRET(TYMapPoint3dToDepth(color_calib, p3d, depthW * depthH, mappedW, mappedH, lut, f_scale_unit), p3d );
387  free(p3d);
388  return TY_STATUS_OK;
389 }
390 
391 static inline TY_STATUS TYMapRGBImageToDepthCoordinate(
392  const TY_CAMERA_CALIB_INFO* depth_calib,
393  uint32_t depthW, uint32_t depthH, const uint16_t* depth,
394  const TY_CAMERA_CALIB_INFO* color_calib,
395  uint32_t rgbW, uint32_t rgbH, const uint8_t* inRgb,
396  uint8_t* mappedRgb, float f_scale_unit)
397 {
398  TY_PIXEL_DESC* lut = (TY_PIXEL_DESC*)malloc(sizeof(TY_PIXEL_DESC) * depthW * depthH);
399  TYMAP_CHECKRET(TYCreateDepthToColorCoordinateLookupTable(
400  depth_calib, depthW, depthH, depth,
401  color_calib, rgbW, rgbH, lut, f_scale_unit), lut);
402  for(uint32_t depthr = 0; depthr < depthH; depthr++)
403  for(uint32_t depthc = 0; depthc < depthW; depthc++)
404  {
405  TY_PIXEL_DESC* plut = &lut[depthr * depthW + depthc];
406  uint8_t* outPtr = &mappedRgb[depthW * depthr * 3 + depthc * 3];
407  if(plut->x < 0 || plut->x >= (int)rgbW || plut->y < 0 || plut->y >= (int)rgbH){
408  outPtr[0] = outPtr[1] = outPtr[2] = 0;
409  } else {
410  const uint8_t* inPtr = &inRgb[rgbW * plut->y * 3 + plut->x * 3];
411  outPtr[0] = inPtr[0];
412  outPtr[1] = inPtr[1];
413  outPtr[2] = inPtr[2];
414  }
415  }
416  free(lut);
417  return TY_STATUS_OK;
418 }
419 
420 static inline TY_STATUS TYMapRGB48ImageToDepthCoordinate(
421  const TY_CAMERA_CALIB_INFO* depth_calib,
422  uint32_t depthW, uint32_t depthH, const uint16_t* depth,
423  const TY_CAMERA_CALIB_INFO* color_calib,
424  uint32_t rgbW, uint32_t rgbH, const uint16_t* inRgb,
425  uint16_t* mappedRgb, float f_scale_unit)
426 {
427  TY_PIXEL_DESC* lut = (TY_PIXEL_DESC*)malloc(sizeof(TY_PIXEL_DESC) * depthW * depthH);
428  TYMAP_CHECKRET(TYCreateDepthToColorCoordinateLookupTable(
429  depth_calib, depthW, depthH, depth,
430  color_calib, rgbW, rgbH, lut, f_scale_unit), lut);
431  for(uint32_t depthr = 0; depthr < depthH; depthr++)
432  for(uint32_t depthc = 0; depthc < depthW; depthc++)
433  {
434  TY_PIXEL_DESC* plut = &lut[depthr * depthW + depthc];
435  uint16_t* outPtr = &mappedRgb[depthW * depthr * 3 + depthc * 3];
436  if(plut->x < 0 || plut->x >= (int)rgbW || plut->y < 0 || plut->y >= (int)rgbH){
437  outPtr[0] = outPtr[1] = outPtr[2] = 0;
438  } else {
439  const uint16_t* inPtr = &inRgb[rgbW * plut->y * 3 + plut->x * 3];
440  outPtr[0] = inPtr[0];
441  outPtr[1] = inPtr[1];
442  outPtr[2] = inPtr[2];
443  }
444  }
445  free(lut);
446  return TY_STATUS_OK;
447 }
448 
449 static inline TY_STATUS TYMapMono16ImageToDepthCoordinate(
450  const TY_CAMERA_CALIB_INFO* depth_calib,
451  uint32_t depthW, uint32_t depthH, const uint16_t* depth,
452  const TY_CAMERA_CALIB_INFO* color_calib,
453  uint32_t rgbW, uint32_t rgbH, const uint16_t* gray,
454  uint16_t* mappedGray, float f_scale_unit)
455 {
456  TY_PIXEL_DESC* lut = (TY_PIXEL_DESC*)malloc(sizeof(TY_PIXEL_DESC) * depthW * depthH);
457  TYMAP_CHECKRET(TYCreateDepthToColorCoordinateLookupTable(
458  depth_calib, depthW, depthH, depth,
459  color_calib, rgbW, rgbH, lut, f_scale_unit), lut);
460  for(uint32_t depthr = 0; depthr < depthH; depthr++)
461  for(uint32_t depthc = 0; depthc < depthW; depthc++)
462  {
463  TY_PIXEL_DESC* plut = &lut[depthr * depthW + depthc];
464  uint16_t* outPtr = &mappedGray[depthW * depthr + depthc];
465  if(plut->x < 0 || plut->x >= (int)rgbW || plut->y < 0 || plut->y >= (int)rgbH){
466  outPtr[0] = 0;
467  } else {
468  const uint16_t* inPtr = &gray[rgbW * plut->y + plut->x];
469  outPtr[0] = inPtr[0];
470  }
471  }
472  free(lut);
473  return TY_STATUS_OK;
474 }
475 
476 static inline TY_STATUS TYMapMono8ImageToDepthCoordinate(
477  const TY_CAMERA_CALIB_INFO* depth_calib,
478  uint32_t depthW, uint32_t depthH, const uint16_t* depth,
479  const TY_CAMERA_CALIB_INFO* color_calib,
480  uint32_t monoW, uint32_t monoH, const uint8_t* inMono,
481  uint8_t* mappedMono, float f_scale_unit)
482 {
483  TY_PIXEL_DESC* lut = (TY_PIXEL_DESC*)malloc(sizeof(TY_PIXEL_DESC) * depthW * depthH);
484  TYMAP_CHECKRET(TYCreateDepthToColorCoordinateLookupTable(
485  depth_calib, depthW, depthH, depth,
486  color_calib, monoW, monoH, lut, f_scale_unit), lut);
487  for(uint32_t depthr = 0; depthr < depthH; depthr++)
488  for(uint32_t depthc = 0; depthc < depthW; depthc++)
489  {
490  TY_PIXEL_DESC* plut = &lut[depthr * depthW + depthc];
491  uint8_t* outPtr = &mappedMono[depthW * depthr + depthc];
492  if(plut->x < 0 || plut->x >= (int)monoW || plut->y < 0 || plut->y >= (int)monoH){
493  outPtr[0] = 0;
494  } else {
495  const uint8_t* inPtr = &inMono[monoW * plut->y + plut->x];
496  outPtr[0] = inPtr[0];
497  }
498  }
499  free(lut);
500  return TY_STATUS_OK;
501 }
502 
503 
504 #endif
TY_CAPI TYInvertExtrinsic(const TY_CAMERA_EXTRINSIC *orgExtrinsic, TY_CAMERA_EXTRINSIC *invExtrinsic)
Calculate 4x4 extrinsic matrix&#39;s inverse matrix.
TY_CAPI TYMapPoint3dToDepthImage(const TY_CAMERA_CALIB_INFO *dst_calib, const TY_VECT_3F *point3d, uint32_t count, uint32_t depthW, uint32_t depthH, uint16_t *depth, float f_target_scale=1.0f)
Map 3D points to depth image. (NAN, NAN, NAN) will be skipped.
TY_CAPI TYMapPoint3dToPoint3d(const TY_CAMERA_EXTRINSIC *extrinsic, const TY_VECT_3F *point3dFrom, int32_t count, TY_VECT_3F *point3dTo)
Map 3D points to another coordinate.
TY_CAPI TYMapDepthToPoint3d(const TY_CAMERA_CALIB_INFO *src_calib, uint32_t depthW, uint32_t depthH, const TY_PIXEL_DESC *depthPixels, uint32_t count, TY_VECT_3F *point3d, float f_scale_unit=1.0f)
Map pixels on depth image to 3D points.
TY_CAPI TYMapDepthImageToPoint3d(const TY_CAMERA_CALIB_INFO *src_calib, int32_t imageW, int32_t imageH, const uint16_t *depth, TY_VECT_3F *point3d, float f_scale_unit=1.0f)
Map depth image to 3D points. 0 depth pixels maps to (NAN, NAN, NAN).
TY_CAPI TYMapPoint3dToDepth(const TY_CAMERA_CALIB_INFO *dst_calib, const TY_VECT_3F *point3d, uint32_t count, uint32_t depthW, uint32_t depthH, TY_PIXEL_DESC *depth, float f_scale_unit=1.0f)
Map 3D points to pixels on depth image. Reverse operation of TYMapDepthToPoint3d. ...
TYApi.h includes camera control and data receiving interface, which supports configuration for image ...