diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 18a09ac3e24f36c3cef6b11c87dfc861e17d14be..68d34c20bfd4e2abe3ca76d331f696d86423effe 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 f726291a9a0480dd2edc71d137511c543d4d3886..ad35fb6c2d4a317c0f48051dc7920bdea76ab77b 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 2f790d6335dadc8d4c96d13d20948a8009da517d..848fa3f12f21e709286a61cd37edb4122fe361bc 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 63f16d1e42b1994632478f293bd2d9c0a9ba2fe1..4df6215468e6aabba07caee1b8d28af225d6fccc 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 f29109d1862d134aa592df4b4c843cac6b0a8bd0..546fafeec33538915a23ed5e96a98400cebd58c4 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 4b278bf8bf8ef16eca8ea61ed4702d85358b6730..7df23b32b9d6f1bf876f796715ff6903fc6fd4bb 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 e80abe7168fd4f66986929566b8300371e0beed3..70d9c6c976190d8a105e5984be914c1d4933a2e9 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 9cd65ccc8b5e95b88234ccd5ef5e2a5f4683fca0..c308bed7130e146599e857170655488d52df3521 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 07450aa2edd1186aa50cac6b8d563c9409ec5ad0..ec7700ae21723253d5c5931d84ff4c151f8644cb 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);
-
}