Revision history [back]

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

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

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~~covariance of course this is just a quick&dirty fix, since the covariance remains the same, but should accumulate 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;
  ...

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~~ covariance

of course this is just a quick&dirty fix, since the covariance remains the ~~same,~~ same but instead should ~~accumulate~~ "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;
  ...