Project Point Cloud to depth

要投影点云到深度图首先输入的点云是 “organized” or “organizable(先处理 cloud 的 width 和 height!)”, 然后需要确定 以那个轴作为深度值或者投影到那个平面.

例如 cloud z to depth:

template<typename Point = pcl::PointXYZ, typename DT = float>
bool cloudZToDepth(
    const pcl::PointCloud<Point>& cloud,
    const bool invalidToNew,
    const DT newValue,
    cv::Mat& depth) noexcept
{
    if (!cloud.isOrganized()) {
        return false;
    }
    int32_t type;
    if (std::is_same<float, typename std::decay<DT>::type>::value) {
        type = CV_32F;
    } else if (std::is_same<double, typename std::decay<DT>::type>::value) {
        type = CV_64F;
    } else if (std::is_same<int32_t, typename std::decay<DT>::type>::value) {
        type = CV_32S;
    } else {
        return false;
    }
    const int32_t width = cloud.width;
    const int32_t height = cloud.height;
    if (std::is_floating_point<typename std::decay<DT>::type>::value) {
        depth.create(height, width, CV_MAKETYPE(type, 1));
    } else {
        depth.create(height, width, CV_MAKETYPE(type, 1));
    }
    if (invalidToNew) {
        for (int32_t y = 0; y < height; ++y) {
            for (int32_t x = 0; x < width; ++x) {
                // NOTE: cv: (row, column), BUT PCL (column, row)
                const auto dv = cloud(x, y).z;
                if (std::isfinite(dv)) {
                    depth.at<DT>(y, x) = DT(dv);
                } else {
                    depth.at<DT>(y, x) = newValue;
                }
            }
        }
    } else {
        for (int32_t y = 0; y < height; ++y) {
            for (int32_t x = 0; x < width; ++x) {
                // NOTE: cv: (row, column), BUT PCL (column, row)
                depth.at<DT>(y, x) = cloud(x, y).z;
            }
        }
    }
    return true;
}

再比如 cloud x, y, z to depth (3 channels):

template<typename Point = pcl::PointXYZ, typename CDT = float>
bool CvHelper::cloudXYZToDepth(
    const pcl::PointCloud<Point>& cloud,
    const bool invalidToNew,
    const CDT newValue,
    cv::Mat& depth,
    const CDT xFactor,
    const CDT yFactor,
    const CDT zFactor) noexcept
{
    if (!cloud.isOrganized()) {
        return false;
    }
    int32_t type;
    if (std::is_same<float, typename std::decay<CDT>::type>::value) {
        type = CV_32F;
    } else if (std::is_same<double, typename std::decay<CDT>::type>::value) {
        type = CV_64F;
    } else if (std::is_same<int32_t, typename std::decay<CDT>::type>::value) {
        type = CV_32S;
    } else {
        return false;
    }
    const int32_t width = cloud.width;
    const int32_t height = cloud.height;
    if (std::is_floating_point<typename std::decay<CDT>::type>::value) {
        depth.create(height, width, CV_MAKETYPE(type, 3));
    } else {
        depth.create(height, width, CV_MAKETYPE(type, 3));
    }
    if (invalidToNew) {
        for (int32_t y = 0; y < height; ++y) {
            for (int32_t x = 0; x < width; ++x) {
                // NOTE: cv: (row, column), BUT PCL (column, row)
                auto dv = cloud(x, y).x;
                if (std::isfinite(dv)) {
                    depth.at<cv::Vec<CDT, 3> >(y, x)[0] = CDT(dv * xFactor);
                } else {
                    depth.at<cv::Vec<CDT, 3> >(y, x)[0] = newValue;
                }
                // NOTE: cv: (row, column), BUT PCL (column, row)
                dv = cloud(x, y).y;
                if (std::isfinite(dv)) {
                    depth.at<cv::Vec<CDT, 3> >(y, x)[1] = CDT(dv * yFactor);
                } else {
                    depth.at<cv::Vec<CDT, 3> >(y, x)[1] = newValue;
                }
                // NOTE: cv: (row, column), BUT PCL (column, row)
                dv = cloud(x, y).z;
                if (std::isfinite(dv)) {
                    depth.at<cv::Vec<CDT, 3> >(y, x)[2] = CDT(dv * zFactor);
                } else {
                    depth.at<cv::Vec<CDT, 3> >(y, x)[2] = newValue;
                }
            }
        }
    } else {
        for (int32_t y = 0; y < height; ++y) {
            for (int32_t x = 0; x < width; ++x) {
                // NOTE: cv: (row, column), BUT PCL (column, row)
                depth.at<cv::Vec<CDT, 3> >(y, x)[0] = cloud(x, y).x * xFactor;
                depth.at<cv::Vec<CDT, 3> >(y, x)[1] = cloud(x, y).y * yFactor;
                depth.at<cv::Vec<CDT, 3> >(y, x)[2] = cloud(x, y).z * zFactor;
            }
        }
    }
    return true;
}

---

• See also https://yuiwong.org/gitlab/pointcloud