From bcc797394a6a311293c554c6f4a37639efd8bdff Mon Sep 17 00:00:00 2001 From: Ryohei Ueda Date: Wed, 2 Jul 2014 22:46:11 +0900 Subject: [PATCH] support appending of GridMap in time series in EnvironmentPlaneModeling --- jsk_pcl_ros/cfg/EnvironmentPlaneModeling.cfg | 2 + .../jsk_pcl_ros/environment_plane_modeling.h | 13 ++- jsk_pcl_ros/include/jsk_pcl_ros/grid_map.h | 3 + .../environment_plane_modeling_nodelet.cpp | 96 ++++++++++++++++--- jsk_pcl_ros/src/geo_util.cpp | 14 ++- jsk_pcl_ros/src/grid_map.cpp | 17 +++- .../src/polygon_array_transformer_nodelet.cpp | 20 +++- 7 files changed, 143 insertions(+), 22 deletions(-) diff --git a/jsk_pcl_ros/cfg/EnvironmentPlaneModeling.cfg b/jsk_pcl_ros/cfg/EnvironmentPlaneModeling.cfg index f416091ac3..6d99542007 100755 --- a/jsk_pcl_ros/cfg/EnvironmentPlaneModeling.cfg +++ b/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) diff --git a/jsk_pcl_ros/include/jsk_pcl_ros/environment_plane_modeling.h b/jsk_pcl_ros/include/jsk_pcl_ros/environment_plane_modeling.h index 64fe9aa1de..45aa66368f 100644 --- a/jsk_pcl_ros/include/jsk_pcl_ros/environment_plane_modeling.h +++ b/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& 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 grid_maps_; TimeAccumulator occlusion_estimate_time_acc_; TimeAccumulator grid_building_time_acc_; TimeAccumulator kdtree_building_time_acc_; diff --git a/jsk_pcl_ros/include/jsk_pcl_ros/grid_map.h b/jsk_pcl_ros/include/jsk_pcl_ros/grid_map.h index 0381e2de33..1617795f69 100644 --- a/jsk_pcl_ros/include/jsk_pcl_ros/grid_map.h +++ b/jsk_pcl_ros/include/jsk_pcl_ros/grid_map.h @@ -45,6 +45,7 @@ #include #include +#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 getCoefficients(); protected: double resolution_; Eigen::Vector3f O_; diff --git a/jsk_pcl_ros/src/environment_plane_modeling_nodelet.cpp b/jsk_pcl_ros/src/environment_plane_modeling_nodelet.cpp index c738729cb6..7c7c4fcaec 100644 --- a/jsk_pcl_ros/src/environment_plane_modeling_nodelet.cpp +++ b/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("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::Ptr>& segmented_clouds, const PolygonArray::ConstPtr& polygons, const ModelCoefficientsArray::ConstPtr& coefficients, - std::vector& grid_maps) + std::vector& ordered_grid_maps) { ScopedTimer timer = grid_building_time_acc_.scopedTimer(); for (size_t i = 0; i < segmented_clouds.size(); i++) { pcl::PointCloud::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 proj; proj.setModelType (pcl::SACMODEL_PLANE); pcl::ModelCoefficients plane_coefficients; @@ -392,10 +408,19 @@ namespace jsk_pcl_ros pcl::PointCloud::Ptr projected_cloud (new pcl::PointCloud); 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& 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 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 ordered_grid_maps; // first, build grid map - std::vector 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 coefficients = grid->getCoefficients(); + NODELET_INFO_STREAM(" " << i << ": " << + coefficients[0] << ", " << + coefficients[1] << ", " << + coefficients[2] << ", " << + coefficients[3]); + } return true; } diff --git a/jsk_pcl_ros/src/geo_util.cpp b/jsk_pcl_ros/src/geo_util.cpp index e810caeb31..d6364d57a6 100644 --- a/jsk_pcl_ros/src/geo_util.cpp +++ b/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) diff --git a/jsk_pcl_ros/src/grid_map.cpp b/jsk_pcl_ros/src/grid_map.cpp index dfb36fe85c..0c667330e7 100644 --- a/jsk_pcl_ros/src/grid_map.cpp +++ b/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 GridMap::getCoefficients() + { + std::vector output; + output.push_back(normal_[0]); + output.push_back(normal_[1]); + output.push_back(normal_[2]); + output.push_back(d_); + return output; + } } diff --git a/jsk_pcl_ros/src/polygon_array_transformer_nodelet.cpp b/jsk_pcl_ros/src/polygon_array_transformer_nodelet.cpp index 7868b9dc41..623ad9a58d 100644 --- a/jsk_pcl_ros/src/polygon_array_transformer_nodelet.cpp +++ b/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); } }