search

ethz-asl / segmap

A map representation based on 3D segments
BSD 3-Clause "New" or "Revised" License
1.05k stars 394 forks source link

Stuck into the optimization using GTSAM #151

Closed LiShuaixin closed 4 years ago

LiShuaixin commented 4 years ago

When I run kitti_loop_closure.launch, everything seems right:

roslaunch segmapper kitti_loop_closure.launch 
... logging to /home/lee/.ros/log/0013130c-c87e-11e9-8e5d-441ca8e3ae23/roslaunch-lee-XPS-15-9550-23012.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://lee-XPS-15-9550:44668/

SUMMARY
========

PARAMETERS
 * /LaserMapper/bag_file: /home/lee/.segmap...
 * /SegMapper/Benchmarker/enable_live_output: False
 * /SegMapper/Benchmarker/results_directory: /tmp/segmap_bench...
 * /SegMapper/Benchmarker/save_statistics_only: False
 * /SegMapper/LaserSlamWorker/assembled_cloud_sub_topic: /velodyne_points
 * /SegMapper/LaserSlamWorker/create_filtered_map: True
 * /SegMapper/LaserSlamWorker/distant_map_pub_topic: distant_map
 * /SegMapper/LaserSlamWorker/full_map_pub_topic: full_map
 * /SegMapper/LaserSlamWorker/local_map_pub_topic: local_map
 * /SegMapper/LaserSlamWorker/map_publication_rate_hz: 3
 * /SegMapper/LaserSlamWorker/odom_frame: world
 * /SegMapper/LaserSlamWorker/odometry_trajectory_pub_topic: odometry_trajectory
 * /SegMapper/LaserSlamWorker/publish_distant_map: False
 * /SegMapper/LaserSlamWorker/publish_full_map: False
 * /SegMapper/LaserSlamWorker/publish_local_map: True
 * /SegMapper/LaserSlamWorker/sensor_frame: velodyne
 * /SegMapper/LaserSlamWorker/trajectory_pub_topic: trajectory
 * /SegMapper/LaserSlamWorker/use_odometry_information: True
 * /SegMapper/OnlineEstimator/LaserTrack/add_m_estimator_on_icp: False
 * /SegMapper/OnlineEstimator/LaserTrack/add_m_estimator_on_odom: False
 * /SegMapper/OnlineEstimator/LaserTrack/force_priors: False
 * /SegMapper/OnlineEstimator/LaserTrack/icp_noise_model: [0.5, 0.05, 0.05,...
 * /SegMapper/OnlineEstimator/LaserTrack/nscan_in_sub_map: 5
 * /SegMapper/OnlineEstimator/LaserTrack/nscan_to_match: 3
 * /SegMapper/OnlineEstimator/LaserTrack/odometry_noise_model: [0.5, 0.5, 0.5, 0...
 * /SegMapper/OnlineEstimator/LaserTrack/save_icp_results: False
 * /SegMapper/OnlineEstimator/LaserTrack/use_icp_factors: True
 * /SegMapper/OnlineEstimator/LaserTrack/use_odom_factors: True
 * /SegMapper/OnlineEstimator/add_m_estimator_on_loop_closures: True
 * /SegMapper/OnlineEstimator/do_icp_step_on_loop_closures: False
 * /SegMapper/OnlineEstimator/loop_closure_noise_model: [0.0005, 0.0005, ...
 * /SegMapper/OnlineEstimator/loop_closures_sub_maps_radius: 3
 * /SegMapper/SegMatchWorker/SegMatch/Classifier/apply_hard_threshold_on_feature_distance: True
 * /SegMapper/SegMatchWorker/SegMatch/Classifier/enable_two_stage_retrieval: True
 * /SegMapper/SegMatchWorker/SegMatch/Classifier/feature_distance_threshold: 0.01
 * /SegMapper/SegMatchWorker/SegMatch/Classifier/knn_feature_dim: 7
 * /SegMapper/SegMatchWorker/SegMatch/Classifier/max_eigen_features_values: [2493.5, 186681.0...
 * /SegMapper/SegMatchWorker/SegMatch/Classifier/n_nearest_neighbours: 120
 * /SegMapper/SegMatchWorker/SegMatch/Classifier/normalize_eigen_for_hard_threshold: True
 * /SegMapper/SegMatchWorker/SegMatch/Classifier/normalize_eigen_for_knn: False
 * /SegMapper/SegMatchWorker/SegMatch/Classifier/threshold_to_accept_match: 0.6
 * /SegMapper/SegMatchWorker/SegMatch/Descriptors/descriptor_types: ['EigenvalueBased']
 * /SegMapper/SegMatchWorker/SegMatch/GeometricConsistency/max_consistency_distance_for_caching: 3.0
 * /SegMapper/SegMatchWorker/SegMatch/GeometricConsistency/min_cluster_size: 6
 * /SegMapper/SegMatchWorker/SegMatch/GeometricConsistency/recognizer_type: Incremental
 * /SegMapper/SegMatchWorker/SegMatch/GeometricConsistency/resolution: 0.4
 * /SegMapper/SegMatchWorker/SegMatch/LocalMap/max_vertical_distance_m: 999.0
 * /SegMapper/SegMatchWorker/SegMatch/LocalMap/min_points_per_voxel: 1
 * /SegMapper/SegMatchWorker/SegMatch/LocalMap/min_vertical_distance_m: -999.0
 * /SegMapper/SegMatchWorker/SegMatch/LocalMap/neighbors_provider_type: KdTree
 * /SegMapper/SegMatchWorker/SegMatch/LocalMap/radius_m: 50
 * /SegMapper/SegMatchWorker/SegMatch/LocalMap/voxel_size_m: 0.1
 * /SegMapper/SegMatchWorker/SegMatch/Segmenters/max_cluster_size: 15000
 * /SegMapper/SegMatchWorker/SegMatch/Segmenters/min_cluster_size: 100
 * /SegMapper/SegMatchWorker/SegMatch/Segmenters/radius_for_growing: 0.2
 * /SegMapper/SegMatchWorker/SegMatch/Segmenters/segmenter_type: IncrementalEuclid...
 * /SegMapper/SegMatchWorker/SegMatch/boundary_radius_m: 999.0
 * /SegMapper/SegMatchWorker/SegMatch/centroid_distance_threshold_m: 2.5
 * /SegMapper/SegMatchWorker/SegMatch/check_pose_lies_below_segments: False
 * /SegMapper/SegMatchWorker/SegMatch/filter_boundary_segments: True
 * /SegMapper/SegMatchWorker/SegMatch/filter_duplicate_segments: True
 * /SegMapper/SegMatchWorker/SegMatch/min_time_between_segment_for_matches_s: 60
 * /SegMapper/SegMatchWorker/align_target_map_on_first_loop_closure: False
 * /SegMapper/SegMatchWorker/close_loops: True
 * /SegMapper/SegMatchWorker/distance_between_segmentations_m: 0.0
 * /SegMapper/SegMatchWorker/distance_to_lower_target_cloud_for_viz_m: 15.0
 * /SegMapper/SegMatchWorker/export_segments_and_matches: False
 * /SegMapper/SegMatchWorker/line_scale_loop_closures: 5.0
 * /SegMapper/SegMatchWorker/line_scale_matches: 0.5
 * /SegMapper/SegMatchWorker/localize: False
 * /SegMapper/SegMatchWorker/publish_predicted_segment_matches: False
 * /SegMapper/SegMatchWorker/ratio_of_points_to_keep_when_publishing: 1.0
 * /SegMapper/clear_local_map_after_loop_closure: False
 * /SegMapper/number_of_robots: 1
 * /SegMapper/publish_world_to_odom: True
 * /SegMapper/robot_prefix: na
 * /SegMapper/tf_publication_rate_hz: 10
 * /SegMapper/world_frame: map
 * /rosdistro: kinetic
 * /rosversion: 1.12.14
 * /segmapper/icp_configuration_file: /home/lee/segmap_...
 * /segmapper/icp_input_filters_file: /home/lee/segmap_...
 * /use_sim_time: True

NODES
  /
    player (rosbag/play)
    segmapper (segmapper/segmapper_node)
    visualizer (rviz/rviz)

auto-starting new master
process[master]: started with pid [23023]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 0013130c-c87e-11e9-8e5d-441ca8e3ae23
process[rosout-1]: started with pid [23036]
started core service [/rosout]
process[visualizer-2]: started with pid [23060]
process[player-3]: started with pid [23061]
process[segmapper-4]: started with pid [23062]
[ INFO] [1566877911.883146759]: Opening /home/lee/.segmap/kitti/2011_09_30_drive_18.bag

Waiting 0.2 seconds after advertising topics...[ INFO] [1566877912.003996294]: rviz version 1.12.17
[ INFO] [1566877912.004038781]: compiled against Qt version 5.5.1
[ INFO] [1566877912.004051209]: compiled against OGRE version 1.9.0 (Ghadamon)
 done.

Hit space to toggle paused, or 's' to step.
[ INFO] [1566877912.651201739, 1317376479.232045294]: Stereo is NOT SUPPORTED           
[ INFO] [1566877912.651292472, 1317376479.232045294]: OpenGl version: 4.6 (GLSL 4.6).
0x16e4ae0 void QWindowPrivate::setTopLevelScreen(QScreen*, bool) ( QScreen(0xaea430) ): Attempt to set a screen on a child window.
0x16ef590 void QWindowPrivate::setTopLevelScreen(QScreen*, bool) ( QScreen(0xaea430) ): Attempt to set a screen on a child window.
0x16eef80 void QWindowPrivate::setTopLevelScreen(QScreen*, bool) ( QScreen(0xaea430) ): Attempt to set a screen on a child window.
0x16f7220 void QWindowPrivate::setTopLevelScreen(QScreen*, bool) ( QScreen(0xaea430) ): Attempt to set a screen on a child window.
WARNING: Logging before InitGoogleLogging() is written to STDERR
I0827 11:51:52.674111 23062 segmatch.cpp:23] Do not forget to initialize SegMatch.
I0827 11:51:52.734742 23062 laser_track.cpp:16] Loading ICP configurations from: /home/lee/segmap_ws/src/segmap/segmapper/launch/kitti/icp_dynamic_outdoor.yaml
I0827 11:51:52.736002 23062 laser_track.cpp:26] Loading ICP input filters from: /home/lee/segmap_ws/src/segmap/segmapper/launch/kitti/input_filters_outdoor.yaml
I0827 11:51:52.736459 23062 incremental_estimator.cpp:31] Creating loop closure noise model with cauchy.
I0827 11:51:52.736490 23062 incremental_estimator.cpp:54] Loading ICP configurations from: /home/lee/segmap_ws/src/segmap/segmapper/launch/kitti/icp_dynamic_outdoor.yaml
[ INFO] [1566877912.738301246]: Number of laser_slam workers: 1
I0827 11:51:52.738483 23062 segmapper.cpp:86] Robot 0 subscribes to /velodyne_points world and velodyne
I0827 11:51:52.738507 23062 segmapper.cpp:89] Robot 0 publishes to trajectory and local_map
I0827 11:51:52.795430 23338 segmapper.cpp:121] publishing local maps
[ WARN] [1566877915.653260172, 1317376479.457412091]: [SegMapper] Timeout while waiting between world and velodyne.
[DEBUG laserSlamWorker]: Using odometry information!!!
[DEBUG segMatchThread]: Get query points!!!
[DEBUG segMatchThread]: Update the local map with the new points and the new pose!!!
[DEBUG segMatchThread]: Loop closure mode!!!
[DEBUG segMatchThread segMatchThread]: Process the source code!!!
[ INFO] [1566877915.723164514, 1317376479.528016839]: [DEBUG segMatchThread]: Reset the counter!!!
[DEBUG laserSlamWorker]: Using odometry information!!!316088 / 287.641810               
I0827 11:51:55.763449 23338 segmapper.cpp:121] publishing local maps
I0827 11:51:55.763713 23338 segmapper.cpp:121] publishing local maps
I0827 11:51:56.106657 23338 segmapper.cpp:121] publishing local maps41810               
I0827 11:51:56.438889 23338 segmapper.cpp:121] publishing local maps41810               
I0827 11:51:56.772980 23338 segmapper.cpp:121] publishing local maps41810               
I0827 11:51:57.104378 23338 segmapper.cpp:121] publishing local maps41810               
I0827 11:51:57.437659 23338 segmapper.cpp:121] publishing local maps41810               
I0827 11:51:57.770608 23338 segmapper.cpp:121] publishing local maps41810               
I0827 11:51:58.104893 23338 segmapper.cpp:121] publishing local maps41810               
I0827 11:51:58.437520 23338 segmapper.cpp:121] publishing local maps41810               
I0827 11:51:58.770038 23338 segmapper.cpp:121] publishing local maps41810               
I0827 11:51:59.103098 23338 segmapper.cpp:121] publishing local maps41810               
I0827 11:51:59.435923 23338 segmapper.cpp:121] publishing local maps41810               
I0827 11:51:59.768755 23338 segmapper.cpp:121] publishing local maps41810               
I0827 11:52:00.100956 23338 segmapper.cpp:121] publishing local maps41810               
I0827 11:52:00.433151 23338 segmapper.cpp:121] publishing local maps41810               
I0827 11:52:00.765566 23338 segmapper.cpp:121] publishing local maps41810

