diff --git a/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h b/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h
index 6bc51007d7936cd4be9e8d37538132e2591d5735..9bf6d342c691df222bddc241300c5f0ca5cb3e98 100644
--- a/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h
+++ b/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h
@@ -238,6 +238,8 @@ class ProcessorForceTorqueInertialPreintDynamics : public ProcessorMotion
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+ bool voteForKeyFrame() const override;
+
public:
virtual VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override;
};
diff --git a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
index 89622044b57cf5f048325a6825a0b6da594be72e..b8a8968c925d3d149907ccd515e9461becae294d 100644
--- a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
+++ b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
@@ -51,12 +51,12 @@ ProcessorForceTorqueInertialPreintDynamics::ProcessorForceTorqueInertialPreintDy
_params_force_torque_inertial_preint_dynamics),
params_force_torque_inertial_preint_dynamics_(_params_force_torque_inertial_preint_dynamics)
{
- // TODO Auto-generated constructor stub
+ //
}
ProcessorForceTorqueInertialPreintDynamics::~ProcessorForceTorqueInertialPreintDynamics()
{
- // TODO Auto-generated destructor stub
+ //
}
CaptureMotionPtr ProcessorForceTorqueInertialPreintDynamics::emplaceCapture(const FrameBasePtr& _frame_own,
@@ -101,8 +101,6 @@ FactorBasePtr ProcessorForceTorqueInertialPreintDynamics::emplaceFactor(FeatureB
auto fac_ftipd = FactorBase::emplace<FactorForceTorqueInertialDynamics>(_feature_motion, ftr_ftipd, _capture_origin, shared_from_this(), params_->apply_loss_function);
return fac_ftipd;
- //FactorBasePtr returnValue;
- //return returnValue;
}
void ProcessorForceTorqueInertialPreintDynamics::setCalibration(const CaptureBasePtr _capture,
@@ -184,7 +182,7 @@ Eigen::VectorXd ProcessorForceTorqueInertialPreintDynamics::correctDelta(const E
VectorXd ProcessorForceTorqueInertialPreintDynamics::getCalibration(const CaptureBaseConstPtr _capture) const
{
if (_capture)
- { //return _capture->getStateBlock('I')->getState(); // IMU bia
+ {
VectorXd calibration(13);
const Vector6d& I_calib = _capture->getStateBlock('I')->getState();
const Vector3d& C_calib = _capture->getStateBlock('C')->getState();
@@ -194,7 +192,6 @@ VectorXd ProcessorForceTorqueInertialPreintDynamics::getCalibration(const Captur
return calibration;
}
else
- //return getSensor()->getStateBlock('I')->getState(); // IMU bias
{
VectorXd calibration(13);
const Vector6d& I_calib = getSensor()->getStateBlock('I')->getState();
@@ -205,6 +202,37 @@ VectorXd ProcessorForceTorqueInertialPreintDynamics::getCalibration(const Captur
return calibration;
}
}
+
+bool ProcessorForceTorqueInertialPreintDynamics::voteForKeyFrame() const
+{
+ // time span
+ if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_force_torque_inertial_preint_dynamics_->max_time_span)
+ {
+ WOLF_DEBUG( "PM: vote: time span" );
+ return true;
+ }
+ // buffer length
+ if (getBuffer().size() > params_force_torque_inertial_preint_dynamics_->max_buff_length)
+ {
+ WOLF_DEBUG( "PM: vote: buffer length" );
+ return true;
+ }
+ // dist_traveled
+ // if (getBuffer().back() > params_force_torque_inertial_preint_dynamics_->dist_traveled)
+ // {
+ // WOLF_DEBUG( "PM: vote: dist traveled" );
+ // return true;
+ // }
+ // angle_turned
+ // if (getBuffer().back() > params_force_torque_inertial_preint_dynamics_->angle_turned)
+ // {
+ // WOLF_DEBUG( "PM: vote: angle turned" );
+ // return true;
+ // }
+
+ return false;
+}
+
} /* namespace wolf */
#include <core/processor/factory_processor.h>
diff --git a/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp b/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..966c14be025502710f2c952199bd5f8a700f7247
--- /dev/null
+++ b/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp
@@ -0,0 +1,156 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
+#include "bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h"
+#include "bodydynamics/sensor/sensor_force_torque_inertial.h"
+#include "bodydynamics/internal/config.h"
+
+#include <core/sensor/factory_sensor.h>
+
+#include <core/utils/utils_gtest.h>
+#include <core/utils/logging.h>
+#include <core/utils/params_server.h>
+#include <core/yaml/parser_yaml.h>
+#include <core/state_block/state_block_derived.h>
+#include <core/state_block/factory_state_block.h>
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+using namespace wolf;
+using namespace bodydynamics::fti;
+
+WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
+
+class Test_ProcessorForceTorqueInertialPreintDynamics_yaml : public testing::Test
+{
+ public:
+ ProblemPtr P;
+ SensorForceTorqueInertialPtr S;
+ ProcessorForceTorqueInertialPreintDynamicsPtr p;
+
+ protected:
+ void SetUp() override
+ {
+ std::string wolf_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
+ ParserYaml parser = ParserYaml("problem_force_torque_inertial_dynamics_processor_test.yaml", wolf_root + "/test/yaml/");
+ ParamsServer server = ParamsServer(parser.getParams());
+ P = Problem::autoSetup(server);
+
+ S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
+ p = std::static_pointer_cast<ProcessorForceTorqueInertialPreintDynamics>(S->getProcessorList().front());
+ }
+};
+
+TEST(ProcessorForceTorqueInertialPreintDynamics_yaml, force_registraion)
+{
+ FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create);
+}
+
+// Test to see if the processor works (yaml files). Here the dron is not moving
+TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, not_moving_test)
+{
+ VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+ data.segment<3>(0) = - gravity();
+ // data.segment<3>(6) = - 1.952*gravity();
+ MatrixXd data_cov = MatrixXd::Identity(12, 12);
+ CaptureMotionPtr C0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
+ CaptureMotionPtr C1 = std::make_shared<CaptureMotion>("CaptureMotion", 0.2, S, data, data_cov, nullptr);
+ CaptureMotionPtr C2 = std::make_shared<CaptureMotion>("CaptureMotion", 0.4, S, data, data_cov, nullptr);
+ CaptureMotionPtr C3 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
+ CaptureMotionPtr C4 = std::make_shared<CaptureMotion>("CaptureMotion", 0.8, S, data, data_cov, nullptr);
+ CaptureMotionPtr C5 = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
+
+ C0->process();
+ CaptureMotionPtr C_Origin = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
+
+ C1->process();
+ C2->process();
+ C3->process();
+ C4->process();
+ C5->process();
+
+ P->print(4, 1, 1, 1);
+ WOLF_INFO("State: ", P->getState("POVL").vector("POVL").transpose());
+
+ ASSERT_MATRIX_APPROX(C5->getStateBlock('P')->getState(), C_Origin->getStateBlock('P')->getState(), 1e-8);
+ ASSERT_QUATERNION_APPROX(Quaterniond(C6->getStateBlock('O')->getState().data()), Quaterniond(C_Origin->getStateBlock('O')->getState().data()), 1e-8);
+ ASSERT_MATRIX_APPROX(C5->getStateBlock('V')->getState(), C_Origin->getStateBlock('V')->getState(), 1e-8);
+ ASSERT_MATRIX_APPROX(C5->getStateBlock('L')->getState(), C_Origin->getStateBlock('L')->getState(), 1e-8);
+
+ ASSERT_EQ(C_Origin->getTimeStamp(), 0.0);
+ ASSERT_EQ(C_Origin, C0);
+ ASSERT_EQ(C5->getTimeStamp(), 1.0);
+
+ Vector3d p0 = C_Origin->getStateBlock('P')->getState();
+ Quaterniond o0 = Quaterniond(C_Origin->getStateBlock('O')->getState().data());
+ Vector3d v0 = C_Origin->getStateBlock('V')->getState();
+ Vector3d l0 = C_Origin->getStateBlock('L')->getState();
+
+
+ Vector3d p1 = C5->getStateBlock('P')->getState();
+ Quaterniond o1 = Quaterniond(C_Origin->getStateBlock('O')->getState().data());
+ Vector3d v1 = C5->getStateBlock('V')->getState();
+ Vector3d l1 = C5->getStateBlock('L')->getState();
+
+
+ //ASSERT_MATRIX_APPROX(betweenStates())
+
+
+ // inline void betweenStates(const MatrixBase<D1>& p1,
+ // const MatrixBase<D2>& v1,
+ // const MatrixBase<D5>& L1,
+ // const QuaternionBase<D6>& q1,
+ // const MatrixBase<D7>& p2,
+ // const MatrixBase<D8>& v2,
+ // const MatrixBase<D11>& L2,
+ // const QuaternionBase<D12>& q2,
+ // const T dt,
+ // MatrixBase<D13>& dpi,
+ // MatrixBase<D14>& dvi,
+ // MatrixBase<D15>& dpd,
+ // MatrixBase<D16>& dvd,
+ // MatrixBase<D17>& dL,
+ // QuaternionBase<D18>& dq)
+
+
+
+}
+
+// Test to see if the processor works (yaml files). Here the dron is moving just 1m in x direction
+// TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, 1m_x_moving_test)
+// {
+// VectorXd data = VectorXd::Zero(12); // [ a, w, f, t ]
+// data.segment<3>(0) = -gravity();
+// data(0) = 2;
+// MatrixXd data_cov = MatrixXd::Identity(12, 12);
+
+
+// }
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ //::testing::GTEST_FLAG(filter) = "Test_ProcessorForceTorqueInertialPreintDynamics_yaml.residual";
+ return RUN_ALL_TESTS();
+}
\ No newline at end of file
diff --git a/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b7fea8d46b0fa871ae295373d0a0469b6d2bc69b
--- /dev/null
+++ b/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml
@@ -0,0 +1,56 @@
+config:
+ problem:
+ frame_structure: "POVL"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ V: [0,0,0]
+ L: [0,0,0]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ V: [1,1,1]
+ L: [1,1,1]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "None"
+ map:
+ type: "MapBase"
+ plugin: "core"
+ sensors:
+ -
+ name: "sensor FTI"
+ type: "SensorForceTorqueInertial"
+ plugin: "bodydynamics"
+ extrinsic:
+ pose: [0,0,0, 0,0,0,1]
+ force_noise_std: 0.1 # std dev of force noise in N
+ torque_noise_std: 0.1 # std dev of torque noise in N/m
+ acc_noise_std: 0.01 # std dev of acc noise in m/s2
+ gyro_noise_std: 0.01 # std dev of gyro noise in rad/s
+
+ com: [0,0,0.0341] # center of mass [m]
+ inertia: [0.017598,0.017957,0.029599] # moments of inertia i_xx, i_yy, i_zz [kg m2]
+ mass: 1.952 # mass [kg]
+
+ processors:
+ -
+ name: "proc FTID"
+ type: "ProcessorForceTorqueInertialPreintDynamics"
+ sensor_name: "sensor FTI"
+ plugin: "bodydynamics"
+ time_tolerance: 0.1
+ apply_loss_function: false
+ unmeasured_perturbation_std: 0.00000001
+ state_getter: true
+ state_priority: 1
+ n_propellers: 3
+ keyframe_vote:
+ voting_active: true
+ max_time_span: 0.995
+ max_buff_length: 999 # motion deltas
+ dist_traveled: 999 # meters
+ angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)