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;
}
}
}
