Merge branch 'master' of https://github.com/sunwaylive/proactive-lear…

…ning-on-scenes
sunwaylive · Jan 10, 2015 · 5473d2a · 5473d2a
2 parents 55a9704 + 2c47df4
commit 5473d2a
Show file tree

Hide file tree

Showing 4 changed files with 88 additions and 47 deletions.
diff --git a/...lity-Driven-Poisson-Guided-Autoscanning/Point Cloud/Algorithm/ObjPreSegment/scene_seg.cpp b/...lity-Driven-Poisson-Guided-Autoscanning/Point Cloud/Algorithm/ObjPreSegment/scene_seg.cpp
@@ -847,7 +847,7 @@ void detect_separation_plane(PointCloudPtr_RGB_NORMAL cloud, vector<MyPointCloud
       break;
     }
 
-    if(l<0.7&&w<0.7){
+    if(l<1.4&&w<1.4){
 
       pcl::copyPointCloud(*remained_tem, *cloud_tem);
       PointCloudPtr_RGB_NORMAL plane_cloud_tem (new PointCloud_RGB_NORMAL);
@@ -918,40 +918,65 @@ void detect_separation_plane(PointCloudPtr_RGB_NORMAL cloud, vector<MyPointCloud
 //detect table
 void detect_table(PointCloudPtr_RGB_NORMAL sourceCloud, pcl::ModelCoefficients& plane_coefficients, PointCloudPtr_RGB_NORMAL planeCloud, PointCloudPtr rect_cloud, PointCloudPtr_RGB_NORMAL remainingCloud){
   pcl::ExtractIndices<Point_RGB_NORMAL> extract;// Create the filtering object
+  PointCloudPtr_RGB_NORMAL cloud_tem(new PointCloud_RGB_NORMAL);
+  PointCloudPtr_RGB_NORMAL cloud_p(new PointCloud_RGB_NORMAL);
 
-  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
-  // Create the segmentation object
-  pcl::SACSegmentation<Point_RGB_NORMAL> seg;
-  // Optional
-  seg.setOptimizeCoefficients (true);
-  // Mandatory
-  seg.setModelType (pcl::SACMODEL_PLANE);
-  seg.setMethodType (pcl::SAC_RANSAC);
-  seg.setMaxIterations (1000);
-  seg.setDistanceThreshold (0.015);
+  pcl::copyPointCloud(*sourceCloud, *cloud_tem);
 
+  while(1){
+    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
+    // Create the segmentation object
+    pcl::SACSegmentation<Point_RGB_NORMAL> seg;
+    // Optional
+    seg.setOptimizeCoefficients (true);
+    // Mandatory
+    seg.setModelType (pcl::SACMODEL_PLANE);
+    seg.setMethodType (pcl::SAC_RANSAC);
+    seg.setMaxIterations (1000);
+    seg.setDistanceThreshold (0.015);
 
-  // Segment the largest planar component from the remaining cloud
-  seg.setInputCloud (sourceCloud);
-  seg.segment (*inliers, plane_coefficients);
-  if (inliers->indices.size () == 0)
-  {
-    std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
-    return;
-  }
+    // Segment the largest planar component from the remaining cloud
+    seg.setInputCloud (cloud_tem);
+    seg.segment (*inliers, plane_coefficients);
+    if (inliers->indices.size () == 0)
+    {
+      std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
+      return;
+    }
 
-  // Extract the inliers
-  extract.setInputCloud (sourceCloud);
-  extract.setIndices (inliers);
-  extract.setNegative (false);
-  extract.filter (*planeCloud);
-  std::cerr << "PointCloud representing the planar component: " << planeCloud->width * planeCloud->height << " data points." << std::endl;
+    //showPointCloud(cloud_tem, "cloud_tem");
 
-  getRectForPlaneCloud(planeCloud, boost::make_shared<pcl::ModelCoefficients>(plane_coefficients), rect_cloud);
+    // Extract the inliers
+    extract.setInputCloud (cloud_tem);
+    extract.setIndices (inliers);
+    extract.setNegative (false);
+    extract.filter (*planeCloud);
+    std::cerr << "PointCloud representing the planar component: " << planeCloud->width * planeCloud->height << " data points." << std::endl;
 
-  // Create the filtering object
-  extract.setNegative (true);
-  extract.filter (*remainingCloud);
+    //showPointCloud(planeCloud, "planeCloud");
+
+    getRectForPlaneCloud(planeCloud, boost::make_shared<pcl::ModelCoefficients>(plane_coefficients), rect_cloud);
+
+    // Create the filtering object
+    extract.setNegative (true);
+    extract.filter (*cloud_p);
+    //showPointCloud(cloud_p, "cloud_p");
+
+    pcl::copyPointCloud(*cloud_p, *cloud_tem);
+
+    Eigen::Vector3d normal;
+    normal << plane_coefficients.values[0], plane_coefficients.values[1], plane_coefficients.values[2];
+    normal.normalize();
+
+    if(std::abs(normal.dot(Eigen::Vector3d(0,0,1)))>0.7){
+      break;
+    }
+    else{
+      appendCloud_RGB_NORMAL(planeCloud, remainingCloud);
+    }
+  }
+
+  appendCloud_RGB_NORMAL(cloud_tem, remainingCloud);
 }
 
 //get transform matrix between plane and x_y plane 
@@ -997,7 +1022,8 @@ void getCloudOnTable(PointCloudPtr_RGB_NORMAL cloud, PointCloudPtr rect_cloud, E
       p.normal_y = cloud_tem->at(k).normal_y;
       p.normal_z = cloud_tem->at(k).normal_z;
 
-      if(cloud_tem->at(k).z < rect_cloud_tem->at(0).z){
+      //=========modify the height of tabletop things
+      if(cloud_tem->at(k).z < rect_cloud_tem->at(0).z + 0.015){
         continue;
       }
 
@@ -1171,7 +1197,7 @@ void getCloudOnTable(PointCloudPtr_RGB_NORMAL cloud, PointCloudPtr rect_cloud, E
 //}
 
 //Euclidean Cluster Extraction
-void object_seg_ECE(PointCloudPtr_RGB_NORMAL cloud, std::vector<PointCloudPtr_RGB_NORMAL> &cluster_points){
+void object_seg_ECE(PointCloudPtr_RGB_NORMAL cloud, std::vector<MyPointCloud_RGB_NORMAL> &cluster_points){
   // Creating the KdTree object for the search method of the extraction
   pcl::search::KdTree<Point_RGB_NORMAL>::Ptr tree (new pcl::search::KdTree<Point_RGB_NORMAL>);
   tree->setInputCloud (cloud);
@@ -1192,7 +1218,10 @@ void object_seg_ECE(PointCloudPtr_RGB_NORMAL cloud, std::vector<PointCloudPtr_RG
     for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++){
       cloud_cluster->points.push_back (cloud->points[*pit]); //*
     }
-    cluster_points.push_back(cloud_cluster);
+
+    MyPointCloud_RGB_NORMAL mpc;
+    PointCloud_RGB_NORMAL2MyPointCloud_RGB_NORMAL(cloud_cluster, mpc);
+    cluster_points.push_back(mpc);
 
     j++;
   }
@@ -2332,6 +2361,7 @@ void object_seg_Plane(PointCloudPtr_RGB_NORMAL sourceCloud, std::vector<pcl::Mod
 
 //VCCS over-segmentation
 void VCCS_over_segmentation(PointCloudPtr_RGB_NORMAL cloud, float voxel_resolution,float seed_resolution,float color_importance,float spatial_importance,float normal_importance,vector<MyPointCloud_RGB_NORMAL>& patch_clouds, PointCloudT::Ptr colored_cloud, PointNCloudT::Ptr normal_cloud){
+
   PointCloudT::Ptr ct(new PointCloudT);
   NormalCloudTPtr normals(new NormalCloudT);
 

diff --git a/...uality-Driven-Poisson-Guided-Autoscanning/Point Cloud/Algorithm/ObjPreSegment/scene_seg.h b/...uality-Driven-Poisson-Guided-Autoscanning/Point Cloud/Algorithm/ObjPreSegment/scene_seg.h
@@ -1,9 +1,9 @@
 #ifndef SCENE_SEG_H
 #define SCENE_SEG_H
 
-#include "Algorithm/Common/common_func.h"
-#include "Algorithm/Common/common_type.h"
-#include "Algorithm/Common/visualizer.h"
+#include "../Common/common_func.h"
+#include "../Common/common_type.h"
+#include "../Common/visualizer.h"
 
 #include <iostream>
 
@@ -57,7 +57,7 @@ void getCloudOnTable(PointCloudPtr_RGB_NORMAL cloud, PointCloudPtr rect_cloud, E
 //detect table plane
 void detect_table_plane(PointCloudPtr_RGB_NORMAL sourceCloud, PointCloudPtr_RGB_NORMAL planeCloud, PointCloudPtr_RGB_NORMAL remainCloud);
 //Euclidean Cluster Extraction
-void object_seg_ECE(PointCloudPtr_RGB_NORMAL clound, vector<PointCloudPtr_RGB_NORMAL> &cluster_points);
+void object_seg_ECE(PointCloudPtr_RGB_NORMAL cloud, std::vector<MyPointCloud_RGB_NORMAL> &cluster_points);
 //find a minimum bounding rect
 void find_min_rect(PointCloudPtr_RGB_NORMAL cloud, cv::Point2f &p0,cv::Point2f &p1,cv::Point2f &p2,cv::Point2f &p3);
 //VCCS over-segmentation

diff --git a/...Poisson-Guided-Autoscanning/Point Cloud/Algorithm/ObjPreSegment/supervoxel_clustering.hpp b/...Poisson-Guided-Autoscanning/Point Cloud/Algorithm/ObjPreSegment/supervoxel_clustering.hpp
@@ -333,16 +333,19 @@ template <typename PointT> void
         for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++){
           cloud_cluster->points.push_back (voxels_tem->points[*pit]); //*
         }
-        
+
         if(cloud_cluster->size()<10){
           for(int i=0;i<cloud_cluster->size();i++){
             LeafContainerT* leaf_arg = adjacency_octree_->getLeafContainerAtPoint(cloud_cluster->at(i));
             VoxelData& data_tem = leaf_arg->getData();
-            data_tem.owner_ = 0;
-            data_tem.distance_ = std::numeric_limits<float>::max ();
 
-            sv_itr->removeLeaf(leaf_arg);
-          }
+            if(leaf_arg){
+              data_tem.owner_ = 0;
+              data_tem.distance_ = std::numeric_limits<float>::max ();
+
+              sv_itr->removeLeaf(leaf_arg);
+            }
+            }
         }
       }
     }

diff --git a/...ently-Using-Quality-Driven-Poisson-Guided-Autoscanning/Point Cloud/UI/dlg_camera_para.cpp b/...ently-Using-Quality-Driven-Poisson-Guided-Autoscanning/Point Cloud/UI/dlg_camera_para.cpp
@@ -1609,19 +1609,23 @@ int CameraParaDlg::getUpdateData()
   float normal_importance = 1.0f;
 
   /******************Euclidean Cluster Extraction************************/
-  std::vector<PointCloudPtr_RGB_NORMAL> cluster_points;
+  std::vector<MyPointCloud_RGB_NORMAL> cluster_points;
 
   object_seg_ECE(tabletopCloud, cluster_points);
 
   for(int i=0;i<cluster_points.size();i++){
-    if(cluster_points.at(i)->size()<200){
+    if(cluster_points.at(i).mypoints.size()<200){
       continue;
     }
 
     PointCloudT::Ptr colored_cloud(new PointCloudT);
     vector<MyPointCloud_RGB_NORMAL> patch_clouds;
     PointNCloudT::Ptr normal_cloud(new PointNCloudT);
-    VCCS_over_segmentation(cluster_points.at(i),voxel_resolution,seed_resolution,color_importance,spatial_importance,normal_importance,patch_clouds,colored_cloud,normal_cloud);
+
+    PointCloudPtr_RGB_NORMAL ct(new PointCloud_RGB_NORMAL);
+    MyPointCloud_RGB_NORMAL2PointCloud_RGB_NORMAL(cluster_points.at(i), ct);
+
+    VCCS_over_segmentation(ct,voxel_resolution,seed_resolution,color_importance,spatial_importance,normal_importance,patch_clouds,colored_cloud,normal_cloud);
 
 		std::stringstream str;
 		str<<"colored_voxel_cloud"<<i;
@@ -1700,19 +1704,23 @@ void CameraParaDlg::runOverSegmentation()
   float normal_importance = 1.0f;
 
   /******************Euclidean Cluster Extraction************************/
-  std::vector<PointCloudPtr_RGB_NORMAL> cluster_points;
+  std::vector<MyPointCloud_RGB_NORMAL> cluster_points;
 
   object_seg_ECE(tabletopCloud, cluster_points);
 
   for(int i=0;i<cluster_points.size();i++){
-    if(cluster_points.at(i)->size()<200){
+    if(cluster_points.at(i).mypoints.size()<200){
       continue;
     }
 
     PointCloudT::Ptr colored_cloud(new PointCloudT);
     vector<MyPointCloud_RGB_NORMAL> patch_clouds;
     PointNCloudT::Ptr normal_cloud(new PointNCloudT);
-    VCCS_over_segmentation(cluster_points.at(i),voxel_resolution,seed_resolution,color_importance,spatial_importance,normal_importance,patch_clouds,colored_cloud,normal_cloud);
+
+    PointCloudPtr_RGB_NORMAL ct(new PointCloud_RGB_NORMAL);
+    MyPointCloud_RGB_NORMAL2PointCloud_RGB_NORMAL(cluster_points.at(i), ct);
+
+    VCCS_over_segmentation(ct,voxel_resolution,seed_resolution,color_importance,spatial_importance,normal_importance,patch_clouds,colored_cloud,normal_cloud);
 
     std::stringstream str;
     str<<"colored_voxel_cloud"<<i;