I am working on a region growing algorithm in python for unstructured point clouds. I try to implement the algorithm according to http://pointclouds.org/documentation/tutorials/region_growing_segmentation.php#region-growing-segmentation and SEGMENTATION OF POINT CLOUDS USING SMOOTHNESS CONSTRAINT.
Using the same point cloud, I fail to reproduce the results with the pcl implementation, though all the steps and the parameters seem to be the same. My implementation yields 217 regions, whereas pcl's one gets 24 (oversegemented). The point cloud can be found:
https://github.com/scrox/region-growing/blob/master/test.txt
What causes this significant difference? I also welcome performance improvements on my python code.
import math
import numpy as np
from sklearn.neighbors import KDTree
unique_rows=np.loadtxt("test.txt")
tree = KDTree(unique_rows, leaf_size=2)
dist,nn_glob = tree.query(unique_rows[:len(unique_rows)], k=30)
def normalsestimation(pointcloud,nn_glob,VP=[0,0,0]):
ViewPoint = np.array(VP)
normals = np.empty((np.shape(pointcloud)))
curv = np.empty((len(pointcloud),1))
for index in range(len(pointcloud)):
nn_loc = pointcloud[nn_glob[index]]
COV = np.cov(nn_loc,rowvar=False)
eigval, eigvec = np.linalg.eig(COV)
idx = np.argsort(eigval)
nor = eigvec[:,idx][:,0]
if nor.dot((ViewPoint-pointcloud[index,:])) > 0:
normals[index] = nor
else:
normals[index] = - nor
curv[index] = eigval[idx][0] / np.sum(eigval)
return normals,curv
def regiongrowing(pointcloud,nn_glob,theta_th = 'auto', cur_th = 'auto'):
normals,curvature = normalsestimation(pointcloud,nn_glob=nn_glob)
order = curvature[:,0].argsort().tolist()
region = []
if theta_th == 'auto':
theta_th = 15.0 / 180.0 * math.pi # in radians
if cur_th == 'auto':
cur_th = np.percentile(curvature,98)
while len(order) > 0:
region_cur = []
seed_cur = []
poi_min = order[0] #poi_order[0]
region_cur.append(poi_min)
seed_cur.append(poi_min)
order.remove(poi_min)
for i in range(len(seed_cur)):
nn_loc = nn_glob[seed_cur[i]]
for j in range(len(nn_loc)):
nn_cur = nn_loc[j]
if all([nn_cur in order , np.arccos(np.abs(np.dot(normals[seed_cur[i]],normals[nn_cur])))<theta_th]):
region_cur.append(nn_cur)
order.remove(nn_cur)
if curvature[nn_cur] < cur_th:
seed_cur.append(nn_cur)
region.append(region_cur)
return region
region = regiongrowing(unique_rows,nn_glob)
Here is the pcl code:
#include <iostream>
#include <vector>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/segmentation/region_growing.h>
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
if ( pcl::io::loadPCDFile <pcl::PointXYZ> ("test.pcd", *cloud) == -1)
{
std::cout << "Cloud reading failed." << std::endl;
return (-1);
}
pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> > (new pcl::search::KdTree<pcl::PointXYZ>);
pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
normal_estimator.setSearchMethod (tree);
normal_estimator.setInputCloud (cloud);
normal_estimator.setKSearch (30);
normal_estimator.compute (*normals);
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
reg.setSearchMethod (tree);
reg.setNumberOfNeighbours (30);
reg.setInputCloud (cloud);
reg.setInputNormals (normals);
reg.setSmoothnessThreshold (15.0 / 180.0 * M_PI);
reg.setCurvatureThreshold (0.088614);
std::vector <pcl::PointIndices> clusters;
reg.extract (clusters);
std::cout << "Number of clusters is equal to " << clusters.size () << std::endl;
std::cout << "First cluster has " << clusters[0].indices.size () << " points." << endl;
std::cout << "These are the indices of the points of the initial" <<
std::endl << "cloud that belong to the first cluster:" << std::endl;
int counter = 0;
while (counter < clusters[0].indices.size ())
{
std::cout << clusters[0].indices[counter] << ", ";
counter++;
}
std::cout << std::endl;
pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud ();
pcl::visualization::CloudViewer viewer ("Cluster viewer");
viewer.showCloud(colored_cloud);
while (!viewer.wasStopped ())
{
}
return (0);
}