But the rviz can only visualize the first segment it extracted and nothing else. issue The same issue occurred when I run kitti_localization.launch. After it load the target map, the system seems still running while nothing changed in rviz window. Does anybody know the reason that caused the issue?

-------------------------------Update-------------------------------------- I found the issue occurred when calling result = incremental_estimator_->estimate(new_factors, new_values, new_scan.time_ns); after processing current laser scan and pose. It stuck into the graph optimization when calling isam::update(). The version of gtsam I use is download and compiled when building gtsam_catkin. To ensure the whole project using the same version of eigen, I set USE_SYSTEM_EIGEN = OFF in eigen_catkin camkelists to download 3.2.10 eigen and set USE_SYSTEM_EIGEN = OFF to use GTSAM-bundled Eigen which is also 3.2.10.

LiShuaixin commented 4 years ago

I think the issue may be caused by the too old version of eigen. I set USE_SYSTEM_EIGEN = OFF in eigen_catkin camkelists and downloaded 3.3.7 eigen instead of 3.2.10. Then set USE_SYSTEM_EIGEN = ON in gtsam_catkin and specify the eigen_library as the one compiled in the project work space (it can also be checked in the built gtsam-src directory, i.e. check the eigen path in gtsam_extra.cmake). Only when all packages are related to the same eigen lib, the project could be compiled and run normally.