void FeatureCalculate::pca(CloudPtr cloud,PlanSegment &plane) { Eigen::MatrixXf X(3,cloud->size()); double avr_x=0.0,avr_y=0.0,avr_z=0.0; for (int i = 0; i <cloud->size(); i++) { avr_x = avr_x * double(i) / (i + 1) + cloud->points[i].x / double(i + 1); avr_y = avr_y * double(i) / (i + 1) + cloud->points[i].y / double(i + 1); avr_z = avr_z * double(i) / (i + 1) + cloud->points[i].z / double(i + 1); } for (int i=0;i<cloud->size();i++) { X(0,i)=cloud->points[i].x-avr_x; X(1,i)=cloud->points[i].y-avr_y; X(2,i)=cloud->points[i].z-avr_z; } Eigen::MatrixXf XT(cloud->size(),3); XT=X.transpose(); Eigen::Matrix3f XXT; XXT=X*XT; EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; pcl::eigen33 (XXT, eigen_value, eigen_vector); plane.normal.normal_x = eigen_vector [0]; plane.normal.normal_y = eigen_vector [1]; plane.normal.normal_z = eigen_vector [2]; plane.distance = - (plane.normal.normal_x * avr_x + plane.normal.normal_y * avr_y + plane.normal.normal_z * avr_z); float eig_sum = XXT.coeff (0) + XXT.coeff (4) + XXT.coeff (8); plane.min_value=eigen_value; if (eig_sum != 0) plane.normal.curvature = fabsf (eigen_value / eig_sum); else plane.normal.curvature = 0.0; }
void transform(string frame) { tf_listener->waitForTransform(frame.data(), pCloud_input->header.frame_id, ros::Time::now(), ros::Duration(5.0)); pcl_ros::transformPointCloud(frame.data(), *(pCloud_input), *pCloud, *tf_listener); pCloud_input.reset(new Cloud); pCloud_input = pCloud; pCloud.reset(new Cloud); }
int main (int argc, char **argv) { double dist = 0.1; pcl::console::parse_argument (argc, argv, "-d", dist); double rans = 0.1; pcl::console::parse_argument (argc, argv, "-r", rans); int iter = 100; pcl::console::parse_argument (argc, argv, "-i", iter); pcl::registration::ELCH<PointType> elch; pcl::IterativeClosestPoint<PointType, PointType>::Ptr icp (new pcl::IterativeClosestPoint<PointType, PointType>); icp->setMaximumIterations (iter); icp->setMaxCorrespondenceDistance (dist); icp->setRANSACOutlierRejectionThreshold (rans); elch.setReg (icp); std::vector<int> pcd_indices; pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd"); CloudVector clouds; for (size_t i = 0; i < pcd_indices.size (); i++) { CloudPtr pc (new Cloud); pcl::io::loadPCDFile (argv[pcd_indices[i]], *pc); clouds.push_back (CloudPair (argv[pcd_indices[i]], pc)); std::cout << "loading file: " << argv[pcd_indices[i]] << " size: " << pc->size () << std::endl; elch.addPointCloud (clouds[i].second); } int first = 0, last = 0; for (size_t i = 0; i < clouds.size (); i++) { if (loopDetection (int (i), clouds, 3.0, first, last)) { std::cout << "Loop between " << first << " (" << clouds[first].first << ") and " << last << " (" << clouds[last].first << ")" << std::endl; elch.setLoopStart (first); elch.setLoopEnd (last); elch.compute (); } } for (const auto &cloud : clouds) { std::string result_filename (cloud.first); result_filename = result_filename.substr (result_filename.rfind ('/') + 1); pcl::io::savePCDFileBinary (result_filename.c_str (), *(cloud.second)); std::cout << "saving result to " << result_filename << std::endl; } return 0; }
void cuttingRange(double range) { for(int i=0;i<pCloud_input->points.size();i++){ double dist = sqrt(pow(pCloud_input->points.at(i).x,2)+pow(pCloud_input->points.at(i).y,2)+pow(pCloud_input->points.at(i).z,2)); if(dist<range){ pCloud->points.push_back(pCloud_input->points.at(i)); } } pCloud_input.reset(new Cloud); pCloud_input = pCloud; pCloud.reset(new Cloud); }
void downsampling(double scale) { CloudConstPtr pCloudConst = (CloudConstPtr)pCloud_input; pcl::VoxelGrid<PointT> vg; vg.setInputCloud(pCloudConst); vg.setLeafSize(scale, scale, scale); // down sampling using a leaf size of 'scale' vg.filter(*pCloud); pCloud_input.reset(new Cloud); pCloud_input = pCloud; pCloud.reset(new Cloud); }
/* ---[ */ int main (int argc, char** argv) { // Check whether we want to enable debug mode bool debug = false; parse_argument (argc, argv, "-debug", debug); if (debug) setVerbosityLevel (L_DEBUG); parse_argument (argc, argv, "-rejection", rejection); parse_argument (argc, argv, "-visualization", visualize); if (visualize) vis.reset (new PCLVisualizer ("Registration example")); // Parse the command line arguments for .pcd and .transform files std::vector<int> p_pcd_file_indices, p_tr_file_indices; p_pcd_file_indices = parse_file_extension_argument (argc, argv, ".pcd"); if (p_pcd_file_indices.size () != 2) { print_error ("Need one input source PCD file and one input target PCD file to continue.\n"); print_error ("Example: %s source.pcd target.pcd output.transform\n", argv[0]); return (-1); } p_tr_file_indices = parse_file_extension_argument (argc, argv, ".transform"); if (p_tr_file_indices.size () != 1) { print_error ("Need one output transform file to continue.\n"); print_error ("Example: %s source.pcd target.pcd output.transform\n", argv[0]); return (-1); } // Load the files print_info ("Loading %s as source and %s as target...\n", argv[p_pcd_file_indices[0]], argv[p_pcd_file_indices[1]]); src.reset (new PointCloud<PointT>); tgt.reset (new PointCloud<PointT>); if (loadPCDFile (argv[p_pcd_file_indices[0]], *src) == -1 || loadPCDFile (argv[p_pcd_file_indices[1]], *tgt) == -1) { print_error ("Error reading the input files!\n"); return (-1); } // Compute the best transformtion Eigen::Matrix4d transform; icp (src, tgt, transform); saveTransform (argv[p_tr_file_indices[0]], transform); cerr.precision (15); std::cerr << transform << std::endl; }
void run () { CloudPtr filecloud; pcl::Grabber* interface; if(filename_.empty()) { interface = new pcl::OpenNIGrabber (device_id_); boost::function<void (const CloudConstPtr&)> f = boost::bind (&PclPyramid::cloud_cb_, this, _1); boost::signals2::connection c = interface->registerCallback (f); interface->start (); } else { pcd_cloud.reset (new pcl::PCLPointCloud2); if(pcd.read (filename_, *pcd_cloud, origin, orientation, version) < 0) cout << "file read failed" << endl; filecloud.reset (new Cloud); pcl::fromPCLPointCloud2(*pcd_cloud, *filecloud); cloud_ = filecloud; } #ifdef NOVIEWER if(!filename_.empty()) get(); else while(TRUE) { if (cloud_) get(); boost::this_thread::sleep (boost::posix_time::microseconds (10000)); } #else while ( !viewer.wasStopped () ) { if (cloud_) { //the call to get() sets the cloud_ to null; viewer.showCloud (get ()); } boost::this_thread::sleep (boost::posix_time::microseconds (10000)); } #endif if(filename_.empty()) { interface->stop (); } }
void planeExtraction(int nPlane) { CloudConstPtr pCloudConst = (CloudConstPtr)pCloud_input; // 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); // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ()); seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.02); // plane segmentation for(int i=0;i<nPlane;i++) { // Segment the largest planar component from the remaining cloud seg.setInputCloud(pCloud_input); 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 (pCloudConst); extract.setIndices (inliers); extract.setNegative (false); // Write the planar inliers to disk CloudPtr cloud_plane; cloud_plane.reset(new Cloud); extract.filter (*cloud_plane); //* // clouds_plane.push_back(cloud_plane); // std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl; // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*pCloud); //* } pCloud_input.reset(new Cloud); pCloud_input = pCloud; pCloud.reset(new Cloud); }
void detection::GraspPointDetector::detect(const CloudPtr &input_object) { clock_t beginCallback = clock(); // If no cloud or no new images since last time, do nothing. if (input_object->size() == 0) return; world_obj_ = input_object->makeShared(); pub_cluster_.publish(world_obj_); // Check if computation succeeded if (doProcessing()) ROS_INFO("[%g ms] Detection Success", durationMillis(beginCallback)); else ROS_ERROR("[%g ms] Detection Failed", durationMillis(beginCallback)); }
void ObjectDetector::processCloud(const CloudConstPtr& cloud) { cloud_pass_through_.reset(new Cloud); // Computation goes here pass_through_.setInputCloud(cloud); pass.filter(*cloud_pass_through_); // Estimate 3d convex hull pcl::ConvexHull<PointType> hr; hr.setInputCloud(cloud_pass_through_); cloud_hull_.reset(new Cloud); hr.reconstruct(*cloud_hull_, vertices_); cloud_ = cloud; new_cloud_ = true; }
//OpenNI Grabber's cloud Callback function void cloud_cb (const CloudConstPtr &cloud) { boost::mutex::scoped_lock lock (mtx_); cloud_pass_.reset (new Cloud); cloud_pass_downsampled_.reset (new Cloud); filterPassThrough (cloud, *cloud_pass_); gridSampleApprox (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_); if(counter < 10){ counter++; }else{ //Track the object tracker_->setInputCloud (cloud_pass_downsampled_); tracker_->compute (); new_cloud_ = true; } }
int main (int argc, char **argv) { pcl::registration::ELCH<PointType> elch; pcl::IterativeClosestPoint<PointType, PointType>::Ptr icp (new pcl::IterativeClosestPoint<PointType, PointType>); icp->setMaximumIterations (100); icp->setMaxCorrespondenceDistance (0.1); icp->setRANSACOutlierRejectionThreshold (0.1); elch.setReg (icp); CloudVector clouds; for (int i = 1; i < argc; i++) { CloudPtr pc (new Cloud); pcl::io::loadPCDFile (argv[i], *pc); clouds.push_back (CloudPair (argv[i], pc)); std::cout << "loading file: " << argv[i] << " size: " << pc->size () << std::endl; elch.addPointCloud (clouds[i-1].second); } int first = 0, last = 0; for (size_t i = 0; i < clouds.size (); i++) { if (loopDetection (int (i), clouds, 3.0, first, last)) { std::cout << "Loop between " << first << " (" << clouds[first].first << ") and " << last << " (" << clouds[last].first << ")" << std::endl; elch.setLoopStart (first); elch.setLoopEnd (last); elch.compute (); } } for (size_t i = 0; i < clouds.size (); i++) { std::string result_filename (clouds[i].first); result_filename = result_filename.substr (result_filename.rfind ("/") + 1); pcl::io::savePCDFileBinary (result_filename.c_str (), *(clouds[i].second)); std::cout << "saving result to " << result_filename << std::endl; } return 0; }
int main (int argc, char **argv) { std::vector<int> pcd_indices; pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd"); pcl::PointCloud<pcl::PointXYZRGB>::Ptr sum_clouds (new pcl::PointCloud<pcl::PointXYZRGB>); CloudVector clouds; for (size_t i = 0; i < pcd_indices.size (); i++) { CloudPtr pc (new Cloud); pcl::io::loadPCDFile (argv[pcd_indices[i]], *pc); std::cout << "loading file: " << argv[pcd_indices[i]] << " size: " << pc->size () << std::endl; *sum_clouds += *pc; } std::string result_filename ("merged"); result_filename += getTimeAsString(); result_filename += ".pcd"; pcl::io::savePCDFileASCII (result_filename.c_str (), *sum_clouds); std::cout << "saving result to " << result_filename << std::endl; return 0; }
void workingspace() { double top = param_workspace_y + param_workspace_height/2; double bottom = param_workspace_y - param_workspace_height/2; double left = param_workspace_x - param_workspace_width/2; double right = param_workspace_x + param_workspace_width/2; double zbottom = param_workspace_z; double ztop = param_workspace_z + param_workspace_zheight; for(int i=0;i<pCloud_input->points.size();i++){ PointT temp_point = pCloud_input->points[i]; // cut off if(temp_point.x<right && temp_point.x>left && temp_point.y<top && temp_point.y>bottom && temp_point.z>zbottom && temp_point.z<ztop) pCloud->points.push_back(temp_point); } pCloud_input.reset(new Cloud); pCloud_input = pCloud; pCloud.reset(new Cloud); workspace.header.frame_id = param_frame_id.data(); workspace.header.stamp = ros::Time::now(); pub_workspace.publish (workspace); }
int Utils::subtractField(const NormalCloudPtr &cloud_in, const CloudPtr &cloud_out) { cloud_out->clear(); for (int i = 0; i < cloud_in->points.size(); i++) { Point pt; pt.x = cloud_in->points[i].x; pt.y = cloud_in->points[i].y; pt.z = cloud_in->points[i].z; pt.r = cloud_in->points[i].r; pt.g = cloud_in->points[i].g; pt.b = cloud_in->points[i].b; pt.a = cloud_in->points[i].a; cloud_out->points.push_back(pt); } return 0; }
/* ---[ */ int main (int argc, char** argv) { if (argc < 3) { std::cerr << "No test file given. Please download `bun0.pcd` and `milk.pcd` pass its path to the test." << std::endl; return (-1); } if (loadPCDFile<PointXYZ> (argv[1], cloud) < 0) { std::cerr << "Failed to read test file. Please download `bun0.pcd` and pass its path to the test." << std::endl; return (-1); } CloudPtr milk_loaded(new PointCloud<PointXYZ>()); if (loadPCDFile<PointXYZ> (argv[2], *milk_loaded) < 0) { std::cerr << "Failed to read test file. Please download `milk.pcd` and pass its path to the test." << std::endl; return (-1); } indices.resize (cloud.points.size ()); for (size_t i = 0; i < indices.size (); ++i) { indices[i] = static_cast<int>(i); } tree.reset (new search::KdTree<PointXYZ> (false)); tree->setInputCloud (cloud.makeShared ()); cloud_milk.reset(new PointCloud<PointXYZ>()); CloudPtr grid; pcl::VoxelGrid < pcl::PointXYZ > grid_; grid_.setInputCloud (milk_loaded); grid_.setLeafSize (leaf_size_, leaf_size_, leaf_size_); grid_.filter (*cloud_milk); tree_milk.reset (new search::KdTree<PointXYZ> (false)); tree_milk->setInputCloud (cloud_milk); testing::InitGoogleTest (&argc, argv); return (RUN_ALL_TESTS ()); }
void FeatureCalculate::rpca_plane(CloudPtr cloud, PlanSegment &plane_rpca, float Pr, float epi,float reliability) { CloudPtr cloud_h(new Cloud); CloudPtr cloud_final(new Cloud); int iteration_number; int h_free; int point_num=cloud->size(); vector<PlanePoint> plane_points; plane_points.resize(point_num); h_free = int(reliability * point_num); iteration_number = compute_iteration_number(Pr, epi, h_free); vector<PlanSegment> planes; for (int i = 0; i < iteration_number; i++) { CloudPtr points(new Cloud); int num[3]; for (int n = 0; n < 3; n++) { num[n] = rand() % point_num; points->push_back(cloud->points[num[n]]); } if (num[0] == num[1] || num[0] == num[2] || num[1] == num[2]) continue; PlanSegment a_plane; pca(points,a_plane); for (int n = 0; n < point_num; n++) { plane_points[n].dis =compute_distance_from_point_to_plane(cloud->points[n],a_plane); plane_points[n].point=cloud->points[n]; } std::sort(plane_points.begin(), plane_points.end(), CmpDis); for (int n = 0; n < h_free; n++) { CloudItem temp_point=plane_points[n].point; cloud_h->points.push_back(temp_point); } pca(cloud_h,a_plane); cloud_h->points.clear(); planes.push_back(a_plane); } sort(planes.begin(), planes.end(), Cmpmin_value); PlanSegment final_plane; final_plane = planes[0]; planes.clear(); final_plane.points_id=plane_rpca.points_id; plane_rpca=final_plane; }
void cb_rgb(const pcl::PCLPointCloud2ConstPtr& input) { static int cnt = 0; // input pCloud_input.reset(new Cloud); pCloud.reset(new Cloud); pcl::fromPCLPointCloud2(*input, *pCloud_input); if(isdebug) cout<<"I heard RGB, # of points: "<<pCloud_input->points.size()<<endl; // downsampling lastT = pcl::getTime (); downsampling(cont_sampling); if(isdebug) cout<<"downsampling computation time : "<<pcl::getTime()-lastT<<endl; if(isdebug) cout<<"downsampling end, # of points: "<<pCloud_input->points.size()<<endl; // transform to the given frame lastT = pcl::getTime (); transform(param_frame_id); if(isdebug) cout<<"transform computation time : "<<pcl::getTime()-lastT<<endl; // workspace lastT = pcl::getTime (); workingspace(); if(isdebug) cout<<"workingspace computation time : "<<pcl::getTime()-lastT<<endl; if(isdebug) cout<<"workingspace end, # of points: "<<pCloud_input->points.size()<<endl; // // planeExtraction // lastT = pcl::getTime (); // planeExtraction(1); // if(isdebug) cout<<"planeExtraction computation time : "<<pcl::getTime()-lastT<<endl; // if(isdebug) cout<<"planeExtraction end, # of points: "<<pCloud_input->points.size()<<endl; // tracking lastT = pcl::getTime (); pctracking->run(pCloud_input); nowT = pcl::getTime (); if(isdebug) cout<<"total tracking computation time : "<<nowT-lastT<<endl; // publish filtered pointcloud pcl::PCLPointCloud2 output_filtered; pcl::toPCLPointCloud2(*pctracking->getFilteredPC(), output_filtered); output_filtered.header.frame_id=param_frame_id.data(); pub_filtered.publish (output_filtered); // publish segmented pointcloud pcl::PCLPointCloud2 output_segmented; pcl::toPCLPointCloud2(*pctracking->getSegmentedPC(), output_segmented); output_segmented.header.frame_id=param_frame_id.data(); pub_segmented.publish (output_segmented); // publish model pointcloud pcl::PointCloud<pcl::PointXYZRGB>::Ptr pCloudOut_model; pCloudOut_model.reset(new pcl::PointCloud<pcl::PointXYZRGB>); pctracking->object_tracks->toPointCloudXYZI_model(*pCloudOut_model); pcl::PCLPointCloud2 output_model; pcl::toPCLPointCloud2(*pCloudOut_model, output_model); output_model.header.frame_id=param_frame_id.data(); pub_objectmodel.publish (output_model); // output tracks pointcloud pcl::PointCloud<pcl::PointXYZRGB>::Ptr pCloudOut; pCloudOut.reset(new pcl::PointCloud<pcl::PointXYZRGB>); pctracking->object_tracks->toPointCloudXYZI(*pCloudOut); // ROS_INFO("# of track points: %d", pCloudOut->points.size()); // publish pointcloud(currentT) of tracks pcl::PCLPointCloud2 output; pcl::toPCLPointCloud2(*pCloudOut, output); output.header.frame_id=param_frame_id.data(); pub_track.publish (output); // // publish lastmodel // pcl::PCLPointCloud2 output_lastmodel; // pcl::toPCLPointCloud2(*pctracking->getLastModel(), output_lastmodel); // output_lastmodel.header.frame_id=param_frame_id.data(); // pub_lastmodel.publish (output_lastmodel); // // publish particles // pcl::PCLPointCloud2 output_particles; // pcl::toPCLPointCloud2(*pctracking->getParticles(), output_particles); // output_particles.header.frame_id=param_frame_id.data(); // pub_particles.publish (output_particles); // publish gaussians pub_gaussians.publish(pctracking->object_tracks->oldGaussians()); pub_gaussians.publish(pctracking->object_tracks->toMarkerGaussians()); // publish gmms // pub_gmms.publish(pctracking->object_tracks->toMarkerGMMs()); // pub_edges.publish(pctracking->object_tracks->toMarkerEdges()); // publish delete id marker of deleted tracks pub_trackID.publish(pctracking->object_tracks->oldMarkerIDs()); pub_trackID.publish(pctracking->object_tracks->toMarkerIDs()); pCloudOut.reset(); cnt++; }
float get_rot_icp_internal(CloudPtr cloud_src, CloudPtr cloud_temp, Matrix4f& mat_rot){ int verbose = 0; bool do_scale = false; bool do_affine = false; bool bulkmode = false; TriMesh::set_verbose(verbose); TriMesh* mesh1 = new TriMesh(); TriMesh* mesh2 = new TriMesh(); mesh1->vertices.resize(cloud_src->size()); mesh2->vertices.resize(cloud_temp->size()); for (int i = 0; i < cloud_src->size(); i++) { mesh1->vertices[i] = point(cloud_src->points[i].x, cloud_src->points[i].y, cloud_src->points[i].z); } for (int i = 0; i < cloud_temp->size(); i++) { mesh2->vertices[i] = point(cloud_temp->points[i].x, cloud_temp->points[i].y, cloud_temp->points[i].z); } xform xf1; xform xf2; KDtree* kd1 = new KDtree(mesh1->vertices); KDtree* kd2 = new KDtree(mesh2->vertices); vector< float> weights1, weights2; if (bulkmode) { float area1 = mesh1->stat(TriMesh::STAT_TOTAL, TriMesh::STAT_FACEAREA); float area2 = mesh2->stat(TriMesh::STAT_TOTAL, TriMesh::STAT_FACEAREA); float overlap_area, overlap_dist; find_overlap(mesh1, mesh2, xf1, xf2, kd1, kd2, overlap_area, overlap_dist); float frac_overlap = overlap_area / min(area1, area2); if (frac_overlap < 0.1f) { TriMesh::eprintf("Insufficient overlap\n"); exit(1); } else { TriMesh::dprintf("%.1f%% overlap\n", frac_overlap * 100.0); } } float err = ICP(mesh1, mesh2, xf1, xf2, kd1, kd2, weights1, weights2, 0, verbose, do_scale, do_affine); if (err >= 0.0f) err = ICP(mesh1, mesh2, xf1, xf2, kd1, kd2, weights1, weights2,0, verbose, do_scale, do_affine); if (err < 0.0f) { TriMesh::eprintf("ICP failed\n"); //exit(1); } //TriMesh::eprintf("ICP succeeded - distance = %f\n", err); delete kd1; delete kd2; delete mesh1; delete mesh2; if (bulkmode) { xform xf12 = inv(xf2) * xf1; for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { mat_rot(i, j) = xf12[i + 4 * j]; } } } else { for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { mat_rot(i, j) = xf2[i + 4 * j]; } } } return err; }
int main (int argc, char** argv) { if (argc < 3) { PCL_WARN("Please set device_id pcd_filename(e.g. $ %s '#1' sample.pcd)\n", argv[0]); exit (1); } //read pcd file target_cloud.reset(new Cloud()); if(pcl::io::loadPCDFile (argv[2], *target_cloud) == -1){ std::cout << "pcd file not found" << std::endl; exit(-1); } std::string device_id = std::string (argv[1]); counter = 0; //Set parameters new_cloud_ = false; downsampling_grid_size_ = 0.002; std::vector<double> default_step_covariance = std::vector<double> (6, 0.015 * 0.015); default_step_covariance[3] *= 40.0; default_step_covariance[4] *= 40.0; default_step_covariance[5] *= 40.0; std::vector<double> initial_noise_covariance = std::vector<double> (6, 0.00001); std::vector<double> default_initial_mean = std::vector<double> (6, 0.0); KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT>::Ptr tracker (new KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT> (8)); ParticleT bin_size; bin_size.x = 0.1f; bin_size.y = 0.1f; bin_size.z = 0.1f; bin_size.roll = 0.1f; bin_size.pitch = 0.1f; bin_size.yaw = 0.1f; //Set all parameters for KLDAdaptiveParticleFilterOMPTracker tracker->setMaximumParticleNum (1000); tracker->setDelta (0.99); tracker->setEpsilon (0.2); tracker->setBinSize (bin_size); //Set all parameters for ParticleFilter tracker_ = tracker; tracker_->setTrans (Eigen::Affine3f::Identity ()); tracker_->setStepNoiseCovariance (default_step_covariance); tracker_->setInitialNoiseCovariance (initial_noise_covariance); tracker_->setInitialNoiseMean (default_initial_mean); tracker_->setIterationNum (1); tracker_->setParticleNum (600); tracker_->setResampleLikelihoodThr(0.00); tracker_->setUseNormal (false); //Setup coherence object for tracking ApproxNearestPairPointCloudCoherence<RefPointType>::Ptr coherence (new ApproxNearestPairPointCloudCoherence<RefPointType>); DistanceCoherence<RefPointType>::Ptr distance_coherence (new DistanceCoherence<RefPointType>); coherence->addPointCoherence (distance_coherence); pcl::search::Octree<RefPointType>::Ptr search (new pcl::search::Octree<RefPointType> (0.01)); coherence->setSearchMethod (search); coherence->setMaximumDistance (0.01); tracker_->setCloudCoherence (coherence); //prepare the model of tracker's target Eigen::Vector4f c; Eigen::Affine3f trans = Eigen::Affine3f::Identity (); CloudPtr transed_ref (new Cloud); CloudPtr transed_ref_downsampled (new Cloud); pcl::compute3DCentroid<RefPointType> (*target_cloud, c); trans.translation ().matrix () = Eigen::Vector3f (c[0], c[1], c[2]); pcl::transformPointCloud<RefPointType> (*target_cloud, *transed_ref, trans.inverse()); gridSampleApprox (transed_ref, *transed_ref_downsampled, downsampling_grid_size_); //set reference model and trans tracker_->setReferenceCloud (transed_ref_downsampled); tracker_->setTrans (trans); //Setup OpenNIGrabber and viewer pcl::visualization::CloudViewer* viewer_ = new pcl::visualization::CloudViewer("PCL OpenNI Tracking Viewer"); pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id); boost::function<void (const CloudConstPtr&)> f = boost::bind (&cloud_cb, _1); interface->registerCallback (f); viewer_->runOnVisualizationThread (boost::bind(&viz_cb, _1), "viz_cb"); //Start viewer and object tracking interface->start(); while (!viewer_->wasStopped ()) std::this_thread::sleep_for(1s); interface->stop(); }
void segment (const PointT &picked_point, int picked_idx, PlanarRegion<PointT> ®ion, PointIndices &indices, CloudPtr &object) { // First frame is segmented using an organized multi plane segmentation approach from points and their normals if (!first_frame_) return; // Estimate normals in the cloud PointCloud<Normal>::Ptr normal_cloud (new PointCloud<Normal>); ne_.setInputCloud (search_.getInputCloud ()); ne_.compute (*normal_cloud); plane_comparator_->setDistanceMap (ne_.getDistanceMap ()); // Segment out all planes mps_.setInputNormals (normal_cloud); mps_.setInputCloud (search_.getInputCloud ()); // Use one of the overloaded segmentAndRefine calls to get all the information that we want out vector<PlanarRegion<PointT> > regions; vector<ModelCoefficients> model_coefficients; vector<PointIndices> inlier_indices; PointCloud<Label>::Ptr labels (new PointCloud<Label>); vector<PointIndices> label_indices; vector<PointIndices> boundary_indices; mps_.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices); PCL_DEBUG ("Number of planar regions detected: %zu for a cloud of %zu points and %zu normals.\n", regions.size (), search_.getInputCloud ()->points.size (), normal_cloud->points.size ()); double max_dist = numeric_limits<double>::max (); // Compute the distances from all the planar regions to the picked point, and select the closest region int idx = -1; for (size_t i = 0; i < regions.size (); ++i) { double dist = pointToPlaneDistance (picked_point, regions[i].getCoefficients ()); if (dist < max_dist) { max_dist = dist; idx = static_cast<int> (i); } } PointIndices::Ptr plane_boundary_indices; // Get the plane that holds the object of interest if (idx != -1) { region = regions[idx]; plane_indices_.reset (new PointIndices (inlier_indices[idx])); plane_boundary_indices.reset (new PointIndices (boundary_indices[idx])); } // Segment the object of interest if (plane_boundary_indices && !plane_boundary_indices->indices.empty ()) { object.reset (new Cloud); segmentObject (picked_idx, search_.getInputCloud (), plane_indices_, plane_boundary_indices, *object); // Save to disk //pcl::io::saveTARPointCloud ("output.ltm", *object, "object.pcd"); } first_frame_ = false; }
int main (int argc, char **argv) { double dist = 2.5; pcl::console::parse_argument (argc, argv, "-d", dist); int iter = 10; pcl::console::parse_argument (argc, argv, "-i", iter); int lumIter = 1; pcl::console::parse_argument (argc, argv, "-l", lumIter); double loopDist = 5.0; pcl::console::parse_argument (argc, argv, "-D", loopDist); int loopCount = 20; pcl::console::parse_argument (argc, argv, "-c", loopCount); pcl::registration::LUM<PointType> lum; lum.setMaxIterations (lumIter); lum.setConvergenceThreshold (0.001f); std::vector<int> pcd_indices; pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd"); CloudVector clouds; for (size_t i = 0; i < pcd_indices.size (); i++) { CloudPtr pc (new Cloud); pcl::io::loadPCDFile (argv[pcd_indices[i]], *pc); clouds.push_back (CloudPair (argv[pcd_indices[i]], pc)); std::cout << "loading file: " << argv[pcd_indices[i]] << " size: " << pc->size () << std::endl; lum.addPointCloud (clouds[i].second); } for (int i = 0; i < iter; i++) { for (size_t i = 1; i < clouds.size (); i++) for (size_t j = 0; j < i; j++) { Eigen::Vector4f ci, cj; pcl::compute3DCentroid (*(clouds[i].second), ci); pcl::compute3DCentroid (*(clouds[j].second), cj); Eigen::Vector4f diff = ci - cj; //std::cout << i << " " << j << " " << diff.norm () << std::endl; if(diff.norm () < loopDist && (i - j == 1 || i - j > loopCount)) { if(i - j > loopCount) std::cout << "add connection between " << i << " (" << clouds[i].first << ") and " << j << " (" << clouds[j].first << ")" << std::endl; pcl::registration::CorrespondenceEstimation<PointType, PointType> ce; ce.setInputTarget (clouds[i].second); ce.setInputSource (clouds[j].second); pcl::CorrespondencesPtr corr (new pcl::Correspondences); ce.determineCorrespondences (*corr, dist); if (corr->size () > 2) lum.setCorrespondences (j, i, corr); } } lum.compute (); for(size_t i = 0; i < lum.getNumVertices (); i++) { //std::cout << i << ": " << lum.getTransformation (i) (0, 3) << " " << lum.getTransformation (i) (1, 3) << " " << lum.getTransformation (i) (2, 3) << std::endl; clouds[i].second = lum.getTransformedCloud (i); } } for(size_t i = 0; i < lum.getNumVertices (); i++) { std::string result_filename (clouds[i].first); result_filename = result_filename.substr (result_filename.rfind ("/") + 1); pcl::io::savePCDFileBinary (result_filename.c_str (), *(clouds[i].second)); //std::cout << "saving result to " << result_filename << std::endl; } return 0; }
void VisMotCoord::getPCScene(CloudPtr &pc_out) { pc_out.reset(new Cloud); *pc_out = *scene.getPCScene(); }
int main(int argc, char *argv[]){ if(argc>1){ CloudPtr cloud (new Cloud); ColorCloudPtr color_cloud (new ColorCloud); if (pcl::io::loadPCDFile(argv[1], *cloud) == -1) //* load the file { PCL_ERROR ("Couldn't read file.\n"); return -1; } for(int i = 0; i<cloud->size(); i++){ Point p1 = cloud->points[i]; ColorPoint p2; p2.x = p1.x; p2.y = p1.y; p2.z = p1.z; p2.r = 0; p2.g = 0; p2.b = 0; color_cloud->push_back(p2); } float centroid[3]; calculateCentroid(cloud,centroid); pcl::PolygonMesh pm; pcl::ConvexHull<ColorPoint> chull; chull.setInputCloud(color_cloud); chull.reconstruct(pm); pcl::fromROSMsg(pm.cloud,*color_cloud); vector<float> distances(color_cloud->size(),999999); for(int i = 0; i<pm.polygons.size(); i++){ int i0 = pm.polygons[i].vertices[0]; int i1 = pm.polygons[i].vertices[1]; int i2 = pm.polygons[i].vertices[2]; ColorPoint p0 = color_cloud->points[i0]; ColorPoint p1 = color_cloud->points[i1]; ColorPoint p2 = color_cloud->points[i2]; Eigen::Vector3f v0; Eigen::Vector3f v1; Eigen::Vector3f v2; v0 << p0.x,p0.y,p0.z; v1 << p1.x,p1.y,p1.z; v2 << p2.x,p2.y,p2.z; Eigen::Vector3f normal = (v1-v0).cross(v2-v0).normalized(); Eigen::Vector3f p; p << centroid[0], centroid[1], centroid[2]; Eigen::Vector3f to_p = p-v0; Eigen::Vector3f projected_p = p-(normal* normal.dot(to_p)); float dist0 = (v1-v0).dot(projected_p-v0)/(v1-v0).norm(); float dist1 = (v2-v0).dot(projected_p-v0)/(v2-v0).norm(); distances[i0] = min(distances[i0],(dist0+dist1)); distances[i1] = min(distances[i1],(dist0+dist1)); distances[i2] = min(distances[i2],(dist0+dist1)); } float max_dist = *max_element(distances.begin(),distances.end()); float thresh = 500; for(int i = 0; i<color_cloud->size(); i++){ ColorPoint* p1 = &color_cloud->points[i]; float color = 255 - distances[i]*(255/max_dist); if(distances[i]>thresh) color = 0; p1->r = color; p1->g = 0; p1->b = 0; } chull.setInputCloud(color_cloud); chull.reconstruct(pm); pcl::io::saveVTKFile("hull.vtk",pm); } }
void detection::GraspPointDetector::setTable(const CloudPtr &table_cloud, const pcl::ModelCoefficientsPtr table_plane) { table_plane_ = boost::make_shared<ModelCoefficients>(*table_plane); table_cloud_ = table_cloud->makeShared(); }
void FeatureCalculate::rpca(CloudPtr cloud,NormalPtr normals, int num_of_neighbors, float Pr, float epi,float reliability) { pcl::KdTreeFLANN<CloudItem> kdtree; kdtree.setInputCloud(cloud); normals->resize(cloud->size()); #pragma omp parallel for for (int j = 0; j < cloud->size(); j++) { CloudPtr cloud_h(new Cloud); CloudPtr cloud_final(new Cloud); vector<int> point_id_search; vector<float> point_distance; int point_num = kdtree.nearestKSearch(j,num_of_neighbors,point_id_search,point_distance); point_distance.clear(); vector<PlanePoint> plane_points; plane_points.resize(point_num); if (point_num > 3) { int iteration_number; int h_free; h_free = int(reliability * point_num); iteration_number = compute_iteration_number(Pr, epi, h_free); vector<PlanSegment> planes; for (int i = 0; i < iteration_number; i++) { CloudPtr points(new Cloud); int num[3]; for (int n = 0; n < 3; n++) { num[n] = rand() % point_num; points->push_back(cloud->points[point_id_search[num[n]]]); } if (num[0] == num[1] || num[0] == num[2] || num[1] == num[2]) continue; PlanSegment a_plane; pca(points,a_plane); for (int n = 0; n < point_num; n++) { plane_points[n].dis =compute_distance_from_point_to_plane(cloud->points[point_id_search[n]],a_plane); plane_points[n].point=cloud->points[point_id_search[n]]; } std::sort(plane_points.begin(), plane_points.end(), CmpDis); for (int n = 0; n < h_free; n++) { CloudItem temp_point=plane_points[n].point; cloud_h->points.push_back(temp_point); } pca(cloud_h,a_plane); cloud_h->points.clear(); planes.push_back(a_plane); } sort(planes.begin(), planes.end(), Cmpmin_value); PlanSegment final_plane; final_plane = planes[0]; planes.clear(); vector<float> distance; vector<float> distance_sort; for (int n = 0; n < point_num; n++) { float dis_from_point_plane; dis_from_point_plane =compute_distance_from_point_to_plane(cloud->points[point_id_search[n]],final_plane); distance.push_back(dis_from_point_plane); distance_sort.push_back(dis_from_point_plane); } sort(distance_sort.begin(), distance_sort.end()); float distance_median; distance_median = distance_sort[point_num / 2]; distance_sort.clear(); float MAD; vector<float> temp_MAD; for (int n = 0; n < point_num; n++) { temp_MAD.push_back(abs(distance[n] - distance_median)); } sort(temp_MAD.begin(), temp_MAD.end()); MAD = 1.4826 * temp_MAD[point_num / 2]; if (MAD == 0) { for (int n = 0; n < point_num; n++) { CloudItem temp_point=cloud->points[point_id_search[n]]; cloud_final->points.push_back(temp_point); } } else { for (int n = 0; n < point_num; n++) { float Rz; Rz = (abs(distance[n] - distance_median)) / MAD; if (Rz < 2.5) { CloudItem temp_point=cloud->points[point_id_search[n]]; cloud_final->points.push_back(temp_point); } } } point_id_search.clear(); if (cloud_final->points.size() > 3) { pca(cloud_final,final_plane); normals->points[j]= final_plane.normal; } else { normals->points[j].normal_x = 0.0; normals->points[j].normal_y = 0.0; normals->points[j].normal_z = 0.0; normals->points[j].curvature = 1.0; } cloud_final->clear(); } else { normals->points[j].normal_x = 0.0; normals->points[j].normal_y = 0.0; normals->points[j].normal_z = 0.0; normals->points[j].curvature = 1.0; } } }