From 484cecfd12af279cace88620a26357e22db2a773 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Thu, 9 Dec 2021 18:41:54 +0100
Subject: [PATCH] Remove frame's time tolerance from all APIs
---
demos/mcapi_pov_estimation.cpp | 2 +-
demos/solo_mocap_imu.cpp | 2 +-
demos/solo_real_pov_estimation.cpp | 4 ++--
.../processor/processor_inertial_kinematics.h | 9 ++++-----
.../processor/processor_point_feet_nomove.h | 7 +++----
src/processor/processor_inertial_kinematics.cpp | 2 +-
src/processor/processor_point_feet_nomove.cpp | 6 +++---
test/gtest_factor_force_torque.cpp | 2 +-
test/gtest_factor_inertial_kinematics.cpp | 2 +-
test/gtest_processor_inertial_kinematics.cpp | 2 +-
test/gtest_processor_point_feet_nomove.cpp | 10 +++++-----
11 files changed, 23 insertions(+), 25 deletions(-)
diff --git a/demos/mcapi_pov_estimation.cpp b/demos/mcapi_pov_estimation.cpp
index 1b84705..c85c220 100644
--- a/demos/mcapi_pov_estimation.cpp
+++ b/demos/mcapi_pov_estimation.cpp
@@ -451,7 +451,7 @@ int main (int argc, char **argv) {
TimeStamp t0(t_arr[0]);
VectorComposite x_origin("POV", {p_ob_gtr_v[0], q_ob_gtr_v[0], v_ob_gtr_v[0]});
VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()});
- FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0, 0.0005);
+ FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0);
proc_imu->setOrigin(KF1);
proc_legodom->setOrigin(KF1);
diff --git a/demos/solo_mocap_imu.cpp b/demos/solo_mocap_imu.cpp
index acead6c..82a0734 100644
--- a/demos/solo_mocap_imu.cpp
+++ b/demos/solo_mocap_imu.cpp
@@ -236,7 +236,7 @@ int main (int argc, char **argv) {
VectorComposite x_origin("POV", {w_p_wi_init, w_qvec_wm, Vector3d::Zero()});
VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()});
// FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0, 0.0005);
- FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0, time_tolerance_mocap); // if mocap used
+ FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0); // if mocap used
proc_imu->setOrigin(KF1);
diff --git a/demos/solo_real_pov_estimation.cpp b/demos/solo_real_pov_estimation.cpp
index 51ade73..9845fc5 100644
--- a/demos/solo_real_pov_estimation.cpp
+++ b/demos/solo_real_pov_estimation.cpp
@@ -302,8 +302,8 @@ int main (int argc, char **argv) {
VectorComposite x_origin("POV", {w_p_wm, w_qvec_wm, Vector3d::Zero()});
VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()});
- // FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0, 0.0005);
- FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0, 0.0005); // if mocap used
+ // FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0);
+ FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0); // if mocap used
proc_imu->setOrigin(KF1);
diff --git a/include/bodydynamics/processor/processor_inertial_kinematics.h b/include/bodydynamics/processor/processor_inertial_kinematics.h
index 5bd8b2e..3bedaa8 100644
--- a/include/bodydynamics/processor/processor_inertial_kinematics.h
+++ b/include/bodydynamics/processor/processor_inertial_kinematics.h
@@ -72,9 +72,9 @@ class ProcessorInertialKinematics : public ProcessorBase{
bool createInertialKinematicsFactor(CaptureInertialKinematicsPtr _cap_ikin, CaptureBasePtr _cap_angvel, CaptureBasePtr _cap_ikin_origin);
void processCapture(CaptureBasePtr _incoming) override;
- void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override;
+ void processKeyFrame(FrameBasePtr _keyframe_ptr) override;
bool triggerInCapture(CaptureBasePtr) const override;
- bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const override;
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override;
bool storeKeyFrame(FrameBasePtr) override;
bool storeCapture(CaptureBasePtr) override;
bool voteForKeyFrame() const override;
@@ -87,8 +87,7 @@ class ProcessorInertialKinematics : public ProcessorBase{
-inline bool ProcessorInertialKinematics::triggerInKeyFrame(FrameBasePtr _keyframe_ptr,
- const double& _time_tolerance) const
+inline bool ProcessorInertialKinematics::triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const
{
return false;
}
@@ -113,7 +112,7 @@ inline bool ProcessorInertialKinematics::voteForKeyFrame() const
return false;
}
-inline void ProcessorInertialKinematics::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
+inline void ProcessorInertialKinematics::processKeyFrame(FrameBasePtr _keyframe_ptr)
{
}
diff --git a/include/bodydynamics/processor/processor_point_feet_nomove.h b/include/bodydynamics/processor/processor_point_feet_nomove.h
index 8a63b72..f9265c5 100644
--- a/include/bodydynamics/processor/processor_point_feet_nomove.h
+++ b/include/bodydynamics/processor/processor_point_feet_nomove.h
@@ -61,9 +61,9 @@ class ProcessorPointFeetNomove : public ProcessorBase{
bool createPointFeetNomoveFactor(CapturePointFeetNomovePtr _cap_pfnomove, CaptureBasePtr _cap_angvel, CaptureBasePtr _cap_pfnomove_origin);
void processCapture(CaptureBasePtr _incoming) override;
- void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override;
+ void processKeyFrame(FrameBasePtr _keyframe_ptr) override;
bool triggerInCapture(CaptureBasePtr) const override;
- bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const override;
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override;
bool storeKeyFrame(FrameBasePtr) override;
bool storeCapture(CaptureBasePtr) override;
bool voteForKeyFrame() const override;
@@ -77,8 +77,7 @@ class ProcessorPointFeetNomove : public ProcessorBase{
-inline bool ProcessorPointFeetNomove::triggerInKeyFrame(FrameBasePtr _keyframe_ptr,
- const double& _time_tolerance) const
+inline bool ProcessorPointFeetNomove::triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const
{
return true;
}
diff --git a/src/processor/processor_inertial_kinematics.cpp b/src/processor/processor_inertial_kinematics.cpp
index 6ab3a72..ff09770 100644
--- a/src/processor/processor_inertial_kinematics.cpp
+++ b/src/processor/processor_inertial_kinematics.cpp
@@ -77,7 +77,7 @@ inline void ProcessorInertialKinematics::processCapture(CaptureBasePtr _capture)
&& (buffer_capture_it != buffer_capture_.getContainer().end()))
{
- bool time_ok = buffer_capture_.simpleCheckTimeTolerance(buffer_frame_it->first, buffer_capture_it->first, buffer_frame_it->second->getTimeTolerance());
+ bool time_ok = buffer_capture_.simpleCheckTimeTolerance(buffer_frame_it->first, buffer_capture_it->first, getTimeTolerance());
if (time_ok) {
CaptureBasePtr cap_angvel = buffer_frame_it->second->getCaptureOf(sensor_angvel);
auto min_ts = (buffer_frame_it->first < buffer_capture_it->first) ? buffer_frame_it->first : buffer_capture_it->first;
diff --git a/src/processor/processor_point_feet_nomove.cpp b/src/processor/processor_point_feet_nomove.cpp
index 2ba76fd..036fee9 100644
--- a/src/processor/processor_point_feet_nomove.cpp
+++ b/src/processor/processor_point_feet_nomove.cpp
@@ -52,8 +52,8 @@ void ProcessorPointFeetNomove::createFactorIfNecessary(){
auto kf2_it = std::next(kf1_it);
TimeStamp t1 = kf1_it->first;
TimeStamp t2 = kf2_it->first;
- auto cap1_it = buffer_capture_.selectIterator(t1, kf1_it->second->getTimeTolerance());
- auto cap2_it = buffer_capture_.selectIterator(t2, kf1_it->second->getTimeTolerance());
+ auto cap1_it = buffer_capture_.selectIterator(t1, getTimeTolerance());
+ auto cap2_it = buffer_capture_.selectIterator(t2, getTimeTolerance());
// check that the first 2 KF have corresponding captures in the capture buffer
// just quit and assume that you will someday have matching captures
@@ -143,7 +143,7 @@ inline void ProcessorPointFeetNomove::processCapture(CaptureBasePtr _capture)
}
-inline void ProcessorPointFeetNomove::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
+inline void ProcessorPointFeetNomove::processKeyFrame(FrameBasePtr _keyframe_ptr)
{
if (!_keyframe_ptr)
{
diff --git a/test/gtest_factor_force_torque.cpp b/test/gtest_factor_force_torque.cpp
index 5b69528..5a62fc0 100644
--- a/test/gtest_factor_force_torque.cpp
+++ b/test/gtest_factor_force_torque.cpp
@@ -241,7 +241,7 @@ class FactorInertialKinematics_2KF : public testing::Test
tA_.set(0);
x_origin_ = VectorComposite("POV", {Vector3d(0,0,0), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
s_origin_ = VectorComposite("POV", {1e-3*Vector3d(1,1,1), 1e-3*Vector3d(1,1,1), 1e-3*Vector3d(1,1,1)});
- KFA_ = problem_->setPriorFactor(x_origin_, s_origin_, tA_, 0.005);
+ KFA_ = problem_->setPriorFactor(x_origin_, s_origin_, tA_);
// ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
diff --git a/test/gtest_factor_inertial_kinematics.cpp b/test/gtest_factor_inertial_kinematics.cpp
index b390ba9..00d43d2 100644
--- a/test/gtest_factor_inertial_kinematics.cpp
+++ b/test/gtest_factor_inertial_kinematics.cpp
@@ -118,7 +118,7 @@ class FactorInertialKinematics_1KF : public testing::Test
x_origin_ = VectorComposite("POV", {Vector3d(0,0,0), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
s_origin_ = VectorComposite("POV", {1e-3*Vector3d(1,1,1), 1e-3*Vector3d(1,1,1), 1e-3*Vector3d(1,1,1)});
t_.set(0.0);
- KF0_ = problem_->setPriorFactor(x_origin_, s_origin_, t_, 0.005);
+ KF0_ = problem_->setPriorFactor(x_origin_, s_origin_, t_);
ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
SIK_ = std::make_shared<SensorInertialKinematics>(Eigen::VectorXd(0), intr_ik);
diff --git a/test/gtest_processor_inertial_kinematics.cpp b/test/gtest_processor_inertial_kinematics.cpp
index 59e1580..ded4ae7 100644
--- a/test/gtest_processor_inertial_kinematics.cpp
+++ b/test/gtest_processor_inertial_kinematics.cpp
@@ -123,7 +123,7 @@ class FactorInertialKinematics_2KF : public testing::Test
VectorXd std_vec = pow(1e-3, 2)*VectorXd::Ones(18);
VectorComposite prior(x_origin_, "POVCDL", {3,4,3,3,3,3});
VectorComposite prior_std(std_vec, "POVCDL", {3,3,3,3,3,3});
- problem_->setPriorFactor(prior, prior_std, t_, 0.001);
+ problem_->setPriorFactor(prior, prior_std, t_);
KF0_ = problem_->getTrajectory()->getLastFrame();
// Set origin of processor Imu
diff --git a/test/gtest_processor_point_feet_nomove.cpp b/test/gtest_processor_point_feet_nomove.cpp
index c4fb675..e501a8a 100644
--- a/test/gtest_processor_point_feet_nomove.cpp
+++ b/test/gtest_processor_point_feet_nomove.cpp
@@ -107,7 +107,7 @@ class PointFeetCaptures : public testing::Test
VectorXd std_vec = pow(1e-3, 2)*VectorXd::Ones(6);
VectorComposite prior(x_origin_, "PO", {3,4});
VectorComposite prior_std(std_vec, "PO", {3,3});
- problem_->setPriorFactor(prior, prior_std, 0.0, 0.001);
+ problem_->setPriorFactor(prior, prior_std, 0.0);
KF0_ = problem_->getTrajectory()->getLastFrame();
// Simulate captures with 4 point feet
@@ -158,10 +158,10 @@ TEST_F(PointFeetCaptures, process_all_capture_first)
// factor creation due to keyFrameCallback
FrameBasePtr KF1 = problem_->emplaceFrame(2.0, x_origin_);
- proc_pfnm_base_->keyFrameCallback(KF1, 0.001);
+ proc_pfnm_base_->keyFrameCallback(KF1);
FrameBasePtr KF2 = problem_->emplaceFrame(3.0, x_origin_);
- proc_pfnm_base_->keyFrameCallback(KF2, 0.001);
+ proc_pfnm_base_->keyFrameCallback(KF2);
problem_->print(4,1,1,1);
}
@@ -171,10 +171,10 @@ TEST_F(PointFeetCaptures, process_all_capture_last)
{
// First, get the key frames from the keyFrameCallback
FrameBasePtr KF1 = problem_->emplaceFrame(2.0, x_origin_);
- proc_pfnm_base_->keyFrameCallback(KF1, 0.001);
+ proc_pfnm_base_->keyFrameCallback(KF1);
FrameBasePtr KF2 = problem_->emplaceFrame(3.0, x_origin_);
- proc_pfnm_base_->keyFrameCallback(KF2, 0.001);
+ proc_pfnm_base_->keyFrameCallback(KF2);
// Then process the captures
CPF0_->process();
--
GitLab