Biorbd Release_1.13.0
Loading...
Searching...
No Matches
KalmanReconsMarkers.cpp
1#define BIORBD_API_EXPORTS
2#include "RigidBody/KalmanReconsMarkers.h"
3
4#include <math.h>
5
6#include <rbdl/Model.h>
7
8#include <rbdl/Kinematics.h>
9
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"
17
18using namespace BIORBD_NAMESPACE;
19
21 : rigidbody::KalmanRecons(),
22 m_PpInitial(std::make_shared<utils::Matrix>()),
23 m_firstIteration(std::make_shared<bool>(true)) {}
24
26 Model &model,
28 : rigidbody::KalmanRecons(model, model.nbTechnicalMarkers() * 3, params),
29 m_PpInitial(std::make_shared<utils::Matrix>()),
30 m_firstIteration(std::make_shared<bool>(true)) {
31 // Initialize the filter
32 initialize();
33}
34
41
48
51
52 // Keep in mind the initial m_Pp
53 *m_PpInitial = *m_Pp;
54}
55
57 utils::Matrix &InvTp,
58 utils::Vector &measure,
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) {
62 InvTp(j, j) = 0; // Artefact due to the fact that m_R has a value
63 // at (j:j+2,j:j+2)
64 measure[j] = 0;
65 }
66}
67
69
71 Model &model,
72 const rigidbody::Markers &Tobs,
76 bool removeAxes) {
77 // Separate the tobs in a big vector
78 utils::Vector T(3 * Tobs.nbMarkers());
79 for (size_t i = 0; i < Tobs.nbMarkers(); ++i) {
80 T.block(i * 3, 0, 3, 1) = Tobs.marker(i);
81 }
82
83 // Reconstruct the kinematics
84 reconstructFrame(model, T, Q, Qdot, Qddot, removeAxes);
85}
86
88 Model &model,
89 const std::vector<NodeSegment> &Tobs,
93 bool removeAxes) {
94 // Separate the tobs in a big vector
95 utils::Vector T(static_cast<size_t>(3 * Tobs.size()));
96 for (size_t i = 0; i < Tobs.size(); ++i) {
97 T.block(i * 3, 0, 3, 1) = Tobs[i];
98 }
99
100 // Reconstruct the kinematics
101 reconstructFrame(model, T, Q, Qdot, Qddot, removeAxes);
102}
103
105 Model &model,
106 const utils::Vector &Tobs,
110 bool removeAxes) {
111 // An iteration of the Kalman filter
112 if (*m_firstIteration) {
113 *m_firstIteration = false;
114 utils::Vector TobsTP(Tobs);
115 TobsTP.block(
116 3 * model.nbTechnicalMarkers(0),
117 0,
118 3 * model.nbTechnicalMarkers() - 3 * model.nbTechnicalMarkers(0),
119 1) =
120 utils::Vector::Zero(
121 3 * model.nbTechnicalMarkers() -
122 3 * model.nbTechnicalMarkers(
123 0)); // Only keep the markers of the root
124 for (size_t j = 0; j < 2;
125 ++j) { // Do the root and then the rest of the body
126 if (j != 0) {
127 TobsTP = Tobs; // Re-take all the markers
128 }
129
130 for (size_t i = 0; i < 50; ++i) {
131 // The first time, call in a recursive manner to get a descent
132 // initial position
133 reconstructFrame(model, TobsTP, nullptr, nullptr, nullptr);
134
135 // Reset Pp to initial (we are not interested in the velocity to
136 // get to the initial position)
137 *m_Pp = *m_PpInitial;
138 m_xp->block(*m_nbDof, 0, *m_nbDof * 2, 1) = utils::Vector::Zero(
139 *m_nbDof * 2); // Set velocity and acceleration to zero
140 }
141 }
142 }
143
144 // Projected state
145 const utils::Vector &xkm(*m_A * *m_xp);
146 const rigidbody::GeneralizedCoordinates &Q_tp(xkm.topRows(*m_nbDof));
147 Model &updatedModel =
148 static_cast<Model &>(model.UpdateKinematicsCustom(&Q_tp));
149
150 // Projected markers
151 const std::vector<rigidbody::NodeSegment> &zest_tp(
152 updatedModel.technicalMarkers(Q_tp, removeAxes, false));
153 // Jacobian
154 const std::vector<utils::Matrix> &J_tp(
155 updatedModel.technicalMarkersJacobian(Q_tp, removeAxes, false));
156 // Create only one matrix for zest and Jacobian
158 utils::Matrix::Zero(
159 *m_nMeasure,
160 *m_nbDof * 3)); // 3*nMarkers => X,Y,Z ; 3*nbDof => Q, Qdot, Qddot
161 utils::Vector zest(utils::Vector::Zero(*m_nMeasure));
162 std::vector<size_t> occlusionIdx;
163 for (size_t i = 0; i < *m_nMeasure / 3;
164 ++i) // Divided by 3 because we are integrate once xyz
165
166 if (
167 // If there is a marker
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()
171#else
172 Tobs(i * 3) * Tobs(i * 3) + Tobs(i * 3 + 1) * Tobs(i * 3 + 1) +
173 Tobs(i * 3 + 2) * Tobs(i * 3 + 2) !=
174 0.0 &&
175 !isnan(
176 Tobs(i * 3) * Tobs(i * 3) + Tobs(i * 3 + 1) * Tobs(i * 3 + 1) +
177 Tobs(i * 3 + 2) * Tobs(i * 3 + 2))
178#endif
179 ) {
180 H.block(i * 3, 0, 3, *m_nbDof) = J_tp[i];
181 zest.block(i * 3, 0, 3, 1) = zest_tp[i];
182 } else {
183 occlusionIdx.push_back(i);
184 }
185
186 // Filter
187 iteration(Tobs, zest, H, occlusionIdx);
188
189 getState(Q, Qdot, Qddot);
190}
191
193 utils::Error::raise("Implémentation impossible");
194}
The actual musculoskeletal model that holds everything in biorbd.
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 ...
Definition Joints.cpp:1888
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.
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.
Definition Markers.h:26
const NodeSegment & marker(size_t idx) const
Return a marker of index idx in the marker set.
Definition Markers.cpp:72
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.
Definition Markers.cpp:384
size_t nbTechnicalMarkers()
Return the number of technical markers.
Definition Markers.cpp:670
size_t nbMarkers() const
Return the number of markers.
Definition Markers.cpp:518
std::vector< utils::Matrix > technicalMarkersJacobian(rigidbody::Joints &updatedModel, const GeneralizedCoordinates &Q, bool removeAxis=true)
Return the jacobian of the technical markers.
Definition Markers.cpp:561
static void raise(const String &message)
Throw an error message.
Definition Error.cpp:6
A wrapper for the Eigen::MatrixXd.
Definition Matrix.h:18
Wrapper of the Eigen VectorXd.
Definition Vector.h:21