PCL 绘制自定义大平面
微信公众号:幼儿园的学霸
目录
缘起
最近在项目中用到了点云平面拟合,采用RANSAC拟合平面后得到了平面参数A,B,C,D
不能直观观察拟合的平面与输入点云的关系,需要将拟合的平面绘制出来。在pcl中,存在2个函数能够添加平面,但是添加的平面其size为1,不能完全覆盖点云区域,仍不能满足需求,搜索资料也未发现能够添加大平面的方法,经过查看pcl添加平面的源码,实现了一种添加大平面的方法。
介绍
在pcl中,添加平面有2个方法可以实现。其声明位于pcl/visualization/pcl_visualizer.h
中,具体的实现在pcl/visualization/src/pcl_visualizer.cpp
中:
/** \brief Add a plane from a set of given model coefficients
* \param coefficients the model coefficients (a, b, c, d with ax+by+cz+d=0)
* \param id the plane id/name (default: "plane")
* \param viewport (optional) the id of the new viewport (default: 0)
*/
bool
pcl::visualization::PCLVisualizer::addPlane (const pcl::ModelCoefficients &coefficients, const std::string &id, int viewport)
{
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
if (am_it != shape_actor_map_->end ())
{
pcl::console::print_warn (stderr, "[addPlane] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
return (false);
}
if (coefficients.values.size () != 4)
{
PCL_WARN ("[addPlane] Coefficients size does not match expected size (expected 4).\n");
return (false);
}
vtkSmartPointer<vtkDataSet> data = createPlane (coefficients);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
actor->GetProperty ()->SetRepresentationToSurface ();
addActorToRenderer (actor, viewport);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
return (true);
}
bool
pcl::visualization::PCLVisualizer::addPlane (const pcl::ModelCoefficients &coefficients, double x, double y, double z, const std::string &id, int viewport)
{
// Check to see if this ID entry already exists (has it been already added to the visualizer?)
ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
if (am_it != shape_actor_map_->end ())
{
pcl::console::print_warn (stderr, "[addPlane] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
return (false);
}
if (coefficients.values.size () != 4)
{
PCL_WARN ("[addPlane] Coefficients size does not match expected size (expected 4).\n");
return (false);
}
vtkSmartPointer<vtkDataSet> data = createPlane (coefficients, x, y, z);
// Create an Actor
vtkSmartPointer<vtkLODActor> actor;
createActorFromVTKDataSet (data, actor);
actor->GetProperty ()->SetRepresentationToSurface ();
addActorToRenderer (actor, viewport);
// Save the pointer/ID pair to the global actor map
(*shape_actor_map_)[id] = actor;
return (true);
}
从以上实现可以发现,pcl中添加平面,主要是通过调用函数vtkSmartPointer<vtkDataSet> data = createPlane (...)
来完成的,函数的命名说明该函数的功能是创建一个平面,随后pcl负责添加并显示该平面。其中createPlane(...)
的实现位于文件pcl/visualization/src/common/common.cpp
中,内容如下:
vtkSmartPointer<vtkDataSet>
pcl::visualization::createPlane (const pcl::ModelCoefficients &coefficients)
{
vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New ();
plane->SetNormal (coefficients.values[0], coefficients.values[1], coefficients.values[2]);
double norm_sqr = coefficients.values[0] * coefficients.values[0]
+ coefficients.values[1] * coefficients.values[1]
+ coefficients.values[2] * coefficients.values[2];
plane->Push (-coefficients.values[3] / sqrt(norm_sqr));
plane->Update ();
return (plane->GetOutput ());
}
vtkSmartPointer<vtkDataSet>
pcl::visualization::createPlane (const pcl::ModelCoefficients &coefficients, double x, double y, double z)
{
vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New ();
double norm_sqr = 1.0 / (coefficients.values[0] * coefficients.values[0] +
coefficients.values[1] * coefficients.values[1] +
coefficients.values[2] * coefficients.values[2] );
// double nx = coefficients.values [0] * norm;
// double ny = coefficients.values [1] * norm;
// double nz = coefficients.values [2] * norm;
// double d = coefficients.values [3] * norm;
// plane->SetNormal (nx, ny, nz);
plane->SetNormal (coefficients.values[0], coefficients.values[1], coefficients.values[2]);
double t = x * coefficients.values[0] + y * coefficients.values[1] + z * coefficients.values[2] + coefficients.values[3];
x -= coefficients.values[0] * t * norm_sqr;
y -= coefficients.values[1] * t * norm_sqr;
z -= coefficients.values[2] * t * norm_sqr;
plane->SetCenter (x, y, z);
plane->Update ();
return (plane->GetOutput ());
}
函数创建平面是通过vtk
创建plane
完成的,但是上面的函数实现中仅仅设置了平面的法向量、center信息,并没有和平面大小相关的信息。通过查看vtkPlaneSource
的头文件可以看到,其方法有SetPoint1,SetPoint2
等,这些方法的说明如下:
// Description:
// Specify a point defining the origin of the plane.
vtkSetVector3Macro(Origin,double);
vtkGetVectorMacro(Origin,double,3);
// Description:
// Specify a point defining the first axis of the plane.
void SetPoint1(double x, double y, double z);
void SetPoint1(double pnt[3]);
vtkGetVectorMacro(Point1,double,3);
// Description:
// Specify a point defining the second axis of the plane.
void SetPoint2(double x, double y, double z);
void SetPoint2(double pnt[3]);
vtkGetVectorMacro(Point2,double,3);
主要是设置平面的原点位置、平面2条边的位置(边长信息)。因此通过修改平面边长可以实现对平面大小的修改。
实现
实现原理简介
以1*x + 0*y + 0*z + (-1) = 0
平面为例,该平面参数为(1,0,0,-1)
,调用pcl中addPlane
函数添加平面(平面center=(3,3,3))后,该平面的origin=(1,2.5,3.5),point1=(1,2.5,2.5),point2=(1,3.5,3.5)
,在坐标系中大概如下图所示:
从上图可以看到平面的大小默认为1,增大平面大小关键在于延长平面的边长OB,OA
或者延长CA,CB
。此处我选择延长OA,OB
。若需要对边长OB
扩大为原长度1的
λ
\lambda
λ大小,那么只需要求出延长后点B的坐标
B
′
B^{'}
B′即可。
假设坐标系原点(0,0,0)
记为字母D
。计算过程如下:
O B ′ ⃗ = λ ( D B ⃗ − D O ⃗ ) 显 然 , D B ⃗ 即 表 示 点 B 的 坐 标 D B ′ ⃗ = D O ⃗ + O B ′ ⃗ \vec {OB^{'}} = \lambda (\vec {DB}- \vec{DO}) 显然, \vec {DB} 即表示点B的坐标 \\ \vec {DB^{'}} = \vec {DO} + \vec {OB^{'}} OB′=λ(DB−DO)显然,DB即表示点B的坐标DB′=DO+OB′
从而延长后点B的坐标可得。
代码实现及效果展示
代码实现如下。2种实现方式分别对应PCL中addPlane的两种不实现。读者可以用以下两种实现替换pcl中对应的函数,并添加默认参数,然后重新编译pcl。如果认为这样麻烦,也可以像我的例子那样使用,来扩大平面。
#include <pcl/visualization/pcl_visualizer.h>
#include <iostream>
#include <vtkPlaneSource.h>
//对应pcl中的bool pcl::visualization::PCLVisualizer::addPlane (const pcl::ModelCoefficients &coefficients, const std::string &id, int viewport)
vtkSmartPointer<vtkPolyData> createPlane(const pcl::ModelCoefficients& coefficients, float scale[2]=nullptr)
{
vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New ();
plane->SetNormal (coefficients.values[0], coefficients.values[1], coefficients.values[2]);
double norm_sqr = coefficients.values[0] * coefficients.values[0]
+ coefficients.values[1] * coefficients.values[1]
+ coefficients.values[2] * coefficients.values[2];
plane->Push(-coefficients.values[3] / sqrt(norm_sqr));
plane->SetResolution(200, 200);
plane->Update();
double pt1[3], pt2[3], orig[3],center[3];
plane->GetPoint1(pt1);
plane->GetPoint2(pt2);
plane->GetOrigin(orig);
plane->GetCenter(center);
double _pt1[3], _pt2[3];
float scale1=3.0;
float scale2=3.0;
if( scale!= nullptr )
{
scale1 = scale[0];
scale2 = scale[1];
}
for(int i = 0; i < 3; i++) {
_pt1[i] = scale1 * (pt1[i] - orig[i]);
_pt2[i] = scale2 * (pt2[i] - orig[i]);
}
for(int i=0; i<3;++i)
{
pt1[i] = orig[i] + _pt1[i];
pt2[i] = orig[i] + _pt2[i];
}
plane->SetPoint1(pt1);
plane->SetPoint2(pt2);
// //延长origin
// double _origin[3];
// for(int i=0; i<3;++i)
// {
// _origin[i] = scale*(orig[i]-pt1[i]);
// }
// for(int i=0; i<3;++i)
// {
// orig[i] = pt1[i] + _origin[i];
// }
// plane->SetOrigin(orig);
plane->Update();
return (plane->GetOutput());
}
vtkSmartPointer<vtkPolyData> /*pcl::visualization::*/createPlane(const pcl::ModelCoefficients &coefficients, double x, double y, double z, float scale[2]=nullptr)
{
vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New();
double norm_sqr = 1.0 / (coefficients.values[0] * coefficients.values[0] +
coefficients.values[1] * coefficients.values[1] +
coefficients.values[2] * coefficients.values[2]);
plane->SetNormal(coefficients.values[0], coefficients.values[1], coefficients.values[2]);
double t = x * coefficients.values[0] + y * coefficients.values[1] + z * coefficients.values[2] + coefficients.values[3];
x -= coefficients.values[0] * t * norm_sqr;
y -= coefficients.values[1] * t * norm_sqr;
z -= coefficients.values[2] * t * norm_sqr;
plane->SetCenter(x, y, z);
{
double pt1[3], pt2[3], orig[3],center[3];
plane->GetPoint1(pt1);
plane->GetPoint2(pt2);
plane->GetOrigin(orig);
plane->GetCenter(center);
float scale1=3.0;
float scale2=3.0;
if( scale!= nullptr )
{
scale1 = scale[0];
scale2 = scale[1];
}
//延长pt1,pt2. 延长origin到pt1连线的方向向量
double _pt1[3], _pt2[3];
for(int i = 0; i < 3; i++) {
_pt1[i] = scale1 * (pt1[i] - orig[i]);
_pt2[i] = scale2 * (pt2[i] - orig[i]);
}
//pt1相对于原坐标系下的坐标值
for(int i=0; i<3;++i)
{
pt1[i] = orig[i] + _pt1[i];
pt2[i] = orig[i] + _pt2[i];
}
plane->SetPoint1(pt1);
plane->SetPoint2(pt2);
// //延长origin
// double _origin[3];
// for(int i=0; i<3;++i)
// {
// _origin[i] = scale*(orig[i]-pt1[i]);
// }
// for(int i=0; i<3;++i)
// {
// orig[i] = pt1[i] + _origin[i];
// }
// plane->SetOrigin(orig);
}
plane->Update();
return (plane->GetOutput());
}
//=================================================//
//使用例子
int main (int argc, char *argv[])
{
pcl::visualization::PCLVisualizer viewer ("PCL visualizer");
pcl::ModelCoefficients::Ptr plane_1 (new pcl::ModelCoefficients);
plane_1->values.resize (4);
plane_1->values[0] = 1;
plane_1->values[1] = 0;
plane_1->values[2] = 0;
plane_1->values[3] = -1;
//添加自定义的平面
float scale[2] = {2,5};
auto plane = createPlane(*plane_1,3,3,3,scale);
viewer.addModelFromPolyData (plane, "plane_1");
viewer.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.9, 0.1, 0.1, "plane_1", 0);
viewer.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.6, "plane_1", 0);
//viewer.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "plane_1", 0);
//pcl默认的平面
viewer.addPlane(*plane_1,3,3,3,"plane_2", 0);
viewer.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.1, 0.1, 0.9, "plane_2", 0);
viewer.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.6, "plane_2", 0);
//viewer.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "plane_2", 0);
viewer.addCoordinateSystem (0.5, "axis", 0);
viewer.setBackgroundColor (0.05, 0.05, 0.05, 0);
viewer.setPosition (800, 400);
while (!viewer.wasStopped ())
{
viewer.spinOnce ();
}
return 0;
}
运行程序,效果如下。可以看新添加的平面变大了,且并非一方形平面。