diff --git a/modules/body/include/openma/body/plugingait.h b/modules/body/include/openma/body/plugingait.h index d500607..3944a9b 100644 --- a/modules/body/include/openma/body/plugingait.h +++ b/modules/body/include/openma/body/plugingait.h @@ -51,7 +51,9 @@ namespace body OPENMA_DECLARE_NODEID(PluginGait, SkeletonHelper) public: - PluginGait(int region, int side, Node* parent = nullptr); + typedef enum {Basic = 0x00, KAD, KADMed} Variant; + + PluginGait(int region, int side, Variant variant, Node* parent = nullptr); double markerDiameter() const _OPENMA_NOEXCEPT; void setMarkerDiameter(double value) _OPENMA_NOEXCEPT; @@ -83,6 +85,18 @@ namespace body void setRightAsisTrochanterAPDistance(double value) _OPENMA_NOEXCEPT; double leftAsisTrochanterAPDistance() const _OPENMA_NOEXCEPT; void setLeftAsisTrochanterAPDistance(double value) _OPENMA_NOEXCEPT; + double rightTibialTorsionOffset() const _OPENMA_NOEXCEPT; + void setRightTibialTorsionOffset(double value) _OPENMA_NOEXCEPT; + double leftTibialTorsionOffset() const _OPENMA_NOEXCEPT; + void setLeftTibialTorsionOffset(double value) _OPENMA_NOEXCEPT; + double rightThighRotationOffset() const _OPENMA_NOEXCEPT; + void setRightThighRotationOffset(double value) _OPENMA_NOEXCEPT; + double leftThighRotationOffset() const _OPENMA_NOEXCEPT; + void setLeftThighRotationOffset(double value) _OPENMA_NOEXCEPT; + double rightShankRotationOffset() const _OPENMA_NOEXCEPT; + void setRightShankRotationOffset(double value) _OPENMA_NOEXCEPT; + double leftShankRotationOffset() const _OPENMA_NOEXCEPT; + void setLeftShankRotationOffset(double value) _OPENMA_NOEXCEPT; double rightKneeWidth() const _OPENMA_NOEXCEPT; void setRightKneeWidth(double value) _OPENMA_NOEXCEPT; double leftKneeWidth() const _OPENMA_NOEXCEPT; @@ -100,6 +114,8 @@ namespace body double rightStaticRotationOffset() const _OPENMA_NOEXCEPT; double leftStaticPlantarFlexionOffset() const _OPENMA_NOEXCEPT; double leftStaticRotationOffset() const _OPENMA_NOEXCEPT; + double rightAnkleAbAddOffset() const _OPENMA_NOEXCEPT; + double leftAnkleAbAddOffset() const _OPENMA_NOEXCEPT; virtual bool calibrate(Node* trials, Subject* subject) override; virtual LandmarksTranslator* defaultLandmarksTranslator() override; diff --git a/modules/body/include/openma/body/plugingait_p.h b/modules/body/include/openma/body/plugingait_p.h index 8d5c9e5..9a992a9 100644 --- a/modules/body/include/openma/body/plugingait_p.h +++ b/modules/body/include/openma/body/plugingait_p.h @@ -47,6 +47,9 @@ #include "openma/base/macros.h" // _OPENMA_CONSTEXPR #include "openma/math.h" +#include +#include + #include namespace ma @@ -84,6 +87,12 @@ namespace body Property {"leftLegLength"}, Property {"rightAsisTrochanterAPDistance"}, Property {"leftAsisTrochanterAPDistance"}, + Property {"rightTibialTorsionOffset"}, + Property {"leftTibialTorsionOffset"}, + Property {"rightThighRotationOffset"}, + Property {"leftThighRotationOffset"}, + Property {"rightShankRotationOffset"}, + Property {"leftShankRotationOffset"}, Property {"rightKneeWidth"}, Property {"leftKneeWidth"}, Property {"rightAnkleWidth"}, @@ -95,18 +104,22 @@ namespace body Property {"rightStaticPlantarFlexionOffset"}, Property {"rightStaticRotationOffset"}, Property {"leftStaticPlantarFlexionOffset"}, - Property {"leftStaticRotationOffset"} + Property {"leftStaticRotationOffset"}, + Property {"rightAnkleAbAddOffset"}, + Property {"leftAnkleAbAddOffset"} ) public: - PluginGaitPrivate(PluginGait* pint, const std::string& name, int region, int side); + PluginGaitPrivate(PluginGait* pint, const std::string& name, int region, int side, int variant); ~PluginGaitPrivate() _OPENMA_NOEXCEPT; - void computeHipJointCenter(double* HJC, double S, double C, double xdis) const _OPENMA_NOEXCEPT; + void computeHipJointCentre(double* HJC, double S, double C, double xdis) const _OPENMA_NOEXCEPT; bool calibrateLowerLimb(int side, const math::Position* HJC, ummp* landmarks) _OPENMA_NOEXCEPT; bool reconstructUpperLimb(Model* model, Trial* trial, int side, const math::Vector* u_torso, const math::Vector* o_torso, ummp* landmarks, double sampleRate, double startTime) const _OPENMA_NOEXCEPT; bool reconstructLowerLimb(Model* model, Trial* trial, int side, const math::Vector* HJC, ummp* landmarks, double sampleRate, double startTime) const _OPENMA_NOEXCEPT; + int Variant; + double MarkerDiameter; bool HeadOffsetEnabled; @@ -125,6 +138,12 @@ namespace body double LeftLegLength; double RightAsisTrochanterAPDistance; double LeftAsisTrochanterAPDistance; + double RightTibialTorsionOffset; + double LeftTibialTorsionOffset; + double RightThighRotationOffset; + double LeftThighRotationOffset; + double RightShankRotationOffset; + double LeftShankRotationOffset; double RightKneeWidth; double LeftKneeWidth; double RightAnkleWidth; @@ -137,9 +156,15 @@ namespace body double RightStaticRotationOffset; double LeftStaticPlantarFlexionOffset; double LeftStaticRotationOffset; + double RightAnkleAbAddOffset; + double LeftAnkleAbAddOffset; + + using CalibrateJointFuncPtr = bool (*)(math::Position* , math::Position*, std::vector& , PluginGaitPrivate* , ummp* , const std::string& , double , double , const math::Position* ); + CalibrateJointFuncPtr CalibrateKneeJointCentre; + CalibrateJointFuncPtr CalibrateAnkleJointCentre; }; - inline void PluginGaitPrivate::computeHipJointCenter(double* HJC, double S, double C, double xdis) const _OPENMA_NOEXCEPT + inline void PluginGaitPrivate::computeHipJointCentre(double* HJC, double S, double C, double xdis) const _OPENMA_NOEXCEPT { //const double theta = 0.49567350756639; // 28.4 * M_PI / 180.0; //const double beta = 0.314159265358979; // 18.0 * M_PI / 180.0; @@ -344,10 +369,11 @@ namespace math typename Nested::type m_Xpr2; typename Nested::type m_Xpr3; double Offset; + double Beta; public: - ChordOp(double offset, const XprBase& x1, const XprBase& x2, const XprBase& x3) - : m_Xpr1(x1), m_Xpr2(x2), m_Xpr3(x3), Offset(offset) + ChordOp(double offset, const XprBase& x1, const XprBase& x2, const XprBase& x3, double beta) + : m_Xpr1(x1), m_Xpr2(x2), m_Xpr3(x3), Offset(offset), Beta(beta) { assert(this->m_Xpr1.rows() == this->m_Xpr2.rows()); assert(this->m_Xpr2.rows() == this->m_Xpr3.rows()); @@ -370,7 +396,7 @@ namespace math const Vector u = v.cross(K - I).normalized(); const Pose local(u, v, u.cross(v), (J + I) / 2.0); // Compute the angle to project I along v - const auto d = (I - J).norm(); + const Scalar d = (I - J).norm(); const auto theta = (d.residuals() >= 0).select((this->Offset / d.values()).asin()*2.0, 0.0); // Do this projection in the local frame Vector t = local.inverse().transform(I); @@ -380,7 +406,73 @@ namespace math t.values().middleCols<1>(1) = _tempX * theta.cos() - _tempY * theta.sin() ; t.values().rightCols<1>() = _tempX * theta.sin() + _tempY * theta.cos() ; // Transform back the projected point and return the result - return Eigen::internal::ChordOpValues(local.transform(t).values()); + Vector P = local.transform(t); + // Is there a second condition that the projected point need to meet? + // In case the beta angle is not null, an iterative solution is used to + // Find the best projected point with the specified offset and the angle. + // NOTE: THE PART BELOW IS AN ADAPTION OF THE CODE PROPOSED IN THE PYCGA PROJECT WHICH TAKES IT FROM MORGAN SANGEUX + if (fabs(this->Beta) > std::numeric_limits::epsilon()) + { + + // Next part is iterative and realized for each sample + const double eps = 1e-5; + for (Index i = 0, len = P.rows() ; i < len ; ++i) + { + if (P.residuals().coeff(i) < 0.0) + continue; + int count = 0; + double sAlpha = 0, iAlpha = this->Beta, dBeta = fabs(this->Beta); + // Angle + double ca = cos(theta.coeff(i) / 2.0); + // Distance where should be the new project point + double r = this->Offset * ca; + // Compute the projection of K on IJ using the Pythagorean theorem + const Eigen::Matrix O = J.values().row(i) - v.values().row(i) * sqrt(d.values().coeff(i) * d.values().coeff(i) - this->Offset * this->Offset * (1 + ca * ca)); + // Compute the local frame used to find the new projected point satisfaying an angle close to beta + Eigen::Matrix Pi = P.values().row(i); + const Eigen::Matrix w2 = v.values().row(i); + const Eigen::Matrix v2 = w2.cross(Pi-O).normalized(); + const Eigen::Matrix u2 = v2.cross(w2).normalized(); + // const Pose local2(v,v2,v2.cross(v).normalized(),O); + while ((dBeta > eps) && (count < 1000)) + { + sAlpha += iAlpha; + // Compute the new projected point + double c = r * cos(sAlpha); + double s = r * sin(sAlpha); + P.values().coeffRef(i,0) = u2.coeff(0) * c + v2.coeff(0) * s + O.coeff(0); + P.values().coeffRef(i,1) = u2.coeff(1) * c + v2.coeff(1) * s + O.coeff(1); + P.values().coeffRef(i,2) = u2.coeff(2) * c + v2.coeff(2) * s + O.coeff(2); + // Verify if the new point projected on the plane corresponds to the angle beta + Pi = P.values().row(i); + const Eigen::Matrix Ii = I.values().row(i); + const Eigen::Matrix Ji = J.values().row(i); + const Eigen::Matrix Ki = K.values().row(i); + Eigen::Matrix nBone = Ji - Pi; + Eigen::Matrix projK = nBone.cross((Ki - Pi).cross(nBone)); + Eigen::Matrix projI = nBone.cross((Ii - Pi).cross(nBone)); + double iBeta = sign((projK.cross(projI) * nBone.transpose()).coeff(0)) * acos((projK * projI.transpose() / projK.norm() / projI.norm()).coeff(0)); + double odBeta = dBeta; + dBeta = fabs(this->Beta - iBeta); + // Look for the direction and magnitude used by the next iteration + if ((dBeta - odBeta) > 0.0) + { + if (count == 0) + { + sAlpha -= iAlpha; + iAlpha *= -1.0; + } + else + { + iAlpha /= -2.0; + } + } + ++count; + } + } + } + // Pass the result to be assigned in the result of the operation chord. + return Eigen::internal::ChordOpValues(P.values()); }; auto residuals() const _OPENMA_NOEXCEPT -> decltype(generate_residuals((OPENMA_MATHS_DECLVAL_NESTED(XprOne).residuals() >= 0.0) && (OPENMA_MATHS_DECLVAL_NESTED(XprTwo).residuals() >= 0.0) && (OPENMA_MATHS_DECLVAL_NESTED(XprThree).residuals() >= 0.0))) @@ -390,9 +482,9 @@ namespace math }; template - static inline ChordOp compute_chord(double offset, const XprBase& I, const XprBase& J, const XprBase& K) + static inline ChordOp compute_chord(double offset, const XprBase& I, const XprBase& J, const XprBase& K, double beta = 0.0) { - return ChordOp(offset,I,J,K); + return ChordOp(offset,I,J,K,beta); }; }; }; diff --git a/modules/body/src/plugingait.cpp b/modules/body/src/plugingait.cpp index 5acf65e..7f655f7 100644 --- a/modules/body/src/plugingait.cpp +++ b/modules/body/src/plugingait.cpp @@ -62,13 +62,159 @@ #ifndef DOXYGEN_SHOULD_SKIP_THIS +static _OPENMA_CONSTEXPR int PartShankTorsioned = ma::body::Part::User+1; + +void _ma_plugingait_construct_thigh_pose(ma::math::Vector* u, ma::math::Vector* v, ma::math::Vector* w, const ma::math::Map* LFE, const ma::math::Position* HJC, const ma::math::Position* KJC, double s) +{ + *w = (*HJC - *KJC).normalized(); + *u = s * w->cross(*LFE - *KJC).normalized(); + *v = w->cross(*u); +}; + +void _ma_plugingait_construct_shank_pose(ma::math::Vector* u, ma::math::Vector* v, ma::math::Vector* w, const ma::math::Map* LTM, const ma::math::Position* KJC, const ma::math::Position* AJC, double s) +{ + *w = (*KJC - *AJC).normalized(); + *u = s * w->cross(*LTM - *AJC).normalized(); + *v = w->cross(*u); +}; + +void _ma_plugingait_compute_shank_rotation_offset(double* offset, const ma::math::Position* AJC, const ma::math::Position* KJC, const ma::math::Map* LS, const ma::math::Map* LTM, double s) +{ + ma::math::Vector u, v, w; + _ma_plugingait_construct_shank_pose(&u, &v, &w, LTM, KJC, AJC, s); + v *= -s; + ma::math::Vector LS_proj_trans_unit = ((*LS - (*LS - *AJC).dot(w).replicate<3>() * w) - *AJC).normalized(); + ma::math::Vector cross = LS_proj_trans_unit.cross(v); + ma::math::Scalar dot = LS_proj_trans_unit.dot(v); + const double ss = s * sign(static_cast(cross.dot(w).mean())); + *offset = ss * cross.norm().atan2(dot).mean(); +}; + +bool _ma_plugingait_calibrate_kjc_basic(ma::math::Position* KJC, ma::math::Position* /**/, std::vector& /*offsets*/, ma::body::PluginGaitPrivate* optr, ma::body::ummp* landmarks, const std::string& prefix, double /*s*/, double kneeWidth, const ma::math::Position* HJC) +{ + const auto& ITB = (*landmarks)[prefix+"ITB"]; + const auto& LFE = (*landmarks)[prefix+"LFE"]; + if (!ITB.isValid() || !LFE.isValid()) + { + ma::error("PluginGait - Missing landmarks to define the thigh. Calibration aborted."); + return false; + } + // Compute the knee joint centre (KJC) + *KJC = ma::math::compute_chord((optr->MarkerDiameter + kneeWidth) / 2.0, LFE, *HJC, ITB); + return true; +}; + +bool _ma_plugingait_calibrate_kjc_kad(ma::math::Position* KJC, ma::math::Position* VLFE, std::vector& offsets, ma::body::PluginGaitPrivate* optr, ma::body::ummp* landmarks, const std::string& prefix, double s, double kneeWidth, const ma::math::Position* HJC) +{ + // Required landmarks: *.ITB, *.LFE + const auto& KAX = (*landmarks)[prefix+"KAX"]; + const auto& KD1 = (*landmarks)[prefix+"KD1"]; + const auto& KD2 = (*landmarks)[prefix+"KD2"]; + const auto& ITB = (*landmarks)[prefix+"ITB"]; + if (!KAX.isValid() || !KD1.isValid() || !KD2.isValid() || !ITB.isValid()) + { + ma::error("PluginGait - Missing landmarks to define the thigh. Calibration aborted."); + return false; + } + // Compute the virtual lateral femoral epicondyle (VLFE) + double dist = ((KAX - KD1).norm() + (KAX - KD2).norm() + (KD1 - KD2).norm()) / sqrt(2.0) / 3.0; + auto n = -s * (KD2 - KD1).cross(KAX - KD1).normalized(); + auto I = (KD1 + KAX) / 2.0; + auto PP1 = (2.0 /3.0 * (I - KD2)) + KD2; + *VLFE = PP1 - (n * sqrt(3.0) * dist / 3.0); + // Compute the knee joint centre (KJC) + *KJC = ma::math::compute_chord((optr->MarkerDiameter + kneeWidth) / 2.0, *VLFE, *HJC, KAX); + // Compute the thigh marker rotation offset + ma::math::Vector w = (*HJC - *KJC).normalized(); + ma::math::Vector ITB_proj_trans_unit = ((ITB - (ITB - *KJC).dot(w).replicate<3>() * w) - *KJC).normalized(); + ma::math::Vector v = (*VLFE - *KJC).normalized(); + ma::math::Vector cross = ITB_proj_trans_unit.cross(v); + ma::math::Scalar dot = ITB_proj_trans_unit.dot(v); + const double ss = s * sign(static_cast(cross.dot(w).mean())); + *offsets[0] = ss * cross.norm().atan2(dot).mean(); + return true; +}; + +bool _ma_plugingait_calibrate_ajc_basic(ma::math::Position* AJC, ma::math::Position* /**/, std::vector& /*offsets*/, ma::body::PluginGaitPrivate* optr, ma::body::ummp* landmarks, const std::string& prefix, double /*s*/, double ankleWidth, const ma::math::Position* KJC) +{ + const auto& LTM = (*landmarks)[prefix+"LTM"]; + const auto& LS = (*landmarks)[prefix+"LS"]; + if (!LTM.isValid() || !LS.isValid()) + { + ma::error("PluginGait - Missing landmarks to define the shank. Calibration aborted."); + return false; + } + // Compute the ankle joint centre (AJC) + *AJC = ma::math::compute_chord((optr->MarkerDiameter + ankleWidth) / 2.0, LTM, *KJC, LS); + return true; +}; + +bool _ma_plugingait_calibrate_ajc_kad(ma::math::Position* AJC, ma::math::Position* /**/, std::vector& offsets, ma::body::PluginGaitPrivate* optr, ma::body::ummp* landmarks, const std::string& prefix, double s, double ankleWidth, const ma::math::Position* KJC) +{ + const auto& LTM = (*landmarks)[prefix+"LTM"]; + const auto& KAX = (*landmarks)[prefix+"KAX"]; + const auto& LS = (*landmarks)[prefix+"LS"]; + if (!LTM.isValid() || !KAX.isValid() || !LS.isValid()) + { + ma::error("PluginGait - Missing landmarks to define the shank. Calibration aborted."); + return false; + } + // Compute the ankle joint centre (AJC) + *AJC = ma::math::compute_chord((optr->MarkerDiameter + ankleWidth) / 2.0, LTM, *KJC, KAX); + // Compute the shank marker rotation offset + _ma_plugingait_compute_shank_rotation_offset(offsets[1], AJC, KJC, &LS, <M, s); + return true; +}; + +bool _ma_plugingait_calibrate_ajc_kadmed(ma::math::Position* AJC, ma::math::Position* VLFE, std::vector& offsets, ma::body::PluginGaitPrivate* optr, ma::body::ummp* landmarks, const std::string& prefix, double s, double ankleWidth, const ma::math::Position* KJC) +{ + const auto& LTM = (*landmarks)[prefix+"LTM"]; + const auto& MTM = (*landmarks)[prefix+"MTM"]; + const auto& LS = (*landmarks)[prefix+"LS"]; + if (!LTM.isValid() || !MTM.isValid() || !LS.isValid()) + { + ma::error("PluginGait - Missing landmarks to define the shank. Calibration aborted."); + return false; + } + // Compute the ankle joint centre (AJC) + *AJC = LTM + (optr->MarkerDiameter + ankleWidth) * 0.5 * (MTM - LTM).normalized(); + // Compute the shank marker rotation offset + _ma_plugingait_compute_shank_rotation_offset(offsets[1], AJC, KJC, &LS, <M, s); + // Compute the tibial torsion offset + ma::math::Vector u, v, w; + // 1. Construct the shank SCS + _ma_plugingait_construct_shank_pose(&u, &v, &w, <M, KJC, AJC, s); + // 2. Construct the ML axis of the thigh SCS + ma::math::Vector temp = -s * (*VLFE - *KJC).normalized(); + // 3. Express this axis in the shank SCS + // NOTE: The following lines are a simplication of the transformation R_shank.transpose().transform(v_thigh); + ma::math::Vector temp_local_shank(KJC->rows()); + temp_local_shank.residuals().setZero(); + temp_local_shank.x() = u.dot(temp); + temp_local_shank.y() = v.dot(temp); + temp_local_shank.z() = w.dot(temp); + // 4. Project it on XY plane and normalize the result + temp_local_shank.values().col(2).setZero(); + ma::math::Vector v_thigh_local_shank = temp_local_shank.normalized(); + // 5. Compute the angle between v_shank and the projected v_thigh_local_shank + // NOTE: v_shank is equal to [0 1 0]. The dot product and crossnorm are simplified + *offsets[2] = s * v_thigh_local_shank.x().atan2(v_thigh_local_shank.y()).mean(); + // Compute the adduction / abduction ankle joint offset + temp = -s * (LTM - * AJC).normalized(); + ma::math::Scalar dot = v.dot(temp); + ma::math::Scalar crossnorm = v.cross(temp).norm(); + *offsets[3] = crossnorm.atan2(dot).mean(); + return true; +}; + namespace ma { namespace body { - PluginGaitPrivate::PluginGaitPrivate(PluginGait* pint, const std::string& name, int region, int side) + PluginGaitPrivate::PluginGaitPrivate(PluginGait* pint, const std::string& name, int region, int side, int variant) : SkeletonHelperPrivate(pint,name,region,side), // All the options are null by default + Variant(variant), MarkerDiameter(0.0), HeadOffsetEnabled(false), RightShoulderOffset(0.0), @@ -84,6 +230,12 @@ namespace body LeftLegLength(0.0), RightAsisTrochanterAPDistance(0.0), LeftAsisTrochanterAPDistance(0.0), + RightTibialTorsionOffset(0.0), + LeftTibialTorsionOffset(0.0), + RightThighRotationOffset(0.0), + LeftThighRotationOffset(0.0), + RightShankRotationOffset(0.0), + LeftShankRotationOffset(0.0), RightKneeWidth(0.0), LeftKneeWidth(0.0), RightAnkleWidth(0.0), @@ -94,9 +246,13 @@ namespace body RightStaticPlantarFlexionOffset(0.0), RightStaticRotationOffset(0.0), LeftStaticPlantarFlexionOffset(0.0), - LeftStaticRotationOffset(0.0) + LeftStaticRotationOffset(0.0), + RightAnkleAbAddOffset(0.0), + LeftAnkleAbAddOffset(0.0), + CalibrateKneeJointCentre(nullptr), + CalibrateAnkleJointCentre(nullptr) {}; - + PluginGaitPrivate::~PluginGaitPrivate() _OPENMA_NOEXCEPT = default; bool PluginGaitPrivate::calibrateLowerLimb(int side, const math::Position* HJC, ummp* landmarks) _OPENMA_NOEXCEPT @@ -104,7 +260,9 @@ namespace body auto pptr = this->pint(); std::string prefix; double s = 0.0, ankleWidth = 0.0, kneeWidth = 0.0, seglength = 0.0; - double *staticPlantarFlexionOffset = nullptr, *staticRotationOffset = nullptr; + double *staticPlantarFlexionOffset = nullptr, *staticRotationOffset = nullptr, + *tibialTorsionOffset = nullptr, *thighRotationOffset = nullptr, + *shankRotationOffset = nullptr, *ankleAbAddOffset = nullptr; bool footFlat = false; if (side == Side::Left) { @@ -115,6 +273,10 @@ namespace body footFlat = this->LeftFootFlatEnabled; staticPlantarFlexionOffset = &(this->LeftStaticPlantarFlexionOffset); staticRotationOffset = &(this->LeftStaticRotationOffset); + tibialTorsionOffset = &(this->LeftTibialTorsionOffset); + thighRotationOffset = &(this->LeftThighRotationOffset); + shankRotationOffset = &(this->LeftShankRotationOffset); + ankleAbAddOffset = &(this->LeftAnkleAbAddOffset); } else if (side == Side::Right) { @@ -125,25 +287,25 @@ namespace body footFlat = this->RightFootFlatEnabled; staticPlantarFlexionOffset = &(this->RightStaticPlantarFlexionOffset); staticRotationOffset = &(this->RightStaticRotationOffset); + tibialTorsionOffset = &(this->RightTibialTorsionOffset); + thighRotationOffset = &(this->RightThighRotationOffset); + shankRotationOffset = &(this->RightShankRotationOffset); + ankleAbAddOffset = &(this->RightAnkleAbAddOffset); } else { error("PluginGait - Unknown side for the lower limb. Calibration aborted."); return false; } + + std::vector offsets{thighRotationOffset, shankRotationOffset, tibialTorsionOffset, ankleAbAddOffset}; // ----------------------------------------- // Thigh // ----------------------------------------- - // Required landmarks: *.ITB, *.LFE - const auto& ITB = (*landmarks)[prefix+"ITB"]; - const auto& LFE = (*landmarks)[prefix+"LFE"]; - if (!ITB.isValid() || !LFE.isValid()) - { - error("PluginGait - Missing landmarks to define the thigh. Calibration aborted."); + math::Position KJC, VLFE; + // NOTE: VLFE is only computed for the KAD and KADMed variants. + if (!this->CalibrateKneeJointCentre(&KJC, &VLFE, offsets, this, landmarks, prefix, s, kneeWidth, HJC)) return false; - } - // Compute the knee joint centre (KJC) - const math::Position KJC = compute_chord((this->MarkerDiameter + kneeWidth) / 2.0, LFE, *HJC, ITB); // Set the segment length seglength = (KJC - *HJC).norm().mean(); pptr->setProperty(prefix+"Thigh.length", seglength); @@ -158,16 +320,10 @@ namespace body // ----------------------------------------- // Shank // ----------------------------------------- - // Required landmarks: *.LTM, *.LS - const auto& LTM = (*landmarks)[prefix+"LTM"]; - const auto& LS = (*landmarks)[prefix+"LS"]; - if (!LTM.isValid() || !LS.isValid()) - { - error("PluginGait - Missing landmarks to define the shank. Calibration aborted."); + math::Position AJC; + // NOTE: VLFE is only used by the KADMed variant. + if (!this->CalibrateAnkleJointCentre(&AJC, &VLFE, offsets, this, landmarks, prefix, s, ankleWidth, &KJC)) return false; - } - // Compute the ankle joint centre (AJC) - const math::Position AJC = compute_chord((this->MarkerDiameter + ankleWidth) / 2.0, LTM, KJC, LS); // Set the segment length seglength = (AJC - KJC).norm().mean(); pptr->setProperty(prefix+"Shank.length", seglength); @@ -178,10 +334,12 @@ namespace body // ----------------------------------------- // Foot // ----------------------------------------- - // Required landmarks: *.MTH2, *.HEE + // Required landmarks: *.MTH2, *.HEE, *.LS, *.LTM const auto& MTH2 = (*landmarks)[prefix+"MTH2"]; math::Position HEE = (*landmarks)[prefix+"HEE"]; // Copy instead of a map due to possible modification on its coordinates if the foot flat option is activated - if (!MTH2.isValid() || !HEE.isValid()) + const auto& LS = (*landmarks)[prefix+"LS"]; + const auto& LTM = (*landmarks)[prefix+"LTM"]; + if (!MTH2.isValid() || !HEE.isValid() || !LS.isValid() || !LTM.isValid()) { error("PluginGait - Missing landmarks to define the foot. Calibration aborted."); return false; @@ -205,9 +363,8 @@ namespace body HEE.block<1>(2) = MTH2.block<1>(2); } // - Shank axes - math::Vector w = (KJC - AJC).normalized(); - math::Vector u = s * w.cross(LS - AJC).normalized(); - math::Vector v_shank = w.cross(u); + math::Vector u, v_shank, w; + _ma_plugingait_construct_shank_pose(&u, &v_shank, &w, <M, &KJC, &AJC, s); // - Foot reference w = (HEE - MTH2).normalized(); u = v_shank.cross(w).normalized(); @@ -329,15 +486,22 @@ namespace body auto pptr = this->pint(); std::string prefix; double s = 0.0, ankleWidth = 0.0, kneeWidth = 0.0, - staticPlantarFlexionOffset = 0.0, staticRotationOffset = 0.0; + thighRotationOffset = 0.0, + shankRotationOffset = 0.0, tibialTorsionOffset = 0.0, + staticPlantarFlexionOffset = 0.0, staticRotationOffset = 0.0, + ankleAbAddOffset = 0.0; if (side == Side::Left) { prefix = "L."; s = -1.0; ankleWidth = this->LeftAnkleWidth; kneeWidth = this->LeftKneeWidth; + thighRotationOffset = -this->LeftThighRotationOffset; + shankRotationOffset = -this->LeftShankRotationOffset; + tibialTorsionOffset = -this->LeftTibialTorsionOffset; staticPlantarFlexionOffset = -this->LeftStaticPlantarFlexionOffset; staticRotationOffset = -this->LeftStaticRotationOffset; + ankleAbAddOffset = this->LeftAnkleAbAddOffset; } else if (side == Side::Right) { @@ -345,8 +509,12 @@ namespace body s = 1.0; ankleWidth = this->RightAnkleWidth; kneeWidth = this->RightKneeWidth; + thighRotationOffset = this->RightThighRotationOffset; + shankRotationOffset = this->RightShankRotationOffset; + tibialTorsionOffset = this->RightTibialTorsionOffset; staticPlantarFlexionOffset = -this->RightStaticPlantarFlexionOffset; staticRotationOffset = this->RightStaticRotationOffset; + ankleAbAddOffset = -this->RightAnkleAbAddOffset; } else { @@ -369,10 +537,8 @@ namespace body return false; } seg = model->segments()->findChild({},{{"side",side},{"part",Part::Thigh}},false); - const math::Position KJC = compute_chord((this->MarkerDiameter + kneeWidth) / 2.0, LFE, *HJC, ITB); - u = s * (*HJC - LFE).cross(ITB - LFE).normalized(); - w = (*HJC - KJC).normalized(); - v = w.cross(u); + const math::Position KJC = compute_chord((this->MarkerDiameter + kneeWidth) / 2.0, LFE, *HJC, ITB, thighRotationOffset); + _ma_plugingait_construct_thigh_pose(&u, &v, &w, &LFE, HJC, &KJC, s); math::to_timesequence(u, v, w, KJC, seg->name()+".SCS", sampleRate, startTime, seg); seg->setProperty("length", pptr->property(seg->name()+".length")); if ((bcs = pptr->findChild(seg->name()+".BCS")) != nullptr) bcs->addParent(seg); @@ -389,11 +555,35 @@ namespace body } seg = model->segments()->findChild({},{{"side",side},{"part",Part::Shank}},false); // Compute the ankle joint centre (AJC) - const math::Position AJC = compute_chord((this->MarkerDiameter + ankleWidth) / 2.0, LTM, KJC, LS); - w = (KJC - AJC).normalized(); - u = s * w.cross(LS - AJC).normalized(); - math::Vector v_shank = w.cross(u); - math::to_timesequence(u, v_shank, w, AJC, seg->name()+".SCS", sampleRate, startTime, seg); + math::Position AJC_ = compute_chord((this->MarkerDiameter + ankleWidth) / 2.0, LTM, KJC, LS, shankRotationOffset); + _ma_plugingait_construct_shank_pose(&u, &v, &w, <M, &KJC, &AJC_, s); + math::Position ajc_trans = AJC_ - LTM; + math::Position tmp(AJC_.rows()); + tmp.residuals().setZero(); + tmp.x() = u.dot(ajc_trans); + math::Scalar y = v.dot(ajc_trans); + math::Scalar z = w.dot(ajc_trans); + const double co = cos(ankleAbAddOffset); + const double so = sin(ankleAbAddOffset); + tmp.y() = co * y + so * z; + tmp.z() = -so * y + co * z; + math::Position AJC(AJC_.rows()); + AJC.residuals().setZero(); + AJC.x() = u.x() * tmp.x() + v.x() * tmp.y() + w.x() * tmp.z(); + AJC.y() = u.y() * tmp.x() + v.y() * tmp.y() + w.y() * tmp.z(); + AJC.z() = u.z() * tmp.x() + v.z() * tmp.y() + w.z() * tmp.z(); + AJC += LTM; + // Compute the torsioned shank reference frame + math::Vector v_shank_torsioned; + _ma_plugingait_construct_shank_pose(&u, &v_shank_torsioned, &w, <M, &KJC, &AJC, s); + auto shankTorsioned = model->segments()->findChild({},{{"side",side},{"part",PartShankTorsioned}},false); + math::to_timesequence(u, v_shank_torsioned, w, AJC, shankTorsioned->name()+".SCS", sampleRate, startTime, shankTorsioned); + // Compute the untorsioned shank reference frame + const double ct = cos(tibialTorsionOffset); + const double st = sin(tibialTorsionOffset); + math::Vector u_shank = ct * u - st * v_shank_torsioned; + math::Vector v_shank = st * u + ct * v_shank_torsioned; + math::to_timesequence(u_shank, v_shank, w, AJC, seg->name()+".SCS", sampleRate, startTime, seg); seg->setProperty("length", pptr->property(seg->name()+".length")); if ((bcs = pptr->findChild(seg->name()+".BCS")) != nullptr) bcs->addParent(seg); // ----------------------------------------- @@ -408,7 +598,15 @@ namespace body } seg = model->segments()->findChild({},{{"side",side},{"part",Part::Foot}},false); w = (AJC - MTH2).normalized(); - u = v_shank.cross(w).normalized(); + // NOTE: IT WAS DISCOVERED THAT THE ORIGINAL PLUGIN GAIT CODE USES THE WRONG ML AXIS. INDEED, THE FOOT OFFSETS ARE DEFINED BASED ON THE TORSIONED SHANK BUT, THE ORIGINAL MODEL USES THE UNTORTIONED SHANK FOR THE RECONSTRUCTION +#if defined(NDEBUG) + u = v_shank_torsioned.cross(w).normalized(); +#else + if (pptr->property("_ma_debug_vicon_original_foot_frame").cast()) + u = v_shank.cross(w).normalized(); + else + u = v_shank_torsioned.cross(w).normalized(); +#endif v = w.cross(u); const double cx = cos(staticRotationOffset), sx = sin(staticRotationOffset), cy = cos(staticPlantarFlexionOffset), sy = sin(staticPlantarFlexionOffset); @@ -770,6 +968,36 @@ namespace body * @sa leftAsisTrochanterAPDistance() setLeftAsisTrochanterAPDistance() */ double LeftAsisTrochanterAPDistance; + /** + * [Optional] This property holds the angle between projection of knee Flexion Axis (Y) with projection of the ankle Flexion Axis (Y) onto the plane perpendicular to the longitudinal shank axis. In case the KADMed variant is used, this offset is automatically computed. + * @sa rightTibialTorsionOffset() setRightTibialTorsionOffset() + */ + double RightTibialTorsionOffset; + /** + * [Optional] This property holds the angle between projection of knee Flexion Axis (Y) with projection of the ankle Flexion Axis (Y) onto the plane perpendicular to the longitudinal shank axis. In case the KADMed variant is used, this offset is automatically computed. + * @sa leftTibialTorsionOffset() setLeftTibialTorsionOffset() + */ + double LeftTibialTorsionOffset; + /** + * [Optional] This property holds the angle between projection of Thigh wand marker with the projection of the knee Flexion Axis (Y) onto the plane perpendicular to the longitudinal thigh axis. By default, this property contains the value 0.0. + * @sa rightThighRotationOffset() setRightThighRotationOffset() + */ + double RightThighRotationOffset; + /** + * [Optional] This property holds the angle between projection of Thigh wand marker with the projection of the knee Flexion Axis (Y) onto the plane perpendicular to the longitudinal thigh axis. By default, this property contains the value 0.0. + * @sa leftThighRotationOffset() setLeftThighRotationOffset() + */ + double LeftThighRotationOffset; + /** + * [Optional] This property holds the angle between projection of Shank wand marker with projection of the ankle Flexion Axis (Y) onto the plane perpendicular to the longitudinal shank axis. By default, this property contains the value 0.0. + * @sa rightShankRotationOffset() setRightShankRotationOffset() + */ + double RightShankRotationOffset; + /** + * [Optional] This property holds the angle between projection of Shank wand marker with projection of the ankle Flexion Axis (Y) onto the plane perpendicular to the longitudinal shank axis. By default, this property contains the value 0.0. + * @sa leftShankRotationOffset() setLeftShankRotationOffset() + */ + double LeftShankRotationOffset; /** * [Required] This property holds the distance between femoral epycondyles. By default, this property contains the value 0.0. * @sa rightKneeWidth() setRightKneeWidth() @@ -825,16 +1053,43 @@ namespace body * @sa leftStaticRotationOffset() */ double LeftStaticRotationOffset; + /** + * [Calculated] This property holds the projection of the ankle flexion axis with projection of the vector delimited by AJC and ANK onto the plane defined by the shank axis. By default, this property contains the value 0.0. + * @sa rightAnkleAbAddOffset() + */ + double RightAnkleAbAddOffset; + /** + * [Calculated] This property holds the projection of the ankle flexion axis with projection of the vector delimited by AJC and ANK onto the plane defined by the shank axis. By default, this property contains the value 0.0. + * @sa leftAnkleAbAddOffset() + */ + double LeftAnkleAbAddOffset; }; #endif /** * Constructor. The name is set to "PluginGait". */ - PluginGait::PluginGait(int region, int side, Node* parent) - : SkeletonHelper(*new PluginGaitPrivate(this, "PluginGait", region, side), parent) - {}; - + PluginGait::PluginGait(int region, int side, Variant variant, Node* parent) + : SkeletonHelper(*new PluginGaitPrivate(this, "PluginGait", region, side, variant), parent) + { + auto optr = this->pimpl(); + if (variant == Basic) + { + optr->CalibrateKneeJointCentre = &_ma_plugingait_calibrate_kjc_basic; + optr->CalibrateAnkleJointCentre = &_ma_plugingait_calibrate_ajc_basic; + } + else if (variant == KAD) + { + optr->CalibrateKneeJointCentre = &_ma_plugingait_calibrate_kjc_kad; + optr->CalibrateAnkleJointCentre = &_ma_plugingait_calibrate_ajc_kad; + } + else + { + optr->CalibrateKneeJointCentre = &_ma_plugingait_calibrate_kjc_kad; + optr->CalibrateAnkleJointCentre = &_ma_plugingait_calibrate_ajc_kadmed; + } + }; + /** * Create segments and joints according to the region and side given to the constructor. */ @@ -915,6 +1170,7 @@ namespace body { Segment* leftThigh = new Segment("L.Thigh", Part::Thigh, Side::Left, segments); Segment* leftShank = new Segment("L.Shank", Part::Shank, Side::Left, segments); + Segment* leftShankTorsioned = new Segment("L.Shank.Torsioned", PartShankTorsioned, Side::Left, segments); Segment* leftFoot = new Segment("L.Foot", Part::Foot, Side::Left, segments); std::vector leftLowerLimbJoints(3); jnt = new Joint("L.Hip", pelvis, Anchor::point("L.HJC"), leftThigh, Anchor::point("L.HJC", pelvis), joints); @@ -925,9 +1181,11 @@ namespace body leftLowerLimbJoints[1] = jnt; new EulerDescriptor("L.Knee.Angle", EulerDescriptor::YXZ, {{1.0, -1.0, -1.0}}, jnt); new DynamicDescriptor({{0,1,2,1,0,2,0,1,2}}, {{1.,1.,1.,-1.,1.,1.,1.,1.,-1.}}, jnt); + jnt = new Joint("L.Ankle", leftShankTorsioned, leftFoot, Anchor::origin(leftShankTorsioned), joints); + new PluginGaitLeftAnkleDescriptor(jnt); + // YES THERE IS TWO JOINTS FOR THE ANKLE DUE TO THE USE OF THE "UNTORSIONED" SHANK AS PROXIMAL FRAME FOR THE ANKLE FORCES AND MOMENTS jnt = new Joint("L.Ankle", leftShank, leftFoot, Anchor::origin(leftShank), joints); leftLowerLimbJoints[2] = jnt; - new PluginGaitLeftAnkleDescriptor(jnt); new DynamicDescriptor({{0,1,2,1,2,0,0,1,2}}, {{-1.,1.,-1.,1.,-1.,1.,1.,1.,-1.}}, jnt); jnt = new Joint("L.Foot.Progress", progression, leftFoot, joints); jnt->setDescription("Left foot relative to progression frame"); @@ -938,7 +1196,8 @@ namespace body { Segment* rightThigh = new Segment("R.Thigh", Part::Thigh, Side::Right, segments); Segment* rightShank = new Segment("R.Shank", Part::Shank, Side::Right, segments); - Segment* rightFoot = new Segment("R.Foot", Part::Foot, Side::Right,segments); + Segment* rightShankTorsioned = new Segment("R.Shank.Torsioned", PartShankTorsioned, Side::Right, segments); + Segment* rightFoot = new Segment("R.Foot", Part::Foot, Side::Right, segments); std::vector rightLowerLimbJoints(3); jnt = new Joint("R.Hip", pelvis, Anchor::point("R.HJC"), rightThigh, Anchor::point("R.HJC", pelvis), joints); rightLowerLimbJoints[0] = jnt; @@ -948,9 +1207,11 @@ namespace body rightLowerLimbJoints[1] = jnt; new EulerDescriptor("R.Knee.Angle", EulerDescriptor::YXZ, jnt); new DynamicDescriptor({{0,1,2,1,0,2,0,1,2}}, {{1.,1.,1.,-1.,-1.,-1.,1.,1.,-1.}}, jnt); + jnt = new Joint("R.Ankle", rightShankTorsioned, rightFoot, Anchor::origin(rightShankTorsioned), joints); + new PluginGaitRightAnkleDescriptor(jnt); + // YES THERE IS TWO JOINTS FOR THE ANKLE DUE TO THE USE OF THE "UNTORSIONED" SHANK AS PROXIMAL FRAME FOR THE ANKLE FORCES AND MOMENTS jnt = new Joint("R.Ankle", rightShank, rightFoot, Anchor::origin(rightShank), joints); rightLowerLimbJoints[2] = jnt; - new PluginGaitRightAnkleDescriptor(jnt); new DynamicDescriptor({{0,1,2,1,2,0,0,1,2}}, {{-1.,1.,-1.,1.,1.,-1.,1.,1.,-1.}}, jnt); jnt = new Joint("R.Foot.Progress", progression, rightFoot, joints); jnt->setDescription("Right foot relative to progression frame"); @@ -1054,10 +1315,11 @@ namespace body optr->RightAsisTrochanterAPDistance = 0.1288 * rightLegLength - 48.56; // - hip joint centre Point *leftHJCH = nullptr, *rightHJCH = nullptr; + double meanLegLength = (leftLegLength + rightLegLength) / 2.0; // - Left if ((leftHJCH = this->findChild("L.HJC",{},false)) == nullptr) { - optr->computeHipJointCenter(_L_HJC.data(), -1.0, leftLegLength * 0.115 - 15.3, optr->LeftAsisTrochanterAPDistance); + optr->computeHipJointCentre(_L_HJC.data(), -1.0, meanLegLength * 0.115 - 15.3, optr->LeftAsisTrochanterAPDistance); new Point("L.HJC", _L_HJC.data(), this); } else @@ -1065,7 +1327,7 @@ namespace body // - Right if ((rightHJCH = this->findChild("R.HJC",{},false)) == nullptr) { - optr->computeHipJointCenter(_R_HJC.data(), 1.0, rightLegLength * 0.115 - 15.3, optr->RightAsisTrochanterAPDistance); + optr->computeHipJointCentre(_R_HJC.data(), 1.0, meanLegLength * 0.115 - 15.3, optr->RightAsisTrochanterAPDistance); new Point("R.HJC", _R_HJC.data(), this); } else @@ -1748,9 +2010,135 @@ namespace body this->modified(); }; - /** + /** + * Returns the internal parameter RightTibialTorsionOffset. + */ + double PluginGait::rightTibialTorsionOffset() const _OPENMA_NOEXCEPT + { + auto optr = this->pimpl(); + return optr->RightTibialTorsionOffset; + }; + + /** + * Sets the internal parameter RightTibialTorsion. + */ + void PluginGait::setRightTibialTorsionOffset(double value) _OPENMA_NOEXCEPT + { + auto optr = this->pimpl(); + if (fabs(value - optr->RightTibialTorsionOffset) < std::numeric_limits::epsilon()) + return; + optr->RightTibialTorsionOffset = value; + this->modified(); + }; + + /** + * Returns the internal parameter LeftTibialTorsionOffset. + */ + double PluginGait::leftTibialTorsionOffset() const _OPENMA_NOEXCEPT + { + auto optr = this->pimpl(); + return optr->LeftTibialTorsionOffset; + }; + + /** + * Sets the internal parameter LeftTibialTorsionOffset. + */ + void PluginGait::setLeftTibialTorsionOffset(double value) _OPENMA_NOEXCEPT + { + auto optr = this->pimpl(); + if (fabs(value - optr->LeftTibialTorsionOffset) < std::numeric_limits::epsilon()) + return; + optr->LeftTibialTorsionOffset = value; + this->modified(); + }; + + /** + * Returns the internal parameter RighThighRotation. + */ + double PluginGait::rightThighRotationOffset() const _OPENMA_NOEXCEPT + { + auto optr = this->pimpl(); + return optr->RightThighRotationOffset; + }; + + /** + * Sets the internal parameter RighThighRotation. + */ + void PluginGait::setRightThighRotationOffset(double value) _OPENMA_NOEXCEPT + { + auto optr = this->pimpl(); + if (fabs(value - optr->RightThighRotationOffset) < std::numeric_limits::epsilon()) + return; + optr->RightThighRotationOffset = value; + this->modified(); + }; + + /** + * Returns the internal parameter LeftThighRotationOffset. + */ + double PluginGait::leftThighRotationOffset() const _OPENMA_NOEXCEPT + { + auto optr = this->pimpl(); + return optr->LeftThighRotationOffset; + }; + + /** + * Sets the internal parameter LeftThighRotationOffset. + */ + void PluginGait::setLeftThighRotationOffset(double value) _OPENMA_NOEXCEPT + { + auto optr = this->pimpl(); + if (fabs(value - optr->LeftThighRotationOffset) < std::numeric_limits::epsilon()) + return; + optr->LeftThighRotationOffset = value; + this->modified(); + }; + + /** + * Returns the internal parameter RighShankRotationOffset. + */ + double PluginGait::rightShankRotationOffset() const _OPENMA_NOEXCEPT + { + auto optr = this->pimpl(); + return optr->RightShankRotationOffset; + }; + + /** + * Sets the internal parameter RighShankRotationOffset. + */ + void PluginGait::setRightShankRotationOffset(double value) _OPENMA_NOEXCEPT + { + auto optr = this->pimpl(); + if (fabs(value - optr->RightShankRotationOffset) < std::numeric_limits::epsilon()) + return; + optr->RightShankRotationOffset = value; + this->modified(); + }; + + /** + * Returns the internal parameter LeftShankRotationOffset. + */ + double PluginGait::leftShankRotationOffset() const _OPENMA_NOEXCEPT + { + auto optr = this->pimpl(); + return optr->LeftShankRotationOffset; + }; + + /** + * Sets the internal parameter LeftShankRotationOffset. + */ + void PluginGait::setLeftShankRotationOffset(double value) _OPENMA_NOEXCEPT + { + auto optr = this->pimpl(); + if (fabs(value - optr->LeftShankRotationOffset) < std::numeric_limits::epsilon()) + return; + optr->LeftShankRotationOffset = value; + this->modified(); + }; + + /** * Returns the internal parameter RightKneeWidth. - */ + */ double PluginGait::rightKneeWidth() const _OPENMA_NOEXCEPT { auto optr = this->pimpl(); @@ -1910,11 +2298,29 @@ namespace body return optr->LeftStaticRotationOffset; }; + /** + * Returns the internal parameter RightAnkleAbAddOffset. + */ + double PluginGait::rightAnkleAbAddOffset() const _OPENMA_NOEXCEPT + { + auto optr = this->pimpl(); + return optr->RightAnkleAbAddOffset; + }; + + /** + * Returns the internal parameter LeftAnkleAbAddOffset. + */ + double PluginGait::leftAnkleAbAddOffset() const _OPENMA_NOEXCEPT + { + auto optr = this->pimpl(); + return optr->LeftAnkleAbAddOffset; + }; + // ----------------------------------------------------------------------- // /* * Generate a landmarks translator for the PiG model. - * The following list presents the label used in the PluginGait helper and its translation in OpenMA: + * The following list presents the label used in the PluginGait helper (Basic variant) and its translation in OpenMA: * - LFHD: L.HF (Left front head: approximately over the left) * - LBHD: L.HB (Left back head: on the back of the head) * - RFHD: R.HF (Right front head: approximately over the) @@ -1950,12 +2356,25 @@ namespace body * - RTOE: R.MTH2 (Head of the second metatarsal for the right) * - LHEE: L.HEE (Left heel marker) * - RHEE: R.HEE (Right heel marker) + * + * Supplementary markers are used in the KAD variant: + * - LKAX: L.KAX (Left knee axis) + * - LKD1: L.KD1 (Left knee device 1) + * - LKD2: L.KD2 (Left knee device 2) + * - RKAX: R.KAX (Right knee axis) + * - RKD1: R.KD1 (Right knee device 1) + * - RKD2: R.KD2 (Right knee device 2) + * + * The variant KADMed uses the markers of the Basic and KAD variants as well as two additional markers: + * - RMED: R.MTM (Right medial malleolus of the tibia) + * - LMED: L.MTM (Left medial malleolus of the tibia) */ LandmarksTranslator* PluginGait::defaultLandmarksTranslator() { return new LandmarksTranslator( "LandmarksTranslator", { + // Basic markers {"LFHD", "L.HF"}, {"LBHD", "L.HB"}, {"RFHD", "R.HF"}, @@ -1991,6 +2410,16 @@ namespace body {"RTOE", "R.MTH2"}, {"LHEE", "L.HEE"}, {"RHEE", "R.HEE"}, + // KAD markers + {"LKAX", "L.KAX"}, + {"LKD1", "L.KD1"}, + {"LKD2", "L.KD2"}, + {"RKAX", "R.KAX"}, + {"RKD1", "R.KD1"}, + {"RKD2", "R.KD2"}, + // Medial markers for the knee and the shank + {"RMED", "R.MTM"}, + {"LMED", "L.MTM"}, }, this); }; @@ -2014,7 +2443,7 @@ namespace body */ PluginGait* PluginGait::clone(Node* parent) const { - auto dest = new PluginGait(0,0); + auto dest = new PluginGait(0,0,Basic); dest->copy(this); dest->addParent(parent); return dest; @@ -2031,6 +2460,7 @@ namespace body auto optr = this->pimpl(); auto optr_src = src->pimpl(); this->SkeletonHelper::copy(src); + optr->Variant = optr_src->Variant; optr->MarkerDiameter = optr_src->MarkerDiameter; optr->HeadOffsetEnabled = optr_src->HeadOffsetEnabled; optr->RightShoulderOffset = optr_src->RightShoulderOffset; @@ -2046,6 +2476,12 @@ namespace body optr->LeftLegLength = optr_src->LeftLegLength; optr->RightAsisTrochanterAPDistance = optr_src->RightAsisTrochanterAPDistance; optr->LeftAsisTrochanterAPDistance = optr_src->LeftAsisTrochanterAPDistance; + optr->RightTibialTorsionOffset = optr_src->RightTibialTorsionOffset; + optr->LeftTibialTorsionOffset = optr_src->LeftTibialTorsionOffset; + optr->RightThighRotationOffset = optr_src->RightThighRotationOffset; + optr->LeftThighRotationOffset = optr_src->LeftThighRotationOffset; + optr->RightShankRotationOffset = optr_src->RightShankRotationOffset; + optr->LeftShankRotationOffset = optr_src->LeftShankRotationOffset; optr->RightKneeWidth = optr_src->RightKneeWidth; optr->LeftKneeWidth = optr_src->LeftKneeWidth; optr->RightAnkleWidth = optr_src->RightAnkleWidth; @@ -2057,6 +2493,8 @@ namespace body optr->RightStaticRotationOffset = optr_src->RightStaticRotationOffset; optr->LeftStaticPlantarFlexionOffset = optr_src->LeftStaticPlantarFlexionOffset; optr->LeftStaticRotationOffset = optr_src->LeftStaticRotationOffset; + optr->RightAnkleAbAddOffset = optr_src->RightAnkleAbAddOffset; + optr->LeftAnkleAbAddOffset = optr_src->LeftAnkleAbAddOffset; }; }; }; \ No newline at end of file diff --git a/modules/body/swig/body/plugingait.i b/modules/body/swig/body/plugingait.i index 5110c0b..7b7ae73 100644 --- a/modules/body/swig/body/plugingait.i +++ b/modules/body/swig/body/plugingait.i @@ -44,8 +44,16 @@ namespace body { public: SWIG_EXTEND_CAST_CONSTRUCTOR(ma::body, PluginGait, SWIGTYPE) + + %extend { + enum class Variant : int { + Basic = PluginGait::Basic, + KAD = PluginGait::KAD, + KADMed = PluginGait::KADMed + }; + }; - PluginGait(int region, int side, Node* parent = nullptr); + PluginGait(int region, int side, Variant variant, Node* parent = nullptr); ~PluginGait(); double markerDiameter() const; @@ -76,6 +84,18 @@ namespace body void setRightAsisTrochanterAPDistance(double value); double leftAsisTrochanterAPDistance() const; void setLeftAsisTrochanterAPDistance(double value); + double rightTibialTorsionOffset() const; + void setRightTibialTorsionOffset(double value); + double leftTibialTorsionOffset() const; + void setLeftTibialTorsionOffset(double value); + double rightThighRotationOffset() const; + void setRightThighRotationOffset(double value); + double leftThighRotationOffset() const; + void setLeftThighRotationOffset(double value); + double rightShankRotationOffset() const; + void setRightShankRotationOffset(double value); + double leftShankRotationOffset() const; + void setLeftShankRotationOffset(double value); double rightKneeWidth() const; void setRightKneeWidth(double value); double leftKneeWidth() const; @@ -93,6 +113,8 @@ namespace body double rightStaticRotationOffset() const; double leftStaticPlantarFlexionOffset() const; double leftStaticRotationOffset() const; + double rightAnkleAbAddOffset() const; + double leftAnkleAbAddOffset() const; }; %clearnodefaultctor; }; diff --git a/modules/body/test/c++/CMakeLists.txt b/modules/body/test/c++/CMakeLists.txt index 122bf62..49121d2 100644 --- a/modules/body/test/c++/CMakeLists.txt +++ b/modules/body/test/c++/CMakeLists.txt @@ -19,4 +19,7 @@ ADD_CXXTEST_TESTDRIVER(openma_body_point pointTest.cpp body) ADD_CXXTEST_TESTDRIVER(openma_body_segment segmentTest.cpp body) ADD_CXXTEST_TESTDRIVER(openma_body_simplegaitforceplatetofeetassigner simplegaitforceplatetofeetassignerTest.cpp INCLUDES ${_TEST_IO_CPP_DIR} LIBRARIES body io) ADD_CXXTEST_TESTDRIVER(openma_body_skeletonhelper skeletonhelperTest.cpp body) -ADD_CXXTEST_TESTDRIVER(openma_body_utils utilsTest.cpp body) \ No newline at end of file +ADD_CXXTEST_TESTDRIVER(openma_body_utils utilsTest.cpp body) + +# To have access to the symbol M_PI +SET_TARGET_PROPERTIES(test_openma_body_plugingait_calibration PROPERTIES COMPILE_DEFINITIONS "_USE_MATH_DEFINES") \ No newline at end of file diff --git a/modules/body/test/c++/plugingaitTest.cpp b/modules/body/test/c++/plugingaitTest.cpp index ce694d0..eeec3bf 100644 --- a/modules/body/test/c++/plugingaitTest.cpp +++ b/modules/body/test/c++/plugingaitTest.cpp @@ -6,7 +6,7 @@ CXXTEST_SUITE(PluginGaitTest) { CXXTEST_TEST(clone) { - ma::body::PluginGait helper(ma::body::Region::Full, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Full, ma::body::Side::Both, ma::body::PluginGait::Basic); fill_parameters_with_fake_value(&helper); ma::Node root("root"); auto clonedhelper = helper.clone(&root); @@ -16,8 +16,8 @@ CXXTEST_SUITE(PluginGaitTest) CXXTEST_TEST(copy) { - ma::body::PluginGait helper(ma::body::Region::Full, ma::body::Side::Both); - ma::body::PluginGait copyhelper(ma::body::Region::Lower, ma::body::Side::Right); + ma::body::PluginGait helper(ma::body::Region::Full, ma::body::Side::Both, ma::body::PluginGait::Basic); + ma::body::PluginGait copyhelper(ma::body::Region::Lower, ma::body::Side::Right, ma::body::PluginGait::Basic); fill_parameters_with_fake_value(&helper); copyhelper.setMarkerDiameter(16.0); copyhelper.setHeadOffsetEnabled(false); diff --git a/modules/body/test/c++/plugingaitTest_calibration.cpp b/modules/body/test/c++/plugingaitTest_calibration.cpp index e0505f1..17cfa51 100644 --- a/modules/body/test/c++/plugingaitTest_calibration.cpp +++ b/modules/body/test/c++/plugingaitTest_calibration.cpp @@ -9,7 +9,7 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) { CXXTEST_TEST(calibrateLeftFullBodyOneFrame) { - ma::body::PluginGait helper(ma::body::Region::Full, ma::body::Side::Left); + ma::body::PluginGait helper(ma::body::Region::Full, ma::body::Side::Left, ma::body::PluginGait::Basic); helper.setMarkerDiameter(16.0); // mm helper.setHeadOffsetEnabled(true); helper.setLeftFootFlatEnabled(true); @@ -36,7 +36,7 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) CXXTEST_TEST(calibrateRightFullBodyOneFrame) { - ma::body::PluginGait helper(ma::body::Region::Full, ma::body::Side::Right); + ma::body::PluginGait helper(ma::body::Region::Full, ma::body::Side::Right, ma::body::PluginGait::Basic); helper.setMarkerDiameter(16.0); // mm helper.setHeadOffsetEnabled(true); helper.setRightFootFlatEnabled(true); @@ -63,7 +63,7 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) CXXTEST_TEST(calibrateBothFullBodyOneFrame) { - ma::body::PluginGait helper(ma::body::Region::Full, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Full, ma::body::Side::Both, ma::body::PluginGait::Basic); helper.setMarkerDiameter(16.0); // mm helper.setHeadOffsetEnabled(true); helper.setLeftFootFlatEnabled(true); @@ -101,7 +101,7 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) CXXTEST_TEST(calibrateBothUpperBodyFullFrames) { - ma::body::PluginGait helper(ma::body::Region::Upper, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Upper, ma::body::Side::Both, ma::body::PluginGait::Basic); helper.setHeadOffsetEnabled(true); ma::Node root("root"); @@ -121,7 +121,7 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) CXXTEST_TEST(calibrateBothLowerBodyFullFrames) { - ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::Basic); helper.setHeadOffsetEnabled(true); helper.setMarkerDiameter(16.0); // mm helper.setLeftFootFlatEnabled(true); @@ -150,7 +150,7 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) CXXTEST_TEST(calibrateBothFullBodyFullFrames) { - ma::body::PluginGait helper(ma::body::Region::Full, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Full, ma::body::Side::Both, ma::body::PluginGait::Basic); helper.setHeadOffsetEnabled(true); helper.setMarkerDiameter(16.0); // mm helper.setLeftFootFlatEnabled(true); @@ -179,7 +179,7 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) CXXTEST_TEST(calibrateBothFullBodyHoleFrames) { - ma::body::PluginGait helper(ma::body::Region::Full, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Full, ma::body::Side::Both, ma::body::PluginGait::Basic); helper.setHeadOffsetEnabled(true); helper.setMarkerDiameter(16.0); // mm helper.setLeftFootFlatEnabled(true); @@ -208,7 +208,7 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) CXXTEST_TEST(calibrate2BothFullBodyNoOption) { - ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::Basic); helper.setMarkerDiameter(14.0); // mm helper.setHeadOffsetEnabled(true); helper.setInterAsisDistance(255.0); // mm @@ -235,7 +235,7 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) CXXTEST_TEST(calibrate3BothLowerBodyFF) { - ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::Basic); helper.setMarkerDiameter(14.0); // mm helper.setHeadOffsetEnabled(true); helper.setLeftFootFlatEnabled(true); @@ -263,7 +263,7 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) CXXTEST_TEST(calibrate3BothLowerBodyFF_N18) { - ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::Basic); helper.setMarkerDiameter(14.0); // mm helper.setHeadOffsetEnabled(true); helper.setLeftFootFlatEnabled(true); @@ -289,7 +289,7 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) CXXTEST_TEST(calibrate3BothLowerBodynoFF) { - ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::Basic); helper.setMarkerDiameter(14.0); // mm helper.setLeftLegLength(920.0); // mm helper.setLeftKneeWidth(102.0); // mm @@ -314,7 +314,7 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) CXXTEST_TEST(calibrate2BothUpperBodyHeadOffsetDisabled) { - ma::body::PluginGait helper(ma::body::Region::Upper, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Upper, ma::body::Side::Both, ma::body::PluginGait::Basic); ma::Node root("root"); generate_trial_from_file(&root, OPENMA_TDD_PATH_IN("c3d/plugingait/PiG_Calibration4.c3d")); @@ -330,6 +330,133 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) TS_ASSERT_EQUALS(helper.rightStaticPlantarFlexionOffset(), 0.0); TS_ASSERT_EQUALS(helper.rightStaticRotationOffset(), 0.0); }; + + CXXTEST_TEST(calibrateFullBodyKAD) + { + ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KAD); + helper.setMarkerDiameter(14.0); // mm + helper.setLeftLegLength(860.0); // mm + helper.setLeftKneeWidth(102.0); // mm + helper.setLeftAnkleWidth(75.3); // mm + helper.setRightLegLength(865.0); // mm + helper.setRightKneeWidth(103.4); // mm + helper.setRightAnkleWidth(72.9); // mm + + ma::Node root("root"); + generate_trial_from_file(&root, OPENMA_TDD_PATH_IN("c3d/plugingait/PiGKad_Calibration.c3d")); + TS_ASSERT_EQUALS(root.children().size(),1u); + helper.calibrate(&root, nullptr); + + TS_ASSERT_DELTA(helper.interAsisDistance(), 211.162, 1e-3); + TS_ASSERT_DELTA(helper.leftAsisTrochanterAPDistance(), 62.208, 1e-3); + TS_ASSERT_DELTA(helper.leftThighRotationOffset(), 8.9585 * M_PI / 180.0, 1e-5); + TS_ASSERT_DELTA(helper.leftShankRotationOffset(), 13.8726 * M_PI / 180.0, 1e-5); + TS_ASSERT_DELTA(helper.leftStaticPlantarFlexionOffset(), 4.72487 * M_PI / 180.0, 1e-5); + TS_ASSERT_DELTA(helper.leftStaticRotationOffset(), 0.233667 * M_PI / 180.0, 1e-5); + TS_ASSERT_DELTA(helper.rightAsisTrochanterAPDistance(), 62.852, 1e-3); + TS_ASSERT_DELTA(helper.rightThighRotationOffset(), -10.0483 * M_PI / 180.0, 1e-5); + TS_ASSERT_DELTA(helper.rightShankRotationOffset(), 15.3639 * M_PI / 180.0, 1e-5); + TS_ASSERT_DELTA(helper.rightStaticPlantarFlexionOffset(), 3.88629 * M_PI / 180.0, 1e-5); + TS_ASSERT_DELTA(helper.rightStaticRotationOffset(), 2.22081 * M_PI / 180.0, 1e-5); + }; + + CXXTEST_TEST(calibrateFullBodyKADFF) + { + ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KAD); + helper.setMarkerDiameter(14.0); // mm + helper.setLeftLegLength(860.0); // mm + helper.setLeftKneeWidth(102.0); // mm + helper.setLeftAnkleWidth(75.3); // mm + helper.setLeftFootFlatEnabled(true); + helper.setRightLegLength(865.0); // mm + helper.setRightKneeWidth(103.4); // mm + helper.setRightAnkleWidth(72.9); // mm + helper.setRightFootFlatEnabled(true); + + + ma::Node root("root"); + generate_trial_from_file(&root, OPENMA_TDD_PATH_IN("c3d/plugingait/PiGKad_Calibration_FootFlat.c3d")); + TS_ASSERT_EQUALS(root.children().size(),1u); + helper.calibrate(&root, nullptr); + + TS_ASSERT_DELTA(helper.interAsisDistance(), 211.162, 1e-3); + TS_ASSERT_DELTA(helper.leftAsisTrochanterAPDistance(), 62.208, 1e-3); + TS_ASSERT_DELTA(helper.leftThighRotationOffset(), 0.156355, 1e-5); + TS_ASSERT_DELTA(helper.leftShankRotationOffset(), 0.242123, 1e-5); + TS_ASSERT_DELTA(helper.leftStaticPlantarFlexionOffset(), 0.0629524, 1e-5); + TS_ASSERT_DELTA(helper.leftStaticRotationOffset(), 0.00504156, 1e-5); + TS_ASSERT_DELTA(helper.rightAsisTrochanterAPDistance(), 62.852, 1e-5); + TS_ASSERT_DELTA(helper.rightThighRotationOffset(), -0.175377, 1e-5); + TS_ASSERT_DELTA(helper.rightShankRotationOffset(), 0.268151, 1e-5); + TS_ASSERT_DELTA(helper.rightStaticPlantarFlexionOffset(), 0.12429, 1e-5); + TS_ASSERT_DELTA(helper.rightStaticRotationOffset(), 0.0345916, 1e-5); + }; + + CXXTEST_TEST(calibrateFullBodyKADMed) + { + ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KADMed); + helper.setMarkerDiameter(14.0); // mm + helper.setLeftLegLength(860.0); // mm + helper.setLeftKneeWidth(102.0); // mm + helper.setLeftAnkleWidth(75.3); // mm + helper.setRightLegLength(865.0); // mm + helper.setRightKneeWidth(103.4); // mm + helper.setRightAnkleWidth(72.9); // mm + + ma::Node root("root"); + generate_trial_from_file(&root, OPENMA_TDD_PATH_IN("c3d/plugingait/PiGKadMed_Calibration.c3d")); + TS_ASSERT_EQUALS(root.children().size(),1u); + helper.calibrate(&root, nullptr); + + TS_ASSERT_DELTA(helper.interAsisDistance(), 211.162, 1e-3); + TS_ASSERT_DELTA(helper.leftAsisTrochanterAPDistance(), 62.208, 1e-3); + TS_ASSERT_DELTA(helper.leftThighRotationOffset(), 8.9585 * M_PI / 180.0, 1e-4); + TS_ASSERT_DELTA(helper.leftShankRotationOffset(), 3.89674 * M_PI / 180.0, 1.1e-4); + TS_ASSERT_DELTA(helper.leftTibialTorsionOffset(), -12.0031 * M_PI / 180.0, 1e-4); + TS_ASSERT_DELTA(helper.leftStaticPlantarFlexionOffset(), 9.75336 * M_PI / 180.0, 1e-5); + TS_ASSERT_DELTA(helper.leftStaticRotationOffset(), -0.406183 * M_PI / 180.0, 1e-5); + TS_ASSERT_DELTA(helper.leftAnkleAbAddOffset(), 11.5186 * M_PI / 180.0, 1e-4); + TS_ASSERT_DELTA(helper.rightAsisTrochanterAPDistance(), 62.852, 1e-3); + TS_ASSERT_DELTA(helper.rightThighRotationOffset(), -10.0483 * M_PI / 180.0, 1e-5); + TS_ASSERT_DELTA(helper.rightShankRotationOffset(), 0.5526 * M_PI / 180.0 , 1.1e-4); + TS_ASSERT_DELTA(helper.rightTibialTorsionOffset(), -17.7351 * M_PI / 180.0, 1e-4); + TS_ASSERT_DELTA(helper.rightStaticPlantarFlexionOffset(), 7.86442 * M_PI / 180.0, 1e-5); + TS_ASSERT_DELTA(helper.rightStaticRotationOffset(), 3.1846 * M_PI / 180.0, 1e-4); + TS_ASSERT_DELTA(helper.rightAnkleAbAddOffset(), 8.70843 * M_PI / 180.0, 1e-5); + }; + + CXXTEST_TEST(calibrateFullBodyKADMed2) + { + ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KADMed); + helper.setMarkerDiameter(25.0); // mm + helper.setLeftLegLength(775.0); // mm + helper.setLeftKneeWidth(105.1); // mm + helper.setLeftAnkleWidth(68.4); // mm + helper.setRightLegLength(770.0); // mm + helper.setRightKneeWidth(107.0); // mm + helper.setRightAnkleWidth(68.6); // mm + + ma::Node root("root"); + generate_trial_from_file(&root, OPENMA_TDD_PATH_IN("c3d/plugingait/PiGKadMed_Calibration2.c3d")); + TS_ASSERT_EQUALS(root.children().size(),1u); + helper.calibrate(&root, nullptr); + + TS_ASSERT_DELTA(helper.interAsisDistance(), 213.74600219726562, 1e-3) + TS_ASSERT_DELTA(helper.leftAsisTrochanterAPDistance(), 51.259998321533203, 1e-3) + TS_ASSERT_DELTA(helper.leftThighRotationOffset(), -0.19650661945343018, 1e-4) + TS_ASSERT_DELTA(helper.leftShankRotationOffset(), 0.036574419587850571, 1e-4) + TS_ASSERT_DELTA(helper.leftTibialTorsionOffset(), 0.18572920560836792, 1e-4) + TS_ASSERT_DELTA(helper.leftStaticPlantarFlexionOffset(), 0.082310251891613007, 1e-4) + TS_ASSERT_DELTA(helper.leftStaticRotationOffset(), 0.08043140172958374, 1e-4) + TS_ASSERT_DELTA(helper.leftAnkleAbAddOffset(), 0.13921844959259033, 1e-4) + TS_ASSERT_DELTA(helper.rightAsisTrochanterAPDistance(), 50.616001129150391, 1e-3) + TS_ASSERT_DELTA(helper.rightThighRotationOffset(), 0.230163544416427610, 1e-4) + TS_ASSERT_DELTA(helper.rightShankRotationOffset(), -0.11728926748037338, 1e-4) + TS_ASSERT_DELTA(helper.rightTibialTorsionOffset(), -0.34318757057189941, 1e-4) + TS_ASSERT_DELTA(helper.rightStaticPlantarFlexionOffset(), 0.032737139612436295, 1e-4) + TS_ASSERT_DELTA(helper.rightStaticRotationOffset(), 0.28456372022628784, 1e-4) + TS_ASSERT_DELTA(helper.rightAnkleAbAddOffset(), 0.13163727521896362, 1e-4) + }; }; CXXTEST_SUITE_REGISTRATION(PluginGaitCalibrationTest) @@ -344,4 +471,8 @@ CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrate2BothFullBodyNoOpt CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrate3BothLowerBodyFF) CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrate3BothLowerBodyFF_N18) CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrate3BothLowerBodynoFF) -CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrate2BothUpperBodyHeadOffsetDisabled) \ No newline at end of file +CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrate2BothUpperBodyHeadOffsetDisabled) +CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyKAD) +CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyKADFF) +CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyKADMed) +CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyKADMed2) \ No newline at end of file diff --git a/modules/body/test/c++/plugingaitTest_kinematics.cpp b/modules/body/test/c++/plugingaitTest_kinematics.cpp index fba4f0f..7913912 100644 --- a/modules/body/test/c++/plugingaitTest_kinematics.cpp +++ b/modules/body/test/c++/plugingaitTest_kinematics.cpp @@ -33,7 +33,7 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) TS_ASSERT_EQUALS(ma::io::read(&dynamictrials, OPENMA_TDD_PATH_IN("c3d/plugingait/PiG_Motion-FlatFoot-One.c3d")), true); TS_ASSERT_EQUALS(dynamictrials.children().size(), 1u); // Skeleton helper creation - ma::body::PluginGait skeletonhelper(ma::body::Region::Lower, ma::body::Side::Both); + ma::body::PluginGait skeletonhelper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::Basic); // Skeleton helper calibration TS_ASSERT_EQUALS(ma::body::calibrate(&skeletonhelper, &statictrials, &subject), true); // Model reconstruction @@ -78,7 +78,7 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) TS_ASSERT_EQUALS(ma::io::read(&dynamictrials, OPENMA_TDD_PATH_IN("c3d/plugingait/PiG_Motion-FlatFoot-One.c3d")), true); TS_ASSERT_EQUALS(dynamictrials.children().size(), 1u); // Skeleton helper creation - ma::body::PluginGait skeletonhelper(ma::body::Region::Upper, ma::body::Side::Both); + ma::body::PluginGait skeletonhelper(ma::body::Region::Upper, ma::body::Side::Both, ma::body::PluginGait::Basic); // Skeleton helper calibration TS_ASSERT_EQUALS(ma::body::calibrate(&skeletonhelper, &statictrials, &subject), true); // Model reconstruction @@ -125,7 +125,7 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) TS_ASSERT_EQUALS(ma::io::read(&dynamictrials, OPENMA_TDD_PATH_IN("c3d/plugingait/PiG_Motion-FlatFoot-Hole.c3d")), true); TS_ASSERT_EQUALS(dynamictrials.children().size(), 1u); // Skeleton helper creation - ma::body::PluginGait skeletonhelper(ma::body::Region::Lower, ma::body::Side::Both); + ma::body::PluginGait skeletonhelper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::Basic); // Skeleton helper calibration TS_ASSERT_EQUALS(ma::body::calibrate(&skeletonhelper, &statictrials, &subject), true); // Model reconstruction @@ -170,7 +170,7 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) TS_ASSERT_EQUALS(ma::io::read(&dynamictrials, OPENMA_TDD_PATH_IN("c3d/plugingait/PiG_Motion-FlatFoot-Hole.c3d")), true); TS_ASSERT_EQUALS(dynamictrials.children().size(), 1u); // Skeleton helper creation - ma::body::PluginGait skeletonhelper(ma::body::Region::Upper, ma::body::Side::Both); + ma::body::PluginGait skeletonhelper(ma::body::Region::Upper, ma::body::Side::Both, ma::body::PluginGait::Basic); // Skeleton helper calibration TS_ASSERT_EQUALS(ma::body::calibrate(&skeletonhelper, &statictrials, &subject), true); // Model reconstruction @@ -227,7 +227,7 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) TS_ASSERT_EQUALS(ma::io::read(&dynamictrials, OPENMA_TDD_PATH_IN("c3d/plugingait/PiG_Motion-FlatFoot-Full.c3d")), true); TS_ASSERT_EQUALS(dynamictrials.children().size(), 1u); // Skeleton helper creation - ma::body::PluginGait skeletonhelper(ma::body::Region::Full, ma::body::Side::Both); + ma::body::PluginGait skeletonhelper(ma::body::Region::Full, ma::body::Side::Both, ma::body::PluginGait::Basic); // Skeleton helper calibration TS_ASSERT_EQUALS(ma::body::calibrate(&skeletonhelper, &statictrials, &subject), true); // Model reconstruction @@ -286,7 +286,7 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) TS_ASSERT_EQUALS(ma::io::read(&dynamictrials, OPENMA_TDD_PATH_IN("c3d/plugingait/PiG_Motion3_FF.c3d")), true); TS_ASSERT_EQUALS(dynamictrials.children().size(), 1u); // Skeleton helper creation - ma::body::PluginGait skeletonhelper(ma::body::Region::Lower, ma::body::Side::Both); + ma::body::PluginGait skeletonhelper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::Basic); // Skeleton helper calibration TS_ASSERT_EQUALS(ma::body::calibrate(&skeletonhelper, &statictrials, &subject), true); // Model reconstruction @@ -330,7 +330,7 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) TS_ASSERT_EQUALS(ma::io::read(&dynamictrials, OPENMA_TDD_PATH_IN("c3d/plugingait/PiG_Motion3_FF_N18.c3d")), true); TS_ASSERT_EQUALS(dynamictrials.children().size(), 1u); // Skeleton helper creation - ma::body::PluginGait skeletonhelper(ma::body::Region::Lower, ma::body::Side::Both); + ma::body::PluginGait skeletonhelper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::Basic); // Skeleton helper calibration TS_ASSERT_EQUALS(ma::body::calibrate(&skeletonhelper, &statictrials, &subject), true); // Model reconstruction @@ -372,7 +372,7 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) TS_ASSERT_EQUALS(ma::io::read(&dynamictrials, OPENMA_TDD_PATH_IN("c3d/plugingait/PiG_Motion3_noFF.c3d")), true); TS_ASSERT_EQUALS(dynamictrials.children().size(), 1u); // Skeleton helper creation - ma::body::PluginGait skeletonhelper(ma::body::Region::Lower, ma::body::Side::Both); + ma::body::PluginGait skeletonhelper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::Basic); // Skeleton helper calibration TS_ASSERT_EQUALS(ma::body::calibrate(&skeletonhelper, &statictrials, &subject), true); // Model reconstruction @@ -423,7 +423,7 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) TS_ASSERT_EQUALS(ma::io::read(&dynamictrials, OPENMA_TDD_PATH_IN("c3d/plugingait/PiG_Motion4_noFF_noHO.c3d")), true); TS_ASSERT_EQUALS(dynamictrials.children().size(), 1u); // Skeleton helper creation - ma::body::PluginGait skeletonhelper(ma::body::Region::Full, ma::body::Side::Both); + ma::body::PluginGait skeletonhelper(ma::body::Region::Full, ma::body::Side::Both, ma::body::PluginGait::Basic); // Skeleton helper calibration TS_ASSERT_EQUALS(ma::body::calibrate(&skeletonhelper, &statictrials, &subject), true); // Model reconstruction @@ -461,6 +461,190 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) compare_joint_kinematics(kinematics, trial, "R.Head.Progress.Angle", "RViconHeadAngles"); compare_joint_kinematics(kinematics, trial, "L.Head.Progress.Angle", "LViconHeadAngles"); }; + + CXXTEST_TEST(kinematicsFullBodyKAD) + { + ma::Subject subject("Anonymous", + {{"markerDiameter", 14.0}, + {"leftLegLength", 860.0}, + {"leftKneeWidth", 102.0}, + {"leftAnkleWidth", 75.3}, + {"rightLegLength", 865.0}, + {"rightKneeWidth", 103.4}, + {"rightAnkleWidth", 72.9}}); + + ma::Node statictrials("statictrials"), dynamictrials("dynamictrials"), models("models"), kinematicsanalyses("kinematicsanalyses"); + // Static trial + TS_ASSERT_EQUALS(ma::io::read(&statictrials, OPENMA_TDD_PATH_IN("c3d/plugingait/PiGKad_Calibration.c3d")), true); + TS_ASSERT_EQUALS(statictrials.children().size(), 1u); + // Dynamic trial + TS_ASSERT_EQUALS(ma::io::read(&dynamictrials, OPENMA_TDD_PATH_IN("c3d/plugingait/PiGKad_Motion.c3d")), true); + TS_ASSERT_EQUALS(dynamictrials.children().size(), 1u); + // Skeleton helper creation + ma::body::PluginGait skeletonhelper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KAD); + // Skeleton helper calibration + TS_ASSERT_EQUALS(ma::body::calibrate(&skeletonhelper, &statictrials, &subject), true); + // Model reconstruction + TS_ASSERT_EQUALS(ma::body::reconstruct(&models, &skeletonhelper, &dynamictrials), true); + TS_ASSERT_EQUALS(models.children().size(), 1u); + // Joint kinematics + TS_ASSERT_EQUALS(ma::body::extract_joint_kinematics(&kinematicsanalyses, &models, true), true); + TS_ASSERT_EQUALS(kinematicsanalyses.children().size(), 1u); + + auto trial = dynamictrials.child(0); + auto kinematics = kinematicsanalyses.child(0); + + compare_joint_kinematics(kinematics, trial, "L.Hip.Angle", "LHipAngles", {{1.5e-4,1e-4,1.1e-3}}); + compare_joint_kinematics(kinematics, trial, "L.Knee.Angle", "LKneeAngles", {{3.5e-4,8e-4,1.2e-3}}); + compare_joint_kinematics(kinematics, trial, "L.Ankle.Angle", "LAnkleAngles",{{1.9e-4,1e-4,1.2e-3}}); + compare_joint_kinematics(kinematics, trial, "R.Hip.Angle", "RHipAngles", {{1.2e-4,1e-4,8e-4}}); + compare_joint_kinematics(kinematics, trial, "R.Knee.Angle", "RKneeAngles", {{3e-4,5.5e-4,1.1e-3}}); + compare_joint_kinematics(kinematics, trial, "R.Ankle.Angle", "RAnkleAngles",{{1.6e-4,1e-4,8e-4}}); + compare_joint_kinematics(kinematics, trial, "R.Pelvis.Progress.Angle", "RPelvisAngles"); + compare_joint_kinematics(kinematics, trial, "L.Pelvis.Progress.Angle", "LPelvisAngles"); + compare_joint_kinematics(kinematics, trial, "R.Foot.Progress.Angle", "RFootProgressAngles",{{1e-4,3.1e-4,1.1e-4}}); + compare_joint_kinematics(kinematics, trial, "L.Foot.Progress.Angle", "LFootProgressAngles",{{1.2e-4,3.5e-4}}); + }; + + CXXTEST_TEST(kinematicsFullBodyKADFF) + { + ma::Subject subject("Anonymous", + {{"markerDiameter", 14.0}, + {"leftLegLength", 860.0}, + {"leftKneeWidth", 102.0}, + {"leftAnkleWidth", 75.3}, + {"leftFootFlatEnabled", true}, + {"rightLegLength", 865.0}, + {"rightKneeWidth", 103.4}, + {"rightAnkleWidth", 72.9}, + {"rightFootFlatEnabled", true}}); + + ma::Node statictrials("statictrials"), dynamictrials("dynamictrials"), models("models"), kinematicsanalyses("kinematicsanalyses"); + // Static trial + TS_ASSERT_EQUALS(ma::io::read(&statictrials, OPENMA_TDD_PATH_IN("c3d/plugingait/PiGKad_Calibration_FootFlat.c3d")), true); + TS_ASSERT_EQUALS(statictrials.children().size(), 1u); + // Dynamic trial + TS_ASSERT_EQUALS(ma::io::read(&dynamictrials, OPENMA_TDD_PATH_IN("c3d/plugingait/PiGKad_Motion_FootFlat.c3d")), true); + TS_ASSERT_EQUALS(dynamictrials.children().size(), 1u); + // Skeleton helper creation + ma::body::PluginGait skeletonhelper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KAD); + // Skeleton helper calibration + TS_ASSERT_EQUALS(ma::body::calibrate(&skeletonhelper, &statictrials, &subject), true); + // Model reconstruction + TS_ASSERT_EQUALS(ma::body::reconstruct(&models, &skeletonhelper, &dynamictrials), true); + TS_ASSERT_EQUALS(models.children().size(), 1u); + // Joint kinematics + TS_ASSERT_EQUALS(ma::body::extract_joint_kinematics(&kinematicsanalyses, &models, true), true); + TS_ASSERT_EQUALS(kinematicsanalyses.children().size(), 1u); + + auto trial = dynamictrials.child(0); + auto kinematics = kinematicsanalyses.child(0); + + compare_joint_kinematics(kinematics, trial, "L.Hip.Angle", "LHipAngles", {{1.6e-4,1e-4,1.1e-3}}); + compare_joint_kinematics(kinematics, trial, "L.Knee.Angle", "LKneeAngles", {{3.5e-4,8.8e-4,1.2e-3}}); + compare_joint_kinematics(kinematics, trial, "L.Ankle.Angle", "LAnkleAngles", {{2.1e-4,1e-4,1.1e-3}}); + compare_joint_kinematics(kinematics, trial, "R.Hip.Angle", "RHipAngles", {{1.2e-4,1e-4,8e-4}}); + compare_joint_kinematics(kinematics, trial, "R.Knee.Angle", "RKneeAngles", {{3e-4,6e-4,1.11e-3}}); + compare_joint_kinematics(kinematics, trial, "R.Ankle.Angle", "RAnkleAngles",{{1.6e-4,1e-4,7.7e-4}}); + compare_joint_kinematics(kinematics, trial, "R.Pelvis.Progress.Angle", "RPelvisAngles"); + compare_joint_kinematics(kinematics, trial, "L.Pelvis.Progress.Angle", "LPelvisAngles"); + compare_joint_kinematics(kinematics, trial, "R.Foot.Progress.Angle", "RFootProgressAngles",{{1.1e-4,2.5e-4,1.3e-4}}); + compare_joint_kinematics(kinematics, trial, "L.Foot.Progress.Angle", "LFootProgressAngles",{{1.2e-4,3.4e-4}}); + }; + + CXXTEST_TEST(kinematicsFullBodyKADMed) + { + ma::Subject subject("Anonymous", + {{"markerDiameter", 14.0}, + {"leftLegLength", 860.0}, + {"leftKneeWidth", 102.0}, + {"leftAnkleWidth", 75.3}, + {"rightLegLength", 865.0}, + {"rightKneeWidth", 103.4}, + {"rightAnkleWidth", 72.9}}); + + ma::Node statictrials("statictrials"), dynamictrials("dynamictrials"), models("models"), kinematicsanalyses("kinematicsanalyses"); + // Static trial + TS_ASSERT_EQUALS(ma::io::read(&statictrials, OPENMA_TDD_PATH_IN("c3d/plugingait/PiGKadMed_Calibration.c3d")), true); + TS_ASSERT_EQUALS(statictrials.children().size(), 1u); + // Dynamic trial + TS_ASSERT_EQUALS(ma::io::read(&dynamictrials, OPENMA_TDD_PATH_IN("c3d/plugingait/PiGKadMed_Motion.c3d")), true); + TS_ASSERT_EQUALS(dynamictrials.children().size(), 1u); + // Skeleton helper creation + ma::body::PluginGait skeletonhelper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KADMed); + skeletonhelper.setProperty("_ma_debug_vicon_original_foot_frame", true); + // Skeleton helper calibration + TS_ASSERT_EQUALS(ma::body::calibrate(&skeletonhelper, &statictrials, &subject), true); + // Model reconstruction + TS_ASSERT_EQUALS(ma::body::reconstruct(&models, &skeletonhelper, &dynamictrials), true); + TS_ASSERT_EQUALS(models.children().size(), 1u); + // Joint kinematics + TS_ASSERT_EQUALS(ma::body::extract_joint_kinematics(&kinematicsanalyses, &models, true), true); + TS_ASSERT_EQUALS(kinematicsanalyses.children().size(), 1u); + + auto trial = dynamictrials.child(0); + auto kinematics = kinematicsanalyses.child(0); + + compare_joint_kinematics(kinematics, trial, "L.Hip.Angle", "LHipAngles", {{1.6e-4,1e-4,1.1e-3}}); + compare_joint_kinematics(kinematics, trial, "L.Knee.Angle", "LKneeAngles", {{5e-4,8.8e-4,1.3e-3}}); + compare_joint_kinematics(kinematics, trial, "R.Hip.Angle", "RHipAngles", {{1.2e-4,1e-4,8e-4}}); + compare_joint_kinematics(kinematics, trial, "R.Knee.Angle", "RKneeAngles", {{4e-4,6e-4,1.11e-3}}); + compare_joint_kinematics(kinematics, trial, "R.Pelvis.Progress.Angle", "RPelvisAngles"); + compare_joint_kinematics(kinematics, trial, "L.Pelvis.Progress.Angle", "LPelvisAngles"); +#if !defined(NDEBUG) + compare_joint_kinematics(kinematics, trial, "L.Ankle.Angle", "LAnkleAngles", {{5e-4,5e-4,3e-3}}); + compare_joint_kinematics(kinematics, trial, "R.Ankle.Angle", "RAnkleAngles", {{3e-4,1.25e-4,9e-4}}); + compare_joint_kinematics(kinematics, trial, "R.Foot.Progress.Angle", "RFootProgressAngles",{{1.1e-3,3e-4}}); + compare_joint_kinematics(kinematics, trial, "L.Foot.Progress.Angle", "LFootProgressAngles",{{2.5e-3,5e-4,2e-4}}); +#endif + }; + + CXXTEST_TEST(kinematicsFullBodyKADMed2) + { + ma::Subject subject("Anonymous", + {{"markerDiameter", 25.0}, + {"leftLegLength", 775.0}, + {"leftKneeWidth", 105.1}, + {"leftAnkleWidth", 68.4}, + {"rightLegLength", 770.0}, + {"rightKneeWidth", 107.0}, + {"rightAnkleWidth", 68.6}}); + + ma::Node statictrials("statictrials"), dynamictrials("dynamictrials"), models("models"), kinematicsanalyses("kinematicsanalyses"); + // Static trial + TS_ASSERT_EQUALS(ma::io::read(&statictrials, OPENMA_TDD_PATH_IN("c3d/plugingait/PiGKadMed_Calibration2.c3d")), true); + TS_ASSERT_EQUALS(statictrials.children().size(), 1u); + // Dynamic trial + TS_ASSERT_EQUALS(ma::io::read(&dynamictrials, OPENMA_TDD_PATH_IN("c3d/plugingait/PiGKadMed_Motion2.c3d")), true); + TS_ASSERT_EQUALS(dynamictrials.children().size(), 1u); + // Skeleton helper creation + ma::body::PluginGait skeletonhelper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KADMed); + skeletonhelper.setProperty("_ma_debug_vicon_original_foot_frame", true); + // Skeleton helper calibration + TS_ASSERT_EQUALS(ma::body::calibrate(&skeletonhelper, &statictrials, &subject), true); + // Model reconstruction + TS_ASSERT_EQUALS(ma::body::reconstruct(&models, &skeletonhelper, &dynamictrials), true); + TS_ASSERT_EQUALS(models.children().size(), 1u); + // Joint kinematics + TS_ASSERT_EQUALS(ma::body::extract_joint_kinematics(&kinematicsanalyses, &models, true), true); + TS_ASSERT_EQUALS(kinematicsanalyses.children().size(), 1u); + + auto trial = dynamictrials.child(0); + auto kinematics = kinematicsanalyses.child(0); + + compare_joint_kinematics(kinematics, trial, "L.Hip.Angle", "LHipAngles", {{3e-4,1e-4,1.6e-3}}); + compare_joint_kinematics(kinematics, trial, "L.Knee.Angle", "LKneeAngles", {{8e-4,1.5e-3,1.3e-3}}); + compare_joint_kinematics(kinematics, trial, "R.Hip.Angle", "RHipAngles", {{2.1e-4,1e-4,1.1e-3}}); + compare_joint_kinematics(kinematics, trial, "R.Knee.Angle", "RKneeAngles", {{4.1e-4,8.1e-4,1.3e-3}}); + compare_joint_kinematics(kinematics, trial, "R.Pelvis.Progress.Angle", "RPelvisAngles"); + compare_joint_kinematics(kinematics, trial, "L.Pelvis.Progress.Angle", "LPelvisAngles"); +#if !defined(NDEBUG) + compare_joint_kinematics(kinematics, trial, "L.Ankle.Angle", "LAnkleAngles",{{6e-4,6e-4,2e-3}}); + compare_joint_kinematics(kinematics, trial, "R.Ankle.Angle", "RAnkleAngles",{{5e-4,3e-4,1.7e-3}}); + compare_joint_kinematics(kinematics, trial, "R.Foot.Progress.Angle", "RFootProgressAngles",{{3e-4,3e-4,3e-4}}); + compare_joint_kinematics(kinematics, trial, "L.Foot.Progress.Angle", "LFootProgressAngles",{{3e-4,4e-4,3e-4}}); +#endif + }; }; CXXTEST_SUITE_REGISTRATION(PluginGaitKinematicsTest) @@ -472,4 +656,8 @@ CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothFullBodyFullFr CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematics3BothLowerBodyFF) CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematics3BothLowerBodyFF_N18) CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematics3BothLowerBodyNoFF) -CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothFullBodyFullFramesHeadOffsetDisabled) \ No newline at end of file +CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothFullBodyFullFramesHeadOffsetDisabled) +CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsFullBodyKAD) +CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsFullBodyKADFF) +CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsFullBodyKADMed) +CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsFullBodyKADMed2) diff --git a/modules/body/test/c++/plugingaitTest_kinetics.cpp b/modules/body/test/c++/plugingaitTest_kinetics.cpp index 5c3de12..ef70a39 100644 --- a/modules/body/test/c++/plugingaitTest_kinetics.cpp +++ b/modules/body/test/c++/plugingaitTest_kinetics.cpp @@ -23,7 +23,7 @@ CXXTEST_SUITE(PluginGaitKineticsTest) { CXXTEST_TEST(inverseDynamicsBothLowerBodyOneFrame) { - ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::Basic); helper.setGravity(std::array{{0,0,-9.81}}); // m/s^2 helper.setMarkerDiameter(16.0); // mm helper.setLeftFootFlatEnabled(true); @@ -166,7 +166,7 @@ CXXTEST_SUITE(PluginGaitKineticsTest) CXXTEST_TEST(inverseDynamicsBothLowerBodyFullFramesHeadOffsetDisabled) { - ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::Basic); helper.setGravity(std::array{{0,0,-9.81}}); // m/s^2 helper.setMarkerDiameter(14.0); // mm helper.setLeftLegLength(780.0); // mm diff --git a/modules/body/test/c++/plugingaitTest_reconstruction.cpp b/modules/body/test/c++/plugingaitTest_reconstruction.cpp index 1d549c6..9ceb195 100644 --- a/modules/body/test/c++/plugingaitTest_reconstruction.cpp +++ b/modules/body/test/c++/plugingaitTest_reconstruction.cpp @@ -9,7 +9,7 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) { CXXTEST_TEST(reconstructBothLowerBodyOneFrame) { - ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::Basic); helper.setMarkerDiameter(16.0); // mm helper.setLeftFootFlatEnabled(true); helper.setLeftLegLength(940.0); // mm @@ -39,7 +39,7 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) CXXTEST_TEST(reconstructBothUpperBodyOneFrame) { - ma::body::PluginGait helper(ma::body::Region::Upper, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Upper, ma::body::Side::Both, ma::body::PluginGait::Basic); helper.setMarkerDiameter(16.0); // mm helper.setHeadOffsetEnabled(true); helper.setLeftShoulderOffset(50.0); // mm @@ -73,7 +73,7 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) CXXTEST_TEST(reconstructBothLowerBodyHoleFrames) { - ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::Basic); helper.setMarkerDiameter(16.0); // mm helper.setLeftFootFlatEnabled(true); helper.setLeftLegLength(940.0); // mm @@ -103,7 +103,7 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) CXXTEST_TEST(reconstructBothUpperBodyHoleFrames) { - ma::body::PluginGait helper(ma::body::Region::Upper, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Upper, ma::body::Side::Both, ma::body::PluginGait::Basic); helper.setMarkerDiameter(16.0); // mm helper.setHeadOffsetEnabled(true); helper.setLeftShoulderOffset(50.0); // mm @@ -138,7 +138,7 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) CXXTEST_TEST(reconstructBothFullBodyFullFrames) { - ma::body::PluginGait helper(ma::body::Region::Full, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Full, ma::body::Side::Both, ma::body::PluginGait::Basic); helper.setMarkerDiameter(16.0); // mm helper.setHeadOffsetEnabled(true); helper.setLeftShoulderOffset(50.0); // mm @@ -188,7 +188,7 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) CXXTEST_TEST(reconstruct3BothLowerBodyFF) { - ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::Basic); helper.setMarkerDiameter(14.0); // mm helper.setLeftFootFlatEnabled(true); helper.setLeftLegLength(920.0); // mm @@ -218,7 +218,7 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) CXXTEST_TEST(reconstruct3BothLowerBodyFF_N18) { - ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::Basic); helper.setMarkerDiameter(14.0); // mm helper.setLeftFootFlatEnabled(true); helper.setLeftLegLength(920.0); // mm @@ -248,7 +248,7 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) CXXTEST_TEST(reconstruct3BothLowerBodyNoFF) { - ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::Basic); helper.setMarkerDiameter(14.0); // mm helper.setLeftLegLength(920.0); // mm helper.setLeftKneeWidth(102.0); // mm @@ -276,7 +276,7 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) CXXTEST_TEST(reconstruct2BothUpperBodyHeadOffsetDisabled) { - ma::body::PluginGait helper(ma::body::Region::Full, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Full, ma::body::Side::Both, ma::body::PluginGait::Basic); helper.setMarkerDiameter(14.0); // mm helper.setLeftShoulderOffset(50.0); // mm helper.setRightShoulderOffset(50.0); // mm @@ -340,6 +340,131 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) TS_ASSERT_DELTA(progression->data(i,12), 0.0, 1e-15); } }; + + CXXTEST_TEST(reconstructFullBodyKAD) + { + ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KAD); + helper.setMarkerDiameter(14.0); // mm + helper.setLeftLegLength(860.0); // mm + helper.setLeftKneeWidth(102.0); // mm + helper.setLeftAnkleWidth(75.3); // mm + helper.setRightLegLength(865.0); // mm + helper.setRightKneeWidth(103.4); // mm + helper.setRightAnkleWidth(72.9); // mm + + ma::Node rootCalibration("rootCalibration"), rootDynamic("rootDynamic"), rootModel("rootModel"); + generate_trial_from_file(&rootCalibration, OPENMA_TDD_PATH_IN("c3d/plugingait/PiGKad_Calibration.c3d")); + TS_ASSERT_EQUALS(rootCalibration.children().size(),1u); + TS_ASSERT(helper.calibrate(&rootCalibration, nullptr)); + generate_trial_from_file(&rootDynamic, OPENMA_TDD_PATH_IN("c3d/plugingait/PiGKad_Motion.c3d")); + TS_ASSERT(helper.reconstruct(&rootModel, &rootDynamic)); + + auto trial = rootDynamic.findChild(); + auto model = rootModel.findChild(); + compare_segment_motion(model, trial, "Pelvis.SCS", {"PELO","PELA","PELL","PELP"}, {5e-4}); + compare_segment_motion(model, trial, "R.Thigh.SCS", {"RFEO","RFEA","RFEL","RFEP"}, {8.1e-4,2e-5,2e-5}); + compare_segment_motion(model, trial, "L.Thigh.SCS", {"LFEO","LFEA","LFEL","LFEP"}, {1.1e-3,2e-5,2e-5}); + compare_segment_motion(model, trial, "R.Shank.SCS", {"RTIO","RTIA","RTIL","RTIP"}, {7e-4,2e-5,2e-5}); + compare_segment_motion(model, trial, "L.Shank.SCS", {"LTIO","LTIA","LTIL","LTIP"}, {8.5e-4,2e-5,2e-5}); + compare_segment_motion(model, trial, "R.Foot.SCS", {"RFOO","RFOA","RFOL","RFOP"}, {1e4}); + compare_segment_motion(model, trial, "L.Foot.SCS", {"LFOO","LFOA","LFOL","LFOP"}, {1e4}); + }; + + CXXTEST_TEST(reconstructFullBodyKADFF) + { + ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KAD); + helper.setMarkerDiameter(14.0); // mm + helper.setLeftLegLength(860.0); // mm + helper.setLeftKneeWidth(102.0); // mm + helper.setLeftAnkleWidth(75.3); // mm + helper.setLeftFootFlatEnabled(true); + helper.setRightLegLength(865.0); // mm + helper.setRightKneeWidth(103.4); // mm + helper.setRightAnkleWidth(72.9); // mm + helper.setRightFootFlatEnabled(true); + + ma::Node rootCalibration("rootCalibration"), rootDynamic("rootDynamic"), rootModel("rootModel"); + generate_trial_from_file(&rootCalibration, OPENMA_TDD_PATH_IN("c3d/plugingait/PiGKad_Calibration_FootFlat.c3d")); + TS_ASSERT_EQUALS(rootCalibration.children().size(),1u); + TS_ASSERT(helper.calibrate(&rootCalibration, nullptr)); + generate_trial_from_file(&rootDynamic, OPENMA_TDD_PATH_IN("c3d/plugingait/PiGKad_Motion_FootFlat.c3d")); + TS_ASSERT(helper.reconstruct(&rootModel, &rootDynamic)); + + auto trial = rootDynamic.findChild(); + auto model = rootModel.findChild(); + compare_segment_motion(model, trial, "Pelvis.SCS", {"PELO","PELA","PELL","PELP"}, {5e-4}); + compare_segment_motion(model, trial, "R.Thigh.SCS", {"RFEO","RFEA","RFEL","RFEP"}, {8.1e-4,2e-5,2e-5}); + compare_segment_motion(model, trial, "L.Thigh.SCS", {"LFEO","LFEA","LFEL","LFEP"}, {1.1e-3,2e-5,2e-5}); + compare_segment_motion(model, trial, "R.Shank.SCS", {"RTIO","RTIA","RTIL","RTIP"}, {7e-4,2e-5,2e-5}); + compare_segment_motion(model, trial, "L.Shank.SCS", {"LTIO","LTIA","LTIL","LTIP"}, {8.5e-4,2e-5,2e-5}); + compare_segment_motion(model, trial, "R.Foot.SCS", {"RFOO","RFOA","RFOL","RFOP"}, {1e4}); + compare_segment_motion(model, trial, "L.Foot.SCS", {"LFOO","LFOA","LFOL","LFOP"}, {1e4}); + }; + + CXXTEST_TEST(reconstructFullBodyKADMed) + { + ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KADMed); + helper.setMarkerDiameter(14.0); // mm + helper.setLeftLegLength(860.0); // mm + helper.setLeftKneeWidth(102.0); // mm + helper.setLeftAnkleWidth(75.3); // mm + helper.setRightLegLength(865.0); // mm + helper.setRightKneeWidth(103.4); // mm + helper.setRightAnkleWidth(72.9); // mm + helper.setProperty("_ma_debug_vicon_original_foot_frame", true); + + ma::Node rootCalibration("rootCalibration"), rootDynamic("rootDynamic"), rootModel("rootModel"); + generate_trial_from_file(&rootCalibration, OPENMA_TDD_PATH_IN("c3d/plugingait/PiGKadMed_Calibration.c3d")); + TS_ASSERT_EQUALS(rootCalibration.children().size(),1u); + TS_ASSERT(helper.calibrate(&rootCalibration, nullptr)); + generate_trial_from_file(&rootDynamic, OPENMA_TDD_PATH_IN("c3d/plugingait/PiGKadMed_Motion.c3d")); + TS_ASSERT(helper.reconstruct(&rootModel, &rootDynamic)); + + auto trial = rootDynamic.findChild(); + auto model = rootModel.findChild(); + compare_segment_motion(model, trial, "Pelvis.SCS", {"PELO","PELA","PELL","PELP"}, {5e-4}); + compare_segment_motion(model, trial, "R.Thigh.SCS", {"RFEO","RFEA","RFEL","RFEP"}, {1.1e-3,2e-5,2e-5}); + compare_segment_motion(model, trial, "L.Thigh.SCS", {"LFEO","LFEA","LFEL","LFEP"}, {1.1e-3,2e-5,2e-5}); + compare_segment_motion(model, trial, "R.Shank.SCS", {"RTIO","RTIA","RTIL","RTIP"}, {6e-4,2e-5,2e-5}); + compare_segment_motion(model, trial, "L.Shank.SCS", {"LTIO","LTIA","LTIL","LTIP"}, {1.1e-3,2e-5,2e-5}); +#if !defined(NDEBUG) + // NOTE: The feet is comparable only in debug mode with the option "_ma_debug_vicon_original_foot_frame" activated. Otherwise, it cannot be compared as it seems there is an error in the definition of their SCS in the original PluginGait KADMed model. + compare_segment_motion(model, trial, "R.Foot.SCS", {"RFOO","RFOA","RFOL","RFOP"}, {1e4}); + compare_segment_motion(model, trial, "L.Foot.SCS", {"LFOO","LFOA","LFOL","LFOP"}, {1e4}); +#endif + }; + + CXXTEST_TEST(reconstructFullBodyKADMed2) + { + ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KADMed); + helper.setMarkerDiameter(25.0); // mm + helper.setLeftLegLength(775.0); // mm + helper.setLeftKneeWidth(105.1); // mm + helper.setLeftAnkleWidth(68.4); // mm + helper.setRightLegLength(770.0); // mm + helper.setRightKneeWidth(107.0); // mm + helper.setRightAnkleWidth(68.6); // mm + helper.setProperty("_ma_debug_vicon_original_foot_frame", true); + + ma::Node rootCalibration("rootCalibration"), rootDynamic("rootDynamic"), rootModel("rootModel"); + generate_trial_from_file(&rootCalibration, OPENMA_TDD_PATH_IN("c3d/plugingait/PiGKadMed_Calibration2.c3d")); + TS_ASSERT_EQUALS(rootCalibration.children().size(),1u); + TS_ASSERT(helper.calibrate(&rootCalibration, nullptr)); + generate_trial_from_file(&rootDynamic, OPENMA_TDD_PATH_IN("c3d/plugingait/PiGKadMed_Motion2.c3d")); + TS_ASSERT(helper.reconstruct(&rootModel, &rootDynamic)); + + auto trial = rootDynamic.findChild(); + auto model = rootModel.findChild(); + compare_segment_motion(model, trial, "Pelvis.SCS", {"PELO","PELA","PELL","PELP"}, {5e-4}); + compare_segment_motion(model, trial, "R.Thigh.SCS", {"RFEO","RFEA","RFEL","RFEP"}, {1.2e-3,2e-5,2e-5}); + compare_segment_motion(model, trial, "L.Thigh.SCS", {"LFEO","LFEA","LFEL","LFEP"}, {1.8e-3,3e-5,3e-5}); + compare_segment_motion(model, trial, "R.Shank.SCS", {"RTIO","RTIA","RTIL","RTIP"}, {1.3e-3,2e-5,2e-5}); + compare_segment_motion(model, trial, "L.Shank.SCS", {"LTIO","LTIA","LTIL","LTIP"}, {1.5e-3,3e-5,3e-5}); +#if !defined(NDEBUG) + compare_segment_motion(model, trial, "R.Foot.SCS", {"RFOO","RFOA","RFOL","RFOP"}, {1e4}); + compare_segment_motion(model, trial, "L.Foot.SCS", {"LFOO","LFOA","LFOL","LFOP"}, {1e4}); +#endif + }; }; CXXTEST_SUITE_REGISTRATION(PluginGaitReconstructionTest) @@ -351,4 +476,8 @@ CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructBothFullBodyF CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstruct3BothLowerBodyFF) CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstruct3BothLowerBodyFF_N18) CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstruct3BothLowerBodyNoFF) -CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstruct2BothUpperBodyHeadOffsetDisabled) \ No newline at end of file +CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstruct2BothUpperBodyHeadOffsetDisabled) +CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyKAD) +CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyKADFF) +CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyKADMed) +CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyKADMed2) \ No newline at end of file diff --git a/modules/body/test/c++/simplegaitforceplatetofeetassignerTest.cpp b/modules/body/test/c++/simplegaitforceplatetofeetassignerTest.cpp index 9cf2380..ecf06d0 100644 --- a/modules/body/test/c++/simplegaitforceplatetofeetassignerTest.cpp +++ b/modules/body/test/c++/simplegaitforceplatetofeetassignerTest.cpp @@ -13,7 +13,7 @@ CXXTEST_SUITE(SimpleGaitForceplateToFeetAssignerTest) { CXXTEST_TEST(TwoForplatesTwoFeet) { - ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::Basic); helper.setMarkerDiameter(14.0); // mm helper.setLeftLegLength(780.0); // mm helper.setLeftKneeWidth(90.0); // mm @@ -42,7 +42,7 @@ CXXTEST_SUITE(SimpleGaitForceplateToFeetAssignerTest) CXXTEST_TEST(TwoForplatesTwoFeetBis) { - ma::body::PluginGait helper(ma::body::Region::Full, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Full, ma::body::Side::Both, ma::body::PluginGait::Basic); helper.setMarkerDiameter(16.0); // mm helper.setLeftFootFlatEnabled(true); helper.setLeftLegLength(800.0); // mm @@ -73,7 +73,7 @@ CXXTEST_SUITE(SimpleGaitForceplateToFeetAssignerTest) CXXTEST_TEST(TwoForplatesTwoFeetTer) { - ma::body::PluginGait helper(ma::body::Region::Full, ma::body::Side::Both); + ma::body::PluginGait helper(ma::body::Region::Full, ma::body::Side::Both, ma::body::PluginGait::Basic); helper.setMarkerDiameter(16.0); // mm helper.setLeftFootFlatEnabled(true); helper.setLeftLegLength(940.0); // mm diff --git a/modules/body/test/python/CMakeLists.txt b/modules/body/test/python/CMakeLists.txt index db10fd4..ec700b4 100644 --- a/modules/body/test/python/CMakeLists.txt +++ b/modules/body/test/python/CMakeLists.txt @@ -3,4 +3,5 @@ SET(TESTS_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}") SET(PTD_DIRS ${PACKAGE_DIRECTORY} ${TESTS_DIRECTORY} "${CMAKE_BINARY_DIR}/modules/io/test/python") ADD_PYTHON_TESTDRIVER(python_body_landmarkstranslator landmarkstranslatorTest INCLUDES ${PTD_DIRS}) +ADD_PYTHON_TESTDRIVER(python_body_plugingait_calibration plugingaitTest_calibration INCLUDES ${PTD_DIRS}) ADD_PYTHON_TESTDRIVER(python_body_plugingait plugingaitTest INCLUDES ${PTD_DIRS}) \ No newline at end of file diff --git a/modules/body/test/python/plugingaitTest_calibration.py b/modules/body/test/python/plugingaitTest_calibration.py new file mode 100644 index 0000000..f423e81 --- /dev/null +++ b/modules/body/test/python/plugingaitTest_calibration.py @@ -0,0 +1,62 @@ +import ma.body +import ma.io +import unittest + +from test_file_path import openma_tdd_path_in + +class PluginGaitCalibrationTest(unittest.TestCase): + def test_calibrateLowerBodyKADMed(self): + helper = ma.body.PluginGait(ma.body.Region_Lower, ma.body.Side_Both, ma.body.PluginGait.Variant_KAD) + subject = ma.Subject('Anonymous', [ + ['markerDiameter', 14], + ['leftLegLength', 860.0], + ['leftKneeWidth', 102.0], + ['leftAnkleWidth', 75.3], + ['leftFootFlatEnabled', True], + ['rightLegLength', 865.0], + ['rightKneeWidth', 103.4], + ['rightAnkleWidth', 72.9], + ['rightFootFlatEnabled', True], + ]) + statictrials = ma.io.read(openma_tdd_path_in("c3d/plugingait/PiGKad_Calibration_FootFlat.c3d")) + self.assertEqual(helper.calibrate(statictrials, subject), True) + self.assertAlmostEqual(helper.interAsisDistance(), 211.162, 3); + self.assertAlmostEqual(helper.leftAsisTrochanterAPDistance(), 62.208, 3); + self.assertAlmostEqual(helper.leftThighRotationOffset(), 0.156355, 4); + self.assertAlmostEqual(helper.leftShankRotationOffset(), 0.242123, 4); + self.assertAlmostEqual(helper.leftStaticPlantarFlexionOffset(), 0.0629524, 4); + self.assertAlmostEqual(helper.leftStaticRotationOffset(), 0.00504156, 4); + self.assertAlmostEqual(helper.rightAsisTrochanterAPDistance(), 62.852, 4); + self.assertAlmostEqual(helper.rightThighRotationOffset(), -0.175377, 4); + self.assertAlmostEqual(helper.rightShankRotationOffset(), 0.268151, 4); + self.assertAlmostEqual(helper.rightStaticPlantarFlexionOffset(), 0.12429, 4); + self.assertAlmostEqual(helper.rightStaticRotationOffset(), 0.0345916, 4); + + def test_calibrateLowerBodyKADMed2(self): + helper = ma.body.PluginGait(ma.body.Region_Lower, ma.body.Side_Both, ma.body.PluginGait.Variant_KADMed) + subject = ma.Subject('Anonymous', [ + ['markerDiameter', 25], + ['leftLegLength', 775.0], + ['leftKneeWidth', 105.1], + ['leftAnkleWidth', 68.4], + ['rightLegLength', 770.0], + ['rightKneeWidth', 107.0], + ['rightAnkleWidth', 68.6] + ]) + statictrials = ma.io.read(openma_tdd_path_in("c3d/plugingait/PiGKadMed_Calibration2.c3d")) + self.assertEqual(helper.calibrate(statictrials, subject), True) + self.assertAlmostEqual(helper.interAsisDistance(), 213.74600219726562, 3) + self.assertAlmostEqual(helper.leftAsisTrochanterAPDistance(), 51.259998321533203, 3) + self.assertAlmostEqual(helper.leftThighRotationOffset(), -0.19650661945343018, 4) + self.assertAlmostEqual(helper.leftShankRotationOffset(), 0.036574419587850571, 4) + self.assertAlmostEqual(helper.leftTibialTorsionOffset(), 0.18572920560836792, 4) + self.assertAlmostEqual(helper.leftStaticPlantarFlexionOffset(), 0.082310251891613007, 4) + self.assertAlmostEqual(helper.leftStaticRotationOffset(), 0.08043140172958374, 4) + self.assertAlmostEqual(helper.leftAnkleAbAddOffset(), 0.13921844959259033, 4) + self.assertAlmostEqual(helper.rightAsisTrochanterAPDistance(), 50.616001129150391, 3) + self.assertAlmostEqual(helper.rightThighRotationOffset(), 0.230163544416427610, 4) + self.assertAlmostEqual(helper.rightShankRotationOffset(), -0.11728926748037338, 4) + self.assertAlmostEqual(helper.rightTibialTorsionOffset(), -0.34318757057189941, 4) + self.assertAlmostEqual(helper.rightStaticPlantarFlexionOffset(), 0.032737139612436295, 4) + self.assertAlmostEqual(helper.rightStaticRotationOffset(), 0.28456372022628784, 4) + self.assertAlmostEqual(helper.rightAnkleAbAddOffset(), 0.13163727521896362, 4) \ No newline at end of file diff --git a/modules/math/include/openma/math/binaryop.h b/modules/math/include/openma/math/binaryop.h index 995651e..16e66cc 100644 --- a/modules/math/include/openma/math/binaryop.h +++ b/modules/math/include/openma/math/binaryop.h @@ -298,11 +298,6 @@ namespace math }; }; - - - - - // Defined here due to the declaration order of the classes. The associated documentation is in the header of the XprBase class. template template @@ -376,6 +371,223 @@ namespace math return TransformOp(*this,other); }; + // ----------------------------------------------------------------------- // + // DOTPRODUCTOP + // ----------------------------------------------------------------------- // + + template + struct Traits> + { + static _OPENMA_CONSTEXPR int Processing = Full; + }; + + template + struct Traits,XprOne,XprTwo>> + { + using Values = typename Traits::Values; + using Residuals = typename Traits::Residuals; + using Index = typename Values::Index; + static _OPENMA_CONSTEXPR int ColsAtCompileTime = 1; + static _OPENMA_CONSTEXPR int Processing = Traits>::Processing; + }; + + // ----------------------------------------------------------------------- // + + /** + * @class DotOp openma/math/binaryop.h + * @brief Compute the dot product of two expressions + * @tparam XprOne type of the left hand side operation + * @tparam XprTwo type of the right hand side operation + * Template expression to compute a geometric dot product. + * @ingroup openma_math + */ + template + class DotOp : public BinaryOp, XprOne, XprTwo> + { + static_assert(XprOne::ColsAtCompileTime == XprTwo::ColsAtCompileTime, "The number of columns must be the same."); + static_assert(XprOne::ColsAtCompileTime == 3 && XprTwo::ColsAtCompileTime == 3, "The cross operation is only available for array with 3 columns."); + + using Index = typename Traits, XprOne, XprTwo>>::Index; ///< Type used to access elements in Values or Residuals. + + public: + /** + * Constructor + */ + DotOp(const XprBase& x1, const XprBase& x2) + : BinaryOp, XprOne, XprTwo>(x1,x2) + { + assert(this->m_Xpr1.rows() == this->m_Xpr2.rows()); + }; + + /** + * Returns the number of rows that shall have the result of this operation. Internaly, this method relies on the number of rows of the first expresion. + */ + Index rows() const _OPENMA_NOEXCEPT {return this->m_Xpr1.rows();}; + + /** + * Returns the dot product of the two expressions as a template expression. + */ + auto values() const _OPENMA_NOEXCEPT -> decltype((OPENMA_MATHS_DECLVAL_NESTED(XprOne).values() * OPENMA_MATHS_DECLVAL_NESTED(XprTwo).values()).rowwise().sum()) + { + return (this->m_Xpr1.values() * this->m_Xpr2.values()).rowwise().sum(); + }; + + /** + * Returns the residuals associated with this operation. The residuals is generated based on the ones of each input. + */ + auto residuals() const _OPENMA_NOEXCEPT -> decltype(generate_residuals((OPENMA_MATHS_DECLVAL_NESTED(XprOne).residuals() >= 0.0) && (OPENMA_MATHS_DECLVAL_NESTED(XprTwo).residuals() >= 0.0))) + { + return generate_residuals((this->m_Xpr1.residuals() >= 0.0) && (this->m_Xpr2.residuals() >= 0.0)); + }; + }; + + // Defined here due to the declaration order of the classes. The associated documentation is in the header of the XprBase class. + template + template + inline const DotOp XprBase::dot(const XprBase& other) const _OPENMA_NOEXCEPT + { + return DotOp(*this,other); + }; + + // ----------------------------------------------------------------------- // + // COEFFICIENTPRODUCTOP + // ----------------------------------------------------------------------- // + + template + struct Traits> + { + static _OPENMA_CONSTEXPR int Processing = Full; + }; + + // ----------------------------------------------------------------------- // + + /** + * @class CoefficientProductOp openma/math/binaryop.h + * @brief Compute the coefficient-wise product of two expressions + * @tparam XprOne type of the left hand side operation + * @tparam XprTwo type of the right hand side operation + * Template expression to compute a coefficient-wise product. + * @ingroup openma_math + */ + template + class CoefficientProductOp : public BinaryOp, XprOne, XprTwo> + { + static_assert(XprOne::ColsAtCompileTime == XprTwo::ColsAtCompileTime, "The number of columns must be the same."); + + using Index = typename Traits, XprOne, XprTwo>>::Index; ///< Type used to access elements in Values or Residuals. + + public: + /** + * Constructor + */ + CoefficientProductOp(const XprBase& x1, const XprBase& x2) + : BinaryOp, XprOne, XprTwo>(x1,x2) + { + assert(this->m_Xpr1.rows() == this->m_Xpr2.rows()); + }; + + /** + * Returns the number of rows that shall have the result of this operation. Internaly, this method relies on the number of rows of the first expresion. + */ + Index rows() const _OPENMA_NOEXCEPT {return this->m_Xpr1.rows();}; + + /** + * Returns the coefficient-wise product of the two expressions as a template expression. + */ + auto values() const _OPENMA_NOEXCEPT -> decltype(OPENMA_MATHS_DECLVAL_NESTED(XprOne).values() * OPENMA_MATHS_DECLVAL_NESTED(XprTwo).values()) + { + return this->m_Xpr1.values() * this->m_Xpr2.values(); + }; + + /** + * Returns the residuals associated with this operation. The residuals is generated based on the ones of each input. + */ + auto residuals() const _OPENMA_NOEXCEPT -> decltype(generate_residuals((OPENMA_MATHS_DECLVAL_NESTED(XprOne).residuals() >= 0.0) && (OPENMA_MATHS_DECLVAL_NESTED(XprTwo).residuals() >= 0.0))) + { + return generate_residuals((this->m_Xpr1.residuals() >= 0.0) && (this->m_Xpr2.residuals() >= 0.0)); + }; + }; + + // ----------------------------------------------------------------------- // + // ARCTANGENT2OP + // ----------------------------------------------------------------------- // + + template + struct Traits> + { + static _OPENMA_CONSTEXPR int Processing = Full; + }; + + template + struct Traits,XprOne,XprTwo>> + { + using Values = typename Traits::Values; + using Residuals = typename Traits::Residuals; + using Index = typename Values::Index; + static _OPENMA_CONSTEXPR int ColsAtCompileTime = 1; + static _OPENMA_CONSTEXPR int Processing = Traits>::Processing; + }; + + // ----------------------------------------------------------------------- // + + /** + * @class ArcTangent2 openma/math/binaryop.h + * @brief Compute the arc tangent of two expressions + * @tparam XprOne type of the left hand side operation + * @tparam XprTwo type of the right hand side operation + * Template expression to compute arc tangent using internally the atan2 function. + * @ingroup openma_math + */ + template + class ArcTangent2Op : public BinaryOp, XprOne, XprTwo> + { + static_assert(XprOne::ColsAtCompileTime == XprTwo::ColsAtCompileTime, "The number of columns must be the same."); + static_assert(XprOne::ColsAtCompileTime == 1 && XprTwo::ColsAtCompileTime == 1, "The atan2 operation is only available for array with 1 columns."); + + using Index = typename Traits, XprOne, XprTwo>>::Index; ///< Type used to access elements in Values or Residuals. + + public: + /** + * Constructor + */ + ArcTangent2Op(const XprBase& x1, const XprBase& x2) + : BinaryOp, XprOne, XprTwo>(x1,x2) + { + assert(this->m_Xpr1.rows() == this->m_Xpr2.rows()); + }; + + /** + * Returns the number of rows that shall have the result of this operation. Internaly, this method relies on the number of rows of the first expresion. + */ + Index rows() const _OPENMA_NOEXCEPT {return this->m_Xpr1.rows();}; + + /** + * Returns the arc tangent of the two expressions as a template expression. + */ + auto values() const _OPENMA_NOEXCEPT -> Eigen::internal::ArcTangent2OpValues + { + using V1 = decltype(this->m_Xpr1.values()); + using V2 = decltype(this->m_Xpr2.values()); + return Eigen::internal::ArcTangent2OpValues(this->m_Xpr1.values(), this->m_Xpr2.values()); + }; + + /** + * Returns the residuals associated with this operation. The residuals is generated based on the ones of each input. + */ + auto residuals() const _OPENMA_NOEXCEPT -> decltype(generate_residuals((OPENMA_MATHS_DECLVAL_NESTED(XprOne).residuals() >= 0.0) && (OPENMA_MATHS_DECLVAL_NESTED(XprTwo).residuals() >= 0.0))) + { + return generate_residuals((this->m_Xpr1.residuals() >= 0.0) && (this->m_Xpr2.residuals() >= 0.0)); + }; + }; + + // Defined here due to the declaration order of the classes. The associated documentation is in the header of the XprBase class. + template + template + inline const ArcTangent2Op XprBase::atan2(const XprBase& other) const _OPENMA_NOEXCEPT + { + return ArcTangent2Op(*this,other); + }; + }; }; diff --git a/modules/math/include/openma/math/forwarddeclarations.h b/modules/math/include/openma/math/forwarddeclarations.h index e06a204..a4cf08e 100644 --- a/modules/math/include/openma/math/forwarddeclarations.h +++ b/modules/math/include/openma/math/forwarddeclarations.h @@ -64,12 +64,15 @@ namespace math template class CrossOp; template class TransposeOp; template class TransformOp; - template class ReplicateOp; + template class ReplicateOp; template class InverseOp; template class DownsampleOp; template class EulerAnglesOp; template class DerivativeOp; template class SkewReduxOp; + template class DotOp; + template class CoefficientProductOp; + template class ArcTangent2Op; }; }; diff --git a/modules/math/include/openma/math/returnbyvalue.h b/modules/math/include/openma/math/returnbyvalue.h index 6728071..5dfb99a 100644 --- a/modules/math/include/openma/math/returnbyvalue.h +++ b/modules/math/include/openma/math/returnbyvalue.h @@ -636,6 +636,38 @@ namespace internal Index rows() const {return this->m_V.rows();}; Index cols() const {return this->m_V.cols();}; }; + + // ----------------------------------------------------------------------- // + // ArcTangent2Op return value + // ----------------------------------------------------------------------- // + + template struct ArcTangent2OpValues; + + template + struct traits> + { + using ReturnType = typename ma::math::Traits::type::ColsAtCompileTime>>::Values; + }; + + template + struct ArcTangent2OpValues : public Eigen::ReturnByValue> + { + using InputType1 = typename std::decay::type; + using InputType2 = typename std::decay::type; + using Index = typename InputType1::Index; + typename InputType1::Nested m_V1; + typename InputType2::Nested m_V2; + public: + ArcTangent2OpValues(const V1& v1, const V2& v2) : m_V1(v1), m_V2(v2) {}; + template inline void evalTo(R& result) const + { + for (Index i = 0, len = this->m_V1.rows() ; i < len ; ++i) + result.coeffRef(i) = atan2(this->m_V1.coeff(i), this->m_V2.coeff(i)); + }; + Index rows() const {return this->m_V1.rows();}; + Index cols() const {return 1;}; + }; + }; }; diff --git a/modules/math/include/openma/math/unaryop.h b/modules/math/include/openma/math/unaryop.h index 43dbdcd..f70c054 100644 --- a/modules/math/include/openma/math/unaryop.h +++ b/modules/math/include/openma/math/unaryop.h @@ -375,12 +375,22 @@ namespace math // REPLICATEOP // ----------------------------------------------------------------------- // - template - struct Traits> + template + struct Traits> { static _OPENMA_CONSTEXPR int Processing = None; }; + template + struct Traits,Xpr>> + { + using Values = typename Traits::Values; + using Residuals = typename Traits::Residuals; + using Index = typename Values::Index; + static _OPENMA_CONSTEXPR int ColsAtCompileTime = Traits::ColsAtCompileTime * U; + static _OPENMA_CONSTEXPR int Processing = Traits>::Processing; + }; + // ----------------------------------------------------------------------- // /** @@ -390,10 +400,10 @@ namespace math * Template expression to replicate the rows and generate the associated residuals. * @ingroup openma_math */ - template - class ReplicateOp : public UnaryOp,Xpr> + template + class ReplicateOp : public UnaryOp,Xpr> { - using Index = typename Traits, Xpr>>::Index; ///< Type used to access elements in Values or Residuals. + using Index = typename Traits, Xpr>>::Index; ///< Type used to access elements in Values or Residuals. Index m_Rows; @@ -402,7 +412,7 @@ namespace math * Constructor */ ReplicateOp(const XprBase& x, Index rows) - : UnaryOp,Xpr>(x), m_Rows(rows) + : UnaryOp,Xpr>(x), m_Rows(rows) {}; /** @@ -418,7 +428,7 @@ namespace math */ auto values() const _OPENMA_NOEXCEPT -> Eigen::Replicate::type, Eigen::Dynamic, Eigen::Dynamic> { - return this->m_Xpr.values().replicate(this->m_Rows,1); + return this->m_Xpr.values().replicate(this->m_Rows,U); }; /** @@ -432,9 +442,10 @@ namespace math // Defined here due to the declaration order of the classes. The associated documentation is in the header of the XprBase class. template - inline const ReplicateOp XprBase::replicate(Index rows) const _OPENMA_NOEXCEPT + template + inline const ReplicateOp XprBase::replicate(Index rows) const _OPENMA_NOEXCEPT { - return ReplicateOp(*this,rows); + return ReplicateOp(*this,rows); }; // ----------------------------------------------------------------------- // diff --git a/modules/math/include/openma/math/xprbase.h b/modules/math/include/openma/math/xprbase.h index 352290d..957d2d9 100644 --- a/modules/math/include/openma/math/xprbase.h +++ b/modules/math/include/openma/math/xprbase.h @@ -136,7 +136,7 @@ namespace math /** * Returns an object representing a replicated version of this @a rows time. */ - const ReplicateOp replicate(Index rows) const _OPENMA_NOEXCEPT; + template const ReplicateOp replicate(Index rows = 1) const _OPENMA_NOEXCEPT; // Next method is defined after the declaration of the class TransformOp @@ -193,6 +193,20 @@ namespace math * Returns an object representing an euler angles operation using the given order @a a0, @a a1, @a a2 for the sequence order. */ const EulerAnglesOp eulerAngles(Index a0, Index a1, Index a2) const _OPENMA_NOEXCEPT; + + // Next method is defined after the declaration of the class DotOp + + /** + * Returns an object representing a dot product operator between @a this object and the @a other object. + */ + template const DotOp dot(const XprBase& other) const _OPENMA_NOEXCEPT; + + // Next method is defined after the declaration of the class ArcTangent2Op + + /** + * Returns an object representing a function operator between @a this object and the @a other object. + */ + template const ArcTangent2Op atan2(const XprBase& other) const _OPENMA_NOEXCEPT; }; // ----------------------------------------------------------------------- // @@ -267,6 +281,17 @@ namespace math assert(fabs(x2) >= std::numeric_limits::epsilon()); return ScaleOp(1.0/x2,x1); }; + + /** + * Convenient multiplication operator to create a coefficient-wise product of @a x1 and @a x2. + * @relates XprBase + * @relates ArrayBase + */ + template + const CoefficientProductOp operator* (const XprBase& x1, const XprBase& x2) + { + return CoefficientProductOp(x1,x2); + }; }; }; diff --git a/modules/math/test/c++/arrayTest.cpp b/modules/math/test/c++/arrayTest.cpp index d58687c..3cc6831 100644 --- a/modules/math/test/c++/arrayTest.cpp +++ b/modules/math/test/c++/arrayTest.cpp @@ -341,6 +341,44 @@ CXXTEST_SUITE(ArrayTest) TS_ASSERT_DELTA(br.coeff(5), 0.0, 1e-15); }; + CXXTEST_TEST(replicateTer) + { + ma::math::Scalar::Values av(2,1); + av << -1.0, 1.0; + + ma::math::Position b = ma::math::Scalar(av,ma::math::Position::Residuals::Zero(av.rows())).replicate<3>(3); + const ma::math::Position::Values& bv = b.values(); + const ma::math::Position::Residuals& br = b.residuals(); + TS_ASSERT_EQUALS(b.rows(), 6); + TS_ASSERT_EQUALS(b.cols(), 3); + + TS_ASSERT_DELTA(bv.coeff(0,0), -1.0, 1e-15); + TS_ASSERT_DELTA(bv.coeff(0,1), -1.0, 1e-15); + TS_ASSERT_DELTA(bv.coeff(0,2), -1.0, 1e-15); + TS_ASSERT_DELTA(bv.coeff(1,0), 1.0, 1e-15); + TS_ASSERT_DELTA(bv.coeff(1,1), 1.0, 1e-15); + TS_ASSERT_DELTA(bv.coeff(1,2), 1.0, 1e-15); + TS_ASSERT_DELTA(bv.coeff(2,0), -1.0, 1e-15); + TS_ASSERT_DELTA(bv.coeff(2,1), -1.0, 1e-15); + TS_ASSERT_DELTA(bv.coeff(2,2), -1.0, 1e-15); + TS_ASSERT_DELTA(bv.coeff(3,0), 1.0, 1e-15); + TS_ASSERT_DELTA(bv.coeff(3,1), 1.0, 1e-15); + TS_ASSERT_DELTA(bv.coeff(3,2), 1.0, 1e-15); + TS_ASSERT_DELTA(bv.coeff(4,0), -1.0, 1e-15); + TS_ASSERT_DELTA(bv.coeff(4,1), -1.0, 1e-15); + TS_ASSERT_DELTA(bv.coeff(4,2), -1.0, 1e-15); + TS_ASSERT_DELTA(bv.coeff(5,0), 1.0, 1e-15); + TS_ASSERT_DELTA(bv.coeff(5,1), 1.0, 1e-15); + TS_ASSERT_DELTA(bv.coeff(5,2), 1.0, 1e-15); + + TS_ASSERT_DELTA(br.coeff(0), 0.0, 1e-15); + TS_ASSERT_DELTA(br.coeff(1), 0.0, 1e-15); + TS_ASSERT_DELTA(br.coeff(2), 0.0, 1e-15); + TS_ASSERT_DELTA(br.coeff(3), 0.0, 1e-15); + TS_ASSERT_DELTA(br.coeff(4), 0.0, 1e-15); + TS_ASSERT_DELTA(br.coeff(5), 0.0, 1e-15); + }; + CXXTEST_TEST(transpose) { ma::math::Array<9> O(2); @@ -748,7 +786,69 @@ CXXTEST_SUITE(ArrayTest) CXXTEST_TEST(resize) { TS_WARN("TODO"); - } + }; + + CXXTEST_TEST(dot) + { + ma::math::Array<3> d1(10), d2(10); + d1.values().setRandom(); d1.residuals().setZero(); + d2.values().setRandom(); d2.residuals().setZero(); + ma::math::Array<1> dd = d1.dot(d2); + const auto& ddv = dd.values(); + ma::math::Array<1>::Values ref = d1.values().col(0) * d2.values().col(0) + d1.values().col(1) * d2.values().col(1) + d1.values().col(2) * d2.values().col(2); + for (unsigned i = 0 ; i < 10 ; ++i) + TS_ASSERT_DELTA(ddv.coeff(i), ref.coeff(i), 1e-15); + }; + + CXXTEST_TEST(dotReplicate) + { + ma::math::Array<3> d1(10), d2(10); + d1.values().setRandom(); d1.residuals().setZero(); + d2.values().setRandom(); d2.residuals().setZero(); + ma::math::Array<3> dd = d1.dot(d2).replicate<3>(); + const auto& ddv = dd.values(); + ma::math::Array<1>::Values ref = d1.values().col(0) * d2.values().col(0) + d1.values().col(1) * d2.values().col(1) + d1.values().col(2) * d2.values().col(2); + for (unsigned i = 0 ; i < 10 ; ++i) + { + TS_ASSERT_DELTA(ddv.coeff(i,0), ref.coeff(i), 1e-15); + TS_ASSERT_DELTA(ddv.coeff(i,1), ref.coeff(i), 1e-15); + TS_ASSERT_DELTA(ddv.coeff(i,2), ref.coeff(i), 1e-15); + } + }; + + CXXTEST_TEST(coefficientProduct) + { + ma::math::Array<3> d1(10), d2(10); + d1.values().setRandom(); d1.residuals().setZero(); + d2.values().setRandom(); d2.residuals().setZero(); d2.residuals().coeffRef(9) = -1.; + ma::math::Array<3> dd = d1 * d2; + const auto& ddv = dd.values(); + ma::math::Array<3>::Values ref = d1.values() * d2.values(); + for (unsigned i = 0 ; i < 9 ; ++i) + TS_ASSERT_DELTA(ddv.coeff(i), ref.coeff(i), 1e-15); + TS_ASSERT_DELTA(ddv.coeff(9), 0.0, 1e-15); + }; + + CXXTEST_TEST(atan2) + { + ma::math::Array<1> x(6), y(6); + x.residuals().setZero(); y.residuals().setZero(); y.residuals().coeffRef(0) = -1.; + x.values() << 0.12312, 0., -0., 0., -0., -0.; + y.values() << 0.24232, 0., 0., 7., 7., -7.; + ma::math::Array<1> arc = y.atan2(x); + TS_ASSERT_DELTA(arc.values().coeff(0), 0.0, 1e-15); + TS_ASSERT_DELTA(arc.residuals().coeff(0), -1.0, 1e-15); + TS_ASSERT_DELTA(arc.values().coeff(1), 0.0, 1e-15); + TS_ASSERT_DELTA(arc.residuals().coeff(1), 0.0, 1e-15); + TS_ASSERT_DELTA(arc.values().coeff(2), 3.14159, 1e-4); + TS_ASSERT_DELTA(arc.residuals().coeff(2), 0.0, 1e-15); + TS_ASSERT_DELTA(arc.values().coeff(3), 1.5708, 1e-4); + TS_ASSERT_DELTA(arc.residuals().coeff(3), 0., 1e-15); + TS_ASSERT_DELTA(arc.values().coeff(4), 1.5708, 1e-4); + TS_ASSERT_DELTA(arc.residuals().coeff(4), 0.0, 1e-15); + TS_ASSERT_DELTA(arc.values().coeff(5), -1.5708, 1e-4); + TS_ASSERT_DELTA(arc.residuals().coeff(5), 0.0, 1e-15); + }; }; CXXTEST_SUITE_REGISTRATION(ArrayTest) @@ -762,6 +862,7 @@ CXXTEST_TEST_REGISTRATION(ArrayTest, crossBis) CXXTEST_TEST_REGISTRATION(ArrayTest, operatorDouble) CXXTEST_TEST_REGISTRATION(ArrayTest, replicate) CXXTEST_TEST_REGISTRATION(ArrayTest, replicateBis) +CXXTEST_TEST_REGISTRATION(ArrayTest, replicateTer) CXXTEST_TEST_REGISTRATION(ArrayTest, transpose) CXXTEST_TEST_REGISTRATION(ArrayTest, transposeBis) CXXTEST_TEST_REGISTRATION(ArrayTest, transposeTer) @@ -771,4 +872,8 @@ CXXTEST_TEST_REGISTRATION(ArrayTest, derivative) CXXTEST_TEST_REGISTRATION(ArrayTest, derivativebis) CXXTEST_TEST_REGISTRATION(ArrayTest, skewRedux) CXXTEST_TEST_REGISTRATION(ArrayTest, downsample) -CXXTEST_TEST_REGISTRATION(ArrayTest, resize) \ No newline at end of file +CXXTEST_TEST_REGISTRATION(ArrayTest, resize) +CXXTEST_TEST_REGISTRATION(ArrayTest, dot) +CXXTEST_TEST_REGISTRATION(ArrayTest, dotReplicate) +CXXTEST_TEST_REGISTRATION(ArrayTest, coefficientProduct) +CXXTEST_TEST_REGISTRATION(ArrayTest, atan2)