# Revision history [back]

I'm a bit confused by your frame setup. If the VIO cameras are affixed to your robot, then I don't see how your transform makes sense. I assume the cameras are just producing visual odometry?

Ignoring the back sensor for a moment, let's say you start your robot, turn it 90 degrees to the left, and drive 10 meters. Your front sensor is mounted 0.4 meters in front of the robot, I'm assuming. So that sensor is first going to get rotated, and then translated 10 meters. But then you are just transforming it with:

 <node pkg="tf2_ros" type="static_transform_publisher" name="odom2front" args="0.4 0 0.02 0 0 0 1 odom front_vio_odom_frame" />


As there is no rotation in that transform, you'll just be subtracting 0.4 meters from the X position, and then fusing that into your state estimate. But your robot turned 90 degrees, so you'd really want to be subtracting 0.4 meters from Y, not X. You are trying to treat your sensor, which is affixed to your robot, as though it is an external fixed sensor.

Moreover, you are assuming that, over time, your two cameras are going to agree with each other's accumulated odometry. This seems unlikely.

My recommendation would be to ensure the camera's velocities are computed in the sensor frame, and then change the child_frame_id in those messages to something like front_vio_velocity_frame, and then change those transforms so that they are going from base_link to front_vio_velocity_frame. Then, in your EKF config, just fuse the velocity data, and not the pose data. Fuse one of the sensors in the EKF to make sure it's behaving as you want, and then add the second.

I'm a bit confused by your frame setup. If the VIO cameras are affixed to your robot, then I don't see how your transform makes sense. I assume the cameras are just producing visual odometry?

Ignoring the back sensor for a moment, let's say you start your robot, turn it 90 degrees to the left, and drive 10 meters. Your front sensor is mounted 0.4 meters in front of the robot, I'm assuming. So that sensor is first going to get rotated, and then translated 10 meters. But then you are just transforming it with:

 <node pkg="tf2_ros" type="static_transform_publisher" name="odom2front" args="0.4 0 0.02 0 0 0 1 odom front_vio_odom_frame" />


As there is no rotation in that transform, you'll just be subtracting 0.4 meters from the X position, and then fusing that into your state estimate. But your robot turned 90 degrees, so you'd really want to be subtracting 0.4 meters from Y, not X. You are trying to treat your sensor, which is affixed to your robot, as though it is an external fixed sensor.

Moreover, you are assuming that, over time, your two cameras are going to agree with each other's accumulated odometry. This seems unlikely.

My recommendation would be to ensure the camera's velocities are computed in the sensor frame, and then change the child_frame_id in those messages to something like front_vio_velocity_frame, and then change those transforms so that they are going from base_link to front_vio_velocity_frame. Then, in your EKF config, just fuse the velocity data, and not the pose data. Fuse one of the sensors in the EKF to make sure it's behaving as you want, and then add the second.

EDIT in response to further question

Why can't an odometry (or pose) source for position be specified with a simple transform from base_link like in this case?

Because the transform required to convert your camera odometry to your odom frame will change depending on your robot's current orientation. The static transform you are defining says that, in all cases, to move from front_vio_odom_frame to odom, you just need to subtract 0.4 from X and 0.02 from Z, and that's it. But that's not true. Rotate your robot 90 degrees after starting up, and you will find that to get from your camera's frame to the odom frame, you need to subtract 0.4 from Y.

What you really need is to rotate that static transform based on the robot's current pose, and _then_ apply it. In other words, you are converting an offset that is specified in the base_link frame and converting it to the odom frame, which is what you want.

The package doesn't currently support pose data that is reported from a sensor that is affixed to the robot at an offset from the origin. You'd need to either (a) use velocity data or (b) have an external sensor with a truly static transform from the world frame of that sensor to your world frame.

Have you tried just fusing one sensor's velocity? I'd also recommend perhaps toying with your sensor covariance and process noise covariance. Furthermore, while you can't fuse _all_ the 3D pose data, you can actually still fuse some of it. The Z coordinate from the camera pose data should, at least with your setup, be directly fuse-able with the transform you've defined. I think the same goes for the orientation. But your X and Y linear motion should come from velocity.

In any case, start with fusing only the data from the front sensor. Get it working, then add in the back sensor. Note, though, that if you fuse absolute pose data from both, and they diverge, the filter is eventually going to start jumping back and forth between them. This is expected (and technically correct) behavior. You should probably fuse one of them using velocity data alone, and fuse the orientation and Z pose data from the other.

I'm a bit confused by your frame setup. If the VIO cameras are affixed to your robot, then I don't see how your transform makes sense. I assume the cameras are just producing visual odometry?

Ignoring the back sensor for a moment, let's say you start your robot, turn it 90 degrees to the left, and drive 10 meters. Your front sensor is mounted 0.4 meters in front of the robot, I'm assuming. So that sensor is first going to get rotated, and then translated 10 meters. But then you are just transforming it with:

 <node pkg="tf2_ros" type="static_transform_publisher" name="odom2front" args="0.4 0 0.02 0 0 0 1 odom front_vio_odom_frame" />


As there is no rotation in that transform, you'll just be subtracting 0.4 meters from the X position, and then fusing that into your state estimate. But your robot turned 90 degrees, so you'd really want to be subtracting 0.4 meters from Y, not X. You are trying to treat your sensor, which is affixed to your robot, as though it is an external fixed sensor.

