diff --git a/demos/mcapi_pov_estimation.cpp b/demos/mcapi_pov_estimation.cpp
index 9fdc2feebbea2a407bd5268d0be86388804e5de3..b465845c1c6b6c75d596dff953f0fd287a975768 100644
--- a/demos/mcapi_pov_estimation.cpp
+++ b/demos/mcapi_pov_estimation.cpp
@@ -475,7 +475,7 @@ int main (int argc, char **argv) {
std::vector<Vector3d> v_ob_fbk_v;
//////////////////////////////////////////
- unsigned int nb_kf = problem->getTrajectory()->getFrameMap().size();
+ unsigned int nb_kf = problem->getTrajectory()->size();
for (unsigned int i=0; i < t_arr.size(); i++){
// TimeStamp ts(t_arr[i]+dt); // works best with tsid trajectories
TimeStamp ts(t_arr[i]); // works best with pyb trajectories
@@ -543,9 +543,9 @@ int main (int argc, char **argv) {
}
// if new KF add
- // unsigned int new_kf_nb = problem->getTrajectory()->getFrameMap().size();
+ // unsigned int new_kf_nb = problem->getTrajectory()->size();
// if (new_kf_nb > nb_kf){
- // auto last_kf = problem->getTrajectory()->getFrameMap().rbegin()->second;
+ // auto last_kf = problem->getTrajectory()->getLastFrame();
// nb_kf = new_kf_nb;
// // ADD ABSOLUTE FACTOR (GPS LIKE)
// }
@@ -664,7 +664,8 @@ int main (int argc, char **argv) {
VectorComposite kf_state;
CaptureBasePtr cap_imu;
VectorComposite bi_bias_est;
- for (auto& elt: problem->getTrajectory()->getFrameMap()){
+ auto frame_map = problem->getTrajectory()->getFrameMap();
+ for (auto& elt : frame_map){
auto kf = elt.second;
kf_state = kf->getState();
cap_imu = kf->getCaptureOf(sen_imu);
diff --git a/demos/mcapi_povcdl_estimation.cpp b/demos/mcapi_povcdl_estimation.cpp
index 82bf7d64e2c9779a17a590fc7b9a6cf2b58cded1..83111ab6ae729acabed35baccfdf4d567cd4b798 100644
--- a/demos/mcapi_povcdl_estimation.cpp
+++ b/demos/mcapi_povcdl_estimation.cpp
@@ -862,7 +862,8 @@ int main (int argc, char **argv) {
VectorComposite bp_bias_est;
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);
diff --git a/demos/solo_mocap_imu.cpp b/demos/solo_mocap_imu.cpp
index 19f8dfaa700dbcb7876dfd2f3587bb996d68ab1b..c196dac545478f6e934205952abd231fa5f82fc8 100644
--- a/demos/solo_mocap_imu.cpp
+++ b/demos/solo_mocap_imu.cpp
@@ -308,13 +308,14 @@ int main (int argc, char **argv) {
CP->process();
// solve every new KF
- if (problem->getTrajectory()->getFrameMap().size() > nb_kf ){
+ if (problem->getTrajectory()->size() > nb_kf )
+ {
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
std::cout << ts << " ";
std::cout << report << std::endl;
// recover covariances at this point
- auto kf_last = problem->getTrajectory()->getFrameMap().rbegin()->second;
+ auto kf_last = problem->getTrajectory()->getLastFrame();
CaptureBasePtr cap_imu = kf_last->getCaptureOf(sen_imu);
@@ -408,7 +409,7 @@ int main (int argc, char **argv) {
// Compute Covariances
- unsigned int Nkf = problem->getTrajectory()->getFrameMap().size();
+ 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];
@@ -422,8 +423,9 @@ int main (int argc, char **argv) {
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;
+ 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;
VectorComposite kf_state = kf->getState();
diff --git a/demos/solo_real_pov_estimation.cpp b/demos/solo_real_pov_estimation.cpp
index fbae4d51a0f97f30aa05c5329ca754986ce72cb2..411597d87ae0261197d424184737716cf9195500 100644
--- a/demos/solo_real_pov_estimation.cpp
+++ b/demos/solo_real_pov_estimation.cpp
@@ -495,11 +495,11 @@ int main (int argc, char **argv) {
// }
// solve every new KF
- if (problem->getTrajectory()->getFrameMap().size() > nb_kf ){
+ if (problem->getTrajectory()->size() > nb_kf ){
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
std::cout << ts << " ";
std::cout << report << std::endl;
- nb_kf = problem->getTrajectory()->getFrameMap().size();
+ nb_kf = problem->getTrajectory()->size();
}
// Store current state estimation
@@ -575,7 +575,7 @@ int main (int argc, char **argv) {
// Compute Covariances
- unsigned int Nkf = problem->getTrajectory()->getFrameMap().size();
+ 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];
@@ -586,8 +586,9 @@ int main (int argc, char **argv) {
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;
+ 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;
VectorComposite kf_state = kf->getState();
diff --git a/demos/solo_real_povcdl_estimation.cpp b/demos/solo_real_povcdl_estimation.cpp
index 0e4e6a4a6ce958563c3b4e84dfe3f31feb42f464..aee06204b9d7dc68151c79f7971f090003205c88 100644
--- a/demos/solo_real_povcdl_estimation.cpp
+++ b/demos/solo_real_povcdl_estimation.cpp
@@ -584,12 +584,12 @@ int main (int argc, char **argv) {
// add zero vel artificial factor
- if (problem->getTrajectory()->getFrameMap().size() > nb_kf){
- auto kf_pair = problem->getTrajectory()->getFrameMap().rbegin();
- std::cout << "New KF " << kf_pair->first << std::endl;
- auto kf = kf_pair->second;
+ if (problem->getTrajectory()->size() > nb_kf)
+ {
+ auto kf = problem->getTrajectory()->getLastFrame();
+ std::cout << "New KF " << kf->getTimeStamp() << std::endl;
- // CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(kf, "Vel0", kf_pair->first);
+ // 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);
@@ -728,7 +728,8 @@ int main (int argc, char **argv) {
VectorComposite bp_bias_est;
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);
diff --git a/include/bodydynamics/capture/capture_force_torque_preint.h b/include/bodydynamics/capture/capture_force_torque_preint.h
index 12f4eb58c9e2a8b8d89e7923bea393e93bae48a5..81db7bf05f90fc9048a907253b477b73f8c748a5 100644
--- a/include/bodydynamics/capture/capture_force_torque_preint.h
+++ b/include/bodydynamics/capture/capture_force_torque_preint.h
@@ -62,7 +62,9 @@ class CaptureForceTorquePreint : public CaptureMotion
~CaptureForceTorquePreint() override;
- CaptureBasePtr getIkinCaptureOther(){ return cap_ikin_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:
diff --git a/include/bodydynamics/factor/factor_force_torque.h b/include/bodydynamics/factor/factor_force_torque.h
index 85d74de654cfd2976eae06ab640086fec74870aa..958398a325c0e6c77fd5b70a2cabb4497fec746b 100644
--- a/include/bodydynamics/factor/factor_force_torque.h
+++ b/include/bodydynamics/factor/factor_force_torque.h
@@ -79,7 +79,7 @@ class FactorForceTorque : public FactorAutodiff<FactorForceTorque, 9, 3,3,3, 3,3
const T* const _qkm,
T* _res) const;
- void computeJac(const FeatureForceTorquePtr& _feat,
+ void computeJac(const FeatureForceTorqueConstPtr& _feat,
const double& _mass,
const double& _dt,
const Eigen::VectorXd& _bp,
@@ -149,25 +149,24 @@ void FactorForceTorque::retrieveMeasurementCovariance(const FeatureForceTorquePt
}
-void FactorForceTorque::computeJac(const FeatureForceTorquePtr& _feat,
+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();
- FeatureForceTorquePtr feat = std::static_pointer_cast<FeatureForceTorque>(_feat);
// 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> tau1(feat->getTorquesMeas().data());
- 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::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> 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);
@@ -226,7 +225,7 @@ bool FactorForceTorque::operator () (
const T* const _qkm,
T* _res) const
{
- FeatureForceTorquePtr feat = std::static_pointer_cast<FeatureForceTorque>(getFeature());
+ auto feat = std::static_pointer_cast<const FeatureForceTorque>(getFeature());
// State variables instanciation
Eigen::Map<const Eigen::Matrix<T,3,1> > ck(_ck);
diff --git a/include/bodydynamics/factor/factor_inertial_kinematics.h b/include/bodydynamics/factor/factor_inertial_kinematics.h
index c0b36d22bdd65b63b575e6d80863ea2eea978405..ff824e20b8673da4d6de79afa06e80da6cd2607e 100644
--- a/include/bodydynamics/factor/factor_inertial_kinematics.h
+++ b/include/bodydynamics/factor/factor_inertial_kinematics.h
@@ -158,7 +158,7 @@ bool FactorInertialKinematics::operator () (
Map<Matrix<T,9,1> > res(_res);
- FeatureInertialKinematicsPtr feat = std::static_pointer_cast<FeatureInertialKinematics>(getFeature());
+ auto feat = std::static_pointer_cast<const FeatureInertialKinematics>(getFeature());
// Measurements retrieval
Map<const Vector3d> pBC_m(getMeasurement().data() ); // B_p_BC
diff --git a/include/bodydynamics/feature/feature_force_torque.h b/include/bodydynamics/feature/feature_force_torque.h
index bbe2fda19d7fcdbde4e88aa564efc3547a6293ef..14594821f3171f7628199e4b3b5f76026f4f7f63 100644
--- a/include/bodydynamics/feature/feature_force_torque.h
+++ b/include/bodydynamics/feature/feature_force_torque.h
@@ -50,19 +50,19 @@ class FeatureForceTorque : public FeatureBase
~FeatureForceTorque() override;
- const double & getDt(){return dt_;}
- const double & getMass(){return 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(){return forces_meas_;}
- const Eigen::Vector6d& getTorquesMeas(){return torques_meas_;}
- const Eigen::Vector3d& getPbcMeas(){return pbc_meas_;}
- const Eigen::Matrix<double,14,1>& getKinMeas(){return kin_meas_;}
- const Eigen::Matrix6d& getCovForcesMeas(){return cov_forces_meas_;}
- const Eigen::Matrix6d& getCovTorquesMeas(){return cov_torques_meas_;}
- const Eigen::Matrix3d& getCovPbcMeas(){return cov_pbc_meas_;}
- const Eigen::Matrix<double,14,14>& getCovKinMeas(){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;}
diff --git a/include/bodydynamics/feature/feature_inertial_kinematics.h b/include/bodydynamics/feature/feature_inertial_kinematics.h
index bb9588ffbc24e78162bedddc5df7a914f7d55c63..98f03fc3e948e1c5feeaf399e3bdf10297a2b1a6 100644
--- a/include/bodydynamics/feature/feature_inertial_kinematics.h
+++ b/include/bodydynamics/feature/feature_inertial_kinematics.h
@@ -43,8 +43,8 @@ class FeatureInertialKinematics : public FeatureBase
~FeatureInertialKinematics() override = default;
- const Eigen::Matrix3d & getBIq(){return BIq_;}
- const Eigen::Vector3d & getBLcm(){return BLcm_;}
+ 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;}
diff --git a/include/bodydynamics/processor/processor_force_torque_preint.h b/include/bodydynamics/processor/processor_force_torque_preint.h
index c834f71b8e1fc42b2d9d8e7dfd1ee43157627d43..6cd089107cfebd55fad2a51ff1b6cf5eddb0b036 100644
--- a/include/bodydynamics/processor/processor_force_torque_preint.h
+++ b/include/bodydynamics/processor/processor_force_torque_preint.h
@@ -92,7 +92,7 @@ class ProcessorForceTorquePreint : public ProcessorMotion{
Eigen::VectorXd deltaZero() const override;
Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
const Eigen::VectorXd& delta_step) const override;
- VectorXd getCalibration (const CaptureBasePtr _capture = nullptr) const override;
+ VectorXd getCalibration (const CaptureBaseConstPtr _capture = nullptr) const override;
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
bool voteForKeyFrame() const override;
diff --git a/src/processor/processor_force_torque_preint.cpp b/src/processor/processor_force_torque_preint.cpp
index 994b91a49c911dc5835b1d642ead04d2f5ca0223..1b84f786077a0f65ee3cc944bf8ebe4f54511499 100644
--- a/src/processor/processor_force_torque_preint.cpp
+++ b/src/processor/processor_force_torque_preint.cpp
@@ -136,14 +136,14 @@ Eigen::VectorXd ProcessorForceTorquePreint::correctDelta (const Eigen::VectorXd&
return bodydynamics::plus(delta_preint, delta_step);
}
-VectorXd ProcessorForceTorquePreint::getCalibration (const CaptureBasePtr _capture) const
+VectorXd ProcessorForceTorquePreint::getCalibration (const CaptureBaseConstPtr _capture) const
{
VectorXd bias_vec(6);
if (_capture) // access from capture is quicker
{
- CaptureForceTorquePreintPtr cap_ft(std::static_pointer_cast<CaptureForceTorquePreint>(_capture));
+ auto cap_ft(std::static_pointer_cast<const CaptureForceTorquePreint>(_capture));
// get calib part from Ikin capture
bias_vec.segment<3>(0) = cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->getState();
@@ -211,7 +211,7 @@ void ProcessorForceTorquePreint::computeCurrentDelta(
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<SensorForceTorque>(getSensor())->getMass(),
+ bodydynamics::body2delta(body, _dt, std::static_pointer_cast<const SensorForceTorque>(getSensor())->getMass(),
nbc_, dimc_,
_delta, jac_delta_body); // Jacobians tested in bodydynamics_tools
diff --git a/src/processor/processor_inertial_kinematics.cpp b/src/processor/processor_inertial_kinematics.cpp
index 46eb8f944900d0eff9596ad239351d19ec74f1a3..360ef7d654952c17a18dce633f53f9177e79e61d 100644
--- a/src/processor/processor_inertial_kinematics.cpp
+++ b/src/processor/processor_inertial_kinematics.cpp
@@ -68,7 +68,6 @@ inline void ProcessorInertialKinematics::processCapture(CaptureBasePtr _capture)
// done AFTER processCapture)
// 1. get corresponding KF
- FrameBasePtr kf;
auto buffer_frame_it = buffer_frame_.getContainer().begin();
auto buffer_capture_it = buffer_capture_.getContainer().begin();