From a3156a9228aa7fe8ec971e0e4915464b0fe2b714 Mon Sep 17 00:00:00 2001
From: cont-integration <CI@iri.upc.edu>
Date: Mon, 11 Dec 2023 13:05:38 +0100
Subject: [PATCH] [skip ci] applied clang format
---
demos/eigenmvn.h | 192 ++--
demos/mcapi_utils.cpp | 48 +-
demos/mcapi_utils.h | 33 +-
demos/solo_imu_kine.cpp | 654 +++++++------
demos/solo_imu_kine_mocap.cpp | 625 ++++++------
demos/solo_imu_mocap.cpp | 568 +++++------
demos/solo_kine_mocap.cpp | 537 ++++++-----
demos/solo_real_povcdl_estimation.cpp | 796 ++++++++-------
.../capture/capture_force_torque_preint.h | 85 +-
.../capture/capture_inertial_kinematics.h | 68 +-
.../bodydynamics/capture/capture_leg_odom.h | 41 +-
.../capture/capture_point_feet_nomove.h | 30 +-
include/bodydynamics/common/bodydynamics.h | 3 +-
.../bodydynamics/factor/factor_force_torque.h | 332 ++++---
.../factor/factor_force_torque_preint.h | 428 +++++----
.../factor/factor_inertial_kinematics.h | 211 ++--
.../factor/factor_point_feet_altitude.h | 136 ++-
.../factor/factor_point_feet_nomove.h | 205 ++--
.../factor/factor_point_feet_zero_velocity.h | 132 ++-
.../feature/feature_force_torque.h | 162 +++-
.../feature/feature_force_torque_preint.h | 88 +-
.../feature/feature_inertial_kinematics.h | 102 +-
.../math/force_torque_delta_tools.h | 907 ++++++++++--------
.../processor/processor_force_torque_preint.h | 175 ++--
.../processor/processor_inertial_kinematics.h | 99 +-
.../processor/processor_point_feet_nomove.h | 89 +-
.../bodydynamics/sensor/sensor_force_torque.h | 77 +-
.../sensor/sensor_inertial_kinematics.h | 64 +-
.../sensor/sensor_point_feet_nomove.h | 82 +-
src/capture/capture_force_torque_preint.cpp | 40 +-
src/capture/capture_inertial_kinematics.cpp | 68 +-
src/capture/capture_leg_odom.cpp | 104 +-
src/capture/capture_point_feet_nomove.cpp | 24 +-
src/feature/feature_force_torque.cpp | 59 +-
src/feature/feature_force_torque_preint.cpp | 22 +-
src/feature/feature_inertial_kinematics.cpp | 24 +-
.../processor_force_torque_preint.cpp | 215 +++--
.../processor_inertial_kinematics.cpp | 166 ++--
src/processor/processor_point_feet_nomove.cpp | 215 +++--
src/sensor/sensor_force_torque.cpp | 51 +-
src/sensor/sensor_inertial_kinematics.cpp | 38 +-
src/sensor/sensor_point_feet_nomove.cpp | 38 +-
test/gtest_capture_inertial_kinematics.cpp | 17 +-
test/gtest_capture_leg_odom.cpp | 157 +--
test/gtest_factor_force_torque.cpp | 610 ++++++------
test/gtest_factor_inertial_kinematics.cpp | 200 ++--
test/gtest_feature_inertial_kinematics.cpp | 48 +-
test/gtest_force_torque_delta_tools.cpp | 196 ++--
test/gtest_processor_force_torque_preint.cpp | 315 +++---
test/gtest_processor_inertial_kinematics.cpp | 119 +--
test/gtest_processor_point_feet_nomove.cpp | 91 +-
test/gtest_sensor_force_torque.cpp | 11 +-
test/gtest_sensor_inertial_kinematics.cpp | 11 +-
53 files changed, 5109 insertions(+), 4699 deletions(-)
diff --git a/demos/eigenmvn.h b/demos/eigenmvn.h
index ad491f7..b622442 100644
--- a/demos/eigenmvn.h
+++ b/demos/eigenmvn.h
@@ -19,17 +19,19 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
/**
* Multivariate Normal distribution sampling using C++11 and Eigen matrices.
- *
+ *
* This is taken from http://stackoverflow.com/questions/16361226/error-while-creating-object-from-templated-class
* (also see http://lost-found-wandering.blogspot.fr/2011/05/sampling-from-multivariate-normal-in-c.html)
- *
+ *
* I have been unable to contact the original author, and I've performed
* the following modifications to the original code:
* - removal of the dependency to Boost, in favor of straight C++11;
* - ability to choose from Solver or Cholesky decomposition (supposedly faster);
* - fixed Cholesky by using LLT decomposition instead of LDLT that was not yielding
- * a correctly rotated variance
- * (see this http://stats.stackexchange.com/questions/48749/how-to-sample-from-a-multivariate-normal-given-the-pt-ldlt-p-decomposition-o )
+ * a correctly rotated variance
+ * (see this
+ * http://stats.stackexchange.com/questions/48749/how-to-sample-from-a-multivariate-normal-given-the-pt-ldlt-p-decomposition-o
+ * )
*/
/**
@@ -44,7 +46,7 @@
* 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 library.
*/
@@ -57,98 +59,120 @@
/*
We need a functor that can pretend it's const,
- but to be a good random number generator
- it needs mutable state. The standard Eigen function
+ but to be a good random number generator
+ it needs mutable state. The standard Eigen function
Random() just calls rand(), which changes a global
variable.
*/
-namespace Eigen {
- namespace internal {
- template<typename Scalar>
- struct scalar_normal_dist_op
- {
- static std::mt19937 rng; // The uniform pseudo-random algorithm
- mutable std::normal_distribution<Scalar> norm; // gaussian combinator
-
- EIGEN_EMPTY_STRUCT_CTOR(scalar_normal_dist_op)
+namespace Eigen
+{
+namespace internal
+{
+template <typename Scalar>
+struct scalar_normal_dist_op
+{
+ static std::mt19937 rng; // The uniform pseudo-random algorithm
+ mutable std::normal_distribution<Scalar> norm; // gaussian combinator
- template<typename Index>
- inline const Scalar operator() (Index, Index = 0) const { return norm(rng); }
- inline void seed(const uint64_t &s) { rng.seed(s); }
- };
+ EIGEN_EMPTY_STRUCT_CTOR(scalar_normal_dist_op)
- template<typename Scalar>
- std::mt19937 scalar_normal_dist_op<Scalar>::rng;
-
- template<typename Scalar>
- struct functor_traits<scalar_normal_dist_op<Scalar> >
- { enum { Cost = 50 * NumTraits<Scalar>::MulCost, PacketAccess = false, IsRepeatable = false }; };
+ template <typename Index>
+ inline const Scalar operator()(Index, Index = 0) const
+ {
+ return norm(rng);
+ }
+ inline void seed(const uint64_t& s)
+ {
+ rng.seed(s);
+ }
+};
+
+template <typename Scalar>
+std::mt19937 scalar_normal_dist_op<Scalar>::rng;
- } // end namespace internal
+template <typename Scalar>
+struct functor_traits<scalar_normal_dist_op<Scalar> >
+{
+ enum
+ {
+ Cost = 50 * NumTraits<Scalar>::MulCost,
+ PacketAccess = false,
+ IsRepeatable = false
+ };
+};
+
+} // end namespace internal
+
+/**
+ Find the eigen-decomposition of the covariance matrix
+ and then store it for sampling from a multi-variate normal
+*/
+template <typename Scalar>
+class EigenMultivariateNormal
+{
+ Matrix<Scalar, Dynamic, Dynamic> _covar;
+ Matrix<Scalar, Dynamic, Dynamic> _transform;
+ Matrix<Scalar, Dynamic, 1> _mean;
+ internal::scalar_normal_dist_op<Scalar> randN; // Gaussian functor
+ bool _use_cholesky;
+ SelfAdjointEigenSolver<Matrix<Scalar, Dynamic, Dynamic> >
+ _eigenSolver; // drawback: this creates a useless eigenSolver when using Cholesky decomposition, but it yields
+ // access to eigenvalues and vectors
- /**
- Find the eigen-decomposition of the covariance matrix
- and then store it for sampling from a multi-variate normal
- */
- template<typename Scalar>
- class EigenMultivariateNormal
- {
- Matrix<Scalar,Dynamic,Dynamic> _covar;
- Matrix<Scalar,Dynamic,Dynamic> _transform;
- Matrix< Scalar, Dynamic, 1> _mean;
- internal::scalar_normal_dist_op<Scalar> randN; // Gaussian functor
- bool _use_cholesky;
- SelfAdjointEigenSolver<Matrix<Scalar,Dynamic,Dynamic> > _eigenSolver; // drawback: this creates a useless eigenSolver when using Cholesky decomposition, but it yields access to eigenvalues and vectors
-
public:
- EigenMultivariateNormal(const Matrix<Scalar,Dynamic,1>& mean,const Matrix<Scalar,Dynamic,Dynamic>& covar,
- const bool use_cholesky=false,const uint64_t &seed=std::mt19937::default_seed)
- :_use_cholesky(use_cholesky)
- {
+ EigenMultivariateNormal(const Matrix<Scalar, Dynamic, 1>& mean,
+ const Matrix<Scalar, Dynamic, Dynamic>& covar,
+ const bool use_cholesky = false,
+ const uint64_t& seed = std::mt19937::default_seed)
+ : _use_cholesky(use_cholesky)
+ {
randN.seed(seed);
- setMean(mean);
- setCovar(covar);
- }
+ setMean(mean);
+ setCovar(covar);
+ }
- void setMean(const Matrix<Scalar,Dynamic,1>& mean) { _mean = mean; }
- void setCovar(const Matrix<Scalar,Dynamic,Dynamic>& covar)
+ void setMean(const Matrix<Scalar, Dynamic, 1>& mean)
+ {
+ _mean = mean;
+ }
+ void setCovar(const Matrix<Scalar, Dynamic, Dynamic>& covar)
{
- _covar = covar;
-
- // Assuming that we'll be using this repeatedly,
- // compute the transformation matrix that will
- // be applied to unit-variance independent normals
-
- if (_use_cholesky)
- {
- Eigen::LLT<Eigen::Matrix<Scalar,Dynamic,Dynamic> > cholSolver(_covar);
- // We can only use the cholesky decomposition if
- // the covariance matrix is symmetric, pos-definite.
- // But a covariance matrix might be pos-semi-definite.
- // In that case, we'll go to an EigenSolver
- if (cholSolver.info()==Eigen::Success)
- {
- // Use cholesky solver
- _transform = cholSolver.matrixL();
- }
- else
- {
- throw std::runtime_error("Failed computing the Cholesky decomposition. Use solver instead");
- }
- }
- else
- {
- _eigenSolver = SelfAdjointEigenSolver<Matrix<Scalar,Dynamic,Dynamic> >(_covar);
- _transform = _eigenSolver.eigenvectors()*_eigenSolver.eigenvalues().cwiseMax(0).cwiseSqrt().asDiagonal();
- }
+ _covar = covar;
+
+ // Assuming that we'll be using this repeatedly,
+ // compute the transformation matrix that will
+ // be applied to unit-variance independent normals
+
+ if (_use_cholesky)
+ {
+ Eigen::LLT<Eigen::Matrix<Scalar, Dynamic, Dynamic> > cholSolver(_covar);
+ // We can only use the cholesky decomposition if
+ // the covariance matrix is symmetric, pos-definite.
+ // But a covariance matrix might be pos-semi-definite.
+ // In that case, we'll go to an EigenSolver
+ if (cholSolver.info() == Eigen::Success)
+ {
+ // Use cholesky solver
+ _transform = cholSolver.matrixL();
+ }
+ else
+ {
+ throw std::runtime_error("Failed computing the Cholesky decomposition. Use solver instead");
+ }
+ }
+ else
+ {
+ _eigenSolver = SelfAdjointEigenSolver<Matrix<Scalar, Dynamic, Dynamic> >(_covar);
+ _transform = _eigenSolver.eigenvectors() * _eigenSolver.eigenvalues().cwiseMax(0).cwiseSqrt().asDiagonal();
+ }
}
/// Draw nn samples from the gaussian and return them
/// as columns in a Dynamic by nn matrix
- Matrix<Scalar,Dynamic,-1> samples(int nn)
- {
- return (_transform * Matrix<Scalar,Dynamic,-1>::NullaryExpr(_covar.rows(),nn,randN)).colwise() + _mean;
- }
- }; // end class EigenMultivariateNormal
-} // end namespace Eigen
+ Matrix<Scalar, Dynamic, -1> samples(int nn)
+ {
+ return (_transform * Matrix<Scalar, Dynamic, -1>::NullaryExpr(_covar.rows(), nn, randN)).colwise() + _mean;
+ }
+}; // end class EigenMultivariateNormal
+} // end namespace Eigen
#endif
diff --git a/demos/mcapi_utils.cpp b/demos/mcapi_utils.cpp
index f43a6cd..513fcf5 100644
--- a/demos/mcapi_utils.cpp
+++ b/demos/mcapi_utils.cpp
@@ -25,50 +25,44 @@ Vector3d computeAccFromSpatial(const Vector3d& ddr, const Vector3d& w, const Vec
return ddr + w.cross(dr);
}
-
std::vector<Vector3d> contacts_from_footrect_center()
{
// compute feet corners coordinates like from the leg_X_6_joint
- double lx = 0.1;
- double ly = 0.065;
- double lz = 0.107;
- std::vector<Vector3d> contacts; contacts.resize(4);
+ double lx = 0.1;
+ double ly = 0.065;
+ double lz = 0.107;
+ std::vector<Vector3d> contacts;
+ contacts.resize(4);
contacts[0] = {-lx, -ly, -lz};
- contacts[1] = {-lx, ly, -lz};
- contacts[2] = { lx, -ly, -lz};
- contacts[3] = { lx, ly, -lz};
+ contacts[1] = {-lx, ly, -lz};
+ contacts[2] = {lx, -ly, -lz};
+ contacts[3] = {lx, ly, -lz};
return contacts;
}
-
-Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contacts,
- const Matrix<double, 12, 1>& cf12)
+Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contacts, const Matrix<double, 12, 1>& cf12)
{
- Vector3d f = cf12.segment<3>(0) + cf12.segment<3>(3) + cf12.segment<3>(6) + cf12.segment<3>(9);
- Vector3d tau = contacts[0].cross(cf12.segment<3>(0))
- + contacts[1].cross(cf12.segment<3>(3))
- + contacts[2].cross(cf12.segment<3>(6))
- + contacts[3].cross(cf12.segment<3>(9));
- Matrix<double, 6, 1> wrench; wrench << f, tau;
+ Vector3d f = cf12.segment<3>(0) + cf12.segment<3>(3) + cf12.segment<3>(6) + cf12.segment<3>(9);
+ Vector3d tau = contacts[0].cross(cf12.segment<3>(0)) + contacts[1].cross(cf12.segment<3>(3)) +
+ contacts[2].cross(cf12.segment<3>(6)) + contacts[3].cross(cf12.segment<3>(9));
+ Matrix<double, 6, 1> wrench;
+ wrench << f, tau;
return wrench;
}
-
-Matrix3d compute_Iw(pinocchio::Model& model,
- pinocchio::Data& data,
- VectorXd& q_static,
- Vector3d& b_p_bc)
+Matrix3d compute_Iw(pinocchio::Model& model, pinocchio::Data& data, VectorXd& q_static, Vector3d& b_p_bc)
{
MatrixXd b_Mc = pinocchio::crba(model, data, q_static); // mass matrix at b frame expressed in b frame
// MatrixXd b_Mc = crba(model_dist, data_dist, q_static); // mass matrix at b frame expressed in b frame
- MatrixXd b_I_b = b_Mc.topLeftCorner<6,6>(); // inertia matrix at b frame expressed in b frame
- pinocchio::SE3 bTc (Eigen::Matrix3d::Identity(), b_p_bc); // "no free flyer robot -> c frame oriented the same as b"
- pinocchio::SE3 cTb = bTc.inverse();
- // Ic = cXb^star * Ib * bXc = bXc^T * Ib * bXc
+ MatrixXd b_I_b = b_Mc.topLeftCorner<6, 6>(); // inertia matrix at b frame expressed in b frame
+ pinocchio::SE3 bTc(Eigen::Matrix3d::Identity(),
+ b_p_bc); // "no free flyer robot -> c frame oriented the same as b"
+ pinocchio::SE3 cTb = bTc.inverse();
+ // Ic = cXb^star * Ib * bXc = bXc^T * Ib * bXc
// c_I_c block diagonal:
// [m*Id3, 03;
// 0, Iw]
MatrixXd c_I_c = cTb.toDualActionMatrix() * b_I_b * bTc.toActionMatrix();
- Matrix3d b_Iw = c_I_c.bottomRightCorner<3,3>(); // meas
+ Matrix3d b_Iw = c_I_c.bottomRightCorner<3, 3>(); // meas
return b_Iw;
}
diff --git a/demos/mcapi_utils.h b/demos/mcapi_utils.h
index 634b043..6ceeca6 100644
--- a/demos/mcapi_utils.h
+++ b/demos/mcapi_utils.h
@@ -22,44 +22,38 @@
#include "Eigen/Dense"
#include "pinocchio/algorithm/crba.hpp"
-
-using namespace Eigen;
-
+using namespace Eigen;
/**
* \brief Compute a 3D acceleration from 6D spatial acceleration
- *
+ *
* \param ddr spatial acc linear part
* \param w spatial acc rotational part
* \param dr spatial velocity linear part
-**/
+ **/
Vector3d computeAccFromSpatial(const Vector3d& ddr, const Vector3d& w, const Vector3d& dr);
-
/**
-* \brief Compute the relative contact points from foot center of a Talos foot.
-*
-* Order is clockwise, starting from bottom left (looking at a forward pointing foot from above).
-* Expressed in local foot coordinates.
-**/
+ * \brief Compute the relative contact points from foot center of a Talos foot.
+ *
+ * Order is clockwise, starting from bottom left (looking at a forward pointing foot from above).
+ * Expressed in local foot coordinates.
+ **/
std::vector<Vector3d> contacts_from_footrect_center();
-
/**
* \brief Compute the wrench at the end effector center expressed in world coordinates.
-* Each contact force (at a foot for instance) is represented in MAPI as a set of
+* Each contact force (at a foot for instance) is represented in MAPI as a set of
* 4 forces at the corners of the contact polygone (foot -> rectangle). These forces,
-* as the other quantities, are expressed in world coordinates. From this 4 3D forces,
+* as the other quantities, are expressed in world coordinates. From this 4 3D forces,
* we can compute the 6D wrench at the center of the origin point of the contacts vectors.
* \param contacts std vector of 3D contact forces
* \param cforces 12D corner forces vector ordered as [fx1, fy1, fz1, fx2, fy2... fz4], same order as contacts
* \param wRl rotation matrix, orientation from world frame to foot frame
**/
-Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contacts,
- const Matrix<double, 12, 1>& cf12);
-
+Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contacts, const Matrix<double, 12, 1>& cf12);
/**
* \brief Compute the centroidal angular inertia used to compute the angular momentum.
@@ -69,7 +63,4 @@ Matrix<double, 6, 1> contact_force_to_wrench(const std::vector<Vector3d>& contac
* \param q_static: configuration of the robot with free flyer pose at origin (local base frame = world frame)
* \param b_p_bc: measure of the CoM position relative to the base
**/
-Matrix3d compute_Iw(pinocchio::Model& model,
- pinocchio::Data& data,
- VectorXd& q_static,
- Vector3d& b_p_bc);
+Matrix3d compute_Iw(pinocchio::Model& model, pinocchio::Data& data, VectorXd& q_static, Vector3d& b_p_bc);
diff --git a/demos/solo_imu_kine.cpp b/demos/solo_imu_kine.cpp
index 47b072d..0e958bc 100644
--- a/demos/solo_imu_kine.cpp
+++ b/demos/solo_imu_kine.cpp
@@ -79,29 +79,31 @@
#include "bodydynamics/capture/capture_point_feet_nomove.h"
#include "bodydynamics/factor/factor_point_feet_nomove.h"
-
-
-
using namespace Eigen;
using namespace wolf;
using namespace pinocchio;
-typedef pinocchio::SE3Tpl<double> SE3;
+typedef pinocchio::SE3Tpl<double> SE3;
typedef pinocchio::ForceTpl<double> Force;
-void printFactorCosts(FrameBasePtr kf_last){
+void printFactorCosts(FrameBasePtr kf_last)
+{
int nb_max_fact_ptf = 4;
- int nb = 0;
- for (auto f: kf_last->getFactorList()){
+ int nb = 0;
+ for (auto f : kf_last->getFactorList())
+ {
auto fimu = std::dynamic_pointer_cast<FactorImu3d>(f);
- if (fimu){
+ if (fimu)
+ {
std::cout << std::setprecision(12) << " C_IMU " << sqrt(fimu->cost()) << std::endl;
}
- else{
+ else
+ {
auto fptn = std::dynamic_pointer_cast<FactorPointFeetNomove>(f);
- if (fptn and (nb < nb_max_fact_ptf)){
+ if (fptn and (nb < nb_max_fact_ptf))
+ {
Vector3d error = fptn->error();
- double cost = fptn->cost();
+ double cost = fptn->cost();
std::cout << std::setprecision(12) << " E_PF" << nb << " " << error.transpose() << std::endl;
std::cout << std::setprecision(12) << " C_PF" << nb << " " << sqrt(cost) << std::endl;
nb++;
@@ -110,159 +112,156 @@ void printFactorCosts(FrameBasePtr kf_last){
}
}
-
-int main (int argc, char **argv) {
+int main(int argc, char** argv)
+{
// retrieve parameters from yaml file
- YAML::Node config = YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/solo_real_estimation.yaml");
+ YAML::Node config =
+ YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/solo_real_estimation.yaml");
double dt = config["dt"].as<double>();
// double min_t = config["min_t"].as<double>();
- double max_t = config["max_t"].as<double>();
+ double max_t = config["max_t"].as<double>();
double solve_every_sec = config["solve_every_sec"].as<double>();
- bool solve_end = config["solve_end"].as<bool>();
+ bool solve_end = config["solve_end"].as<bool>();
// Ceres setup
- int max_num_iterations = config["max_num_iterations"].as<int>();
- double func_tol = config["func_tol"].as<double>();
- bool compute_cov = config["compute_cov"].as<bool>();
-
+ int max_num_iterations = config["max_num_iterations"].as<int>();
+ double func_tol = config["func_tol"].as<double>();
+ bool compute_cov = config["compute_cov"].as<bool>();
+
// robot
std::string robot_urdf = config["robot_urdf"].as<std::string>();
- int dimc = config["dimc"].as<int>();
- int nb_feet = config["nb_feet"].as<int>();
-
+ int dimc = config["dimc"].as<int>();
+ int nb_feet = config["nb_feet"].as<int>();
// priors
- double std_prior_p = config["std_prior_p"].as<double>();
- double std_prior_o = config["std_prior_o"].as<double>();
- double std_prior_v = config["std_prior_v"].as<double>();
- double std_abs_bias_pbc = config["std_abs_bias_pbc"].as<double>();
- double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
- double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
- double std_bp_drift = config["std_bp_drift"].as<double>();
- std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
+ double std_prior_p = config["std_prior_p"].as<double>();
+ double std_prior_o = config["std_prior_o"].as<double>();
+ double std_prior_v = config["std_prior_v"].as<double>();
+ double std_abs_bias_pbc = config["std_abs_bias_pbc"].as<double>();
+ double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
+ double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
+ double std_bp_drift = config["std_bp_drift"].as<double>();
+ std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
Eigen::Map<Vector6d> bias_imu_prior(bias_imu_prior_vec.data());
- std::vector<double> i_p_im_vec = config["i_p_im"].as<std::vector<double>>();
+ std::vector<double> i_p_im_vec = config["i_p_im"].as<std::vector<double>>();
Eigen::Map<Vector3d> i_p_im(i_p_im_vec.data());
- std::vector<double> i_q_m_vec = config["i_q_m"].as<std::vector<double>>();
+ std::vector<double> i_q_m_vec = config["i_q_m"].as<std::vector<double>>();
Eigen::Map<Vector4d> i_qvec_m(i_q_m_vec.data());
- std::vector<double> m_p_mb_vec = config["m_p_mb"].as<std::vector<double>>();
+ std::vector<double> m_p_mb_vec = config["m_p_mb"].as<std::vector<double>>();
Eigen::Map<Vector3d> m_p_mb(m_p_mb_vec.data());
- std::vector<double> m_q_b_vec = config["m_q_b"].as<std::vector<double>>();
+ std::vector<double> m_q_b_vec = config["m_q_b"].as<std::vector<double>>();
Eigen::Map<Vector4d> m_qvec_b(m_q_b_vec.data());
-
// kinematics
- double std_foot_nomove = config["std_foot_nomove"].as<double>();
- double std_altitude = config["std_altitude"].as<double>();
- double foot_radius = config["foot_radius"].as<double>();
- std::vector<double> delta_qa_vec = config["delta_qa"].as<std::vector<double>>();
- Eigen::Map<Eigen::Matrix<double,12,1>> delta_qa(delta_qa_vec.data());
- std::vector<double> alpha_qa_vec = config["alpha_qa"].as<std::vector<double>>();
- Eigen::Map<Eigen::Matrix<double,12,1>> alpha_qa(alpha_qa_vec.data());
- std::vector<double> l_p_lg_vec = config["l_p_lg"].as<std::vector<double>>();
- Eigen::Map<Vector3d> l_p_lg(l_p_lg_vec.data());
- double fz_thresh = config["fz_thresh"].as<double>();
-
+ double std_foot_nomove = config["std_foot_nomove"].as<double>();
+ double std_altitude = config["std_altitude"].as<double>();
+ double foot_radius = config["foot_radius"].as<double>();
+ std::vector<double> delta_qa_vec = config["delta_qa"].as<std::vector<double>>();
+ Eigen::Map<Eigen::Matrix<double, 12, 1>> delta_qa(delta_qa_vec.data());
+ std::vector<double> alpha_qa_vec = config["alpha_qa"].as<std::vector<double>>();
+ Eigen::Map<Eigen::Matrix<double, 12, 1>> alpha_qa(alpha_qa_vec.data());
+ std::vector<double> l_p_lg_vec = config["l_p_lg"].as<std::vector<double>>();
+ Eigen::Map<Vector3d> l_p_lg(l_p_lg_vec.data());
+ double fz_thresh = config["fz_thresh"].as<double>();
// MOCAP
- double std_pose_p = config["std_pose_p"].as<double>();
- double std_pose_o_deg = config["std_pose_o_deg"].as<double>();
- double std_mocap_extr_p = config["std_mocap_extr_p"].as<double>();
- double std_mocap_extr_o_deg = config["std_mocap_extr_o_deg"].as<double>();
- bool unfix_extr_sensor_pose = config["unfix_extr_sensor_pose"].as<bool>();
- bool time_tolerance_mocap = config["time_tolerance_mocap"].as<double>();
- double time_shift_mocap = config["time_shift_mocap"].as<double>();
-
-
- std::string data_file_path = config["data_file_path"].as<std::string>();
+ double std_pose_p = config["std_pose_p"].as<double>();
+ double std_pose_o_deg = config["std_pose_o_deg"].as<double>();
+ double std_mocap_extr_p = config["std_mocap_extr_p"].as<double>();
+ double std_mocap_extr_o_deg = config["std_mocap_extr_o_deg"].as<double>();
+ bool unfix_extr_sensor_pose = config["unfix_extr_sensor_pose"].as<bool>();
+ bool time_tolerance_mocap = config["time_tolerance_mocap"].as<double>();
+ double time_shift_mocap = config["time_shift_mocap"].as<double>();
+
+ std::string data_file_path = config["data_file_path"].as<std::string>();
std::string out_npz_file_path = config["out_npz_file_path"].as<std::string>();
// MOCAP to IMU transform
Quaterniond i_q_m(i_qvec_m);
i_q_m.normalize();
assert(i_q_m.normalized().isApprox(i_q_m));
- SE3 i_T_m(i_q_m, i_p_im);
- Matrix3d i_R_m = i_T_m.rotation();
+ SE3 i_T_m(i_q_m, i_p_im);
+ Matrix3d i_R_m = i_T_m.rotation();
// IMU to MOCAP transform
- SE3 m_T_i = i_T_m.inverse();
- Vector3d m_p_mi = m_T_i.translation();
- Quaterniond m_q_i = Quaterniond(m_T_i.rotation());
+ SE3 m_T_i = i_T_m.inverse();
+ Vector3d m_p_mi = m_T_i.translation();
+ Quaterniond m_q_i = Quaterniond(m_T_i.rotation());
// MOCAP to BASE transform
Quaterniond m_q_b(m_qvec_b);
m_q_b.normalize();
assert(m_q_b.normalized().isApprox(m_q_b));
- SE3 m_T_b(m_q_b, m_p_mb);
- Matrix3d m_R_b = m_T_b.rotation();
+ SE3 m_T_b(m_q_b, m_p_mb);
+ Matrix3d m_R_b = m_T_b.rotation();
// IMU to BASE transform (composition)
- SE3 i_T_b = i_T_m*m_T_b;
+ SE3 i_T_b = i_T_m * m_T_b;
Vector3d i_p_ib = i_T_b.translation();
- Matrix3d i_R_b = i_T_b.rotation();
-
-
+ Matrix3d i_R_b = i_T_b.rotation();
// initialize some vectors and fill them with dicretized data
cnpy::npz_t arr_map = cnpy::npz_load(data_file_path);
- //load it into a new array
- cnpy::NpyArray t_npyarr = arr_map["t"];
+ // load it into a new array
+ cnpy::NpyArray t_npyarr = arr_map["t"];
cnpy::NpyArray imu_acc_npyarr = arr_map["imu_acc"];
// cnpy::NpyArray o_a_oi_npyarr = arr_map["o_a_oi"];
- cnpy::NpyArray o_q_i_npyarr = arr_map["o_q_i"];
+ cnpy::NpyArray o_q_i_npyarr = arr_map["o_q_i"];
cnpy::NpyArray i_omg_oi_npyarr = arr_map["i_omg_oi"];
- cnpy::NpyArray qa_npyarr = arr_map["qa"];
- cnpy::NpyArray dqa_npyarr = arr_map["dqa"];
- cnpy::NpyArray tau_npyarr = arr_map["tau"];
+ cnpy::NpyArray qa_npyarr = arr_map["qa"];
+ cnpy::NpyArray dqa_npyarr = arr_map["dqa"];
+ cnpy::NpyArray tau_npyarr = arr_map["tau"];
cnpy::NpyArray l_forces_npyarr = arr_map["l_forces"];
// cnpy::NpyArray w_pose_wm_npyarr = arr_map["w_pose_wm"];
cnpy::NpyArray m_v_wm_npyarr = arr_map["m_v_wm"];
cnpy::NpyArray w_v_wm_npyarr = arr_map["w_v_wm"];
// cnpy::NpyArray o_R_i_npyarr = arr_map["o_R_i"];
- cnpy::NpyArray w_p_wm_npyarr = arr_map["w_p_wm"];
- cnpy::NpyArray w_q_m_npyarr = arr_map["w_q_m"];
+ cnpy::NpyArray w_p_wm_npyarr = arr_map["w_p_wm"];
+ cnpy::NpyArray w_q_m_npyarr = arr_map["w_q_m"];
cnpy::NpyArray contact_npyarr = arr_map["contacts"];
// cnpy::NpyArray b_p_bl_mocap_npyarr = arr_map["b_p_bl_mocap"];
// cnpy::NpyArray w_R_m_npyarr = arr_map["w_R_m"];
- double* t_arr = t_npyarr.data<double>();
+ double* t_arr = t_npyarr.data<double>();
double* imu_acc_arr = imu_acc_npyarr.data<double>();
// double* o_a_oi_arr = o_a_oi_npyarr.data<double>();
double* i_omg_oi_arr = i_omg_oi_npyarr.data<double>();
- double* qa_arr = qa_npyarr.data<double>();
- double* dqa_arr = dqa_npyarr.data<double>();
- double* tau_arr = tau_npyarr.data<double>();
+ double* qa_arr = qa_npyarr.data<double>();
+ double* dqa_arr = dqa_npyarr.data<double>();
+ double* tau_arr = tau_npyarr.data<double>();
double* l_forces_arr = l_forces_npyarr.data<double>();
// double* w_pose_wm_arr = w_pose_wm_npyarr.data<double>();
double* m_v_wm_arr = m_v_wm_npyarr.data<double>();
double* w_v_wm_arr = w_v_wm_npyarr.data<double>();
- double* o_q_i_arr = o_q_i_npyarr.data<double>();
+ double* o_q_i_arr = o_q_i_npyarr.data<double>();
// double* o_R_i_arr = o_R_i_npyarr.data<double>();
double* w_p_wm_arr = w_p_wm_npyarr.data<double>();
- double* w_q_m_arr = w_q_m_npyarr.data<double>();
-
+ double* w_q_m_arr = w_q_m_npyarr.data<double>();
+
double* contact_arr = contact_npyarr.data<double>();
// double* b_p_bl_mocap_arr = b_p_bl_mocap_npyarr.data<double>();
// double* w_R_m_arr = w_R_m_npyarr.data<double>();
unsigned int N = t_npyarr.shape[0];
- if (max_t > 0){
- N = N < int(max_t/dt) ? N : int(max_t/dt);
+ if (max_t > 0)
+ {
+ N = N < int(max_t / dt) ? N : int(max_t / dt);
}
// Load the urdf model
Model model;
pinocchio::urdf::buildModel(robot_urdf, JointModelFreeFlyer(), model);
std::cout << "model name: " << model.name << std::endl;
- Data data(model);
+ Data data(model);
std::vector<std::string> ee_names = {"FL_ANKLE", "FR_ANKLE", "HL_ANKLE", "HR_ANKLE"};
- unsigned int nbc = ee_names.size();
+ unsigned int nbc = ee_names.size();
// Recover end effector frame ids
std::vector<int> ee_ids;
std::cout << "Frame ids" << std::endl;
- for (auto ee_name: ee_names){
+ for (auto ee_name : ee_names)
+ {
ee_ids.push_back(model.getFrameId(ee_name));
std::cout << ee_name << std::endl;
}
@@ -274,18 +273,18 @@ int main (int argc, char **argv) {
ProblemPtr problem = Problem::create("POV", 3);
- // add a tree manager for sliding window optimization
- ParserYaml parser = ParserYaml("demos/tree_manager.yaml", bodydynamics_root_dir);
- ParamsServer server = ParamsServer(parser.getParams());
- auto tree_manager = TreeManagerSlidingWindow::create("tree_manager", server);
+ // add a tree manager for sliding window optimization
+ ParserYaml parser = ParserYaml("demos/tree_manager.yaml", bodydynamics_root_dir);
+ ParamsServer server = ParamsServer(parser.getParams());
+ auto tree_manager = TreeManagerSlidingWindow::create("tree_manager", server);
problem->setTreeManager(tree_manager);
//////////////////////
// CERES WRAPPER
- SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
+ SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
solver->getSolverOptions().max_num_iterations = max_num_iterations;
- solver->getSolverOptions().function_tolerance = func_tol; // 1e-6
- solver->getSolverOptions().gradient_tolerance = 1e-4*solver->getSolverOptions().function_tolerance;
+ solver->getSolverOptions().function_tolerance = func_tol; // 1e-6
+ solver->getSolverOptions().gradient_tolerance = 1e-4 * solver->getSolverOptions().function_tolerance;
// solver->getSolverOptions().minimizer_type = ceres::TRUST_REGION;
// solver->getSolverOptions().trust_region_strategy_type = ceres::DOGLEG;
@@ -294,109 +293,120 @@ int main (int argc, char **argv) {
// solver->getSolverOptions().minimizer_type = ceres::LINE_SEARCH;
// solver->getSolverOptions().line_search_direction_type = ceres::LBFGS;
-
//===================================================== INITIALIZATION
// SENSOR + PROCESSOR IMU
- SensorBasePtr sen_imu_base = problem->installSensor("SensorImu", "SenImu", (Vector7d() << 0,0,0, 0,0,0,1).finished(), bodydynamics_root_dir + "/demos/sensor_imu_solo12.yaml");
- SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base);
- ProcessorBasePtr proc_ftp_base = problem->installProcessor("ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu_solo12.yaml");
+ SensorBasePtr sen_imu_base = problem->installSensor("SensorImu",
+ "SenImu",
+ (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(),
+ bodydynamics_root_dir + "/demos/sensor_imu_solo12.yaml");
+ SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base);
+ ProcessorBasePtr proc_ftp_base = problem->installProcessor(
+ "ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu_solo12.yaml");
ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_ftp_base);
// SENSOR + PROCESSOR POINT FEET NOMOVE
ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>();
- intr_pfn->std_foot_nomove_ = std_foot_nomove;
- intr_pfn->std_altitude_ = std_altitude;
- intr_pfn->foot_radius_ = foot_radius;
- VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
+ intr_pfn->std_foot_nomove_ = std_foot_nomove;
+ intr_pfn->std_altitude_ = std_altitude;
+ intr_pfn->foot_radius_ = foot_radius;
+ VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
SensorBasePtr sen_pfnm_base = problem->installSensor("SensorPointFeetNomove", "SenPfnm", extr, intr_pfn);
- SensorPointFeetNomovePtr sen_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base);
+ SensorPointFeetNomovePtr sen_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base);
ParamsProcessorPointFeetNomovePtr params_pfnm = std::make_shared<ParamsProcessorPointFeetNomove>();
- params_pfnm->voting_active = false;
- params_pfnm->time_tolerance = 0.0;
- params_pfnm->max_time_vote = 0.0;
- ProcessorBasePtr proc_pfnm_base = problem->installProcessor("ProcessorPointFeetNomove", "PPointFeetNomove", sen_pfnm, params_pfnm);
-
+ params_pfnm->voting_active = false;
+ params_pfnm->time_tolerance = 0.0;
+ params_pfnm->max_time_vote = 0.0;
+ ProcessorBasePtr proc_pfnm_base =
+ problem->installProcessor("ProcessorPointFeetNomove", "PPointFeetNomove", sen_pfnm, params_pfnm);
// SETPRIOR RETRO-ENGINEERING
// We are not using setPrior because of processors initial captures problems so we have to
// - Manually create the first KF and its pose and velocity priors
- // - call setOrigin on processors MotionProvider since the flag prior_is_set is not true when doing installProcessor (later)
+ // - call setOrigin on processors MotionProvider since the flag prior_is_set is not true when doing
+ // installProcessor (later)
//////////////////////
// COMPUTE INITIAL STATE
- TimeStamp t0(t_arr[0]);
+ TimeStamp t0(t_arr[0]);
Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr);
Eigen::Map<Vector4d> w_qvec_m_init(w_q_m_arr);
w_qvec_m_init.normalize(); // not close enough to wolf eps sometimes
- Quaterniond w_q_m_init(w_qvec_m_init);
- Vector3d w_p_wi_init = w_p_wm + w_q_m_init*m_p_mi;
- Quaterniond w_q_i_init = w_q_m_init*m_q_i;
+ Quaterniond w_q_m_init(w_qvec_m_init);
+ Vector3d w_p_wi_init = w_p_wm + w_q_m_init * m_p_mi;
+ Quaterniond w_q_i_init = w_q_m_init * m_q_i;
VectorComposite x_origin("POV", {w_p_wi_init, w_q_i_init.coeffs(), Vector3d::Zero()});
- VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()});
+ 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);
FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0); // if mocap used
-
+
proc_imu->setOrigin(KF1);
//////////////////////////////////////////
// INITIAL BIAS FACTORS
// Add absolute factor on Imu biases to simulate a fix()
- Vector6d std_abs_imu; std_abs_imu << std_abs_bias_acc*Vector3d::Ones(), std_abs_bias_gyro*Vector3d::Ones();
- Matrix6d Q_bi = std_abs_imu.array().square().matrix().asDiagonal();
- CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
+ Vector6d std_abs_imu;
+ std_abs_imu << std_abs_bias_acc * Vector3d::Ones(), std_abs_bias_gyro * Vector3d::Ones();
+ Matrix6d Q_bi = std_abs_imu.array().square().matrix().asDiagonal();
+ CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", bias_imu_prior, Q_bi);
- FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
+ FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(
+ featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
KF1->getCaptureOf(sen_imu)->getStateBlock('I')->setState(bias_imu_prior);
sen_imu->getStateBlock('I')->setState(bias_imu_prior);
-
// Allocate on the heap to prevent stack overflow for large datasets
- double* o_p_ob_fbk_carr = new double[3*N];
- double* o_q_b_fbk_carr = new double[4*N];
- double* o_v_ob_fbk_carr = new double[3*N];
- double* o_p_oi_fbk_carr = new double[3*N];
- double* o_q_i_fbk_carr = new double[4*N];
- double* o_v_oi_fbk_carr = new double[3*N];
- double* imu_bias_fbk_carr = new double[6*N];
- double* i_pose_im_fbk_carr = new double[7*N];
+ double* o_p_ob_fbk_carr = new double[3 * N];
+ double* o_q_b_fbk_carr = new double[4 * N];
+ double* o_v_ob_fbk_carr = new double[3 * N];
+ double* o_p_oi_fbk_carr = new double[3 * N];
+ double* o_q_i_fbk_carr = new double[4 * N];
+ double* o_v_oi_fbk_carr = new double[3 * N];
+ double* imu_bias_fbk_carr = new double[6 * N];
+ double* i_pose_im_fbk_carr = new double[7 * N];
// covariances computed on the fly
- std::vector<Vector3d> Qp_fbk_v; Qp_fbk_v.push_back(Vector3d::Zero());
- std::vector<Vector3d> Qo_fbk_v; Qo_fbk_v.push_back(Vector3d::Zero());
- std::vector<Vector3d> Qv_fbk_v; Qv_fbk_v.push_back(Vector3d::Zero());
- std::vector<Vector6d> Qbi_fbk_v; Qbi_fbk_v.push_back(Vector6d::Zero());
- std::vector<Vector6d> Qm_fbk_v; Qm_fbk_v.push_back(Vector6d::Zero());
-
-
- std::vector<Vector3d> i_omg_oi_v;
+ std::vector<Vector3d> Qp_fbk_v;
+ Qp_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector3d> Qo_fbk_v;
+ Qo_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector3d> Qv_fbk_v;
+ Qv_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector6d> Qbi_fbk_v;
+ Qbi_fbk_v.push_back(Vector6d::Zero());
+ std::vector<Vector6d> Qm_fbk_v;
+ Qm_fbk_v.push_back(Vector6d::Zero());
+
+ std::vector<Vector3d> i_omg_oi_v;
//////////////////////////////////////////
unsigned int nb_kf = 1;
- for (unsigned int i=0; i < N; i++){
+ for (unsigned int i = 0; i < N; i++)
+ {
TimeStamp ts(t_arr[i]);
////////////////////////////////////
// Retrieve current state
- VectorComposite curr_state = problem->getState();
- Vector4d o_qvec_i_curr = curr_state['O'];
- Quaterniond o_q_i_curr(o_qvec_i_curr);
- Vector3d o_v_oi_curr = curr_state['V'];
-
+ VectorComposite curr_state = problem->getState();
+ Vector4d o_qvec_i_curr = curr_state['O'];
+ Quaterniond o_q_i_curr(o_qvec_i_curr);
+ Vector3d o_v_oi_curr = curr_state['V'];
//////////////
// PROCESS IMU
//////////////
// Get measurements
// retrieve traj data
- Eigen::Map<Vector3d> imu_acc(imu_acc_arr+3*i);
- Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr+3*i);
+ Eigen::Map<Vector3d> imu_acc(imu_acc_arr + 3 * i);
+ Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr + 3 * i);
// store i_omg_oi for later computation of o_v_ob from o_v_oi
i_omg_oi_v.push_back(i_omg_oi);
- Vector6d imu_bias = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
+ Vector6d imu_bias = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>());
- Vector6d acc_gyr_meas; acc_gyr_meas << imu_acc, i_omg_oi;
+ Vector6d acc_gyr_meas;
+ acc_gyr_meas << imu_acc, i_omg_oi;
CaptureImuPtr CImu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, sen_imu->getNoiseCov());
CImu->process();
//////////////
@@ -405,43 +415,46 @@ int main (int argc, char **argv) {
//////////////////////////////////
// Kinematics + forces
// retrieve traj data
- Eigen::Map<Matrix<double,12, 1>> tau(tau_arr+12*i);
- Eigen::Map<Matrix<double,12, 1>> qa(qa_arr+12*i);
- Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr+12*i);
- Eigen::Map<Matrix<double,4, 1>> contact_gtr(contact_arr+4*i); // int conversion does not work!
+ Eigen::Map<Matrix<double, 12, 1>> tau(tau_arr + 12 * i);
+ Eigen::Map<Matrix<double, 12, 1>> qa(qa_arr + 12 * i);
+ Eigen::Map<Matrix<double, 12, 1>> dqa(dqa_arr + 12 * i);
+ Eigen::Map<Matrix<double, 4, 1>> contact_gtr(contact_arr + 4 * i); // int conversion does not work!
// std::cout << contact_gtr.transpose() << std::endl;
// Eigen::Map<Matrix<double,12, 1>> b_p_bl_mocap(b_p_bl_mocap_arr+12*i); // int conversion does not work!
// Kinematics model
qa = qa + delta_qa + alpha_qa.cwiseProduct(tau);
-
+
Matrix3d o_R_i = Quaterniond(o_q_i_curr).toRotationMatrix(); // IMU internal filter
// Or else, retrieve from preprocessed dataset
- Eigen::Map<Matrix<double,12, 1>> l_forces(l_forces_arr+12*i);
+ Eigen::Map<Matrix<double, 12, 1>> l_forces(l_forces_arr + 12 * i);
- Vector3d o_f_tot = Vector3d::Zero();
+ Vector3d o_f_tot = Vector3d::Zero();
std::vector<Vector3d> l_f_l_vec;
std::vector<Vector3d> o_f_l_vec;
std::vector<Vector3d> i_p_il_vec;
std::vector<Vector4d> i_qvec_l_vec;
// needing relative kinematics measurements
- VectorXd q_static(19); q_static << 0,0,0, 0,0,0,1, qa;
- VectorXd dq_static(18); dq_static << 0,0,0, 0,0,0, dqa;
+ VectorXd q_static(19);
+ q_static << 0, 0, 0, 0, 0, 0, 1, qa;
+ VectorXd dq_static(18);
+ dq_static << 0, 0, 0, 0, 0, 0, dqa;
forwardKinematics(model, data, q_static, dq_static);
updateFramePlacements(model, data);
- for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
+ for (unsigned int i_ee = 0; i_ee < nbc; i_ee++)
+ {
auto b_T_l = data.oMf[ee_ids[i_ee]];
- auto i_T_l = i_T_b * b_T_l;
- Vector3d i_p_il = i_T_l.translation();
- Matrix3d i_R_l = i_T_l.rotation();
+ auto i_T_l = i_T_b * b_T_l;
+ Vector3d i_p_il = i_T_l.translation();
+ Matrix3d i_R_l = i_T_l.rotation();
Vector4d i_qvec_l = Quaterniond(i_R_l).coeffs();
- i_p_il = i_p_il + i_R_l*l_p_lg;
+ i_p_il = i_p_il + i_R_l * l_p_lg;
- Vector3d l_f_l = l_forces.segment(3*i_ee, 3);
- Vector3d o_f_l = o_R_i*i_R_l * l_f_l; // for contact test
+ Vector3d l_f_l = l_forces.segment(3 * i_ee, 3);
+ Vector3d o_f_l = o_R_i * i_R_l * l_f_l; // for contact test
l_f_l_vec.push_back(l_f_l);
o_f_l_vec.push_back(o_f_l);
@@ -450,16 +463,19 @@ int main (int argc, char **argv) {
}
// Detect feet in contact
- int nb_feet_in_contact = 0;
+ int nb_feet_in_contact = 0;
std::unordered_map<int, Vector7d> kin_incontact;
// std::cout << "" << std::endl;
- for (unsigned int i_ee=0; i_ee<nbc; i_ee++){
+ for (unsigned int i_ee = 0; i_ee < nbc; i_ee++)
+ {
// basic contact detection based on normal foot vel, things break if no foot is in contact
// if ((o_f_l_vec[i_ee](2) > fz_thresh) && (nb_feet_in_contact < nb_feet)){
// if ((contact_gtr[i_ee] > 0.5) && (nb_feet_in_contact < nb_feet)){
- if (contact_gtr[i_ee] > 0.5){
+ if (contact_gtr[i_ee] > 0.5)
+ {
nb_feet_in_contact++;
- Vector7d i_pose_il; i_pose_il << i_p_il_vec[i_ee], i_qvec_l_vec[i_ee];
+ Vector7d i_pose_il;
+ i_pose_il << i_p_il_vec[i_ee], i_qvec_l_vec[i_ee];
kin_incontact.insert({i_ee, i_pose_il});
}
}
@@ -468,7 +484,8 @@ int main (int argc, char **argv) {
CPF->process();
// solve every new KF
- if (problem->getTrajectory()->size() > nb_kf ){
+ if (problem->getTrajectory()->size() > nb_kf)
+ {
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
// ////////////////////////////////
@@ -480,7 +497,7 @@ int main (int argc, char **argv) {
// std::cout << "P E R T U B!" << std::endl;
// // problem->perturb(0.000001);
// kf_last->perturb();
- // // std::cout << "kf_last P " << kf_last->getP()->getState().transpose() << std::endl;
+ // // std::cout << "kf_last P " << kf_last->getP()->getState().transpose() << std::endl;
// report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
// printFactorCosts(kf_last);
// }
@@ -488,10 +505,10 @@ int main (int argc, char **argv) {
// }
// ///////////////////////
-
////////////////////////////////
// FOR REAL
- if (solver->iterations() == 1){
+ if (solver->iterations() == 1)
+ {
// kf_last->perturb(0.000001);
report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
}
@@ -503,33 +520,36 @@ int main (int argc, char **argv) {
// printFactorCosts(kf_last);
-
// compute covariances of KF and capture stateblocks
- Eigen::MatrixXd Qp_fbk = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd Qo_fbk = Eigen::Matrix3d::Identity(); // global or local?
- Eigen::MatrixXd Qv_fbk = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qp_fbk = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qo_fbk = Eigen::Matrix3d::Identity(); // global or local?
+ Eigen::MatrixXd Qv_fbk = Eigen::Matrix3d::Identity();
Eigen::MatrixXd Qbi_fbk = Eigen::Matrix6d::Identity();
- Eigen::MatrixXd Qmp_fbk = Eigen::Matrix3d::Identity(); // extr mocap
- Eigen::MatrixXd Qmo_fbk = Eigen::Matrix3d::Identity(); // extr mocap
-
+ Eigen::MatrixXd Qmp_fbk = Eigen::Matrix3d::Identity(); // extr mocap
+ Eigen::MatrixXd Qmo_fbk = Eigen::Matrix3d::Identity(); // extr mocap
+
if (compute_cov)
- solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+ solver->computeCovariances(
+ SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see
+ // below
problem->getCovarianceBlock(kf_last->getStateBlock('P'), Qp_fbk);
problem->getCovarianceBlock(kf_last->getStateBlock('O'), Qo_fbk);
problem->getCovarianceBlock(kf_last->getStateBlock('V'), Qv_fbk);
- CaptureBasePtr cap_imu = kf_last->getCaptureOf(sen_imu);
+ CaptureBasePtr cap_imu = kf_last->getCaptureOf(sen_imu);
if (compute_cov)
- solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
+ solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other
+ // computed covs -> issue to raise
problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi_fbk);
// Retrieve diagonals
- Vector3d Qp_fbk_diag = Qp_fbk.diagonal();
- Vector3d Qo_fbk_diag = Qo_fbk.diagonal();
- Vector3d Qv_fbk_diag = Qv_fbk.diagonal();
- Vector6d Qbi_fbk_diag = Qbi_fbk.diagonal();
- Vector6d Qm_fbk_diag; Qm_fbk_diag << Qmp_fbk.diagonal(), Qmo_fbk.diagonal();
- Vector3d Qmo_fbk_diag = Qmo_fbk.diagonal();
+ Vector3d Qp_fbk_diag = Qp_fbk.diagonal();
+ Vector3d Qo_fbk_diag = Qo_fbk.diagonal();
+ Vector3d Qv_fbk_diag = Qv_fbk.diagonal();
+ Vector6d Qbi_fbk_diag = Qbi_fbk.diagonal();
+ Vector6d Qm_fbk_diag;
+ Qm_fbk_diag << Qmp_fbk.diagonal(), Qmo_fbk.diagonal();
+ Vector3d Qmo_fbk_diag = Qmo_fbk.diagonal();
Qp_fbk_v.push_back(Qp_fbk_diag);
Qo_fbk_v.push_back(Qo_fbk_diag);
@@ -542,32 +562,31 @@ int main (int argc, char **argv) {
// Store current state estimation
VectorComposite state_fbk = problem->getState(ts);
- Vector3d o_p_oi = state_fbk['P'];
- Vector4d o_qvec_i = state_fbk['O'];
- Vector3d o_v_oi = state_fbk['V'];
+ Vector3d o_p_oi = state_fbk['P'];
+ Vector4d o_qvec_i = state_fbk['O'];
+ Vector3d o_v_oi = state_fbk['V'];
- // IMU to base frame
- o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
- Matrix3d o_R_b = o_R_i * i_R_b;
+ // IMU to base frame
+ o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+ Matrix3d o_R_b = o_R_i * i_R_b;
Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
- Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
+ Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
// Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_p_ib);
Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi.cross(i_p_ib);
imu_bias = sen_imu->getIntrinsic()->getState();
- Vector7d i_pose_im_fbk; i_pose_im_fbk << i_p_im, i_qvec_m;
-
- mempcpy(o_p_ob_fbk_carr+3*i, o_p_ob.data(), 3*sizeof(double));
- mempcpy(o_q_b_fbk_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
- mempcpy(o_v_ob_fbk_carr+3*i, o_v_ob.data(), 3*sizeof(double));
- mempcpy(o_p_oi_fbk_carr+3*i, o_p_oi.data(), 3*sizeof(double));
- mempcpy(o_q_i_fbk_carr +4*i, o_qvec_i.data(), 4*sizeof(double));
- mempcpy(o_v_oi_fbk_carr+3*i, o_v_oi.data(), 3*sizeof(double));
- mempcpy(imu_bias_fbk_carr+6*i, imu_bias.data(), 6*sizeof(double));
- mempcpy(i_pose_im_fbk_carr+7*i, i_pose_im_fbk.data(), 7*sizeof(double));
+ Vector7d i_pose_im_fbk;
+ i_pose_im_fbk << i_p_im, i_qvec_m;
+
+ mempcpy(o_p_ob_fbk_carr + 3 * i, o_p_ob.data(), 3 * sizeof(double));
+ mempcpy(o_q_b_fbk_carr + 4 * i, o_qvec_b.data(), 4 * sizeof(double));
+ mempcpy(o_v_ob_fbk_carr + 3 * i, o_v_ob.data(), 3 * sizeof(double));
+ mempcpy(o_p_oi_fbk_carr + 3 * i, o_p_oi.data(), 3 * sizeof(double));
+ mempcpy(o_q_i_fbk_carr + 4 * i, o_qvec_i.data(), 4 * sizeof(double));
+ mempcpy(o_v_oi_fbk_carr + 3 * i, o_v_oi.data(), 3 * sizeof(double));
+ mempcpy(imu_bias_fbk_carr + 6 * i, imu_bias.data(), 6 * sizeof(double));
+ mempcpy(i_pose_im_fbk_carr + 7 * i, i_pose_im_fbk.data(), 7 * sizeof(double));
}
-
-
///////////////////////////////////////////
//////////////// SOLVING //////////////////
@@ -578,164 +597,165 @@ int main (int argc, char **argv) {
// std::cout << report << std::endl;
// }
- double* o_p_ob_carr = new double[3*N];
- double* o_q_b_carr = new double[4*N];
- double* o_v_ob_carr = new double[3*N];
- double* o_p_oi_carr = new double[3*N];
- double* o_q_i_carr = new double[4*N];
- double* o_v_oi_carr = new double[3*N];
- double* imu_bias_carr = new double[6*N];
+ double* o_p_ob_carr = new double[3 * N];
+ double* o_q_b_carr = new double[4 * N];
+ double* o_v_ob_carr = new double[3 * N];
+ double* o_p_oi_carr = new double[3 * N];
+ double* o_q_i_carr = new double[4 * N];
+ double* o_v_oi_carr = new double[3 * N];
+ double* imu_bias_carr = new double[6 * N];
- for (unsigned int i=0; i < N; i++) {
+ for (unsigned int i = 0; i < N; i++)
+ {
VectorComposite state_est = problem->getState(t_arr[i]);
- Vector3d i_omg_oi = i_omg_oi_v[i];
- Vector3d o_p_oi = state_est['P'];
- Vector4d o_qvec_i = state_est['O'];
- Vector3d o_v_oi = state_est['V'];
+ Vector3d i_omg_oi = i_omg_oi_v[i];
+ Vector3d o_p_oi = state_est['P'];
+ Vector4d o_qvec_i = state_est['O'];
+ Vector3d o_v_oi = state_est['V'];
- auto sb = sen_imu->getStateBlockDynamic('I', t_arr[i]);
- Vector6d imu_bias = sb->getState();
+ auto sb = sen_imu->getStateBlockDynamic('I', t_arr[i]);
+ Vector6d imu_bias = sb->getState();
Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>());
- Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
- Matrix3d o_R_b = o_R_i * i_R_b;
+ Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+ Matrix3d o_R_b = o_R_i * i_R_b;
Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
- Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
+ Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
// Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_p_ib);
Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi.cross(i_p_ib);
- mempcpy(o_p_ob_carr+3*i, o_p_ob.data(), 3*sizeof(double));
- mempcpy(o_q_b_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
- mempcpy(o_v_ob_carr+3*i, o_v_ob.data(), 3*sizeof(double));
- mempcpy(o_p_oi_carr+3*i, o_p_oi.data(), 3*sizeof(double));
- mempcpy(o_q_i_carr +4*i, o_qvec_i.data(), 4*sizeof(double));
- mempcpy(o_v_oi_carr+3*i, o_v_oi.data(), 3*sizeof(double));
- mempcpy(imu_bias_carr+6*i, imu_bias.data(), 6*sizeof(double));
+ mempcpy(o_p_ob_carr + 3 * i, o_p_ob.data(), 3 * sizeof(double));
+ mempcpy(o_q_b_carr + 4 * i, o_qvec_b.data(), 4 * sizeof(double));
+ mempcpy(o_v_ob_carr + 3 * i, o_v_ob.data(), 3 * sizeof(double));
+ mempcpy(o_p_oi_carr + 3 * i, o_p_oi.data(), 3 * sizeof(double));
+ mempcpy(o_q_i_carr + 4 * i, o_qvec_i.data(), 4 * sizeof(double));
+ mempcpy(o_v_oi_carr + 3 * i, o_v_oi.data(), 3 * sizeof(double));
+ mempcpy(imu_bias_carr + 6 * i, imu_bias.data(), 6 * sizeof(double));
}
-
// Compute Covariances
- unsigned int Nkf = problem->getTrajectory()->size();
- double* tkf_carr = new double[1*Nkf];
- double* Qp_carr = new double[3*Nkf];
- double* Qo_carr = new double[3*Nkf];
- double* Qv_carr = new double[3*Nkf];
- double* Qbi_carr = new double[6*Nkf];
- double* Qm_carr = new double[6*Nkf];
+ unsigned int Nkf = problem->getTrajectory()->size();
+ double* tkf_carr = new double[1 * Nkf];
+ double* Qp_carr = new double[3 * Nkf];
+ double* Qo_carr = new double[3 * Nkf];
+ double* Qv_carr = new double[3 * Nkf];
+ double* Qbi_carr = new double[6 * Nkf];
+ double* Qm_carr = new double[6 * Nkf];
// feedback covariances
- double* Qp_fbk_carr = new double[3*Nkf];
- double* Qo_fbk_carr = new double[3*Nkf];
- double* Qv_fbk_carr = new double[3*Nkf];
- double* Qbi_fbk_carr = new double[6*Nkf];
- double* Qm_fbk_carr = new double[6*Nkf];
+ double* Qp_fbk_carr = new double[3 * Nkf];
+ double* Qo_fbk_carr = new double[3 * Nkf];
+ double* Qv_fbk_carr = new double[3 * Nkf];
+ double* Qbi_fbk_carr = new double[6 * Nkf];
+ double* Qm_fbk_carr = new double[6 * Nkf];
// factor errors
- double* fac_imu_err_carr = new double[9*Nkf];
- double* fac_pose_err_carr = new double[6*Nkf];
- int i = 0;
- for (auto elt: problem->getTrajectory()->getFrameMap())
+ double* fac_imu_err_carr = new double[9 * Nkf];
+ double* fac_pose_err_carr = new double[6 * Nkf];
+ int i = 0;
+ for (auto elt : problem->getTrajectory()->getFrameMap())
{
std::cout << "Traj " << i << "/" << problem->getTrajectory()->size() << std::endl;
- tkf_carr[i] = elt.first.get();
- auto kf = elt.second;
+ tkf_carr[i] = elt.first.get();
+ auto kf = elt.second;
VectorComposite kf_state = kf->getState();
-
+
// compute covariances of KF and capture stateblocks
- Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local?
- Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local?
+ Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity();
Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity();
- Eigen::MatrixXd Qmp = Eigen::Matrix3d::Identity(); // extr mocap
- Eigen::MatrixXd Qmo = Eigen::Matrix3d::Identity(); // extr mocap
-
+ Eigen::MatrixXd Qmp = Eigen::Matrix3d::Identity(); // extr mocap
+ Eigen::MatrixXd Qmo = Eigen::Matrix3d::Identity(); // extr mocap
+
if (compute_cov)
- solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+ solver->computeCovariances(
+ SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
- CaptureBasePtr cap_imu = kf->getCaptureOf(sen_imu);
+ CaptureBasePtr cap_imu = kf->getCaptureOf(sen_imu);
if (compute_cov)
- solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
+ solver->computeCovariances(
+ cap_imu
+ ->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
// diagonal components
- Vector3d Qp_diag = Qp.diagonal();
- Vector3d Qo_diag = Qo.diagonal();
- Vector3d Qv_diag = Qv.diagonal();
- Vector6d Qbi_diag = Qbi.diagonal();
- Vector6d Qm_diag; Qm_diag << Qmp.diagonal(), Qmo.diagonal();
-
- memcpy(Qp_carr + 3*i, Qp_diag.data(), 3*sizeof(double));
- memcpy(Qo_carr + 3*i, Qo_diag.data(), 3*sizeof(double));
- memcpy(Qv_carr + 3*i, Qv_diag.data(), 3*sizeof(double));
- memcpy(Qbi_carr + 6*i, Qbi_diag.data(), 6*sizeof(double));
- memcpy(Qm_carr + 6*i, Qm_diag.data(), 6*sizeof(double));
+ Vector3d Qp_diag = Qp.diagonal();
+ Vector3d Qo_diag = Qo.diagonal();
+ Vector3d Qv_diag = Qv.diagonal();
+ Vector6d Qbi_diag = Qbi.diagonal();
+ Vector6d Qm_diag;
+ Qm_diag << Qmp.diagonal(), Qmo.diagonal();
+
+ memcpy(Qp_carr + 3 * i, Qp_diag.data(), 3 * sizeof(double));
+ memcpy(Qo_carr + 3 * i, Qo_diag.data(), 3 * sizeof(double));
+ memcpy(Qv_carr + 3 * i, Qv_diag.data(), 3 * sizeof(double));
+ memcpy(Qbi_carr + 6 * i, Qbi_diag.data(), 6 * sizeof(double));
+ memcpy(Qm_carr + 6 * i, Qm_diag.data(), 6 * sizeof(double));
// Recover feedback covariances
- memcpy(Qp_fbk_carr + 3*i, Qp_fbk_v[i].data(), 3*sizeof(double));
- memcpy(Qo_fbk_carr + 3*i, Qo_fbk_v[i].data(), 3*sizeof(double));
- memcpy(Qv_fbk_carr + 3*i, Qv_fbk_v[i].data(), 3*sizeof(double));
- memcpy(Qbi_fbk_carr + 6*i, Qbi_fbk_v[i].data(), 6*sizeof(double));
- memcpy(Qm_fbk_carr + 6*i, Qm_fbk_v[i].data(), 6*sizeof(double));
-
+ memcpy(Qp_fbk_carr + 3 * i, Qp_fbk_v[i].data(), 3 * sizeof(double));
+ memcpy(Qo_fbk_carr + 3 * i, Qo_fbk_v[i].data(), 3 * sizeof(double));
+ memcpy(Qv_fbk_carr + 3 * i, Qv_fbk_v[i].data(), 3 * sizeof(double));
+ memcpy(Qbi_fbk_carr + 6 * i, Qbi_fbk_v[i].data(), 6 * sizeof(double));
+ memcpy(Qm_fbk_carr + 6 * i, Qm_fbk_v[i].data(), 6 * sizeof(double));
// Factor errors
// std::cout << "Factors " << kf->getTimeStamp().get() << std::endl;
- FactorBasePtrList fac_lst;
+ FactorBasePtrList fac_lst;
kf->getFactorList(fac_lst);
- Vector9d imu_err = Vector9d::Zero();
+ Vector9d imu_err = Vector9d::Zero();
Vector6d pose_err = Vector6d::Zero();
i++;
}
-
// Save trajectories in npz file
- // save ground truth
- cnpy::npz_save(out_npz_file_path, "t", t_arr, {N}, "w"); // "w" overwrites any file with same name
- cnpy::npz_save(out_npz_file_path, "w_p_wm", w_p_wm_arr, {N,3}, "a"); // "a" appends to the file we created above
- cnpy::npz_save(out_npz_file_path, "w_q_m", w_q_m_arr, {N,4}, "a");
- cnpy::npz_save(out_npz_file_path, "w_v_wm", w_v_wm_arr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "m_v_wm", m_v_wm_arr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_i_imu", o_q_i_arr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "qa", qa_arr, {N,12}, "a");
- cnpy::npz_save(out_npz_file_path, "i_omg_oi", i_omg_oi_arr, {N,3}, "a");
+ // save ground truth
+ cnpy::npz_save(out_npz_file_path, "t", t_arr, {N}, "w"); // "w" overwrites any file with same name
+ cnpy::npz_save(out_npz_file_path, "w_p_wm", w_p_wm_arr, {N, 3}, "a"); // "a" appends to the file we created above
+ cnpy::npz_save(out_npz_file_path, "w_q_m", w_q_m_arr, {N, 4}, "a");
+ cnpy::npz_save(out_npz_file_path, "w_v_wm", w_v_wm_arr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "m_v_wm", m_v_wm_arr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i_imu", o_q_i_arr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "qa", qa_arr, {N, 12}, "a");
+ cnpy::npz_save(out_npz_file_path, "i_omg_oi", i_omg_oi_arr, {N, 3}, "a");
// Online estimates
- cnpy::npz_save(out_npz_file_path, "o_p_ob_fbk", o_p_ob_fbk_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_b_fbk", o_q_b_fbk_carr, {N,4}, "a");
- cnpy::npz_save(out_npz_file_path, "o_v_ob_fbk", o_v_ob_fbk_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_p_oi_fbk", o_p_oi_fbk_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_i_fbk", o_q_i_fbk_carr, {N,4}, "a");
- cnpy::npz_save(out_npz_file_path, "o_v_oi_fbk", o_v_oi_fbk_carr, {N,3}, "a");
-
+ cnpy::npz_save(out_npz_file_path, "o_p_ob_fbk", o_p_ob_fbk_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_b_fbk", o_q_b_fbk_carr, {N, 4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_ob_fbk", o_v_ob_fbk_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_p_oi_fbk", o_p_oi_fbk_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i_fbk", o_q_i_fbk_carr, {N, 4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_oi_fbk", o_v_oi_fbk_carr, {N, 3}, "a");
+
// offline states
- cnpy::npz_save(out_npz_file_path, "o_p_ob", o_p_ob_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_b", o_q_b_carr, {N,4}, "a");
- cnpy::npz_save(out_npz_file_path, "o_v_ob", o_v_ob_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_p_oi", o_p_oi_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_carr, {N,4}, "a");
- cnpy::npz_save(out_npz_file_path, "o_v_oi", o_v_oi_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_p_ob", o_p_ob_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_b", o_q_b_carr, {N, 4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_ob", o_v_ob_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_p_oi", o_p_oi_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_carr, {N, 4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_oi", o_v_oi_carr, {N, 3}, "a");
// and biases/extrinsics
- cnpy::npz_save(out_npz_file_path, "imu_bias_fbk", imu_bias_fbk_carr, {N,6}, "a");
- cnpy::npz_save(out_npz_file_path, "imu_bias", imu_bias_carr, {N,6}, "a");
- cnpy::npz_save(out_npz_file_path, "i_pose_im_fbk", i_pose_im_fbk_carr, {N,7}, "a");
+ cnpy::npz_save(out_npz_file_path, "imu_bias_fbk", imu_bias_fbk_carr, {N, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "imu_bias", imu_bias_carr, {N, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "i_pose_im_fbk", i_pose_im_fbk_carr, {N, 7}, "a");
// covariances
cnpy::npz_save(out_npz_file_path, "tkf", tkf_carr, {Nkf}, "a");
- cnpy::npz_save(out_npz_file_path, "Qp", Qp_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qo", Qo_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qv", Qv_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qbi", Qbi_carr, {Nkf,6}, "a");
- cnpy::npz_save(out_npz_file_path, "Qm", Qm_carr, {Nkf,6}, "a");
- cnpy::npz_save(out_npz_file_path, "Qp_fbk", Qp_fbk_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qo_fbk", Qo_fbk_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qv_fbk", Qv_fbk_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qbi_fbk", Qbi_fbk_carr, {Nkf,6}, "a");
- cnpy::npz_save(out_npz_file_path, "Qm_fbk", Qm_fbk_carr, {Nkf,6}, "a");
- cnpy::npz_save(out_npz_file_path, "fac_imu_err", fac_imu_err_carr, {Nkf,9}, "a");
- cnpy::npz_save(out_npz_file_path, "fac_pose_err", fac_pose_err_carr, {Nkf,6}, "a");
-
+ cnpy::npz_save(out_npz_file_path, "Qp", Qp_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qo", Qo_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qv", Qv_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qbi", Qbi_carr, {Nkf, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qm", Qm_carr, {Nkf, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qp_fbk", Qp_fbk_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qo_fbk", Qo_fbk_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qv_fbk", Qv_fbk_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qbi_fbk", Qbi_fbk_carr, {Nkf, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qm_fbk", Qm_fbk_carr, {Nkf, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "fac_imu_err", fac_imu_err_carr, {Nkf, 9}, "a");
+ cnpy::npz_save(out_npz_file_path, "fac_pose_err", fac_pose_err_carr, {Nkf, 6}, "a");
}
diff --git a/demos/solo_imu_kine_mocap.cpp b/demos/solo_imu_kine_mocap.cpp
index bc400c3..7c44588 100644
--- a/demos/solo_imu_kine_mocap.cpp
+++ b/demos/solo_imu_kine_mocap.cpp
@@ -78,20 +78,18 @@
#include "bodydynamics/sensor/sensor_point_feet_nomove.h"
#include "bodydynamics/processor/processor_point_feet_nomove.h"
-
-
-
using namespace Eigen;
using namespace wolf;
using namespace pinocchio;
-typedef pinocchio::SE3Tpl<double> SE3;
+typedef pinocchio::SE3Tpl<double> SE3;
typedef pinocchio::ForceTpl<double> Force;
-
-int main (int argc, char **argv) {
+int main(int argc, char** argv)
+{
// retrieve parameters from yaml file
- YAML::Node config = YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/solo_real_estimation.yaml");
+ YAML::Node config =
+ YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/solo_real_estimation.yaml");
double dt = config["dt"].as<double>();
// double min_t = config["min_t"].as<double>();
double max_t = config["max_t"].as<double>();
@@ -104,53 +102,52 @@ int main (int argc, char **argv) {
// int nb_feet = config["nb_feet"].as<int>();
// Ceres setup
- int max_num_iterations = config["max_num_iterations"].as<int>();
- bool compute_cov = config["compute_cov"].as<bool>();
-
+ int max_num_iterations = config["max_num_iterations"].as<int>();
+ bool compute_cov = config["compute_cov"].as<bool>();
+
// estimator sensor noises
// double std_pbc_est = config["std_pbc_est"].as<double>();
// double std_vbc_est = config["std_vbc_est"].as<double>();
// double std_f_est = config["std_f_est"].as<double>();
// double std_tau_est = config["std_tau_est"].as<double>();
double std_foot_nomove = config["std_foot_nomove"].as<double>();
-
+
// priors
double std_prior_p = config["std_prior_p"].as<double>();
double std_prior_o = config["std_prior_o"].as<double>();
double std_prior_v = config["std_prior_v"].as<double>();
// double std_abs_bias_pbc = config["std_abs_bias_pbc"].as<double>();
- double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
- double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
+ double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
+ double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
// double std_bp_drift = config["std_bp_drift"].as<double>();
- std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
+ std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
Eigen::Map<Vector6d> bias_imu_prior(bias_imu_prior_vec.data());
- std::vector<double> i_p_im_vec = config["i_p_im"].as<std::vector<double>>();
+ std::vector<double> i_p_im_vec = config["i_p_im"].as<std::vector<double>>();
Eigen::Map<Vector3d> i_p_im(i_p_im_vec.data());
- std::vector<double> i_q_m_vec = config["i_q_m"].as<std::vector<double>>();
+ std::vector<double> i_q_m_vec = config["i_q_m"].as<std::vector<double>>();
Eigen::Map<Vector4d> i_qvec_m(i_q_m_vec.data());
- std::vector<double> m_p_mb_vec = config["m_p_mb"].as<std::vector<double>>();
+ std::vector<double> m_p_mb_vec = config["m_p_mb"].as<std::vector<double>>();
Eigen::Map<Vector3d> m_p_mb(m_p_mb_vec.data());
- std::vector<double> m_q_b_vec = config["m_q_b"].as<std::vector<double>>();
+ std::vector<double> m_q_b_vec = config["m_q_b"].as<std::vector<double>>();
Eigen::Map<Vector4d> m_qvec_b(m_q_b_vec.data());
// contacts
- std::vector<double> l_p_lg_vec = config["l_p_lg"].as<std::vector<double>>();
+ std::vector<double> l_p_lg_vec = config["l_p_lg"].as<std::vector<double>>();
Eigen::Map<Vector3d> l_p_lg(l_p_lg_vec.data());
// double fz_thresh = config["fz_thresh"].as<double>();
- std::vector<double> delta_qa_vec = config["delta_qa"].as<std::vector<double>>();
- Eigen::Map<Eigen::Matrix<double,12,1>> delta_qa(delta_qa_vec.data());
+ std::vector<double> delta_qa_vec = config["delta_qa"].as<std::vector<double>>();
+ Eigen::Map<Eigen::Matrix<double, 12, 1>> delta_qa(delta_qa_vec.data());
// MOCAP
- double std_pose_p = config["std_pose_p"].as<double>();
- double std_pose_o_deg = config["std_pose_o_deg"].as<double>();
- double std_mocap_extr_p = config["std_mocap_extr_p"].as<double>();
- double std_mocap_extr_o_deg = config["std_mocap_extr_o_deg"].as<double>();
- bool unfix_extr_sensor_pose = config["unfix_extr_sensor_pose"].as<bool>();
- bool time_tolerance_mocap = config["time_tolerance_mocap"].as<double>();
- double time_shift_mocap = config["time_shift_mocap"].as<double>();
-
-
- std::string data_file_path = config["data_file_path"].as<std::string>();
+ double std_pose_p = config["std_pose_p"].as<double>();
+ double std_pose_o_deg = config["std_pose_o_deg"].as<double>();
+ double std_mocap_extr_p = config["std_mocap_extr_p"].as<double>();
+ double std_mocap_extr_o_deg = config["std_mocap_extr_o_deg"].as<double>();
+ bool unfix_extr_sensor_pose = config["unfix_extr_sensor_pose"].as<bool>();
+ bool time_tolerance_mocap = config["time_tolerance_mocap"].as<double>();
+ double time_shift_mocap = config["time_shift_mocap"].as<double>();
+
+ std::string data_file_path = config["data_file_path"].as<std::string>();
std::string out_npz_file_path = config["out_npz_file_path"].as<std::string>();
// MOCAP to IMU transform
@@ -161,9 +158,9 @@ int main (int argc, char **argv) {
// Matrix3d i_R_m = i_T_m.rotation();
// IMU to MOCAP transform
- SE3 m_T_i = i_T_m.inverse();
- Vector3d m_p_mi = m_T_i.translation();
- Quaterniond m_q_i = Quaterniond(m_T_i.rotation());
+ SE3 m_T_i = i_T_m.inverse();
+ Vector3d m_p_mi = m_T_i.translation();
+ Quaterniond m_q_i = Quaterniond(m_T_i.rotation());
// MOCAP to BASE transform
Quaterniond m_q_b(m_qvec_b);
@@ -173,72 +170,72 @@ int main (int argc, char **argv) {
// Matrix3d m_R_b = m_T_b.rotation();
// IMU to BASE transform (composition)
- SE3 i_T_b = i_T_m*m_T_b;
+ SE3 i_T_b = i_T_m * m_T_b;
Vector3d i_p_ib = i_T_b.translation();
- Matrix3d i_R_b = i_T_b.rotation();
-
-
+ Matrix3d i_R_b = i_T_b.rotation();
// initialize some vectors and fill them with dicretized data
cnpy::npz_t arr_map = cnpy::npz_load(data_file_path);
- //load it into a new array
- cnpy::NpyArray t_npyarr = arr_map["t"];
+ // load it into a new array
+ cnpy::NpyArray t_npyarr = arr_map["t"];
cnpy::NpyArray imu_acc_npyarr = arr_map["imu_acc"];
// cnpy::NpyArray o_a_oi_npyarr = arr_map["o_a_oi"];
- cnpy::NpyArray o_q_i_npyarr = arr_map["o_q_i"];
+ cnpy::NpyArray o_q_i_npyarr = arr_map["o_q_i"];
cnpy::NpyArray i_omg_oi_npyarr = arr_map["i_omg_oi"];
- cnpy::NpyArray qa_npyarr = arr_map["qa"];
- cnpy::NpyArray dqa_npyarr = arr_map["dqa"];
- cnpy::NpyArray tau_npyarr = arr_map["tau"];
+ cnpy::NpyArray qa_npyarr = arr_map["qa"];
+ cnpy::NpyArray dqa_npyarr = arr_map["dqa"];
+ cnpy::NpyArray tau_npyarr = arr_map["tau"];
cnpy::NpyArray l_forces_npyarr = arr_map["l_forces"];
// cnpy::NpyArray w_pose_wm_npyarr = arr_map["w_pose_wm"];
cnpy::NpyArray m_v_wm_npyarr = arr_map["m_v_wm"];
cnpy::NpyArray w_v_wm_npyarr = arr_map["w_v_wm"];
// cnpy::NpyArray o_R_i_npyarr = arr_map["o_R_i"];
- cnpy::NpyArray w_p_wm_npyarr = arr_map["w_p_wm"];
- cnpy::NpyArray w_q_m_npyarr = arr_map["w_q_m"];
+ cnpy::NpyArray w_p_wm_npyarr = arr_map["w_p_wm"];
+ cnpy::NpyArray w_q_m_npyarr = arr_map["w_q_m"];
cnpy::NpyArray contact_npyarr = arr_map["contacts"];
// cnpy::NpyArray b_p_bl_mocap_npyarr = arr_map["b_p_bl_mocap"];
// cnpy::NpyArray w_R_m_npyarr = arr_map["w_R_m"];
- double* t_arr = t_npyarr.data<double>();
+ double* t_arr = t_npyarr.data<double>();
double* imu_acc_arr = imu_acc_npyarr.data<double>();
// double* o_a_oi_arr = o_a_oi_npyarr.data<double>();
double* i_omg_oi_arr = i_omg_oi_npyarr.data<double>();
- double* qa_arr = qa_npyarr.data<double>();
- double* dqa_arr = dqa_npyarr.data<double>();
- double* tau_arr = tau_npyarr.data<double>();
+ double* qa_arr = qa_npyarr.data<double>();
+ double* dqa_arr = dqa_npyarr.data<double>();
+ double* tau_arr = tau_npyarr.data<double>();
double* l_forces_arr = l_forces_npyarr.data<double>();
// double* w_pose_wm_arr = w_pose_wm_npyarr.data<double>();
double* m_v_wm_arr = m_v_wm_npyarr.data<double>();
double* w_v_wm_arr = w_v_wm_npyarr.data<double>();
- double* o_q_i_arr = o_q_i_npyarr.data<double>();
+ double* o_q_i_arr = o_q_i_npyarr.data<double>();
// double* o_R_i_arr = o_R_i_npyarr.data<double>();
double* w_p_wm_arr = w_p_wm_npyarr.data<double>();
- double* w_q_m_arr = w_q_m_npyarr.data<double>();
-
+ double* w_q_m_arr = w_q_m_npyarr.data<double>();
+
double* contact_arr = contact_npyarr.data<double>();
// double* b_p_bl_mocap_arr = b_p_bl_mocap_npyarr.data<double>();
// double* w_R_m_arr = w_R_m_npyarr.data<double>();
unsigned int N = t_npyarr.shape[0];
- if (max_t > 0){
- N = N < int(max_t/dt) ? N : int(max_t/dt);
+ if (max_t > 0)
+ {
+ N = N < int(max_t / dt) ? N : int(max_t / dt);
}
// Load the urdf model
Model model;
pinocchio::urdf::buildModel(robot_urdf, JointModelFreeFlyer(), model);
std::cout << "model name: " << model.name << std::endl;
- Data data(model);
+ Data data(model);
std::vector<std::string> ee_names = {"FL_ANKLE", "FR_ANKLE", "HL_ANKLE", "HR_ANKLE"};
- unsigned int nbc = ee_names.size();
+ unsigned int nbc = ee_names.size();
// Recover end effector frame ids
std::vector<int> ee_ids;
std::cout << "Frame ids" << std::endl;
- for (auto ee_name: ee_names){
+ for (auto ee_name : ee_names)
+ {
ee_ids.push_back(model.getFrameId(ee_name));
std::cout << ee_name << std::endl;
}
@@ -250,156 +247,172 @@ int main (int argc, char **argv) {
ProblemPtr problem = Problem::create("POV", 3);
- // add a tree manager for sliding window optimization
- ParserYaml parser = ParserYaml("demos/tree_manager.yaml", bodydynamics_root_dir);
- ParamsServer server = ParamsServer(parser.getParams());
- auto tree_manager = TreeManagerSlidingWindow::create("tree_manager", server);
+ // add a tree manager for sliding window optimization
+ ParserYaml parser = ParserYaml("demos/tree_manager.yaml", bodydynamics_root_dir);
+ ParamsServer server = ParamsServer(parser.getParams());
+ auto tree_manager = TreeManagerSlidingWindow::create("tree_manager", server);
problem->setTreeManager(tree_manager);
//////////////////////
// CERES WRAPPER
- SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
+ SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
solver->getSolverOptions().max_num_iterations = max_num_iterations;
//===================================================== INITIALIZATION
// SENSOR + PROCESSOR IMU
- SensorBasePtr sen_imu_base = problem->installSensor("SensorImu", "SenImu", (Vector7d() << 0,0,0, 0,0,0,1).finished(), bodydynamics_root_dir + "/demos/sensor_imu_solo12.yaml");
- SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base);
- ProcessorBasePtr proc_ftp_base = problem->installProcessor("ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu_solo12.yaml");
+ SensorBasePtr sen_imu_base = problem->installSensor("SensorImu",
+ "SenImu",
+ (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(),
+ bodydynamics_root_dir + "/demos/sensor_imu_solo12.yaml");
+ SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base);
+ ProcessorBasePtr proc_ftp_base = problem->installProcessor(
+ "ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu_solo12.yaml");
ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_ftp_base);
// SENSOR + PROCESSOR POINT FEET NOMOVE
ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>();
- intr_pfn->std_foot_nomove_ = std_foot_nomove;
- VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
+ intr_pfn->std_foot_nomove_ = std_foot_nomove;
+ VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
SensorBasePtr sen_pfnm_base = problem->installSensor("SensorPointFeetNomove", "SenPfnm", extr, intr_pfn);
- SensorPointFeetNomovePtr sen_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base);
+ SensorPointFeetNomovePtr sen_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base);
ParamsProcessorPointFeetNomovePtr params_pfnm = std::make_shared<ParamsProcessorPointFeetNomove>();
- params_pfnm->voting_active = false;
- params_pfnm->time_tolerance = 0.0;
- params_pfnm->max_time_vote = 0.0;
- ProcessorBasePtr proc_pfnm_base = problem->installProcessor("ProcessorPointFeetNomove", "PPointFeetNomove", sen_pfnm, params_pfnm);
-
+ params_pfnm->voting_active = false;
+ params_pfnm->time_tolerance = 0.0;
+ params_pfnm->max_time_vote = 0.0;
+ ProcessorBasePtr proc_pfnm_base =
+ problem->installProcessor("ProcessorPointFeetNomove", "PPointFeetNomove", sen_pfnm, params_pfnm);
// SENSOR + PROCESSOR POSE (for mocap)
// pose sensor and proc (to get extrinsics in the prb)
- auto intr_sp = std::make_shared<ParamsSensorPose>();
+ auto intr_sp = std::make_shared<ParamsSensorPose>();
intr_sp->std_p = std_pose_p;
intr_sp->std_o = toRad(std_pose_o_deg);
- Vector7d extr_pose; extr_pose << i_p_im, i_q_m.coeffs();
- auto sensor_pose = problem->installSensor("SensorPose", "pose", extr_pose, intr_sp);
- auto params_proc = std::make_shared<ParamsProcessorPose>();
+ Vector7d extr_pose;
+ extr_pose << i_p_im, i_q_m.coeffs();
+ auto sensor_pose = problem->installSensor("SensorPose", "pose", extr_pose, intr_sp);
+ auto params_proc = std::make_shared<ParamsProcessorPose>();
params_proc->time_tolerance = time_tolerance_mocap;
- auto proc_pose = problem->installProcessor("ProcessorPose", "pose", sensor_pose, params_proc);
+ auto proc_pose = problem->installProcessor("ProcessorPose", "pose", sensor_pose, params_proc);
// somehow by default the sensor extrinsics is fixed
- Matrix3d cov_sp_p = pow(std_mocap_extr_p, 2)*Matrix3d::Identity();
- Matrix3d cov_sp_o = pow(std_mocap_extr_o_deg/60, 2)*Matrix3d::Identity();
+ Matrix3d cov_sp_p = pow(std_mocap_extr_p, 2) * Matrix3d::Identity();
+ Matrix3d cov_sp_o = pow(std_mocap_extr_o_deg / 60, 2) * Matrix3d::Identity();
sensor_pose->addPriorParameter('P', extr_pose.head<3>(), cov_sp_p);
sensor_pose->addPriorParameter('O', extr_pose.tail<4>(), cov_sp_o);
- if (unfix_extr_sensor_pose){
+ if (unfix_extr_sensor_pose)
+ {
sensor_pose->unfixExtrinsics();
}
- else{
+ else
+ {
sensor_pose->fixExtrinsics();
}
// SETPRIOR RETRO-ENGINEERING
// We are not using setPrior because of processors initial captures problems so we have to
// - Manually create the first KF and its pose and velocity priors
- // - call setOrigin on processors isMotion since the flag prior_is_set is not true when doing installProcessor (later)
+ // - call setOrigin on processors isMotion since the flag prior_is_set is not true when doing installProcessor
+ // (later)
//////////////////////
// COMPUTE INITIAL STATE
- TimeStamp t0(t_arr[0]);
+ TimeStamp t0(t_arr[0]);
Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr);
Eigen::Map<Vector4d> w_qvec_m_init(w_q_m_arr);
w_qvec_m_init.normalize(); // not close enough to wolf eps sometimes
- Quaterniond w_q_m_init(w_qvec_m_init);
- Vector3d w_p_wi_init = w_p_wm + w_q_m_init*m_p_mi;
- Quaterniond w_q_i_init = w_q_m_init*m_q_i;
+ Quaterniond w_q_m_init(w_qvec_m_init);
+ Vector3d w_p_wi_init = w_p_wm + w_q_m_init * m_p_mi;
+ Quaterniond w_q_i_init = w_q_m_init * m_q_i;
VectorComposite x_origin("POV", {w_p_wi_init, w_q_i_init.coeffs(), Vector3d::Zero()});
- VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()});
+ 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); // needed to anchor the problem
FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0); // if mocap used
-
+
proc_imu->setOrigin(KF1);
//////////////////////////////////////////
// INITIAL BIAS FACTORS
// Add absolute factor on Imu biases to simulate a fix()
- Vector6d std_abs_imu; std_abs_imu << std_abs_bias_acc*Vector3d::Ones(), std_abs_bias_gyro*Vector3d::Ones();
- Matrix6d Q_bi = std_abs_imu.array().square().matrix().asDiagonal();
- CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
+ Vector6d std_abs_imu;
+ std_abs_imu << std_abs_bias_acc * Vector3d::Ones(), std_abs_bias_gyro * Vector3d::Ones();
+ Matrix6d Q_bi = std_abs_imu.array().square().matrix().asDiagonal();
+ CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", bias_imu_prior, Q_bi);
- FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
+ FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(
+ featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
KF1->getCaptureOf(sen_imu)->getStateBlock('I')->setState(bias_imu_prior);
sen_imu->getStateBlock('I')->setState(bias_imu_prior);
-
// Allocate on the heap to prevent stack overflow for large datasets
- double* o_p_ob_fbk_carr = new double[3*N];
- double* o_q_b_fbk_carr = new double[4*N];
- double* o_v_ob_fbk_carr = new double[3*N];
- double* o_p_oi_fbk_carr = new double[3*N];
- double* o_q_i_fbk_carr = new double[4*N];
- double* o_v_oi_fbk_carr = new double[3*N];
- double* imu_bias_fbk_carr = new double[6*N];
- double* i_pose_im_fbk_carr = new double[7*N];
+ double* o_p_ob_fbk_carr = new double[3 * N];
+ double* o_q_b_fbk_carr = new double[4 * N];
+ double* o_v_ob_fbk_carr = new double[3 * N];
+ double* o_p_oi_fbk_carr = new double[3 * N];
+ double* o_q_i_fbk_carr = new double[4 * N];
+ double* o_v_oi_fbk_carr = new double[3 * N];
+ double* imu_bias_fbk_carr = new double[6 * N];
+ double* i_pose_im_fbk_carr = new double[7 * N];
// covariances computed on the fly
- std::vector<Vector3d> Qp_fbk_v; Qp_fbk_v.push_back(Vector3d::Zero());
- std::vector<Vector3d> Qo_fbk_v; Qo_fbk_v.push_back(Vector3d::Zero());
- std::vector<Vector3d> Qv_fbk_v; Qv_fbk_v.push_back(Vector3d::Zero());
- std::vector<Vector6d> Qbi_fbk_v; Qbi_fbk_v.push_back(Vector6d::Zero());
- std::vector<Vector6d> Qm_fbk_v; Qm_fbk_v.push_back(Vector6d::Zero());
-
-
- std::vector<Vector3d> i_omg_oi_v;
+ std::vector<Vector3d> Qp_fbk_v;
+ Qp_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector3d> Qo_fbk_v;
+ Qo_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector3d> Qv_fbk_v;
+ Qv_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector6d> Qbi_fbk_v;
+ Qbi_fbk_v.push_back(Vector6d::Zero());
+ std::vector<Vector6d> Qm_fbk_v;
+ Qm_fbk_v.push_back(Vector6d::Zero());
+
+ std::vector<Vector3d> i_omg_oi_v;
//////////////////////////////////////////
unsigned int nb_kf = 1;
- for (unsigned int i=0; i < N; i++){
+ for (unsigned int i = 0; i < N; i++)
+ {
TimeStamp ts(t_arr[i]);
////////////////////////////////////
// Retrieve current state
- VectorComposite curr_state = problem->getState();
- Vector4d o_qvec_i_curr = curr_state['O'];
- Quaterniond o_q_i_curr(o_qvec_i_curr);
- // Vector3d o_v_oi_curr = curr_state['V'];
-
+ VectorComposite curr_state = problem->getState();
+ Vector4d o_qvec_i_curr = curr_state['O'];
+ Quaterniond o_q_i_curr(o_qvec_i_curr);
+ // Vector3d o_v_oi_curr = curr_state['V'];
//////////////
// PROCESS IMU
//////////////
// Get measurements
// retrieve traj data
- Eigen::Map<Vector3d> imu_acc(imu_acc_arr+3*i);
- Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr+3*i);
+ Eigen::Map<Vector3d> imu_acc(imu_acc_arr + 3 * i);
+ Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr + 3 * i);
// store i_omg_oi for later computation of o_v_ob from o_v_oi
i_omg_oi_v.push_back(i_omg_oi);
- Vector6d imu_bias = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
+ Vector6d imu_bias = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>());
- Vector6d acc_gyr_meas; acc_gyr_meas << imu_acc, i_omg_oi;
+ Vector6d acc_gyr_meas;
+ acc_gyr_meas << imu_acc, i_omg_oi;
CaptureImuPtr CImu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, sen_imu->getNoiseCov());
CImu->process();
//////////////
//////////////
-
////////////////
// PROCESS MOCAP
////////////////
// Get measurements
// retrieve traj data
- Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr+3*i);
- Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr+4*i);
+ Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr + 3 * i);
+ Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr + 4 * i);
w_qvec_wm.normalize(); // not close enough to wolf eps sometimes
- Vector7d pose_meas; pose_meas << w_p_wm, w_qvec_wm;
- CapturePosePtr CP = std::make_shared<CapturePose>(ts+time_shift_mocap, sensor_pose, pose_meas, sensor_pose->getNoiseCov());
+ Vector7d pose_meas;
+ pose_meas << w_p_wm, w_qvec_wm;
+ CapturePosePtr CP =
+ std::make_shared<CapturePose>(ts + time_shift_mocap, sensor_pose, pose_meas, sensor_pose->getNoiseCov());
CP->process();
////////////////
////////////////
@@ -407,40 +420,43 @@ int main (int argc, char **argv) {
//////////////////////////////////
// Kinematics + forces
// retrieve traj data
- Eigen::Map<Matrix<double,12, 1>> tau(tau_arr+12*i);
- Eigen::Map<Matrix<double,12, 1>> qa(qa_arr+12*i);
- Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr+12*i);
- Eigen::Map<Matrix<double,4, 1>> contact_gtr(contact_arr+4*i); // int conversion does not work!
+ Eigen::Map<Matrix<double, 12, 1>> tau(tau_arr + 12 * i);
+ Eigen::Map<Matrix<double, 12, 1>> qa(qa_arr + 12 * i);
+ Eigen::Map<Matrix<double, 12, 1>> dqa(dqa_arr + 12 * i);
+ Eigen::Map<Matrix<double, 4, 1>> contact_gtr(contact_arr + 4 * i); // int conversion does not work!
// Eigen::Map<Matrix<double,12, 1>> b_p_bl_mocap(b_p_bl_mocap_arr+12*i); // int conversion does not work!
-
+
qa = qa + delta_qa;
Matrix3d o_R_i = Quaterniond(o_q_i_curr).toRotationMatrix(); // IMU internal filter
// Or else, retrieve from preprocessed dataset
- Eigen::Map<Matrix<double,12, 1>> l_forces(l_forces_arr+12*i);
+ Eigen::Map<Matrix<double, 12, 1>> l_forces(l_forces_arr + 12 * i);
std::vector<Vector3d> l_f_l_vec;
std::vector<Vector3d> o_f_l_vec;
std::vector<Vector3d> i_p_il_vec;
std::vector<Vector4d> i_qvec_l_vec;
// needing relative kinematics measurements
- VectorXd q_static(19); q_static << 0,0,0, 0,0,0,1, qa;
- VectorXd dq_static(18); dq_static << 0,0,0, 0,0,0, dqa;
+ VectorXd q_static(19);
+ q_static << 0, 0, 0, 0, 0, 0, 1, qa;
+ VectorXd dq_static(18);
+ dq_static << 0, 0, 0, 0, 0, 0, dqa;
forwardKinematics(model, data, q_static, dq_static);
updateFramePlacements(model, data);
- for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
+ for (unsigned int i_ee = 0; i_ee < nbc; i_ee++)
+ {
auto b_T_l = data.oMf[ee_ids[i_ee]];
- auto i_T_l = i_T_b * b_T_l;
- Vector3d i_p_il = i_T_l.translation();
- Matrix3d i_R_l = i_T_l.rotation();
+ auto i_T_l = i_T_b * b_T_l;
+ Vector3d i_p_il = i_T_l.translation();
+ Matrix3d i_R_l = i_T_l.rotation();
Vector4d i_qvec_l = Quaterniond(i_R_l).coeffs();
- i_p_il = i_p_il + i_R_l*l_p_lg;
+ i_p_il = i_p_il + i_R_l * l_p_lg;
- Vector3d l_f_l = l_forces.segment(3*i_ee, 3);
- Vector3d o_f_l = o_R_i*i_R_l * l_f_l; // for contact test
+ Vector3d l_f_l = l_forces.segment(3 * i_ee, 3);
+ Vector3d o_f_l = o_R_i * i_R_l * l_f_l; // for contact test
l_f_l_vec.push_back(l_f_l);
o_f_l_vec.push_back(o_f_l);
@@ -449,16 +465,19 @@ int main (int argc, char **argv) {
}
// Detect feet in contact
- int nb_feet_in_contact = 0;
+ int nb_feet_in_contact = 0;
std::unordered_map<int, Vector7d> kin_incontact;
// std::cout << "" << std::endl;
- for (unsigned int i_ee=0; i_ee<nbc; i_ee++){
+ for (unsigned int i_ee = 0; i_ee < nbc; i_ee++)
+ {
// basic contact detection based on normal foot vel, things break if no foot is in contact
// if ((o_f_l_vec[i_ee](2) > fz_thresh) && (nb_feet_in_contact < nb_feet)){
// if ((contact_gtr[i_ee] > 0.5) && (nb_feet_in_contact < nb_feet)){
- if (contact_gtr[i_ee] > 0.5){
+ if (contact_gtr[i_ee] > 0.5)
+ {
nb_feet_in_contact++;
- Vector7d i_pose_il; i_pose_il << i_p_il_vec[i_ee], i_qvec_l_vec[i_ee];
+ Vector7d i_pose_il;
+ i_pose_il << i_p_il_vec[i_ee], i_qvec_l_vec[i_ee];
kin_incontact.insert({i_ee, i_pose_il});
}
}
@@ -467,7 +486,8 @@ int main (int argc, char **argv) {
CPF->process();
// solve every new KF
- if (problem->getTrajectory()->getFrameMap().size() > nb_kf ){
+ if (problem->getTrajectory()->getFrameMap().size() > nb_kf)
+ {
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
std::cout << ts << " ";
std::cout << report << std::endl;
@@ -475,36 +495,41 @@ int main (int argc, char **argv) {
auto kf_last = problem->getTrajectory()->getFrameMap().rbegin()->second;
// compute covariances of KF and capture stateblocks
- Eigen::MatrixXd Qp_fbk = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd Qo_fbk = Eigen::Matrix3d::Identity(); // global or local?
- Eigen::MatrixXd Qv_fbk = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qp_fbk = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qo_fbk = Eigen::Matrix3d::Identity(); // global or local?
+ Eigen::MatrixXd Qv_fbk = Eigen::Matrix3d::Identity();
Eigen::MatrixXd Qbi_fbk = Eigen::Matrix6d::Identity();
- Eigen::MatrixXd Qmp_fbk = Eigen::Matrix3d::Identity(); // extr mocap
- Eigen::MatrixXd Qmo_fbk = Eigen::Matrix3d::Identity(); // extr mocap
-
+ Eigen::MatrixXd Qmp_fbk = Eigen::Matrix3d::Identity(); // extr mocap
+ Eigen::MatrixXd Qmo_fbk = Eigen::Matrix3d::Identity(); // extr mocap
+
if (compute_cov)
- solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+ solver->computeCovariances(
+ SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see
+ // below
problem->getCovarianceBlock(kf_last->getStateBlock('P'), Qp_fbk);
problem->getCovarianceBlock(kf_last->getStateBlock('O'), Qo_fbk);
problem->getCovarianceBlock(kf_last->getStateBlock('V'), Qv_fbk);
- CaptureBasePtr cap_imu = kf_last->getCaptureOf(sen_imu);
+ CaptureBasePtr cap_imu = kf_last->getCaptureOf(sen_imu);
if (compute_cov)
- solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
+ solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other
+ // computed covs -> issue to raise
problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi_fbk);
std::vector<StateBlockPtr> extr_sb_vec = {sensor_pose->getP(), sensor_pose->getO()};
if (compute_cov)
- solver->computeCovariances(extr_sb_vec); // not computed by ALL and destroys other computed covs -> issue to raise
+ solver->computeCovariances(
+ extr_sb_vec); // not computed by ALL and destroys other computed covs -> issue to raise
problem->getCovarianceBlock(sensor_pose->getP(), Qmp_fbk);
problem->getCovarianceBlock(sensor_pose->getO(), Qmo_fbk);
// Retrieve diagonals
- Vector3d Qp_fbk_diag = Qp_fbk.diagonal();
- Vector3d Qo_fbk_diag = Qo_fbk.diagonal();
- Vector3d Qv_fbk_diag = Qv_fbk.diagonal();
- Vector6d Qbi_fbk_diag = Qbi_fbk.diagonal();
- Vector6d Qm_fbk_diag; Qm_fbk_diag << Qmp_fbk.diagonal(), Qmo_fbk.diagonal();
+ Vector3d Qp_fbk_diag = Qp_fbk.diagonal();
+ Vector3d Qo_fbk_diag = Qo_fbk.diagonal();
+ Vector3d Qv_fbk_diag = Qv_fbk.diagonal();
+ Vector6d Qbi_fbk_diag = Qbi_fbk.diagonal();
+ Vector6d Qm_fbk_diag;
+ Qm_fbk_diag << Qmp_fbk.diagonal(), Qmo_fbk.diagonal();
Qp_fbk_v.push_back(Qp_fbk_diag);
Qo_fbk_v.push_back(Qo_fbk_diag);
@@ -517,200 +542,202 @@ int main (int argc, char **argv) {
// Store current state estimation
VectorComposite state_fbk = problem->getState(ts);
- Vector3d o_p_oi = state_fbk['P'];
- Vector4d o_qvec_i = state_fbk['O'];
- Vector3d o_v_oi = state_fbk['V'];
+ Vector3d o_p_oi = state_fbk['P'];
+ Vector4d o_qvec_i = state_fbk['O'];
+ Vector3d o_v_oi = state_fbk['V'];
- // IMU to base frame
- o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
- Matrix3d o_R_b = o_R_i * i_R_b;
+ // IMU to base frame
+ o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+ Matrix3d o_R_b = o_R_i * i_R_b;
Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
- Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
- Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_p_ib);
+ Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
+ Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_p_ib);
imu_bias = sen_imu->getIntrinsic()->getState();
- Vector7d i_pose_im_fbk; i_pose_im_fbk << sensor_pose->getP()->getState(), sensor_pose->getO()->getState();
-
- mempcpy(o_p_ob_fbk_carr+3*i, o_p_ob.data(), 3*sizeof(double));
- mempcpy(o_q_b_fbk_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
- mempcpy(o_v_ob_fbk_carr+3*i, o_v_ob.data(), 3*sizeof(double));
- mempcpy(o_p_oi_fbk_carr+3*i, o_p_oi.data(), 3*sizeof(double));
- mempcpy(o_q_i_fbk_carr +4*i, o_qvec_i.data(), 4*sizeof(double));
- mempcpy(o_v_oi_fbk_carr+3*i, o_v_oi.data(), 3*sizeof(double));
- mempcpy(imu_bias_fbk_carr+6*i, imu_bias.data(), 6*sizeof(double));
- mempcpy(i_pose_im_fbk_carr+7*i, i_pose_im_fbk.data(), 7*sizeof(double));
+ Vector7d i_pose_im_fbk;
+ i_pose_im_fbk << sensor_pose->getP()->getState(), sensor_pose->getO()->getState();
+
+ mempcpy(o_p_ob_fbk_carr + 3 * i, o_p_ob.data(), 3 * sizeof(double));
+ mempcpy(o_q_b_fbk_carr + 4 * i, o_qvec_b.data(), 4 * sizeof(double));
+ mempcpy(o_v_ob_fbk_carr + 3 * i, o_v_ob.data(), 3 * sizeof(double));
+ mempcpy(o_p_oi_fbk_carr + 3 * i, o_p_oi.data(), 3 * sizeof(double));
+ mempcpy(o_q_i_fbk_carr + 4 * i, o_qvec_i.data(), 4 * sizeof(double));
+ mempcpy(o_v_oi_fbk_carr + 3 * i, o_v_oi.data(), 3 * sizeof(double));
+ mempcpy(imu_bias_fbk_carr + 6 * i, imu_bias.data(), 6 * sizeof(double));
+ mempcpy(i_pose_im_fbk_carr + 7 * i, i_pose_im_fbk.data(), 7 * sizeof(double));
}
-
-
///////////////////////////////////////////
//////////////// SOLVING //////////////////
///////////////////////////////////////////
- if (solve_end){
+ if (solve_end)
+ {
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem->print(4,true,true,true);
+ problem->print(4, true, true, true);
std::cout << report << std::endl;
}
- double* o_p_ob_carr = new double[3*N];
- double* o_q_b_carr = new double[4*N];
- double* o_v_ob_carr = new double[3*N];
- double* o_p_oi_carr = new double[3*N];
- double* o_q_i_carr = new double[4*N];
- double* o_v_oi_carr = new double[3*N];
- double* imu_bias_carr = new double[6*N];
+ double* o_p_ob_carr = new double[3 * N];
+ double* o_q_b_carr = new double[4 * N];
+ double* o_v_ob_carr = new double[3 * N];
+ double* o_p_oi_carr = new double[3 * N];
+ double* o_q_i_carr = new double[4 * N];
+ double* o_v_oi_carr = new double[3 * N];
+ double* imu_bias_carr = new double[6 * N];
- for (unsigned int i=0; i < N; i++) {
+ for (unsigned int i = 0; i < N; i++)
+ {
VectorComposite state_est = problem->getState(t_arr[i]);
- Vector3d i_omg_oi = i_omg_oi_v[i];
- Vector3d o_p_oi = state_est['P'];
- Vector4d o_qvec_i = state_est['O'];
- Vector3d o_v_oi = state_est['V'];
+ Vector3d i_omg_oi = i_omg_oi_v[i];
+ Vector3d o_p_oi = state_est['P'];
+ Vector4d o_qvec_i = state_est['O'];
+ Vector3d o_v_oi = state_est['V'];
- auto sb = sen_imu->getStateBlockDynamic('I', t_arr[i]);
- Vector6d imu_bias = sb->getState();
+ auto sb = sen_imu->getStateBlockDynamic('I', t_arr[i]);
+ Vector6d imu_bias = sb->getState();
Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>());
- Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
- Matrix3d o_R_b = o_R_i * i_R_b;
+ Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+ Matrix3d o_R_b = o_R_i * i_R_b;
Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
- Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
- Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_p_ib);
-
- mempcpy(o_p_ob_carr+3*i, o_p_ob.data(), 3*sizeof(double));
- mempcpy(o_q_b_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
- mempcpy(o_v_ob_carr+3*i, o_v_ob.data(), 3*sizeof(double));
- mempcpy(o_p_oi_carr+3*i, o_p_oi.data(), 3*sizeof(double));
- mempcpy(o_q_i_carr +4*i, o_qvec_i.data(), 4*sizeof(double));
- mempcpy(o_v_oi_carr+3*i, o_v_oi.data(), 3*sizeof(double));
- mempcpy(imu_bias_carr+6*i, imu_bias.data(), 6*sizeof(double));
+ Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
+ Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_p_ib);
+
+ mempcpy(o_p_ob_carr + 3 * i, o_p_ob.data(), 3 * sizeof(double));
+ mempcpy(o_q_b_carr + 4 * i, o_qvec_b.data(), 4 * sizeof(double));
+ mempcpy(o_v_ob_carr + 3 * i, o_v_ob.data(), 3 * sizeof(double));
+ mempcpy(o_p_oi_carr + 3 * i, o_p_oi.data(), 3 * sizeof(double));
+ mempcpy(o_q_i_carr + 4 * i, o_qvec_i.data(), 4 * sizeof(double));
+ mempcpy(o_v_oi_carr + 3 * i, o_v_oi.data(), 3 * sizeof(double));
+ mempcpy(imu_bias_carr + 6 * i, imu_bias.data(), 6 * sizeof(double));
}
-
// Compute Covariances
- unsigned int Nkf = problem->getTrajectory()->getFrameMap().size();
- double* tkf_carr = new double[1*Nkf];
- double* Qp_carr = new double[3*Nkf];
- double* Qo_carr = new double[3*Nkf];
- double* Qv_carr = new double[3*Nkf];
- double* Qbi_carr = new double[6*Nkf];
- double* Qm_carr = new double[6*Nkf];
+ unsigned int Nkf = problem->getTrajectory()->getFrameMap().size();
+ double* tkf_carr = new double[1 * Nkf];
+ double* Qp_carr = new double[3 * Nkf];
+ double* Qo_carr = new double[3 * Nkf];
+ double* Qv_carr = new double[3 * Nkf];
+ double* Qbi_carr = new double[6 * Nkf];
+ double* Qm_carr = new double[6 * Nkf];
// feedback covariances
- double* Qp_fbk_carr = new double[3*Nkf];
- double* Qo_fbk_carr = new double[3*Nkf];
- double* Qv_fbk_carr = new double[3*Nkf];
- double* Qbi_fbk_carr = new double[6*Nkf];
- double* Qm_fbk_carr = new double[6*Nkf];
+ double* Qp_fbk_carr = new double[3 * Nkf];
+ double* Qo_fbk_carr = new double[3 * Nkf];
+ double* Qv_fbk_carr = new double[3 * Nkf];
+ double* Qbi_fbk_carr = new double[6 * Nkf];
+ double* Qm_fbk_carr = new double[6 * Nkf];
// factor errors
- double* fac_imu_err_carr = new double[9*Nkf];
- double* fac_pose_err_carr = new double[6*Nkf];
- int i = 0;
- for (auto& elt: problem->getTrajectory()->getFrameMap()){
+ double* fac_imu_err_carr = new double[9 * Nkf];
+ double* fac_pose_err_carr = new double[6 * Nkf];
+ int i = 0;
+ for (auto& elt : problem->getTrajectory()->getFrameMap())
+ {
std::cout << "Traj " << i << "/" << problem->getTrajectory()->getFrameMap().size() << std::endl;
- tkf_carr[i] = elt.first.get();
- auto kf = elt.second;
+ tkf_carr[i] = elt.first.get();
+ auto kf = elt.second;
VectorComposite kf_state = kf->getState();
-
+
// compute covariances of KF and capture stateblocks
- Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local?
- Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local?
+ Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity();
Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity();
- Eigen::MatrixXd Qmp = Eigen::Matrix3d::Identity(); // extr mocap
- Eigen::MatrixXd Qmo = Eigen::Matrix3d::Identity(); // extr mocap
-
+ Eigen::MatrixXd Qmp = Eigen::Matrix3d::Identity(); // extr mocap
+ Eigen::MatrixXd Qmo = Eigen::Matrix3d::Identity(); // extr mocap
+
if (compute_cov)
- solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+ solver->computeCovariances(
+ SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
- CaptureBasePtr cap_imu = kf->getCaptureOf(sen_imu);
+ CaptureBasePtr cap_imu = kf->getCaptureOf(sen_imu);
if (compute_cov)
- solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
+ solver->computeCovariances(
+ cap_imu
+ ->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
// diagonal components
- Vector3d Qp_diag = Qp.diagonal();
- Vector3d Qo_diag = Qo.diagonal();
- Vector3d Qv_diag = Qv.diagonal();
- Vector6d Qbi_diag = Qbi.diagonal();
- Vector6d Qm_diag; Qm_diag << Qmp.diagonal(), Qmo.diagonal();
-
- memcpy(Qp_carr + 3*i, Qp_diag.data(), 3*sizeof(double));
- memcpy(Qo_carr + 3*i, Qo_diag.data(), 3*sizeof(double));
- memcpy(Qv_carr + 3*i, Qv_diag.data(), 3*sizeof(double));
- memcpy(Qbi_carr + 6*i, Qbi_diag.data(), 6*sizeof(double));
- memcpy(Qm_carr + 6*i, Qm_diag.data(), 6*sizeof(double));
+ Vector3d Qp_diag = Qp.diagonal();
+ Vector3d Qo_diag = Qo.diagonal();
+ Vector3d Qv_diag = Qv.diagonal();
+ Vector6d Qbi_diag = Qbi.diagonal();
+ Vector6d Qm_diag;
+ Qm_diag << Qmp.diagonal(), Qmo.diagonal();
+
+ memcpy(Qp_carr + 3 * i, Qp_diag.data(), 3 * sizeof(double));
+ memcpy(Qo_carr + 3 * i, Qo_diag.data(), 3 * sizeof(double));
+ memcpy(Qv_carr + 3 * i, Qv_diag.data(), 3 * sizeof(double));
+ memcpy(Qbi_carr + 6 * i, Qbi_diag.data(), 6 * sizeof(double));
+ memcpy(Qm_carr + 6 * i, Qm_diag.data(), 6 * sizeof(double));
// Recover feedback covariances
- memcpy(Qp_fbk_carr + 3*i, Qp_fbk_v[i].data(), 3*sizeof(double));
- memcpy(Qo_fbk_carr + 3*i, Qo_fbk_v[i].data(), 3*sizeof(double));
- memcpy(Qv_fbk_carr + 3*i, Qv_fbk_v[i].data(), 3*sizeof(double));
- memcpy(Qbi_fbk_carr + 6*i, Qbi_fbk_v[i].data(), 6*sizeof(double));
- memcpy(Qm_fbk_carr + 6*i, Qm_fbk_v[i].data(), 6*sizeof(double));
-
+ memcpy(Qp_fbk_carr + 3 * i, Qp_fbk_v[i].data(), 3 * sizeof(double));
+ memcpy(Qo_fbk_carr + 3 * i, Qo_fbk_v[i].data(), 3 * sizeof(double));
+ memcpy(Qv_fbk_carr + 3 * i, Qv_fbk_v[i].data(), 3 * sizeof(double));
+ memcpy(Qbi_fbk_carr + 6 * i, Qbi_fbk_v[i].data(), 6 * sizeof(double));
+ memcpy(Qm_fbk_carr + 6 * i, Qm_fbk_v[i].data(), 6 * sizeof(double));
// Factor errors
// std::cout << "Factors " << kf->getTimeStamp().get() << std::endl;
- FactorBasePtrList fac_lst;
+ FactorBasePtrList fac_lst;
kf->getFactorList(fac_lst);
i++;
}
-
// Save trajectories in npz file
- // save ground truth
- cnpy::npz_save(out_npz_file_path, "t", t_arr, {N}, "w"); // "w" overwrites any file with same name
- cnpy::npz_save(out_npz_file_path, "w_p_wm", w_p_wm_arr, {N,3}, "a"); // "a" appends to the file we created above
- cnpy::npz_save(out_npz_file_path, "w_q_m", w_q_m_arr, {N,4}, "a");
- cnpy::npz_save(out_npz_file_path, "w_v_wm", w_v_wm_arr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "m_v_wm", m_v_wm_arr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_i_imu", o_q_i_arr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "qa", qa_arr, {N,12}, "a");
- cnpy::npz_save(out_npz_file_path, "i_omg_oi", i_omg_oi_arr, {N,3}, "a");
+ // save ground truth
+ cnpy::npz_save(out_npz_file_path, "t", t_arr, {N}, "w"); // "w" overwrites any file with same name
+ cnpy::npz_save(out_npz_file_path, "w_p_wm", w_p_wm_arr, {N, 3}, "a"); // "a" appends to the file we created above
+ cnpy::npz_save(out_npz_file_path, "w_q_m", w_q_m_arr, {N, 4}, "a");
+ cnpy::npz_save(out_npz_file_path, "w_v_wm", w_v_wm_arr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "m_v_wm", m_v_wm_arr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i_imu", o_q_i_arr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "qa", qa_arr, {N, 12}, "a");
+ cnpy::npz_save(out_npz_file_path, "i_omg_oi", i_omg_oi_arr, {N, 3}, "a");
// Online estimates
- cnpy::npz_save(out_npz_file_path, "o_p_ob_fbk", o_p_ob_fbk_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_b_fbk", o_q_b_fbk_carr, {N,4}, "a");
- cnpy::npz_save(out_npz_file_path, "o_v_ob_fbk", o_v_ob_fbk_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_p_oi_fbk", o_p_oi_fbk_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_i_fbk", o_q_i_fbk_carr, {N,4}, "a");
- cnpy::npz_save(out_npz_file_path, "o_v_oi_fbk", o_v_oi_fbk_carr, {N,3}, "a");
-
+ cnpy::npz_save(out_npz_file_path, "o_p_ob_fbk", o_p_ob_fbk_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_b_fbk", o_q_b_fbk_carr, {N, 4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_ob_fbk", o_v_ob_fbk_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_p_oi_fbk", o_p_oi_fbk_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i_fbk", o_q_i_fbk_carr, {N, 4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_oi_fbk", o_v_oi_fbk_carr, {N, 3}, "a");
+
// offline states
- cnpy::npz_save(out_npz_file_path, "o_p_ob", o_p_ob_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_b", o_q_b_carr, {N,4}, "a");
- cnpy::npz_save(out_npz_file_path, "o_v_ob", o_v_ob_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_p_oi", o_p_oi_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_carr, {N,4}, "a");
- cnpy::npz_save(out_npz_file_path, "o_v_oi", o_v_oi_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_p_ob", o_p_ob_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_b", o_q_b_carr, {N, 4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_ob", o_v_ob_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_p_oi", o_p_oi_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_carr, {N, 4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_oi", o_v_oi_carr, {N, 3}, "a");
// imu states
- cnpy::npz_save(out_npz_file_path, "o_p_oi", o_p_oi_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_carr, {N,4}, "a");
- cnpy::npz_save(out_npz_file_path, "o_v_oi", o_v_oi_carr, {N,3}, "a");
-
+ cnpy::npz_save(out_npz_file_path, "o_p_oi", o_p_oi_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_carr, {N, 4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_oi", o_v_oi_carr, {N, 3}, "a");
+
// and biases/extrinsics
- cnpy::npz_save(out_npz_file_path, "imu_bias_fbk", imu_bias_fbk_carr, {N,6}, "a");
- cnpy::npz_save(out_npz_file_path, "imu_bias", imu_bias_carr, {N,6}, "a");
- cnpy::npz_save(out_npz_file_path, "i_pose_im_fbk", i_pose_im_fbk_carr, {N,7}, "a");
+ cnpy::npz_save(out_npz_file_path, "imu_bias_fbk", imu_bias_fbk_carr, {N, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "imu_bias", imu_bias_carr, {N, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "i_pose_im_fbk", i_pose_im_fbk_carr, {N, 7}, "a");
// covariances
cnpy::npz_save(out_npz_file_path, "tkf", tkf_carr, {Nkf}, "a");
- cnpy::npz_save(out_npz_file_path, "Qp", Qp_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qo", Qo_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qv", Qv_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qbi", Qbi_carr, {Nkf,6}, "a");
- cnpy::npz_save(out_npz_file_path, "Qm", Qm_carr, {Nkf,6}, "a");
- cnpy::npz_save(out_npz_file_path, "Qp_fbk", Qp_fbk_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qo_fbk", Qo_fbk_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qv_fbk", Qv_fbk_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qbi_fbk", Qbi_fbk_carr, {Nkf,6}, "a");
- cnpy::npz_save(out_npz_file_path, "Qm_fbk", Qm_fbk_carr, {Nkf,6}, "a");
- cnpy::npz_save(out_npz_file_path, "fac_imu_err", fac_imu_err_carr, {Nkf,9}, "a");
- cnpy::npz_save(out_npz_file_path, "fac_pose_err", fac_pose_err_carr, {Nkf,6}, "a");
-
+ cnpy::npz_save(out_npz_file_path, "Qp", Qp_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qo", Qo_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qv", Qv_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qbi", Qbi_carr, {Nkf, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qm", Qm_carr, {Nkf, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qp_fbk", Qp_fbk_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qo_fbk", Qo_fbk_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qv_fbk", Qv_fbk_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qbi_fbk", Qbi_fbk_carr, {Nkf, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qm_fbk", Qm_fbk_carr, {Nkf, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "fac_imu_err", fac_imu_err_carr, {Nkf, 9}, "a");
+ cnpy::npz_save(out_npz_file_path, "fac_pose_err", fac_pose_err_carr, {Nkf, 6}, "a");
}
diff --git a/demos/solo_imu_mocap.cpp b/demos/solo_imu_mocap.cpp
index 9092155..9dc58a9 100644
--- a/demos/solo_imu_mocap.cpp
+++ b/demos/solo_imu_mocap.cpp
@@ -65,68 +65,66 @@
// BODYDYNAMICS
#include "bodydynamics/internal/config.h"
-
-
using namespace Eigen;
using namespace wolf;
using namespace pinocchio;
-typedef pinocchio::SE3Tpl<double> SE3;
+typedef pinocchio::SE3Tpl<double> SE3;
typedef pinocchio::ForceTpl<double> Force;
-
-int main (int argc, char **argv) {
+int main(int argc, char** argv)
+{
// retrieve parameters from yaml file
- YAML::Node config = YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/solo_real_estimation.yaml");
- double dt = config["dt"].as<double>();
- double max_t = config["max_t"].as<double>();
- bool solve_end = config["solve_end"].as<bool>();
+ YAML::Node config =
+ YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/solo_real_estimation.yaml");
+ double dt = config["dt"].as<double>();
+ double max_t = config["max_t"].as<double>();
+ bool solve_end = config["solve_end"].as<bool>();
// Ceres setup
- int max_num_iterations = config["max_num_iterations"].as<int>();
- bool compute_cov = config["compute_cov"].as<bool>();
-
+ int max_num_iterations = config["max_num_iterations"].as<int>();
+ bool compute_cov = config["compute_cov"].as<bool>();
+
// priors
- double std_prior_p = config["std_prior_p"].as<double>();
- double std_prior_o = config["std_prior_o"].as<double>();
- double std_prior_v = config["std_prior_v"].as<double>();
- double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
- double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
- std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
+ double std_prior_p = config["std_prior_p"].as<double>();
+ double std_prior_o = config["std_prior_o"].as<double>();
+ double std_prior_v = config["std_prior_v"].as<double>();
+ double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
+ double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
+ std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
Eigen::Map<Vector6d> bias_imu_prior(bias_imu_prior_vec.data());
- std::vector<double> i_p_im_vec = config["i_p_im"].as<std::vector<double>>();
+ std::vector<double> i_p_im_vec = config["i_p_im"].as<std::vector<double>>();
Eigen::Map<Vector3d> i_p_im(i_p_im_vec.data());
- std::vector<double> i_q_m_vec = config["i_q_m"].as<std::vector<double>>();
+ std::vector<double> i_q_m_vec = config["i_q_m"].as<std::vector<double>>();
Eigen::Map<Vector4d> i_qvec_m(i_q_m_vec.data());
- std::vector<double> m_p_mb_vec = config["m_p_mb"].as<std::vector<double>>();
+ std::vector<double> m_p_mb_vec = config["m_p_mb"].as<std::vector<double>>();
Eigen::Map<Vector3d> m_p_mb(m_p_mb_vec.data());
- std::vector<double> m_q_b_vec = config["m_q_b"].as<std::vector<double>>();
+ std::vector<double> m_q_b_vec = config["m_q_b"].as<std::vector<double>>();
Eigen::Map<Vector4d> m_qvec_b(m_q_b_vec.data());
// MOCAP
- double std_pose_p = config["std_pose_p"].as<double>();
- double std_pose_o_deg = config["std_pose_o_deg"].as<double>();
- double std_mocap_extr_p = config["std_mocap_extr_p"].as<double>();
- double std_mocap_extr_o_deg = config["std_mocap_extr_o_deg"].as<double>();
- bool unfix_extr_sensor_pose = config["unfix_extr_sensor_pose"].as<bool>();
- bool time_tolerance_mocap = config["time_tolerance_mocap"].as<double>();
- double time_shift_mocap = config["time_shift_mocap"].as<double>();
-
-
- std::string data_file_path = config["data_file_path"].as<std::string>();
+ double std_pose_p = config["std_pose_p"].as<double>();
+ double std_pose_o_deg = config["std_pose_o_deg"].as<double>();
+ double std_mocap_extr_p = config["std_mocap_extr_p"].as<double>();
+ double std_mocap_extr_o_deg = config["std_mocap_extr_o_deg"].as<double>();
+ bool unfix_extr_sensor_pose = config["unfix_extr_sensor_pose"].as<bool>();
+ bool time_tolerance_mocap = config["time_tolerance_mocap"].as<double>();
+ double time_shift_mocap = config["time_shift_mocap"].as<double>();
+
+ std::string data_file_path = config["data_file_path"].as<std::string>();
std::string out_npz_file_path = config["out_npz_file_path"].as<std::string>();
// MOCAP to IMU transform
Quaterniond i_q_m(i_qvec_m);
i_q_m.normalize();
assert(i_q_m.normalized().isApprox(i_q_m));
- SE3 i_T_m(i_q_m, i_p_im);
- Matrix3d i_R_m = i_T_m.rotation();
+ SE3 i_T_m(i_q_m, i_p_im);
+ Matrix3d i_R_m = i_T_m.rotation();
// IMU to MOCAP transform
- SE3 m_T_i = i_T_m.inverse();
- Vector3d m_p_mi = m_T_i.translation();
- Quaterniond m_q_i = Quaterniond(m_T_i.rotation());
+ SE3 m_T_i = i_T_m.inverse();
+ Vector3d m_p_mi = m_T_i.translation();
+ Quaterniond m_q_i = Quaterniond(m_T_i.rotation());
// MOCAP to BASE transform
Quaterniond m_q_b(m_qvec_b);
@@ -136,25 +134,24 @@ int main (int argc, char **argv) {
// Matrix3d m_R_b = m_T_b.rotation();
// IMU to BASE transform (composition)
- SE3 i_T_b = i_T_m*m_T_b;
+ SE3 i_T_b = i_T_m * m_T_b;
Vector3d i_p_ib = i_T_b.translation();
- Matrix3d i_R_b = i_T_b.rotation();
-
+ Matrix3d i_R_b = i_T_b.rotation();
// initialize some vectors and fill them with dicretized data
cnpy::npz_t arr_map = cnpy::npz_load(data_file_path);
- //load it into a new array
- cnpy::NpyArray t_npyarr = arr_map["t"];
- cnpy::NpyArray w_p_wm_npyarr = arr_map["w_p_wm"];
- cnpy::NpyArray w_q_m_npyarr = arr_map["w_q_m"];
+ // load it into a new array
+ cnpy::NpyArray t_npyarr = arr_map["t"];
+ cnpy::NpyArray w_p_wm_npyarr = arr_map["w_p_wm"];
+ cnpy::NpyArray w_q_m_npyarr = arr_map["w_q_m"];
cnpy::NpyArray imu_acc_npyarr = arr_map["imu_acc"];
// cnpy::NpyArray o_a_oi_npyarr = arr_map["o_a_oi"];
- cnpy::NpyArray o_q_i_npyarr = arr_map["o_q_i"];
+ cnpy::NpyArray o_q_i_npyarr = arr_map["o_q_i"];
cnpy::NpyArray i_omg_oi_npyarr = arr_map["i_omg_oi"];
- cnpy::NpyArray qa_npyarr = arr_map["qa"];
- cnpy::NpyArray dqa_npyarr = arr_map["dqa"];
- cnpy::NpyArray tau_npyarr = arr_map["tau"];
+ cnpy::NpyArray qa_npyarr = arr_map["qa"];
+ cnpy::NpyArray dqa_npyarr = arr_map["dqa"];
+ cnpy::NpyArray tau_npyarr = arr_map["tau"];
cnpy::NpyArray l_forces_npyarr = arr_map["l_forces"];
// cnpy::NpyArray w_pose_wm_npyarr = arr_map["w_pose_wm"];
cnpy::NpyArray m_v_wm_npyarr = arr_map["m_v_wm"];
@@ -185,30 +182,31 @@ int main (int argc, char **argv) {
// std::cout << "\n\n\n\n\n\n" << std::endl;
// cnpy::NpyArray w_R_m_npyarr = arr_map["w_R_m"];
- double* t_arr = t_npyarr.data<double>();
+ double* t_arr = t_npyarr.data<double>();
double* imu_acc_arr = imu_acc_npyarr.data<double>();
// double* o_a_oi_arr = o_a_oi_npyarr.data<double>();
double* i_omg_oi_arr = i_omg_oi_npyarr.data<double>();
- double* qa_arr = qa_npyarr.data<double>();
+ double* qa_arr = qa_npyarr.data<double>();
// double* dqa_arr = dqa_npyarr.data<double>();
// double* tau_arr = tau_npyarr.data<double>();
// double* l_forces_arr = l_forces_npyarr.data<double>();
// double* w_pose_wm_arr = w_pose_wm_npyarr.data<double>();
double* m_v_wm_arr = m_v_wm_npyarr.data<double>();
double* w_v_wm_arr = w_v_wm_npyarr.data<double>();
- double* o_q_i_arr = o_q_i_npyarr.data<double>();
+ double* o_q_i_arr = o_q_i_npyarr.data<double>();
// double* o_R_i_arr = o_R_i_npyarr.data<double>();
double* w_p_wm_arr = w_p_wm_npyarr.data<double>();
- double* w_q_m_arr = w_q_m_npyarr.data<double>();
-
+ double* w_q_m_arr = w_q_m_npyarr.data<double>();
+
// double* contact_arr = contact_npyarr.data<double>();
// double* b_p_bl_mocap_arr = b_p_bl_mocap_npyarr.data<double>();
// double* w_R_m_arr = w_R_m_npyarr.data<double>();
unsigned int N = t_npyarr.shape[0];
- if (max_t > 0){
- unsigned int N_max = (max_t/dt);
- N = N < N_max ? N : N_max;
+ if (max_t > 0)
+ {
+ unsigned int N_max = (max_t / dt);
+ N = N < N_max ? N : N_max;
}
/////////////////////////
@@ -218,15 +216,15 @@ int main (int argc, char **argv) {
ProblemPtr problem = Problem::create("POV", 3);
- // add a tree manager for sliding window optimization
- ParserYaml parser = ParserYaml("demos/tree_manager.yaml", bodydynamics_root_dir);
- ParamsServer server = ParamsServer(parser.getParams());
- auto tree_manager = TreeManagerSlidingWindow::create("tree_manager", server);
+ // add a tree manager for sliding window optimization
+ ParserYaml parser = ParserYaml("demos/tree_manager.yaml", bodydynamics_root_dir);
+ ParamsServer server = ParamsServer(parser.getParams());
+ auto tree_manager = TreeManagerSlidingWindow::create("tree_manager", server);
problem->setTreeManager(tree_manager);
//////////////////////
// COMPUTE INITIAL STATE
- TimeStamp t0(t_arr[0]);
+ TimeStamp t0(t_arr[0]);
Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr);
Eigen::Map<Vector4d> w_qvec_m_init(w_q_m_arr);
w_qvec_m_init.normalize(); // not close enough to wolf eps sometimes
@@ -234,103 +232,116 @@ int main (int argc, char **argv) {
//////////////////////
// CERES WRAPPER
- SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
+ SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
solver->getSolverOptions().max_num_iterations = max_num_iterations;
//===================================================== INITIALIZATION
// SENSOR + PROCESSOR IMU
- SensorBasePtr sen_imu_base = problem->installSensor("SensorImu", "SenImu", (Vector7d() << 0,0,0, 0,0,0,1).finished(), bodydynamics_root_dir + "/demos/sensor_imu_solo12.yaml");
- SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base);
- ProcessorBasePtr proc_imu_base = problem->installProcessor("ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu_solo12.yaml");
+ SensorBasePtr sen_imu_base = problem->installSensor("SensorImu",
+ "SenImu",
+ (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(),
+ bodydynamics_root_dir + "/demos/sensor_imu_solo12.yaml");
+ SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base);
+ ProcessorBasePtr proc_imu_base = problem->installProcessor(
+ "ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu_solo12.yaml");
ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_imu_base);
// SENSOR + PROCESSOR POSE (for mocap)
// pose sensor and proc (to get extrinsics in the prb)
- auto intr_sp = std::make_shared<ParamsSensorPose>();
+ auto intr_sp = std::make_shared<ParamsSensorPose>();
intr_sp->std_p = std_pose_p;
intr_sp->std_o = toRad(std_pose_o_deg);
- Vector7d extr_pose; extr_pose << i_p_im, i_q_m.coeffs();
- auto sensor_pose = problem->installSensor("SensorPose", "pose", extr_pose, intr_sp);
- auto params_proc = std::make_shared<ParamsProcessorPose>();
+ Vector7d extr_pose;
+ extr_pose << i_p_im, i_q_m.coeffs();
+ auto sensor_pose = problem->installSensor("SensorPose", "pose", extr_pose, intr_sp);
+ auto params_proc = std::make_shared<ParamsProcessorPose>();
params_proc->time_tolerance = time_tolerance_mocap;
- auto proc_pose = problem->installProcessor("ProcessorPose", "pose", sensor_pose, params_proc);
+ auto proc_pose = problem->installProcessor("ProcessorPose", "pose", sensor_pose, params_proc);
// somehow by default the sensor extrinsics is fixed
- Matrix3d cov_sp_p = pow(std_mocap_extr_p, 2)*Matrix3d::Identity();
- Matrix3d cov_sp_o = pow(std_mocap_extr_o_deg/60, 2)*Matrix3d::Identity();
+ Matrix3d cov_sp_p = pow(std_mocap_extr_p, 2) * Matrix3d::Identity();
+ Matrix3d cov_sp_o = pow(std_mocap_extr_o_deg / 60, 2) * Matrix3d::Identity();
sensor_pose->addPriorParameter('P', extr_pose.head<3>(), cov_sp_p);
sensor_pose->addPriorParameter('O', extr_pose.tail<4>(), cov_sp_o);
- if (unfix_extr_sensor_pose){
+ if (unfix_extr_sensor_pose)
+ {
sensor_pose->unfixExtrinsics();
}
- else{
+ else
+ {
sensor_pose->fixExtrinsics();
}
-
- Quaterniond w_q_m_init(w_qvec_m_init);
- Vector3d w_p_wi_init = w_p_wm + w_q_m_init*m_p_mi;
- Quaterniond w_q_i_init = w_q_m_init*m_q_i;
+ Quaterniond w_q_m_init(w_qvec_m_init);
+ Vector3d w_p_wi_init = w_p_wm + w_q_m_init * m_p_mi;
+ Quaterniond w_q_i_init = w_q_m_init * m_q_i;
VectorComposite x_origin("POV", {w_p_wi_init, w_q_i_init.coeffs(), Vector3d::Zero()});
- VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()});
+ 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);
FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0); // if mocap used
-
+
proc_imu->setOrigin(KF1);
//////////////////////////////////////////
// INITIAL BIAS FACTORS
// Add absolute factor on Imu biases to simulate a fix()
- Vector6d std_abs_imu; std_abs_imu << std_abs_bias_acc*Vector3d::Ones(), std_abs_bias_gyro*Vector3d::Ones();
- Matrix6d Q_bi = std_abs_imu.array().square().matrix().asDiagonal();
- CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
+ Vector6d std_abs_imu;
+ std_abs_imu << std_abs_bias_acc * Vector3d::Ones(), std_abs_bias_gyro * Vector3d::Ones();
+ Matrix6d Q_bi = std_abs_imu.array().square().matrix().asDiagonal();
+ CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", bias_imu_prior, Q_bi);
- FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
+ FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(
+ featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
KF1->getCaptureOf(sen_imu)->getStateBlock('I')->setState(bias_imu_prior);
sen_imu->getStateBlock('I')->setState(bias_imu_prior);
-
// Allocate on the heap to prevent stack overflow for large datasets
- double* o_p_ob_fbk_carr = new double[3*N];
- double* o_q_b_fbk_carr = new double[4*N];
- double* o_v_ob_fbk_carr = new double[3*N];
- double* o_p_oi_fbk_carr = new double[3*N];
- double* o_q_i_fbk_carr = new double[4*N];
- double* o_v_oi_fbk_carr = new double[3*N];
- double* imu_bias_fbk_carr = new double[6*N];
- double* i_pose_im_fbk_carr = new double[7*N];
-
+ double* o_p_ob_fbk_carr = new double[3 * N];
+ double* o_q_b_fbk_carr = new double[4 * N];
+ double* o_v_ob_fbk_carr = new double[3 * N];
+ double* o_p_oi_fbk_carr = new double[3 * N];
+ double* o_q_i_fbk_carr = new double[4 * N];
+ double* o_v_oi_fbk_carr = new double[3 * N];
+ double* imu_bias_fbk_carr = new double[6 * N];
+ double* i_pose_im_fbk_carr = new double[7 * N];
// covariances computed on the fly
- std::vector<Vector3d> Qp_fbk_v; Qp_fbk_v.push_back(Vector3d::Zero());
- std::vector<Vector3d> Qo_fbk_v; Qo_fbk_v.push_back(Vector3d::Zero());
- std::vector<Vector3d> Qv_fbk_v; Qv_fbk_v.push_back(Vector3d::Zero());
- std::vector<Vector6d> Qbi_fbk_v; Qbi_fbk_v.push_back(Vector6d::Zero());
- std::vector<Vector6d> Qm_fbk_v; Qm_fbk_v.push_back(Vector6d::Zero());
+ std::vector<Vector3d> Qp_fbk_v;
+ Qp_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector3d> Qo_fbk_v;
+ Qo_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector3d> Qv_fbk_v;
+ Qv_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector6d> Qbi_fbk_v;
+ Qbi_fbk_v.push_back(Vector6d::Zero());
+ std::vector<Vector6d> Qm_fbk_v;
+ Qm_fbk_v.push_back(Vector6d::Zero());
//
- std::vector<Vector3d> i_omg_oi_v;
+ std::vector<Vector3d> i_omg_oi_v;
//////////////////////////////////////////
unsigned int nb_kf = 1;
- for (unsigned int i=0; i < N; i++){
+ for (unsigned int i = 0; i < N; i++)
+ {
TimeStamp ts(t_arr[i]);
-
//////////////
// PROCESS IMU
//////////////
// Get measurements
// retrieve traj data
- Eigen::Map<Vector3d> imu_acc(imu_acc_arr+3*i);
- Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr+3*i); // Hyp: b=i (not the case)
+ Eigen::Map<Vector3d> imu_acc(imu_acc_arr + 3 * i);
+ Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr + 3 * i); // Hyp: b=i (not the case)
// store i_omg_oi for later computation of o_v_ob from o_v_oi
i_omg_oi_v.push_back(i_omg_oi);
- Vector6d imu_bias = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
+ Vector6d imu_bias = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>());
- Vector6d acc_gyr_meas; acc_gyr_meas << imu_acc, i_omg_oi;
+ Vector6d acc_gyr_meas;
+ acc_gyr_meas << imu_acc, i_omg_oi;
Matrix6d acc_gyr_cov = sen_imu->getNoiseCov();
CaptureImuPtr CImu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, acc_gyr_cov);
@@ -338,25 +349,24 @@ int main (int argc, char **argv) {
//////////////
//////////////
-
////////////////
// PROCESS MOCAP
////////////////
// retrieve traj data
- Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr+3*i);
- Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr+4*i);
+ Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr + 3 * i);
+ Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr + 4 * i);
w_qvec_wm.normalize(); // not close enough to wolf eps sometimes
- Vector7d pose_meas; pose_meas << w_p_wm, w_qvec_wm;
- CapturePosePtr CP = std::make_shared<CapturePose>(ts+time_shift_mocap, sensor_pose, pose_meas, sensor_pose->getNoiseCov());
+ Vector7d pose_meas;
+ pose_meas << w_p_wm, w_qvec_wm;
+ CapturePosePtr CP =
+ std::make_shared<CapturePose>(ts + time_shift_mocap, sensor_pose, pose_meas, sensor_pose->getNoiseCov());
CP->process();
////////////////
////////////////
-
-
// solve every new KF
- if (problem->getTrajectory()->size() > nb_kf )
+ if (problem->getTrajectory()->size() > nb_kf)
{
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
std::cout << ts << " ";
@@ -364,38 +374,43 @@ int main (int argc, char **argv) {
// recover covariances at this point
auto kf_last = problem->getTrajectory()->getLastFrame();
-
+
// compute covariances of KF and capture stateblocks
- Eigen::MatrixXd Qp_fbk = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd Qo_fbk = Eigen::Matrix3d::Identity(); // global or local?
- Eigen::MatrixXd Qv_fbk = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qp_fbk = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qo_fbk = Eigen::Matrix3d::Identity(); // global or local?
+ Eigen::MatrixXd Qv_fbk = Eigen::Matrix3d::Identity();
Eigen::MatrixXd Qbi_fbk = Eigen::Matrix6d::Identity();
- Eigen::MatrixXd Qmp_fbk = Eigen::Matrix3d::Identity(); // extr mocap
- Eigen::MatrixXd Qmo_fbk = Eigen::Matrix3d::Identity(); // extr mocap
-
+ Eigen::MatrixXd Qmp_fbk = Eigen::Matrix3d::Identity(); // extr mocap
+ Eigen::MatrixXd Qmo_fbk = Eigen::Matrix3d::Identity(); // extr mocap
+
if (compute_cov)
- solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+ solver->computeCovariances(
+ SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see
+ // below
problem->getCovarianceBlock(kf_last->getStateBlock('P'), Qp_fbk);
problem->getCovarianceBlock(kf_last->getStateBlock('O'), Qo_fbk);
problem->getCovarianceBlock(kf_last->getStateBlock('V'), Qv_fbk);
- CaptureBasePtr cap_imu = kf_last->getCaptureOf(sen_imu);
+ CaptureBasePtr cap_imu = kf_last->getCaptureOf(sen_imu);
if (compute_cov)
- solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
+ solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other
+ // computed covs -> issue to raise
problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi_fbk);
std::vector<StateBlockPtr> extr_sb_vec = {sensor_pose->getP(), sensor_pose->getO()};
if (compute_cov)
- solver->computeCovariances(extr_sb_vec); // not computed by ALL and destroys other computed covs -> issue to raise
+ solver->computeCovariances(
+ extr_sb_vec); // not computed by ALL and destroys other computed covs -> issue to raise
problem->getCovarianceBlock(sensor_pose->getP(), Qmp_fbk);
problem->getCovarianceBlock(sensor_pose->getO(), Qmo_fbk);
// Retrieve diagonals
- Vector3d Qp_fbk_diag = Qp_fbk.diagonal();
- Vector3d Qo_fbk_diag = Qo_fbk.diagonal();
- Vector3d Qv_fbk_diag = Qv_fbk.diagonal();
- Vector6d Qbi_fbk_diag = Qbi_fbk.diagonal();
- Vector6d Qm_fbk_diag; Qm_fbk_diag << Qmp_fbk.diagonal(), Qmo_fbk.diagonal();
+ Vector3d Qp_fbk_diag = Qp_fbk.diagonal();
+ Vector3d Qo_fbk_diag = Qo_fbk.diagonal();
+ Vector3d Qv_fbk_diag = Qv_fbk.diagonal();
+ Vector6d Qbi_fbk_diag = Qbi_fbk.diagonal();
+ Vector6d Qm_fbk_diag;
+ Qm_fbk_diag << Qmp_fbk.diagonal(), Qmo_fbk.diagonal();
Qp_fbk_v.push_back(Qp_fbk_diag);
Qo_fbk_v.push_back(Qo_fbk_diag);
@@ -409,223 +424,230 @@ int main (int argc, char **argv) {
// Store current state estimation
VectorComposite state_fbk = problem->getState();
// VectorComposite state_fbk = problem->getState(ts);
- Vector3d o_p_oi = state_fbk['P'];
+ Vector3d o_p_oi = state_fbk['P'];
Vector4d o_qvec_i = state_fbk['O'];
- Vector3d o_v_oi = state_fbk['V'];
+ Vector3d o_v_oi = state_fbk['V'];
- // IMU to base frame
+ // IMU to base frame
Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
// o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
- Matrix3d o_R_b = o_R_i * i_R_b;
+ Matrix3d o_R_b = o_R_i * i_R_b;
Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
- Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
- Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_p_im);
+ Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
+ Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_p_im);
// Vector3d o_p_ob = o_p_oi;
// Vector3d o_v_ob = o_v_oi;
imu_bias = sen_imu->getIntrinsic()->getState();
- Vector7d i_pose_im_fbk; i_pose_im_fbk << sensor_pose->getP()->getState(), sensor_pose->getO()->getState();
-
- mempcpy(o_p_ob_fbk_carr+3*i, o_p_ob.data(), 3*sizeof(double));
- mempcpy(o_q_b_fbk_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
- mempcpy(o_v_ob_fbk_carr+3*i, o_v_ob.data(), 3*sizeof(double));
- mempcpy(o_p_oi_fbk_carr+3*i, o_p_oi.data(), 3*sizeof(double));
- mempcpy(o_q_i_fbk_carr +4*i, o_qvec_i.data(), 4*sizeof(double));
- mempcpy(o_v_oi_fbk_carr+3*i, o_v_oi.data(), 3*sizeof(double));
- mempcpy(imu_bias_fbk_carr+6*i, imu_bias.data(), 6*sizeof(double));
- mempcpy(i_pose_im_fbk_carr+7*i, i_pose_im_fbk.data(), 7*sizeof(double));
+ Vector7d i_pose_im_fbk;
+ i_pose_im_fbk << sensor_pose->getP()->getState(), sensor_pose->getO()->getState();
+
+ mempcpy(o_p_ob_fbk_carr + 3 * i, o_p_ob.data(), 3 * sizeof(double));
+ mempcpy(o_q_b_fbk_carr + 4 * i, o_qvec_b.data(), 4 * sizeof(double));
+ mempcpy(o_v_ob_fbk_carr + 3 * i, o_v_ob.data(), 3 * sizeof(double));
+ mempcpy(o_p_oi_fbk_carr + 3 * i, o_p_oi.data(), 3 * sizeof(double));
+ mempcpy(o_q_i_fbk_carr + 4 * i, o_qvec_i.data(), 4 * sizeof(double));
+ mempcpy(o_v_oi_fbk_carr + 3 * i, o_v_oi.data(), 3 * sizeof(double));
+ mempcpy(imu_bias_fbk_carr + 6 * i, imu_bias.data(), 6 * sizeof(double));
+ mempcpy(i_pose_im_fbk_carr + 7 * i, i_pose_im_fbk.data(), 7 * sizeof(double));
}
-
-
///////////////////////////////////////////
//////////////// SOLVING //////////////////
///////////////////////////////////////////
- if (solve_end){
+ if (solve_end)
+ {
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem->print(4,true,true,true);
+ problem->print(4, true, true, true);
std::cout << report << std::endl;
}
- double* o_p_ob_carr = new double[3*N];
- double* o_q_b_carr = new double[4*N];
- double* o_v_ob_carr = new double[3*N];
- double* o_p_oi_carr = new double[3*N];
- double* o_q_i_carr = new double[4*N];
- double* o_v_oi_carr = new double[3*N];
- double* imu_bias_carr = new double[6*N];
+ double* o_p_ob_carr = new double[3 * N];
+ double* o_q_b_carr = new double[4 * N];
+ double* o_v_ob_carr = new double[3 * N];
+ double* o_p_oi_carr = new double[3 * N];
+ double* o_q_i_carr = new double[4 * N];
+ double* o_v_oi_carr = new double[3 * N];
+ double* imu_bias_carr = new double[6 * N];
- for (unsigned int i=0; i < N; i++) {
+ for (unsigned int i = 0; i < N; i++)
+ {
VectorComposite state_est = problem->getState(t_arr[i]);
- Vector3d i_omg_oi = i_omg_oi_v[i];
- Vector3d o_p_oi = state_est['P'];
- Vector4d o_qvec_i = state_est['O'];
- Vector3d o_v_oi = state_est['V'];
+ Vector3d i_omg_oi = i_omg_oi_v[i];
+ Vector3d o_p_oi = state_est['P'];
+ Vector4d o_qvec_i = state_est['O'];
+ Vector3d o_v_oi = state_est['V'];
- auto sb = sen_imu->getStateBlockDynamic('I', t_arr[i]);
- Vector6d imu_bias = sb->getState();
+ auto sb = sen_imu->getStateBlockDynamic('I', t_arr[i]);
+ Vector6d imu_bias = sb->getState();
Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>());
- Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
- Matrix3d o_R_b = o_R_i * i_R_m;
+ Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+ Matrix3d o_R_b = o_R_i * i_R_m;
Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
- Vector3d o_p_ob = o_p_oi + o_R_i * i_p_im;
- Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_p_im);
-
- mempcpy(o_p_ob_carr+3*i, o_p_ob.data(), 3*sizeof(double));
- mempcpy(o_q_b_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
- mempcpy(o_v_ob_carr+3*i, o_v_ob.data(), 3*sizeof(double));
- mempcpy(o_p_oi_carr+3*i, o_p_oi.data(), 3*sizeof(double));
- mempcpy(o_q_i_carr +4*i, o_qvec_i.data(), 4*sizeof(double));
- mempcpy(o_v_oi_carr+3*i, o_v_oi.data(), 3*sizeof(double));
- mempcpy(imu_bias_carr+6*i, imu_bias.data(), 6*sizeof(double));
+ Vector3d o_p_ob = o_p_oi + o_R_i * i_p_im;
+ Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_p_im);
+
+ mempcpy(o_p_ob_carr + 3 * i, o_p_ob.data(), 3 * sizeof(double));
+ mempcpy(o_q_b_carr + 4 * i, o_qvec_b.data(), 4 * sizeof(double));
+ mempcpy(o_v_ob_carr + 3 * i, o_v_ob.data(), 3 * sizeof(double));
+ mempcpy(o_p_oi_carr + 3 * i, o_p_oi.data(), 3 * sizeof(double));
+ mempcpy(o_q_i_carr + 4 * i, o_qvec_i.data(), 4 * sizeof(double));
+ mempcpy(o_v_oi_carr + 3 * i, o_v_oi.data(), 3 * sizeof(double));
+ mempcpy(imu_bias_carr + 6 * i, imu_bias.data(), 6 * sizeof(double));
}
-
// Compute Covariances
- unsigned int Nkf = problem->getTrajectory()->size();
- double* tkf_carr = new double[1*Nkf];
- double* Qp_carr = new double[3*Nkf];
- double* Qo_carr = new double[3*Nkf];
- double* Qv_carr = new double[3*Nkf];
- double* Qbi_carr = new double[6*Nkf];
- double* Qm_carr = new double[6*Nkf];
+ unsigned int Nkf = problem->getTrajectory()->size();
+ double* tkf_carr = new double[1 * Nkf];
+ double* Qp_carr = new double[3 * Nkf];
+ double* Qo_carr = new double[3 * Nkf];
+ double* Qv_carr = new double[3 * Nkf];
+ double* Qbi_carr = new double[6 * Nkf];
+ double* Qm_carr = new double[6 * Nkf];
// feedback covariances
- double* Qp_fbk_carr = new double[3*Nkf];
- double* Qo_fbk_carr = new double[3*Nkf];
- double* Qv_fbk_carr = new double[3*Nkf];
- double* Qbi_fbk_carr = new double[6*Nkf];
- double* Qm_fbk_carr = new double[6*Nkf];
-
+ double* Qp_fbk_carr = new double[3 * Nkf];
+ double* Qo_fbk_carr = new double[3 * Nkf];
+ double* Qv_fbk_carr = new double[3 * Nkf];
+ double* Qbi_fbk_carr = new double[6 * Nkf];
+ double* Qm_fbk_carr = new double[6 * Nkf];
+
// factor errors
- double* fac_imu_err_carr = new double[9*Nkf];
- double* fac_pose_err_carr = new double[6*Nkf];
- int i = 0;
- for (auto elt: problem->getTrajectory()->getFrameMap())
+ double* fac_imu_err_carr = new double[9 * Nkf];
+ double* fac_pose_err_carr = new double[6 * Nkf];
+ int i = 0;
+ for (auto elt : problem->getTrajectory()->getFrameMap())
{
std::cout << "Traj " << i << "/" << problem->getTrajectory()->size() << std::endl;
- tkf_carr[i] = elt.first.get();
- auto kf = elt.second;
+ tkf_carr[i] = elt.first.get();
+ auto kf = elt.second;
VectorComposite kf_state = kf->getState();
-
+
// compute covariances of KF and capture stateblocks
- Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local?
- Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local?
+ Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity();
Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity();
- Eigen::MatrixXd Qmp = Eigen::Matrix3d::Identity(); // extr mocap
- Eigen::MatrixXd Qmo = Eigen::Matrix3d::Identity(); // extr mocap
-
+ Eigen::MatrixXd Qmp = Eigen::Matrix3d::Identity(); // extr mocap
+ Eigen::MatrixXd Qmo = Eigen::Matrix3d::Identity(); // extr mocap
+
if (compute_cov)
- solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+ solver->computeCovariances(
+ SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
- CaptureBasePtr cap_imu = kf->getCaptureOf(sen_imu);
+ CaptureBasePtr cap_imu = kf->getCaptureOf(sen_imu);
std::vector<StateBlockPtr> extr_sb_vec = {sensor_pose->getP(), sensor_pose->getO()};
if (compute_cov)
- solver->computeCovariances(extr_sb_vec); // not computed by ALL and destroys other computed covs -> issue to raise
+ solver->computeCovariances(
+ extr_sb_vec); // not computed by ALL and destroys other computed covs -> issue to raise
problem->getCovarianceBlock(sensor_pose->getP(), Qmp);
problem->getCovarianceBlock(sensor_pose->getO(), Qmo);
if (compute_cov)
- solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
+ solver->computeCovariances(
+ cap_imu
+ ->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
// diagonal components
- Vector3d Qp_diag = Qp.diagonal();
- Vector3d Qo_diag = Qo.diagonal();
- Vector3d Qv_diag = Qv.diagonal();
- Vector6d Qbi_diag = Qbi.diagonal();
- Vector6d Qm_diag; Qm_diag << Qmp.diagonal(), Qmo.diagonal();
-
- memcpy(Qp_carr + 3*i, Qp_diag.data(), 3*sizeof(double));
- memcpy(Qo_carr + 3*i, Qo_diag.data(), 3*sizeof(double));
- memcpy(Qv_carr + 3*i, Qv_diag.data(), 3*sizeof(double));
- memcpy(Qbi_carr + 6*i, Qbi_diag.data(), 6*sizeof(double));
- memcpy(Qm_carr + 6*i, Qm_diag.data(), 6*sizeof(double));
+ Vector3d Qp_diag = Qp.diagonal();
+ Vector3d Qo_diag = Qo.diagonal();
+ Vector3d Qv_diag = Qv.diagonal();
+ Vector6d Qbi_diag = Qbi.diagonal();
+ Vector6d Qm_diag;
+ Qm_diag << Qmp.diagonal(), Qmo.diagonal();
+
+ memcpy(Qp_carr + 3 * i, Qp_diag.data(), 3 * sizeof(double));
+ memcpy(Qo_carr + 3 * i, Qo_diag.data(), 3 * sizeof(double));
+ memcpy(Qv_carr + 3 * i, Qv_diag.data(), 3 * sizeof(double));
+ memcpy(Qbi_carr + 6 * i, Qbi_diag.data(), 6 * sizeof(double));
+ memcpy(Qm_carr + 6 * i, Qm_diag.data(), 6 * sizeof(double));
// Recover feedback covariances
- memcpy(Qp_fbk_carr + 3*i, Qp_fbk_v[i].data(), 3*sizeof(double));
- memcpy(Qo_fbk_carr + 3*i, Qo_fbk_v[i].data(), 3*sizeof(double));
- memcpy(Qv_fbk_carr + 3*i, Qv_fbk_v[i].data(), 3*sizeof(double));
- memcpy(Qbi_fbk_carr + 6*i, Qbi_fbk_v[i].data(), 6*sizeof(double));
- memcpy(Qm_fbk_carr + 6*i, Qm_fbk_v[i].data(), 6*sizeof(double));
-
+ memcpy(Qp_fbk_carr + 3 * i, Qp_fbk_v[i].data(), 3 * sizeof(double));
+ memcpy(Qo_fbk_carr + 3 * i, Qo_fbk_v[i].data(), 3 * sizeof(double));
+ memcpy(Qv_fbk_carr + 3 * i, Qv_fbk_v[i].data(), 3 * sizeof(double));
+ memcpy(Qbi_fbk_carr + 6 * i, Qbi_fbk_v[i].data(), 6 * sizeof(double));
+ memcpy(Qm_fbk_carr + 6 * i, Qm_fbk_v[i].data(), 6 * sizeof(double));
// Factor errors
// std::cout << "Factors " << kf->getTimeStamp().get() << std::endl;
- FactorBasePtrList fac_lst;
+ FactorBasePtrList fac_lst;
kf->getFactorList(fac_lst);
- Vector9d imu_err = Vector9d::Zero();
+ Vector9d imu_err = Vector9d::Zero();
Vector6d pose_err = Vector6d::Zero();
- for (auto fac: fac_lst){
- if (fac->getProcessor() == proc_imu){
+ for (auto fac : fac_lst)
+ {
+ if (fac->getProcessor() == proc_imu)
+ {
auto f = std::dynamic_pointer_cast<FactorImu3d>(fac);
- if (f){
+ if (f)
+ {
imu_err = f->error();
}
}
- if (fac->getProcessor() == proc_pose){
+ if (fac->getProcessor() == proc_pose)
+ {
auto f = std::dynamic_pointer_cast<FactorPose3dWithExtrinsics>(fac);
- if (f){
+ if (f)
+ {
pose_err = f->error();
}
}
}
- memcpy(fac_imu_err_carr + 9*i, imu_err.data(), 9*sizeof(double));
- memcpy(fac_pose_err_carr + 6*i, pose_err.data(), 6*sizeof(double));
-
+ memcpy(fac_imu_err_carr + 9 * i, imu_err.data(), 9 * sizeof(double));
+ memcpy(fac_pose_err_carr + 6 * i, pose_err.data(), 6 * sizeof(double));
i++;
}
// Save trajectories in npz file
- // save ground truth
- cnpy::npz_save(out_npz_file_path, "t", t_arr, {N}, "w"); // "w" overwrites any file with same name
- cnpy::npz_save(out_npz_file_path, "w_p_wm", w_p_wm_arr, {N,3}, "a"); // "a" appends to the file we created above
- cnpy::npz_save(out_npz_file_path, "w_q_m", w_q_m_arr, {N,4}, "a");
- cnpy::npz_save(out_npz_file_path, "w_v_wm", w_v_wm_arr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "m_v_wm", m_v_wm_arr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_arr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "qa", qa_arr, {N,12}, "a");
- cnpy::npz_save(out_npz_file_path, "i_omg_oi", i_omg_oi_arr, {N,3}, "a");
+ // save ground truth
+ cnpy::npz_save(out_npz_file_path, "t", t_arr, {N}, "w"); // "w" overwrites any file with same name
+ cnpy::npz_save(out_npz_file_path, "w_p_wm", w_p_wm_arr, {N, 3}, "a"); // "a" appends to the file we created above
+ cnpy::npz_save(out_npz_file_path, "w_q_m", w_q_m_arr, {N, 4}, "a");
+ cnpy::npz_save(out_npz_file_path, "w_v_wm", w_v_wm_arr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "m_v_wm", m_v_wm_arr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_arr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "qa", qa_arr, {N, 12}, "a");
+ cnpy::npz_save(out_npz_file_path, "i_omg_oi", i_omg_oi_arr, {N, 3}, "a");
// Online estimates
- cnpy::npz_save(out_npz_file_path, "o_p_ob_fbk", o_p_ob_fbk_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_b_fbk", o_q_b_fbk_carr, {N,4}, "a");
- cnpy::npz_save(out_npz_file_path, "o_v_ob_fbk", o_v_ob_fbk_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_p_oi_fbk", o_p_oi_fbk_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_i_fbk", o_q_i_fbk_carr, {N,4}, "a");
- cnpy::npz_save(out_npz_file_path, "o_v_oi_fbk", o_v_oi_fbk_carr, {N,3}, "a");
-
+ cnpy::npz_save(out_npz_file_path, "o_p_ob_fbk", o_p_ob_fbk_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_b_fbk", o_q_b_fbk_carr, {N, 4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_ob_fbk", o_v_ob_fbk_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_p_oi_fbk", o_p_oi_fbk_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i_fbk", o_q_i_fbk_carr, {N, 4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_oi_fbk", o_v_oi_fbk_carr, {N, 3}, "a");
+
// offline states
- cnpy::npz_save(out_npz_file_path, "o_p_ob", o_p_ob_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_b", o_q_b_carr, {N,4}, "a");
- cnpy::npz_save(out_npz_file_path, "o_v_ob", o_v_ob_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_p_oi", o_p_oi_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_carr, {N,4}, "a");
- cnpy::npz_save(out_npz_file_path, "o_v_oi", o_v_oi_carr, {N,3}, "a");
-
+ cnpy::npz_save(out_npz_file_path, "o_p_ob", o_p_ob_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_b", o_q_b_carr, {N, 4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_ob", o_v_ob_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_p_oi", o_p_oi_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_carr, {N, 4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_oi", o_v_oi_carr, {N, 3}, "a");
+
// and biases/extrinsics
- cnpy::npz_save(out_npz_file_path, "imu_bias_fbk", imu_bias_fbk_carr, {N,6}, "a");
- cnpy::npz_save(out_npz_file_path, "imu_bias", imu_bias_carr, {N,6}, "a");
- cnpy::npz_save(out_npz_file_path, "i_pose_im_fbk", i_pose_im_fbk_carr, {N,7}, "a");
+ cnpy::npz_save(out_npz_file_path, "imu_bias_fbk", imu_bias_fbk_carr, {N, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "imu_bias", imu_bias_carr, {N, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "i_pose_im_fbk", i_pose_im_fbk_carr, {N, 7}, "a");
// covariances
cnpy::npz_save(out_npz_file_path, "tkf", tkf_carr, {Nkf}, "a");
- cnpy::npz_save(out_npz_file_path, "Qp", Qp_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qo", Qo_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qv", Qv_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qbi", Qbi_carr, {Nkf,6}, "a");
- cnpy::npz_save(out_npz_file_path, "Qm", Qm_carr, {Nkf,6}, "a");
- cnpy::npz_save(out_npz_file_path, "Qp_fbk", Qp_fbk_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qo_fbk", Qo_fbk_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qv_fbk", Qv_fbk_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qbi_fbk", Qbi_fbk_carr, {Nkf,6}, "a");
- cnpy::npz_save(out_npz_file_path, "Qm_fbk", Qm_fbk_carr, {Nkf,6}, "a");
- cnpy::npz_save(out_npz_file_path, "fac_imu_err", fac_imu_err_carr, {Nkf,9}, "a");
- cnpy::npz_save(out_npz_file_path, "fac_pose_err", fac_pose_err_carr, {Nkf,6}, "a");
-
+ cnpy::npz_save(out_npz_file_path, "Qp", Qp_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qo", Qo_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qv", Qv_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qbi", Qbi_carr, {Nkf, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qm", Qm_carr, {Nkf, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qp_fbk", Qp_fbk_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qo_fbk", Qo_fbk_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qv_fbk", Qv_fbk_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qbi_fbk", Qbi_fbk_carr, {Nkf, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qm_fbk", Qm_fbk_carr, {Nkf, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "fac_imu_err", fac_imu_err_carr, {Nkf, 9}, "a");
+ cnpy::npz_save(out_npz_file_path, "fac_pose_err", fac_pose_err_carr, {Nkf, 6}, "a");
}
diff --git a/demos/solo_kine_mocap.cpp b/demos/solo_kine_mocap.cpp
index 346bf88..5265d51 100644
--- a/demos/solo_kine_mocap.cpp
+++ b/demos/solo_kine_mocap.cpp
@@ -68,28 +68,28 @@
#include "bodydynamics/processor/processor_point_feet_nomove.h"
#include "bodydynamics/capture/capture_point_feet_nomove.h"
-
using namespace Eigen;
using namespace wolf;
using namespace pinocchio;
-typedef pinocchio::SE3Tpl<double> SE3;
+typedef pinocchio::SE3Tpl<double> SE3;
typedef pinocchio::ForceTpl<double> Force;
-
-int main (int argc, char **argv) {
+int main(int argc, char** argv)
+{
// retrieve parameters from yaml file
- YAML::Node config = YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/solo_real_estimation.yaml");
- double dt = config["dt"].as<double>();
- double min_t = config["min_t"].as<double>();
- double max_t = config["max_t"].as<double>();
+ YAML::Node config =
+ YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/solo_real_estimation.yaml");
+ double dt = config["dt"].as<double>();
+ double min_t = config["min_t"].as<double>();
+ double max_t = config["max_t"].as<double>();
double solve_every_sec = config["solve_every_sec"].as<double>();
- bool solve_end = config["solve_end"].as<bool>();
+ bool solve_end = config["solve_end"].as<bool>();
// robot
std::string robot_urdf = config["robot_urdf"].as<std::string>();
- int dimc = config["dimc"].as<int>();
- int nb_feet = config["nb_feet"].as<int>();
+ int dimc = config["dimc"].as<int>();
+ int nb_feet = config["nb_feet"].as<int>();
// Ceres setup
int max_num_iterations = config["max_num_iterations"].as<int>();
@@ -100,100 +100,99 @@ int main (int argc, char **argv) {
// double std_f_est = config["std_f_est"].as<double>();
// double std_tau_est = config["std_tau_est"].as<double>();
double std_foot_nomove = config["std_foot_nomove"].as<double>();
-
+
// priors
double std_prior_p = config["std_prior_p"].as<double>();
double std_prior_o = config["std_prior_o"].as<double>();
double std_prior_v = config["std_prior_v"].as<double>();
// IMU
- double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
- double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
- std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
+ double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
+ double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
+ std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
Eigen::Map<Vector6d> bias_imu_prior(bias_imu_prior_vec.data());
// base extrinsics
- std::vector<double> b_p_bi_vec = config["b_p_bi"].as<std::vector<double>>();
- std::vector<double> b_q_i_vec = config["b_q_i"].as<std::vector<double>>();
+ std::vector<double> b_p_bi_vec = config["b_p_bi"].as<std::vector<double>>();
+ std::vector<double> b_q_i_vec = config["b_q_i"].as<std::vector<double>>();
Eigen::Map<Vector3d> b_p_bi(b_p_bi_vec.data());
Eigen::Map<Vector4d> b_qvec_i(b_q_i_vec.data());
// contacts
- std::vector<double> l_p_lg_vec = config["l_p_lg"].as<std::vector<double>>();
+ std::vector<double> l_p_lg_vec = config["l_p_lg"].as<std::vector<double>>();
Eigen::Map<Vector3d> l_p_lg(l_p_lg_vec.data());
- double fz_thresh = config["fz_thresh"].as<double>();
-
+ double fz_thresh = config["fz_thresh"].as<double>();
// MOCAP
- double std_pose_p = config["std_pose_p"].as<double>();
- double std_pose_o_deg = config["std_pose_o_deg"].as<double>();
- double std_mocap_extr_p = config["std_mocap_extr_p"].as<double>();
- double std_mocap_extr_o_deg = config["std_mocap_extr_o_deg"].as<double>();
- bool unfix_extr_sensor_pose = config["unfix_extr_sensor_pose"].as<bool>();
- bool time_tolerance_mocap = config["time_tolerance_mocap"].as<double>();
- double time_shift_mocap = config["time_shift_mocap"].as<double>();
-
-
- std::string data_file_path = config["data_file_path"].as<std::string>();
+ double std_pose_p = config["std_pose_p"].as<double>();
+ double std_pose_o_deg = config["std_pose_o_deg"].as<double>();
+ double std_mocap_extr_p = config["std_mocap_extr_p"].as<double>();
+ double std_mocap_extr_o_deg = config["std_mocap_extr_o_deg"].as<double>();
+ bool unfix_extr_sensor_pose = config["unfix_extr_sensor_pose"].as<bool>();
+ bool time_tolerance_mocap = config["time_tolerance_mocap"].as<double>();
+ double time_shift_mocap = config["time_shift_mocap"].as<double>();
+
+ std::string data_file_path = config["data_file_path"].as<std::string>();
std::string out_npz_file_path = config["out_npz_file_path"].as<std::string>();
// Base to IMU transform
Quaterniond b_q_i(b_qvec_i);
assert(b_q_i.normalized().isApprox(b_q_i));
Quaterniond i_q_b = b_q_i.conjugate();
- SE3 b_T_i(b_q_i, b_p_bi);
- SE3 i_T_b = b_T_i.inverse();
- Matrix3d b_R_i = b_T_i.rotation();
- Vector3d i_p_ib = i_T_b.translation();
- Matrix3d i_R_b = i_T_b.rotation();
+ SE3 b_T_i(b_q_i, b_p_bi);
+ SE3 i_T_b = b_T_i.inverse();
+ Matrix3d b_R_i = b_T_i.rotation();
+ Vector3d i_p_ib = i_T_b.translation();
+ Matrix3d i_R_b = i_T_b.rotation();
// initialize some vectors and fill them with dicretized data
cnpy::npz_t arr_map = cnpy::npz_load(data_file_path);
- //load it into a new array
- cnpy::NpyArray t_npyarr = arr_map["t"];
+ // load it into a new array
+ cnpy::NpyArray t_npyarr = arr_map["t"];
cnpy::NpyArray imu_acc_npyarr = arr_map["imu_acc"];
// cnpy::NpyArray o_a_oi_npyarr = arr_map["o_a_oi"];
- cnpy::NpyArray o_q_i_npyarr = arr_map["o_q_i"];
+ cnpy::NpyArray o_q_i_npyarr = arr_map["o_q_i"];
cnpy::NpyArray i_omg_oi_npyarr = arr_map["i_omg_oi"];
- cnpy::NpyArray qa_npyarr = arr_map["qa"];
- cnpy::NpyArray dqa_npyarr = arr_map["dqa"];
- cnpy::NpyArray tau_npyarr = arr_map["tau"];
+ cnpy::NpyArray qa_npyarr = arr_map["qa"];
+ cnpy::NpyArray dqa_npyarr = arr_map["dqa"];
+ cnpy::NpyArray tau_npyarr = arr_map["tau"];
cnpy::NpyArray l_forces_npyarr = arr_map["l_forces"];
// cnpy::NpyArray w_pose_wm_npyarr = arr_map["w_pose_wm"];
cnpy::NpyArray m_v_wm_npyarr = arr_map["m_v_wm"];
cnpy::NpyArray w_v_wm_npyarr = arr_map["w_v_wm"];
// cnpy::NpyArray o_R_i_npyarr = arr_map["o_R_i"];
- cnpy::NpyArray w_p_wm_npyarr = arr_map["w_p_wm"];
- cnpy::NpyArray w_q_m_npyarr = arr_map["w_q_m"];
+ cnpy::NpyArray w_p_wm_npyarr = arr_map["w_p_wm"];
+ cnpy::NpyArray w_q_m_npyarr = arr_map["w_q_m"];
cnpy::NpyArray contact_npyarr = arr_map["contacts"];
// cnpy::NpyArray b_p_bl_mocap_npyarr = arr_map["b_p_bl_mocap"];
// cnpy::NpyArray w_R_m_npyarr = arr_map["w_R_m"];
- double* t_arr = t_npyarr.data<double>();
+ double* t_arr = t_npyarr.data<double>();
double* imu_acc_arr = imu_acc_npyarr.data<double>();
// double* o_a_oi_arr = o_a_oi_npyarr.data<double>();
double* i_omg_oi_arr = i_omg_oi_npyarr.data<double>();
- double* qa_arr = qa_npyarr.data<double>();
- double* dqa_arr = dqa_npyarr.data<double>();
- double* tau_arr = tau_npyarr.data<double>();
+ double* qa_arr = qa_npyarr.data<double>();
+ double* dqa_arr = dqa_npyarr.data<double>();
+ double* tau_arr = tau_npyarr.data<double>();
double* l_forces_arr = l_forces_npyarr.data<double>();
// double* w_pose_wm_arr = w_pose_wm_npyarr.data<double>();
double* m_v_wm_arr = m_v_wm_npyarr.data<double>();
double* w_v_wm_arr = w_v_wm_npyarr.data<double>();
- double* o_q_i_arr = o_q_i_npyarr.data<double>();
+ double* o_q_i_arr = o_q_i_npyarr.data<double>();
// double* o_R_i_arr = o_R_i_npyarr.data<double>();
double* w_p_wm_arr = w_p_wm_npyarr.data<double>();
- double* w_q_m_arr = w_q_m_npyarr.data<double>();
-
+ double* w_q_m_arr = w_q_m_npyarr.data<double>();
+
double* contact_arr = contact_npyarr.data<double>();
// double* b_p_bl_mocap_arr = b_p_bl_mocap_npyarr.data<double>();
// double* w_R_m_arr = w_R_m_npyarr.data<double>();
unsigned int N = t_npyarr.shape[0];
- if (max_t > 0){
- unsigned int N_max = (unsigned int)max_t/dt;
- N = N < N_max ? N : N_max;
+ if (max_t > 0)
+ {
+ unsigned int N_max = (unsigned int)max_t / dt;
+ N = N < N_max ? N : N_max;
}
std::cout << "N: " << N << std::endl;
@@ -201,14 +200,15 @@ int main (int argc, char **argv) {
Model model;
pinocchio::urdf::buildModel(robot_urdf, JointModelFreeFlyer(), model);
std::cout << "model name: " << model.name << std::endl;
- Data data(model);
+ Data data(model);
std::vector<std::string> ee_names = {"FL_ANKLE", "FR_ANKLE", "HL_ANKLE", "HR_ANKLE"};
- unsigned int nbc = ee_names.size();
+ unsigned int nbc = ee_names.size();
// Recover end effector frame ids
std::vector<int> ee_ids;
std::cout << "Frame ids" << std::endl;
- for (auto ee_name: ee_names){
+ for (auto ee_name : ee_names)
+ {
ee_ids.push_back(model.getFrameId(ee_name));
std::cout << ee_name << std::endl;
}
@@ -221,104 +221,114 @@ int main (int argc, char **argv) {
ProblemPtr problem = Problem::create("PO", 3);
// ProblemPtr problem = Problem::create("POV", 3);
- // add a tree manager for sliding window optimization
- ParserYaml parser = ParserYaml("demos/tree_manager.yaml", bodydynamics_root_dir);
- ParamsServer server = ParamsServer(parser.getParams());
- auto tree_manager = TreeManagerSlidingWindow::create("tree_manager", server);
+ // add a tree manager for sliding window optimization
+ ParserYaml parser = ParserYaml("demos/tree_manager.yaml", bodydynamics_root_dir);
+ ParamsServer server = ParamsServer(parser.getParams());
+ auto tree_manager = TreeManagerSlidingWindow::create("tree_manager", server);
problem->setTreeManager(tree_manager);
//////////////////////
// COMPUTE INITIAL STATE
- TimeStamp t0(t_arr[0]);
- Eigen::Map<Vector4d> o_q_i(o_q_i_arr); // initialize with IMU orientation
+ TimeStamp t0(t_arr[0]);
+ Eigen::Map<Vector4d> o_q_i(o_q_i_arr); // initialize with IMU orientation
Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr); // Hyp: b=i (not the case)
Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr);
Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr);
w_qvec_wm.normalize(); // not close enough to wolf eps sometimes
// initial configuration
- Eigen::Map<Matrix<double,12, 1>> qa(qa_arr);
- Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr);
- VectorXd q(19); q << 0,0,0, o_q_i, qa; // TODO: put current base orientation estimate? YES
- VectorXd dq(18); dq << 0,0,0, i_omg_oi, dqa; // TODO: put current base velocity estimate? YES
+ Eigen::Map<Matrix<double, 12, 1>> qa(qa_arr);
+ Eigen::Map<Matrix<double, 12, 1>> dqa(dqa_arr);
+ VectorXd q(19);
+ q << 0, 0, 0, o_q_i, qa; // TODO: put current base orientation estimate? YES
+ VectorXd dq(18);
+ dq << 0, 0, 0, i_omg_oi, dqa; // TODO: put current base velocity estimate? YES
//////////////////////
// CERES WRAPPER
- SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
+ SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
solver->getSolverOptions().max_num_iterations = max_num_iterations;
//===================================================== INITIALIZATION
// SENSOR + PROCESSOR POINT FEET NOMOVE
ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>();
- intr_pfn->std_foot_nomove_ = std_foot_nomove;
- VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
+ intr_pfn->std_foot_nomove_ = std_foot_nomove;
+ VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
SensorBasePtr sen_pfnm_base = problem->installSensor("SensorPointFeetNomove", "SenPfnm", extr, intr_pfn);
- SensorPointFeetNomovePtr sen_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base);
+ SensorPointFeetNomovePtr sen_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base);
ParamsProcessorPointFeetNomovePtr params_pfnm = std::make_shared<ParamsProcessorPointFeetNomove>();
- params_pfnm->voting_active = true;
- params_pfnm->time_tolerance = 0.001;
- params_pfnm->max_time_vote = 0.19999999;
- ProcessorBasePtr proc_pfnm_base = problem->installProcessor("ProcessorPointFeetNomove", "PPointFeetNomove", sen_pfnm, params_pfnm);
-
+ params_pfnm->voting_active = true;
+ params_pfnm->time_tolerance = 0.001;
+ params_pfnm->max_time_vote = 0.19999999;
+ ProcessorBasePtr proc_pfnm_base =
+ problem->installProcessor("ProcessorPointFeetNomove", "PPointFeetNomove", sen_pfnm, params_pfnm);
// SENSOR + PROCESSOR POSE (for mocap)
// pose sensor and proc (to get extrinsics in the prb)
- auto intr_sp = std::make_shared<ParamsSensorPose>();
+ auto intr_sp = std::make_shared<ParamsSensorPose>();
intr_sp->std_p = std_pose_p;
intr_sp->std_o = toRad(std_pose_o_deg);
- Vector7d extr_pose; extr_pose << i_p_ib, i_q_b.coeffs();
- auto sensor_pose = problem->installSensor("SensorPose", "pose", extr_pose, intr_sp);
- auto params_proc = std::make_shared<ParamsProcessorPose>();
+ Vector7d extr_pose;
+ extr_pose << i_p_ib, i_q_b.coeffs();
+ auto sensor_pose = problem->installSensor("SensorPose", "pose", extr_pose, intr_sp);
+ auto params_proc = std::make_shared<ParamsProcessorPose>();
params_proc->time_tolerance = time_tolerance_mocap;
- auto proc_pose = problem->installProcessor("ProcessorPose", "pose", sensor_pose, params_proc);
+ auto proc_pose = problem->installProcessor("ProcessorPose", "pose", sensor_pose, params_proc);
// somehow by default the sensor extrinsics is fixed
- Matrix3d cov_sp_p = pow(std_mocap_extr_p, 2)*Matrix3d::Identity();
- Matrix3d cov_sp_o = pow(std_mocap_extr_o_deg/60, 2)*Matrix3d::Identity();
+ Matrix3d cov_sp_p = pow(std_mocap_extr_p, 2) * Matrix3d::Identity();
+ Matrix3d cov_sp_o = pow(std_mocap_extr_o_deg / 60, 2) * Matrix3d::Identity();
sensor_pose->addPriorParameter('P', extr_pose.head<3>(), cov_sp_p);
sensor_pose->addPriorParameter('O', extr_pose.tail<4>(), cov_sp_o);
- if (unfix_extr_sensor_pose){
+ if (unfix_extr_sensor_pose)
+ {
sensor_pose->unfixExtrinsics();
}
- else{
+ else
+ {
sensor_pose->fixExtrinsics();
}
- Matrix3d w_R_m_init = q2R(w_qvec_wm);
- Vector3d w_p_wi_init = w_p_wm + w_R_m_init*b_p_bi;
+ Matrix3d w_R_m_init = q2R(w_qvec_wm);
+ Vector3d w_p_wi_init = w_p_wm + w_R_m_init * b_p_bi;
VectorComposite x_origin("PO", {w_p_wi_init, w_qvec_wm});
- VectorComposite std_origin("PO", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones()});
+ VectorComposite std_origin("PO", {std_prior_p * Vector3d::Ones(), std_prior_o * 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
-
//////////////////////////////////////////
- // Vectors to store estimates at the time of data processing
+ // Vectors to store estimates at the time of data processing
// fbk -> feedback: to check if the estimation is stable enough for feedback control
- std::vector<Vector3d> p_ob_fbk_v;
- std::vector<Vector4d> q_ob_fbk_v;
- std::vector<Vector3d> v_ob_fbk_v;
+ std::vector<Vector3d> p_ob_fbk_v;
+ std::vector<Vector4d> q_ob_fbk_v;
+ std::vector<Vector3d> v_ob_fbk_v;
// Allocate on the heap to prevent stack overflow for large datasets
- double* o_p_ob_fbk_carr = new double[3*N];
- double* o_q_b_fbk_carr = new double[4*N];
- double* o_v_ob_fbk_carr = new double[3*N];
- double* imu_bias_fbk_carr = new double[6*N];
- double* i_pose_ib_fbk_carr = new double[7*N];
+ double* o_p_ob_fbk_carr = new double[3 * N];
+ double* o_q_b_fbk_carr = new double[4 * N];
+ double* o_v_ob_fbk_carr = new double[3 * N];
+ double* imu_bias_fbk_carr = new double[6 * N];
+ double* i_pose_ib_fbk_carr = new double[7 * N];
// covariances computed on the fly
- std::vector<Vector3d> Qp_fbk_v; Qp_fbk_v.push_back(Vector3d::Zero());
- std::vector<Vector3d> Qo_fbk_v; Qo_fbk_v.push_back(Vector3d::Zero());
- std::vector<Vector3d> Qv_fbk_v; Qv_fbk_v.push_back(Vector3d::Zero());
- std::vector<Vector6d> Qbi_fbk_v; Qbi_fbk_v.push_back(Vector6d::Zero());
- std::vector<Vector6d> Qm_fbk_v; Qm_fbk_v.push_back(Vector6d::Zero());
+ std::vector<Vector3d> Qp_fbk_v;
+ Qp_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector3d> Qo_fbk_v;
+ Qo_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector3d> Qv_fbk_v;
+ Qv_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector6d> Qbi_fbk_v;
+ Qbi_fbk_v.push_back(Vector6d::Zero());
+ std::vector<Vector6d> Qm_fbk_v;
+ Qm_fbk_v.push_back(Vector6d::Zero());
//
- std::vector<Vector3d> i_omg_oi_v;
+ std::vector<Vector3d> i_omg_oi_v;
//////////////////////////////////////////
unsigned int nb_kf = 1;
- for (unsigned int i=0; i < N; i++){
+ for (unsigned int i = 0; i < N; i++)
+ {
TimeStamp ts(t_arr[i]);
////////////////
@@ -326,13 +336,15 @@ int main (int argc, char **argv) {
////////////////
// Get measurements
// retrieve traj data
- Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr+3*i);
- Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr+4*i);
+ Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr + 3 * i);
+ Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr + 4 * i);
w_qvec_wm.normalize(); // not close enough to wolf eps sometimes
// Vector7d pose_meas; pose_meas << w_p_wm, w_qvec_wm;
- Vector7d pose_meas; pose_meas << w_p_wm, w_qvec_wm;
- CapturePosePtr CP = std::make_shared<CapturePose>(ts+time_shift_mocap, sensor_pose, pose_meas, sensor_pose->getNoiseCov());
+ Vector7d pose_meas;
+ pose_meas << w_p_wm, w_qvec_wm;
+ CapturePosePtr CP =
+ std::make_shared<CapturePose>(ts + time_shift_mocap, sensor_pose, pose_meas, sensor_pose->getNoiseCov());
CP->process();
////////////////
////////////////
@@ -340,36 +352,38 @@ int main (int argc, char **argv) {
/////////////////////
// PROCESS KINEMATICS
/////////////////////
- Eigen::Map<Matrix<double,12, 1>> tau(tau_arr+12*i);
- Eigen::Map<Matrix<double,12, 1>> qa(qa_arr+12*i);
- Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr+12*i);
- Eigen::Map<Matrix<double,4, 1>> contact_gtr(contact_arr+4*i); // int conversion does not work!
-
+ Eigen::Map<Matrix<double, 12, 1>> tau(tau_arr + 12 * i);
+ Eigen::Map<Matrix<double, 12, 1>> qa(qa_arr + 12 * i);
+ Eigen::Map<Matrix<double, 12, 1>> dqa(dqa_arr + 12 * i);
+ Eigen::Map<Matrix<double, 4, 1>> contact_gtr(contact_arr + 4 * i); // int conversion does not work!
// Or else, retrieve from preprocessed dataset
- Eigen::Map<Matrix<double,12, 1>> l_forces(l_forces_arr+12*i);
+ Eigen::Map<Matrix<double, 12, 1>> l_forces(l_forces_arr + 12 * i);
- Vector3d o_f_tot = Vector3d::Zero();
+ Vector3d o_f_tot = Vector3d::Zero();
std::vector<Vector3d> l_f_l_vec;
// std::vector<Vector3d> o_f_l_vec;
std::vector<Vector3d> i_p_il_vec;
std::vector<Vector4d> i_qvec_l_vec;
// needing relative kinematics measurements
- VectorXd q_static(19); q_static << 0,0,0, 0,0,0,1, qa;
- VectorXd dq_static(18); dq_static << 0,0,0, 0,0,0, dqa;
+ VectorXd q_static(19);
+ q_static << 0, 0, 0, 0, 0, 0, 1, qa;
+ VectorXd dq_static(18);
+ dq_static << 0, 0, 0, 0, 0, 0, dqa;
forwardKinematics(model, data, q_static, dq_static);
updateFramePlacements(model, data);
- for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
+ for (unsigned int i_ee = 0; i_ee < nbc; i_ee++)
+ {
auto b_T_l = data.oMf[ee_ids[i_ee]];
- auto i_T_l = i_T_b * b_T_l;
- Vector3d i_p_il = i_T_l.translation(); // meas
- Matrix3d i_R_l = i_T_l.rotation();
+ auto i_T_l = i_T_b * b_T_l;
+ Vector3d i_p_il = i_T_l.translation(); // meas
+ Matrix3d i_R_l = i_T_l.rotation();
Vector4d i_qvec_l = Quaterniond(i_R_l).coeffs(); // meas
- i_p_il = i_p_il + i_R_l*l_p_lg;
+ i_p_il = i_p_il + i_R_l * l_p_lg;
- Vector3d l_f_l = l_forces.segment(3*i_ee, 3); // meas
+ Vector3d l_f_l = l_forces.segment(3 * i_ee, 3); // meas
// Vector3d o_f_l = o_R_i*i_R_l * l_f_l; // for contact test
l_f_l_vec.push_back(l_f_l);
@@ -379,27 +393,29 @@ int main (int argc, char **argv) {
}
// Detect feet in contact
- int nb_feet_in_contact = 0;
+ int nb_feet_in_contact = 0;
std::unordered_map<int, Vector7d> kin_incontact;
// std::cout << "" << std::endl;
- for (unsigned int i_ee=0; i_ee<nbc; i_ee++){
+ for (unsigned int i_ee = 0; i_ee < nbc; i_ee++)
+ {
// basic contact detection based on normal foot vel, things break if no foot is in contact
// if ((o_f_l_vec[i_ee](2) > fz_thresh) && (nb_feet_in_contact < nb_feet)){
// if ((contact_gtr[i_ee] > 0.5) && (nb_feet_in_contact < nb_feet)){
- if (contact_gtr[i_ee] > 0.5){
+ if (contact_gtr[i_ee] > 0.5)
+ {
nb_feet_in_contact++;
- Vector7d i_pose_il; i_pose_il << i_p_il_vec[i_ee], i_qvec_l_vec[i_ee];
+ Vector7d i_pose_il;
+ i_pose_il << i_p_il_vec[i_ee], i_qvec_l_vec[i_ee];
kin_incontact.insert({i_ee, i_pose_il});
}
}
-
CapturePointFeetNomovePtr CPF = std::make_shared<CapturePointFeetNomove>(ts, sen_pfnm, kin_incontact);
CPF->process();
-
// solve every new KF
- if (problem->getTrajectory()->getFrameMap().size() > nb_kf ){
+ if (problem->getTrajectory()->getFrameMap().size() > nb_kf)
+ {
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
std::cout << ts << " ";
std::cout << report << std::endl;
@@ -407,33 +423,34 @@ int main (int argc, char **argv) {
// recover covariances at this point
auto kf_last = problem->getTrajectory()->getFrameMap().rbegin()->second;
-
// compute covariances of KF and capture stateblocks
- Eigen::MatrixXd Qp_fbk = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd Qo_fbk = Eigen::Matrix3d::Identity(); // global or local?
- Eigen::MatrixXd Qv_fbk = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qp_fbk = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qo_fbk = Eigen::Matrix3d::Identity(); // global or local?
+ Eigen::MatrixXd Qv_fbk = Eigen::Matrix3d::Identity();
Eigen::MatrixXd Qbi_fbk = Eigen::Matrix6d::Identity();
- Eigen::MatrixXd Qmp_fbk = Eigen::Matrix3d::Identity(); // extr mocap
- Eigen::MatrixXd Qmo_fbk = Eigen::Matrix3d::Identity(); // extr mocap
-
- solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+ Eigen::MatrixXd Qmp_fbk = Eigen::Matrix3d::Identity(); // extr mocap
+ Eigen::MatrixXd Qmo_fbk = Eigen::Matrix3d::Identity(); // extr mocap
+
+ solver->computeCovariances(
+ SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
problem->getCovarianceBlock(kf_last->getStateBlock('P'), Qp_fbk);
problem->getCovarianceBlock(kf_last->getStateBlock('O'), Qo_fbk);
// problem->getCovarianceBlock(kf_last->getStateBlock('V'), Qv_fbk);
std::vector<StateBlockPtr> extr_sb_vec = {sensor_pose->getP(), sensor_pose->getO()};
- solver->computeCovariances(extr_sb_vec); // not computed by ALL and destroys other computed covs -> issue to raise
+ solver->computeCovariances(
+ extr_sb_vec); // not computed by ALL and destroys other computed covs -> issue to raise
problem->getCovarianceBlock(sensor_pose->getP(), Qmp_fbk);
problem->getCovarianceBlock(sensor_pose->getO(), Qmo_fbk);
-
// Retrieve diagonals
- Vector3d Qp_fbk_diag = Qp_fbk.diagonal();
- Vector3d Qo_fbk_diag = Qo_fbk.diagonal();
- Vector3d Qv_fbk_diag = Qv_fbk.diagonal();
- Vector6d Qbi_fbk_diag = Qbi_fbk.diagonal();
- Vector6d Qm_fbk_diag; Qm_fbk_diag << Qmp_fbk.diagonal(), Qmo_fbk.diagonal();
- Vector3d Qmo_fbk_diag = Qmo_fbk.diagonal();
+ Vector3d Qp_fbk_diag = Qp_fbk.diagonal();
+ Vector3d Qo_fbk_diag = Qo_fbk.diagonal();
+ Vector3d Qv_fbk_diag = Qv_fbk.diagonal();
+ Vector6d Qbi_fbk_diag = Qbi_fbk.diagonal();
+ Vector6d Qm_fbk_diag;
+ Qm_fbk_diag << Qmp_fbk.diagonal(), Qmo_fbk.diagonal();
+ Vector3d Qmo_fbk_diag = Qmo_fbk.diagonal();
Qp_fbk_v.push_back(Qp_fbk_diag);
Qo_fbk_v.push_back(Qo_fbk_diag);
@@ -441,24 +458,23 @@ int main (int argc, char **argv) {
Qbi_fbk_v.push_back(Qbi_fbk_diag);
Qm_fbk_v.push_back(Qm_fbk_diag);
-
nb_kf++;
}
// Store current state estimation
VectorComposite state_fbk = problem->getState();
// VectorComposite state_fbk = problem->getState(ts);
- Vector3d o_p_oi = state_fbk['P'];
+ Vector3d o_p_oi = state_fbk['P'];
Vector4d o_qvec_i = state_fbk['O'];
// Vector3d o_v_oi = state_fbk['V'];
Vector3d o_v_oi = Vector3d::Zero();
- // IMU to base frame
+ // IMU to base frame
Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
// o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
- Matrix3d o_R_b = o_R_i * i_R_b;
+ Matrix3d o_R_b = o_R_i * i_R_b;
Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
- Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
+ Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
// Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi.cross(i_p_ib);
// Vector3d o_p_ob = o_p_oi;
Vector3d o_v_ob = o_v_oi;
@@ -466,117 +482,121 @@ int main (int argc, char **argv) {
Vector3d imu_bias = Vector3d::Zero();
// imu_bias = sen_imu->getIntrinsic()->getState();
// imu_bias = sen_imu->getIntrinsic(ts)->getState();
- Vector7d i_pose_ib_est; i_pose_ib_est << sensor_pose->getP()->getState(), sensor_pose->getO()->getState();
+ Vector7d i_pose_ib_est;
+ i_pose_ib_est << sensor_pose->getP()->getState(), sensor_pose->getO()->getState();
- mempcpy(o_p_ob_fbk_carr+3*i, o_p_ob.data(), 3*sizeof(double));
- mempcpy(o_q_b_fbk_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
- mempcpy(o_v_ob_fbk_carr+3*i, o_v_ob.data(), 3*sizeof(double));
- mempcpy(imu_bias_fbk_carr+6*i, imu_bias.data(), 6*sizeof(double));
- mempcpy(i_pose_ib_fbk_carr+7*i, i_pose_ib_est.data(), 7*sizeof(double));
+ mempcpy(o_p_ob_fbk_carr + 3 * i, o_p_ob.data(), 3 * sizeof(double));
+ mempcpy(o_q_b_fbk_carr + 4 * i, o_qvec_b.data(), 4 * sizeof(double));
+ mempcpy(o_v_ob_fbk_carr + 3 * i, o_v_ob.data(), 3 * sizeof(double));
+ mempcpy(imu_bias_fbk_carr + 6 * i, imu_bias.data(), 6 * sizeof(double));
+ mempcpy(i_pose_ib_fbk_carr + 7 * i, i_pose_ib_est.data(), 7 * sizeof(double));
p_ob_fbk_v.push_back(o_p_ob);
q_ob_fbk_v.push_back(o_qvec_b);
v_ob_fbk_v.push_back(o_v_ob);
}
-
-
///////////////////////////////////////////
//////////////// SOLVING //////////////////
///////////////////////////////////////////
- if (solve_end){
+ if (solve_end)
+ {
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem->print(4,true,true,true);
+ problem->print(4, true, true, true);
std::cout << report << std::endl;
}
- double* o_p_ob_carr = new double[3*N];
- double* o_q_b_carr = new double[4*N];
- double* o_v_ob_carr = new double[6*N];
- double* imu_bias_carr = new double[6*N];
- for (unsigned int i=0; i < N; i++) {
+ double* o_p_ob_carr = new double[3 * N];
+ double* o_q_b_carr = new double[4 * N];
+ double* o_v_ob_carr = new double[6 * N];
+ double* imu_bias_carr = new double[6 * N];
+ for (unsigned int i = 0; i < N; i++)
+ {
VectorComposite state_est = problem->getState(t_arr[i]);
- Vector3d o_p_oi = state_est['P'];
- Vector4d o_qvec_i = state_est['O'];
- Vector3d o_v_oi = Vector3d::Zero();
+ Vector3d o_p_oi = state_est['P'];
+ Vector4d o_qvec_i = state_est['O'];
+ Vector3d o_v_oi = Vector3d::Zero();
- Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
- Matrix3d o_R_b = o_R_i * i_R_b;
+ Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+ Matrix3d o_R_b = o_R_i * i_R_b;
Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
- Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
+ Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
// Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_R_b*b_p_bi);
// Vector3d o_p_ob = o_p_oi;
Vector3d o_v_ob = o_v_oi;
- mempcpy(o_p_ob_carr+3*i, o_p_ob.data(), 3*sizeof(double));
- mempcpy(o_q_b_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
- mempcpy(o_v_ob_carr+3*i, o_v_ob.data(), 3*sizeof(double));
+ mempcpy(o_p_ob_carr + 3 * i, o_p_ob.data(), 3 * sizeof(double));
+ mempcpy(o_q_b_carr + 4 * i, o_qvec_b.data(), 4 * sizeof(double));
+ mempcpy(o_v_ob_carr + 3 * i, o_v_ob.data(), 3 * sizeof(double));
// mempcpy(imu_bias_carr+6*i, imu_bias.data(), 6*sizeof(double));
}
-
// Compute Covariances
- unsigned int Nkf = problem->getTrajectory()->getFrameMap().size();
- double* tkf_carr = new double[1*Nkf];
- double* Qp_carr = new double[3*Nkf];
- double* Qo_carr = new double[3*Nkf];
- double* Qv_carr = new double[3*Nkf];
- double* Qbi_carr = new double[6*Nkf];
- double* Qm_carr = new double[6*Nkf];
+ unsigned int Nkf = problem->getTrajectory()->getFrameMap().size();
+ double* tkf_carr = new double[1 * Nkf];
+ double* Qp_carr = new double[3 * Nkf];
+ double* Qo_carr = new double[3 * Nkf];
+ double* Qv_carr = new double[3 * Nkf];
+ double* Qbi_carr = new double[6 * Nkf];
+ double* Qm_carr = new double[6 * Nkf];
// feedback covariances
- double* Qp_fbk_carr = new double[3*Nkf];
- double* Qo_fbk_carr = new double[3*Nkf];
- double* Qv_fbk_carr = new double[3*Nkf];
- double* Qbi_fbk_carr = new double[6*Nkf];
- double* Qm_fbk_carr = new double[6*Nkf];
-
+ double* Qp_fbk_carr = new double[3 * Nkf];
+ double* Qo_fbk_carr = new double[3 * Nkf];
+ double* Qv_fbk_carr = new double[3 * Nkf];
+ double* Qbi_fbk_carr = new double[6 * Nkf];
+ double* Qm_fbk_carr = new double[6 * Nkf];
+
// factor errors
- double* fac_imu_err_carr = new double[9*Nkf];
- double* fac_pose_err_carr = new double[6*Nkf];
- int i = 0;
- for (auto& elt: problem->getTrajectory()->getFrameMap()){
+ double* fac_imu_err_carr = new double[9 * Nkf];
+ double* fac_pose_err_carr = new double[6 * Nkf];
+ int i = 0;
+ for (auto& elt : problem->getTrajectory()->getFrameMap())
+ {
std::cout << "Traj " << i << "/" << problem->getTrajectory()->getFrameMap().size() << std::endl;
- tkf_carr[i] = elt.first.get();
- auto kf = elt.second;
+ tkf_carr[i] = elt.first.get();
+ auto kf = elt.second;
VectorComposite kf_state = kf->getState();
-
+
// compute covariances of KF and capture stateblocks
- Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local?
- Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local?
+ Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity();
Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity();
- Eigen::MatrixXd Qmp = Eigen::Matrix3d::Identity(); // extr mocap
- Eigen::MatrixXd Qmo = Eigen::Matrix3d::Identity(); // extr mocap
-
- solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+ Eigen::MatrixXd Qmp = Eigen::Matrix3d::Identity(); // extr mocap
+ Eigen::MatrixXd Qmo = Eigen::Matrix3d::Identity(); // extr mocap
+
+ solver->computeCovariances(
+ SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
// problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
std::vector<StateBlockPtr> extr_sb_vec = {sensor_pose->getP(), sensor_pose->getO()};
- solver->computeCovariances(extr_sb_vec); // not computed by ALL and destroys other computed covs -> issue to raise
+ solver->computeCovariances(
+ extr_sb_vec); // not computed by ALL and destroys other computed covs -> issue to raise
problem->getCovarianceBlock(sensor_pose->getP(), Qmp);
problem->getCovarianceBlock(sensor_pose->getO(), Qmo);
// diagonal components
- Vector3d Qp_diag = Qp.diagonal();
- Vector3d Qo_diag = Qo.diagonal();
- Vector3d Qv_diag = Qv.diagonal();
- Vector6d Qbi_diag = Qbi.diagonal();
- Vector6d Qm_diag; Qm_diag << Qmp.diagonal(), Qmo.diagonal();
-
- memcpy(Qp_carr + 3*i, Qp_diag.data(), 3*sizeof(double));
- memcpy(Qo_carr + 3*i, Qo_diag.data(), 3*sizeof(double));
- memcpy(Qv_carr + 3*i, Qv_diag.data(), 3*sizeof(double));
- memcpy(Qbi_carr + 6*i, Qbi_diag.data(), 6*sizeof(double));
- memcpy(Qm_carr + 6*i, Qm_diag.data(), 6*sizeof(double));
+ Vector3d Qp_diag = Qp.diagonal();
+ Vector3d Qo_diag = Qo.diagonal();
+ Vector3d Qv_diag = Qv.diagonal();
+ Vector6d Qbi_diag = Qbi.diagonal();
+ Vector6d Qm_diag;
+ Qm_diag << Qmp.diagonal(), Qmo.diagonal();
+
+ memcpy(Qp_carr + 3 * i, Qp_diag.data(), 3 * sizeof(double));
+ memcpy(Qo_carr + 3 * i, Qo_diag.data(), 3 * sizeof(double));
+ memcpy(Qv_carr + 3 * i, Qv_diag.data(), 3 * sizeof(double));
+ memcpy(Qbi_carr + 6 * i, Qbi_diag.data(), 6 * sizeof(double));
+ memcpy(Qm_carr + 6 * i, Qm_diag.data(), 6 * sizeof(double));
// Recover feedback covariances
- memcpy(Qp_fbk_carr + 3*i, Qp_fbk_v[i].data(), 3*sizeof(double));
- memcpy(Qo_fbk_carr + 3*i, Qo_fbk_v[i].data(), 3*sizeof(double));
- memcpy(Qv_fbk_carr + 3*i, Qv_fbk_v[i].data(), 3*sizeof(double));
- memcpy(Qbi_fbk_carr + 6*i, Qbi_fbk_v[i].data(), 6*sizeof(double));
- memcpy(Qm_fbk_carr + 6*i, Qm_fbk_v[i].data(), 6*sizeof(double));
+ memcpy(Qp_fbk_carr + 3 * i, Qp_fbk_v[i].data(), 3 * sizeof(double));
+ memcpy(Qo_fbk_carr + 3 * i, Qo_fbk_v[i].data(), 3 * sizeof(double));
+ memcpy(Qv_fbk_carr + 3 * i, Qv_fbk_v[i].data(), 3 * sizeof(double));
+ memcpy(Qbi_fbk_carr + 6 * i, Qbi_fbk_v[i].data(), 6 * sizeof(double));
+ memcpy(Qm_fbk_carr + 6 * i, Qm_fbk_v[i].data(), 6 * sizeof(double));
i++;
}
@@ -584,56 +604,55 @@ int main (int argc, char **argv) {
// problem->check(1);
// Save trajectories in npz file
- // save ground truth
- cnpy::npz_save(out_npz_file_path, "t", t_arr, {N}, "w"); // "w" overwrites any file with same name
- cnpy::npz_save(out_npz_file_path, "w_p_wm", w_p_wm_arr, {N,3}, "a"); // "a" appends to the file we created above
- cnpy::npz_save(out_npz_file_path, "w_q_m", w_q_m_arr, {N,4}, "a");
- cnpy::npz_save(out_npz_file_path, "w_v_wm", w_v_wm_arr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "m_v_wm", m_v_wm_arr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_arr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "qa", qa_arr, {N,12}, "a");
- cnpy::npz_save(out_npz_file_path, "i_omg_oi", i_omg_oi_arr, {N,3}, "a");
+ // save ground truth
+ cnpy::npz_save(out_npz_file_path, "t", t_arr, {N}, "w"); // "w" overwrites any file with same name
+ cnpy::npz_save(out_npz_file_path, "w_p_wm", w_p_wm_arr, {N, 3}, "a"); // "a" appends to the file we created above
+ cnpy::npz_save(out_npz_file_path, "w_q_m", w_q_m_arr, {N, 4}, "a");
+ cnpy::npz_save(out_npz_file_path, "w_v_wm", w_v_wm_arr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "m_v_wm", m_v_wm_arr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_arr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "qa", qa_arr, {N, 12}, "a");
+ cnpy::npz_save(out_npz_file_path, "i_omg_oi", i_omg_oi_arr, {N, 3}, "a");
// Online estimates
- cnpy::npz_save(out_npz_file_path, "o_p_ob_fbk", o_p_ob_fbk_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_b_fbk", o_q_b_fbk_carr, {N,4}, "a");
- cnpy::npz_save(out_npz_file_path, "o_v_ob_fbk", o_v_ob_fbk_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_p_oi_fbk", o_p_ob_fbk_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_i_fbk", o_q_b_fbk_carr, {N,4}, "a");
- cnpy::npz_save(out_npz_file_path, "o_v_oi_fbk", o_v_ob_fbk_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_p_ob_fbk", o_p_ob_fbk_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_b_fbk", o_q_b_fbk_carr, {N, 4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_ob_fbk", o_v_ob_fbk_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_p_oi_fbk", o_p_ob_fbk_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i_fbk", o_q_b_fbk_carr, {N, 4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_oi_fbk", o_v_ob_fbk_carr, {N, 3}, "a");
// cnpy::npz_save(out_npz_file_path, "o_p_oi_fbk", o_p_oi_fbk_carr, {N,3}, "a");
// cnpy::npz_save(out_npz_file_path, "o_q_i_fbk", o_q_i_fbk_carr, {N,4}, "a");
// cnpy::npz_save(out_npz_file_path, "o_v_oi_fbk", o_v_oi_fbk_carr, {N,3}, "a");
-
+
// offline states
- cnpy::npz_save(out_npz_file_path, "o_p_ob", o_p_ob_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_b", o_q_b_carr, {N,4}, "a");
- cnpy::npz_save(out_npz_file_path, "o_v_ob", o_v_ob_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_p_oi", o_p_ob_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_b_carr, {N,4}, "a");
- cnpy::npz_save(out_npz_file_path, "o_v_oi", o_v_ob_carr, {N,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_p_ob", o_p_ob_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_b", o_q_b_carr, {N, 4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_ob", o_v_ob_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_p_oi", o_p_ob_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_b_carr, {N, 4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_oi", o_v_ob_carr, {N, 3}, "a");
// cnpy::npz_save(out_npz_file_path, "o_p_oi", o_p_oi_carr, {N,3}, "a");
// cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_carr, {N,4}, "a");
// cnpy::npz_save(out_npz_file_path, "o_v_oi", o_v_oi_carr, {N,3}, "a");
-
+
// and biases/extrinsics
- cnpy::npz_save(out_npz_file_path, "imu_bias_fbk", imu_bias_fbk_carr, {N,6}, "a");
- cnpy::npz_save(out_npz_file_path, "imu_bias", imu_bias_carr, {N,6}, "a");
- cnpy::npz_save(out_npz_file_path, "i_pose_ib_fbk", i_pose_ib_fbk_carr, {N,7}, "a");
+ cnpy::npz_save(out_npz_file_path, "imu_bias_fbk", imu_bias_fbk_carr, {N, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "imu_bias", imu_bias_carr, {N, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "i_pose_ib_fbk", i_pose_ib_fbk_carr, {N, 7}, "a");
// covariances
cnpy::npz_save(out_npz_file_path, "tkf", tkf_carr, {Nkf}, "a");
- cnpy::npz_save(out_npz_file_path, "Qp", Qp_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qo", Qo_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qv", Qv_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qbi", Qbi_carr, {Nkf,6}, "a");
- cnpy::npz_save(out_npz_file_path, "Qm", Qm_carr, {Nkf,6}, "a");
- cnpy::npz_save(out_npz_file_path, "Qp_fbk", Qp_fbk_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qo_fbk", Qo_fbk_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qv_fbk", Qv_fbk_carr, {Nkf,3}, "a");
- cnpy::npz_save(out_npz_file_path, "Qbi_fbk", Qbi_fbk_carr, {Nkf,6}, "a");
- cnpy::npz_save(out_npz_file_path, "Qm_fbk", Qm_fbk_carr, {Nkf,6}, "a");
- cnpy::npz_save(out_npz_file_path, "fac_imu_err", fac_imu_err_carr, {Nkf,9}, "a");
- cnpy::npz_save(out_npz_file_path, "fac_pose_err", fac_pose_err_carr, {Nkf,6}, "a");
-
+ cnpy::npz_save(out_npz_file_path, "Qp", Qp_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qo", Qo_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qv", Qv_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qbi", Qbi_carr, {Nkf, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qm", Qm_carr, {Nkf, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qp_fbk", Qp_fbk_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qo_fbk", Qo_fbk_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qv_fbk", Qv_fbk_carr, {Nkf, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qbi_fbk", Qbi_fbk_carr, {Nkf, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qm_fbk", Qm_fbk_carr, {Nkf, 6}, "a");
+ cnpy::npz_save(out_npz_file_path, "fac_imu_err", fac_imu_err_carr, {Nkf, 9}, "a");
+ cnpy::npz_save(out_npz_file_path, "fac_pose_err", fac_pose_err_carr, {Nkf, 6}, "a");
}
diff --git a/demos/solo_real_povcdl_estimation.cpp b/demos/solo_real_povcdl_estimation.cpp
index a29f965..f1e6b1d 100644
--- a/demos/solo_real_povcdl_estimation.cpp
+++ b/demos/solo_real_povcdl_estimation.cpp
@@ -36,7 +36,7 @@
#include "pinocchio/algorithm/frames.hpp"
// CNPY
-#include<cnpy.h>
+#include <cnpy.h>
// WOLF
#include <core/problem/problem.h>
@@ -70,111 +70,112 @@
#include "mcapi_utils.h"
-
using namespace Eigen;
using namespace wolf;
using namespace pinocchio;
-typedef pinocchio::SE3Tpl<double> SE3;
+typedef pinocchio::SE3Tpl<double> SE3;
typedef pinocchio::ForceTpl<double> Force;
-
-int main (int argc, char **argv) {
+int main(int argc, char** argv)
+{
// retrieve parameters from yaml file
- YAML::Node config = YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/solo_real_estimation.yaml");
- std::string robot_urdf = config["robot_urdf"].as<std::string>();
- int dimc = config["dimc"].as<int>();
- double mass_offset = config["mass_offset"].as<double>();
- double dt = config["dt"].as<double>();
+ YAML::Node config =
+ YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/solo_real_estimation.yaml");
+ std::string robot_urdf = config["robot_urdf"].as<std::string>();
+ int dimc = config["dimc"].as<int>();
+ double mass_offset = config["mass_offset"].as<double>();
+ double dt = config["dt"].as<double>();
// double min_t = config["min_t"].as<double>();
- double max_t = config["max_t"].as<double>();
+ double max_t = config["max_t"].as<double>();
double solve_every_sec = config["solve_every_sec"].as<double>();
- bool solve_end = config["solve_end"].as<bool>();
+ bool solve_end = config["solve_end"].as<bool>();
// Ceres setup
int max_num_iterations = config["max_num_iterations"].as<int>();
// estimator sensor noises
- double std_pbc_est = config["std_pbc_est"].as<double>();
- double std_vbc_est = config["std_vbc_est"].as<double>();
- double std_f_est = config["std_f_est"].as<double>();
- double std_tau_est = config["std_tau_est"].as<double>();
+ double std_pbc_est = config["std_pbc_est"].as<double>();
+ double std_vbc_est = config["std_vbc_est"].as<double>();
+ double std_f_est = config["std_f_est"].as<double>();
+ double std_tau_est = config["std_tau_est"].as<double>();
double std_foot_nomove = config["std_foot_nomove"].as<double>();
-
+
// priors
- double std_prior_p = config["std_prior_p"].as<double>();
- double std_prior_o = config["std_prior_o"].as<double>();
- double std_prior_v = config["std_prior_v"].as<double>();
- double std_abs_bias_pbc = config["std_abs_bias_pbc"].as<double>();
- double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
- double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
- double std_bp_drift = config["std_bp_drift"].as<double>();
- std::vector<double> bias_pbc_prior_vec = config["bias_pbc_prior"].as<std::vector<double>>();
+ double std_prior_p = config["std_prior_p"].as<double>();
+ double std_prior_o = config["std_prior_o"].as<double>();
+ double std_prior_v = config["std_prior_v"].as<double>();
+ double std_abs_bias_pbc = config["std_abs_bias_pbc"].as<double>();
+ double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
+ double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();
+ double std_bp_drift = config["std_bp_drift"].as<double>();
+ std::vector<double> bias_pbc_prior_vec = config["bias_pbc_prior"].as<std::vector<double>>();
Eigen::Map<Vector3d> bias_pbc_prior(bias_pbc_prior_vec.data());
- std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
+ std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
Eigen::Map<Vector6d> bias_imu_prior(bias_imu_prior_vec.data());
- double fz_thresh = config["fz_thresh"].as<double>();
- std::vector<double> b_p_bi_vec = config["b_p_bi"].as<std::vector<double>>();
- std::vector<double> b_q_i_vec = config["b_q_i"].as<std::vector<double>>();
+ double fz_thresh = config["fz_thresh"].as<double>();
+ std::vector<double> b_p_bi_vec = config["b_p_bi"].as<std::vector<double>>();
+ std::vector<double> b_q_i_vec = config["b_q_i"].as<std::vector<double>>();
Eigen::Map<Vector3d> b_p_bi(b_p_bi_vec.data());
Eigen::Map<Vector4d> b_qvec_i(b_q_i_vec.data());
- std::vector<double> artificial_bias_p_vec = config["artificial_bias_p"].as<std::vector<double>>();
+ std::vector<double> artificial_bias_p_vec = config["artificial_bias_p"].as<std::vector<double>>();
Eigen::Map<Vector3d> artificial_bias_p(artificial_bias_p_vec.data());
- std::string data_file_path = config["data_file_path"].as<std::string>();
+ std::string data_file_path = config["data_file_path"].as<std::string>();
std::string out_npz_file_path = config["out_npz_file_path"].as<std::string>();
std::vector<std::string> ee_names = {"FL_ANKLE", "FR_ANKLE", "HL_ANKLE", "HR_ANKLE"};
- unsigned int nbc = ee_names.size();
+ unsigned int nbc = ee_names.size();
// Base to IMU transform
Quaterniond b_q_i(b_qvec_i);
- SE3 b_T_i(b_q_i, b_p_bi);
- SE3 i_T_b = b_T_i.inverse();
- Matrix3d b_R_i = b_T_i.rotation();
- Vector3d i_p_ib = i_T_b.translation();
- Matrix3d i_R_b = i_T_b.rotation();
+ SE3 b_T_i(b_q_i, b_p_bi);
+ SE3 i_T_b = b_T_i.inverse();
+ Matrix3d b_R_i = b_T_i.rotation();
+ Vector3d i_p_ib = i_T_b.translation();
+ Matrix3d i_R_b = i_T_b.rotation();
// initialize some vectors and fill them with dicretized data
cnpy::npz_t arr_map = cnpy::npz_load(data_file_path);
- //load it into a new array
- cnpy::NpyArray t_npyarr = arr_map["t"];
+ // load it into a new array
+ cnpy::NpyArray t_npyarr = arr_map["t"];
cnpy::NpyArray imu_acc_npyarr = arr_map["imu_acc"];
// cnpy::NpyArray o_a_oi_npyarr = arr_map["o_a_oi"];
- cnpy::NpyArray o_q_i_npyarr = arr_map["o_q_i"];
+ cnpy::NpyArray o_q_i_npyarr = arr_map["o_q_i"];
cnpy::NpyArray i_omg_oi_npyarr = arr_map["i_omg_oi"];
- cnpy::NpyArray qa_npyarr = arr_map["qa"];
- cnpy::NpyArray dqa_npyarr = arr_map["dqa"];
- cnpy::NpyArray tau_npyarr = arr_map["tau"];
+ cnpy::NpyArray qa_npyarr = arr_map["qa"];
+ cnpy::NpyArray dqa_npyarr = arr_map["dqa"];
+ cnpy::NpyArray tau_npyarr = arr_map["tau"];
cnpy::NpyArray l_forces_npyarr = arr_map["l_forces"];
// cnpy::NpyArray w_pose_wm_npyarr = arr_map["w_pose_wm"];
cnpy::NpyArray m_v_wm_npyarr = arr_map["m_v_wm"];
cnpy::NpyArray w_v_wm_npyarr = arr_map["w_v_wm"];
// cnpy::NpyArray o_R_i_npyarr = arr_map["o_R_i"];
cnpy::NpyArray w_p_wm_npyarr = arr_map["w_p_wm"];
- cnpy::NpyArray w_q_m_npyarr = arr_map["w_q_m"];
+ cnpy::NpyArray w_q_m_npyarr = arr_map["w_q_m"];
// cnpy::NpyArray w_R_m_npyarr = arr_map["w_R_m"];
- double* t_arr = t_npyarr.data<double>();
+ double* t_arr = t_npyarr.data<double>();
double* imu_acc_arr = imu_acc_npyarr.data<double>();
// double* o_a_oi_arr = o_a_oi_npyarr.data<double>();
double* i_omg_oi_arr = i_omg_oi_npyarr.data<double>();
- double* qa_arr = qa_npyarr.data<double>();
- double* dqa_arr = dqa_npyarr.data<double>();
- double* tau_arr = tau_npyarr.data<double>();
+ double* qa_arr = qa_npyarr.data<double>();
+ double* dqa_arr = dqa_npyarr.data<double>();
+ double* tau_arr = tau_npyarr.data<double>();
double* l_forces_arr = l_forces_npyarr.data<double>();
// double* w_pose_wm_arr = w_pose_wm_npyarr.data<double>();
double* m_v_wm_arr = m_v_wm_npyarr.data<double>();
double* w_v_wm_arr = w_v_wm_npyarr.data<double>();
- double* o_q_i_arr = o_q_i_npyarr.data<double>();
+ double* o_q_i_arr = o_q_i_npyarr.data<double>();
// double* o_R_i_arr = o_R_i_npyarr.data<double>();
double* w_p_wm_arr = w_p_wm_npyarr.data<double>();
- double* w_q_m_arr = w_q_m_npyarr.data<double>();
+ double* w_q_m_arr = w_q_m_npyarr.data<double>();
// double* w_R_m_arr = w_R_m_npyarr.data<double>();
unsigned int N = t_npyarr.shape[0];
- if (max_t > 0){
- N = N < int(max_t/dt) ? N : int(max_t/dt);
+ if (max_t > 0)
+ {
+ N = N < int(max_t / dt) ? N : int(max_t / dt);
}
// Creating measurements from simulated trajectory
@@ -188,24 +189,26 @@ int main (int argc, char **argv) {
model.inertias[1].mass() = model.inertias[1].mass() + mass_offset;
// add artificial lever to test bias estimation on real data
- VectorXd q_toto(19); q_toto << 0,0,0, 0,0,0,1, 0,0,0,0,0,0,0,0,0,0,0,0; // TODO: put current base orientation estimate? YES
+ VectorXd q_toto(19);
+ q_toto << 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0; // TODO: put current base orientation estimate? YES
Data data(model);
-
- centerOfMass(model, data, q_toto);
+
+ centerOfMass(model, data, q_toto);
std::cout << "artificial_bias_p: " << artificial_bias_p.transpose() << std::endl;
std::cout << "model.inertias[1].lever(): " << model.inertias[1].lever().transpose() << std::endl;
std::cout << "data.com[0]: " << data.com[0].transpose() << std::endl;
model.inertias[1] = pinocchio::Inertia(model.inertias[1].mass(), artificial_bias_p, model.inertias[1].inertia());
- centerOfMass(model, data, q_toto);
+ centerOfMass(model, data, q_toto);
std::cout << "model.inertias[1].lever(): " << model.inertias[1].lever().transpose() << std::endl;
std::cout << "data.com[0]: " << data.com[0].transpose() << std::endl;
-
// Recover end effector frame ids
std::vector<int> ee_ids;
std::cout << "Frame ids" << std::endl;
- for (auto ee_name: ee_names){
+ for (auto ee_name : ee_names)
+ {
ee_ids.push_back(model.getFrameId(ee_name));
std::cout << ee_name << std::endl;
}
@@ -219,112 +222,129 @@ int main (int argc, char **argv) {
//////////////////////
// COMPUTE INITIAL STATE
- TimeStamp t0(t_arr[0]);
- Eigen::Map<Matrix<double,12, 1>> qa(qa_arr);
- Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr);
- Eigen::Map<Vector4d> o_q_i(o_q_i_arr); // initialize with IMU orientation
- Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr); // Hyp: b=i (not the case)
- Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr);
- Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr);
+ TimeStamp t0(t_arr[0]);
+ Eigen::Map<Matrix<double, 12, 1>> qa(qa_arr);
+ Eigen::Map<Matrix<double, 12, 1>> dqa(dqa_arr);
+ Eigen::Map<Vector4d> o_q_i(o_q_i_arr); // initialize with IMU orientation
+ Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr); // Hyp: b=i (not the case)
+ Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr);
+ Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr);
w_qvec_wm.normalize(); // not close enough to wolf eps sometimes
// initial state
- VectorXd q(19); q << 0,0,0, o_q_i, qa; // TODO: put current base orientation estimate? YES
- VectorXd dq(18); dq << 0,0,0, i_omg_oi, dqa; // TODO: put current base velocity estimate? YES
+ VectorXd q(19);
+ q << 0, 0, 0, o_q_i, qa; // TODO: put current base orientation estimate? YES
+ VectorXd dq(18);
+ dq << 0, 0, 0, i_omg_oi, dqa; // TODO: put current base velocity estimate? YES
-
- centerOfMass(model, data, q, dq);
- // Vector3d w_p_wc = data.com[0];
+ centerOfMass(model, data, q, dq);
+ // Vector3d w_p_wc = data.com[0];
// Vector3d w_v_wc = data.vcom[0];
// gesticulation in base frame: just compute centroidal momentum with static q and vq
// Vector3d w_Lc = computeCentroidalMomentum(model, data, q, dq).angular();
// hyp: robot not moving initially (no vel and angular momentum)
- VectorXd x_origin; x_origin.resize(19);
+ VectorXd x_origin;
+ x_origin.resize(19);
// x_origin << w_p_wm, w_qvec_wm, 0,0,0, w_p_wc, 0,0,0, 0,0,0;
- x_origin << 0,0,0, o_q_i, 0,0,0, 0,0,0, 0,0,0, 0,0,0;
+ x_origin << 0, 0, 0, o_q_i, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
//////////////////////
//////////////////////
// CERES WRAPPER
- SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
+ SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
solver->getSolverOptions().max_num_iterations = max_num_iterations;
-
+
//===================================================== INITIALIZATION
// SENSOR + PROCESSOR INERTIAL KINEMATICS
ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
- intr_ik->std_pbc = std_pbc_est;
- intr_ik->std_vbc = std_vbc_est;
- VectorXd extr0; // default size for dynamic vector is 0-0 -> what we need
+ intr_ik->std_pbc = std_pbc_est;
+ intr_ik->std_vbc = std_vbc_est;
+ VectorXd extr0; // default size for dynamic vector is 0-0 -> what we need
SensorBasePtr sen_ikin_base = problem->installSensor("SensorInertialKinematics", "SenIK", extr0, intr_ik);
- // SensorBasePtr sen_ikin_base = problem->installSensor("SensorInertialKinematics", "SenIK", extr, bodydynamics_root_dir + "/demos/sensor_inertial_kinematics.yaml"); // TODO: does not work!
+ // SensorBasePtr sen_ikin_base = problem->installSensor("SensorInertialKinematics", "SenIK", extr,
+ // bodydynamics_root_dir + "/demos/sensor_inertial_kinematics.yaml"); // TODO: does not work!
SensorInertialKinematicsPtr sen_ikin = std::static_pointer_cast<SensorInertialKinematics>(sen_ikin_base);
ParamsProcessorInertialKinematicsPtr params_ik = std::make_shared<ParamsProcessorInertialKinematics>();
- params_ik->sensor_angvel_name = "SenImu";
- params_ik->std_bp_drift = std_bp_drift;
- ProcessorBasePtr proc_ikin_base = problem->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", sen_ikin, params_ik);
- // ProcessorBasePtr proc_ikin_base = problem->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", "SenIK", bodydynamics_root_dir + "/demos/processor_inertial_kinematics.yaml");
+ params_ik->sensor_angvel_name = "SenImu";
+ params_ik->std_bp_drift = std_bp_drift;
+ ProcessorBasePtr proc_ikin_base =
+ problem->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", sen_ikin, params_ik);
+ // ProcessorBasePtr proc_ikin_base = problem->installProcessor("ProcessorInertialKinematics",
+ // "PInertialKinematics", "SenIK", bodydynamics_root_dir + "/demos/processor_inertial_kinematics.yaml");
ProcessorInertialKinematicsPtr proc_inkin = std::static_pointer_cast<ProcessorInertialKinematics>(proc_ikin_base);
// SENSOR + PROCESSOR IMU
- SensorBasePtr sen_imu_base = problem->installSensor("SensorImu", "SenImu", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(), bodydynamics_root_dir + "/demos/sensor_imu_solo12.yaml");
- SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base);
- ProcessorBasePtr proc_ftp_base = problem->installProcessor("ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu_solo12.yaml");
+ SensorBasePtr sen_imu_base = problem->installSensor("SensorImu",
+ "SenImu",
+ (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(),
+ bodydynamics_root_dir + "/demos/sensor_imu_solo12.yaml");
+ SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base);
+ ProcessorBasePtr proc_ftp_base = problem->installProcessor(
+ "ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu_solo12.yaml");
ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_ftp_base);
// SENSOR + PROCESSOR FT PREINT
ParamsSensorForceTorquePtr intr_ft = std::make_shared<ParamsSensorForceTorque>();
- intr_ft->std_f = std_f_est;
- intr_ft->std_tau = std_tau_est;
- intr_ft->mass = data.mass[0];
- std::cout << std::setprecision(std::numeric_limits<long double>::digits10 + 1) << "\n\nROBOT MASS: " << intr_ft->mass << std::endl;
+ intr_ft->std_f = std_f_est;
+ intr_ft->std_tau = std_tau_est;
+ intr_ft->mass = data.mass[0];
+ std::cout << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+ << "\n\nROBOT MASS: " << intr_ft->mass << std::endl;
SensorBasePtr sen_ft_base = problem->installSensor("SensorForceTorque", "SenFT", VectorXd(0), intr_ft);
- // SensorBasePtr sen_ft_base = problem->installSensor("SensorForceTorque", "SenFT", VectorXd(0), bodydynamics_root_dir + "/demos/sensor_ft.yaml");
- SensorForceTorquePtr sen_ft = std::static_pointer_cast<SensorForceTorque>(sen_ft_base);
+ // SensorBasePtr sen_ft_base = problem->installSensor("SensorForceTorque", "SenFT", VectorXd(0),
+ // bodydynamics_root_dir + "/demos/sensor_ft.yaml");
+ SensorForceTorquePtr sen_ft = std::static_pointer_cast<SensorForceTorque>(sen_ft_base);
ParamsProcessorForceTorquePreintPtr params_sen_ft = std::make_shared<ParamsProcessorForceTorquePreint>();
- params_sen_ft->sensor_ikin_name = "SenIK";
- params_sen_ft->sensor_angvel_name = "SenImu";
- params_sen_ft->nbc = nbc;
- params_sen_ft->dimc = dimc;
- params_sen_ft->time_tolerance = 0.0005;
- params_sen_ft->unmeasured_perturbation_std = 0.000001;
- params_sen_ft->max_time_span = 1000;
- params_sen_ft->max_buff_length = 500;
- params_sen_ft->dist_traveled = 20000.0;
- params_sen_ft->angle_turned = 1000;
- params_sen_ft->voting_active = false;
- ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", sen_ft, params_sen_ft);
- // ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint.yaml");
+ params_sen_ft->sensor_ikin_name = "SenIK";
+ params_sen_ft->sensor_angvel_name = "SenImu";
+ params_sen_ft->nbc = nbc;
+ params_sen_ft->dimc = dimc;
+ params_sen_ft->time_tolerance = 0.0005;
+ params_sen_ft->unmeasured_perturbation_std = 0.000001;
+ params_sen_ft->max_time_span = 1000;
+ params_sen_ft->max_buff_length = 500;
+ params_sen_ft->dist_traveled = 20000.0;
+ params_sen_ft->angle_turned = 1000;
+ params_sen_ft->voting_active = false;
+ ProcessorBasePtr proc_ft_base =
+ problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", sen_ft, params_sen_ft);
+ // ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", "SenFT",
+ // bodydynamics_root_dir + "/demos/processor_ft_preint.yaml");
ProcessorForceTorquePreintPtr proc_ftpreint = std::static_pointer_cast<ProcessorForceTorquePreint>(proc_ft_base);
// SENSOR + PROCESSOR POINT FEET NOMOVE
ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>();
- intr_pfn->std_foot_nomove_ = std_foot_nomove;
- VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
+ intr_pfn->std_foot_nomove_ = std_foot_nomove;
+ VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
SensorBasePtr sen_pfnm_base = problem->installSensor("SensorPointFeetNomove", "SenPfnm", extr, intr_pfn);
- SensorPointFeetNomovePtr sen_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base);
+ SensorPointFeetNomovePtr sen_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base);
ParamsProcessorPointFeetNomovePtr params_pfnm = std::make_shared<ParamsProcessorPointFeetNomove>();
- ProcessorBasePtr proc_pfnm_base = problem->installProcessor("ProcessorPointFeetNomove", "PPointFeetNomove", sen_pfnm, params_pfnm);
-
+ ProcessorBasePtr proc_pfnm_base =
+ problem->installProcessor("ProcessorPointFeetNomove", "PPointFeetNomove", sen_pfnm, params_pfnm);
// SETPRIOR RETRO-ENGINEERING
// We are not using setPrior because of processors initial captures problems so we have to
// - Manually create the first KF and its pose and velocity priors
- // - call setOrigin on processors MotionProvider since the flag prior_is_set is not true when doing installProcessor (later)
-
- MatrixXd P_origin(9,9); P_origin.setIdentity();
- P_origin.block<3,3>(0,0) = pow(std_prior_p, 2) * Matrix3d::Identity();
- P_origin.block<3,3>(3,3) = pow(std_prior_o, 2) * Matrix3d::Identity();
- P_origin.block<3,3>(6,6) = pow(std_prior_v, 2) * Matrix3d::Identity();
- FrameBasePtr KF1 = problem->emplaceFrame(t0, x_origin);
+ // - call setOrigin on processors MotionProvider since the flag prior_is_set is not true when doing
+ // installProcessor (later)
+
+ MatrixXd P_origin(9, 9);
+ P_origin.setIdentity();
+ P_origin.block<3, 3>(0, 0) = pow(std_prior_p, 2) * Matrix3d::Identity();
+ P_origin.block<3, 3>(3, 3) = pow(std_prior_o, 2) * Matrix3d::Identity();
+ P_origin.block<3, 3>(6, 6) = pow(std_prior_v, 2) * Matrix3d::Identity();
+ FrameBasePtr KF1 = problem->emplaceFrame(t0, x_origin);
// Prior pose factor
- CapturePosePtr pose_prior_capture = CaptureBase::emplace<CapturePose>(KF1, t0, nullptr, x_origin.head(7), P_origin.topLeftCorner(6, 6));
+ CapturePosePtr pose_prior_capture =
+ CaptureBase::emplace<CapturePose>(KF1, t0, nullptr, x_origin.head(7), P_origin.topLeftCorner(6, 6));
pose_prior_capture->emplaceFeatureAndFactor();
// Prior velocity factor
CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF1, "Vel0", t0);
- FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.segment<3>(7), P_origin.block<3, 3>(6, 6));
+ FeatureBasePtr featV0 =
+ FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.segment<3>(7), P_origin.block<3, 3>(6, 6));
FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, featV0, KF1->getV(), nullptr, false);
// Special trick to make things work in the IMU + IKin + FTPreint case
@@ -337,21 +357,24 @@ int main (int argc, char **argv) {
// proc_legodom->setOrigin(KF1);
// INERTIAL KINEMATICS CAPTURE DATA ZERO MOTION
- VectorXd q_static = q; q_static.head<7>() << 0,0,0, 0,0,0,1;
- VectorXd dq_static = dq; dq_static.head<6>() << 0,0,0, 0,0,0;
- centerOfMass(model, data, q_static, dq_static);
- Vector3d b_p_bc = data.com[0]; // meas
+ VectorXd q_static = q;
+ q_static.head<7>() << 0, 0, 0, 0, 0, 0, 1;
+ VectorXd dq_static = dq;
+ dq_static.head<6>() << 0, 0, 0, 0, 0, 0;
+ centerOfMass(model, data, q_static, dq_static);
+ Vector3d b_p_bc = data.com[0]; // meas
Vector3d i_p_ic = i_T_b.act(b_p_bc);
- Vector3d i_v_ic = i_R_b*data.vcom[0]; // meas
- Matrix3d b_Iw = compute_Iw(model, data, q_static, b_p_bc);
- Matrix3d i_Iw = i_R_b*b_Iw*b_R_i; // !! rotate on both sides to keep a symmetric matrix
+ Vector3d i_v_ic = i_R_b * data.vcom[0]; // meas
+ Matrix3d b_Iw = compute_Iw(model, data, q_static, b_p_bc);
+ Matrix3d i_Iw = i_R_b * b_Iw * b_R_i; // !! rotate on both sides to keep a symmetric matrix
// Vector3d b_Lc_gesti = computeCentroidalMomentum(model, data, q_static, dq_static).angular();
- auto b_centro_momentum_gesti = computeCentroidalMomentum(model, data, q_static, dq_static);
- auto i_centro_momentum_gesti = i_T_b.act(b_centro_momentum_gesti);
- Vector3d i_Lc_gesti = i_centro_momentum_gesti.angular();
+ auto b_centro_momentum_gesti = computeCentroidalMomentum(model, data, q_static, dq_static);
+ auto i_centro_momentum_gesti = i_T_b.act(b_centro_momentum_gesti);
+ Vector3d i_Lc_gesti = i_centro_momentum_gesti.angular();
// momentum parameters
- VectorXd meas_ikin(9); meas_ikin << i_p_ic, i_v_ic, i_omg_oi;
+ VectorXd meas_ikin(9);
+ meas_ikin << i_p_ic, i_v_ic, i_omg_oi;
auto CIKin0 = std::make_shared<CaptureInertialKinematics>(t0, sen_ikin, meas_ikin, i_Iw, i_Lc_gesti);
CIKin0->process();
proc_ftpreint->setOrigin(KF1);
@@ -359,86 +382,93 @@ int main (int argc, char **argv) {
////////////////////////////////////////////
// INITIAL BIAS FACTORS
// Add absolute factor on Imu biases to simulate a fix()
- Vector6d std_abs_imu; std_abs_imu << std_abs_bias_acc*Vector3d::Ones(), std_abs_bias_gyro*Vector3d::Ones();
- Matrix6d Q_bi = std_abs_imu.array().square().matrix().asDiagonal();
- CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
+ Vector6d std_abs_imu;
+ std_abs_imu << std_abs_bias_acc * Vector3d::Ones(), std_abs_bias_gyro * Vector3d::Ones();
+ Matrix6d Q_bi = std_abs_imu.array().square().matrix().asDiagonal();
+ CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", bias_imu_prior, Q_bi);
- FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
+ FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(
+ featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
sen_imu->getStateBlock('I')->setState(bias_imu_prior);
- // Add loose absolute factor on initial bp bias since dynamical trajectories
- Matrix3d Q_bp = pow(std_abs_bias_pbc, 2)* Matrix3d::Identity();
- CaptureBasePtr cap_bp0 = CaptureBase::emplace<CaptureBase>(KF1, "bp0", t0);
+ // Add loose absolute factor on initial bp bias since dynamical trajectories
+ Matrix3d Q_bp = pow(std_abs_bias_pbc, 2) * Matrix3d::Identity();
+ CaptureBasePtr cap_bp0 = CaptureBase::emplace<CaptureBase>(KF1, "bp0", t0);
FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc_prior, Q_bp);
- FactorBasePtr fac_bp0 = FactorBase::emplace<FactorBlockAbsolute>(feat_bp0, feat_bp0, KF1->getCaptureOf(sen_ikin)->getSensorIntrinsic(), nullptr, false);
+ FactorBasePtr fac_bp0 = FactorBase::emplace<FactorBlockAbsolute>(
+ feat_bp0, feat_bp0, KF1->getCaptureOf(sen_ikin)->getSensorIntrinsic(), nullptr, false);
sen_ikin->getStateBlock('I')->setState(bias_pbc_prior);
-
///////////////////////////////////////////
// process measurements in sequential order
double ts_since_last_kf = 0;
// for point feet factor
std::vector<Vector3d> i_p_il_vec_origin;
- FrameBasePtr KF_origin = KF1;
+ FrameBasePtr KF_origin = KF1;
//////////////////////////////////////////
- // Vectors to store estimates at the time of data processing
+ // Vectors to store estimates at the time of data processing
// fbk -> feedback: to check if the estimation is stable enough for feedback control
- std::vector<Vector3d> p_ob_fbk_v;
- std::vector<Vector4d> q_ob_fbk_v;
- std::vector<Vector3d> v_ob_fbk_v;
- std::vector<Vector3d> c_ob_fbk_v;
- std::vector<Vector3d> cd_ob_fbk_v;
- std::vector<Vector3d> Lc_ob_fbk_v;
-
- std::vector<Vector3d> i_omg_oi_v;
+ std::vector<Vector3d> p_ob_fbk_v;
+ std::vector<Vector4d> q_ob_fbk_v;
+ std::vector<Vector3d> v_ob_fbk_v;
+ std::vector<Vector3d> c_ob_fbk_v;
+ std::vector<Vector3d> cd_ob_fbk_v;
+ std::vector<Vector3d> Lc_ob_fbk_v;
+
+ std::vector<Vector3d> i_omg_oi_v;
//////////////////////////////////////////
unsigned int nb_kf = 1;
- for (unsigned int i=0; i < N; i++){
+ for (unsigned int i = 0; i < N; i++)
+ {
TimeStamp ts(t_arr[i]);
////////////////////////////////////
- // Retrieve current state
- VectorComposite curr_state = problem->getState();
- Vector4d o_qvec_i_curr = curr_state['O'];
- Quaterniond o_q_i_curr(o_qvec_i_curr);
- Vector3d o_v_oi_curr = curr_state['V'];
+ // Retrieve current state
+ VectorComposite curr_state = problem->getState();
+ Vector4d o_qvec_i_curr = curr_state['O'];
+ Quaterniond o_q_i_curr(o_qvec_i_curr);
+ Vector3d o_v_oi_curr = curr_state['V'];
////////////////////////////////////
////////////////////////////////////
// Get measurements
// retrieve traj data
- Eigen::Map<Vector3d> imu_acc(imu_acc_arr+3*i);
- Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr+3*i); // Hyp: b=i (not the case)
- Eigen::Map<Matrix<double,12, 1>> tau(tau_arr+12*i);
- Eigen::Map<Matrix<double,12, 1>> qa(qa_arr+12*i);
- Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr+12*i);
+ Eigen::Map<Vector3d> imu_acc(imu_acc_arr + 3 * i);
+ Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr + 3 * i); // Hyp: b=i (not the case)
+ Eigen::Map<Matrix<double, 12, 1>> tau(tau_arr + 12 * i);
+ Eigen::Map<Matrix<double, 12, 1>> qa(qa_arr + 12 * i);
+ Eigen::Map<Matrix<double, 12, 1>> dqa(dqa_arr + 12 * i);
// store i_omg_oi for later computation of o_v_ob from o_v_oi
i_omg_oi_v.push_back(i_omg_oi);
////////////////////////////////////
// IMU
- // imu_acc << -wolf::gravity();
+ // imu_acc << -wolf::gravity();
// i_omg_oi << 0,0,0;
- Vector6d acc_gyr_meas; acc_gyr_meas << imu_acc, i_omg_oi;
+ Vector6d acc_gyr_meas;
+ acc_gyr_meas << imu_acc, i_omg_oi;
Matrix6d acc_gyr_cov = sen_imu->getNoiseCov();
////////////////////////////////////
// Kinematics + forces
- SE3 o_T_i(o_q_i_curr, Vector3d::Zero()); // TODO: put current orientation estimate here
- Matrix3d o_R_i = o_T_i.rotation();
- Matrix3d i_R_o = o_R_i.transpose();
- Vector3d i_v_oi_curr = i_R_o * o_v_oi_curr;
- Vector3d i_acc = imu_acc + i_R_o * gravity();
- Vector3d i_asp_i = i_acc - i_omg_oi.cross(i_v_oi_curr);
-
- VectorXd q(19); q << 0,0,0, o_q_i_curr.coeffs(), qa;
+ SE3 o_T_i(o_q_i_curr, Vector3d::Zero()); // TODO: put current orientation estimate here
+ Matrix3d o_R_i = o_T_i.rotation();
+ Matrix3d i_R_o = o_R_i.transpose();
+ Vector3d i_v_oi_curr = i_R_o * o_v_oi_curr;
+ Vector3d i_acc = imu_acc + i_R_o * gravity();
+ Vector3d i_asp_i = i_acc - i_omg_oi.cross(i_v_oi_curr);
+
+ VectorXd q(19);
+ q << 0, 0, 0, o_q_i_curr.coeffs(), qa;
// VectorXd dq(18); dq << i_v_oi_curr, i_omg_oi, dqa;
- VectorXd dq(18); dq << 0,0,0, i_omg_oi, dqa;
- VectorXd ddq(model.nv); ddq.setZero();
+ VectorXd dq(18);
+ dq << 0, 0, 0, i_omg_oi, dqa;
+ VectorXd ddq(model.nv);
+ ddq.setZero();
ddq.head<3>() = i_asp_i;
// TODO: add other terms to compute forces (ddqa...)
@@ -447,46 +477,52 @@ int main (int argc, char **argv) {
updateFramePlacements(model, data);
// compute all linear jacobians in feet frames (only for quadruped)
- MatrixXd Jlinvel(12, model.nv); Jlinvel.setZero();
- for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
- MatrixXd Jee(6, model.nv); Jee.setZero();
+ MatrixXd Jlinvel(12, model.nv);
+ Jlinvel.setZero();
+ for (unsigned int i_ee = 0; i_ee < nbc; i_ee++)
+ {
+ MatrixXd Jee(6, model.nv);
+ Jee.setZero();
computeFrameJacobian(model, data, q, ee_ids[i_ee], Jee);
- Jlinvel.block(3*i_ee, 0, 3, model.nv) = Jee.topRows<3>();
+ Jlinvel.block(3 * i_ee, 0, 3, model.nv) = Jee.topRows<3>();
}
// estimate forces from torques
- VectorXd tau_cf = rnea(model, data, q, dq, ddq); // compute total torques applied on the joints (and free flyer)
- tau_cf.tail(model.nv-6) -= tau; // remove measured joint torque (not on the free flyer)
+ VectorXd tau_cf =
+ rnea(model, data, q, dq, ddq); // compute total torques applied on the joints (and free flyer)
+ tau_cf.tail(model.nv - 6) -= tau; // remove measured joint torque (not on the free flyer)
- // VectorXd forces = Jlinvel_reduced.transpose().lu().solve(tau_joints); // works only for exactly determined system, removing free flyer from the system -> 12x12 J mat
- // Solve in Least square sense (3 ways):
- // VectorXd forces = Jlinvel.transpose().bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(tau_cf); // SVD
+ // VectorXd forces = Jlinvel_reduced.transpose().lu().solve(tau_joints); // works only for exactly determined
+ // system, removing free flyer from the system -> 12x12 J mat Solve in Least square sense (3 ways): VectorXd
+ // forces = Jlinvel.transpose().bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(tau_cf); // SVD
// VectorXd l_forces = Jlinvel.transpose().colPivHouseholderQr().solve(tau_cf); // QR
- // (A.transpose() * A).ldlt().solve(A.transpose() * b) // solve the normal equation (twice less accurate than other 2)
+ // (A.transpose() * A).ldlt().solve(A.transpose() * b) // solve the normal equation (twice less accurate than
+ // other 2)
// Or else, retrieve from preprocessed dataset
- Eigen::Map<Matrix<double,12, 1>> l_forces(l_forces_arr+12*i);
+ Eigen::Map<Matrix<double, 12, 1>> l_forces(l_forces_arr + 12 * i);
-
- Vector3d o_f_tot = Vector3d::Zero();
+ Vector3d o_f_tot = Vector3d::Zero();
std::vector<Vector3d> l_f_l_vec;
std::vector<Vector3d> o_f_l_vec;
std::vector<Vector3d> i_p_il_vec;
std::vector<Vector4d> i_qvec_l_vec;
// needing relative kinematics measurements
- VectorXd q_static = q; q_static.head<7>() << 0,0,0, 0,0,0,1;
+ VectorXd q_static = q;
+ q_static.head<7>() << 0, 0, 0, 0, 0, 0, 1;
forwardKinematics(model, data, q_static);
updateFramePlacements(model, data);
- for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
- auto b_T_l = data.oMf[ee_ids[i_ee]];
- auto i_T_l = i_T_b * b_T_l;
- Vector3d i_p_il = i_T_l.translation(); // meas
+ for (unsigned int i_ee = 0; i_ee < nbc; i_ee++)
+ {
+ auto b_T_l = data.oMf[ee_ids[i_ee]];
+ auto i_T_l = i_T_b * b_T_l;
+ Vector3d i_p_il = i_T_l.translation(); // meas
Vector4d i_qvec_l = Quaterniond(i_T_l.rotation()).coeffs(); // meas
- Vector3d l_f_l = l_forces.segment(3*i_ee, 3); // meas
- Vector3d o_f_l = o_R_i*i_T_l.rotation() * l_f_l; // for contact test
+ Vector3d l_f_l = l_forces.segment(3 * i_ee, 3); // meas
+ Vector3d o_f_l = o_R_i * i_T_l.rotation() * l_f_l; // for contact test
o_f_tot += o_f_l;
- // // put the measure to a constant for unit testing
+ // // put the measure to a constant for unit testing
// i_p_il << 0,0,0;
// i_qvec_l << 0,0,0,1;
// l_f_l << -(intr_ft->mass-0.01)*wolf::gravity()/4;
@@ -502,30 +538,30 @@ int main (int argc, char **argv) {
// i_p_il_vec[3] << 0,-1, 0;
// initialization for point feet factor
- if (i == 0){
+ if (i == 0)
+ {
i_p_il_vec_origin = i_p_il_vec;
}
// std::cout << "o_f_tot: " << o_f_tot.transpose() << std::endl;
// std::cout << "o_f_tot + m*g: " << (o_f_tot + data.mass[0]*model.gravity.linear()).transpose() << std::endl;
-
//////////////////////////////////////////////
// COM relative position and velocity measures
- centerOfMass(model, data, q_static, dq_static);
- Vector3d b_p_bc = data.com[0];
- // std::cout << "b_p_bc: " << b_p_bc.transpose() << std::endl;
+ centerOfMass(model, data, q_static, dq_static);
+ Vector3d b_p_bc = data.com[0];
+ // std::cout << "b_p_bc: " << b_p_bc.transpose() << std::endl;
Vector3d i_p_ic = i_T_b.act(b_p_bc); // meas
- // velocity due to gesticulation only in base frame
- // -> set world frame = base frame and set body frame spatial vel to zero
- Vector3d i_v_ic = i_R_b*data.vcom[0]; // meas
- // Centroidal inertia and gesticulation kinetic momentum
+ // velocity due to gesticulation only in base frame
+ // -> set world frame = base frame and set body frame spatial vel to zero
+ Vector3d i_v_ic = i_R_b * data.vcom[0]; // meas
+ // Centroidal inertia and gesticulation kinetic momentum
Matrix3d b_Iw = compute_Iw(model, data, q_static, b_p_bc);
- Matrix3d i_Iw = i_R_b*b_Iw*b_R_i; // !! rotate on both sides to keep a symmetric matrix
+ Matrix3d i_Iw = i_R_b * b_Iw * b_R_i; // !! rotate on both sides to keep a symmetric matrix
// gesticulation in base frame: just compute centroidal momentum with static q and vq
// Vector3d b_Lc_gesti = computeCentroidalMomentum(model, data, q_static, dq_static).angular();
- auto b_centro_momentum_gesti = computeCentroidalMomentum(model, data, q_static, dq_static);
- auto i_centro_momentum_gesti = i_T_b.act(b_centro_momentum_gesti);
- Vector3d i_Lc_gesti = i_centro_momentum_gesti.angular();
+ auto b_centro_momentum_gesti = computeCentroidalMomentum(model, data, q_static, dq_static);
+ auto i_centro_momentum_gesti = i_T_b.act(b_centro_momentum_gesti);
+ Vector3d i_Lc_gesti = i_centro_momentum_gesti.angular();
// Inertial kinematics
meas_ikin << i_p_ic, i_v_ic, i_omg_oi;
@@ -534,22 +570,23 @@ int main (int argc, char **argv) {
// Force Torque
////////////////////
- VectorXd f_meas(nbc*3);
- VectorXd tau_meas(nbc*3);
- VectorXd kin_meas(nbc*7);
- for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
- f_meas.segment<3>(3*i_ee) = l_f_l_vec[i_ee];
- kin_meas.segment<3>(i_ee*3) = i_p_il_vec[i_ee];
- kin_meas.segment<4>(nbc*3 + i_ee*4) = i_qvec_l_vec[i_ee];
+ VectorXd f_meas(nbc * 3);
+ VectorXd tau_meas(nbc * 3);
+ VectorXd kin_meas(nbc * 7);
+ for (unsigned int i_ee = 0; i_ee < nbc; i_ee++)
+ {
+ f_meas.segment<3>(3 * i_ee) = l_f_l_vec[i_ee];
+ kin_meas.segment<3>(i_ee * 3) = i_p_il_vec[i_ee];
+ kin_meas.segment<4>(nbc * 3 + i_ee * 4) = i_qvec_l_vec[i_ee];
}
- VectorXd cap_ftp_data(dimc*nbc+3+3+nbc*7);
- MatrixXd Qftp(dimc*nbc+3+3,dimc*nbc+3+3); // kin not in covariance mat
+ VectorXd cap_ftp_data(dimc * nbc + 3 + 3 + nbc * 7);
+ MatrixXd Qftp(dimc * nbc + 3 + 3, dimc * nbc + 3 + 3); // kin not in covariance mat
cap_ftp_data << f_meas, i_p_ic, i_omg_oi, kin_meas;
Qftp.setIdentity();
- Qftp.block(0,0, nbc*3,nbc*3) *= pow(std_f_est, 2);
- Qftp.block<3, 3>(nbc*dimc, nbc*dimc, 3,3) *= pow(sen_ikin->getPbcNoiseStd(), 2);
- Qftp.block<3, 3>(nbc*dimc+3,nbc*dimc+3, 3,3) = acc_gyr_cov.bottomRightCorner<3,3>();
+ Qftp.block(0, 0, nbc * 3, nbc * 3) *= pow(std_f_est, 2);
+ Qftp.block<3, 3>(nbc * dimc, nbc * dimc, 3, 3) *= pow(sen_ikin->getPbcNoiseStd(), 2);
+ Qftp.block<3, 3>(nbc * dimc + 3, nbc * dimc + 3, 3, 3) = acc_gyr_cov.bottomRightCorner<3, 3>();
////////////////////
CaptureImuPtr CImu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, acc_gyr_cov);
@@ -559,16 +596,17 @@ int main (int argc, char **argv) {
auto CFTpreint = std::make_shared<CaptureForceTorquePreint>(ts, sen_ft, CIKin, CImu, cap_ftp_data, Qftp);
CFTpreint->process();
-
// 1) detect feet in contact
- std::vector<int> feet_in_contact;
- int nb_feet_in_contact = 0;
+ std::vector<int> feet_in_contact;
+ int nb_feet_in_contact = 0;
std::unordered_map<int, Vector3d> kin_incontact;
- for (unsigned int i_ee=0; i_ee<nbc; i_ee++){
+ for (unsigned int i_ee = 0; i_ee < nbc; i_ee++)
+ {
// basic contact detection based on normal foot vel, things break if no foot is in contact
// std::cout << "F" << ee_names[i_ee] << " norm: " << wrench_meas_v[i_ee][i].norm() << std::endl;
// std::cout << "o_f_l_vec[i_ee]: " << o_f_l_vec[i_ee].transpose() << std::endl;
- if (o_f_l_vec[i_ee](2) > fz_thresh){
+ if (o_f_l_vec[i_ee](2) > fz_thresh)
+ {
feet_in_contact.push_back(i_ee);
nb_feet_in_contact++;
kin_incontact.insert({i_ee, i_p_il_vec[i_ee]});
@@ -579,9 +617,6 @@ int main (int argc, char **argv) {
// CapturePointFeetNomovePtr CPF = std::make_shared<CapturePointFeetNomove>(ts, sen_pfnm, kin_incontact);
// CPF->process();
-
-
-
// add zero vel artificial factor
if (problem->getTrajectory()->size() > nb_kf)
{
@@ -589,17 +624,17 @@ int main (int argc, char **argv) {
std::cout << "New KF " << kf->getTimeStamp() << std::endl;
// CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(kf, "Vel0", kf->getTimeStamp());
- // FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", Vector3d::Zero(), 0.00001*Matrix3d::Ones());
- // FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, featV0, kf->getV(), nullptr, false);
+ // FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", Vector3d::Zero(),
+ // 0.00001*Matrix3d::Ones()); FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0,
+ // featV0, kf->getV(), nullptr, false);
nb_kf++;
KF_origin = kf;
}
-
-
ts_since_last_kf += dt;
- if ((solve_every_sec > 0) && (ts_since_last_kf >= solve_every_sec)){
+ if ((solve_every_sec > 0) && (ts_since_last_kf >= solve_every_sec))
+ {
// problem->print(4,true,true,true);
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
std::cout << report << std::endl;
@@ -607,16 +642,16 @@ int main (int argc, char **argv) {
}
// Store current state estimation
- VectorComposite state_fbk = problem->getState(ts);
- Vector3d o_p_oi_new = state_fbk['P'];
- Vector4d o_qvec_i_new = state_fbk['O'];
- Vector3d o_v_oi_new = state_fbk['V'];
+ VectorComposite state_fbk = problem->getState(ts);
+ Vector3d o_p_oi_new = state_fbk['P'];
+ Vector4d o_qvec_i_new = state_fbk['O'];
+ Vector3d o_v_oi_new = state_fbk['V'];
- Matrix3d o_R_i_new = Quaterniond(o_qvec_i_new).toRotationMatrix();
- Matrix3d o_R_b_new = o_R_i_new * i_R_b;
+ Matrix3d o_R_i_new = Quaterniond(o_qvec_i_new).toRotationMatrix();
+ Matrix3d o_R_b_new = o_R_i_new * i_R_b;
Vector4d o_qvec_b_new = Quaterniond(o_R_b_new).coeffs();
- Vector3d o_p_ob_new = o_p_oi_new + o_R_i_new * i_p_ib;
- Vector3d o_v_ob_new = o_v_oi_new + o_R_b_new * i_omg_oi.cross(b_p_bi);
+ Vector3d o_p_ob_new = o_p_oi_new + o_R_i_new * i_p_ib;
+ Vector3d o_v_ob_new = o_v_oi_new + o_R_b_new * i_omg_oi.cross(b_p_bi);
p_ob_fbk_v.push_back(o_p_ob_new);
q_ob_fbk_v.push_back(o_qvec_b_new);
@@ -625,16 +660,15 @@ int main (int argc, char **argv) {
cd_ob_fbk_v.push_back(state_fbk['D']);
Lc_ob_fbk_v.push_back(state_fbk['L']);
}
-
-
///////////////////////////////////////////
//////////////// SOLVING //////////////////
///////////////////////////////////////////
- problem->print(4,true,true,true);
- if (solve_end){
+ problem->print(4, true, true, true);
+ if (solve_end)
+ {
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem->print(4,true,true,true);
+ problem->print(4, true, true, true);
std::cout << report << std::endl;
}
@@ -643,40 +677,44 @@ int main (int argc, char **argv) {
// STORE DATA //
//////////////////////////////////////
//////////////////////////////////////
- std::fstream file_est;
- std::fstream file_fbk;
- std::fstream file_kfs;
- std::fstream file_cov;
- file_est.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/est.csv", std::fstream::out);
- file_fbk.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/fbk.csv", std::fstream::out);
- file_kfs.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/kfs.csv", std::fstream::out);
- file_cov.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/cov.csv", std::fstream::out);
+ std::fstream file_est;
+ std::fstream file_fbk;
+ std::fstream file_kfs;
+ std::fstream file_cov;
+ file_est.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/est.csv", std::fstream::out);
+ file_fbk.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/fbk.csv", std::fstream::out);
+ file_kfs.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/kfs.csv", std::fstream::out);
+ file_cov.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/cov.csv", std::fstream::out);
std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,cx,cy,cz,cdx,cdy,cdz,Lx,Ly,Lz\n";
file_est << header_est;
file_fbk << header_est;
- std::string header_kfs = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax,bay,baz,bwx,bwy,bwz,cx,cy,cz,cdx,cdy,cdz,Lx,Ly,Lz,bpx,bpy,bpz\n";
+ std::string header_kfs =
+ "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax,bay,baz,bwx,bwy,bwz,cx,cy,cz,cdx,cdy,cdz,Lx,Ly,Lz,bpx,bpy,bpz\n";
file_kfs << header_kfs;
- std::string header_cov = "t,Qpx,Qpy,Qpz,Qqx,Qqy,Qqz,Qvx,Qvy,Qvz,Qbax,Qbay,Qbaz,Qbwx,Qbwy,Qbwz,Qcx,Qcy,Qcz,Qcdx,Qcdy,Qcdz,QLx,QLy,QLz,Qbpx,Qbpy,Qbpz\n";
- file_cov << header_cov;
+ std::string header_cov =
+ "t,Qpx,Qpy,Qpz,Qqx,Qqy,Qqz,Qvx,Qvy,Qvz,Qbax,Qbay,Qbaz,Qbwx,Qbwy,Qbwz,Qcx,Qcy,Qcz,Qcdx,Qcdy,Qcdz,QLx,QLy,QLz,"
+ "Qbpx,Qbpy,Qbpz\n";
+ file_cov << header_cov;
VectorComposite state_est;
- double o_p_ob_carr[3*N];
- double o_q_b_carr[4*N];
- double o_v_ob_carr[3*N];
- double o_p_oc_carr[3*N];
- double o_v_oc_carr[3*N];
- double o_Lc_carr[3*N];
- for (unsigned int i=0; i < N; i++) {
- state_est = problem->getState(t_arr[i]);
+ double o_p_ob_carr[3 * N];
+ double o_q_b_carr[4 * N];
+ double o_v_ob_carr[3 * N];
+ double o_p_oc_carr[3 * N];
+ double o_v_oc_carr[3 * N];
+ double o_Lc_carr[3 * N];
+ for (unsigned int i = 0; i < N; i++)
+ {
+ state_est = problem->getState(t_arr[i]);
Vector3d i_omg_oi = i_omg_oi_v[i];
- Vector3d o_p_oi = state_est['P'];
+ Vector3d o_p_oi = state_est['P'];
Vector4d o_qvec_i = state_est['O'];
- Vector3d o_v_oi = state_est['V'];
+ Vector3d o_v_oi = state_est['V'];
Vector6d bias_acc_gyro = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
- Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
- Matrix3d o_R_b = o_R_i * i_R_b;
+ Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+ Matrix3d o_R_b = o_R_i * i_R_b;
Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
// std::cout << "o_p_oi: " << o_p_oi.transpose() << std::endl;
// std::cout << "i_p_ib: " << i_p_ib.transpose() << std::endl;
@@ -687,99 +725,62 @@ int main (int argc, char **argv) {
Vector3d o_p_oc = state_est['C'];
Vector3d o_v_oc = state_est['D'];
- Vector3d o_Lc = state_est['L'];
-
- mempcpy(o_p_ob_carr+3*i, o_p_ob.data(), 3*sizeof(double));
- mempcpy(o_q_b_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
- mempcpy(o_v_ob_carr+3*i, o_v_ob.data(), 3*sizeof(double));
- mempcpy(o_p_oc_carr+3*i, o_p_oc.data(), 3*sizeof(double));
- mempcpy(o_v_oc_carr+3*i, o_v_oc.data(), 3*sizeof(double));
- mempcpy(o_Lc_carr +3*i, o_Lc.data(), 3*sizeof(double));
-
- file_est << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << t_arr[i] << ","
- << o_p_ob(0) << ","
- << o_p_ob(1) << ","
- << o_p_ob(2) << ","
- << o_qvec_b(0) << ","
- << o_qvec_b(1) << ","
- << o_qvec_b(2) << ","
- << o_qvec_b(3) << ","
- << o_v_ob(0) << ","
- << o_v_ob(1) << ","
- << o_v_ob(2) << ","
-
- << o_p_oc(0) << ","
- << o_p_oc(1) << ","
- << o_p_oc(2) << ","
- << o_v_oc(0) << ","
- << o_v_oc(1) << ","
- << o_v_oc(2) << ","
- << o_Lc(0) << ","
- << o_Lc(1) << ","
- << o_Lc(2)
- << "\n";
+ Vector3d o_Lc = state_est['L'];
+
+ mempcpy(o_p_ob_carr + 3 * i, o_p_ob.data(), 3 * sizeof(double));
+ mempcpy(o_q_b_carr + 4 * i, o_qvec_b.data(), 4 * sizeof(double));
+ mempcpy(o_v_ob_carr + 3 * i, o_v_ob.data(), 3 * sizeof(double));
+ mempcpy(o_p_oc_carr + 3 * i, o_p_oc.data(), 3 * sizeof(double));
+ mempcpy(o_v_oc_carr + 3 * i, o_v_oc.data(), 3 * sizeof(double));
+ mempcpy(o_Lc_carr + 3 * i, o_Lc.data(), 3 * sizeof(double));
+
+ file_est << std::setprecision(std::numeric_limits<long double>::digits10 + 1) << t_arr[i] << "," << o_p_ob(0)
+ << "," << o_p_ob(1) << "," << o_p_ob(2) << "," << o_qvec_b(0) << "," << o_qvec_b(1) << ","
+ << o_qvec_b(2) << "," << o_qvec_b(3) << "," << o_v_ob(0) << "," << o_v_ob(1) << "," << o_v_ob(2)
+ << ","
+
+ << o_p_oc(0) << "," << o_p_oc(1) << "," << o_p_oc(2) << "," << o_v_oc(0) << "," << o_v_oc(1) << ","
+ << o_v_oc(2) << "," << o_Lc(0) << "," << o_Lc(1) << "," << o_Lc(2) << "\n";
}
-
VectorComposite kf_state;
- CaptureBasePtr cap_ikin;
+ CaptureBasePtr cap_ikin;
VectorComposite bp_bias_est;
- CaptureBasePtr cap_imu;
+ CaptureBasePtr cap_imu;
VectorComposite bi_bias_est;
- for (auto elt: problem->getTrajectory()->getFrameMap())
+ for (auto elt : problem->getTrajectory()->getFrameMap())
{
- auto kf = elt.second;
- kf_state = kf->getState();
- cap_ikin = kf->getCaptureOf(sen_ikin);
+ auto kf = elt.second;
+ kf_state = kf->getState();
+ cap_ikin = kf->getCaptureOf(sen_ikin);
bp_bias_est = cap_ikin->getState();
- cap_imu = kf->getCaptureOf(sen_imu);
+ cap_imu = kf->getCaptureOf(sen_imu);
bi_bias_est = cap_imu->getState();
- file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << kf->getTimeStamp().get() << ","
- << kf_state['P'](0) << ","
- << kf_state['P'](1) << ","
- << kf_state['P'](2) << ","
- << kf_state['O'](0) << ","
- << kf_state['O'](1) << ","
- << kf_state['O'](2) << ","
- << kf_state['O'](3) << ","
- << kf_state['V'](0) << ","
- << kf_state['V'](1) << ","
- << kf_state['V'](2) << ","
- << bi_bias_est['I'](0) << ","
- << bi_bias_est['I'](1) << ","
- << bi_bias_est['I'](2) << ","
- << bi_bias_est['I'](3) << ","
- << bi_bias_est['I'](4) << ","
- << bi_bias_est['I'](5) << ","
- << kf_state['C'](0) << ","
- << kf_state['C'](1) << ","
- << kf_state['C'](2) << ","
- << kf_state['D'](0) << ","
- << kf_state['D'](1) << ","
- << kf_state['D'](2) << ","
- << kf_state['L'](0) << ","
- << kf_state['L'](1) << ","
- << kf_state['L'](2) << ","
- << bp_bias_est['I'](0) << ","
- << bp_bias_est['I'](1) << ","
- << bp_bias_est['I'](2)
- << "\n";
+ file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1) << kf->getTimeStamp().get()
+ << "," << kf_state['P'](0) << "," << kf_state['P'](1) << "," << kf_state['P'](2) << ","
+ << kf_state['O'](0) << "," << kf_state['O'](1) << "," << kf_state['O'](2) << "," << kf_state['O'](3)
+ << "," << kf_state['V'](0) << "," << kf_state['V'](1) << "," << kf_state['V'](2) << ","
+ << bi_bias_est['I'](0) << "," << bi_bias_est['I'](1) << "," << bi_bias_est['I'](2) << ","
+ << bi_bias_est['I'](3) << "," << bi_bias_est['I'](4) << "," << bi_bias_est['I'](5) << ","
+ << kf_state['C'](0) << "," << kf_state['C'](1) << "," << kf_state['C'](2) << "," << kf_state['D'](0)
+ << "," << kf_state['D'](1) << "," << kf_state['D'](2) << "," << kf_state['L'](0) << ","
+ << kf_state['L'](1) << "," << kf_state['L'](2) << "," << bp_bias_est['I'](0) << ","
+ << bp_bias_est['I'](1) << "," << bp_bias_est['I'](2) << "\n";
// compute covariances of KF and capture stateblocks
- Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local?
- Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity();
- Eigen::MatrixXd Qc = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd Qd = Eigen::Matrix3d::Identity();
- Eigen::MatrixXd QL = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local?
+ Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity();
+ Eigen::MatrixXd Qc = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qd = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd QL = Eigen::Matrix3d::Identity();
Eigen::MatrixXd Qbp = Eigen::Matrix3d::Identity();
// solver->computeCovariances(kf->getStateBlockVec()); // !! does not work as intended...
-
- solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+
+ solver->computeCovariances(
+ SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
@@ -787,90 +788,53 @@ int main (int argc, char **argv) {
problem->getCovarianceBlock(kf->getStateBlock('D'), Qd);
problem->getCovarianceBlock(kf->getStateBlock('L'), QL);
- solver->computeCovariances(cap_ikin->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
+ solver->computeCovariances(
+ cap_ikin->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
problem->getCovarianceBlock(cap_ikin->getSensorIntrinsic(), Qbp);
// std::cout << "Qbp\n" << Qbp << std::endl;
- solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
+ solver->computeCovariances(
+ cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
- file_cov << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << kf->getTimeStamp().get() << ","
- << Qp(0,0) << ","
- << Qp(1,1) << ","
- << Qp(2,2) << ","
- << Qo(0,0) << ","
- << Qo(1,1) << ","
- << Qo(2,2) << ","
- << Qv(0,0) << ","
- << Qv(1,1) << ","
- << Qv(2,2) << ","
- << Qbi(0,0) << ","
- << Qbi(1,1) << ","
- << Qbi(2,2) << ","
- << Qbi(3,3) << ","
- << Qbi(4,4) << ","
- << Qbi(5,5) << ","
- << Qc(0,0) << ","
- << Qc(1,1) << ","
- << Qc(2,2) << ","
- << Qd(0,0) << ","
- << Qd(1,1) << ","
- << Qd(2,2) << ","
- << QL(0,0) << ","
- << QL(1,1) << ","
- << QL(2,2) << ","
- << Qbp(0,0) << ","
- << Qbp(1,1) << ","
- << Qbp(2,2)
- << "\n";
+ file_cov << std::setprecision(std::numeric_limits<long double>::digits10 + 1) << kf->getTimeStamp().get()
+ << "," << Qp(0, 0) << "," << Qp(1, 1) << "," << Qp(2, 2) << "," << Qo(0, 0) << "," << Qo(1, 1) << ","
+ << Qo(2, 2) << "," << Qv(0, 0) << "," << Qv(1, 1) << "," << Qv(2, 2) << "," << Qbi(0, 0) << ","
+ << Qbi(1, 1) << "," << Qbi(2, 2) << "," << Qbi(3, 3) << "," << Qbi(4, 4) << "," << Qbi(5, 5) << ","
+ << Qc(0, 0) << "," << Qc(1, 1) << "," << Qc(2, 2) << "," << Qd(0, 0) << "," << Qd(1, 1) << ","
+ << Qd(2, 2) << "," << QL(0, 0) << "," << QL(1, 1) << "," << QL(2, 2) << "," << Qbp(0, 0) << ","
+ << Qbp(1, 1) << "," << Qbp(2, 2) << "\n";
}
- for (unsigned int i=0; i < N; i++) {
- file_fbk << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- << t_arr[i] << ","
- << p_ob_fbk_v[i](0) << ","
- << p_ob_fbk_v[i](1) << ","
- << p_ob_fbk_v[i](2) << ","
- << q_ob_fbk_v[i](0) << ","
- << q_ob_fbk_v[i](1) << ","
- << q_ob_fbk_v[i](2) << ","
- << q_ob_fbk_v[i](3) << ","
- << v_ob_fbk_v[i](0) << ","
- << v_ob_fbk_v[i](1) << ","
- << v_ob_fbk_v[i](2) << ","
- << c_ob_fbk_v[i](0) << ","
- << c_ob_fbk_v[i](1) << ","
- << c_ob_fbk_v[i](2) << ","
- << cd_ob_fbk_v[i](0) << ","
- << cd_ob_fbk_v[i](1) << ","
- << cd_ob_fbk_v[i](2) << ","
- << Lc_ob_fbk_v[i](0) << ","
- << Lc_ob_fbk_v[i](1) << ","
- << Lc_ob_fbk_v[i](2)
- << "\n";
+ for (unsigned int i = 0; i < N; i++)
+ {
+ file_fbk << std::setprecision(std::numeric_limits<long double>::digits10 + 1) << t_arr[i] << ","
+ << p_ob_fbk_v[i](0) << "," << p_ob_fbk_v[i](1) << "," << p_ob_fbk_v[i](2) << "," << q_ob_fbk_v[i](0)
+ << "," << q_ob_fbk_v[i](1) << "," << q_ob_fbk_v[i](2) << "," << q_ob_fbk_v[i](3) << ","
+ << v_ob_fbk_v[i](0) << "," << v_ob_fbk_v[i](1) << "," << v_ob_fbk_v[i](2) << "," << c_ob_fbk_v[i](0)
+ << "," << c_ob_fbk_v[i](1) << "," << c_ob_fbk_v[i](2) << "," << cd_ob_fbk_v[i](0) << ","
+ << cd_ob_fbk_v[i](1) << "," << cd_ob_fbk_v[i](2) << "," << Lc_ob_fbk_v[i](0) << ","
+ << Lc_ob_fbk_v[i](1) << "," << Lc_ob_fbk_v[i](2) << "\n";
}
file_est.close();
file_kfs.close();
file_cov.close();
-
// Save trajectories in npz file
// save ground truth
- cnpy::npz_save(out_npz_file_path, "t", t_arr, {N}, "w"); // "w" overwrites any file with same name
- cnpy::npz_save(out_npz_file_path, "w_p_wm", w_p_wm_arr, {N,3}, "a"); // "a" appends to the file we created above
- cnpy::npz_save(out_npz_file_path, "w_q_m", w_q_m_arr, {N,4}, "a");
- cnpy::npz_save(out_npz_file_path, "w_v_wm", w_v_wm_arr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "m_v_wm", m_v_wm_arr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_arr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "qa", qa_arr, {N,12}, "a");
+ cnpy::npz_save(out_npz_file_path, "t", t_arr, {N}, "w"); // "w" overwrites any file with same name
+ cnpy::npz_save(out_npz_file_path, "w_p_wm", w_p_wm_arr, {N, 3}, "a"); // "a" appends to the file we created above
+ cnpy::npz_save(out_npz_file_path, "w_q_m", w_q_m_arr, {N, 4}, "a");
+ cnpy::npz_save(out_npz_file_path, "w_v_wm", w_v_wm_arr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "m_v_wm", m_v_wm_arr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_i", o_q_i_arr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "qa", qa_arr, {N, 12}, "a");
// save estimates
- cnpy::npz_save(out_npz_file_path, "o_p_ob", o_p_ob_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_q_b", o_q_b_carr, {N,4}, "a");
- cnpy::npz_save(out_npz_file_path, "o_v_ob", o_v_ob_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_p_oc", o_p_oc_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_v_oc", o_v_oc_carr, {N,3}, "a");
- cnpy::npz_save(out_npz_file_path, "o_Lc", o_Lc_carr, {N,3}, "a");
-
+ cnpy::npz_save(out_npz_file_path, "o_p_ob", o_p_ob_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_q_b", o_q_b_carr, {N, 4}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_ob", o_v_ob_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_p_oc", o_p_oc_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_v_oc", o_v_oc_carr, {N, 3}, "a");
+ cnpy::npz_save(out_npz_file_path, "o_Lc", o_Lc_carr, {N, 3}, "a");
}
diff --git a/include/bodydynamics/capture/capture_force_torque_preint.h b/include/bodydynamics/capture/capture_force_torque_preint.h
index a2331fd..e0eaa5a 100644
--- a/include/bodydynamics/capture/capture_force_torque_preint.h
+++ b/include/bodydynamics/capture/capture_force_torque_preint.h
@@ -21,7 +21,7 @@
#ifndef CAPTURE_FORCE_TORQUE_PREINT_H
#define CAPTURE_FORCE_TORQUE_PREINT_H
-//Wolf includes
+// Wolf includes
#include "bodydynamics/math/force_torque_delta_tools.h"
#include "core/capture/capture_base.h"
#include "core/capture/capture_motion.h"
@@ -29,48 +29,59 @@
#include <core/state_block/vector_composite.h>
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(CaptureForceTorquePreint);
class CaptureForceTorquePreint : public CaptureMotion
{
- public:
- /*
- capture _data
- name, size: description
- f1 , 3 : force of foot 1 in foot 1 frame
- f2 , 3 : force of foot 2 in foot 2 frame
- tau1, 3 : torque of foot 1 in foot 1 frame
- tau2, 3 : torque of foot 2 in foot 2 frame
- pbc , 3 : position of COM relative to the base in base
- wb , 3 : angular velocity of body frame in body frame
- pbl1, 3 : position of foot 1 relative to the base in base frame
- pbl2, 3 : position of foot 2 relative to the base in base frame
- qbl1, 4 : orientation of foot 1 in base frame
- qbl2, 4 : orientation of foot 2 in base frame
- */
- CaptureForceTorquePreint(
- const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- CaptureInertialKinematicsPtr _capture_IK_ptr, // to get the pbc bias
- CaptureMotionPtr _capture_motion_ptr, // to get the gyro bias
- const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- CaptureBasePtr _capture_origin_ptr = nullptr);
-
- ~CaptureForceTorquePreint() override;
+ public:
+ /*
+ capture _data
+ name, size: description
+ f1 , 3 : force of foot 1 in foot 1 frame
+ f2 , 3 : force of foot 2 in foot 2 frame
+ tau1, 3 : torque of foot 1 in foot 1 frame
+ tau2, 3 : torque of foot 2 in foot 2 frame
+ pbc , 3 : position of COM relative to the base in base
+ wb , 3 : angular velocity of body frame in body frame
+ pbl1, 3 : position of foot 1 relative to the base in base frame
+ pbl2, 3 : position of foot 2 relative to the base in base frame
+ qbl1, 4 : orientation of foot 1 in base frame
+ qbl2, 4 : orientation of foot 2 in base frame
+ */
+ CaptureForceTorquePreint(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ CaptureInertialKinematicsPtr _capture_IK_ptr, // to get the pbc bias
+ CaptureMotionPtr _capture_motion_ptr, // to get the gyro bias
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ CaptureBasePtr _capture_origin_ptr = nullptr);
+
+ ~CaptureForceTorquePreint() override;
- CaptureBaseConstPtr getIkinCaptureOther() const { return cap_ikin_other_;}
- CaptureBasePtr getIkinCaptureOther(){ return cap_ikin_other_;}
- CaptureBaseConstPtr getGyroCaptureOther() const { return cap_gyro_other_;}
- CaptureBasePtr getGyroCaptureOther(){ return cap_gyro_other_;}
+ CaptureBaseConstPtr getIkinCaptureOther() const
+ {
+ return cap_ikin_other_;
+ }
+ CaptureBasePtr getIkinCaptureOther()
+ {
+ return cap_ikin_other_;
+ }
+ CaptureBaseConstPtr getGyroCaptureOther() const
+ {
+ return cap_gyro_other_;
+ }
+ CaptureBasePtr getGyroCaptureOther()
+ {
+ return cap_gyro_other_;
+ }
- private:
- CaptureBasePtr cap_ikin_other_;
- CaptureBasePtr cap_gyro_other_;
+ private:
+ CaptureBasePtr cap_ikin_other_;
+ CaptureBasePtr cap_gyro_other_;
};
-} // namespace wolf
+} // namespace wolf
-#endif // CAPTURE_FORCE_TORQUE_PREINT_H
+#endif // CAPTURE_FORCE_TORQUE_PREINT_H
diff --git a/include/bodydynamics/capture/capture_inertial_kinematics.h b/include/bodydynamics/capture/capture_inertial_kinematics.h
index 72c8b0e..94438ee 100644
--- a/include/bodydynamics/capture/capture_inertial_kinematics.h
+++ b/include/bodydynamics/capture/capture_inertial_kinematics.h
@@ -21,43 +21,51 @@
#ifndef CAPTURE_INERTIAL_KINEMATICS_H
#define CAPTURE_INERTIAL_KINEMATICS_H
-//Wolf includes
+// Wolf includes
#include "bodydynamics/math/force_torque_delta_tools.h"
#include "core/capture/capture_motion.h"
#include "core/capture/capture_base.h"
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(CaptureInertialKinematics);
class CaptureInertialKinematics : public CaptureBase
{
- public:
-
- CaptureInertialKinematics(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::Vector9d& _data, // pbc, vbc, wb
- const Eigen::Matrix3d & _B_I_q, // Centroidal inertia matrix expressed in body frame
- const Eigen::Vector3d & _B_Lc_m, // Centroidal angular momentum expressed in body frame
- const Vector3d& _bias);
-
- CaptureInertialKinematics(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::Vector9d& _data, // pbc, vbc, wb
- const Eigen::Matrix3d & _B_I_q, // Centroidal inertia matrix expressed in body frame
- const Eigen::Vector3d & _B_Lc_m); // Centroidal angular momentum expressed in body frame
-
- ~CaptureInertialKinematics() override;
-
- const Eigen::Vector9d& getData() {return data_;}
- const Eigen::Matrix3d& getBIq() {return B_I_q_;}
- const Eigen::Vector3d& getBLcm() {return B_Lc_m_;}
-
- private:
- Eigen::Vector9d data_;
- Eigen::Matrix3d B_I_q_;
- Eigen::Vector3d B_Lc_m_;
+ public:
+ CaptureInertialKinematics(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::Vector9d& _data, // pbc, vbc, wb
+ const Eigen::Matrix3d& _B_I_q, // Centroidal inertia matrix expressed in body frame
+ const Eigen::Vector3d& _B_Lc_m, // Centroidal angular momentum expressed in body frame
+ const Vector3d& _bias);
+
+ CaptureInertialKinematics(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::Vector9d& _data, // pbc, vbc, wb
+ const Eigen::Matrix3d& _B_I_q, // Centroidal inertia matrix expressed in body frame
+ const Eigen::Vector3d& _B_Lc_m); // Centroidal angular momentum expressed in body frame
+
+ ~CaptureInertialKinematics() override;
+
+ const Eigen::Vector9d& getData()
+ {
+ return data_;
+ }
+ const Eigen::Matrix3d& getBIq()
+ {
+ return B_I_q_;
+ }
+ const Eigen::Vector3d& getBLcm()
+ {
+ return B_Lc_m_;
+ }
+
+ private:
+ Eigen::Vector9d data_;
+ Eigen::Matrix3d B_I_q_;
+ Eigen::Vector3d B_Lc_m_;
};
-} // namespace wolf
+} // namespace wolf
-#endif // CAPTURE_INERTIAL_KINEMATICS_H
+#endif // CAPTURE_INERTIAL_KINEMATICS_H
diff --git a/include/bodydynamics/capture/capture_leg_odom.h b/include/bodydynamics/capture/capture_leg_odom.h
index a627834..dbc89ad 100644
--- a/include/bodydynamics/capture/capture_leg_odom.h
+++ b/include/bodydynamics/capture/capture_leg_odom.h
@@ -21,12 +21,12 @@
#ifndef CAPTURE_LEG_ODOM_H
#define CAPTURE_LEG_ODOM_H
-//Wolf includes
+// Wolf includes
#include "core/capture/capture_base.h"
#include "core/capture/capture_motion.h"
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(CaptureLegOdom);
typedef enum
@@ -36,30 +36,29 @@ typedef enum
HUMANOID_FEET_RELATIVE_KIN
} CaptureLegOdomType;
-
class CaptureLegOdom : public CaptureMotion
{
- public:
- /*
- capture _data
- */
- CaptureLegOdom(
- const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::MatrixXd& _data_kin,
- Eigen::Matrix6d _data_cov, // for the moment only 6x6 mat, see if more involved computations are necessary (contact confidence...)
- double dt,
- CaptureLegOdomType lo_type);
-
- ~CaptureLegOdom() override;
+ public:
+ /*
+ capture _data
+ */
+ CaptureLegOdom(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::MatrixXd& _data_kin,
+ Eigen::Matrix6d _data_cov, // for the moment only 6x6 mat, see if more involved computations are
+ // necessary (contact confidence...)
+ double dt,
+ CaptureLegOdomType lo_type);
+ ~CaptureLegOdom() override;
};
-// inline Eigen::VectorXd CaptureLegOdom::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error_biases) const
+// inline Eigen::VectorXd CaptureLegOdom::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error_biases)
+// const
// {
-// return
+// return
// }
-} // namespace wolf
+} // namespace wolf
-#endif // CAPTURE_LEG_ODOM_H
+#endif // CAPTURE_LEG_ODOM_H
diff --git a/include/bodydynamics/capture/capture_point_feet_nomove.h b/include/bodydynamics/capture/capture_point_feet_nomove.h
index cc6370c..addd2ce 100644
--- a/include/bodydynamics/capture/capture_point_feet_nomove.h
+++ b/include/bodydynamics/capture/capture_point_feet_nomove.h
@@ -21,28 +21,26 @@
#ifndef CAPTURE_POINT_FEET_NOMOVE_H
#define CAPTURE_POINT_FEET_NOMOVE_H
-//Wolf includes
+// Wolf includes
#include "core/capture/capture_base.h"
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(CapturePointFeetNomove);
class CapturePointFeetNomove : public CaptureBase
{
- public:
- CapturePointFeetNomove(
- const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const std::unordered_map<int, Eigen::Vector7d>& kin_incontact
- );
-
- ~CapturePointFeetNomove() override;
-
- // <foot in contact number (0,1,2,3), b_p_bf>
- std::unordered_map<int, Eigen::Vector7d> kin_incontact_;
+ public:
+ CapturePointFeetNomove(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const std::unordered_map<int, Eigen::Vector7d>& kin_incontact);
+
+ ~CapturePointFeetNomove() override;
+
+ // <foot in contact number (0,1,2,3), b_p_bf>
+ std::unordered_map<int, Eigen::Vector7d> kin_incontact_;
};
-} // namespace wolf
+} // namespace wolf
-#endif // CAPTURE_POINT_FEET_NOMOVE_H
+#endif // CAPTURE_POINT_FEET_NOMOVE_H
diff --git a/include/bodydynamics/common/bodydynamics.h b/include/bodydynamics/common/bodydynamics.h
index 9faa3bc..ad68f44 100644
--- a/include/bodydynamics/common/bodydynamics.h
+++ b/include/bodydynamics/common/bodydynamics.h
@@ -26,8 +26,7 @@
namespace wolf
{
-
// Folder schema Registry
WOLF_REGISTER_FOLDER(_WOLF_BODYDYNAMICS_SCHEMA_DIR);
-}
+} // namespace wolf
diff --git a/include/bodydynamics/factor/factor_force_torque.h b/include/bodydynamics/factor/factor_force_torque.h
index 36729a2..5f5f05d 100644
--- a/include/bodydynamics/factor/factor_force_torque.h
+++ b/include/bodydynamics/factor/factor_force_torque.h
@@ -30,148 +30,145 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorForceTorque);
-class FactorForceTorque : public FactorAutodiff<FactorForceTorque, 9, 3,3,3, 3,3,3,3,4>
+class FactorForceTorque : public FactorAutodiff<FactorForceTorque, 9, 3, 3, 3, 3, 3, 3, 3, 4>
{
- public:
- FactorForceTorque(const FeatureBasePtr& _feat,
- const FrameBasePtr& _frame_other,
- const StateBlockPtr& _sb_bp_other,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE);
-
- ~FactorForceTorque() override { /* nothing */ }
-
- /*
- _ck : COM position in world frame, time k
- _cdk : COM velocity in world frame, time k
- _Lck : Angular momentum in world frame, time k
- _ckm : COM position in world frame, time k-1
- _cdkm : COM velocity in world frame, time k-1
- _Lckm : Angular momentum in world frame, time k-1
- _bpkm : COM position measurement bias, time k-1
- _qkm : Base orientation in world frame, time k-1
- */
- template<typename T>
- bool operator () (
- const T* const _ck,
- const T* const _cdk,
- const T* const _Lck,
- const T* const _ckm,
- const T* const _cdkm,
- const T* const _Lckm,
- const T* const _bpkm,
- const T* const _qkm,
- T* _res) const;
-
- void computeJac(const FeatureForceTorqueConstPtr& _feat,
- const double& _mass,
- const double& _dt,
- const Eigen::VectorXd& _bp,
- Eigen::Matrix<double, 9, 15> & _J_ny_nz) const;
-
- // void recomputeJac(const FeatureForceTorquePtr& _feat,
- // const double& _dt,
- // const Eigen::VectorXd& _bp,
- // Eigen::Matrix<double, 9, 15>& _J_ny_nz) const;
-
- void retrieveMeasurementCovariance(const FeatureForceTorquePtr& feat, Eigen::Matrix<double,15,15>& cov);
-
- StateBlockPtr sb_bp_other_;
- double mass_;
- double dt_;
- Eigen::Matrix<double,15,15> cov_meas_;
- // Eigen::Matrix<double, 9, 15> J_ny_nz_; // cannot be modified in operator() since const function
- // Eigen::Matrix9d errCov_;
+ public:
+ FactorForceTorque(const FeatureBasePtr& _feat,
+ const FrameBasePtr& _frame_other,
+ const StateBlockPtr& _sb_bp_other,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorForceTorque() override
+ { /* nothing */
+ }
+
+ /*
+ _ck : COM position in world frame, time k
+ _cdk : COM velocity in world frame, time k
+ _Lck : Angular momentum in world frame, time k
+ _ckm : COM position in world frame, time k-1
+ _cdkm : COM velocity in world frame, time k-1
+ _Lckm : Angular momentum in world frame, time k-1
+ _bpkm : COM position measurement bias, time k-1
+ _qkm : Base orientation in world frame, time k-1
+ */
+ template <typename T>
+ bool operator()(const T* const _ck,
+ const T* const _cdk,
+ const T* const _Lck,
+ const T* const _ckm,
+ const T* const _cdkm,
+ const T* const _Lckm,
+ const T* const _bpkm,
+ const T* const _qkm,
+ T* _res) const;
+
+ void computeJac(const FeatureForceTorqueConstPtr& _feat,
+ const double& _mass,
+ const double& _dt,
+ const Eigen::VectorXd& _bp,
+ Eigen::Matrix<double, 9, 15>& _J_ny_nz) const;
+
+ // void recomputeJac(const FeatureForceTorquePtr& _feat,
+ // const double& _dt,
+ // const Eigen::VectorXd& _bp,
+ // Eigen::Matrix<double, 9, 15>& _J_ny_nz) const;
+
+ void retrieveMeasurementCovariance(const FeatureForceTorquePtr& feat, Eigen::Matrix<double, 15, 15>& cov);
+
+ StateBlockPtr sb_bp_other_;
+ double mass_;
+ double dt_;
+ Eigen::Matrix<double, 15, 15> cov_meas_;
+ // Eigen::Matrix<double, 9, 15> J_ny_nz_; // cannot be modified in operator() since const function
+ // Eigen::Matrix9d errCov_;
};
} /* namespace wolf */
-
-namespace wolf {
-
-FactorForceTorque::FactorForceTorque(
- const FeatureBasePtr& _feat,
- const FrameBasePtr& _frame_other,
- const StateBlockPtr& _sb_bp_other,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) :
- FactorAutodiff("FactorForceTorque",
- TOP_GEOM,
- _feat,
- _frame_other, // _frame_other_ptr
- nullptr, // _capture_other_ptr
- nullptr, // _feature_other_ptr
- nullptr, // _landmark_other_ptr
- _processor_ptr,
- _apply_loss_function,
- _status,
- _feat->getFrame()->getStateBlock('C'), // COM position, current
- _feat->getFrame()->getStateBlock('D'), // COM velocity (bad name), current
- _feat->getFrame()->getStateBlock('L'), // Angular momentum, current
- _frame_other->getStateBlock('C'), // COM position, previous
- _frame_other->getStateBlock('D'), // COM velocity (bad name), previous
- _frame_other->getStateBlock('L'), // Angular momentum, previous
- _sb_bp_other, // BC relative position bias, previous
- _frame_other->getStateBlock('O') // Base orientation, previous
- ),
- sb_bp_other_(_sb_bp_other)
+namespace wolf
+{
+FactorForceTorque::FactorForceTorque(const FeatureBasePtr& _feat,
+ const FrameBasePtr& _frame_other,
+ const StateBlockPtr& _sb_bp_other,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status)
+ : FactorAutodiff("FactorForceTorque",
+ TOP_GEOM,
+ _feat,
+ _frame_other, // _frame_other_ptr
+ nullptr, // _capture_other_ptr
+ nullptr, // _feature_other_ptr
+ nullptr, // _landmark_other_ptr
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _feat->getFrame()->getStateBlock('C'), // COM position, current
+ _feat->getFrame()->getStateBlock('D'), // COM velocity (bad name), current
+ _feat->getFrame()->getStateBlock('L'), // Angular momentum, current
+ _frame_other->getStateBlock('C'), // COM position, previous
+ _frame_other->getStateBlock('D'), // COM velocity (bad name), previous
+ _frame_other->getStateBlock('L'), // Angular momentum, previous
+ _sb_bp_other, // BC relative position bias, previous
+ _frame_other->getStateBlock('O') // Base orientation, previous
+ ),
+ sb_bp_other_(_sb_bp_other)
{
FeatureForceTorquePtr feat = std::static_pointer_cast<FeatureForceTorque>(_feat);
- mass_ = feat->getMass();
- dt_ = feat->getDt();
+ mass_ = feat->getMass();
+ dt_ = feat->getDt();
retrieveMeasurementCovariance(feat, cov_meas_);
}
-
-void FactorForceTorque::retrieveMeasurementCovariance(const FeatureForceTorquePtr& feat, Eigen::Matrix<double,15,15>& cov)
+void FactorForceTorque::retrieveMeasurementCovariance(const FeatureForceTorquePtr& feat,
+ Eigen::Matrix<double, 15, 15>& cov)
{
cov.setZero();
- cov.block<6,6>(0,0) = feat->getCovForcesMeas(); // reset some extra zeros
- cov.block<6,6>(6,6) = feat->getCovTorquesMeas(); // reset some extra zeros
- cov.block<3,3>(12,12) = feat->getCovPbcMeas();
+ cov.block<6, 6>(0, 0) = feat->getCovForcesMeas(); // reset some extra zeros
+ cov.block<6, 6>(6, 6) = feat->getCovTorquesMeas(); // reset some extra zeros
+ cov.block<3, 3>(12, 12) = feat->getCovPbcMeas();
}
-
-void FactorForceTorque::computeJac(const FeatureForceTorqueConstPtr& _feat,
- const double& _mass,
- const double& _dt,
- const Eigen::VectorXd& _bp,
- Eigen::Matrix<double, 9, 15>& _J_ny_nz) const
+void FactorForceTorque::computeJac(const FeatureForceTorqueConstPtr& _feat,
+ const double& _mass,
+ const double& _dt,
+ const Eigen::VectorXd& _bp,
+ Eigen::Matrix<double, 9, 15>& _J_ny_nz) const
{
_J_ny_nz.setZero();
// Measurements retrieval
- Eigen::Map<const Eigen::Vector3d> f1 (_feat->getForcesMeas().data());
- Eigen::Map<const Eigen::Vector3d> f2 (_feat->getForcesMeas().data() + 3 );
+ Eigen::Map<const Eigen::Vector3d> f1(_feat->getForcesMeas().data());
+ Eigen::Map<const Eigen::Vector3d> f2(_feat->getForcesMeas().data() + 3);
Eigen::Map<const Eigen::Vector3d> tau1(_feat->getTorquesMeas().data());
- Eigen::Map<const Eigen::Vector3d> tau2(_feat->getTorquesMeas().data() + 3 );
+ Eigen::Map<const Eigen::Vector3d> tau2(_feat->getTorquesMeas().data() + 3);
Eigen::Map<const Eigen::Vector3d> pbl1(_feat->getKinMeas().data());
- Eigen::Map<const Eigen::Vector3d> pbl2(_feat->getKinMeas().data() + 3 );
- Eigen::Map<const Eigen::Quaterniond> bql1(_feat->getKinMeas().data() + 6);
- Eigen::Map<const Eigen::Quaterniond> bql2(_feat->getKinMeas().data() + 10);
- Eigen::Map<const Eigen::Vector3d> pbc (_feat->getPbcMeas().data());
-
- Eigen::Matrix3d bRl1 = q2R(bql1);
- Eigen::Matrix3d bRl2 = q2R(bql2);
- _J_ny_nz.block<3,3>(0,0) = 0.5*bRl1*pow(_dt,2)/_mass;
- _J_ny_nz.block<3,3>(0,3) = 0.5*bRl2*pow(_dt,2)/_mass;
- _J_ny_nz.block<3,3>(3,0) = bRl1*_dt/_mass;
- _J_ny_nz.block<3,3>(3,3) = bRl2*_dt/_mass;
- _J_ny_nz.block<3,3>(6,0) = skew(pbl1 - pbc + _bp)*bRl1*_dt;
- _J_ny_nz.block<3,3>(6,3) = skew(pbl2 - pbc + _bp)*bRl2*_dt;
- _J_ny_nz.block<3,3>(6,6) = bRl1*_dt;
- _J_ny_nz.block<3,3>(6,9) = bRl2*_dt;
- _J_ny_nz.block<3,3>(6,12) = (skew(bRl1*f1) + skew(bRl2*f2))*_dt;
+ Eigen::Map<const Eigen::Vector3d> pbl2(_feat->getKinMeas().data() + 3);
+ Eigen::Map<const Eigen::Quaterniond> bql1(_feat->getKinMeas().data() + 6);
+ Eigen::Map<const Eigen::Quaterniond> bql2(_feat->getKinMeas().data() + 10);
+ Eigen::Map<const Eigen::Vector3d> pbc(_feat->getPbcMeas().data());
+
+ Eigen::Matrix3d bRl1 = q2R(bql1);
+ Eigen::Matrix3d bRl2 = q2R(bql2);
+ _J_ny_nz.block<3, 3>(0, 0) = 0.5 * bRl1 * pow(_dt, 2) / _mass;
+ _J_ny_nz.block<3, 3>(0, 3) = 0.5 * bRl2 * pow(_dt, 2) / _mass;
+ _J_ny_nz.block<3, 3>(3, 0) = bRl1 * _dt / _mass;
+ _J_ny_nz.block<3, 3>(3, 3) = bRl2 * _dt / _mass;
+ _J_ny_nz.block<3, 3>(6, 0) = skew(pbl1 - pbc + _bp) * bRl1 * _dt;
+ _J_ny_nz.block<3, 3>(6, 3) = skew(pbl2 - pbc + _bp) * bRl2 * _dt;
+ _J_ny_nz.block<3, 3>(6, 6) = bRl1 * _dt;
+ _J_ny_nz.block<3, 3>(6, 9) = bRl2 * _dt;
+ _J_ny_nz.block<3, 3>(6, 12) = (skew(bRl1 * f1) + skew(bRl2 * f2)) * _dt;
}
-// void FactorForceTorque::recomputeJac(const FeatureForceTorquePtr& _feat,
-// const double& _dt,
-// const Eigen::VectorXd& _bp,
+// void FactorForceTorque::recomputeJac(const FeatureForceTorquePtr& _feat,
+// const double& _dt,
+// const Eigen::VectorXd& _bp,
// Eigen::Matrix<double, 9, 15>& _J_ny_nz)
// {
// FeatureForceTorquePtr feat = std::static_pointer_cast<FeatureForceTorque>(_feat);
@@ -187,84 +184,81 @@ void FactorForceTorque::computeJac(const FeatureForceTorqueConstPtr& _feat,
// Eigen::Map<const Eigen::Quaterniond> bql2(feat->getKinMeas().data() + 10);
// Eigen::Map<const Eigen::Vector3d> pbc (feat->getPbcMeas().data());
-// Eigen::Matrix3d bRl1 = q2R(bql1);
-// Eigen::Matrix3d bRl2 = q2R(bql2);
+// Eigen::Matrix3d bRl1 = q2R(bql1);
+// Eigen::Matrix3d bRl2 = q2R(bql2);
// // _J_ny_nz.block<3,3>(0,0) = 0.5*bRl1*pow(_dt,2)/_mass;
// // _J_ny_nz.block<3,3>(0,3) = 0.5*bRl2*pow(_dt,2)/_mass;
// // _J_ny_nz.block<3,3>(3,0) = bRl1*_dt/_mass;
// // _J_ny_nz.block<3,3>(3,3) = bRl2*_dt/_mass;
// _J_ny_nz.block<3,3>(6,0) = skew(pbl1 - pbc + _bp)*bRl1*_dt;
// _J_ny_nz.block<3,3>(6,3) = skew(pbl2 - pbc + _bp)*bRl2*_dt;
-// // _J_ny_nz.block<3,3>(6,6) = bRl1*_dt;
-// // _J_ny_nz.block<3,3>(6,9) = bRl2*_dt;
-// // _J_ny_nz.block<3,3>(6,12) = (skew(bRl1*f1) + skew(bRl2*f2))*_dt;
+// // _J_ny_nz.block<3,3>(6,6) = bRl1*_dt;
+// // _J_ny_nz.block<3,3>(6,9) = bRl2*_dt;
+// // _J_ny_nz.block<3,3>(6,12) = (skew(bRl1*f1) + skew(bRl2*f2))*_dt;
// }
-
-template<typename T>
-bool FactorForceTorque::operator () (
- const T* const _ck,
- const T* const _cdk,
- const T* const _Lck,
- const T* const _ckm,
- const T* const _cdkm,
- const T* const _Lckm,
- const T* const _bpkm,
- const T* const _qkm,
- T* _res) const
+template <typename T>
+bool FactorForceTorque::operator()(const T* const _ck,
+ const T* const _cdk,
+ const T* const _Lck,
+ const T* const _ckm,
+ const T* const _cdkm,
+ const T* const _Lckm,
+ const T* const _bpkm,
+ const T* const _qkm,
+ T* _res) const
{
auto feat = std::static_pointer_cast<const FeatureForceTorque>(getFeature());
// State variables instanciation
- Eigen::Map<const Eigen::Matrix<T,3,1> > ck(_ck);
- Eigen::Map<const Eigen::Matrix<T,3,1> > cdk(_cdk);
- Eigen::Map<const Eigen::Matrix<T,3,1> > Lck(_Lck);
- Eigen::Map<const Eigen::Matrix<T,3,1> > ckm(_ckm);
- Eigen::Map<const Eigen::Matrix<T,3,1> > cdkm(_cdkm);
- Eigen::Map<const Eigen::Matrix<T,3,1> > Lckm(_Lckm);
- Eigen::Map<const Eigen::Matrix<T,3,1> > bpkm(_bpkm);
- Eigen::Map<const Eigen::Quaternion<T> > qkm(_qkm);
- Eigen::Map<Eigen::Matrix<T,9,1> > res(_res);
+ Eigen::Map<const Eigen::Matrix<T, 3, 1> > ck(_ck);
+ Eigen::Map<const Eigen::Matrix<T, 3, 1> > cdk(_cdk);
+ Eigen::Map<const Eigen::Matrix<T, 3, 1> > Lck(_Lck);
+ Eigen::Map<const Eigen::Matrix<T, 3, 1> > ckm(_ckm);
+ Eigen::Map<const Eigen::Matrix<T, 3, 1> > cdkm(_cdkm);
+ Eigen::Map<const Eigen::Matrix<T, 3, 1> > Lckm(_Lckm);
+ Eigen::Map<const Eigen::Matrix<T, 3, 1> > bpkm(_bpkm);
+ Eigen::Map<const Eigen::Quaternion<T> > qkm(_qkm);
+ Eigen::Map<Eigen::Matrix<T, 9, 1> > res(_res);
// Retrieve all measurements
- Eigen::Map<const Eigen::Vector3d> f1 (feat->getForcesMeas().data());
- Eigen::Map<const Eigen::Vector3d> f2 (feat->getForcesMeas().data() + 3 );
+ Eigen::Map<const Eigen::Vector3d> f1(feat->getForcesMeas().data());
+ Eigen::Map<const Eigen::Vector3d> f2(feat->getForcesMeas().data() + 3);
Eigen::Map<const Eigen::Vector3d> tau1(feat->getTorquesMeas().data());
- Eigen::Map<const Eigen::Vector3d> tau2(feat->getTorquesMeas().data() + 3 );
- Eigen::Map<const Eigen::Vector3d> pbc (feat->getPbcMeas().data());
+ Eigen::Map<const Eigen::Vector3d> tau2(feat->getTorquesMeas().data() + 3);
+ Eigen::Map<const Eigen::Vector3d> pbc(feat->getPbcMeas().data());
Eigen::Map<const Eigen::Vector3d> pbl1(feat->getKinMeas().data());
- Eigen::Map<const Eigen::Vector3d> pbl2(feat->getKinMeas().data() + 3 );
- Eigen::Map<const Eigen::Quaterniond> bql1(feat->getKinMeas().data() + 6);
- Eigen::Map<const Eigen::Quaterniond> bql2(feat->getKinMeas().data() + 10);
+ Eigen::Map<const Eigen::Vector3d> pbl2(feat->getKinMeas().data() + 3);
+ Eigen::Map<const Eigen::Quaterniond> bql1(feat->getKinMeas().data() + 6);
+ Eigen::Map<const Eigen::Quaterniond> bql2(feat->getKinMeas().data() + 10);
// Recompute the error variable covariance according to the new bias bp estimation
Eigen::Matrix<double, 9, 15> J_ny_nz;
computeJac(feat, mass_, dt_, sb_bp_other_->getState(), J_ny_nz); // bp
- Eigen::Matrix9d errCov = J_ny_nz * cov_meas_ * J_ny_nz.transpose();
- errCov.block<6,6>(0,0) = errCov.block<6,6>(0,0) + 1e-12 * Eigen::Matrix6d::Identity();
+ Eigen::Matrix9d errCov = J_ny_nz * cov_meas_ * J_ny_nz.transpose();
+ errCov.block<6, 6>(0, 0) = errCov.block<6, 6>(0, 0) + 1e-12 * Eigen::Matrix6d::Identity();
// Error variable instanciation
- Eigen::Matrix<T, 9, 1> err;
- Eigen::Map<Eigen::Matrix<T, 3, 1> > err_c (err.data());
- Eigen::Map<Eigen::Matrix<T, 3, 1> > err_cd(err.data() + 3);
- Eigen::Map<Eigen::Matrix<T, 3, 1> > err_Lc(err.data() + 6);
-
- err_c = qkm.conjugate()*(ck - ckm - cdkm * dt_ - 0.5*wolf::gravity()*pow(dt_, 2))
- - (0.5/mass_) * (bql1 * f1 * pow(dt_,2) + bql2 * f2 * pow(dt_,2));
-
- err_cd = qkm.conjugate()*(cdk - cdkm - wolf::gravity()*dt_)
- - (1/mass_) * (bql1 * f1 * dt_ + bql2 * f2 * dt_);
-
- err_Lc = qkm.conjugate()*(Lck - Lckm)
- - ( bql1 * tau1 + (pbl1 - pbc + bpkm).cross(bql1 * f1)
- + bql2 * tau2 + (pbl2 - pbc + bpkm).cross(bql2 * f2));
-
+ Eigen::Matrix<T, 9, 1> err;
+ Eigen::Map<Eigen::Matrix<T, 3, 1> > err_c(err.data());
+ Eigen::Map<Eigen::Matrix<T, 3, 1> > err_cd(err.data() + 3);
+ Eigen::Map<Eigen::Matrix<T, 3, 1> > err_Lc(err.data() + 6);
+
+ err_c = qkm.conjugate() * (ck - ckm - cdkm * dt_ - 0.5 * wolf::gravity() * pow(dt_, 2)) -
+ (0.5 / mass_) * (bql1 * f1 * pow(dt_, 2) + bql2 * f2 * pow(dt_, 2));
+
+ err_cd =
+ qkm.conjugate() * (cdk - cdkm - wolf::gravity() * dt_) - (1 / mass_) * (bql1 * f1 * dt_ + bql2 * f2 * dt_);
+
+ err_Lc = qkm.conjugate() * (Lck - Lckm) -
+ (bql1 * tau1 + (pbl1 - pbc + bpkm).cross(bql1 * f1) + bql2 * tau2 + (pbl2 - pbc + bpkm).cross(bql2 * f2));
+
res = wolf::computeSqrtUpper(errCov.inverse()) * err;
return true;
}
-} // namespace wolf
+} // namespace wolf
#endif /* FACTOR_FORCE_TORQUE_H_ */
diff --git a/include/bodydynamics/factor/factor_force_torque_preint.h b/include/bodydynamics/factor/factor_force_torque_preint.h
index 52d3fec..6e9cd36 100644
--- a/include/bodydynamics/factor/factor_force_torque_preint.h
+++ b/include/bodydynamics/factor/factor_force_torque_preint.h
@@ -21,201 +21,198 @@
#ifndef FACTOR_FORCE_TORQUE_PREINT_THETA_H_
#define FACTOR_FORCE_TORQUE_PREINT_THETA_H_
-//Wolf includes
+// Wolf includes
#include "bodydynamics/capture/capture_force_torque_preint.h"
#include "bodydynamics/feature/feature_force_torque_preint.h"
#include "bodydynamics/sensor/sensor_force_torque.h"
#include "core/factor/factor_autodiff.h"
#include "core/math/rotations.h"
-//Eigen include
+// Eigen include
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(FactorForceTorquePreint);
-//class
-class FactorForceTorquePreint : public FactorAutodiff<FactorForceTorquePreint, 12, 3,3,3,4,3,6, 3,3,3,4>
+// class
+class FactorForceTorquePreint : public FactorAutodiff<FactorForceTorquePreint, 12, 3, 3, 3, 4, 3, 6, 3, 3, 3, 4>
{
- public:
- FactorForceTorquePreint(const FeatureForceTorquePreintPtr& _ftr_ptr,
- const CaptureForceTorquePreintPtr& _cap_origin_ptr, // gets to bp1, bw1
- const ProcessorBasePtr& _processor_ptr,
- const CaptureBasePtr& _cap_ikin_other,
- const CaptureBasePtr& _cap_gyro_other,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE);
-
- ~FactorForceTorquePreint() override = default;
-
- /** \brief : compute the residual from the state blocks being iterated by the solver.
- -> computes the expected measurement
- -> corrects actual measurement with new bias
- -> compares the corrected measurement with the expected one
- -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
- */
- template<typename T>
- bool operator ()(const T* const _c1,
- const T* const _cd1,
- const T* const _Lc1,
- const T* const _q1,
- const T* const _bp1,
- const T* const _bw1,
- const T* const _c2,
- const T* const _cd2,
- const T* const _Lc2,
- const T* const _q2,
- T* _res) const;
-
- /** \brief : compute the residual from the state blocks being iterated by the solver. (same as operator())
- -> computes the expected measurement
- -> corrects actual measurement with new bias
- -> compares the corrected measurement with the expected one
- -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
- * params :
- * _c1 : COM position at time t1 in world frame
- * _cd1: COM velocity at time t1 in world frame
- * _Lc1: Angular momentum at time t1 in world frame
- * _q1 : Base orientation at time t1
- * _bp1: COM position measurement at time t1 in body frame
- * _bw1: gyroscope bias at time t1 in body frame
- * _c2 : COM position at time t2 in world frame
- * _cd2: COM velocity at time t2 in world frame
- * _Lc2: Angular momentum at time t2 in world frame
- * _q2 : Base orientation at time t2
- * _res: Factor residuals (c,cd,Lc,o) O is rotation vector... NOT A QUATERNION
- */
- template<typename D1, typename D2, typename D3, typename D4>
- bool residual(const MatrixBase<D1> & _c1,
- const MatrixBase<D1> & _cd1,
- const MatrixBase<D1> & _Lc1,
- const QuaternionBase<D2> & _q1,
- const MatrixBase<D1> & _bp1,
- const MatrixBase<D1> & _bw1,
- const MatrixBase<D1> & _c2,
- const MatrixBase<D1> & _cd2,
- const MatrixBase<D1> & _Lc2,
- const QuaternionBase<D3> & _q2,
- MatrixBase<D4> & _res) const;
-
- // Matrix<double,12,1> residual() const;
- // double cost() const;
-
- private:
- /// Preintegrated delta
- Vector3d dc_preint_;
- Vector3d dcd_preint_;
- Vector3d dLc_preint_;
- Quaterniond dq_preint_;
-
- // Biases used during preintegration
- Vector3d pbc_bias_preint_;
- Vector3d gyro_bias_preint_;
-
- // Jacobians of preintegrated deltas wrt biases
- Matrix3d J_dLc_pb_;
- Matrix3d J_dc_wb_;
- Matrix3d J_dcd_wb_;
- Matrix3d J_dLc_wb_;
- Matrix3d J_dq_wb_;
-
- /// Metrics
- double dt_; ///< delta-time and delta-time-squared between keyframes
- double mass_; ///< constant total robot mass
-
-};
-
-///////////////////// IMPLEMENTATION ////////////////////////////
-
-inline FactorForceTorquePreint::FactorForceTorquePreint(
- const FeatureForceTorquePreintPtr& _ftr_ptr,
- const CaptureForceTorquePreintPtr& _cap_origin_ptr,
+ public:
+ FactorForceTorquePreint(const FeatureForceTorquePreintPtr& _ftr_ptr,
+ const CaptureForceTorquePreintPtr& _cap_origin_ptr, // gets to bp1, bw1
const ProcessorBasePtr& _processor_ptr,
const CaptureBasePtr& _cap_ikin_other,
const CaptureBasePtr& _cap_gyro_other,
bool _apply_loss_function,
- FactorStatus _status) :
- FactorAutodiff<FactorForceTorquePreint, 12, 3,3,3,4,3,6, 3,3,3,4>(
- "FactorForceTorquePreint",
- TOP_MOTION,
- _ftr_ptr,
- _cap_origin_ptr->getFrame(),
- _cap_origin_ptr,
- nullptr,
- nullptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _cap_origin_ptr->getFrame()->getStateBlock('C'),
- _cap_origin_ptr->getFrame()->getStateBlock('D'),
- _cap_origin_ptr->getFrame()->getStateBlock('L'),
- _cap_origin_ptr->getFrame()->getO(),
- _cap_ikin_other->getSensorIntrinsic(),
- _cap_gyro_other->getSensorIntrinsic(),
- _ftr_ptr->getFrame()->getStateBlock('C'),
- _ftr_ptr->getFrame()->getStateBlock('D'),
- _ftr_ptr->getFrame()->getStateBlock('L'),
- _ftr_ptr->getFrame()->getO()
- ),
- dc_preint_(_ftr_ptr->getMeasurement().head<3>()), // dc, dcd, dLc, dq at preintegration time
- dcd_preint_(_ftr_ptr->getMeasurement().segment<3>(3)),
- dLc_preint_(_ftr_ptr->getMeasurement().segment<3>(6)),
- dq_preint_(_ftr_ptr->getMeasurement().data()+9),
- pbc_bias_preint_( std::static_pointer_cast<FeatureForceTorquePreint>(_ftr_ptr)->getPbcBiasPreint()),
- gyro_bias_preint_(std::static_pointer_cast<FeatureForceTorquePreint>(_ftr_ptr)->getGyroBiasPreint()),
- J_dLc_pb_(_ftr_ptr->getJacobianBias().block<3,3>(6,0)), // Jac dLc wrt pbc bias
- J_dc_wb_ (_ftr_ptr->getJacobianBias().block<3,3>(0,3)), // Jac dc wrt gyro bias
- J_dcd_wb_(_ftr_ptr->getJacobianBias().block<3,3>(3,3)), // Jac dc wrt gyro bias
- J_dLc_wb_(_ftr_ptr->getJacobianBias().block<3,3>(6,3)), // Jac dcd wrt gyro bias
- J_dq_wb_ (_ftr_ptr->getJacobianBias().block<3,3>(9,3)), // Jac dLc wrt gyro bias
- dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getFrame()->getTimeStamp()),
- mass_(std::static_pointer_cast<SensorForceTorque>(_ftr_ptr->getCapture()->getSensor())->getMass())
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorForceTorquePreint() override = default;
+
+ /** \brief : compute the residual from the state blocks being iterated by the solver.
+ -> computes the expected measurement
+ -> corrects actual measurement with new bias
+ -> compares the corrected measurement with the expected one
+ -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
+ */
+ template <typename T>
+ bool operator()(const T* const _c1,
+ const T* const _cd1,
+ const T* const _Lc1,
+ const T* const _q1,
+ const T* const _bp1,
+ const T* const _bw1,
+ const T* const _c2,
+ const T* const _cd2,
+ const T* const _Lc2,
+ const T* const _q2,
+ T* _res) const;
+
+ /** \brief : compute the residual from the state blocks being iterated by the solver. (same as operator())
+ -> computes the expected measurement
+ -> corrects actual measurement with new bias
+ -> compares the corrected measurement with the expected one
+ -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
+ * params :
+ * _c1 : COM position at time t1 in world frame
+ * _cd1: COM velocity at time t1 in world frame
+ * _Lc1: Angular momentum at time t1 in world frame
+ * _q1 : Base orientation at time t1
+ * _bp1: COM position measurement at time t1 in body frame
+ * _bw1: gyroscope bias at time t1 in body frame
+ * _c2 : COM position at time t2 in world frame
+ * _cd2: COM velocity at time t2 in world frame
+ * _Lc2: Angular momentum at time t2 in world frame
+ * _q2 : Base orientation at time t2
+ * _res: Factor residuals (c,cd,Lc,o) O is rotation vector... NOT A QUATERNION
+ */
+ template <typename D1, typename D2, typename D3, typename D4>
+ bool residual(const MatrixBase<D1>& _c1,
+ const MatrixBase<D1>& _cd1,
+ const MatrixBase<D1>& _Lc1,
+ const QuaternionBase<D2>& _q1,
+ const MatrixBase<D1>& _bp1,
+ const MatrixBase<D1>& _bw1,
+ const MatrixBase<D1>& _c2,
+ const MatrixBase<D1>& _cd2,
+ const MatrixBase<D1>& _Lc2,
+ const QuaternionBase<D3>& _q2,
+ MatrixBase<D4>& _res) const;
+
+ // Matrix<double,12,1> residual() const;
+ // double cost() const;
+
+ private:
+ /// Preintegrated delta
+ Vector3d dc_preint_;
+ Vector3d dcd_preint_;
+ Vector3d dLc_preint_;
+ Quaterniond dq_preint_;
+
+ // Biases used during preintegration
+ Vector3d pbc_bias_preint_;
+ Vector3d gyro_bias_preint_;
+
+ // Jacobians of preintegrated deltas wrt biases
+ Matrix3d J_dLc_pb_;
+ Matrix3d J_dc_wb_;
+ Matrix3d J_dcd_wb_;
+ Matrix3d J_dLc_wb_;
+ Matrix3d J_dq_wb_;
+
+ /// Metrics
+ double dt_; ///< delta-time and delta-time-squared between keyframes
+ double mass_; ///< constant total robot mass
+};
+
+///////////////////// IMPLEMENTATION ////////////////////////////
+
+inline FactorForceTorquePreint::FactorForceTorquePreint(const FeatureForceTorquePreintPtr& _ftr_ptr,
+ const CaptureForceTorquePreintPtr& _cap_origin_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ const CaptureBasePtr& _cap_ikin_other,
+ const CaptureBasePtr& _cap_gyro_other,
+ bool _apply_loss_function,
+ FactorStatus _status)
+ : FactorAutodiff<FactorForceTorquePreint, 12, 3, 3, 3, 4, 3, 6, 3, 3, 3, 4>(
+ "FactorForceTorquePreint",
+ TOP_MOTION,
+ _ftr_ptr,
+ _cap_origin_ptr->getFrame(),
+ _cap_origin_ptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _cap_origin_ptr->getFrame()->getStateBlock('C'),
+ _cap_origin_ptr->getFrame()->getStateBlock('D'),
+ _cap_origin_ptr->getFrame()->getStateBlock('L'),
+ _cap_origin_ptr->getFrame()->getO(),
+ _cap_ikin_other->getSensorIntrinsic(),
+ _cap_gyro_other->getSensorIntrinsic(),
+ _ftr_ptr->getFrame()->getStateBlock('C'),
+ _ftr_ptr->getFrame()->getStateBlock('D'),
+ _ftr_ptr->getFrame()->getStateBlock('L'),
+ _ftr_ptr->getFrame()->getO()),
+ dc_preint_(_ftr_ptr->getMeasurement().head<3>()), // dc, dcd, dLc, dq at preintegration time
+ dcd_preint_(_ftr_ptr->getMeasurement().segment<3>(3)),
+ dLc_preint_(_ftr_ptr->getMeasurement().segment<3>(6)),
+ dq_preint_(_ftr_ptr->getMeasurement().data() + 9),
+ pbc_bias_preint_(std::static_pointer_cast<FeatureForceTorquePreint>(_ftr_ptr)->getPbcBiasPreint()),
+ gyro_bias_preint_(std::static_pointer_cast<FeatureForceTorquePreint>(_ftr_ptr)->getGyroBiasPreint()),
+ J_dLc_pb_(_ftr_ptr->getJacobianBias().block<3, 3>(6, 0)), // Jac dLc wrt pbc bias
+ J_dc_wb_(_ftr_ptr->getJacobianBias().block<3, 3>(0, 3)), // Jac dc wrt gyro bias
+ J_dcd_wb_(_ftr_ptr->getJacobianBias().block<3, 3>(3, 3)), // Jac dc wrt gyro bias
+ J_dLc_wb_(_ftr_ptr->getJacobianBias().block<3, 3>(6, 3)), // Jac dcd wrt gyro bias
+ J_dq_wb_(_ftr_ptr->getJacobianBias().block<3, 3>(9, 3)), // Jac dLc wrt gyro bias
+ dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getFrame()->getTimeStamp()),
+ mass_(std::static_pointer_cast<SensorForceTorque>(_ftr_ptr->getCapture()->getSensor())->getMass())
{
-//
+ //
}
-template<typename T>
-inline bool FactorForceTorquePreint::operator ()(const T* const _c1,
- const T* const _cd1,
- const T* const _Lc1,
- const T* const _q1,
- const T* const _bp1,
- const T* const _bw1,
- const T* const _c2,
- const T* const _cd2,
- const T* const _Lc2,
- const T* const _q2,
- T* _res) const
+template <typename T>
+inline bool FactorForceTorquePreint::operator()(const T* const _c1,
+ const T* const _cd1,
+ const T* const _Lc1,
+ const T* const _q1,
+ const T* const _bp1,
+ const T* const _bw1,
+ const T* const _c2,
+ const T* const _cd2,
+ const T* const _Lc2,
+ const T* const _q2,
+ T* _res) const
{
- Map<const Matrix<T,3,1> > c1(_c1);
- Map<const Matrix<T,3,1> > cd1(_cd1);
- Map<const Matrix<T,3,1> > Lc1(_Lc1);
- Map<const Quaternion<T> > q1(_q1);
- Map<const Matrix<T,3,1> > bp1(_bp1);
- Map<const Matrix<T,3,1> > bw1(_bw1 + 3); // bw1 = bimu = [ba, bw]
- Map<const Matrix<T,3,1> > c2(_c2);
- Map<const Matrix<T,3,1> > cd2(_cd2);
- Map<const Matrix<T,3,1> > Lc2(_Lc2);
- Map<const Quaternion<T> > q2(_q2);
- Map<Matrix<T,12,1> > res(_res);
+ Map<const Matrix<T, 3, 1> > c1(_c1);
+ Map<const Matrix<T, 3, 1> > cd1(_cd1);
+ Map<const Matrix<T, 3, 1> > Lc1(_Lc1);
+ Map<const Quaternion<T> > q1(_q1);
+ Map<const Matrix<T, 3, 1> > bp1(_bp1);
+ Map<const Matrix<T, 3, 1> > bw1(_bw1 + 3); // bw1 = bimu = [ba, bw]
+ Map<const Matrix<T, 3, 1> > c2(_c2);
+ Map<const Matrix<T, 3, 1> > cd2(_cd2);
+ Map<const Matrix<T, 3, 1> > Lc2(_Lc2);
+ Map<const Quaternion<T> > q2(_q2);
+ Map<Matrix<T, 12, 1> > res(_res);
residual(c1, cd1, Lc1, q1, bp1, bw1, c2, cd2, Lc2, q2, res);
return true;
}
-template<typename D1, typename D2, typename D3, typename D4>
-inline bool FactorForceTorquePreint::residual(const MatrixBase<D1> & _c1,
- const MatrixBase<D1> & _cd1,
- const MatrixBase<D1> & _Lc1,
- const QuaternionBase<D2> & _q1,
- const MatrixBase<D1> & _bp1,
- const MatrixBase<D1> & _bw1,
- const MatrixBase<D1> & _c2,
- const MatrixBase<D1> & _cd2,
- const MatrixBase<D1> & _Lc2,
- const QuaternionBase<D3> & _q2,
- MatrixBase<D4> & _res) const
+template <typename D1, typename D2, typename D3, typename D4>
+inline bool FactorForceTorquePreint::residual(const MatrixBase<D1>& _c1,
+ const MatrixBase<D1>& _cd1,
+ const MatrixBase<D1>& _Lc1,
+ const QuaternionBase<D2>& _q1,
+ const MatrixBase<D1>& _bp1,
+ const MatrixBase<D1>& _bw1,
+ const MatrixBase<D1>& _c2,
+ const MatrixBase<D1>& _cd2,
+ const MatrixBase<D1>& _Lc2,
+ const QuaternionBase<D3>& _q2,
+ MatrixBase<D4>& _res) const
{
/* Help for the Imu residual function
*
@@ -237,9 +234,8 @@ inline bool FactorForceTorquePreint::residual(const MatrixBase<D1> & _c1,
* T_step = J_preint * (b - b_preint) // compute the delta correction step due to bias change
*
* Method 1:
- * D_corr = plus(D_preint, T_step) // correct the pre-integrated delta with correction step due to bias change
- * T_err = diff(D_exp, D_corr) // compare against expected delta
- * res = W.sqrt * T_err
+ * D_corr = plus(D_preint, T_step) // correct the pre-integrated delta with correction step due to bias
+ * change T_err = diff(D_exp, D_corr) // compare against expected delta res = W.sqrt * T_err
*
* results in:
* res = W.sqrt * ( diff ( D_exp, plus(D_preint, J_preint * (b - b_preint) ) ) )
@@ -255,49 +251,66 @@ inline bool FactorForceTorquePreint::residual(const MatrixBase<D1> & _c1,
* NOTE: See optimization report at the end of this file for comparisons of both methods.
*/
- //needed typedefs
+ // needed typedefs
typedef typename D1::Scalar T;
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D4, 12);
// 1. Expected delta from states
- Matrix<T, 3, 1 > dc_exp;
- Matrix<T, 3, 1 > dcd_exp;
- Matrix<T, 3, 1 > dLc_exp;
- Quaternion<T> dq_exp;
+ Matrix<T, 3, 1> dc_exp;
+ Matrix<T, 3, 1> dcd_exp;
+ Matrix<T, 3, 1> dLc_exp;
+ Quaternion<T> dq_exp;
bodydynamics::betweenStates(_c1, _cd1, _Lc1, _q1, _c2, _cd2, _Lc2, _q2, dt_, dc_exp, dcd_exp, dLc_exp, dq_exp);
// 2. Corrected integrated delta: delta_corr = delta_preint (+) J_bias * (bias_current - bias_preint)
// 2.a. Compute the delta step in tangent space: step = J_bias * (bias - bias_preint)
- Matrix<T, 3, 1> dc_step = J_dc_wb_ * (_bw1 - gyro_bias_preint_);
- Matrix<T, 3, 1> dcd_step = J_dcd_wb_ * (_bw1 - gyro_bias_preint_);
+ Matrix<T, 3, 1> dc_step = J_dc_wb_ * (_bw1 - gyro_bias_preint_);
+ Matrix<T, 3, 1> dcd_step = J_dcd_wb_ * (_bw1 - gyro_bias_preint_);
Matrix<T, 3, 1> dLc_step = J_dLc_pb_ * (_bp1 - pbc_bias_preint_) + J_dLc_wb_ * (_bw1 - gyro_bias_preint_);
- Matrix<T, 3, 1> do_step = J_dq_wb_ * (_bw1 - gyro_bias_preint_);
+ Matrix<T, 3, 1> do_step = J_dq_wb_ * (_bw1 - gyro_bias_preint_);
// 2.b. Add the correction step to the preintegrated delta: delta_cor = delta_preint (+) step
- Matrix<T,3,1> dc_correct;
- Matrix<T,3,1> dcd_correct;
- Matrix<T,3,1> dLc_correct;
- Quaternion<T> dq_correct;
-
- bodydynamics::plus(dc_preint_.cast<T>(), dcd_preint_.cast<T>(), dLc_preint_.cast<T>(), dq_preint_.cast<T>(),
- dc_step, dcd_step, dLc_step, do_step,
- dc_correct, dcd_correct, dLc_correct, dq_correct);
+ Matrix<T, 3, 1> dc_correct;
+ Matrix<T, 3, 1> dcd_correct;
+ Matrix<T, 3, 1> dLc_correct;
+ Quaternion<T> dq_correct;
+
+ bodydynamics::plus(dc_preint_.cast<T>(),
+ dcd_preint_.cast<T>(),
+ dLc_preint_.cast<T>(),
+ dq_preint_.cast<T>(),
+ dc_step,
+ dcd_step,
+ dLc_step,
+ do_step,
+ dc_correct,
+ dcd_correct,
+ dLc_correct,
+ dq_correct);
// 3. Delta error in minimal form: D_err = diff(D_exp , D_corr)
// Note the Dt here is zero because it's the delta-time between the same time stamps!
- Matrix<T, 12, 1> d_error;
- Map<Matrix<T, 3, 1> > dc_error (d_error.data() );
- Map<Matrix<T, 3, 1> > dcd_error(d_error.data() + 3);
- Map<Matrix<T, 3, 1> > dLc_error(d_error.data() + 6);
- Map<Matrix<T, 3, 1> > do_error (d_error.data() + 9);
-
-
- bodydynamics::diff(dc_exp, dcd_exp, dLc_exp, dq_exp,
- dc_correct, dcd_correct, dLc_correct, dq_correct,
- dc_error, dcd_error, dLc_error, do_error);
+ Matrix<T, 12, 1> d_error;
+ Map<Matrix<T, 3, 1> > dc_error(d_error.data());
+ Map<Matrix<T, 3, 1> > dcd_error(d_error.data() + 3);
+ Map<Matrix<T, 3, 1> > dLc_error(d_error.data() + 6);
+ Map<Matrix<T, 3, 1> > do_error(d_error.data() + 9);
+
+ bodydynamics::diff(dc_exp,
+ dcd_exp,
+ dLc_exp,
+ dq_exp,
+ dc_correct,
+ dcd_correct,
+ dLc_correct,
+ dq_correct,
+ dc_error,
+ dcd_error,
+ dLc_error,
+ do_error);
_res = getMeasurementSquareRootInformationUpper() * d_error;
@@ -308,12 +321,11 @@ inline bool FactorForceTorquePreint::residual(const MatrixBase<D1> & _c1,
// {
// Matrix<double,12,1> res;
-
// FrameBasePtr frm_prev = getFrameOther();
// FrameBasePtr frm_curr = getFeature()->getFrame();
// CaptureBaseWPtrList cap_lst = getCaptureOtherList(); // !! not retrieving the rigt captures...
-// auto cap_ikin_other = cap_lst.front().lock();
+// auto cap_ikin_other = cap_lst.front().lock();
// auto cap_gyro_other = cap_lst.back().lock();
// Map<const Matrix<double,3,1> > c1( frm_prev->getStateBlock('C')->getState().data());
@@ -321,11 +333,11 @@ inline bool FactorForceTorquePreint::residual(const MatrixBase<D1> & _c1,
// Map<const Matrix<double,3,1> > Lc1(frm_prev->getStateBlock('L')->getState().data());
// Map<const Quaternion<double> > q1( frm_prev->getStateBlock('O')->getState().data());
// Map<const Matrix<double,3,1> > bp1(cap_ikin_other->getStateBlock('I')->getState().data());
-// Map<const Matrix<double,3,1> > bw1(cap_gyro_other->getStateBlock('I')->getState().data() + 3); // bw1 = bimu = [ba, bw]
-// Map<const Matrix<double,3,1> > c2 (frm_curr->getStateBlock('C')->getState().data());
-// Map<const Matrix<double,3,1> > cd2(frm_curr->getStateBlock('D')->getState().data());
-// Map<const Matrix<double,3,1> > Lc2(frm_curr->getStateBlock('L')->getState().data());
-// Map<const Quaternion<double> > q2 (frm_curr->getStateBlock('O')->getState().data());
+// Map<const Matrix<double,3,1> > bw1(cap_gyro_other->getStateBlock('I')->getState().data() + 3); // bw1 = bimu =
+// [ba, bw] Map<const Matrix<double,3,1> > c2 (frm_curr->getStateBlock('C')->getState().data()); Map<const
+// Matrix<double,3,1> > cd2(frm_curr->getStateBlock('D')->getState().data()); Map<const Matrix<double,3,1> >
+// Lc2(frm_curr->getStateBlock('L')->getState().data()); Map<const Quaternion<double> > q2
+// (frm_curr->getStateBlock('O')->getState().data());
// residual(c1, cd1, Lc1, q1, bp1, bw1, c2, cd2, Lc2, q2, res);
@@ -337,6 +349,6 @@ inline bool FactorForceTorquePreint::residual(const MatrixBase<D1> & _c1,
// Matrix<double,12,1> toto = residual();
// }
-} // namespace wolf
+} // namespace wolf
#endif
diff --git a/include/bodydynamics/factor/factor_inertial_kinematics.h b/include/bodydynamics/factor/factor_inertial_kinematics.h
index 00e1758..5e84c67 100644
--- a/include/bodydynamics/factor/factor_inertial_kinematics.h
+++ b/include/bodydynamics/factor/factor_inertial_kinematics.h
@@ -26,152 +26,149 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorInertialKinematics);
-class FactorInertialKinematics : public FactorAutodiff<FactorInertialKinematics, 9,3,4,3, 3,3,3,3,6>
+class FactorInertialKinematics : public FactorAutodiff<FactorInertialKinematics, 9, 3, 4, 3, 3, 3, 3, 3, 6>
{
- public:
- FactorInertialKinematics(const FeatureBasePtr& _feat,
- const StateBlockPtr& _sb_bias_imu,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE);
-
- ~FactorInertialKinematics() override { /* nothing */ }
-
- // to keep track of the imu bias, not the standard way to proceed
- StateBlockPtr sb_bias_imu_;
-
- /*
- Factor state blocks:
- _pb: W_p_WB -> base position in world frame
- _qb: W_q_B -> base orientation in world frame
- _vb: W_v_WB -> base velocity in world frame
- _c: W_p_WC -> COM position in world frame
- _cd: W_v_WC -> COM velocity in world frame
- _Lc: W_Lc -> angular momentum at the COM in body frame
- _bp: B_b_BC -> bias on relative body->COM position kinematic measurement
- _baw: 6d Imu bias containing [acc bias, gyro bias]
- */
- template<typename T>
- bool operator () (
- const T* const _pb,
- const T* const _qb,
- const T* const _vb,
- const T* const _c,
- const T* const _cd,
- const T* const _Lc,
- const T* const _bp,
- const T* const _baw,
- T* _res) const;
-
- Vector9d residual() const;
- double cost() const;
+ public:
+ FactorInertialKinematics(const FeatureBasePtr& _feat,
+ const StateBlockPtr& _sb_bias_imu,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorInertialKinematics() override
+ { /* nothing */
+ }
+
+ // to keep track of the imu bias, not the standard way to proceed
+ StateBlockPtr sb_bias_imu_;
+
+ /*
+ Factor state blocks:
+ _pb: W_p_WB -> base position in world frame
+ _qb: W_q_B -> base orientation in world frame
+ _vb: W_v_WB -> base velocity in world frame
+ _c: W_p_WC -> COM position in world frame
+ _cd: W_v_WC -> COM velocity in world frame
+ _Lc: W_Lc -> angular momentum at the COM in body frame
+ _bp: B_b_BC -> bias on relative body->COM position kinematic measurement
+ _baw: 6d Imu bias containing [acc bias, gyro bias]
+ */
+ template <typename T>
+ bool operator()(const T* const _pb,
+ const T* const _qb,
+ const T* const _vb,
+ const T* const _c,
+ const T* const _cd,
+ const T* const _Lc,
+ const T* const _bp,
+ const T* const _baw,
+ T* _res) const;
+ Vector9d residual() const;
+ double cost() const;
};
} /* namespace wolf */
-
-namespace wolf {
-
-FactorInertialKinematics::FactorInertialKinematics(
- const FeatureBasePtr& _feat,
- const StateBlockPtr& _sb_bias_imu,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) :
- FactorAutodiff("FactorInertialKinematics",
- TOP_GEOM,
- _feat,
- nullptr, // _frame_other_ptr
- nullptr, // _capture_other_ptr
- nullptr, // _feature_other_ptr
- nullptr, // _landmark_other_ptr
- _processor_ptr,
- _apply_loss_function,
- _status,
- _feat->getFrame()->getP(),
- _feat->getFrame()->getO(),
- _feat->getFrame()->getV(),
- _feat->getFrame()->getStateBlock('C'), // COM position
- _feat->getFrame()->getStateBlock('D'), // COM velocity (bad name)
- _feat->getFrame()->getStateBlock('L'), // Angular momentum
- _feat->getCapture()->getStateBlock('I'), // body-to-COM relative position bias
- _sb_bias_imu
- ),
- sb_bias_imu_(_sb_bias_imu)
+namespace wolf
+{
+FactorInertialKinematics::FactorInertialKinematics(const FeatureBasePtr& _feat,
+ const StateBlockPtr& _sb_bias_imu,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status)
+ : FactorAutodiff("FactorInertialKinematics",
+ TOP_GEOM,
+ _feat,
+ nullptr, // _frame_other_ptr
+ nullptr, // _capture_other_ptr
+ nullptr, // _feature_other_ptr
+ nullptr, // _landmark_other_ptr
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _feat->getFrame()->getP(),
+ _feat->getFrame()->getO(),
+ _feat->getFrame()->getV(),
+ _feat->getFrame()->getStateBlock('C'), // COM position
+ _feat->getFrame()->getStateBlock('D'), // COM velocity (bad name)
+ _feat->getFrame()->getStateBlock('L'), // Angular momentum
+ _feat->getCapture()->getStateBlock('I'), // body-to-COM relative position bias
+ _sb_bias_imu),
+ sb_bias_imu_(_sb_bias_imu)
{
}
-Vector9d FactorInertialKinematics::residual() const{
+Vector9d FactorInertialKinematics::residual() const
+{
Vector9d res;
- double * pb, * qb, * vb, * c, * cd, * Lc, * bp, * baw;
- pb = getFrame()->getP()->getState().data();
- qb = getFrame()->getO()->getState().data();
- vb = getFrame()->getV()->getState().data();
- c = getFrame()->getStateBlock('C')->getState().data();
- cd = getFrame()->getStateBlock('D')->getState().data();
- Lc = getFrame()->getStateBlock('L')->getState().data();
- bp = getCapture()->getStateBlock('I')->getState().data();
+ double * pb, *qb, *vb, *c, *cd, *Lc, *bp, *baw;
+ pb = getFrame()->getP()->getState().data();
+ qb = getFrame()->getO()->getState().data();
+ vb = getFrame()->getV()->getState().data();
+ c = getFrame()->getStateBlock('C')->getState().data();
+ cd = getFrame()->getStateBlock('D')->getState().data();
+ Lc = getFrame()->getStateBlock('L')->getState().data();
+ bp = getCapture()->getStateBlock('I')->getState().data();
baw = sb_bias_imu_->getState().data();
- operator() (pb, qb, vb, c, cd, Lc, bp, baw, res.data());
+ operator()(pb, qb, vb, c, cd, Lc, bp, baw, res.data());
// std::cout << "res: " << res.transpose() << std::endl;
return res;
}
-double FactorInertialKinematics::cost() const{
+double FactorInertialKinematics::cost() const
+{
return residual().squaredNorm();
}
-template<typename T>
-bool FactorInertialKinematics::operator () (
- const T* const _pb,
- const T* const _qb,
- const T* const _vb,
- const T* const _c,
- const T* const _cd,
- const T* const _Lc,
- const T* const _bp,
- const T* const _baw,
- T* _res) const
+template <typename T>
+bool FactorInertialKinematics::operator()(const T* const _pb,
+ const T* const _qb,
+ const T* const _vb,
+ const T* const _c,
+ const T* const _cd,
+ const T* const _Lc,
+ const T* const _bp,
+ const T* const _baw,
+ T* _res) const
{
// State variables instanciation
- Map<const Matrix<T,3,1> > pb(_pb);
- Map<const Quaternion<T> > qb(_qb);
- Map<const Matrix<T,3,1> > vb(_vb);
- Map<const Matrix<T,3,1> > c(_c);
- Map<const Matrix<T,3,1> > cd(_cd);
- Map<const Matrix<T,3,1> > Lc(_Lc);
- Map<const Matrix<T,3,1> > bp(_bp);
- Map<const Matrix<T,6,1> > baw(_baw);
+ Map<const Matrix<T, 3, 1> > pb(_pb);
+ Map<const Quaternion<T> > qb(_qb);
+ Map<const Matrix<T, 3, 1> > vb(_vb);
+ Map<const Matrix<T, 3, 1> > c(_c);
+ Map<const Matrix<T, 3, 1> > cd(_cd);
+ Map<const Matrix<T, 3, 1> > Lc(_Lc);
+ Map<const Matrix<T, 3, 1> > bp(_bp);
+ Map<const Matrix<T, 6, 1> > baw(_baw);
- Map<Matrix<T,9,1> > res(_res);
+ Map<Matrix<T, 9, 1> > res(_res);
auto feat = std::static_pointer_cast<const FeatureInertialKinematics>(getFeature());
// Measurements retrieval
- Map<const Vector3d> pBC_m(getMeasurement().data() ); // B_p_BC
+ Map<const Vector3d> pBC_m(getMeasurement().data()); // B_p_BC
Map<const Vector3d> vBC_m(getMeasurement().data() + 3); // B_v_BC
- Map<const Vector3d> w_m (getMeasurement().data() + 6); // B_wB
+ Map<const Vector3d> w_m(getMeasurement().data() + 6); // B_wB
// Error variable instanciation
- Matrix<T, 9, 1> err;
- Map<Matrix<T, 3, 1> > err_c (err.data() );
+ Matrix<T, 9, 1> err;
+ Map<Matrix<T, 3, 1> > err_c(err.data());
Map<Matrix<T, 3, 1> > err_cd(err.data() + 3);
Map<Matrix<T, 3, 1> > err_Lc(err.data() + 6);
- err_c = pBC_m - (qb.conjugate()*(c - pb) + bp);
- err_cd = vBC_m + (w_m - baw.tail(3)).cross(pBC_m - bp) - qb.conjugate()*(cd - vb);
- err_Lc = feat->getBIq() * (w_m - baw.tail(3)) + feat->getBLcm() - qb.conjugate()*Lc;
+ err_c = pBC_m - (qb.conjugate() * (c - pb) + bp);
+ err_cd = vBC_m + (w_m - baw.tail(3)).cross(pBC_m - bp) - qb.conjugate() * (cd - vb);
+ err_Lc = feat->getBIq() * (w_m - baw.tail(3)) + feat->getBLcm() - qb.conjugate() * Lc;
res = getMeasurementSquareRootInformationUpper() * err;
return true;
}
-} // namespace wolf
+} // namespace wolf
#endif /* FACTOR__INERTIAL_KINEMATICS_H_ */
diff --git a/include/bodydynamics/factor/factor_point_feet_altitude.h b/include/bodydynamics/factor/factor_point_feet_altitude.h
index d0ad5cd..e32b331 100644
--- a/include/bodydynamics/factor/factor_point_feet_altitude.h
+++ b/include/bodydynamics/factor/factor_point_feet_altitude.h
@@ -25,113 +25,101 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorPointFeetAltitude);
-class FactorPointFeetAltitude : public FactorAutodiff<FactorPointFeetAltitude,
- 1,
- 3,
- 4
- >
+class FactorPointFeetAltitude : public FactorAutodiff<FactorPointFeetAltitude, 1, 3, 4>
{
- public:
- FactorPointFeetAltitude(const FeatureBasePtr& _ftr_current_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE);
-
- ~FactorPointFeetAltitude() override { /* nothing */ }
-
- /*
- Factor state blocks:
- _pb: W_p_WB -> base position in world frame
- _qb: W_q_B -> base orientation in world frame
- */
- template<typename T>
- bool operator () (
- const T* const _pb,
- const T* const _qb,
- T* _res) const;
-
- Vector1d error() const;
- Vector1d res() const;
- double cost() const;
-
+ public:
+ FactorPointFeetAltitude(const FeatureBasePtr& _ftr_current_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorPointFeetAltitude() override
+ { /* nothing */
+ }
+
+ /*
+ Factor state blocks:
+ _pb: W_p_WB -> base position in world frame
+ _qb: W_q_B -> base orientation in world frame
+ */
+ template <typename T>
+ bool operator()(const T* const _pb, const T* const _qb, T* _res) const;
+
+ Vector1d error() const;
+ Vector1d res() const;
+ double cost() const;
};
} /* namespace wolf */
-
-namespace wolf {
-
-FactorPointFeetAltitude::FactorPointFeetAltitude(
- const FeatureBasePtr& _ftr_current_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) :
- FactorAutodiff("FactorPointFeetAltitude",
- TOP_GEOM,
- _ftr_current_ptr,
- nullptr, // _frame_other_ptr
- nullptr, // _capture_other_ptr
- nullptr, // _feature_other_ptr
- nullptr, // _landmark_other_ptr
- _processor_ptr,
- _apply_loss_function,
- _status,
- _ftr_current_ptr->getFrame()->getP(),
- _ftr_current_ptr->getFrame()->getO()
- )
+namespace wolf
+{
+FactorPointFeetAltitude::FactorPointFeetAltitude(const FeatureBasePtr& _ftr_current_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status)
+ : FactorAutodiff("FactorPointFeetAltitude",
+ TOP_GEOM,
+ _ftr_current_ptr,
+ nullptr, // _frame_other_ptr
+ nullptr, // _capture_other_ptr
+ nullptr, // _feature_other_ptr
+ nullptr, // _landmark_other_ptr
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_current_ptr->getFrame()->getP(),
+ _ftr_current_ptr->getFrame()->getO())
{
}
-Vector1d FactorPointFeetAltitude::error() const{
-
- Vector3d pb = getFrame()->getP()->getState();
- Vector4d qb_vec = getFrame()->getO()->getState();
+Vector1d FactorPointFeetAltitude::error() const
+{
+ Vector3d pb = getFrame()->getP()->getState();
+ Vector4d qb_vec = getFrame()->getO()->getState();
Quaterniond qb(qb_vec);
// Measurements retrieval
- Eigen::Matrix<double,4,1> meas = getMeasurement();
- Vector3d b_p_bl = meas.topRows(3);
- Vector1d z = meas.bottomRows(1);
+ Eigen::Matrix<double, 4, 1> meas = getMeasurement();
+ Vector3d b_p_bl = meas.topRows(3);
+ Vector1d z = meas.bottomRows(1);
- return (pb + qb*b_p_bl).block(2,0,1,1) - z;
+ return (pb + qb * b_p_bl).block(2, 0, 1, 1) - z;
}
-Vector1d FactorPointFeetAltitude::res() const{
+Vector1d FactorPointFeetAltitude::res() const
+{
return getMeasurementSquareRootInformationUpper() * error();
}
-
-double FactorPointFeetAltitude::cost() const{
+double FactorPointFeetAltitude::cost() const
+{
return res().squaredNorm();
}
-template<typename T>
-bool FactorPointFeetAltitude::operator () (
- const T* const _pb,
- const T* const _qb,
- T* _res) const
+template <typename T>
+bool FactorPointFeetAltitude::operator()(const T* const _pb, const T* const _qb, T* _res) const
{
- Map<Matrix<T,1,1> > res(_res);
+ Map<Matrix<T, 1, 1> > res(_res);
// State variables instanciation
- Map<const Matrix<T,3,1> > pb(_pb);
- Map<const Quaternion<T> > qb(_qb);
+ Map<const Matrix<T, 3, 1> > pb(_pb);
+ Map<const Quaternion<T> > qb(_qb);
// Measurements retrieval
- Eigen::Matrix<T,4,1> meas = getMeasurement().cast<T>();
- Matrix<T,3,1> b_p_bl = meas.topRows(3); // Bpast_p_BpastL
- Matrix<T,1,1> z = meas.bottomRows(1); // altitude
+ Eigen::Matrix<T, 4, 1> meas = getMeasurement().cast<T>();
+ Matrix<T, 3, 1> b_p_bl = meas.topRows(3); // Bpast_p_BpastL
+ Matrix<T, 1, 1> z = meas.bottomRows(1); // altitude
- Matrix<T,1,1> err = (pb + qb*b_p_bl).block(2,0,1,1) - z;
+ Matrix<T, 1, 1> err = (pb + qb * b_p_bl).block(2, 0, 1, 1) - z;
res = getMeasurementSquareRootInformationUpper() * err;
return true;
}
-} // namespace wolf
+} // namespace wolf
#endif /* FACTOR__POINT_FEET_ALTITUDE_H_ */
diff --git a/include/bodydynamics/factor/factor_point_feet_nomove.h b/include/bodydynamics/factor/factor_point_feet_nomove.h
index efe0dcf..ada68c9 100644
--- a/include/bodydynamics/factor/factor_point_feet_nomove.h
+++ b/include/bodydynamics/factor/factor_point_feet_nomove.h
@@ -25,167 +25,154 @@
#include "core/factor/factor_autodiff.h"
#include "bodydynamics/sensor/sensor_point_feet_nomove.h"
-
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorPointFeetNomove);
-class FactorPointFeetNomove : public FactorAutodiff<FactorPointFeetNomove,
- 3,
- 3,
- 4,
- 3,
- 4
- >
+class FactorPointFeetNomove : public FactorAutodiff<FactorPointFeetNomove, 3, 3, 4, 3, 4>
{
- public:
- FactorPointFeetNomove(const FeatureBasePtr& _ftr_current_ptr,
- const FrameBasePtr& _frame_past_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE);
-
- ~FactorPointFeetNomove() override { /* nothing */ }
-
- /*
- Factor state blocks:
- _pb: W_p_WB -> base position in world frame
- _qb: W_q_B -> base orientation in world frame
- _pb_past: W_p_WB -> base position in world frame, pastious KF
- _qb_past: W_q_B -> base orientation in world frame, pastious KF
- */
- template<typename T>
- bool operator () (
- const T* const _pb,
- const T* const _qb,
- const T* const _pb_past,
- const T* const _qb_past,
- T* _res) const;
-
- Vector3d error() const;
- Vector3d res() const;
- double cost() const;
+ public:
+ FactorPointFeetNomove(const FeatureBasePtr& _ftr_current_ptr,
+ const FrameBasePtr& _frame_past_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorPointFeetNomove() override
+ { /* nothing */
+ }
+ /*
+ Factor state blocks:
+ _pb: W_p_WB -> base position in world frame
+ _qb: W_q_B -> base orientation in world frame
+ _pb_past: W_p_WB -> base position in world frame, pastious KF
+ _qb_past: W_q_B -> base orientation in world frame, pastious KF
+ */
+ template <typename T>
+ bool operator()(const T* const _pb, const T* const _qb, const T* const _pb_past, const T* const _qb_past, T* _res)
+ const;
+
+ Vector3d error() const;
+ Vector3d res() const;
+ double cost() const;
};
} /* namespace wolf */
-
-namespace wolf {
-
-FactorPointFeetNomove::FactorPointFeetNomove(
- const FeatureBasePtr& _ftr_current_ptr,
- const FrameBasePtr& _frame_past_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) :
- FactorAutodiff("FactorPointFeetNomove",
- TOP_GEOM,
- _ftr_current_ptr,
- _frame_past_ptr, // _frame_other_ptr
- nullptr, // _capture_other_ptr
- nullptr, // _feature_other_ptr
- nullptr, // _landmark_other_ptr
- _processor_ptr,
- _apply_loss_function,
- _status,
- _ftr_current_ptr->getFrame()->getP(),
- _ftr_current_ptr->getFrame()->getO(),
- _frame_past_ptr->getP(),
- _frame_past_ptr->getO()
- )
+namespace wolf
+{
+FactorPointFeetNomove::FactorPointFeetNomove(const FeatureBasePtr& _ftr_current_ptr,
+ const FrameBasePtr& _frame_past_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status)
+ : FactorAutodiff("FactorPointFeetNomove",
+ TOP_GEOM,
+ _ftr_current_ptr,
+ _frame_past_ptr, // _frame_other_ptr
+ nullptr, // _capture_other_ptr
+ nullptr, // _feature_other_ptr
+ nullptr, // _landmark_other_ptr
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_current_ptr->getFrame()->getP(),
+ _ftr_current_ptr->getFrame()->getO(),
+ _frame_past_ptr->getP(),
+ _frame_past_ptr->getO())
{
}
-Vector3d FactorPointFeetNomove::error() const{
-
- Vector3d pb = getFrame()->getP()->getState();
- Vector4d qb_vec = getFrame()->getO()->getState();
+Vector3d FactorPointFeetNomove::error() const
+{
+ Vector3d pb = getFrame()->getP()->getState();
+ Vector4d qb_vec = getFrame()->getO()->getState();
Quaterniond qb(qb_vec);
- Vector3d pbm = getFrameOther()->getP()->getState();
- Vector4d qb_past_vec = getFrameOther()->getO()->getState();
+ Vector3d pbm = getFrameOther()->getP()->getState();
+ Vector4d qb_past_vec = getFrameOther()->getO()->getState();
Quaterniond qbm(qb_past_vec);
// Measurements retrieval
- Matrix<double,6,1> meas = getMeasurement();
- Vector3d bm_p_bml = meas.topRows(3);
- Vector3d b_p_bl = meas.bottomRows(3);
-
- return (pbm + qbm*bm_p_bml) - (pb + qb*b_p_bl);
+ Matrix<double, 6, 1> meas = getMeasurement();
+ Vector3d bm_p_bml = meas.topRows(3);
+ Vector3d b_p_bl = meas.bottomRows(3);
+ return (pbm + qbm * bm_p_bml) - (pb + qb * b_p_bl);
}
-Vector3d FactorPointFeetNomove::res() const{
+Vector3d FactorPointFeetNomove::res() const
+{
return getMeasurementSquareRootInformationUpper() * error();
}
-
-double FactorPointFeetNomove::cost() const{
+double FactorPointFeetNomove::cost() const
+{
return res().squaredNorm();
}
-template<typename T>
-bool FactorPointFeetNomove::operator () (
- const T* const _pb,
- const T* const _qb,
- const T* const _pb_past,
- const T* const _qb_past,
- T* _res) const
+template <typename T>
+bool FactorPointFeetNomove::operator()(const T* const _pb,
+ const T* const _qb,
+ const T* const _pb_past,
+ const T* const _qb_past,
+ T* _res) const
{
- Map<Matrix<T,3,1> > res(_res);
+ Map<Matrix<T, 3, 1> > res(_res);
// State variables instanciation
- Map<const Matrix<T,3,1> > pb(_pb);
- Map<const Quaternion<T> > qb(_qb);
- Map<const Matrix<T,3,1> > pbm(_pb_past);
- Map<const Quaternion<T> > qbm(_qb_past);
+ Map<const Matrix<T, 3, 1> > pb(_pb);
+ Map<const Quaternion<T> > qb(_qb);
+ Map<const Matrix<T, 3, 1> > pbm(_pb_past);
+ Map<const Quaternion<T> > qbm(_qb_past);
// Measurements retrieval
- auto& meas = getMeasurement();
- Matrix<T,3,1> bm_p_bml = meas.segment(0, 3).cast<T>(); // Bminus_p_BminusL
- Matrix<T,4,1> bm_qvec_l = meas.segment(3, 4).cast<T>(); // Bminus_q_BminusL
- Matrix<T,3,1> b_p_bl = meas.segment(7, 3).cast<T>(); // B_p_BL
- Matrix<T,4,1> b_qvec_l = meas.segment(10, 4).cast<T>(); // B_q_BL
+ auto& meas = getMeasurement();
+ Matrix<T, 3, 1> bm_p_bml = meas.segment(0, 3).cast<T>(); // Bminus_p_BminusL
+ Matrix<T, 4, 1> bm_qvec_l = meas.segment(3, 4).cast<T>(); // Bminus_q_BminusL
+ Matrix<T, 3, 1> b_p_bl = meas.segment(7, 3).cast<T>(); // B_p_BL
+ Matrix<T, 4, 1> b_qvec_l = meas.segment(10, 4).cast<T>(); // B_q_BL
Eigen::Quaternion<T> bm_q_l(bm_qvec_l);
Eigen::Quaternion<T> b_q_l(b_qvec_l);
- // double radius = std::dynamic_pointer_cast<SensorPointFeetNomove>(getSensor())->getFootRadius(); // DOES NOT COMPILE
+ // double radius = std::dynamic_pointer_cast<SensorPointFeetNomove>(getSensor())->getFootRadius(); // DOES NOT
+ // COMPILE
double radius = 0.0; // deactivate, was not doing so great anyway
// If the radius of the foot is not negligeable, try to account for it with a more complex model
- Matrix<T,3,1> err;
- if (std::fabs(radius) < wolf::Constants::EPS){
- err = (pbm + qbm*bm_p_bml) - (pb + qb*b_p_bl);
+ Matrix<T, 3, 1> err;
+ if (std::fabs(radius) < wolf::Constants::EPS)
+ {
+ err = (pbm + qbm * bm_p_bml) - (pb + qb * b_p_bl);
}
- else{
+ else
+ {
/*
- n: frame with (xy) plane == world but orientation aligned in yaw with the robot base frame
- sometimes called "Navigation frame" in some papers.
+ n: frame with (xy) plane == world but orientation aligned in yaw with the robot base frame
+ sometimes called "Navigation frame" in some papers.
Simple roll without slipping model: we assume that most of actual movement
of the frame attached to the center of the foot is due to rolling along y axis of the foot (for solo)
*/
- Quaternion<T> l_q_lm = (qbm*bm_q_l).inverse() * qb*b_q_l;
- Matrix<T,3,1> l_aa_lm = log_q(l_q_lm);
- Matrix<T,3,1> n_p_lml = Matrix<T,3,1>::Zero();
- n_p_lml(0) = -l_aa_lm(1)*radius;
+ Quaternion<T> l_q_lm = (qbm * bm_q_l).inverse() * qb * b_q_l;
+ Matrix<T, 3, 1> l_aa_lm = log_q(l_q_lm);
+ Matrix<T, 3, 1> n_p_lml = Matrix<T, 3, 1>::Zero();
+ n_p_lml(0) = -l_aa_lm(1) * radius;
- // Factor: w_p_wl = w_p_wlm + w_R_n * n_p_lml
+ // Factor: w_p_wl = w_p_wlm + w_R_n * n_p_lml
// By def, w_R_n is w_R_b with roll and pitch set to zero
// Matrix<T,3,1> aa_bm = q2e(qbm); // DOES NOT WORK -> equivalent angle axis in this case
- Matrix<T,3,1> aa_bm = log_q(qbm);
- aa_bm.segment(0,2) = Matrix<T,2,1>::Zero();
- Quaternion<T> w_q_nm = exp_q(aa_bm);
+ Matrix<T, 3, 1> aa_bm = log_q(qbm);
+ aa_bm.segment(0, 2) = Matrix<T, 2, 1>::Zero();
+ Quaternion<T> w_q_nm = exp_q(aa_bm);
- err = (pbm + qbm*bm_p_bml) + w_q_nm*n_p_lml - (pb + qb*b_p_bl);
+ err = (pbm + qbm * bm_p_bml) + w_q_nm * n_p_lml - (pb + qb * b_p_bl);
}
-
res = getMeasurementSquareRootInformationUpper() * err;
return true;
}
-} // namespace wolf
+} // namespace wolf
#endif /* FACTOR__POINT_FEET_NOMOVE_H_ */
diff --git a/include/bodydynamics/factor/factor_point_feet_zero_velocity.h b/include/bodydynamics/factor/factor_point_feet_zero_velocity.h
index ac18bc8..0bc0154 100644
--- a/include/bodydynamics/factor/factor_point_feet_zero_velocity.h
+++ b/include/bodydynamics/factor/factor_point_feet_zero_velocity.h
@@ -23,104 +23,86 @@
#include "core/factor/factor_autodiff.h"
-
-
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorPointFeetNomove);
-class FactorPointFeetNomove : public FactorAutodiff<FactorPointFeetNomove,
- 3,
- 3,
- 4,
- 3
- >
+class FactorPointFeetNomove : public FactorAutodiff<FactorPointFeetNomove, 3, 3, 4, 3>
{
- public:
- FactorPointFeetNomove(const FeatureBasePtr& _ftr_current_ptr,
- const StateBlockPtr& _bias_imu,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE);
-
- ~FactorPointFeetNomove() override { /* nothing */ }
-
- /*
- Factor state blocks:
- _pb: W_p_WB -> base position in world frame
- _qb: W_q_B -> base orientation in world frame
- _pbm: W_p_WB -> base position in world frame, previous KF
- _qbm: W_q_B -> base orientation in world frame, previous KF
- */
- template<typename T>
- bool operator () (
- const T* const _vb,
- const T* const _qb,
- const T* const _bi,
- T* _res) const;
-
- // Vector9d residual() const;
- // double cost() const;
-
+ public:
+ FactorPointFeetNomove(const FeatureBasePtr& _ftr_current_ptr,
+ const StateBlockPtr& _bias_imu,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorPointFeetNomove() override
+ { /* nothing */
+ }
+
+ /*
+ Factor state blocks:
+ _pb: W_p_WB -> base position in world frame
+ _qb: W_q_B -> base orientation in world frame
+ _pbm: W_p_WB -> base position in world frame, previous KF
+ _qbm: W_q_B -> base orientation in world frame, previous KF
+ */
+ template <typename T>
+ bool operator()(const T* const _vb, const T* const _qb, const T* const _bi, T* _res) const;
+
+ // Vector9d residual() const;
+ // double cost() const;
};
} /* namespace wolf */
-
-namespace wolf {
-
-FactorPointFeetNomove::FactorPointFeetNomove(
- const FeatureBasePtr& _ftr_current_ptr,
- const StateBlockPtr& _bias_imu,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) :
- FactorAutodiff("FactorPointFeetNomove",
- TOP_GEOM,
- _ftr_current_ptr,
- nullptr, // _frame_other_ptr
- nullptr, // _capture_other_ptr
- nullptr, // _feature_other_ptr
- nullptr, // _landmark_other_ptr
- _processor_ptr,
- _apply_loss_function,
- _status,
- _ftr_current_ptr->getFrame()->getV(),
- _ftr_current_ptr->getFrame()->getO(),
- _bias_imu
- )
+namespace wolf
+{
+FactorPointFeetNomove::FactorPointFeetNomove(const FeatureBasePtr& _ftr_current_ptr,
+ const StateBlockPtr& _bias_imu,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status)
+ : FactorAutodiff("FactorPointFeetNomove",
+ TOP_GEOM,
+ _ftr_current_ptr,
+ nullptr, // _frame_other_ptr
+ nullptr, // _capture_other_ptr
+ nullptr, // _feature_other_ptr
+ nullptr, // _landmark_other_ptr
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_current_ptr->getFrame()->getV(),
+ _ftr_current_ptr->getFrame()->getO(),
+ _bias_imu)
{
}
-template<typename T>
-bool FactorPointFeetNomove::operator () (
- const T* const _vb,
- const T* const _qb,
- const T* const _bi,
- T* _res) const
+template <typename T>
+bool FactorPointFeetNomove::operator()(const T* const _vb, const T* const _qb, const T* const _bi, T* _res) const
{
- Map<Matrix<T,3,1> > res(_res);
+ Map<Matrix<T, 3, 1> > res(_res);
// State variables instanciation
- Map<const Matrix<T,3,1> > vb(_vb);
- Map<const Quaternion<T> > qb(_qb);
- Map<const Matrix<T,3,1> > bw(_pbm+3); // gyro bias
- Map<const Quaternion<T> > qbm(_qbm);
+ Map<const Matrix<T, 3, 1> > vb(_vb);
+ Map<const Quaternion<T> > qb(_qb);
+ Map<const Matrix<T, 3, 1> > bw(_pbm + 3); // gyro bias
+ Map<const Quaternion<T> > qbm(_qbm);
// Measurements retrieval
- Eigen::Matrix<T,6,1> meas = getMeasurement().cast<T>();
- Eigen::Matrix<T,3,1> b_v_bl = meas.head<3>();
- Eigen::Matrix<T,3,1> i_omg_i = meas.segment<3>(3);
- Eigen::Matrix<T,3,1> b_p_bl = meas.tail<3>();
+ Eigen::Matrix<T, 6, 1> meas = getMeasurement().cast<T>();
+ Eigen::Matrix<T, 3, 1> b_v_bl = meas.head<3>();
+ Eigen::Matrix<T, 3, 1> i_omg_i = meas.segment<3>(3);
+ Eigen::Matrix<T, 3, 1> b_p_bl = meas.tail<3>();
- Matrix<T,3,1> err = qb.conjugate()*vb + (i_omg_i - bw).cross(b_p_bl) + b_v_bl;
+ Matrix<T, 3, 1> err = qb.conjugate() * vb + (i_omg_i - bw).cross(b_p_bl) + b_v_bl;
res = getMeasurementSquareRootInformationUpper() * err;
return true;
}
-} // namespace wolf
+} // namespace wolf
#endif /* FACTOR__POINT_FEET_ZERO_VELOCITY_H_ */
diff --git a/include/bodydynamics/feature/feature_force_torque.h b/include/bodydynamics/feature/feature_force_torque.h
index 90bb950..b432843 100644
--- a/include/bodydynamics/feature/feature_force_torque.h
+++ b/include/bodydynamics/feature/feature_force_torque.h
@@ -21,71 +21,127 @@
#ifndef FEATURE_FORCE_TORQUE_H_
#define FEATURE_FORCE_TORQUE_H_
-//Wolf includes
+// Wolf includes
#include "core/feature/feature_base.h"
-//std includes
-namespace wolf {
-
+// std includes
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(FeatureForceTorque);
-//class FeatureApriltag
+// class FeatureApriltag
class FeatureForceTorque : public FeatureBase
{
- public:
-
- FeatureForceTorque(
- const double& _dt,
- const double& _mass,
- const Eigen::Vector6d& _forces_meas,
- const Eigen::Vector6d& _torques_meas,
- const Eigen::Vector3d& _pbc_meas,
- const Eigen::Matrix<double,14,1>& _kin_meas,
- const Eigen::Matrix6d& _cov_forces_meas,
- const Eigen::Matrix6d& _cov_torques_meas,
- const Eigen::Matrix3d& _cov_pbc_meas,
- const Eigen::Matrix<double,14,14>& _cov_kin_meas = Eigen::Matrix<double,14,14>::Zero()
- );
+ public:
+ FeatureForceTorque(const double& _dt,
+ const double& _mass,
+ const Eigen::Vector6d& _forces_meas,
+ const Eigen::Vector6d& _torques_meas,
+ const Eigen::Vector3d& _pbc_meas,
+ const Eigen::Matrix<double, 14, 1>& _kin_meas,
+ const Eigen::Matrix6d& _cov_forces_meas,
+ const Eigen::Matrix6d& _cov_torques_meas,
+ const Eigen::Matrix3d& _cov_pbc_meas,
+ const Eigen::Matrix<double, 14, 14>& _cov_kin_meas = Eigen::Matrix<double, 14, 14>::Zero());
- ~FeatureForceTorque() override;
+ ~FeatureForceTorque() override;
- const double & getDt() const {return dt_;}
- const double & getMass() const {return mass_;}
- void setDt(const double & _dt){dt_ = _dt;}
- void setMass(const double & _mass){mass_ = _mass;}
+ const double& getDt() const
+ {
+ return dt_;
+ }
+ const double& getMass() const
+ {
+ return mass_;
+ }
+ void setDt(const double& _dt)
+ {
+ dt_ = _dt;
+ }
+ void setMass(const double& _mass)
+ {
+ mass_ = _mass;
+ }
- const Eigen::Vector6d& getForcesMeas() const {return forces_meas_;}
- const Eigen::Vector6d& getTorquesMeas() const {return torques_meas_;}
- const Eigen::Vector3d& getPbcMeas() const {return pbc_meas_;}
- const Eigen::Matrix<double,14,1>& getKinMeas() const {return kin_meas_;}
- const Eigen::Matrix6d& getCovForcesMeas() const {return cov_forces_meas_;}
- const Eigen::Matrix6d& getCovTorquesMeas() const {return cov_torques_meas_;}
- const Eigen::Matrix3d& getCovPbcMeas() const {return cov_pbc_meas_;}
- const Eigen::Matrix<double,14,14>& getCovKinMeas() const {return cov_kin_meas_;}
+ const Eigen::Vector6d& getForcesMeas() const
+ {
+ return forces_meas_;
+ }
+ const Eigen::Vector6d& getTorquesMeas() const
+ {
+ return torques_meas_;
+ }
+ const Eigen::Vector3d& getPbcMeas() const
+ {
+ return pbc_meas_;
+ }
+ const Eigen::Matrix<double, 14, 1>& getKinMeas() const
+ {
+ return kin_meas_;
+ }
+ const Eigen::Matrix6d& getCovForcesMeas() const
+ {
+ return cov_forces_meas_;
+ }
+ const Eigen::Matrix6d& getCovTorquesMeas() const
+ {
+ return cov_torques_meas_;
+ }
+ const Eigen::Matrix3d& getCovPbcMeas() const
+ {
+ return cov_pbc_meas_;
+ }
+ const Eigen::Matrix<double, 14, 14>& getCovKinMeas() const
+ {
+ return cov_kin_meas_;
+ }
- void setForcesMeas(const Eigen::Vector6d& _forces_meas){forces_meas_ = _forces_meas;}
- void setTorquesMeas(const Eigen::Vector6d& _torques_meas){torques_meas_ = _torques_meas;}
- void setKinMeas(const Eigen::Matrix<double,14,1>& _kin_meas){kin_meas_ = _kin_meas;}
- void setPbcMeas(const Eigen::Vector3d& _pbc_meas){pbc_meas_ = _pbc_meas;}
- void setCovForcesMeas(const Eigen::Matrix6d& _cov_forces_meas){cov_forces_meas_ = _cov_forces_meas;}
- void setCovTorquesMeas(const Eigen::Matrix6d& _cov_torques_meas){cov_torques_meas_ = _cov_torques_meas;}
- void setCovPbcMeas(const Eigen::Matrix3d& _cov_pbc_meas){cov_pbc_meas_ = _cov_pbc_meas;}
- void setCovKinMeas(const Eigen::Matrix<double,14,14>& _cov_kin_meas){cov_kin_meas_ = _cov_kin_meas;}
-
- private:
- double dt_;
- double mass_;
- Eigen::Vector6d forces_meas_;
- Eigen::Vector6d torques_meas_;
- Eigen::Vector3d pbc_meas_;
- Eigen::Matrix<double,14,1> kin_meas_;
- Eigen::Matrix6d cov_forces_meas_;
- Eigen::Matrix6d cov_torques_meas_;
- Eigen::Matrix3d cov_pbc_meas_;
- Eigen::Matrix<double,14,14> cov_kin_meas_;
+ void setForcesMeas(const Eigen::Vector6d& _forces_meas)
+ {
+ forces_meas_ = _forces_meas;
+ }
+ void setTorquesMeas(const Eigen::Vector6d& _torques_meas)
+ {
+ torques_meas_ = _torques_meas;
+ }
+ void setKinMeas(const Eigen::Matrix<double, 14, 1>& _kin_meas)
+ {
+ kin_meas_ = _kin_meas;
+ }
+ void setPbcMeas(const Eigen::Vector3d& _pbc_meas)
+ {
+ pbc_meas_ = _pbc_meas;
+ }
+ void setCovForcesMeas(const Eigen::Matrix6d& _cov_forces_meas)
+ {
+ cov_forces_meas_ = _cov_forces_meas;
+ }
+ void setCovTorquesMeas(const Eigen::Matrix6d& _cov_torques_meas)
+ {
+ cov_torques_meas_ = _cov_torques_meas;
+ }
+ void setCovPbcMeas(const Eigen::Matrix3d& _cov_pbc_meas)
+ {
+ cov_pbc_meas_ = _cov_pbc_meas;
+ }
+ void setCovKinMeas(const Eigen::Matrix<double, 14, 14>& _cov_kin_meas)
+ {
+ cov_kin_meas_ = _cov_kin_meas;
+ }
+ private:
+ double dt_;
+ double mass_;
+ Eigen::Vector6d forces_meas_;
+ Eigen::Vector6d torques_meas_;
+ Eigen::Vector3d pbc_meas_;
+ Eigen::Matrix<double, 14, 1> kin_meas_;
+ Eigen::Matrix6d cov_forces_meas_;
+ Eigen::Matrix6d cov_torques_meas_;
+ Eigen::Matrix3d cov_pbc_meas_;
+ Eigen::Matrix<double, 14, 14> cov_kin_meas_;
};
-} // namespace wolf
+} // namespace wolf
#endif
diff --git a/include/bodydynamics/feature/feature_force_torque_preint.h b/include/bodydynamics/feature/feature_force_torque_preint.h
index 6d9115c..5ec4d1f 100644
--- a/include/bodydynamics/feature/feature_force_torque_preint.h
+++ b/include/bodydynamics/feature/feature_force_torque_preint.h
@@ -21,57 +21,55 @@
#ifndef FEATURE_FORCE_TORQUE_PREINT_H_
#define FEATURE_FORCE_TORQUE_PREINT_H_
-//Wolf includes
+// Wolf includes
#include <core/feature/feature_base.h>
#include <core/common/wolf.h>
-//std includes
+// std includes
-namespace wolf {
-
-//WOLF_PTR_TYPEDEFS(CaptureImu);
+namespace wolf
+{
+// WOLF_PTR_TYPEDEFS(CaptureImu);
WOLF_PTR_TYPEDEFS(FeatureForceTorquePreint);
class FeatureForceTorquePreint : public FeatureBase
{
- public:
-
- /** \brief Constructor from and measures
- *
- * \param _measurement the measurement
- * \param _meas_covariance the noise of the measurement
- * \param _J_delta_bias Jacobians of preintegrated delta wrt [pbc,gyro] biases
- * \param _pbc_bias COM position relative to bias bias of origin frame time
- * \param _gyro_bias gyroscope bias of origin frame time
- * \param _cap_ftpreint_ptr pointer to parent CaptureMotion (CaptureForceTorquePreint)
- */
- FeatureForceTorquePreint(const Eigen::VectorXd& _delta_preintegrated,
- const Eigen::MatrixXd& _delta_preintegrated_covariance,
- const Eigen::Vector6d& _biases_preint,
- const Eigen::Matrix<double,12,6>& _J_delta_biases);
-
-// /** \brief Constructor from capture pointer
-// *
-// * \param _cap_imu_ptr pointer to parent CaptureMotion
-// */
-// FeatureForceTorquePreint(CaptureMotionPtr _cap_imu_ptr);
-
- ~FeatureForceTorquePreint() override = default;
-
- const Eigen::Vector3d& getPbcBiasPreint() const;
- const Eigen::Vector3d& getGyroBiasPreint() const;
- const Eigen::Matrix<double, 12, 6>& getJacobianBias() const;
-
- private:
-
- // Used biases
- Eigen::Vector3d pbc_bias_preint_; ///< Acceleration bias used for delta preintegration
- Eigen::Vector3d gyro_bias_preint_; ///< Gyrometer bias used for delta preintegration
-
- Eigen::Matrix<double, 12, 6> J_delta_bias_;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+ public:
+ /** \brief Constructor from and measures
+ *
+ * \param _measurement the measurement
+ * \param _meas_covariance the noise of the measurement
+ * \param _J_delta_bias Jacobians of preintegrated delta wrt [pbc,gyro] biases
+ * \param _pbc_bias COM position relative to bias bias of origin frame time
+ * \param _gyro_bias gyroscope bias of origin frame time
+ * \param _cap_ftpreint_ptr pointer to parent CaptureMotion (CaptureForceTorquePreint)
+ */
+ FeatureForceTorquePreint(const Eigen::VectorXd& _delta_preintegrated,
+ const Eigen::MatrixXd& _delta_preintegrated_covariance,
+ const Eigen::Vector6d& _biases_preint,
+ const Eigen::Matrix<double, 12, 6>& _J_delta_biases);
+
+ // /** \brief Constructor from capture pointer
+ // *
+ // * \param _cap_imu_ptr pointer to parent CaptureMotion
+ // */
+ // FeatureForceTorquePreint(CaptureMotionPtr _cap_imu_ptr);
+
+ ~FeatureForceTorquePreint() override = default;
+
+ const Eigen::Vector3d& getPbcBiasPreint() const;
+ const Eigen::Vector3d& getGyroBiasPreint() const;
+ const Eigen::Matrix<double, 12, 6>& getJacobianBias() const;
+
+ private:
+ // Used biases
+ Eigen::Vector3d pbc_bias_preint_; ///< Acceleration bias used for delta preintegration
+ Eigen::Vector3d gyro_bias_preint_; ///< Gyrometer bias used for delta preintegration
+
+ Eigen::Matrix<double, 12, 6> J_delta_bias_;
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};
inline const Eigen::Vector3d& FeatureForceTorquePreint::getPbcBiasPreint() const
@@ -89,6 +87,6 @@ inline const Eigen::Matrix<double, 12, 6>& FeatureForceTorquePreint::getJacobian
return J_delta_bias_;
}
-} // namespace wolf
+} // namespace wolf
-#endif // FEATURE_FORCE_TORQUE_PREINT_H_
+#endif // FEATURE_FORCE_TORQUE_PREINT_H_
diff --git a/include/bodydynamics/feature/feature_inertial_kinematics.h b/include/bodydynamics/feature/feature_inertial_kinematics.h
index 2d33c37..9e0bebc 100644
--- a/include/bodydynamics/feature/feature_inertial_kinematics.h
+++ b/include/bodydynamics/feature/feature_inertial_kinematics.h
@@ -21,60 +21,68 @@
#ifndef FEATURE_INERTIAL_KINEMATICS_H_
#define FEATURE_INERTIAL_KINEMATICS_H_
-//Wolf includes
+// Wolf includes
#include "core/feature/feature_base.h"
-//std includes
-namespace wolf {
-
+// std includes
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(FeatureInertialKinematics);
-//class FeatureApriltag
+// class FeatureApriltag
class FeatureInertialKinematics : public FeatureBase
{
- public:
-
- FeatureInertialKinematics(const Eigen::Vector9d & _meas_pvw,
- const Eigen::Matrix9d & _Qerr,
- const Eigen::Matrix3d & _B_I_q, // Centroidal inertia matrix expressed in body frame
- const Eigen::Vector3d & _B_Lc_m, // Centroidal angular momentum expressed in body frame
- UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
- ~FeatureInertialKinematics() override = default;
-
-
- const Eigen::Matrix3d & getBIq() const {return BIq_;}
- const Eigen::Vector3d & getBLcm() const {return BLcm_;}
- void setBIq(const Eigen::Matrix3d & _BIq){BIq_ = _BIq;}
- void setBLcm(const Eigen::Vector3d & _BLcm){BLcm_ = _BLcm;}
-
-
- private:
- Eigen::Matrix3d BIq_;
- Eigen::Vector3d BLcm_;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
-
+ public:
+ FeatureInertialKinematics(const Eigen::Vector9d& _meas_pvw,
+ const Eigen::Matrix9d& _Qerr,
+ const Eigen::Matrix3d& _B_I_q, // Centroidal inertia matrix expressed in body frame
+ const Eigen::Vector3d& _B_Lc_m, // Centroidal angular momentum expressed in body frame
+ UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
+ ~FeatureInertialKinematics() override = default;
+
+ const Eigen::Matrix3d& getBIq() const
+ {
+ return BIq_;
+ }
+ const Eigen::Vector3d& getBLcm() const
+ {
+ return BLcm_;
+ }
+ void setBIq(const Eigen::Matrix3d& _BIq)
+ {
+ BIq_ = _BIq;
+ }
+ void setBLcm(const Eigen::Vector3d& _BLcm)
+ {
+ BLcm_ = _BLcm;
+ }
+
+ private:
+ Eigen::Matrix3d BIq_;
+ Eigen::Vector3d BLcm_;
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};
- Eigen::Matrix9d computeIKinJac(const Eigen::Vector3d& _w_unb,
- const Eigen::Vector3d& _p_unb,
- const Eigen::Matrix3d& _Iq);
-
- Eigen::Matrix9d computeIKinCov(const Eigen::Matrix3d& _Qp,
- const Eigen::Matrix3d& _Qv,
- const Eigen::Matrix3d& _Qw,
- const Eigen::Vector3d& _w_unb,
- const Eigen::Vector3d& _p_unb,
- const Eigen::Matrix3d& _Iq);
-
- void recomputeIKinCov(const Eigen::Matrix3d& Qp,
- const Eigen::Matrix3d& Qv,
- const Eigen::Matrix3d& Qw,
- const Eigen::Vector3d& p_unb,
- const Eigen::Vector3d& w_unb,
- const Eigen::Matrix3d& Iq);
-
-} // namespace wolf
+Eigen::Matrix9d computeIKinJac(const Eigen::Vector3d& _w_unb,
+ const Eigen::Vector3d& _p_unb,
+ const Eigen::Matrix3d& _Iq);
+
+Eigen::Matrix9d computeIKinCov(const Eigen::Matrix3d& _Qp,
+ const Eigen::Matrix3d& _Qv,
+ const Eigen::Matrix3d& _Qw,
+ const Eigen::Vector3d& _w_unb,
+ const Eigen::Vector3d& _p_unb,
+ const Eigen::Matrix3d& _Iq);
+
+void recomputeIKinCov(const Eigen::Matrix3d& Qp,
+ const Eigen::Matrix3d& Qv,
+ const Eigen::Matrix3d& Qw,
+ const Eigen::Vector3d& p_unb,
+ const Eigen::Vector3d& w_unb,
+ const Eigen::Matrix3d& Iq);
+
+} // namespace wolf
#endif
diff --git a/include/bodydynamics/math/force_torque_delta_tools.h b/include/bodydynamics/math/force_torque_delta_tools.h
index f9aec8e..f6cfb0d 100644
--- a/include/bodydynamics/math/force_torque_delta_tools.h
+++ b/include/bodydynamics/math/force_torque_delta_tools.h
@@ -60,48 +60,60 @@
* - body2delta: construct a delta from body magnitudes
*/
-namespace wolf
+namespace wolf
+{
+namespace bodydynamics
{
-namespace bodydynamics {
using namespace Eigen;
-template<typename D1, typename D2, typename D3, typename D4>
+template <typename D1, typename D2, typename D3, typename D4>
inline void identity(MatrixBase<D1>& dc, MatrixBase<D2>& dcd, MatrixBase<D3>& dLc, QuaternionBase<D4>& dq)
{
- dc = MatrixBase<D1>::Zero(3,1);
- dcd = MatrixBase<D3>::Zero(3,1);
- dLc = MatrixBase<D3>::Zero(3,1);
+ dc = MatrixBase<D1>::Zero(3, 1);
+ dcd = MatrixBase<D3>::Zero(3, 1);
+ dLc = MatrixBase<D3>::Zero(3, 1);
dq = QuaternionBase<D4>::Identity();
}
-template<typename D1, typename D2, typename D3, typename D4>
+template <typename D1, typename D2, typename D3, typename D4>
inline void identity(MatrixBase<D1>& dc, MatrixBase<D2>& dcd, MatrixBase<D3>& dLc, MatrixBase<D4>& dq)
{
typedef typename D1::Scalar T1;
typedef typename D2::Scalar T2;
typedef typename D3::Scalar T3;
typedef typename D4::Scalar T4;
- dc << T1(0), T1(0), T1(0);
+ dc << T1(0), T1(0), T1(0);
dcd << T2(0), T2(0), T2(0);
dLc << T3(0), T3(0), T3(0);
- dq << T4(0), T4(0), T4(0), T4(1);
+ dq << T4(0), T4(0), T4(0), T4(1);
}
-template<typename T = double>
+template <typename T = double>
inline Matrix<T, 13, 1> identity()
{
Matrix<T, 13, 1> ret;
- ret<< T(0), T(0), T(0),
- T(0), T(0), T(0),
- T(0), T(0), T(0),
- T(0), T(0), T(0), T(1);
+ ret << T(0), T(0), T(0), T(0), T(0), T(0), T(0), T(0), T(0), T(0), T(0), T(0), T(1);
return ret;
}
-template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, class T>
-inline void inverse(MatrixBase<D1>& dc, MatrixBase<D2>& dcd, MatrixBase<D3>& dLc, QuaternionBase<D4>& dq,
- const T dt,
- MatrixBase<D5>& idc, MatrixBase<D6>& idcd, MatrixBase<D7>& idLc, QuaternionBase<D8>& idq)
+template <typename D1,
+ typename D2,
+ typename D3,
+ typename D4,
+ typename D5,
+ typename D6,
+ typename D7,
+ typename D8,
+ class T>
+inline void inverse(MatrixBase<D1>& dc,
+ MatrixBase<D2>& dcd,
+ MatrixBase<D3>& dLc,
+ QuaternionBase<D4>& dq,
+ const T dt,
+ MatrixBase<D5>& idc,
+ MatrixBase<D6>& idcd,
+ MatrixBase<D7>& idLc,
+ QuaternionBase<D8>& idq)
{
MatrixSizeCheck<3, 1>::check(dc);
MatrixSizeCheck<3, 1>::check(dcd);
@@ -110,312 +122,352 @@ inline void inverse(MatrixBase<D1>& dc, MatrixBase<D2>& dcd, MatrixBase<D3>& dLc
MatrixSizeCheck<3, 1>::check(idcd);
MatrixSizeCheck<3, 1>::check(idLc);
- idc = - ( dq.conjugate() * (dc - dcd * typename D3::Scalar(dt)));
- idcd = - ( dq.conjugate() * dcd );
- idLc = - ( dq.conjugate() * dLc );
- idq = ( dq.conjugate());
+ idc = -(dq.conjugate() * (dc - dcd * typename D3::Scalar(dt)));
+ idcd = -(dq.conjugate() * dcd);
+ idLc = -(dq.conjugate() * dLc);
+ idq = (dq.conjugate());
}
-template<typename D1, typename D2, class T>
-inline void inverse(const MatrixBase<D1>& d,
- T dt,
- MatrixBase<D2>& id)
+template <typename D1, typename D2, class T>
+inline void inverse(const MatrixBase<D1>& d, T dt, MatrixBase<D2>& id)
{
MatrixSizeCheck<13, 1>::check(d);
MatrixSizeCheck<13, 1>::check(id);
- Map<const Matrix<typename D1::Scalar, 3, 1> > dc ( & d( 0 ) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dcd ( & d( 3 ) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dLc ( & d( 6 ) );
- Map<const Quaternion<typename D1::Scalar> > dq ( & d( 9 ) );
- Map<Matrix<typename D2::Scalar, 3, 1> > idc ( & id( 0 ) );
- Map<Matrix<typename D2::Scalar, 3, 1> > idcd ( & id( 3 ) );
- Map<Matrix<typename D2::Scalar, 3, 1> > idLc ( & id( 6 ) );
- Map<Quaternion<typename D1::Scalar> > idq ( & id( 9 ) );
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dc(&d(0));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dcd(&d(3));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dLc(&d(6));
+ Map<const Quaternion<typename D1::Scalar>> dq(&d(9));
+ Map<Matrix<typename D2::Scalar, 3, 1>> idc(&id(0));
+ Map<Matrix<typename D2::Scalar, 3, 1>> idcd(&id(3));
+ Map<Matrix<typename D2::Scalar, 3, 1>> idLc(&id(6));
+ Map<Quaternion<typename D1::Scalar>> idq(&id(9));
inverse(dc, dcd, dLc, dq, dt, idc, idcd, idLc, idq);
}
-template<typename D, class T>
-inline Matrix<typename D::Scalar, 13, 1> inverse(const MatrixBase<D>& d,
- T dt)
+template <typename D, class T>
+inline Matrix<typename D::Scalar, 13, 1> inverse(const MatrixBase<D>& d, T dt)
{
Matrix<typename D::Scalar, 13, 1> id;
inverse(d, dt, id);
return id;
}
-template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, typename D10, typename D11, typename D12, class T>
-inline void compose(const MatrixBase<D1>& dc1, const MatrixBase<D2>& dcd1, const MatrixBase<D3>& dLc1, const QuaternionBase<D4>& dq1,
- const MatrixBase<D5>& dc2, const MatrixBase<D6>& dcd2, const MatrixBase<D7>& dLc2, const QuaternionBase<D8>& dq2,
- const T dt,
- MatrixBase<D9>& sum_c, MatrixBase<D10>& sum_cd, MatrixBase<D11>& sum_Lc, QuaternionBase<D12>& sum_q)
-{
- MatrixSizeCheck<3, 1>::check(dc1);
- MatrixSizeCheck<3, 1>::check(dcd1);
- MatrixSizeCheck<3, 1>::check(dLc1);
- MatrixSizeCheck<3, 1>::check(dc2);
- MatrixSizeCheck<3, 1>::check(dcd2);
- MatrixSizeCheck<3, 1>::check(dLc2);
- MatrixSizeCheck<3, 1>::check(sum_c);
- MatrixSizeCheck<3, 1>::check(sum_cd);
- MatrixSizeCheck<3, 1>::check(sum_Lc);
-
- sum_c = dc1 + dcd1*dt + dq1*dc2;
- sum_cd = dcd1 + dq1*dcd2;
- sum_Lc = dLc1 + dq1*dLc2;
- sum_q = dq1*dq2;
-}
-
-template<typename D1, typename D2, typename D3, class T>
-inline void compose(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- T dt,
- MatrixBase<D3>& sum)
+template <typename D1,
+ typename D2,
+ typename D3,
+ typename D4,
+ typename D5,
+ typename D6,
+ typename D7,
+ typename D8,
+ typename D9,
+ typename D10,
+ typename D11,
+ typename D12,
+ class T>
+inline void compose(const MatrixBase<D1>& dc1,
+ const MatrixBase<D2>& dcd1,
+ const MatrixBase<D3>& dLc1,
+ const QuaternionBase<D4>& dq1,
+ const MatrixBase<D5>& dc2,
+ const MatrixBase<D6>& dcd2,
+ const MatrixBase<D7>& dLc2,
+ const QuaternionBase<D8>& dq2,
+ const T dt,
+ MatrixBase<D9>& sum_c,
+ MatrixBase<D10>& sum_cd,
+ MatrixBase<D11>& sum_Lc,
+ QuaternionBase<D12>& sum_q)
+{
+ MatrixSizeCheck<3, 1>::check(dc1);
+ MatrixSizeCheck<3, 1>::check(dcd1);
+ MatrixSizeCheck<3, 1>::check(dLc1);
+ MatrixSizeCheck<3, 1>::check(dc2);
+ MatrixSizeCheck<3, 1>::check(dcd2);
+ MatrixSizeCheck<3, 1>::check(dLc2);
+ MatrixSizeCheck<3, 1>::check(sum_c);
+ MatrixSizeCheck<3, 1>::check(sum_cd);
+ MatrixSizeCheck<3, 1>::check(sum_Lc);
+
+ sum_c = dc1 + dcd1 * dt + dq1 * dc2;
+ sum_cd = dcd1 + dq1 * dcd2;
+ sum_Lc = dLc1 + dq1 * dLc2;
+ sum_q = dq1 * dq2;
+}
+
+template <typename D1, typename D2, typename D3, class T>
+inline void compose(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, T dt, MatrixBase<D3>& sum)
{
MatrixSizeCheck<13, 1>::check(d1);
MatrixSizeCheck<13, 1>::check(d2);
MatrixSizeCheck<13, 1>::check(sum);
- Map<const Matrix<typename D1::Scalar, 3, 1> > dc1 ( & d1( 0 ) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dcd1 ( & d1( 3 ) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dLc1 ( & d1( 6 ) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1( 9 ) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dc2 ( & d2( 0 ) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dcd2 ( & d2( 3 ) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dLc2 ( & d2( 6 ) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2( 9 ) );
- Map<Matrix<typename D3::Scalar, 3, 1> > sum_c ( & sum( 0 ) );
- Map<Matrix<typename D3::Scalar, 3, 1> > sum_cd ( & sum( 3 ) );
- Map<Matrix<typename D3::Scalar, 3, 1> > sum_Lc ( & sum( 6 ) );
- Map<Quaternion<typename D3::Scalar> > sum_q ( & sum( 9 ) );
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dc1(&d1(0));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dcd1(&d1(3));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dLc1(&d1(6));
+ Map<const Quaternion<typename D1::Scalar>> dq1(&d1(9));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dc2(&d2(0));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dcd2(&d2(3));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dLc2(&d2(6));
+ Map<const Quaternion<typename D2::Scalar>> dq2(&d2(9));
+ Map<Matrix<typename D3::Scalar, 3, 1>> sum_c(&sum(0));
+ Map<Matrix<typename D3::Scalar, 3, 1>> sum_cd(&sum(3));
+ Map<Matrix<typename D3::Scalar, 3, 1>> sum_Lc(&sum(6));
+ Map<Quaternion<typename D3::Scalar>> sum_q(&sum(9));
compose(dc1, dcd1, dLc1, dq1, dc2, dcd2, dLc2, dq2, dt, sum_c, sum_cd, sum_Lc, sum_q);
}
-template<typename D1, typename D2, class T>
-inline Matrix<typename D1::Scalar, 13, 1> compose(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- T dt)
+template <typename D1, typename D2, class T>
+inline Matrix<typename D1::Scalar, 13, 1> compose(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, T dt)
{
- Matrix<typename D1::Scalar, 13, 1> ret;
+ Matrix<typename D1::Scalar, 13, 1> ret;
compose(d1, d2, dt, ret);
return ret;
}
-template<typename D1, typename D2, typename D3, typename D4, typename D5, class T>
+template <typename D1, typename D2, typename D3, typename D4, typename D5, class T>
inline void compose(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
- T dt,
- MatrixBase<D3>& sum,
- MatrixBase<D4>& J_sum_d1,
- MatrixBase<D5>& J_sum_d2)
+ T dt,
+ MatrixBase<D3>& sum,
+ MatrixBase<D4>& J_sum_d1,
+ MatrixBase<D5>& J_sum_d2)
{
MatrixSizeCheck<13, 1>::check(d1);
MatrixSizeCheck<13, 1>::check(d2);
MatrixSizeCheck<13, 1>::check(sum);
- MatrixSizeCheck< 12, 12>::check(J_sum_d1);
- MatrixSizeCheck< 12, 12>::check(J_sum_d2);
+ MatrixSizeCheck<12, 12>::check(J_sum_d1);
+ MatrixSizeCheck<12, 12>::check(J_sum_d2);
// Some useful temporaries
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1( 9 ) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dc2 ( & d2( 0 ) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dcd2 ( & d2( 3 ) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dLc2 ( & d2( 6 ) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2( 9 ) );
+ Map<const Quaternion<typename D1::Scalar>> dq1(&d1(9));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dc2(&d2(0));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dcd2(&d2(3));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dLc2(&d2(6));
+ Map<const Quaternion<typename D2::Scalar>> dq2(&d2(9));
Matrix<typename D1::Scalar, 3, 3> dR1 = q2R(dq1);
Matrix<typename D2::Scalar, 3, 3> dR2 = q2R(dq2);
// // Jac wrt first delta
- J_sum_d1.setIdentity();
- J_sum_d1.template block<3,3>(0,3) = Matrix3d::Identity() * dt;
- J_sum_d1.template block<3,3>(0,9) = - dR1 * skew(dc2);
- J_sum_d1.template block<3,3>(3,9) = - dR1 * skew(dcd2);
- J_sum_d1.template block<3,3>(6,9) = - dR1 * skew(dLc2);
- J_sum_d1.template block<3,3>(9,9) = dR2.transpose();
+ J_sum_d1.setIdentity();
+ J_sum_d1.template block<3, 3>(0, 3) = Matrix3d::Identity() * dt;
+ J_sum_d1.template block<3, 3>(0, 9) = -dR1 * skew(dc2);
+ J_sum_d1.template block<3, 3>(3, 9) = -dR1 * skew(dcd2);
+ J_sum_d1.template block<3, 3>(6, 9) = -dR1 * skew(dLc2);
+ J_sum_d1.template block<3, 3>(9, 9) = dR2.transpose();
// // // Jac wrt second delta
J_sum_d2.setIdentity();
- J_sum_d2.template block<3,3>(0,0) = dR1;
- J_sum_d2.template block<3,3>(3,3) = dR1;
- J_sum_d2.template block<3,3>(6,6) = dR1;
+ J_sum_d2.template block<3, 3>(0, 0) = dR1;
+ J_sum_d2.template block<3, 3>(3, 3) = dR1;
+ J_sum_d2.template block<3, 3>(6, 6) = dR1;
- // compose deltas -- done here to avoid aliasing when calling with input
+ // compose deltas -- done here to avoid aliasing when calling with input
// `d1` and result `sum` referencing the same variable
compose(d1, d2, dt, sum);
}
-template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, typename D10, typename D11, typename D12, class T>
-inline void between(const MatrixBase<D1>& dc1, const MatrixBase<D2>& dcd1, const MatrixBase<D3>& dLc1, const QuaternionBase<D4>& dq1,
- const MatrixBase<D5>& dc2, const MatrixBase<D6>& dcd2, const MatrixBase<D7>& dLc2, const QuaternionBase<D8>& dq2,
- const T dt,
- MatrixBase<D9>& diff_c, MatrixBase<D10>& diff_cd, MatrixBase<D11>& diff_Lc, QuaternionBase<D12>& diff_q )
-{
- MatrixSizeCheck<3, 1>::check(dc1);
- MatrixSizeCheck<3, 1>::check(dcd1);
- MatrixSizeCheck<3, 1>::check(dLc1);
- MatrixSizeCheck<3, 1>::check(dc2);
- MatrixSizeCheck<3, 1>::check(dcd2);
- MatrixSizeCheck<3, 1>::check(dLc2);
- MatrixSizeCheck<3, 1>::check(diff_c);
- MatrixSizeCheck<3, 1>::check(diff_cd);
- MatrixSizeCheck<3, 1>::check(diff_Lc);
-
- diff_c = dq1.conjugate() * (dc2 - dc1 - dcd1*dt);
- diff_cd = dq1.conjugate() * (dcd2 - dcd1);
- diff_Lc = dq1.conjugate() * (dLc2 - dLc1);
- diff_q = dq1.conjugate() * dq2;
-}
-
-template<typename D1, typename D2, typename D3, class T>
-inline void between(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- T dt,
- MatrixBase<D3>& d2_minus_d1)
+template <typename D1,
+ typename D2,
+ typename D3,
+ typename D4,
+ typename D5,
+ typename D6,
+ typename D7,
+ typename D8,
+ typename D9,
+ typename D10,
+ typename D11,
+ typename D12,
+ class T>
+inline void between(const MatrixBase<D1>& dc1,
+ const MatrixBase<D2>& dcd1,
+ const MatrixBase<D3>& dLc1,
+ const QuaternionBase<D4>& dq1,
+ const MatrixBase<D5>& dc2,
+ const MatrixBase<D6>& dcd2,
+ const MatrixBase<D7>& dLc2,
+ const QuaternionBase<D8>& dq2,
+ const T dt,
+ MatrixBase<D9>& diff_c,
+ MatrixBase<D10>& diff_cd,
+ MatrixBase<D11>& diff_Lc,
+ QuaternionBase<D12>& diff_q)
+{
+ MatrixSizeCheck<3, 1>::check(dc1);
+ MatrixSizeCheck<3, 1>::check(dcd1);
+ MatrixSizeCheck<3, 1>::check(dLc1);
+ MatrixSizeCheck<3, 1>::check(dc2);
+ MatrixSizeCheck<3, 1>::check(dcd2);
+ MatrixSizeCheck<3, 1>::check(dLc2);
+ MatrixSizeCheck<3, 1>::check(diff_c);
+ MatrixSizeCheck<3, 1>::check(diff_cd);
+ MatrixSizeCheck<3, 1>::check(diff_Lc);
+
+ diff_c = dq1.conjugate() * (dc2 - dc1 - dcd1 * dt);
+ diff_cd = dq1.conjugate() * (dcd2 - dcd1);
+ diff_Lc = dq1.conjugate() * (dLc2 - dLc1);
+ diff_q = dq1.conjugate() * dq2;
+}
+
+template <typename D1, typename D2, typename D3, class T>
+inline void between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, T dt, MatrixBase<D3>& d2_minus_d1)
{
MatrixSizeCheck<13, 1>::check(d1);
MatrixSizeCheck<13, 1>::check(d2);
MatrixSizeCheck<13, 1>::check(d2_minus_d1);
- Map<const Matrix<typename D1::Scalar, 3, 1> > dc1 ( & d1(0) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dcd1 ( & d1(3) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dLc1 ( & d1(6) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(9) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dc2 ( & d2(0) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dcd2 ( & d2(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dLc2 ( & d2(6) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(9) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_c ( & d2_minus_d1(0) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_cd ( & d2_minus_d1(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_Lc ( & d2_minus_d1(6) );
- Map<Quaternion<typename D3::Scalar> > diff_q ( & d2_minus_d1(9) );
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dc1(&d1(0));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dcd1(&d1(3));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dLc1(&d1(6));
+ Map<const Quaternion<typename D1::Scalar>> dq1(&d1(9));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dc2(&d2(0));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dcd2(&d2(3));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dLc2(&d2(6));
+ Map<const Quaternion<typename D2::Scalar>> dq2(&d2(9));
+ Map<Matrix<typename D3::Scalar, 3, 1>> diff_c(&d2_minus_d1(0));
+ Map<Matrix<typename D3::Scalar, 3, 1>> diff_cd(&d2_minus_d1(3));
+ Map<Matrix<typename D3::Scalar, 3, 1>> diff_Lc(&d2_minus_d1(6));
+ Map<Quaternion<typename D3::Scalar>> diff_q(&d2_minus_d1(9));
between(dc1, dcd1, dLc1, dq1, dc2, dcd2, dLc2, dq2, dt, diff_c, diff_cd, diff_Lc, diff_q);
}
-template<typename D1, typename D2, class T>
-inline Matrix<typename D1::Scalar, 13, 1> between(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- T dt)
+template <typename D1, typename D2, class T>
+inline Matrix<typename D1::Scalar, 13, 1> between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, T dt)
{
Matrix<typename D1::Scalar, 13, 1> diff;
between(d1, d2, dt, diff);
return diff;
}
-template<typename D1, typename D2, typename D3, class T>
-inline void composeOverState(const MatrixBase<D1>& x,
- const MatrixBase<D2>& d,
- T dt,
- MatrixBase<D3>& x_plus_d)
+template <typename D1, typename D2, typename D3, class T>
+inline void composeOverState(const MatrixBase<D1>& x, const MatrixBase<D2>& d, T dt, MatrixBase<D3>& x_plus_d)
{
MatrixSizeCheck<13, 1>::check(x);
MatrixSizeCheck<13, 1>::check(d);
MatrixSizeCheck<13, 1>::check(x_plus_d);
- Map<const Matrix<typename D1::Scalar, 3, 1> > c ( & x(0) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > cd ( & x(3) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > Lc ( & x(6) );
- Map<const Quaternion<typename D1::Scalar> > q ( & x(9) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dc ( & d(0) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dcd ( & d(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dLc ( & d(6) );
- Map<const Quaternion<typename D2::Scalar> > dq ( & d(9) );
- Map<Matrix<typename D3::Scalar, 3, 1> > c_plus_d ( & x_plus_d(0) );
- Map<Matrix<typename D3::Scalar, 3, 1> > cd_plus_d ( & x_plus_d(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > Lc_plus_d ( & x_plus_d(6) );
- Map<Quaternion<typename D3::Scalar> > q_plus_d ( & x_plus_d(9) );
-
- c_plus_d = c + cd*dt + 0.5*gravity()*dt*dt + q*dc;
- cd_plus_d = cd + gravity()*dt + q*dcd;
- Lc_plus_d = Lc + q*dLc;
- q_plus_d = q*dq; // dq here to avoid possible aliasing between x and x_plus_d --> WHAT?
-}
-
-template<typename D1, typename D2, class T>
-inline Matrix<typename D1::Scalar, 13, 1> composeOverState(const MatrixBase<D1>& x,
- const MatrixBase<D2>& d,
- T dt)
-{
- Matrix<typename D1::Scalar, 13, 1> ret;
+ Map<const Matrix<typename D1::Scalar, 3, 1>> c(&x(0));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> cd(&x(3));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> Lc(&x(6));
+ Map<const Quaternion<typename D1::Scalar>> q(&x(9));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dc(&d(0));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dcd(&d(3));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dLc(&d(6));
+ Map<const Quaternion<typename D2::Scalar>> dq(&d(9));
+ Map<Matrix<typename D3::Scalar, 3, 1>> c_plus_d(&x_plus_d(0));
+ Map<Matrix<typename D3::Scalar, 3, 1>> cd_plus_d(&x_plus_d(3));
+ Map<Matrix<typename D3::Scalar, 3, 1>> Lc_plus_d(&x_plus_d(6));
+ Map<Quaternion<typename D3::Scalar>> q_plus_d(&x_plus_d(9));
+
+ c_plus_d = c + cd * dt + 0.5 * gravity() * dt * dt + q * dc;
+ cd_plus_d = cd + gravity() * dt + q * dcd;
+ Lc_plus_d = Lc + q * dLc;
+ q_plus_d = q * dq; // dq here to avoid possible aliasing between x and x_plus_d --> WHAT?
+}
+
+template <typename D1, typename D2, class T>
+inline Matrix<typename D1::Scalar, 13, 1> composeOverState(const MatrixBase<D1>& x, const MatrixBase<D2>& d, T dt)
+{
+ Matrix<typename D1::Scalar, 13, 1> ret;
composeOverState(x, d, dt, ret);
return ret;
}
-template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, typename D10, typename D11, typename D12, class T>
-inline void betweenStates(const MatrixBase<D1>& c1, const MatrixBase<D2>& cd1, const MatrixBase<D3>& Lc1, const QuaternionBase<D4>& q1,
- const MatrixBase<D5>& c2, const MatrixBase<D6>& cd2, const MatrixBase<D7>& Lc2, const QuaternionBase<D8>& q2,
- const T dt,
- MatrixBase<D9>& dc, MatrixBase<D10>& dcd, MatrixBase<D11>& dLc, QuaternionBase<D12>& dq)
-{
- MatrixSizeCheck<3, 1>::check(c1);
- MatrixSizeCheck<3, 1>::check(cd1);
- MatrixSizeCheck<3, 1>::check(Lc1);
- MatrixSizeCheck<3, 1>::check(c2);
- MatrixSizeCheck<3, 1>::check(cd2);
- MatrixSizeCheck<3, 1>::check(Lc2);
- MatrixSizeCheck<3, 1>::check(dc);
- MatrixSizeCheck<3, 1>::check(dcd);
- MatrixSizeCheck<3, 1>::check(dLc);
+template <typename D1,
+ typename D2,
+ typename D3,
+ typename D4,
+ typename D5,
+ typename D6,
+ typename D7,
+ typename D8,
+ typename D9,
+ typename D10,
+ typename D11,
+ typename D12,
+ class T>
+inline void betweenStates(const MatrixBase<D1>& c1,
+ const MatrixBase<D2>& cd1,
+ const MatrixBase<D3>& Lc1,
+ const QuaternionBase<D4>& q1,
+ const MatrixBase<D5>& c2,
+ const MatrixBase<D6>& cd2,
+ const MatrixBase<D7>& Lc2,
+ const QuaternionBase<D8>& q2,
+ const T dt,
+ MatrixBase<D9>& dc,
+ MatrixBase<D10>& dcd,
+ MatrixBase<D11>& dLc,
+ QuaternionBase<D12>& dq)
+{
+ MatrixSizeCheck<3, 1>::check(c1);
+ MatrixSizeCheck<3, 1>::check(cd1);
+ MatrixSizeCheck<3, 1>::check(Lc1);
+ MatrixSizeCheck<3, 1>::check(c2);
+ MatrixSizeCheck<3, 1>::check(cd2);
+ MatrixSizeCheck<3, 1>::check(Lc2);
+ MatrixSizeCheck<3, 1>::check(dc);
+ MatrixSizeCheck<3, 1>::check(dcd);
+ MatrixSizeCheck<3, 1>::check(dLc);
- dc = q1.conjugate() * ( c2 - c1 - cd1*dt - 0.5*gravity() *dt * dt );
- dcd = q1.conjugate() * ( cd2 - cd1 - gravity()*dt );
- dLc = q1.conjugate() * ( Lc2 - Lc1);
- dq = q1.conjugate() * q2;
+ dc = q1.conjugate() * (c2 - c1 - cd1 * dt - 0.5 * gravity() * dt * dt);
+ dcd = q1.conjugate() * (cd2 - cd1 - gravity() * dt);
+ dLc = q1.conjugate() * (Lc2 - Lc1);
+ dq = q1.conjugate() * q2;
}
-template<typename D1, typename D2, typename D3, class T>
-inline void betweenStates(const MatrixBase<D1>& x1,
- const MatrixBase<D2>& x2,
- T dt,
- MatrixBase<D3>& x2_minus_x1)
+template <typename D1, typename D2, typename D3, class T>
+inline void betweenStates(const MatrixBase<D1>& x1, const MatrixBase<D2>& x2, T dt, MatrixBase<D3>& x2_minus_x1)
{
MatrixSizeCheck<13, 1>::check(x1);
MatrixSizeCheck<13, 1>::check(x2);
MatrixSizeCheck<13, 1>::check(x2_minus_x1);
- Map<const Matrix<typename D1::Scalar, 3, 1> > c1 ( & x1(0) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > cd1 ( & x1(3) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > Lc1 ( & x1(6) );
- Map<const Quaternion<typename D1::Scalar> > q1 ( & x1(9) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > c2 ( & x2(0) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > cd2 ( & x2(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > Lc2 ( & x2(6) );
- Map<const Quaternion<typename D2::Scalar> > q2 ( & x2(9) );
- Map<Matrix<typename D3::Scalar, 3, 1> > dc ( & x2_minus_x1(0) );
- Map<Matrix<typename D3::Scalar, 3, 1> > dcd ( & x2_minus_x1(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > dLc ( & x2_minus_x1(6) );
- Map<Quaternion<typename D3::Scalar> > dq ( & x2_minus_x1(9) );
+ Map<const Matrix<typename D1::Scalar, 3, 1>> c1(&x1(0));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> cd1(&x1(3));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> Lc1(&x1(6));
+ Map<const Quaternion<typename D1::Scalar>> q1(&x1(9));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> c2(&x2(0));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> cd2(&x2(3));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> Lc2(&x2(6));
+ Map<const Quaternion<typename D2::Scalar>> q2(&x2(9));
+ Map<Matrix<typename D3::Scalar, 3, 1>> dc(&x2_minus_x1(0));
+ Map<Matrix<typename D3::Scalar, 3, 1>> dcd(&x2_minus_x1(3));
+ Map<Matrix<typename D3::Scalar, 3, 1>> dLc(&x2_minus_x1(6));
+ Map<Quaternion<typename D3::Scalar>> dq(&x2_minus_x1(9));
betweenStates(c1, cd1, Lc1, q1, c2, cd2, Lc2, q2, dt, dc, dcd, dLc, dq);
}
-template<typename D1, typename D2, class T>
-inline Matrix<typename D1::Scalar, 13, 1> betweenStates(const MatrixBase<D1>& x1,
- const MatrixBase<D2>& x2,
- T dt)
+template <typename D1, typename D2, class T>
+inline Matrix<typename D1::Scalar, 13, 1> betweenStates(const MatrixBase<D1>& x1, const MatrixBase<D2>& x2, T dt)
{
Matrix<typename D1::Scalar, 13, 1> ret;
betweenStates(x1, x2, dt, ret);
return ret;
}
-template<typename Derived>
+template <typename Derived>
Matrix<typename Derived::Scalar, 12, 1> log_FT(const MatrixBase<Derived>& delta_in)
{
MatrixSizeCheck<13, 1>::check(delta_in);
Matrix<typename Derived::Scalar, 12, 1> ret;
- Map<const Matrix<typename Derived::Scalar, 3, 1> > dc_in ( & delta_in(0) );
- Map<const Matrix<typename Derived::Scalar, 3, 1> > dcd_in ( & delta_in(3) );
- Map<const Matrix<typename Derived::Scalar, 3, 1> > dLc_in ( & delta_in(6) );
- Map<const Quaternion<typename Derived::Scalar> > dq_in ( & delta_in(9) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > dc_ret ( & ret(0) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > dcd_ret ( & ret(3) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > dLc_ret ( & ret(6) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > do_ret ( & ret(9) );
+ Map<const Matrix<typename Derived::Scalar, 3, 1>> dc_in(&delta_in(0));
+ Map<const Matrix<typename Derived::Scalar, 3, 1>> dcd_in(&delta_in(3));
+ Map<const Matrix<typename Derived::Scalar, 3, 1>> dLc_in(&delta_in(6));
+ Map<const Quaternion<typename Derived::Scalar>> dq_in(&delta_in(9));
+ Map<Matrix<typename Derived::Scalar, 3, 1>> dc_ret(&ret(0));
+ Map<Matrix<typename Derived::Scalar, 3, 1>> dcd_ret(&ret(3));
+ Map<Matrix<typename Derived::Scalar, 3, 1>> dLc_ret(&ret(6));
+ Map<Matrix<typename Derived::Scalar, 3, 1>> do_ret(&ret(9));
dc_ret = dc_in;
dcd_ret = dcd_in;
@@ -425,21 +477,21 @@ Matrix<typename Derived::Scalar, 12, 1> log_FT(const MatrixBase<Derived>& delta_
return ret;
}
-template<typename Derived>
+template <typename Derived>
Matrix<typename Derived::Scalar, 13, 1> exp_FT(const MatrixBase<Derived>& d_alg_in)
{
MatrixSizeCheck<12, 1>::check(d_alg_in);
Matrix<typename Derived::Scalar, 13, 1> ret;
- Map<const Matrix<typename Derived::Scalar, 3, 1> > dc_alg_in ( & d_alg_in(0) );
- Map<const Matrix<typename Derived::Scalar, 3, 1> > dcd_alg_in ( & d_alg_in(3) );
- Map<const Matrix<typename Derived::Scalar, 3, 1> > dLc_alg_in ( & d_alg_in(6) );
- Map<const Matrix<typename Derived::Scalar, 3, 1> > do_alg_in ( & d_alg_in(9) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > dc_ret ( & ret(0) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > dcd_ret ( & ret(3) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > dLc_ret ( & ret(6) );
- Map<Quaternion<typename Derived::Scalar> > dq_ret ( & ret(9) );
+ Map<const Matrix<typename Derived::Scalar, 3, 1>> dc_alg_in(&d_alg_in(0));
+ Map<const Matrix<typename Derived::Scalar, 3, 1>> dcd_alg_in(&d_alg_in(3));
+ Map<const Matrix<typename Derived::Scalar, 3, 1>> dLc_alg_in(&d_alg_in(6));
+ Map<const Matrix<typename Derived::Scalar, 3, 1>> do_alg_in(&d_alg_in(9));
+ Map<Matrix<typename Derived::Scalar, 3, 1>> dc_ret(&ret(0));
+ Map<Matrix<typename Derived::Scalar, 3, 1>> dcd_ret(&ret(3));
+ Map<Matrix<typename Derived::Scalar, 3, 1>> dLc_ret(&ret(6));
+ Map<Quaternion<typename Derived::Scalar>> dq_ret(&ret(9));
dc_ret = dc_alg_in;
dcd_ret = dcd_alg_in;
@@ -449,114 +501,169 @@ Matrix<typename Derived::Scalar, 13, 1> exp_FT(const MatrixBase<Derived>& d_alg_
return ret;
}
-template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, typename D10, typename D11, typename D12>
-inline void plus(const MatrixBase<D1>& dc1, const MatrixBase<D2>& dcd1, const MatrixBase<D3>& dLc1, const QuaternionBase<D4>& dq1,
- const MatrixBase<D5>& diff_c2, const MatrixBase<D6>& diff_cd2, const MatrixBase<D7>& diff_Lc2, const MatrixBase<D8>& diff_o2,
- MatrixBase<D9>& plus_c, MatrixBase<D10>& plus_cd, MatrixBase<D11>& plus_Lc, QuaternionBase<D12>& plus_q)
-{
- plus_c = dc1 + diff_c2;
- plus_cd = dcd1 + diff_cd2;
- plus_Lc = dLc1 + diff_Lc2;
- plus_q = dq1 * exp_q(diff_o2);
+template <typename D1,
+ typename D2,
+ typename D3,
+ typename D4,
+ typename D5,
+ typename D6,
+ typename D7,
+ typename D8,
+ typename D9,
+ typename D10,
+ typename D11,
+ typename D12>
+inline void plus(const MatrixBase<D1>& dc1,
+ const MatrixBase<D2>& dcd1,
+ const MatrixBase<D3>& dLc1,
+ const QuaternionBase<D4>& dq1,
+ const MatrixBase<D5>& diff_c2,
+ const MatrixBase<D6>& diff_cd2,
+ const MatrixBase<D7>& diff_Lc2,
+ const MatrixBase<D8>& diff_o2,
+ MatrixBase<D9>& plus_c,
+ MatrixBase<D10>& plus_cd,
+ MatrixBase<D11>& plus_Lc,
+ QuaternionBase<D12>& plus_q)
+{
+ plus_c = dc1 + diff_c2;
+ plus_cd = dcd1 + diff_cd2;
+ plus_Lc = dLc1 + diff_Lc2;
+ plus_q = dq1 * exp_q(diff_o2);
}
-
// Composite plus
-template<typename D1, typename D2, typename D3>
-inline void plus(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& diff2,
- MatrixBase<D3>& d)
-{
- Map<const Matrix<typename D1::Scalar, 3, 1> > dc1 ( & d1(0) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dcd1 ( & d1(3) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dLc1 ( & d1(6) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(9) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > diff_dc2 ( & diff2(0) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > diff_dcd2 ( & diff2(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > diff_dLc2 ( & diff2(6) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > diff_do2 ( & diff2(9) );
- Map<Matrix<typename D3::Scalar, 3, 1> > d_c ( & d(0) );
- Map<Matrix<typename D3::Scalar, 3, 1> > d_cd ( & d(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > d_Lc ( & d(6) );
- Map<Quaternion<typename D3::Scalar> > d_q ( & d(9) );
-
- plus(dc1, dcd1, dLc1, dq1,
- diff_dc2, diff_dcd2, diff_dLc2, diff_do2,
- d_c, d_cd, d_Lc, d_q);
-}
-
-template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 13, 1> plus(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2)
+template <typename D1, typename D2, typename D3>
+inline void plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& diff2, MatrixBase<D3>& d)
{
- Matrix<typename D1::Scalar, 13, 1> ret;
- plus(d1, d2, ret);
- return ret;
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dc1(&d1(0));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dcd1(&d1(3));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dLc1(&d1(6));
+ Map<const Quaternion<typename D1::Scalar>> dq1(&d1(9));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> diff_dc2(&diff2(0));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> diff_dcd2(&diff2(3));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> diff_dLc2(&diff2(6));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> diff_do2(&diff2(9));
+ Map<Matrix<typename D3::Scalar, 3, 1>> d_c(&d(0));
+ Map<Matrix<typename D3::Scalar, 3, 1>> d_cd(&d(3));
+ Map<Matrix<typename D3::Scalar, 3, 1>> d_Lc(&d(6));
+ Map<Quaternion<typename D3::Scalar>> d_q(&d(9));
+
+ plus(dc1, dcd1, dLc1, dq1, diff_dc2, diff_dcd2, diff_dLc2, diff_do2, d_c, d_cd, d_Lc, d_q);
}
-template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, typename D10, typename D11, typename D12>
-inline void diff(const MatrixBase<D1>& dc1, const MatrixBase<D2>& dcd1, const MatrixBase<D3>& dLc1, const QuaternionBase<D4>& dq1,
- const MatrixBase<D5>& dc2, const MatrixBase<D6>& dcd2, const MatrixBase<D7>& dLc2, const QuaternionBase<D8>& dq2,
- MatrixBase<D9>& diff_c, MatrixBase<D10>& diff_cd, MatrixBase<D11>& diff_Lc, MatrixBase<D12>& diff_o)
+template <typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 13, 1> plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2)
{
- diff_c = dc2 - dc1;
- diff_cd = dcd2 - dcd1;
- diff_Lc = dLc2 - dLc1;
- diff_o = log_q(dq1.conjugate() * dq2);
+ Matrix<typename D1::Scalar, 13, 1> ret;
+ plus(d1, d2, ret);
+ return ret;
}
-template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, typename D10, typename D11, typename D12, typename D13, typename D14>
-inline void diff(const MatrixBase<D1>& dc1, const MatrixBase<D2>& dcd1, const MatrixBase<D3>& dLc1, const QuaternionBase<D4>& dq1,
- const MatrixBase<D5>& dc2, const MatrixBase<D6>& dcd2, const MatrixBase<D7>& dLc2, const QuaternionBase<D8>& dq2,
- MatrixBase<D9>& diff_c, MatrixBase<D10>& diff_cd, MatrixBase<D11>& diff_Lc, MatrixBase<D12>& diff_o,
- MatrixBase<D13>& J_do_dq1, MatrixBase<D14>& J_do_dq2)
+template <typename D1,
+ typename D2,
+ typename D3,
+ typename D4,
+ typename D5,
+ typename D6,
+ typename D7,
+ typename D8,
+ typename D9,
+ typename D10,
+ typename D11,
+ typename D12>
+inline void diff(const MatrixBase<D1>& dc1,
+ const MatrixBase<D2>& dcd1,
+ const MatrixBase<D3>& dLc1,
+ const QuaternionBase<D4>& dq1,
+ const MatrixBase<D5>& dc2,
+ const MatrixBase<D6>& dcd2,
+ const MatrixBase<D7>& dLc2,
+ const QuaternionBase<D8>& dq2,
+ MatrixBase<D9>& diff_c,
+ MatrixBase<D10>& diff_cd,
+ MatrixBase<D11>& diff_Lc,
+ MatrixBase<D12>& diff_o)
+{
+ diff_c = dc2 - dc1;
+ diff_cd = dcd2 - dcd1;
+ diff_Lc = dLc2 - dLc1;
+ diff_o = log_q(dq1.conjugate() * dq2);
+}
+
+template <typename D1,
+ typename D2,
+ typename D3,
+ typename D4,
+ typename D5,
+ typename D6,
+ typename D7,
+ typename D8,
+ typename D9,
+ typename D10,
+ typename D11,
+ typename D12,
+ typename D13,
+ typename D14>
+inline void diff(const MatrixBase<D1>& dc1,
+ const MatrixBase<D2>& dcd1,
+ const MatrixBase<D3>& dLc1,
+ const QuaternionBase<D4>& dq1,
+ const MatrixBase<D5>& dc2,
+ const MatrixBase<D6>& dcd2,
+ const MatrixBase<D7>& dLc2,
+ const QuaternionBase<D8>& dq2,
+ MatrixBase<D9>& diff_c,
+ MatrixBase<D10>& diff_cd,
+ MatrixBase<D11>& diff_Lc,
+ MatrixBase<D12>& diff_o,
+ MatrixBase<D13>& J_do_dq1,
+ MatrixBase<D14>& J_do_dq2)
{
diff(dc1, dcd1, dLc1, dq1, dc2, dcd2, dLc2, dq2, diff_c, diff_cd, diff_Lc, diff_o);
- J_do_dq1 = - jac_SO3_left_inv(diff_o);
- J_do_dq2 = jac_SO3_right_inv(diff_o);
+ J_do_dq1 = -jac_SO3_left_inv(diff_o);
+ J_do_dq2 = jac_SO3_right_inv(diff_o);
}
-template<typename D1, typename D2, typename D3>
-inline void diff(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& diff_d1_d2)
-{
- Map<const Matrix<typename D1::Scalar, 3, 1> > dc1 ( & d1(0) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dcd1 ( & d1(3) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dLc1 ( & d1(6) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(9) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dc2 ( & d2(0) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dcd2 ( & d2(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dLc2 ( & d2(6) );
- Map<const Quaternion<typename D1::Scalar> > dq2 ( & d2(9) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_c ( & diff_d1_d2(0) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_cd ( & diff_d1_d2(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_Lc ( & diff_d1_d2(6) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & diff_d1_d2(9) );
+template <typename D1, typename D2, typename D3>
+inline void diff(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& diff_d1_d2)
+{
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dc1(&d1(0));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dcd1(&d1(3));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dLc1(&d1(6));
+ Map<const Quaternion<typename D1::Scalar>> dq1(&d1(9));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dc2(&d2(0));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dcd2(&d2(3));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dLc2(&d2(6));
+ Map<const Quaternion<typename D1::Scalar>> dq2(&d2(9));
+ Map<Matrix<typename D3::Scalar, 3, 1>> diff_c(&diff_d1_d2(0));
+ Map<Matrix<typename D3::Scalar, 3, 1>> diff_cd(&diff_d1_d2(3));
+ Map<Matrix<typename D3::Scalar, 3, 1>> diff_Lc(&diff_d1_d2(6));
+ Map<Matrix<typename D3::Scalar, 3, 1>> diff_o(&diff_d1_d2(9));
diff(dc1, dcd1, dLc1, dq1, dc2, dcd2, dLc2, dq2, diff_c, diff_cd, diff_Lc, diff_o);
}
-template<typename D1, typename D2, typename D3, typename D4, typename D5>
+template <typename D1, typename D2, typename D3, typename D4, typename D5>
inline void diff(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
- MatrixBase<D3>& diff_d1_d2,
- MatrixBase<D4>& J_diff_d1,
- MatrixBase<D5>& J_diff_d2)
-{
- Map<const Matrix<typename D1::Scalar, 3, 1> > dc1 ( & d1(0) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dcd1 ( & d1(3) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dLc1 ( & d1(6) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(9) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dc2 ( & d2(0) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dcd2 ( & d2(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dLc2 ( & d2(6) );
- Map<const Quaternion<typename D1::Scalar> > dq2 ( & d2(9) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_c ( & diff_d1_d2(0) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_cd ( & diff_d1_d2(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_Lc ( & diff_d1_d2(6) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & diff_d1_d2(9) );
+ MatrixBase<D3>& diff_d1_d2,
+ MatrixBase<D4>& J_diff_d1,
+ MatrixBase<D5>& J_diff_d2)
+{
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dc1(&d1(0));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dcd1(&d1(3));
+ Map<const Matrix<typename D1::Scalar, 3, 1>> dLc1(&d1(6));
+ Map<const Quaternion<typename D1::Scalar>> dq1(&d1(9));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dc2(&d2(0));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dcd2(&d2(3));
+ Map<const Matrix<typename D2::Scalar, 3, 1>> dLc2(&d2(6));
+ Map<const Quaternion<typename D1::Scalar>> dq2(&d2(9));
+ Map<Matrix<typename D3::Scalar, 3, 1>> diff_c(&diff_d1_d2(0));
+ Map<Matrix<typename D3::Scalar, 3, 1>> diff_cd(&diff_d1_d2(3));
+ Map<Matrix<typename D3::Scalar, 3, 1>> diff_Lc(&diff_d1_d2(6));
+ Map<Matrix<typename D3::Scalar, 3, 1>> diff_o(&diff_d1_d2(9));
Matrix<typename D4::Scalar, 3, 3> J_do_dq1, J_do_dq2;
@@ -573,111 +680,114 @@ inline void diff(const MatrixBase<D1>& d1,
* J_do_dq2 = J_r_inv(theta)
*/
- J_diff_d1 = - Matrix<typename D4::Scalar, 12, 12>::Identity(); // d(c2 - c1) / d(c1) = - Identity, for instance
- J_diff_d1.template block<3,3>(9,9) = J_do_dq1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta)
- J_diff_d2.setIdentity(); // d(c2 - c1) / d(c2) = Identity, for instance
- J_diff_d2.template block<3,3>(9,9) = J_do_dq2; // d(R1.tr*R2) / d(R2) = J_r_inv(theta)
+ J_diff_d1 = -Matrix<typename D4::Scalar, 12, 12>::Identity(); // d(c2 - c1) / d(c1) = - Identity, for instance
+ J_diff_d1.template block<3, 3>(9, 9) = J_do_dq1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta)
+ J_diff_d2.setIdentity(); // d(c2 - c1) / d(c2) = Identity, for instance
+ J_diff_d2.template block<3, 3>(9, 9) = J_do_dq2; // d(R1.tr*R2) / d(R2) = J_r_inv(theta)
}
-template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 12, 1> diff(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2)
+template <typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 12, 1> diff(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2)
{
Matrix<typename D1::Scalar, 12, 1> ret;
diff(d1, d2, ret);
return ret;
}
-template<typename D1, typename D2>
-inline void body2delta(const MatrixBase<D1>& body,
- const typename D1::Scalar& dt,
- const typename D1::Scalar& mass,
- int nbc,
- int dimc,
- MatrixBase<D2>& delta)
+template <typename D1, typename D2>
+inline void body2delta(const MatrixBase<D1>& body,
+ const typename D1::Scalar& dt,
+ const typename D1::Scalar& mass,
+ int nbc,
+ int dimc,
+ MatrixBase<D2>& delta)
{
typedef typename D1::Scalar T;
delta.setZero();
- Map< Matrix<T, 3, 1>> dc ( & delta(0) );
- Map< Matrix<T, 3, 1>> dcd( & delta(3) );
- Map< Matrix<T, 3, 1>> dLc( & delta(6) );
- Map< Quaternion<T>> dq ( & delta(9) );
+ Map<Matrix<T, 3, 1>> dc(&delta(0));
+ Map<Matrix<T, 3, 1>> dcd(&delta(3));
+ Map<Matrix<T, 3, 1>> dLc(&delta(6));
+ Map<Quaternion<T>> dq(&delta(9));
- Vector3d pbc = body.segment(nbc*dimc, 3);
- Vector3d wb = body.segment(nbc*dimc + 3, 3);
+ Vector3d pbc = body.segment(nbc * dimc, 3);
+ Vector3d wb = body.segment(nbc * dimc + 3, 3);
// general computation for any 3D or 6D contacts
- for (int i=0; i<nbc; i++){
- Vector3d pbli = body.segment(nbc*dimc + 6 + i*3, 3);
- Vector4d qbli_v = body.segment(nbc*dimc + 6 + nbc*3 + i*4, 4);
- Quaterniond qbli(qbli_v);
- Vector3d fi = body.segment(i*3, 3);
-
- dc += qbli*fi;
- dcd += qbli*fi;
+ for (int i = 0; i < nbc; i++)
+ {
+ Vector3d pbli = body.segment(nbc * dimc + 6 + i * 3, 3);
+ Vector4d qbli_v = body.segment(nbc * dimc + 6 + nbc * 3 + i * 4, 4);
+ Quaterniond qbli(qbli_v);
+ Vector3d fi = body.segment(i * 3, 3);
+
+ dc += qbli * fi;
+ dcd += qbli * fi;
dLc += (pbli - pbc).cross(qbli * fi);
- if (dimc == 6){
- Vector3d taui = body.segment(3*nbc + i*3, 3);
- dLc += qbli*taui;
+ if (dimc == 6)
+ {
+ Vector3d taui = body.segment(3 * nbc + i * 3, 3);
+ dLc += qbli * taui;
}
}
// formulas for 2 6D contacts:
// dc = 0.5 * (qbl1*f1 + qbl2*f2) * dt * dt / mass;
// dcd = (qbl1*f1 + qbl2*f2) * dt / mass;
- // dLc = (qbl1*tau1 + (pbl1-pbc).cross(qbl1*f1)
+ // dLc = (qbl1*tau1 + (pbl1-pbc).cross(qbl1*f1)
// + qbl2*tau2 + (pbl2-pbc).cross(qbl2*f2)) * dt;
- dc *= (0.5*dt*dt/mass);
- dcd *= (dt/mass);
+ dc *= (0.5 * dt * dt / mass);
+ dcd *= (dt / mass);
dLc *= dt;
dq = exp_q(wb * dt);
}
-template<typename D1>
-inline Matrix<typename D1::Scalar, 13, 1> body2delta(const MatrixBase<D1>& body,
+template <typename D1>
+inline Matrix<typename D1::Scalar, 13, 1> body2delta(const MatrixBase<D1>& body,
const typename D1::Scalar& dt,
const typename D1::Scalar& mass,
- int nbc,
- int dimc)
+ int nbc,
+ int dimc)
{
typedef typename D1::Scalar T;
- Matrix<T, 13, 1> delta;
+ Matrix<T, 13, 1> delta;
body2delta(body, dt, mass, nbc, dimc, delta);
return delta;
}
-template<typename D1, typename D2, typename D3>
-inline void body2delta(const MatrixBase<D1>& body,
+template <typename D1, typename D2, typename D3>
+inline void body2delta(const MatrixBase<D1>& body,
const typename D1::Scalar& dt,
const typename D1::Scalar& mass,
- int nbc,
- int dimc,
- MatrixBase<D2>& delta,
- MatrixBase<D3>& jac_delta_body)
+ int nbc,
+ int dimc,
+ MatrixBase<D2>& delta,
+ MatrixBase<D3>& jac_delta_body)
{
- Vector3d pbc = body.segment(nbc*dimc, 3);
- Vector3d wb = body.segment(nbc*dimc + 3, 3);
+ Vector3d pbc = body.segment(nbc * dimc, 3);
+ Vector3d wb = body.segment(nbc * dimc + 3, 3);
jac_delta_body.setZero();
- for (int i=0; i<nbc; i++){
- Vector3d pbli = body.segment(nbc*dimc + 6 + i*3, 3);
- Vector4d qbli_v = body.segment(nbc*dimc + 6 + nbc*3 + i*4, 4);
+ for (int i = 0; i < nbc; i++)
+ {
+ Vector3d pbli = body.segment(nbc * dimc + 6 + i * 3, 3);
+ Vector4d qbli_v = body.segment(nbc * dimc + 6 + nbc * 3 + i * 4, 4);
Quaterniond qbli(qbli_v);
- Matrix3d bRli = q2R(qbli);
- Vector3d fi = body.segment(i*3, 3);
-
- jac_delta_body.template block<3,3>(0,i*3) = 0.5 * bRli * dt * dt / mass;
- jac_delta_body.template block<3,3>(3,i*3) = bRli * dt / mass;
- jac_delta_body.template block<3,3>(6,i*3) = skew(pbli - pbc) * bRli * dt;
- if (dimc == 6){
- jac_delta_body.template block<3,3>(6,nbc*3 + i*3) = bRli * dt;
+ Matrix3d bRli = q2R(qbli);
+ Vector3d fi = body.segment(i * 3, 3);
+
+ jac_delta_body.template block<3, 3>(0, i * 3) = 0.5 * bRli * dt * dt / mass;
+ jac_delta_body.template block<3, 3>(3, i * 3) = bRli * dt / mass;
+ jac_delta_body.template block<3, 3>(6, i * 3) = skew(pbli - pbc) * bRli * dt;
+ if (dimc == 6)
+ {
+ jac_delta_body.template block<3, 3>(6, nbc * 3 + i * 3) = bRli * dt;
}
- jac_delta_body.template block<3,3>(6,nbc*dimc) += skew(bRli*fi) * dt;
+ jac_delta_body.template block<3, 3>(6, nbc * dimc) += skew(bRli * fi) * dt;
}
- jac_delta_body.template block<3,3>(9,nbc*dimc+3) = dt * jac_SO3_right(wb * dt);
+ jac_delta_body.template block<3, 3>(9, nbc * dimc + 3) = dt * jac_SO3_right(wb * dt);
// formulas for 2 6D contacts:
// jac_delta_body.template block<3,3>(0,0) = 0.5 * bRl1 * dt * dt / mass;
@@ -696,27 +806,34 @@ inline void body2delta(const MatrixBase<D1>& body,
body2delta(body, dt, mass, nbc, dimc, delta);
}
-template<typename D1, typename D2, typename D3>
-inline void debiasData(const MatrixBase<D1>& _data, const MatrixBase<D2>& _bias, int nbc, int dimc, MatrixBase<D3>& _body)
+template <typename D1, typename D2, typename D3>
+inline void debiasData(const MatrixBase<D1>& _data,
+ const MatrixBase<D2>& _bias,
+ int nbc,
+ int dimc,
+ MatrixBase<D3>& _body)
{
- MatrixSizeCheck<6 , 1>::check(_bias);
- _body = _data;
- _body.template block<6,1>(nbc*dimc,0) = _data.template block<6,1>(nbc*dimc,0) - _bias;
+ MatrixSizeCheck<6, 1>::check(_bias);
+ _body = _data;
+ _body.template block<6, 1>(nbc * dimc, 0) = _data.template block<6, 1>(nbc * dimc, 0) - _bias;
}
-template<typename D1, typename D2, typename D3, typename D4>
-inline void debiasData(const MatrixBase<D1>& _data, const MatrixBase<D2>& _bias, int nbc, int dimc, MatrixBase<D3>& _body, MatrixBase<D4>& _J_body_bias)
+template <typename D1, typename D2, typename D3, typename D4>
+inline void debiasData(const MatrixBase<D1>& _data,
+ const MatrixBase<D2>& _bias,
+ int nbc,
+ int dimc,
+ MatrixBase<D3>& _body,
+ MatrixBase<D4>& _J_body_bias)
{
debiasData(_data, _bias, nbc, dimc, _body);
// MatrixSizeCheck<30, 6>::check(_J_body_bias); // 30 because the 2 last body quantities are quaternions
_J_body_bias.setZero();
- _J_body_bias.template block<3,3>(nbc*dimc, 0) = -Matrix<typename D1::Scalar, 3, 3>::Identity();
- _J_body_bias.template block<3,3>(nbc*dimc+3, 3) = -Matrix<typename D1::Scalar, 3, 3>::Identity();
+ _J_body_bias.template block<3, 3>(nbc * dimc, 0) = -Matrix<typename D1::Scalar, 3, 3>::Identity();
+ _J_body_bias.template block<3, 3>(nbc * dimc + 3, 3) = -Matrix<typename D1::Scalar, 3, 3>::Identity();
}
-
-
-} // namespace bodydynamics
-} // namespace wolf
+} // namespace bodydynamics
+} // namespace wolf
#endif /* FORCE_TORQUE_DELTA_TOOLS_H_ */
diff --git a/include/bodydynamics/processor/processor_force_torque_preint.h b/include/bodydynamics/processor/processor_force_torque_preint.h
index b5c188a..74f7bed 100644
--- a/include/bodydynamics/processor/processor_force_torque_preint.h
+++ b/include/bodydynamics/processor/processor_force_torque_preint.h
@@ -24,105 +24,103 @@
// Wolf core
#include <core/processor/processor_motion.h>
-namespace wolf {
+namespace wolf
+{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorquePreint);
struct ParamsProcessorForceTorquePreint : public ParamsProcessorMotion
{
- std::string sensor_ikin_name;
- std::string sensor_angvel_name;
- int nbc; // Number of contacts
- int dimc; // Dimension of the contact: 3D == point feet = 3D force, 6D = humanoid = wrench
-
- ParamsProcessorForceTorquePreint() = default;
- ParamsProcessorForceTorquePreint(std::string _unique_name, const ParamsServer& _server):
- ParamsProcessorMotion(_unique_name, _server)
- {
- sensor_ikin_name = _server.getParam<std::string>(_unique_name + "/sensor_ikin_name");
- sensor_angvel_name = _server.getParam<std::string>(_unique_name + "/sensor_angvel_name");
- nbc = _server.getParam<int>(_unique_name + "/nbc_");
- dimc = _server.getParam<int>(_unique_name + "/dimc_");
- }
- ~ParamsProcessorForceTorquePreint() override = default;
- std::string print() const override
- {
- return ParamsProcessorMotion::print() + "\n"
- + "sensor_ikin_name: " + sensor_ikin_name + "\n"
- + "sensor_angvel_name: " + sensor_angvel_name + "\n"
- + "nbc_: " + toString(nbc) + "\n"
- + "dimc_: " + toString(dimc) + "\n";
-
- }
+ std::string sensor_ikin_name;
+ std::string sensor_angvel_name;
+ int nbc; // Number of contacts
+ int dimc; // Dimension of the contact: 3D == point feet = 3D force, 6D = humanoid = wrench
+
+ ParamsProcessorForceTorquePreint() = default;
+ ParamsProcessorForceTorquePreint(std::string _unique_name, const ParamsServer& _server)
+ : ParamsProcessorMotion(_unique_name, _server)
+ {
+ sensor_ikin_name = _server.getParam<std::string>(_unique_name + "/sensor_ikin_name");
+ sensor_angvel_name = _server.getParam<std::string>(_unique_name + "/sensor_angvel_name");
+ nbc = _server.getParam<int>(_unique_name + "/nbc_");
+ dimc = _server.getParam<int>(_unique_name + "/dimc_");
+ }
+ ~ParamsProcessorForceTorquePreint() override = default;
+ std::string print() const override
+ {
+ return ParamsProcessorMotion::print() + "\n" + "sensor_ikin_name: " + sensor_ikin_name + "\n" +
+ "sensor_angvel_name: " + sensor_angvel_name + "\n" + "nbc_: " + toString(nbc) + "\n" +
+ "dimc_: " + toString(dimc) + "\n";
+ }
};
WOLF_PTR_TYPEDEFS(ProcessorForceTorquePreint);
-
-//class
-class ProcessorForceTorquePreint : public ProcessorMotion{
- public:
- ProcessorForceTorquePreint(ParamsProcessorForceTorquePreintPtr _params_motion_force_torque_preint);
- ~ProcessorForceTorquePreint() override;
- void configure(SensorBasePtr _sensor) override;
-
- WOLF_PROCESSOR_CREATE(ProcessorForceTorquePreint, ParamsProcessorForceTorquePreint);
-
- protected:
- void computeCurrentDelta(const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- const Eigen::VectorXd& _calib,
- const double _dt,
- Eigen::VectorXd& _delta,
- Eigen::MatrixXd& _delta_cov,
- Eigen::MatrixXd& _jacobian_calib) const override;
- void deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
- const Eigen::VectorXd& _delta,
- const double _dt,
- Eigen::VectorXd& _delta_preint_plus_delta) const override;
- void deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
- const Eigen::VectorXd& _delta,
- const double _dt,
- Eigen::VectorXd& _delta_preint_plus_delta,
- Eigen::MatrixXd& _jacobian_delta_preint,
- Eigen::MatrixXd& _jacobian_delta) const override;
- void statePlusDelta(const VectorComposite& _x,
- const Eigen::VectorXd& _delta,
- const double _dt,
- VectorComposite& _x_plus_delta ) const override;
- Eigen::VectorXd deltaZero() const override;
- Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
- const Eigen::VectorXd& delta_step) const override;
- VectorXd getCalibration (const CaptureBaseConstPtr _capture = nullptr) const override;
- void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
-
- bool voteForKeyFrame() const override;
- CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _calib,
- const VectorXd& _calib_preint,
- const CaptureBasePtr& _capture_origin) override;
- FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
- FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
- CaptureBasePtr _capture_origin) override;
-
- private:
- ParamsProcessorForceTorquePreintPtr params_motion_force_torque_preint_;
- SensorBasePtr sensor_ikin_;
- SensorBasePtr sensor_angvel_;
- int nbc_;
- int dimc_;
+
+// class
+class ProcessorForceTorquePreint : public ProcessorMotion
+{
+ public:
+ ProcessorForceTorquePreint(ParamsProcessorForceTorquePreintPtr _params_motion_force_torque_preint);
+ ~ProcessorForceTorquePreint() override;
+ void configure(SensorBasePtr _sensor) override;
+
+ WOLF_PROCESSOR_CREATE(ProcessorForceTorquePreint, ParamsProcessorForceTorquePreint);
+
+ protected:
+ void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta,
+ Eigen::MatrixXd& _jacobian_delta_preint,
+ Eigen::MatrixXd& _jacobian_delta) const override;
+ void statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ VectorComposite& _x_plus_delta) const override;
+ Eigen::VectorXd deltaZero() const override;
+ Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const override;
+ VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override;
+ void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+
+ bool voteForKeyFrame() const override;
+ CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin) override;
+ FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
+ FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) override;
+
+ private:
+ ParamsProcessorForceTorquePreintPtr params_motion_force_torque_preint_;
+ SensorBasePtr sensor_ikin_;
+ SensorBasePtr sensor_angvel_;
+ int nbc_;
+ int dimc_;
};
-}
+} // namespace wolf
/////////////////////////////////////////////////////////
// IMPLEMENTATION. Put your implementation includes here
/////////////////////////////////////////////////////////
-namespace wolf{
-
+namespace wolf
+{
inline void ProcessorForceTorquePreint::configure(SensorBasePtr _sensor)
{
sensor_ikin_ = _sensor->getProblem()->findSensor(params_motion_force_torque_preint_->sensor_ikin_name);
@@ -131,9 +129,10 @@ inline void ProcessorForceTorquePreint::configure(SensorBasePtr _sensor)
inline Eigen::VectorXd ProcessorForceTorquePreint::deltaZero() const
{
- return (Eigen::VectorXd(13) << 0,0,0, 0,0,0, 0,0,0, 0,0,0,1 ).finished(); // com, com vel, ang momentum, orientation
+ return (Eigen::VectorXd(13) << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1)
+ .finished(); // com, com vel, ang momentum, orientation
}
-} // namespace wolf
+} // namespace wolf
-#endif // PROCESSOR_FORCE_TORQUE_PREINT_H
+#endif // PROCESSOR_FORCE_TORQUE_PREINT_H
diff --git a/include/bodydynamics/processor/processor_inertial_kinematics.h b/include/bodydynamics/processor/processor_inertial_kinematics.h
index 03fab7a..19050d2 100644
--- a/include/bodydynamics/processor/processor_inertial_kinematics.h
+++ b/include/bodydynamics/processor/processor_inertial_kinematics.h
@@ -33,58 +33,58 @@
#include "bodydynamics/feature/feature_inertial_kinematics.h"
#include "bodydynamics/factor/factor_inertial_kinematics.h"
-namespace wolf {
+namespace wolf
+{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorInertialKinematics);
struct ParamsProcessorInertialKinematics : public ParamsProcessorBase
{
std::string sensor_angvel_name;
- double std_bp_drift;
+ double std_bp_drift;
ParamsProcessorInertialKinematics() = default;
- ParamsProcessorInertialKinematics(std::string _unique_name, const ParamsServer& _server):
- ParamsProcessorBase(_unique_name, _server)
+ ParamsProcessorInertialKinematics(std::string _unique_name, const ParamsServer& _server)
+ : ParamsProcessorBase(_unique_name, _server)
{
sensor_angvel_name = _server.getParam<std::string>(_unique_name + "/sensor_angvel_name");
- std_bp_drift = _server.getParam<double>(_unique_name + "/std_bp_drift");
+ std_bp_drift = _server.getParam<double>(_unique_name + "/std_bp_drift");
}
~ParamsProcessorInertialKinematics() override = default;
std::string print() const override
{
- return ParamsProcessorBase::print() + "\n"
- + "sensor_angvel_name: " + sensor_angvel_name + "\n"
- + "std_bp_drift: " + toString(std_bp_drift) + "\n";
+ return ParamsProcessorBase::print() + "\n" + "sensor_angvel_name: " + sensor_angvel_name + "\n" +
+ "std_bp_drift: " + toString(std_bp_drift) + "\n";
}
};
WOLF_PTR_TYPEDEFS(ProcessorInertialKinematics);
-
-//class
-class ProcessorInertialKinematics : public ProcessorBase{
- public:
- ProcessorInertialKinematics(ParamsProcessorInertialKinematicsPtr _params_ikin);
- ~ProcessorInertialKinematics() override = default;
- WOLF_PROCESSOR_CREATE(ProcessorInertialKinematics, ParamsProcessorInertialKinematics);
-
- void configure(SensorBasePtr _sensor) override;
-
- bool createInertialKinematicsFactor(CaptureInertialKinematicsPtr _cap_ikin, CaptureBasePtr _cap_angvel, CaptureBasePtr _cap_ikin_origin);
-
- void processCapture(CaptureBasePtr _incoming) override;
- void processKeyFrame(FrameBasePtr _keyframe_ptr) override;
- bool triggerInCapture(CaptureBasePtr) const override;
- bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override;
- bool storeKeyFrame(FrameBasePtr) override;
- bool storeCapture(CaptureBasePtr) override;
- bool voteForKeyFrame() const override;
-
-
- protected:
- ParamsProcessorInertialKinematicsPtr params_ikin_;
- CaptureBasePtr cap_origin_ptr_;
-};
-
+// class
+class ProcessorInertialKinematics : public ProcessorBase
+{
+ public:
+ ProcessorInertialKinematics(ParamsProcessorInertialKinematicsPtr _params_ikin);
+ ~ProcessorInertialKinematics() override = default;
+ WOLF_PROCESSOR_CREATE(ProcessorInertialKinematics, ParamsProcessorInertialKinematics);
+
+ void configure(SensorBasePtr _sensor) override;
+
+ bool createInertialKinematicsFactor(CaptureInertialKinematicsPtr _cap_ikin,
+ CaptureBasePtr _cap_angvel,
+ CaptureBasePtr _cap_ikin_origin);
+
+ void processCapture(CaptureBasePtr _incoming) override;
+ void processKeyFrame(FrameBasePtr _keyframe_ptr) override;
+ bool triggerInCapture(CaptureBasePtr) const override;
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override;
+ bool storeKeyFrame(FrameBasePtr) override;
+ bool storeCapture(CaptureBasePtr) override;
+ bool voteForKeyFrame() const override;
+
+ protected:
+ ParamsProcessorInertialKinematicsPtr params_ikin_;
+ CaptureBasePtr cap_origin_ptr_;
+};
inline bool ProcessorInertialKinematics::triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const
{
@@ -92,7 +92,7 @@ inline bool ProcessorInertialKinematics::triggerInKeyFrame(FrameBasePtr _keyfram
}
inline bool ProcessorInertialKinematics::triggerInCapture(CaptureBasePtr _capture) const
-{
+{
return true;
}
@@ -111,26 +111,20 @@ inline bool ProcessorInertialKinematics::voteForKeyFrame() const
return false;
}
-inline void ProcessorInertialKinematics::processKeyFrame(FrameBasePtr _keyframe_ptr)
-{
-}
-
+inline void ProcessorInertialKinematics::processKeyFrame(FrameBasePtr _keyframe_ptr) {}
//////////////////////////
// Covariance computations
//////////////////////////
-
-Eigen::Matrix9d computeIKinJac(const Eigen::Vector3d& w_unb,
- const Eigen::Vector3d& p_unb,
- const Eigen::Matrix3d& Iq);
+Eigen::Matrix9d computeIKinJac(const Eigen::Vector3d& w_unb, const Eigen::Vector3d& p_unb, const Eigen::Matrix3d& Iq);
Eigen::Matrix9d computeIKinCov(const Eigen::Matrix3d& Qp,
- const Eigen::Matrix3d& Qv,
- const Eigen::Matrix3d& Qw,
- const Eigen::Vector3d& p_unb,
- const Eigen::Vector3d& w_unb,
- const Eigen::Matrix3d& Iq);
+ const Eigen::Matrix3d& Qv,
+ const Eigen::Matrix3d& Qw,
+ const Eigen::Vector3d& p_unb,
+ const Eigen::Vector3d& w_unb,
+ const Eigen::Matrix3d& Iq);
} /* namespace wolf */
@@ -138,9 +132,8 @@ Eigen::Matrix9d computeIKinCov(const Eigen::Matrix3d& Qp,
// IMPLEMENTATION. Put your implementation includes here
/////////////////////////////////////////////////////////
+namespace wolf
+{
+} // namespace wolf
-namespace wolf{
-
-} // namespace wolf
-
-#endif // PROCESSOR_INERTIAL_KINEMATICS_H
+#endif // PROCESSOR_INERTIAL_KINEMATICS_H
diff --git a/include/bodydynamics/processor/processor_point_feet_nomove.h b/include/bodydynamics/processor/processor_point_feet_nomove.h
index 9486270..8f6585f 100644
--- a/include/bodydynamics/processor/processor_point_feet_nomove.h
+++ b/include/bodydynamics/processor/processor_point_feet_nomove.h
@@ -30,7 +30,8 @@
#include "bodydynamics/factor/factor_point_feet_nomove.h"
#include "bodydynamics/factor/factor_point_feet_altitude.h"
-namespace wolf {
+namespace wolf
+{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorPointFeetNomove);
struct ParamsProcessorPointFeetNomove : public ParamsProcessorBase
@@ -38,55 +39,53 @@ struct ParamsProcessorPointFeetNomove : public ParamsProcessorBase
double max_time_vote = -1;
ParamsProcessorPointFeetNomove() = default;
- ParamsProcessorPointFeetNomove(std::string _unique_name, const ParamsServer& _server):
- ParamsProcessorBase(_unique_name, _server)
+ ParamsProcessorPointFeetNomove(std::string _unique_name, const ParamsServer& _server)
+ : ParamsProcessorBase(_unique_name, _server)
{
max_time_vote = _server.getParam<double>(_unique_name + "/max_time_vote");
}
~ParamsProcessorPointFeetNomove() override = default;
std::string print() const override
{
- return "\n" + ParamsProcessorBase::print() + "\n"
- + "max_time_vote: " + toString(max_time_vote) + "\n";
+ return "\n" + ParamsProcessorBase::print() + "\n" + "max_time_vote: " + toString(max_time_vote) + "\n";
}
};
WOLF_PTR_TYPEDEFS(ProcessorPointFeetNomove);
-
-//class
-class ProcessorPointFeetNomove : public ProcessorBase{
- public:
- ProcessorPointFeetNomove(ParamsProcessorPointFeetNomovePtr _params_pfnomove);
- ~ProcessorPointFeetNomove() override = default;
- WOLF_PROCESSOR_CREATE(ProcessorPointFeetNomove, ParamsProcessorPointFeetNomove);
-
- void configure(SensorBasePtr _sensor) override;
-
- bool createPointFeetNomoveFactor(CapturePointFeetNomovePtr _cap_pfnomove, CaptureBasePtr _cap_angvel, CaptureBasePtr _cap_pfnomove_origin);
-
- void processCapture(CaptureBasePtr _incoming) override;
- void processKeyFrame(FrameBasePtr _keyframe_ptr) override;
- bool triggerInCapture(CaptureBasePtr) const override;
- bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override;
- bool storeKeyFrame(FrameBasePtr) override;
- bool storeCapture(CaptureBasePtr) override;
- bool voteForKeyFrame() const override;
-
- void createFactorConsecutive();
-
- void processCaptureCreateFactorFar(CaptureBasePtr _capture);
- void processKeyFrameCreateFactorFar();
-
-
-
- protected:
- ParamsProcessorPointFeetNomovePtr params_pfnomove_;
- CaptureBasePtr origin_;
- CaptureBasePtr incoming_;
- std::map<int, CaptureBasePtr> tracks_;
-};
-
+// class
+class ProcessorPointFeetNomove : public ProcessorBase
+{
+ public:
+ ProcessorPointFeetNomove(ParamsProcessorPointFeetNomovePtr _params_pfnomove);
+ ~ProcessorPointFeetNomove() override = default;
+ WOLF_PROCESSOR_CREATE(ProcessorPointFeetNomove, ParamsProcessorPointFeetNomove);
+
+ void configure(SensorBasePtr _sensor) override;
+
+ bool createPointFeetNomoveFactor(CapturePointFeetNomovePtr _cap_pfnomove,
+ CaptureBasePtr _cap_angvel,
+ CaptureBasePtr _cap_pfnomove_origin);
+
+ void processCapture(CaptureBasePtr _incoming) override;
+ void processKeyFrame(FrameBasePtr _keyframe_ptr) override;
+ bool triggerInCapture(CaptureBasePtr) const override;
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override;
+ bool storeKeyFrame(FrameBasePtr) override;
+ bool storeCapture(CaptureBasePtr) override;
+ bool voteForKeyFrame() const override;
+
+ void createFactorConsecutive();
+
+ void processCaptureCreateFactorFar(CaptureBasePtr _capture);
+ void processKeyFrameCreateFactorFar();
+
+ protected:
+ ParamsProcessorPointFeetNomovePtr params_pfnomove_;
+ CaptureBasePtr origin_;
+ CaptureBasePtr incoming_;
+ std::map<int, CaptureBasePtr> tracks_;
+};
inline bool ProcessorPointFeetNomove::triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const
{
@@ -94,7 +93,7 @@ inline bool ProcessorPointFeetNomove::triggerInKeyFrame(FrameBasePtr _keyframe_p
}
inline bool ProcessorPointFeetNomove::triggerInCapture(CaptureBasePtr _capture) const
-{
+{
return true;
}
@@ -108,16 +107,14 @@ inline bool ProcessorPointFeetNomove::storeCapture(CaptureBasePtr)
return true;
}
-
} /* namespace wolf */
/////////////////////////////////////////////////////////
// IMPLEMENTATION. Put your implementation includes here
/////////////////////////////////////////////////////////
+namespace wolf
+{
+} // namespace wolf
-namespace wolf{
-
-} // namespace wolf
-
-#endif // PROCESSOR_POINT_FEET_NOMOVE_H
+#endif // PROCESSOR_POINT_FEET_NOMOVE_H
diff --git a/include/bodydynamics/sensor/sensor_force_torque.h b/include/bodydynamics/sensor/sensor_force_torque.h
index 1321160..8bb6a3b 100644
--- a/include/bodydynamics/sensor/sensor_force_torque.h
+++ b/include/bodydynamics/sensor/sensor_force_torque.h
@@ -21,62 +21,57 @@
#ifndef SENSOR_FORCE_TORQUE_H
#define SENSOR_FORCE_TORQUE_H
-//wolf includes
+// wolf includes
#include "core/sensor/sensor_base.h"
#include "core/utils/params_server.h"
#include <iostream>
-namespace wolf {
-
+namespace wolf
+{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorForceTorque);
-
struct ParamsSensorForceTorque : public ParamsSensorBase
{
- //noise std dev
- double mass = 10; // total mass of the robot (kg)
- double std_f = 0.001; // standard deviation of the force sensors (N)
- double std_tau = 0.001; // standard deviation of the torque sensors (N.m)
-
- ParamsSensorForceTorque() = default;
- ParamsSensorForceTorque(std::string _unique_name, const ParamsServer& _server):
- ParamsSensorBase(_unique_name, _server)
- {
- mass = _server.getParam<double>(_unique_name + "/mass");
- std_f = _server.getParam<double>(_unique_name + "/std_f");
- std_tau = _server.getParam<double>(_unique_name + "/std_tau");
- }
- ~ParamsSensorForceTorque() override = default;
- std::string print() const override
- {
- return ParamsSensorBase::print() + "\n"
- + "mass: " + toString(mass) + "\n"
- + "std_f: " + toString(std_f) + "\n"
- + "std_tau: " + toString(std_tau) + "\n";
- }
+ // noise std dev
+ double mass = 10; // total mass of the robot (kg)
+ double std_f = 0.001; // standard deviation of the force sensors (N)
+ double std_tau = 0.001; // standard deviation of the torque sensors (N.m)
+
+ ParamsSensorForceTorque() = default;
+ ParamsSensorForceTorque(std::string _unique_name, const ParamsServer& _server)
+ : ParamsSensorBase(_unique_name, _server)
+ {
+ mass = _server.getParam<double>(_unique_name + "/mass");
+ std_f = _server.getParam<double>(_unique_name + "/std_f");
+ std_tau = _server.getParam<double>(_unique_name + "/std_tau");
+ }
+ ~ParamsSensorForceTorque() override = default;
+ std::string print() const override
+ {
+ return ParamsSensorBase::print() + "\n" + "mass: " + toString(mass) + "\n" + "std_f: " + toString(std_f) +
+ "\n" + "std_tau: " + toString(std_tau) + "\n";
+ }
};
WOLF_PTR_TYPEDEFS(SensorForceTorque);
class SensorForceTorque : public SensorBase
{
+ protected:
+ // noise std dev
+ double mass_; // total mass of the robot (kg)
+ double std_f_; // standard deviation of the force sensors (N)
+ double std_tau_; // standard deviation of the torque sensors (N.m)
- protected:
- //noise std dev
- double mass_; // total mass of the robot (kg)
- double std_f_; // standard deviation of the force sensors (N)
- double std_tau_; // standard deviation of the torque sensors (N.m)
-
- public:
-
- SensorForceTorque(const Eigen::VectorXd& _extrinsics, ParamsSensorForceTorquePtr _params);
- ~SensorForceTorque() override = default;
+ public:
+ SensorForceTorque(const Eigen::VectorXd& _extrinsics, ParamsSensorForceTorquePtr _params);
+ ~SensorForceTorque() override = default;
- WOLF_SENSOR_CREATE(SensorForceTorque, ParamsSensorForceTorque, 0);
+ WOLF_SENSOR_CREATE(SensorForceTorque, ParamsSensorForceTorque, 0);
- double getMass() const;
- double getForceNoiseStd() const;
- double getTorqueNoiseStd() const;
+ double getMass() const;
+ double getForceNoiseStd() const;
+ double getTorqueNoiseStd() const;
};
inline double SensorForceTorque::getMass() const
@@ -94,6 +89,6 @@ inline double SensorForceTorque::getTorqueNoiseStd() const
return std_tau_;
}
-} // namespace wolf
+} // namespace wolf
-#endif // SENSOR_FORCE_TORQUE_H
+#endif // SENSOR_FORCE_TORQUE_H
diff --git a/include/bodydynamics/sensor/sensor_inertial_kinematics.h b/include/bodydynamics/sensor/sensor_inertial_kinematics.h
index 59a856a..764be0f 100644
--- a/include/bodydynamics/sensor/sensor_inertial_kinematics.h
+++ b/include/bodydynamics/sensor/sensor_inertial_kinematics.h
@@ -21,55 +21,51 @@
#ifndef SENSOR_INERTIAL_KINEMATICS_H
#define SENSOR_INERTIAL_KINEMATICS_H
-//wolf includes
+// wolf includes
#include "core/sensor/sensor_base.h"
#include "core/utils/params_server.h"
#include <iostream>
-namespace wolf {
-
+namespace wolf
+{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorInertialKinematics);
-
struct ParamsSensorInertialKinematics : public ParamsSensorBase
{
- //noise std dev
- double std_pbc; // standard deviation of the com position measurement relative to the base frame (m)
- double std_vbc; // standard deviation of the com velocity measurement relative to the base frame (m/s)
-
- ParamsSensorInertialKinematics() = default;
- ParamsSensorInertialKinematics(std::string _unique_name, const ParamsServer& _server):
- ParamsSensorBase(_unique_name, _server)
- {
- std_pbc = _server.getParam<double>(_unique_name + "/std_pbc");
- std_vbc = _server.getParam<double>(_unique_name + "/std_vbc");
- }
- ~ParamsSensorInertialKinematics() override = default;
- std::string print() const override
- {
- return ParamsSensorBase::print() + "\n"
- + "std_pbc: " + toString(std_pbc) + "\n"
- + "std_vbc: " + toString(std_vbc) + "\n";
- }
+ // noise std dev
+ double std_pbc; // standard deviation of the com position measurement relative to the base frame (m)
+ double std_vbc; // standard deviation of the com velocity measurement relative to the base frame (m/s)
+
+ ParamsSensorInertialKinematics() = default;
+ ParamsSensorInertialKinematics(std::string _unique_name, const ParamsServer& _server)
+ : ParamsSensorBase(_unique_name, _server)
+ {
+ std_pbc = _server.getParam<double>(_unique_name + "/std_pbc");
+ std_vbc = _server.getParam<double>(_unique_name + "/std_vbc");
+ }
+ ~ParamsSensorInertialKinematics() override = default;
+ std::string print() const override
+ {
+ return ParamsSensorBase::print() + "\n" + "std_pbc: " + toString(std_pbc) + "\n" +
+ "std_vbc: " + toString(std_vbc) + "\n";
+ }
};
WOLF_PTR_TYPEDEFS(SensorInertialKinematics);
class SensorInertialKinematics : public SensorBase
{
+ protected:
+ ParamsSensorInertialKinematicsPtr intr_ikin_;
- protected:
- ParamsSensorInertialKinematicsPtr intr_ikin_;
-
- public:
-
- SensorInertialKinematics(const Eigen::VectorXd& _extrinsics, ParamsSensorInertialKinematicsPtr _intr_ikin);
- ~SensorInertialKinematics() override;
+ public:
+ SensorInertialKinematics(const Eigen::VectorXd& _extrinsics, ParamsSensorInertialKinematicsPtr _intr_ikin);
+ ~SensorInertialKinematics() override;
- WOLF_SENSOR_CREATE(SensorInertialKinematics, ParamsSensorInertialKinematics, 0);
+ WOLF_SENSOR_CREATE(SensorInertialKinematics, ParamsSensorInertialKinematics, 0);
- double getPbcNoiseStd() const;
- double getVbcNoiseStd() const;
+ double getPbcNoiseStd() const;
+ double getVbcNoiseStd() const;
};
inline double SensorInertialKinematics::getPbcNoiseStd() const
@@ -82,6 +78,6 @@ inline double SensorInertialKinematics::getVbcNoiseStd() const
return intr_ikin_->std_vbc;
}
-} // namespace wolf
+} // namespace wolf
-#endif // SENSOR_INERTIAL_KINEMATICS_H
+#endif // SENSOR_INERTIAL_KINEMATICS_H
diff --git a/include/bodydynamics/sensor/sensor_point_feet_nomove.h b/include/bodydynamics/sensor/sensor_point_feet_nomove.h
index ad6ad45..14e8346 100644
--- a/include/bodydynamics/sensor/sensor_point_feet_nomove.h
+++ b/include/bodydynamics/sensor/sensor_point_feet_nomove.h
@@ -21,64 +21,59 @@
#ifndef SENSOR_POINT_FEET_NOMOVE_H
#define SENSOR_POINT_FEET_NOMOVE_H
-//wolf includes
+// wolf includes
#include "core/sensor/sensor_base.h"
#include "core/utils/params_server.h"
#include <iostream>
-namespace wolf {
-
+namespace wolf
+{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorPointFeetNomove);
-
struct ParamsSensorPointFeetNomove : public ParamsSensorBase
{
- // standard deviation on the assumption that the feet are not moving when in contact
- // the noise represents a random walk on the foot position, hence the unit
- double std_foot_nomove_; // m/(s^2 sqrt(dt))
- // certainty on current ground altitude
- double std_altitude_ = 1000; // m, deactivated by default
- double foot_radius_ = 0; // m
-
- ParamsSensorPointFeetNomove() = default;
- ParamsSensorPointFeetNomove(std::string _unique_name, const ParamsServer& _server):
- ParamsSensorBase(_unique_name, _server)
- {
- std_foot_nomove_ = _server.getParam<double>(_unique_name + "/std_foot_nomove");
- std_altitude_ = _server.getParam<double>(_unique_name + "/std_altitude");
- foot_radius_ = _server.getParam<double>(_unique_name + "/foot_radius");
- }
- ~ParamsSensorPointFeetNomove() override = default;
- std::string print() const override
- {
- return "\n" + ParamsSensorBase::print() + "\n"
- + "std_altitude: " + toString(std_altitude_) + "\n"
- + "std_foot_nomove: " + toString(std_foot_nomove_) + "\n";
- + "foot_radius_: " + toString(foot_radius_) + "\n";
-
- }
+ // standard deviation on the assumption that the feet are not moving when in contact
+ // the noise represents a random walk on the foot position, hence the unit
+ double std_foot_nomove_; // m/(s^2 sqrt(dt))
+ // certainty on current ground altitude
+ double std_altitude_ = 1000; // m, deactivated by default
+ double foot_radius_ = 0; // m
+
+ ParamsSensorPointFeetNomove() = default;
+ ParamsSensorPointFeetNomove(std::string _unique_name, const ParamsServer& _server)
+ : ParamsSensorBase(_unique_name, _server)
+ {
+ std_foot_nomove_ = _server.getParam<double>(_unique_name + "/std_foot_nomove");
+ std_altitude_ = _server.getParam<double>(_unique_name + "/std_altitude");
+ foot_radius_ = _server.getParam<double>(_unique_name + "/foot_radius");
+ }
+ ~ParamsSensorPointFeetNomove() override = default;
+ std::string print() const override
+ {
+ return "\n" + ParamsSensorBase::print() + "\n" + "std_altitude: " + toString(std_altitude_) + "\n" +
+ "std_foot_nomove: " + toString(std_foot_nomove_) + "\n";
+ +"foot_radius_: " + toString(foot_radius_) + "\n";
+ }
};
WOLF_PTR_TYPEDEFS(SensorPointFeetNomove);
class SensorPointFeetNomove : public SensorBase
{
+ protected:
+ Matrix3d cov_foot_nomove_; // random walk covariance in (m/s^2/sqrt(Hz))^2
+ Matrix1d cov_altitude_; // m^2
+ double foot_radius_;
- protected:
- Matrix3d cov_foot_nomove_; // random walk covariance in (m/s^2/sqrt(Hz))^2
- Matrix1d cov_altitude_; // m^2
- double foot_radius_;
+ public:
+ SensorPointFeetNomove(const Eigen::VectorXd& _extrinsics, const ParamsSensorPointFeetNomovePtr& _intr_pfnm);
+ ~SensorPointFeetNomove() override;
- public:
+ WOLF_SENSOR_CREATE(SensorPointFeetNomove, ParamsSensorPointFeetNomove, 0);
- SensorPointFeetNomove(const Eigen::VectorXd& _extrinsics, const ParamsSensorPointFeetNomovePtr& _intr_pfnm);
- ~SensorPointFeetNomove() override;
-
- WOLF_SENSOR_CREATE(SensorPointFeetNomove, ParamsSensorPointFeetNomove, 0);
-
- Matrix3d getCovFootNomove() const;
- Matrix1d getCovAltitude() const;
- double getFootRadius() const;
+ Matrix3d getCovFootNomove() const;
+ Matrix1d getCovAltitude() const;
+ double getFootRadius() const;
};
inline Matrix3d SensorPointFeetNomove::getCovFootNomove() const
@@ -96,7 +91,6 @@ inline double SensorPointFeetNomove::getFootRadius() const
return foot_radius_;
}
+} // namespace wolf
-} // namespace wolf
-
-#endif // SENSOR_POINT_FEET_NOMOVE_H
+#endif // SENSOR_POINT_FEET_NOMOVE_H
diff --git a/src/capture/capture_force_torque_preint.cpp b/src/capture/capture_force_torque_preint.cpp
index 06b59e2..ac8276c 100644
--- a/src/capture/capture_force_torque_preint.cpp
+++ b/src/capture/capture_force_torque_preint.cpp
@@ -23,26 +23,30 @@
#include "bodydynamics/sensor/sensor_force_torque.h"
#include "core/state_block/state_quaternion.h"
-namespace wolf {
-
+namespace wolf
+{
CaptureForceTorquePreint::CaptureForceTorquePreint(
- const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- CaptureInertialKinematicsPtr _capture_IK_ptr, // to get the pbc bias
- CaptureMotionPtr _capture_motion_ptr, // to get the gyro bias
- const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov, // TODO: no uncertainty from kinematics
- CaptureBasePtr _capture_origin_ptr) :
- CaptureMotion("CaptureForceTorquePreint", _init_ts, _sensor_ptr, _data, _data_cov,
- _capture_origin_ptr, nullptr, nullptr, nullptr),
- cap_ikin_other_(_capture_IK_ptr),
- cap_gyro_other_(_capture_motion_ptr)
+ const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ CaptureInertialKinematicsPtr _capture_IK_ptr, // to get the pbc bias
+ CaptureMotionPtr _capture_motion_ptr, // to get the gyro bias
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov, // TODO: no uncertainty from kinematics
+ CaptureBasePtr _capture_origin_ptr)
+ : CaptureMotion("CaptureForceTorquePreint",
+ _init_ts,
+ _sensor_ptr,
+ _data,
+ _data_cov,
+ _capture_origin_ptr,
+ nullptr,
+ nullptr,
+ nullptr),
+ cap_ikin_other_(_capture_IK_ptr),
+ cap_gyro_other_(_capture_motion_ptr)
{
}
-CaptureForceTorquePreint::~CaptureForceTorquePreint()
-{
-
-}
+CaptureForceTorquePreint::~CaptureForceTorquePreint() {}
-} //namespace wolf
+} // namespace wolf
diff --git a/src/capture/capture_inertial_kinematics.cpp b/src/capture/capture_inertial_kinematics.cpp
index 8121547..b5ba297 100644
--- a/src/capture/capture_inertial_kinematics.cpp
+++ b/src/capture/capture_inertial_kinematics.cpp
@@ -23,42 +23,44 @@
#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
#include "core/state_block/state_quaternion.h"
-namespace wolf {
-
-CaptureInertialKinematics::CaptureInertialKinematics(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::Vector9d& _data, // pbc, vbc, wb
- const Eigen::Matrix3d & _B_I_q, // Centroidal inertia matrix expressed in body frame
- const Eigen::Vector3d & _B_Lc_m, // Centroidal angular momentum expressed in body frame
- const Vector3d& _bias_pbc) :
- CaptureBase("CaptureInertialKinematics",
- _init_ts,
- _sensor_ptr,
- nullptr,
- nullptr,
- std::make_shared<StateBlock>(_bias_pbc, false)),
- data_(_data),
- B_I_q_(_B_I_q),
- B_Lc_m_(_B_Lc_m)
+namespace wolf
+{
+CaptureInertialKinematics::CaptureInertialKinematics(
+ const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::Vector9d& _data, // pbc, vbc, wb
+ const Eigen::Matrix3d& _B_I_q, // Centroidal inertia matrix expressed in body frame
+ const Eigen::Vector3d& _B_Lc_m, // Centroidal angular momentum expressed in body frame
+ const Vector3d& _bias_pbc)
+ : CaptureBase("CaptureInertialKinematics",
+ _init_ts,
+ _sensor_ptr,
+ nullptr,
+ nullptr,
+ std::make_shared<StateBlock>(_bias_pbc, false)),
+ data_(_data),
+ B_I_q_(_B_I_q),
+ B_Lc_m_(_B_Lc_m)
{
//
}
-
-CaptureInertialKinematics::CaptureInertialKinematics(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::Vector9d& _data, // pbc, vbc, wb
- const Eigen::Matrix3d & _B_I_q, // Centroidal inertia matrix expressed in body frame
- const Eigen::Vector3d & _B_Lc_m) : // Centroidal angular momentum expressed in body frame
- CaptureBase("CaptureInertialKinematics",
- _init_ts,
- _sensor_ptr,
- nullptr,
- nullptr,
- std::make_shared<StateBlock>(3, false)),
- data_(_data),
- B_I_q_(_B_I_q),
- B_Lc_m_(_B_Lc_m)
+CaptureInertialKinematics::CaptureInertialKinematics(
+ const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::Vector9d& _data, // pbc, vbc, wb
+ const Eigen::Matrix3d& _B_I_q, // Centroidal inertia matrix expressed in body frame
+ const Eigen::Vector3d& _B_Lc_m)
+ : // Centroidal angular momentum expressed in body frame
+ CaptureBase("CaptureInertialKinematics",
+ _init_ts,
+ _sensor_ptr,
+ nullptr,
+ nullptr,
+ std::make_shared<StateBlock>(3, false)),
+ data_(_data),
+ B_I_q_(_B_I_q),
+ B_Lc_m_(_B_Lc_m)
{
//
}
@@ -68,4 +70,4 @@ CaptureInertialKinematics::~CaptureInertialKinematics()
//
}
-} //namespace wolf
+} // namespace wolf
diff --git a/src/capture/capture_leg_odom.cpp b/src/capture/capture_leg_odom.cpp
index f6d9770..9e4c30f 100644
--- a/src/capture/capture_leg_odom.cpp
+++ b/src/capture/capture_leg_odom.cpp
@@ -18,101 +18,115 @@
// 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/>.
-
#include "core/math/rotations.h"
#include "core/capture/capture_base.h"
#include "bodydynamics/capture/capture_leg_odom.h"
-
-namespace wolf {
-
+namespace wolf
+{
// bm: base at time t-dt
// b: base at current time
-CaptureLegOdom::CaptureLegOdom(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::MatrixXd& _data_kin, // cols: b_p_bl1m, b_p_bl1, b_p_bl2m, b_p_bl2..., rows: xyz
- Eigen::Matrix6d _data_cov, // for the moment only 6x6 mat, see if more involved computations are necessary (contact confidence...)
- double dt,
- CaptureLegOdomType lo_type) :
- CaptureMotion("CaptureLegOdom", _init_ts, _sensor_ptr,
- Eigen::Vector7d::Zero(), Eigen::Matrix6d::Identity(), // dummy data and data covariance filled in the constructor
- nullptr, nullptr, nullptr, nullptr)
+CaptureLegOdom::CaptureLegOdom(
+ const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::MatrixXd& _data_kin, // cols: b_p_bl1m, b_p_bl1, b_p_bl2m, b_p_bl2..., rows: xyz
+ Eigen::Matrix6d _data_cov, // for the moment only 6x6 mat, see if more involved computations are necessary
+ // (contact confidence...)
+ double dt,
+ CaptureLegOdomType lo_type)
+ : CaptureMotion("CaptureLegOdom",
+ _init_ts,
+ _sensor_ptr,
+ Eigen::Vector7d::Zero(),
+ Eigen::Matrix6d::Identity(), // dummy data and data covariance filled in the constructor
+ nullptr,
+ nullptr,
+ nullptr,
+ nullptr)
{
Vector7d data;
- if (_data_kin.cols() < 2){
+ if (_data_kin.cols() < 2)
+ {
// no feet in contact, flying robot -> dummy data with high cov
- data << 0,0,0, 0,0,0,1;
+ data << 0, 0, 0, 0, 0, 0, 1;
_data_cov = pow(1000, 2) * _data_cov;
return;
}
- if (lo_type == CaptureLegOdomType::POINT_FEET_RELATIVE_KIN){
+ if (lo_type == CaptureLegOdomType::POINT_FEET_RELATIVE_KIN)
+ {
assert(_data_kin.rows() == 3);
// relative base-feet position + angular velocity
- Eigen::Vector3d i_omg_oi = _data_kin.rightCols<1>();
- Eigen::Quaterniond bm_q_b (v2q(i_omg_oi*dt));
+ Eigen::Vector3d i_omg_oi = _data_kin.rightCols<1>();
+ Eigen::Quaterniond bm_q_b(v2q(i_omg_oi * dt));
std::vector<Vector3d> bm_p_bmb_vec;
- for (unsigned int i=0; i < (_data_kin.cols()-1); i+=2){
+ for (unsigned int i = 0; i < (_data_kin.cols() - 1); i += 2)
+ {
Eigen::Vector3d bm_p_bml = _data_kin.col(i);
- Eigen::Vector3d b_p_bl = _data_kin.col(i+1);
+ Eigen::Vector3d b_p_bl = _data_kin.col(i + 1);
bm_p_bmb_vec.push_back(bm_p_bml - bm_q_b * b_p_bl);
}
// for the moment simple mean, to be improved
Vector3d bm_p_bmb = Vector3d::Zero();
- for (unsigned int i=0; i < bm_p_bmb_vec.size(); i++){
+ for (unsigned int i = 0; i < bm_p_bmb_vec.size(); i++)
+ {
bm_p_bmb += bm_p_bmb_vec[i];
}
bm_p_bmb /= bm_p_bmb_vec.size();
- data.head<3>() = bm_p_bmb;
- data.tail<4>() = bm_q_b.coeffs();
-
+ data.head<3>() = bm_p_bmb;
+ data.tail<4>() = bm_q_b.coeffs();
}
- else if(lo_type == CaptureLegOdomType::POINT_FEET_DIFFERENTIAL_KIN){
+ else if (lo_type == CaptureLegOdomType::POINT_FEET_DIFFERENTIAL_KIN)
+ {
assert(_data_kin.rows() == 10);
// TODO: document data matrix organization
std::vector<Vector3d> b_v_ob_vec;
- Vector3d i_omg_oi = _data_kin.block<3,1>(0, _data_kin.cols()-1); // retrieve angular vel first
- Eigen::Quaterniond bm_q_b (v2q(i_omg_oi*dt));
- for (unsigned int i=0; i < _data_kin.cols()-1; i++){
- Vector3d b_p_bl = _data_kin.block<3,1>(0,i);
- Quaterniond b_q_l(_data_kin.block<4,1>(3,i));
- Vector3d l_v_bl = _data_kin.block<3,1>(7,i);
- Vector3d b_v_ob = b_p_bl.cross(i_omg_oi) - b_q_l * l_v_bl;
+ Vector3d i_omg_oi = _data_kin.block<3, 1>(0, _data_kin.cols() - 1); // retrieve angular vel first
+ Eigen::Quaterniond bm_q_b(v2q(i_omg_oi * dt));
+ for (unsigned int i = 0; i < _data_kin.cols() - 1; i++)
+ {
+ Vector3d b_p_bl = _data_kin.block<3, 1>(0, i);
+ Quaterniond b_q_l(_data_kin.block<4, 1>(3, i));
+ Vector3d l_v_bl = _data_kin.block<3, 1>(7, i);
+ Vector3d b_v_ob = b_p_bl.cross(i_omg_oi) - b_q_l * l_v_bl;
b_v_ob_vec.push_back(b_v_ob);
}
// Simple mean for the moment
Vector3d b_v_ob_mean = Vector3d::Zero();
- for (unsigned int i=0; i < b_v_ob_vec.size(); i++){
+ for (unsigned int i = 0; i < b_v_ob_vec.size(); i++)
+ {
b_v_ob_mean += b_v_ob_vec[i];
}
b_v_ob_mean /= b_v_ob_vec.size();
- data.head<3>() = b_v_ob_mean*dt;
- data.tail<4>() = bm_q_b.coeffs();
+ data.head<3>() = b_v_ob_mean * dt;
+ data.tail<4>() = bm_q_b.coeffs();
}
- else if(lo_type == CaptureLegOdomType::HUMANOID_FEET_RELATIVE_KIN){
+ else if (lo_type == CaptureLegOdomType::HUMANOID_FEET_RELATIVE_KIN)
+ {
assert(_data_kin.rows() == 7);
// relative transformations
// most simple option: odometry of one of the feet
- Eigen::Vector7d bm_pose_l = _data_kin.col(0);
- Eigen::Vector7d b_pose_l = _data_kin.col(1);
+ Eigen::Vector7d bm_pose_l = _data_kin.col(0);
+ Eigen::Vector7d b_pose_l = _data_kin.col(1);
Eigen::Quaterniond bm_q_l(bm_pose_l.tail<4>());
Eigen::Quaterniond b_q_l(b_pose_l.tail<4>());
- Eigen::Vector3d bm_p_bml(bm_pose_l.head<3>());
- Eigen::Vector3d b_p_bl(b_pose_l.head<3>());
+ Eigen::Vector3d bm_p_bml(bm_pose_l.head<3>());
+ Eigen::Vector3d b_p_bl(b_pose_l.head<3>());
Eigen::Quaterniond bm_q_b(bm_q_l * b_q_l.inverse());
- Eigen::Vector3d bm_p_bmb = bm_p_bml - bm_q_b * b_p_bl;
+ Eigen::Vector3d bm_p_bmb = bm_p_bml - bm_q_b * b_p_bl;
- data.head<3>() = bm_p_bmb;
- data.tail<4>() = bm_q_b.coeffs();
+ data.head<3>() = bm_p_bmb;
+ data.tail<4>() = bm_q_b.coeffs();
}
- else {
+ else
+ {
std::cout << "Unkown CaptureLegOdomType! ERROR (TODO)" << std::endl;
}
setData(data);
@@ -124,4 +138,4 @@ CaptureLegOdom::~CaptureLegOdom()
//
}
-} //namespace wolf
+} // namespace wolf
diff --git a/src/capture/capture_point_feet_nomove.cpp b/src/capture/capture_point_feet_nomove.cpp
index 904db89..51ba014 100644
--- a/src/capture/capture_point_feet_nomove.cpp
+++ b/src/capture/capture_point_feet_nomove.cpp
@@ -18,29 +18,23 @@
// 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/>.
-
#include "core/capture/capture_base.h"
#include "bodydynamics/capture/capture_point_feet_nomove.h"
-
-namespace wolf {
-
+namespace wolf
+{
// bm: base at time t-dt
// b: base at current time
-CapturePointFeetNomove::CapturePointFeetNomove(
- const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const std::unordered_map<int, Eigen::Vector7d>& kin_incontact
- ) :
- CaptureBase("CapturePointFeetNomove",
- _init_ts,
- _sensor_ptr),
- kin_incontact_(kin_incontact)
-{}
+CapturePointFeetNomove::CapturePointFeetNomove(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
+ const std::unordered_map<int, Eigen::Vector7d>& kin_incontact)
+ : CaptureBase("CapturePointFeetNomove", _init_ts, _sensor_ptr), kin_incontact_(kin_incontact)
+{
+}
CapturePointFeetNomove::~CapturePointFeetNomove()
{
//
}
-} //namespace wolf
+} // namespace wolf
diff --git a/src/feature/feature_force_torque.cpp b/src/feature/feature_force_torque.cpp
index 41cd9d0..477c0f4 100644
--- a/src/feature/feature_force_torque.cpp
+++ b/src/feature/feature_force_torque.cpp
@@ -20,34 +20,39 @@
#include "bodydynamics/feature/feature_force_torque.h"
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(FeatureForceTorque);
-FeatureForceTorque::FeatureForceTorque(
- const double& _dt,
- const double& _mass,
- const Eigen::Vector6d& _forces_meas,
- const Eigen::Vector6d& _torques_meas,
- const Eigen::Vector3d& _pbc_meas,
- const Eigen::Matrix<double,14,1>& _kin_meas,
- const Eigen::Matrix6d& _cov_forces_meas,
- const Eigen::Matrix6d& _cov_torques_meas,
- const Eigen::Matrix3d& _cov_pbc_meas,
- const Eigen::Matrix<double,14,14>& _cov_kin_meas) :
- FeatureBase("FORCETORQUE", 42*Eigen::Matrix1d::Identity(), 42*Eigen::Matrix1d::Identity(), UNCERTAINTY_IS_COVARIANCE), // Pass dummmy values -> we are bypassing the way meas and cov is usually stored because too many of them == vector is too big -> error prone
- dt_(_dt),
- mass_(_mass),
- forces_meas_(_forces_meas),
- torques_meas_(_torques_meas),
- pbc_meas_(_pbc_meas),
- kin_meas_(_kin_meas),
- cov_forces_meas_(_cov_forces_meas),
- cov_torques_meas_(_cov_torques_meas),
- cov_pbc_meas_(_cov_pbc_meas),
- cov_kin_meas_(_cov_kin_meas)
-{}
+FeatureForceTorque::FeatureForceTorque(const double& _dt,
+ const double& _mass,
+ const Eigen::Vector6d& _forces_meas,
+ const Eigen::Vector6d& _torques_meas,
+ const Eigen::Vector3d& _pbc_meas,
+ const Eigen::Matrix<double, 14, 1>& _kin_meas,
+ const Eigen::Matrix6d& _cov_forces_meas,
+ const Eigen::Matrix6d& _cov_torques_meas,
+ const Eigen::Matrix3d& _cov_pbc_meas,
+ const Eigen::Matrix<double, 14, 14>& _cov_kin_meas)
+ : FeatureBase(
+ "FORCETORQUE",
+ 42 * Eigen::Matrix1d::Identity(),
+ 42 * Eigen::Matrix1d::Identity(),
+ UNCERTAINTY_IS_COVARIANCE), // Pass dummmy values -> we are bypassing the way meas and cov is usually stored
+ // because too many of them == vector is too big -> error prone
+ dt_(_dt),
+ mass_(_mass),
+ forces_meas_(_forces_meas),
+ torques_meas_(_torques_meas),
+ pbc_meas_(_pbc_meas),
+ kin_meas_(_kin_meas),
+ cov_forces_meas_(_cov_forces_meas),
+ cov_torques_meas_(_cov_torques_meas),
+ cov_pbc_meas_(_cov_pbc_meas),
+ cov_kin_meas_(_cov_kin_meas)
+{
+}
-FeatureForceTorque::~FeatureForceTorque(){}
+FeatureForceTorque::~FeatureForceTorque() {}
-} // namespace wolf
+} // namespace wolf
diff --git a/src/feature/feature_force_torque_preint.cpp b/src/feature/feature_force_torque_preint.cpp
index 3abefee..28e4233 100644
--- a/src/feature/feature_force_torque_preint.cpp
+++ b/src/feature/feature_force_torque_preint.cpp
@@ -19,17 +19,17 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "bodydynamics/feature/feature_force_torque_preint.h"
-namespace wolf {
-
-FeatureForceTorquePreint::FeatureForceTorquePreint(const Eigen::VectorXd& _delta_preintegrated,
- const Eigen::MatrixXd& _delta_preintegrated_covariance,
- const Eigen::Vector6d& _biases_preint,
- const Eigen::Matrix<double,12,6>& _J_delta_biases) :
- FeatureBase("FeatureForceTorquePreint", _delta_preintegrated, _delta_preintegrated_covariance),
- pbc_bias_preint_(_biases_preint.head<3>()),
- gyro_bias_preint_(_biases_preint.tail<3>()),
- J_delta_bias_(_J_delta_biases)
+namespace wolf
+{
+FeatureForceTorquePreint::FeatureForceTorquePreint(const Eigen::VectorXd& _delta_preintegrated,
+ const Eigen::MatrixXd& _delta_preintegrated_covariance,
+ const Eigen::Vector6d& _biases_preint,
+ const Eigen::Matrix<double, 12, 6>& _J_delta_biases)
+ : FeatureBase("FeatureForceTorquePreint", _delta_preintegrated, _delta_preintegrated_covariance),
+ pbc_bias_preint_(_biases_preint.head<3>()),
+ gyro_bias_preint_(_biases_preint.tail<3>()),
+ J_delta_bias_(_J_delta_biases)
{
}
-} // namespace wolf
+} // namespace wolf
diff --git a/src/feature/feature_inertial_kinematics.cpp b/src/feature/feature_inertial_kinematics.cpp
index e50b4a4..3495009 100644
--- a/src/feature/feature_inertial_kinematics.cpp
+++ b/src/feature/feature_inertial_kinematics.cpp
@@ -22,19 +22,17 @@
#include "Eigen/Dense"
#include "core/math/rotations.h"
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(FeatureInertialKinematics);
-FeatureInertialKinematics::FeatureInertialKinematics(
- const Eigen::Vector9d & _meas_pbc_vbc_w,
- const Eigen::Matrix9d & _Qerr,
- const Eigen::Matrix3d & _BIq,
- const Eigen::Vector3d & _BLcm,
- UncertaintyType _uncertainty_type) :
- FeatureBase("InertialKinematics", _meas_pbc_vbc_w, _Qerr, _uncertainty_type),
- BIq_(_BIq),
- BLcm_(_BLcm)
-{}
+FeatureInertialKinematics::FeatureInertialKinematics(const Eigen::Vector9d& _meas_pbc_vbc_w,
+ const Eigen::Matrix9d& _Qerr,
+ const Eigen::Matrix3d& _BIq,
+ const Eigen::Vector3d& _BLcm,
+ UncertaintyType _uncertainty_type)
+ : FeatureBase("InertialKinematics", _meas_pbc_vbc_w, _Qerr, _uncertainty_type), BIq_(_BIq), BLcm_(_BLcm)
+{
+}
-} // namespace wolf
+} // namespace wolf
diff --git a/src/processor/processor_force_torque_preint.cpp b/src/processor/processor_force_torque_preint.cpp
index e52af86..900182d 100644
--- a/src/processor/processor_force_torque_preint.cpp
+++ b/src/processor/processor_force_torque_preint.cpp
@@ -25,24 +25,34 @@
#include "bodydynamics/processor/processor_force_torque_preint.h"
#include "bodydynamics/factor/factor_force_torque_preint.h"
-namespace wolf {
-
-int compute_data_size(int nbc, int dimc){
+namespace wolf
+{
+int compute_data_size(int nbc, int dimc)
+{
// compute the size of the data/body vectors used by processorMotion API depending
// on the number of contacts (nbc) and contact dimension (dimc)
- // result: forces (+torques for 6D contacts) + pbc + wb + contact/base positions + contact/base orientations
- return nbc*dimc + 3 + 3 + nbc*3 + nbc*4;
+ // result: forces (+torques for 6D contacts) + pbc + wb + contact/base positions + contact/base orientations
+ return nbc * dimc + 3 + 3 + nbc * 3 + nbc * 4;
}
using namespace Eigen;
-ProcessorForceTorquePreint::ProcessorForceTorquePreint(ParamsProcessorForceTorquePreintPtr _params_motion_force_torque_preint) :
- ProcessorMotion("ProcessorForceTorquePreint", "CDLO", 3, 13, 13, 12,
- compute_data_size(_params_motion_force_torque_preint->nbc, _params_motion_force_torque_preint->dimc),
- 6, _params_motion_force_torque_preint),
- params_motion_force_torque_preint_(std::make_shared<ParamsProcessorForceTorquePreint>(*_params_motion_force_torque_preint)),
- nbc_(_params_motion_force_torque_preint->nbc),
- dimc_(_params_motion_force_torque_preint->dimc)
+ProcessorForceTorquePreint::ProcessorForceTorquePreint(
+ ParamsProcessorForceTorquePreintPtr _params_motion_force_torque_preint)
+ : ProcessorMotion(
+ "ProcessorForceTorquePreint",
+ "CDLO",
+ 3,
+ 13,
+ 13,
+ 12,
+ compute_data_size(_params_motion_force_torque_preint->nbc, _params_motion_force_torque_preint->dimc),
+ 6,
+ _params_motion_force_torque_preint),
+ params_motion_force_torque_preint_(
+ std::make_shared<ParamsProcessorForceTorquePreint>(*_params_motion_force_torque_preint)),
+ nbc_(_params_motion_force_torque_preint->nbc),
+ dimc_(_params_motion_force_torque_preint->dimc)
{
//
@@ -58,60 +68,59 @@ bool ProcessorForceTorquePreint::voteForKeyFrame() const
// time span
if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_motion_force_torque_preint_->max_time_span)
{
- WOLF_DEBUG( "PM: vote: time span" );
+ WOLF_DEBUG("PM: vote: time span");
return true;
}
// buffer length
if (getBuffer().size() > params_motion_force_torque_preint_->max_buff_length)
{
- WOLF_DEBUG( "PM: vote: buffer length" );
+ WOLF_DEBUG("PM: vote: buffer length");
return true;
}
-
+
// Some other measure of movement?
- //WOLF_DEBUG( "PM: do not vote" );
+ // WOLF_DEBUG( "PM: do not vote" );
return false;
}
-
-CaptureMotionPtr ProcessorForceTorquePreint::emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _calib,
- const VectorXd& _calib_preint,
- const CaptureBasePtr& _capture_origin)
+CaptureMotionPtr ProcessorForceTorquePreint::emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin)
{
-
// Here we have to retrieve the origin capture no
// !! PROBLEM:
// when doing setOrigin, emplaceCapture is called 2 times
// - first on the first KF (usually created by setPrior), this one contains the biases
// - secondly on the inner frame (last) which does not contains other captures
- auto capture_ikin = _frame_own->getCaptureOf(sensor_ikin_);
+ auto capture_ikin = _frame_own->getCaptureOf(sensor_ikin_);
auto capture_angvel = _frame_own->getCaptureOf(sensor_angvel_);
- if ((capture_ikin == nullptr) || (capture_angvel == nullptr)){
+ if ((capture_ikin == nullptr) || (capture_angvel == nullptr))
+ {
// We have to retrieve the bias from a former Keyframe: origin
- capture_ikin = origin_ptr_->getFrame()->getCaptureOf(sensor_ikin_);
- capture_angvel = origin_ptr_->getFrame()->getCaptureOf(sensor_angvel_);
+ capture_ikin = origin_ptr_->getFrame()->getCaptureOf(sensor_ikin_);
+ capture_angvel = origin_ptr_->getFrame()->getCaptureOf(sensor_angvel_);
}
auto cap = CaptureBase::emplace<CaptureForceTorquePreint>(
- _frame_own,
- _ts,
- _sensor,
- std::static_pointer_cast<CaptureInertialKinematics>(capture_ikin),
- std::static_pointer_cast<CaptureMotion>(capture_angvel),
- _data,
- _data_cov,
- _capture_origin);
+ _frame_own,
+ _ts,
+ _sensor,
+ std::static_pointer_cast<CaptureInertialKinematics>(capture_ikin),
+ std::static_pointer_cast<CaptureMotion>(capture_angvel),
+ _data,
+ _data_cov,
+ _capture_origin);
// Cannot be recovered from the _calib and _calib_preint which are initialized using calib_size_
// which is zero in our case since the calibration stateblocks are not actually in the FTPreint sensor/captures
- auto cap_ftpreint = std::static_pointer_cast<CaptureForceTorquePreint>(cap);
- Vector6d calib = getCalibration(cap_ftpreint);
+ auto cap_ftpreint = std::static_pointer_cast<CaptureForceTorquePreint>(cap);
+ Vector6d calib = getCalibration(cap_ftpreint);
setCalibration(cap_ftpreint, calib);
cap_ftpreint->setCalibrationPreint(calib);
@@ -121,26 +130,26 @@ CaptureMotionPtr ProcessorForceTorquePreint::emplaceCapture(const FrameBasePtr&
FeatureBasePtr ProcessorForceTorquePreint::emplaceFeature(CaptureMotionPtr _capture_motion)
{
// Retrieving the origin capture and getting the bias from here should be done here?
- auto feature = FeatureBase::emplace<FeatureForceTorquePreint>(_capture_motion,
- _capture_motion->getBuffer().back().delta_integr_,
- _capture_motion->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
- _capture_motion->getCalibrationPreint(),
- _capture_motion->getBuffer().back().jacobian_calib_);
+ auto feature = FeatureBase::emplace<FeatureForceTorquePreint>(
+ _capture_motion,
+ _capture_motion->getBuffer().back().delta_integr_,
+ _capture_motion->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
+ _capture_motion->getCalibrationPreint(),
+ _capture_motion->getBuffer().back().jacobian_calib_);
return feature;
}
-Eigen::VectorXd ProcessorForceTorquePreint::correctDelta (const Eigen::VectorXd& delta_preint,
- const Eigen::VectorXd& delta_step) const
+Eigen::VectorXd ProcessorForceTorquePreint::correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const
{
return bodydynamics::plus(delta_preint, delta_step);
}
-VectorXd ProcessorForceTorquePreint::getCalibration (const CaptureBaseConstPtr _capture) const
+VectorXd ProcessorForceTorquePreint::getCalibration(const CaptureBaseConstPtr _capture) const
{
-
VectorXd bias_vec(6);
- if (_capture) // access from capture is quicker
+ if (_capture) // access from capture is quicker
{
auto cap_ft(std::static_pointer_cast<const CaptureForceTorquePreint>(_capture));
@@ -148,20 +157,27 @@ VectorXd ProcessorForceTorquePreint::getCalibration (const CaptureBaseConstPtr _
bias_vec.segment<3>(0) = cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->getState();
// get calib part from IMU capture
- bias_vec.segment<3>(3) = cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->getState().tail<3>(); // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias]
+ bias_vec.segment<3>(3) =
+ cap_ft->getGyroCaptureOther()
+ ->getSensorIntrinsic()
+ ->getState()
+ .tail<3>(); // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias]
}
- else // access from sensor is slower
+ else // access from sensor is slower
{
// get calib part from Ikin capture
bias_vec.segment<3>(0) = sensor_ikin_->getIntrinsic()->getState();
// get calib part from IMU capture
- bias_vec.segment<3>(3) = sensor_angvel_->getIntrinsic()->getState().tail<3>(); // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias]
+ bias_vec.segment<3>(3) =
+ sensor_angvel_->getIntrinsic()
+ ->getState()
+ .tail<3>(); // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias]
}
return bias_vec;
}
-void ProcessorForceTorquePreint::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
+void ProcessorForceTorquePreint::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration)
{
CaptureForceTorquePreintPtr cap_ft(std::static_pointer_cast<CaptureForceTorquePreint>(_capture));
@@ -177,48 +193,54 @@ void ProcessorForceTorquePreint::setCalibration (const CaptureBasePtr _capture,
FactorBasePtr ProcessorForceTorquePreint::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
{
- CaptureForceTorquePreintPtr cap_ftpreint_origin = std::static_pointer_cast<CaptureForceTorquePreint>(_capture_origin);
+ CaptureForceTorquePreintPtr cap_ftpreint_origin =
+ std::static_pointer_cast<CaptureForceTorquePreint>(_capture_origin);
FeatureForceTorquePreintPtr ftr_ftpreint = std::static_pointer_cast<FeatureForceTorquePreint>(_feature_motion);
- CaptureForceTorquePreintPtr cap_ftpreint_new = std::static_pointer_cast<CaptureForceTorquePreint>(ftr_ftpreint->getCapture());
-
- auto fac_ftpreint = FactorBase::emplace<FactorForceTorquePreint>(
- ftr_ftpreint,
- ftr_ftpreint,
- cap_ftpreint_origin,
- shared_from_this(),
- cap_ftpreint_origin->getIkinCaptureOther(), // to retrieve sb_bp1
- cap_ftpreint_origin->getGyroCaptureOther(), // to retrieve sb_w1
- false);
+ CaptureForceTorquePreintPtr cap_ftpreint_new =
+ std::static_pointer_cast<CaptureForceTorquePreint>(ftr_ftpreint->getCapture());
+
+ auto fac_ftpreint =
+ FactorBase::emplace<FactorForceTorquePreint>(ftr_ftpreint,
+ ftr_ftpreint,
+ cap_ftpreint_origin,
+ shared_from_this(),
+ cap_ftpreint_origin->getIkinCaptureOther(), // to retrieve sb_bp1
+ cap_ftpreint_origin->getGyroCaptureOther(), // to retrieve sb_w1
+ false);
return fac_ftpreint;
}
-void ProcessorForceTorquePreint::computeCurrentDelta(
- const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- const Eigen::VectorXd& _calib,
- const double _dt,
- Eigen::VectorXd& _delta,
- Eigen::MatrixXd& _delta_cov,
- Eigen::MatrixXd& _jac_delta_calib) const
+void ProcessorForceTorquePreint::computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jac_delta_calib) const
{
assert(_data.size() == data_size_ && "Wrong data size!");
// create delta
- MatrixXd jac_body_bias(data_size_-nbc_,6);
+ MatrixXd jac_body_bias(data_size_ - nbc_, 6);
VectorXd body(data_size_);
bodydynamics::debiasData(_data, _calib, nbc_, dimc_, body, jac_body_bias);
- MatrixXd jac_delta_body(12,data_size_-nbc_);
- bodydynamics::body2delta(body, _dt, std::static_pointer_cast<const SensorForceTorque>(getSensor())->getMass(),
- nbc_, dimc_,
- _delta, jac_delta_body); // Jacobians tested in bodydynamics_tools
+ MatrixXd jac_delta_body(12, data_size_ - nbc_);
+ bodydynamics::body2delta(body,
+ _dt,
+ std::static_pointer_cast<const SensorForceTorque>(getSensor())->getMass(),
+ nbc_,
+ dimc_,
+ _delta,
+ jac_delta_body); // Jacobians tested in bodydynamics_tools
// [f], [tau], pbc, wb
- MatrixXd jac_delta_body_reduced = jac_delta_body.leftCols(nbc_*dimc_+6);
+ MatrixXd jac_delta_body_reduced = jac_delta_body.leftCols(nbc_ * dimc_ + 6);
// compute delta_cov (using uncertainty on f,tau,pbc)
- _delta_cov = jac_delta_body_reduced * _data_cov * jac_delta_body_reduced.transpose(); // trivially: jac_body_delta = Identity
+ _delta_cov = jac_delta_body_reduced * _data_cov *
+ jac_delta_body_reduced.transpose(); // trivially: jac_body_delta = Identity
// _delta_cov = jac_delta_body * _data_cov * jac_delta_body.transpose(); // trivially: jac_body_delta = Identity
// compute jacobian_calib
@@ -226,17 +248,17 @@ void ProcessorForceTorquePreint::computeCurrentDelta(
}
void ProcessorForceTorquePreint::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
- const Eigen::VectorXd& _delta,
- const double _dt,
- Eigen::VectorXd& _delta_preint_plus_delta) const
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta) const
{
_delta_preint_plus_delta = bodydynamics::compose(_delta_preint, _delta, _dt);
}
void ProcessorForceTorquePreint::statePlusDelta(const VectorComposite& _x,
- const Eigen::VectorXd& _delta,
- const double _dt,
- VectorComposite& _x_plus_delta) const
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ VectorComposite& _x_plus_delta) const
{
assert(_delta.size() == 13 && "Wrong _delta vector size");
assert(_dt >= 0 && "Time interval _dt is negative!");
@@ -247,20 +269,25 @@ void ProcessorForceTorquePreint::statePlusDelta(const VectorComposite& _x,
VectorXd x_plus_delta = bodydynamics::composeOverState(x, _delta, _dt);
- _x_plus_delta = VectorComposite(x_plus_delta, "CDLO", {3,3,3,4});
+ _x_plus_delta = VectorComposite(x_plus_delta, "CDLO", {3, 3, 3, 4});
}
void ProcessorForceTorquePreint::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
- const Eigen::VectorXd& _delta,
- const double _dt,
- Eigen::VectorXd& _delta_preint_plus_delta,
- Eigen::MatrixXd& _jacobian_delta_preint,
- Eigen::MatrixXd& _jacobian_delta) const
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta,
+ Eigen::MatrixXd& _jacobian_delta_preint,
+ Eigen::MatrixXd& _jacobian_delta) const
{
- bodydynamics::compose(_delta_preint, _delta, _dt, _delta_preint_plus_delta, _jacobian_delta_preint, _jacobian_delta); // jacobians tested in bodydynamics_tools
+ bodydynamics::compose(_delta_preint,
+ _delta,
+ _dt,
+ _delta_preint_plus_delta,
+ _jacobian_delta_preint,
+ _jacobian_delta); // jacobians tested in bodydynamics_tools
}
-} // namespace wolf
+} // namespace wolf
// Register in the FactorySensor
#include "core/processor/factory_processor.h"
diff --git a/src/processor/processor_inertial_kinematics.cpp b/src/processor/processor_inertial_kinematics.cpp
index 513575d..b90e6fb 100644
--- a/src/processor/processor_inertial_kinematics.cpp
+++ b/src/processor/processor_inertial_kinematics.cpp
@@ -20,21 +20,16 @@
#include "bodydynamics/processor/processor_inertial_kinematics.h"
-
-namespace wolf{
-
-
-inline ProcessorInertialKinematics::ProcessorInertialKinematics(ParamsProcessorInertialKinematicsPtr _params_ikin) :
- ProcessorBase("ProcessorInertialKinematics", 3, _params_ikin),
- params_ikin_(std::make_shared<ParamsProcessorInertialKinematics>(*_params_ikin)),
- cap_origin_ptr_(nullptr)
+namespace wolf
{
-}
-
-void ProcessorInertialKinematics::configure(SensorBasePtr _sensor)
+inline ProcessorInertialKinematics::ProcessorInertialKinematics(ParamsProcessorInertialKinematicsPtr _params_ikin)
+ : ProcessorBase("ProcessorInertialKinematics", 3, _params_ikin),
+ params_ikin_(std::make_shared<ParamsProcessorInertialKinematics>(*_params_ikin)),
+ cap_origin_ptr_(nullptr)
{
}
+void ProcessorInertialKinematics::configure(SensorBasePtr _sensor) {}
inline void ProcessorInertialKinematics::processCapture(CaptureBasePtr _capture)
{
@@ -45,12 +40,14 @@ inline void ProcessorInertialKinematics::processCapture(CaptureBasePtr _capture)
}
// nothing to do if any of the two buffer is empty
- if(buffer_frame_.empty()){
- WOLF_DEBUG("PInertialKinematic: KF pack buffer empty, time ", _capture->getTimeStamp());
+ if (buffer_frame_.empty())
+ {
+ WOLF_DEBUG("PInertialKinematic: KF pack buffer empty, time ", _capture->getTimeStamp());
return;
}
- if(buffer_frame_.empty()){
- WOLF_DEBUG("PInertialKinematics: Capture buffer empty, time ", _capture->getTimeStamp());
+ if (buffer_frame_.empty())
+ {
+ WOLF_DEBUG("PInertialKinematics: Capture buffer empty, time ", _capture->getTimeStamp());
return;
}
@@ -60,32 +57,33 @@ inline void ProcessorInertialKinematics::processCapture(CaptureBasePtr _capture)
// done AFTER processCapture)
// 1. get corresponding KF
- auto buffer_frame_it = buffer_frame_.getContainer().begin();
+ auto buffer_frame_it = buffer_frame_.getContainer().begin();
auto buffer_capture_it = buffer_capture_.getContainer().begin();
auto sensor_angvel = getProblem()->findSensor(params_ikin_->sensor_angvel_name);
- while ((buffer_frame_it != buffer_frame_.getContainer().end())
- && (buffer_capture_it != buffer_capture_.getContainer().end()))
+ while ((buffer_frame_it != buffer_frame_.getContainer().end()) &&
+ (buffer_capture_it != buffer_capture_.getContainer().end()))
{
-
bool time_ok = checkTimeTolerance(buffer_frame_it->first, buffer_capture_it->first);
- if (time_ok) {
+ 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;
- if (cap_angvel && cap_angvel->getStateBlock('I')){ // TODO: or only cap_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_frame_it->second;
+ 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,
- cap_angvel,
- cap_origin_ptr_);
+ createInertialKinematicsFactor(cap_ikin, cap_angvel, cap_origin_ptr_);
// 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_frame_it++;
}
- else {
+ 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++;
@@ -97,61 +95,69 @@ inline void ProcessorInertialKinematics::processCapture(CaptureBasePtr _capture)
buffer_frame_.removeUpTo(min_ts);
buffer_capture_.removeUpTo(min_ts);
}
- else {
+ else
+ {
// if a time difference between captures and KF pack, we increment the oldest iterator
- if (buffer_capture_it->first < buffer_frame_it->first){
+ if (buffer_capture_it->first < buffer_frame_it->first)
+ {
buffer_capture_it++;
}
- else {
+ else
+ {
buffer_frame_it++;
}
}
}
}
-
-inline bool ProcessorInertialKinematics::createInertialKinematicsFactor(CaptureInertialKinematicsPtr _cap_ikin, CaptureBasePtr _cap_angvel, CaptureBasePtr _cap_ikin_origin)
+inline bool ProcessorInertialKinematics::createInertialKinematicsFactor(CaptureInertialKinematicsPtr _cap_ikin,
+ CaptureBasePtr _cap_angvel,
+ CaptureBasePtr _cap_ikin_origin)
{
// 3. explore all observations in the capture and create feature
- Eigen::Vector9d data = _cap_ikin->getData();
- Eigen::Matrix3d B_I_q = _cap_ikin->getBIq();
- Eigen::Vector3d B_Lc_m = _cap_ikin->getBLcm();
- auto sensor_ikin = std::static_pointer_cast<SensorInertialKinematics>(getSensor());
- auto sensor_angvel_base = getProblem()->findSensor(params_ikin_->sensor_angvel_name);
- Eigen::Matrix3d Qp = pow(sensor_ikin->getPbcNoiseStd(), 2) * Eigen::Matrix3d::Identity();
- Eigen::Matrix3d Qv = pow(sensor_ikin->getVbcNoiseStd(), 2) * Eigen::Matrix3d::Identity();
- Eigen::Matrix3d Qw = sensor_angvel_base->getNoiseCov().bottomRightCorner<3,3>();
- Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp, Qv, Qw,
- data.head<3>() - _cap_ikin->getSensorIntrinsic()->getState(), // bias value of the capture that was just created
- data.tail<3>() - _cap_angvel->getSensorIntrinsic()->getState().tail<3>(),
- B_I_q);
+ Eigen::Vector9d data = _cap_ikin->getData();
+ Eigen::Matrix3d B_I_q = _cap_ikin->getBIq();
+ Eigen::Vector3d B_Lc_m = _cap_ikin->getBLcm();
+ auto sensor_ikin = std::static_pointer_cast<SensorInertialKinematics>(getSensor());
+ auto sensor_angvel_base = getProblem()->findSensor(params_ikin_->sensor_angvel_name);
+ Eigen::Matrix3d Qp = pow(sensor_ikin->getPbcNoiseStd(), 2) * Eigen::Matrix3d::Identity();
+ Eigen::Matrix3d Qv = pow(sensor_ikin->getVbcNoiseStd(), 2) * Eigen::Matrix3d::Identity();
+ Eigen::Matrix3d Qw = sensor_angvel_base->getNoiseCov().bottomRightCorner<3, 3>();
+ Eigen::Matrix9d Q_ikin_err = computeIKinCov(
+ Qp,
+ Qv,
+ Qw,
+ data.head<3>() -
+ _cap_ikin->getSensorIntrinsic()->getState(), // bias value of the capture that was just created
+ data.tail<3>() - _cap_angvel->getSensorIntrinsic()->getState().tail<3>(),
+ B_I_q);
auto feat_ikin = FeatureBase::emplace<FeatureInertialKinematics>(_cap_ikin, data, Q_ikin_err, B_I_q, B_Lc_m);
// 4. Create inertial kinematics factor
- auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_ikin,
- feat_ikin,
- _cap_angvel->getSensorIntrinsic(),
- shared_from_this(),
- false);
-
- // 5. Create bias drift factor if an origin capture exists -> zero difference + covariance as integration of a Brownian noise
- if (_cap_ikin_origin){
- double dt_drift = _cap_ikin->getTimeStamp() - _cap_ikin_origin->getTimeStamp();
+ auto fac = FactorBase::emplace<FactorInertialKinematics>(
+ feat_ikin, feat_ikin, _cap_angvel->getSensorIntrinsic(), shared_from_this(), false);
+
+ // 5. Create bias drift factor if an origin capture exists -> zero difference + covariance as integration of a
+ // Brownian noise
+ if (_cap_ikin_origin)
+ {
+ double dt_drift = _cap_ikin->getTimeStamp() - _cap_ikin_origin->getTimeStamp();
Eigen::Matrix3d cov_drift = Eigen::Matrix3d::Identity() * pow(params_ikin_->std_bp_drift, 2) * dt_drift;
- FeatureBasePtr feat_diff = FeatureBase::emplace<FeatureBase>(_cap_ikin, "ComBiasDrift", Eigen::Vector3d::Zero(), cov_drift,
- FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
- StateBlockPtr sb_pbc1 = _cap_ikin_origin->getSensorIntrinsic();
- StateBlockPtr sb_pbc2 = _cap_ikin->getSensorIntrinsic();
- FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
- feat_diff, feat_diff, sb_pbc1, sb_pbc2, nullptr, _cap_ikin_origin
- );
+ FeatureBasePtr feat_diff =
+ FeatureBase::emplace<FeatureBase>(_cap_ikin,
+ "ComBiasDrift",
+ Eigen::Vector3d::Zero(),
+ cov_drift,
+ FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ StateBlockPtr sb_pbc1 = _cap_ikin_origin->getSensorIntrinsic();
+ StateBlockPtr sb_pbc2 = _cap_ikin->getSensorIntrinsic();
+ FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
+ feat_diff, feat_diff, sb_pbc1, sb_pbc2, nullptr, _cap_ikin_origin);
}
return true;
}
-
-
//////////////////////////
// Covariance computations
//////////////////////////
@@ -160,25 +166,25 @@ Eigen::Matrix9d computeIKinJac(const Eigen::Vector3d& b_p_bc_unbiased,
const Eigen::Vector3d& b_omg_b_unbiased,
const Eigen::Matrix3d& Iq)
{
- Eigen::Matrix9d J; J.setZero();
+ Eigen::Matrix9d J;
+ J.setZero();
Eigen::Matrix3d wx = skew(b_omg_b_unbiased);
Eigen::Matrix3d px = skew(b_p_bc_unbiased);
// Starting from top left, to the right and then next row
- J.topLeftCorner<3,3>() = Eigen::Matrix3d::Identity();
+ J.topLeftCorner<3, 3>() = Eigen::Matrix3d::Identity();
// J.block<3,3>(0,3) = Matrix3d::Zero();
// J.topRightCorner<3,3>() = Matrix3d::Zero();
- J.block<3,3>(3,0) = - wx;
- J.block<3,3>(3,3) = - Eigen::Matrix3d::Identity();
- J.block<3,3>(3,6) = px;
+ J.block<3, 3>(3, 0) = -wx;
+ J.block<3, 3>(3, 3) = -Eigen::Matrix3d::Identity();
+ J.block<3, 3>(3, 6) = px;
// J.bottomLeftCorner<3,3>() = Matrix3d::Zero();
// J.block<3,3>(6,3) = Matrix3d::Zero();
- J.bottomRightCorner<3,3>() = -Iq;
+ J.bottomRightCorner<3, 3>() = -Iq;
return J;
}
-
Eigen::Matrix9d computeIKinCov(const Eigen::Matrix3d& Qp,
const Eigen::Matrix3d& Qv,
const Eigen::Matrix3d& Qw,
@@ -186,20 +192,21 @@ Eigen::Matrix9d computeIKinCov(const Eigen::Matrix3d& Qp,
const Eigen::Vector3d& b_omg_b_unbiased,
const Eigen::Matrix3d& Iq)
{
- Eigen::Matrix9d cov; cov.setZero();
+ Eigen::Matrix9d cov;
+ cov.setZero();
Eigen::Matrix3d px = skew(b_p_bc_unbiased);
Eigen::Matrix3d wx = skew(b_omg_b_unbiased);
// Starting from top left, to the right and then next row
- cov.topLeftCorner<3,3>() = Qp;
- cov.block<3,3>(0,3) = Qp * wx;
+ cov.topLeftCorner<3, 3>() = Qp;
+ cov.block<3, 3>(0, 3) = Qp * wx;
// cov.topRightCorner<3,3>() = Matrix3d::Zero();
- cov.block<3,3>(3,0) = cov.block<3,3>(0,3).transpose();
- cov.block<3,3>(3,3) = -wx*Qp*wx + Qv - px*Qw*px;
- cov.block<3,3>(3,6) = -px*Qw*Iq;
+ cov.block<3, 3>(3, 0) = cov.block<3, 3>(0, 3).transpose();
+ cov.block<3, 3>(3, 3) = -wx * Qp * wx + Qv - px * Qw * px;
+ cov.block<3, 3>(3, 6) = -px * Qw * Iq;
// cov.bottomLeftCorner<3,3>() = Matrix3d::Zero();
- cov.block<3,3>(6,3) = Iq*Qw*px;
- cov.bottomRightCorner<3,3>() = Iq*Qw*Iq;
+ cov.block<3, 3>(6, 3) = Iq * Qw * px;
+ cov.bottomRightCorner<3, 3>() = Iq * Qw * Iq;
return cov;
}
@@ -208,7 +215,8 @@ Eigen::Matrix9d computeIKinCov(const Eigen::Matrix3d& Qp,
// Register in the FactoryProcessor
#include "core/processor/factory_processor.h"
-namespace wolf {
+namespace wolf
+{
WOLF_REGISTER_PROCESSOR(ProcessorInertialKinematics);
WOLF_REGISTER_PROCESSOR_AUTO(ProcessorInertialKinematics);
-} // namespace wolf
+} // namespace wolf
diff --git a/src/processor/processor_point_feet_nomove.cpp b/src/processor/processor_point_feet_nomove.cpp
index 110ea42..e0cc3e8 100644
--- a/src/processor/processor_point_feet_nomove.cpp
+++ b/src/processor/processor_point_feet_nomove.cpp
@@ -20,28 +20,22 @@
#include "bodydynamics/processor/processor_point_feet_nomove.h"
-
-namespace wolf{
-
-
-inline ProcessorPointFeetNomove::ProcessorPointFeetNomove(ParamsProcessorPointFeetNomovePtr _params_pfnomove) :
- ProcessorBase("ProcessorPointFeetNomove", 3, _params_pfnomove),
- params_pfnomove_(std::make_shared<ParamsProcessorPointFeetNomove>(*_params_pfnomove)),
- origin_(nullptr)
+namespace wolf
+{
+inline ProcessorPointFeetNomove::ProcessorPointFeetNomove(ParamsProcessorPointFeetNomovePtr _params_pfnomove)
+ : ProcessorBase("ProcessorPointFeetNomove", 3, _params_pfnomove),
+ params_pfnomove_(std::make_shared<ParamsProcessorPointFeetNomove>(*_params_pfnomove)),
+ origin_(nullptr)
{
// Init tracks to nothing
-
- for (int i=0; i < 4; i++){
+
+ for (int i = 0; i < 4; i++)
+ {
tracks_[0] = nullptr;
}
-
-}
-
-void ProcessorPointFeetNomove::configure(SensorBasePtr _sensor)
-{
}
-
+void ProcessorPointFeetNomove::configure(SensorBasePtr _sensor) {}
inline void ProcessorPointFeetNomove::processCapture(CaptureBasePtr _capture)
{
@@ -53,18 +47,19 @@ inline void ProcessorPointFeetNomove::processCapture(CaptureBasePtr _capture)
incoming_ = _capture;
// nothing to do if any of the two buffer is empty
- if(buffer_frame_.empty()){
- WOLF_DEBUG("PInertialKinematic: KF pack buffer empty, time ", _capture->getTimeStamp());
+ if (buffer_frame_.empty())
+ {
+ WOLF_DEBUG("PInertialKinematic: KF pack buffer empty, time ", _capture->getTimeStamp());
return;
}
- if(buffer_frame_.empty()){
- WOLF_DEBUG("PInertialKinematics: Capture buffer empty, time ", _capture->getTimeStamp());
+ if (buffer_frame_.empty())
+ {
+ WOLF_DEBUG("PInertialKinematics: Capture buffer empty, time ", _capture->getTimeStamp());
return;
}
createFactorConsecutive();
// processCaptureCreateFactorFar(_capture);
-
}
inline void ProcessorPointFeetNomove::processKeyFrame(FrameBasePtr _keyframe_ptr)
@@ -75,75 +70,93 @@ inline void ProcessorPointFeetNomove::processKeyFrame(FrameBasePtr _keyframe_ptr
return;
}
// nothing to do if any of the two buffer is empty
- if(buffer_frame_.empty()){
- WOLF_DEBUG("ProcessorPointFeetNomove: KF pack buffer empty, time ", _keyframe_ptr->getTimeStamp());
+ if (buffer_frame_.empty())
+ {
+ WOLF_DEBUG("ProcessorPointFeetNomove: KF pack buffer empty, time ", _keyframe_ptr->getTimeStamp());
return;
}
- if(buffer_frame_.empty()){
- WOLF_DEBUG("ProcessorPointFeetNomove: Capture buffer empty, time ", _keyframe_ptr->getTimeStamp());
+ if (buffer_frame_.empty())
+ {
+ WOLF_DEBUG("ProcessorPointFeetNomove: Capture buffer empty, time ", _keyframe_ptr->getTimeStamp());
return;
}
createFactorConsecutive();
// processKeyFrameCreateFactorFar();
-
}
-void ProcessorPointFeetNomove::processCaptureCreateFactorFar(CaptureBasePtr _capture){
+void ProcessorPointFeetNomove::processCaptureCreateFactorFar(CaptureBasePtr _capture)
+{
// Far away factors
- auto cap_curr = std::static_pointer_cast<CapturePointFeetNomove>(_capture);
+ auto cap_curr = std::static_pointer_cast<CapturePointFeetNomove>(_capture);
auto kin_incontact_current = cap_curr->kin_incontact_;
- for (int i=0; i<4; i++){
- if (!kin_incontact_current.count(i)){
+ for (int i = 0; i < 4; i++)
+ {
+ if (!kin_incontact_current.count(i))
+ {
tracks_[i] = nullptr;
}
}
}
-void ProcessorPointFeetNomove::processKeyFrameCreateFactorFar(){
-
+void ProcessorPointFeetNomove::processKeyFrameCreateFactorFar()
+{
auto sensor_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(getSensor());
- for (auto kf_it: buffer_frame_.getContainer()){
- TimeStamp ts = kf_it.first;
- auto kf = kf_it.second;
- auto cap_it = buffer_capture_.selectIterator(ts, params_->time_tolerance);
+ for (auto kf_it : buffer_frame_.getContainer())
+ {
+ TimeStamp ts = kf_it.first;
+ auto kf = kf_it.second;
+ auto cap_it = buffer_capture_.selectIterator(ts, params_->time_tolerance);
// We have a match between the KF and a capture from the capture buffer
- if (cap_it != buffer_capture_.getContainer().end()){
+ if (cap_it != buffer_capture_.getContainer().end())
+ {
auto cap_curr = std::static_pointer_cast<CapturePointFeetNomove>(cap_it->second);
-
- for (auto kin: cap_curr->kin_incontact_){
+
+ for (auto kin : cap_curr->kin_incontact_)
+ {
// check if each leg in contact has a current active track
- int leg_id = kin.first;
+ int leg_id = kin.first;
auto cap_origin = tracks_[leg_id];
- if (!cap_origin){
+ if (!cap_origin)
+ {
// if not, define current KF as the leg track origin and link the capture to this KF
- // Error occurs if we try to link several time the same Cap->kf due to the loop on legs -> do only once
- if (!cap_curr->getFrame()){
+ // Error occurs if we try to link several time the same Cap->kf due to the loop on legs -> do only
+ // once
+ if (!cap_curr->getFrame())
+ {
cap_curr->link(kf);
}
tracks_[leg_id] = cap_curr;
}
- else {
- if (!cap_curr->getFrame()){
+ else
+ {
+ if (!cap_curr->getFrame())
+ {
cap_curr->link(kf);
}
// if it has, create a factor
auto cap_origin = std::static_pointer_cast<CapturePointFeetNomove>(tracks_[leg_id]);
- Matrix<double, 14, 1> meas; meas << cap_origin->kin_incontact_[leg_id], kin.second;
- double dt_kf = cap_curr->getTimeStamp() - cap_origin->getTimeStamp();
- Matrix3d cov_int = dt_kf*sensor_pfnm->getCovFootNomove(); // integrate zero velocity cov during dt_kf
+ Matrix<double, 14, 1> meas;
+ meas << cap_origin->kin_incontact_[leg_id], kin.second;
+ double dt_kf = cap_curr->getTimeStamp() - cap_origin->getTimeStamp();
+ Matrix3d cov_int =
+ dt_kf * sensor_pfnm->getCovFootNomove(); // integrate zero velocity cov during dt_kf
// Matrix3d cov_int = sensor_pfnm->getCovFootNomove(); // integrate zero velocity cov during dt_kf
FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap_curr, "PointFeet", meas, cov_int);
- FactorPointFeetNomovePtr fac = FactorBase::emplace<FactorPointFeetNomove>(feat, feat, cap_origin->getFrame(), nullptr, true);
+ FactorPointFeetNomovePtr fac =
+ FactorBase::emplace<FactorPointFeetNomove>(feat, feat, cap_origin->getFrame(), nullptr, true);
// Zero Altitude factor
- Vector4d meas_altitude; meas_altitude << kin.second.head<3>(), 0;
- Matrix1d alt_cov = pow(0.01, 2)*Matrix1d::Identity();
+ Vector4d meas_altitude;
+ meas_altitude << kin.second.head<3>(), 0;
+ Matrix1d alt_cov = pow(0.01, 2) * Matrix1d::Identity();
CaptureBasePtr cap_alt = CaptureBase::emplace<CaptureBase>(kf, "Alt0", cap_curr->getTimeStamp());
- FeatureBasePtr feat_alt = FeatureBase::emplace<FeatureBase>(cap_alt, "Alt0", meas_altitude, alt_cov);
- FactorBasePtr fac_alt = FactorBase::emplace<FactorPointFeetAltitude>(feat_alt, feat_alt, nullptr, true);
+ FeatureBasePtr feat_alt =
+ FeatureBase::emplace<FeatureBase>(cap_alt, "Alt0", meas_altitude, alt_cov);
+ FactorBasePtr fac_alt =
+ FactorBase::emplace<FactorPointFeetAltitude>(feat_alt, feat_alt, nullptr, true);
}
}
buffer_frame_.removeUpTo(cap_curr->getTimeStamp());
@@ -152,52 +165,59 @@ void ProcessorPointFeetNomove::processKeyFrameCreateFactorFar(){
}
}
-void ProcessorPointFeetNomove::createFactorConsecutive(){
-
+void ProcessorPointFeetNomove::createFactorConsecutive()
+{
auto sensor_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(getSensor());
while (buffer_frame_.size() >= 2)
{
- 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, getTimeTolerance());
- auto cap2_it = buffer_capture_.selectIterator(t2, getTimeTolerance());
-
- // check that the first 2 KF have corresponding captures in the capture buffer
+ 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, 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
- if ( (cap1_it == buffer_capture_.getContainer().end()) or (cap2_it == buffer_capture_.getContainer().end()) )
+ if ((cap1_it == buffer_capture_.getContainer().end()) or (cap2_it == buffer_capture_.getContainer().end()))
{
return;
}
else
{
// store the initial contact/kinematic meas map
- auto cap1 = std::static_pointer_cast<CapturePointFeetNomove>(cap1_it->second);
+ auto cap1 = std::static_pointer_cast<CapturePointFeetNomove>(cap1_it->second);
auto kin_incontact_from_t1 = cap1->kin_incontact_;
// then loop over the captures in between cap1 and cap2
- auto cap_it = std::next(cap1_it);
+ auto cap_it = std::next(cap1_it);
auto it_after_capt_t2 = std::next(cap2_it); // used to detect the end of the loop
// loop throught the captures between t1 and t2
- while (cap_it != it_after_capt_t2){
- // for each new capture, filter the initial contact/kin meas map to track which feet stay in contact throughout
+ while (cap_it != it_after_capt_t2)
+ {
+ // for each new capture, filter the initial contact/kin meas map to track which feet stay in contact
+ // throughout
auto cap_curr = std::static_pointer_cast<CapturePointFeetNomove>(cap_it->second);
- if (!cap_curr){
+ if (!cap_curr)
+ {
WOLF_ERROR("Wrong capture type in buffer")
}
-
+
// check which feet are currently in contact, if one is missing wrt the previous
// timestamp, remove it from kin_incontact_from_t1
auto kin_incontact_current = cap_curr->kin_incontact_;
- for (auto elt_it = kin_incontact_from_t1.cbegin(); elt_it != kin_incontact_from_t1.cend(); /* no increment */){
- if (kin_incontact_current.find(elt_it->first) == kin_incontact_current.end()){
+ for (auto elt_it = kin_incontact_from_t1.cbegin(); elt_it != kin_incontact_from_t1.cend();
+ /* no increment */)
+ {
+ if (kin_incontact_current.find(elt_it->first) == kin_incontact_current.end())
+ {
// The foot has lost contact with the ground since t1
elt_it = kin_incontact_from_t1.erase(elt_it); // erase and jump (from stackoverflow)
}
- else{
+ else
+ {
elt_it++;
}
}
@@ -207,52 +227,57 @@ void ProcessorPointFeetNomove::createFactorConsecutive(){
if (!kin_incontact_from_t1.empty())
{
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
+ // 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);
auto kin_incontact_t2 = cap2->kin_incontact_;
- for (auto kin_pair1: kin_incontact_from_t1){
+ for (auto kin_pair1 : kin_incontact_from_t1)
+ {
auto kin_pair2_it = kin_incontact_t2.find(kin_pair1.first);
- Matrix<double, 14, 1> meas; meas << kin_pair1.second, kin_pair2_it->second;
- double dt_kf = cap2->getTimeStamp() - cap1->getTimeStamp();
- Matrix3d cov_int = dt_kf*sensor_pfnm->getCovFootNomove(); // integrate zero velocity cov during dt_kf
+ Matrix<double, 14, 1> meas;
+ meas << kin_pair1.second, kin_pair2_it->second;
+ double dt_kf = cap2->getTimeStamp() - cap1->getTimeStamp();
+ Matrix3d cov_int =
+ dt_kf * sensor_pfnm->getCovFootNomove(); // integrate zero velocity cov during dt_kf
FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap2, "PointFeet", meas, cov_int);
- FactorPointFeetNomovePtr fac = FactorBase::emplace<FactorPointFeetNomove>(feat, feat, kf1_it->second, nullptr, true);
+ FactorPointFeetNomovePtr fac =
+ FactorBase::emplace<FactorPointFeetNomove>(feat, feat, kf1_it->second, nullptr, true);
// Zero Altitude factor
- Vector4d meas_altitude; meas_altitude << kin_pair2_it->second.head<3>(), 0;
- Matrix1d alt_cov = pow(0.01, 2)*Matrix1d::Identity();
- CaptureBasePtr cap_alt = CaptureBase::emplace<CaptureBase>(kf2_it->second, "Alt0", cap2->getTimeStamp());
- FeatureBasePtr feat_alt = FeatureBase::emplace<FeatureBase>(cap_alt, "Alt0", meas_altitude, alt_cov);
- FactorBasePtr fac_alt = FactorBase::emplace<FactorPointFeetAltitude>(feat_alt, feat_alt, nullptr, true);
+ Vector4d meas_altitude;
+ meas_altitude << kin_pair2_it->second.head<3>(), 0;
+ Matrix1d alt_cov = pow(0.01, 2) * Matrix1d::Identity();
+ CaptureBasePtr cap_alt =
+ CaptureBase::emplace<CaptureBase>(kf2_it->second, "Alt0", cap2->getTimeStamp());
+ FeatureBasePtr feat_alt =
+ FeatureBase::emplace<FeatureBase>(cap_alt, "Alt0", meas_altitude, alt_cov);
+ FactorBasePtr fac_alt =
+ FactorBase::emplace<FactorPointFeetAltitude>(feat_alt, feat_alt, 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_frame_.getContainer().erase(buffer_frame_.getContainer().begin(), kf2_it); // !! works only if getContainer returns a non const reference
+ // 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_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);
}
}
}
-
-
-
inline bool ProcessorPointFeetNomove::voteForKeyFrame() const
{
return false;
}
-
-
} /* namespace wolf */
// Register in the FactoryProcessor
#include "core/processor/factory_processor.h"
-namespace wolf {
+namespace wolf
+{
WOLF_REGISTER_PROCESSOR(ProcessorPointFeetNomove);
WOLF_REGISTER_PROCESSOR_AUTO(ProcessorPointFeetNomove);
-} // namespace wolf
+} // namespace wolf
diff --git a/src/sensor/sensor_force_torque.cpp b/src/sensor/sensor_force_torque.cpp
index 77cf60a..39ab212 100644
--- a/src/sensor/sensor_force_torque.cpp
+++ b/src/sensor/sensor_force_torque.cpp
@@ -23,32 +23,41 @@
#include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
-namespace wolf {
-
-SensorForceTorque::SensorForceTorque(const Eigen::VectorXd& _extrinsics, ParamsSensorForceTorquePtr _params) :
- SensorBase("SensorForceTorque",
- nullptr,
- nullptr,
- nullptr,
- (Eigen::Matrix<double, 12, 1>() << _params->std_f,_params->std_f,_params->std_f,
- _params->std_f,_params->std_f,_params->std_f,
- _params->std_tau,_params->std_tau,_params->std_tau,
- _params->std_tau,_params->std_tau,_params->std_tau).finished(),
- false,
- false,
- true),
- mass_(_params->mass),
- std_f_(_params->std_f),
- std_tau_(_params->std_tau)
+namespace wolf
+{
+SensorForceTorque::SensorForceTorque(const Eigen::VectorXd& _extrinsics, ParamsSensorForceTorquePtr _params)
+ : SensorBase("SensorForceTorque",
+ nullptr,
+ nullptr,
+ nullptr,
+ (Eigen::Matrix<double, 12, 1>() << _params->std_f,
+ _params->std_f,
+ _params->std_f,
+ _params->std_f,
+ _params->std_f,
+ _params->std_f,
+ _params->std_tau,
+ _params->std_tau,
+ _params->std_tau,
+ _params->std_tau,
+ _params->std_tau,
+ _params->std_tau)
+ .finished(),
+ false,
+ false,
+ true),
+ mass_(_params->mass),
+ std_f_(_params->std_f),
+ std_tau_(_params->std_tau)
{
assert(_extrinsics.size() == 0 && "Wrong extrinsics vector size! Should be 0");
}
-
-} // namespace wolf
+} // namespace wolf
// Register in the FactorySensor
#include "core/sensor/factory_sensor.h"
-namespace wolf {
+namespace wolf
+{
WOLF_REGISTER_SENSOR(SensorForceTorque)
-} // namespace wolf
+} // namespace wolf
diff --git a/src/sensor/sensor_inertial_kinematics.cpp b/src/sensor/sensor_inertial_kinematics.cpp
index b7dd3f5..a883140 100644
--- a/src/sensor/sensor_inertial_kinematics.cpp
+++ b/src/sensor/sensor_inertial_kinematics.cpp
@@ -23,17 +23,25 @@
#include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
-namespace wolf {
-
-SensorInertialKinematics::SensorInertialKinematics(const Eigen::VectorXd& _extrinsics, ParamsSensorInertialKinematicsPtr _params) :
- SensorBase("SensorInertialKinematics",
- nullptr, // no position
- nullptr, // no orientation
- std::make_shared<StateBlock>(Eigen::Vector3d(0,0,0), false), // bias; false = unfixed
- (Eigen::Vector6d() << _params->std_pbc,_params->std_pbc,_params->std_pbc,
- _params->std_vbc,_params->std_vbc,_params->std_vbc).finished(),
- false, false, true),
- intr_ikin_(_params)
+namespace wolf
+{
+SensorInertialKinematics::SensorInertialKinematics(const Eigen::VectorXd& _extrinsics,
+ ParamsSensorInertialKinematicsPtr _params)
+ : SensorBase("SensorInertialKinematics",
+ nullptr, // no position
+ nullptr, // no orientation
+ std::make_shared<StateBlock>(Eigen::Vector3d(0, 0, 0), false), // bias; false = unfixed
+ (Eigen::Vector6d() << _params->std_pbc,
+ _params->std_pbc,
+ _params->std_pbc,
+ _params->std_vbc,
+ _params->std_vbc,
+ _params->std_vbc)
+ .finished(),
+ false,
+ false,
+ true),
+ intr_ikin_(_params)
{
assert(_extrinsics.size() == 0 && "Wrong extrinsics vector size! Should be 0.");
assert(getIntrinsic()->getState().size() == 3 && "Wrong extrinsics size! Should be 3.");
@@ -45,11 +53,11 @@ SensorInertialKinematics::~SensorInertialKinematics()
//
}
-
-} // namespace wolf
+} // namespace wolf
// Register in the FactorySensor
#include "core/sensor/factory_sensor.h"
-namespace wolf {
+namespace wolf
+{
WOLF_REGISTER_SENSOR(SensorInertialKinematics)
-} // namespace wolf
+} // namespace wolf
diff --git a/src/sensor/sensor_point_feet_nomove.cpp b/src/sensor/sensor_point_feet_nomove.cpp
index 4152bfc..3882b61 100644
--- a/src/sensor/sensor_point_feet_nomove.cpp
+++ b/src/sensor/sensor_point_feet_nomove.cpp
@@ -20,36 +20,38 @@
#include "bodydynamics/sensor/sensor_point_feet_nomove.h"
-namespace wolf {
-
-SensorPointFeetNomove::SensorPointFeetNomove(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorPointFeetNomovePtr& _params) :
- SensorBase("SensorPointFeetNomove",
- nullptr,
- nullptr,
- nullptr,
- (Eigen::Vector3d() << _params->std_foot_nomove_,_params->std_foot_nomove_,_params->std_foot_nomove_).finished(),
- false,
- false,
- false)
+namespace wolf
+{
+SensorPointFeetNomove::SensorPointFeetNomove(const Eigen::VectorXd& _extrinsics_pq,
+ const ParamsSensorPointFeetNomovePtr& _params)
+ : SensorBase("SensorPointFeetNomove",
+ nullptr,
+ nullptr,
+ nullptr,
+ (Eigen::Vector3d() << _params->std_foot_nomove_, _params->std_foot_nomove_, _params->std_foot_nomove_)
+ .finished(),
+ false,
+ false,
+ false)
{
assert(_extrinsics_pq.size() == 0 && "Bad extrinsics vector size! Should be 0 since not used.");
- cov_foot_nomove_ = pow(_params->std_foot_nomove_, 2)*Matrix3d::Identity();
- cov_altitude_ = pow(_params->std_altitude_, 2)*Matrix1d::Identity();
- foot_radius_ = _params->foot_radius_;
+ cov_foot_nomove_ = pow(_params->std_foot_nomove_, 2) * Matrix3d::Identity();
+ cov_altitude_ = pow(_params->std_altitude_, 2) * Matrix1d::Identity();
+ foot_radius_ = _params->foot_radius_;
}
-
SensorPointFeetNomove::~SensorPointFeetNomove()
{
//
}
-} // namespace wolf
+} // namespace wolf
// Register in the FactorySensor
#include "core/sensor/factory_sensor.h"
-namespace wolf {
+namespace wolf
+{
WOLF_REGISTER_SENSOR(SensorPointFeetNomove);
WOLF_REGISTER_SENSOR_AUTO(SensorPointFeetNomove);
-}
+} // namespace wolf
diff --git a/test/gtest_capture_inertial_kinematics.cpp b/test/gtest_capture_inertial_kinematics.cpp
index bf576e1..a2fa85f 100644
--- a/test/gtest_capture_inertial_kinematics.cpp
+++ b/test/gtest_capture_inertial_kinematics.cpp
@@ -30,19 +30,18 @@ using namespace wolf;
TEST(CaptureInertialKinematics, constructor_and_getters)
{
- SensorInertialKinematicsPtr sen;
- Vector9d data = Vector9d::Random();
- Matrix3d Iq = Matrix3d::Random();
- Vector3d Lcm = Vector3d::Random();
- Vector3d bias = Vector3d::Random();
- CaptureInertialKinematicsPtr C = std::make_shared<CaptureInertialKinematics>(0, sen, data, Iq, Lcm, bias);
+ SensorInertialKinematicsPtr sen;
+ Vector9d data = Vector9d::Random();
+ Matrix3d Iq = Matrix3d::Random();
+ Vector3d Lcm = Vector3d::Random();
+ Vector3d bias = Vector3d::Random();
+ CaptureInertialKinematicsPtr C = std::make_shared<CaptureInertialKinematics>(0, sen, data, Iq, Lcm, bias);
ASSERT_MATRIX_APPROX(C->getData(), data, 1e-20);
}
int main(int argc, char **argv)
{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
}
-
diff --git a/test/gtest_capture_leg_odom.cpp b/test/gtest_capture_leg_odom.cpp
index 040f186..989e44e 100644
--- a/test/gtest_capture_leg_odom.cpp
+++ b/test/gtest_capture_leg_odom.cpp
@@ -28,84 +28,103 @@
using namespace wolf;
using namespace Eigen;
-
TEST(CaptureLegOdom, point_feet_relative_kin)
-{
- TimeStamp ts(0.0);
- double dt = 1;
- // 2 point feet + wb
- MatrixXd data_kin(3,5);
- Vector3d bm_p_bml1; bm_p_bml1 << 0,0,0;
- Vector3d b_p_bl1; b_p_bl1 << -1,0,0;
- Vector3d bm_p_bml2; bm_p_bml2 << 0,0,0;
- Vector3d b_p_bl2; b_p_bl2 << -1,0,0;
- Vector3d i_omg_oi; i_omg_oi << 0,0,0;
- data_kin.col(0) = bm_p_bml1;
- data_kin.col(1) = b_p_bl1;
- data_kin.col(2) = bm_p_bml2;
- data_kin.col(3) = b_p_bl2;
- data_kin.col(4) = i_omg_oi;
-
- Matrix6d data_cov = Matrix6d::Identity();
- CaptureLegOdomPtr CLO = std::make_shared<CaptureLegOdom>(ts, nullptr, data_kin, data_cov, dt, CaptureLegOdomType::POINT_FEET_RELATIVE_KIN);
-
- Vector3d bm_p_bmb_exp; bm_p_bmb_exp << 1, 0, 0;
- ASSERT_MATRIX_APPROX(CLO->getData().head<3>(), bm_p_bmb_exp, 1e-6)
+{
+ TimeStamp ts(0.0);
+ double dt = 1;
+ // 2 point feet + wb
+ MatrixXd data_kin(3, 5);
+ Vector3d bm_p_bml1;
+ bm_p_bml1 << 0, 0, 0;
+ Vector3d b_p_bl1;
+ b_p_bl1 << -1, 0, 0;
+ Vector3d bm_p_bml2;
+ bm_p_bml2 << 0, 0, 0;
+ Vector3d b_p_bl2;
+ b_p_bl2 << -1, 0, 0;
+ Vector3d i_omg_oi;
+ i_omg_oi << 0, 0, 0;
+ data_kin.col(0) = bm_p_bml1;
+ data_kin.col(1) = b_p_bl1;
+ data_kin.col(2) = bm_p_bml2;
+ data_kin.col(3) = b_p_bl2;
+ data_kin.col(4) = i_omg_oi;
+
+ Matrix6d data_cov = Matrix6d::Identity();
+ CaptureLegOdomPtr CLO = std::make_shared<CaptureLegOdom>(
+ ts, nullptr, data_kin, data_cov, dt, CaptureLegOdomType::POINT_FEET_RELATIVE_KIN);
+
+ Vector3d bm_p_bmb_exp;
+ bm_p_bmb_exp << 1, 0, 0;
+ ASSERT_MATRIX_APPROX(CLO->getData().head<3>(), bm_p_bmb_exp, 1e-6)
}
TEST(CaptureLegOdom, point_feet_differential_kin)
-{
- TimeStamp ts(0.0);
- double dt = 1;
- // 2 point feet + wb
- MatrixXd data_kin(10,3);
- Vector3d i_omg_oi; i_omg_oi << 0,1,0;
-
- // 1rst foot: only relative foot/base motion from J*dqa, no lever so no influence from angular vel
- Vector3d b_p_bl1; b_p_bl1 << 0,0,0;
- Vector4d b_qvec_l1; b_qvec_l1 << 0,0,0,1;
- Vector3d l_v_bl1; l_v_bl1 << -1,0,0;
-
- // 2nd feet: rigid rotation pivoting around the foot point
- Vector3d b_p_bl2; b_p_bl2 << 0,0,-1;
- Vector4d b_qvec_l2; b_qvec_l2 << 0,0,0,1;
- Vector3d l_v_bl2; l_v_bl2 << 0,0,0;
- data_kin.col(0) << b_p_bl1, b_qvec_l1, l_v_bl1;
- data_kin.col(1) << b_p_bl2, b_qvec_l2, l_v_bl2;
- data_kin.col(2) << i_omg_oi, 0,0,0,0,0,0,0;
-
- Matrix6d data_cov = Matrix6d::Identity();
- CaptureLegOdomPtr CLO = std::make_shared<CaptureLegOdom>(ts, nullptr, data_kin, data_cov, dt, CaptureLegOdomType::POINT_FEET_DIFFERENTIAL_KIN);
-
- Vector3d bm_p_bmb_exp; bm_p_bmb_exp << 1, 0, 0;
- ASSERT_MATRIX_APPROX(CLO->getData().head<3>(), bm_p_bmb_exp, 1e-6)
+{
+ TimeStamp ts(0.0);
+ double dt = 1;
+ // 2 point feet + wb
+ MatrixXd data_kin(10, 3);
+ Vector3d i_omg_oi;
+ i_omg_oi << 0, 1, 0;
+
+ // 1rst foot: only relative foot/base motion from J*dqa, no lever so no influence from angular vel
+ Vector3d b_p_bl1;
+ b_p_bl1 << 0, 0, 0;
+ Vector4d b_qvec_l1;
+ b_qvec_l1 << 0, 0, 0, 1;
+ Vector3d l_v_bl1;
+ l_v_bl1 << -1, 0, 0;
+
+ // 2nd feet: rigid rotation pivoting around the foot point
+ Vector3d b_p_bl2;
+ b_p_bl2 << 0, 0, -1;
+ Vector4d b_qvec_l2;
+ b_qvec_l2 << 0, 0, 0, 1;
+ Vector3d l_v_bl2;
+ l_v_bl2 << 0, 0, 0;
+ data_kin.col(0) << b_p_bl1, b_qvec_l1, l_v_bl1;
+ data_kin.col(1) << b_p_bl2, b_qvec_l2, l_v_bl2;
+ data_kin.col(2) << i_omg_oi, 0, 0, 0, 0, 0, 0, 0;
+
+ Matrix6d data_cov = Matrix6d::Identity();
+ CaptureLegOdomPtr CLO = std::make_shared<CaptureLegOdom>(
+ ts, nullptr, data_kin, data_cov, dt, CaptureLegOdomType::POINT_FEET_DIFFERENTIAL_KIN);
+
+ Vector3d bm_p_bmb_exp;
+ bm_p_bmb_exp << 1, 0, 0;
+ ASSERT_MATRIX_APPROX(CLO->getData().head<3>(), bm_p_bmb_exp, 1e-6)
}
TEST(CaptureLegOdom, humanoid_feet)
-{
- TimeStamp ts(0.0);
- double dt = 1;
- // 1 humanoid feet + wb
- MatrixXd data_kin(7,2);
- // only one foot taken into account
- Vector3d bm_p_bml1; bm_p_bml1 << 0,0,0;
- Vector4d bm_q_bml1; bm_q_bml1 << 0,0,0,1;
- Vector3d b_p_bl1; b_p_bl1 << -1,0,0;
- Vector4d b_q_bl1; b_q_bl1 << 0,0,0,1;
- data_kin.col(0) << bm_p_bml1, bm_q_bml1;
- data_kin.col(1) << b_p_bl1, b_q_bl1;
-
- Matrix6d data_cov = Matrix6d::Identity();
- CaptureLegOdomPtr CLO = std::make_shared<CaptureLegOdom>(ts, nullptr, data_kin, data_cov, dt, CaptureLegOdomType::HUMANOID_FEET_RELATIVE_KIN);
-
- Vector3d bm_p_bmb_exp; bm_p_bmb_exp << 1, 0, 0;
- ASSERT_MATRIX_APPROX(CLO->getData().head<3>(), bm_p_bmb_exp, 1e-6)
-
+{
+ TimeStamp ts(0.0);
+ double dt = 1;
+ // 1 humanoid feet + wb
+ MatrixXd data_kin(7, 2);
+ // only one foot taken into account
+ Vector3d bm_p_bml1;
+ bm_p_bml1 << 0, 0, 0;
+ Vector4d bm_q_bml1;
+ bm_q_bml1 << 0, 0, 0, 1;
+ Vector3d b_p_bl1;
+ b_p_bl1 << -1, 0, 0;
+ Vector4d b_q_bl1;
+ b_q_bl1 << 0, 0, 0, 1;
+ data_kin.col(0) << bm_p_bml1, bm_q_bml1;
+ data_kin.col(1) << b_p_bl1, b_q_bl1;
+
+ Matrix6d data_cov = Matrix6d::Identity();
+ CaptureLegOdomPtr CLO = std::make_shared<CaptureLegOdom>(
+ ts, nullptr, data_kin, data_cov, dt, CaptureLegOdomType::HUMANOID_FEET_RELATIVE_KIN);
+
+ Vector3d bm_p_bmb_exp;
+ bm_p_bmb_exp << 1, 0, 0;
+ ASSERT_MATRIX_APPROX(CLO->getData().head<3>(), bm_p_bmb_exp, 1e-6)
}
int main(int argc, char **argv)
{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
}
-
diff --git a/test/gtest_factor_force_torque.cpp b/test/gtest_factor_force_torque.cpp
index dd394ae..82a19e1 100644
--- a/test/gtest_factor_force_torque.cpp
+++ b/test/gtest_factor_force_torque.cpp
@@ -19,10 +19,11 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
/*
-Organisation: For each test, the problem, sensors, factors (if possible) are instanciated in a base class inheriting fromtesting::Test only.
-Then, each test case is derived in a child class which defines the data processed by the different sensors and expected estimated test values.
-Finally, each of these child classes is used in one or several Test in which basically the expected values are compared against estimation and
-solve is done with a perturbated system.
+Organisation: For each test, the problem, sensors, factors (if possible) are instanciated in a base class inheriting
+fromtesting::Test only. Then, each test case is derived in a child class which defines the data processed by the
+different sensors and expected estimated test values. Finally, each of these child classes is used in one or several
+Test in which basically the expected values are compared against estimation and solve is done with a perturbated
+system.
Tests list:
@@ -32,7 +33,6 @@ FactorInertialKinematics_2KF_m1p_pfix,ZeroMvt:
FactorInertialKinematics_2KF_1v_bfix,ZeroMvt:
*/
-
// debug
#include <iostream>
#include <fstream>
@@ -74,70 +74,67 @@ using namespace wolf;
using namespace Eigen;
using namespace std;
-
// SOME CONSTANTS
-const double Acc1 = 1;
-const double Acc2 = 3;
+const double Acc1 = 1;
+const double Acc2 = 3;
const Vector3d zero3 = Vector3d::Zero();
const Vector6d zero6 = Vector6d::Zero();
-
-
-Matrix9d computeKinJac(const Vector3d& w_unb,
- const Vector3d& p_unb,
- const Matrix3d& Iq)
+Matrix9d computeKinJac(const Vector3d& w_unb, const Vector3d& p_unb, const Matrix3d& Iq)
{
- Matrix9d J; J.setZero();
+ Matrix9d J;
+ J.setZero();
Matrix3d wx = skew(w_unb);
Matrix3d px = skew(p_unb);
// Starting from top left, to the right and then next row
- J.topLeftCorner<3,3>() = Matrix3d::Identity();
+ J.topLeftCorner<3, 3>() = Matrix3d::Identity();
// J.block<3,3>(0,3) = Matrix3d::Zero();
// J.topRightCorner<3,3>() = Matrix3d::Zero();
- J.block<3,3>(3,0) = -wx;
- J.block<3,3>(3,3) = -Matrix3d::Identity();
- J.block<3,3>(3,6) = px;
+ J.block<3, 3>(3, 0) = -wx;
+ J.block<3, 3>(3, 3) = -Matrix3d::Identity();
+ J.block<3, 3>(3, 6) = px;
// J.bottomLeftCorner<3,3>() = Matrix3d::Zero();
// J.block<3,3>(6,3) = Matrix3d::Zero();
- J.bottomRightCorner<3,3>() = -Iq;
+ J.bottomRightCorner<3, 3>() = -Iq;
return J;
}
Matrix9d computeKinCov(const Matrix3d& Qp,
- const Matrix3d& Qv,
- const Matrix3d& Qw,
- const Vector3d& w_unb,
- const Vector3d& p_unb,
- const Matrix3d& Iq)
+ const Matrix3d& Qv,
+ const Matrix3d& Qw,
+ const Vector3d& w_unb,
+ const Vector3d& p_unb,
+ const Matrix3d& Iq)
{
- Matrix9d cov; cov.setZero();
+ Matrix9d cov;
+ cov.setZero();
Matrix3d wx = skew(w_unb);
Matrix3d px = skew(p_unb);
// Starting from top left, to the right and then next row
- cov.topLeftCorner<3,3>() = Qp;
- cov.block<3,3>(0,3) = Qp * wx;
+ cov.topLeftCorner<3, 3>() = Qp;
+ cov.block<3, 3>(0, 3) = Qp * wx;
// cov.topRightCorner<3,3>() = Matrix3d::Zero();
- cov.block<3,3>(3,0) = cov.block<3,3>(0,3).transpose();
- cov.block<3,3>(3,3) = -wx*Qp*wx + Qv - px*Qw*px;
- cov.block<3,3>(3,6) = -px*Qw*Iq;
+ cov.block<3, 3>(3, 0) = cov.block<3, 3>(0, 3).transpose();
+ cov.block<3, 3>(3, 3) = -wx * Qp * wx + Qv - px * Qw * px;
+ cov.block<3, 3>(3, 6) = -px * Qw * Iq;
// cov.bottomLeftCorner<3,3>() = Matrix3d::Zero();
- cov.block<3,3>(6,3) = Iq*Qw*px;
- cov.bottomRightCorner<3,3>() = Iq*Qw*Iq;
+ cov.block<3, 3>(6, 3) = Iq * Qw * px;
+ cov.bottomRightCorner<3, 3>() = Iq * Qw * Iq;
return cov;
}
-
-void perturbateIfUnFixed(const FrameBasePtr& KF, char sb_name){
- if(!KF->getStateBlock(sb_name)->isFixed())
+void perturbateIfUnFixed(const FrameBasePtr& KF, char sb_name)
+{
+ if (!KF->getStateBlock(sb_name)->isFixed())
{
if (sb_name == 'O')
{
- double max_rad_pert = M_PI / 3;
- Vector3d do_pert = max_rad_pert*Vector3d::Random();
+ double max_rad_pert = M_PI / 3;
+ Vector3d do_pert = max_rad_pert * Vector3d::Random();
Quaterniond curr_state_q(KF->getO()->getState().data());
KF->getStateBlock('O')->setState((curr_state_q * exp_q(do_pert)).coeffs());
}
@@ -145,7 +142,8 @@ void perturbateIfUnFixed(const FrameBasePtr& KF, char sb_name){
{
VectorXd pert;
pert.resize(KF->getStateBlock(sb_name)->getSize());
- pert.setRandom(); pert *= 0.5;
+ pert.setRandom();
+ pert *= 0.5;
KF->getStateBlock(sb_name)->setState(KF->getStateBlock(sb_name)->getState() + pert);
}
}
@@ -153,197 +151,226 @@ void perturbateIfUnFixed(const FrameBasePtr& KF, char sb_name){
void perturbateAllIfUnFixed(const FrameBasePtr& KF)
{
- for (auto it: KF->getStateBlockMap()){
+ for (auto it : KF->getStateBlockMap())
+ {
perturbateIfUnFixed(KF, it.first);
}
}
-FrameBasePtr createKFWithCDLI(const ProblemPtr& problem, const TimeStamp& t, VectorComposite x_origin,
- Vector3d c, Vector3d cd, Vector3d Lc, Vector6d bias_imu)
+FrameBasePtr createKFWithCDLI(const ProblemPtr& problem,
+ const TimeStamp& t,
+ VectorComposite x_origin,
+ Vector3d c,
+ Vector3d cd,
+ Vector3d Lc,
+ Vector6d bias_imu)
{
- FrameBasePtr KF = FrameBase::emplace<FrameBase>(problem->getTrajectory(), t, "POV", x_origin);
- StateBlockPtr sbc = make_shared<StateBlock>(c); KF->addStateBlock('C', sbc, problem);
- StateBlockPtr sbd = make_shared<StateBlock>(cd); KF->addStateBlock('D', sbd, problem);
- StateBlockPtr sbL = make_shared<StateBlock>(Lc); KF->addStateBlock('L', sbL, problem);
- StateBlockPtr sbbimu = make_shared<StateBlock>(bias_imu); KF->addStateBlock('I', sbbimu, problem); // Simulating IMU
+ FrameBasePtr KF = FrameBase::emplace<FrameBase>(problem->getTrajectory(), t, "POV", x_origin);
+ StateBlockPtr sbc = make_shared<StateBlock>(c);
+ KF->addStateBlock('C', sbc, problem);
+ StateBlockPtr sbd = make_shared<StateBlock>(cd);
+ KF->addStateBlock('D', sbd, problem);
+ StateBlockPtr sbL = make_shared<StateBlock>(Lc);
+ KF->addStateBlock('L', sbL, problem);
+ StateBlockPtr sbbimu = make_shared<StateBlock>(bias_imu);
+ KF->addStateBlock('I', sbbimu, problem); // Simulating IMU
return KF;
}
-
class FactorInertialKinematics_2KF : public testing::Test
{
- public:
- double mass_;
- wolf::TimeStamp tA_;
- wolf::TimeStamp tB_;
- ProblemPtr problem_;
- SolverCeresPtr solver_;
- VectorComposite x_origin_; // initial state
- VectorComposite s_origin_; // initial sigmas for prior
- SensorInertialKinematicsPtr SIK_;
- CaptureInertialKinematicsPtr CIKA_, CIKB_;
- FrameBasePtr KFA_;
- FrameBasePtr KFB_;
- Matrix3d Qp_, Qv_, Qw_;
- Vector3d bias_p_;
- Vector6d bias_imu_;
- FeatureInertialKinematicsPtr FIKA_;
- FeatureInertialKinematicsPtr FIKB_;
- FeatureForceTorquePtr FFTAB_;
- // SensorImuPtr sen_imu_;
- // ProcessorImuPtr processor_imu_;
-
- protected:
+ public:
+ double mass_;
+ wolf::TimeStamp tA_;
+ wolf::TimeStamp tB_;
+ ProblemPtr problem_;
+ SolverCeresPtr solver_;
+ VectorComposite x_origin_; // initial state
+ VectorComposite s_origin_; // initial sigmas for prior
+ SensorInertialKinematicsPtr SIK_;
+ CaptureInertialKinematicsPtr CIKA_, CIKB_;
+ FrameBasePtr KFA_;
+ FrameBasePtr KFB_;
+ Matrix3d Qp_, Qv_, Qw_;
+ Vector3d bias_p_;
+ Vector6d bias_imu_;
+ FeatureInertialKinematicsPtr FIKA_;
+ FeatureInertialKinematicsPtr FIKB_;
+ FeatureForceTorquePtr FFTAB_;
+ // SensorImuPtr sen_imu_;
+ // ProcessorImuPtr processor_imu_;
+
+ protected:
void SetUp() override
{
-
std::string bodydynamics_root = _WOLF_BODYDYNAMICS_CODE_DIR;
- mass_ = 10.0; // Small 10 kg robot
+ mass_ = 10.0; // Small 10 kg robot
//===================================================== SETTING PROBLEM
// WOLF PROBLEM
problem_ = Problem::create("POV", 3);
- VectorXd extr_ik(0);
+ VectorXd extr_ik(0);
ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
- intr_ik->std_pbc = 0.1;
- intr_ik->std_vbc = 0.1;
+ intr_ik->std_pbc = 0.1;
+ intr_ik->std_vbc = 0.1;
auto senik = problem_->installSensor("SensorInertialKinematics", "senIK", extr_ik, intr_ik);
- SIK_ = std::static_pointer_cast<SensorInertialKinematics>(senik);
+ SIK_ = std::static_pointer_cast<SensorInertialKinematics>(senik);
// CERES WRAPPER
- solver_ = std::make_shared<SolverCeres>(problem_);
+ solver_ = std::make_shared<SolverCeres>(problem_);
solver_->getSolverOptions().max_num_iterations = 1e3;
// solver_->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
// solver_->getSolverOptions().max_line_search_step_contraction = 1e-3;
// INSTALL Imu SENSOR
// Vector7d extrinsics; extrinsics << 0,0,0,0,0,0,1;
- // SensorBasePtr sen_imu = problem_->installSensor("SensorImu", "Main Imu Sensor", extrinsics, bodydynamics_root + "/demos/sensor_imu.yaml");
- // sen_imu_ = std::static_pointer_cast<SensorImu>(sen_imu);
- // ProcessorBasePtr prc_imu = problem_->installProcessor("ProcessorImu", "Imu PROC", "Main Imu Sensor", bodydynamics_root + "/demos/processor_imu.yaml");
- // Vector6d data = zero6;
- // wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(0, sen_imu_, data, sen_imu_->getNoiseCov(), zero6);
+ // SensorBasePtr sen_imu = problem_->installSensor("SensorImu", "Main Imu Sensor", extrinsics,
+ // bodydynamics_root + "/demos/sensor_imu.yaml"); sen_imu_ = std::static_pointer_cast<SensorImu>(sen_imu);
+ // ProcessorBasePtr prc_imu = problem_->installProcessor("ProcessorImu", "Imu PROC", "Main Imu Sensor",
+ // bodydynamics_root + "/demos/processor_imu.yaml"); Vector6d data = zero6; wolf::CaptureImuPtr imu_ptr =
+ // std::make_shared<CaptureImu>(0, sen_imu_, data, sen_imu_->getNoiseCov(), zero6);
// // sen_imu_->process(imu_ptr);
// ======================= INITIALIZATION KFA WITH PRIOR + INERTIAL KINEMATIC FACTOR
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)});
+ 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_);
-
// ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
- bias_p_ = zero3;
- bias_imu_ = zero6;
- StateBlockPtr sbcA = make_shared<StateBlock>(zero3); KFA_->addStateBlock('C', sbcA, problem_);
- StateBlockPtr sbdA = make_shared<StateBlock>(zero3); KFA_->addStateBlock('D', sbdA, problem_);
- StateBlockPtr sbLA = make_shared<StateBlock>(zero3); KFA_->addStateBlock('L', sbLA, problem_);
- StateBlockPtr sbbimuA = make_shared<StateBlock>(bias_imu_); KFA_->addStateBlock('I', sbbimuA, problem_);
+ bias_p_ = zero3;
+ bias_imu_ = zero6;
+ StateBlockPtr sbcA = make_shared<StateBlock>(zero3);
+ KFA_->addStateBlock('C', sbcA, problem_);
+ StateBlockPtr sbdA = make_shared<StateBlock>(zero3);
+ KFA_->addStateBlock('D', sbdA, problem_);
+ StateBlockPtr sbLA = make_shared<StateBlock>(zero3);
+ KFA_->addStateBlock('L', sbLA, problem_);
+ StateBlockPtr sbbimuA = make_shared<StateBlock>(bias_imu_);
+ KFA_->addStateBlock('I', sbbimuA, problem_);
// Fix the one we cannot estimate
// KFA_->getP()->fix();
// KFA_->getO()->fix();
// KFA_->getV()->fix();
- KFA_->getStateBlock('I')->fix(); // Imu
+ KFA_->getStateBlock('I')->fix(); // Imu
// INERTIAL KINEMATICS FACTOR
// Measurements
Vector3d pBC_measA = zero3;
Vector3d vBC_measA = zero3;
- Vector3d w_measA = zero3;
- Vector9d meas_ikinA; meas_ikinA << pBC_measA, vBC_measA, w_measA;
+ Vector3d w_measA = zero3;
+ Vector9d meas_ikinA;
+ meas_ikinA << pBC_measA, vBC_measA, w_measA;
// momentum parameters
- Matrix3d Iq; Iq.setIdentity();
+ Matrix3d Iq;
+ Iq.setIdentity();
Vector3d Lq = zero3;
- Qp_ = pow(1e-2, 2)*Matrix3d::Identity();
- Qv_ = pow(1e-2, 2)*Matrix3d::Identity();
- Qw_ = pow(1e-2, 2)*Matrix3d::Identity();
- Eigen::Matrix9d Q_ikin_err = computeKinCov(Qp_, Qv_, Qw_, meas_ikinA.head<3>()-bias_p_, meas_ikinA.tail<3>()-bias_imu_.tail<3>(), Iq);
+ Qp_ = pow(1e-2, 2) * Matrix3d::Identity();
+ Qv_ = pow(1e-2, 2) * Matrix3d::Identity();
+ Qw_ = pow(1e-2, 2) * Matrix3d::Identity();
+ Eigen::Matrix9d Q_ikin_err = computeKinCov(
+ Qp_, Qv_, Qw_, meas_ikinA.head<3>() - bias_p_, meas_ikinA.tail<3>() - bias_imu_.tail<3>(), Iq);
CIKA_ = CaptureBase::emplace<CaptureInertialKinematics>(KFA_, tA_, SIK_, meas_ikinA, Iq, Lq, bias_p_);
- CIKA_->getStateBlock('I')->fix(); // IK bias
- FIKA_ = FeatureBase::emplace<FeatureInertialKinematics>(CIKA_, meas_ikinA, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ CIKA_->getStateBlock('I')->fix(); // IK bias
+ FIKA_ = FeatureBase::emplace<FeatureInertialKinematics>(
+ CIKA_, meas_ikinA, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
FactorBase::emplace<FactorInertialKinematics>(FIKA_, FIKA_, sbbimuA, nullptr, false);
-
// =============== NEW KFB WITH CORRESPONDING STATEBLOCKS
tB_.set(1);
- KFB_ = createKFWithCDLI(problem_, tB_, x_origin_,
- zero3, zero3, zero3, bias_imu_);
+ KFB_ = createKFWithCDLI(problem_, tB_, x_origin_, zero3, zero3, zero3, bias_imu_);
// Fix the one we cannot estimate
// KFB_->getP()->fix();
KFB_->getO()->fix(); // Not in the FT factor, so should be fixed (or absolute factor)
// KFB_->getV()->fix();
KFB_->getStateBlock('I')->fix(); // Imu
-
// // ================ PROCESS Imu DATA
// Vector6d data_imu;
// data_imu << -wolf::gravity(), 0,0,0;
- // CaptureImuPtr cap_imu = std::make_shared<CaptureImu>(tB_, sen_imu_, data_imu, sen_imu_->getNoiseCov(), bias_imu_); //set data on Imu (measure only gravity here)
+ // CaptureImuPtr cap_imu = std::make_shared<CaptureImu>(tB_, sen_imu_, data_imu, sen_imu_->getNoiseCov(),
+ // bias_imu_); //set data on Imu (measure only gravity here)
// // process data in capture
// // sen_imu_->process(cap_imu);
// ================ FACTOR INERTIAL KINEMATICS ON KFB
Vector3d pBC_measB = zero3;
Vector3d vBC_measB = zero3;
- Vector3d w_measB = zero3;
- Vector9d meas_ikinB; meas_ikinB << pBC_measB, vBC_measB, w_measB;
+ Vector3d w_measB = zero3;
+ Vector9d meas_ikinB;
+ meas_ikinB << pBC_measB, vBC_measB, w_measB;
CIKB_ = CaptureBase::emplace<CaptureInertialKinematics>(KFB_, tB_, SIK_, meas_ikinB, Iq, Lq, bias_p_);
- CIKB_->getSensorIntrinsic()->fix(); // IK bias
- FIKB_ = FeatureBase::emplace<FeatureInertialKinematics>(CIKB_, meas_ikinB, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
- auto fac_inkinB = FactorBase::emplace<FactorInertialKinematics>(FIKB_, FIKB_, KFB_->getStateBlock('I'), nullptr, false);
-
+ CIKB_->getSensorIntrinsic()->fix(); // IK bias
+ FIKB_ = FeatureBase::emplace<FeatureInertialKinematics>(
+ CIKB_, meas_ikinB, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ auto fac_inkinB =
+ FactorBase::emplace<FactorInertialKinematics>(FIKB_, FIKB_, KFB_->getStateBlock('I'), nullptr, false);
// FORCE TORQUE FACTOR BETWEEEN KFA AND KFB
- Vector3d f1; f1 << -mass_*wolf::gravity()/2; // Only gravity -> static robot on both feet
- Vector3d tau1; tau1 << 0,0,0;
- Vector3d pbl1; pbl1 << 0,0,0;
- Vector4d bql1; bql1 << 0,0,0,1;
- Vector3d f2; f2 << -mass_*wolf::gravity()/2; // Only gravity -> static robot on both feet
- Vector3d tau2; tau2 << 0,0,0;
- Vector3d pbl2; pbl2 << 0,0,0;
- Vector4d bql2; bql2 << 0,0,0,1;
- Vector3d pbc; pbc << 0,0,0;
+ Vector3d f1;
+ f1 << -mass_ * wolf::gravity() / 2; // Only gravity -> static robot on both feet
+ Vector3d tau1;
+ tau1 << 0, 0, 0;
+ Vector3d pbl1;
+ pbl1 << 0, 0, 0;
+ Vector4d bql1;
+ bql1 << 0, 0, 0, 1;
+ Vector3d f2;
+ f2 << -mass_ * wolf::gravity() / 2; // Only gravity -> static robot on both feet
+ Vector3d tau2;
+ tau2 << 0, 0, 0;
+ Vector3d pbl2;
+ pbl2 << 0, 0, 0;
+ Vector4d bql2;
+ bql2 << 0, 0, 0, 1;
+ Vector3d pbc;
+ pbc << 0, 0, 0;
// aggregated meas
- Vector6d f_meas; f_meas << f1, f2;
- Vector6d tau_meas; tau_meas << tau1, tau2;
- Matrix<double,14,1> kin_meas; kin_meas << pbl1, pbl2, bql1, bql2;
-
- Matrix6d cov_f = 1e-4 * Matrix6d::Identity();
+ Vector6d f_meas;
+ f_meas << f1, f2;
+ Vector6d tau_meas;
+ tau_meas << tau1, tau2;
+ Matrix<double, 14, 1> kin_meas;
+ kin_meas << pbl1, pbl2, bql1, bql2;
+
+ Matrix6d cov_f = 1e-4 * Matrix6d::Identity();
Matrix6d cov_tau = 1e-4 * Matrix6d::Identity();
Matrix3d cov_pbc = 1e-4 * Matrix3d::Identity();
CaptureBasePtr cap_ftB = CaptureBase::emplace<CaptureBase>(KFB_, "CaptureForceTorque", tB_, nullptr);
- FFTAB_ = FeatureBase::emplace<FeatureForceTorque>(cap_ftB,
- tB_ - tA_, mass_,
- f_meas, tau_meas, pbc, kin_meas,
- cov_f, cov_tau, cov_pbc);
-
- FactorForceTorquePtr fac_ftAB = FactorBase::emplace<FactorForceTorque>(FFTAB_, FFTAB_, KFA_, CIKA_->getSensorIntrinsic(), nullptr, false);
+ FFTAB_ = FeatureBase::emplace<FeatureForceTorque>(
+ cap_ftB, tB_ - tA_, mass_, f_meas, tau_meas, pbc, kin_meas, cov_f, cov_tau, cov_pbc);
+ FactorForceTorquePtr fac_ftAB =
+ FactorBase::emplace<FactorForceTorque>(FFTAB_, FFTAB_, KFA_, CIKA_->getSensorIntrinsic(), nullptr, false);
}
};
-
class FactorInertialKinematics_2KF_foot1turned : public FactorInertialKinematics_2KF
{
- protected:
+ protected:
void SetUp() override
{
cout << "\n\n\nFactorInertialKinematics_2KF_foot1turned" << endl;
FactorInertialKinematics_2KF::SetUp();
- problem_->print(3,false,true,true);
- Vector4d WqL; WqL.setRandom(); WqL.normalize(); // random unitary quaternion
- Quaterniond quat_WqL(WqL.data());
- Matrix<double, 14, 1> kin_meas; kin_meas << zero3, zero3, WqL, 0,0,0,1;
- Vector3d f1 = -mass_*wolf::gravity()/2;
- f1 = quat_WqL.conjugate() * f1; // Rotate force meas accordingly
- Vector3d f2 = -mass_*wolf::gravity()/2;
- Vector6d fmeas; fmeas << f1, f2;
+ problem_->print(3, false, true, true);
+ Vector4d WqL;
+ WqL.setRandom();
+ WqL.normalize(); // random unitary quaternion
+ Quaterniond quat_WqL(WqL.data());
+ Matrix<double, 14, 1> kin_meas;
+ kin_meas << zero3, zero3, WqL, 0, 0, 0, 1;
+ Vector3d f1 = -mass_ * wolf::gravity() / 2;
+ f1 = quat_WqL.conjugate() * f1; // Rotate force meas accordingly
+ Vector3d f2 = -mass_ * wolf::gravity() / 2;
+ Vector6d fmeas;
+ fmeas << f1, f2;
FFTAB_->setKinMeas(kin_meas);
FFTAB_->setForcesMeas(fmeas);
// problem_->print(3,false,true,true);
@@ -352,17 +379,21 @@ class FactorInertialKinematics_2KF_foot1turned : public FactorInertialKinematics
class FactorInertialKinematics_2KF_foot2turned : public FactorInertialKinematics_2KF
{
- protected:
+ protected:
void SetUp() override
{
FactorInertialKinematics_2KF::SetUp();
- Vector4d WqL; WqL.setRandom(); WqL.normalize(); // random unitary quaternion
- Quaterniond quat_WqL(WqL.data());
- Matrix<double, 14, 1> kin_meas; kin_meas << zero3, zero3, 0,0,0,1, WqL;
- Vector3d f1 = -mass_*wolf::gravity()/2;
- Vector3d f2 = -mass_*wolf::gravity()/2;
- f2 = quat_WqL.conjugate() * f2; // Rotate force meas accordingly
- Vector6d fmeas; fmeas << f1, f2;
+ Vector4d WqL;
+ WqL.setRandom();
+ WqL.normalize(); // random unitary quaternion
+ Quaterniond quat_WqL(WqL.data());
+ Matrix<double, 14, 1> kin_meas;
+ kin_meas << zero3, zero3, 0, 0, 0, 1, WqL;
+ Vector3d f1 = -mass_ * wolf::gravity() / 2;
+ Vector3d f2 = -mass_ * wolf::gravity() / 2;
+ f2 = quat_WqL.conjugate() * f2; // Rotate force meas accordingly
+ Vector6d fmeas;
+ fmeas << f1, f2;
FFTAB_->setKinMeas(kin_meas);
FFTAB_->setForcesMeas(fmeas);
}
@@ -370,17 +401,21 @@ class FactorInertialKinematics_2KF_foot2turned : public FactorInertialKinematics
class FactorInertialKinematics_2KF_singleContactFoot2turned : public FactorInertialKinematics_2KF
{
- protected:
+ protected:
void SetUp() override
{
FactorInertialKinematics_2KF::SetUp();
- Vector4d WqL; WqL.setRandom(); WqL.normalize(); // random unitary quaternion
- Quaterniond quat_WqL(WqL.data());
- Matrix<double, 14, 1> kin_meas; kin_meas << zero3, zero3, WqL, 0,0,0,1;
- Vector3d f1 = -mass_*wolf::gravity();
- f1 = quat_WqL.conjugate() * f1; // Rotate force meas accordingly
+ Vector4d WqL;
+ WqL.setRandom();
+ WqL.normalize(); // random unitary quaternion
+ Quaterniond quat_WqL(WqL.data());
+ Matrix<double, 14, 1> kin_meas;
+ kin_meas << zero3, zero3, WqL, 0, 0, 0, 1;
+ Vector3d f1 = -mass_ * wolf::gravity();
+ f1 = quat_WqL.conjugate() * f1; // Rotate force meas accordingly
Vector3d f2 = zero3;
- Vector6d fmeas; fmeas << f1, f2;
+ Vector6d fmeas;
+ fmeas << f1, f2;
FFTAB_->setKinMeas(kin_meas);
FFTAB_->setForcesMeas(fmeas);
}
@@ -388,72 +423,76 @@ class FactorInertialKinematics_2KF_singleContactFoot2turned : public FactorInert
class FactorInertialKinematics_2KF_singleContactFoot1turned : public FactorInertialKinematics_2KF
{
- protected:
+ protected:
void SetUp() override
{
FactorInertialKinematics_2KF::SetUp();
- Vector4d WqL; WqL.setRandom(); WqL.normalize(); // random unitary quaternion
- Quaterniond quat_WqL(WqL.data());
- Matrix<double, 14, 1> kin_meas; kin_meas << zero3, zero3, 0,0,0,1, WqL;
+ Vector4d WqL;
+ WqL.setRandom();
+ WqL.normalize(); // random unitary quaternion
+ Quaterniond quat_WqL(WqL.data());
+ Matrix<double, 14, 1> kin_meas;
+ kin_meas << zero3, zero3, 0, 0, 0, 1, WqL;
Vector3d f1 = zero3;
- Vector3d f2 = -mass_*wolf::gravity();
- f2 = quat_WqL.conjugate() * f2; // Rotate force meas accordingly
- Vector6d fmeas; fmeas << f1, f2;
+ Vector3d f2 = -mass_ * wolf::gravity();
+ f2 = quat_WqL.conjugate() * f2; // Rotate force meas accordingly
+ Vector6d fmeas;
+ fmeas << f1, f2;
FFTAB_->setKinMeas(kin_meas);
FFTAB_->setForcesMeas(fmeas);
}
};
-
-
-
class FactorInertialKinematics_2KF_ForceX : public FactorInertialKinematics_2KF
{
- protected:
+ protected:
void SetUp() override
{
FactorInertialKinematics_2KF::SetUp();
- Vector3d f1 = -mass_*wolf::gravity()/2 + ((Vector3d()<<mass_*Acc1/2,0,0).finished());
- Vector3d f2 = -mass_*wolf::gravity()/2 + ((Vector3d()<<mass_*Acc1/2,0,0).finished());
- Vector6d fmeas; fmeas << f1, f2;
+ Vector3d f1 = -mass_ * wolf::gravity() / 2 + ((Vector3d() << mass_ * Acc1 / 2, 0, 0).finished());
+ Vector3d f2 = -mass_ * wolf::gravity() / 2 + ((Vector3d() << mass_ * Acc1 / 2, 0, 0).finished());
+ Vector6d fmeas;
+ fmeas << f1, f2;
FFTAB_->setForcesMeas(fmeas);
}
};
class FactorInertialKinematics_2KF_ForceY : public FactorInertialKinematics_2KF
{
- protected:
+ protected:
void SetUp() override
{
FactorInertialKinematics_2KF::SetUp();
- Vector3d f1 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,mass_*Acc2/2,0).finished());
- Vector3d f2 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,mass_*Acc2/2,0).finished());
- Vector6d fmeas; fmeas << f1, f2;
+ Vector3d f1 = -mass_ * wolf::gravity() / 2 + ((Vector3d() << 0, mass_ * Acc2 / 2, 0).finished());
+ Vector3d f2 = -mass_ * wolf::gravity() / 2 + ((Vector3d() << 0, mass_ * Acc2 / 2, 0).finished());
+ Vector6d fmeas;
+ fmeas << f1, f2;
FFTAB_->setForcesMeas(fmeas);
}
};
class FactorInertialKinematics_2KF_ForceZ : public FactorInertialKinematics_2KF
{
- protected:
+ protected:
void SetUp() override
{
FactorInertialKinematics_2KF::SetUp();
- Vector3d f1 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,0,mass_*Acc1/2).finished());
- Vector3d f2 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,0,mass_*Acc1/2).finished());
- Vector6d fmeas; fmeas << f1, f2;
+ Vector3d f1 = -mass_ * wolf::gravity() / 2 + ((Vector3d() << 0, 0, mass_ * Acc1 / 2).finished());
+ Vector3d f2 = -mass_ * wolf::gravity() / 2 + ((Vector3d() << 0, 0, mass_ * Acc1 / 2).finished());
+ Vector6d fmeas;
+ fmeas << f1, f2;
FFTAB_->setForcesMeas(fmeas);
}
};
class FactorInertialKinematics_3KF_ForceX : public FactorInertialKinematics_2KF_ForceX
{
- public:
- FrameBasePtr KFC_;
- CaptureInertialKinematicsPtr CIKC_;
- TimeStamp tC_;
+ public:
+ FrameBasePtr KFC_;
+ CaptureInertialKinematicsPtr CIKC_;
+ TimeStamp tC_;
- protected:
+ protected:
void SetUp() override
{
FactorInertialKinematics_2KF_ForceX::SetUp();
@@ -461,8 +500,7 @@ class FactorInertialKinematics_3KF_ForceX : public FactorInertialKinematics_2KF_
// KFB_->getStateBlock("O")->unfix();
- KFC_ = createKFWithCDLI(problem_, tC_, x_origin_,
- zero3, zero3, zero3, bias_imu_);
+ KFC_ = createKFWithCDLI(problem_, tC_, x_origin_, zero3, zero3, zero3, bias_imu_);
// Fix the one we cannot estimate
// KFB_->getP()->fix();
// KFC_->getO()->fix(); // Not in the FT factor, so should be fixed (or absolute factor)
@@ -472,57 +510,73 @@ class FactorInertialKinematics_3KF_ForceX : public FactorInertialKinematics_2KF_
// ================ FACTOR INERTIAL KINEMATICS ON KFB
Vector3d pBC_measC = zero3;
Vector3d vBC_measC = zero3;
- Vector3d w_measC = zero3;
- Vector9d meas_ikinC; meas_ikinC << pBC_measC, vBC_measC, w_measC;
- Matrix3d Iq; Iq.setIdentity();
- Vector3d Lq = zero3;
- Eigen::Matrix9d Q_ikin_errC = computeKinCov(Qp_, Qv_, Qw_, meas_ikinC.head<3>()-bias_p_, meas_ikinC.tail<3>()-bias_imu_.tail<3>(), Iq);
+ Vector3d w_measC = zero3;
+ Vector9d meas_ikinC;
+ meas_ikinC << pBC_measC, vBC_measC, w_measC;
+ Matrix3d Iq;
+ Iq.setIdentity();
+ Vector3d Lq = zero3;
+ Eigen::Matrix9d Q_ikin_errC = computeKinCov(
+ Qp_, Qv_, Qw_, meas_ikinC.head<3>() - bias_p_, meas_ikinC.tail<3>() - bias_imu_.tail<3>(), Iq);
CIKC_ = CaptureBase::emplace<CaptureInertialKinematics>(KFC_, tC_, SIK_, meas_ikinC, Iq, Lq, bias_p_);
- CIKC_->getStateBlock('I')->fix(); // IK bias
- auto feat_ikin_C = FeatureBase::emplace<FeatureInertialKinematics>(CIKC_, meas_ikinC, Q_ikin_errC, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
- FactorBase::emplace<FactorInertialKinematics>(feat_ikin_C, feat_ikin_C, CIKC_->getStateBlock('I'), nullptr, false);
-
+ CIKC_->getStateBlock('I')->fix(); // IK bias
+ auto feat_ikin_C = FeatureBase::emplace<FeatureInertialKinematics>(
+ CIKC_, meas_ikinC, Q_ikin_errC, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ FactorBase::emplace<FactorInertialKinematics>(
+ feat_ikin_C, feat_ikin_C, CIKC_->getStateBlock('I'), nullptr, false);
// FORCE TORQUE FACTOR BETWEEEN KFA AND KFB
- Vector3d f1; f1 << -mass_*wolf::gravity()/2; // Only gravity -> static robot on both feet
- Vector3d tau1; tau1 << 0,0,0;
- Vector3d pbl1; pbl1 << 0,0,0;
- Vector4d bql1; bql1 << 0,0,0,1;
- Vector3d f2; f2 << -mass_*wolf::gravity()/2; // Only gravity -> static robot on both feet
- Vector3d tau2; tau2 << 0,0,0;
- Vector3d pbl2; pbl2 << 0,0,0;
- Vector4d bql2; bql2 << 0,0,0,1;
- Vector3d pbc; pbc << 0,0,0;
+ Vector3d f1;
+ f1 << -mass_ * wolf::gravity() / 2; // Only gravity -> static robot on both feet
+ Vector3d tau1;
+ tau1 << 0, 0, 0;
+ Vector3d pbl1;
+ pbl1 << 0, 0, 0;
+ Vector4d bql1;
+ bql1 << 0, 0, 0, 1;
+ Vector3d f2;
+ f2 << -mass_ * wolf::gravity() / 2; // Only gravity -> static robot on both feet
+ Vector3d tau2;
+ tau2 << 0, 0, 0;
+ Vector3d pbl2;
+ pbl2 << 0, 0, 0;
+ Vector4d bql2;
+ bql2 << 0, 0, 0, 1;
+ Vector3d pbc;
+ pbc << 0, 0, 0;
// aggregated meas
- Vector6d f_meas; f_meas << f1, f2;
- Vector6d tau_meas; tau_meas << tau1, tau2;
- Matrix<double,14,1> kin_meas; kin_meas << pbl1, pbl2, bql1, bql2;
-
- Matrix6d cov_f = 1e-4 * Matrix6d::Identity();
+ Vector6d f_meas;
+ f_meas << f1, f2;
+ Vector6d tau_meas;
+ tau_meas << tau1, tau2;
+ Matrix<double, 14, 1> kin_meas;
+ kin_meas << pbl1, pbl2, bql1, bql2;
+
+ Matrix6d cov_f = 1e-4 * Matrix6d::Identity();
Matrix6d cov_tau = 1e-4 * Matrix6d::Identity();
Matrix3d cov_pbc = 1e-4 * Matrix3d::Identity();
- CaptureBasePtr cap_ftC = CaptureBase::emplace<CaptureBase>(KFC_, "CaptureForceTorque", tC_, nullptr);
- auto feat_ftBC = FeatureBase::emplace<FeatureForceTorque>(cap_ftC,
- tC_ - tB_, mass_,
- f_meas, tau_meas, pbc, kin_meas,
- cov_f, cov_tau, cov_pbc);
- FactorForceTorquePtr fac_ftBC = FactorBase::emplace<FactorForceTorque>(feat_ftBC, feat_ftBC, KFB_, CIKB_->getSensorIntrinsic(), nullptr, false);
+ CaptureBasePtr cap_ftC = CaptureBase::emplace<CaptureBase>(KFC_, "CaptureForceTorque", tC_, nullptr);
+ auto feat_ftBC = FeatureBase::emplace<FeatureForceTorque>(
+ cap_ftC, tC_ - tB_, mass_, f_meas, tau_meas, pbc, kin_meas, cov_f, cov_tau, cov_pbc);
+ FactorForceTorquePtr fac_ftBC = FactorBase::emplace<FactorForceTorque>(
+ feat_ftBC, feat_ftBC, KFB_, CIKB_->getSensorIntrinsic(), nullptr, false);
}
};
class FactorInertialKinematics_2KF_ForceX_Odom3d : public FactorInertialKinematics_2KF_ForceX
{
- protected:
+ protected:
void SetUp() override
{
FactorInertialKinematics_2KF_ForceX::SetUp();
- Vector7d pose_A_B; pose_A_B << (tB_-tA_)*Acc1/2,0,0, 0,0,0,1;
+ Vector7d pose_A_B;
+ pose_A_B << (tB_ - tA_) * Acc1 / 2, 0, 0, 0, 0, 0, 1;
Matrix6d rel_pose_cov = Matrix6d::Identity();
rel_pose_cov.topLeftCorner(3, 3) *= pow(1e-3, 2);
- rel_pose_cov.bottomRightCorner(3, 3) *= pow(M_TORAD*1e-3, 2);
+ rel_pose_cov.bottomRightCorner(3, 3) *= pow(M_TORAD * 1e-3, 2);
CaptureBasePtr cap_pose_base = CaptureBase::emplace<CapturePose>(KFB_, tB_, nullptr, pose_A_B, rel_pose_cov);
FeatureBasePtr ftr_pose_base = FeatureBase::emplace<FeaturePose>(cap_pose_base, pose_A_B, rel_pose_cov);
@@ -535,10 +589,6 @@ class FactorInertialKinematics_2KF_ForceX_Odom3d : public FactorInertialKinemati
}
};
-
-
-
-
////////////////////////////////////////////////////////
////////////////////////////////////////////////////////
////////////////////////////////////////////////////////
@@ -547,21 +597,21 @@ class FactorInertialKinematics_2KF_ForceX_Odom3d : public FactorInertialKinemati
////////////////////////////////////////////////////////
////////////////////////////////////////////////////////
-
-TEST_F(FactorInertialKinematics_2KF,ZeroMvt)
+TEST_F(FactorInertialKinematics_2KF, ZeroMvt)
{
Vector3d c_exp = zero3;
Vector3d cd_exp = zero3;
Vector3d Lc_exp = zero3;
Vector3d bp_exp = zero3;
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ string report =
+ solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
// problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
@@ -578,20 +628,21 @@ TEST_F(FactorInertialKinematics_2KF,ZeroMvt)
ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
}
-TEST_F(FactorInertialKinematics_2KF_foot1turned,ZeroMvt)
+TEST_F(FactorInertialKinematics_2KF_foot1turned, ZeroMvt)
{
Vector3d c_exp = zero3;
Vector3d cd_exp = zero3;
Vector3d Lc_exp = zero3;
Vector3d bp_exp = zero3;
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ string report =
+ solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
// problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
@@ -608,20 +659,21 @@ TEST_F(FactorInertialKinematics_2KF_foot1turned,ZeroMvt)
ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
}
-TEST_F(FactorInertialKinematics_2KF_foot2turned,ZeroMvt)
+TEST_F(FactorInertialKinematics_2KF_foot2turned, ZeroMvt)
{
Vector3d c_exp = zero3;
Vector3d cd_exp = zero3;
Vector3d Lc_exp = zero3;
Vector3d bp_exp = zero3;
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ string report =
+ solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
// problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
@@ -638,21 +690,21 @@ TEST_F(FactorInertialKinematics_2KF_foot2turned,ZeroMvt)
ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
}
-
-TEST_F(FactorInertialKinematics_2KF_singleContactFoot1turned,ZeroMvt)
+TEST_F(FactorInertialKinematics_2KF_singleContactFoot1turned, ZeroMvt)
{
Vector3d c_exp = zero3;
Vector3d cd_exp = zero3;
Vector3d Lc_exp = zero3;
Vector3d bp_exp = zero3;
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ string report =
+ solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
// problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
@@ -669,21 +721,21 @@ TEST_F(FactorInertialKinematics_2KF_singleContactFoot1turned,ZeroMvt)
ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
}
-
-TEST_F(FactorInertialKinematics_2KF_singleContactFoot2turned,ZeroMvt)
+TEST_F(FactorInertialKinematics_2KF_singleContactFoot2turned, ZeroMvt)
{
Vector3d c_exp = zero3;
Vector3d cd_exp = zero3;
Vector3d Lc_exp = zero3;
Vector3d bp_exp = zero3;
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ string report =
+ solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
// problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
@@ -700,18 +752,17 @@ TEST_F(FactorInertialKinematics_2KF_singleContactFoot2turned,ZeroMvt)
ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
}
-
-
-TEST_F(FactorInertialKinematics_2KF_ForceX,ZeroMvt)
+TEST_F(FactorInertialKinematics_2KF_ForceX, ZeroMvt)
{
// problem_->print(4,true,true,true);
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ string report =
+ solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
// problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
@@ -722,22 +773,24 @@ TEST_F(FactorInertialKinematics_2KF_ForceX,ZeroMvt)
ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(
+ KFB_->getStateBlock('C')->getState(), (Vector3d() << (tB_ - tA_) * Acc1 / 2, 0, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(
+ KFB_->getStateBlock('D')->getState(), (Vector3d() << (tB_ - tA_) * Acc1, 0, 0).finished(), 1e-5);
ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
}
-
-TEST_F(FactorInertialKinematics_2KF_ForceY,ZeroMvt)
+TEST_F(FactorInertialKinematics_2KF_ForceY, ZeroMvt)
{
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ string report =
+ solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
// problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
@@ -748,22 +801,24 @@ TEST_F(FactorInertialKinematics_2KF_ForceY,ZeroMvt)
ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<0, (tB_ - tA_)*Acc2/2, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<0, (tB_ - tA_)*Acc2, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(
+ KFB_->getStateBlock('C')->getState(), (Vector3d() << 0, (tB_ - tA_) * Acc2 / 2, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(
+ KFB_->getStateBlock('D')->getState(), (Vector3d() << 0, (tB_ - tA_) * Acc2, 0).finished(), 1e-5);
ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
}
-
-TEST_F(FactorInertialKinematics_2KF_ForceZ,ZeroMvt)
+TEST_F(FactorInertialKinematics_2KF_ForceZ, ZeroMvt)
{
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ string report =
+ solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
// problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
@@ -774,15 +829,18 @@ TEST_F(FactorInertialKinematics_2KF_ForceZ,ZeroMvt)
ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<0, 0, (tB_-tA_)*Acc1/2).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<0, 0, (tB_-tA_)*Acc1).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(
+ KFB_->getStateBlock('C')->getState(), (Vector3d() << 0, 0, (tB_ - tA_) * Acc1 / 2).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(
+ KFB_->getStateBlock('D')->getState(), (Vector3d() << 0, 0, (tB_ - tA_) * Acc1).finished(), 1e-5);
ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
}
// TEST_F(FactorInertialKinematics_3KF_ForceX,ZeroMvt)
// {
-// string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+// string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2:
+// FullReport;
// perturbateAllIfUnFixed(KFA_);
// perturbateAllIfUnFixed(KFB_);
@@ -800,26 +858,27 @@ TEST_F(FactorInertialKinematics_2KF_ForceZ,ZeroMvt)
// ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
// ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
// ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
-// ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
-// ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(),
+// 1e-5); ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(),
+// 1e-5); ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
// ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
-// ASSERT_MATRIX_APPROX(KFC_->getStateBlock('C')->getState(), (Vector3d()<<(tC_-tB_)*Acc1/2 + (tB_-tA_)*Acc1, 0, 0).finished(), 1e-5); // + -> due to initial vel of KFB
-// ASSERT_MATRIX_APPROX(KFC_->getStateBlock('D')->getState(), (Vector3d()<<(tC_-tB_)*Acc1, 0, 0).finished(), 1e-5); // No acc btw B and C -> same vel
-// ASSERT_MATRIX_APPROX(KFC_->getStateBlock('L')->getState(), zero3, 1e-5);
+// ASSERT_MATRIX_APPROX(KFC_->getStateBlock('C')->getState(), (Vector3d()<<(tC_-tB_)*Acc1/2 + (tB_-tA_)*Acc1, 0,
+// 0).finished(), 1e-5); // + -> due to initial vel of KFB
+// ASSERT_MATRIX_APPROX(KFC_->getStateBlock('D')->getState(), (Vector3d()<<(tC_-tB_)*Acc1, 0, 0).finished(), 1e-5);
+// // No acc btw B and C -> same vel ASSERT_MATRIX_APPROX(KFC_->getStateBlock('L')->getState(), zero3, 1e-5);
// ASSERT_MATRIX_APPROX(CIKC_->getStateBlock('I')->getState(), zero3, 1e-5);
// }
-
-TEST_F(FactorInertialKinematics_2KF_ForceX_Odom3d,ZeroMvt)
+TEST_F(FactorInertialKinematics_2KF_ForceX_Odom3d, ZeroMvt)
{
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ string report =
+ solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
perturbateAllIfUnFixed(KFA_);
perturbateAllIfUnFixed(KFB_);
// problem_->print(4,true,true,true);
- report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// std::cout << report << std::endl;
// problem_->print(4,true,true,true);
@@ -830,16 +889,17 @@ TEST_F(FactorInertialKinematics_2KF_ForceX_Odom3d,ZeroMvt)
ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
- ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(
+ KFB_->getStateBlock('C')->getState(), (Vector3d() << (tB_ - tA_) * Acc1 / 2, 0, 0).finished(), 1e-5);
+ ASSERT_MATRIX_APPROX(
+ KFB_->getStateBlock('D')->getState(), (Vector3d() << (tB_ - tA_) * Acc1, 0, 0).finished(), 1e-5);
ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
}
-
-int main(int argc, char **argv)
+int main(int argc, char** argv)
{
- testing::InitGoogleTest(&argc, argv);
+ testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
+ return RUN_ALL_TESTS();
}
diff --git a/test/gtest_factor_inertial_kinematics.cpp b/test/gtest_factor_inertial_kinematics.cpp
index 941699e..03d4728 100644
--- a/test/gtest_factor_inertial_kinematics.cpp
+++ b/test/gtest_factor_inertial_kinematics.cpp
@@ -19,10 +19,11 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
/*
-Organisation: For each test, the problem, sensors, factors (if possible) are instanciated in a base class inheriting fromtesting::Test only.
-Then, each test case is derived in a child class which defines the data processed by the different sensors and expected estimated test values.
-Finally, each of these child classes is used in one or several Test in which basically the expected values are compared against estimation and
-solve is done with a perturbated system.
+Organisation: For each test, the problem, sensors, factors (if possible) are instanciated in a base class inheriting
+fromtesting::Test only. Then, each test case is derived in a child class which defines the data processed by the
+different sensors and expected estimated test values. Finally, each of these child classes is used in one or several
+Test in which basically the expected values are compared against estimation and solve is done with a perturbated
+system.
Tests list:
@@ -32,7 +33,6 @@ FactorInertialKinematics_1KF_m1p_pfix,ZeroMvt:
FactorInertialKinematics_1KF_1v_bfix,ZeroMvt:
*/
-
// debug
#include <iostream>
#include <fstream>
@@ -73,23 +73,23 @@ const Vector6d ZERO6 = Vector6d::Zero();
class FactorInertialKinematics_1KF : public testing::Test
{
- public:
- wolf::TimeStamp t_;
- ProblemPtr problem_;
- // SensorImuPtr sen_imu;
- SolverCeresPtr solver_;
- // ProcessorBasePtr processor;
- // ProcessorImuPtr processor_imu;
- VectorComposite x_origin_; // initial state
- VectorComposite s_origin_; // initial sigmas for prior
- FrameBasePtr KF0_;
- SensorInertialKinematicsPtr SIK_;
- CaptureInertialKinematicsPtr CIK0_;
- Eigen::Matrix3d Qp_, Qv_, Qw_;
- FeatureInertialKinematicsPtr feat_in_;
- StateBlockPtr sbbimu_;
- Vector3d bias_p_;
- Vector6d bias_imu_;
+ public:
+ wolf::TimeStamp t_;
+ ProblemPtr problem_;
+ // SensorImuPtr sen_imu;
+ SolverCeresPtr solver_;
+ // ProcessorBasePtr processor;
+ // ProcessorImuPtr processor_imu;
+ VectorComposite x_origin_; // initial state
+ VectorComposite s_origin_; // initial sigmas for prior
+ FrameBasePtr KF0_;
+ SensorInertialKinematicsPtr SIK_;
+ CaptureInertialKinematicsPtr CIK0_;
+ Eigen::Matrix3d Qp_, Qv_, Qw_;
+ FeatureInertialKinematicsPtr feat_in_;
+ StateBlockPtr sbbimu_;
+ Vector3d bias_p_;
+ Vector6d bias_imu_;
void SetUp() override
{
@@ -104,8 +104,9 @@ class FactorInertialKinematics_1KF : public testing::Test
// solver_->getSolverOptions().max_num_iterations = 1e4;
//===================================================== INITIALIZATION
- 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)});
+ 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_);
@@ -113,13 +114,13 @@ class FactorInertialKinematics_1KF : public testing::Test
SIK_ = std::make_shared<SensorInertialKinematics>(Eigen::VectorXd(0), intr_ik);
// ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
- bias_p_ = ZERO3;
- bias_imu_ = ZERO6;
+ bias_p_ = ZERO3;
+ bias_imu_ = ZERO6;
StateBlockPtr sbc = make_shared<StateBlock>(ZERO3);
StateBlockPtr sbd = make_shared<StateBlock>(ZERO3);
StateBlockPtr sbL = make_shared<StateBlock>(ZERO3);
StateBlockPtr sbb = make_shared<StateBlock>(bias_p_);
- sbbimu_ = make_shared<StateBlock>(bias_imu_);
+ sbbimu_ = make_shared<StateBlock>(bias_imu_);
KF0_->addStateBlock('C', sbc, problem_);
KF0_->addStateBlock('D', sbd, problem_);
@@ -133,25 +134,26 @@ class FactorInertialKinematics_1KF : public testing::Test
// Measurements
Vector3d pBC_meas = ZERO3;
Vector3d vBC_meas = ZERO3;
- Vector3d w_meas = ZERO3;
+ Vector3d w_meas = ZERO3;
// momentum parameters
- Matrix3d Iq; Iq.setIdentity();
+ Matrix3d Iq;
+ Iq.setIdentity();
Vector3d Lq = ZERO3;
- Qp_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
- Qv_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
- Qw_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
+ Qp_ = pow(1e-2, 2) * Eigen::Matrix3d::Identity();
+ Qv_ = pow(1e-2, 2) * Eigen::Matrix3d::Identity();
+ Qw_ = pow(1e-2, 2) * Eigen::Matrix3d::Identity();
// =============== CREATE CAPURE
- Vector9d meas_ikin0; meas_ikin0 << pBC_meas, vBC_meas, w_meas;
+ Vector9d meas_ikin0;
+ meas_ikin0 << pBC_meas, vBC_meas, w_meas;
CIK0_ = CaptureBase::emplace<CaptureInertialKinematics>(KF0_, t_, SIK_, meas_ikin0, Iq, Lq, bias_p_);
}
- void TearDown() override{}
+ void TearDown() override {}
};
-
class FactorInertialKinematics_1KF_Meas0_bpfix : public FactorInertialKinematics_1KF
{
void SetUp() override
@@ -164,15 +166,19 @@ class FactorInertialKinematics_1KF_Meas0_bpfix : public FactorInertialKinematics
Eigen::Vector3d pBC_meas = ZERO3;
Eigen::Vector3d vBC_meas = ZERO3;
- Eigen::Vector3d w_meas = ZERO3;
- Eigen::Vector9d meas; meas << pBC_meas, vBC_meas, w_meas;
+ Eigen::Vector3d w_meas = ZERO3;
+ Eigen::Vector9d meas;
+ meas << pBC_meas, vBC_meas, w_meas;
// momentum parameters
- Eigen::Matrix3d Iq; Iq.setIdentity();
+ Eigen::Matrix3d Iq;
+ Iq.setIdentity();
Eigen::Vector3d Lq = ZERO3;
// =============== CREATE FEATURE/FACTOR
- Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>()-bias_p_, meas.tail<3>()-bias_imu_.tail<3>(), Iq);
- feat_in_ = FeatureBase::emplace<FeatureInertialKinematics>(CIK0_, meas, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ Eigen::Matrix9d Q_ikin_err =
+ computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>() - bias_p_, meas.tail<3>() - bias_imu_.tail<3>(), Iq);
+ feat_in_ = FeatureBase::emplace<FeatureInertialKinematics>(
+ CIK0_, meas, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu_, nullptr, false);
}
};
@@ -187,17 +193,22 @@ class FactorInertialKinematics_1KF_1p_bpfix : public FactorInertialKinematics_1K
KF0_->getStateBlock('C')->unfix();
Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
- Eigen::Vector3d pBC_meas; pBC_meas << 1,0,0;
+ Eigen::Vector3d pBC_meas;
+ pBC_meas << 1, 0, 0;
Eigen::Vector3d vBC_meas = ZERO3;
- Eigen::Vector3d w_meas = ZERO3;
- Eigen::Vector9d meas; meas << pBC_meas, vBC_meas, w_meas;
+ Eigen::Vector3d w_meas = ZERO3;
+ Eigen::Vector9d meas;
+ meas << pBC_meas, vBC_meas, w_meas;
// momentum parameters
- Eigen::Matrix3d Iq; Iq.setIdentity();
+ Eigen::Matrix3d Iq;
+ Iq.setIdentity();
Eigen::Vector3d Lq = ZERO3;
// =============== CREATE FEATURE/FACTOR
- Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>()-bias_p_, meas.tail<3>()-bias_imu_.tail<3>(), Iq);
- feat_in_ = FeatureBase::emplace<FeatureInertialKinematics>(CIK0_, meas, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ Eigen::Matrix9d Q_ikin_err =
+ computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>() - bias_p_, meas.tail<3>() - bias_imu_.tail<3>(), Iq);
+ feat_in_ = FeatureBase::emplace<FeatureInertialKinematics>(
+ CIK0_, meas, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu_, nullptr, false);
}
};
@@ -212,18 +223,23 @@ class FactorInertialKinematics_1KF_m1p_pfix : public FactorInertialKinematics_1K
KF0_->getStateBlock('C')->fix();
Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
- Eigen::Vector3d pBC_meas; pBC_meas << 1,0,0;
+ Eigen::Vector3d pBC_meas;
+ pBC_meas << 1, 0, 0;
Eigen::Vector3d vBC_meas = ZERO3;
- Eigen::Vector3d w_meas = ZERO3;
- bias_p_ << 1,0,0;
- Eigen::Vector9d meas; meas << pBC_meas, vBC_meas, w_meas;
+ Eigen::Vector3d w_meas = ZERO3;
+ bias_p_ << 1, 0, 0;
+ Eigen::Vector9d meas;
+ meas << pBC_meas, vBC_meas, w_meas;
// momentum parameters
- Eigen::Matrix3d Iq; Iq.setIdentity();
+ Eigen::Matrix3d Iq;
+ Iq.setIdentity();
Eigen::Vector3d Lq = ZERO3;
// =============== CREATE FEATURE/FACTOR
- Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>()-bias_p_, meas.tail<3>()-bias_imu_.tail<3>(), Iq);
- feat_in_ = FeatureBase::emplace<FeatureInertialKinematics>(CIK0_, meas, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ Eigen::Matrix9d Q_ikin_err =
+ computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>() - bias_p_, meas.tail<3>() - bias_imu_.tail<3>(), Iq);
+ feat_in_ = FeatureBase::emplace<FeatureInertialKinematics>(
+ CIK0_, meas, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu_, nullptr, false);
}
};
@@ -241,22 +257,26 @@ class FactorInertialKinematics_1KF_1v_bfix : public FactorInertialKinematics_1KF
CIK0_->getStateBlock('I')->setState(bias_p_);
Eigen::Vector3d pBC_meas = ZERO3;
- Eigen::Vector3d vBC_meas; vBC_meas << 1,0,0;
+ Eigen::Vector3d vBC_meas;
+ vBC_meas << 1, 0, 0;
Eigen::Vector3d w_meas = ZERO3;
- Eigen::Vector9d meas; meas << pBC_meas, vBC_meas, w_meas;
+ Eigen::Vector9d meas;
+ meas << pBC_meas, vBC_meas, w_meas;
// momentum parameters
- Eigen::Matrix3d Iq; Iq.setIdentity();
+ Eigen::Matrix3d Iq;
+ Iq.setIdentity();
Eigen::Vector3d Lq = ZERO3;
// =============== CREATE FEATURE/FACTOR
- Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>()-bias_p_, meas.tail<3>()-bias_imu_.tail<3>(), Iq);
- feat_in_ = FeatureBase::emplace<FeatureInertialKinematics>(CIK0_, meas, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ Eigen::Matrix9d Q_ikin_err =
+ computeIKinCov(Qp_, Qv_, Qw_, meas.head<3>() - bias_p_, meas.tail<3>() - bias_imu_.tail<3>(), Iq);
+ feat_in_ = FeatureBase::emplace<FeatureInertialKinematics>(
+ CIK0_, meas, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu_, nullptr, false);
}
};
-
-//class FactorInertialKinematic_imu : public testing::Test
+// class FactorInertialKinematic_imu : public testing::Test
//{
// public:
// wolf::TimeStamp t_;
@@ -301,15 +321,17 @@ class FactorInertialKinematics_1KF_1v_bfix : public FactorInertialKinematics_1KF
// KF0_ = problem_->setPrior(x_origin_, P_origin_.topLeftCorner<6,6>(), t_, 0.005);
// // Specific factor for origin velocity (not covered by setPrior)
// CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t_);
-// FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin_.tail(3), P_origin_.bottomRightCorner<3,3>());
-// FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV());
+// FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin_.tail(3),
+// P_origin_.bottomRightCorner<3,3>()); FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0,
+// KF0_->getV());
//
//
// // SENSOR + PROCESSOR Imu
-// SensorBasePtr sen0_ptr = problem_->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), bodydynamics_root + "/demos/sensor_imu.yaml");
-// sen_imu_ = std::static_pointer_cast<SensorImu>(sen0_ptr);
-// ProcessorBasePtr proc = problem_->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", bodydynamics_root + "/demos/processor_imu.yaml");
-// processor_imu_ = std::static_pointer_cast<ProcessorImu>(proc);
+// SensorBasePtr sen0_ptr = problem_->installSensor("SensorImu", "Main Imu",
+// (Vector7d()<<0,0,0,0,0,0,1).finished(), bodydynamics_root + "/demos/sensor_imu.yaml"); sen_imu_ =
+// std::static_pointer_cast<SensorImu>(sen0_ptr); ProcessorBasePtr proc =
+// problem_->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", bodydynamics_root +
+// "/demos/processor_imu.yaml"); processor_imu_ = std::static_pointer_cast<ProcessorImu>(proc);
//
//
// // ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
@@ -358,22 +380,21 @@ class FactorInertialKinematics_1KF_1v_bfix : public FactorInertialKinematics_1KF
// // =============== CREATE CAPURE/FEATURE/FACTOR
// Vector9d meas; meas << pBC_meas, vBC_meas, w_meas;
// CaptureBasePtr cap = CaptureBase::emplace<CaptureBase>(KF0_, "CaptureInertialKinematics", t_, nullptr);
-// auto feat = FeatureBase::emplace<FeatureInertialKinematics>(cap, meas, Qkin, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
-// feat_in_ = static_pointer_cast<FeatureInertialKinematics>(feat); // !! auto feat_in_ creates no compilation error but a segfault
+// auto feat = FeatureBase::emplace<FeatureInertialKinematics>(cap, meas, Qkin, Iq, Lq,
+// FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE); feat_in_ =
+// static_pointer_cast<FeatureInertialKinematics>(feat); // !! auto feat_in_ creates no compilation error but a
+// segfault
// // auto fac = FactorBase::emplace<FactorInertialKinematics>(feat_in_, feat_in_, sbbimu_, nullptr, false);
// }
//
// virtual void TearDown(){}
//};
-
-
////////////////////////////////////////////////////////
// =============== TEST FONCTIONS ======================
////////////////////////////////////////////////////////
-
-TEST_F(FactorInertialKinematics_1KF_Meas0_bpfix,ZeroMvt)
+TEST_F(FactorInertialKinematics_1KF_Meas0_bpfix, ZeroMvt)
{
Eigen::Vector3d c_exp = Eigen::Vector3d::Zero();
Eigen::Vector3d cd_exp = Eigen::Vector3d::Zero();
@@ -381,7 +402,8 @@ TEST_F(FactorInertialKinematics_1KF_Meas0_bpfix,ZeroMvt)
// Eigen::Vector3d bp_exp = Eigen::Vector3d::Zero();
KF0_->perturb();
- string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ string report =
+ solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
ASSERT_MATRIX_APPROX(KF0_->getP()->getState(), x_origin_.at('P'), 1e-5);
ASSERT_MATRIX_APPROX(KF0_->getStateBlock('C')->getState(), c_exp, 1e-5);
@@ -389,31 +411,34 @@ TEST_F(FactorInertialKinematics_1KF_Meas0_bpfix,ZeroMvt)
ASSERT_MATRIX_APPROX(KF0_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
}
-
-TEST_F(FactorInertialKinematics_1KF_1p_bpfix,ZeroMvt)
+TEST_F(FactorInertialKinematics_1KF_1p_bpfix, ZeroMvt)
{
- Eigen::Vector3d c_exp; c_exp << 1,0,0;
+ Eigen::Vector3d c_exp;
+ c_exp << 1, 0, 0;
Eigen::Vector3d cd_exp = Eigen::Vector3d::Zero();
Eigen::Vector3d Lc_exp = Eigen::Vector3d::Zero();
// Eigen::Vector3d bp_exp = Eigen::Vector3d::Zero();
KF0_->perturb();
- std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::string report =
+ solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
ASSERT_MATRIX_APPROX(KF0_->getStateBlock('C')->getState(), c_exp, 1e-5);
ASSERT_MATRIX_APPROX(KF0_->getStateBlock('D')->getState(), cd_exp, 1e-5);
ASSERT_MATRIX_APPROX(KF0_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
}
-TEST_F(FactorInertialKinematics_1KF_m1p_pfix,ZeroMvt)
+TEST_F(FactorInertialKinematics_1KF_m1p_pfix, ZeroMvt)
{
// Eigen::Vector3d c_exp = Eigen::Vector3d::Zero();
Eigen::Vector3d cd_exp = Eigen::Vector3d::Zero();
Eigen::Vector3d Lc_exp = Eigen::Vector3d::Zero();
- Eigen::Vector3d bp_exp; bp_exp << 1,0,0;
+ Eigen::Vector3d bp_exp;
+ bp_exp << 1, 0, 0;
KF0_->perturb();
- std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::string report =
+ solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// ASSERT_MATRIX_APPROX(KF0_->getStateBlock('C')->getState(), c_exp, 1e-5);
ASSERT_MATRIX_APPROX(KF0_->getStateBlock('D')->getState(), cd_exp, 1e-5);
@@ -421,25 +446,26 @@ TEST_F(FactorInertialKinematics_1KF_m1p_pfix,ZeroMvt)
ASSERT_MATRIX_APPROX(CIK0_->getStateBlock('I')->getState(), bp_exp, 1e-5);
}
-TEST_F(FactorInertialKinematics_1KF_1v_bfix,ZeroMvt)
+TEST_F(FactorInertialKinematics_1KF_1v_bfix, ZeroMvt)
{
- Eigen::Vector3d c_exp = Eigen::Vector3d::Zero();
- Eigen::Vector3d cd_exp; cd_exp << 1,0,0;
+ Eigen::Vector3d c_exp = Eigen::Vector3d::Zero();
+ Eigen::Vector3d cd_exp;
+ cd_exp << 1, 0, 0;
Eigen::Vector3d Lc_exp = Eigen::Vector3d::Zero();
// Eigen::Vector3d bp_exp = Eigen::Vector3d::Zero();
KF0_->perturb();
- std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::string report =
+ solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
ASSERT_MATRIX_APPROX(KF0_->getStateBlock('C')->getState(), c_exp, 1e-5);
ASSERT_MATRIX_APPROX(KF0_->getStateBlock('D')->getState(), cd_exp, 1e-5);
ASSERT_MATRIX_APPROX(KF0_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
}
-
int main(int argc, char **argv)
{
- testing::InitGoogleTest(&argc, argv);
+ testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
+ return RUN_ALL_TESTS();
}
diff --git a/test/gtest_feature_inertial_kinematics.cpp b/test/gtest_feature_inertial_kinematics.cpp
index 41a3ff1..03547ee 100644
--- a/test/gtest_feature_inertial_kinematics.cpp
+++ b/test/gtest_feature_inertial_kinematics.cpp
@@ -28,32 +28,32 @@
using namespace wolf;
using namespace Eigen;
-
TEST(FeatureInertialKinematics, jacobian_covariance_and_constructor)
-{
- Vector3d pBC_meas = Vector3d::Random();
+{
+ Vector3d pBC_meas = Vector3d::Random();
Vector3d vBC_meas = Vector3d::Random();
- Vector3d w_meas = Vector3d::Random();
-
- Eigen::Matrix3d Qp = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
- Eigen::Matrix3d Qv = pow(2e-2, 2)*Eigen::Matrix3d::Identity();
- Eigen::Matrix3d Qw = pow(3e-2, 2)*Eigen::Matrix3d::Identity();
-
+ Vector3d w_meas = Vector3d::Random();
+
+ Eigen::Matrix3d Qp = pow(1e-2, 2) * Eigen::Matrix3d::Identity();
+ Eigen::Matrix3d Qv = pow(2e-2, 2) * Eigen::Matrix3d::Identity();
+ Eigen::Matrix3d Qw = pow(3e-2, 2) * Eigen::Matrix3d::Identity();
+
// momentum parameters
- Matrix3d Iq = Matrix3d::Identity();
- Vector3d Lq = Vector3d::Random();
- Vector9d meas_ikin; meas_ikin << pBC_meas, vBC_meas, w_meas;
-
+ Matrix3d Iq = Matrix3d::Identity();
+ Vector3d Lq = Vector3d::Random();
+ Vector9d meas_ikin;
+ meas_ikin << pBC_meas, vBC_meas, w_meas;
+
// direct computation of error covariance (analytical formula of first order propagation)
Eigen::Matrix9d Q_ikin_err = computeIKinCov(Qp, Qv, Qw, pBC_meas, w_meas, Iq);
// covariance from the first order propagation (using jacobian analytical formula)
- Matrix9d J_err_noise = computeIKinJac(pBC_meas, w_meas, Iq);
- Eigen::Matrix9d Q_meas = Eigen::Matrix9d::Zero();
- Q_meas.block<3,3>(0,0) = Qp;
- Q_meas.block<3,3>(3,3) = Qv;
- Q_meas.block<3,3>(6,6) = Qw;
- Eigen::Matrix9d Q_ikin_err_from_Jc = J_err_noise * Q_meas * J_err_noise.transpose();
+ Matrix9d J_err_noise = computeIKinJac(pBC_meas, w_meas, Iq);
+ Eigen::Matrix9d Q_meas = Eigen::Matrix9d::Zero();
+ Q_meas.block<3, 3>(0, 0) = Qp;
+ Q_meas.block<3, 3>(3, 3) = Qv;
+ Q_meas.block<3, 3>(6, 6) = Qw;
+ Eigen::Matrix9d Q_ikin_err_from_Jc = J_err_noise * Q_meas * J_err_noise.transpose();
FeatureInertialKinematicsPtr feat_ptr = std::make_shared<FeatureInertialKinematics>(meas_ikin, Q_ikin_err, Iq, Lq);
// Test if analytic covariance matches with the explicit jacobian propagation formula
@@ -62,18 +62,16 @@ TEST(FeatureInertialKinematics, jacobian_covariance_and_constructor)
// Simple constructor tests
ASSERT_MATRIX_APPROX(Iq, feat_ptr->getBIq(), 1e-3);
ASSERT_MATRIX_APPROX(Lq, feat_ptr->getBLcm(), 1e-3);
- Iq = 2*Matrix3d::Identity();
- Lq = Vector3d::Random();
+ Iq = 2 * Matrix3d::Identity();
+ Lq = Vector3d::Random();
feat_ptr->setBIq(Iq);
feat_ptr->setBLcm(Lq);
ASSERT_MATRIX_APPROX(Iq, feat_ptr->getBIq(), 1e-3);
ASSERT_MATRIX_APPROX(Lq, feat_ptr->getBLcm(), 1e-3);
-
}
int main(int argc, char **argv)
{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
}
-
diff --git a/test/gtest_force_torque_delta_tools.cpp b/test/gtest_force_torque_delta_tools.cpp
index cef2d67..4563957 100644
--- a/test/gtest_force_torque_delta_tools.cpp
+++ b/test/gtest_force_torque_delta_tools.cpp
@@ -43,7 +43,7 @@ TEST(FT_tools, identity)
TEST(FT_tools, identity_split)
{
- VectorXd c(3), cd(3), Lc(3), qv(4);
+ VectorXd c(3), cd(3), Lc(3), qv(4);
Quaterniond q;
identity(c, cd, Lc, q);
@@ -55,37 +55,37 @@ TEST(FT_tools, identity_split)
identity(c, cd, Lc, qv);
ASSERT_MATRIX_APPROX(c, Vector3d::Zero(), 1e-10);
ASSERT_MATRIX_APPROX(cd, Vector3d::Zero(), 1e-10);
- ASSERT_MATRIX_APPROX(Lc, Vector3d::Zero(), 1e-10);
- ASSERT_MATRIX_APPROX(qv, (Vector4d()<<0,0,0,1).finished(), 1e-10);
+ ASSERT_MATRIX_APPROX(Lc, Vector3d::Zero(), 1e-10);
+ ASSERT_MATRIX_APPROX(qv, (Vector4d() << 0, 0, 0, 1).finished(), 1e-10);
}
TEST(FT_tools, inverse)
{
VectorXd d(13), id(13), iid(13), iiid(13), I(13);
Vector4d qv;
- double dt = 0.1;
+ double dt = 0.1;
qv = (Vector4d() << 9, 10, 11, 12).finished().normalized();
d << 0, 1, 2, 3, 4, 5, 6, 7, 8, qv;
- id = inverse(d, dt);
+ id = inverse(d, dt);
compose(id, d, dt, I);
ASSERT_MATRIX_APPROX(I, identity(), 1e-10);
- compose(d, id, -dt, I); // Observe -dt is reversed !!
+ compose(d, id, -dt, I); // Observe -dt is reversed !!
ASSERT_MATRIX_APPROX(I, identity(), 1e-10);
- inverse(id, -dt, iid); // Observe -dt is reversed !!
- ASSERT_MATRIX_APPROX( d, iid, 1e-10);
+ inverse(id, -dt, iid); // Observe -dt is reversed !!
+ ASSERT_MATRIX_APPROX(d, iid, 1e-10);
iiid = inverse(iid, dt);
ASSERT_MATRIX_APPROX(id, iiid, 1e-10);
}
TEST(FT_tools, compose_between)
{
- VectorXd dx1(13), dx2(13), dx3(13);
+ VectorXd dx1(13), dx2(13), dx3(13);
Matrix<double, 13, 1> d1, d2, d3;
- double dt = 0.1;
+ double dt = 0.1;
dx1 << Vector9d::Random(), Vector4d::Random().normalized();
d1 = dx1;
@@ -102,8 +102,8 @@ TEST(FT_tools, compose_between)
ASSERT_MATRIX_APPROX(d3, dx3, 1e-10);
// // minus(d1, d3) should recover delta_2
- VectorXd diffx(13);
- Matrix<double,13,1> diff;
+ VectorXd diffx(13);
+ Matrix<double, 13, 1> diff;
between(d1, d3, dt, diff);
ASSERT_MATRIX_APPROX(diff, d2, 1e-10);
@@ -115,7 +115,7 @@ TEST(FT_tools, compose_between)
TEST(FT_tools, compose_between_with_state)
{
VectorXd x(13), d(13), x2(13), x3(13), d2(13), d3(13);
- double dt = 0.1;
+ double dt = 0.1;
x << Vector9d::Random(), Vector4d::Random().normalized();
d << Vector9d::Random(), Vector4d::Random().normalized();
@@ -140,17 +140,19 @@ TEST(FT_tools, compose_between_with_state)
TEST(FT_tools, lift_retract)
{
- VectorXd d_min(12); d_min << Vector9d::Random(), .1*Vector3d::Random(); // use angles in the ball theta < pi
+ VectorXd d_min(12);
+ d_min << Vector9d::Random(), .1 * Vector3d::Random(); // use angles in the ball theta < pi
// transform to delta
VectorXd delta = exp_FT(d_min);
// expected delta
- Vector3d dc = d_min.head(3);
- Vector3d dcd = d_min.segment<3>(3);
- Vector3d dLc = d_min.segment<3>(6);
- Quaterniond dq = v2q(d_min.segment<3>(9));
- VectorXd delta_true(13); delta_true << dc, dcd, dLc, dq.coeffs();
+ Vector3d dc = d_min.head(3);
+ Vector3d dcd = d_min.segment<3>(3);
+ Vector3d dLc = d_min.segment<3>(6);
+ Quaterniond dq = v2q(d_min.segment<3>(9));
+ VectorXd delta_true(13);
+ delta_true << dc, dcd, dLc, dq.coeffs();
ASSERT_MATRIX_APPROX(delta, delta_true, 1e-10);
// transform to a new tangent -- should be the original tangent
@@ -167,13 +169,13 @@ TEST(FT_tools, plus)
VectorXd d1(13), d2(13), d3(13);
VectorXd err(12);
Vector4d qv = Vector4d::Random().normalized();
- d1 << Vector9d::Random(), qv;
+ d1 << Vector9d::Random(), qv;
err << Matrix<double, 12, 1>::Random();
- d3.head<3>() = d1.head<3>() + err.head<3>();
+ d3.head<3>() = d1.head<3>() + err.head<3>();
d3.segment<3>(3) = d1.segment<3>(3) + err.segment<3>(3);
d3.segment<3>(6) = d1.segment<3>(6) + err.segment<3>(6);
- d3.tail<4>() = (Quaterniond(qv.data()) * exp_q(err.tail<3>())).coeffs();
+ d3.tail<4>() = (Quaterniond(qv.data()) * exp_q(err.tail<3>())).coeffs();
plus(d1, err, d2);
ASSERT_MATRIX_APPROX(diff(d3, d2), VectorXd::Zero(12), 1e-10);
@@ -184,7 +186,7 @@ TEST(FT_tools, diff)
VectorXd d1(13), d2(13);
VectorXd err(12);
Vector4d qv = Vector4d::Random().normalized();
- d1 << Vector9d::Random(), qv;
+ d1 << Vector9d::Random(), qv;
d2 = d1;
diff(d1, d2, err);
@@ -192,24 +194,25 @@ TEST(FT_tools, diff)
ASSERT_MATRIX_APPROX(diff(d1, d2), VectorXd::Zero(12), 1e-10);
VectorXd d3(13);
- d3.setRandom(); d3.tail<4>().normalize();
- err.head<3>() = d3.head<3>() - d1.head<3>();
+ d3.setRandom();
+ d3.tail<4>().normalize();
+ err.head<3>() = d3.head<3>() - d1.head<3>();
err.segment<3>(3) = d3.segment<3>(3) - d1.segment<3>(3);
- err.segment<3>(6) = d3.segment<3>(6) - d1.segment<3>(6);
- err.tail<3>() = log_q(Quaterniond(d1.data()+9).conjugate()*Quaterniond(d3.data()+9));
+ err.segment<3>(6) = d3.segment<3>(6) - d1.segment<3>(6);
+ err.tail<3>() = log_q(Quaterniond(d1.data() + 9).conjugate() * Quaterniond(d3.data() + 9));
ASSERT_MATRIX_APPROX(err, diff(d1, d3), 1e-10);
}
TEST(FT_tools, compose_jacobians)
{
- VectorXd d1(13), d2(13), d3(13), d1_pert(13), d2_pert(13), d3_pert(13); // deltas
- VectorXd t1(12), t2(12); // tangent elements
+ VectorXd d1(13), d2(13), d3(13), d1_pert(13), d2_pert(13), d3_pert(13); // deltas
+ VectorXd t1(12), t2(12); // tangent elements
Matrix<double, 12, 12> J1_a, J2_a, J1_n, J2_n;
- Vector4d qv1, qv2;
- double dt = 0.1;
- double dx = 1e-6;
- qv1 = Vector4d::Random().normalized();
+ Vector4d qv1, qv2;
+ double dt = 0.1;
+ double dx = 1e-6;
+ qv1 = Vector4d::Random().normalized();
d1 << Vector9d::Random(), qv1;
qv2 = Vector4d::Random().normalized();
d2 << Vector9d::Random(), qv2;
@@ -218,22 +221,22 @@ TEST(FT_tools, compose_jacobians)
compose(d1, d2, dt, d3, J1_a, J2_a);
// numerical jacobians
- for (unsigned int i = 0; i<12; i++)
+ for (unsigned int i = 0; i < 12; i++)
{
- t1 . setZero();
- t1(i) = dx;
+ t1.setZero();
+ t1(i) = dx;
// Jac wrt first delta
- d1_pert = plus(d1, t1); // (d1 + t1)
- d3_pert = compose(d1_pert, d2, dt); // (d1 + t1) + d2
- t2 = diff(d3, d3_pert); // { (d2 + t1) + d2 } - { d1 + d2 }
- J1_n.col(i) = t2/dx; // [ { (d2 + t1) + d2 } - { d1 + d2 } ] / dx
+ d1_pert = plus(d1, t1); // (d1 + t1)
+ d3_pert = compose(d1_pert, d2, dt); // (d1 + t1) + d2
+ t2 = diff(d3, d3_pert); // { (d2 + t1) + d2 } - { d1 + d2 }
+ J1_n.col(i) = t2 / dx; // [ { (d2 + t1) + d2 } - { d1 + d2 } ] / dx
// Jac wrt second delta
- d2_pert = plus(d2, t1); // (d2 + t1)
- d3_pert = compose(d1, d2_pert, dt); // d1 + (d2 + t1)
- t2 = diff(d3, d3_pert); // { d1 + (d2 + t1) } - { d1 + d2 }
- J2_n.col(i) = t2/dx; // [ { d1 + (d2 + t1) } - { d1 + d2 } ] / dx
+ d2_pert = plus(d2, t1); // (d2 + t1)
+ d3_pert = compose(d1, d2_pert, dt); // d1 + (d2 + t1)
+ t2 = diff(d3, d3_pert); // { d1 + (d2 + t1) } - { d1 + d2 }
+ J2_n.col(i) = t2 / dx; // [ { d1 + (d2 + t1) } - { d1 + d2 } ] / dx
}
// check that numerical and analytical match
@@ -243,12 +246,12 @@ TEST(FT_tools, compose_jacobians)
TEST(FT_tools, diff_jacobians)
{
- VectorXd d1(13), d2(13), diff3(12), d1_pert(13), d2_pert(13), diff3_pert(12); // deltas
- VectorXd t1(12), t2(12); // tangents
+ VectorXd d1(13), d2(13), diff3(12), d1_pert(13), d2_pert(13), diff3_pert(12); // deltas
+ VectorXd t1(12), t2(12); // tangents
Matrix<double, 12, 12> J1_a, J2_a, J1_n, J2_n;
- Vector4d qv1, qv2;
- double dx = 1e-6;
- qv1 = Vector4d::Random().normalized();
+ Vector4d qv1, qv2;
+ double dx = 1e-6;
+ qv1 = Vector4d::Random().normalized();
d1 << Vector9d::Random(), qv1;
qv2 = Vector4d::Random().normalized();
d2 << Vector9d::Random(), qv2;
@@ -257,22 +260,22 @@ TEST(FT_tools, diff_jacobians)
diff(d1, d2, diff3, J1_a, J2_a);
// numerical jacobians
- for (unsigned int i = 0; i<12; i++)
+ for (unsigned int i = 0; i < 12; i++)
{
- t1 . setZero();
- t1(i) = dx;
+ t1.setZero();
+ t1(i) = dx;
// Jac wrt first delta
- d1_pert = plus(d1, t1); // (d1 + t1)
- diff3_pert = diff(d1_pert, d2); // d2 - (d1 + t1)
- t2 = diff3_pert - diff3; // { d2 - (d1 + t1) } - { d2 - d1 }
- J1_n.col(i) = t2/dx; // [ { d2 - (d1 + t1) } - { d2 - d1 } ] / dx
+ d1_pert = plus(d1, t1); // (d1 + t1)
+ diff3_pert = diff(d1_pert, d2); // d2 - (d1 + t1)
+ t2 = diff3_pert - diff3; // { d2 - (d1 + t1) } - { d2 - d1 }
+ J1_n.col(i) = t2 / dx; // [ { d2 - (d1 + t1) } - { d2 - d1 } ] / dx
// Jac wrt second delta
- d2_pert = plus(d2, t1); // (d2 + t1)
- diff3_pert = diff(d1, d2_pert); // (d2 + t1) - d1
- t2 = diff3_pert - diff3; // { (d2 + t1) - d1 } - { d2 - d1 }
- J2_n.col(i) = t2/dx; // [ { (d2 + t1) - d1 } - { d2 - d1 } ] / dx
+ d2_pert = plus(d2, t1); // (d2 + t1)
+ diff3_pert = diff(d1, d2_pert); // (d2 + t1) - d1
+ t2 = diff3_pert - diff3; // { (d2 + t1) - d1 } - { d2 - d1 }
+ J2_n.col(i) = t2 / dx; // [ { (d2 + t1) - d1 } - { d2 - d1 } ] / dx
}
// check that numerical and analytical match
@@ -282,14 +285,14 @@ TEST(FT_tools, diff_jacobians)
TEST(FT_tools, body2delta_jacobians)
{
- VectorXd delta(13), delta_pert(13); // deltas
- VectorXd body(32), pert(32), body_pert(32);
- VectorXd tang(12); // tangents
- Matrix<double, 12, 30> J_a, J_n; // analytic and numerical jacs
- double dt = 0.1;
- double mass = 1;
- double dx = 1e-6;
- Vector4d qv = Vector4d::Random().normalized();
+ VectorXd delta(13), delta_pert(13); // deltas
+ VectorXd body(32), pert(32), body_pert(32);
+ VectorXd tang(12); // tangents
+ Matrix<double, 12, 30> J_a, J_n; // analytic and numerical jacs
+ double dt = 0.1;
+ double mass = 1;
+ double dx = 1e-6;
+ Vector4d qv = Vector4d::Random().normalized();
delta << Vector9d::Random(), qv;
Vector4d bql1 = Vector4d::Random().normalized();
Vector4d bql2 = Vector4d::Random().normalized();
@@ -300,31 +303,31 @@ TEST(FT_tools, body2delta_jacobians)
// numerical jacobians
J_n.setZero();
- for (unsigned int i = 0; i<18; i++)
+ for (unsigned int i = 0; i < 18; i++)
{
- pert . setZero();
- pert(i) = dx;
+ pert.setZero();
+ pert(i) = dx;
// Jac wrt first delta
body_pert = body + pert;
- delta_pert = body2delta(body_pert, dt, mass, 2, 6); // delta(body + pert)
- tang = diff(delta, delta_pert); // delta(body + pert) -- delta(body)
- J_n.col(i) = tang/dx; // ( delta(body + pert) -- delta(body) ) / dx
+ delta_pert = body2delta(body_pert, dt, mass, 2, 6); // delta(body + pert)
+ tang = diff(delta, delta_pert); // delta(body + pert) -- delta(body)
+ J_n.col(i) = tang / dx; // ( delta(body + pert) -- delta(body) ) / dx
}
// check that numerical and analytical match
- // ASSERT_MATRIX_APPROX(J_a.block(0,0,12,18), J_n.block(0,0,12,18), 1e-4); // only compare the implemented parts -> kinematics uncertainty not considered here
+ // ASSERT_MATRIX_APPROX(J_a.block(0,0,12,18), J_n.block(0,0,12,18), 1e-4); // only compare the implemented parts
+ // -> kinematics uncertainty not considered here
ASSERT_MATRIX_APPROX(J_a, J_n, 1e-4);
}
-
TEST(FT_tools, debiasData_jacobians)
{
- VectorXd data(32), bias(6), bias_pert(6), pert(6), body(32), body_pert(32), tang(32);
- Matrix<double, 30, 6> J_a, J_n; // analytic and numerical jacs
- double dx = 1e-6;
- Vector4d bql1 = Vector4d::Random().normalized();
- Vector4d bql2 = Vector4d::Random().normalized();
+ VectorXd data(32), bias(6), bias_pert(6), pert(6), body(32), body_pert(32), tang(32);
+ Matrix<double, 30, 6> J_a, J_n; // analytic and numerical jacs
+ double dx = 1e-6;
+ Vector4d bql1 = Vector4d::Random().normalized();
+ Vector4d bql2 = Vector4d::Random().normalized();
data << Matrix<double, 24, 1>::Random(), bql1, bql2; //
bias << Vector6d::Random();
@@ -333,29 +336,27 @@ TEST(FT_tools, debiasData_jacobians)
// numerical jacobian
J_n.setZero();
- for (unsigned int i = 0; i<6; i++)
+ for (unsigned int i = 0; i < 6; i++)
{
- pert . setZero();
- pert(i) = dx;
+ pert.setZero();
+ pert(i) = dx;
// Jac wrt first delta
- bias_pert = bias + pert;
+ bias_pert = bias + pert;
debiasData(data, bias_pert, 2, 6, body_pert);
- // The next line is actually a cheating:
- // we know that the quaternions at the end of the body vector are not influenced by the
+ // The next line is actually a cheating:
+ // we know that the quaternions at the end of the body vector are not influenced by the
// bias perturbation so we just take the euclidean difference of the 2 vectors and remove the last 2 elements
// to get the tangent space vector
- tang = (body_pert - body).segment<30>(0);
+ tang = (body_pert - body).segment<30>(0);
- J_n.col(i) = tang/dx;
+ J_n.col(i) = tang / dx;
}
// check that numerical and analytical match
ASSERT_MATRIX_APPROX(J_a, J_n, 1e-4);
}
-
-
// TEST(motion2data, zero)
// {
// Vector6d motion;
@@ -459,7 +460,8 @@ TEST(FT_tools, debiasData_jacobians)
// * Output:
// * return: the preintegrated delta
// */
-// VectorXd integrateDelta(int N, const Quaterniond& q0, const VectorXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt)
+// VectorXd integrateDelta(int N, const Quaterniond& q0, const VectorXd& motion, const VectorXd& bias_real, const
+// VectorXd& bias_preint, double dt)
// {
// VectorXd data(6);
// VectorXd body(6);
@@ -490,7 +492,8 @@ TEST(FT_tools, debiasData_jacobians)
// * J_D_b: the Jacobian of the preintegrated delta wrt the bias
// * return: the preintegrated delta
// */
-// VectorXd integrateDelta(int N, const Quaterniond& q0, const VectorXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt, Matrix<double, 9, 6>& J_D_b)
+// VectorXd integrateDelta(int N, const Quaterniond& q0, const VectorXd& motion, const VectorXd& bias_real, const
+// VectorXd& bias_preint, double dt, Matrix<double, 9, 6>& J_D_b)
// {
// VectorXd data(6);
// VectorXd body(6);
@@ -683,8 +686,7 @@ TEST(FT_tools, debiasData_jacobians)
int main(int argc, char **argv)
{
- testing::InitGoogleTest(&argc, argv);
-// ::testing::GTEST_FLAG(filter) = "FT_tools.delta_correction";
- return RUN_ALL_TESTS();
+ testing::InitGoogleTest(&argc, argv);
+ // ::testing::GTEST_FLAG(filter) = "FT_tools.delta_correction";
+ return RUN_ALL_TESTS();
}
-
diff --git a/test/gtest_processor_force_torque_preint.cpp b/test/gtest_processor_force_torque_preint.cpp
index 0fb5a73..5a77efe 100644
--- a/test/gtest_processor_force_torque_preint.cpp
+++ b/test/gtest_processor_force_torque_preint.cpp
@@ -64,27 +64,26 @@ const Vector3d ONES3 = Vector3d::Ones();
const Vector3d BIAS_PBC_SMAL = {0.01, 0.02, 0.03};
-const double ACC = 1;
-const double MASS = 1;
-const double DT = 1.0;
-const Vector3d PBCX = {-0.1, 0, 0}; // X axis offset
-const Vector3d PBCY = {0, -0.1, 0}; // Y axis offset
+const double ACC = 1;
+const double MASS = 1;
+const double DT = 1.0;
+const Vector3d PBCX = {-0.1, 0, 0}; // X axis offset
+const Vector3d PBCY = {0, -0.1, 0}; // Y axis offset
const Vector3d PBCZ = {0, 0, -0.1}; // Z axis offset
-
class ProcessorForceTorquePreintImuOdom3dIkin2KF : public testing::Test
{
-public:
- wolf::TimeStamp t0_;
- ProblemPtr problem_;
- SensorImuPtr sen_imu_;
- SolverCeresPtr solver_;
- SensorInertialKinematicsPtr sen_ikin_;
- SensorForceTorquePtr sen_ft_;
- ProcessorImuPtr proc_imu_;
+ public:
+ wolf::TimeStamp t0_;
+ ProblemPtr problem_;
+ SensorImuPtr sen_imu_;
+ SolverCeresPtr solver_;
+ SensorInertialKinematicsPtr sen_ikin_;
+ SensorForceTorquePtr sen_ft_;
+ ProcessorImuPtr proc_imu_;
ProcessorInertialKinematicsPtr proc_inkin_;
- ProcessorForceTorquePreintPtr proc_ftpreint_;
- FrameBasePtr KF1_;
+ ProcessorForceTorquePreintPtr proc_ftpreint_;
+ FrameBasePtr KF1_;
void SetUp() override
{
@@ -101,58 +100,67 @@ public:
//===================================================== INITIALIZATION
// SENSOR + PROCESSOR INERTIAL KINEMATICS
ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
- intr_ik->std_pbc = 0.1;
- intr_ik->std_vbc = 0.1;
- VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
+ intr_ik->std_pbc = 0.1;
+ intr_ik->std_vbc = 0.1;
+ VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
SensorBasePtr sen_ikin_base = problem_->installSensor("SensorInertialKinematics", "SenIK", extr, intr_ik);
- // SensorBasePtr sen_ikin_base = problem_->installSensor("SensorInertialKinematics", "SenIK", extr, bodydynamics_root_dir + "/demos/sensor_inertial_kinematics.yaml"); // TODO: does not work!
+ // SensorBasePtr sen_ikin_base = problem_->installSensor("SensorInertialKinematics", "SenIK", extr,
+ // bodydynamics_root_dir + "/demos/sensor_inertial_kinematics.yaml"); // TODO: does not work!
sen_ikin_ = std::static_pointer_cast<SensorInertialKinematics>(sen_ikin_base);
ParamsProcessorInertialKinematicsPtr params_ik = std::make_shared<ParamsProcessorInertialKinematics>();
- params_ik->sensor_angvel_name = "SenImu";
- params_ik->std_bp_drift = 1e-2;
- ProcessorBasePtr proc_ikin_base = problem_->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", sen_ikin_, params_ik);
- // ProcessorBasePtr proc_ikin_base = problem_->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", "SenIK", bodydynamics_root_dir + "/test/processor_inertial_kinematics.yaml");
+ params_ik->sensor_angvel_name = "SenImu";
+ params_ik->std_bp_drift = 1e-2;
+ ProcessorBasePtr proc_ikin_base =
+ problem_->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", sen_ikin_, params_ik);
+ // ProcessorBasePtr proc_ikin_base = problem_->installProcessor("ProcessorInertialKinematics",
+ // "PInertialKinematics", "SenIK", bodydynamics_root_dir + "/test/processor_inertial_kinematics.yaml");
proc_inkin_ = std::static_pointer_cast<ProcessorInertialKinematics>(proc_ikin_base);
// SENSOR + PROCESSOR IMU
- SensorBasePtr sen_imu_base = problem_->installSensor("SensorImu", "SenImu", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(), bodydynamics_root_dir + "/test/sensor_imu.yaml");
- sen_imu_ = std::static_pointer_cast<SensorImu>(sen_imu_base);
- ProcessorBasePtr proc_ftp_base = problem_->installProcessor("ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/test/processor_imu.yaml");
+ SensorBasePtr sen_imu_base = problem_->installSensor("SensorImu",
+ "SenImu",
+ (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(),
+ bodydynamics_root_dir + "/test/sensor_imu.yaml");
+ sen_imu_ = std::static_pointer_cast<SensorImu>(sen_imu_base);
+ ProcessorBasePtr proc_ftp_base = problem_->installProcessor(
+ "ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/test/processor_imu.yaml");
proc_imu_ = std::static_pointer_cast<ProcessorImu>(proc_ftp_base);
// SENSOR + PROCESSOR FT PREINT
ParamsSensorForceTorquePtr intr_ft = std::make_shared<ParamsSensorForceTorque>();
- intr_ft->std_f = 0.001;
- intr_ft->std_tau = 0.001;
- intr_ft->mass = MASS;
+ intr_ft->std_f = 0.001;
+ intr_ft->std_tau = 0.001;
+ intr_ft->mass = MASS;
SensorBasePtr sen_ft_base = problem_->installSensor("SensorForceTorque", "SenFT", VectorXd(0), intr_ft);
- // SensorBasePtr sen_ft_base = problem_->installSensor("SensorForceTorque", "SenFT", VectorXd(0), bodydynamics_root_dir + "/demos/sensor_ft.yaml");
- sen_ft_ = std::static_pointer_cast<SensorForceTorque>(sen_ft_base);
+ // SensorBasePtr sen_ft_base = problem_->installSensor("SensorForceTorque", "SenFT", VectorXd(0),
+ // bodydynamics_root_dir + "/demos/sensor_ft.yaml");
+ sen_ft_ = std::static_pointer_cast<SensorForceTorque>(sen_ft_base);
ParamsProcessorForceTorquePreintPtr params_sen_ft = std::make_shared<ParamsProcessorForceTorquePreint>();
- params_sen_ft->sensor_ikin_name = "SenIK";
- params_sen_ft->sensor_angvel_name = "SenImu";
- params_sen_ft->nbc = 2;
- params_sen_ft->dimc = 6;
- params_sen_ft->time_tolerance = 0.0005;
- params_sen_ft->unmeasured_perturbation_std = 1e-4;
- params_sen_ft->max_time_span = 1000;
- params_sen_ft->max_buff_length = 500;
- params_sen_ft->dist_traveled = 20000.0;
- params_sen_ft->angle_turned = 1000;
- params_sen_ft->voting_active = true;
- ProcessorBasePtr proc_ft_base = problem_->installProcessor("ProcessorForceTorquePreint", "PFTPreint", sen_ft_, params_sen_ft);
- // ProcessorBasePtr proc_ft_base = problem_->installProcessor("ProcessorForceTorquePreint", "PFTPreint", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint");
+ params_sen_ft->sensor_ikin_name = "SenIK";
+ params_sen_ft->sensor_angvel_name = "SenImu";
+ params_sen_ft->nbc = 2;
+ params_sen_ft->dimc = 6;
+ params_sen_ft->time_tolerance = 0.0005;
+ params_sen_ft->unmeasured_perturbation_std = 1e-4;
+ params_sen_ft->max_time_span = 1000;
+ params_sen_ft->max_buff_length = 500;
+ params_sen_ft->dist_traveled = 20000.0;
+ params_sen_ft->angle_turned = 1000;
+ params_sen_ft->voting_active = true;
+ ProcessorBasePtr proc_ft_base =
+ problem_->installProcessor("ProcessorForceTorquePreint", "PFTPreint", sen_ft_, params_sen_ft);
+ // ProcessorBasePtr proc_ft_base = problem_->installProcessor("ProcessorForceTorquePreint", "PFTPreint",
+ // "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint");
proc_ftpreint_ = std::static_pointer_cast<ProcessorForceTorquePreint>(proc_ft_base);
}
void TearDown() override {}
};
-
class Cst2KFZeroMotion : public ProcessorForceTorquePreintImuOdom3dIkin2KF
{
-public:
+ public:
FrameBasePtr KF2_;
// Unitary data
@@ -174,8 +182,8 @@ public:
Matrix6d cov_tau_;
Matrix3d cov_pbc_;
Matrix3d cov_wb_;
-
- // Aggregated data to construct Captures
+
+ // Aggregated data to construct Captures
Vector6d acc_gyr_meas_;
Matrix6d acc_gyr_cov_;
Vector9d meas_ikin_;
@@ -188,8 +196,6 @@ public:
VectorXd x_origin_;
-
-
Cst2KFZeroMotion()
{
bias_pbc_ = ZERO3;
@@ -205,25 +211,31 @@ public:
{
ProcessorForceTorquePreintImuOdom3dIkin2KF::SetUp();
t0_.set(0.0);
- TimeStamp t1; t1.set(1*DT);
- TimeStamp t2; t2.set(2*DT);
- TimeStamp t3; t3.set(3*DT);
- TimeStamp t4; t4.set(3*DT);
+ TimeStamp t1;
+ t1.set(1 * DT);
+ TimeStamp t2;
+ t2.set(2 * DT);
+ TimeStamp t3;
+ t3.set(3 * DT);
+ TimeStamp t4;
+ t4.set(3 * DT);
// SETPRIOR RETRO-ENGINEERING
// We are not using setPrior because of processors initial captures problems so we have to
// - Manually set problem prior
// - call setOrigin on processors MotionProvider
setOriginState();
- MatrixXd P_origin_ = pow(1e-3, 2) * MatrixXd::Identity(18,18);
- KF1_ = problem_->emplaceFrame( t0_, x_origin_);
+ MatrixXd P_origin_ = pow(1e-3, 2) * MatrixXd::Identity(18, 18);
+ KF1_ = problem_->emplaceFrame(t0_, x_origin_);
// Prior pose factor
- CapturePosePtr pose_prior_capture = CaptureBase::emplace<CapturePose>(KF1_, t0_, nullptr, x_origin_.head(7), P_origin_.topLeftCorner(6, 6));
+ CapturePosePtr pose_prior_capture =
+ CaptureBase::emplace<CapturePose>(KF1_, t0_, nullptr, x_origin_.head(7), P_origin_.topLeftCorner(6, 6));
pose_prior_capture->emplaceFeatureAndFactor();
///////////////////////////////////////////////////
// Prior velocity factor
CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF1_, "Vel0", t0_);
- FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin_.segment<3>(7), P_origin_.block<3, 3>(6, 6));
+ FeatureBasePtr featV0 =
+ FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin_.segment<3>(7), P_origin_.block<3, 3>(6, 6));
FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, featV0, KF1_->getV(), nullptr, false);
initializeData1();
@@ -247,10 +259,8 @@ public:
auto CFTpreint1 = std::make_shared<CaptureForceTorquePreint>(t1, sen_ft_, CIKin1, CImu1, cap_ftp_data_, Qftp_);
CFTpreint1->process();
-
changeForData2();
-
// T2
CaptureImuPtr CImu2 = std::make_shared<CaptureImu>(t2, sen_imu_, acc_gyr_meas_, acc_gyr_cov_);
CImu2->process();
@@ -264,7 +274,8 @@ public:
// T3
CaptureImuPtr CImu3 = std::make_shared<CaptureImu>(t3, sen_imu_, acc_gyr_meas_, acc_gyr_cov_);
CImu3->process();
- CaptureInertialKinematicsPtr CIKin3 = std::make_shared<CaptureInertialKinematics>(t3, sen_ikin_, meas_ikin_, Iq_, Lq_);
+ CaptureInertialKinematicsPtr CIKin3 =
+ std::make_shared<CaptureInertialKinematics>(t3, sen_ikin_, meas_ikin_, Iq_, Lq_);
CIKin3->process();
auto CFTpreint3 = std::make_shared<CaptureForceTorquePreint>(t3, sen_ft_, CIKin3, CImu3, cap_ftp_data_, Qftp_);
CFTpreint3->process();
@@ -272,7 +283,8 @@ public:
// T4, just here to make sure the KF at t3 is created
CaptureImuPtr CImu4 = std::make_shared<CaptureImu>(t4, sen_imu_, acc_gyr_meas_, acc_gyr_cov_);
CImu4->process();
- CaptureInertialKinematicsPtr CIKin4 = std::make_shared<CaptureInertialKinematics>(t4, sen_ikin_, meas_ikin_, Iq_, Lq_);
+ CaptureInertialKinematicsPtr CIKin4 =
+ std::make_shared<CaptureInertialKinematics>(t4, sen_ikin_, meas_ikin_, Iq_, Lq_);
CIKin4->process();
auto CFTpreint4 = std::make_shared<CaptureForceTorquePreint>(t4, sen_ft_, CIKin3, CImu3, cap_ftp_data_, Qftp_);
CFTpreint4->process();
@@ -283,44 +295,51 @@ public:
Matrix6d rel_pose_cov = 1e-6 * Matrix6d::Identity();
KF2_ = problem_->getTrajectory()->getLastFrame();
- CaptureBasePtr cap_pose_base = CaptureBase::emplace<CapturePose>(KF2_, KF2_->getTimeStamp(), nullptr, prev_pose_curr_, rel_pose_cov);
- FeatureBasePtr ftr_odom3d_base = FeatureBase::emplace<FeaturePose>(cap_pose_base, prev_pose_curr_, rel_pose_cov);
+ CaptureBasePtr cap_pose_base =
+ CaptureBase::emplace<CapturePose>(KF2_, KF2_->getTimeStamp(), nullptr, prev_pose_curr_, rel_pose_cov);
+ FeatureBasePtr ftr_odom3d_base =
+ FeatureBase::emplace<FeaturePose>(cap_pose_base, prev_pose_curr_, rel_pose_cov);
FactorBase::emplace<FactorRelativePose3d>(ftr_odom3d_base, ftr_odom3d_base, KF1_, nullptr, false, TOP_MOTION);
////////////////////////////////////////////
// Add absolute factor on Imu biases to simulate a fix()
- Matrix6d Q_bi = 1e-8 * Matrix6d::Identity();
- CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1_, "bi0", t0_);
+ Matrix6d Q_bi = 1e-8 * Matrix6d::Identity();
+ CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1_, "bi0", t0_);
FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", ZERO6, Q_bi);
- FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1_->getCaptureOf(sen_imu_)->getSensorIntrinsic(),nullptr,false);
+ FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(
+ featbi0, featbi0, KF1_->getCaptureOf(sen_imu_)->getSensorIntrinsic(), nullptr, false);
- // Add loose absolute factor on initial bp bias since dynamical trajectories
+ // Add loose absolute factor on initial bp bias since dynamical trajectories
// able to excite the system enough to make COM and bias_p observable hard to generate in a simple gtest
- Matrix3d Q_bp = 1e-1 * Matrix3d::Identity();
- CaptureBasePtr cap_bp0 = CaptureBase::emplace<CaptureBase>(KF1_, "bp0", t0_);
+ Matrix3d Q_bp = 1e-1 * Matrix3d::Identity();
+ CaptureBasePtr cap_bp0 = CaptureBase::emplace<CaptureBase>(KF1_, "bp0", t0_);
FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc_, Q_bp);
- // FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc_ + (Vector3d()<<0.1,0.1,0.1).finished(), Q_bp);
- FactorBasePtr fac_bp0 = FactorBase::emplace<FactorBlockAbsolute>(feat_bp0, feat_bp0, KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic(),nullptr,false);
-
+ // FeatureBasePtr feat_bp0 = FeatureBase::emplace<FeatureBase>(cap_bp0, "bp0", bias_pbc_ +
+ // (Vector3d()<<0.1,0.1,0.1).finished(), Q_bp);
+ FactorBasePtr fac_bp0 = FactorBase::emplace<FactorBlockAbsolute>(
+ feat_bp0, feat_bp0, KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic(), nullptr, false);
}
- void buildDataVectors(){
+ void buildDataVectors()
+ {
pbc_meas_ = pbc_ + bias_pbc_;
acc_gyr_meas_ << acc_meas_, w_meas_;
acc_gyr_cov_ = 1e-3 * Matrix6d::Identity();
meas_ikin_ << pbc_meas_, vBC_meas_, w_meas_;
- Vector6d f_meas; f_meas << f1_, f2_;
- Vector6d tau_meas; tau_meas << tau1_, tau2_;
+ Vector6d f_meas;
+ f_meas << f1_, f2_;
+ Vector6d tau_meas;
+ tau_meas << tau1_, tau2_;
Matrix<double, 14, 1> kin_meas;
kin_meas << pbl1_, pbl2_, bql1_, bql2_;
- cap_ftp_data_.resize(32,1);
+ cap_ftp_data_.resize(32, 1);
cap_ftp_data_ << f_meas, tau_meas, pbc_meas_, acc_gyr_meas_.tail<3>(), kin_meas;
- Qftp_ = Matrix<double, 18, 18>::Identity();
- Qftp_.block<6, 6>(0, 0) = cov_f_;
- Qftp_.block<6, 6>(6, 6) = cov_tau_;
+ Qftp_ = Matrix<double, 18, 18>::Identity();
+ Qftp_.block<6, 6>(0, 0) = cov_f_;
+ Qftp_.block<6, 6>(6, 6) = cov_tau_;
Qftp_.block<3, 3>(12, 12) = cov_pbc_;
Qftp_.block<3, 3>(15, 15) = cov_wb_;
}
@@ -329,29 +348,29 @@ public:
{
// IMU CAPTURE DATA ZERO MOTION
acc_meas_ = -gravity();
- w_meas_ = ZERO3;
+ w_meas_ = ZERO3;
// INERTIAL KINEMATICS CAPTURE DATA ZERO MOTION
- pbc_ = ZERO3;
+ pbc_ = ZERO3;
vBC_meas_ = ZERO3;
// momentum parameters
Iq_.setIdentity();
Lq_.setZero();
// FORCE TORQUE CAPTURE DATA ZERO MOTION
- f1_ << -MASS * wolf::gravity() / 2; // Only gravity -> static robot on both feet
+ f1_ << -MASS * wolf::gravity() / 2; // Only gravity -> static robot on both feet
tau1_ << ZERO3;
pbl1_ << ZERO3;
bql1_ << ZERO3, 1;
- f2_ << -MASS * wolf::gravity() / 2; // Only gravity -> static robot on both feet
+ f2_ << -MASS * wolf::gravity() / 2; // Only gravity -> static robot on both feet
tau2_ << ZERO3;
pbl2_ << ZERO3;
bql2_ << ZERO3, 1;
- cov_f_ = 1e-4 * Matrix6d::Identity();
+ cov_f_ = 1e-4 * Matrix6d::Identity();
cov_tau_ = 1e-4 * Matrix6d::Identity();
cov_pbc_ = 1e-4 * Matrix3d::Identity();
- cov_wb_ = 1e-4 * Matrix3d::Identity();
+ cov_wb_ = 1e-4 * Matrix3d::Identity();
}
virtual void initializeData1()
@@ -374,12 +393,11 @@ public:
{
prev_pose_curr_ << ZERO6, 1;
}
-
};
class Cst2KFZeroMotionBias : public Cst2KFZeroMotion
{
-public:
+ public:
void SetUp() override
{
bias_pbc_ = BIAS_PBC_SMAL;
@@ -389,7 +407,7 @@ public:
class Cst2KFXaxisLinearMotion : public Cst2KFZeroMotion
{
-public:
+ public:
void initializeData1() override
{
setZeroMotionData();
@@ -403,14 +421,13 @@ public:
void setOdomData() override
{
- prev_pose_curr_ << 0.5*ACC*pow(3*DT,2), 0, 0, 0, 0, 0, 1;
+ prev_pose_curr_ << 0.5 * ACC * pow(3 * DT, 2), 0, 0, 0, 0, 0, 1;
}
};
class Cst2KFXaxisLinearMotionPbc : public Cst2KFZeroMotion
{
-public:
-
+ public:
void SetUp() override
{
Cst2KFZeroMotion::SetUp();
@@ -419,7 +436,7 @@ public:
void setOriginState() override
{
Cst2KFZeroMotion::setOriginState();
- x_origin_.segment<3>(10) = PBCX + PBCY + PBCZ; // Ok if PO is 0,0,0, 0,0,0,1
+ x_origin_.segment<3>(10) = PBCX + PBCY + PBCZ; // Ok if PO is 0,0,0, 0,0,0,1
}
void initializeData1() override
@@ -430,21 +447,21 @@ public:
f1_[0] += sen_ft_->getMass() * ACC / 2;
f2_[0] += sen_ft_->getMass() * ACC / 2;
// taus according to static Euler eq
- tau1_ = -(pbl1_- pbc_).cross(f1_); // torque at C due to f1
- tau2_ = -(pbl2_- pbc_).cross(f2_); // torque at C due to f2
+ tau1_ = -(pbl1_ - pbc_).cross(f1_); // torque at C due to f1
+ tau2_ = -(pbl2_ - pbc_).cross(f2_); // torque at C due to f2
buildDataVectors();
}
void setOdomData() override
{
- prev_pose_curr_ << 0.5*ACC*pow(3*DT,2), 0, 0, 0, 0, 0, 1;
+ prev_pose_curr_ << 0.5 * ACC * pow(3 * DT, 2), 0, 0, 0, 0, 0, 1;
}
};
class Cst2KFXaxisLinearMotionPbcBias : public Cst2KFZeroMotion
{
-public:
+ public:
void SetUp() override
{
bias_pbc_ = BIAS_PBC_SMAL;
@@ -454,7 +471,7 @@ public:
void setOriginState() override
{
Cst2KFZeroMotion::setOriginState();
- x_origin_.segment<3>(10) = PBCX + PBCY + PBCZ; // Ok if PO is 0,0,0, 0,0,0,1
+ x_origin_.segment<3>(10) = PBCX + PBCY + PBCZ; // Ok if PO is 0,0,0, 0,0,0,1
}
void initializeData1() override
@@ -465,20 +482,20 @@ public:
f1_[0] += sen_ft_->getMass() * ACC / 2;
f2_[0] += sen_ft_->getMass() * ACC / 2;
// taus according to static Euler eq
- tau1_ = -(pbl1_- pbc_).cross(f1_); // torque at C due to f1
- tau2_ = -(pbl2_- pbc_).cross(f2_); // torque at C due to f2
+ tau1_ = -(pbl1_ - pbc_).cross(f1_); // torque at C due to f1
+ tau2_ = -(pbl2_ - pbc_).cross(f2_); // torque at C due to f2
buildDataVectors();
}
void setOdomData() override
{
- prev_pose_curr_ << 0.5*ACC*pow(3*DT,2), 0, 0, 0, 0, 0, 1;
+ prev_pose_curr_ << 0.5 * ACC * pow(3 * DT, 2), 0, 0, 0, 0, 0, 1;
}
};
class Cst2KFXaxisLinearMotionPbcBiasPQbl : public Cst2KFZeroMotion
{
-public:
+ public:
void SetUp() override
{
bias_pbc_ = BIAS_PBC_SMAL;
@@ -488,7 +505,7 @@ public:
void setOriginState() override
{
Cst2KFZeroMotion::setOriginState();
- x_origin_.segment<3>(10) = PBCX + PBCY + PBCZ; // Ok if PO is 0,0,0, 0,0,0,1
+ x_origin_.segment<3>(10) = PBCX + PBCY + PBCZ; // Ok if PO is 0,0,0, 0,0,0,1
}
void initializeData1() override
@@ -508,21 +525,21 @@ public:
f2_ = quat_bl2.inverse() * f2_;
// taus according to static Euler eq
- tau1_ = -(quat_bl1.inverse()*(pbl1_- pbc_)).cross(f1_); // torque at C due to f1
- tau2_ = -(quat_bl2.inverse()*(pbl2_- pbc_)).cross(f2_); // torque at C due to f2
+ tau1_ = -(quat_bl1.inverse() * (pbl1_ - pbc_)).cross(f1_); // torque at C due to f1
+ tau2_ = -(quat_bl2.inverse() * (pbl2_ - pbc_)).cross(f2_); // torque at C due to f2
buildDataVectors();
}
void setOdomData() override
{
- prev_pose_curr_ << 0.5*ACC*pow(3*DT,2), 0, 0, 0, 0, 0, 1;
+ prev_pose_curr_ << 0.5 * ACC * pow(3 * DT, 2), 0, 0, 0, 0, 0, 1;
}
};
class Cst2KFXaxisRotationPureTorque : public Cst2KFZeroMotion
{
-public:
+ public:
void SetUp() override
{
Cst2KFZeroMotion::SetUp();
@@ -533,15 +550,15 @@ public:
setZeroMotionData();
// taus according to static Euler eq
- tau1_ << M_PI/3, 0, 0; // with Identity rotational inertial matrix, rotation of PI/2 rad.s-2
- w_meas_ << M_PI/3, 0, 0;
+ tau1_ << M_PI / 3, 0, 0; // with Identity rotational inertial matrix, rotation of PI/2 rad.s-2
+ w_meas_ << M_PI / 3, 0, 0;
buildDataVectors();
}
void setOdomData() override
{
- prev_pose_curr_ << 0.5*ACC*pow(3*DT,2), 0, 0, 0, 0, 0, 1;
+ prev_pose_curr_ << 0.5 * ACC * pow(3 * DT, 2), 0, 0, 0, 0, 0, 1;
}
};
@@ -571,7 +588,8 @@ TEST_F(Cst2KFZeroMotion, ZeroMotionZeroBias)
// THEN SOLVE
problem_->perturb();
- std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::string report =
+ solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
@@ -595,7 +613,8 @@ TEST_F(Cst2KFZeroMotionBias, ZeroMotionBias)
{
problem_->perturb();
- std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::string report =
+ solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// problem_->print(4,true,true,true);
// std::cout << report << std::endl;
@@ -613,17 +632,18 @@ TEST_F(Cst2KFZeroMotionBias, ZeroMotionBias)
ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
// Bias value on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(
+ KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(
+ KF2_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
}
TEST_F(Cst2KFXaxisLinearMotion, XLinearMotionZeroBias)
{
-
Vector3d posi_diff = ZERO3;
- posi_diff[0] = 0.5*ACC * pow(3*DT,2);
- Vector3d vel_diff = ZERO3;
- vel_diff[0] = ACC * 3*DT;
+ posi_diff[0] = 0.5 * ACC * pow(3 * DT, 2);
+ Vector3d vel_diff = ZERO3;
+ vel_diff[0] = ACC * 3 * DT;
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// // COM position on Z axis is not observable with this problem
@@ -640,7 +660,8 @@ TEST_F(Cst2KFXaxisLinearMotion, XLinearMotionZeroBias)
problem_->perturb();
- std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::string report =
+ solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// problem_->print(4, true, true, true);
// std::cout << report << std::endl;
@@ -666,9 +687,9 @@ TEST_F(Cst2KFXaxisLinearMotion, XLinearMotionZeroBias)
TEST_F(Cst2KFXaxisLinearMotionPbc, XLinearMotionBias)
{
Vector3d posi_diff = ZERO3;
- posi_diff[0] = 0.5*ACC * pow(3*DT,2);
- Vector3d vel_diff = ZERO3;
- vel_diff[0] = ACC * 3*DT;
+ posi_diff[0] = 0.5 * ACC * pow(3 * DT, 2);
+ Vector3d vel_diff = ZERO3;
+ vel_diff[0] = ACC * 3 * DT;
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
// ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// // COM position on Z axis is not observable with this problem
@@ -679,13 +700,14 @@ TEST_F(Cst2KFXaxisLinearMotionPbc, XLinearMotionBias)
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), posi_diff, 1e-6);
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), vel_diff, 1e-6);
// // COM position on Z axis is not observable with this problem
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
- // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), vel_diff, 1e-6);
+ // ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2) +
+ // posi_diff.head(2), 1e-6); ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), vel_diff, 1e-6);
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
problem_->perturb();
- std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::string report =
+ solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// std::cout << report << std::endl;
ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
@@ -698,7 +720,8 @@ TEST_F(Cst2KFXaxisLinearMotionPbc, XLinearMotionBias)
ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), posi_diff, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), vel_diff, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(
+ KF2_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), vel_diff, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
@@ -711,14 +734,15 @@ TEST_F(Cst2KFXaxisLinearMotionPbcBias, XLinearMotionBias)
{
problem_->perturb();
- std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::string report =
+ solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// problem_->print(4, true, true, true);
// std::cout << report << std::endl;
Vector3d posi_diff = ZERO3;
- posi_diff[0] = 0.5*ACC * pow(3*DT,2);
- Vector3d vel_diff = ZERO3;
- vel_diff[0] = ACC * 3*DT;
+ posi_diff[0] = 0.5 * ACC * pow(3 * DT, 2);
+ Vector3d vel_diff = ZERO3;
+ vel_diff[0] = ACC * 3 * DT;
ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem
@@ -729,27 +753,31 @@ TEST_F(Cst2KFXaxisLinearMotionPbcBias, XLinearMotionBias)
ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), posi_diff, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), vel_diff, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(
+ KF2_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), vel_diff, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
// Bias value on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(
+ KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(
+ KF2_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
}
TEST_F(Cst2KFXaxisLinearMotionPbcBiasPQbl, XLinearMotionBias)
{
problem_->perturb();
- std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::string report =
+ solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
// problem_->print(4, true, true, true);
// std::cout << report << std::endl;
Vector3d posi_diff = ZERO3;
- posi_diff[0] = 0.5*ACC * pow(3*DT,2);
- Vector3d vel_diff = ZERO3;
- vel_diff[0] = ACC * 3*DT;
+ posi_diff[0] = 0.5 * ACC * pow(3 * DT, 2);
+ Vector3d vel_diff = ZERO3;
+ vel_diff[0] = ACC * 3 * DT;
ASSERT_MATRIX_APPROX(KF1_->getStateBlock('P')->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF1_->getStateBlock('V')->getState(), ZERO3, 1e-6);
// COM position on Z axis is not observable with this problem
@@ -760,16 +788,18 @@ TEST_F(Cst2KFXaxisLinearMotionPbcBiasPQbl, XLinearMotionBias)
ASSERT_MATRIX_APPROX(KF2_->getStateBlock('P')->getState(), posi_diff, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock('V')->getState(), vel_diff, 1e-6);
// COM position on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF2_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(
+ KF2_->getStateBlock('C')->getState().head(2), (PBCX + PBCY + PBCZ).head(2) + posi_diff.head(2), 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock('D')->getState(), vel_diff, 1e-6);
ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), ZERO3, 1e-6);
// Bias value on Z axis is not observable with this problem
- ASSERT_MATRIX_APPROX(KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(
+ KF1_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(
+ KF2_->getCaptureOf(sen_ikin_)->getSensorIntrinsic()->getState().head(2), BIAS_PBC_SMAL.head(2), 1e-6);
}
-
// TEST_F(Cst2KFXaxisRotationPureTorque, RotationOnlyNoSolve)
// {
@@ -785,7 +815,6 @@ TEST_F(Cst2KFXaxisLinearMotionPbcBiasPQbl, XLinearMotionBias)
// ASSERT_MATRIX_APPROX(KF2_->getStateBlock('L')->getState(), Lc_diff, 1e-6);
// }
-
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_processor_inertial_kinematics.cpp b/test/gtest_processor_inertial_kinematics.cpp
index 43bb47d..8e394e8 100644
--- a/test/gtest_processor_inertial_kinematics.cpp
+++ b/test/gtest_processor_inertial_kinematics.cpp
@@ -55,18 +55,17 @@ using namespace std;
const Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
const Eigen::Vector6d ZERO6 = Eigen::Vector6d::Zero();
-
class FactorInertialKinematics_2KF : public testing::Test
{
- public:
- wolf::TimeStamp t_;
- ProblemPtr problem_;
- SensorImuPtr sen_imu_;
- SolverCeresPtr solver_;
- SensorInertialKinematicsPtr sen_ikin_;
- VectorXd x_origin_;
- MatrixXd P_origin_;
- FrameBasePtr KF0_;
+ public:
+ wolf::TimeStamp t_;
+ ProblemPtr problem_;
+ SensorImuPtr sen_imu_;
+ SolverCeresPtr solver_;
+ SensorInertialKinematicsPtr sen_ikin_;
+ VectorXd x_origin_;
+ MatrixXd P_origin_;
+ FrameBasePtr KF0_;
void SetUp() override
{
@@ -85,35 +84,39 @@ class FactorInertialKinematics_2KF : public testing::Test
// SENSOR + PROCESSOR INERTIAL KINEMATICS
ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
- intr_ik->std_pbc = 0.1;
- intr_ik->std_vbc = 0.1;
- VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
+ intr_ik->std_pbc = 0.1;
+ intr_ik->std_vbc = 0.1;
+ VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
SensorBasePtr sen_ikin_base = problem_->installSensor("SensorInertialKinematics", "SenIK", extr, intr_ik);
- // SensorBasePtr sen_ikin_base = problem_->installSensor("SensorInertialKinematics", "SenIK", extr, bodydynamics_root_dir + "/test/sensor_inertial_kinematics.yaml"); // TODO: does not work!
+ // SensorBasePtr sen_ikin_base = problem_->installSensor("SensorInertialKinematics", "SenIK", extr,
+ // bodydynamics_root_dir + "/test/sensor_inertial_kinematics.yaml"); // TODO: does not work!
sen_ikin_ = std::static_pointer_cast<SensorInertialKinematics>(sen_ikin_base);
-
// SENSOR + PROCESSOR Imu
- SensorBasePtr sen_imu_base = problem_->installSensor("SensorImu", "SenImu", (Vector7d()<<0,0,0,0,0,0,1).finished(), bodydynamics_root_dir + "/test/sensor_imu.yaml");
- sen_imu_ = std::static_pointer_cast<SensorImu>(sen_imu_base);
- ProcessorBasePtr proc = problem_->installProcessor("ProcessorImu", "Imu pre-integrator", "SenImu", bodydynamics_root_dir + "/test/processor_imu.yaml");
+ SensorBasePtr sen_imu_base = problem_->installSensor("SensorImu",
+ "SenImu",
+ (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(),
+ bodydynamics_root_dir + "/test/sensor_imu.yaml");
+ sen_imu_ = std::static_pointer_cast<SensorImu>(sen_imu_base);
+ ProcessorBasePtr proc = problem_->installProcessor(
+ "ProcessorImu", "Imu pre-integrator", "SenImu", bodydynamics_root_dir + "/test/processor_imu.yaml");
// auto processor_imu = std::static_pointer_cast<ProcessorImu>(proc);
-
-
ParamsProcessorInertialKinematicsPtr params_ik = std::make_shared<ParamsProcessorInertialKinematics>();
- params_ik->sensor_angvel_name = "SenImu";
- params_ik->std_bp_drift = 0.0000001;
- ProcessorBasePtr proc_ikin_base = problem_->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", sen_ikin_, params_ik);
- // ProcessorBasePtr proc_ikin_base = problem_->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", "SenIK", bodydynamics_root_dir + "/test/processor_inertial_kinematics.yaml");
- // auto proc_inkin = std::static_pointer_cast<ProcessorInertialKinematics>(proc_ikin_base);
+ params_ik->sensor_angvel_name = "SenImu";
+ params_ik->std_bp_drift = 0.0000001;
+ ProcessorBasePtr proc_ikin_base =
+ problem_->installProcessor("ProcessorInertialKinematics", "PInertialKinematics", sen_ikin_, params_ik);
+ // ProcessorBasePtr proc_ikin_base = problem_->installProcessor("ProcessorInertialKinematics",
+ // "PInertialKinematics", "SenIK", bodydynamics_root_dir + "/test/processor_inertial_kinematics.yaml"); auto
+ // proc_inkin = std::static_pointer_cast<ProcessorInertialKinematics>(proc_ikin_base);
// Set origin of the problem
x_origin_.resize(19);
- x_origin_ << ZERO3, 0,0,0,1, ZERO3, ZERO3, ZERO3, ZERO3;
- 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});
+ x_origin_ << ZERO3, 0, 0, 0, 1, ZERO3, ZERO3, ZERO3, ZERO3;
+ 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_);
KF0_ = problem_->getTrajectory()->getLastFrame();
@@ -122,37 +125,43 @@ class FactorInertialKinematics_2KF : public testing::Test
////////////////////////////////////////////
// Add absolute factor on Imu biases to simulate a fix()
- Matrix6d Q_bi = 1e-8 * Matrix6d::Identity();
- CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF0_, "bi0", t_);
+ Matrix6d Q_bi = 1e-8 * Matrix6d::Identity();
+ CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF0_, "bi0", t_);
FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", ZERO6, Q_bi);
- FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF0_->getCaptureOf(sen_imu_)->getSensorIntrinsic(),nullptr,false);
-
+ FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(
+ featbi0, featbi0, KF0_->getCaptureOf(sen_imu_)->getSensorIntrinsic(), nullptr, false);
}
- void TearDown() override{}
+ void TearDown() override {}
};
TEST_F(FactorInertialKinematics_2KF, sensor_and_processors_registration)
{
- TimeStamp t0; t0.set(0);
- TimeStamp t1; t1.set(1);
- TimeStamp t2; t2.set(2);
- TimeStamp t3; t3.set(3);
+ TimeStamp t0;
+ t0.set(0);
+ TimeStamp t1;
+ t1.set(1);
+ TimeStamp t2;
+ t2.set(2);
+ TimeStamp t3;
+ t3.set(3);
// PROCESS ONE Imu CAPTURE
Vector6d acc_gyr_meas;
- acc_gyr_meas << -gravity(), 0,0,0;
- acc_gyr_meas[0] = 1;
- Matrix6d acc_gyr_cov = 1e-3*Matrix6d::Identity();
+ acc_gyr_meas << -gravity(), 0, 0, 0;
+ acc_gyr_meas[0] = 1;
+ Matrix6d acc_gyr_cov = 1e-3 * Matrix6d::Identity();
// PROCESS ONE INERTIAL KINEMATICS CAPTURE
Vector3d pBC_meas = Vector3d::Zero();
Vector3d vBC_meas = Vector3d::Zero();
- Vector3d w_meas = Vector3d::Zero();
+ Vector3d w_meas = Vector3d::Zero();
// momentum parameters
- Matrix3d Iq; Iq.setIdentity();
+ Matrix3d Iq;
+ Iq.setIdentity();
Vector3d Lq = Vector3d::Zero();
- Vector9d meas_ikin0; meas_ikin0 << pBC_meas, vBC_meas, w_meas;
+ Vector9d meas_ikin0;
+ meas_ikin0 << pBC_meas, vBC_meas, w_meas;
// sen_imu_->getIntrinsic()->fix();
@@ -173,7 +182,6 @@ TEST_F(FactorInertialKinematics_2KF, sensor_and_processors_registration)
sen_ikin_->process(CIKin1);
CIKin1->getSensorIntrinsic()->fix();
-
// T2
CaptureImuPtr CImu2 = std::make_shared<CaptureImu>(t2, sen_imu_, acc_gyr_meas, acc_gyr_cov);
// CImu2->getSensorIntrinsic()->fix();
@@ -188,22 +196,25 @@ TEST_F(FactorInertialKinematics_2KF, sensor_and_processors_registration)
// CImu3->getSensorIntrinsic()->fix();
sen_imu_->process(CImu3);
- CaptureInertialKinematicsPtr CIKin3 = std::make_shared<CaptureInertialKinematics>(t3, sen_ikin_, meas_ikin0, Iq, Lq);
+ CaptureInertialKinematicsPtr CIKin3 =
+ std::make_shared<CaptureInertialKinematics>(t3, sen_ikin_, meas_ikin0, Iq, Lq);
sen_ikin_->process(CIKin3);
CIKin3->getSensorIntrinsic()->fix();
KF0_->perturb();
- std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+ std::string report =
+ solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
std::cout << report << std::endl;
- problem_->print(4,true,true,true);
+ problem_->print(4, true, true, true);
- Vector3d pdiff; pdiff << 4.5, 0, 0;
- Vector3d vdiff; vdiff << 3, 0, 0;
+ Vector3d pdiff;
+ pdiff << 4.5, 0, 0;
+ Vector3d vdiff;
+ vdiff << 3, 0, 0;
FrameBasePtr KF1 = problem_->getTrajectory()->getLastFrame();
-
ASSERT_MATRIX_APPROX(KF0_->getStateBlock('P')->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF0_->getStateBlock('V')->getState(), ZERO3, 1e-6);
ASSERT_MATRIX_APPROX(KF0_->getStateBlock('C')->getState(), ZERO3, 1e-6);
@@ -217,11 +228,9 @@ TEST_F(FactorInertialKinematics_2KF, sensor_and_processors_registration)
ASSERT_MATRIX_APPROX(KF1->getStateBlock('L')->getState(), ZERO3, 1e-6);
}
-
-
int main(int argc, char **argv)
{
- testing::InitGoogleTest(&argc, argv);
+ testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
+ return RUN_ALL_TESTS();
}
diff --git a/test/gtest_processor_point_feet_nomove.cpp b/test/gtest_processor_point_feet_nomove.cpp
index 9657f6d..6ada4c4 100644
--- a/test/gtest_processor_point_feet_nomove.cpp
+++ b/test/gtest_processor_point_feet_nomove.cpp
@@ -49,25 +49,24 @@ using namespace std;
const Eigen::Vector3d ZERO3 = Eigen::Vector3d::Zero();
// const Eigen::Vector6d ZERO6 = Eigen::Vector6d::Zero();
-
class PointFeetCaptures : public testing::Test
{
- public:
- wolf::TimeStamp t_;
- ProblemPtr problem_;
- SensorPointFeetNomovePtr sen_pfnm_;
- ProcessorBasePtr proc_pfnm_base_;
- VectorXd x_origin_;
- // MatrixXd P_origin_;
- FrameBasePtr KF0_;
-
- CapturePointFeetNomovePtr CPF0_;
- CapturePointFeetNomovePtr CPF1_;
- CapturePointFeetNomovePtr CPF2_;
- CapturePointFeetNomovePtr CPF3_;
- CapturePointFeetNomovePtr CPF4_;
- CapturePointFeetNomovePtr CPF5_;
- CapturePointFeetNomovePtr CPF6_;
+ public:
+ wolf::TimeStamp t_;
+ ProblemPtr problem_;
+ SensorPointFeetNomovePtr sen_pfnm_;
+ ProcessorBasePtr proc_pfnm_base_;
+ VectorXd x_origin_;
+ // MatrixXd P_origin_;
+ FrameBasePtr KF0_;
+
+ CapturePointFeetNomovePtr CPF0_;
+ CapturePointFeetNomovePtr CPF1_;
+ CapturePointFeetNomovePtr CPF2_;
+ CapturePointFeetNomovePtr CPF3_;
+ CapturePointFeetNomovePtr CPF4_;
+ CapturePointFeetNomovePtr CPF5_;
+ CapturePointFeetNomovePtr CPF6_;
void SetUp() override
{
@@ -85,29 +84,26 @@ class PointFeetCaptures : public testing::Test
// SENSOR + PROCESSOR POINT FEET NOMOVE
ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>();
- intr_pfn->std_foot_nomove_ = 0.1;
- VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
+ intr_pfn->std_foot_nomove_ = 0.1;
+ VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
SensorBasePtr sen_pfnm_base = problem_->installSensor("SensorPointFeetNomove", "SenPfnm", extr, intr_pfn);
- sen_pfnm_ = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base);
+ sen_pfnm_ = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base);
ParamsProcessorPointFeetNomovePtr params_pfnm = std::make_shared<ParamsProcessorPointFeetNomove>();
- proc_pfnm_base_ = problem_->installProcessor("ProcessorPointFeetNomove", "PPointFeetNomove", sen_pfnm_, params_pfnm);
+ proc_pfnm_base_ =
+ problem_->installProcessor("ProcessorPointFeetNomove", "PPointFeetNomove", sen_pfnm_, params_pfnm);
// Set origin of the problem
x_origin_.resize(7);
- x_origin_ << 0,0,0, 0,0,0,1;
- 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});
+ x_origin_ << 0, 0, 0, 0, 0, 0, 1;
+ 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);
KF0_ = problem_->getTrajectory()->getLastFrame();
// Simulate captures with 4 point feet
- std::unordered_map<int, Vector7d> kin_incontact_A({
- {1, Vector7d::Zero()},
- {2, Vector7d::Zero()},
- {3, Vector7d::Zero()},
- {4, Vector7d::Zero()}
- });
+ std::unordered_map<int, Vector7d> kin_incontact_A(
+ {{1, Vector7d::Zero()}, {2, Vector7d::Zero()}, {3, Vector7d::Zero()}, {4, Vector7d::Zero()}});
CPF0_ = std::make_shared<CapturePointFeetNomove>(0.0, sen_pfnm_, kin_incontact_A);
std::cout << CPF0_->getName() << std::endl;
@@ -115,37 +111,30 @@ class PointFeetCaptures : public testing::Test
CPF2_ = std::make_shared<CapturePointFeetNomove>(2.0, sen_pfnm_, kin_incontact_A);
// contact of the 2nd foot lost
- std::unordered_map<int, Eigen::Vector7d> kin_incontact_B({
- {1, Vector7d::Ones()},
- {3, Vector7d::Ones()},
- {4, Vector7d::Ones()}
- });
+ std::unordered_map<int, Eigen::Vector7d> kin_incontact_B(
+ {{1, Vector7d::Ones()}, {3, Vector7d::Ones()}, {4, Vector7d::Ones()}});
CPF3_ = std::make_shared<CapturePointFeetNomove>(3.0, sen_pfnm_, kin_incontact_B);
CPF4_ = std::make_shared<CapturePointFeetNomove>(4.0, sen_pfnm_, kin_incontact_B);
// contact of the 3rd foot lost
- std::unordered_map<int, Eigen::Vector7d> kin_incontact_C({
- {1, 2*Vector7d::Ones()},
- {4, 2*Vector7d::Ones()}
- });
+ std::unordered_map<int, Eigen::Vector7d> kin_incontact_C(
+ {{1, 2 * Vector7d::Ones()}, {4, 2 * Vector7d::Ones()}});
CPF5_ = std::make_shared<CapturePointFeetNomove>(5.0, sen_pfnm_, kin_incontact_C);
CPF6_ = std::make_shared<CapturePointFeetNomove>(6.0, sen_pfnm_, kin_incontact_C);
-
}
- void TearDown() override{}
+ void TearDown() override {}
};
TEST_F(PointFeetCaptures, process_all_capture_first)
{
-
CPF0_->process();
CPF1_->process();
CPF2_->process();
CPF3_->process();
CPF4_->process();
CPF5_->process();
- CPF6_->process(); // one last capture to create the factor
+ CPF6_->process(); // one last capture to create the factor
// factor creation due to keyFrameCallback
FrameBasePtr KF1 = problem_->emplaceFrame(2.0, x_origin_);
@@ -154,10 +143,9 @@ TEST_F(PointFeetCaptures, process_all_capture_first)
FrameBasePtr KF2 = problem_->emplaceFrame(3.0, x_origin_);
proc_pfnm_base_->keyFrameCallback(KF2);
- problem_->print(4,1,1,1);
+ problem_->print(4, 1, 1, 1);
}
-
TEST_F(PointFeetCaptures, process_all_capture_last)
{
// First, get the key frames from the keyFrameCallback
@@ -174,17 +162,14 @@ TEST_F(PointFeetCaptures, process_all_capture_last)
CPF3_->process();
CPF4_->process();
CPF5_->process();
- CPF6_->process(); // one last capture to create the factor
+ CPF6_->process(); // one last capture to create the factor
- problem_->print(4,1,1,1);
+ problem_->print(4, 1, 1, 1);
}
-
-
-
int main(int argc, char **argv)
{
- testing::InitGoogleTest(&argc, argv);
+ testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
+ return RUN_ALL_TESTS();
}
diff --git a/test/gtest_sensor_force_torque.cpp b/test/gtest_sensor_force_torque.cpp
index a486849..37a84e5 100644
--- a/test/gtest_sensor_force_torque.cpp
+++ b/test/gtest_sensor_force_torque.cpp
@@ -31,9 +31,9 @@ using namespace Eigen;
TEST(SensorForceTorque, constructor_and_getters)
{
ParamsSensorForceTorquePtr intr = std::make_shared<ParamsSensorForceTorque>();
- intr->std_f = 1;
- intr->std_tau = 2;
- VectorXd extr(0);
+ intr->std_f = 1;
+ intr->std_tau = 2;
+ VectorXd extr(0);
SensorForceTorque S(extr, intr);
ASSERT_EQ(S.getForceNoiseStd(), 1);
@@ -42,7 +42,6 @@ TEST(SensorForceTorque, constructor_and_getters)
int main(int argc, char **argv)
{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
}
-
diff --git a/test/gtest_sensor_inertial_kinematics.cpp b/test/gtest_sensor_inertial_kinematics.cpp
index 9a62320..9bfb680 100644
--- a/test/gtest_sensor_inertial_kinematics.cpp
+++ b/test/gtest_sensor_inertial_kinematics.cpp
@@ -31,9 +31,9 @@ using namespace Eigen;
TEST(SensorInertialKinematics, constructor_and_getters)
{
ParamsSensorInertialKinematicsPtr intr = std::make_shared<ParamsSensorInertialKinematics>();
- intr->std_pbc = 1;
- intr->std_vbc = 2;
- VectorXd extr(0);
+ intr->std_pbc = 1;
+ intr->std_vbc = 2;
+ VectorXd extr(0);
SensorInertialKinematics S(extr, intr);
ASSERT_EQ(S.getPbcNoiseStd(), 1);
@@ -42,7 +42,6 @@ TEST(SensorInertialKinematics, constructor_and_getters)
int main(int argc, char **argv)
{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
}
-
--
GitLab