0

I have an RGBD image, I'm trying to transform it in 3D space, the way I do it is deproject the pixels to 3D space -> apply transform -> project them back to 2D space and this is the result.

Those lines converge the sharper the angle gets as shown here. This is the code I'm using:

            void transform_2d_in_3d_space(
                const cv::Mat& src_color,
                const cv::Mat& src_depth,
                cv::Mat& dst,
                data_structures::intrinsic& intrinsic,
                float depth_scale, cv::Mat transform)
            {
                float fx = intrinsic.fx;
                float fy = intrinsic.fy;
                float cx = intrinsic.cx;
                float cy = intrinsic.cy;
                float x, y, z, r2, f, dx, dy;
                int width = src_color.cols;
                int height = src_color.rows;
                cv::Mat p = (cv::Mat_<float>(4, 1) << 0, 0, 0, 1);
                unsigned short pixel;
                dst = cv::Mat(src_color.size(), src_color.type(), cv::Scalar(0, 0, 0));

                for (size_t i = 0; i < src_depth.rows; i++)
                {
                    for (size_t j = 0; j < src_depth.cols; j++)
                    {
                        pixel = src_depth.at<unsigned short>(i, j);

                        if (pixel > 0)
                        {
                            p.at<float>(2, 0) = z = pixel * depth_scale;
                            p.at<float>(0, 0) = z * ((float)j - cx) / fx;
                            p.at<float>(1, 0) = z * ((float)i - cy) / fy;
                            p = transform * p;
                            x = p.at<float>(0, 0) / p.at<float>(2, 0);
                            y = p.at<float>(1, 0) / p.at<float>(2, 0);
                            
                            r2 = x * x + y * y;
                            f = 1 + intrinsic.distortion[0] * r2 + intrinsic.distortion[1] * r2 * r2 + intrinsic.distortion[4] * r2 * r2 * r2;
                            x *= f;
                            y *= f;
                            dx = x + 2 * intrinsic.distortion[2] * x * y + intrinsic.distortion[3] * (r2 + 2 * x * x);
                            dy = y + 2 * intrinsic.distortion[3] * x * y + intrinsic.distortion[2] * (r2 + 2 * y * y);
                            x = dx;
                            y = dy;
                            x = x * fx + cx;
                            y = y * fy + cy;

                            if (x > 0 && x < width && y > 0 && y < height)
                            {
                                if (j < width - 2 && i < height - 2)
                                {
                                    dst.at<cv::Vec3b>(floor(y), floor(x)) = interpolate_rgb_pixel(src_color, j, i, x, y);
                                }
                            }
                        }
                    }
                }
            }
VladB
  • 36
  • 3
  • Why are you using reference to cv::Mat? – macroland Nov 24 '20 at 14:50
  • 1
    @macroland Is there a reason I shouldn't be? – VladB Nov 24 '20 at 14:56
  • Use the Q matrix obtained form stereocalibration with the reprojectto3D function in opencv to do the propper reprojection. See https://stackoverflow.com/questions/27374970/q-matrix-for-the-reprojectimageto3d-function-in-opencv – Dr Yuan Shenghai Nov 24 '20 at 15:35
  • @DrYuanShenghai I'm not using a stereo camera but realsense l515 lidar. How do I form the Q matrix? – VladB Nov 24 '20 at 16:23
  • in this case, see here. you calculation seems wrong @ p.at(2, 0) = z = pixel * depth_scale; p.at(0, 0) = z * ((float)j - cx) / fx; p.at(1, 0) = z * ((float)i - cy) / fy;. the correct one should be P3D.x = (x_d - cx_d) * depth(x_d,y_d) / fx_d P3D.y = (y_d - cy_d) * depth(x_d,y_d) / fy_d P3D.z = depth(x_d,y_d) – Dr Yuan Shenghai Nov 24 '20 at 20:22
  • That seems to be exactly the same. The depth frame is aligned to the color frame so I’m using the color intrinsics, could that be the issue? It seems logical to use the color cameras intrinsics. – VladB Nov 24 '20 at 21:32

0 Answers0