static localization error accumulation

[zcb5216]

Hi ,Carlos Miguel :

[zcb5216]

i have done a static localization experiment during about 14 hours,firstly,the localization [ INFO] [1545125065.326365530]: Initial pose: TF position -> [ x: 2.38207 | y: -0.400354 | z: 0 ] TF orientation -> [ qx: 0 | qy: 0 | qz: -0.270181 | qw: 0.962809 ] [/home/xxx/drl/src/dynamic_robot_localization-kinetic-devel/include/dynamic_robot_localization/localization/impl/localization.hpp [1208]
[ INFO] [1545125065.349219308]: Corrected pose: TF position -> [ x: 2.3818 | y: -0.398378 | z: 0 ] TF orientation -> [ qx: 0 | qy: 0 | qz: -0.270279 | qw: 0.962782 ] [/home/xxx/drl/src/dynamic_robot_localization-kinetic-devel/include/dynamic_robot_localization/localization/impl/localization.hpp [1290]]
[ INFO] [1545125065.369185035]: transform_base_link_to_odom:
TF translation -> [ x: 1.92417 | y: -1.41175 | z: 0 ]
TF orientation -> [ qx: 0 | qy: 0 | qz: -0.506916 | qw: 0.861996 ] [/home/xxx/drl/src/dynamic_robot_localization-kinetic-devel/include/dynamic_robot_localization/localization/impl/localization.hpp [1178]
[ INFO] [1545125065.369213864]: last_accepted_pose_odom_to_map_:
TF translation -> [ x: 0.0116315 | y: -0.119453 | z: 0 ]
TF orientation -> [ qx: 0 | qy: 0 | qz: 0.255071 | qw: 0.966923 ] [/home/xxx/drl/src/dynamic_robot_localization-kinetic-devel/include/dynamic_robot_localization/localization/impl/localization.hpp [1191] after 14hours,the localization was ,[ INFO] [1545154548.614135536]: Initial pose: TF position -> [ x: 2.42611 | y: -0.426822 | z: 0 ] TF orientation -> [ qx: 0 | qy: 0 | qz: -0.277419 | qw: 0.960749 ] [/home/xxx/drl/src/dynamic_robot_localization-kinetic-devel/include/dynamic_robot_localization/localization/impl/localization.hpp [1208]]
[ INFO] [1545154548.638683977]: Corrected pose: TF position -> [ x: 2.42675 | y: -0.426095 | z: 0 ] TF orientation -> [ qx: 0 | qy: 0 | qz: -0.277418 | qw: 0.960749 ] [/home/xxx/drl/src/dynamic_robot_localization-kinetic-devel/include/dynamic_robot_localization/localization/impl/localization.hpp [1290]
[ INFO] [1545154548.656896716]: transform_base_link_to_odom:
TF translation -> [ x: 1.92417 | y: -1.41175 | z: 0 ]
TF orientation -> [ qx: 0 | qy: 0 | qz: -0.506916 | qw: 0.861996 ] [/home/xxx/drl/src/dynamic_robot_localization-kinetic-devel/include/dynamic_robot_localization/localization/impl/localization.hpp [1178]
[ INFO] [1545154548.656925738]: last_accepted_pose_odom_to_map_:
TF translation -> [ x: 0.0138424 | y: -0.119098 | z: 0 ]
TF orientation -> [ qx: 0 | qy: 0 | qz: 0.254194 | qw: 0.97244 ] [/home/xxx/drl/src/dynamic_robot_localization-kinetic-devel/include/dynamic_robot_localization/localization/impl/localization.hpp [1191]

[carlosmccosta]

Good afternoon,

Point cloud registration algorithms (such as ICP) usually don't suffer from significant error accumulation if they are given a good / unambiguous map and sensor data with low noise.

However, if:

• You have a map with repeating patterns
• Your sensor has limited range
• Your robot wheel odometry is not accurate
• You specify a low max_correspondence_distance
• The initial alignment between the sensor data and reference point cloud is not accurate enough

-> Then, the point cloud registration algorithms may have difficulty establishing the correct correspondences between [sensor data -> map], which will result in error accumulation / drift.
The robustness against these problems will also highly depend on the registration algorithm that you choose and how it is configured / fine-tuned.

Also, if you give a map with walls that have points associated with their 2 sides, the establishment of correspondences [sensor data -> map] using only the closest point may not be optimal. I faced these issues when estimating the pose of thin objects with a reference point cloud generated from CAD models. For this case, I recommend you use an improved version of the CorrespondenceEstimationBackProjection algorithm that is on my PCL fork, by specifying on the drl configuration file:
correspondence_estimation_approach: 'CorrespondenceEstimationBackProjection'
and ensuring that the sensor and reference point clouds have normals, either computed by drl or by an outside package / CAD / mesh program.
Example in the object_recognition repository.
This improved version of the CorrespondenceEstimationBackProjection besides using distance between correspondences it also has a penalty cost associated with the deviation of the normals between the points in the sensor data and the reference cloud. As such, since two points in opposite sides of a wall will have a normal difference of +-180º, this approach works very well to solve this issue of establishing correspondences when the reference point cloud has thin geometry that falls within the search radius of the correspondence estimation algorithms.

On the other hand, if you are doing mapping, the sensor noise and dynamic objects may cause the registration algorithms to generate small offsets that will accumulate over time / space and will be magnified if the environment does not have good geometry for correctly aligning the sensor -> map pointclouds.

Have a nice day :)