How to fuse Visual SLAM (ORB_SLAM2) with detected obstacle using 2D Lidar and IMU to obtain robot localization?

edit

Hi

I would like to fuse Visual SLAM (ORB_SLAM2) with detected obstacle using 2D Lidar and IMU to obtain robot localization(lateral position and heading angle of the robot). Already successfully detected the obstacles around using the 2D Lidar. So from my code I have the Center X,Y of the obstacle and also the angle. Here the obstacle detection code. Also the scan_matching node gives 2DPose

#include "../include/obstacle_detector.h"

using namespace std;
using namespace obstacle_detector;

ObstacleDetector::ObstacleDetector() : nh_(""), nh_local_("~") {
  updateParams();

  if (p_use_scan_)
    scan_sub_ = nh_.subscribe("/scan", 1, &ObstacleDetector::scanCallback, this);
  else if (p_use_pcl_)
    pcl_sub_ = nh_.subscribe("pcl", 10, &ObstacleDetector::pclCallback, this);

  obstacles_pub_ = nh_.advertise<obstacle_detector::Obstacles>("obstacles", 5);

  ROS_INFO("Obstacle Detector [OK]");
  ros::spin();
}

void ObstacleDetector::updateParams() {
  nh_local_.param<std::string>("world_frame", p_world_frame_, "world");
  nh_local_.param<std::string>("scanner_frame", p_scanner_frame_, "laser_frame");

  nh_local_.param<bool>("use_scan", p_use_scan_, true);
  nh_local_.param<bool>("use_pcl", p_use_pcl_, false);
  nh_local_.param<bool>("transform_to_world", p_transform_to_world, true);
  nh_local_.param<bool>("use_split_and_merge", p_use_split_and_merge_, false);

  nh_local_.param<int>("min_group_points", p_min_group_points_, 3);

  nh_local_.param<double>("max_group_distance", p_max_group_distance_, 0.100);
  nh_local_.param<double>("distance_proportion", p_distance_proportion_, 0.006136);
  nh_local_.param<double>("max_split_distance", p_max_split_distance_, 0.100);
  nh_local_.param<double>("max_merge_separation", p_max_merge_separation_, 0.200);
  nh_local_.param<double>("max_merge_spread", p_max_merge_spread_, 0.070);
  nh_local_.param<double>("max_circle_radius", p_max_circle_radius_, 0.300);
  nh_local_.param<double>("radius_enlargement", p_radius_enlargement_, 0.050);

  nh_local_.param<double>("max_scanner_range", p_max_scanner_range_, 5.0);
  nh_local_.param<double>("max_x_range", p_max_x_range_, 2.0);
  nh_local_.param<double>("min_x_range", p_min_x_range_, -2.0);
  nh_local_.param<double>("max_y_range", p_max_y_range_, 2.0);
  nh_local_.param<double>("min_y_range", p_min_y_range_, -2.0);
}

void ObstacleDetector::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan) {
  initial_points_.clear();

  double phi = scan->angle_min - scan->angle_increment;

  for (const float r : scan->ranges) {
    phi += scan->angle_increment;

    if (r >= scan->range_min && r <= scan->range_max && r <= p_max_scanner_range_)
      initial_points_.push_back(Point::fromPoolarCoords(r, phi));
  }

  processPoints();
}

void ObstacleDetector::pclCallback(const sensor_msgs::PointCloud::ConstPtr& pcl) {
  initial_points_.clear();

  for (const geometry_msgs::Point32& p : pcl->points)
    if (Point(p.x, p.y).lengthSquared() <= pow(p_max_scanner_range_, 2.0))
      initial_points_.push_back(Point(p.x, p.y));

  processPoints();
}

void ObstacleDetector::processPoints() {
  segments_.clear();
  circles_.clear();

  groupPointsAndDetectSegments();
  mergeSegments();

  detectCircles();
  mergeCircles();

  if (p_transform_to_world)
    transformToWorld();

  publishObstacles();
}

void ObstacleDetector::groupPointsAndDetectSegments() {
  list<Point> point_set;

  for (const Point& point : initial_points_) {
    if (point_set.size() != 0) {
      double r = point.length();

      if ((point - point_set.back()).lengthSquared() > pow(p_max_group_distance_ + r * p_distance_proportion_, 2.0)) {
        detectSegments(point_set);
        point_set.clear();
      }
    }
    point_set.push_back(point);
  }

  detectSegments(point_set); // Check the last point set too!
}

void ObstacleDetector::detectSegments(list<Point>& point_set) {
  if (point_set.size() < p_min_group_points_)
    return;

  Segment segment(Point(0.0, 0.0), Point(1.0, 0.0));
  if (p_use_split_and_merge_)
    segment = fitSegment(point_set);
  else // Use Iterative End Point Fit
    segment = Segment(point_set.front(), point_set.back());

  list<Point>::iterator set_divider;
  double max_distance = 0.0;
  double distance     = 0.0;

  // Seek the point of division; omit first and last point of the set
  for (auto point_itr = ++point_set.begin(); point_itr != --point_set.end(); ++point_itr) {
    if ((distance = segment.distanceTo(*point_itr)) >= max_distance) {
      double r = (*point_itr).length();

      if (distance > p_max_split_distance_ + r * p_distance_proportion_) {
        max_distance = distance;
        set_divider = point_itr;
      }
    }
  }

  if (max_distance > 0.0) { // Split the set
    point_set.insert(set_divider, *set_divider);  // Clone the dividing point for each group

    list<Point> subset1, subset2;
    subset1.splice(subset1.begin(), point_set, point_set.begin(), set_divider);
    subset2.splice(subset2.begin(), point_set, set_divider, point_set.end());

    detectSegments(subset1);
    detectSegments(subset2);
  } else ...