diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index cde987b69c7b41a95af481940cf7b7561e204637..7b2d492256e0421030771b0a10bc5c7eca00352b 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -1,4 +1,4 @@
-image: segaleran/ceres
+image: labrobotica/ceres:1.14
before_script:
- ls
diff --git a/include/bodydynamics/processor/processor_force_torque_preint.h b/include/bodydynamics/processor/processor_force_torque_preint.h
index d2dd19cc8b46d3cd7cbf38c86a5d9b1153a9c3bf..cd38fb90ffa4ad84d66519a73bfd0f771787cd08 100644
--- a/include/bodydynamics/processor/processor_force_torque_preint.h
+++ b/include/bodydynamics/processor/processor_force_torque_preint.h
@@ -24,9 +24,9 @@ struct ParamsProcessorForceTorquePreint : public ParamsProcessorMotion
dimc = _server.getParam<int>(_unique_name + "/dimc_");
}
~ParamsProcessorForceTorquePreint() override = default;
- std::string print() const
+ std::string print() const override
{
- return "\n" + ParamsProcessorMotion::print() + "\n"
+ return ParamsProcessorMotion::print() + "\n"
+ "sensor_ikin_name: " + sensor_ikin_name + "\n"
+ "sensor_angvel_name: " + sensor_angvel_name + "\n"
+ "nbc_: " + std::to_string(nbc) + "\n"
@@ -71,7 +71,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) const override;
+ VectorXd getCalibration (const CaptureBasePtr _capture = nullptr) const override;
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
bool voteForKeyFrame() const override;
diff --git a/include/bodydynamics/processor/processor_inertial_kinematics.h b/include/bodydynamics/processor/processor_inertial_kinematics.h
index 043f3786aecbae1b674f1fd032553ab8cbf46f12..fcdc050cb2aef10c77b0755a81694ecbd852663a 100644
--- a/include/bodydynamics/processor/processor_inertial_kinematics.h
+++ b/include/bodydynamics/processor/processor_inertial_kinematics.h
@@ -29,9 +29,9 @@ struct ParamsProcessorInertialKinematics : public ParamsProcessorBase
std_bp_drift = _server.getParam<double>(_unique_name + "/std_bp_drift");
}
~ParamsProcessorInertialKinematics() override = default;
- std::string print() const
+ std::string print() const override
{
- return "\n" + ParamsProcessorBase::print() + "\n"
+ return ParamsProcessorBase::print() + "\n"
+ "sensor_angvel_name: " + sensor_angvel_name + "\n"
+ "std_bp_drift: " + std::to_string(std_bp_drift) + "\n";
}
diff --git a/include/bodydynamics/sensor/sensor_force_torque.h b/include/bodydynamics/sensor/sensor_force_torque.h
index af68202f47b1784fa83d5af60e5b64bf39147d8b..761ba5c99ba408954d6ef056ab5d158f2b01fd50 100644
--- a/include/bodydynamics/sensor/sensor_force_torque.h
+++ b/include/bodydynamics/sensor/sensor_force_torque.h
@@ -27,9 +27,9 @@ struct ParamsSensorForceTorque : public ParamsSensorBase
std_tau = _server.getParam<double>(_unique_name + "/std_tau");
}
~ParamsSensorForceTorque() override = default;
- std::string print() const
+ std::string print() const override
{
- return "\n" + ParamsSensorBase::print() + "\n"
+ return ParamsSensorBase::print() + "\n"
+ "mass: " + std::to_string(mass) + "\n"
+ "std_f: " + std::to_string(std_f) + "\n"
+ "std_tau: " + std::to_string(std_tau) + "\n";
diff --git a/include/bodydynamics/sensor/sensor_inertial_kinematics.h b/include/bodydynamics/sensor/sensor_inertial_kinematics.h
index 6c59b0c625f77f67c81ff311e3d8e2616f9065de..73c1832a3ac5e18dddd00aa4504f7bcaa94d0855 100644
--- a/include/bodydynamics/sensor/sensor_inertial_kinematics.h
+++ b/include/bodydynamics/sensor/sensor_inertial_kinematics.h
@@ -25,9 +25,9 @@ struct ParamsSensorInertialKinematics : public ParamsSensorBase
std_vbc = _server.getParam<double>(_unique_name + "/std_vbc");
}
~ParamsSensorInertialKinematics() override = default;
- std::string print() const
+ std::string print() const override
{
- return "\n" + ParamsSensorBase::print() + "\n"
+ return ParamsSensorBase::print() + "\n"
+ "std_pbc: " + std::to_string(std_pbc) + "\n"
+ "std_vbc: " + std::to_string(std_vbc) + "\n";
}
diff --git a/src/processor/processor_force_torque_preint.cpp b/src/processor/processor_force_torque_preint.cpp
index c73189ab1188ce2ff5bc87a23bbcfac9539676a8..17cb798053d93f941e1e33da26516a00ff486c45 100644
--- a/src/processor/processor_force_torque_preint.cpp
+++ b/src/processor/processor_force_torque_preint.cpp
@@ -121,15 +121,27 @@ Eigen::VectorXd ProcessorForceTorquePreint::correctDelta (const Eigen::VectorXd&
VectorXd ProcessorForceTorquePreint::getCalibration (const CaptureBasePtr _capture) const
{
+
VectorXd bias_vec(6);
- CaptureForceTorquePreintPtr cap_ft(std::static_pointer_cast<CaptureForceTorquePreint>(_capture));
- // get calib part from Ikin capture
- bias_vec.segment<3>(0) = cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->getState();
+ if (_capture) // access from capture is quicker
+ {
+ CaptureForceTorquePreintPtr cap_ft(std::static_pointer_cast<CaptureForceTorquePreint>(_capture));
- // 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]
+ // get calib part from Ikin capture
+ 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]
+ }
+ 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]
+ }
return bias_vec;
}
diff --git a/test/gtest_factor_force_torque.cpp b/test/gtest_factor_force_torque.cpp
index d529469ae5a1801ff365149b7c53ce26d3bf13d5..2a8ef4179695edba64fe2447f3179330637dc8de 100644
--- a/test/gtest_factor_force_torque.cpp
+++ b/test/gtest_factor_force_torque.cpp
@@ -151,7 +151,7 @@ void perturbateAllIfUnFixed(const FrameBasePtr& KF)
FrameBasePtr createKFWithCDLI(const ProblemPtr& problem, const TimeStamp& t, VectorComposite x_origin,
Vector3d c, Vector3d cd, Vector3d Lc, Vector6d bias_imu)
{
- FrameBasePtr KF = FrameBase::emplaceKeyFrame<FrameBase>(problem->getTrajectory(), t, "POV", x_origin);
+ 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);
diff --git a/test/gtest_processor_force_torque_preint.cpp b/test/gtest_processor_force_torque_preint.cpp
index 479538dd4fd23b872237b523a30dc16654d8bed2..287b9525096e5a83f680611acb351b0648874c87 100644
--- a/test/gtest_processor_force_torque_preint.cpp
+++ b/test/gtest_processor_force_torque_preint.cpp
@@ -129,7 +129,6 @@ public:
params_sen_ft->dist_traveled = 20000.0;
params_sen_ft->angle_turned = 1000;
params_sen_ft->voting_active = true;
- params_sen_ft->voting_aux_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");
proc_ftpreint_ = std::static_pointer_cast<ProcessorForceTorquePreint>(proc_ft_base);
@@ -205,7 +204,7 @@ public:
// - call setOrigin on processors isMotion
setOriginState();
MatrixXd P_origin_ = pow(1e-3, 2) * MatrixXd::Identity(18,18);
- KF1_ = problem_->emplaceKeyFrame( t0_, x_origin_);
+ 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));
pose_prior_capture->emplaceFeatureAndFactor();