RGBDSLAM+IMU

edit

Hi guys,

I am using the RGBDSLAM package to build a 3D map using a stereo camera (Bumblebee2) :

Video result

Sometimes the 3D reconstruction (pointcloud matching) is not done in a good way. I want to improve the mapping results with IMU information. I just want to know which variable is taking the Odometry information to create an edge in the Graph. I suppose that it is "dom_edge.informationMatrix" in "graph_manager.cpp"

if(!invalid_odometry)
{
  ROS_INFO("Adding odometry motion edge for Node %i (if available, otherwise using identity)", (int)graph_.rbegin()->second->id_);
  LoadedEdge3D odom_edge;
  odom_edge.id1 = sequentially_previous_id;
  odom_edge.id2 = new_node->id_;
  odom_edge.mean = tf2G2O(odom_delta_tf);

  //Real odometry
  //FIXME get odometry information matrix and transform it to the optical frame
  odom_edge.informationMatrix = Eigen::Matrix<double,6,6>::Identity(); //0.1m /0.1rad error
  addEdgeToG2O(odom_edge,graph_[sequentially_previous_id],new_node, true,true, curr_motion_estimate);
  graph_[new_node->id_] = new_node; //Needs to be added

Under //Real odometry, all what i see is :

odom_edge.informationMatrix = Eigen::Matrix<double,6,6&gt;::identity();< p="">

In which identity matrix is used. Where is the real information matrix ??