《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;
}