nebula-autonomy / locus Goto Github PK
View Code? Open in Web Editor NEWRobust Lidar Odometry System
License: MIT License
Robust Lidar Odometry System
License: MIT License
Including :
First of all, thank you very much for a great job, but I am having some problems compiling and would appreciate your help.
After install all required tools (pcl_ros catkin_tools...), i taked locus and common_nebula_slam unzip into catkin_ws/src , my folders are layered as follows:
catkin_ws
src
core_msgs
docker
frontend_utils
geometry_utils
locus
......
and i catkin build locus at catkin_ws/, some errors appeared:
moreover, my environment:Ubuntu18.04 ROS melodic PCL1.8 BOOST1.65
I am looking forward to your reply,thanks
Hi, thank you for sharing your work.
We are testing Locus in different environments with a drone (currently hand carried) with 1 Velodyne VLP-16 and a Vectornav IMU.
Where there are sufficient planes and edges all is well, now we are testing it with a rosbag made in a very monotonous railway tunnel about 50 m long.
Arriving about 9 m from the start Locus hangs for some time until the drone approaches the exit.
In this way it does not track about 25 meters.
Video here: https://www.youtube.com/watch?v=RqD9AQTF9YE
I have tried many combinations of these parameters:
leaf_size: 0.25/0.10/0.20/0.30/0.50
recompute_covariance_local_map: true/false
icp_covariance_method: 1/0
icp_max_covariance: 0.01/0.1
Iterate ICP this many times.
iterations: 30/25/20/15/10
Radius for normal search
normal_search_radius: 10/20/30
I did not get much difference.
Do you know of any other configurations to try in this environment?
If this, as with other slams, is a difficult environment does a second point cloud have a good chance of solving the problem?
And, since we need the first lidar horizontal for collision avoidance, what orientation do you think would be optimal for a second lidar?
See you soon.
Cleanup comments, the less the better, just keep the very relevant ones
Hi, thanks for sharing this great work!
I have been trying run the demo using dataset C of husky. However, It just stuck. I found something weird in rviz:
I followed the instruction's data structure as printed below:
├── C_Husky4_Urban_Alpha1
│ ├── fiducial_calibration_husky4.yaml
│ ├── h4_a1_gt_map.pcd
│ ├── h4_a1_gt_map.ply
│ ├── husky4_sensors.yaml
│ ├── locus_test_01
│ │ ├── delay.txt
│ │ ├── lidar_callback_duration.bag.active
│ │ ├── odometry.bag.active
│ │ ├── rate.txt
│ │ ├── scan_to_map_duration.bag.active
│ │ ├── scan_to_scan_duration.bag.active
│ │ └── stationary.bag.active
│ ├── odometry.bag
│ └── rosbag
│ ├── husky4_lidar_2020-02-21-15-04-16_0.bag
│ ├── imu.bag
│ └── odometry.bag
However, the odometry topic seems to be empty when I run rostopic echo, I am now trying fix it, do you have any idea? Thanks!
Hi,
The launch file provides us with the parameter : b_pub_odom_on_timer.
I was a bit lost on its importance.
Added to this, in lines 430-436 of locus.cc we are defining the variable "stamp"
if (b_adaptive_input_voxelization_) {
ApplyAdaptiveInputVoxelization(msg);
}
CheckMsgDropRate(msg);
ros::Time stamp = pcl_conversions::fromPCL(msg->header.stamp);
Please correct me if I am wrong. This time stamp is used to acquire the corresponding odometry and IMU messages from the buffer. In other words, is this the time stamp that is used in the health monitor and the subsequent lidar-odometry and mapping?
Have videos showing complete runs of datasets it sped-up mode
Often lidar odometry algorithms have an output tf that helps for visualization + comparisons.
In our system, we removed tf publishing to be in the transform manager.
Should we:
Hi, thank you publicly sharing your work.
I have tested your default data sets and they have been working fine. Then I have been trying to integrate the system with one of our robots which is a 2 wheel differential driven robot with two casters with a Velodyne VLP-16 and an IMU running at 20Hz. We were testing in a warehouse which is a n50m X 30m environment.
But this time I get an error ""Failed to find eigen values when computing ICP covariance. " which seems the localization is failing. Is there any pointers to fix this issue.
The rosbag file can be downloaded from here
sensors.yaml file is as follows.
velodyne: name: velodyne parent: arch position: x: -0.17 y: 0.0 z: 0.50 orientation: r: 0.0 p: 0.0 y: 3.14 rate: 10 topic_name: velodyne_points vn100: name: vn100 parent: arch topic_name: imu/data position: x: 0.20 y: 0.00 z: 0.23 orientation: r: 0 p: 0 y: 1.571 rate: 20
localization parameters are as follows.
localization: registration_method: gicp compute_icp_covariance: true icp_covariance_method: 1 icp_max_covariance: 0.01 compute_icp_observability: false tf_epsilon: 0.00001 corr_dist: 0.5 iterations: 25 transform_thresholding: true max_translation: 1.0 max_rotation: 1.0 enable_timing_output: false normal_search_radius: 10 recompute_covariance_local_map: false recompute_covariance_scan: false
robot description
<xacro:include filename="$(find sensor_description)/urdf/sensors/VLP-16.urdf.xacro" />
<xacro:VLP-16
parent="${sensor_props['velodyne']['parent']}"
name="${sensor_props['velodyne']['name']}"
topic="${sensor_props['velodyne']['topic_name']}"
hz="${sensor_props['velodyne']['rate']}"
samples="440"
noise="0.016"
gpu="true">
</xacro:VLP-16>
<xacro:include filename="$(find sensor_description)/urdf/sensors/imu_vn100.urdf.xacro"/>
<xacro:vn100
robot_namespace="$(arg robot_namespace)"
name="${sensor_props['vn100']['name']}"
topic_name="${sensor_props['vn100']['topic_name']}"
hz="${sensor_props['vn100']['rate']}"
parent="imu_link">
</xacro:vn100>
Lo_settings has been updated to match the frames of the datalogs.
`frame_id:
fixed: $(arg robot_namespace)/map
base: $(arg robot_namespace)/base_link
imu: imu_link
bd_odometry: odom
artifacts_in_global: true
frame_conversions:
b_convert_imu_to_base_link_frame: true`
Hi, Thanks for sharing this great work!
I am now trying to use LOCUS on my own Lidar dataset. However, as said in the repo, there is no MDC module provided.
I want to know if I just add a deskew module from another repo, can I fix this problem? Or is there anything else to take care of?
Thanks!
Optional to remove the point cloud filter. I prefer to keep it in case users want to try other filter combinations (currently it is not used at all)
Hi,
I was hoping to use a realsense pointcloud as input for LoCUS. Can you please tell me if this would be possible.
I have converted the realsense's depth cloud to a pointcloud using http://wiki.ros.org/depth_image_proc. I have also made the necessary TF changes. However, I keep running into the error
[pcl::MultithreadedGeneralizedIterativeClosestPoint::" "computeCovariances] Number or points in cloud (%lu) is less " "than k_correspondences_ (%lu)!\n"
which is in line 70 of gicp.hpp.
Can you please tell me if there should be parameter change to incorporate the use of realsense?
For the time being imu preintegration relies on the orientation field from sensor_msgs/IMU, but in theory we could use somehow angular velocity to have it for init guess (some sensors do not publish that)
Copying from old todo on the readme
Create steps to test the sim
A line of code in LoFrontend::GetImuQuaternion
caught my eye:
LOCUS/lo_frontend/src/LoFrontend.cc
Line 809 in 16d147e
This line of code transforms the IMU orientation into the base link frame, and only runs if the user has set frame_conversions/b_convert_imu_to_base_link_frame
to true
(it is false
in lo_frames.yaml
). The line of code does: q' = q_e * q * q_e^-1 where q is the imu data (in the IMU frame), q' is the imu data transformed to the base link frame, and q_e is the extrinsic mapping the rotation from IMU to base link.
I had recently seen how the IMU to lidar frame conversion is done in LIO-SAM at https://github.com/TixiaoShan/LIO-SAM/blob/0cdc9e8b010fff947275d68aa3545062afb059da/include/utility.h#L269 where they do q' = q * q_e and so this line stuck out to me because of the different operatoin.
My understanding of quaternion math is pretty basic, so forgive me if I am completely wrong here, but after looking at a couple of online resources, I think that the q' = pqp^-1 operation is for rotating vectors and q' = q*p is for chaining rotations. Having said that, when running LIO-SAM I did have to pre-transform the IMU data into a body frame (from an NED frame) in a pre-processing step before applying the extrinsic in their code to get things working, so I'm not completely confident in my assessment.
It would be good to get your thoughts on this and/or confirm that LOCUS is doing the right thing.
Thank you for the excellent work your team has done! @BenjaminMorrell @femust . How to quickly get the evaluation result of trajectory APE using the odometry.bag (GT) and odometry.bag obtained by LOCUS? Is there anything to watch out for? Such as time alignment or coordinate transformation.
Hello,
I am trying to run LOCUS on my dataset. My dataset consists in a rosbag with imu (vectornav vn100), 3D lidar (VLP16), odometry topic (from a visual slam, type nav_msgs/Odometry) ad a camera image.
When I try to run locus alone and start the rosbag, I get this output:
roslaunch locus locus.launch
... logging to /var/log/extars/ros/6dabe0b2-09a6-11ed-bbec-54b203083f30/roslaunch-extars-28081.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
xacro: in-order processing became default in ROS Melodic. You can drop the option.
Using load_yaml() directly is deprecated. Use xacro.load_yaml() instead.
started roslaunch server http://extars:45129/
SUMMARY
========
PARAMETERS
* /extars/body_filter/max_x: 0.6
* /extars/body_filter/max_y: 0.5
* /extars/body_filter/max_z: 1.8
* /extars/body_filter/min_x: -1.0
* /extars/body_filter/min_y: -0.5
* /extars/body_filter/min_z: -1.6
* /extars/body_filter/rotation: 0.0
* /extars/extars/robot_description: <?xml version="1....
* /extars/extars/tf_prefix: extars
* /extars/locus/b_adaptive_input_voxelization: True
* /extars/locus/b_add_keyframes_enabled: True
* /extars/locus/b_debug_transforms: False
* /extars/locus/b_enable_computation_time_profiling: True
* /extars/locus/b_enable_msw: True
* /extars/locus/b_integrate_interpolated_odom: True
* /extars/locus/b_is_flat_ground_assumption: False
* /extars/locus/b_pub_odom_on_timer: False
* /extars/locus/b_publish_xy_cross_section: True
* /extars/locus/b_run_with_gt_point_cloud: False
* /extars/locus/b_sub_to_lsm: False
* /extars/locus/b_verbose: False
* /extars/locus/box_filter_size: 20
* /extars/locus/buffers/imu_buffer_size_limit: 100
* /extars/locus/buffers/odometry_buffer_size_limit: 100
* /extars/locus/data_integration/mode: 0
* /extars/locus/filtering/decimate_percentage: 0.9
* /extars/locus/filtering/decimate_percentage_open_space: 0.93
* /extars/locus/filtering/extract_features: False
* /extars/locus/filtering/grid_filter: False
* /extars/locus/filtering/grid_res: 0.2
* /extars/locus/filtering/outlier_filter: False
* /extars/locus/filtering/outlier_knn: 10
* /extars/locus/filtering/outlier_std: 1.0
* /extars/locus/filtering/radius: 0.15
* /extars/locus/filtering/radius_filter: False
* /extars/locus/filtering/radius_knn: 3
* /extars/locus/filtering/random_filter: False
* /extars/locus/frame_conversions/b_convert_imu_to_base_link_frame: True
* /extars/locus/frame_id/artifacts_in_global: True
* /extars/locus/frame_id/base: extars/base_link
* /extars/locus/frame_id/bd_odometry: extars/odom
* /extars/locus/frame_id/fixed: extars/map
* /extars/locus/frame_id/imu: extars/imu_link
* /extars/locus/gt_point_cloud_filename: /home/costar/data...
* /extars/locus/icp/corr_dist: 1.0
* /extars/locus/icp/enable_timing_output: False
* /extars/locus/icp/iterations: 20
* /extars/locus/icp/max_rotation: 1.0
* /extars/locus/icp/max_translation: 1.0
* /extars/locus/icp/num_threads: 4
* /extars/locus/icp/recompute_covariances: False
* /extars/locus/icp/registration_method: gicp
* /extars/locus/icp/tf_epsilon: 0.001
* /extars/locus/icp/transform_thresholding: True
* /extars/locus/localization/compute_icp_covariance: True
* /extars/locus/localization/compute_icp_observability: False
* /extars/locus/localization/corr_dist: 0.2
* /extars/locus/localization/enable_timing_output: False
* /extars/locus/localization/icp_covariance_method: 1
* /extars/locus/localization/icp_max_covariance: 0.01
* /extars/locus/localization/iterations: 20
* /extars/locus/localization/max_rotation: 1.0
* /extars/locus/localization/max_translation: 1.0
* /extars/locus/localization/normal_search_radius: 10
* /extars/locus/localization/num_threads: 4
* /extars/locus/localization/recompute_covariance_local_map: False
* /extars/locus/localization/recompute_covariance_scan: False
* /extars/locus/localization/registration_method: gicp
* /extars/locus/localization/tf_epsilon: 1e-05
* /extars/locus/localization/transform_thresholding: True
* /extars/locus/map/b_publish_map_info: False
* /extars/locus/map/b_publish_only_with_subscribers: True
* /extars/locus/map/default_map_buffer_max_size: 7
* /extars/locus/map/octree_resolution: 0.001
* /extars/locus/map/volume_voxel_size: 1.024
* /extars/locus/map_publishment/b_publish_map: True
* /extars/locus/map_publishment/meters: 1
* /extars/locus/mapper/num_threads: 4
* /extars/locus/number_of_points_open_space: 9999999
* /extars/locus/odom_pub_rate: 10
* /extars/locus/points_to_process_in_callback: 3000
* /extars/locus/publish_diagnostics: True
* /extars/locus/queues/imu_queue_size: 100
* /extars/locus/queues/lidar_queue_size: 1
* /extars/locus/queues/odom_queue_size: 100
* /extars/locus/robot_name: extars
* /extars/locus/rotation_threshold_closed_space_kf: 0.3
* /extars/locus/rotation_threshold_kf: 0.3
* /extars/locus/rotation_threshold_open_space_kf: 0.6
* /extars/locus/rotational_velocity_threshold: 1
* /extars/locus/sensor_health_timeout: 0.4
* /extars/locus/statistics_time_window: 5
* /extars/locus/statistics_verbosity_level: low
* /extars/locus/tf_prefix: extars
* /extars/locus/transform_wait_duration: 0.01
* /extars/locus/translation_threshold_closed_space_kf: 1.0
* /extars/locus/translation_threshold_kf: 1.0
* /extars/locus/translation_threshold_msw: 5
* /extars/locus/translation_threshold_open_space_kf: 2.0
* /extars/locus/translational_velocity_threshold: 0.1
* /extars/locus/velocity_buffer_size: 10
* /extars/locus/wait_for_odom_transform_timeout: 0.1
* /extars/locus/window_local_mapping_type: mapper
* /extars/locus/xy_cross_section_threshold: 2500
* /extars/normal_computation/num_threads: 4
* /extars/transform_points_base_link/filter_field_name: z
* /extars/transform_points_base_link/filter_limit_max: 100
* /extars/transform_points_base_link/filter_limit_min: -100
* /extars/transform_points_base_link/output_frame: extars/base_link
* /extars/voxel_grid/filter_field_name: z
* /extars/voxel_grid/filter_limit_max: 100
* /extars/voxel_grid/filter_limit_min: -100
* /extars/voxel_grid/filter_limit_negative: False
* /extars/voxel_grid/leaf_size: 0.25
* /extars/voxel_grid/output_frame: extars/base_link
* /rosdistro: melodic
* /rosversion: 1.14.13
NODES
/
odometry_republish (locus/odometry_republish.py)
vectornav_republish (locus/vectornav_republish.py)
/extars/
body_filter (nodelet/nodelet)
locus (locus/locus_node)
nodelet_manager (nodelet/nodelet)
normal_computation (nodelet/nodelet)
transform_points_base_link (nodelet/nodelet)
voxel_grid (nodelet/nodelet)
/extars/extars/
robot_state_publisher_extars_28081_4633833944357679098 (robot_state_publisher/robot_state_publisher)
ROS_MASTER_URI=http://localhost:11311
process[extars/extars/robot_state_publisher_extars_28081_4633833944357679098-1]: started with pid [28117]
process[extars/locus-2]: started with pid [28118]
process[extars/transform_points_base_link-3]: started with pid [28119]
process[extars/nodelet_manager-4]: started with pid [28126]
process[extars/body_filter-5]: started with pid [28129]
process[extars/voxel_grid-6]: started with pid [28134]
process[extars/normal_computation-7]: started with pid [28135]
process[vectornav_republish-8]: started with pid [28141]
[ INFO] [1658497761.825094395]: Loading nodelet /extars/voxel_grid of type point_cloud_filter/CustomVoxelGrid to manager nodelet_manager with the following remappings:
[ INFO] [1658497761.825719007]: /extars/voxel_grid/input -> /extars/body_filter/output
[ INFO] [1658497761.826968117]: waitForService: Service [/extars/nodelet_manager/load_nodelet] has not been advertised, waiting...
process[odometry_republish-9]: started with pid [28157]
[ INFO] [1658497761.847798486]: waitForService: Service [/extars/nodelet_manager/load_nodelet] is now available.
[ INFO] [1658497761.893490299]: Locus::Initialize
[ INFO] [1658497761.906050224]: PointCloudOdometry - Initialize
[ INFO] [1658497761.906096521]: PointCloudOdometry - LoadParameters
[ WARN] [1658497761.907359380]: /extars/locus: Failed to search for parameter 'fiducial_calibration/position/x'.
[ WARN] [1658497761.907754290]: /extars/locus: Failed to search for parameter 'fiducial_calibration/position/y'.
[ WARN] [1658497761.908131623]: /extars/locus: Failed to search for parameter 'fiducial_calibration/position/z'.
[ WARN] [1658497761.908492397]: /extars/locus: Failed to search for parameter 'fiducial_calibration/orientation/x'.
[ WARN] [1658497761.908852964]: /extars/locus: Failed to search for parameter 'fiducial_calibration/orientation/y'.
[ WARN] [1658497761.909207968]: /extars/locus: Failed to search for parameter 'fiducial_calibration/orientation/z'.
[ WARN] [1658497761.909582912]: /extars/locus: Failed to search for parameter 'fiducial_calibration/orientation/w'.
[ WARN] [1658497761.923560601]: Can't find fiducials, using origin
[ INFO] [1658497761.923600350]: PointCloudOdometry - RegisterCallbacks
[ INFO] [1658497761.928339872]: PointCloudOdometry - SetupICP
[ INFO] [1658497761.928374459]: RegistrationMethod::GICP activated.
[ INFO] [1658497761.928437935]: GICP
[ INFO] [1658497761.928463325]: getMaxCorrespondenceDistance: 1
[ INFO] [1658497761.928479733]: getMaximumIterations: 20
[ INFO] [1658497761.928499676]: getTransformationEpsilon: 0.001
[ INFO] [1658497761.928516765]: getEuclideanFitnessEpsilon: 0.005
[ INFO] [1658497761.928531274]: Ransac: 0
[ INFO] [1658497761.928545951]: getRANSACIterations: 0
[ INFO] [1658497761.928561101]: ROTATION EPSILION: 0.002
[ INFO] [1658497761.928577956]: getCorrespondenceRandomness:20
[ INFO] [1658497761.928591456]: getMaximumOptimizerIterations:20
[ INFO] [1658497761.928606934]: getConvergeCriteria:0x5601f407b460
[ INFO] [1658497761.928622116]: RANSACOutlie: 0.05
[ INFO] [1658497761.928635933]: CLASS NAME: MultithreadedGeneralizedIterativeClosestPoint
[ INFO] [1658497761.928666491]: PointCloudLocalization - LoadParameters
[ WARN] [1658497761.931287005]: /extars/locus: Failed to search for parameter 'fiducial_calibration/position/x'.
[ WARN] [1658497761.931646403]: /extars/locus: Failed to search for parameter 'fiducial_calibration/position/y'.
[ WARN] [1658497761.932002469]: /extars/locus: Failed to search for parameter 'fiducial_calibration/position/z'.
[ WARN] [1658497761.932341422]: /extars/locus: Failed to search for parameter 'fiducial_calibration/orientation/x'.
[ WARN] [1658497761.932700739]: /extars/locus: Failed to search for parameter 'fiducial_calibration/orientation/y'.
[ WARN] [1658497761.933046714]: /extars/locus: Failed to search for parameter 'fiducial_calibration/orientation/z'.
[ WARN] [1658497761.933393085]: /extars/locus: Failed to search for parameter 'fiducial_calibration/orientation/w'.
[ WARN] [1658497761.950052801]: Can't find fiducials, using origin
[ INFO] [1658497761.954335861]: PointCloudLocalization - SetupICP
[ INFO] [1658497761.954417290]: RegistrationMethod::GICP activated.
[ INFO] [1658497761.954486440]: GICP activated.
[ INFO] [1658497761.954539493]: MaxCorrespondenceDistance: 0.2
[ INFO] [1658497761.954584835]: MaximumIterations: 20
[ INFO] [1658497761.954630136]: TransformationEpsilon: 1e-05
[ INFO] [1658497761.954674394]: EuclideanFitnessEpsilon: 0.01
[ INFO] [1658497761.954718245]: RANSACIterations: 0
[ INFO] [1658497761.954766369]: RotationEpsilon 0.002
[ INFO] [1658497761.954809102]: CorrespondenceRandomness:20
[ INFO] [1658497761.954851512]: MaximumOptimizerIterations:50
[ INFO] [1658497761.954895828]: ConvergeCriteria:0x5601f40844e0
[ INFO] [1658497761.954940364]: RANSACOutlierRejectionThreshold: 0.05
[ INFO] [1658497761.954984364]: Locus::LoadParameters
[ INFO] [1658497761.999801117]: b_integrate_interpolated_odom_: 1
[ INFO] [1658497761.999894934]: MappingMethod::MAPPER activated.
[ INFO] [1658497761.999963894]: Setting up number threads for local mapping: 4
[ INFO] [1658497762.015930592]: No integration requested
[ INFO] [1658497762.015969254]: Locus::RegisterCallbacks
[ INFO] [1658497762.026594844]: Locus::CreatePublishers
[ INFO] [1658497762.033209693]: Locus::LoadCalibrationFromTfTree
[ INFO] [1658497762.133604480]: Loaded pose_sensor to imu calibration B_T_L:
-0.19
0
-0.149
1 0 0
0 1 0
0 0 1
[ INFO] [1658497762.133705735]: q: x: 0.000, y: 0.000, z: 0.000, w: 1.000
[ INFO] [1658497762.137128002]: [Locus::setAsyncSpinners] : New subscriber for IMU
[ INFO] [1658497762.138932859]: [Locus::setAsyncSpinners] : New subscriber for odom
[ INFO] [1658497762.140212971]: [Locus::setAsyncSpinners] : New subscriber for lidar
[INFO] [1658497762.262007]: waiting for subscribed topics to be ready
[INFO] [1658497762.279811]: waiting for subscribed topics to be ready
Failed to find match for field 'normal_x'.
Failed to find match for field 'normal_y'.
Failed to find match for field 'normal_z'.
Failed to find match for field 'curvature'.
locus_node: /build/pcl-OilVEB/pcl-1.8.1+dfsg1/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp:136: int pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch(const PointT&, int, std::vector<int>&, std::vector<float>&) const [with PointT = pcl::PointXYZINormal; Dist = flann::L2_Simple<float>]: Assertion `point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"' failed.
locus_node: /build/pcl-OilVEB/pcl-1.8.1+dfsg1/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp:136: int pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch(const PointT&, int, std::vector<int>&, std::vector<float>&) const [with PointT = pcl::PointXYZINormal; Dist = flann::L2_Simple<float>]: Assertion `point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"' failed.
locus_node: /build/pcl-OilVEB/pcl-1.8.1+dfsg1/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp:136: int pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch(const PointT&, int, std::vector<int>&, std::vector<float>&) const [with PointT = pcl::PointXYZINormal; Dist = flann::L2_Simple<float>]: Assertion `point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"' failed.
[extars/locus-2] process has died [pid 28118, exit code -6, cmd /home/ctrazzi/locus_ws/devel/lib/locus/locus_node ~LIDAR_TOPIC:=/velodyne_points ~IMU_TOPIC:=/vectornav_base_link ~SPACE_MONITOR_TOPIC:=/localizer_space_monitor/xy_cross_section __name:=locus __log:=/var/log/extars/ros/6dabe0b2-09a6-11ed-bbec-54b203083f30/extars-locus-2.log].
log file: /var/log/extars/ros/6dabe0b2-09a6-11ed-bbec-54b203083f30/extars-locus-2*.log
[INFO] [1658497766.283824]: republish measure vectornav
[INFO] [1658497767.265861]: republish measure odometry
^C[odometry_republish-9] killing on exit
[vectornav_republish-8] killing on exit
[extars/normal_computation-7] killing on exit
[extars/voxel_grid-6] killing on exit
[extars/body_filter-5] killing on exit
[ INFO] [1658497783.787002610]: Unloading nodelet /extars/voxel_grid from manager nodelet_manager
[extars/transform_points_base_link-3] killing on exit
[extars/extars/robot_state_publisher_extars_28081_4633833944357679098-1] killing on exit
[extars/nodelet_manager-4] killing on exit
Exception in thread Thread-8:
Traceback (most recent call last):
File "/usr/lib/python2.7/threading.py", line 801, in __bootstrap_inner
self.run()
File "/usr/lib/python2.7/threading.py", line 754, in run
self.__target(*self.__args, **self.__kwargs)
File "/home/ctrazzi/locus_ws/src/LOCUS/locus/scripts/vectornav_republish.py", line 47, in send_measure
imu_pub.publish(vn_imu)
File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/topics.py", line 882, in publish
self.impl.publish(data)
File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/topics.py", line 1041, in publish
raise ROSException("publish() to a closed topic")
ROSException: publish() to a closed topic
shutting down processing monitor...
... shutting down processing monitor complete
done
And when I try to run it following your example I get:
~/locus_ws/src/LOCUS/tmuxp_config$ tmuxp load extars_sanpretronilla_run.yaml
Traceback (most recent call last):
File "/usr/bin/tmuxp", line 11, in <module>
load_entry_point('tmuxp==1.3.5', 'console_scripts', 'tmuxp')()
File "/home/ctrazzi/.local/lib/python3.6/site-packages/pkg_resources/__init__.py", line 479, in load_entry_point
return get_distribution(dist).load_entry_point(group, name)
File "/home/ctrazzi/.local/lib/python3.6/site-packages/pkg_resources/__init__.py", line 2861, in load_entry_point
return ep.load()
File "/home/ctrazzi/.local/lib/python3.6/site-packages/pkg_resources/__init__.py", line 2465, in load
return self.resolve()
File "/home/ctrazzi/.local/lib/python3.6/site-packages/pkg_resources/__init__.py", line 2471, in resolve
module = __import__(self.module_name, fromlist=['__name__'], level=0)
File "/usr/lib/python3/dist-packages/tmuxp/__init__.py", line 20, in <module>
from . import config, util, cli
File "/usr/lib/python3/dist-packages/tmuxp/cli.py", line 550, in <module>
help='Like -2, but indicates that the terminal supports 88 colours.')
File "/home/ctrazzi/.local/lib/python3.6/site-packages/click/decorators.py", line 223, in decorator
_param_memo(f, ArgumentClass(param_decls, **attrs))
File "/home/ctrazzi/.local/lib/python3.6/site-packages/click/core.py", line 2905, in __init__
super().__init__(param_decls, required=required, **attrs)
File "/home/ctrazzi/.local/lib/python3.6/site-packages/click/core.py", line 2030, in __init__
param_decls or (), expose_value
File "/home/ctrazzi/.local/lib/python3.6/site-packages/click/core.py", line 2941, in _parse_decls
"Arguments take exactly one parameter declaration, got"
TypeError: Arguments take exactly one parameter declaration, got 2.
Can you please help me to make it work?
Can it be realated to the no dense pointcloud?
Hi,
I want to create a ground truth trajectory with LOCUS. I already managed to sucessfully load a prior scanned map into LOCUS and record the odometry msgs for further use with evo. Are there some parameters other than the b_run_with_gt_point_cloud and b_add_keyframes_enabled that have to be changed for LOCUS to run at maximal accuracy sacrificing real-time, as mentioned in the odometry dataset?
Thank you in advance!
Hello, I have finally manage to run your example dataset.
Now I am trying to run mine, but I can not remap the topic correctly.
When I launch my modified tmux scrip I get the following output from the two terminals:
first terminal:
rosparam set /use_sim_time true
sleep $TIME_TO_MAP_SAVE; rosrun pcl_ros pointcloud_to_pcd input:=/$ROBOT_NAME/locus/octree_map _prefix:=$DATA_PATH/locus_$RUN_NUMBER/map_locus
ctrazzi@extars:~/locus_ws/src/LOCUS/tmuxp_config$ rosparam set /use_sim_time true
ctrazzi@extars:~/locus_ws/src/LOCUS/tmuxp_config$ sleep $TIME_TO_MAP_SAVE; rosrun pcl_ros pointcloud_to_pcd input:=/$ROBOT_NAME/locus/octree_map _prefix:=$DATA_PATH/locus_$RUN_NUMBER/map_locus
second terminal:
rosparam set /use_sim_time true
ctrazzi@extars:~/locus_ws/src/LOCUS/tmuxp_config$ rosparam set /use_sim_time true
This is the rviz output:
And this is the rqt_graph:
I think this is a remapping problem, because the rosbag topics are non connected to locus.
This is my run_locus.yaml
session_name: locus_dataset
environment:
# Path for the dataset
DATA_PATH: /home/ctrazzi/data/Sanp
DATA_FOLDER: rosbag/
# Change the run number to save the output data in a different folder
RUN_NUMBER: "test_01"
# version of logging. If unsure, select 2 (the latest). 1 is for tunnel datasets
LOG_VERSION: "3"
# Number of lidars to use (if available)
NUM_VLP: "1"
# Change the robot name and type to match the dataset
ROBOT_NAME: extars
ROBOT_TYPE: extars
# If unsure, leave as base1
BASE_NAME: base1
# Rosbag parameters
START: "0"
RATE: "1.0"
# See https://github.com/NeBula-Autonomy/nebula-odometry-dataset/blob/main/pages/dataset.md
# for the expected length of datasets
# This param can be used to automatically close the session at the end of the run (currently disabled)
TIME_TO_END: "600"
# This param is used to start saving the map near the end of the run (try 1 minute before the end)
TIME_TO_MAP_SAVE: "500"
# The topic for the odometry to record
ODOM: locus/odometry
options:
default-command: /bin/bash
windows:
- window_name: locus
focus: true
layout: tiled
shell_command_before:
- rosparam set /use_sim_time true
# Set up the parameter files from the GT products
- cp $DATA_PATH/fiducial_calibration_$ROBOT_NAME.yaml ~/.ros/
- if [ $ROBOT_TYPE = 'husky' ]; then
cp $DATA_PATH/$ROBOT_NAME*.yaml $(rospack find sensor_description)/config/ ;
fi;
- if [ $ROBOT_TYPE = 'spot' ]; then
cp $DATA_PATH/$ROBOT_NAME*.yaml $(rospack find sensor_description)/config/ ;
fi;
- if [ $ROBOT_TYPE = 'extars' ]; then
cp $DATA_PATH/$ROBOT_NAME*.yaml $(rospack find sensor_description)/config/ ;
fi;
panes:
################################### RUN ###################################
# Bags and topics of interest (switching if the log versions is older)
- if [ $LOG_VERSION -eq 2 ]; then
sleep 10; rosbag play $DATA_PATH/$DATA_FOLDER*.bag -s$START -r$RATE --clock --topics clock:=/clock
/$ROBOT_NAME/velodyne_points /$ROBOT_NAME/velodyne_front/velodyne_points /$ROBOT_NAME/velodyne_rear/velodyne_points
/$ROBOT_NAME/hero/wio_ekf/odom /$ROBOT_NAME/vn100/imu_wori_wcov /$ROBOT_NAME/visual_odom
/$ROBOT_NAME/wheel_odom /$ROBOT_NAME/hvm/lidar/points /$ROBOT_NAME/hvm/odometry
/$ROBOT_NAME/unreconciled_artifact /$ROBOT_NAME/uwb_frontend/range_measurements ;
fi;
if [ $LOG_VERSION -eq 1 ]; then
sleep 10;rosbag play $DATA_PATH/$DATA_FOLDER*.bag -s$START -r$RATE --clock --prefix $ROBOT_NAME/
--topics clock:=/clock velodyne_points velodyne_front/velodyne_points /velodyne_rear/velodyne_points
hero/wio_ekf/odom vn100/imu_wori_wcov vn100/imu visual_odom /tf_static ;
fi;
- if [ $LOG_VERSION -eq 3 ]; then
rosbag play $DATA_PATH/$DATA_FOLDER/test_manuale.bag ;
fi;
# launch rviz
- rviz -d $(rospack find locus)/rviz/$(echo $ROBOT_NAME)_locus.rviz
# Front-end
- if [ $ROBOT_TYPE = 'husky' ]; then
roslaunch locus locus.launch robot_namespace:=$ROBOT_NAME number_of_velodynes:=$NUM_VLP;
fi;
if [ $ROBOT_TYPE = 'spot' ]; then
roslaunch locus locus.launch robot_namespace:=$ROBOT_NAME;
fi
- if [ $ROBOT_TYPE = 'extars' ]; then
roslaunch locus locus_extars.launch robot_namespace:=$ROBOT_NAME number_of_velodynes:=$NUM_VLP;
fi;
- window_name: record_at_end
focus: false
layout: tiled
shell_command_before:
- rosparam set /use_sim_time true
panes:
# Map
# Front-End Map
- sleep $TIME_TO_MAP_SAVE; rosrun pcl_ros pointcloud_to_pcd input:=/$ROBOT_NAME/locus/octree_map _prefix:=$DATA_PATH/locus_$RUN_NUMBER/map_locus
# ..
-
- window_name: record
focus: false
layout: tiled
shell_command_before:
- mkdir $DATA_PATH/locus_$RUN_NUMBER
- rosparam set /use_sim_time true
panes:
################################### RECORD ###################################
# Odometry
- rosbag record -O $DATA_PATH/locus_$RUN_NUMBER/odometry.bag /$ROBOT_NAME/$ODOM /$ROBOT_NAME/locus/localization_integrated_estimate /$ROBOT_NAME/locus/odometry_integrated_estimate /$ROBOT_NAME/locus/localization_incremental_estimate /$ROBOT_NAME/locus/odometry_incremental_estimate
- rosbag record -O $DATA_PATH/locus_$RUN_NUMBER/stationary.bag /$ROBOT_NAME/stationary_accel
# Odometry Rate and Delay
- rostopic hz /$ROBOT_NAME/$ODOM -w3 >> $DATA_PATH/locus_$RUN_NUMBER/rate.txt
- rostopic delay /$ROBOT_NAME/$ODOM -w3 >> $DATA_PATH/locus_$RUN_NUMBER/delay.txt
# Computation Time Profiling
- rosbag record -O $DATA_PATH/locus_$RUN_NUMBER/lidar_callback_duration.bag /$ROBOT_NAME/locus/lidar_callback_duration
- rosbag record -O $DATA_PATH/locus_$RUN_NUMBER/scan_to_scan_duration.bag /$ROBOT_NAME/locus/scan_to_scan_duration
- rosbag record -O $DATA_PATH/locus_$RUN_NUMBER/scan_to_map_duration.bag /$ROBOT_NAME/locus/scan_to_submap_duration
################################ END RECORDING ###############################
# Uncomment the line below to automatically end the run
# - sleep $TIME_TO_END; rosnode kill -a
# Use the line below to prepare a kill command when you want to stop
- rosnode kill -a \
And this is my locus.launch:
launch>
<arg name="robot_namespace" default="extars"/>
<arg name="use_gdb" default="true"/>
<arg name="nodelet_manager" value="nodelet_manager"/>
<arg name="launch_prefix" value="gdb -ex run --args" if="$(arg use_gdb)"/>
<arg name="launch_prefix" value="" unless="$(arg use_gdb)"/>
<arg name="nodelet_args" value="--no-bond"/>
<arg name="respawn" value="false" if="$(arg use_gdb)"/>
<arg name="respawn" value="true" unless="$(arg use_gdb)"/>
<arg name="robot_type" value="extars" if="$(eval robot_namespace.startswith('extars'))"/>
<arg name="number_of_velodynes" default="1" />
<arg name="b_use_multiple_pc" value="$(eval arg('number_of_velodynes') > 1)"/>
<arg name="pc_input" value="velodyne_points"/>
<!-- 0:TOP, 1:FRONT, 2:REAR -->
<arg name="pc_trans_in_0" default="velodyne_points/transformed"/>
<arg name="pc_trans_in_1" default="velodyne_front/velodyne_points/transformed"/>
<arg name="pc_trans_in_2" default="velodyne_rear/velodyne_points/transformed"/>
<group ns="$(arg robot_namespace)">
<!-- Load parameters -->
<rosparam file="$(find locus)/config/body_filter_params_$(arg robot_type).yaml"
subst_value="true"/>
<!-- Load robot description -->
<include file="$(find locus)/launch/robot_description_extars.launch">
<arg name="robot_namespace" value="$(arg robot_namespace)"/>
</include>
<node
pkg="locus"
name="locus"
type="locus_node"
output="screen">
<remap from="~LIDAR_TOPIC" to="$(arg pc_input)"/>
<remap from="~ODOMETRY_TOPIC" to="lvi_sam/vins/odometry/odometry" if="$(eval robot_namespace.startswith('extars'))"/>
<remap from="~IMU_TOPIC" to="vectornav/IMU"/>
<remap from="~POSE_TOPIC" to="not_currently_used"/>
<remap from="~SPACE_MONITOR_TOPIC" to="localizer_space_monitor/xy_cross_section"/>
<!-- For Sim -->
<!-- <remap from="~ODOMETRY_TOPIC" to="wheel_odom"/> -->
<remap from="/diagnostics" to="/diagnostics"/>
<param name="robot_name" value="$(arg robot_namespace)"/>
<param name="tf_prefix" value="$(arg robot_namespace)"/>
<param name="b_integrate_interpolated_odom" value="false" if="$(eval robot_namespace.startswith('extars'))"/>
<rosparam file="$(find locus)/config/lo_settings_extars.yaml" />
<param name="b_pub_odom_on_timer" value="false" if="$(eval robot_namespace.startswith('extars'))"/>
<rosparam file="$(find locus)/config/lo_frames_extars.yaml" subst_value="true"/>
<rosparam file="$(find point_cloud_filter)/config/parameters_extars.yaml"/>
<rosparam file="$(find point_cloud_odometry)/config/parameters_extars.yaml"/>
<rosparam file="$(find point_cloud_localization)/config/parameters_extars.yaml" if="$(eval robot_type == 'extars')"/>
<rosparam file="$(find point_cloud_mapper)/config/parameters_extars.yaml"/>
<param name="localization/num_threads" value="4" if="$(eval robot_namespace.startswith('extars'))" />
<param name="icp/num_threads" value="4" if="$(eval robot_namespace.startswith('extars'))" />
<param name="mapper/num_threads" value="4" if="$(eval robot_namespace.startswith('extars'))" />
</node>
<node pkg="locus" name="sensors_health_monitor" type="sensors_health_monitor.py" output="screen" if="$(eval number_of_velodynes > 1)">
<remap from="failure_detection" to="point_cloud_merger_lo/failure_detection"/>
<remap from="resurrection_detection" to="point_cloud_merger_lo/resurrection_detection"/>
</node>
<node pkg="nodelet"
type="nodelet"
name="transform_points_base_link"
args="standalone pcl/PassThrough">
<remap from="~input" to="velodyne_points"/>
<remap from="~output" to="$(arg pc_trans_in_0)"/>
<rosparam subst_value="true">
filter_field_name: z
filter_limit_min: -100
filter_limit_max: 100
output_frame: $(arg robot_namespace)/base_link
</rosparam>
</node>
<node if="$(eval arg('number_of_velodynes') > 1)"
pkg="nodelet"
type="nodelet"
name="transform_points_base_link_front"
args="standalone pcl/PassThrough">
<remap from="~input" to="velodyne_front/velodyne_points"/>
<remap from="~output" to="$(arg pc_trans_in_1)"/>
<rosparam subst_value="true">
filter_field_name: z
filter_limit_min: -100
filter_limit_max: 100
output_frame: $(arg robot_namespace)/base_link
</rosparam>
</node>
<node if="$(eval arg('number_of_velodynes') > 2)"
pkg="nodelet"
type="nodelet"
name="transform_points_base_link_rear"
args="standalone pcl/PassThrough">
<remap from="~input" to="velodyne_rear/velodyne_points"/>
<remap from="~output" to="$(arg pc_trans_in_2)"/>
<rosparam subst_value="true">
filter_field_name: z
filter_limit_min: -100
filter_limit_max: 100
output_frame: $(arg robot_namespace)/base_link
</rosparam>
</node>
<node if="$(arg b_use_multiple_pc)" pkg="point_cloud_merger" type="point_cloud_merger_node" name="point_cloud_merger_lo" output="screen">
<rosparam file="$(find point_cloud_merger)/config/parameters.yaml"/>
<param name="merging/number_of_velodynes" value="$(arg number_of_velodynes)"/>
<remap from="~pcld0" to="$(arg pc_trans_in_0)"/>
<remap from="~pcld1" to="$(arg pc_trans_in_1)"/>
<remap from="~pcld2" to="$(arg pc_trans_in_2)"/>
<remap from="~combined_point_cloud" to="combined_point_cloud"/>
</node>
<node pkg="nodelet"
type="nodelet"
name="$(arg nodelet_manager)"
launch-prefix="$(arg launch_prefix)"
args="manager"
respawn="$(arg respawn)"/>
<node pkg="nodelet"
type="nodelet"
name="body_filter"
args="load point_cloud_filter/BodyFilter $(arg nodelet_manager) $(arg nodelet_args)"
respawn="$(arg respawn)">
<remap from="~input" to="combined_point_cloud" if="$(arg b_use_multiple_pc)"/>
<remap from="~input" to="velodyne_points/transformed" unless="$(arg b_use_multiple_pc)"/>
</node>
<!-- old way. TODO: removing after locus testing-->
<!--node pkg="nodelet" type="nodelet" name="voxel_grid" args="load pcl/VoxelGrid $(arg nodelet_manager)" output="screen" respawn="true"-->
<node pkg="nodelet" type="nodelet" name="voxel_grid" args="load point_cloud_filter/CustomVoxelGrid $(arg nodelet_manager)" output="screen" respawn="true">
<remap from="~input" to="body_filter/output" />
<rosparam subst_value="true">
filter_field_name: z
filter_limit_min: -100
filter_limit_max: 100
filter_limit_negative: False
leaf_size: 0.25
output_frame: $(arg robot_namespace)/base_link
</rosparam>
</node>
<node pkg="nodelet"
type="nodelet"
name="normal_computation"
args="load point_cloud_filter/NormalComputation $(arg nodelet_manager) $(arg nodelet_args)"
respawn="$(arg respawn)">
<remap from="~input" to="voxel_grid/output"/>
<remap from="~output" to="$(arg pc_input)" />
<param name="num_threads" value="1" if="$(eval robot_namespace.startswith('extars'))" />
</node>
</group>
</launch>
Can you please help me?
Thanks in advance.
Hi, Thanks for your sharing!
However, when I check the test bag, I wonder how can I get the wio_ekf ? since I only got wheel odometry in my dataset.
Hello, thanks for the great work!
I want to running LOCUS on other datasets, my datasets format is .bag
rosbag play xxx.bag
then publish the topic
according to README, it seems that only run_locus.yaml
and locus.launch
need to be modified.
i wonder to know what i need to modify can connect the datasets and system, such in other SLAM (just need to modify the topic in config)
i think need to modify the remap in locus.launch
and data path in run_locus.yaml
but not success, provide some help please.
Moreover, my datasets have LiDAR data and IMU data with .bag format, but there is no data of odometry(wheel odometry)(input).
It might be useful to release a minimal version of GT Analysis (without computation and delay analysis) as just a wrapper around EVO to make it easy for people to analyze and compare datasets. It might be released with the datasets, or with LOCUS.
Hi,
In the LOCUS 2.0 paper, there is a reference to "Space Monitor" in Fig 2. It was however not comprehensively referred in the paper. Is it related to "moving window". I notice that in the code, "lo_settings.yaml" has two separate settings, to enable the space monitor and the moving window.
Can you please shed some light on this and how it would help in improving the mapping?
Solved by @LossOfHumanIdentity
A declarative, efficient, and flexible JavaScript library for building user interfaces.
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
The Web framework for perfectionists with deadlines.
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
We are working to build community through open source technology. NB: members must have two-factor auth.
Open source projects and samples from Microsoft.
Google ❤️ Open Source for everyone.
Alibaba Open Source for everyone
Data-Driven Documents codes.
China tencent open source team.