int
main()
{
Complex c_max;
Complex c_min;
Complex c_factor;
/* ------------------------------------- */
initialise( &c_max, &c_min, &c_factor);
compute_plane( &c_max, &c_min, &c_factor);
return (0);
}
void callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
ROS_INFO("Got a pointcloud, calibrating...");
pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::fromROSMsg(*msg, cloud);
int nbr = cloud.size();
int max = 1000; // ransac iterations
double threshold = 0.02; // threshold for plane inliers
Eigen::Vector4f best_plane; // best plane parameters found
int best_inliers = -1; // best number of inliers
int inds[3];
Eigen::Vector4f plane;
int inliers;
for (int i = 0; i < max; ++i) {
for (int j = 0; j < 3; ++j) {
inds[j] = rand() % nbr; // get a random point
}
// check that the points aren't the same
if (inds[0] == inds[1] || inds[0] == inds[2] || inds[1] == inds[2]) {
continue;
}
compute_plane(plane, cloud, inds);
inliers = 0;
for (int j = 0; j < nbr; j += 30) { // count number of inliers
if (is_inlier(cloud[j].getVector3fMap(), plane, threshold)) {
++inliers;
}
}
if (inliers > best_inliers) {
best_plane = plane;
best_inliers = inliers;
}
}
extract_height_and_angle(best_plane); // find parameters and feed them to datacentre
plot_best_plane(cloud, best_plane, threshold); // visually evaluate plane fit
exit(0);
}
bool compute_object(const int i) {
// Call get_plane, get_cylinder, get_sphere
float plane_score = compute_plane(); // plane_inliers_ has the pc.
float cylinder_score = compute_cylinder(); // cylinder_inliers_ has the pc
// Select the lowest score and store it in object_inliers_.
inliers_object_ = (plane_score > cylinder_score ? inliers_plane_ : inliers_cylinder_);
// inliers_object_ = inliers_plane_;
// inliers_object_ = inliers_cylinder_;
// Test if the point cloud is too small
if (inliers_object_->indices.size() < CUT_THRESHOLD) {
std::cerr << "object inliers has "<< inliers_object_->indices.size()
<< " < " << CUT_THRESHOLD <<" points. Aborting..." << std::endl;
return false;
}
/*
// Debugging info
pcl::PointCloud<Point>::Ptr best_object_(new pcl::PointCloud<Point>);
pcl::copyPointCloud(*cloud_filtered, *inliers_object_, *best_object_);
std::stringstream debug_s0;
debug_s0 << "no_cluster_object_" << i << ".pcd";
pcl::io::savePCDFile(debug_s0.str(), *best_object_);
*/
// Euclidean Clustering
std::vector<pcl::PointIndices> clusters;
cluster_.setInputCloud(cloud_filtered);
cluster_.setIndices(inliers_object_);
cluster_.extract(clusters);
std::cout << "Number of Euclidian clusters found: " << clusters.size() << std::endl;
// Check if there is no clusters
if (clusters.size() == 0) {
std::cerr << "No clusters found. Aborting..." << std::endl;
return false;
}
main_cluster_.reset(new pcl::PointIndices(clusters.at(0)));
// Extract the inliers from the input cloud
extract.setInputCloud(cloud_filtered);
extract.setIndices(main_cluster_);
extract.setNegative(false);
pcl::PointCloud<Point>::Ptr cloud_plane(new pcl::PointCloud<Point > ());
extract.filter(*cloud_plane);
// Test if cluster[0] is too small
if (main_cluster_->indices.size() < CUT_THRESHOLD) {
std::cerr << "object cluster[0] has "<< main_cluster_->indices.size()
<< " < " << CUT_THRESHOLD <<" points. Not writing to disk..." << std::endl;
} else {
// Write the inliers to disk
std::stringstream debug_s;
if (inliers_object_ == inliers_cylinder_) {
debug_s << "cylinder_object_" << i << ".pcd";
} else if (inliers_object_ == inliers_plane_) {
debug_s << "plane_object_" << i << ".pcd";
} else {
debug_s << "unknown_object_" << i << ".pcd";
}
pcl::io::savePCDFile(debug_s.str(), *cloud_plane);
std::cout << "PointCloud representing the extracted component: " <<
cloud_plane->points.size() << " data points." << std::endl;
}
// Remove cluster[0], update cloud_filtered and cloud_normals
extract.setNegative(true);
extract.filter(*cloud_filtered);
extract_normals.setNegative(true);
extract_normals.setInputCloud(cloud_normals);
extract_normals.setIndices(main_cluster_);
extract_normals.filter(*cloud_normals);
return true;
}
