search

cartographer-project / cartographer

Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor configurations.
Apache License 2.0
7.14k stars 2.25k forks source link

the timing of the angular velocity update process is slow when there is no IMU data #1648

Open sec-matsunaga opened 4 years ago

sec-matsunaga commented 4 years ago

This is my first post. Forgive me if I make you feel bad by using incorrect words in English, i don't mean it at all.

I think there is a problem with the posture calculation without IMU data. I think the timing of the angular velocity update process is slow when there is no IMU data. Therefore, the posture used when calculating the amount of rotation by the ExtrapolateRotation when estimating the posture of the PoseExtrapolator is the posture of the previous time and the posture of the previous time, and there is one tempo delay. Specifically, the code is as follows:.

cartographer\mapping\pose_extrapolator.cc

Original：line 200 // 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_);

Modified // 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());

The reason for this is: In the process of "AddImuAngularVelocityObservation", the angular velocity is updated. In the original code, imu_tracker->Advance(time); imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());

The posture calculation uses the previous angular velocity, rather than the most recent angular velocity it wanted to use. Therefore, when calculating the amount of rotation in ExtrapolateRotation, This posture - What we want to do in the previous posture is the previous posture and the previous posture.

By making the above corrections, Specifies the angular velocity updated in "AddImuAngularVelocityObservation" so the current and previous postures are used to calculate the amount of rotation in ExtrapolateRotation.

Please kindly consider this matter.

sec-matsunaga commented 4 years ago

Was the English explanation insufficient? Or should I report a more detailed explanation of the problem? If you have any problems with this issue, please let us know.