Merge pull request #186 from garaemon/support-time-series-appending-o…

…f-grid-map

Support time series appending of grid map

jsk_pcl_ros/cfg/EnvironmentPlaneModeling.cfg

@@ -17,6 +17,8 @@ gen = ParameterGenerator ()
 
 gen.add("plane_angle_threshold", double_t, 0, "angle threshold", 10.0 / 180.0 * pi, 0.0, pi)
 gen.add("plane_distance_threshold", double_t, 0, "distance threshold", 0.05, 0.0, 2.0)
+gen.add("grid_map_angle_threshold", double_t, 0, "grid map angle threshold", 10.0 / 180.0 * pi, 0.0, pi)
+gen.add("grid_map_distance_threshold", double_t, 0, "grid map distance threshold", 0.05, 0.0, 2.0)
 gen.add("distance_threshold", double_t, 0, "distance threshold to check collision", 0.01, 0.0, 0.1)
 gen.add("collision_check_sampling_d", double_t, 0, "sampling distance of the polygon when checking collision", 0.01, 0.0, 0.5)
 gen.add("resolution_size", double_t, 0, "resolution of occupancy grid", 0.01, 0.0, 0.5)

jsk_pcl_ros/include/jsk_pcl_ros/environment_plane_modeling.h

@@ -167,7 +167,11 @@ namespace jsk_pcl_ros
                             PCLIndicesMsg& output);
     virtual void updateDiagnostic(
       diagnostic_updater::DiagnosticStatusWrapper &stat);
-
+    // for historical_accumulation_
+    virtual int findCorrespondGridMap(
+      const std::vector<float>& coefficients);
+    virtual void registerGridMap(const GridMap::Ptr new_grid_map);
+
 
     boost::mutex mutex_;
 
@@ -219,9 +223,12 @@ namespace jsk_pcl_ros
     // parameters for occlusion
     double plane_distance_threshold_;
     double plane_angle_threshold_;
+    // grid map
+    double grid_map_distance_threshold_;
+    double grid_map_angle_threshold_;
     bool continuous_estimation_;
-
-
+    bool history_accumulation_;
+    std::vector<GridMap::Ptr> grid_maps_;
     TimeAccumulator occlusion_estimate_time_acc_;
     TimeAccumulator grid_building_time_acc_;
     TimeAccumulator kdtree_building_time_acc_;

jsk_pcl_ros/include/jsk_pcl_ros/grid_map.h

@@ -45,6 +45,7 @@
 #include <set>
 #include <Eigen/Geometry>
 
+#include "jsk_pcl_ros/geo_util.h"
 
 namespace jsk_pcl_ros
 {
@@ -80,6 +81,8 @@ namespace jsk_pcl_ros
     // toMsg does not fill header, be carefull
     virtual void originPose(Eigen::Affine3d& output);
     virtual void toMsg(SparseOccupancyGrid& grid);
+    virtual Plane toPlane();
+    virtual std::vector<float> getCoefficients();
   protected:    
     double resolution_;
     Eigen::Vector3f O_;

jsk_pcl_ros/src/environment_plane_modeling_nodelet.cpp

@@ -58,6 +58,7 @@ namespace jsk_pcl_ros
     diagnostic_updater_->add("Modeling Stats", boost::bind(&EnvironmentPlaneModeling::updateDiagnostic,
                                                            this,
                                                            _1));
+    pnh_->param("history_accumulation", history_accumulation_, false);
     // setup publisher
     debug_polygon_pub_
       = pnh_->advertise<geometry_msgs::PolygonStamped>("debug_polygon", 1);
@@ -183,7 +184,15 @@ namespace jsk_pcl_ros
     plane_angle_threshold_ = config.plane_angle_threshold;
     distance_thr_ = config.distance_threshold;
     sampling_d_ = config.collision_check_sampling_d;
-    resolution_size_ = config.resolution_size;
+    if (resolution_size_ != config.resolution_size) {
+      NODELET_INFO_STREAM(
+        "clearing grid maps because of the change of resolution size: "
+        << resolution_size_ << " -> " << config.resolution_size);
+      resolution_size_ = config.resolution_size;
+      grid_maps_.clear();
+    }
+    grid_map_angle_threshold_ = config.grid_map_angle_threshold;
+    grid_map_distance_threshold_ = config.grid_map_distance_threshold;
   }
 
   void EnvironmentPlaneModeling::updateAppendingInfo(
@@ -372,18 +381,25 @@ namespace jsk_pcl_ros
     occlusion_result_coefficients_pub_.publish(coefficients);
   }
 