Moreover, you are assuming that, over time, your two cameras are going to agree with each other's accumulated odometry. This seems unlikely.

My recommendation would be to ensure the camera's velocities are computed in the sensor frame, and then change the child_frame_id in those messages to something like front_vio_velocity_frame, and then change those transforms so that they are going from base_link to front_vio_velocity_frame. Then, in your EKF config, just fuse the velocity data, and not the pose data. Fuse one of the sensors in the EKF to make sure it's behaving as you want, and then add the second.

EDIT in response to further question

Why can't an odometry (or pose) source for position be specified with a simple transform from base_link like in this case?

Because the transform required to convert your camera odometry to your odom frame will change depending on your robot's current orientation. The static transform you are defining says that, in all cases, to move from front_vio_odom_frame to odom, you just need to subtract 0.4 from X and 0.02 from Z, and that's it. But that's not true. Rotate your robot 90 degrees after starting up, and you will find that to get from your camera's frame to the odom frame, you need to subtract 0.4 from Y.

What you really need is to rotate that static transform based on the robot's current pose, and _then_ then apply it. In other words, you are converting need to convert an offset that is specified in the base_link frame and converting it to the odom frame, which is what you want.

The package doesn't currently support pose data that is reported from a sensor that is affixed to the robot at an offset from the origin. You'd need to either (a) use velocity data or (b) have an external sensor with a truly static transform from the world frame of that sensor to your world frame.

Have you tried just fusing one sensor's velocity? I'd also recommend perhaps toying with your sensor covariance and process noise covariance. Furthermore, while you can't fuse _all_ the 3D pose data, you can actually still fuse some of it. The Z coordinate from the camera pose data should, at least with your setup, be directly fuse-able with the transform you've defined. I think the same goes for the orientation. But your X and Y linear motion should come from velocity.

In any case, start with fusing only the data from the front sensor. Get it working, then add in the back sensor. Note, though, that if you fuse absolute pose data from both, and they diverge, the filter is eventually going to start jumping back and forth between them. This is expected (and technically correct) behavior. You should probably fuse one of them using velocity data alone, and fuse the orientation and Z pose data from the other.

I'm a bit confused by your frame setup. If the VIO cameras are affixed to your robot, then I don't see how your transform makes sense. I assume the cameras are just producing visual odometry?

Ignoring the back sensor for a moment, let's say you start your robot, turn it 90 degrees to the left, and drive 10 meters. Your front sensor is mounted 0.4 meters in front of the robot, I'm assuming. So that sensor is first going to get rotated, and then translated 10 meters. But then you are just transforming it with:

 <node pkg="tf2_ros" type="static_transform_publisher" name="odom2front" args="0.4 0 0.02 0 0 0 1 odom front_vio_odom_frame" />


As there is no rotation in that transform, you'll just be subtracting 0.4 meters from the X position, and then fusing that into your state estimate. But your robot turned 90 degrees, so you'd really want to be subtracting 0.4 meters from Y, not X. You are trying to treat your sensor, which is affixed to your robot, as though it is an external fixed sensor.

Moreover, you are assuming that, over time, your two cameras are going to agree with each other's accumulated odometry. This seems unlikely.

My recommendation would be to ensure the camera's velocities are computed in the sensor frame, and then change the child_frame_id in those messages to something like front_vio_velocity_frame, and then change those transforms so that they are going from base_link to front_vio_velocity_frame. Then, in your EKF config, just fuse the velocity data, and not the pose data. Fuse one of the sensors in the EKF to make sure it's behaving as you want, and then add the second.

EDIT in response to further question

Why can't an odometry (or pose) source for position be specified with a simple transform from base_link like in this case?

Because the transform required to convert your camera odometry to your odom frame will change depending on your robot's current orientation. The static transform you are defining says that, in all cases, to move from front_vio_odom_frame to odom, you just need to subtract 0.4 from X and 0.02 from Z, and that's it. But that's not true. Rotate your robot 90 degrees after starting up, and you will find that to get from your camera's frame to the odom frame, you need to subtract 0.4 from Y.

What you really need is to rotate that static transform based on the robot's current pose, and then apply it. In other words, you need to convert an offset that is specified in the base_link frame and converting convert it to the odom frame, which is what you want.

The package doesn't currently support pose data that is reported from a sensor that is affixed to the robot at an offset from the origin. You'd need to either (a) use velocity data or (b) have an external sensor with a truly static transform from the world frame of that sensor to your world frame.

Have you tried just fusing one sensor's velocity? I'd also recommend perhaps toying with your sensor covariance and process noise covariance. Furthermore, while you can't fuse _all_ all the 3D pose data, you can actually still fuse some of it. The Z coordinate from the camera pose data should, at least with your setup, be directly fuse-able with the transform you've defined. I think the same goes for the orientation. But your X and Y linear motion should come from velocity.

In any case, start with fusing only the data from the front sensor. Get it working, then add in the back sensor. Note, though, that if you fuse absolute pose data from both, and they diverge, the filter is eventually going to start jumping back and forth between them. This is expected (and technically correct) behavior. You should probably fuse one of them using velocity data alone, and fuse the orientation and Z pose data from the other.