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;
}
Example #2
0
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 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));
}
Example #4
0
File: elch.cpp Project: Bardo91/pcl
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;
}
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;
}
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;
}
Example #7
0
File: lum.cpp Project: 2php/pcl
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 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;
        }    
    }
}
Example #9
0
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);
    }
}
Example #10
0
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;
}