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;
}