+  void EnvironmentPlaneModeling::registerGridMap(
+    const GridMap::Ptr new_grid_map)
+  {
+    grid_maps_.push_back(new_grid_map);
+  }
+
   // build the grid map for each planes
   void EnvironmentPlaneModeling::buildGridMap(
     const std::vector<pcl::PointCloud<PointT>::Ptr>& segmented_clouds,
     const PolygonArray::ConstPtr& polygons,
     const ModelCoefficientsArray::ConstPtr& coefficients,
-    std::vector<GridMap::Ptr>& grid_maps)
+    std::vector<GridMap::Ptr>& ordered_grid_maps)
   {
     ScopedTimer timer = grid_building_time_acc_.scopedTimer();
     for (size_t i = 0; i < segmented_clouds.size(); i++) {
       pcl::PointCloud<PointT>::Ptr cloud = segmented_clouds[i];
       // we need to project the point clouds on to the plane
       // because of the noise of the pointclouds.
+      int grid_map_index = findCorrespondGridMap(coefficients->coefficients[i].values);
       pcl::ProjectInliers<PointT> proj;
       proj.setModelType (pcl::SACMODEL_PLANE);
       pcl::ModelCoefficients plane_coefficients;
@@ -392,10 +408,19 @@ namespace jsk_pcl_ros
       pcl::PointCloud<PointT>::Ptr projected_cloud (new pcl::PointCloud<PointT>);
       proj.setInputCloud(cloud);
       proj.filter(*projected_cloud);
-      GridMap::Ptr grid(new GridMap(resolution_size_,
-                                    coefficients->coefficients[i].values));
-      grid->registerPointCloud(projected_cloud);
-      grid_maps.push_back(grid);
+      if (grid_map_index == -1) {
+        GridMap::Ptr grid(new GridMap(resolution_size_,
+                                      coefficients->coefficients[i].values));
+        grid->registerPointCloud(projected_cloud);
+        registerGridMap(grid);
+        ordered_grid_maps.push_back(grid);
+      }
+      else {
+        GridMap::Ptr grid = grid_maps_[grid_map_index];
+        grid->registerPointCloud(projected_cloud);
+        ordered_grid_maps.push_back(grid);
+      }
+
     }
   }
 
@@ -484,7 +509,36 @@ namespace jsk_pcl_ros
       output.push_back(segmented_cloud);
     }
   }
-
+
+  int EnvironmentPlaneModeling::findCorrespondGridMap(
+    const std::vector<float>& coefficients)
+  {
+    Plane new_grid_map(coefficients);
+    int min_index = -1;
+    double min_distance = DBL_MAX;
+    for (size_t i = 0; i < grid_maps_.size(); i++) {
+      Plane grid_plane = grid_maps_[i]->toPlane();
+      double angle = grid_plane.angle(new_grid_map);
+      std::vector<float> grid_coefficients = grid_maps_[i]->getCoefficients();
+      NODELET_INFO("comparing [%f, %f, %f, %f] and [%f, %f, %f, %f]",
+                    coefficients[0], coefficients[1], coefficients[2], coefficients[3],
+                    grid_coefficients[0], grid_coefficients[1], grid_coefficients[2], grid_coefficients[3]);
+      NODELET_INFO_STREAM("  angle: " << angle);
+      if (angle < grid_map_angle_threshold_) {
+        double distance = grid_plane.distance(new_grid_map);
+        NODELET_INFO_STREAM("  distance: " << distance);
+        if (distance < grid_map_distance_threshold_) {
+          if (min_distance > distance) {
+            min_index = i;
+            min_distance = distance;
+          }
+        }
+      }
+    }
+    ROS_INFO_STREAM("min distance is " << min_distance);
+    return min_index;
+  }
+
   bool EnvironmentPlaneModeling::lockCallback(
     EnvironmentLock::Request& req,
     EnvironmentLock::Response& res)
@@ -530,13 +584,18 @@ namespace jsk_pcl_ros
       pcl_cloud,
       processing_input_indices_,
       segmented_clouds);
-
+
+    // if you don't need to have history of grid maps,
+    // clear them.
+    if (!history_accumulation_) { 
+      grid_maps_.clear();
+    }
+    std::vector<GridMap::Ptr> ordered_grid_maps;  
     // first, build grid map
-    std::vector<GridMap::Ptr> grid_maps;
     buildGridMap(segmented_clouds,
                  processing_input_polygons_,
                  processing_input_coefficients_,
-                 grid_maps);
+                 ordered_grid_maps);
 
 
     PolygonArray::Ptr result_polygons (new PolygonArray);
@@ -547,7 +606,7 @@ namespace jsk_pcl_ros
     estimateOcclusion(pcl_cloud,
                       processing_input_indices_,
                       segmented_clouds,
-                      grid_maps,
+                      ordered_grid_maps,
                       processing_input_polygons_,
                       processing_input_coefficients_,
                       processing_static_polygons_,
@@ -556,7 +615,7 @@ namespace jsk_pcl_ros
                       result_coefficients,
                       result_pointcloud,
                       result_indices);
