ROS订阅激光点云数据并画直线

最近遇到一个点云处理的问题，需要将激光点云拟合出几条直线最后在rviz中显示出来。主要用到了点云数据的订阅、坐标变换、直线检测以及rviz显示其实还是挺简单的，就是需要用到的一些东西/工具需要提前了解一下.

cv::approxPolyDP

首先第一个是opencv库中的点云到直线拟合工具cv::approxPolyDP，这个工具箱还是很好用的，它的输入是点云数据，输出是拟合出来的直线的起点与终点坐标。

void approxPolyDP(InputArray curve, OutputArray approxCurve, double epsilon, bool closed)

该函数主要功能是把一个连续光滑曲线折线化，其中一共有四个参数：

参数详解:
        InputArray curve:一般是由图像的轮廓点组成的点集
        OutputArray approxCurve：表示输出的多边形点集
        double epsilon：主要表示输出的精度，就是另个轮廓点之间最大距离数，5,6,7，，8，，,,可以理解为点到直线能接受的偏离阈值，这个值越大分出的直线越细
        bool closed：表示输出的多边形是否封闭

通过这个函数，我们可以将点云数据输入进去，最后得到拟合后的直线信息。

visualization_msgs::Marker

visualization_msgs::Marker函数是用于rviz可视化界面的，它的参数其实挺多的，但是好像也不用每个参数都设置过去。

marker的基本格式：

//各种标志物类型的定义，每一个的具体介绍和形状可以到这里查看：http://wiki.ros.org/rviz/DisplayTypes/Marker
uint8 ARROW=0//箭头
uint8 CUBE=1//立方体
uint8 SPHERE=2//球
uint8 CYLINDER=3//圆柱体
uint8 LINE_STRIP=4//线条（点的连线）
uint8 LINE_LIST=5//线条序列
uint8 CUBE_LIST=6//立方体序列
uint8 SPHERE_LIST=7//球序列
uint8 POINTS=8//点集
uint8 TEXT_VIEW_FACING=9//显示3D的文字
uint8 MESH_RESOURCE=10//网格？
uint8 TRIANGLE_LIST=11//三角形序列
//对标记的操作
uint8 ADD=0
uint8 MODIFY=0
uint8 DELETE=2
uint8 DELETEALL=3
 
Header header
string ns   //命名空间namespace，就是你理解的那样
int32 id    //与命名空间联合起来，形成唯一的id，这个唯一的id可以将各个标志物区分开来，使得程序可以对指定的标志物进行操作
int32 type  //类型
int32 action 	//操作，是添加还是修改还是删除
geometry_msgs/Pose pose       # Pose of the object
geometry_msgs/Vector3 scale   # Scale of the object 1,1,1 means default (usually 1 meter square)
std_msgs/ColorRGBA color      # Color [0.0-1.0]
duration lifetime             # How long the object should last before being automatically deleted.  0 means forever
bool frame_locked             # If this marker should be frame-locked, i.e. retransformed into its frame every timestep
 
#Only used if the type specified has some use for them (eg. POINTS, LINE_STRIP, ...)
geometry_msgs/Point[] points//这个是在序列、点集中才会用到，指明序列中每个点的位置
#Only used if the type specified has some use for them (eg. POINTS, LINE_STRIP, ...)
#number of colors must either be 0 or equal to the number of points
#NOTE: alpha is not yet used
std_msgs/ColorRGBA[] colors
 
# NOTE: only used for text markers
string text
 
# NOTE: only used for MESH_RESOURCE markers
string mesh_resource
bool mesh_use_embedded_materials

有点复杂，最后我写出来能用的是这样子：

void addLine(double x1,double y1,double x2,double y2,visualization_msgs::Marker &lines)
{	
	/*geometry_msgs::Point p;
	p.x = x1;
	p.y = y1;
	lines.points.push_back(p);

	p.x = x2;
	p.y = y2;
	lines.points.push_back(p);*/
	lines.ns = "line_extraction";                           //命名空间namespace，就是你理解的那样
	lines.id = 0;                                           //与命名空间联合起来，形成唯一的id，这个唯一的id可以将各个标志物区分开来，使得程序可以对指定的标志物进行操作
	lines.type = visualization_msgs::Marker::LINE_LIST;     //类型
	// Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
    //lines.action = visualization_msgs::Marker::ADD;
    lines.pose.orientation.x=0.0;
	lines.pose.orientation.y=0.0;
	lines.pose.orientation.z=0.0;
	lines.pose.orientation.w=1.0;
	lines.scale.x = 0.1;
	//设置线的颜色，a应该是透明度
	lines.color.r = 1.0;
	lines.color.g = 0.0;
	lines.color.b = 0.0;
	lines.color.a = 1.0;
	//这个是指定marker在被自动清除前可以逗留多长时间。这里 ros::Duration()表示不自动删除。如果在lifetime结束之前有新内容到达了，它就会被重置。
    lines.lifetime = ros::Duration(0.5);
 
	//线的初始点
	geometry_msgs::Point p_start;
	p_start.x = x1;
	p_start.y = y1;
	p_start.z = 0;	
	//if(lines.points.size() >= 40)
	//    line_strips.points.erase(line_strips.points.begin());
	//将直线存储到marker容器
	lines.points.push_back(p_start);
	//线的终点
	geometry_msgs::Point p_end;
	p_end.x = x2;
	p_end.y = y2;
	p_end.z = 0;
	//if(lines.points.size() >= 40)
	//    line_strips.points.erase(line_strips.points.begin());
	lines.points.push_back(p_end);
	//消息的frame_id以及时间辍
	lines.header.frame_id = "base_link";
	lines.header.stamp = ros::Time::now();
}

最后的运行结果如下，对激光数据进行了一定的角度上的限定没有把所有的直线都画出来：


具体代码在下述连接中：

https://download.csdn.net/download/YiYeZhiNian/85209939