一、定义
通过相机拍摄得到的点云是将深度图经过坐标转换成点云数据。深度图像上的每个像素点的值表达是场景物体离相机的距离。那么如果已知点云,如何转成深度图像呢!
二、使用的函数
头文件: #include <pcl/visualization/range_image_visualizer.h>
函数:range_image.createFromPointCloud(......)
RangeImage::createFromPointCloud (const PointCloudType& point_cloud, float angular_resolution,
float max_angle_width, float max_angle_height,
const Eigen::Affine3f& sensor_pose, RangeImage::CoordinateFrame coordinate_frame,
float noise_level, float min_range, int border_size)
三、代码
#define _CRT_SECURE_NO_WARNINGS
#pragma warning(disable:4996)
#include <pcl/range_image/range_image.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
void
setViewerPose(pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
{
Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f(0, 0, 0);
Eigen::Vector3f look_at_vector = viewer_pose.rotation() * Eigen::Vector3f(0, 0, 1) + pos_vector;
Eigen::Vector3f up_vector = viewer_pose.rotation() * Eigen::Vector3f(0, -1, 0);
viewer.setCameraPosition(
pos_vector[0],
pos_vector[1],
pos_vector[2],
look_at_vector[0],
look_at_vector[1],
look_at_vector[2],
up_vector[0],
up_vector[1],
up_vector[2]);
}
int
main(int argc, char** argv) {
pcl::PointCloud<pcl::PointXYZ> pointCloud;
//生成数据
for (float y = -0.5f; y <= 0.5f; y += 0.01f)
{
for (float z = -0.5f; z <= 0.5f; z += 0.01f)
{
pcl::PointXYZ point;
point.x = 2.0f - y;
point.y = y;
point.z = z;
pointCloud.points.push_back(point);
}
}
pointCloud.width = (uint32_t)pointCloud.points.size();
pointCloud.height = 1;
/*以1度为角分辨率,从上面创建的点云创建深度图像。*/
float angularResolution = (float)(1.0f * (M_PI / 180.0f)); // 1度转弧度
/*模拟深度传感器在水平方向的最大采样角度:360°视角。*/
float maxAngleWidth = (float)(360.0f * (M_PI / 180.0f)); // 360.0度转弧度
/*模拟深度传感器在垂直方向的最大采样角度:180°视角。*/
float maxAngleHeight = (float)(180.0f * (M_PI / 180.0f)); // 180.0度转弧度
/*
sensorPose定义模拟深度传感器的自由度位置:
横滚角roll、俯仰角pitch、偏航角yaw,默认初始值均为0。
*/
Eigen::Affine3f sensorPose = (Eigen::Affine3f)Eigen::Translation3f(0.0f, 0.0f, 0.0f);
/*
定义坐标轴正方向:
CAMERA_FRAME:X轴向右,Y轴向下,Z轴向前;
LASER_FRAME:X轴向前,Y轴向左,Z轴向上;
*/
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
/*
noiseLevel设置周围点对当前点深度值的影响:
如 noiseLevel=0.05,可以理解为,深度距离值是通过查询点半径为 Scm 的圆内包含的点用来平均计算而得到的。
*/
float noiseLevel = 0.00;
/*
min_range设置最小的获取距离,小于最小获取距离的位置为盲区
*/
float minRange = 0.0f;
/*border_size获得深度图像的边缘的宽度:在裁剪图像时,如果 borderSize>O ,将在图像周围留下当前视点不可见点的边界*/
int borderSize = 1;
boost::shared_ptr<pcl::RangeImage> range_image_ptr(new pcl::RangeImage);
pcl::RangeImage& range_image = *range_image_ptr;
range_image.createFromPointCloud(pointCloud, angularResolution, maxAngleWidth, maxAngleHeight, sensorPose, coordinate_frame, noiseLevel, minRange, borderSize);
std::cout << range_image << "\n";
//创建3D视图并且添加点云进行显示
pcl::visualization::PCLVisualizer viewer("3D Viewer");
viewer.setBackgroundColor(1, 1, 1);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler(range_image_ptr, 0, 0, 0);
viewer.addPointCloud(range_image_ptr, range_image_color_handler, "range image");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
viewer.initCameraParameters();
setViewerPose(viewer, range_image.getTransformationToWorldSystem());
//显示深度图像
pcl::visualization::RangeImageVisualizer range_image_widget("Range image");
//range_image_widget.setRangeImage(range_image);
range_image_widget.showRangeImage(range_image); //图像可视化方式显示深度图像
Eigen::Affine3f scene_sensor_pose(Eigen::Affine3f::Identity());
//主循环
while (!viewer.wasStopped())
{
range_image_widget.spinOnce();
viewer.spinOnce();
//pcl_Sleep(0.01);
if (1)
{
scene_sensor_pose = viewer.getViewerPose();
range_image.createFromPointCloud(pointCloud,
angularResolution, pcl::deg2rad(360.0f),
pcl::deg2rad(180.0f),
scene_sensor_pose,
pcl::RangeImage::LASER_FRAME,
noiseLevel,
minRange,
borderSize);
range_image_widget.showRangeImage(range_image);
}
}
}
四、效果
不明白:为什么深度图会产生成对称形式的啦。。。。。。。。
五、解释
angularResolution
maxAngleWidth=360 maxAngleHeight=180
,表示我们模拟的距离传感器对周围环境是一个完整的360度视图。 通过调整这个值设置激光器扫描的视角,从而减少计算量。 比如:一个激光只需要描述前方的180度的数据,后面的数据可以忽视,那么设置 maxAngleWidth=180
就足够了。
传感器姿态,包含6DoF (6个自由度), 其中roll=pitch=yaw=0.
coordinate_frame=CAMERA_FRAME
表示x面向右,y向下,z轴向前。另一种选择是LASER_FRAME
, x面向前,y向左,z向向上。
noiseLevel=0
表示 深度图使用一个归一化的z缓冲区创建的。如果想让邻近点都落在同一个像素单元,用户可以使用较高的值。例如noiseLevel=0.05,深度距离值是通过查询点半径为5cm的圆内包含的点,平均计算而得到的。
如果minRange
是大于0的,那么所有深度值小于这个值的点都将被忽略。
自由度总共有6个,可分成两种不同的类型:平移和旋转。
1. 平移运动 刚体可以在3个自由度中平移:向前/向后,向上/向下,向左/向右。
2. 旋转运动 刚体也可以在3个自由度中旋转:纵摇(Pitch)、横摇(Roll)和垂摇(Yaw)。