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 6ab3a7231c47e422c486a07dda2756ae52f3efb5..ff097708d415206e703f9f80f03e0e8b44916a38 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 2ba76fd1feb93ade82fd71aca17e842a722ce02e..036fee9c13114c7f0188ace02904d3ce2564ffbb 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 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();