ERROR: double free or corruption (out) #67
Description
I want to try the multi-drone mode (estimation_mode: 2) using the TUM dataset. However, the code crashed with "double free or corruption (out)". The errors presented in the terminal are as follows:
double free or corruption (out)
Stack trace (most recent call last) in thread 304:
#14 Object "", at 0xffffffffffffffff, in
#13 Source "../sysdeps/unix/sysv/linux/x86_64/clone.S", line 95, in __clone [0x7fbfe303e352]
#12 Source "/build/glibc-FcRMwW/glibc-2.31/nptl/pthread_create.c", line 477, in start_thread [0x7fbfe564d608]
#11 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.28", at 0x7fbfe3351df3, in
#13 Source "../sysdeps/unix/sysv/linux/x86_64/clone.S", line 95, in __clone [0x7f419111f352]
#12 Source "/build/glibc-FcRMwW/glibc-2.31/nptl/pthread_create.c", line 477, in start_thread [0x7f419372e608]
#11 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.28", at 0x7f4191432df3, in
#10 | Source "/usr/include/c++/9/thread", line 195, in operator() 0.0ms
| 194: ayivoidnum 2 t_played_bag 3.1 error 0.44ms error_sys 0.2ms
| > 195: _M_run() { _M_func(); }
| 196: };
| Source "/usr/include/c++/9/thread", line 251, in _M_invoke<0>
| 249: using _Indices
| 250: = typename _Build_index_tuple<tuple_size<_Tuple>::value>::__type;
| > 251: return _M_invoke(_Indices());
| 252: }
| 253: };
| Source "/usr/include/c++/9/thread", line 244, in __invoke<D2VINSNode::Init(ros::NodeHandle&)::<lambda()> >
| 242: typename __result<_Tuple>::type
| 243: _M_invoke(_Index_tuple<_Ind...>)
| > 244: { return std::__invoke(std::get<_Ind>(std::move(_M_t))...); }
| 245:
| 246: typename __result<_Tuple>::type
| Source "/usr/include/c++/9/bits/invoke.h", line 95, in __invoke_impl<void, D2VINSNode::Init(ros::NodeHandle&)::<lambda()> >
| 93: using __type = typename __result::type;ror_sys 0.1ms
| 94: using __tag = typename __result::__invoke_type;
| > 95: return std::__invoke_impl<__type>(__tag{}, std::forward<_Callable>(__fn),
| 96: std::forward<_Args>(__args)...);
| 97: }
| Source "/usr/include/c++/9/bits/invoke.h", line 60, in operator()
| 58: constexpr _Res
| 59: __invoke_impl(__invoke_other, _Fn&& __f, _Args&&... __args)
| > 60: { return std::forward<_Fn>(__f)(std::forward<_Args>(__args)...); }
| 61:
| 62: template<typename _Res, typename _MemFun, typename _Tp, typename... _Args>
| Source "/root/swarm_ws/src/D2SLAM/d2vins/src/d2vins_node.cpp", line 296, in solverThread
| 294: solver_rate_ptr_ = std::make_unique<ros::Rate>(params->estimator_timer_freq);
| 295: //开启一个新的线程
| > 296: thread_solver = std::thread([&] { solverThread(); });
| 297: }
Source "/root/swarm_ws/src/D2SLAM/d2vins/src/d2vins_node.cpp", line 245, in _M_run [0x55975ed606bc]
242: while (thread_solver_running) {
243: if (need_solve) {
244: ROS_INFO("D2VINS::solverThread: solving...");
> 245: estimator->solveinDistributedMode();
246: ROS_INFO("updateOutModuleSldWinAndLandmarkDB");
247: updateOutModuleSldWinAndLandmarkDB();
248: if (!params->consensus_sync_to_start) {
#10 | Source "/usr/include/c++/9/thread", line 195, in operator() 0.1ms
| 194: void
| > 195: _M_run() { _M_func(); }
| 196: };
| Source "/usr/include/c++/9/thread", line 251, in _M_invoke<0>
| 249: using _Indices
| 250: = typename _Build_index_tuple<tuple_size<_Tuple>::value>::__type;
| > 251: return _M_invoke(_Indices());
| 252: }
| 253: };
| Source "/usr/include/c++/9/thread", line 244, in __invoke<D2VINSNode::Init(ros::NodeHandle&)::<lambda()> >
| 242: typename __result<_Tuple>::type
| 243: _M_invoke(_Index_tuple<_Ind...>)
| > 244: { return std::__invoke(std::get<_Ind>(std::move(_M_t))...); }
| 245:
| 246: typename __result<_Tuple>::type
| Source "/usr/include/c++/9/bits/invoke.h", line 95, in __invoke_impl<void, D2VINSNode::Init(ros::NodeHandle&)::<lambda()> >
| 93: using __type = typename __result::type;
| 94: using __tag = typename __result::__invoke_type;
| > 95: return std::__invoke_impl<__type>(__tag{}, std::forward<_Callable>(__fn),
| 96: std::forward<_Args>(__args)...);
| 97: }
| Source "/usr/include/c++/9/bits/invoke.h", line 60, in operator()
| 58: constexpr _Res
| 59: __invoke_impl(__invoke_other, _Fn&& __f, _Args&&... __args)
| > 60: { return std::forward<_Fn>(__f)(std::forward<_Args>(__args)...); }
| 61:
| 62: template<typename _Res, typename _MemFun, typename _Tp, typename... _Args>
| Source "/root/swarm_ws/src/D2SLAM/d2vins/src/d2vins_node.cpp", line 296, in solverThread
| 294: solver_rate_ptr_ = std::make_unique<ros::Rate>(params->estimator_timer_freq);
| 295: //开启一个新的线程
| > 296: thread_solver = std::thread([&] { solverThread(); });
| 297: }
Source "/root/swarm_ws/src/D2SLAM/d2vins/src/d2vins_node.cpp", line 245, in _M_run [0x563e6a4ae6bc]
242: while (thread_solver_running) {
243: if (need_solve) {
244: ROS_INFO("D2VINS::solverThread: solving...");
> 245: estimator->solveinDistributedMode();
246: ROS_INFO("updateOutModuleSldWinAndLandmarkDB");
247: updateOutModuleSldWinAndLandmarkDB();
248: if (!params->consensus_sync_to_start) {
[ INFO] [1746445792.058270703]: img_desc_a.landmark_descriptor.size = 8128
[11:49:52][warning][d2featuretracker.cpp,trackRemote,L338] matchLocalFeatures failed
#9 Source "/root/swarm_ws/src/D2SLAM/d2vins/src/estimator/d2estimator.cpp", line 545, in solveinDistributedMode [0x7f4191670929]
542: ROS_INFO("pre_solve");
543: state.preSolve(imu_bufs);
544: ROS_INFO("solver->reset");
> 545: solver->reset();
546: ROS_INFO("set Factors");
547: setupImuFactors();
548: setupLandmarkFactors();
#9 Source "/root/swarm_ws/src/D2SLAM/d2vins/src/estimator/d2estimator.cpp", line 545, in solveinDistributedMode [0x7fbfe358f929]
542: ROS_INFO("pre_solve");
543: state.preSolve(imu_bufs);
544: ROS_INFO("solver->reset");
> 545: solver->reset();
546: ROS_INFO("set Factors");
547: setupImuFactors();
548: setupLandmarkFactors();
#8 Source "/root/swarm_ws/src/D2SLAM/d2common/src/solver/ConsensusSolver.cpp", line 17, in reset [0x7f4193a31d4a]
15: void ConsensusSolver::reset() {
16: ROS_INFO("ConsensusSolver reset");
> 17: SolverWrapper::reset();
18: consenus_params.clear();
19: all_estimating_params.clear();
20: active_params.clear();
#8 Source "/root/swarm_ws/src/D2SLAM/d2common/src/solver/ConsensusSolver.cpp", line 17, in reset [0x7fbfe5950d4a]
15: void ConsensusSolver::reset() {
16: ROS_INFO("ConsensusSolver reset");
> 17: SolverWrapper::reset();
18: consenus_params.clear();
19: all_estimating_params.clear();
20: active_params.clear();
#7 Source "/root/swarm_ws/src/D2SLAM/d2common/src/solver/SolverWrapper.cpp", line 26, in reset [0x7f4193a29b8f]
23: ROS_INFO_STREAM("problem = "<<problem);
24: if (problem != nullptr) {
25: //todo 这个地方出现了重复delete，需要排查
> 26: delete problem;
27: problem = nullptr;
28: }
#7 Source "/root/swarm_ws/src/D2SLAM/d2common/src/solver/SolverWrapper.cpp", line 26, in reset [0x7fbfe5948b8f]
23: ROS_INFO_STREAM("problem = "<<problem);
24: if (problem != nullptr) {
25: //todo 这个地方出现了重复delete，需要排查
> 26: delete problem;
27: problem = nullptr;
28: }
It shows that the error happened in the function "SolverWrapper::reset()" when it is called by the function ConsensusSolver::reset(). Specifically, this error happened when it ran "delete problem". I don't figure out why this happened. I tried to use a Mutex to protect "problem", but this didn't work.
The following are the config files:
(1) tum_multi.yaml:
%YAML:1.0
#inputs
imu: 1
is_fisheye: 0
imu_topic: "/imu0"
image0_topic: "/cam0/image_raw"
image1_topic: "/cam1/image_raw"
is_compressed_images: 1
imu_freq: 200
image_freq: 20
frame_step: 2
#Camera configuration
camera_configuration: 0 #STEREO_PINHOLE = 0, STEREO_FISHEYE = 1, PINHOLE_DEPTH = 2, FOURCORNER_FISHEYE = 3
num_of_cam: 2
cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 512
image_height: 512
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ -0.999506, 0.00759167, -0.030488, 0.0447659,
0.0302105, -0.0343071, -0.998955, -0.0755245,
-0.00862969, -0.999383, 0.0340608, -0.0465419,
0, 0, 0, 1 ]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ -0.999497, 0.00813335, -0.0306525, -0.0561178,
0.0307588, 0.0132798, -0.999439, -0.0738562,
-0.00772172, -0.999879, -0.0135233, -0.0494102,
0, 0, 0, 1]
sp_track_use_lk: 1
#frontend frequency
loop_detection_freq: 1
lcm_thread_freq: 1
#estimation
estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
estimate_td: 0 # online estimate time offset between camera and imu
td: 0.00 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
estimation_mode: 2 #0:single; 1:solve all; 2: distributed; 3:server
double_counting_common_feature: 0
#0:single; 1:solve all; 2: distributed; 3:server
#optimization parameters
max_solver_time: 0.08 # max solver itration time (ms), to guarantee real time
max_num_iterations: 8 # max solver itrations, to guarantee real time
consensus_max_steps: 1
timout_wait_sync: 50
rho_landmark: 1.0
rho_frame_T: 100
rho_frame_theta: 100
relaxation_alpha: 0.
#Note these two param must set to 0 when lazy_broadcast_keyframe=1
consensus_sync_for_averaging: 0
consensus_sync_to_start: 0 #Is sync on start of the solving..
consensus_trigger_time_err_us: 100
#depth fusing
depth_far_thres: 3.0 # The max depth in frontend
depth_near_thres: 0.3
fuse_dep: 0 #if fuse depth measurement
max_depth_to_fuse: 5.0
min_depth_to_fuse: 0.3
#Multi-drone
track_remote_netvlad_thres: 0.8
#Initialization
init_method: 0 #0 IMU, 1 PnP
depth_estimate_baseline: 0.03
#sliding window
max_sld_win_size: 11
landmark_estimate_tracks: 4 #when use depth or stereo, 3 is OK.
min_solve_frames: 6
#outlier rejection
thres_outlier : 10.0
perform_outlier_rejection_num: 100
tri_max_err: 0.2
#Marginalization
enable_marginalization: 1
margin_sparse_solver: 0
always_fixed_first_pose: 0
remove_base_when_margin_remote: 2
#feature tracker parameters
max_cnt: 150 # max feature number in feature tracking
max_superpoint_cnt: 150 # max feature extraction by superpoint
max_solve_cnt: 150
check_essential: 1
enable_lk_optical_flow: 1 #enable lk opticalflow featuretrack to enhance ego-motion estimation.
remote_min_match_num: 40
enable_superglue_local: 0
enable_superglue_remote: 0
ransacReprojThreshold: 10.0
enable_search_local_aera: 1
search_local_max_dist: 0.04
feature_min_dist: 15
parallex_thres: 0.022
knn_match_ratio: 0.8 #This apply to superpoint feature track & loop clouse detection.
#CNN
cnn_use_onnx: 1
enable_pca_superpoint: 1
enable_pca_netvlad: 0
nn_engine_type: 1 # 0 use onnx; 1 use tensorrt raw engine
superpoint_trt_path: "/root/swarm_ws/src/D2SLAM/models/superpoint_v1_dyn_size_onnx_200_300.trt"
moblieNetVlad_trt_path: "/root/swarm_ws/src/D2SLAM/models/mobilenetvlad_dyn_size_onnx_200_300.trt"
#Superpoint
superpoint_config:
onnx_path: "/root/swarm_ws/src/D2SLAM/models/superpoint_series/superpoint_v1_sim_int32.onnx"
trt_engine_path: "/root/swarm_ws/src/D2SLAM/models/superpoint_series/superpoint_v1_sim_int32.trt"
enable_fp16: 1
input_width: 320
input_height: 160
input_tensor_names:
- "input"
output_tensor_names:
- "scores" # keypoint output
- "descriptors" # descriptor output
# these two parameters sequence must follow the given output sequence.
threshold: 0.15
nms_radius: 20
enable_pca: 1
pca_mean_path: "/root/swarm_ws/src/D2SLAM/models/superpoint_series/mean_.csv"
pca_comp_path: "/root/swarm_ws/src/D2SLAM/models/superpoint_series/components_.csv"
superpoint_pca_dims: 64
netvlad_pca_dims: 1024
#下面这两个参数设为1有2问题
cnn_enable_tensorrt: 0
cnn_enable_tensorrt_int8: 0
netvlad_int8_calib_table_name: "mobilenetvlad_calibration.flatbuffers"
superpoint_int8_calib_table_name: "superpoint_calibration.flatbuffers"
acc_n: 0.1 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.05 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.002 # accelerometer bias random work noise standard deviation. #0.002
gyr_w: 0.0004 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.805 # gravity magnitude
#Loop Closure Detection
loop_detection_netvlad_thres: 0.8
enable_homography_test: 0
loop_inlier_feature_num: 60
accept_loop_max_yaw: 10
accept_loop_max_pos: 1.0
lazy_broadcast_keyframe: 0 # 0: greedy broadcast; 1: compact mode broadcast
nearby_drone_dist: 3.0
gravity_check_thres: 0.03
enable_pcm: 1
pcm_thres: 3.5
#PGO
pgo_solver_time: 1.0
solver_timer_freq: 0.2
pgo_mode: 1
pgo_rho_frame_T: 0.01
pgo_rho_frame_theta: 2.0
pgo_eta_k: 1.0
write_g2o: 1
g2o_output_path: "output.g2o"
write_pgo_to_file: 1
debug_save_g2o_only: 0
#outputs
output_path: "/root/output/"
debug_print_sldwin: 0
debug_print_states: 0
enable_perf_output: 0
print_network_status: 1
debug_write_margin_matrix: 0
show_track_id: 0
write_tracking_image_to_file: 0
(2) d2vins.yaml:
dataset:
swarm1: #drone 1
id: 1
config_path: "Configs/"
bag: "dataset-corridor1_512_16-sync-comp.bag"
swarm2: #drone 2
id: 2
config_path: "Configs/"
bag: "dataset-corridor2_512_16-sync-comp.bag"
#swarm3: #drone 2
# id: 3
# config_path: "Configs/"
# bag: "dataset-corridor3_512_16-sync-comp.bag"
output_path: "outputs/d2vins/"
workspace: "/home/mao/D2SLAM/src/D2SLAM"
image_name: "hkustswarm/d2slam:x86"
exclude_topics: ["/uwb_node/incoming_broadcast_data"]
rate: 0.5
t_start: 0
duration: 140
start_latency: 10
entry_point_script: |
#!/bin/bash
RVIZ=false
SHOW=false
if [ $DRONE_ID -eq 1 ]
then
SHOW=false
RVIZ=true
roslaunch d2vins tum.launch show:=$SHOW self_id:=$DRONE_ID \
config:=/root/swarm_ws/src/D2SLAM/config/tum/tum_multi.yaml \
enable_loop:=true enable_pgo:=true rviz:=$RVIZ send_whole_img_desc:=true send_img:=true output:="screen"
else
roslaunch d2vins tum.launch show:=$SHOW self_id:=$DRONE_ID \
config:=/root/swarm_ws/src/D2SLAM/config/tum/tum_multi.yaml \
enable_loop:=true enable_pgo:=true rviz:=$RVIZ send_whole_img_desc:=true send_img:=true output:="log"
fi