combine visual odometry data from ccny_rgbd with encoders/IMU

asked 2013-07-02 23:44:54 -0600

rossi
73 ●8 ●14 ●14

updated 2014-01-28 17:17:07 -0600

ngrennan
1 ●1 ●1

Hello Ros-community,

I want to combine the visual odometry from a kinect with IMU and encoder data. I think it must be possible with the EKF-package for a good calculated robot-pose. With the ccny_rgbd i try to make simultaneously a 3D map and use the published vo data for the robot pose. If I use just the vo data, the robot pose jump sometimes when it turns too fast around. Is it possible to combine all information to visualize the current best hypothesis about the robot's pose? Is a scan-matcher maybe the right way?

Edit: I try to use the laser_scan_matcher. It could not find the node. I think it doesnt work, because it is just for fuerte available and i installed groovy. But i found a very easy way to combine them: just set in the EKF package visual odometry to TRUE. NOW i get a big Error: Covariance specified for measurement on topic vo is zero

I found this:

Each measurement that is processed by the robot pose ekf needs to have a covariance associated with it. The diagonal elements of the covariance matrix cannot be zero. This error is shown when one of the diagonal elements is zero. Messages with an invalid covariance will not be used to update the filter.

With rostopic echo /vo i get these infos:

child_frame_id: '' pose: pose: position: x: 0.0725274412988 y: -0.0736182447672 z: -0.00655200519239 orientation: x: -0.00375973887428 y: -0.00384862482967 z: 0.0079803370668 w: 1.00007968675

covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

twist: twist: linear: x: 0.0 y: 0.0 z: 0.0

angular: 
  x: 0.0
  y: 0.0
  z: 0.0

Why is the covariance matrix zero? Is it because vo of ccny_rgbd could t generate the covariances? or is this option not implemented?

answered 2013-07-03 21:50:51 -0600

goetz.marc

203 ●1 ●4 ●11

updated 2013-07-03 21:54:25 -0600

hi rossi

the matrix is zero because ccny_rgbd doesn't calculate the covariance for the visual odometry, so the matrix is initialized as all-zero

i had the same issue, so i just manually set the covariance

of course this is just a quick&dirty fix, since the covariance remains the same but instead should "grow" over time, but yeah, it is enough for my needs like this

ccny_rgbd_tools/ccny_rgbd/src/apps/visual_odometry.cpp, line ~384

void VisualOdometry::publishOdom(const std_msgs::Header& header)
{
  OdomMsg odom;
  // this prevents the "out of sync" / "no time" error
  odom.header.stamp = ros::Time::now();
  odom.header.frame_id = fixed_frame_;
  odom.child_frame_id = "base_link";
  tf::poseTFToMsg(f2b_, odom.pose.pose);
  // robot_pose_ekf doesn't accept all-zeros in the covariance matrix
  odom.pose.covariance = boost::assign::list_of(VISUAL_COVARIANCE)(0)(0)(0)(0)(0)
                                               (0)(VISUAL_COVARIANCE)(0)(0)(0)(0)
                                               (0)(0)(999999)(0)(0)(0)
                                               (0)(0)(0)(999999)(0)(0)
                                               (0)(0)(0)(0)(999999)(0)
                                               (0)(0)(0)(0)(0)(VISUAL_COVARIANCE);
    odom.twist.covariance = boost::assign::list_of(VISUAL_COVARIANCE)(0)(0)(0)(0)(0)
                                               (0)(VISUAL_COVARIANCE)(0)(0)(0)(0)
                                               (0)(0)(999999)(0)(0)(0)
                                               (0)(0)(0)(999999)(0)(0)
                                               (0)(0)(0)(0)(999999)(0)
                                               (0)(0)(0)(0)(0)(VISUAL_COVARIANCE);
  odom_publisher_.publish(odom);
}

VISUAL_COVARIANCE is a rosparam, which i can set via the launch-file

ccny_rgbd_tools/ccny_rgbd/src/apps/visual_odometry.cpp, line ~107

void VisualOdometry::initParams()
{
    nh_private_.param<double>("VISUAL_COVARIANCE", VISUAL_COVARIANCE, 0.2);

  if (!nh_private_.getParam ("publish_tf", publish_tf_))
    publish_tf_ = true;
  ...

edit flag offensive delete link

Comments

Thank you for your answer!!! you are my lifesaver!! But i get two errors by making the .cpp...first i need to define VISUAL_COVARIANCE....the second error: ‘boost::assign’ has not been declared...is it necessary to include boost an how could i install boost?

rossi ( 2013-07-07 22:44:14 -0600 )edit

for VISUAL_COVARIANCE: after the includes put "double VISUAL_COVARIANCE;", or just replace it in the code with your desired value, for boost it should be enough to "#include <boost/assign/list_of.hpp>", alternatively you can try to find out how to assign the list without boost

goetz.marc ( 2013-07-07 23:55:06 -0600 )edit

you may need to install boost first: sudo apt-get install libboost-all-dev

goetz.marc ( 2013-07-07 23:57:04 -0600 )edit

Thanks! It worked fine!

rossi ( 2013-07-08 00:00:06 -0600 )edit

Hi @goetz.marc does this fix also works for a case where I have a 6DOFmotion, like a UAV? Can you please tell me what are each matrix diagonal values? The first two are cov_x, cov_y and the last are rcov_z. What about the others? Are they cov_z, rcov_x, rcov_y?

TSC ( 2014-07-09 06:17:14 -0600 )edit

Hi @TSC, the values are: X, Y, Z, Roll, Pitch, Yaw; so yes, it should work for UAVs

goetz.marc ( 2014-07-09 10:30:10 -0600 )edit

add a comment

combine visual odometry data from ccny_rgbd with encoders/IMU

1 Answer

Comments

Question Tools

Stats

Related questions

combine visual odometry data from ccny_rgbd with encoders/IMU edit

1 Answer

Comments

Question Tools

Stats

Related questions

combine visual odometry data from ccny_rgbd with encoders/IMU