-    publishGridMap(processing_input_->header, grid_maps);
+    publishGridMap(processing_input_->header, grid_maps_);
 
     {
       ScopedTimer timer = kdtree_building_time_acc_.scopedTimer();
@@ -591,7 +650,18 @@ namespace jsk_pcl_ros
       }
       res.environment_id = ++environment_id_;
 
-    }    
+    }
+
+    NODELET_INFO_STREAM("grid maps: " << grid_maps_.size());
+    for (size_t i = 0; i < grid_maps_.size(); i++) {
+      GridMap::Ptr grid = grid_maps_[i];
+      std::vector<float> coefficients = grid->getCoefficients();
+      NODELET_INFO_STREAM("  " << i << ": " <<
+                          coefficients[0] << ", " <<
+                          coefficients[1] << ", " <<
+                          coefficients[2] << ", " <<
+                          coefficients[3]);
+    }
     return true;
   }
 

jsk_pcl_ros/src/geo_util.cpp

@@ -164,7 +164,19 @@ namespace jsk_pcl_ros
 
   double Plane::angle(const Plane& another)
   {
-    return acos(normal_.dot(another.normal_));
+    double dot = normal_.dot(another.normal_);
+    if (dot > 1.0) {
+      dot = 1.0;
+    }
+    else if (dot < -1.0) {
+      dot = -1.0;
+    }
+    double theta = acos(dot);
+    if (theta > M_PI / 2.0) {
+      return M_PI - theta;
+    }
+
+    return acos(dot);
   }
 
   void Plane::project(const Eigen::Vector3d& p, Eigen::Vector3d& output)

jsk_pcl_ros/src/grid_map.cpp

@@ -50,7 +50,7 @@ namespace jsk_pcl_ros
     d_ = coefficients[3];
     if (normal_.norm() != 1.0) {
       d_ = d_ / normal_.norm();
-      normal_ = normal_ / normal_.norm();
+      normal_.normalize();
     }
     O_ = - d_ * normal_;
     // decide ex_ and ey_
@@ -262,5 +262,20 @@ namespace jsk_pcl_ros
       grid.columns.push_back(ros_column);
     }
   }
+
+  Plane GridMap::toPlane()
+  {
+    return Plane(Eigen::Vector3d(normal_[0], normal_[1], normal_[2]), d_);
+  }
+
+  std::vector<float> GridMap::getCoefficients()
+  {
+    std::vector<float> output;
+    output.push_back(normal_[0]);
+    output.push_back(normal_[1]);
+    output.push_back(normal_[2]);
+    output.push_back(d_);
+    return output;
+  }
 
 }

jsk_pcl_ros/src/polygon_array_transformer_nodelet.cpp

@@ -80,7 +80,7 @@ namespace jsk_pcl_ros
     coefficient.values.push_back(b);
     coefficient.values.push_back(c);
     coefficient.values.push_back(d);
-
+    
   }
 
   void PolygonArrayTransformer::transformModelCoefficient(const Eigen::Affine3d& transform,
@@ -91,7 +91,7 @@ namespace jsk_pcl_ros
     n[0] = coefficient.values[0];
     n[1] = coefficient.values[1];
     n[2] = coefficient.values[2];
-    n[3] = 0;
+    n[3] = coefficient.values[3];
     Eigen::Matrix4d m = transform.matrix();
     Eigen::Vector4d n_d = m.transpose() * n;
     Eigen::Vector4d n_dd = n_d.normalized();
@@ -101,7 +101,11 @@ namespace jsk_pcl_ros
     result.values.push_back(n_dd[0]);
     result.values.push_back(n_dd[1]);
     result.values.push_back(n_dd[2]);
-    result.values.push_back(d_dd);
+    //result.values.push_back(d_dd);
+    result.values.push_back(n_dd[3]);
+    NODELET_INFO("[%f, %f, %f, %f] => [%f, %f, %f, %f]",
+                 coefficient.values[0], coefficient.values[1], coefficient.values[2], coefficient.values[3],
+                 result.values[0], result.values[1], result.values[2], result.values[3]);
   }
 
   void PolygonArrayTransformer::transformPolygon(const Eigen::Affine3d& transform,
@@ -176,8 +180,16 @@ namespace jsk_pcl_ros
         }
         else {
           transformPolygon(eigen_transform, polygon, transformed_polygon);
+          //computeCoefficients(transformed_polygon, transformed_coefficient);
+          transformModelCoefficient(eigen_transform, coefficient,
+                                    transformed_coefficient);
+          if (isnan(transformed_coefficient.values[0]) ||
+              isnan(transformed_coefficient.values[1]) ||
+              isnan(transformed_coefficient.values[2]) ||
+              isnan(transformed_coefficient.values[3])) {
+            continue;
+          }
           transformed_polygon_array.polygons.push_back(transformed_polygon);
-          computeCoefficients(transformed_polygon, transformed_coefficient);
           transformed_model_coefficients_array.coefficients.push_back(transformed_coefficient);
         }
       }