How to start

[melhousni]

Hello
Im trying to use this package but it's a little bit overwhelming!
Let's suppose I have a rosbag with pointcloud and imu data, what would be the first steps to generate a map and use it for navigation? Thank you

[carlosmccosta]

Good night :)

The 3D processing pipeline can be configured for different use cases, such as 3/6 dof mapping or 3/6 dof localization of a robot / sensor / object.
Have a look at the different launch files (and their associated yaml files that specify the pipeline configuration) for each use case (in dynamic_robot_localization_tests and below) and let me know if you need any help :)

If you need fast pose estimation (such as +- 20 or 30 Hz), I would suggest to first make the map without setting registration time limits and playing the bag file at low rate. This way you can generate a high quality map with surface estimation using the moving least squares.
Then you can tune the processing pipeline ICP registration parameters according to your requirements of registration accuracy and processing time.
For example, for higher accuracy, you can set the convergence thresholds (transformation_epsilon | euclidean_fitness_epsilon | convergence_absolute_mse_threshold | convergence_rotation_threshold) with low values and let the ICP max iterations run higher (max_number_of_registration_iterations) while also setting the convergence_time_limit_seconds also higher.

Example of launch files and videos for each use case:

• 6 DoF localization / mapping of a flying kinect (using the ethzasl iros dataset)
  • Launch file
  • Video
• 6 DoF robot localization (map from CAD data)
  • Launch file
  • Robot localization video
• 6 DoF object pose estimation
  • Launch file
  • Pose estimation video with projection mapping
  • Pick and place
• 3 DoF robot localization / mapping
  • Launch file
  • Robot mapping video
  • Robot localization video

Have a nice day :)

[kkVishnu]

Hi @carlosmccosta ,
I am really new to this dynamic robot localization pipeline processing.

As a beginner, I am trying to launch the file "ethzasl_kinect_dataset.launch" using the ethzasl dataset. So when i check the package 'dynamic_robot_localization_tests' I found some use full launch files like ethzasl_kinect_dataset_high_complexity_slow_fly_movement.launch in /dynamic_robot_localization_tests/launch/environments/asl/bags/ .

So, i want to see the same output referred in the video https://www.youtube.com/watch?v=UslDiUkm7wE (Video 6: Free fly test with Kinect in the ETHZ RGB-D dataset using the 6 DoF localization system) in my Ubuntu Laptop.
For that I am doing following steps:

source /home/catkin_ws_drl/devel_release/setup.bash
cd /home/catkin_ws_drl/dynamic_robot_localization_tests/launch/environments/asl/bags/
roslaunch ethzasl_kinect_dataset_high_complexity_slow_fly_movement.launch

--> After this step Rviz got displayed and then I pressed space bar from the current terminal. Now point clouds are displaying but there are some mismatches in my output when i compare it to the video https://www.youtube.com/watch?v=UslDiUkm7wE .

The output I received is recorded as a video (Output_video.mp4) and it is attached.
It would be really helpful if you can confirm whether any additional steps needs to be done at my end ? or Is there any modification expected in ethzasl_kinect_dataset.launch or ethzasl_kinect_dataset_high_complexity_slow_fly_movement.launch to get the same output?

Development environment :
OS : Ubuntu 18.04
ROS: ROS Melodic
RAM: 16GB
Processor: Intel® Core™ i7-10810U CPU @ 1.10GHz × 12
Graphics: Intel® UHD Graphics (CML GT2)

Output_video.mp4

[carlosmccosta]

Hello,

Thank you for reporting the problem :)

Please check the response I gave below, in which I discussed this problem and its solution.
#27

Have a nice day,

[kkVishnu]

Hi @carlosmccosta ,
Thank you for the detailed information and now I am getting the same output.

I have one doubt, when i checked the rostopic list i found multiple topics for pose like /dynamic_robot_localization/localization_pose , /dynamic_robot_localization/localization_poses etc . If i understand correctly the topic "/dynamic_robot_localization/localization_pose" is used for visualizing 6DoF sensor pose in Rviz viewer.
Could you please confirm the exact topic name to get the 6DoF sensor pose ?

eth_slow_fly_movement.mp4

[carlosmccosta]

Hello,

I updated the rviz file for having labels that are easier to understand (please do git pull in the dynamic_robot_localization_tests).

• /dynamic_robot_localization/localization_pose [geometry_msgs/PoseStamped] -> 6 DoF pose estimated by drl for the last point cloud
• /dynamic_robot_localization/localization_poses [geometry_msgs/PoseArray] -> Array with all the (red) poses estimated by drl
• /dynamic_robot_localization/groundtruth_poses [geometry_msgs/PoseArray] -> Array with the (green) ground truth poses associated with each drl pose (for every drl pose, I lookup in TF the associated ground truth pose) -> the ground truth was generated by Vicon cameras (for more info, check the dataset page)
• /trajectory [nav_msgs/Path] -> Red path associated with the drl poses
• /trajectory_gt [nav_msgs/Path]-> Green path associated with the ground truth poses

