void callback(const sensor_msgs::PointCloud2::ConstPtr& pc)
{
PointCloudT::Ptr cloud = boost::make_shared<PointCloudT>();
pcl::fromROSMsg(*pc, *cloud);
size_t height = 8; // 8 layers
size_t width = round((start_angle_ - end_angle_) / angular_resolution_) + 1;
PointT invalid;
invalid.x = invalid.y = invalid.z = std::numeric_limits<float>::quiet_NaN();
PointCloudT::Ptr cloud_out = boost::make_shared<PointCloudT>(width, height, invalid);
cloud_out->is_dense = false;
for (size_t i = 0; i < cloud->size(); i++)
{
const PointT &p = cloud->points[i];
int col = round((atan2(p.y, p.x) - end_angle_) / angular_resolution_);
int row = p.layer;
if (col < 0 || col >= width || row < 0 || row >= height)
{
ROS_ERROR("Invalid coordinates: (%d, %d) is outside (0, 0)..(%zu, %zu)!", col, row, width, height);
continue;
}
(*cloud_out)[row * width + col] = p;
}
sensor_msgs::PointCloud2::Ptr msg = boost::make_shared<sensor_msgs::PointCloud2>();
pcl::toROSMsg(*cloud_out, *msg);
msg->header = pc->header;
pub.publish(msg);
}
/* collects a cloud by aggregating k successive frames */
void waitForCloudK(int k){
ros::Rate r(30);
cloud_aggregated->clear();
int counter = 0;
while (ros::ok()){
ros::spinOnce();
r.sleep();
if (new_cloud_available_flag){
*cloud_aggregated+=*cloud;
new_cloud_available_flag = false;
counter ++;
if (counter >= k){
cloud_aggregated->header = cloud->header;
break;
}
}
}
}
void move_to_frame(const PointCloudT::Ptr &input, const string &target_frame, PointCloudT::Ptr &output) {
ROS_INFO("Transforming Input Point Cloud to %s frame...", target_frame.c_str());
ROS_INFO(" Input Cloud Size: %zu", input->size());
if (input->header.frame_id == target_frame) {
output = input;
return;
}
while (ros::ok()) {
tf::StampedTransform stamped_transform;
try {
// Look up transform
tf_listener->lookupTransform(target_frame, input->header.frame_id, ros::Time(0), stamped_transform);
// Apply transform
pcl_ros::transformPointCloud(*input, *output, stamped_transform);
// Store Header Details
output->header.frame_id = target_frame;
pcl_conversions::toPCL(ros::Time::now(), output->header.stamp);
break;
}
//keep trying until we get the transform
catch (tf::TransformException &ex) {
ROS_ERROR_THROTTLE(1, "%s", ex.what());
ROS_WARN_THROTTLE(1, " Waiting for transform from cloud frame (%s) to %s frame. Trying again",
input->header.frame_id.c_str(), target_frame.c_str());
continue;
}
}
}
void trk3dFeatures::trkSeq(const PointCloudT::Ptr &cloud2,
const PointCloudT::Ptr &keyPts2,
const float params[],
PointCloudT::Ptr &keyPts,
pcl::PointCloud<SHOT1344>::Ptr &Shot1344Cloud_1,
std::vector<u16> &matchIdx1, std::vector<u16> &matchIdx2,
std::vector<f32> &matchDist)
{
// construct descriptors
pcl::PointCloud<SHOT1344>::Ptr Shot1344Cloud_2(new pcl::PointCloud<SHOT1344>);
extractFeatures featureDetector;
Shot1344Cloud_2 = featureDetector.Shot1344Descriptor(cloud2, keyPts2, params);
// // match keypoints
// featureDetector.crossMatching(Shot1344Cloud_1, Shot1344Cloud_2,
// &matchIdx1, &matchIdx2, &matchDist);
// find the matching from desc_1 -> desc_2
std::cout<<"size of Shot1344Cloud_1 in trkSeq: "<<Shot1344Cloud_1->points.size()<<std::endl;
std::cout<<"size of Shot1344Cloud_2 in trkSeq: "<<Shot1344Cloud_2->points.size()<<std::endl;
featureDetector.matchKeyPts(Shot1344Cloud_1, Shot1344Cloud_2, &matchIdx2, &matchDist);
std::cout<<"size of matches in trkSeq: "<<matchIdx2.size()<<std::endl;
std::cout<<"size of matchDist in trkSeq: "<<matchDist.size()<<std::endl;
Shot1344Cloud_1.reset(new pcl::PointCloud<SHOT1344>);
keyPts->points.clear();
for(size_t i=0; i<matchIdx2.size();i++)
{
keyPts->push_back(keyPts2->points.at(matchIdx2[i]));
Shot1344Cloud_1->push_back(Shot1344Cloud_2->points.at(matchIdx2[i]));
}
}
double calculate_density(const PointCloudT::Ptr &cloud, const bwi_perception::BoundingBox &box) {
// TODO: Calculate true volume
// If the cloud is one point thick in some dimension, we'll assign that dimension a magnitude of 1cm
double x_dim = max(abs(box.max[0] - box.min[0]), 0.01f);
double y_dim = max(abs(box.max[1] - box.min[1]), 0.01f);
double z_dim = max(abs(box.max[2] - box.min[2]), 0.01f);
double volume = x_dim * y_dim * z_dim;
return (double) cloud->size() / volume;
}
void removeLowConfidencePoints(cv::Mat& confidence_image, int threshold, PointCloudT::Ptr& cloud)
{
for (int i=0;i<cloud->height;i++)
{
for (int j=0;j<cloud->width;j++)
{
if (confidence_image.at<unsigned char>(i,j) < threshold)
{
cloud->at(j,i).x = std::numeric_limits<float>::quiet_NaN();
cloud->at(j,i).y = std::numeric_limits<float>::quiet_NaN();
cloud->at(j,i).z = std::numeric_limits<float>::quiet_NaN();
confidence_image.at<unsigned char>(i,j) = 0; // just for visualization
}
else
confidence_image.at<unsigned char>(i,j) = 255; // just for visualization
}
}
cloud->is_dense = false;
}
void computeNeonVoxels(PointCloudT::Ptr in, PointCloudT::Ptr green, PointCloudT::Ptr orange) {
green->clear();
orange->clear();
//Point Cloud to store out neon cap
//PointCloudT::Ptr temp_neon_cloud (new PointCloudT);
for (int i = 0; i < in->points.size(); i++) {
unsigned int r, g, b;
r = in->points[i].r;
g = in->points[i].g;
b = in->points[i].b;
// Look for mostly neon value points
if (g > 175 && (r + b) < 150) {
green->push_back(in->points[i]);
}
else if(r > 200 && (g + b) < 150){
orange->push_back(in->points[i]);
}
}
}
void PlayWindow::ShowPC(PointCloudT::Ptr PC)
{
updateModelMutex.lock();
if (ui->checkBox_ShowPC->isChecked())
{
if (!viewer->updatePointCloud(PC))
viewer->addPointCloud(PC);
Eigen::Matrix4f currentPose = Eigen::Matrix4f::Identity();
currentPose.block(0,0,3,3) = PC->sensor_orientation_.matrix();
currentPose.block(0,3,3,1) = PC->sensor_origin_.head(3);
viewer->updatePointCloudPose("cloud",Eigen::Affine3f(currentPose) );
}
ui->qvtkWidget->update ();
updateModelMutex.unlock();
// Timing
times.push_back(QDateTime::currentDateTime().toMSecsSinceEpoch() - PC->header.stamp);
double mean;
if (times.size() > 60)
{
mean = std::accumulate(times.begin(), times.end(), 0.0) / times.size();
times.erase(times.begin());
}
if (ui->checkBox_SavePC->isChecked())
pcl::io::savePCDFileASCII(PC->header.frame_id, *PC);
std::vector<int> ind;
pcl::removeNaNFromPointCloud(*PC, *PC, ind);
ui->label_nPoint->setText(QString("Nb Point : %1").arg(PC->size()));
ui->label_time->setText(QString("Time (ms) : %1").arg(mean));
ui->label_fps->setText(QString("FPS (Hz) : %1").arg(1000/mean));
return;
}
int main(int argc, char** argv)
{
if (argc != 3)
{
printUsage(argv);
return -1;
}
std::string filename_in = argv[1];
std::string filename_out = argv[2];
// read in
printf("Reading cloud\n");
PointCloudT::Ptr cloud;
cloud.reset(new rgbdtools::PointCloudT());
pcl::PCDReader reader;
reader.read(filename_in, *cloud);
alignGlobalCloud(cloud);
return 0;
}
void PC_to_Mat(PointCloudT::Ptr &cloud, cv::Mat &result){
if (cloud->isOrganized()) {
std::cout << "PointCloud is organized..." << std::endl;
result = cv::Mat(cloud->height, cloud->width, CV_8UC3);
if (!cloud->empty()) {
for (int h=0; h<result.rows; h++) {
for (int w=0; w<result.cols; w++) {
PointT point = cloud->at(w, h);
Eigen::Vector3i rgb = point.getRGBVector3i();
result.at<cv::Vec3b>(h,w)[0] = rgb[2];
result.at<cv::Vec3b>(h,w)[1] = rgb[1];
result.at<cv::Vec3b>(h,w)[2] = rgb[0];
}
}
}
}
}
void filter_PC_from_BB(PointCloudT::Ptr &cloud, cv::Mat &result, int x, int y, int width, int height){
const float bad_point = std::numeric_limits<float>::quiet_NaN();
if (cloud->isOrganized()) {
std::cout << "PointCloud is organized..." << std::endl;
result = cv::Mat(cloud->height, cloud->width, CV_8UC3);
if (!cloud->empty()) {
for (int h=0; h<result.rows; h++) {
for (int w=0; w<result.cols; w++) {
// Check if in bounding window
if ( (h>y && h<(y+height)) && ((w > x) && w < (x+width)) ){
// do nothing
} else {
// remove point
//PointT point = cloud->at(w, h);
//cloud->at(w, h);
cloud->at(w, h).x = bad_point;
cloud->at(w, h).y = bad_point;
cloud->at(w, h).z = bad_point;
cloud->at(w, h).r = bad_point;
cloud->at(w, h).g = bad_point;
cloud->at(w, h).b = bad_point;
}
}
}
}
}
}
// Align a rigid object to a scene with clutter and occlusions
int
main (int argc, char **argv)
{
// Point clouds
PointCloudT::Ptr object (new PointCloudT);
PointCloudT::Ptr object_aligned (new PointCloudT);
PointCloudT::Ptr scene (new PointCloudT);
FeatureCloudT::Ptr object_features (new FeatureCloudT);
FeatureCloudT::Ptr scene_features (new FeatureCloudT);
// Get input object and scene
if (argc != 3)
{
pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]);
return (1);
}
// Load object and scene
pcl::console::print_highlight ("Loading point clouds...\n");
if (pcl::io::loadPCDFile<PointNT> (argv[1], *object) < 0 ||
pcl::io::loadPCDFile<PointNT> (argv[2], *scene) < 0)
{
pcl::console::print_error ("Error loading object/scene file!\n");
return (1);
}
// Downsample
pcl::console::print_highlight ("Downsampling...\n");
pcl::VoxelGrid<PointNT> grid;
const float leaf = 0.005f;
grid.setLeafSize (leaf, leaf, leaf);
grid.setInputCloud (object);
grid.filter (*object);
grid.setInputCloud (scene);
grid.filter (*scene);
// Estimate normals for scene
pcl::console::print_highlight ("Estimating scene normals...\n");
pcl::NormalEstimationOMP<PointNT,PointNT> nest;
nest.setRadiusSearch (0.01);
nest.setInputCloud (scene);
nest.compute (*scene);
// Estimate features
pcl::console::print_highlight ("Estimating features...\n");
FeatureEstimationT fest;
fest.setRadiusSearch (0.025);
fest.setInputCloud (object);
fest.setInputNormals (object);
fest.compute (*object_features);
fest.setInputCloud (scene);
fest.setInputNormals (scene);
fest.compute (*scene_features);
// Perform alignment
pcl::console::print_highlight ("Starting alignment...\n");
pcl::SampleConsensusPrerejective<PointNT,PointNT,FeatureT> align;
align.setInputSource (object);
align.setSourceFeatures (object_features);
align.setInputTarget (scene);
align.setTargetFeatures (scene_features);
align.setMaximumIterations (100000); // Number of RANSAC iterations
align.setNumberOfSamples (3); // Number of points to sample for generating/prerejecting a pose
align.setCorrespondenceRandomness (5); // Number of nearest features to use
align.setSimilarityThreshold (0.9f); // Polygonal edge length similarity threshold
align.setMaxCorrespondenceDistance (2.5f * leaf); // Inlier threshold
align.setInlierFraction (0.25f); // Required inlier fraction for accepting a pose hypothesis
{
pcl::ScopeTime t("Alignment");
align.align (*object_aligned);
}
if (align.hasConverged ())
{
// Print results
printf ("\n");
Eigen::Matrix4f transformation = align.getFinalTransformation ();
pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2));
pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2));
pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2));
pcl::console::print_info ("\n");
pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3));
pcl::console::print_info ("\n");
pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ());
// Show alignment
pcl::visualization::PCLVisualizer visu("Alignment");
visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene");
visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned");
visu.spin ();
}
else
{
pcl::console::print_error ("Alignment failed!\n");
return (1);
}
return (0);
}
int main (int argc, char** argv)
{
PointCloudT::Ptr cloud (new PointCloudT);
PointCloudT::Ptr new_cloud (new PointCloudT);
bool new_cloud_available_flag = false;
//pcl::Grabber* grab = new pcl::OpenNIGrabber ();
PointCloudT::Ptr ddd;
boost::function<void (const PointCloudT::ConstPtr&)> f =
boost::bind(&grabberCallback, _1, cloud, &new_cloud_available_flag);
grab->registerCallback (f);
viewer->registerKeyboardCallback(keyboardEventOccurred);
grab->start ();
bool first_time = true;
FILE* objects;
objects = fopen ("objects.txt","a");
while(!new_cloud_available_flag)
boost::this_thread::sleep(boost::posix_time::milliseconds(1));
new_cloud_available_flag=false;
////////////////////
// invert correction
////////////////////
Eigen::Matrix4f transMat = Eigen::Matrix4f::Identity();
transMat (1,1) = -1;
////////////////////
// pass filter
////////////////////
PointCloudT::Ptr passed_cloud;
pcl::PassThrough<PointT> pass;
passed_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT);
////////////////////
// voxel grid
////////////////////
PointCloudT::Ptr voxelized_cloud;
voxelized_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT);
pcl::VoxelGrid<PointT> vg;
vg.setLeafSize (0.001, 0.001, 0.001);
////////////////////
// sac segmentation
////////////////////
PointCloudT::Ptr cloud_f;
PointCloudT::Ptr cloud_plane;
PointCloudT::Ptr cloud_filtered;
cloud_f = boost::shared_ptr<PointCloudT>(new PointCloudT);
cloud_plane = boost::shared_ptr<PointCloudT> (new PointCloudT);
cloud_filtered = boost::shared_ptr<PointCloudT> (new PointCloudT);
pcl::SACSegmentation<PointT> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
////////////////////
// euclidean clustering
////////////////////
std::vector<pcl::PointIndices> cluster_indices;
std::vector<PointCloudT::Ptr> extracted_clusters;
pcl::search::KdTree<PointT>::Ptr eucl_tree (new pcl::search::KdTree<PointT>);
pcl::EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance (0.04);
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (eucl_tree);
PointCloudT::Ptr cloud_cluster;
////////////////////
// vfh estimate
////////////////////
pcl::NormalEstimation<PointT, pcl::Normal> ne;
pcl::search::KdTree<PointT>::Ptr vfh_tree1 (new pcl::search::KdTree<PointT> ());
pcl::VFHEstimation<PointT, pcl::Normal, pcl::VFHSignature308> vfh;
pcl::search::KdTree<PointT>::Ptr vfh_tree2 (new pcl::search::KdTree<PointT> ());
std::vector<pcl::PointCloud<pcl::VFHSignature308>::Ptr> computed_vfhs;
ne.setSearchMethod (vfh_tree1);
ne.setRadiusSearch (0.05);
vfh.setSearchMethod (vfh_tree2);
vfh.setRadiusSearch (0.05);
pcl::PointCloud<pcl::Normal>::Ptr normals;
pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs;
////////////////////
// nearest neighbour
////////////////////
int k = 6;
std::string kdtree_idx_file_name = "kdtree.idx";
std::string training_data_h5_file_name = "training_data.h5";
std::string training_data_list_file_name = "training_data.list";
// std::vector<vfh_model> models;
// flann::Matrix<float> data;
// loadFileList (models, training_data_list_file_name);
// flann::load_from_file (data,
// training_data_h5_file_name,
// "training_data");
//
// flann::Index<flann::ChiSquareDistance<float> > index (data,
// flann::SavedIndexParams
// ("kdtree.idx"));
////////////////////
// process nearest neighbour
////////////////////
std::vector<hypothesis> final_hypothesis;
final_hypothesis.clear();
double last = pcl::getTime();
while (! viewer->wasStopped())
{
if (new_cloud_available_flag)
{
new_cloud_available_flag = false;
double now = pcl::getTime();
////////////////////
// pass filter
////////////////////
//passed_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT);
////////////////////
// voxel grid
////////////////////
//voxelized_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT);
////////////////////
// sac segmentation
////////////////////
//cloud_f = boost::shared_ptr<PointCloudT>(new PointCloudT);
//cloud_plane = boost::shared_ptr<PointCloudT> (new PointCloudT);
//cloud_filtered = boost::shared_ptr<PointCloudT> (new PointCloudT);
////////////////////
// euclidean clustering
////////////////////
extracted_clusters.clear();
cluster_indices.clear();
////////////////////
// vfh estimate
////////////////////
computed_vfhs.clear();
////////////////////
// nearest neighbour
////////////////////
cloud_mutex.lock();
//displayCloud(viewer,cloud);
boost::thread displayCloud_(displayCloud,viewer,cloud);
if(now-last > 13 || first_time)
{
first_time = false;
last=now;
////////////////////
// invert correction
////////////////////
pcl::transformPointCloud(*cloud,*new_cloud,transMat);
////////////////////
// pass filter
////////////////////
pass.setInputCloud (new_cloud);
pass.setFilterFieldName ("x");
pass.setFilterLimits (-0.5, 0.5);
//pass.setFilterLimitsNegative (true);
pass.filter (*passed_cloud);
////////////////////
// voxel grid
////////////////////
vg.setInputCloud (passed_cloud);
vg.filter (*voxelized_cloud);
////////////////////
// sac segmentation
////////////////////
*cloud_filtered = *voxelized_cloud;
int i=0, nr_points = (int) voxelized_cloud->points.size ();
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Couldnt estimate a planar model for the dataset.\n";
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<PointT> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// Get the points associated with the planar surface
extract.filter (*cloud_plane);
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud_filtered = *cloud_f;
}
////////////////////
// euclidean clustering
////////////////////
// Creating the KdTree object for the search method of the extraction
eucl_tree->setInputCloud (cloud_filtered);
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
//PointCloudT::Ptr cloud_cluster (new PointCloudT);
cloud_cluster = boost::shared_ptr<PointCloudT>(new PointCloudT);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud_filtered->points [*pit]);
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
extracted_clusters.push_back(cloud_cluster);
}
cloud_cluster.reset();
////////////////////
// vfh estimate
////////////////////
for (int z=0; z<extracted_clusters.size(); ++z)
{
vfhs = boost::shared_ptr<pcl::PointCloud<pcl::VFHSignature308> > (new pcl::PointCloud<pcl::VFHSignature308>);
normals = boost::shared_ptr<pcl::PointCloud<pcl::Normal> > (new pcl::PointCloud<pcl::Normal>);
ne.setInputCloud (extracted_clusters.at(z));
ne.compute (*normals);
vfh.setInputCloud (extracted_clusters.at(z));
vfh.setInputNormals (normals);
vfh.compute (*vfhs);
computed_vfhs.push_back(vfhs);
}
vfhs.reset();
normals.reset();
////////////////////
// nearest neighbour
////////////////////
std::vector<vfh_model> models;
flann::Matrix<int> k_indices;
flann::Matrix<float> k_distances;
flann::Matrix<float> data;
loadFileList (models, training_data_list_file_name);
flann::load_from_file (data,
training_data_h5_file_name,
"training_data");
flann::Index<flann::ChiSquareDistance<float> > index
(data,
flann::SavedIndexParams
("kdtree.idx"));
for(int z=0; z<computed_vfhs.size(); ++z)
{
vfh_model histogram;
histogram.second.resize(308);
for (size_t i = 0; i < 308; ++i)
{
histogram.second[i] = computed_vfhs.at(z)->points[0].histogram[i];
}
index.buildIndex ();
nearestKSearch (index, histogram, k, k_indices, k_distances);
hypothesis x;
x.distance = k_distances[0][0];
size_t index = models.at(k_indices[0][0]).first.find("_v",5);
x.object_name = models.at(k_indices[0][0]).first.substr(5,index-5);
ddd = boost::shared_ptr<PointCloudT>(new PointCloudT);
pcl::transformPointCloud(*extracted_clusters.at(z),*ddd,transMat);
x.cluster = ddd;
ddd.reset();
std::string filename ="";
filename += "inputcloud_" + boost::lexical_cast<std::string>(j+1);
filename += "_" + boost::lexical_cast<std::string>(z) + ".pcd";
x.cluster_name = filename.c_str();
final_hypothesis.push_back(x);
x.cluster.reset();
//delete x;
// std::string filename ="";
// filename += "inputcloud_" + boost::lexical_cast<std::string>(j+1);
// filename += "_" + boost::lexical_cast<std::string>(z) + ".pcd";
// const char* filen = filename.c_str();
// fprintf(objects,"%s",filen);
// fprintf(objects,"::");
// fprintf(objects,models.at (k_indices[0][0]).first.c_str());
// fprintf(objects,"::");
// fprintf(objects,"%f",k_distances[0][0]);
// fprintf(objects,"\n");
}
delete[] k_indices.ptr ();
delete[] k_distances.ptr ();
delete[] data.ptr ();
std::cout << final_hypothesis.size() << std::endl;
viewer->removeAllShapes();
for(int z=0; z<final_hypothesis.size();++z)
{
if(final_hypothesis.at(z).distance < 100)
{
fprintf(objects,"%s",final_hypothesis.at(z).cluster_name.c_str());
fprintf(objects,"::");
fprintf(objects,"%s",final_hypothesis.at(z).object_name.c_str());
fprintf(objects,"::");
fprintf(objects,"%f",final_hypothesis.at(z).distance);
fprintf(objects,"\n");
std::stringstream ddd;
ddd << final_hypothesis.at(z).object_name;
ddd << "\n" << "(";
ddd << final_hypothesis.at(z).distance;
ddd << ")";
viewer->addText3D(ddd.str().c_str(),
final_hypothesis.at(z).cluster->points[0],
0.02,1,1,1,
boost::lexical_cast<std::string>(z));
drawBoundingBox(final_hypothesis.at(z).cluster,viewer,z);
}
}
//boost::thread allBoxes_(allBoxes,viewer,final_hypothesis);
//allBoxes_.join();
viewer->spinOnce();
final_hypothesis.clear();
j++;
}
// for(int z=0; z<extracted_clusters.size(); ++z)
// {
// //viewer->addPointCloud<PointT>(extracted_clusters.at(z),
// // boost::lexical_cast<std::string>(z));
//
// std::string filename ="";
// filename += "inputcloud_" + boost::lexical_cast<std::string>(j);
// filename += "_" + boost::lexical_cast<std::string>(z) + ".pcd";
// pcl::io::savePCDFile(filename,*extracted_clusters.at(z),false);
// }
// for(int z=0; z<computed_vfhs.size(); ++z)
// {
// //viewer->addPointCloud<PointT>(extracted_clusters.at(z),
// // boost::lexical_cast<std::string>(z));
//
// std::string filename ="";
// filename += "inputcloud_" + boost::lexical_cast<std::string>(j);
// filename += "_" + boost::lexical_cast<std::string>(z) + "_vfhs.pcd";
// pcl::io::savePCDFileASCII<pcl::VFHSignature308> (filename, *computed_vfhs.at(z));
// }
//viewer->removeAllShapes();
// viewer->removeAllPointClouds();
// viewer->setCameraPosition(0, 0, 0, 0, 0, 1, 0, -1, 0);
// pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
// viewer->addPointCloud<PointT>(cloud,rgb,"input_cloud");
// viewer->spinOnce();
//std::cout << final_hypothesis.at(0).cluster->points[0];
//boost::this_thread::sleep(boost::posix_time::milliseconds(10));
displayCloud_.join();
cloud_mutex.unlock();
}
}
grab->stop();
return 0;
}
int
main (int argc, char ** argv)
{
if (argc < 2)
{
pcl::console::print_info ("Syntax is: %s {-p <pcd-file> OR -r <rgb-file> -d <depth-file>} \n --NT (disables use of single camera transform) \n -o <output-file> \n -O <refined-output-file> \n-l <output-label-file> \n -L <refined-output-label-file> \n-v <voxel resolution> \n-s <seed resolution> \n-c <color weight> \n-z <spatial weight> \n-n <normal_weight>] \n", argv[0]);
return (1);
}
/////////////////////////////// //////////////////////////////
////////////////////////////// //////////////////////////////
////// THIS IS ALL JUST INPUT HANDLING - Scroll down until
////// pcl::SupervoxelClustering<pcl::PointXYZRGB> super
////////////////////////////// //////////////////////////////
std::string rgb_path;
bool rgb_file_specified = pcl::console::find_switch (argc, argv, "-r");
if (rgb_file_specified)
pcl::console::parse (argc, argv, "-r", rgb_path);
std::string depth_path;
bool depth_file_specified = pcl::console::find_switch (argc, argv, "-d");
if (depth_file_specified)
pcl::console::parse (argc, argv, "-d", depth_path);
PointCloudT::Ptr cloud = boost::make_shared < PointCloudT >();
NormalCloudT::Ptr input_normals = boost::make_shared < NormalCloudT > ();
bool pcd_file_specified = pcl::console::find_switch (argc, argv, "-p");
std::string pcd_path;
if (!depth_file_specified || !rgb_file_specified)
{
std::cout << "Using point cloud\n";
if (!pcd_file_specified)
{
std::cout << "No cloud specified!\n";
return (1);
}else
{
pcl::console::parse (argc,argv,"-p",pcd_path);
}
}
bool disable_transform = pcl::console::find_switch (argc, argv, "--NT");
bool ignore_provided_normals = pcl::console::find_switch (argc, argv, "--nonormals");
bool has_normals = false;
std::string out_path = "test_output.png";;
pcl::console::parse (argc, argv, "-o", out_path);
std::string out_label_path = "test_output_labels.png";
pcl::console::parse (argc, argv, "-l", out_label_path);
std::string refined_out_path = "refined_test_output.png";
pcl::console::parse (argc, argv, "-O", refined_out_path);
std::string refined_out_label_path = "refined_test_output_labels.png";;
pcl::console::parse (argc, argv, "-L", refined_out_label_path);
float voxel_resolution = 0.008f;
pcl::console::parse (argc, argv, "-v", voxel_resolution);
float seed_resolution = 0.08f;
pcl::console::parse (argc, argv, "-s", seed_resolution);
float color_importance = 0.2f;
pcl::console::parse (argc, argv, "-c", color_importance);
float spatial_importance = 0.4f;
pcl::console::parse (argc, argv, "-z", spatial_importance);
float normal_importance = 1.0f;
pcl::console::parse (argc, argv, "-n", normal_importance);
if (!pcd_file_specified)
{
//Read the images
vtkSmartPointer<vtkImageReader2Factory> reader_factory = vtkSmartPointer<vtkImageReader2Factory>::New ();
vtkImageReader2* rgb_reader = reader_factory->CreateImageReader2 (rgb_path.c_str ());
//qDebug () << "RGB File="<< QString::fromStdString(rgb_path);
if ( ! rgb_reader->CanReadFile (rgb_path.c_str ()))
{
std::cout << "Cannot read rgb image file!";
return (1);
}
rgb_reader->SetFileName (rgb_path.c_str ());
rgb_reader->Update ();
//qDebug () << "Depth File="<<QString::fromStdString(depth_path);
vtkImageReader2* depth_reader = reader_factory->CreateImageReader2 (depth_path.c_str ());
if ( ! depth_reader->CanReadFile (depth_path.c_str ()))
{
std::cout << "Cannot read depth image file!";
return (1);
}
depth_reader->SetFileName (depth_path.c_str ());
depth_reader->Update ();
vtkSmartPointer<vtkImageFlip> flipXFilter = vtkSmartPointer<vtkImageFlip>::New();
flipXFilter->SetFilteredAxis(0); // flip x axis
flipXFilter->SetInputConnection(rgb_reader->GetOutputPort());
flipXFilter->Update();
vtkSmartPointer<vtkImageFlip> flipXFilter2 = vtkSmartPointer<vtkImageFlip>::New();
flipXFilter2->SetFilteredAxis(0); // flip x axis
flipXFilter2->SetInputConnection(depth_reader->GetOutputPort());
flipXFilter2->Update();
vtkSmartPointer<vtkImageData> rgb_image = flipXFilter->GetOutput ();
int *rgb_dims = rgb_image->GetDimensions ();
vtkSmartPointer<vtkImageData> depth_image = flipXFilter2->GetOutput ();
int *depth_dims = depth_image->GetDimensions ();
if (rgb_dims[0] != depth_dims[0] || rgb_dims[1] != depth_dims[1])
{
std::cout << "Depth and RGB dimensions to not match!";
std::cout << "RGB Image is of size "<<rgb_dims[0] << " by "<<rgb_dims[1];
std::cout << "Depth Image is of size "<<depth_dims[0] << " by "<<depth_dims[1];
return (1);
}
cloud->points.reserve (depth_dims[0] * depth_dims[1]);
cloud->width = depth_dims[0];
cloud->height = depth_dims[1];
cloud->is_dense = false;
// Fill in image data
int centerX = static_cast<int>(cloud->width / 2.0);
int centerY = static_cast<int>(cloud->height / 2.0);
unsigned short* depth_pixel;
unsigned char* color_pixel;
float scale = 1.0f/1000.0f;
float focal_length = 525.0f;
float fl_const = 1.0f / focal_length;
depth_pixel = static_cast<unsigned short*>(depth_image->GetScalarPointer (depth_dims[0]-1,depth_dims[1]-1,0));
color_pixel = static_cast<unsigned char*> (rgb_image->GetScalarPointer (depth_dims[0]-1,depth_dims[1]-1,0));
for (size_t y=0; y<cloud->height; ++y)
{
for (size_t x=0; x<cloud->width; ++x, --depth_pixel, color_pixel-=3)
{
PointT new_point;
// uint8_t* p_i = &(cloud_blob->data[y * cloud_blob->row_step + x * cloud_blob->point_step]);
float depth = static_cast<float>(*depth_pixel) * scale;
if (depth == 0.0f)
{
new_point.x = new_point.y = new_point.z = std::numeric_limits<float>::quiet_NaN ();
}
else
{
new_point.x = (static_cast<float> (x) - centerX) * depth * fl_const;
new_point.y = (static_cast<float> (centerY) - y) * depth * fl_const; // vtk seems to start at the bottom left image corner
new_point.z = depth;
}
uint32_t rgb = static_cast<uint32_t>(color_pixel[0]) << 16 | static_cast<uint32_t>(color_pixel[1]) << 8 | static_cast<uint32_t>(color_pixel[2]);
new_point.rgb = *reinterpret_cast<float*> (&rgb);
cloud->points.push_back (new_point);
}
}
}
else
{
/// check if the provided pcd file contains normals
pcl::PCLPointCloud2 input_pointcloud2;
if (pcl::io::loadPCDFile (pcd_path, input_pointcloud2))
{
PCL_ERROR ("ERROR: Could not read input point cloud %s.\n", pcd_path.c_str ());
return (3);
}
pcl::fromPCLPointCloud2 (input_pointcloud2, *cloud);
if (!ignore_provided_normals)
{
if (hasField (input_pointcloud2,"normal_x"))
{
std::cout << "Using normals contained in file. Set --nonormals option to disable this.\n";
pcl::fromPCLPointCloud2 (input_pointcloud2, *input_normals);
has_normals = true;
}
}
}
std::cout << "Done making cloud!\n";
/////////////////////////////// //////////////////////////////
////////////////////////////// //////////////////////////////
////// This is how to use supervoxels
////////////////////////////// //////////////////////////////
////////////////////////////// //////////////////////////////
// If we're using the single camera transform no negative z allowed since we use log(z)
if (!disable_transform)
{
for (PointCloudT::iterator cloud_itr = cloud->begin (); cloud_itr != cloud->end (); ++cloud_itr)
if (cloud_itr->z < 0)
{
PCL_ERROR ("Points found with negative Z values, this is not compatible with the single camera transform!\n");
PCL_ERROR ("Set the --NT option to disable the single camera transform!\n");
return 1;
}
std::cout <<"You have the single camera transform enabled - this should be used with point clouds captured from a single camera.\n";
std::cout <<"You can disable the transform with the --NT flag\n";
}
pcl::SupervoxelClustering<PointT> super (voxel_resolution, seed_resolution,!disable_transform);
super.setInputCloud (cloud);
if (has_normals)
super.setNormalCloud (input_normals);
super.setColorImportance (color_importance);
super.setSpatialImportance (spatial_importance);
super.setNormalImportance (normal_importance);
std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr > supervoxel_clusters;
std::cout << "Extracting supervoxels!\n";
super.extract (supervoxel_clusters);
std::cout << "Found " << supervoxel_clusters.size () << " Supervoxels!\n";
PointLCloudT::Ptr labeled_voxel_cloud = super.getLabeledVoxelCloud ();
PointCloudT::Ptr voxel_centroid_cloud = super.getVoxelCentroidCloud ();
PointNCloudT::Ptr sv_normal_cloud = super.makeSupervoxelNormalCloud (supervoxel_clusters);
PointLCloudT::Ptr full_labeled_cloud = super.getLabeledCloud ();
std::cout << "Getting supervoxel adjacency\n";
std::multimap<uint32_t, uint32_t> label_adjacency;
super.getSupervoxelAdjacency (label_adjacency);
std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr > refined_supervoxel_clusters;
std::cout << "Refining supervoxels \n";
super.refineSupervoxels (3, refined_supervoxel_clusters);
PointLCloudT::Ptr refined_labeled_voxel_cloud = super.getLabeledVoxelCloud ();
PointNCloudT::Ptr refined_sv_normal_cloud = super.makeSupervoxelNormalCloud (refined_supervoxel_clusters);
PointLCloudT::Ptr refined_full_labeled_cloud = super.getLabeledCloud ();
// THESE ONLY MAKE SENSE FOR ORGANIZED CLOUDS
if (cloud->isOrganized ())
{
pcl::io::savePNGFile (out_label_path, *full_labeled_cloud, "label");
pcl::io::savePNGFile (refined_out_label_path, *refined_full_labeled_cloud, "label");
//Save RGB from labels
pcl::io::PointCloudImageExtractorFromLabelField<PointLT> pcie (pcie.io::PointCloudImageExtractorFromLabelField<PointLT>::COLORS_RGB_GLASBEY);
//We need to set this to account for NAN points in the organized cloud
pcie.setPaintNaNsWithBlack (true);
pcl::PCLImage image;
pcie.extract (*full_labeled_cloud, image);
pcl::io::savePNGFile (out_path, image);
pcie.extract (*refined_full_labeled_cloud, image);
pcl::io::savePNGFile (refined_out_path, image);
}
std::cout << "Constructing Boost Graph Library Adjacency List...\n";
typedef boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, uint32_t, float> VoxelAdjacencyList;
typedef VoxelAdjacencyList::vertex_descriptor VoxelID;
typedef VoxelAdjacencyList::edge_descriptor EdgeID;
VoxelAdjacencyList supervoxel_adjacency_list;
super.getSupervoxelAdjacencyList (supervoxel_adjacency_list);
std::cout << "Loading visualization...\n";
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0);
viewer->registerKeyboardCallback(keyboard_callback, 0);
bool refined_normal_shown = show_refined;
bool refined_sv_normal_shown = show_refined;
bool sv_added = false;
bool normals_added = false;
bool graph_added = false;
std::vector<std::string> poly_names;
std::cout << "Loading viewer...\n";
while (!viewer->wasStopped ())
{
if (show_supervoxels)
{
if (!viewer->updatePointCloud ((show_refined)?refined_labeled_voxel_cloud:labeled_voxel_cloud, "colored voxels"))
{
viewer->addPointCloud ((show_refined)?refined_labeled_voxel_cloud:labeled_voxel_cloud, "colored voxels");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3.0, "colored voxels");
}
}
else
{
viewer->removePointCloud ("colored voxels");
}
if (show_voxel_centroids)
{
if (!viewer->updatePointCloud (voxel_centroid_cloud, "voxel centroids"))
{
viewer->addPointCloud (voxel_centroid_cloud, "voxel centroids");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,2.0, "voxel centroids");
}
}
else
{
viewer->removePointCloud ("voxel centroids");
}
if (show_supervoxel_normals)
{
if (refined_sv_normal_shown != show_refined || !sv_added)
{
viewer->removePointCloud ("supervoxel_normals");
viewer->addPointCloudNormals<PointNormal> ((show_refined)?refined_sv_normal_cloud:sv_normal_cloud,1,0.05f, "supervoxel_normals");
sv_added = true;
}
refined_sv_normal_shown = show_refined;
}
else if (!show_supervoxel_normals)
{
viewer->removePointCloud ("supervoxel_normals");
}
if (show_normals)
{
std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr>::iterator sv_itr,sv_itr_end;
sv_itr = ((show_refined)?refined_supervoxel_clusters.begin ():supervoxel_clusters.begin ());
sv_itr_end = ((show_refined)?refined_supervoxel_clusters.end ():supervoxel_clusters.end ());
for (; sv_itr != sv_itr_end; ++sv_itr)
{
std::stringstream ss;
ss << sv_itr->first <<"_normal";
if (refined_normal_shown != show_refined || !normals_added)
{
viewer->removePointCloud (ss.str ());
viewer->addPointCloudNormals<PointT,Normal> ((sv_itr->second)->voxels_,(sv_itr->second)->normals_,10,0.02f,ss.str ());
// std::cout << (sv_itr->second)->normals_->points[0]<<"\n";
}
}
normals_added = true;
refined_normal_shown = show_refined;
}
else if (!show_normals)
{
std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr>::iterator sv_itr,sv_itr_end;
sv_itr = ((show_refined)?refined_supervoxel_clusters.begin ():supervoxel_clusters.begin ());
sv_itr_end = ((show_refined)?refined_supervoxel_clusters.end ():supervoxel_clusters.end ());
for (; sv_itr != sv_itr_end; ++sv_itr)
{
std::stringstream ss;
ss << sv_itr->first << "_normal";
viewer->removePointCloud (ss.str ());
}
}
if (show_graph && !graph_added)
{
poly_names.clear ();
std::multimap<uint32_t,uint32_t>::iterator label_itr = label_adjacency.begin ();
for ( ; label_itr != label_adjacency.end (); )
{
//First get the label
uint32_t supervoxel_label = label_itr->first;
//Now get the supervoxel corresponding to the label
pcl::Supervoxel<PointT>::Ptr supervoxel = supervoxel_clusters.at (supervoxel_label);
//Now we need to iterate through the adjacent supervoxels and make a point cloud of them
PointCloudT adjacent_supervoxel_centers;
std::multimap<uint32_t,uint32_t>::iterator adjacent_itr = label_adjacency.equal_range (supervoxel_label).first;
for ( ; adjacent_itr!=label_adjacency.equal_range (supervoxel_label).second; ++adjacent_itr)
{
pcl::Supervoxel<PointT>::Ptr neighbor_supervoxel = supervoxel_clusters.at (adjacent_itr->second);
adjacent_supervoxel_centers.push_back (neighbor_supervoxel->centroid_);
}
//Now we make a name for this polygon
std::stringstream ss;
ss << "supervoxel_" << supervoxel_label;
poly_names.push_back (ss.str ());
addSupervoxelConnectionsToViewer (supervoxel->centroid_, adjacent_supervoxel_centers, ss.str (), viewer);
//Move iterator forward to next label
label_itr = label_adjacency.upper_bound (supervoxel_label);
}
graph_added = true;
}
else if (!show_graph && graph_added)
{
for (std::vector<std::string>::iterator name_itr = poly_names.begin (); name_itr != poly_names.end (); ++name_itr)
{
viewer->removeShape (*name_itr);
}
graph_added = false;
}
if (show_help)
{
viewer->removeShape ("help_text");
printText (viewer);
}
else
{
removeText (viewer);
if (!viewer->updateText("Press h to show help", 5, 10, 12, 1.0, 1.0, 1.0,"help_text") )
viewer->addText("Press h to show help", 5, 10, 12, 1.0, 1.0, 1.0,"help_text");
}
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
return (0);
}
int main (int argc, char** argv)
{
ros::init(argc, argv, "cv_proj");
//std::string input_file = "/home/igor/pcds/assembly_objs/ardrone_02_indoor.pcd";
//std::string input_file = "/home/igor/pcds/assembly_objs/ardrone_03_outdoor.pcd";
/*
std::string input_file = "/home/igor/pcds/cluttered/3_objs_ardrone_indoor.pcd";
std::string output_file = "/home/igor/pcds/cv_proj_out/out_cluttered_indoor_01.pcd";
std::string template_file = "/home/igor/pcds/templates/indoor_template.pcd";
std::string out_rgb = "/home/igor/pcds/cv_proj_out/out_result_05.jpg";
*/
std::string input_file, out_pcd, template_file, out_rgb, out_transf_pcd;
ros::param::param<std::string>("/cv_proj/input_file", input_file, "/home/igor/pcds/cluttered/3_objs_ardrone_indoor.pcd");
//ros::param::param<std::string>("/cv_proj/out_pcd", out_pcd, "/home/igor/pcds/cv_proj_out/out_cluttered_indoor_01.pcd");
ros::param::param<std::string>("/cv_proj/template_file", template_file, "/home/igor/pcds/templates/indoor_template.pcd");
//ros::param::param<std::string>("/cv_proj/out_rgb", out_rgb, "/home/igor/pcds/cv_proj_out/out_result_05.jpg");
boost::filesystem::path filepath(input_file);
boost::filesystem::path filename = filepath.filename();
filename = filename.stem(); // Get rid of the extension
boost::filesystem::path dir = filepath.parent_path();
std::string opencv_out_ext = "_filtered.png";
std::string pcl_out_ext = "_filtered.pcd";
std::string output_folder = "/output_cv_proj/";
std::string output_stem;
output_stem = dir.string() + output_folder + filename.string();
out_rgb = output_stem + opencv_out_ext;
out_pcd = output_stem + pcl_out_ext;
out_transf_pcd = output_stem + "_templ" + pcl_out_ext;
std::cout << out_rgb << std::endl;
std::cout << out_pcd << std::endl;
// Read in the cloud data
pcl::PCDReader reader;
PointCloudT::Ptr cloud (new PointCloudT), cloud_f (new PointCloudT);
reader.read (input_file, *cloud);
std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*
if (cloud->isOrganized()) {
std::cout << "-- PointCloud cloud is organized" << std::endl;
std::cout << "-- PointCloud cloud width: " << cloud->width << std::endl;
std::cout << "-- PointCloud cloud height: " << cloud->height << std::endl;
}
/*
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<PointT> vg;
PointCloudT::Ptr cloud_filtered (new PointCloudT);
vg.setInputCloud (cloud);
//vg.setLeafSize (0.01f, 0.01f, 0.01f);
vg.setLeafSize (0.001f, 0.001f, 0.001f);
//
vg.filter (*cloud_filtered);
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //*
*/
/**/
PointCloudT::Ptr cloud_filtered (new PointCloudT);
*cloud_filtered = *cloud;
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<PointT> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
PointCloudT::Ptr cloud_plane (new PointCloudT ());
pcl::PCDWriter writer;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
int i=0, nr_points = (int) cloud_filtered->points.size ();
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<PointT> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
//extract.setUserFilterValue(999);
extract.setKeepOrganized(true);
// Get the points associated with the planar surface
extract.filter (*cloud_plane);
//extract.filterDirectly(cloud_plane);
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
// To keep point cloud organized
//extract.setKeepOrganized(true);
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
//extract.filterDirectly(cloud_f);
*cloud_filtered = *cloud_f;
}
cv::Mat result;
PC_to_Mat(cloud_filtered,result);
imwrite( out_rgb, result );
int rgb_x, rgb_y, rgb_width, rgb_height;
rgb_x = 240;
rgb_y = 150;
rgb_width = 200;
rgb_height = 200;
filter_PC_from_BB ( cloud_filtered, result, rgb_x, rgb_y, rgb_width, rgb_height);
// For templates
/*
// Box filter
pcl::PassThrough<PointT> pass_x;
pass_x.setInputCloud (cloud_filtered);
pass_x.setFilterFieldName ("x");
pass_x.setFilterLimits (-0.2, 0.4);
//pass.setFilterLimitsNegative (true);
pass_x.filter (*cloud_filtered);
pcl::PassThrough<PointT> pass_y;
pass_y.setInputCloud (cloud_filtered);
pass_y.setFilterFieldName ("y");
pass_y.setFilterLimits (-0.2, 0.4);
//pass.setFilterLimitsNegative (true);
pass_y.filter (*cloud_filtered);
*/
// Save file
pcl::io::savePCDFileASCII (out_pcd, *cloud_filtered);
std::cerr << "Saved " << (*cloud_filtered).points.size () << " data points to PCD file." << std::endl;
// Read in template file
PointCloudT::Ptr cloud_template (new PointCloudT);
reader.read (template_file, *cloud_template);
pcl::VoxelGrid<PointT> vg_in;
PointCloudT::Ptr cloud_filtered_vg (new PointCloudT);
vg_in.setInputCloud (cloud_filtered);
vg_in.setLeafSize (0.001f, 0.001f, 0.001f); //vg.setLeafSize (0.01f, 0.01f, 0.01f);
vg_in.filter (*cloud_filtered_vg);
pcl::VoxelGrid<PointT> vg_temp;
PointCloudT::Ptr cloud_template_vg (new PointCloudT);
vg_temp.setInputCloud (cloud_template);
vg_temp.setLeafSize (0.001f, 0.001f, 0.001f); //vg.setLeafSize (0.01f, 0.01f, 0.01f);
vg_temp.filter (*cloud_template_vg);
// ICP
PointCloudT::Ptr cloud_in (new PointCloudT), cloud_out (new PointCloudT);
*cloud_in = *cloud_template_vg;
*cloud_out = *cloud_filtered_vg;
pcl::IterativeClosestPoint<PointT, PointT> icp;
icp.setInputCloud(cloud_in);
icp.setInputTarget(cloud_out);
PointCloudT Final;
icp.align(Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
Eigen::Matrix4f transform = icp.getFinalTransformation ();
std::cout << transform << std::endl;
// Transform template to original frame
PointCloudT::Ptr cloud_out2 (new PointCloudT);
pcl::transformPointCloud(*cloud_in,*cloud_out2,transform);
pcl::io::savePCDFileASCII (out_transf_pcd, *cloud_out2);
std::cerr << "Saved " << (*cloud_out2).points.size () << " data points to PCD file." << std::endl;
return (0);
}
// Align a rigid object to a scene with clutter and occlusions
int main (int argc, char **argv)
{
// Point clouds
PointCloudT::Ptr object (new PointCloudT);
PointCloudT::Ptr scene (new PointCloudT);
PointCloudT::Ptr object_aligned (new PointCloudT);
// Get input object and scene
if (argc < 3)
{
pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]);
return (1);
}
pcl::console::parse_argument (argc, argv, "--max_iterations", in_max_iterations);
pcl::console::parse_argument (argc, argv, "--num_samples", in_num_samples);
pcl::console::parse_argument (argc, argv, "--s_thresh", in_similarity_thresh);
pcl::console::parse_argument (argc, argv, "--max_cdist", in_max_corresp_dist);
pcl::console::parse_argument (argc, argv, "--inlier_frac", in_inlier_frac);
pcl::console::parse_argument (argc, argv, "--leaf", in_leaf);
pcl::console::parse_argument (argc, argv, "--normal_radius", normal_radius);
pcl::console::parse_argument (argc, argv, "--feature_radius", feature_radius);
pcl::console::parse_argument (argc, argv, "--icp", in_icp);
pcl::console::parse_argument (argc, argv, "--max_corr_icp", max_corr_icp);
pcl::console::parse_argument (argc, argv, "--icp_eps", max_eps_icp);
// Load object and scene
pcl::console::print_highlight ("Loading point clouds...\n");
pcl_tools::cloud_from_stl(argv[2], *object);
if (pcl::io::loadPCDFile<PointNT> (argv[1], *scene) < 0)
{
pcl::console::print_error ("Error loading object/scene file!\n");
return (1);
}
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*scene, *scene, indices);
pcl::removeNaNFromPointCloud(*object, *object, indices);
// /*pcl_tools::icp_result align = */alp_align(object, scene, object_aligned, 50000, 3, 0.9f, 5.5f * leaf, 0.7f);
// /*pcl_tools::icp_result align = */alp_align(object_aligned, scene, object_aligned, 50000, 3, 0.9f, 7.5f * leaf, 0.4f);
std::cout << "Inlier frac " << in_inlier_frac << std::endl;
pcl_tools::icp_result align = alp_align(object, scene, object_aligned, in_max_iterations, in_num_samples, in_similarity_thresh, in_max_corresp_dist, in_inlier_frac, in_leaf);
pcl::visualization::PCLVisualizer visu("Alignment");
if (align.converged)
{
pcl::console::print_info ("Inliers: %i/%i, scene: %i\n", align.inliers, object->size (), scene->size ());
// Show alignment
visu.addPointCloud (object, ColorHandlerT (object, 255.0, 0.0, 0.0), "object");
visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene");
visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned");
// visu.addPointCloudNormals<PointNT>(object);
visu.spin ();
}
else
{
pcl::console::print_error ("Alignment failed!\n");
return (1);
}
if (in_icp) {
pcl::console::print_highlight ("Applying ICP now\n");
pcl::IterativeClosestPointNonLinear<PointNT, PointNT> icp;
// pcl::IterativeClosestPoint<PointNT, PointNT> icp;
pcl_tools::affine_cloud(Eigen::Vector3f::UnitZ(), 0.0, Eigen::Vector3f(0.0, 0.0, 0.02), *object_aligned, *object_aligned);
icp.setMaximumIterations (100);
icp.setMaxCorrespondenceDistance(max_corr_icp);
icp.setTransformationEpsilon (max_eps_icp);
icp.setInputSource (object_aligned);
icp.setInputTarget (scene);
icp.align (*object_aligned);
if (icp.hasConverged()) {
pcl::console::print_highlight ("ICP: Converged with fitness %f\n", icp.getFitnessScore());
}
// pcl::visualization::PCLVisualizer visu("Alignment");
// visu.addPointCloud (object, ColorHandlerT (object, 255.0, 0.0, 0.0), "object");
// visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene");
visu.updatePointCloud (object_aligned, ColorHandlerT (object_aligned, 100.0, 50.0, 200.0), "object_aligned");
// visu.addPointCloudNormals<PointNT>(object);
visu.spin ();
}
return (0);
}
