1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
|
#include <pcl/surface/on_nurbs/fitting_curve_2d_pdm.h>
#include <pcl/surface/on_nurbs/fitting_curve_2d_tdm.h>
#include <pcl/surface/on_nurbs/fitting_curve_2d_sdm.h>
#include <pcl/surface/on_nurbs/triangulation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
pcl::visualization::PCLVisualizer viewer ("Curve Fitting PDM (red), SDM (green), TDM (blue)");
void
PointCloud2Vector2d (pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::on_nurbs::vector_vec2d &data)
{
for (const auto &p : *cloud)
{
if (!std::isnan (p.x) && !std::isnan (p.y))
data.emplace_back (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 (20);
pcl::on_nurbs::FittingCurve2dPDM::Parameter curve_params;
curve_params.smoothness = 0.000001;
curve_params.rScale = 1.0;
// #################### CURVE FITTING #########################
ON_NurbsCurve curve = pcl::on_nurbs::FittingCurve2dPDM::initNurbsCurve2D (order, data.interior, n_control_points);
pcl::on_nurbs::FittingCurve2dPDM fit_pdm (&data, curve);
fit_pdm.assemble (curve_params);
fit_pdm.solve ();
pcl::on_nurbs::FittingCurve2dSDM fit_sdm (&data, curve);
fit_sdm.assemble (curve_params);
fit_sdm.solve ();
pcl::on_nurbs::FittingCurve2dTDM fit_tdm (&data, curve);
fit_tdm.assemble (curve_params);
fit_tdm.solve ();
// visualize
VisualizeCurve (fit_pdm.m_nurbs, 1.0, 0.0, 0.0, false);
VisualizeCurve (fit_sdm.m_nurbs, 0.0, 1.0, 0.0, false);
VisualizeCurve (fit_tdm.m_nurbs, 0.0, 0.0, 1.0, false);
viewer.spin ();
return 0;
}
|
