最近遇到一个点云处理的问题，需要将激光点云拟合出几条直线最后在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
