Time step miss match between "scan" topic and "scan_matched_points2" topic

[yczhangRS]

I was doing simulation in stage with a 10Hz laser scanner.

As shown in the picture.
The robot is having a 1 rad/s constant rotation speed.
I found the /scan_matched_points2 (green line) and /scan topic(red line) always have a constant angle difference.
I found the problem is the /scan_matched_points2 topic is always lagged 1 step behind the /scan topic.
This means at 1 rad/s there would be a constant 5.7 degree(1 rad/s / 10Hz) difference.

Can anyone suggest a fix to this?

[gaschler]

Topic "/scan_matched_points2" shows the result of LocalTrajectoryBuilder.
So naturally it is published later than its input data.
It should not be used for other purposes for debugging/visualization.

I would say it works as intended?

[yczhangRS]

No it is not working well.
You can see from this picture.If I keep turning the robot left and right it will generate a unusable map.
I found the local map is aligned with the green line which is published by /scan_matched_points2 topic not the the actual laser scan input (the red line). So even if this topic is not used in scan matching I'm still guessing there is some internal timing that is wrong.
Or could it because I am using a simulated time from stage?

I was using a perfect odometry simulated by stage so even if the scan matcher is not working it should be able to generate a good map.

I can see if I activate the real_time_correlative_scan_matcher, the problem is gone.
But I don't think that really solved the problem. It is only because this scan matcher is too powerful that the error is corrected very fast.

[gaschler]

Or could it because I am using a simulated time from stage?

Please verify that you set
<param name="/use_sim_time" value="true" />

So even if this topic is not used in scan matching I'm still guessing there is some internal timing that is wrong.

Hard to say from here.
You could check if the behavior was better before #891, which recently changed the timers.

Other than that, you would need to provide a fork with a reproducible roslaunch config as explained in https://raw.githubusercontent.com/googlecartographer/cartographer_ros/master/.github/ISSUE_TEMPLATE

[yczhangRS]

1. /use_sim_time is set to true.

2. I have verified that this is not related to Fix memory leak in simulations by removing wall timers. #891

3. I have looked a bit into the code and I think I have got the reason.
   The problem comes with odom.
   I found the local scan matcher is executed after the odom msg is received.
   However the latest laser scan is not arrived at that time (for stage /scan is always received later than /odom)
   The scan matching is done based on the current odom and the laserscan from the last time step.
   For stage, the odom topic is updated 10Hz which is very slow. This amplifies the problem.
   This could explain the angle shift.

   Is there any motion prediction done here? Because the odom and laserscan would not come at the same time, especially in real application. The time different here would always bring some error.

   I am thinking this might be a cartographer problem not a cartographer_ros problem.

4. Sorry I haven't prepare the bag and launch file yet. If you need it, maybe I will do it tomorrow.

[gaschler]

I have verified that this is not related to #891

Thank you! Good to know.

Is there any motion prediction done here?

Yes, if PoseExtrapolator received odometry data, it will extrapolate from that using both translational and angular velocity. 10 Hz does sound low, so this will be poor estimate.
The default tuning will not fit here.
You can reduce ceres_scan_matcher.translation_weight and .rotation_weight to reduce the cost of scan matching away from that estimate, and you may need to increase ceres_solver_options.max_num_iterations to converge.

Of course, you can always set use_odometry = false and compare.

If you need it, maybe I will do it tomorrow.

I don't think this is a bug (but rather a tuning/documentation issue), so there is currently no need to put much effort into this.

[yczhangRS]

You can reduce ceres_scan_matcher.translation_weight and .rotation_weight to reduce the cost of scan matching away from that estimate, and you may need to increase ceres_solver_options.max_num_iterations to converge.

I have done this but I'm afraid the error is too huge for the ceres_scan_matcher to correct. I have tried a lot of values but does not help. I know we can use scan matcher to correct this error but it doesn't seems a good solution to me.

Yes, if PoseExtrapolator received odometry data, it will extrapolate from that using both translational and angular velocity. 10 Hz does sound low, so this will be poor estimate.

