# TYCamport3: TYCoordinateMapper.h Source File

TYCoordinateMapper.h
Go to the documentation of this file.
#ifndef TY_COORDINATE_MAPPER_H_
#define TY_COORDINATE_MAPPER_H_
#include <stdlib.h>
#include "TYApi.h"
typedef struct TY_PIXEL_DESC
{
int16_t x; // x coordinate in pixels
int16_t y; // y coordinate in pixels
uint16_t depth; // depth value
uint16_t rsvd;
}TY_PIXEL_DESC;
typedef struct TY_PIXEL_COLOR_DESC
{
int16_t x; // x coordinate in pixels
int16_t y; // y coordinate in pixels
uint8_t bgr_ch1; // color info <channel 1>
uint8_t bgr_ch2; // color info <channel 2>
uint8_t bgr_ch3; // color info <channel 3>
uint8_t rsvd;
}TY_PIXEL_COLOR_DESC;
// ------------------------------
// base convertion
// ------------------------------
TY_CAPI TYInvertExtrinsic (const TY_CAMERA_EXTRINSIC* orgExtrinsic,
TY_CAMERA_EXTRINSIC* invExtrinsic);
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);
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);
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);
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);
TY_CAPI TYMapPoint3dToPoint3d (const TY_CAMERA_EXTRINSIC* extrinsic,
const TY_VECT_3F* point3dFrom, int32_t count,
TY_VECT_3F* point3dTo);
// ------------------------------
// inlines
// ------------------------------
static inline TY_STATUS TYMapDepthToColorCoordinate(
const TY_CAMERA_CALIB_INFO* depth_calib,
uint32_t depthW, uint32_t depthH,
const TY_PIXEL_DESC* depth, uint32_t count,
const TY_CAMERA_CALIB_INFO* color_calib,
uint32_t mappedW, uint32_t mappedH,
TY_PIXEL_DESC* mappedDepth,
float f_scale_unit = 1.0f);
static inline TY_STATUS TYMapDepthImageToColorCoordinate(
const TY_CAMERA_CALIB_INFO* depth_calib,
uint32_t depthW, uint32_t depthH, const uint16_t* depth,
const TY_CAMERA_CALIB_INFO* color_calib,
uint32_t mappedW, uint32_t mappedH, uint16_t* mappedDepth,
float f_scale_unit = 1.0f);
static inline TY_STATUS TYCreateDepthToColorCoordinateLookupTable(
const TY_CAMERA_CALIB_INFO* depth_calib,
uint32_t depthW, uint32_t depthH, const uint16_t* depth,
const TY_CAMERA_CALIB_INFO* color_calib,
uint32_t mappedW, uint32_t mappedH,
TY_PIXEL_DESC* lut,
float f_scale_unit = 1.0f);
static inline TY_STATUS TYMapRGBPixelsToDepthCoordinate(
const TY_CAMERA_CALIB_INFO* depth_calib,
uint32_t depthW, uint32_t depthH, const uint16_t* depth,
const TY_CAMERA_CALIB_INFO* color_calib,
uint32_t rgbW, uint32_t rgbH,
TY_PIXEL_COLOR_DESC* src, uint32_t cnt,
uint32_t min_distance,
uint32_t max_distance,
TY_PIXEL_COLOR_DESC* dst,
float f_scale_unit = 1.0f);
static inline TY_STATUS TYMapRGBImageToDepthCoordinate(
const TY_CAMERA_CALIB_INFO* depth_calib,
uint32_t depthW, uint32_t depthH, const uint16_t* depth,
const TY_CAMERA_CALIB_INFO* color_calib,
uint32_t rgbW, uint32_t rgbH, const uint8_t* inRgb,
uint8_t* mappedRgb,
float f_scale_unit = 1.0f);
static inline TY_STATUS TYMapRGB48ImageToDepthCoordinate(
const TY_CAMERA_CALIB_INFO* depth_calib,
uint32_t depthW, uint32_t depthH, const uint16_t* depth,
const TY_CAMERA_CALIB_INFO* color_calib,
uint32_t rgbW, uint32_t rgbH, const uint16_t* inRgb,
uint16_t* mappedRgb,
float f_scale_unit = 1.0f);
static inline TY_STATUS TYMapMono16ImageToDepthCoordinate(
const TY_CAMERA_CALIB_INFO* depth_calib,
uint32_t depthW, uint32_t depthH, const uint16_t* depth,
const TY_CAMERA_CALIB_INFO* color_calib,
uint32_t rgbW, uint32_t rgbH, const uint16_t* gray,
uint16_t* mappedGray,
float f_scale_unit = 1.0f);
static inline TY_STATUS TYMapMono8ImageToDepthCoordinate(
const TY_CAMERA_CALIB_INFO* depth_calib,
uint32_t depthW, uint32_t depthH, const uint16_t* depth,
const TY_CAMERA_CALIB_INFO* color_calib,
uint32_t monoW, uint32_t monoH, const uint8_t* inMono,
uint8_t* mappedMono,
float f_scale_unit = 1.0f);
#define TYMAP_CHECKRET(f, bufToFree) \
do{ \
TY_STATUS err = (f); \
if(err){ \
if(bufToFree) \
free(bufToFree); \
return err; \
} \
} while(0)
static inline TY_STATUS TYMapDepthToColorCoordinate(
const TY_CAMERA_CALIB_INFO* depth_calib,
uint32_t depthW, uint32_t depthH,
const TY_PIXEL_DESC* depth, uint32_t count,
const TY_CAMERA_CALIB_INFO* color_calib,
uint32_t mappedW, uint32_t mappedH,
TY_PIXEL_DESC* mappedDepth,
float f_scale_unit)
{
TY_VECT_3F* p3d = (TY_VECT_3F*)malloc(sizeof(TY_VECT_3F) * count);
TYMAP_CHECKRET(TYMapDepthToPoint3d(depth_calib, depthW, depthH, depth, count, p3d, f_scale_unit), p3d );
TY_CAMERA_EXTRINSIC extri_inv;
TYMAP_CHECKRET(TYInvertExtrinsic(&color_calib->extrinsic, &extri_inv), p3d);
TYMAP_CHECKRET(TYMapPoint3dToPoint3d(&extri_inv, p3d, count, p3d), p3d );
TYMAP_CHECKRET(TYMapPoint3dToDepth(color_calib, p3d, count, mappedW, mappedH, mappedDepth, f_scale_unit), p3d );
free(p3d);
return TY_STATUS_OK;
}
static inline TY_STATUS TYMapDepthImageToColorCoordinate(
const TY_CAMERA_CALIB_INFO* depth_calib,
uint32_t depthW, uint32_t depthH, const uint16_t* depth,
const TY_CAMERA_CALIB_INFO* color_calib,
uint32_t mappedW, uint32_t mappedH, uint16_t* mappedDepth, float f_scale_unit)
{
TY_VECT_3F* p3d = (TY_VECT_3F*)malloc(sizeof(TY_VECT_3F) * depthW * depthH);
TYMAP_CHECKRET(TYMapDepthImageToPoint3d(depth_calib, depthW, depthH, depth, p3d, f_scale_unit), p3d);
TY_CAMERA_EXTRINSIC extri_inv;
TYMAP_CHECKRET(TYInvertExtrinsic(&color_calib->extrinsic, &extri_inv), p3d);
TYMAP_CHECKRET(TYMapPoint3dToPoint3d(&extri_inv, p3d, depthW * depthH, p3d), p3d);
TYMAP_CHECKRET(TYMapPoint3dToDepthImage(
color_calib, p3d, depthW * depthH, mappedW, mappedH, mappedDepth, f_scale_unit), p3d);
free(p3d);
return TY_STATUS_OK;
}
static inline TY_STATUS TYMapRGBPixelsToDepthCoordinate(
const TY_CAMERA_CALIB_INFO* depth_calib,
uint32_t depthW, uint32_t depthH, const uint16_t* depth,
const TY_CAMERA_CALIB_INFO* color_calib,
uint32_t rgbW, uint32_t rgbH,
TY_PIXEL_COLOR_DESC* src, uint32_t cnt,
uint32_t min_distance,
uint32_t max_distance,
TY_PIXEL_COLOR_DESC* dst,
float f_scale_unit)
{
uint32_t m_distance_range = max_distance - min_distance;
TY_CAMERA_EXTRINSIC extri = color_calib->extrinsic;
TY_PIXEL_DESC* pixels_array = (TY_PIXEL_DESC*)malloc(sizeof(TY_PIXEL_DESC) * m_distance_range);
TY_PIXEL_DESC* pixels_mapped_array = (TY_PIXEL_DESC*)malloc(sizeof(TY_PIXEL_DESC) * m_distance_range);
TY_VECT_3F* p3d_array = (TY_VECT_3F*)malloc(sizeof(TY_VECT_3F) * m_distance_range);
for (uint32_t i = 0; i < cnt; i++) {
for (uint32_t m = 0; m < m_distance_range; m++) {
pixels_array[m].x = src[i].x;
pixels_array[m].y = src[i].y;
pixels_array[m].depth = m + min_distance;
}
TYMapDepthToPoint3d(color_calib, rgbW, rgbH, pixels_array, m_distance_range, &p3d_array[0], f_scale_unit);
TYMapPoint3dToPoint3d(&extri, &p3d_array[0], m_distance_range, &p3d_array[0]);
TYMapPoint3dToDepth(depth_calib, p3d_array, m_distance_range, depthW, depthH, pixels_mapped_array, f_scale_unit);
uint16_t m_min_delt = 0xffff;
dst[i].x = -1;
dst[i].y = -1;
for (uint32_t m = 0; m < m_distance_range; m++) {
int16_t pixel_x = pixels_mapped_array[m].x;
int16_t pixel_y = pixels_mapped_array[m].y;
uint16_t delt = abs(pixels_mapped_array[m].depth - depth[pixel_y*depthW + pixel_x]);
if (delt < m_min_delt) {
m_min_delt = delt;
if (m_min_delt < 10) {
dst[i].x = pixel_x;
dst[i].y = pixel_y;
dst[i].bgr_ch1 = src[i].bgr_ch1;
dst[i].bgr_ch2 = src[i].bgr_ch2;
dst[i].bgr_ch3 = src[i].bgr_ch3;
}
}
}
}
free(pixels_array);
free(pixels_mapped_array);
free(p3d_array);
return TY_STATUS_OK;
}
static inline TY_STATUS TYCreateDepthToColorCoordinateLookupTable(
const TY_CAMERA_CALIB_INFO* depth_calib,
uint32_t depthW, uint32_t depthH, const uint16_t* depth,
const TY_CAMERA_CALIB_INFO* color_calib,
uint32_t mappedW, uint32_t mappedH,
TY_PIXEL_DESC* lut,
float f_scale_unit)
{
TY_VECT_3F* p3d = (TY_VECT_3F*)malloc(sizeof(TY_VECT_3F) * depthW * depthH);
TYMAP_CHECKRET(TYMapDepthImageToPoint3d(depth_calib, depthW, depthH, depth, p3d, f_scale_unit), p3d);
TY_CAMERA_EXTRINSIC extri_inv;
TYMAP_CHECKRET(TYInvertExtrinsic(&color_calib->extrinsic, &extri_inv), p3d);
TYMAP_CHECKRET(TYMapPoint3dToPoint3d(&extri_inv, p3d, depthW * depthH, p3d), p3d);
TYMAP_CHECKRET(TYMapPoint3dToDepth(color_calib, p3d, depthW * depthH, mappedW, mappedH, lut, f_scale_unit), p3d );
free(p3d);
return TY_STATUS_OK;
}
static inline TY_STATUS TYMapRGBImageToDepthCoordinate(
const TY_CAMERA_CALIB_INFO* depth_calib,
uint32_t depthW, uint32_t depthH, const uint16_t* depth,
const TY_CAMERA_CALIB_INFO* color_calib,
uint32_t rgbW, uint32_t rgbH, const uint8_t* inRgb,
uint8_t* mappedRgb, float f_scale_unit)
{
TY_PIXEL_DESC* lut = (TY_PIXEL_DESC*)malloc(sizeof(TY_PIXEL_DESC) * depthW * depthH);
TYMAP_CHECKRET(TYCreateDepthToColorCoordinateLookupTable(
depth_calib, depthW, depthH, depth,
color_calib, rgbW, rgbH, lut, f_scale_unit), lut);
for(uint32_t depthr = 0; depthr < depthH; depthr++)
for(uint32_t depthc = 0; depthc < depthW; depthc++)
{
TY_PIXEL_DESC* plut = &lut[depthr * depthW + depthc];
uint8_t* outPtr = &mappedRgb[depthW * depthr * 3 + depthc * 3];
if(plut->x < 0 || plut->x >= (int)rgbW || plut->y < 0 || plut->y >= (int)rgbH){
outPtr[0] = outPtr[1] = outPtr[2] = 0;
} else {
const uint8_t* inPtr = &inRgb[rgbW * plut->y * 3 + plut->x * 3];
outPtr[0] = inPtr[0];
outPtr[1] = inPtr[1];
outPtr[2] = inPtr[2];
}
}
free(lut);
return TY_STATUS_OK;
}
static inline TY_STATUS TYMapRGB48ImageToDepthCoordinate(
const TY_CAMERA_CALIB_INFO* depth_calib,
uint32_t depthW, uint32_t depthH, const uint16_t* depth,
const TY_CAMERA_CALIB_INFO* color_calib,
uint32_t rgbW, uint32_t rgbH, const uint16_t* inRgb,
uint16_t* mappedRgb, float f_scale_unit)
{
TY_PIXEL_DESC* lut = (TY_PIXEL_DESC*)malloc(sizeof(TY_PIXEL_DESC) * depthW * depthH);
TYMAP_CHECKRET(TYCreateDepthToColorCoordinateLookupTable(
depth_calib, depthW, depthH, depth,
color_calib, rgbW, rgbH, lut, f_scale_unit), lut);
for(uint32_t depthr = 0; depthr < depthH; depthr++)
for(uint32_t depthc = 0; depthc < depthW; depthc++)
{
TY_PIXEL_DESC* plut = &lut[depthr * depthW + depthc];
uint16_t* outPtr = &mappedRgb[depthW * depthr * 3 + depthc * 3];
if(plut->x < 0 || plut->x >= (int)rgbW || plut->y < 0 || plut->y >= (int)rgbH){
outPtr[0] = outPtr[1] = outPtr[2] = 0;
} else {
const uint16_t* inPtr = &inRgb[rgbW * plut->y * 3 + plut->x * 3];
outPtr[0] = inPtr[0];
outPtr[1] = inPtr[1];
outPtr[2] = inPtr[2];
}
}
free(lut);
return TY_STATUS_OK;
}
static inline TY_STATUS TYMapMono16ImageToDepthCoordinate(
const TY_CAMERA_CALIB_INFO* depth_calib,
uint32_t depthW, uint32_t depthH, const uint16_t* depth,
const TY_CAMERA_CALIB_INFO* color_calib,
uint32_t rgbW, uint32_t rgbH, const uint16_t* gray,
uint16_t* mappedGray, float f_scale_unit)
{
TY_PIXEL_DESC* lut = (TY_PIXEL_DESC*)malloc(sizeof(TY_PIXEL_DESC) * depthW * depthH);
TYMAP_CHECKRET(TYCreateDepthToColorCoordinateLookupTable(
depth_calib, depthW, depthH, depth,
color_calib, rgbW, rgbH, lut, f_scale_unit), lut);
for(uint32_t depthr = 0; depthr < depthH; depthr++)
for(uint32_t depthc = 0; depthc < depthW; depthc++)
{
TY_PIXEL_DESC* plut = &lut[depthr * depthW + depthc];
uint16_t* outPtr = &mappedGray[depthW * depthr + depthc];
if(plut->x < 0 || plut->x >= (int)rgbW || plut->y < 0 || plut->y >= (int)rgbH){
outPtr[0] = 0;
} else {
const uint16_t* inPtr = &gray[rgbW * plut->y + plut->x];
outPtr[0] = inPtr[0];
}
}
free(lut);
return TY_STATUS_OK;
}
static inline TY_STATUS TYMapMono8ImageToDepthCoordinate(
const TY_CAMERA_CALIB_INFO* depth_calib,
uint32_t depthW, uint32_t depthH, const uint16_t* depth,
const TY_CAMERA_CALIB_INFO* color_calib,
uint32_t monoW, uint32_t monoH, const uint8_t* inMono,
uint8_t* mappedMono, float f_scale_unit)
{
TY_PIXEL_DESC* lut = (TY_PIXEL_DESC*)malloc(sizeof(TY_PIXEL_DESC) * depthW * depthH);
TYMAP_CHECKRET(TYCreateDepthToColorCoordinateLookupTable(
depth_calib, depthW, depthH, depth,
color_calib, monoW, monoH, lut, f_scale_unit), lut);
for(uint32_t depthr = 0; depthr < depthH; depthr++)
for(uint32_t depthc = 0; depthc < depthW; depthc++)
{
TY_PIXEL_DESC* plut = &lut[depthr * depthW + depthc];
uint8_t* outPtr = &mappedMono[depthW * depthr + depthc];
if(plut->x < 0 || plut->x >= (int)monoW || plut->y < 0 || plut->y >= (int)monoH){
outPtr[0] = 0;
} else {
const uint8_t* inPtr = &inMono[monoW * plut->y + plut->x];
outPtr[0] = inPtr[0];
}
}
free(lut);
return TY_STATUS_OK;
}
#endif
TYInvertExtrinsic
TY_CAPI TYInvertExtrinsic(const TY_CAMERA_EXTRINSIC *orgExtrinsic, TY_CAMERA_EXTRINSIC *invExtrinsic)
Calculate 4x4 extrinsic matrix&#39;s inverse matrix.
TYMapPoint3dToDepthImage
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.
TYMapPoint3dToPoint3d
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_VECT_3F
Definition: TYApi.h:734
TYMapDepthToPoint3d
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.
TYMapDepthImageToPoint3d
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_CAMERA_EXTRINSIC
Definition: TYApi.h:771
TY_CAMERA_CALIB_INFO
Definition: TYApi.h:791
TY_PIXEL_COLOR_DESC
Definition: TYCoordinateMapper.h:20
TYMapPoint3dToDepth
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
TYApi.h includes camera control and data receiving interface, which supports configuration for image ...
TY_PIXEL_DESC
Definition: TYCoordinateMapper.h:12