search

lukasvst / dm-vio

Source code for the paper DM-VIO: Delayed Marginalization Visual-Inertial Odometry
GNU General Public License v3.0
1.02k stars 183 forks source link

crash after large rotation #16

Closed rancheng closed 2 years ago

rancheng commented 2 years ago

Thanks for realeseing this awesome project, I tested with my own sensor, Realsense D455, I calibrated with basalt and prepared the full dataset here:

https://drive.google.com/file/d/15B9qwuYwPDqlkmGxKPgyjYxCOanzWDRN/view?usp=sharing

it can initialize sucessfully and the track looks good though:

Peek 2022-06-16 17-42

The parameter I use to run this dataset is as following:

dmvio_dataset files=/home/ran/d455/061601/cam0/images \
                      vignette=/home/ran/d455/061601/cam0/vignette.png \
                      imuFile=/home/ran/d455/061601/imu.txt \
                      calib=/home/ran/d455/061601/cam0/camera.txt \
                      gamma=/home/ran/d455/061601/cam0/pcalib.txt \
                      imuCalib=/home/ran/d455/061601/cam0/camchain.yaml \
                      mode=0 \
                      use16Bit=0 \
                      preset=0 \
                      nogui=0 \
                      useimu=1 \
                      resultsPrefix=/home/ran/d455/061601/result/ \
                      settingsFile=/home/ran/d455/061601/imu_util_setting.yaml \
                      start=1

The full error log is as shown below:

Frame history size: 100
Preparing keyframe: 99
Frames between KFs: 1
SPARSITY:  MinActDist 3.100000 (need 1000 points, have 1003 points)!
OPTIMIZE 1352 pts, 8438 active res, 0 lin res!
Initial Error           A(1412939.548978)=(AV 4.795). Num: A(7682) + M(35736); ab -0.048004 -5.945345!
Dynamic weight: 1
STEPS: A 2.8; B 7.0; R 54.0; T 45.3.    ACCEPT 0 (L -5.00, dir nan, ss 1.0):    A(1393530.952595)=(AV 4.625). Num: A(8144) + M(35736); ab -0.000027 -7.002813!
Optimized scale: 0.760244
Dynamic weight: 1
STEPS: A 0.0; B 1.6; R 1.9; T 3.6.  REJECT 1 (L -5.00, dir 0.23, ss 1.0):   A(1434222.438504)=(AV 4.699). Num: A(8118) + M(35736); ab -0.000016 -6.850309!
Dynamic weight: 1
STEPS: A 0.0; B 1.6; R 1.6; T 1.9.  REJECT 2 (L -3.00, dir 0.67, ss 1.0):   A(1434039.078252)=(AV 4.699). Num: A(8118) + M(35736); ab -0.000016 -6.851277!
Dynamic weight: 1
STEPS: A 0.0; B 1.2; R 0.3; T 0.8.  REJECT 3 (L -1.00, dir 0.77, ss 1.0):   A(1432385.235909)=(AV 4.696). Num: A(8118) + M(35736); ab -0.000017 -6.895350!
Dynamic weight: 1
STEPS: A 0.0; B 0.1; R 0.0; T 0.0.  REJECT 4 (L 1.00, dir 0.85, ss 1.0):    A(1427214.858555)=(AV 4.688). Num: A(8118) + M(35736); ab -0.000026 -6.995111!
Num BA Iterations done: 5
Error: Not Eq:
-6.73509e-05 -3.98478e-05  3.01999e-05 -7.31209e-09  1.30427e-08 -2.78251e-08
-3.98478e-05 -4.44526e-05  4.78257e-05  1.85159e-08  1.43971e-08 -2.13536e-08
 3.01999e-05  4.78257e-05 -5.64214e-05 -3.26859e-08 -1.63767e-08  2.18551e-08
-7.31209e-09  1.85159e-08 -3.26859e-08 -5.60378e-11  2.72399e-10 -9.15505e-11
 1.30427e-08  1.43971e-08 -1.63767e-08  2.72399e-10    1.582e-10 -2.14103e-11
-2.78251e-08 -2.13536e-08  2.18551e-08 -9.15505e-11 -2.14103e-11 -1.12186e-11
first:
     3.30707     0.977633    -0.251573  0.000916987 -7.55153e-07 -1.16048e-05
    0.977633      1.64606     -1.91115  0.000228962  0.000445313  -0.00032959
   -0.251573     -1.91115      2.67072  3.21103e-05 -0.000628347  0.000464945
 0.000916987  0.000228962  3.21103e-05  0.000194585    1.122e-05  8.35902e-07
