Implement Semantic SLAM

[eborghi10]

Describe the solution you'd like

Implement a Semantic Segmentation application to detect a free path, for example, to navigate using a camera.

Semantic mapping can be defined as the process of building a representation of the environment, incorporating semantic knowledge obtained from sensory information. Semantic properties can be extracted from various sources such as objects, topology of the environment, size and shape of rooms and room appearance.

Working branch

Describe alternatives you've considered

• ros-semantic-segmentation

• semantic_slam

• SimVODIS

• ros_object_analytics

• orb-slam2_with_semantic_labelling

• bom

• Object Detection and Tracking

• apc-vision-toolbox

• bonnet

• coral_usb_ros --> Docker

• semantic_3d_mapping

• FCNN for road detection

• DS-SLAM

Additional context

An overview of semantic image segmentation.

Semantic Mapping in ROS by Xavier Gallart Del Burgo

Kimera: an Open-Source Library for Real-Time Metric-Semantic Localization and Mapping

[Lobotuerk]

PLUGINLIB_EXPORT_CLASS(ugv_filter::UGVFilter, costmap_2d::Layer)

namespace ugv_filter {

UGVFilter::UGVFilter():
  tfListener_(this->tfBuffer_)
{};

void UGVFilter::onInitialize(){
  current_ = true;
  this->nh_  = ros::NodeHandle("~/" + name_);
  std::string nodeName = ros::this_node::getName();
  // this->nh_.param<float>(nodeName + "/flat_height", this->flat_height_, -0.25);
  this->nh_.param<std::string>(nodeName + "/flat_frame", this->flat_frame_, "map");
  this->nh_.param<std::string>(nodeName + "/name", this->name_, "X1");
  // this->nh_.param<std::string>(nodeName + "/new_name", this->new_name_, "fake");
  // this->nh_.param<bool>(nodeName + "/delete", this->delete_, true);
  // this->nh_.param<float>(nodeName + "/slope", this->slope_threshold_, 1.5);
  // ROS_INFO("UGV Filter Ready");
  grid_map::GridMap map({"original", "elevation", "slope"});
  map.setFrameId("X1/map");
  map.setGeometry(grid_map::Length(10.0, 10.0), 0.05);
  ROS_INFO("Created map with size %f x %f m (%i x %i cells). Frame in position %f x %f",
  map.getLength().x(), map.getLength().y(),
  map.getSize()(0), map.getSize()(1),
  map.getPosition()(0), map.getPosition()(1));
  this->map_ = map;
  this->pc2Sub_ = this->nh_.subscribe("/X1/points", 1, &UGVFilter::filterPC2, this);
  this->pc2Pub_ = this->nh_.advertise<grid_map_msgs::GridMap>("output", 1, true);
  // this->map_["original"].setConstant(0.0);
  // ros::Time time = ros::Time::now();
  // this->map_.setTimestamp(time.toNSec());
  // grid_map_msgs::GridMap message;
  // grid_map::GridMapRosConverter::toMessage(this->map_, message);
  // this->pc2Pub_.publish(message);
};

void UGVFilter::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y)
{
  // ROS_INFO_STREAM("x: " << robot_x << ", y: " << robot_y << " min:" << *min_x << "," << *min_y << " max:" << *max_x << "," << *max_y);
  this->map_.move(grid_map::Position(robot_x, robot_y));
  mark_x_ = robot_x;
  mark_y_ = robot_y;

  *min_x = std::min(*min_x, mark_x_ - getSizeInMetersX() / 2);
  *min_y = std::min(*min_y, mark_y_ - getSizeInMetersX() / 2);
  *max_x = std::max(*max_x, mark_x_ + getSizeInMetersX() / 2);
  *max_y = std::max(*max_y, mark_y_ + getSizeInMetersX() / 2);
}

