diff --git a/include/core/factor/factor_block_absolute.h b/include/core/factor/factor_block_absolute.h index eb55faae757f7fe1b02d2fe29c75a049e0b04299..0743548b7b4aad0517ecdd2590955c4cdf7c023c 100644 --- a/include/core/factor/factor_block_absolute.h +++ b/include/core/factor/factor_block_absolute.h @@ -41,7 +41,7 @@ class FactorBlockAbsolute : public FactorAnalytic ProcessorBasePtr _processor_ptr = nullptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) : - FactorAnalytic("BLOCK ABS", + FactorAnalytic("FactorBlockAbsolute", nullptr, nullptr, nullptr, diff --git a/include/core/factor/factor_block_difference.h b/include/core/factor/factor_block_difference.h index 8f62b63838d79c6a654c80120bff4e4a144f4098..2ece75d51225d3c163244fd5d39b0dd2ace08e13 100644 --- a/include/core/factor/factor_block_difference.h +++ b/include/core/factor/factor_block_difference.h @@ -39,6 +39,10 @@ class FactorBlockDifference : public FactorAnalytic FactorBlockDifference( StateBlockPtr _sb1_ptr, StateBlockPtr _sb2_ptr, + FrameBasePtr _frame_other = nullptr, + CaptureBasePtr _cap_other = nullptr, + FeatureBasePtr _feat_other = nullptr, + LandmarkBasePtr _lmk_other = nullptr, unsigned int _start_idx1 = 0, int _size1 = -1, unsigned int _start_idx2 = 0, @@ -46,11 +50,11 @@ class FactorBlockDifference : public FactorAnalytic ProcessorBasePtr _processor_ptr = nullptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) : - FactorAnalytic("BLOCK ABS", - nullptr, - nullptr, - nullptr, - nullptr, + FactorAnalytic("FactorBlockDifference", + _frame_other, + _cap_other, + _feat_other, + _lmk_other, _processor_ptr, _apply_loss_function, _status, @@ -146,6 +150,8 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma assert(_st_vector[1].size() == sb2_size_ && "Wrong StateBlock size"); assert(_st_vector[0].size() >= getMeasurement().size() && "StateBlock size and measurement size should match"); assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match"); + assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() && "Wrong jacobian sb1 or covariance size"); + assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size"); // normalized jacobian _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_; @@ -163,6 +169,8 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma assert(_st_vector[1].size() == sb2_size_ && "Wrong StateBlock size"); assert(_st_vector[0].size() >= getMeasurement().size() && "StateBlock size and measurement size should match"); assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match"); + assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() && "Wrong jacobian sb1 or covariance size"); + assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size"); // normalized jacobian _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_; diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp index 20bbe2c42ed27688820569a3da345596da3903f1..1e06c7a80377b99197a721e85473319f9bf34862 100644 --- a/test/gtest_factor_block_difference.cpp +++ b/test/gtest_factor_block_difference.cpp @@ -50,18 +50,17 @@ class FixtureFactorBlockDifference : public testing::Test Vector10d x_origin = problem_->zeroState(); Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity(); - KF0_ =problem_->setPrior(x_origin, cov_prior, t0, 0.1); + KF0_ = problem_->setPrior(x_origin, cov_prior, t0, 0.1); CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t0); FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.tail(3), cov_prior.bottomRightCorner<3,3>()); FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV()); - // KF0_ = problem_->emplaceFrame(KEY, problem_->zeroState(), t0); KF1_ = problem_->emplaceFrame(KEY, problem_->zeroState(), t1); - Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "DIFF", t1); + Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "BlockDifference", t1); Eigen::Matrix3d cov = 0.2 * Eigen::Matrix3d::Identity(); - Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "DIFF", zero3, cov); + Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", zero3, cov); } virtual void TearDown() override {} @@ -150,6 +149,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPx) Feat_->setMeasurementCovariance(cov_diff); FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>( Feat_, KF0_->getP(), KF1_->getP(), + nullptr, nullptr, nullptr, nullptr, 0, 1, 0, 1 ); @@ -174,6 +174,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPxy) Feat_->setMeasurementCovariance(cov_diff); FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>( Feat_, KF0_->getP(), KF1_->getP(), + nullptr, nullptr, nullptr, nullptr, 0, 2, 0, 2 ); @@ -195,6 +196,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPyz) Feat_->setMeasurementCovariance(cov_diff); FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>( Feat_, KF0_->getP(), KF1_->getP(), + nullptr, nullptr, nullptr, nullptr, 1, 2, 1, 2 );