Commit 3767497e authored by Niklas Yann Wettengel's avatar Niklas Yann Wettengel
Browse files

depth_occupancy_map min_distance parameter

parent fa153573
......@@ -133,10 +133,12 @@ void depth_occupancy_map::updateDepthData(const sensor_msgs::PointCloud2::ConstP
pcl_ros::transformPointCloud(cloud_in, cloud, transform);
float min_distance = 0.4;
ros::param::getCached("/depth_mapping/min_distance", min_distance);
const float max_range_z = 1.6;
const float min_range_z = 0.3;
const float max_distance = MAX_DISTANCE * MAX_DISTANCE;
const float min_distance = 0.2;
const float min_distance_squared = min_distance * min_distance;
geometry_msgs::Point point;
std::map<std::pair<int,int>,std::vector<float>> points_2d;
......@@ -150,7 +152,7 @@ void depth_occupancy_map::updateDepthData(const sensor_msgs::PointCloud2::ConstP
point.x = cloud.points[i].x;
point.y = cloud.points[i].y;
const float distance = (point.x - robot.x) * (point.x - robot.x) + (point.y - robot.y) * (point.y - robot.y);
if (distance < max_distance && distance > min_distance)
if (distance < max_distance && distance > min_distance_squared)
{
auto sensor_map_point = map_tools::toMapCoords(point, m_map.info.origin, m_map.info.resolution);
points_2d[std::make_pair<int,int>((int)(sensor_map_point.x()), (int)(sensor_map_point.y()))].push_back(cloud.points[i].z);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment