1#define BIORBD_API_EXPORTS
2#include "RigidBody/KalmanReconsMarkers.h"
8#include <rbdl/Kinematics.h>
10#include "BiorbdModel.h"
11#include "RigidBody/GeneralizedAcceleration.h"
12#include "RigidBody/GeneralizedCoordinates.h"
13#include "RigidBody/GeneralizedVelocity.h"
14#include "RigidBody/NodeSegment.h"
15#include "Utils/Error.h"
16#include "Utils/Matrix.h"
18using namespace BIORBD_NAMESPACE;
28 : rigidbody::
KalmanRecons(model, model.nbTechnicalMarkers() * 3, params),
59 const std::vector<size_t> &occlusion) {
60 for (
size_t i = 0; i < occlusion.size(); ++i)
61 for (
size_t j = occlusion[i] * 3; j < occlusion[i] * 3 + 3; ++j) {
79 for (
size_t i = 0; i < Tobs.
nbMarkers(); ++i) {
80 T.block(i * 3, 0, 3, 1) = Tobs.
marker(i);
89 const std::vector<NodeSegment> &Tobs,
96 for (
size_t i = 0; i < Tobs.size(); ++i) {
97 T.block(i * 3, 0, 3, 1) = Tobs[i];
124 for (
size_t j = 0; j < 2;
130 for (
size_t i = 0; i < 50; ++i) {
147 Model &updatedModel =
151 const std::vector<rigidbody::NodeSegment> &zest_tp(
154 const std::vector<utils::Matrix> &J_tp(
162 std::vector<size_t> occlusionIdx;
168#ifdef BIORBD_USE_CASADI_MATH
169 !Tobs(i * 3).is_zero() && !Tobs(i * 3 + 1).is_zero() &&
170 !Tobs(i * 3 + 2).is_zero()
172 Tobs(i * 3) * Tobs(i * 3) + Tobs(i * 3 + 1) * Tobs(i * 3 + 1) +
173 Tobs(i * 3 + 2) * Tobs(i * 3 + 2) !=
176 Tobs(i * 3) * Tobs(i * 3) + Tobs(i * 3 + 1) * Tobs(i * 3 + 1) +
177 Tobs(i * 3 + 2) * Tobs(i * 3 + 2))
180 H.block(i * 3, 0, 3, *
m_nbDof) = J_tp[i];
181 zest.block(i * 3, 0, 3, 1) = zest_tp[i];
183 occlusionIdx.push_back(i);
The actual musculoskeletal model that holds everything in biorbd.
Class GeneralizedAcceleration.
Class GeneralizedCoordinates.
Class GeneralizedVelocity.
Joints & UpdateKinematicsCustom(const GeneralizedCoordinates *Q=nullptr, const GeneralizedVelocity *Qdot=nullptr, const rigidbody::GeneralizedAcceleration *Qddot=nullptr)
Update the kinematic variables such as body velocities and accelerations in the model to reflect the ...
Parameters of the reconstruction.
Class Kinematic reconstruction algorithm using an Extended Kalman Filter using skin markers.
std::shared_ptr< utils::Matrix > m_PpInitial
Initial covariance matrix.
virtual void reconstructFrame()
This function cannot be used to reconstruct frames.
KalmanReconsMarkers DeepCopy() const
Deep copy of the Kalman reconstruction.
virtual void initialize()
Initialization of the filter.
KalmanReconsMarkers()
Initialize the Kalman filter and Kalman reconstruction for Markers data.
virtual void manageOcclusionDuringIteration(utils::Matrix &InvTp, utils::Vector &measure, const std::vector< size_t > &occlusion)
Manage the occlusion during the iteration.
virtual void reconstructFrame(Model &model, const Markers &Tobs, GeneralizedCoordinates *Q, GeneralizedVelocity *Qdot, GeneralizedAcceleration *Qddot, bool removeAxes=true)
Reconstruct the kinematics from markers data.
bool first()
Return if the first iteration was done.
std::shared_ptr< bool > m_firstIteration
If first iteration was done.
std::shared_ptr< utils::Matrix > m_Pp
Covariance matrix.
void iteration(utils::Vector measure, const utils::Vector &projectedMeasure, const utils::Matrix &Hessian, const std::vector< size_t > &occlusion=std::vector< size_t >())
Compute an iteration of the Kalman filter.
std::shared_ptr< size_t > m_nMeasure
Number of measurements.
std::shared_ptr< size_t > m_nbDof
Number of states.
virtual void initialize()
Initialization of the filter.
void DeepCopy(const KalmanRecons &other)
Deep copy of Kalman reconstruction.
std::shared_ptr< utils::Vector > m_xp
State vector.
KalmanRecons()
Kalman reconstruction.
void getState(GeneralizedCoordinates *Q=nullptr, GeneralizedVelocity *Qdot=nullptr, GeneralizedAcceleration *Qddot=nullptr)
Get the state (Q, Qdot, Qddot).
std::shared_ptr< utils::Matrix > m_A
Evolution matrix.
Holder for the marker set.
const NodeSegment & marker(size_t idx) const
Return a marker of index idx in the marker set.
std::vector< NodeSegment > technicalMarkers(rigidbody::Joints &updatedModel, const GeneralizedCoordinates &Q, bool removeAxis=true)
Return all the technical markers at a given Q in the global reference frame.
size_t nbTechnicalMarkers()
Return the number of technical markers.
size_t nbMarkers() const
Return the number of markers.
std::vector< utils::Matrix > technicalMarkersJacobian(rigidbody::Joints &updatedModel, const GeneralizedCoordinates &Q, bool removeAxis=true)
Return the jacobian of the technical markers.
static void raise(const String &message)
Throw an error message.
A wrapper for the Eigen::MatrixXd.
Wrapper of the Eigen VectorXd.