text: // License: Apache 2.0. See LICENSE file in root directory.
  // Copyright(c) 2017 Intel Corporation. All Rights Reserved
  
  #include "realsense_ros_slam/slam_nodelet.h"
  #include <tf2/convert.h>
  #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
  #include <tf2/LinearMath/Matrix3x3.h>
  #include <tf2/LinearMath/Vector3.h>
  #include <geometry_msgs/PoseStamped.h>
  #include <nav_msgs/Odometry.h>
  #include <realsense_ros_slam/TrackingAccuracy.h>
  #include <geometry_msgs/TransformStamped.h>
  #include <tf2_ros/transform_listener.h>
  #include <tf2_ros/buffer.h>
  #include <tf/transform_broadcaster.h>
  #include <boost/make_unique.hpp>
  
  PLUGINLIB_EXPORT_CLASS(realsense_ros_slam::SNodeletSlam, nodelet::Nodelet)
  
  uint64_t seq_depth = 0, seq_fisheye = 0;
  cv::Mat image_depth[100], image_fisheye[100];
  uint64_t cpt_depth = 0, cpt_fisheye = 0;
  rs::core::image_info info_fisheye[100], info_depth[100];
  
  std::string pkgpath;
  std::string relocalization_filename_in, occupancy_filename_in;
  std::string trajectory_filename_out, relocalization_filename_out, occupancyFilenameOut;
  std::string topic_camera_pose, topic_reloc_pose, topic_pose2d, topic_map, topic_tracking_accuracy, topic_odom;
  double map_resolution, hoi_min, hoi_max, doi_min, doi_max;
  bool is_pub_odom = false;
  
  ros::Publisher pub_pose2d, pub_pose, pub_map, pub_accuracy, pub_reloc, pub_odom;
  geometry_msgs::Pose2D pose2d;
  
  IplImage * ipNavMap = NULL;
  std::vector< stRobotPG > g_robotPGStack;
  std::vector< CvPoint > g_relocalizationPointStack;
  
  ros::ServiceServer reset_srv, save_output_srv;
  
  namespace realsense_ros_slam
  {
  
  SubscribeTopics::SubscribeTopics()
  {
  }
  
  
  SubscribeTopics::~SubscribeTopics()
  {
  }
  
  
  void SubscribeTopics::onInit(ros::NodeHandle & nh, rs::slam::slam * slam)
  {
    l_nh = nh;
    l_slam = slam;
  }
  
  void SubscribeTopics::subscribeStreamMessages()
  {
    std::string depthImageStream =  "camera/depth/image_raw";
    std::string fisheyeImageStream =  "camera/fisheye/image_raw";
    ROS_INFO_STREAM("Listening on " << depthImageStream);
    ROS_INFO_STREAM("Listening on " << fisheyeImageStream);
    l_depth_sub = l_nh.subscribe(depthImageStream, 100, & SubscribeTopics::depthMessageCallback, this);
    l_fisheye_sub = l_nh.subscribe(fisheyeImageStream, 100, & SubscribeTopics::fisheyeMessageCallback, this);
  }
  
  void SubscribeTopics::subscribeMotion()
  {
    std::string motionInfo_gyro = "camera/gyro/sample";
    std::string motionInfo_accel = "camera/accel/sample";
    ROS_INFO_STREAM("Listening on " << motionInfo_gyro);
    ROS_INFO_STREAM("Listening on " << motionInfo_accel);
    l_motion_gyro_sub = l_nh.subscribe(motionInfo_gyro, 10000, & SubscribeTopics::motionGyroCallback, this);
    l_motion_accel_sub = l_nh.subscribe(motionInfo_accel, 10000, & SubscribeTopics::motionAccelCallback, this);
  }
  
  void SubscribeTopics::depthMessageCallback(const sensor_msgs::ImageConstPtr &depthImageMsg)
  {
    mut_depth.lock();
    SubscribeTopics::getStreamSample(depthImageMsg, rs::core::stream_type::depth);
    mut_depth.unlock();
  }
  
  
  void SubscribeTopics::fisheyeMessageCallback(const sensor_msgs::ImageConstPtr &fisheyeImageMsg)
  {
    mut_fisheye.lock();
    SubscribeTopics::getStreamSample(fisheyeImageMsg, rs::core::stream_type::fisheye);
    mut_fisheye.unlock();
  }
  
  void SubscribeTopics::getStreamSample(const sensor_msgs::ImageConstPtr &imageMsg, rs::core::stream_type stream)
  {
    int width = imageMsg->width;
    int height = imageMsg->height;
    rs::core::correlated_sample_set sample_set = {};
    if (stream == rs::core::stream_type::fisheye)
    {
      info_fisheye[cpt_fisheye] =
      {
        width,
        height,
        rs::utils::convert_pixel_format(rs::format::raw8),
        1 * width
      };
      image_fisheye[cpt_fisheye] = cv::Mat(height, width, CV_8UC1, (unsigned char*)imageMsg->data.data()).clone();
      sample_set[stream] = rs::core::image_interface::create_instance_from_raw_data(
                             & info_fisheye[cpt_fisheye],
                             image_fisheye[cpt_fisheye].data,
                             stream,
                             rs::core::image_interface::flag::any,
                             /* ms */imageMsg->header.stamp.toSec() * /* seconds to ms */1000,
                             (uint64_t)imageMsg->header.seq,
                             rs::core::timestamp_domain::microcontroller
                           );
      if (cpt_fisheye < 99)
      {
        cpt_fisheye++;
      }
      else
      {
        cpt_fisheye = 0;
      }
    }
    else
    {
      info_depth[cpt_depth] =
      {
        width,
        height,
        rs::utils::convert_pixel_format(rs::format::z16),
        2 * width
      };
      image_depth[cpt_depth] = cv::Mat(height, width, CV_16UC1, (unsigned char*)imageMsg->data.data()).clone();
      sample_set[stream] = rs::core::image_interface::create_instance_from_raw_data(
                             & info_depth[cpt_depth],
                             image_depth[cpt_depth].data,
                             stream,
                             rs::core::image_interface::flag::any,
                             /* ms */ imageMsg->header.stamp.toSec() * /* seconds to ms */1000,
                             (uint64_t)imageMsg->header.seq,
                             rs::core::timestamp_domain::microcontroller
                           );
      if (cpt_depth < 99)
      {
        cpt_depth++;
      }
      else
      {
        cpt_depth = 0;
      }
    }
    if (l_slam->process_sample_set(sample_set) < rs::core::status_no_error)
    {
      ROS_ERROR("error: failed to process sample");
    }
    sample_set[stream]->release();
  }//end of getStreamSample
  
  
  void SubscribeTopics::motionGyroCallback(const sensor_msgs::ImuConstPtr &imuMsg)
  {
    mut_gyro_imu.lock();
    SubscribeTopics::getMotionSample(imuMsg, rs::core::motion_type::gyro);
    mut_gyro_imu.unlock();
  }
  
  
  void SubscribeTopics::motionAccelCallback(const sensor_msgs::ImuConstPtr &imuMsg)
  {
    mut_accel_imu.lock();
    SubscribeTopics::getMotionSample(imuMsg, rs::core::motion_type::accel);
    mut_accel_imu.unlock();
  }
  
  
  void SubscribeTopics::getMotionSample(const sensor_msgs::ImuConstPtr &imuMsg, rs::core::motion_type motionType)
  {
    rs::core::correlated_sample_set sample_set = {};
    sample_set[motionType].timestamp /* ms */ = imuMsg->header.stamp.toSec() * /* seconds to ms */1000;
    sample_set[motionType].type = motionType;
    sample_set[motionType].frame_number = imuMsg->header.seq;
    if (motionType == rs::core::motion_type::accel)
    {
      sample_set[motionType].data[0] = (float)imuMsg->linear_acceleration.x;
      sample_set[motionType].data[1] = (float)imuMsg->linear_acceleration.y;
      sample_set[motionType].data[2] = (float)imuMsg->linear_acceleration.z;
    }
    else if (motionType == rs::core::motion_type::gyro)
    {
      sample_set[motionType].data[0] = (float)imuMsg->angular_velocity.x;
      sample_set[motionType].data[1] = (float)imuMsg->angular_velocity.y;
      sample_set[motionType].data[2] = (float)imuMsg->angular_velocity.z;
    }
  
    if (l_slam->process_sample_set(sample_set) < rs::core::status_no_error)
    {
      ROS_ERROR("error: failed to process sample");
    }
  }
  
  
  class slam_tracking_event_handler : public rs::slam::tracking_event_handler
  {
  public:
    slam_tracking_event_handler()
    {
      ROS_INFO("tracking_event_handler");
    }
  
  
    void on_restart()
    {
      ROS_INFO("Restart--------------------------------");
      fflush(stdout);
    }
  
  
    ~slam_tracking_event_handler()
    {
      ROS_INFO("deconstructure");
    }
  };
  
  
  void convertToPG(rs::slam::PoseMatrix4f & pose, Eigen::Vector3f & gravity, stRobotPG & robotPG)
  {
    Eigen::Vector3f g = gravity;
    Eigen::Vector3f g_startG(0.0f, 1.0f, 0.0f);
    Eigen::Vector3f x3D(1.0f, 0.0f, 0.0f);
    Eigen::Vector3f y3D(0.0f, 1.0f, 0.0f);
    Eigen::Vector3f z3D(0.0f, 0.0f, 1.0f);
    Eigen::Vector3f x3D_(pose.at(0, 0), pose.at(0, 1), pose.at(0, 2));
    Eigen::Vector3f y3D_(pose.at(1, 0), pose.at(1, 1), pose.at(1, 2));
    Eigen::Vector3f z3D_(pose.at(2, 0), pose.at(2, 1), pose.at(2, 2));
    Eigen::Vector3f z3DTo_(z3D.dot(x3D_), z3D.dot(y3D_), z3D.dot(z3D_));
    Eigen::Vector3f ProjX = g.cross(z3D);
    Eigen::Vector3f ProjY = ProjX.cross(g);
    Eigen::Vector2f v(z3DTo_.dot(ProjX), z3DTo_.dot(ProjY));
    Eigen::Vector2f v_(z3D.dot(ProjX), z3D.dot(ProjY));
    float theta1 = atan2(v.x(), v.y());
    float theta2 = atan2(v_.x(), v_.y());
    float theta = theta2 - theta1;
    theta += CV_PI / 2; // Needed to point the right direction
    if (theta < 0)
    {
      theta += 2 * CV_PI;
    }
    else if (theta >= 2 * CV_PI)
    {
      theta -= 2 * CV_PI;
    }
    Eigen::Vector3f origin_(pose.at(0, 3), pose.at(1, 3), pose.at(2, 3));
    Eigen::Vector3f ProjX0 = g_startG.cross(z3D);
    Eigen::Vector3f ProjY0 = ProjX0.cross(g_startG);
    float x = origin_.dot(ProjX0);
    float y = origin_.dot(ProjY0);
    robotPG.initdefault();
    robotPG.theta = theta;
    robotPG.x = x;
    robotPG.y = y;
  }
  
  
  void extrinsicsMsgToRSExtrinsics(const realsense_ros_camera::ExtrinsicsConstPtr &extrinsicsMsg, rs::core::extrinsics &rsExtrinsics)
  {
    for (int i = 0; i < 9; ++i)
    {
      rsExtrinsics.rotation[i] = extrinsicsMsg->rotation[i];
      if (i < 3) rsExtrinsics.translation[i] = extrinsicsMsg->translation[i];
    }
  }
  
  void imuInfoMsgToRSImuIntrinsics(const realsense_ros_camera::IMUInfoConstPtr &imuInfoMsg, rs::core::motion_device_intrinsics &rsImuIntrinsics)
  {
    int index = 0;
    for (int i = 0; i < 3; ++i)
    {
      for (int j = 0; j < 4; ++j)
      {
        rsImuIntrinsics.data[i][j] = imuInfoMsg->data[index];
        ++index;
      }
      rsImuIntrinsics.noise_variances[i] = imuInfoMsg->noise_variances[i];
      rsImuIntrinsics.bias_variances[i] = imuInfoMsg->bias_variances[i];
    }
  }
  
  tf2::Quaternion quaternionFromPoseMatrix(rs::slam::PoseMatrix4f cameraPose)
  {
    tf2::Matrix3x3 rotMat = tf2::Matrix3x3(
                              cameraPose.at(0, 0),
                              cameraPose.at(0, 1),
                              cameraPose.at(0, 2),
                              cameraPose.at(1, 0),
                              cameraPose.at(1, 1),
                              cameraPose.at(1, 2),
                              cameraPose.at(2, 0),
                              cameraPose.at(2, 1),
                              cameraPose.at(2, 2)
                            );
  
    tf2::Quaternion quat;
    rotMat.getRotation(quat);
    return quat;
  }
  
  geometry_msgs::Pose poseMatrixToMsg(rs::slam::PoseMatrix4f camera_pose)
  {
    tf2::Quaternion quat = quaternionFromPoseMatrix(camera_pose);
    geometry_msgs::Quaternion quat_msg;
    tf2::convert<tf2::Quaternion, geometry_msgs::Quaternion>(quat, quat_msg);
  
    geometry_msgs::Point point_msg;
    point_msg.x = camera_pose.at(0, 3);
    point_msg.y = camera_pose.at(1, 3);
    point_msg.z = camera_pose.at(2, 3);
  
    geometry_msgs::Pose pose_msg;
    pose_msg.orientation = quat_msg;
    pose_msg.position = point_msg;
    return pose_msg;
  }
  
  geometry_msgs::PoseStamped getPoseStampedMsg(rs::slam::PoseMatrix4f cameraPose, uint64_t frameNum, double timestamp_ms)
  {
    std_msgs::Header header;
    header.stamp = ros::Time(timestamp_ms / 1000);
    header.frame_id = "camera_link";
    if (frameNum > INT32_MAX)
    {
      header.seq = frameNum - INT32_MAX;
    }
    else
    {
      header.seq = frameNum;
    }
  
    geometry_msgs::PoseStamped pose_stamped_msg;
    pose_stamped_msg.header = header;
    pose_stamped_msg.pose = poseMatrixToMsg(cameraPose);
    return pose_stamped_msg;
  }
  
  class slam_event_handler : public rs::core::video_module_interface::processing_event_handler
  {
  public:
    std::shared_ptr< rs::slam::occupancy_map > occ_map;
    slam_event_handler()
    {
      std::cout << "created.........." << std::endl;
    }
  
    void module_output_ready(rs::core::video_module_interface * sender, rs::core::correlated_sample_set * sample)
    {
      rs::slam::slam *slamPtr = dynamic_cast< rs::slam::slam * >(sender);
      uint64_t feFrameNum = sample->images[static_cast<uint8_t>(rs::core::stream_type::fisheye)]->query_frame_number();
      double feTimeStamp = sample->images[static_cast<uint8_t>(rs::core::stream_type::fisheye)]->query_time_stamp();
  
      // Publish camera pose
      rs::slam::PoseMatrix4f cameraPose;
      slamPtr->get_camera_pose(cameraPose);
      geometry_msgs::PoseStamped pose_msg = getPoseStampedMsg(cameraPose, feFrameNum, feTimeStamp);
      pub_pose.publish(pose_msg);
  
      // Publish relocalized camera pose, if any
      rs::slam::PoseMatrix4f relocPose;
      if (slamPtr->get_relocalization_pose(relocPose))
      {
        geometry_msgs::PoseStamped reloc_pose_msg = getPoseStampedMsg(relocPose, feFrameNum, feTimeStamp);
        pub_reloc.publish(reloc_pose_msg);
      }
  
      // Publish tracking accuracy
      rs::slam::tracking_accuracy accuracy = slamPtr->get_tracking_accuracy();
      TrackingAccuracy accuracyMsg;
      accuracyMsg.header.stamp = ros::Time(feTimeStamp);
      accuracyMsg.header.seq = feFrameNum;
      accuracyMsg.tracking_accuracy = (uint32_t)accuracy;
      pub_accuracy.publish(accuracyMsg);
  
      // Publish 2D pose
      Eigen::Vector3f gravity = Eigen::Vector3f(0, 1, 0);
      stRobotPG robotPG;
      convertToPG(cameraPose, gravity, robotPG);
      pose2d.x = robotPG.x;
      pose2d.y = robotPG.y;
      pose2d.theta = robotPG.theta;
      pub_pose2d.publish(pose2d);
      g_robotPGStack.push_back(robotPG);
  
      // Publish odometry
      if (is_pub_odom)
      {
        nav_msgs::Odometry odom;
        odom.header.stamp = ros::Time(feTimeStamp);
        odom.header.seq = feFrameNum;
        odom.header.frame_id = "odom";
        odom.pose.pose.position.x = pose2d.x;
        odom.pose.pose.position.y = pose2d.y;
        tf2::Quaternion quat(tf2::Vector3(0, 0, 1), pose2d.theta); // Rotate around the z axis by angle theta
        tf2::convert<tf2::Quaternion, geometry_msgs::Quaternion>(quat, odom.pose.pose.orientation);
        pub_odom.publish(odom);
      }
  
      // Publish occupancy map
      int wmap = 512;
      int hmap = 512;
      if (!occ_map)
      {
        occ_map = slamPtr->create_occupancy_map(wmap * hmap);
        std::cout << " creating occupancy map: resolution: " << slamPtr->get_occupancy_map_resolution() << std::endl;
      }
      if (ipNavMap == NULL)
      {
        ipNavMap = cvCreateImage(cvSize(wmap, hmap), 8, 1);
        cvSet(ipNavMap, 10, NULL);
      }
      int status = slamPtr->get_occupancy_map_update(occ_map);
      int count = occ_map->get_tile_count();
      nav_msgs::OccupancyGrid map_msg;
      if (status >= 0 && count > 0)
      {
        const int32_t* map = occ_map->get_tile_coordinates();
        for (int i = 0; i < count; i++)
        {
          int _x = map[3 * i] + wmap / 2;
          int _y = map[3 * i + 1] + hmap / 2;
          if (_x >= 0 && _x < wmap)
          {
            if (_y >= 0 && _y < hmap)
            {
              int V = map[3 * i + 2];
              ipNavMap->imageData[wmap * _y + _x] = V;
            }
          }
        }
      }
      std::vector<signed char> vMap(ipNavMap->imageData, ipNavMap->imageData + wmap * hmap);
      map_msg.data = vMap;
      map_msg.info.resolution = map_resolution;
      map_msg.info.width      = wmap;
      map_msg.info.height     = hmap;
      map_msg.info.origin.position.x = -(wmap / 2) * map_resolution;
      map_msg.info.origin.position.y = -(hmap / 2) * map_resolution;
      pub_map.publish(map_msg);
    }
  
    ~slam_event_handler()
    {
      ROS_INFO("deconstructure event handler");
    }
  };
  
  
  SNodeletSlam::SNodeletSlam()
  {
  }
  
  
  SNodeletSlam::~SNodeletSlam()
  {
  }
  
  
  void SNodeletSlam::onInit()
  {
    ros::NodeHandle pnh = getPrivateNodeHandle();
    pnh.param< double >("map_resolution", map_resolution, 0.05);
    pnh.param< double >("hoi_min", hoi_min, -0.5);
    pnh.param< double >("hoi_max", hoi_max, 0.1);
    pnh.param< double >("doi_min", doi_min, 0.3);
    pnh.param< double >("doi_max", doi_max, 3.0);
    pnh.param< std::string >("load_occupancy_map", occupancy_filename_in, "");
    pnh.param< std::string >("load_relocalization_map", relocalization_filename_in, "");
    pnh.param< std::string >("trajectoryFilename", trajectory_filename_out, "trajectory.ppm");
    pnh.param< std::string >("relocalizationFilename", relocalization_filename_out, "relocalization.bin");
    pnh.param< std::string >("occupancyFilename", occupancyFilenameOut, "occupancy.bin");
    pnh.param< std::string >("topic_camera_pose", topic_camera_pose, "camera_pose");
    pnh.param< std::string >("topic_reloc_pose", topic_reloc_pose, "reloc_pose");
    pnh.param< std::string >("topic_pose2d", topic_pose2d, "pose2d");
    pnh.param< std::string >("topic_map", topic_map, "map");
    pnh.param< std::string >("topic_tracking_accuracy", topic_tracking_accuracy, "tracking_accuracy");
    pnh.param< std::string >("topic_odom", topic_odom, "odom");
    pnh.param< bool >("publish_odometry", is_pub_odom, false);
  
    nh = getMTNodeHandle();
    pub_pose = nh.advertise< geometry_msgs::PoseStamped >(topic_camera_pose, 1, true);
    pub_reloc = nh.advertise< geometry_msgs::PoseStamped >(topic_reloc_pose, 1, true);
    pub_pose2d = nh.advertise< geometry_msgs::Pose2D >(topic_pose2d, 2, true);
    pub_map = nh.advertise< nav_msgs::OccupancyGrid >(topic_map, 1, true);
    pub_accuracy = nh.advertise< realsense_ros_slam::TrackingAccuracy >(topic_tracking_accuracy, 1, true);
    if (is_pub_odom) pub_odom = nh.advertise< nav_msgs::Odometry >(topic_odom, 1, true);
    pkgpath = ros::package::getPath("realsense_ros_slam") + "/";
  
    sub_depthInfo = nh.subscribe("camera/depth/camera_info", 1, &SNodeletSlam::depthInfoCallback, this);
    sub_fisheyeInfo = nh.subscribe("camera/fisheye/camera_info", 1, &SNodeletSlam::fisheyeInfoCallback, this);
    sub_accelInfo = nh.subscribe("camera/accel/imu_info", 1, &SNodeletSlam::accelInfoCallback, this);
    sub_gyroInfo = nh.subscribe("camera/gyro/imu_info", 1, &SNodeletSlam::gyroInfoCallback, this);
    sub_fe2imu = nh.subscribe("camera/extrinsics/fisheye2imu", 1, &SNodeletSlam::fe2ImuCallback, this);
    sub_fe2depth = nh.subscribe("camera/extrinsics/fisheye2depth", 1, &SNodeletSlam::fe2depthCallback, this);
    
    reset_srv = nh.advertiseService("realsense_ros_slam/reset", &SNodeletSlam::reset, this);
    save_output_srv = nh.advertiseService("realsense_ros_slam/save_output", &SNodeletSlam::saveOutput, this);
  
    actual_config = {};
    
    slam_ = boost::make_unique<rs::slam::slam>();
    
    ROS_INFO("end of onInit");
  }//end onInit
  
  void SNodeletSlam::depthInfoCallback(const sensor_msgs::CameraInfoConstPtr& depthCameraInfo)
  {
    sub_depthInfo.shutdown();
    mut_init_.lock();
    depthCameraInfo_ = depthCameraInfo;
    startIfReady();
    mut_init_.unlock();
  }
  
  void SNodeletSlam::fisheyeInfoCallback(const sensor_msgs::CameraInfoConstPtr& fisheyeCameraInfo)
  {
    sub_fisheyeInfo.shutdown();
    mut_init_.lock();
    fisheyeCameraInfo_ = fisheyeCameraInfo;
    startIfReady();
    mut_init_.unlock();
  }
  
  void SNodeletSlam::accelInfoCallback(const realsense_ros_camera::IMUInfoConstPtr& accelInfo)
  {
    sub_accelInfo.shutdown();
    mut_init_.lock();
    accelInfo_ = accelInfo;
    startIfReady();
    mut_init_.unlock();
  }
  
  void SNodeletSlam::gyroInfoCallback(const realsense_ros_camera::IMUInfoConstPtr& gyroInfo)
  {
    sub_gyroInfo.shutdown();
    mut_init_.lock();
    gyroInfo_ = gyroInfo;
    startIfReady();
    mut_init_.unlock();
  }
  
  void SNodeletSlam::fe2ImuCallback(const realsense_ros_camera::ExtrinsicsConstPtr& fe2imu)
  {
    sub_fe2imu.shutdown();
    mut_init_.lock();
    fe2imu_ = fe2imu;
    startIfReady();
    mut_init_.unlock();
  }
  
  void SNodeletSlam::fe2depthCallback(const realsense_ros_camera::ExtrinsicsConstPtr& fe2depth)
  {
    sub_fe2depth.shutdown();
    mut_init_.lock();
    fe2depth_ = fe2depth;
    startIfReady();
    mut_init_.unlock();
  }
  
  void SNodeletSlam::startIfReady()
  {
    // If we have all the messages we were waiting for, start SLAM.
    if (depthCameraInfo_ && fisheyeCameraInfo_ && accelInfo_ && gyroInfo_ && fe2imu_ && fe2depth_)
    {
      startSlam();
    }
  }
  
  void SNodeletSlam::startSlam()
  {
    ROS_INFO("Starting SLAM...");
  
    slam_->set_occupancy_map_resolution(map_resolution);
    slam_->set_occupancy_map_height_of_interest(hoi_min, hoi_max);
    slam_->set_occupancy_map_depth_of_interest(doi_min, doi_max);
    slam_->force_relocalization_pose(false);  
    
    slam_event_handler scenePerceptionEventHandler;
    slam_->register_event_handler(&scenePerceptionEventHandler);
  
    slam_tracking_event_handler trackingEventHandler;
    slam_->register_tracking_event_handler(&trackingEventHandler);
  
    supported_config = {};
    if (slam_->query_supported_module_config(0, supported_config) < rs::core::status_no_error)
    {
      std::cerr << "error : failed to query the first supported module configuration" << std::endl;
      return ;
    }
  
    // Set camera intrinsics
    rs::core::intrinsics  depth_intrinsics, fisheye_intrinsics;
    SNodeletSlam::setCalibrationData(depthCameraInfo_, depth_intrinsics);
    SNodeletSlam::setCalibrationData(fisheyeCameraInfo_, fisheye_intrinsics);
  
    std::map< rs::core::stream_type, rs::core::intrinsics > intrinsics;
    intrinsics[rs::core::stream_type::depth] = depth_intrinsics;
    intrinsics[rs::core::stream_type::fisheye] = fisheye_intrinsics;
    SNodeletSlam::setStreamConfigIntrin(rs::core::stream_type::depth, intrinsics);
    SNodeletSlam::setStreamConfigIntrin(rs::core::stream_type::fisheye, intrinsics);
  
    // Set IMU intrinsics
    rs::core::motion_device_intrinsics acc, gyro;
    imuInfoMsgToRSImuIntrinsics(accelInfo_, acc);
    imuInfoMsgToRSImuIntrinsics(gyroInfo_, gyro);
    actual_config[rs::core::motion_type::accel].is_enabled = true;
    actual_config[rs::core::motion_type::accel].intrinsics = acc;
    actual_config[rs::core::motion_type::gyro].is_enabled = true;
    actual_config[rs::core::motion_type::gyro].intrinsics = gyro;
  
    // Set extrinsics
    rs::core::extrinsics fe2imu, fe2depth;
    extrinsicsMsgToRSExtrinsics(fe2imu_, fe2imu);
    extrinsicsMsgToRSExtrinsics(fe2depth_, fe2depth);
    actual_config[rs::core::stream_type::fisheye].extrinsics_motion = fe2imu;
    actual_config[rs::core::stream_type::fisheye].extrinsics = fe2depth;
  
    // Set actual config
    if (slam_->set_module_config(actual_config) < rs::core::status_no_error)
    {
      NODELET_ERROR("error : failed to set the enabled module configuration");
      return ;
    }
      
    if (relocalization_filename_in.length() > 0)
    {
        if(slam_->load_relocalization_map(relocalization_filename_in) == rs::core::status_no_error)
        {
          ROS_INFO("Loaded relocalization map.");
        }
        else
        {
          ROS_WARN("Failed to load relocalization map.");
        }
    }
    if (occupancy_filename_in.length() > 0) 
    {
        if (slam_->load_occupancy_map(occupancy_filename_in) == rs::core::status_no_error)
        {
          ROS_INFO("Loaded occupancy map.");
        }
        else
        {
          ROS_WARN("Failed to load occupancy map.");
        }
    }
  
    //slam->load_relocalization_map(relocalizationFilename);
    sub.onInit(nh, slam_.get());
    sub.subscribeStreamMessages();
    sub.subscribeMotion();
  
    ros::Rate r(30);
    while (ros::ok())
    {
      r.sleep();
    }
  
    slam_->flush_resources();
  }//end of callback
  
  
  void SNodeletSlam::setCalibrationData(const sensor_msgs::CameraInfoConstPtr & cameraInfoMsg, rs::core::intrinsics & cameraInfo)
  {
    NODELET_INFO("setCalibrationData ");
    cameraInfo.width = cameraInfoMsg->width;
    cameraInfo.height = cameraInfoMsg->height;
    cameraInfo.fx = cameraInfoMsg->K[0];
    cameraInfo.fy = cameraInfoMsg->K[4];
    cameraInfo.ppx = cameraInfoMsg->K[2];
    cameraInfo.ppy = cameraInfoMsg->K[5];
    for (int i = 0; i < 5; i++)
      cameraInfo.coeffs[i] = (float)cameraInfoMsg->D[i];
    std::string distortion_model = cameraInfoMsg->distortion_model;
    if (distortion_model.compare("modified_brown_conrady") == 0)
    {
      cameraInfo.model = rs::core::distortion_type::modified_brown_conrady;
    }
    else if (distortion_model.compare("none") == 0)
    {
      cameraInfo.model = rs::core::distortion_type::none;
    }
    else if (distortion_model.compare("distortion_ftheta") == 0)
    {
      cameraInfo.model = rs::core::distortion_type::distortion_ftheta;
    }
    else
    {
      cameraInfo.model = rs::core::distortion_type::none;
    }
  }
  
  
  void SNodeletSlam::setStreamConfigIntrin(rs::core::stream_type stream, std::map< rs::core::stream_type, rs::core::intrinsics > intrinsics)
  {
    auto & supported_stream_config = supported_config[stream];
    if (!supported_stream_config.is_enabled || supported_stream_config.size.width != intrinsics[stream].width || supported_stream_config.size.height != intrinsics[stream].height)
    {
      ROS_ERROR("size of stream is not supported by slam");
      ROS_ERROR_STREAM("  supported: stream " << (uint32_t) stream << ", width: " << supported_stream_config.size.width  << " height: " << supported_stream_config.size.height);
      ROS_ERROR_STREAM("  received: stream " << (uint32_t) stream << ", width: " << intrinsics[stream].width  << " height: " << intrinsics[stream].height);
  
      return;
    }
    rs::core::video_module_interface::actual_image_stream_config &actual_stream_config = actual_config[stream];
    actual_config[stream].size.width = intrinsics[stream].width;
    actual_config[stream].size.height = intrinsics[stream].height;
    actual_stream_config.frame_rate = supported_stream_config.frame_rate;
    actual_stream_config.intrinsics = intrinsics[stream];
    actual_stream_config.is_enabled = true;
  }//end of setStremConfigIntrin
  
  
  void SNodeletSlam::setMotionData(realsense_ros_camera::IMUInfo & imu_res, rs::core::motion_device_intrinsics & motion_intrin)
  {
    int index = 0;
    for (int i = 0; i < 3; ++i)
    {
      for (int j = 0; j < 4; ++j)
      {
        motion_intrin.data[i][j] = imu_res.data[index];
        ++index;
      }
      motion_intrin.noise_variances[i] = imu_res.noise_variances[i];
      motion_intrin.bias_variances[i] = imu_res.bias_variances[i];
    }
  }//end of setMotionData
  
  
  void SNodeletSlam::setMotionConfigIntrin(rs::core::motion_type motion, std::map< rs::core::motion_type, rs::core::motion_device_intrinsics > motion_intrinsics)
  {
    actual_config[motion].is_enabled = true;
    actual_config[motion].intrinsics = motion_intrinsics[motion];
  }//end of setMotionConfigIntrin
  
  
  void SNodeletSlam::setExtrinData(realsense_ros_camera::Extrinsics & fe_res, rs::core::extrinsics & extrinsics)
  {
    for (int i = 0; i < 9; ++i)
    {
      extrinsics.rotation[i] = fe_res.rotation[i];
      if (i < 3)
      {
        extrinsics.translation[i] = fe_res.translation[i];
      }
    }
  }//end of setExtrinData
  
  bool SNodeletSlam::reset(realsense_ros_slam::Reset::Request &req, realsense_ros_slam::Reset::Response &resp)
  {
    ROS_INFO("Resetting SLAM...");
    resp.status = slam_->restart() == rs::core::status::status_no_error;
    return true;
  }
  
  bool SNodeletSlam::saveOutput(realsense_ros_slam::SaveOutput::Request &req, realsense_ros_slam::SaveOutput::Response &resp)
  {
    bool success = true;
    
    if (slam_->save_occupancy_map_as_ppm(pkgpath + trajectory_filename_out, true) == rs::core::status::status_no_error)
    {
      std::cout << "Saved trajectory to PPM file." << std::endl;
    }
    else
    {
      std::cout << "FAILED to save trajectory to PPM file." << std::endl;
      success = false;
    }
    
    if(slam_->save_occupancy_map(pkgpath + occupancyFilenameOut) == rs::core::status::status_no_error)
    {
      std::cout << "Saved occupancy map to BIN file." << std::endl;
    }
    else
    {
      std::cout << "FAILED to save occupancy map to BIN file." << std::endl;
      success = false;
    }
    
    if(slam_->save_relocalization_map(pkgpath + relocalization_filename_out) == rs::core::status::status_no_error)
    {
      std::cout << "Saved relocalization map to BIN file." << std::endl;
    }
    else
    {
      std::cout << "FAILED to save relocalization map to BIN file." << std::endl;
      success = false;
    }
    
    resp.status = success;
    return true;  
  }
  
  }//end namespace
  