主要思想:使用的点云为16行,每行1800个点,点云每个点自带ring,再根据xy可以求出来点在哪一列。然后把点的深度和强度信息传输给Mat的矩阵即可。
问题:因为使用的点云原因,得到的图像是很扁的,所以看起来并不好看。尝试使用opencv拉伸,但是失败了。
和上一篇类似,为了主程序的简洁,我们建立一个头文件myPointType.h,在这里面我们定义我们所需要的点云类型,具体代码如下:
#ifndef PCL_NO_PRECOMPILE
#define PCL_NO_PRECOMPILE
#include
#include
#include
#include
#include
#include
#include
/*
*一个具有XYZ、intensity、ring的点云类型
*/
struct PointXYZIR
{
PCL_ADD_POINT4D
PCL_ADD_INTENSITY;
uint16_t ring;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIR,
(float, x, x) (float, y, y)
(float, z, z) (float, intensity, intensity)
(uint16_t, ring, ring)
)
/*
* 一个具有XYZ、RGB、intensity、ring的点云类型
*/
struct PointXYZRGBIR
{
PCL_ADD_POINT4D;
PCL_ADD_RGB;
PCL_ADD_INTENSITY;
uint16_t ring;
uint16_t label;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZRGBIR,
(float, x, x)
(float, y, y)
(float, z, z)
(float, rgb, rgb)
(float, intensity, intensity)
(uint16_t, label, label)
(uint16_t, ring, ring)
)
#endif //PCL_NO_PRECOMPILE
另外加一个参数的头文件pamarm.h,具体如下:
#ifndef PAMARM_NO_PRECOMPILE
#define PAMARM_NO_PRECOMPILE
#include
extern const int N_SCAN = 16;
extern const int Horizon_SCAN = 1800;
extern const float ang_res_x = 0.2; //水平上每条线间隔0.2°
extern const float sensorMinimumRange = 1.0;
#endif // PAMARM_NO_PRECOMPILE
主要程序为pcl_projection.cpp,具体代码如下:
// #include
#include
#include "myPointType.h"
#include "pamarm.h"
class pcl_projection
{
private:
ros::NodeHandle n;
ros::Subscriber subCloud;
cv::Mat rangeMat; //点云的range矩阵
cv::Mat rangeImg; //点云的range图像
cv::Mat intensityMat; //点云的反射强度矩阵,也是float类型
cv::Mat intensityImg; //点云的反射强度矩阵
public:
pcl_projection():
n("~"){
subCloud = n.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 1, &pcl_projection::getcloud, this);
void resetParameters();
}
void resetParameters(){
rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(111111));
rangeImg = cv::Mat(N_SCAN, Horizon_SCAN, CV_8UC1, cv::Scalar(255));
intensityMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(111111));
intensityImg = cv::Mat(N_SCAN, Horizon_SCAN, CV_8UC1, cv::Scalar(255));
}
//点云回调函数
void getcloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
float x,y,z,horizonAngle, range, intensity; //xyz,水平角,距离
size_t rowIdn, columnIdn, cloudSize; //行索引,列索引,点云数量
uint8_t range_int, intensity_int;
pcl::PointCloud<PointXYZIR>::Ptr raw_pcl_ptr (new pcl::PointCloud<PointXYZIR>);
pcl::fromROSMsg(*laserCloudMsg, *raw_pcl_ptr); //把msg消息指针转化为点云指正
cloudSize = raw_pcl_ptr->points.size();
for (int i = 0; i < cloudSize; i++)
{
x = raw_pcl_ptr->points[i].x;
y = raw_pcl_ptr->points[i].y;
z = raw_pcl_ptr->points[i].z;
rowIdn = raw_pcl_ptr->points[i].ring; //行索引
intensity = raw_pcl_ptr->points[i].intensity;
if (rowIdn < 0 || rowIdn >= N_SCAN)
continue;
horizonAngle = atan2(x, y) * 180 / M_PI; //水平角
columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2; //列索引
if (columnIdn >= Horizon_SCAN)
columnIdn -= Horizon_SCAN;
if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
continue;
range = sqrt(x * x + y *y + z * z); //距离
if (range < sensorMinimumRange)
continue;
rangeMat.at<float>(rowIdn, columnIdn) = range; //距离传递出去
range_int = uint8_t(range*5); //使得距离尽量在0-255内。
if (range_int > 255)
range_int = 255;
rangeImg.at<uint8_t>(rowIdn, columnIdn) = range_int; //强度展示
intensityMat.at<float>(rowIdn, columnIdn) = intensity; //强度传递出去
intensity_int = uint8_t(intensity*255/100);
intensityImg.at<uint8_t>(rowIdn, columnIdn) = intensity_int; //强度展示
}
cv::imshow("range", rangeImg); //可视化
cv::imshow("intensity", intensityImg);
}
~pcl_projection(){};
};
int main(int argc, char **argv)
{
ros::init(argc, argv, "pcl_projection");
cv::namedWindow("range"); //两个窗口,分别可视化range和intensity
cv::namedWindow("intensity");
cv::startWindowThread();
pcl_projection pp;
pp.resetParameters(); //初始化参数
ros::spin();
cv::destroyWindow("range");
cv::destroyWindow("intensity");
}
CMakeLists.txt文件为:
cmake_minimum_required(VERSION 2.6 FATAL_ERROR)
project(pcl_projection)
set(PACKAGE_DEPENDENCIES
roscpp
sensor_msgs
pcl_ros
pcl_conversions
std_srvs
message_generation
std_msgs
)
find_package(OpenCV REQUIRED)
find_package(PCL 1.3 REQUIRED COMPONENTS common io)
find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES})
include_directories(include ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable(pcl_projection pcl_projection.cpp)
target_link_libraries(pcl_projection ${PCL_LIBRARIES} ${catkin_LIBRARIES} ${OpenCV_LIBS})