diff --git a/demos/mcapi_pov_estimation.cpp b/demos/mcapi_pov_estimation.cpp
index 1b84705e586c9329eee38b02ece3dfe7769cbec8..c85c22064ffd6b13a93228122cb013c633277aa8 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 acead6c9ea853b9780ad7415eddab56d4a595387..82a0734156de7d8a32f33a0bc16073dfdb77c007 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 51ade73abcc7638ae11f204f1cf338acb3bf75e2..9845fc57afb2826d541f1aedf9b9650e69a36f84 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 5bd8b2ea7aebb21d2dc7e9f01785144dec9da29c..3bedaa8c540ea9da34ee28cabaff10969c5812ea 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 8a63b72681578d0aa86ecf1bfecac5e06c3db32b..f9265c5ae653edd9f2814f7bae987c57a8830715 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 a6a7c78e03ca80d2d89482f43f109f776c0822af..28305970d7877cb8239e9009e1db3eb5356df4b8 100644
--- a/src/processor/processor_inertial_kinematics.cpp
+++ b/src/processor/processor_inertial_kinematics.cpp
@@ -53,11 +53,11 @@ inline void ProcessorInertialKinematics::processCapture(CaptureBasePtr _capture)
}
// nothing to do if any of the two buffer is empty
- if(buffer_pack_kf_.empty()){
+ if(buffer_frame_.empty()){
WOLF_DEBUG("PInertialKinematic: KF pack buffer empty, time ", _capture->getTimeStamp());
return;
}
- if(buffer_pack_kf_.empty()){
+ if(buffer_frame_.empty()){
WOLF_DEBUG("PInertialKinematics: Capture buffer empty, time ", _capture->getTimeStamp());
return;
}
@@ -69,21 +69,21 @@ inline void ProcessorInertialKinematics::processCapture(CaptureBasePtr _capture)
// 1. get corresponding KF
FrameBasePtr kf;
- auto buffer_pack_kf_it = buffer_pack_kf_.getContainer().begin();
+ auto buffer_frame_it = buffer_frame_.getContainer().begin();
auto buffer_capture_it = buffer_capture_.getContainer().begin();
auto sensor_angvel = getProblem()->getSensor(params_ikin_->sensor_angvel_name);
- while ((buffer_pack_kf_it != buffer_pack_kf_.getContainer().end())
+ while ((buffer_frame_it != buffer_frame_.getContainer().end())
&& (buffer_capture_it != buffer_capture_.getContainer().end()))
{
- bool time_ok = buffer_capture_.simpleCheckTimeTolerance(buffer_pack_kf_it->first, buffer_capture_it->first, buffer_pack_kf_it->second->time_tolerance);
+ bool time_ok = checkTimeTolerance(buffer_frame_it->first, buffer_capture_it->first);
if (time_ok) {
- CaptureBasePtr cap_angvel = buffer_pack_kf_it->second->key_frame->getCaptureOf(sensor_angvel);
- auto min_ts = (buffer_pack_kf_it->first < buffer_capture_it->first) ? buffer_pack_kf_it->first : buffer_capture_it->first;
+ 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;
if (cap_angvel && cap_angvel->getStateBlock('I')){ // TODO: or only cap_angvel?
// cast incoming capture to the InertialKinematics type, add it to the keyframe
- auto kf = buffer_pack_kf_it->second->key_frame;
+ auto kf = buffer_frame_it->second;
auto cap_ikin = std::static_pointer_cast<CaptureInertialKinematics>(buffer_capture_it->second);
cap_ikin->link(kf);
createInertialKinematicsFactor(cap_ikin,
@@ -92,27 +92,27 @@ inline void ProcessorInertialKinematics::processCapture(CaptureBasePtr _capture)
// update pointer to origin capture (the previous one attached to a KF) if we have created a new factor
cap_origin_ptr_ = buffer_capture_it->second;
buffer_capture_it++;
- buffer_pack_kf_it++;
+ buffer_frame_it++;
}
else {
// if time ok but no capture angvel yet, there is not gonna be any in the next KF of the buffer
break;
buffer_capture_it++;
- buffer_pack_kf_it++;
+ buffer_frame_it++;
}
////////////////
// remove everything before (Inclusive if equal) this timestamp -> the cap_angvel is yet to come
- buffer_pack_kf_.removeUpTo(min_ts);
+ buffer_frame_.removeUpTo(min_ts);
buffer_capture_.removeUpTo(min_ts);
}
else {
// if a time difference between captures and KF pack, we increment the oldest iterator
- if (buffer_capture_it->first < buffer_pack_kf_it->first){
+ if (buffer_capture_it->first < buffer_frame_it->first){
buffer_capture_it++;
}
else {
- buffer_pack_kf_it++;
+ buffer_frame_it++;
}
}
}
diff --git a/src/processor/processor_point_feet_nomove.cpp b/src/processor/processor_point_feet_nomove.cpp
index d11d87acb96ce1885dde22ecf45d04c11bee5985..036fee9c13114c7f0188ace02904d3ce2564ffbb 100644
--- a/src/processor/processor_point_feet_nomove.cpp
+++ b/src/processor/processor_point_feet_nomove.cpp
@@ -46,14 +46,14 @@ void ProcessorPointFeetNomove::configure(SensorBasePtr _sensor)
void ProcessorPointFeetNomove::createFactorIfNecessary(){
auto sensor_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(getSensor());
- while (buffer_pack_kf_.size() >= 2)
+ while (buffer_frame_.size() >= 2)
{
- auto kf1_it = buffer_pack_kf_.getContainer().begin();
+ auto kf1_it = buffer_frame_.getContainer().begin();
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->time_tolerance);
- auto cap2_it = buffer_capture_.selectIterator(t2, kf1_it->second->time_tolerance);
+ 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
@@ -101,7 +101,7 @@ void ProcessorPointFeetNomove::createFactorIfNecessary(){
auto cap2 = std::static_pointer_cast<CapturePointFeetNomove>(cap2_it->second);
// link (kind of arbitrarily) factor from KF1 to KF2 with a feature and capture on KF2 to mimic capture motion
// however, this capture solely does not contain enough information to recreate the factor
- cap2->link(kf2_it->second->key_frame);
+ cap2->link(kf2_it->second);
auto kin_incontact_t2 = cap2->kin_incontact_;
for (auto kin_pair1: kin_incontact_from_t1){
@@ -109,13 +109,13 @@ void ProcessorPointFeetNomove::createFactorIfNecessary(){
Vector6d meas; meas << kin_pair1.second, kin_pair2_it->second;
FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap2, "PointFeet", meas, sensor_pfnm->getCovFootNomove());
- FactorPointFeetNomovePtr fac = FactorBase::emplace<FactorPointFeetNomove>(feat, feat, kf1_it->second->key_frame, nullptr, true);
+ FactorPointFeetNomovePtr fac = FactorBase::emplace<FactorPointFeetNomove>(feat, feat, kf1_it->second, nullptr, true);
}
}
// Once the factors are created, remove kf1 and all the captures until ts 2 NOT INCLUDING the one at ts 2 since we need it for the next
// Note: erase by range does not include the end range iterator
- buffer_pack_kf_.getContainer().erase(buffer_pack_kf_.getContainer().begin(), kf2_it); // !! works only if getContainer returns a non const reference
+ buffer_frame_.getContainer().erase(buffer_frame_.getContainer().begin(), kf2_it); // !! works only if getContainer returns a non const reference
buffer_capture_.getContainer().erase(buffer_capture_.getContainer().begin(), cap2_it);
}
}
@@ -130,11 +130,11 @@ inline void ProcessorPointFeetNomove::processCapture(CaptureBasePtr _capture)
return;
}
// nothing to do if any of the two buffer is empty
- if(buffer_pack_kf_.empty()){
+ if(buffer_frame_.empty()){
WOLF_DEBUG("PInertialKinematic: KF pack buffer empty, time ", _capture->getTimeStamp());
return;
}
- if(buffer_pack_kf_.empty()){
+ if(buffer_frame_.empty()){
WOLF_DEBUG("PInertialKinematics: Capture buffer empty, time ", _capture->getTimeStamp());
return;
}
@@ -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)
{
@@ -151,11 +151,11 @@ inline void ProcessorPointFeetNomove::processKeyFrame(FrameBasePtr _keyframe_ptr
return;
}
// nothing to do if any of the two buffer is empty
- if(buffer_pack_kf_.empty()){
+ if(buffer_frame_.empty()){
WOLF_DEBUG("ProcessorPointFeetNomove: KF pack buffer empty, time ", _keyframe_ptr->getTimeStamp());
return;
}
- if(buffer_pack_kf_.empty()){
+ if(buffer_frame_.empty()){
WOLF_DEBUG("ProcessorPointFeetNomove: Capture buffer empty, time ", _keyframe_ptr->getTimeStamp());
return;
}
diff --git a/test/gtest_factor_force_torque.cpp b/test/gtest_factor_force_torque.cpp
index 5b6952859fdf1e45c3cd6342a9d6bfa8338ecb6e..5a62fc046caa8bc778e505312485314a9dcae3a9 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 b390ba96a1e2057117fcc7f28aa91f09f31eddbb..00d43d2a4f2c9280beb6b986f9ba392cdfb29f9d 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 59e15808614cb4dbf2951f4e9cbc57d1f2895915..ded4ae7a52d7201e9aeaa77204d3c881f9d58954 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 c4fb675cfddbeb9ead8edb31af3d00cbb1a471cb..e501a8ae9b7523e06b6cd527e41505c8fa969413 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();