void reconstructMesh(const PointCloud<PointXYZ>::ConstPtr &cloud,
PointCloud<PointXYZ> &output_cloud, std::vector<Vertices> &triangles)
{
boost::shared_ptr<std::vector<int> > indices(new std::vector<int>);
indices->resize(cloud->points.size ());
for (size_t i = 0; i < indices->size (); ++i) { (*indices)[i] = i; }
pcl::search::KdTree<PointXYZ>::Ptr tree(new pcl::search::KdTree<PointXYZ>);
tree->setInputCloud(cloud);
PointCloud<PointXYZ>::Ptr mls_points(new PointCloud<PointXYZ>);
PointCloud<PointNormal>::Ptr mls_normals(new PointCloud<PointNormal>);
MovingLeastSquares<PointXYZ, PointNormal> mls;
mls.setInputCloud(cloud);
mls.setIndices(indices);
mls.setPolynomialFit(true);
mls.setSearchMethod(tree);
mls.setSearchRadius(0.03);
mls.process(*mls_normals);
ConvexHull<PointXYZ> ch;
ch.setInputCloud(mls_points);
ch.reconstruct(output_cloud, triangles);
}
void HxCPDSpatialGraphWarp::applyNLDeformationToSlice(
SpatialGraphSelection& slice, HxSpatialGraph* spatialGraph,
const McDArray<McVec3f>& origCoords,
const McDArray<McVec3f>& shiftedCoords) {
McWatch watch;
watch.start();
MovingLeastSquares mls;
mls.setAlpha(portAlphaForMLS.getValue());
mls.setLandmarks(origCoords, shiftedCoords);
ma::SliceSelector ssh(spatialGraph, "TransformInfo");
SpatialGraphSelection fullSliceSelection;
ssh.getSlice(ssh.getSliceAttributeValueFromIndex(1), fullSliceSelection);
for (int i = 0; i < fullSliceSelection.getNumSelectedEdges(); i++) {
int edge = fullSliceSelection.getSelectedEdge(i);
McDArray<McVec3f> newEdgePoints;
for (int j = 0; j < spatialGraph->getNumEdgePoints(edge); j++) {
McVec3f edgePoint = spatialGraph->getEdgePoint(edge, j);
warpPoint(edgePoint, edgePoint, mls);
newEdgePoints.append(edgePoint);
}
spatialGraph->setEdgePoints(edge, newEdgePoints);
}
for (int i = 0; i < fullSliceSelection.getNumSelectedVertices(); i++) {
int curVertex = fullSliceSelection.getSelectedVertex(i);
McVec3f curCoord = spatialGraph->getVertexCoords(curVertex);
McVec3f curCoordWarped;
warpPoint(curCoord, curCoordWarped, mls);
spatialGraph->setVertexCoords(curVertex, curCoordWarped);
// Add new segments that indicate shift.
if (slice.isSelectedVertex(curVertex)) {
int newVertex = spatialGraph->addVertex(curCoord);
McDArray<McVec3f> edgePoints(2);
edgePoints[0] = curCoord;
edgePoints[1] = curCoordWarped;
spatialGraph->addEdge(newVertex, curVertex, edgePoints);
}
}
std::cout << "\n Apply deformation took " << watch.stop() << " seconds.";
}
int main (int argc, char **argv)
{
/* if (argc != 1)
{
PCL_ERROR ("Syntax: %s input.pcd output.ply\n", argv[0]);
return -1;
}*/
PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ> ());
io::loadPCDFile (argv[1], *cloud);
MovingLeastSquares<PointXYZ, PointXYZ> mls;
mls.setInputCloud (cloud);
mls.setSearchRadius (4);
mls.setPolynomialFit (true);
mls.setPolynomialOrder (1);
mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointXYZ>::SAMPLE_LOCAL_PLANE);
mls.setUpsamplingRadius (1);
mls.setUpsamplingStepSize (0.03);
PointCloud<PointXYZ>::Ptr cloud_smoothed (new PointCloud<PointXYZ> ());
mls.process (*cloud_smoothed);
NormalEstimationOMP<PointXYZ, Normal> ne;
ne.setNumberOfThreads (8);
ne.setInputCloud (cloud_smoothed);
ne.setRadiusSearch (0.8);
Eigen::Vector4f centroid;
compute3DCentroid (*cloud_smoothed, centroid);
ne.setViewPoint (centroid[0], centroid[1], centroid[2]);
PointCloud<Normal>::Ptr cloud_normals (new PointCloud<Normal> ());
ne.compute (*cloud_normals);
for (size_t i = 0; i < cloud_normals->size (); ++i)
{
cloud_normals->points[i].normal_x *= -1;
cloud_normals->points[i].normal_y *= -1;
cloud_normals->points[i].normal_z *= -1;
}
PointCloud<PointNormal>::Ptr cloud_smoothed_normals (new PointCloud<PointNormal> ());
concatenateFields (*cloud_smoothed, *cloud_normals, *cloud_smoothed_normals);
Poisson<pcl::PointNormal> poisson;
poisson.setDepth (9);
poisson.setInputCloud (cloud_smoothed_normals);
PolygonMesh mesh_poisson;
poisson.reconstruct (mesh_poisson);
pcl::io::savePLYFile("mesh.ply", mesh_poisson);
return 0;
}
TEST(MovingLeastSquares, shouldInterpolateSmoothly) {
const double rbegin = 0;
const double rend = 5;
const double repsmarks = 2.0; // Spacing for landmarks.
const double shift = 1; // Fixed shift.
const double sigma = 0.2; // For random shifts.
const double tolerance = 0.01;
const Landmarks marks = randomGrid(rbegin, rend, repsmarks, shift, sigma);
MovingLeastSquares mls;
mls.setLandmarks(marks.src, marks.dst);
// Exactly interpolate landmarks.
for (int i = 0; i < marks.src.size(); i++) {
const McVec2d pos = McVec2d(marks.src[i][0], marks.src[i][1]);
const McVec2d tpos = mls.interpolate(pos);
const McVec2d tdiff = tpos - McVec2d(marks.dst[i][0], marks.dst[i][1]);
EXPECT_FLOAT_EQ(0, tdiff.length());
}
const int seed = 38273;
McRandom rand(seed);
// Interpolate points close to landmarks.
for (int i = 0; i < marks.src.size(); i++) {
const double d = sigma * (rand.nextNumber() - 0.5);
const McVec2d delta(d, d);
const McVec2d pos = McVec2d(marks.src[i][0], marks.src[i][1]) + delta;
const McVec2d tpos = mls.interpolate(pos);
const McVec2d tdiff =
tpos - delta - McVec2d(marks.dst[i][0], marks.dst[i][1]);
EXPECT_THAT(tdiff.length(), Lt(tolerance));
#if 0 // Useful for debugging.
printf("tdiff = %f.\n", tdiff.length());
#endif
}
}
static void computeNormals(PointCloudPtr pcl_cloud_input, pcl::PointCloud<PointNormal>::Ptr pcl_cloud_normals, const double search_radius)
{
typedef pcl::search::KdTree<PointT> KdTree;
typedef typename KdTree::Ptr KdTreePtr;
// Create a KD-Tree
KdTreePtr tree = boost::make_shared<pcl::search::KdTree<PointT> >();
MovingLeastSquares<PointT, PointNormal> mls;
mls.setComputeNormals(true);
mls.setInputCloud(pcl_cloud_input);
//mls.setPolynomialFit(true);
mls.setSearchMethod(tree);
mls.setSearchRadius(search_radius);
mls.process(*pcl_cloud_normals);
}
void RScloud::smooth_cloud() {
//MLS SMOOTHING
MovingLeastSquares<pointT, pointT> mls;
mls.setInputCloud (pointcloud);
mls.setSearchRadius (0.004);
mls.setPolynomialFit (true);
mls.setPolynomialOrder (2);
PointCloud<pointT>::Ptr cloud_smoothed (new PointCloud<pointT> ());
mls.process (*cloud_smoothed);
//remove nans
for(int i=0;i<cloud_smoothed->points.size();i++){
if (!(pcl::isFinite(cloud_smoothed->at(i)))){
cloud_smoothed->at(i).x = 0.0;
cloud_smoothed->at(i).y = 0.0;
cloud_smoothed->at(i).z = 0.0;
}
}
pointcloud = cloud_smoothed;
}
TEST(MovingLeastSquares, isInvertibleByExchangingLandmarks) {
const double rbegin = 0;
const double rend = 5;
const double repsmarks = 2.0; // Spacing for landmarks.
const double repstest = 0.1; // Spacing for test positions.
const double shift = 1; // Fixed shift.
const double sigma = 0.2; // For random shifts.
const double tolerance = 0.03;
const Landmarks marks = randomGrid(rbegin, rend, repsmarks, shift, sigma);
MovingLeastSquares forward;
forward.setLandmarks(marks.src, marks.dst);
MovingLeastSquares backward;
backward.setLandmarks(marks.dst, marks.src);
// Test that forward transform does something and forward-backward
// transform gets back to original position.
for (double x = rbegin; x < rend; x += repstest) {
for (double y = rbegin; y < rend; y += repstest) {
const McVec2d pos(x, y);
const McVec2d tpos = forward.interpolate(pos);
const McVec2d ttpos = backward.interpolate(tpos);
const McVec2d tdiff = tpos - pos;
const McVec2d ttdiff = ttpos - pos;
EXPECT_THAT(tdiff.length(), Gt(tolerance));
EXPECT_THAT(ttdiff.length(), Lt(tolerance));
#if 0 // Useful for debugging.
printf("tdiff = %f, ttdiff = %f\n", tdiff.length(),
ttdiff.length());
#endif
}
}
}
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
double search_radius, bool sqr_gauss_param_set, double sqr_gauss_param,
bool use_polynomial_fit, int polynomial_order)
{
PointCloud<PointXYZ>::Ptr xyz_cloud_pre (new pcl::PointCloud<PointXYZ> ()),
xyz_cloud (new pcl::PointCloud<PointXYZ> ());
fromROSMsg (*input, *xyz_cloud_pre);
// Filter the NaNs from the cloud
for (size_t i = 0; i < xyz_cloud_pre->size (); ++i)
if (pcl_isfinite (xyz_cloud_pre->points[i].x))
xyz_cloud->push_back (xyz_cloud_pre->points[i]);
xyz_cloud->header = xyz_cloud_pre->header;
xyz_cloud->height = 1;
xyz_cloud->width = static_cast<uint32_t> (xyz_cloud->size ());
xyz_cloud->is_dense = false;
PointCloud<PointNormal>::Ptr xyz_cloud_smoothed (new PointCloud<PointNormal> ());
MovingLeastSquares<PointXYZ, PointNormal> mls;
mls.setInputCloud (xyz_cloud);
mls.setSearchRadius (search_radius);
if (sqr_gauss_param_set) mls.setSqrGaussParam (sqr_gauss_param);
mls.setPolynomialFit (use_polynomial_fit);
mls.setPolynomialOrder (polynomial_order);
// mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::SAMPLE_LOCAL_PLANE);
// mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::RANDOM_UNIFORM_DENSITY);
// mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::VOXEL_GRID_DILATION);
mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::NONE);
mls.setPointDensity (60000 * int (search_radius)); // 300 points in a 5 cm radius
mls.setUpsamplingRadius (0.025);
mls.setUpsamplingStepSize (0.015);
mls.setDilationIterations (2);
mls.setDilationVoxelSize (0.01f);
search::KdTree<PointXYZ>::Ptr tree (new search::KdTree<PointXYZ> ());
mls.setSearchMethod (tree);
mls.setComputeNormals (true);
PCL_INFO ("Computing smoothed surface and normals with search_radius %f , sqr_gaussian_param %f, polynomial fitting %d, polynomial order %d\n",
mls.getSearchRadius(), mls.getSqrGaussParam(), mls.getPolynomialFit(), mls.getPolynomialOrder());
TicToc tt;
tt.tic ();
mls.process (*xyz_cloud_smoothed);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", xyz_cloud_smoothed->width * xyz_cloud_smoothed->height); print_info (" points]\n");
toROSMsg (*xyz_cloud_smoothed, output);
}
void HxMovingLeastSquaresWarp::compute() {
if (!portAction.wasHit())
return;
HxUniformScalarField3* fromImage =
(HxUniformScalarField3*)portFromImage.getSource();
HxLandmarkSet* pointSet = hxconnection_cast<HxLandmarkSet>(portData);
if (!fromImage)
return;
/* It is ok to warp without landmarks, if method is rigid and
input has a transformation */
if (!pointSet) {
return;
} else if (pointSet->getNumSets() < 2) {
theMsg->printf(
"Error: LandmarkWarp data has to contain at least 2 sets.");
return;
}
HxUniformScalarField3* outputImage;
HxUniformVectorField3* outputVectorField = 0;
outputImage = createOutputDataSet();
outputVectorField = createOutputVectorDataSet();
float* outputImageData = (float*)outputImage->lattice().dataPtr();
McDArray<McVec2d> landmarks1, landmarks2;
prepareLandmarks(landmarks1, landmarks2);
MovingLeastSquares mls;
mls.setAlpha(portAlpha.getValue());
mls.setLandmarks(landmarks2, landmarks1);
const McDim3l& dims = outputImage->lattice().getDims();
McVec3f voxelSizeInOutputImage = outputImage->getVoxelSize();
const McBox3f& bboxOfOutputImage = outputImage->getBoundingBox();
const McBox3f& bboxOfFromImage = fromImage->getBoundingBox();
#ifdef _OPENMP
#pragma omp parallel for
#endif
for (int i = 0; i < dims[0]; i++) {
HxLocation3* locationInFromImage = fromImage->createLocation();
std::cout << "\n" << i << " of " << dims[0];
for (int j = 0; j < dims[1]; j++) {
McVec2d currentPositionInOutput = McVec2d(
bboxOfOutputImage[0] + (float)(i)*voxelSizeInOutputImage.x,
bboxOfOutputImage[2] + (float)(j)*voxelSizeInOutputImage.y);
McVec2d warpedCoordInFromImage =
mls.interpolate(currentPositionInOutput);
McVec3f displacement = McVec3f(
warpedCoordInFromImage.x - currentPositionInOutput.x,
warpedCoordInFromImage.y - currentPositionInOutput.y, 0);
displacement = displacement * -1;
locationInFromImage->move(McVec3f(warpedCoordInFromImage.x,
warpedCoordInFromImage.y,
bboxOfFromImage[4]));
float resultValue[1];
fromImage->eval(*locationInFromImage, resultValue);
unsigned long pos = latticePos(i, j, 0, dims);
outputImageData[pos] = resultValue[0];
outputVectorField->lattice().set(i, j, 0, displacement.getValue());
}
delete locationInFromImage;
}
outputImage->touch();
outputImage->fire();
}
void HxCPDSpatialGraphWarp::warpPoint(const McVec3f& source, McVec3f& target,
MovingLeastSquares& mlsInterpolator) {
McVec2d curPoint = McVec2d(source.x, source.y);
McVec2d warpedPoint = mlsInterpolator.interpolate(curPoint);
target = McVec3f(warpedPoint.x, warpedPoint.y, source.z);
}
int main(int argc, char** argv)
{
readOptions(argc, argv);
PointCloud<PointXYZRGB>::Ptr in(new PointCloud<PointXYZRGB>);
PointCloud<Normal>::Ptr n(new PointCloud<Normal>);
PointCloud<Normal>::Ptr n_mls(new PointCloud<Normal>);
PointCloud<PointXYZRGB>::Ptr p_mls(new PointCloud<PointXYZRGB>);
PointCloud<FPFHSignature33>::Ptr fpfh(new PointCloud<FPFHSignature33>);
PointCloud<PrincipalCurvatures>::Ptr pc(new PointCloud<PrincipalCurvatures>);
//PointCloud<PrincipalRadiiRSD>::Ptr rsd(new PointCloud<PrincipalRadiiRSD>);
for (size_t i = 0; i < scenes_.size(); i++)
{
io::loadPCDFile<PointXYZRGB>(folder_ + "raw_labeled/" + scenes_[i] + ".pcd", *in);
PassThrough<PointXYZRGB> pass;
pass.setInputCloud(in);
pass.setFilterFieldName("z");
pass.setFilterLimits(0.0f, limit_);
pass.filter(*in);
#ifdef PCL_VERSION_COMPARE //fuerte
pcl::search::KdTree<PointXYZRGB>::Ptr tree (new pcl::search::KdTree<PointXYZRGB>());
#else //electric
pcl::KdTreeFLANN<PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<PointXYZRGB> ());
#endif
tree->setInputCloud(in);
for (float r_n = r_min_; r_n <= r_max_; r_n += r_step_)
{
string str_rn = fl2label(r_n,3) + "rn";
NormalEstimationOMP<PointXYZRGB, Normal> norm;
norm.setNumberOfThreads(1);
norm.setInputCloud(in);
norm.setSearchMethod(tree);
norm.setRadiusSearch(r_n);
norm.compute(*n);
#ifdef PCL_MINOR_VERSION
#if PCL_MINOR_VERSION >=6
std::cerr << "current implementation of mls doesn't work with pcl1.7" << std::endl;
exit(-1);
#else
MovingLeastSquares<PointXYZRGB, Normal> mls;
mls.setInputCloud(in);
mls.setOutputNormals(n_mls);
mls.setPolynomialFit(true);
mls.setPolynomialOrder(2);
mls.setSearchMethod(tree);
mls.setSearchRadius(r_n);
mls.reconstruct(*p_mls);
#endif
#endif
for (size_t p = 0; p < n_mls->points.size(); ++p)
{
flipNormalTowardsViewpoint(p_mls->points[p], 0.0f, 0.0f, 0.0f,
n_mls->points[p].normal[0],
n_mls->points[p].normal[1],
n_mls->points[p].normal[2]);
}
io::savePCDFileASCII<PointXYZRGB>(folder_+"normals/"+scenes_[i]+
"/mls_"+scenes_[i]+"_"+str_rn+".pcd",*p_mls);
#ifdef PCL_VERSION_COMPARE //fuerte
pcl::search::KdTree<PointXYZRGB>::Ptr tree_mls (new pcl::search::KdTree<PointXYZRGB>());
#else //electric
pcl::KdTreeFLANN<PointXYZRGB>::Ptr tree_mls (new pcl::KdTreeFLANN<PointXYZRGB> ());
#endif
tree_mls->setInputCloud(p_mls);
for (float r_f = r_n; r_f <= r_max_; r_f += r_step_)
{
cout << "scene: " << scenes_[i] << "\tnormals: " << r_n << "\tfeatures: " << r_f << endl;
string str_rf = fl2label(r_f,3) + "rf";
FPFHEstimationOMP<PointXYZRGB, Normal, FPFHSignature33> fpfh_est;
fpfh_est.setNumberOfThreads(1);
fpfh_est.setInputCloud(in);
fpfh_est.setInputNormals(n);
fpfh_est.setSearchMethod(tree);
fpfh_est.setRadiusSearch(r_f);
fpfh_est.compute(*fpfh);
// filename example:
// <folder_>/fpfh/kitchen01/fpfh_kitchen01_020rn_025rf.pcd
// <folder_>/fpfh/kitchen02/fpfh_kitchen02_030rnmls_060rf.pcd
io::savePCDFileASCII<FPFHSignature33>(folder_ + "0_fpfh/" + scenes_[i] +
"/fpfh_" + scenes_[i] +"_"+str_rn+"_"+str_rf+".pcd",
*fpfh);
FPFHEstimationOMP<PointXYZRGB, Normal, FPFHSignature33> fpfh_est_mls;
fpfh_est_mls.setNumberOfThreads(1);
fpfh_est_mls.setInputCloud(p_mls);
fpfh_est_mls.setInputNormals(n_mls);
fpfh_est_mls.setSearchMethod(tree_mls);
fpfh_est_mls.setRadiusSearch(r_f);
fpfh_est_mls.compute(*fpfh);
io::savePCDFileASCII<FPFHSignature33>(folder_ + "0_fpfh/" + scenes_[i] +
"/fpfh_" + scenes_[i] +"_"+str_rn+"mls_"+str_rf+".pcd",
*fpfh);
PrincipalCurvaturesEstimation<PointXYZRGB, Normal, PrincipalCurvatures> pc_est;
pc_est.setInputCloud(in);
pc_est.setInputNormals(n);
pc_est.setSearchMethod(tree);
pc_est.setRadiusSearch(r_f);
pc_est.compute(*pc);
io::savePCDFileASCII<PrincipalCurvatures>(folder_ + "0_pc/" + scenes_[i] +
"/pc_" + scenes_[i] +"_"+str_rn+"_"+str_rf+".pcd",
*pc);
PrincipalCurvaturesEstimation<PointXYZRGB, Normal, PrincipalCurvatures> pc_est_mls;
pc_est_mls.setInputCloud(p_mls);
pc_est_mls.setInputNormals(n_mls);
pc_est_mls.setSearchMethod(tree_mls);
pc_est_mls.setRadiusSearch(r_f);
pc_est_mls.compute(*pc);
io::savePCDFileASCII<PrincipalCurvatures>(folder_ + "0_pc/" + scenes_[i] +
"/pc_" + scenes_[i] +"_"+str_rn+"mls_"+str_rf+".pcd",
*pc);
/*RSDEstimation<PointXYZRGB, Normal, PrincipalRadiiRSD> rsd_est;
rsd_est.setInputCloud(in);
rsd_est.setInputNormals(n);
rsd_est.setSearchMethod(tree);
rsd_est.setPlaneRadius(0.5);
rsd_est.setRadiusSearch(r_f);
rsd_est.compute(*rsd);
io::savePCDFileASCII<PrincipalRadiiRSD>(folder_ + "0_rsd/" + scenes_[i] +
"/rsd_" + scenes_[i] +"_"+str_rn+"_"+str_rf+".pcd",
*rsd);
RSDEstimation<PointXYZRGB, Normal, PrincipalRadiiRSD> rsd_est_mls;
rsd_est_mls.setInputCloud(p_mls);
rsd_est_mls.setInputNormals(n_mls);
rsd_est_mls.setSearchMethod(tree_mls);
rsd_est_mls.setPlaneRadius(0.5);
rsd_est_mls.setRadiusSearch(r_f);
rsd_est_mls.compute(*rsd);
io::savePCDFileASCII<PrincipalRadiiRSD>(folder_ + "0_rsd/" + scenes_[i] +
"/rsd_" + scenes_[i] +"_"+str_rn+"mls_"+str_rf+".pcd",
*rsd);*/
}
}
}
return 0;
}
void processRSD(const PointCloud<PointXYZRGB>::Ptr in,
PointCloud<PointXYZRGB>::Ptr ref_out,
PointCloud<PointXYZRGB>::Ptr rsd_out)
{
PointCloud<Normal>::Ptr n(new PointCloud<Normal>());
PointCloud<PrincipalRadiiRSD>::Ptr rsd(new PointCloud<PrincipalRadiiRSD>());
// passthrough filtering (needed to remove NaNs)
cout << "RSD: Pass (with " << in->points.size() << " points)" << endl;
PassThrough<PointXYZRGB> pass;
pass.setInputCloud(in);
pass.setFilterFieldName("z");
pass.setFilterLimits(0.0f, pass_depth_);
pass.filter(*ref_out);
// optional voxelgrid filtering
if (rsd_vox_enable_)
{
cout << "RSD: Voxel (with " << ref_out->points.size() << " points)" << endl;
VoxelGrid<PointXYZRGB> vox;
vox.setInputCloud(ref_out);
vox.setLeafSize(rsd_vox_, rsd_vox_, rsd_vox_);
vox.filter(*ref_out);
}
#ifdef PCL_VERSION_COMPARE //fuerte
pcl::search::KdTree<PointXYZRGB>::Ptr tree (new pcl::search::KdTree<PointXYZRGB>());
#else //electric
KdTreeFLANN<PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<PointXYZRGB> ());
#endif
tree->setInputCloud(ref_out);
// optional surface smoothing
if(rsd_mls_enable_)
{
cout << "RSD: MLS (with " << ref_out->points.size() << " points)" << endl;
#ifdef PCL_VERSION_COMPARE
std::cerr << "MLS has changed completely in PCL 1.7! Requires redesign of entire program" << std::endl;
exit(0);
#else
MovingLeastSquares<PointXYZRGB, Normal> mls;
mls.setInputCloud(ref_out);
mls.setOutputNormals(n);
mls.setPolynomialFit(true);
mls.setPolynomialOrder(2);
mls.setSearchMethod(tree);
mls.setSearchRadius(rsd_rn_);
mls.reconstruct(*ref_out);
#endif
cout << "RSD: flip normals (with " << ref_out->points.size() << " points)" << endl;
for (size_t i = 0; i < ref_out->points.size(); ++i)
{
flipNormalTowardsViewpoint(ref_out->points[i], 0.0f, 0.0f, 0.0f,
n->points[i].normal[0],
n->points[i].normal[1],
n->points[i].normal[2]);
}
}
else
{
cout << "RSD: Normals (with " << ref_out->points.size() << " points)" << endl;
NormalEstimation<PointXYZRGB, Normal> norm;
norm.setInputCloud(ref_out);
norm.setSearchMethod(tree);
norm.setRadiusSearch(rsd_rn_);
norm.compute(*n);
}
tree->setInputCloud(ref_out);
// RSD estimation
cout << "RSD: estimation (with " << ref_out->points.size() << " points)" << endl;
RSDEstimation<PointXYZRGB, Normal, PrincipalRadiiRSD> rsdE;
rsdE.setInputCloud(ref_out);
rsdE.setInputNormals(n);
rsdE.setSearchMethod(tree);
rsdE.setPlaneRadius(r_limit_);
rsdE.setRadiusSearch(rsd_rf_);
rsdE.compute(*rsd);
cout << "RSD: classification " << endl;
*rsd_out = *ref_out;
// apply RSD rules for classification
int exp_rgb, pre_rgb;
float r_max,r_min;
cob_3d_mapping_common::LabelResults stats(fl2label(rsd_rn_),fl2label(rsd_rf_),rsd_mls_enable_);
for (size_t idx = 0; idx < ref_out->points.size(); idx++)
{
exp_rgb = *reinterpret_cast<int*>(&ref_out->points[idx].rgb); // expected label
r_max = rsd->points[idx].r_max;
r_min = rsd->points[idx].r_min;
if ( r_min > r_high )
{
pre_rgb = LBL_PLANE;
if (exp_rgb != LBL_PLANE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_PLANE]++;
}
else if (r_min < r_low)
{
if (r_max < r_r2 * r_min)
{
pre_rgb = LBL_COR;
if (exp_rgb != LBL_COR && exp_rgb != LBL_UNDEF) stats.fp[EVAL_COR]++;
}
else
{
pre_rgb = LBL_EDGE;
if (exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGE]++;
}
// special case: combined class for corner and edge
if (exp_rgb != LBL_COR && exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF)
stats.fp[EVAL_EDGECORNER]++;
}
else
{
if (r_max < r_r1 * r_min)
{
pre_rgb = LBL_SPH;
if (exp_rgb != LBL_SPH && exp_rgb != LBL_UNDEF) stats.fp[EVAL_SPH]++;
}
else
{
pre_rgb = LBL_CYL;
if (exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CYL]++;
}
// special case: combined class for sphere and cylinder
if (exp_rgb != LBL_SPH && exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF)
stats.fp[EVAL_CURVED]++;
}
switch(exp_rgb)
{
case LBL_PLANE:
if (pre_rgb != exp_rgb) stats.fn[EVAL_PLANE]++;
stats.exp[EVAL_PLANE]++;
break;
case LBL_EDGE:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_EDGE]++;
if (pre_rgb != LBL_COR) stats.fn[EVAL_EDGECORNER]++;
}
stats.exp[EVAL_EDGE]++;
stats.exp[EVAL_EDGECORNER]++;
break;
case LBL_COR:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_COR]++;
if (pre_rgb != LBL_EDGE) stats.fn[EVAL_EDGECORNER]++;
}
stats.exp[EVAL_COR]++;
stats.exp[EVAL_EDGECORNER]++;
break;
case LBL_SPH:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_SPH]++;
if (pre_rgb != LBL_CYL) stats.fn[EVAL_CURVED]++;
}
stats.exp[EVAL_SPH]++;
stats.exp[EVAL_CURVED]++;
break;
case LBL_CYL:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_CYL]++;
if (pre_rgb != LBL_SPH) stats.fn[EVAL_CURVED]++;
}
stats.exp[EVAL_CYL]++;
stats.exp[EVAL_CURVED]++;
break;
default:
stats.undef++;
break;
}
rsd_out->points[idx].rgb = *reinterpret_cast<float*>(&pre_rgb);
}
cout << "RSD:\n" << stats << endl << endl;
}
/*! @brief runs the whole processing pipeline for FPFH features
*
* @note At the moment the evaluation results will be printed to console.
*
* @param[in] in the labeled input point cloud
* @param[out] ref_out the reference point cloud after the preprocessing steps
* @param[out] fpfh_out the labeled point cloud after the classifing process
*/
void processFPFH(const PointCloud<PointXYZRGB>::Ptr in,
PointCloud<PointXYZRGB>::Ptr ref_out,
PointCloud<PointXYZRGB>::Ptr fpfh_out)
{
PointCloud<Normal>::Ptr n(new PointCloud<Normal>());
PointCloud<FPFHSignature33>::Ptr fpfh(new PointCloud<FPFHSignature33>());
// Passthrough filtering (needs to be done to remove NaNs)
cout << "FPFH: Pass (with " << in->points.size() << " points)" << endl;
PassThrough<PointXYZRGB> pass;
pass.setInputCloud(in);
pass.setFilterFieldName("z");
pass.setFilterLimits(0.0f, pass_depth_);
pass.filter(*ref_out);
// Optional voxelgrid filtering
if (fpfh_vox_enable_)
{
cout << "FPFH: Voxel (with " << ref_out->points.size() << " points)" << endl;
VoxelGrid<PointXYZRGB> vox;
vox.setInputCloud(ref_out);
vox.setLeafSize(fpfh_vox_, fpfh_vox_, fpfh_vox_);
vox.filter(*ref_out);
}
#ifdef PCL_VERSION_COMPARE //fuerte
pcl::search::KdTree<PointXYZRGB>::Ptr tree (new pcl::search::KdTree<PointXYZRGB>());
#else //electric
pcl::KdTreeFLANN<PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<PointXYZRGB> ());
#endif
//KdTree<PointXYZRGB>::Ptr tree(new KdTreeFLANN<PointXYZRGB>());
tree->setInputCloud(ref_out);
// Optional surface smoothing
if(fpfh_mls_enable_)
{
cout << "FPFH: MLS (with " << ref_out->points.size() << " points)" << endl;
#ifdef PCL_VERSION_COMPARE
std::cerr << "MLS has changed completely in PCL 1.7! Requires redesign of entire program" << std::endl;
exit(0);
#else
MovingLeastSquares<PointXYZRGB, Normal> mls;
mls.setInputCloud(ref_out);
mls.setOutputNormals(n);
mls.setPolynomialFit(true);
mls.setPolynomialOrder(2);
mls.setSearchMethod(tree);
mls.setSearchRadius(fpfh_rn_);
mls.reconstruct(*ref_out);
#endif
cout << "FPFH: flip normals (with " << ref_out->points.size() << " points)" << endl;
for (size_t i = 0; i < ref_out->points.size(); ++i)
{
flipNormalTowardsViewpoint(ref_out->points[i], 0.0f, 0.0f, 0.0f,
n->points[i].normal[0],
n->points[i].normal[1],
n->points[i].normal[2]);
}
}
else
{
cout << "FPFH: Normals (with " << ref_out->points.size() << " points)" << endl;
NormalEstimation<PointXYZRGB, Normal> norm;
norm.setInputCloud(ref_out);
norm.setSearchMethod(tree);
norm.setRadiusSearch(fpfh_rn_);
norm.compute(*n);
}
// FPFH estimation
#ifdef PCL_VERSION_COMPARE //fuerte
tree.reset(new pcl::search::KdTree<PointXYZRGB>());
#else //electric
tree.reset(new KdTreeFLANN<PointXYZRGB> ());
#endif
tree->setInputCloud(ref_out);
cout << "FPFH: estimation (with " << ref_out->points.size() << " points)" << endl;
FPFHEstimation<PointXYZRGB, Normal, FPFHSignature33> fpfhE;
fpfhE.setInputCloud(ref_out);
fpfhE.setInputNormals(n);
fpfhE.setSearchMethod(tree);
fpfhE.setRadiusSearch(fpfh_rf_);
fpfhE.compute(*fpfh);
cout << "FPFH: classification " << endl;
*fpfh_out = *ref_out;
CvSVM svm;
svm.load(fpfh_svm_model_.c_str());
cv::Mat fpfh_histo(1, 33, CV_32FC1);
int exp_rgb, pre_rgb, predict;
cob_3d_mapping_common::LabelResults stats(fl2label(fpfh_rn_),fl2label(fpfh_rf_),fpfh_mls_enable_);
for (size_t idx = 0; idx < ref_out->points.size(); idx++)
{
exp_rgb = *reinterpret_cast<int*>(&ref_out->points[idx].rgb); // expected label
memcpy(fpfh_histo.ptr<float>(0), fpfh->points[idx].histogram, sizeof(fpfh->points[idx].histogram));
predict = (int)svm.predict(fpfh_histo);
//cout << predict << endl;
switch(predict)
{
case SVM_PLANE:
pre_rgb = LBL_PLANE;
if (exp_rgb != LBL_PLANE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_PLANE]++;
break;
case SVM_EDGE:
pre_rgb = LBL_EDGE;
if (exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGE]++;
if (exp_rgb != LBL_COR && exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGECORNER]++;
break;
case SVM_COR:
pre_rgb = LBL_COR;
if (exp_rgb != LBL_COR && exp_rgb != LBL_UNDEF) stats.fp[EVAL_COR]++;
if (exp_rgb != LBL_COR && exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGECORNER]++;
break;
case SVM_SPH:
pre_rgb = LBL_SPH;
if (exp_rgb != LBL_SPH && exp_rgb != LBL_UNDEF) stats.fp[EVAL_SPH]++;
if (exp_rgb != LBL_SPH && exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CURVED]++;
break;
case SVM_CYL:
pre_rgb = LBL_CYL;
if (exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CYL]++;
if (exp_rgb != LBL_SPH && exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CURVED]++;
break;
default:
pre_rgb = LBL_UNDEF;
break;
}
switch(exp_rgb)
{
case LBL_PLANE:
if (pre_rgb != exp_rgb) stats.fn[EVAL_PLANE]++;
stats.exp[EVAL_PLANE]++;
break;
case LBL_EDGE:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_EDGE]++;
if (pre_rgb != LBL_COR) stats.fn[EVAL_EDGECORNER]++;
}
stats.exp[EVAL_EDGE]++;
stats.exp[EVAL_EDGECORNER]++;
break;
case LBL_COR:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_COR]++;
if (pre_rgb != LBL_EDGE) stats.fn[EVAL_EDGECORNER]++;
}
stats.exp[EVAL_COR]++;
stats.exp[EVAL_EDGECORNER]++;
break;
case LBL_SPH:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_SPH]++;
if (pre_rgb != LBL_CYL) stats.fn[EVAL_CURVED]++;
}
stats.exp[EVAL_SPH]++;
stats.exp[EVAL_CURVED]++;
break;
case LBL_CYL:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_CYL]++;
if (pre_rgb != LBL_SPH) stats.fn[EVAL_CURVED]++;
}
stats.exp[EVAL_CYL]++;
stats.exp[EVAL_CURVED]++;
break;
default:
stats.undef++;
break;
}
fpfh_out->points[idx].rgb = *reinterpret_cast<float*>(&pre_rgb);
}
cout << "FPFH:\n" << stats << endl << endl;
}
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
double search_radius, bool sqr_gauss_param_set, double sqr_gauss_param,
bool use_polynomial_fit, int polynomial_order)
{
PointCloud<PointXYZ>::Ptr xyz_cloud_pre (new pcl::PointCloud<PointXYZ> ()),
xyz_cloud (new pcl::PointCloud<PointXYZ> ());
fromROSMsg (*input, *xyz_cloud_pre);
// Filter the NaNs from the cloud
for (size_t i = 0; i < xyz_cloud_pre->size (); ++i)
if (pcl_isfinite (xyz_cloud_pre->points[i].x))
xyz_cloud->push_back (xyz_cloud_pre->points[i]);
xyz_cloud->header = xyz_cloud_pre->header;
xyz_cloud->height = 1;
xyz_cloud->width = xyz_cloud->size ();
xyz_cloud->is_dense = false;
// io::savePCDFile ("test.pcd", *xyz_cloud);
PointCloud<PointXYZ>::Ptr xyz_cloud_smoothed (new PointCloud<PointXYZ> ());
MovingLeastSquares<PointXYZ, Normal> mls;
mls.setInputCloud (xyz_cloud);
mls.setSearchRadius (search_radius);
if (sqr_gauss_param_set) mls.setSqrGaussParam (sqr_gauss_param);
mls.setPolynomialFit (use_polynomial_fit);
mls.setPolynomialOrder (polynomial_order);
// mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, Normal>::SAMPLE_LOCAL_PLANE);
mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, Normal>::UNIFORM_DENSITY);
// mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, Normal>::FILL_HOLES);
// mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, Normal>::NONE);
mls.setFillingStepSize (0.02);
mls.setPointDensity (50000*search_radius); // 300 points in a 5 cm radius
mls.setUpsamplingRadius (0.025);
mls.setUpsamplingStepSize (0.015);
search::KdTree<PointXYZ>::Ptr tree (new search::KdTree<PointXYZ> ());
mls.setSearchMethod (tree);
PointCloud<Normal>::Ptr mls_normals (new PointCloud<Normal> ());
mls.setOutputNormals (mls_normals);
PCL_INFO ("Computing smoothed surface and normals with search_radius %f , sqr_gaussian_param %f, polynomial fitting %d, polynomial order %d\n",
mls.getSearchRadius(), mls.getSqrGaussParam(), mls.getPolynomialFit(), mls.getPolynomialOrder());
TicToc tt;
tt.tic ();
mls.reconstruct (*xyz_cloud_smoothed);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", xyz_cloud_smoothed->width * xyz_cloud_smoothed->height); print_info (" points]\n");
sensor_msgs::PointCloud2 output_positions, output_normals;
// printf ("sizes: %d %d %d\n", xyz_cloud_smoothed->width, xyz_cloud_smoothed->height, xyz_cloud_smoothed->size ());
toROSMsg (*xyz_cloud_smoothed, output_positions);
toROSMsg (*mls_normals, output_normals);
concatenateFields (output_positions, output_normals, output);
PointCloud<PointXYZ> xyz_vg;
VoxelGrid<PointXYZ> vg;
vg.setInputCloud (xyz_cloud_smoothed);
vg.setLeafSize (0.005, 0.005, 0.005);
vg.filter (xyz_vg);
sensor_msgs::PointCloud2 xyz_vg_2;
toROSMsg (xyz_vg, xyz_vg_2);
pcl::io::savePCDFile ("cloud_vg.pcd", xyz_vg_2, Eigen::Vector4f::Zero (),
Eigen::Quaternionf::Identity (), true);
}
