From 9a3acdd141975c37d7ae6b3ca9d10a84f1a75670 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= <mfourmy@laas.fr> Date: Tue, 27 Apr 2021 16:31:21 +0200 Subject: [PATCH] Fix sensor pose covariance setting in constructor --- include/core/capture/capture_base.h | 1 + .../factor/factor_pose_3d_with_extrinsics.h | 68 ++++++++++++++++--- include/core/frame/frame_base.h | 1 + include/core/sensor/sensor_pose.h | 2 +- src/capture/capture_base.cpp | 7 ++ src/frame/frame_base.cpp | 7 ++ src/processor/processor_pose.cpp | 2 +- src/sensor/sensor_pose.cpp | 3 +- ...sor_and_factor_pose_3d_with_extrinsics.cpp | 15 +++- 9 files changed, 92 insertions(+), 14 deletions(-) diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h index 18a09ac3e..68d34c20b 100644 --- a/include/core/capture/capture_base.h +++ b/include/core/capture/capture_base.h @@ -67,6 +67,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s const FeatureBasePtrList& getFeatureList() const; void getFactorList(FactorBasePtrList& _fac_list) const; + FactorBasePtrList getFactorList() const; SensorBasePtr getSensor() const; virtual void setSensor(const SensorBasePtr sensor_ptr); diff --git a/include/core/factor/factor_pose_3d_with_extrinsics.h b/include/core/factor/factor_pose_3d_with_extrinsics.h index f726291a9..ad35fb6c2 100644 --- a/include/core/factor/factor_pose_3d_with_extrinsics.h +++ b/include/core/factor/factor_pose_3d_with_extrinsics.h @@ -40,6 +40,18 @@ class FactorPose3dWithExtrinsics : public FactorAutodiff<FactorPose3dWithExtrins ~FactorPose3dWithExtrinsics() override = default; + template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7> + inline void error(const Eigen::MatrixBase<D1> & w_p_wb, + const Eigen::QuaternionBase<D2> & w_q_b, + const Eigen::MatrixBase<D3> & b_p_bm, + const Eigen::QuaternionBase<D4> & b_q_m, + const Eigen::MatrixBase<D5> & w_p_wm, + const Eigen::QuaternionBase<D6> & w_q_m, + Eigen::MatrixBase<D7> & p_err, + Eigen::MatrixBase<D7> & o_err) const; + + inline Eigen::Vector6d error() const; + template<typename T> bool operator ()(const T* const _p, const T* const _q, @@ -48,6 +60,50 @@ class FactorPose3dWithExtrinsics : public FactorAutodiff<FactorPose3dWithExtrins T* _residuals) const; }; +template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7> +inline void FactorPose3dWithExtrinsics::error(const Eigen::MatrixBase<D1> & w_p_wb, + const Eigen::QuaternionBase<D2> & w_q_b, + const Eigen::MatrixBase<D3> & b_p_bm, + const Eigen::QuaternionBase<D4> & b_q_m, + const Eigen::MatrixBase<D5> & w_p_wm, + const Eigen::QuaternionBase<D6> & w_q_m, + Eigen::MatrixBase<D7> & p_err, + Eigen::MatrixBase<D7> & o_err) const +{ + // Transformation definitions: + // w_p_wm, w_q_m: world to measurement frame (robot attached mocap frame) + // w_p_wb, w_q_b: world to robot base frame + // b_p_bm, b_q_m: base to measurement frame (extrinsics) + // Error function: + // err = w_T_m |-| (w_T_b*b_T_m) + + p_err = w_p_wm - (w_p_wb + w_q_b*b_p_bm); + o_err = q2v((w_q_b * b_q_m).conjugate() * w_q_m); +} + + +inline Eigen::Vector6d FactorPose3dWithExtrinsics::error() const +{ + // get current frame and extrinsics estimates + const Eigen::Vector3d w_p_wb = (getFeature()->getFrame()->getP()->getState()); + const Eigen::Quaterniond w_q_b (getFeature()->getFrame()->getO()->getState().data()); + const Eigen::Vector3d b_p_bm = (getFeature()->getCapture()->getSensorP()->getState()); + const Eigen::Quaterniond b_q_m (getFeature()->getCapture()->getSensorO()->getState().data()); + + // measurements + Eigen::Vector3d w_p_wm(getMeasurement().data() + 0); // measurements + Eigen::Quaterniond w_q_m (getMeasurement().data() + 3); // measurements + + Vector6d err; + Eigen::Map<Vector3d> p_err(err.data() + 0); + Eigen::Map<Vector3d> o_err(err.data() + 3); + + error(w_p_wb, w_q_b, b_p_bm, b_q_m, w_p_wm, w_q_m, p_err, o_err); + + return err; +} + + template<typename T> inline bool FactorPose3dWithExtrinsics::operator ()(const T* const _p, const T* const _q, @@ -65,16 +121,10 @@ inline bool FactorPose3dWithExtrinsics::operator ()(const T* const _p, Eigen::Vector3d w_p_wm(getMeasurement().data() + 0); // measurements Eigen::Quaterniond w_q_m (getMeasurement().data() + 3); // measurements - // Transformation definitions: - // w_p_wm, w_q_m: world to measurement frame (robot attached mocap frame) - // w_p_wb, w_q_b: world to robot base frame - // b_p_bm, b_q_m: base to measurement frame (extrinsics) - // Error function: - // err = w_T_m |-| (w_T_b*b_T_m) - Eigen::Matrix<T, 6, 1> err; // error - err.head(3) = w_p_wm - (w_p_wb + w_q_b*b_p_bm); - err.tail(3) = q2v((w_q_b * b_q_m).conjugate() * w_q_m.cast<T>()); + Eigen::Map<Matrix<T, 3, 1> > p_err(err.data() + 0); + Eigen::Map<Matrix<T, 3, 1> > o_err(err.data() + 3); + error(w_p_wb, w_q_b, b_p_bm, b_q_m, w_p_wm.cast<T>(), w_q_m.cast<T>(), p_err, o_err); // Residuals Eigen::Map<Eigen::Matrix<T,6,1> > res(_residuals); diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h index 2f790d633..848fa3f12 100644 --- a/include/core/frame/frame_base.h +++ b/include/core/frame/frame_base.h @@ -108,6 +108,7 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr) const; FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const; + FactorBasePtrList getFactorList() const; void getFactorList(FactorBasePtrList& _fac_list) const; unsigned int getHits() const; diff --git a/include/core/sensor/sensor_pose.h b/include/core/sensor/sensor_pose.h index 63f16d1e4..4df621546 100644 --- a/include/core/sensor/sensor_pose.h +++ b/include/core/sensor/sensor_pose.h @@ -34,7 +34,7 @@ struct ParamsSensorPose : public ParamsSensorBase { return ParamsSensorBase::print() + "\n" + "std_p: " + std::to_string(std_p) + "\n" - + "std_o: " + std::to_string(std_o) + "\n"; + + "std_o: " + std::to_string(std_o) + "\n"; } ~ParamsSensorPose() override = default; }; diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp index f29109d18..546fafeec 100644 --- a/src/capture/capture_base.cpp +++ b/src/capture/capture_base.cpp @@ -119,6 +119,13 @@ void CaptureBase::removeFeature(FeatureBasePtr _ft_ptr) feature_list_.remove(_ft_ptr); } +FactorBasePtrList CaptureBase::getFactorList() const +{ + FactorBasePtrList fac_list; + getFactorList(fac_list); + return fac_list; +} + void CaptureBase::getFactorList(FactorBasePtrList& _fac_list) const { for (auto f_ptr : getFeatureList()) diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp index 4b278bf8b..7df23b32b 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/frame_base.cpp @@ -216,6 +216,13 @@ FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr) cons return nullptr; } +FactorBasePtrList FrameBase::getFactorList() const +{ + FactorBasePtrList fac_list; + getFactorList(fac_list); + return fac_list; +} + void FrameBase::getFactorList(FactorBasePtrList& _fac_list) const { for (auto c_ptr : getCaptureList()) diff --git a/src/processor/processor_pose.cpp b/src/processor/processor_pose.cpp index e80abe716..70d9c6c97 100644 --- a/src/processor/processor_pose.cpp +++ b/src/processor/processor_pose.cpp @@ -36,7 +36,7 @@ void ProcessorPose::createFactorIfNecessary(){ } auto cap_it = buffer_capture_.selectIterator(t, time_tolerance); - // if capture with corresponding timestamp is not found, stop and assume you will get it later + // if capture with corresponding timestamp is not found, assume you will get it later if (cap_it != buffer_capture_.getContainer().end()) { // if a corresponding capture exists, link it to the KF and create a factor diff --git a/src/sensor/sensor_pose.cpp b/src/sensor/sensor_pose.cpp index 9cd65ccc8..c308bed71 100644 --- a/src/sensor/sensor_pose.cpp +++ b/src/sensor/sensor_pose.cpp @@ -20,8 +20,7 @@ SensorPose::SensorPose(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensor { assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3d."); - noise_cov_ = Matrix6d::Identity(); - // noise_cov_ = (Eigen::Vector6d() << std_p_*std_p_, std_p_*std_p_, std_p_*std_p_, std_o_*std_o_, std_o_*std_o_, std_o_*std_o_).finished().asDiagonal(); + noise_cov_ = (Eigen::Vector6d() << std_p_*std_p_, std_p_*std_p_, std_p_*std_p_, std_o_*std_o_, std_o_*std_o_, std_o_*std_o_).finished().asDiagonal(); setNoiseCov(noise_cov_); // sets also noise_std_ } diff --git a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp index 07450aa2e..ec7700ae2 100644 --- a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp +++ b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp @@ -254,6 +254,20 @@ TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, check_tree) ASSERT_TRUE(problem_->check(0)); } +TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, error) +{ + // Since initialized at the right state, error function should return 0 + std::vector<FrameBasePtr> KF_vec = {KF1_, KF2_, KF3_}; + for (auto KF: KF_vec){ + for (auto fac: KF->getFactorList()){ + auto f = std::dynamic_pointer_cast<FactorPose3dWithExtrinsics>(fac); + if (f){ + ASSERT_MATRIX_APPROX(f->error(), Vector6d::Zero(), 1e-6); + } + } + } +} + TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, solve) { problem_->perturb(); @@ -264,7 +278,6 @@ TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, solve) ASSERT_MATRIX_APPROX(KF2_->getStateVector(), pose2_, 1e-6); ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6); ASSERT_MATRIX_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6); - } -- GitLab