Have a nice day,

[kkVishnu]

Hi @carlosmccosta ,

Thanks for the updated info.

I am trying to check the covariance data in 6DoF pose , so I used below command to check the covariance matrix after launching ethzasl_kinect_dataset (roslaunch dynamic_robot_localization_tests ethzasl_kinect_dataset_high_complexity_slow_fly_movement.launch) :
$ rostopic echo /dynamic_robot_localization/localization_pose_with_covariance

And I am getting zero values in covariance matrix like below:

---

header:
seq: 0
stamp:
secs: 1298112282
nsecs: 131209000
frame_id: "map"
pose:
pose:
position:
x: 1.33109009299
y: -0.721230991265
z: 1.64216395765
orientation:
x: 0.034314302265
y: 0.27709929409
z: -0.086945950686
w: 0.956283907391
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

header:
seq: 1
stamp:
secs: 1298112282
nsecs: 163552000
frame_id: "map"
pose:
pose:
position:
x: 1.33042057397
y: -0.719728996275
z: 1.64075392505
orientation:
x: 0.0371451415499
y: 0.27721823527
z: -0.0889289796376
w: 0.955961257099
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

header:
seq: 2
stamp:
secs: 1298112282
nsecs: 197002000
frame_id: "map"
pose:
pose:
position:
x: 1.33172996881
y: -0.724027524406
z: 1.63992342201
orientation:
x: 0.0387589196238
y: 0.278446604681
z: -0.0881394768412
w: 0.955613241386
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
.
.

header:
seq: 492
stamp:
secs: 1298112298
nsecs: 713698000
frame_id: "map"
pose:
pose:
position:
x: 1.51738627546
y: -0.422942714822
z: 1.62615816055
orientation:
x: -0.030203352863
y: 0.309130183314
z: 0.126192669006
w: 0.942126157969
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

header:
seq: 493
stamp:
secs: 1298112298
nsecs: 745706000
frame_id: "map"
pose:
pose:
position:
x: 1.51747147745
y: -0.424794380711
z: 1.61823682282
orientation:
x: -0.030589948911
y: 0.295414033612
z: 0.142660588958
w: 0.944162464901
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

---

So, Is there any modification required in config files to see the non zero values in covariance matrix ?

As a feasibility, I just tried to enable covariance estimation by setting the value of the parameter use_ukf_filtering to true in dynamic_robot_localization_system.launch and I got the covariance matrix data like below:

header:
seq: 40
stamp:
secs: 1298112297
nsecs: 614265000
frame_id: "map"
pose:
pose:
position:
x: 1.2452508024
y: -0.341765669624
z: 1.6733128578
orientation:
x: -0.0327468638118
y: 0.345034015552
z: 0.0571303942339
w: 0.936277356918
covariance: [8.322670749491787e-05, 0.0006038435095328906, -0.0009473106897271304, -0.00032819290830502954, -0.0006521085737453264, 0.00021856153037840112, 0.000283488925504368, 0.002511830504512719, -0.0032960034214290445, -0.0013604571162814418, -0.0022787010965023896, 0.0008267099862323427, -0.0013190780156427688, -0.00811981180556256, 0.025421383814962725, 0.004572977620946034, 0.017065855113389275, -0.002605547557033824, -0.0001655671981052962, -0.0014196339131391188, 0.0020757143863241673, 0.0007778748179944001, 0.0014278397958099834, -0.0004654964513865794, -0.0008864952054467843, -0.005500096214836103, 0.01685953895909498, 0.003093361860456376, 0.011327121049082059, -0.0017713540452520176, 3.984311989951568e-05, 0.0004356486353551097, -0.00016135504424426056, -0.00023152568880322963, -0.00012576586448384512, 0.00017161182060686784]
header:
seq: 41
stamp:
secs: 1298112297
nsecs: 943168000
frame_id: "map"
pose:
pose:
position:
x: 1.31724688542
y: -0.347363685798
z: 1.67021400222
orientation:
x: -0.012969565149
y: 0.3491134697
z: 0.068240990981
w: 0.934502403851
covariance: [3.57334719948985e-05, -2.1386369671865505e-05, 0.0004068811595192397, 7.553497733976124e-06, 0.0002750885346059532, 6.316624945644801e-05, -2.2916889422349365e-05, 0.0006599473546868549, -0.001544524870764529, -0.00035254159267184633, -0.0010983812242726183, -3.173546787617299e-05, 0.0003369943255486637, -0.001526691126086904, 0.007560056471568315, 0.0007926285364053246, 0.0052374648372253, 0.000647597957399242, 9.751129806478871e-06, -0.0003527832366927695, 0.0008076345814603617, 0.0001944605328762297, 0.0005755305238520314, 1.104563210143974e-05, 0.00022793014407367201, -0.0010864500215894337, 0.0052341371756649425, 0.0005655261962178497, 0.0036325806743840723, 0.0004371603333579863, 3.6304608668023816e-05, -2.733987769286224e-05, 0.0005119221386532092, 9.248378889406358e-06, 0.00034686726345715334, 9.208101491518648e-05]

