I'm struggling with the depth color mapping of the kinect v2. I'm using recorded, not live data, so I can't use the CoordinateMapper.
I'm fowllowing this approach: http://nicolas.burrus.name/index.php/Research/KinectCalibration
Unfortunately I cannot find the distortion parameters of the kinect v2 anywhere, so I'm skipping the Transformation of raw depth values into meters. Can anyone provide those? My point cloud looks quite good!, but my colormapping doesn't work at all. In order to debug, I tried to create an transformed image (color_img_transformed) of the bgr image, but it looks very wrong:
for (int y = 0; y < depth.rows; ++y)
{
for (int x = 0; x < depth.cols; ++x)
{
float depthValue = (float) depth.at<float>(y,x);
pcl::PointXYZRGBA pt;
if (depthValue == -1000.f || depthValue == 0.0f || boost::math::isnan(depthValue))
{
}
else
{
//depthValue *= 1000; // i think values are already in mm
pt.x = (static_cast<float>(x + tmp_x) - centerX) * scaleFactorX * depthValue;
pt.y = (centerY - static_cast<float>(y + tmp_y)) * scaleFactorY * depthValue;
pt.z = depthValue;
// new try to map - https://www.codefull.org/2016/03/align-depth-and-color-frames-depth-and-rgb-registration/
// Apply RGB intrinsics
Eigen::Vector4f tmp = extrinsics_ * Eigen::Vector4f(pt.x,pt.y,pt.z,1);
float x_ = (tmp[0] * fx_rgb / tmp[2]) + cx_rgb;
float y_ = (tmp[1] * fy_rgb / tmp[2]) + cy_rgb;
// "x" and "y" are indices into the RGB frame, but they may contain
// invalid values (which correspond to the parts of the scene not visible
// to the RGB camera.
// Do we have a valid index?
if (x_ > bgr.size().width || y_ > bgr.size().height || x_ < 1 || y < 1)
continue;
// Need some kind of interpolation. I just did it the lazy way
int round_x = (int) floor(x_);
int round_y = (int) floor(y_);
cv::Vec3b colorValue = bgr.at<cv::Vec3b>(round_y, round_x);
RGBValue color;
color.Red = colorValue[2];
color.Green = colorValue[1];
color.Blue = colorValue[0];
pt.rgba = color.long_value;
color_img_transformed.at<cv::Vec3b>(round_y, round_x)[0] = 255;//colorValue[0];
color_img_transformed.at<cv::Vec3b>(round_y, round_x)[1] = 0;//colorValue[1];
color_img_transformed.at<cv::Vec3b>(round_y, round_x)[2] = 0;
}
}
}
I'm not quite sure if I use the correct extrinsics/intrinsics either. Can anyone comment theirs?