Unable to reset RGB-D odometry pose

classic Classic list List threaded Threaded
2 messages Options
Reply | Threaded
Open this post in threaded view

Unable to reset RGB-D odometry pose

This post was updated on .
Hi Mathieu,
Following this approach, I implemented a node to reset visual odometry when the tracking is lost (null transform received). I am using the last XY pose from wheel odometry and the yaw heading from an IMU sensor, this is the code calling the "reset_odom_to_pose" service after gathering odometries in a approximated time callback:
void fuseOdometry(const Odometry::ConstPtr &wheel_odom, const Odometry::ConstPtr &visual_odom, const Imu::ConstPtr &imu_data,
                  ros::Publisher* publisher, ros::ServiceClient* client)
    Odometry fused_odom = *visual_odom;
    Point visual_pose = visual_odom->pose.pose.position;
    Quaternion visual_quat = visual_odom->pose.pose.orientation;
    if (visual_pose.x == 0 && visual_pose.y == 0 && visual_pose.z == 0 && 
        visual_quat.x == 0 && visual_quat.y == 0 && visual_quat.z == 0 && visual_quat.w == 0)
        ROS_DEBUG("Null visual odometry detected! Fusing wheels and IMU measurements...");
        fused_odom.pose.pose.position.x = wheel_odom->pose.pose.position.x;
        fused_odom.pose.pose.position.y = wheel_odom->pose.pose.position.y;
        fused_odom.pose.pose.orientation = imu_data->orientation;
        rtabmap_ros::ResetPose::Request request;
        request.x = wheel_odom->pose.pose.position.x;
        request.y = wheel_odom->pose.pose.position.y;
        request.z = 0;
        request.roll  = 0;
        request.pitch = 0;
        request.yaw = tf::getYaw(imu_data->orientation);
        ROS_INFO("YAW = %f", request.yaw);
        rtabmap_ros::ResetPose::Response response;
        if (client->call(request, response))
            ROS_INFO("Visual odometry successfully reset");
            ROS_ERROR("Unable to reset visual odometry!");
When null odometry is detected, the service is correctly called and response received, but even if I set only XYH values (I have a non-holonomic planar robot), rgbd_odometry gives this error:
[ INFO] [1449495190.331007237]: visual_odoemtry: reset odom to pose xyz=0.369134,-0.075018,0.000000 rpy=0.000000,0.000000,1.463692
[ WARN] (2015-12-07 14:33:10.327) Odometry.cpp:129::reset() Force2D=true and the initial pose contains
z, roll or pitch values (xyz=0.369134,-0.075018,0.000000 rpy=0.000000,0.000000,1.463692). They are set to null.
[ INFO] [1449495191.331007237]: Visual odometry successfully reset
Using RViz to display this, I see that visual odometry seems to align with wheel odometry, but the yaw is always zero and many times the position is wrong, too... am I missing something?

Reply | Threaded
Open this post in threaded view

Re: Unable to reset RGB-D odometry pose

Hi Guido,

it is a bug, the yaw value is set to null instead of the pitch. It is fixed in this commit.

thx for the observation!