-7.55153e-07  0.000445313 -0.000628347    1.122e-05  0.000189684  -7.9984e-06
-1.16048e-05  -0.00032959  0.000464945  8.35902e-07  -7.9984e-06  0.000167261
second
     3.30714     0.977673    -0.251603  0.000916994 -7.68196e-07  -1.1577e-05
    0.977673       1.6461      -1.9112  0.000228943  0.000445299 -0.000329568
   -0.251603      -1.9112      2.67077   3.2143e-05 -0.000628331  0.000464923
 0.000916994  0.000228943   3.2143e-05  0.000194585  1.12198e-05  8.35994e-07
-7.68196e-07  0.000445299 -0.000628331  1.12198e-05  0.000189684 -7.99838e-06
 -1.1577e-05 -0.000329568  0.000464923  8.35994e-07 -7.99838e-06  0.000167261
Max diff: 6.73509e-05
 0# void dmvio::assertEqEigen<Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, double) in /home/ran/CLionProjects/dm-vio/cmake-build-debug/bin/dmvio_dataset
 1# dmvio::BAIMULogic::computeFactorForCoarseGraphAndMarginalCovariances() in /home/ran/CLionProjects/dm-vio/cmake-build-debug/bin/dmvio_dataset
 2# dmvio::BAIMULogic::postOptimization(int) in /home/ran/CLionProjects/dm-vio/cmake-build-debug/bin/dmvio_dataset
 3# dmvio::IMUIntegration::postOptimization(int) in /home/ran/CLionProjects/dm-vio/cmake-build-debug/bin/dmvio_dataset
 4# dso::FullSystem::makeKeyFrame(dso::FrameHessian*) in /home/ran/CLionProjects/dm-vio/cmake-build-debug/bin/dmvio_dataset
 5# dso::FullSystem::deliverTrackedFrame(dso::FrameHessian*, bool) in /home/ran/CLionProjects/dm-vio/cmake-build-debug/bin/dmvio_dataset
 6# dso::FullSystem::addActiveFrame(dso::ImageAndExposure*, int, std::vector<dmvio::IMUMeasurement, std::allocator<dmvio::IMUMeasurement> >*, dmvio::GTData*) in /home/ran/CLionProjects/dm-vio/cmake-build-debug/bin/dmvio_dataset
 7# run(ImageFolderReader*, dso::IOWrap::PangolinDSOViewer*) in /home/ran/CLionProjects/dm-vio/cmake-build-debug/bin/dmvio_dataset
 8# 0x00007F028FBE0BCD in /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.65.1
 9# 0x00007F028E8BD6DB in /lib/x86_64-linux-gnu/libpthread.so.0
10# clone in /lib/x86_64-linux-gnu/libc.so.6
dmvio_dataset: /home/ran/CLionProjects/dm-vio/src/GTSAMIntegration/ExtUtils.h:61: void dmvio::assertEqEigen(const T&, const T&, double) [with T = Eigen::Matrix<double, -1, -1>]: Assertion `0' failed.

Process finished with exit code 134 (interrupted by signal 6: SIGABRT)
lukasvst commented 2 years ago

Thanks for providing all the relevant info in your issue! Can you try to compile DM-VIO in the mode RelWithDebInfo info instead of Debug?

A bit of background on this: In debug mode I added lots of asserts for certain properties (e.g. PoseTransformation inverse should be correct, analytic Jacobians should be equal to numeric ones, etc.). However the epsilon thresholds are sometimes a bit tight and while they are satisfied on the EuRoC dataset, on datasets with different motions they might fail. Hence, Debug mode is currently only supported on the EuRoC dataset and on other datasets you should use RelWithDebInfo.

Lastly: You might find the new version of the code interesting. While the Realsense-Live-version currently only supports T265 officially, I think it should also work well on your camera with some adjustments.

rancheng commented 2 years ago

haha, you are correct, adding the RelWithDebInfo flag didn't prompt error anymore, at the same time me and my colleague wrote the D455 and D435i version in our folk, we will clean the code and create a pull request as soon.

Thanks again for your quick reply!

As there is no problem in this case, I will close this issue for now.

antithing commented 1 year ago

@rancheng are you using the depth information from the D455 to give accurate world scale? Or just running monocular?

Thanks