But , in the terminal window 'EuclideanTransformationValidator' related warnings are continuously displaying and point cloud visualization is interrupting in Rviz viewer :

[ WARN] [1616159927.162274823, 1298112284.878350388]: Dropping pointcloud because tf between base_link and odom isn't available
[ WARN] [1616159927.347638437, 1298112285.059382184]: EuclideanTransformationValidator rejected new pose at time 1298112285.059382184 due to max_transformation_distance ->
correction translation: 0.847421 | (max_transformation_distance: 0.13)
correction rotation: 0.567632 | (max_transformation_angle: 0.4)
new pose diff translation: 0.847421 | (max_new_pose_diff_distance: 1.3)
new pose diff rotation: 0.567632 | (max_new_pose_diff_angle: 1.57)
root_mean_square_error: 0.0288139 | (max_root_mean_square_error: 0.05)
root_mean_square_error_reference_pointcloud: 0 | (max_root_mean_square_error_reference_pointcloud: -1)
outliers_percentage: 0.507075 | (max_outliers_percentage: 0.65)
outliers_percentage_reference_pointcloud: 0 | (max_outliers_percentage_reference_pointcloud: -1)
inliers_angular_distribution: 2 | (min_inliers_angular_distribution: 0.125)
outliers_angular_distribution: -2 | (max_outliers_angular_distribution: 0.875)
[ WARN] [1616159927.347692350, 1298112285.059382184]: Discarded cloud because localization couldn't be calculated
[ WARN] [1616159927.347713749, 1298112285.059382184]: Failed point cloud processing with error [FailedPoseEstimation]
[ WARN] [1616159927.513563056, 1298112285.220270194]: EuclideanTransformationValidator rejected new pose at time 1298112285.220270194 due to max_transformation_distance ->
correction translation: 0.421928 | (max_transformation_distance: 0.13)
correction rotation: 0.502953 | (max_transformation_angle: 0.4)
new pose diff translation: 0.421928 | (max_new_pose_diff_distance: 1.3)
new pose diff rotation: 0.502953 | (max_new_pose_diff_angle: 1.57)
root_mean_square_error: 0.0115015 | (max_root_mean_square_error: 0.05)
root_mean_square_error_reference_pointcloud: 0 | (max_root_mean_square_error_reference_pointcloud: -1)
outliers_percentage: 0 | (max_outliers_percentage: 0.65)
outliers_percentage_reference_pointcloud: 0 | (max_outliers_percentage_reference_pointcloud: -1)
inliers_angular_distribution: 2 | (min_inliers_angular_distribution: 0.125)
outliers_angular_distribution: -2 | (max_outliers_angular_distribution: 0.875)
[ WARN] [1616159927.513624557, 1298112285.220270194]: Discarded cloud because localization couldn't be calculated
[ WARN] [1616159927.513653001, 1298112285.220270194]: Failed point cloud processing with error [FailedPoseEstimation]
[ WARN] [1616159927.534715892, 1298112285.250433885]: Trajectory Server: Transform from odom to base_link failed: Could not find a connection between 'odom' and 'base_link' because they are not part of the same tree.Tf has two or more unconnected trees.

To solve these warnings I have modified the values in cluttered_environments_dynamic_large_map_3d.yaml\euclidean_transformation_validator and it removed the EuclideanTransformationValidator related warnings but still point cloud dropping is happening with below warnings:

[ WARN] [1616136733.418682887, 1298112292.123088082]: Dropping pointcloud because tf between base_link and odom isn't available[ WARN] [1616136736.616209652, 1298112295.320244665]: Trajectory Server: Transform from odom to base_link failed: Could not find a connection between 'odom' and 'base_link' because they are not part of the same tree.Tf has two or more unconnected trees

Could you please share the steps or exact configuration settings to enable covariance estimation ?

[carlosmccosta]

Hello,

For enabling covariance estimation only within drl you can set:

<arg name="use_ukf_filtering" default="false" />
<arg name="yaml_configuration_covariance_filename" default="$(find dynamic_robot_localization)/yaml/configs/covariance_estimation/covariance_estimation.yaml" />

For enabling covariance estimation within drl for using the tracking from ukf_localization_node from robot_localization package, you can enable:

<arg name="use_ukf_filtering" default="true" />

It is disabled by default because during the tests I made the time it takes to compute the covariance was longer than the entire drl pipeline and it was not resulting in better tracking when paired with the robot_localization. Also, the covariance estimation accuracy was not high.

As such, if you really need the covariance, you can enabled it in drl.
If you only need the 6 DoF tracking, I suggest keeping it disabled.

Have a nice day,