6 #ifndef TY_COORDINATE_MAPPER_H_ 7 #define TY_COORDINATE_MAPPER_H_ 51 uint32_t depthW, uint32_t depthH,
54 float f_scale_unit = 1.0f);
66 uint32_t depthW, uint32_t depthH,
68 float f_scale_unit = 1.0f);
78 int32_t imageW, int32_t imageH,
79 const uint16_t* depth,
81 float f_scale_unit = 1.0f);
93 uint32_t depthW, uint32_t depthH, uint16_t* depth,
94 float f_target_scale = 1.0f);
121 static inline TY_STATUS TYMapDepthToColorCoordinate(
123 uint32_t depthW, uint32_t depthH,
126 uint32_t mappedW, uint32_t mappedH,
128 float f_scale_unit = 1.0f);
140 static inline TY_STATUS TYMapDepthImageToColorCoordinate(
142 uint32_t depthW, uint32_t depthH,
const uint16_t* depth,
144 uint32_t mappedW, uint32_t mappedH, uint16_t* mappedDepth,
145 float f_scale_unit = 1.0f);
157 static inline TY_STATUS TYCreateDepthToColorCoordinateLookupTable(
159 uint32_t depthW, uint32_t depthH,
const uint16_t* depth,
161 uint32_t mappedW, uint32_t mappedH,
163 float f_scale_unit = 1.0f);
179 static inline TY_STATUS TYMapRGBPixelsToDepthCoordinate(
181 uint32_t depthW, uint32_t depthH,
const uint16_t* depth,
183 uint32_t rgbW, uint32_t rgbH,
185 uint32_t min_distance,
186 uint32_t max_distance,
188 float f_scale_unit = 1.0f);
201 static inline TY_STATUS TYMapRGBImageToDepthCoordinate(
203 uint32_t depthW, uint32_t depthH,
const uint16_t* depth,
205 uint32_t rgbW, uint32_t rgbH,
const uint8_t* inRgb,
207 float f_scale_unit = 1.0f);
220 static inline TY_STATUS TYMapRGB48ImageToDepthCoordinate(
222 uint32_t depthW, uint32_t depthH,
const uint16_t* depth,
224 uint32_t rgbW, uint32_t rgbH,
const uint16_t* inRgb,
226 float f_scale_unit = 1.0f);
239 static inline TY_STATUS TYMapMono16ImageToDepthCoordinate(
241 uint32_t depthW, uint32_t depthH,
const uint16_t* depth,
243 uint32_t rgbW, uint32_t rgbH,
const uint16_t* gray,
244 uint16_t* mappedGray,
245 float f_scale_unit = 1.0f);
259 static inline TY_STATUS TYMapMono8ImageToDepthCoordinate(
261 uint32_t depthW, uint32_t depthH,
const uint16_t* depth,
263 uint32_t monoW, uint32_t monoH,
const uint8_t* inMono,
265 float f_scale_unit = 1.0f);
268 #define TYMAP_CHECKRET(f, bufToFree) \ 270 TY_STATUS err = (f); \ 279 static inline TY_STATUS TYMapDepthToColorCoordinate(
281 uint32_t depthW, uint32_t depthH,
284 uint32_t mappedW, uint32_t mappedH,
289 TYMAP_CHECKRET(
TYMapDepthToPoint3d(depth_calib, depthW, depthH, depth, count, p3d, f_scale_unit), p3d );
293 TYMAP_CHECKRET(
TYMapPoint3dToDepth(color_calib, p3d, count, mappedW, mappedH, mappedDepth, f_scale_unit), p3d );
299 static inline TY_STATUS TYMapDepthImageToColorCoordinate(
301 uint32_t depthW, uint32_t depthH,
const uint16_t* depth,
303 uint32_t mappedW, uint32_t mappedH, uint16_t* mappedDepth,
float f_scale_unit)
311 color_calib, p3d, depthW * depthH, mappedW, mappedH, mappedDepth, f_scale_unit), p3d);
316 static inline TY_STATUS TYMapRGBPixelsToDepthCoordinate(
318 uint32_t depthW, uint32_t depthH,
const uint16_t* depth,
320 uint32_t rgbW, uint32_t rgbH,
322 uint32_t min_distance,
323 uint32_t max_distance,
327 uint32_t m_distance_range = max_distance - min_distance;
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;
340 TYMapDepthToPoint3d(color_calib, rgbW, rgbH, pixels_array, m_distance_range, &p3d_array[0], f_scale_unit);
343 TYMapPoint3dToDepth(depth_calib, p3d_array, m_distance_range, depthW, depthH, pixels_mapped_array, f_scale_unit);
345 uint16_t m_min_delt = 0xffff;
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) {
354 if (m_min_delt < 10) {
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;
366 free(pixels_mapped_array);
373 static inline TY_STATUS TYCreateDepthToColorCoordinateLookupTable(
375 uint32_t depthW, uint32_t depthH,
const uint16_t* depth,
377 uint32_t mappedW, uint32_t mappedH,
386 TYMAP_CHECKRET(
TYMapPoint3dToDepth(color_calib, p3d, depthW * depthH, mappedW, mappedH, lut, f_scale_unit), p3d );
391 static inline TY_STATUS TYMapRGBImageToDepthCoordinate(
393 uint32_t depthW, uint32_t depthH,
const uint16_t* depth,
395 uint32_t rgbW, uint32_t rgbH,
const uint8_t* inRgb,
396 uint8_t* mappedRgb,
float f_scale_unit)
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++)
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;
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];
420 static inline TY_STATUS TYMapRGB48ImageToDepthCoordinate(
422 uint32_t depthW, uint32_t depthH,
const uint16_t* depth,
424 uint32_t rgbW, uint32_t rgbH,
const uint16_t* inRgb,
425 uint16_t* mappedRgb,
float f_scale_unit)
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++)
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;
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];
449 static inline TY_STATUS TYMapMono16ImageToDepthCoordinate(
451 uint32_t depthW, uint32_t depthH,
const uint16_t* depth,
453 uint32_t rgbW, uint32_t rgbH,
const uint16_t* gray,
454 uint16_t* mappedGray,
float f_scale_unit)
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++)
464 uint16_t* outPtr = &mappedGray[depthW * depthr + depthc];
465 if(plut->x < 0 || plut->x >= (
int)rgbW || plut->y < 0 || plut->y >= (
int)rgbH){
468 const uint16_t* inPtr = &gray[rgbW * plut->y + plut->x];
469 outPtr[0] = inPtr[0];
476 static inline TY_STATUS TYMapMono8ImageToDepthCoordinate(
478 uint32_t depthW, uint32_t depthH,
const uint16_t* depth,
480 uint32_t monoW, uint32_t monoH,
const uint8_t* inMono,
481 uint8_t* mappedMono,
float f_scale_unit)
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++)
491 uint8_t* outPtr = &mappedMono[depthW * depthr + depthc];
492 if(plut->x < 0 || plut->x >= (
int)monoW || plut->y < 0 || plut->y >= (
int)monoH){
495 const uint8_t* inPtr = &inMono[monoW * plut->y + plut->x];
496 outPtr[0] = inPtr[0];
TY_CAPI TYInvertExtrinsic(const TY_CAMERA_EXTRINSIC *orgExtrinsic, TY_CAMERA_EXTRINSIC *invExtrinsic)
Calculate 4x4 extrinsic matrix'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 ...