In my dataset, when I applied the 'quad_cam_calib-camchain-imucam-2.yaml' parameters, the execution proceeded normally regardless of the result. However, when I ran it with the parameters calibrated for my camera (camera calibration was done using tartancalib, and imu-cam extrinsic calibration was done using kalibr), the following error occurred. How can I solve this?
cam0
Camera cam0 model omni-radtan
width: 960, height: 540, xi: 2.141351, gamma1: 931.474978, gamma2: 929.901062, u0: 497.405520, v0: 327.989634, k1: -0.161814, k2: 0.782521, p1: 0.001038, p2: -0.000557
T_cam_imu:
0.999895 -0.00961883 0.0108709 0.053197
-0.0110255 -0.0161634 0.999809 0.210112
-0.00944128 -0.999823 -0.0162677 0.353193
0 0 0 1
pose:
T [+0.053,+0.210,+0.353] YPR [-0.6,+0.5,-90.9]
cam1
Camera cam1 model omni-radtan
width: 960, height: 540, xi: 2.124677, gamma1: 926.491939, gamma2: 925.968186, u0: 510.838305, v0: 305.832064, k1: -0.150126, k2: 0.704146, p1: 0.000638, p2: 0.000162
T_cam_imu:
-0.0240746 0.0346944 0.999108 0.252547
-0.999473 -0.0225929 -0.0232988 0.0040778
0.0217644 -0.999143 0.03522 0.363299
0 0 0 1
pose:
T [+0.253,+0.004,+0.363] YPR [-91.4,-1.2,-88.0]
cam2
Camera cam2 model omni-radtan
width: 960, height: 540, xi: 2.118641, gamma1: 921.436202, gamma2: 921.115019, u0: 516.173765, v0: 302.943656, k1: -0.140171, k2: 0.629740, p1: 0.001022, p2: -0.000296
T_cam_imu:
-0.999833 0.012815 -0.0130043 0.0369455
0.0131476 0.0111612 -0.999851 -0.21379
-0.012668 -0.999856 -0.0113278 0.337499
0 0 0 1
pose:
T [+0.037,-0.214,+0.337] YPR [+179.3,+0.7,-90.7]
cam3
Camera cam3 model omni-radtan
width: 960, height: 540, xi: 2.156635, gamma1: 941.317089, gamma2: 941.002109, u0: 514.206918, v0: 313.380957, k1: -0.094390, k2: 0.501424, p1: 0.000080, p2: -0.000696
T_cam_imu:
-0.00139846 0.0292838 -0.99957 -0.173432
0.999922 -0.0123397 -0.00176046 -0.00013578
-0.0123859 -0.999495 -0.0292642 0.365919
0 0 0 1
pose:
T [-0.173,-0.000,+0.366] YPR [+90.1,+0.7,-91.7]
[D2Frontend] No photometric calibration file provided.
[FisheyeUndist] Generating Cylinder maps, fov 170.0 deg
no photometric calibration file found
[FisheyeUndist] Generating Cylinder maps, fov 170.0 deg
no photometric calibration file found
[FisheyeUndist] Generating Cylinder maps, fov 170.0 deg
[quadcam_depth_est-5] process has died [pid 1393655, exit code -6, cmd /root/swarm_ws/devel/lib/quadcam_depth_est/quadcam_depth_est_node __name:=quadcam_depth_est __log:=/root/.ros/log/039ed24e-5402-11ef-bffc-0242ac110002/quadcam_depth_est-5.log].
log file: /root/.ros/log/039ed24e-5402-11ef-bffc-0242ac110002/quadcam_depth_est-5*.log
no photometric calibration file found
[FisheyeUndist] Generating Cylinder maps, fov 170.0 deg
no photometric calibration file found
[D2Frontend] Focal length initialize to: 460.0
[D2FrontEnd::LoopCam] Init CNNs using onnx
Trying to init MobileNetVLADONNX@/root/swarm_ws/src/D2SLAM/d2frontend/../models/mobilenetvlad_dyn_size.onnx tensorrt 0 fp16 1 int8 0 pca 0
Init SuperPointONNX: /root/swarm_ws/src/D2SLAM/d2frontend/../models/superpoint_v1_dyn_size.onnx size 936 312
[D2FrontEnd::LoopCam] Deepnet ready
[D2VINS::D2VINSConfig] readConfig from file /root/swarm_ws/src/D2SLAM/d2vins/../config/quadcam/d2slam_jackal.yaml
[D2VINS::D2VINSConfig] VINS use focal length 460.00
[D2Estimator::init] init done estimator on drone 1
[D2VINS::D2Estimator] extrinsic 1000: T [+0.053,+0.210,+0.353] YPR [-0.6,+0.5,-90.9]
[D2VINS::D2Estimator] extrinsic 1001: T [+0.253,+0.004,+0.363] YPR [-91.4,-1.2,-88.0]
[D2VINS::D2Estimator] extrinsic 1002: T [+0.037,-0.214,+0.337] YPR [+179.3,+0.7,-90.7]
[D2VINS::D2Estimator] extrinsic 1003: T [-0.173,-0.000,+0.366] YPR [+90.1,+0.7,-91.7]
[ INFO] [1722955311.828397310]: D2VINS node 1 initialized. Ready to start.
[ INFO] [1722955311.828516819]: Starting d2vins_net lcm.
[D2Frontend::LoopCam] KF Count 0 loop_cam cost avg 860.6ms cur 860.6ms
[ INFO] [1722955313.477486575]: [D2FeatureTracker] receive first, will init kf
Gtk-Message: 14:41:53.697: Failed to load module "canberra-gtk-module"
[D2VINS::D2Estimator] tryinitFirstPose imu buf 465
[D2Estimator::tryinitFirstPose] not enough imu data 2/465 for init
[LoopDetector] processImageArray 1000000 from drone 1 images: 4 landmark: 400 lazy: 0 matched_to -1@D-1
[LoopDetector] Add KF 1000000 with 4 images from 1 to local keyframe database. Total frames: 1
[D2Frontend::LoopCam] KF Count 1 loop_cam cost avg 458.0ms cur 55.5ms
[D2VINS::D2Estimator] tryinitFirstPose imu buf 497
[D2Estimator::tryinitFirstPose] not enough imu data 2/497 for init
[LoopDetector] processImageArray 1000001 from drone 1 images: 4 landmark: 400 lazy: 0 matched_to -1@D-1
[LoopDetector] Add KF 1000001 with 4 images from 1 to local keyframe database. Total frames: 2
[D2Frontend::LoopCam] KF Count 2 loop_cam cost avg 324.8ms cur 58.4ms
[D2Frontend::LoopCam] KF Count 3 loop_cam cost avg 259.0ms cur 61.4ms
[D2VINS::D2Estimator] tryinitFirstPose imu buf 571
[D2Estimator::tryinitFirstPose] not enough imu data 2/571 for init
[D2Frontend::LoopCam] KF Count 4 loop_cam cost avg 218.8ms cur 58.3ms
[D2Frontend::LoopCam] KF Count 5 loop_cam cost avg 191.2ms cur 53.1ms
[D2VINS::D2Estimator] tryinitFirstPose imu buf 637
[D2VINS::D2Estimator] Add prior for first frame and extrinsic 0: 0
Admin 6
[D2VINS::D2Estimator] Initial firstPose 1000005
[D2VINS::D2Estimator] Init pose with IMU: Pose T [+0.000,+0.000,+0.000] YPR [+0.0,+0.3,+0.0] Vel 0.00 0.00 0.00
[D2VINS::D2Estimator] Mean acc -0.056 0.007 9.792
[D2VINS::D2Estimator] Gyro bias: -0.016 -0.003 -0.003
[D2VINS::D2Estimator] Acc bias: 0.000 -0.000 -0.015
[D2VINS] force landmarks 0 to broadcast
[LoopNet@1] broadcast image array: 1000005 lazy: 0 size 174137 need_send_features 1
[D2Frontend::LoopCam] KF Count 6 loop_cam cost avg 171.5ms cur 52.9ms
[D2Frontend::LoopCam] KF Count 7 loop_cam cost avg 156.7ms cur 53.5ms
[D2VINS] force landmarks 0 to broadcast
[LoopNet@1] broadcast image array: 1000007 lazy: 0 size 176162 need_send_features 1
[D2Frontend::LoopCam] KF Count 8 loop_cam cost avg 145.4ms cur 55.0ms
[D2Frontend::LoopCam] KF Count 9 loop_cam cost avg 136.2ms cur 52.9ms
[D2VINS] force landmarks 0 to broadcast
[LoopNet@1] broadcast image array: 1000009 lazy: 0 size 177229 need_send_features 1
[D2Frontend::LoopCam] KF Count 10 loop_cam cost avg 128.6ms cur 52.9ms
[D2VINS::D2Estimator] Local IMU error freq: 420.477 start_t 1718106570.640/1718106570.640 end_t 1718106570.682/1718106570.686
[D2Frontend::LoopCam] KF Count 11 loop_cam cost avg 122.2ms cur 52.0ms
[D2VINS] force landmarks 0 to broadcast
[LoopNet@1] broadcast image array: 1000011 lazy: 0 size 175580 need_send_features 1
[D2Frontend::LoopCam] KF Count 12 loop_cam cost avg 117.0ms cur 55.0ms
[D2VINS::D2Estimator] Local IMU error freq: 421.861 start_t 1718106570.724/1718106570.725 end_t 1718106570.767/1718106570.769
[D2Frontend::LoopCam] KF Count 13 loop_cam cost avg 112.5ms cur 53.5ms
[D2VINS] force landmarks 0 to broadcast
[LoopNet@1] broadcast image array: 1000013 lazy: 0 size 176751 need_send_features 1
[D2Frontend::LoopCam] KF Count 14 loop_cam cost avg 108.6ms cur 53.5ms
[D2VINS::D2Estimator] Local IMU error freq: 379.990 start_t 1718106570.808/1718106570.811 end_t 1718106570.850/1718106570.851
[D2Frontend::LoopCam] KF Count 15 loop_cam cost avg 105.2ms cur 53.8ms
WARNING: Logging before InitGoogleLogging() is written to STDERR
F0806 14:41:54.934617 1393793 problem_impl.cc:513] Parameter block not found: 0x55c5b4ca5a00. You must add the parameter block to the problem before it can be set constant.
*** Check failure stack trace: ***
Stack trace (most recent call last) in thread 1393793:
#16 Object "", at 0xffffffffffffffff, in
#15 Source "../sysdeps/unix/sysv/linux/x86_64/clone.S", line 95, in __clone [0x7f73d0423352]
#14 Source "/build/glibc-e2p3jK/glibc-2.31/nptl/pthread_create.c", line 477, in start_thread [0x7f73d2a1d608]
#13 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.28", at 0x7f73d0736df3, in
[D2Frontend::LoopCam] KF Count 16 loop_cam cost avg 102.0ms cur 51.9ms
#12 | Source "/usr/include/c++/9/thread", line 195, in operator()
| 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 219, in _M_run [0x55c56db30e30]
216: imu_sub = nh.subscribe(params->imu_topic, 1000, &D2VINSNode::imuCallback, this, ros::TransportHints().tcpNoDelay()); //We need a big queue for IMU.
217: pgo_fused_sub = nh.subscribe("/d2pgo/swarm_fused", 1, &D2VINSNode::pgoSwarmFusedCallback, this, ros::TransportHints().tcpNoDelay());
218: thread_viokf = std::thread([&] {
> 219: processVIOKFThread();
220: printf("[D2VINS] processVIOKFThread exit.\n");
221: });
222: if (params->estimation_mode == D2VINSConfig::DISTRIBUTED_CAMERA_CONSENUS && params->consensus_sync_to_start) {
[D2Frontend::LoopCam] KF Count 17 loop_cam cost avg 99.3ms cur 52.1ms
#11 Source "/root/swarm_ws/src/D2SLAM/d2vins/src/d2vins_node.cpp", line 110, in processVIOKFThread [0x55c56db30456]
107: bool ret;
108: {
109: Utility::TicToc input;
> 110: ret = estimator->inputImage(viokf);
111: double input_time = input.toc();
112: Utility::TicToc loop;
113: if (viokf.is_keyframe) {
[D2Frontend::LoopCam] KF Count 18 loop_cam cost avg 96.8ms cur 52.9ms
#10 Source "/root/swarm_ws/src/D2SLAM/d2vins/src/estimator/d2estimator.cpp", line 307, in inputImage [0x7f73d0940407]
305: auto frame = addFrame(_frame);
306: if (state.size() >= params->min_solve_frames && params->estimation_mode != D2VINSConfig::DISTRIBUTED_CAMERA_CONSENUS) {
> 307: solveNonDistrib();
308: }
309: addSldWinToFrame(_frame);
310: frame_count ++;
[D2Frontend::LoopCam] KF Count 19 loop_cam cost avg 94.7ms cur 53.7ms
[D2Frontend::LoopCam] KF Count 20 loop_cam cost avg 92.6ms cur 52.1ms
#9 Source "/root/swarm_ws/src/D2SLAM/d2vins/src/estimator/d2estimator.cpp", line 549, in solveNonDistrib [0x7f73d093f327]
546: setupImuFactors();
547: setupLandmarkFactors();
548: setupPriorFactor();
> 549: setStateProperties();
550: SolverReport report = solver->solve();
551: state.syncFromState(used_landmarks);
[D2Frontend::LoopCam] KF Count 21 loop_cam cost avg 90.9ms cur 53.7ms
#8 Source "/root/swarm_ws/src/D2SLAM/d2vins/src/estimator/d2estimator.cpp", line 357, in setStateProperties [0x7f73d093c3d3]
354: if (!params->estimate_td || state.size() < params->max_sld_win_size ||
355: state.lastFrame().odom.vel().norm() < params->estimate_extrinsic_vel_thres) {
356: // printf("[D2Estimator::setStateProperties@%d] set td to fixed sld_size %d/%d \n", self_id, state.size(), params->max_sld_win_size);
> 357: problem.SetParameterBlockConstant(state.getTdState(self_id));
358: }
359:
360: if (!state.getPrior() || params->always_fixed_first_pose) {
[D2Frontend::LoopCam] KF Count 22 loop_cam cost avg 89.2ms cur 52.7ms
[D2Frontend::LoopCam] KF Count 23 loop_cam cost avg 87.7ms cur 52.4ms
#7 Object "/root/swarm_ws/devel/.private/d2common/lib/libd2common.so", at 0x7f73d2d13561, in ceres::internal::ProblemImpl::SetParameterBlockConstant(double const*)
#6 Object "/usr/lib/x86_64-linux-gnu/libglog.so.0.0.0", at 0x7f73d084f6ee, in google::LogMessageFatal::~LogMessageFatal()
#5 Object "/usr/lib/x86_64-linux-gnu/libglog.so.0.0.0", at 0x7f73d084eebe, in google::LogMessage::Flush()
#4 Object "/usr/lib/x86_64-linux-gnu/libglog.so.0.0.0", at 0x7f73d085425a, in google::LogMessage::SendToLog()
#3 Object "/usr/lib/x86_64-linux-gnu/libglog.so.0.0.0", at 0x7f73d084f1c2, in google::LogMessage::Fail()
#2 Object "/usr/lib/x86_64-linux-gnu/libglog.so.0.0.0", at 0x7f73d084be4b, in
#1 Source "/build/glibc-e2p3jK/glibc-2.31/stdlib/abort.c", line 79, in abort [0x7f73d0326858]
#0 Source "../sysdeps/unix/sysv/linux/raise.c", line 51, in raise [0x7f73d034700b]
Aborted (Signal sent by tkill() 1393652 0)
[d2vins-2] process has died [pid 1393652, exit code -6, cmd /root/swarm_ws/devel/lib/d2vins/d2vins_node __name:=d2vins __log:=/root/.ros/log/039ed24e-5402-11ef-bffc-0242ac110002/d2vins-2.log].
log file: /root/.ros/log/039ed24e-5402-11ef-bffc-0242ac110002/d2vins-2*.log
Thank you for sharing this valuable research!!
In my dataset, when I applied the 'quad_cam_calib-camchain-imucam-2.yaml' parameters, the execution proceeded normally regardless of the result. However, when I ran it with the parameters calibrated for my camera (camera calibration was done using tartancalib, and imu-cam extrinsic calibration was done using kalibr), the following error occurred. How can I solve this?