diff --git a/svo_ceres_backend/src/estimator.cpp b/svo_ceres_backend/src/estimator.cpp index 98e2222..18dfc5d 100644 --- a/svo_ceres_backend/src/estimator.cpp +++ b/svo_ceres_backend/src/estimator.cpp @@ -251,6 +251,8 @@ bool Estimator::addStates(const FrameBundleConstPtr& frame_bundle, information(0,0) = 1.0e8; information(1,1) = 1.0e8; information(2,2) = 1.0e8; + information(3,3) = 1.0e-8; // numeric issue + information(4,4) = 1.0e-8; // numeric issue std::shared_ptr pose_error = std::make_shared(T_WS, information); map_ptr_->addResidualBlock(pose_error, nullptr, pose_parameter_block); @@ -563,7 +565,7 @@ bool Estimator::addVelocityPrior(BackendId nframe_id, Eigen::Matrix information; information.setIdentity(); information.topLeftCorner<3, 3>() *= speed_information; - information.bottomLeftCorner<6, 6>() *= bias_information; + information.bottomRightCorner<6, 6>() *= bias_information; std::shared_ptr prior = std::make_shared(speed_and_bias, information); ceres::ResidualBlockId id = @@ -1663,6 +1665,8 @@ bool Estimator::setFrameFixed(const BundleId &fixed_frame_bundle_id, information(1, 1) = 1.0e35; information(2, 2) = 1.0e35; information(5, 5) = 1.0e35; + information(3, 3) = 1.0e-8; // numeric issue + information(4, 4) = 1.0e-8; // numeric issue std::shared_ptr pose_error = std::make_shared(T_WS_new, information); std::shared_ptr block_ptr = @@ -1694,6 +1698,8 @@ void Estimator::setOldestFrameFixed() information(1, 1) = 1.0e14; information(2, 2) = 1.0e14; information(5, 5) = 1.0e14; + information(3, 3) = 1.0e-8; // numeric issue + information(4, 4) = 1.0e-8; // numeric issue std::shared_ptr pose_error = std::make_shared(T_WS_0, information); map_ptr_->addResidualBlock( diff --git a/svo_direct/src/feature_detection_utils.cpp b/svo_direct/src/feature_detection_utils.cpp index e0b5f54..e0f012e 100644 --- a/svo_direct/src/feature_detection_utils.cpp +++ b/svo_direct/src/feature_detection_utils.cpp @@ -168,8 +168,8 @@ void fastDetector( img_pyr[level].rows, img_pyr[level].step, threshold, fast_corners); #else fast::fast_corner_detect_10( - (fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, - img_pyr[L].rows, img_pyr[L].step, threshold, fast_corners); + (fast::fast_byte*) img_pyr[level].data, img_pyr[level].cols, + img_pyr[level].rows, img_pyr[level].step, threshold, fast_corners); #endif std::vector scores, nm_corners; fast::fast_corner_score_10((fast::fast_byte*) img_pyr[level].data, img_pyr[level].step,