0
$\begingroup$

Rosanswers logo

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?


Originally posted by rossi on ROS Answers with karma: 73 on 2013-07-02

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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;
  ...

Originally posted by goetz.marc with karma: 203 on 2013-07-03

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by rossi on 2013-07-07:
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?

Comment by goetz.marc on 2013-07-07:
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

Comment by goetz.marc on 2013-07-07:
you may need to install boost first: sudo apt-get install libboost-all-dev

Comment by rossi on 2013-07-08:
Thanks! It worked fine!

Comment by TSC on 2014-07-09:
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?

Comment by goetz.marc on 2014-07-09:
Hi @TSC, the values are: X, Y, Z, Roll, Pitch, Yaw; so yes, it should work for UAVs

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.