void UGVFilter::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
  for (grid_map::GridMapIterator it(this->map_); !it.isPastEnd(); ++it) {
    const grid_map::Index gridmapIndex(*it);
    grid_map::Position position;
    this->map_.getPosition(gridmapIndex, position);
    double px = position.x();
    double py = position.y();
    // Now we need to compute the map coordinates for the observation.
    unsigned int mx, my;
    if (!master_grid.worldToMap(px, py, mx, my))  // If point outside of local costmap, ignore.
    {
      continue;
    }

    if (this->map_.at("elevation", gridmapIndex) > 1)
    {
      master_grid.setCost(mx, my, 254);
    }
    else
    {
      master_grid.setCost(mx, my, 0);
    }
  }
  updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
}

void UGVFilter::filterPC2(const sensor_msgs::PointCloud2::ConstPtr & pc){
  pcl::PointCloud<pcl::PointXYZ>::Ptr transformedbody(new pcl::PointCloud<pcl::PointXYZ>);
  sensor_msgs::PointCloud2 result;
  geometry_msgs::TransformStamped transformStamped;
  try {
    transformStamped = this->tfBuffer_.lookupTransform(this->name_ + "/" + this->flat_frame_, pc->header.frame_id, ros::Time(0), ros::Duration(5.0));
    tf2::doTransform(*pc, result, transformStamped);
  }
  catch (tf2::TransformException &ex) {
    ROS_WARN("%s",ex.what());
    ros::Duration(1.0).sleep();
    return;
  }
  pcl::fromROSMsg(result, *transformedbody);
  for (size_t i = 0; i < transformedbody->size(); i++) {
    pcl::PointXYZ transformedpoint = transformedbody->at(i);
    float minX = -this->map_.getLength().x()/2 + mark_x_;
    float maxX = this->map_.getLength().x()/2 + mark_x_;
    float minY = -this->map_.getLength().y()/2 + mark_y_;
    float maxY = this->map_.getLength().y()/2 + mark_y_;
    double position_x = transformedpoint.x + mark_x_;
    double position_y = transformedpoint.y + mark_y_;
    if (position_x <= maxX && position_x >= minX && position_y <= maxY  && position_y >= minY )
    {
      grid_map::Position position(position_x, position_y);
      grid_map::Index gridmapIndex;
      this->map_.getIndex(position, gridmapIndex);
      if (std::isnan(this->map_.at("original", gridmapIndex)) ||  std::abs(this->map_.at("original", gridmapIndex)) < std::abs(transformedpoint.z))
      {
        this->map_.at("original", gridmapIndex) = transformedpoint.z;
        this->map_.at("elevation", gridmapIndex) = transformedpoint.z;
      }
    }
  }

  ROS_INFO("INTERPOLATING MAP");
  for (grid_map::GridMapIterator it(this->map_); !it.isPastEnd(); ++it) {
    if (std::isnan(this->map_.at("elevation", *it)))
    {
      const grid_map::Index index(*it);
      grid_map::Position position;
      this->map_.getPosition(index, position);
      int points = 0;
      double height = 0;
      double distances = 0;
      int n_points = 3;
      float radius = 1;
      while (points < n_points)
      {
        int x,y;
        double resolution = this->map_.getResolution();
        double min_x = -this->map_.getLength().x()/2 + mark_x_;
        double max_x = this->map_.getLength().x()/2 + mark_x_;
        double min_y = -this->map_.getLength().y()/2 + mark_y_;
        double max_y = this->map_.getLength().y()/2 + mark_y_;
        min_x = std::max(position(0) - radius * resolution, min_x);
        min_y = std::max(position(1) - radius * resolution, min_y);
        max_x = std::min(position(0) + radius * resolution, max_x);
        max_y = std::min(position(1) + radius * resolution, max_y);
        grid_map::Index index_max, index_min;
        this->map_.getIndex(grid_map::Position(max_x, max_y), index_max);
        this->map_.getIndex(grid_map::Position(min_x, min_y), index_min);
        x = index_max.x();
        y = index.y();
        while(x <= index.x() && y <= index_min.y())
        {
          grid_map::Index temp_index(x,y);
          double temp_height = this->map_.at("original", temp_index);
          grid_map::Position temp_position;
          this->map_.getPosition(temp_index, temp_position);
          if (!(std::isnan(temp_height)))
          {
            points++;
            double temp_distance = sqrt(pow(position(0) - temp_position(0), 2.0) + pow(position(1) - temp_position(1), 2.0));
            temp_distance = pow(temp_distance, -n_points);
            height += temp_height * temp_distance;
            distances += temp_distance;
            if (points >= n_points && distances != 0.0)
            {
              this->map_.at("elevation", index) = height / distances;
            }
          }
          x++;
          y++;
        }
        x = index.x();
        y = index_min.y();
        while(x <= index_min.x() && y >= index.y())
        {
          grid_map::Index temp_index(x,y);
          double temp_height = this->map_.at("original", temp_index);
          grid_map::Position temp_position;
          this->map_.getPosition(temp_index, temp_position);
          if (!(std::isnan(temp_height)))
          {
            points++;
            double temp_distance = sqrt(pow(position(0) - temp_position(0), 2.0) + pow(position(1) - temp_position(1), 2.0));
            temp_distance = pow(temp_distance, -n_points);
            height += temp_height * temp_distance;
            distances += temp_distance;
            if (points >= n_points && distances != 0.0)
            {
              this->map_.at("elevation", index) = height / distances;
            }
          }
          x++;
          y--;
        }
        x = index_min.x();
        y = index.y();
        while(x >= index.x() && y >= index_max.y())
        {
          grid_map::Index temp_index(x,y);
          double temp_height = this->map_.at("original", temp_index);
          grid_map::Position temp_position;
          this->map_.getPosition(temp_index, temp_position);
          if (!(std::isnan(temp_height)))
          {
            points++;
            double temp_distance = sqrt(pow(position(0) - temp_position(0), 2.0) + pow(position(1) - temp_position(1), 2.0));
            temp_distance = pow(temp_distance, -n_points);
            height += temp_height * temp_distance;
            distances += temp_distance;
            if (points >= n_points && distances != 0.0)
            {
              this->map_.at("elevation", index) = height / distances;
            }
          }
          x--;
          y--;
        }
        x = index.x();
        y = index_max.y();
        while(x >= index_max.x() && y <= index.y())
        {
          grid_map::Index temp_index(x,y);
          double temp_height = this->map_.at("original", temp_index);
          grid_map::Position temp_position;
          this->map_.getPosition(temp_index, temp_position);
          if (!(std::isnan(temp_height)))
          {
            points++;
            double temp_distance = sqrt(pow(position(0) - temp_position(0), 2.0) + pow(position(1) - temp_position(1), 2.0));
            temp_distance = pow(temp_distance, -n_points);
            height += temp_height * temp_distance;
            distances += temp_distance;
            if (points >= n_points && distances != 0.0)
            {
              this->map_.at("elevation", index) = height / distances;
            }
          }
          x--;
          y++;
        }
        radius += 1.0;
        break;
      }
    }
    break;
  }
  ROS_INFO("MAP INTERPOLATED");
  // Publish grid map.
  ros::Time time = ros::Time::now();
  this->map_.setTimestamp(time.toNSec());
  grid_map_msgs::GridMap message;
  grid_map::GridMapRosConverter::toMessage(this->map_, message);
  this->pc2Pub_.publish(message);
};

[hidmic]

@eborghi10 I see you've already prototype'd a branch using semantic_slam. Following its architecture, I think we should be able to split this problem in 3D SLAM using ORB-SLAM or RTAB-Map, 3D representation using OctoMap or Voxblox, and semantic segmentation using the same trained Torch CNNs or the more recent mseg-dataset/mseg-semantic (with a few tweaks here and there).

With that in mind, I propose we:

• [ ] Bring that prototype branch back to life
• [ ] Reconfigure ORB-SLAM2 (allegedly better than RTAB-Map, see benchmark)
  • [ ] Use RGB-D camera and lidar
• [ ] Refactor semantic_cloud
  • [ ] Use published camera info instead of parameters
  • [ ] Decouple depth image processing from segmentation
  • [ ] Enable mseg-dataset/mseg-semantic usage.
  • [ ] Publish annotated objects' centroids.

OctoMap and Voxblox are neat 3D representations but overkill for 2D navigation. Object detection, on the other hand, would allow for semantic navigation commands.

FYI @ivanpauno @Blast545 @rjcausarano