I have been looking at PoseExtrapolator but I have found no documentation about it. The only parameter I found is kExtrapolationEstimationTimeSec which is not changeable outside the source code. Maybe a bit of documentation on this could be helpful.

I don't think this is a bug (but rather a tuning/documentation issue), so there is currently no need to put much effort into this.

I agree this is not a bug but I'm thinking there might be possible improvements.
First, although I have to do more test to say this, but the PoseExtrapolator doesn't seems to be very helpful.
Second, we could wait for another laser frame to come (or odom frame) and choose the pair with smaller time different to send to the scanmatcher. Although this could bring some other problem.

The error caused by time frame mismatch could be reduced by increasing the frame rate. But even if I increase the odom rate to 50Hz the error could still reach 0.6degree which is still significant.

Thank you for your help with the problem. I am getting more clear about the problem. I'll try to look deeper into the code.

[gaschler]

we could wait for another laser frame to come (or odom frame) and choose the pair with smaller time different to send to the scanmatcher.

We do wait for all sensors to have data before processing. This is what sensor::Collator does.
We use exact timestamps of input data and interpolate/extrapolate when fusing sensors (not just the closest timestamp).

PoseExtrapolator doesn't seems to be very helpful

There are many practical examples that need it.
Here is an example for the combination 2D lidar + odometry:
https://github.com/magazino/cartographer_magazino/blob/master/cartographer_toru/configuration_files/toru_base.lua

Perhaps your abrupt change from left to right rotation in your simulated world is unlikely in real-world recordings?

[yczhangRS]

We do wait for all sensors to have data before processing. This is what sensor::Collator does.
We use exact timestamps of input data and interpolate/extrapolate when fusing sensors (not just the closest timestamp).

Thanks for the info. However, it seems that is not work for me. But I'll look into that, could it again be some setting issue?

There are many practical examples that need it.
Here is an example for the combination 2D lidar + odometry:
https://github.com/magazino/cartographer_magazino/blob/master/cartographer_toru/configuration_files/toru_base.lua

Thanks I've already looked at it. Although there is not much setting different from mine except it is a 3d slam.

Perhaps your abrupt change from left to right rotation in your simulated world is unlikely in real-world recordings?

The simulated world do not have acceleration could that be a problem?

I'll do more test and maybe update again here. Hope this is not a big issue but it could be a good starting point to understand the code.

[yczhangRS]

BTW, I found my problem very similar with #820.

[yczhangRS]

Hi, I think I might have found a problem in the code related to my issue.

In void AdvanceImuTracker(common::Time time, ImuTracker* imu_tracker) const;
the following code handles the odom data if no IMU data.

  if (imu_data_.empty() || time < imu_data_.front().time) {
    // There is no IMU data until 'time', so we advance the ImuTracker and use
    // the angular velocities from poses and fake gravity to help 2D stability.
    imu_tracker->Advance(time);
    imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());
    imu_tracker->AddImuAngularVelocityObservation(
        odometry_data_.size() < 2 ? angular_velocity_from_poses_
                                  : angular_velocity_from_odometry_);
    return;
  }

The latest angular velocity observation is added after advance is called. The velocity observation is delayed for one time step.

I have tried to change the code to the following and my problem is solved:

if (imu_data_.empty() || time < imu_data_.front().time) {
    // There is no IMU data until 'time', so we advance the ImuTracker and use
    // the angular velocities from poses and fake gravity to help 2D stability.
    imu_tracker->AddImuAngularVelocityObservation(
    odometry_data_.size() < 2 ? angular_velocity_from_poses_
                              : angular_velocity_from_odometry_);
    imu_tracker->Advance(time);
    imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());
    return;
  }

I'm not sure if this will cause other problems.

I think this won't affect the performance when IMU is provided because imu_tracker->Advance(time); is called at the end.

[MichaelGrupp]

If you think you've found a bug, you can propose a pull request so this can be reviewed in detail. Closing.