《PCL点云库学习&VS2010(X64)》Part 24 PCL&VTK&Eigen Spline曲线拟合
1、PCL曲线拟合:
由于需要编译PCL的源码才能使用曲线拟合功能,所以,暂时在另外一台计算机上编译成功PCL1.8.0。出现了一个问题,编译时勾选上了BUILD_surface_on_nurbs,同时勾选了库中的Example。编译出来后例子都能运行成功。但是自己调试程序的时候,出现了一个与max()函数相关的问题,该问题也解决了,编译成功。
这个是拟合二维点云的程序,拟合三维点云时需要做一个平面投影,该投影算法很简单,可以去官网找找。
nurbs_fitting_curve2dcpp:
#include <pcl/surface/on_nurbs/fitting_curve_2d.h>
#include <pcl/surface/on_nurbs/triangulation.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
pcl::visualization::PCLVisualizer viewer ("Curve Fitting 2D");
void
PointCloud2Vector2d (pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::on_nurbs::vector_vec2d &data)
{
for (unsigned i = 0; i < cloud->size (); i++)
{
pcl::PointXYZ &p = cloud->at (i);
if (!pcl_isnan (p.x) && !pcl_isnan (p.y))
data.push_back (Eigen::Vector2d (p.x, p.y));
}
}
void
VisualizeCurve (ON_NurbsCurve &curve, double r, double g, double b, bool show_cps)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::on_nurbs::Triangulation::convertCurve2PointCloud (curve, cloud, 8);
for (std::size_t i = 0; i < cloud->size () - 1; i++)
{
pcl::PointXYZRGB &p1 = cloud->at (i);
pcl::PointXYZRGB &p2 = cloud->at (i + 1);
std::ostringstream os;
os << "line_" << r << "_" << g << "_" << b << "_" << i;
viewer.addLine<pcl::PointXYZRGB> (p1, p2, r, g, b, os.str ());
}
if (show_cps)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cps (new pcl::PointCloud<pcl::PointXYZ>);
for (int i = 0; i < curve.CVCount (); i++)
{
ON_3dPoint cp;
curve.GetCV (i, cp);
pcl::PointXYZ p;
p.x = float (cp.x);
p.y = float (cp.y);
p.z = float (cp.z);
cps->push_back (p);
}
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> handler (cps, 255 * r, 255 * g, 255 * b);
viewer.addPointCloud<pcl::PointXYZ> (cps, handler, "cloud_cps");
}
}
int
main (int argc, char *argv[])
{
std::string pcd_file;
if (argc > 1)
{
pcd_file = argv[1];
}
else
{
printf ("\nUsage: pcl_example_nurbs_fitting_curve pcd-file \n\n");
printf (" pcd-file point-cloud file\n");
exit (0);
}
// #################### LOAD FILE #########################
printf (" loading %s\n", pcd_file.c_str ());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCLPointCloud2 cloud2;
if (pcl::io::loadPCDFile (pcd_file, cloud2) == -1)
throw std::runtime_error (" PCD file not found.");
fromPCLPointCloud2 (cloud2, *cloud);
// convert to NURBS data structure
pcl::on_nurbs::NurbsDataCurve2d data;
PointCloud2Vector2d (cloud, data.interior);
viewer.setSize (800, 600);
viewer.addPointCloud<pcl::PointXYZ> (cloud, "cloud");
// #################### CURVE PARAMETERS #########################
unsigned order (3);
unsigned n_control_points (10);
pcl::on_nurbs::FittingCurve2d::Parameter curve_params;
curve_params.smoothness = 0.000001;
curve_params.rScale = 1.0;
// #################### CURVE FITTING #########################
ON_NurbsCurve curve = pcl::on_nurbs::FittingCurve2d::initNurbsPCA (order, &data, n_control_points);
pcl::on_nurbs::FittingCurve2d fit (&data, curve);
fit.assemble (curve_params);
Eigen::Vector2d fix1 (0.1, 0.1);
Eigen::Vector2d fix2 (1.0, 0.0);
// fit.addControlPointConstraint (0, fix1, 100.0);
// fit.addControlPointConstraint (curve.CVCount () - 1, fix2, 100.0);
fit.solve ();
// visualize
VisualizeCurve (fit.m_nurbs, 1.0, 0.0, 0.0, true);
viewer.spin ();
return 0;
}
2、VTK拟合空间中的点生成spline曲线(PCL1.7.2+VTK6.2)
例1:
代码:注意高版本的VTK如VTK7.0需要用到vtkRenderingOpenGL2
#include <vtkAutoInit.h>
VTK_MODULE_INIT(vtkRenderingOpenGL);
VTK_MODULE_INIT(vtkInteractionStyle);
VTK_MODULE_INIT(vtkRenderingFreeType);
#include <vtkSmartPointer.h>
#include <vtkCellArray.h>
#include <vtkCellData.h>
#include <vtkDoubleArray.h>
#include <vtkPoints.h>
#include <vtkParametricSpline.h>
#include <vtkPolyData.h>
#include <vtkPolyDataMapper.h>
#include <vtkActor.h>
#include <vtkRenderWindow.h>
#include <vtkRenderer.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkParametricFunctionSource.h>
int main(int, char *[])
{
// Create three points. We will join (Origin and P0) with a red line and (Origin and P1) with a green line
double origin[3] = { 0.0, 0.0, 0.0 };
double p0[3] = { 1.0, 0.0, 0.0 };
double p1[3] = { 0.0, 1.0, 0.0 };
double p2[3] = { 0.0, 1.0, 2.0 };
double p3[3] = { 1.0, 2.0, 3.0 };
// Create a vtkPoints object and store the points in it
vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
points->InsertNextPoint(origin);
points->InsertNextPoint(p0);
points->InsertNextPoint(p1);
points->InsertNextPoint(p2);
points->InsertNextPoint(p3);
vtkSmartPointer<vtkParametricSpline> spline =
vtkSmartPointer<vtkParametricSpline>::New();
spline->SetPoints(points);
vtkSmartPointer<vtkParametricFunctionSource> functionSource =
vtkSmartPointer<vtkParametricFunctionSource>::New();
functionSource->SetParametricFunction(spline);
functionSource->Update();
// Setup actor and mapper
vtkSmartPointer<vtkPolyDataMapper> mapper =
vtkSmartPointer<vtkPolyDataMapper>::New();
mapper->SetInputConnection(functionSource->GetOutputPort());
vtkSmartPointer<vtkActor> actor =
vtkSmartPointer<vtkActor>::New();
actor->SetMapper(mapper);
// Setup render window, renderer, and interactor
vtkSmartPointer<vtkRenderer> renderer =
vtkSmartPointer<vtkRenderer>::New();
vtkSmartPointer<vtkRenderWindow> renderWindow =
vtkSmartPointer<vtkRenderWindow>::New();
renderWindow->AddRenderer(renderer);
vtkSmartPointer<vtkRenderWindowInteractor> renderWindowInteractor =
vtkSmartPointer<vtkRenderWindowInteractor>::New();
renderWindowInteractor->SetRenderWindow(renderWindow);
renderer->AddActor(actor);
renderWindow->Render();
renderWindowInteractor->Start();
return EXIT_SUCCESS;
}
例2:
代码:相对代码一来说比较精简,生成的圆柱体管道并没有显示出来,只有样条曲线。其中有个函数要注意:setInputData()与原文的有点出入。
代码:
#include <vtkAutoInit.h>
VTK_MODULE_INIT(vtkRenderingOpenGL);
VTK_MODULE_INIT(vtkInteractionStyle);
VTK_MODULE_INIT(vtkRenderingFreeType);
#include "vtkActor.h"
#include "vtkCamera.h"
#include "vtkCellArray.h"
#include "vtkPoints.h"
#include "vtkPolyData.h"
#include "vtkPolyDataMapper.h"
#include "vtkRenderWindow.h"
#include "vtkRenderWindowInteractor.h"
#include "vtkRenderer.h"
#include "vtkProperty.h"
#include "vtkTubeFilter.h"
#include "vtkParametricSpline.h"
#include "vtkParametricFunctionSource.h"
int main()
{
vtkPoints *points = vtkPoints::New();
points->InsertPoint(0, 0.0, 0.0, 0.0);
points->InsertPoint(1, 1.0, 1.0, 1.0);
points->InsertPoint(2, 1.0, 0.0, 0.0);
points->InsertPoint(3, 1.0, 0.0, 1.0);
//
//插值为样条曲线
vtkParametricSpline *spline = vtkParametricSpline::New();
spline->SetPoints(points);
spline->ClosedOff();
vtkParametricFunctionSource *splineSource = vtkParametricFunctionSource::New();
splineSource->SetParametricFunction(spline);
vtkPolyDataMapper *splineMapper = vtkPolyDataMapper::New();
splineMapper->SetInputConnection(splineSource->GetOutputPort());
vtkActor *splineActor = vtkActor::New();
splineActor->SetMapper(splineMapper);
splineActor->GetProperty()->SetColor(0.8, 0.8, 0.1600);
//
vtkTubeFilter *tube = vtkTubeFilter::New();
tube->SetInputData(splineSource->GetOutput());
tube->SetNumberOfSides(20);
tube->SetRadius(0.08);
vtkPolyDataMapper *tubeMapper = vtkPolyDataMapper::New();
tubeMapper->SetInputData(tube->GetOutput());
vtkActor *tubeActor = vtkActor::New();
tubeActor->SetMapper(tubeMapper);
vtkRenderer *ren1 = vtkRenderer::New();
vtkRenderWindow *renWin = vtkRenderWindow::New();
renWin->AddRenderer(ren1);
vtkRenderWindowInteractor *iren = vtkRenderWindowInteractor::New();
iren->SetRenderWindow(renWin);
ren1->AddActor(splineActor);
ren1->AddActor(tubeActor);
ren1->SetBackground(0.5, 0.5, 0.5);
renWin->SetSize(960, 480);
renWin->Render();
iren->Start();
return 0;
}
这个图,没有圆柱管道出现,函数可能有点变动。
例3:原文中有SCurve,自己编译的VTK库里面没有这个头文件,暂时注释掉。
代码:
#include <vtkAutoInit.h>
VTK_MODULE_INIT(vtkRenderingOpenGL);
VTK_MODULE_INIT(vtkInteractionStyle);
VTK_MODULE_INIT(vtkRenderingFreeType);
#include <vtkSmartPointer.h>
#include <vtkPolyData.h>
#include <vtkPolyDataMapper.h>
#include <vtkActor.h>
#include <vtkRenderer.h>
#include <vtkRenderWindow.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkProperty.h>
#include <vtkPoints.h>
#include <vtkPointSource.h>
#include <vtkSpline.h>
#include <vtkCardinalSpline.h>
#include <vtkParametricSpline.h>
#include <vtkParametricFunctionSource.h>
#include <vtkKochanekSpline.h>
//#include <vtkSCurveSpline.h>
int main()
{
/*
//1.原例子设置点的方式
vtkSmartPointer<vtkPointSource> pointSource =
vtkSmartPointer<vtkPointSource>::New();
pointSource->SetNumberOfPoints(5);
pointSource->Update();
vtkPoints* points = pointSource->GetOutput()->GetPoints();
*/
//2.自己设置定点,也可以运行
vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
points->InsertNextPoint(0, 0, 0);
points->InsertNextPoint(0, 1, 0);
points->InsertNextPoint(1, 1, 0);
points->InsertNextPoint(1, 0, 0);
points->InsertNextPoint(0, 0, 0);
//scurve spline
//vtkSmartPointer<vtkSCurveSpline> xSpline =
//vtkSmartPointer<vtkSCurveSpline>::New();
//vtkSmartPointer<vtkSCurveSpline> ySpline =
//vtkSmartPointer<vtkSCurveSpline>::New();
//vtkSmartPointer<vtkSCurveSpline> zSpline =
//vtkSmartPointer<vtkSCurveSpline>::New();
//cardinal spline
vtkSmartPointer<vtkCardinalSpline> xSpline = vtkSmartPointer<vtkCardinalSpline>::New();
vtkSmartPointer<vtkCardinalSpline> ySpline = vtkSmartPointer<vtkCardinalSpline>::New();
vtkSmartPointer<vtkCardinalSpline> zSpline = vtkSmartPointer<vtkCardinalSpline>::New();
//kochanek spline
//vtkSmartPointer<vtkKochanekSpline> xSpline = vtkSmartPointer<vtkKochanekSpline>::New();
//vtkSmartPointer<vtkKochanekSpline> ySpline = vtkSmartPointer<vtkKochanekSpline>::New();
//vtkSmartPointer<vtkKochanekSpline> zSpline = vtkSmartPointer<vtkKochanekSpline>::New();
vtkSmartPointer<vtkParametricSpline> spline =
vtkSmartPointer<vtkParametricSpline>::New();
spline->SetXSpline(xSpline);
spline->SetYSpline(ySpline);
spline->SetZSpline(zSpline);
spline->SetPoints(points);
vtkSmartPointer<vtkParametricFunctionSource> functionSource =
vtkSmartPointer<vtkParametricFunctionSource>::New();
functionSource->SetParametricFunction(spline);
functionSource->Update();
// Setup actor and mapper
vtkSmartPointer<vtkPolyDataMapper> mapper =
vtkSmartPointer<vtkPolyDataMapper>::New();
mapper->SetInputConnection(functionSource->GetOutputPort());
vtkSmartPointer<vtkActor> actor =
vtkSmartPointer<vtkActor>::New();
actor->SetMapper(mapper);
// Setup render window, renderer, and interactor
vtkSmartPointer<vtkRenderer> renderer =
vtkSmartPointer<vtkRenderer>::New();
vtkSmartPointer<vtkRenderWindow> renderWindow =
vtkSmartPointer<vtkRenderWindow>::New();
renderWindow->AddRenderer(renderer);
vtkSmartPointer<vtkRenderWindowInteractor> renderWindowInteractor =
vtkSmartPointer<vtkRenderWindowInteractor>::New();
renderWindowInteractor->SetRenderWindow(renderWindow);
renderer->AddActor(actor);
renderer->SetBackground(0.5, 0.5, 0.5);
renderWindow->SetSize(960, 640);
renderWindow->Render();
renderWindowInteractor->Start();
return 0;
}
3、Eigen拟合Spline
相关的论文有介绍,自己可以找找。
stackoverflow里面的拟合spline得得到对应y值:
#include <Eigen/Core>
#include <unsupported/Eigen/Splines>
#include <iostream>
class SplineFunction {
public:
SplineFunction(Eigen::VectorXd const &x_vec,
Eigen::VectorXd const &y_vec)
: x_min(x_vec.minCoeff()),
x_max(x_vec.maxCoeff()),
// Spline fitting here. X values are scaled down to [0, 1] for this.
spline_(Eigen::SplineFitting<Eigen::Spline<double, 1>>::Interpolate(
y_vec.transpose(),
// No more than cubic spline, but accept short vectors.
std::min<int>(x_vec.rows() - 1, 3),
scaled_values(x_vec)))
{ }
double operator()(double x) const {
// x values need to be scaled down in extraction as well.
return spline_(scaled_value(x))(0);
}
private:
// Helpers to scale X values down to [0, 1]
double scaled_value(double x) const {
return (x - x_min) / (x_max - x_min);
}
Eigen::RowVectorXd scaled_values(Eigen::VectorXd const &x_vec) const {
return x_vec.unaryExpr([this](double x) { return scaled_value(x); }).transpose();
}
double x_min;
double x_max;
// Spline of one-dimensional "points."
Eigen::Spline<double, 1> spline_;
};
int main(int argc, char const* argv[])
{
Eigen::VectorXd xvals(3);
Eigen::VectorXd yvals(xvals.rows());
xvals << 0, 15, 30;
yvals << 0, 12, 17;
SplineFunction s(xvals, yvals);
std::cout << s(12.34) << std::endl;
}