From 8368bb9e86380dd9fa4dce637f555745de15d4c7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Fri, 23 Sep 2016 14:23:30 -0400 Subject: [PATCH 01/35] New static properties to get/set supplementary offset associated with the Plug-in Gait model. --- modules/body/include/openma/body/plugingait.h | 14 ++ .../body/include/openma/body/plugingait_p.h | 20 +- modules/body/src/plugingait.cpp | 210 +++++++++++++++++- modules/body/swig/body/plugingait.i | 14 ++ 4 files changed, 251 insertions(+), 7 deletions(-) diff --git a/modules/body/include/openma/body/plugingait.h b/modules/body/include/openma/body/plugingait.h index d500607..0e86af7 100644 --- a/modules/body/include/openma/body/plugingait.h +++ b/modules/body/include/openma/body/plugingait.h @@ -83,6 +83,18 @@ namespace body void setRightAsisTrochanterAPDistance(double value) _OPENMA_NOEXCEPT; double leftAsisTrochanterAPDistance() const _OPENMA_NOEXCEPT; void setLeftAsisTrochanterAPDistance(double value) _OPENMA_NOEXCEPT; + double rightTibialTorsion() const _OPENMA_NOEXCEPT; + void setRightTibialTorsion(double value) _OPENMA_NOEXCEPT; + double leftTibialTorsion() const _OPENMA_NOEXCEPT; + void setLeftTibialTorsion(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 +112,8 @@ namespace body double rightStaticRotationOffset() const _OPENMA_NOEXCEPT; double leftStaticPlantarFlexionOffset() const _OPENMA_NOEXCEPT; double leftStaticRotationOffset() const _OPENMA_NOEXCEPT; + double rightAnkleAbAdd() const _OPENMA_NOEXCEPT; + double leftAnkleAbAdd() 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..6687ce1 100644 --- a/modules/body/include/openma/body/plugingait_p.h +++ b/modules/body/include/openma/body/plugingait_p.h @@ -84,6 +84,12 @@ namespace body Property {"leftLegLength"}, Property {"rightAsisTrochanterAPDistance"}, Property {"leftAsisTrochanterAPDistance"}, + Property {"rightTibialTorsion"}, + Property {"leftTibialTorsion"}, + Property {"rightThighRotationOffset"}, + Property {"leftThighRotationOffset"}, + Property {"rightShankRotationOffset"}, + Property {"leftShankRotationOffset"}, Property {"rightKneeWidth"}, Property {"leftKneeWidth"}, Property {"rightAnkleWidth"}, @@ -95,11 +101,13 @@ namespace body Property {"rightStaticPlantarFlexionOffset"}, Property {"rightStaticRotationOffset"}, Property {"leftStaticPlantarFlexionOffset"}, - Property {"leftStaticRotationOffset"} + Property {"leftStaticRotationOffset"}, + Property {"rightAnkleAbAdd"}, + Property {"leftAnkleAbAdd"} ) 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; @@ -125,6 +133,12 @@ namespace body double LeftLegLength; double RightAsisTrochanterAPDistance; double LeftAsisTrochanterAPDistance; + double RightTibialTorsion; + double LeftTibialTorsion; + double RightThighRotationOffset; + double LeftThighRotationOffset; + double RightShankRotationOffset; + double LeftShankRotationOffset; double RightKneeWidth; double LeftKneeWidth; double RightAnkleWidth; @@ -137,6 +151,8 @@ namespace body double RightStaticRotationOffset; double LeftStaticPlantarFlexionOffset; double LeftStaticRotationOffset; + double RightAnkleAbAdd; + double LeftAnkleAbAdd; }; inline void PluginGaitPrivate::computeHipJointCenter(double* HJC, double S, double C, double xdis) const _OPENMA_NOEXCEPT diff --git a/modules/body/src/plugingait.cpp b/modules/body/src/plugingait.cpp index c927c74..afb389b 100644 --- a/modules/body/src/plugingait.cpp +++ b/modules/body/src/plugingait.cpp @@ -84,6 +84,12 @@ namespace body LeftLegLength(0.0), RightAsisTrochanterAPDistance(0.0), LeftAsisTrochanterAPDistance(0.0), + RightTibialTorsion(0.0), + LeftTibialTorsion(0.0), + RightThighRotationOffset(0.0), + LeftThighRotationOffset(0.0), + RightShankRotationOffset(0.0), + LeftShankRotationOffset(0.0), RightKneeWidth(0.0), LeftKneeWidth(0.0), RightAnkleWidth(0.0), @@ -94,7 +100,9 @@ namespace body RightStaticPlantarFlexionOffset(0.0), RightStaticRotationOffset(0.0), LeftStaticPlantarFlexionOffset(0.0), - LeftStaticRotationOffset(0.0) + LeftStaticRotationOffset(0.0), + RightAnkleAbAdd(0.0), + LeftAnkleAbAdd(0.0) {}; PluginGaitPrivate::~PluginGaitPrivate() _OPENMA_NOEXCEPT = default; @@ -770,6 +778,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. By default, this property contains the value 0.0. + * @sa rightTibialTorsion() setRightTibialTorsion() + */ + double RightTibialTorsion; + /** + * [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. By default, this property contains the value 0.0. + * @sa leftTibialTorsion() setLeftTibialTorsion() + */ + double LeftTibialTorsion; + /** + * [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,14 +863,24 @@ 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 rightAnkleAbAdd() + */ + double RightAnkleAbAdd; + /** + * [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 leftAnkleAbAdd() + */ + double LeftAnkleAbAdd; }; #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) {}; /** @@ -1749,9 +1797,135 @@ namespace body this->modified(); }; - /** + /** + * Returns the internal parameter RightTibialTorsion. + */ + double PluginGait::rightTibialTorsion() const _OPENMA_NOEXCEPT + { + auto optr = this->pimpl(); + return optr->RightTibialTorsion; + }; + + /** + * Sets the internal parameter RightTibialTorsion. + */ + void PluginGait::setRightTibialTorsion(double value) _OPENMA_NOEXCEPT + { + auto optr = this->pimpl(); + if (fabs(value - optr->RightTibialTorsion) < std::numeric_limits::epsilon()) + return; + optr->RightTibialTorsion = value; + this->modified(); + }; + + /** + * Returns the internal parameter LeftTibialTorsion. + */ + double PluginGait::leftTibialTorsion() const _OPENMA_NOEXCEPT + { + auto optr = this->pimpl(); + return optr->LeftTibialTorsion; + }; + + /** + * Sets the internal parameter LeftTibialTorsion. + */ + void PluginGait::setLeftTibialTorsion(double value) _OPENMA_NOEXCEPT + { + auto optr = this->pimpl(); + if (fabs(value - optr->LeftTibialTorsion) < std::numeric_limits::epsilon()) + return; + optr->LeftTibialTorsion = 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(); @@ -1911,6 +2085,24 @@ namespace body return optr->LeftStaticRotationOffset; }; + /** + * Returns the internal parameter RightAnkleAbAdd. + */ + double PluginGait::rightAnkleAbAdd() const _OPENMA_NOEXCEPT + { + auto optr = this->pimpl(); + return optr->RightAnkleAbAdd; + }; + + /** + * Returns the internal parameter LeftAnkleAbAdd. + */ + double PluginGait::leftAnkleAbAdd() const _OPENMA_NOEXCEPT + { + auto optr = this->pimpl(); + return optr->LeftAnkleAbAdd; + }; + // ----------------------------------------------------------------------- // /* @@ -2047,6 +2239,12 @@ namespace body optr->LeftLegLength = optr_src->LeftLegLength; optr->RightAsisTrochanterAPDistance = optr_src->RightAsisTrochanterAPDistance; optr->LeftAsisTrochanterAPDistance = optr_src->LeftAsisTrochanterAPDistance; + optr->RightTibialTorsion = optr_src->RightTibialTorsion; + optr->LeftTibialTorsion = optr_src->LeftTibialTorsion; + 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; @@ -2058,6 +2256,8 @@ namespace body optr->RightStaticRotationOffset = optr_src->RightStaticRotationOffset; optr->LeftStaticPlantarFlexionOffset = optr_src->LeftStaticPlantarFlexionOffset; optr->LeftStaticRotationOffset = optr_src->LeftStaticRotationOffset; + optr->RightAnkleAbAdd = optr_src->RightAnkleAbAdd; + optr->LeftAnkleAbAdd = optr_src->LeftAnkleAbAdd; }; }; }; \ No newline at end of file diff --git a/modules/body/swig/body/plugingait.i b/modules/body/swig/body/plugingait.i index 5110c0b..4d8245c 100644 --- a/modules/body/swig/body/plugingait.i +++ b/modules/body/swig/body/plugingait.i @@ -76,6 +76,18 @@ namespace body void setRightAsisTrochanterAPDistance(double value); double leftAsisTrochanterAPDistance() const; void setLeftAsisTrochanterAPDistance(double value); + double rightTibialTorsion() const; + void setRightTibialTorsion(double value); + double leftTibialTorsion() const; + void setLeftTibialTorsion(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 +105,8 @@ namespace body double rightStaticRotationOffset() const; double leftStaticPlantarFlexionOffset() const; double leftStaticRotationOffset() const; + double rightAnkleAbAdd() const; + double leftAnkleAbAdd() const; }; %clearnodefaultctor; }; From d174d73a679a1f1a9fca0f419066732970cd478d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Fri, 23 Sep 2016 14:27:26 -0400 Subject: [PATCH 02/35] New option in the Plug-in Gait constructor to choose the "variant" to use. WARNING: Currrently, this variant has no effect! Only the basic model is implemented. --- modules/body/include/openma/body/plugingait.h | 4 +++- .../body/include/openma/body/plugingait_p.h | 2 ++ modules/body/src/plugingait.cpp | 6 +++-- modules/body/swig/body/plugingait.i | 10 +++++++- modules/body/test/c++/plugingaitTest.cpp | 6 ++--- .../test/c++/plugingaitTest_calibration.cpp | 24 +++++++++---------- .../test/c++/plugingaitTest_kinematics.cpp | 18 +++++++------- .../body/test/c++/plugingaitTest_kinetics.cpp | 4 ++-- .../c++/plugingaitTest_reconstruction.cpp | 18 +++++++------- ...simplegaitforceplatetofeetassignerTest.cpp | 6 ++--- 10 files changed, 56 insertions(+), 42 deletions(-) diff --git a/modules/body/include/openma/body/plugingait.h b/modules/body/include/openma/body/plugingait.h index 0e86af7..687590d 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; diff --git a/modules/body/include/openma/body/plugingait_p.h b/modules/body/include/openma/body/plugingait_p.h index 6687ce1..b8c89f3 100644 --- a/modules/body/include/openma/body/plugingait_p.h +++ b/modules/body/include/openma/body/plugingait_p.h @@ -115,6 +115,8 @@ namespace body 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; diff --git a/modules/body/src/plugingait.cpp b/modules/body/src/plugingait.cpp index afb389b..499f773 100644 --- a/modules/body/src/plugingait.cpp +++ b/modules/body/src/plugingait.cpp @@ -66,9 +66,10 @@ 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), @@ -2207,7 +2208,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; @@ -2224,6 +2225,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; diff --git a/modules/body/swig/body/plugingait.i b/modules/body/swig/body/plugingait.i index 4d8245c..b596cb9 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; 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..98b9da5 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")); diff --git a/modules/body/test/c++/plugingaitTest_kinematics.cpp b/modules/body/test/c++/plugingaitTest_kinematics.cpp index f2a19da..aaa9b5f 100644 --- a/modules/body/test/c++/plugingaitTest_kinematics.cpp +++ b/modules/body/test/c++/plugingaitTest_kinematics.cpp @@ -32,7 +32,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 @@ -77,7 +77,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 @@ -124,7 +124,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 @@ -169,7 +169,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 @@ -226,7 +226,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 @@ -285,7 +285,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 @@ -329,7 +329,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 @@ -371,7 +371,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 @@ -422,7 +422,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 diff --git a/modules/body/test/c++/plugingaitTest_kinetics.cpp b/modules/body/test/c++/plugingaitTest_kinetics.cpp index d919bfd..bcf5bbd 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..23f8814 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 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 From 4ba96275f2791197f6f155a66f9bf0f27d78e843 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Sat, 24 Sep 2016 00:38:59 -0400 Subject: [PATCH 03/35] Marker names mapping for the KAD and KADMed variants added. --- modules/body/src/plugingait.cpp | 25 ++++++++++++++++++++++++- 1 file changed, 24 insertions(+), 1 deletion(-) diff --git a/modules/body/src/plugingait.cpp b/modules/body/src/plugingait.cpp index 499f773..e9f9be1 100644 --- a/modules/body/src/plugingait.cpp +++ b/modules/body/src/plugingait.cpp @@ -2108,7 +2108,7 @@ namespace body /* * 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) @@ -2144,12 +2144,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"}, @@ -2185,6 +2198,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); }; From 96f5c05ecdd0c6f92d4a23e60692ef391d6fca96 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Sat, 24 Sep 2016 10:06:34 -0400 Subject: [PATCH 04/35] [WIP] Code refactored for the calibration of the knee joint centre and ankle joint centre using the KAD. --- .../body/include/openma/body/plugingait_p.h | 4 + modules/body/src/plugingait.cpp | 113 ++++++++++++++---- 2 files changed, 94 insertions(+), 23 deletions(-) diff --git a/modules/body/include/openma/body/plugingait_p.h b/modules/body/include/openma/body/plugingait_p.h index b8c89f3..e6b12ee 100644 --- a/modules/body/include/openma/body/plugingait_p.h +++ b/modules/body/include/openma/body/plugingait_p.h @@ -155,6 +155,10 @@ namespace body double LeftStaticRotationOffset; double RightAnkleAbAdd; double LeftAnkleAbAdd; + + using CalibrateJointFuncPtr = bool (*)(math::Position* , PluginGaitPrivate* , ummp* , const std::string& , double , const math::Position* ); + CalibrateJointFuncPtr CalibrateKneeJointCentre; + CalibrateJointFuncPtr CalibrateAnkleJointCentre; }; inline void PluginGaitPrivate::computeHipJointCenter(double* HJC, double S, double C, double xdis) const _OPENMA_NOEXCEPT diff --git a/modules/body/src/plugingait.cpp b/modules/body/src/plugingait.cpp index e9f9be1..6212c75 100644 --- a/modules/body/src/plugingait.cpp +++ b/modules/body/src/plugingait.cpp @@ -62,6 +62,72 @@ #ifndef DOXYGEN_SHOULD_SKIP_THIS +bool _ma_plugingait_calibrate_hjc_basic(ma::math::Position* KJC, ma::body::PluginGaitPrivate* optr, ma::body::ummp* landmarks, const std::string& prefix, 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_hjc_kad(ma::math::Position* KJC, ma::body::PluginGaitPrivate* optr, ma::body::ummp* landmarks, const std::string& prefix, 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"]; + if (!KAX.isValid() || !KD1.isValid() || !KD2.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 = (KD2 - KD1).cross(KAX - KD1).normalized(); + auto I = (KD1 + KAX) / 2.; + auto PP1 = (2.0 /3.0 * (I - KD2)) + KD2; + ma::math::Position O = PP1 - (n * sqrt(3.0) * dist / 3.0); + auto uKAXO = (O-KAX).normalized(); + const ma::math::Position& VLFE = O; + // Compute the knee joint centre (KJC) + *KJC = VLFE + uKAXO * (optr->MarkerDiameter + kneeWidth) / 2.0; + return true; +}; + +bool _ma_plugingait_calibrate_ajc_basic(ma::math::Position* AJC, ma::body::PluginGaitPrivate* optr, ma::body::ummp* landmarks, const std::string& prefix, 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::body::PluginGaitPrivate* optr, ma::body::ummp* landmarks, const std::string& prefix, double ankleWidth, const ma::math::Position* KJC) +{ + const auto& LTM = (*landmarks)[prefix+"LTM"]; + const auto& KAX = (*landmarks)[prefix+"KAX"]; + if (!LTM.isValid() || !KAX.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); + return true; +}; + namespace ma { namespace body @@ -103,9 +169,11 @@ namespace body LeftStaticPlantarFlexionOffset(0.0), LeftStaticRotationOffset(0.0), RightAnkleAbAdd(0.0), - LeftAnkleAbAdd(0.0) + LeftAnkleAbAdd(0.0), + CalibrateKneeJointCentre(nullptr), + CalibrateAnkleJointCentre(nullptr) {}; - + PluginGaitPrivate::~PluginGaitPrivate() _OPENMA_NOEXCEPT = default; bool PluginGaitPrivate::calibrateLowerLimb(int side, const math::Position* HJC, ummp* landmarks) _OPENMA_NOEXCEPT @@ -143,16 +211,9 @@ namespace body // ----------------------------------------- // 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; + if (!this->CalibrateKneeJointCentre(&KJC, this, landmarks, prefix, 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); @@ -167,16 +228,9 @@ 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; + if (!this->CalibrateAnkleJointCentre(&AJC, this, landmarks, prefix, 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); @@ -187,10 +241,11 @@ namespace body // ----------------------------------------- // Foot // ----------------------------------------- - // Required landmarks: *.MTH2, *.HEE + // Required landmarks: *.MTH2, *.HEE, *.LS 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"]; + if (!MTH2.isValid() || !HEE.isValid() || !LS.isValid()) { error("PluginGait - Missing landmarks to define the foot. Calibration aborted."); return false; @@ -882,7 +937,19 @@ namespace body */ 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_hjc_basic; + optr->CalibrateAnkleJointCentre = &_ma_plugingait_calibrate_ajc_basic; + } + else + { + optr->CalibrateKneeJointCentre = &_ma_plugingait_calibrate_hjc_kad; + optr->CalibrateAnkleJointCentre = &_ma_plugingait_calibrate_ajc_kad; + } + }; /** * Create segments and joints according to the region and side given to the constructor. From 10bab50c11cf210dbd8e92648d09fb262953e003 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Sat, 24 Sep 2016 22:49:01 -0400 Subject: [PATCH 05/35] s/computeHipJointCenter/computeHipJointCentre. --- modules/body/include/openma/body/plugingait_p.h | 4 ++-- modules/body/src/plugingait.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/modules/body/include/openma/body/plugingait_p.h b/modules/body/include/openma/body/plugingait_p.h index e6b12ee..7874209 100644 --- a/modules/body/include/openma/body/plugingait_p.h +++ b/modules/body/include/openma/body/plugingait_p.h @@ -110,7 +110,7 @@ namespace body 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; @@ -161,7 +161,7 @@ namespace body 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; diff --git a/modules/body/src/plugingait.cpp b/modules/body/src/plugingait.cpp index 6212c75..4d0d3be 100644 --- a/modules/body/src/plugingait.cpp +++ b/modules/body/src/plugingait.cpp @@ -1173,7 +1173,7 @@ namespace body // - 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, leftLegLength * 0.115 - 15.3, optr->LeftAsisTrochanterAPDistance); new Point("L.HJC", _L_HJC.data(), this); } else @@ -1181,7 +1181,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, rightLegLength * 0.115 - 15.3, optr->RightAsisTrochanterAPDistance); new Point("R.HJC", _R_HJC.data(), this); } else From a1eb16f81835febcda945cca53814601c3c2a8eb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Thu, 29 Sep 2016 10:50:51 -0400 Subject: [PATCH 06/35] Modification of the method ma::math::XprBase::relicate to be able to replicate columns. --- .../include/openma/math/forwarddeclarations.h | 2 +- modules/math/include/openma/math/unaryop.h | 29 +++++++++----- modules/math/include/openma/math/xprbase.h | 2 +- modules/math/test/c++/arrayTest.cpp | 39 +++++++++++++++++++ 4 files changed, 61 insertions(+), 11 deletions(-) diff --git a/modules/math/include/openma/math/forwarddeclarations.h b/modules/math/include/openma/math/forwarddeclarations.h index e06a204..78c930b 100644 --- a/modules/math/include/openma/math/forwarddeclarations.h +++ b/modules/math/include/openma/math/forwarddeclarations.h @@ -64,7 +64,7 @@ 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; diff --git a/modules/math/include/openma/math/unaryop.h b/modules/math/include/openma/math/unaryop.h index 5d366e7..be3496b 100644 --- a/modules/math/include/openma/math/unaryop.h +++ b/modules/math/include/openma/math/unaryop.h @@ -379,12 +379,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; + }; + // ----------------------------------------------------------------------- // /** @@ -394,10 +404,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; @@ -406,7 +416,7 @@ namespace math * Constructor */ ReplicateOp(const XprBase& x, Index rows) - : UnaryOp,Xpr>(x), m_Rows(rows) + : UnaryOp,Xpr>(x), m_Rows(rows) {}; /** @@ -422,7 +432,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); }; /** @@ -436,9 +446,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..f91af56 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 diff --git a/modules/math/test/c++/arrayTest.cpp b/modules/math/test/c++/arrayTest.cpp index d58687c..c2bfb40 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); @@ -762,6 +800,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) From 6fb2436cf4f83d3505aa8102c2719cde0d395670 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Thu, 29 Sep 2016 11:02:37 -0400 Subject: [PATCH 07/35] New method ma::math::XprBase::dot(). --- modules/math/include/openma/math/binaryop.h | 82 +++++++++++++++++-- .../include/openma/math/forwarddeclarations.h | 1 + modules/math/include/openma/math/xprbase.h | 7 ++ modules/math/test/c++/arrayTest.cpp | 34 +++++++- 4 files changed, 117 insertions(+), 7 deletions(-) diff --git a/modules/math/include/openma/math/binaryop.h b/modules/math/include/openma/math/binaryop.h index 995651e..ceed3c8 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,83 @@ 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); + }; }; }; diff --git a/modules/math/include/openma/math/forwarddeclarations.h b/modules/math/include/openma/math/forwarddeclarations.h index 78c930b..0ed202b 100644 --- a/modules/math/include/openma/math/forwarddeclarations.h +++ b/modules/math/include/openma/math/forwarddeclarations.h @@ -70,6 +70,7 @@ namespace math template class EulerAnglesOp; template class DerivativeOp; template class SkewReduxOp; + template class DotOp; }; }; diff --git a/modules/math/include/openma/math/xprbase.h b/modules/math/include/openma/math/xprbase.h index f91af56..bc6248f 100644 --- a/modules/math/include/openma/math/xprbase.h +++ b/modules/math/include/openma/math/xprbase.h @@ -193,6 +193,13 @@ 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; }; // ----------------------------------------------------------------------- // diff --git a/modules/math/test/c++/arrayTest.cpp b/modules/math/test/c++/arrayTest.cpp index c2bfb40..4a91600 100644 --- a/modules/math/test/c++/arrayTest.cpp +++ b/modules/math/test/c++/arrayTest.cpp @@ -786,7 +786,35 @@ 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_SUITE_REGISTRATION(ArrayTest) @@ -810,4 +838,6 @@ 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) From 130567ab0c26279575d40a667bd5c55662b426df Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Thu, 29 Sep 2016 11:05:27 -0400 Subject: [PATCH 08/35] New method to compute coefficient-wise product between two ma::math::XprBase objects. --- modules/math/include/openma/math/binaryop.h | 60 +++++++++++++++++++ .../include/openma/math/forwarddeclarations.h | 1 + modules/math/include/openma/math/xprbase.h | 11 ++++ modules/math/test/c++/arrayTest.cpp | 14 +++++ 4 files changed, 86 insertions(+) diff --git a/modules/math/include/openma/math/binaryop.h b/modules/math/include/openma/math/binaryop.h index ceed3c8..95bea9d 100644 --- a/modules/math/include/openma/math/binaryop.h +++ b/modules/math/include/openma/math/binaryop.h @@ -448,6 +448,66 @@ namespace math { 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)); + }; + }; + }; }; diff --git a/modules/math/include/openma/math/forwarddeclarations.h b/modules/math/include/openma/math/forwarddeclarations.h index 0ed202b..adccab1 100644 --- a/modules/math/include/openma/math/forwarddeclarations.h +++ b/modules/math/include/openma/math/forwarddeclarations.h @@ -71,6 +71,7 @@ namespace math template class DerivativeOp; template class SkewReduxOp; template class DotOp; + template class CoefficientProductOp; }; }; diff --git a/modules/math/include/openma/math/xprbase.h b/modules/math/include/openma/math/xprbase.h index bc6248f..4979da0 100644 --- a/modules/math/include/openma/math/xprbase.h +++ b/modules/math/include/openma/math/xprbase.h @@ -274,6 +274,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 4a91600..ef6871b 100644 --- a/modules/math/test/c++/arrayTest.cpp +++ b/modules/math/test/c++/arrayTest.cpp @@ -815,6 +815,19 @@ CXXTEST_SUITE(ArrayTest) 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_SUITE_REGISTRATION(ArrayTest) @@ -841,3 +854,4 @@ CXXTEST_TEST_REGISTRATION(ArrayTest, downsample) CXXTEST_TEST_REGISTRATION(ArrayTest, resize) CXXTEST_TEST_REGISTRATION(ArrayTest, dot) CXXTEST_TEST_REGISTRATION(ArrayTest, dotReplicate) +CXXTEST_TEST_REGISTRATION(ArrayTest, coefficientProduct) From c4592208478d196f35b3b0c03326d4efb8ebc167 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Thu, 29 Sep 2016 11:12:02 -0400 Subject: [PATCH 09/35] New method ma::math::XprBase::atan2. --- modules/math/include/openma/math/binaryop.h | 80 +++++++++++++++++++ .../include/openma/math/forwarddeclarations.h | 1 + .../math/include/openma/math/returnbyvalue.h | 32 ++++++++ modules/math/include/openma/math/xprbase.h | 7 ++ modules/math/test/c++/arrayTest.cpp | 22 +++++ 5 files changed, 142 insertions(+) diff --git a/modules/math/include/openma/math/binaryop.h b/modules/math/include/openma/math/binaryop.h index 95bea9d..16e66cc 100644 --- a/modules/math/include/openma/math/binaryop.h +++ b/modules/math/include/openma/math/binaryop.h @@ -508,6 +508,86 @@ namespace math }; }; + // ----------------------------------------------------------------------- // + // 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 adccab1..a4cf08e 100644 --- a/modules/math/include/openma/math/forwarddeclarations.h +++ b/modules/math/include/openma/math/forwarddeclarations.h @@ -72,6 +72,7 @@ namespace math 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/xprbase.h b/modules/math/include/openma/math/xprbase.h index 4979da0..957d2d9 100644 --- a/modules/math/include/openma/math/xprbase.h +++ b/modules/math/include/openma/math/xprbase.h @@ -200,6 +200,13 @@ namespace math * 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; }; // ----------------------------------------------------------------------- // diff --git a/modules/math/test/c++/arrayTest.cpp b/modules/math/test/c++/arrayTest.cpp index ef6871b..3cc6831 100644 --- a/modules/math/test/c++/arrayTest.cpp +++ b/modules/math/test/c++/arrayTest.cpp @@ -828,6 +828,27 @@ CXXTEST_SUITE(ArrayTest) 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) @@ -855,3 +876,4 @@ 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) From b0f327306332404ffa8d22816890a8cfbe2284c5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Thu, 29 Sep 2016 11:17:24 -0400 Subject: [PATCH 10/35] Computation of the ThighRotation offset for the Plug-in Gait KAD variant. --- .../body/include/openma/body/plugingait_p.h | 2 +- modules/body/src/plugingait.cpp | 119 +++++++++++++++--- .../test/c++/plugingaitTest_calibration.cpp | 31 ++++- 3 files changed, 135 insertions(+), 17 deletions(-) diff --git a/modules/body/include/openma/body/plugingait_p.h b/modules/body/include/openma/body/plugingait_p.h index 7874209..e24fa46 100644 --- a/modules/body/include/openma/body/plugingait_p.h +++ b/modules/body/include/openma/body/plugingait_p.h @@ -156,7 +156,7 @@ namespace body double RightAnkleAbAdd; double LeftAnkleAbAdd; - using CalibrateJointFuncPtr = bool (*)(math::Position* , PluginGaitPrivate* , ummp* , const std::string& , double , const math::Position* ); + using CalibrateJointFuncPtr = bool (*)(math::Position* , std::vector& , PluginGaitPrivate* , ummp* , const std::string& , double , double , const math::Position* ); CalibrateJointFuncPtr CalibrateKneeJointCentre; CalibrateJointFuncPtr CalibrateAnkleJointCentre; }; diff --git a/modules/body/src/plugingait.cpp b/modules/body/src/plugingait.cpp index 4d0d3be..07b3830 100644 --- a/modules/body/src/plugingait.cpp +++ b/modules/body/src/plugingait.cpp @@ -62,7 +62,7 @@ #ifndef DOXYGEN_SHOULD_SKIP_THIS -bool _ma_plugingait_calibrate_hjc_basic(ma::math::Position* KJC, ma::body::PluginGaitPrivate* optr, ma::body::ummp* landmarks, const std::string& prefix, double kneeWidth, const ma::math::Position* HJC) +bool _ma_plugingait_calibrate_kjc_basic(ma::math::Position* KJC, 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"]; @@ -76,31 +76,101 @@ bool _ma_plugingait_calibrate_hjc_basic(ma::math::Position* KJC, ma::body::Plugi return true; }; -bool _ma_plugingait_calibrate_hjc_kad(ma::math::Position* KJC, ma::body::PluginGaitPrivate* optr, ma::body::ummp* landmarks, const std::string& prefix, double kneeWidth, const ma::math::Position* /* HJC */) +bool _ma_plugingait_calibrate_kjc_kad(ma::math::Position* KJC, 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"]; - if (!KAX.isValid() || !KD1.isValid() || !KD2.isValid()) + const auto& ITB = (*landmarks)[prefix+"ITB"]; +#if 1 + 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 = (KD2 - KD1).cross(KAX - KD1).normalized(); + auto n = -s * (KD2 - KD1).cross(KAX - KD1).normalized(); auto I = (KD1 + KAX) / 2.; auto PP1 = (2.0 /3.0 * (I - KD2)) + KD2; ma::math::Position O = PP1 - (n * sqrt(3.0) * dist / 3.0); - auto uKAXO = (O-KAX).normalized(); + // auto uKAXO = (O-KAX).normalized(); const ma::math::Position& VLFE = O; + // std::cout << "\n" + prefix + "HJC:\n" << HJC->mean().values() << std::endl; // Compute the knee joint centre (KJC) - *KJC = VLFE + uKAXO * (optr->MarkerDiameter + kneeWidth) / 2.0; + *KJC = ma::math::compute_chord((optr->MarkerDiameter + kneeWidth) / 2.0, VLFE, *HJC, KAX); + // Compute the virtual lateral femoral epicondyle (VLFE) + // 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 v = w.cross(u); + // ma::math::Pose thigh(v.cross(w),v,w,*KJC); + // std::cout << "\n" + prefix + "Thigh AF:\n" << thigh.values() << std::endl; + // ma::math::Vector ITB_local_proj = thigh.inverse().transform(ITB); +#else + ma::math::Position mKAX = KAX.mean(); + ma::math::Position mKD1 = KD1.mean(); + ma::math::Position mKD2 = KD2.mean(); + ma::math::Position mHJC = HJC->mean(); + double mdist = ((mKAX - mKD1).norm() + (mKAX - mKD2).norm() + (mKD1 - mKD2).norm()) / sqrt(2.0) / 3.0; + auto mn = -s * (mKD2 - mKD1).cross(mKAX - mKD1).normalized(); + auto mI = (mKD1 + mKAX) / 2.; + auto mPP1 = (2.0 /3.0 * (mI - mKD2)) + mKD2; + ma::math::Position mO = mPP1 - (mn * sqrt(3.0) * mdist / 3.0); + // auto uKAXO = (O-KAX).normalized(); + const ma::math::Position& mVLFE = mO; + ma::math::Position mKJC = ma::math::compute_chord((optr->MarkerDiameter + kneeWidth) / 2.0, mVLFE, mHJC, mKAX); + std::cout << "\n" + prefix + "mKAX:\n" << mKAX.values() << std::endl; + std::cout << "\n" + prefix + "mKD1:\n" << mKD1.values() << std::endl; + std::cout << "\n" + prefix + "mKD2:\n" << mKD2.values() << std::endl; + std::cout << "\n" + prefix + "mdist:\n" << mdist << std::endl; + std::cout << "\n" + prefix + "mn:\n" << mn.values() << std::endl; + std::cout << "\n" + prefix + "mI:\n" << mI.values() << std::endl; + std::cout << "\n" + prefix + "mPP1:\n" << mPP1.values() << std::endl; + std::cout << "\n" + prefix + "mO:\n" << mO.values() << std::endl; + std::cout << "\n" + prefix + "mHJC:\n" << mHJC.values() << std::endl; + std::cout << "\n" + prefix + "mKJC:\n" << mKJC.values() << std::endl; + // Compute the thigh marker rotation offset + ma::math::Vector w = (mHJC - mKJC).normalized(); + // ma::math::Vector ITB_proj = ITB - (ITB - *KJC).dot(w).replicate<3>() * w; + // std::cout << "\n" + prefix + "ITB:\n" << ITB.values() << std::endl; + ma::math::Vector v = s * (mVLFE - mKJC).normalized(); + // ma::math::Vector v = w.cross(u); + ma::math::Pose thigh(v.cross(w),v,w,mKJC); + std::cout << "\n" + prefix + "Thigh AF:\n" << thigh.values() << std::endl; + ma::math::Vector ITB_local_proj = thigh.inverse().transform(ITB.mean()); +#endif + // ITB_local_proj.values().col(2).setZero(); + // std::cout << "\n" + prefix + "ITB_local_proj:\n" << ITB_local_proj.values() << std::endl; + // std::cout << "\n" << ITB_local_proj.values() << std::endl; + // ma::math::Vector y(ITB_local_proj.rows()); + // y.values().setZero(); y.values().col(1).setConstant(s); y.residuals().setZero(); + // ma::math::Scalar dot = ITB_local_proj.normalized().dot(y); + // ma::math::Scalar crossnorm = ITB_local_proj.normalized().cross(y).norm(); + // ma::math::Scalar res(ITB_local_proj.rows()); + // res.residuals().setZero(); + // for (int i = 0 ; i < res.rows() ; ++i) + // { + // res.values().coeffRef(i) = atan2(crossnorm.values().coeff(i), dot.values().coeff(i)); + // } + // *offsets[0] = res.mean(); + // *offsets[0] = (ITB_proj - *KJC).normalized().cross(v).norm().asin().mean(); + ma::math::Scalar dot = ITB_proj_trans_unit.dot(v); + ma::math::Scalar crossnorm = ITB_proj_trans_unit.cross(v).norm(); + // ma::math::Scalar res(ITB_proj_trans_unit.rows()); res.residuals().setZero(); + // for (int i = 0 ; i < res.rows() ; ++i) + // { + // res.values().coeffRef(i) = atan2(crossnorm.values().coeff(i), dot.values().coeff(i)); + // } + // *offsets[0] = -s * res.mean() * 180. / M_PI; + *offsets[0] = -s * crossnorm.atan2(dot).mean(); + // std::cout << "\n - " << prefix << "ThighRotationOffset: " << *offsets[0] * 180. / M_PI << std::endl; return true; }; -bool _ma_plugingait_calibrate_ajc_basic(ma::math::Position* AJC, ma::body::PluginGaitPrivate* optr, ma::body::ummp* landmarks, const std::string& prefix, double ankleWidth, const ma::math::Position* KJC) +bool _ma_plugingait_calibrate_ajc_basic(ma::math::Position* AJC, 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"]; @@ -114,7 +184,7 @@ bool _ma_plugingait_calibrate_ajc_basic(ma::math::Position* AJC, ma::body::Plugi return true; }; -bool _ma_plugingait_calibrate_ajc_kad(ma::math::Position* AJC, ma::body::PluginGaitPrivate* optr, ma::body::ummp* landmarks, const std::string& prefix, double ankleWidth, const ma::math::Position* KJC) +bool _ma_plugingait_calibrate_ajc_kad(ma::math::Position* AJC, 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"]; @@ -181,7 +251,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, + *tibialTorsion = nullptr, *thighRotationOffset = nullptr, + *shankRotationOffset = nullptr, *ankleAbAdd = nullptr; bool footFlat = false; if (side == Side::Left) { @@ -192,6 +264,10 @@ namespace body footFlat = this->LeftFootFlatEnabled; staticPlantarFlexionOffset = &(this->LeftStaticPlantarFlexionOffset); staticRotationOffset = &(this->LeftStaticRotationOffset); + tibialTorsion = &(this->LeftTibialTorsion); + thighRotationOffset = &(this->LeftThighRotationOffset); + shankRotationOffset = &(this->LeftShankRotationOffset); + ankleAbAdd = &(this->LeftAnkleAbAdd); } else if (side == Side::Right) { @@ -202,17 +278,22 @@ namespace body footFlat = this->RightFootFlatEnabled; staticPlantarFlexionOffset = &(this->RightStaticPlantarFlexionOffset); staticRotationOffset = &(this->RightStaticRotationOffset); + tibialTorsion = &(this->RightTibialTorsion); + thighRotationOffset = &(this->RightThighRotationOffset); + shankRotationOffset = &(this->RightShankRotationOffset); + ankleAbAdd = &(this->RightAnkleAbAdd); } else { error("PluginGait - Unknown side for the lower limb. Calibration aborted."); return false; } + std::vector offsets{thighRotationOffset, shankRotationOffset, tibialTorsion, ankleAbAdd}; // ----------------------------------------- // Thigh // ----------------------------------------- math::Position KJC; - if (!this->CalibrateKneeJointCentre(&KJC, this, landmarks, prefix, kneeWidth, HJC)) + if (!this->CalibrateKneeJointCentre(&KJC, offsets, this, landmarks, prefix, s, kneeWidth, HJC)) return false; // Set the segment length seglength = (KJC - *HJC).norm().mean(); @@ -229,7 +310,7 @@ namespace body // Shank // ----------------------------------------- math::Position AJC; - if (!this->CalibrateAnkleJointCentre(&AJC, this, landmarks, prefix, ankleWidth, &KJC)) + if (!this->CalibrateAnkleJointCentre(&AJC, offsets, this, landmarks, prefix, s, ankleWidth, &KJC)) return false; // Set the segment length seglength = (AJC - KJC).norm().mean(); @@ -941,12 +1022,12 @@ namespace body auto optr = this->pimpl(); if (variant == Basic) { - optr->CalibrateKneeJointCentre = &_ma_plugingait_calibrate_hjc_basic; + optr->CalibrateKneeJointCentre = &_ma_plugingait_calibrate_kjc_basic; optr->CalibrateAnkleJointCentre = &_ma_plugingait_calibrate_ajc_basic; } else { - optr->CalibrateKneeJointCentre = &_ma_plugingait_calibrate_hjc_kad; + optr->CalibrateKneeJointCentre = &_ma_plugingait_calibrate_kjc_kad; optr->CalibrateAnkleJointCentre = &_ma_plugingait_calibrate_ajc_kad; } }; @@ -1170,10 +1251,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->computeHipJointCentre(_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 @@ -1181,7 +1263,7 @@ namespace body // - Right if ((rightHJCH = this->findChild("R.HJC",{},false)) == nullptr) { - optr->computeHipJointCentre(_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 @@ -1223,6 +1305,13 @@ namespace body } if ((optr->Side & Side::Right) == Side::Right) { + // const math::Position mSC = (L_PSIS.mean() + R_PSIS.mean()) / 2.0; + // const math::Vector mv = (L_ASIS.mean() - R_ASIS.mean()).normalized(); + // const math::Vector mw = ((R_ASIS.mean() - mSC).cross(L_ASIS.mean() - mSC)).normalized(); + // const math::Pose mpelvis(mv.cross(mw), mv, mw, (L_ASIS.mean() + R_ASIS.mean()) / 2.0); + // const math::Position mHJC = mpelvis.transform(R_HJC.replicate(mpelvis.rows())); + // if (!optr->calibrateLowerLimb(Side::Right, &mHJC, &landmarks)) + // return false; const math::Position HJC = pelvis.transform(R_HJC.replicate(pelvis.rows())); if (!optr->calibrateLowerLimb(Side::Right, &HJC, &landmarks)) return false; diff --git a/modules/body/test/c++/plugingaitTest_calibration.cpp b/modules/body/test/c++/plugingaitTest_calibration.cpp index 98b9da5..5dd3915 100644 --- a/modules/body/test/c++/plugingaitTest_calibration.cpp +++ b/modules/body/test/c++/plugingaitTest_calibration.cpp @@ -330,6 +330,34 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) TS_ASSERT_EQUALS(helper.rightStaticPlantarFlexionOffset(), 0.0); TS_ASSERT_EQUALS(helper.rightStaticRotationOffset(), 0.0); }; + + CXXTEST_TEST(calibrateFullBodyFrameBasicKAD) + { + 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_Basic.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-3); + // TS_ASSERT_DELTA(helper.leftStaticPlantarFlexionOffset() * 180.0 / M_PI, 4.72487, 1e-3); + // TS_ASSERT_DELTA(helper.leftStaticRotationOffset() * 180.0 / M_PI, 0.233667, 1e-5); + TS_ASSERT_DELTA(helper.rightAsisTrochanterAPDistance(), 62.852, 1e-3); + TS_ASSERT_DELTA(helper.rightThighRotationOffset(), -10.0483 * M_PI / 180.0, 1e-3); + + // TS_ASSERT_DELTA(helper.rightStaticPlantarFlexionOffset() * 180.0 / M_PI, 3.88629, 1e-3); + // TS_ASSERT_DELTA(helper.rightStaticRotationOffset() * 180.0 / M_PI, 2.22081, 1e-3); + }; }; CXXTEST_SUITE_REGISTRATION(PluginGaitCalibrationTest) @@ -344,4 +372,5 @@ 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, calibrateFullBodyFrameBasicKAD) \ No newline at end of file From 72b3cb67b7431ecc0fe0748722ee9cdb90131002 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Thu, 29 Sep 2016 11:36:06 -0400 Subject: [PATCH 11/35] Code cleanup. --- modules/body/src/plugingait.cpp | 75 +-------------------------------- 1 file changed, 2 insertions(+), 73 deletions(-) diff --git a/modules/body/src/plugingait.cpp b/modules/body/src/plugingait.cpp index 07b3830..d9265cd 100644 --- a/modules/body/src/plugingait.cpp +++ b/modules/body/src/plugingait.cpp @@ -83,7 +83,6 @@ bool _ma_plugingait_calibrate_kjc_kad(ma::math::Position* KJC, std::vectormean().values() << std::endl; + const ma::math::Position& 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 virtual lateral femoral epicondyle (VLFE) // 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 v = w.cross(u); - // ma::math::Pose thigh(v.cross(w),v,w,*KJC); - // std::cout << "\n" + prefix + "Thigh AF:\n" << thigh.values() << std::endl; - // ma::math::Vector ITB_local_proj = thigh.inverse().transform(ITB); -#else - ma::math::Position mKAX = KAX.mean(); - ma::math::Position mKD1 = KD1.mean(); - ma::math::Position mKD2 = KD2.mean(); - ma::math::Position mHJC = HJC->mean(); - double mdist = ((mKAX - mKD1).norm() + (mKAX - mKD2).norm() + (mKD1 - mKD2).norm()) / sqrt(2.0) / 3.0; - auto mn = -s * (mKD2 - mKD1).cross(mKAX - mKD1).normalized(); - auto mI = (mKD1 + mKAX) / 2.; - auto mPP1 = (2.0 /3.0 * (mI - mKD2)) + mKD2; - ma::math::Position mO = mPP1 - (mn * sqrt(3.0) * mdist / 3.0); - // auto uKAXO = (O-KAX).normalized(); - const ma::math::Position& mVLFE = mO; - ma::math::Position mKJC = ma::math::compute_chord((optr->MarkerDiameter + kneeWidth) / 2.0, mVLFE, mHJC, mKAX); - std::cout << "\n" + prefix + "mKAX:\n" << mKAX.values() << std::endl; - std::cout << "\n" + prefix + "mKD1:\n" << mKD1.values() << std::endl; - std::cout << "\n" + prefix + "mKD2:\n" << mKD2.values() << std::endl; - std::cout << "\n" + prefix + "mdist:\n" << mdist << std::endl; - std::cout << "\n" + prefix + "mn:\n" << mn.values() << std::endl; - std::cout << "\n" + prefix + "mI:\n" << mI.values() << std::endl; - std::cout << "\n" + prefix + "mPP1:\n" << mPP1.values() << std::endl; - std::cout << "\n" + prefix + "mO:\n" << mO.values() << std::endl; - std::cout << "\n" + prefix + "mHJC:\n" << mHJC.values() << std::endl; - std::cout << "\n" + prefix + "mKJC:\n" << mKJC.values() << std::endl; - // Compute the thigh marker rotation offset - ma::math::Vector w = (mHJC - mKJC).normalized(); - // ma::math::Vector ITB_proj = ITB - (ITB - *KJC).dot(w).replicate<3>() * w; - // std::cout << "\n" + prefix + "ITB:\n" << ITB.values() << std::endl; - ma::math::Vector v = s * (mVLFE - mKJC).normalized(); - // ma::math::Vector v = w.cross(u); - ma::math::Pose thigh(v.cross(w),v,w,mKJC); - std::cout << "\n" + prefix + "Thigh AF:\n" << thigh.values() << std::endl; - ma::math::Vector ITB_local_proj = thigh.inverse().transform(ITB.mean()); -#endif - // ITB_local_proj.values().col(2).setZero(); - // std::cout << "\n" + prefix + "ITB_local_proj:\n" << ITB_local_proj.values() << std::endl; - // std::cout << "\n" << ITB_local_proj.values() << std::endl; - // ma::math::Vector y(ITB_local_proj.rows()); - // y.values().setZero(); y.values().col(1).setConstant(s); y.residuals().setZero(); - // ma::math::Scalar dot = ITB_local_proj.normalized().dot(y); - // ma::math::Scalar crossnorm = ITB_local_proj.normalized().cross(y).norm(); - // ma::math::Scalar res(ITB_local_proj.rows()); - // res.residuals().setZero(); - // for (int i = 0 ; i < res.rows() ; ++i) - // { - // res.values().coeffRef(i) = atan2(crossnorm.values().coeff(i), dot.values().coeff(i)); - // } - // *offsets[0] = res.mean(); - // *offsets[0] = (ITB_proj - *KJC).normalized().cross(v).norm().asin().mean(); ma::math::Scalar dot = ITB_proj_trans_unit.dot(v); ma::math::Scalar crossnorm = ITB_proj_trans_unit.cross(v).norm(); - // ma::math::Scalar res(ITB_proj_trans_unit.rows()); res.residuals().setZero(); - // for (int i = 0 ; i < res.rows() ; ++i) - // { - // res.values().coeffRef(i) = atan2(crossnorm.values().coeff(i), dot.values().coeff(i)); - // } - // *offsets[0] = -s * res.mean() * 180. / M_PI; *offsets[0] = -s * crossnorm.atan2(dot).mean(); - // std::cout << "\n - " << prefix << "ThighRotationOffset: " << *offsets[0] * 180. / M_PI << std::endl; return true; }; @@ -1305,13 +1241,6 @@ namespace body } if ((optr->Side & Side::Right) == Side::Right) { - // const math::Position mSC = (L_PSIS.mean() + R_PSIS.mean()) / 2.0; - // const math::Vector mv = (L_ASIS.mean() - R_ASIS.mean()).normalized(); - // const math::Vector mw = ((R_ASIS.mean() - mSC).cross(L_ASIS.mean() - mSC)).normalized(); - // const math::Pose mpelvis(mv.cross(mw), mv, mw, (L_ASIS.mean() + R_ASIS.mean()) / 2.0); - // const math::Position mHJC = mpelvis.transform(R_HJC.replicate(mpelvis.rows())); - // if (!optr->calibrateLowerLimb(Side::Right, &mHJC, &landmarks)) - // return false; const math::Position HJC = pelvis.transform(R_HJC.replicate(pelvis.rows())); if (!optr->calibrateLowerLimb(Side::Right, &HJC, &landmarks)) return false; From 0bb45a5d284c28cf5e1a7df5b6f85994a0a21e3b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Thu, 29 Sep 2016 21:01:33 -0400 Subject: [PATCH 12/35] Computation of the shank marker rotation offset done. --- modules/body/src/plugingait.cpp | 12 ++++++++++-- modules/body/test/c++/plugingaitTest_calibration.cpp | 3 ++- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/modules/body/src/plugingait.cpp b/modules/body/src/plugingait.cpp index d9265cd..b98e32c 100644 --- a/modules/body/src/plugingait.cpp +++ b/modules/body/src/plugingait.cpp @@ -120,17 +120,25 @@ bool _ma_plugingait_calibrate_ajc_basic(ma::math::Position* AJC, std::vector& /* offsets */, ma::body::PluginGaitPrivate* optr, ma::body::ummp* landmarks, const std::string& prefix, double /* s */, double ankleWidth, const ma::math::Position* KJC) +bool _ma_plugingait_calibrate_ajc_kad(ma::math::Position* AJC, 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"]; - if (!LTM.isValid() || !KAX.isValid()) + 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::math::Vector w = (*KJC - *AJC).normalized(); + ma::math::Vector LS_proj_trans_unit = ((LS - (LS - *AJC).dot(w).replicate<3>() * w) - *AJC).normalized(); + ma::math::Vector v = (LTM - *AJC).normalized(); + ma::math::Scalar dot = LS_proj_trans_unit.dot(v); + ma::math::Scalar crossnorm = LS_proj_trans_unit.cross(v).norm(); + *offsets[1] = crossnorm.atan2(dot).mean(); return true; }; diff --git a/modules/body/test/c++/plugingaitTest_calibration.cpp b/modules/body/test/c++/plugingaitTest_calibration.cpp index 5dd3915..bddf8bd 100644 --- a/modules/body/test/c++/plugingaitTest_calibration.cpp +++ b/modules/body/test/c++/plugingaitTest_calibration.cpp @@ -350,11 +350,12 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) 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-3); + TS_ASSERT_DELTA(helper.leftShankRotationOffset(), 13.8726 * M_PI / 180.0, 1e-3); // TS_ASSERT_DELTA(helper.leftStaticPlantarFlexionOffset() * 180.0 / M_PI, 4.72487, 1e-3); // TS_ASSERT_DELTA(helper.leftStaticRotationOffset() * 180.0 / M_PI, 0.233667, 1e-5); TS_ASSERT_DELTA(helper.rightAsisTrochanterAPDistance(), 62.852, 1e-3); TS_ASSERT_DELTA(helper.rightThighRotationOffset(), -10.0483 * M_PI / 180.0, 1e-3); - + TS_ASSERT_DELTA(helper.rightShankRotationOffset(), 15.3639 * M_PI / 180.0, 1e-3); // TS_ASSERT_DELTA(helper.rightStaticPlantarFlexionOffset() * 180.0 / M_PI, 3.88629, 1e-3); // TS_ASSERT_DELTA(helper.rightStaticRotationOffset() * 180.0 / M_PI, 2.22081, 1e-3); }; From 749824e69f723acc7c0120dda781eb4b9785d7a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Thu, 29 Sep 2016 21:36:08 -0400 Subject: [PATCH 13/35] Comparison for the foot offset angles activated. --- modules/body/test/c++/plugingaitTest_calibration.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/modules/body/test/c++/plugingaitTest_calibration.cpp b/modules/body/test/c++/plugingaitTest_calibration.cpp index bddf8bd..6ffc43e 100644 --- a/modules/body/test/c++/plugingaitTest_calibration.cpp +++ b/modules/body/test/c++/plugingaitTest_calibration.cpp @@ -351,13 +351,13 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) TS_ASSERT_DELTA(helper.leftAsisTrochanterAPDistance(), 62.208, 1e-3); TS_ASSERT_DELTA(helper.leftThighRotationOffset(), 8.9585 * M_PI / 180.0, 1e-3); TS_ASSERT_DELTA(helper.leftShankRotationOffset(), 13.8726 * M_PI / 180.0, 1e-3); - // TS_ASSERT_DELTA(helper.leftStaticPlantarFlexionOffset() * 180.0 / M_PI, 4.72487, 1e-3); - // TS_ASSERT_DELTA(helper.leftStaticRotationOffset() * 180.0 / M_PI, 0.233667, 1e-5); + TS_ASSERT_DELTA(helper.leftStaticPlantarFlexionOffset(), 4.72487 * M_PI / 180.0, 1e-3); + TS_ASSERT_DELTA(helper.leftStaticRotationOffset(), 0.233667 * 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-3); TS_ASSERT_DELTA(helper.rightShankRotationOffset(), 15.3639 * M_PI / 180.0, 1e-3); - // TS_ASSERT_DELTA(helper.rightStaticPlantarFlexionOffset() * 180.0 / M_PI, 3.88629, 1e-3); - // TS_ASSERT_DELTA(helper.rightStaticRotationOffset() * 180.0 / M_PI, 2.22081, 1e-3); + TS_ASSERT_DELTA(helper.rightStaticPlantarFlexionOffset(), 3.88629 * M_PI / 180.0, 1e-3); + TS_ASSERT_DELTA(helper.rightStaticRotationOffset(), 2.22081 * M_PI / 180.0, 1e-3); }; }; From a30494fc08242b5faa65f97688524e543e25dfcb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Thu, 27 Oct 2016 21:59:00 -0400 Subject: [PATCH 14/35] Adaptation of the chord function for the KAD. --- .../body/include/openma/body/plugingait_p.h | 82 +++++++++++++++++-- 1 file changed, 76 insertions(+), 6 deletions(-) diff --git a/modules/body/include/openma/body/plugingait_p.h b/modules/body/include/openma/body/plugingait_p.h index e24fa46..8955351 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 @@ -366,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()); @@ -392,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); @@ -402,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))) @@ -412,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); }; }; }; From 86b4351f3cb8aec92a83d1b5de9e11b35ac1db58 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Thu, 27 Oct 2016 22:19:39 -0400 Subject: [PATCH 15/35] Integration of the thigh and shank offsets in the computation of the SCSs. --- modules/body/src/plugingait.cpp | 34 +++++++++++--- .../test/c++/plugingaitTest_kinematics.cpp | 47 ++++++++++++++++++- .../c++/plugingaitTest_reconstruction.cpp | 33 ++++++++++++- 3 files changed, 106 insertions(+), 8 deletions(-) diff --git a/modules/body/src/plugingait.cpp b/modules/body/src/plugingait.cpp index b98e32c..6af3429 100644 --- a/modules/body/src/plugingait.cpp +++ b/modules/body/src/plugingait.cpp @@ -418,6 +418,7 @@ namespace body auto pptr = this->pint(); std::string prefix; double s = 0.0, ankleWidth = 0.0, kneeWidth = 0.0, + thighRotationOffset = 0.0, shankRotationOffset = 0.0, staticPlantarFlexionOffset = 0.0, staticRotationOffset = 0.0; if (side == Side::Left) { @@ -425,6 +426,8 @@ namespace body s = -1.0; ankleWidth = this->LeftAnkleWidth; kneeWidth = this->LeftKneeWidth; + thighRotationOffset = -this->LeftThighRotationOffset; + shankRotationOffset = -this->LeftShankRotationOffset; staticPlantarFlexionOffset = -this->LeftStaticPlantarFlexionOffset; staticRotationOffset = -this->LeftStaticRotationOffset; } @@ -434,6 +437,8 @@ namespace body s = 1.0; ankleWidth = this->RightAnkleWidth; kneeWidth = this->RightKneeWidth; + thighRotationOffset = this->RightThighRotationOffset; + shankRotationOffset = this->RightShankRotationOffset; staticPlantarFlexionOffset = -this->RightStaticPlantarFlexionOffset; staticRotationOffset = this->RightStaticRotationOffset; } @@ -458,10 +463,18 @@ 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(); + const math::Position KJC = compute_chord((this->MarkerDiameter + kneeWidth) / 2.0, LFE, *HJC, ITB, thighRotationOffset); w = (*HJC - KJC).normalized(); - v = w.cross(u); + if (this->Variant == PluginGait::Basic) + { + u = s * (*HJC - LFE).cross(ITB - LFE).normalized(); + v = w.cross(u); + } + else + { + v = -s * (LFE - KJC).normalized(); + u = v.cross(w); + } 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); @@ -478,10 +491,19 @@ 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); + const math::Position AJC = compute_chord((this->MarkerDiameter + ankleWidth) / 2.0, LTM, KJC, LS, shankRotationOffset); w = (KJC - AJC).normalized(); - u = s * w.cross(LS - AJC).normalized(); - math::Vector v_shank = w.cross(u); + math::Vector v_shank; + if (this->Variant == PluginGait::Basic) + { + u = s * w.cross(LS - AJC).normalized(); + v_shank = w.cross(u); + } + else + { + v_shank = -s * (LTM - AJC).normalized(); + u = v_shank.cross(w); + } math::to_timesequence(u, 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); diff --git a/modules/body/test/c++/plugingaitTest_kinematics.cpp b/modules/body/test/c++/plugingaitTest_kinematics.cpp index aaa9b5f..a1a76d0 100644 --- a/modules/body/test/c++/plugingaitTest_kinematics.cpp +++ b/modules/body/test/c++/plugingaitTest_kinematics.cpp @@ -460,6 +460,50 @@ 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(kinematicsFullBodyFrameBasicKAD) + { + 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_Basic.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_Basic.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", {{3e-3,1e-4,2.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", {{3.5e-2,1.5e-2,5.4e-2}}); + 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", {{3.2e-2,1e-2,6e-2}}); + compare_joint_kinematics(kinematics, trial, "L.Foot.Progress.Angle", "LFootProgressAngles", {{2.7e-4,3.5e-4,1.75e-3}}); + }; }; CXXTEST_SUITE_REGISTRATION(PluginGaitKinematicsTest) @@ -471,4 +515,5 @@ 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, kinematicsFullBodyFrameBasicKAD) diff --git a/modules/body/test/c++/plugingaitTest_reconstruction.cpp b/modules/body/test/c++/plugingaitTest_reconstruction.cpp index 23f8814..507386c 100644 --- a/modules/body/test/c++/plugingaitTest_reconstruction.cpp +++ b/modules/body/test/c++/plugingaitTest_reconstruction.cpp @@ -340,6 +340,35 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) TS_ASSERT_DELTA(progression->data(i,12), 0.0, 1e-15); } }; + + CXXTEST_TEST(reconstructFullBodyFrameBasicKAD) + { + 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_Basic.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_Basic.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"}, {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"}, {7.5e-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,1e-3,1e-3,1e-3}); + compare_segment_motion(model, trial, "L.Foot.SCS", {"LFOO","LFOA","LFOL","LFOP"}, {1e4,1e-3,1e-3,1e-3}); + } }; CXXTEST_SUITE_REGISTRATION(PluginGaitReconstructionTest) @@ -351,4 +380,6 @@ 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, reconstructFullBodyFrameBasicKAD) + \ No newline at end of file From 798c83ad99b8324c3ba33e5c4264e4045f25ec1e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Thu, 27 Oct 2016 23:46:19 -0400 Subject: [PATCH 16/35] Unit tests added for the model PiG + KAD + foot flat options activated. --- .../test/c++/plugingaitTest_calibration.cpp | 35 +++++++++++++- .../test/c++/plugingaitTest_kinematics.cpp | 47 +++++++++++++++++++ .../c++/plugingaitTest_reconstruction.cpp | 35 +++++++++++++- 3 files changed, 114 insertions(+), 3 deletions(-) diff --git a/modules/body/test/c++/plugingaitTest_calibration.cpp b/modules/body/test/c++/plugingaitTest_calibration.cpp index 6ffc43e..cb35d78 100644 --- a/modules/body/test/c++/plugingaitTest_calibration.cpp +++ b/modules/body/test/c++/plugingaitTest_calibration.cpp @@ -359,6 +359,38 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) TS_ASSERT_DELTA(helper.rightStaticPlantarFlexionOffset(), 3.88629 * M_PI / 180.0, 1e-3); TS_ASSERT_DELTA(helper.rightStaticRotationOffset(), 2.22081 * M_PI / 180.0, 1e-3); }; + + CXXTEST_TEST(calibrateFullBodyFrameFootFlatKAD) + { + 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-4); + TS_ASSERT_DELTA(helper.rightAsisTrochanterAPDistance(), 62.852, 1e-3); + 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-3); + TS_ASSERT_DELTA(helper.rightStaticRotationOffset(), 0.0345916, 2e-3); + }; }; CXXTEST_SUITE_REGISTRATION(PluginGaitCalibrationTest) @@ -374,4 +406,5 @@ CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrate3BothLowerBodyFF) CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrate3BothLowerBodyFF_N18) CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrate3BothLowerBodynoFF) CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrate2BothUpperBodyHeadOffsetDisabled) -CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyFrameBasicKAD) \ No newline at end of file +CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyFrameBasicKAD) +CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyFrameFootFlatKAD) \ 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 a1a76d0..d59a1e4 100644 --- a/modules/body/test/c++/plugingaitTest_kinematics.cpp +++ b/modules/body/test/c++/plugingaitTest_kinematics.cpp @@ -504,6 +504,52 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) compare_joint_kinematics(kinematics, trial, "R.Foot.Progress.Angle", "RFootProgressAngles", {{3.2e-2,1e-2,6e-2}}); compare_joint_kinematics(kinematics, trial, "L.Foot.Progress.Angle", "LFootProgressAngles", {{2.7e-4,3.5e-4,1.75e-3}}); }; + + CXXTEST_TEST(kinematicsFullBodyFrameFootFlatKAD) + { + 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", {{3e-3,1e-4,2.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,6e-4,1.11e-3}}); + compare_joint_kinematics(kinematics, trial, "R.Ankle.Angle", "RAnkleAngles", {{3.51e-2,1.5e-2,9.7e-2}}); + 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", {{3.2e-2,1e-2,1.1e-1}}); + compare_joint_kinematics(kinematics, trial, "L.Foot.Progress.Angle", "LFootProgressAngles", {{2.7e-4,3.5e-4,1.75e-3}}); + }; }; CXXTEST_SUITE_REGISTRATION(PluginGaitKinematicsTest) @@ -517,3 +563,4 @@ CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematics3BothLowerBodyFF_N CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematics3BothLowerBodyNoFF) CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothFullBodyFullFramesHeadOffsetDisabled) CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsFullBodyFrameBasicKAD) +CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsFullBodyFrameFootFlatKAD) diff --git a/modules/body/test/c++/plugingaitTest_reconstruction.cpp b/modules/body/test/c++/plugingaitTest_reconstruction.cpp index 507386c..3f620a3 100644 --- a/modules/body/test/c++/plugingaitTest_reconstruction.cpp +++ b/modules/body/test/c++/plugingaitTest_reconstruction.cpp @@ -368,7 +368,38 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) 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,1e-3,1e-3,1e-3}); compare_segment_motion(model, trial, "L.Foot.SCS", {"LFOO","LFOA","LFOL","LFOP"}, {1e4,1e-3,1e-3,1e-3}); - } + }; + + CXXTEST_TEST(reconstructFullBodyFrameFootFlatKAD) + { + 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"}, {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"}, {7.5e-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,1e-3,2e-3,2e-3}); + compare_segment_motion(model, trial, "L.Foot.SCS", {"LFOO","LFOA","LFOL","LFOP"}, {1e4,1e-3,1e-3,1e-3}); + }; }; CXXTEST_SUITE_REGISTRATION(PluginGaitReconstructionTest) @@ -382,4 +413,4 @@ CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstruct3BothLowerBod CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstruct3BothLowerBodyNoFF) CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstruct2BothUpperBodyHeadOffsetDisabled) CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyFrameBasicKAD) - \ No newline at end of file +CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyFrameFootFlatKAD) \ No newline at end of file From 343f9dc91867bd43f324410089eb3c7c1550466f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Wed, 2 Nov 2016 12:39:52 -0400 Subject: [PATCH 17/35] Suffix 'Offset' added to the members '*TibialTorsion' and '*AnkleAbAdd' and associated methods. --- modules/body/include/openma/body/plugingait.h | 12 +-- .../body/include/openma/body/plugingait_p.h | 16 ++-- modules/body/src/plugingait.cpp | 80 +++++++++---------- modules/body/swig/body/plugingait.i | 12 +-- 4 files changed, 60 insertions(+), 60 deletions(-) diff --git a/modules/body/include/openma/body/plugingait.h b/modules/body/include/openma/body/plugingait.h index 687590d..3944a9b 100644 --- a/modules/body/include/openma/body/plugingait.h +++ b/modules/body/include/openma/body/plugingait.h @@ -85,10 +85,10 @@ namespace body void setRightAsisTrochanterAPDistance(double value) _OPENMA_NOEXCEPT; double leftAsisTrochanterAPDistance() const _OPENMA_NOEXCEPT; void setLeftAsisTrochanterAPDistance(double value) _OPENMA_NOEXCEPT; - double rightTibialTorsion() const _OPENMA_NOEXCEPT; - void setRightTibialTorsion(double value) _OPENMA_NOEXCEPT; - double leftTibialTorsion() const _OPENMA_NOEXCEPT; - void setLeftTibialTorsion(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; @@ -114,8 +114,8 @@ namespace body double rightStaticRotationOffset() const _OPENMA_NOEXCEPT; double leftStaticPlantarFlexionOffset() const _OPENMA_NOEXCEPT; double leftStaticRotationOffset() const _OPENMA_NOEXCEPT; - double rightAnkleAbAdd() const _OPENMA_NOEXCEPT; - double leftAnkleAbAdd() 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 8955351..dfae675 100644 --- a/modules/body/include/openma/body/plugingait_p.h +++ b/modules/body/include/openma/body/plugingait_p.h @@ -87,8 +87,8 @@ namespace body Property {"leftLegLength"}, Property {"rightAsisTrochanterAPDistance"}, Property {"leftAsisTrochanterAPDistance"}, - Property {"rightTibialTorsion"}, - Property {"leftTibialTorsion"}, + Property {"rightTibialTorsionOffset"}, + Property {"leftTibialTorsionOffset"}, Property {"rightThighRotationOffset"}, Property {"leftThighRotationOffset"}, Property {"rightShankRotationOffset"}, @@ -105,8 +105,8 @@ namespace body Property {"rightStaticRotationOffset"}, Property {"leftStaticPlantarFlexionOffset"}, Property {"leftStaticRotationOffset"}, - Property {"rightAnkleAbAdd"}, - Property {"leftAnkleAbAdd"} + Property {"rightAnkleAbAddOffset"}, + Property {"leftAnkleAbAddOffset"} ) public: @@ -138,8 +138,8 @@ namespace body double LeftLegLength; double RightAsisTrochanterAPDistance; double LeftAsisTrochanterAPDistance; - double RightTibialTorsion; - double LeftTibialTorsion; + double RightTibialTorsionOffset; + double LeftTibialTorsionOffset; double RightThighRotationOffset; double LeftThighRotationOffset; double RightShankRotationOffset; @@ -156,8 +156,8 @@ namespace body double RightStaticRotationOffset; double LeftStaticPlantarFlexionOffset; double LeftStaticRotationOffset; - double RightAnkleAbAdd; - double LeftAnkleAbAdd; + double RightAnkleAbAddOffset; + double LeftAnkleAbAddOffset; using CalibrateJointFuncPtr = bool (*)(math::Position* , std::vector& , PluginGaitPrivate* , ummp* , const std::string& , double , double , const math::Position* ); CalibrateJointFuncPtr CalibrateKneeJointCentre; diff --git a/modules/body/src/plugingait.cpp b/modules/body/src/plugingait.cpp index 6af3429..8738e73 100644 --- a/modules/body/src/plugingait.cpp +++ b/modules/body/src/plugingait.cpp @@ -165,8 +165,8 @@ namespace body LeftLegLength(0.0), RightAsisTrochanterAPDistance(0.0), LeftAsisTrochanterAPDistance(0.0), - RightTibialTorsion(0.0), - LeftTibialTorsion(0.0), + RightTibialTorsionOffset(0.0), + LeftTibialTorsionOffset(0.0), RightThighRotationOffset(0.0), LeftThighRotationOffset(0.0), RightShankRotationOffset(0.0), @@ -182,8 +182,8 @@ namespace body RightStaticRotationOffset(0.0), LeftStaticPlantarFlexionOffset(0.0), LeftStaticRotationOffset(0.0), - RightAnkleAbAdd(0.0), - LeftAnkleAbAdd(0.0), + RightAnkleAbAddOffset(0.0), + LeftAnkleAbAddOffset(0.0), CalibrateKneeJointCentre(nullptr), CalibrateAnkleJointCentre(nullptr) {}; @@ -208,10 +208,10 @@ namespace body footFlat = this->LeftFootFlatEnabled; staticPlantarFlexionOffset = &(this->LeftStaticPlantarFlexionOffset); staticRotationOffset = &(this->LeftStaticRotationOffset); - tibialTorsion = &(this->LeftTibialTorsion); + tibialTorsionOffset = &(this->LeftTibialTorsionOffset); thighRotationOffset = &(this->LeftThighRotationOffset); shankRotationOffset = &(this->LeftShankRotationOffset); - ankleAbAdd = &(this->LeftAnkleAbAdd); + ankleAbAddOffset = &(this->LeftAnkleAbAddOffset); } else if (side == Side::Right) { @@ -222,17 +222,17 @@ namespace body footFlat = this->RightFootFlatEnabled; staticPlantarFlexionOffset = &(this->RightStaticPlantarFlexionOffset); staticRotationOffset = &(this->RightStaticRotationOffset); - tibialTorsion = &(this->RightTibialTorsion); + tibialTorsionOffset = &(this->RightTibialTorsionOffset); thighRotationOffset = &(this->RightThighRotationOffset); shankRotationOffset = &(this->RightShankRotationOffset); - ankleAbAdd = &(this->RightAnkleAbAdd); + ankleAbAddOffset = &(this->RightAnkleAbAddOffset); } else { error("PluginGait - Unknown side for the lower limb. Calibration aborted."); return false; } - std::vector offsets{thighRotationOffset, shankRotationOffset, tibialTorsion, ankleAbAdd}; + std::vector offsets{thighRotationOffset, shankRotationOffset, tibialTorsionOffset, ankleAbAddOffset}; // ----------------------------------------- // Thigh // ----------------------------------------- @@ -883,14 +883,14 @@ namespace body 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. By default, this property contains the value 0.0. - * @sa rightTibialTorsion() setRightTibialTorsion() + * @sa rightTibialTorsionOffset() setRightTibialTorsionOffset() */ - double RightTibialTorsion; + 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. By default, this property contains the value 0.0. - * @sa leftTibialTorsion() setLeftTibialTorsion() + * @sa leftTibialTorsionOffset() setLeftTibialTorsionOffset() */ - double LeftTibialTorsion; + 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() @@ -968,14 +968,14 @@ namespace body 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 rightAnkleAbAdd() + * @sa rightAnkleAbAddOffset() */ - double RightAnkleAbAdd; + 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 leftAnkleAbAdd() + * @sa leftAnkleAbAddOffset() */ - double LeftAnkleAbAdd; + double LeftAnkleAbAddOffset; }; #endif @@ -1914,44 +1914,44 @@ namespace body }; /** - * Returns the internal parameter RightTibialTorsion. + * Returns the internal parameter RightTibialTorsionOffset. */ - double PluginGait::rightTibialTorsion() const _OPENMA_NOEXCEPT + double PluginGait::rightTibialTorsionOffset() const _OPENMA_NOEXCEPT { auto optr = this->pimpl(); - return optr->RightTibialTorsion; + return optr->RightTibialTorsionOffset; }; /** * Sets the internal parameter RightTibialTorsion. */ - void PluginGait::setRightTibialTorsion(double value) _OPENMA_NOEXCEPT + void PluginGait::setRightTibialTorsionOffset(double value) _OPENMA_NOEXCEPT { auto optr = this->pimpl(); - if (fabs(value - optr->RightTibialTorsion) < std::numeric_limits::epsilon()) + if (fabs(value - optr->RightTibialTorsionOffset) < std::numeric_limits::epsilon()) return; - optr->RightTibialTorsion = value; + optr->RightTibialTorsionOffset = value; this->modified(); }; /** - * Returns the internal parameter LeftTibialTorsion. + * Returns the internal parameter LeftTibialTorsionOffset. */ - double PluginGait::leftTibialTorsion() const _OPENMA_NOEXCEPT + double PluginGait::leftTibialTorsionOffset() const _OPENMA_NOEXCEPT { auto optr = this->pimpl(); - return optr->LeftTibialTorsion; + return optr->LeftTibialTorsionOffset; }; /** - * Sets the internal parameter LeftTibialTorsion. + * Sets the internal parameter LeftTibialTorsionOffset. */ - void PluginGait::setLeftTibialTorsion(double value) _OPENMA_NOEXCEPT + void PluginGait::setLeftTibialTorsionOffset(double value) _OPENMA_NOEXCEPT { auto optr = this->pimpl(); - if (fabs(value - optr->LeftTibialTorsion) < std::numeric_limits::epsilon()) + if (fabs(value - optr->LeftTibialTorsionOffset) < std::numeric_limits::epsilon()) return; - optr->LeftTibialTorsion = value; + optr->LeftTibialTorsionOffset = value; this->modified(); }; @@ -2202,21 +2202,21 @@ namespace body }; /** - * Returns the internal parameter RightAnkleAbAdd. + * Returns the internal parameter RightAnkleAbAddOffset. */ - double PluginGait::rightAnkleAbAdd() const _OPENMA_NOEXCEPT + double PluginGait::rightAnkleAbAddOffset() const _OPENMA_NOEXCEPT { auto optr = this->pimpl(); - return optr->RightAnkleAbAdd; + return optr->RightAnkleAbAddOffset; }; /** - * Returns the internal parameter LeftAnkleAbAdd. + * Returns the internal parameter LeftAnkleAbAddOffset. */ - double PluginGait::leftAnkleAbAdd() const _OPENMA_NOEXCEPT + double PluginGait::leftAnkleAbAddOffset() const _OPENMA_NOEXCEPT { auto optr = this->pimpl(); - return optr->LeftAnkleAbAdd; + return optr->LeftAnkleAbAddOffset; }; // ----------------------------------------------------------------------- // @@ -2379,8 +2379,8 @@ namespace body optr->LeftLegLength = optr_src->LeftLegLength; optr->RightAsisTrochanterAPDistance = optr_src->RightAsisTrochanterAPDistance; optr->LeftAsisTrochanterAPDistance = optr_src->LeftAsisTrochanterAPDistance; - optr->RightTibialTorsion = optr_src->RightTibialTorsion; - optr->LeftTibialTorsion = optr_src->LeftTibialTorsion; + 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; @@ -2396,8 +2396,8 @@ namespace body optr->RightStaticRotationOffset = optr_src->RightStaticRotationOffset; optr->LeftStaticPlantarFlexionOffset = optr_src->LeftStaticPlantarFlexionOffset; optr->LeftStaticRotationOffset = optr_src->LeftStaticRotationOffset; - optr->RightAnkleAbAdd = optr_src->RightAnkleAbAdd; - optr->LeftAnkleAbAdd = optr_src->LeftAnkleAbAdd; + 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 b596cb9..7b7ae73 100644 --- a/modules/body/swig/body/plugingait.i +++ b/modules/body/swig/body/plugingait.i @@ -84,10 +84,10 @@ namespace body void setRightAsisTrochanterAPDistance(double value); double leftAsisTrochanterAPDistance() const; void setLeftAsisTrochanterAPDistance(double value); - double rightTibialTorsion() const; - void setRightTibialTorsion(double value); - double leftTibialTorsion() const; - void setLeftTibialTorsion(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; @@ -113,8 +113,8 @@ namespace body double rightStaticRotationOffset() const; double leftStaticPlantarFlexionOffset() const; double leftStaticRotationOffset() const; - double rightAnkleAbAdd() const; - double leftAnkleAbAdd() const; + double rightAnkleAbAddOffset() const; + double leftAnkleAbAddOffset() const; }; %clearnodefaultctor; }; From 33eddb830ab8887ec8d6b9aa8fc47d138583fb92 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Wed, 2 Nov 2016 12:42:37 -0400 Subject: [PATCH 18/35] Use of function pointers to reconstruct segments (thigh and shank) instead of conditions. --- .../body/include/openma/body/plugingait_p.h | 4 ++ modules/body/src/plugingait.cpp | 58 ++++++++++++------- 2 files changed, 40 insertions(+), 22 deletions(-) diff --git a/modules/body/include/openma/body/plugingait_p.h b/modules/body/include/openma/body/plugingait_p.h index dfae675..8968883 100644 --- a/modules/body/include/openma/body/plugingait_p.h +++ b/modules/body/include/openma/body/plugingait_p.h @@ -162,6 +162,10 @@ namespace body using CalibrateJointFuncPtr = bool (*)(math::Position* , std::vector& , PluginGaitPrivate* , ummp* , const std::string& , double , double , const math::Position* ); CalibrateJointFuncPtr CalibrateKneeJointCentre; CalibrateJointFuncPtr CalibrateAnkleJointCentre; + + using ConstructSegmentPosePtr = void (*)(math::Vector* , math::Vector* , math::Vector* , const math::Map* , const math::Map* , const math::Position* , const math::Position* , double ); + ConstructSegmentPosePtr ConstructThighPose; + ConstructSegmentPosePtr ConstructShankPose; }; inline void PluginGaitPrivate::computeHipJointCentre(double* HJC, double S, double C, double xdis) const _OPENMA_NOEXCEPT diff --git a/modules/body/src/plugingait.cpp b/modules/body/src/plugingait.cpp index 8738e73..69368ca 100644 --- a/modules/body/src/plugingait.cpp +++ b/modules/body/src/plugingait.cpp @@ -141,6 +141,33 @@ bool _ma_plugingait_calibrate_ajc_kad(ma::math::Position* AJC, std::vector* ITB, const ma::math::Map* LFE, const ma::math::Position* HJC, const ma::math::Position* KJC, double s) +{ + *w = (*HJC - *KJC).normalized(); + *u = s * (*HJC - *LFE).cross(*ITB - *LFE).normalized(); + *v = w->cross(*u); +}; + +void _ma_plugingait_construct_thigh_pose_kad(ma::math::Vector* u, ma::math::Vector* v, ma::math::Vector* w, const ma::math::Map* /*ITB*/, const ma::math::Map* LFE, const ma::math::Position* HJC, const ma::math::Position* KJC, double s) +{ + *w = (*HJC - *KJC).normalized(); + *v = -s * (*LFE - *KJC).normalized(); + *u = v->cross(*w); +} + +void _ma_plugingait_construct_shank_pose_basic(ma::math::Vector* u, ma::math::Vector* v, ma::math::Vector* w, const ma::math::Map* LS, 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(*LS - *AJC).normalized(); + *v = w->cross(*u); +}; + +void _ma_plugingait_construct_shank_pose_kad(ma::math::Vector* u, ma::math::Vector* v, ma::math::Vector* w, const ma::math::Map* /*LS*/, const ma::math::Map* LTM, const ma::math::Position* KJC, const ma::math::Position* AJC, double s) +{ + *w = (*KJC - *AJC).normalized(); + *v = -s * (*LTM - *AJC).normalized(); + *u = v->cross(*w); +}; namespace ma { @@ -185,7 +212,9 @@ namespace body RightAnkleAbAddOffset(0.0), LeftAnkleAbAddOffset(0.0), CalibrateKneeJointCentre(nullptr), - CalibrateAnkleJointCentre(nullptr) + CalibrateAnkleJointCentre(nullptr), + ConstructThighPose(nullptr), + ConstructShankPose(nullptr) {}; PluginGaitPrivate::~PluginGaitPrivate() _OPENMA_NOEXCEPT = default; @@ -464,17 +493,7 @@ namespace body } seg = model->segments()->findChild({},{{"side",side},{"part",Part::Thigh}},false); const math::Position KJC = compute_chord((this->MarkerDiameter + kneeWidth) / 2.0, LFE, *HJC, ITB, thighRotationOffset); - w = (*HJC - KJC).normalized(); - if (this->Variant == PluginGait::Basic) - { - u = s * (*HJC - LFE).cross(ITB - LFE).normalized(); - v = w.cross(u); - } - else - { - v = -s * (LFE - KJC).normalized(); - u = v.cross(w); - } + this->ConstructThighPose(&u, &v, &w, &ITB, &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); @@ -494,16 +513,7 @@ namespace body const math::Position AJC = compute_chord((this->MarkerDiameter + ankleWidth) / 2.0, LTM, KJC, LS, shankRotationOffset); w = (KJC - AJC).normalized(); math::Vector v_shank; - if (this->Variant == PluginGait::Basic) - { - u = s * w.cross(LS - AJC).normalized(); - v_shank = w.cross(u); - } - else - { - v_shank = -s * (LTM - AJC).normalized(); - u = v_shank.cross(w); - } + this->ConstructShankPose(&u, &v_shank, &w, &LS, <M, &KJC, &AJC, s); math::to_timesequence(u, 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); @@ -990,11 +1000,15 @@ namespace body { optr->CalibrateKneeJointCentre = &_ma_plugingait_calibrate_kjc_basic; optr->CalibrateAnkleJointCentre = &_ma_plugingait_calibrate_ajc_basic; + optr->ConstructThighPose = &_ma_plugingait_construct_thigh_pose_basic; + optr->ConstructShankPose = &_ma_plugingait_construct_shank_pose_basic; } else { optr->CalibrateKneeJointCentre = &_ma_plugingait_calibrate_kjc_kad; optr->CalibrateAnkleJointCentre = &_ma_plugingait_calibrate_ajc_kad; + optr->ConstructThighPose = &_ma_plugingait_construct_thigh_pose_kad; + optr->ConstructShankPose = &_ma_plugingait_construct_shank_pose_kad; } }; From 37020d557ae5da5082118205354b04ea02bcacb5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Wed, 2 Nov 2016 13:44:22 -0400 Subject: [PATCH 19/35] Variables declaration were not adapted to the addition of the suffix 'Offset' (see commit 343f9dc91867bd43f324410089eb3c7c1550466f). --- modules/body/src/plugingait.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/body/src/plugingait.cpp b/modules/body/src/plugingait.cpp index 69368ca..7398980 100644 --- a/modules/body/src/plugingait.cpp +++ b/modules/body/src/plugingait.cpp @@ -225,8 +225,8 @@ namespace body std::string prefix; double s = 0.0, ankleWidth = 0.0, kneeWidth = 0.0, seglength = 0.0; double *staticPlantarFlexionOffset = nullptr, *staticRotationOffset = nullptr, - *tibialTorsion = nullptr, *thighRotationOffset = nullptr, - *shankRotationOffset = nullptr, *ankleAbAdd = nullptr; + *tibialTorsionOffset = nullptr, *thighRotationOffset = nullptr, + *shankRotationOffset = nullptr, *ankleAbAddOffset = nullptr; bool footFlat = false; if (side == Side::Left) { From 4d89f32421de01aefe786d0c123f54556af83bd7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Wed, 2 Nov 2016 13:46:09 -0400 Subject: [PATCH 20/35] Fix an error in the computation of the feet offsets when the KAD is used. --- modules/body/src/plugingait.cpp | 10 ++++---- .../test/c++/plugingaitTest_calibration.cpp | 24 +++++++++---------- .../test/c++/plugingaitTest_kinematics.cpp | 16 ++++++------- .../c++/plugingaitTest_reconstruction.cpp | 8 +++---- 4 files changed, 29 insertions(+), 29 deletions(-) diff --git a/modules/body/src/plugingait.cpp b/modules/body/src/plugingait.cpp index 7398980..604cb19 100644 --- a/modules/body/src/plugingait.cpp +++ b/modules/body/src/plugingait.cpp @@ -295,11 +295,12 @@ namespace body // ----------------------------------------- // Foot // ----------------------------------------- - // Required landmarks: *.MTH2, *.HEE, *.LS + // 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 const auto& LS = (*landmarks)[prefix+"LS"]; - if (!MTH2.isValid() || !HEE.isValid() || !LS.isValid()) + 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; @@ -323,9 +324,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; + this->ConstructShankPose(&u, &v_shank, &w, &LS, <M, &KJC, &AJC, s); // - Foot reference w = (HEE - MTH2).normalized(); u = v_shank.cross(w).normalized(); diff --git a/modules/body/test/c++/plugingaitTest_calibration.cpp b/modules/body/test/c++/plugingaitTest_calibration.cpp index cb35d78..81ceebe 100644 --- a/modules/body/test/c++/plugingaitTest_calibration.cpp +++ b/modules/body/test/c++/plugingaitTest_calibration.cpp @@ -349,15 +349,15 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) 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-3); - TS_ASSERT_DELTA(helper.leftShankRotationOffset(), 13.8726 * M_PI / 180.0, 1e-3); - TS_ASSERT_DELTA(helper.leftStaticPlantarFlexionOffset(), 4.72487 * M_PI / 180.0, 1e-3); - TS_ASSERT_DELTA(helper.leftStaticRotationOffset(), 0.233667 * M_PI / 180.0, 1e-4); + 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-3); - TS_ASSERT_DELTA(helper.rightShankRotationOffset(), 15.3639 * M_PI / 180.0, 1e-3); - TS_ASSERT_DELTA(helper.rightStaticPlantarFlexionOffset(), 3.88629 * M_PI / 180.0, 1e-3); - TS_ASSERT_DELTA(helper.rightStaticRotationOffset(), 2.22081 * M_PI / 180.0, 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(calibrateFullBodyFrameFootFlatKAD) @@ -384,12 +384,12 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) 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-4); - TS_ASSERT_DELTA(helper.rightAsisTrochanterAPDistance(), 62.852, 1e-3); + 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-3); - TS_ASSERT_DELTA(helper.rightStaticRotationOffset(), 0.0345916, 2e-3); + TS_ASSERT_DELTA(helper.rightStaticPlantarFlexionOffset(), 0.12429, 1e-5); + TS_ASSERT_DELTA(helper.rightStaticRotationOffset(), 0.0345916, 1e-5); }; }; diff --git a/modules/body/test/c++/plugingaitTest_kinematics.cpp b/modules/body/test/c++/plugingaitTest_kinematics.cpp index d59a1e4..c1f4a7e 100644 --- a/modules/body/test/c++/plugingaitTest_kinematics.cpp +++ b/modules/body/test/c++/plugingaitTest_kinematics.cpp @@ -495,14 +495,14 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) 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", {{3e-3,1e-4,2.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", {{3.5e-2,1.5e-2,5.4e-2}}); + 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", {{3.2e-2,1e-2,6e-2}}); - compare_joint_kinematics(kinematics, trial, "L.Foot.Progress.Angle", "LFootProgressAngles", {{2.7e-4,3.5e-4,1.75e-3}}); + 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(kinematicsFullBodyFrameFootFlatKAD) @@ -541,14 +541,14 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) 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", {{3e-3,1e-4,2.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", {{3.51e-2,1.5e-2,9.7e-2}}); + 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", {{3.2e-2,1e-2,1.1e-1}}); - compare_joint_kinematics(kinematics, trial, "L.Foot.Progress.Angle", "LFootProgressAngles", {{2.7e-4,3.5e-4,1.75e-3}}); + 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}}); }; }; diff --git a/modules/body/test/c++/plugingaitTest_reconstruction.cpp b/modules/body/test/c++/plugingaitTest_reconstruction.cpp index 3f620a3..2631c96 100644 --- a/modules/body/test/c++/plugingaitTest_reconstruction.cpp +++ b/modules/body/test/c++/plugingaitTest_reconstruction.cpp @@ -366,8 +366,8 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) 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"}, {7.5e-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,1e-3,1e-3,1e-3}); - compare_segment_motion(model, trial, "L.Foot.SCS", {"LFOO","LFOA","LFOL","LFOP"}, {1e4,1e-3,1e-3,1e-3}); + 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(reconstructFullBodyFrameFootFlatKAD) @@ -397,8 +397,8 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) 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"}, {7.5e-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,1e-3,2e-3,2e-3}); - compare_segment_motion(model, trial, "L.Foot.SCS", {"LFOO","LFOA","LFOL","LFOP"}, {1e4,1e-3,1e-3,1e-3}); + 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}); }; }; From f6d9065264a642b9c178d84386551d72defef1f9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Wed, 2 Nov 2016 13:50:22 -0400 Subject: [PATCH 21/35] Remove extra spaces for commented input names. --- modules/body/src/plugingait.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/body/src/plugingait.cpp b/modules/body/src/plugingait.cpp index 604cb19..758c989 100644 --- a/modules/body/src/plugingait.cpp +++ b/modules/body/src/plugingait.cpp @@ -62,7 +62,7 @@ #ifndef DOXYGEN_SHOULD_SKIP_THIS -bool _ma_plugingait_calibrate_kjc_basic(ma::math::Position* KJC, std::vector& /* offsets */, ma::body::PluginGaitPrivate* optr, ma::body::ummp* landmarks, const std::string& prefix, double /* s */, double kneeWidth, const ma::math::Position* HJC) +bool _ma_plugingait_calibrate_kjc_basic(ma::math::Position* KJC, 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"]; @@ -106,7 +106,7 @@ bool _ma_plugingait_calibrate_kjc_kad(ma::math::Position* KJC, std::vector& /* offsets */, ma::body::PluginGaitPrivate* optr, ma::body::ummp* landmarks, const std::string& prefix, double /* s */, double ankleWidth, const ma::math::Position* KJC) +bool _ma_plugingait_calibrate_ajc_basic(ma::math::Position* AJC, 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"]; From ceae1ce2144164d16641934709ad06e95ea2a93f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Thu, 3 Nov 2016 00:25:36 -0400 Subject: [PATCH 22/35] [WIP] Computation ot the (Left|Right)TibialTorsionOffset parameters for the KADMed variant. Modifications were also realized about the reconstruction of the thigh and shank SCS. --- .../body/include/openma/body/plugingait_p.h | 6 +- modules/body/src/plugingait.cpp | 136 +++++++++++------- .../test/c++/plugingaitTest_calibration.cpp | 37 ++++- .../c++/plugingaitTest_reconstruction.cpp | 10 +- 4 files changed, 126 insertions(+), 63 deletions(-) diff --git a/modules/body/include/openma/body/plugingait_p.h b/modules/body/include/openma/body/plugingait_p.h index 8968883..9a992a9 100644 --- a/modules/body/include/openma/body/plugingait_p.h +++ b/modules/body/include/openma/body/plugingait_p.h @@ -159,13 +159,9 @@ namespace body double RightAnkleAbAddOffset; double LeftAnkleAbAddOffset; - using CalibrateJointFuncPtr = bool (*)(math::Position* , std::vector& , PluginGaitPrivate* , ummp* , const std::string& , double , double , const math::Position* ); + using CalibrateJointFuncPtr = bool (*)(math::Position* , math::Position*, std::vector& , PluginGaitPrivate* , ummp* , const std::string& , double , double , const math::Position* ); CalibrateJointFuncPtr CalibrateKneeJointCentre; CalibrateJointFuncPtr CalibrateAnkleJointCentre; - - using ConstructSegmentPosePtr = void (*)(math::Vector* , math::Vector* , math::Vector* , const math::Map* , const math::Map* , const math::Position* , const math::Position* , double ); - ConstructSegmentPosePtr ConstructThighPose; - ConstructSegmentPosePtr ConstructShankPose; }; inline void PluginGaitPrivate::computeHipJointCentre(double* HJC, double S, double C, double xdis) const _OPENMA_NOEXCEPT diff --git a/modules/body/src/plugingait.cpp b/modules/body/src/plugingait.cpp index 758c989..79cb17f 100644 --- a/modules/body/src/plugingait.cpp +++ b/modules/body/src/plugingait.cpp @@ -62,7 +62,31 @@ #ifndef DOXYGEN_SHOULD_SKIP_THIS -bool _ma_plugingait_calibrate_kjc_basic(ma::math::Position* KJC, std::vector& /*offsets*/, ma::body::PluginGaitPrivate* optr, ma::body::ummp* landmarks, const std::string& prefix, double /*s*/, double kneeWidth, const ma::math::Position* HJC) +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) +{ + ma::math::Vector w = (*KJC - *AJC).normalized(); + ma::math::Vector LS_proj_trans_unit = ((*LS - (*LS - *AJC).dot(w).replicate<3>() * w) - *AJC).normalized(); + ma::math::Vector v = (*LTM - *AJC).normalized(); + ma::math::Scalar dot = LS_proj_trans_unit.dot(v); + ma::math::Scalar crossnorm = LS_proj_trans_unit.cross(v).norm(); + *offset = crossnorm.atan2(dot).mean(); +} + +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); +}; + +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"]; @@ -76,7 +100,7 @@ bool _ma_plugingait_calibrate_kjc_basic(ma::math::Position* KJC, std::vector& offsets, ma::body::PluginGaitPrivate* optr, ma::body::ummp* landmarks, const std::string& prefix, double s, double kneeWidth, const ma::math::Position* HJC) +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"]; @@ -93,20 +117,20 @@ bool _ma_plugingait_calibrate_kjc_kad(ma::math::Position* KJC, std::vectorMarkerDiameter + kneeWidth) / 2.0, VLFE, *HJC, KAX); + *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 v = (*VLFE - *KJC).normalized(); ma::math::Scalar dot = ITB_proj_trans_unit.dot(v); ma::math::Scalar crossnorm = ITB_proj_trans_unit.cross(v).norm(); *offsets[0] = -s * crossnorm.atan2(dot).mean(); return true; }; -bool _ma_plugingait_calibrate_ajc_basic(ma::math::Position* AJC, std::vector& /*offsets*/, ma::body::PluginGaitPrivate* optr, ma::body::ummp* landmarks, const std::string& prefix, double /*s*/, double ankleWidth, const ma::math::Position* KJC) +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"]; @@ -120,7 +144,7 @@ bool _ma_plugingait_calibrate_ajc_basic(ma::math::Position* AJC, std::vector& offsets, ma::body::PluginGaitPrivate* optr, ma::body::ummp* landmarks, const std::string& prefix, double /* s */, double ankleWidth, const ma::math::Position* KJC) +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"]; @@ -133,41 +157,47 @@ bool _ma_plugingait_calibrate_ajc_kad(ma::math::Position* AJC, std::vectorMarkerDiameter + ankleWidth) / 2.0, LTM, *KJC, KAX); // Compute the shank marker rotation offset - ma::math::Vector w = (*KJC - *AJC).normalized(); - ma::math::Vector LS_proj_trans_unit = ((LS - (LS - *AJC).dot(w).replicate<3>() * w) - *AJC).normalized(); - ma::math::Vector v = (LTM - *AJC).normalized(); - ma::math::Scalar dot = LS_proj_trans_unit.dot(v); - ma::math::Scalar crossnorm = LS_proj_trans_unit.cross(v).norm(); - *offsets[1] = crossnorm.atan2(dot).mean(); + _ma_plugingait_compute_shank_rotation_offset(offsets[1], AJC, KJC, &LS, <M); return true; }; -void _ma_plugingait_construct_thigh_pose_basic(ma::math::Vector* u, ma::math::Vector* v, ma::math::Vector* w, const ma::math::Map* ITB, const ma::math::Map* LFE, const ma::math::Position* HJC, const ma::math::Position* KJC, double s) + +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) { - *w = (*HJC - *KJC).normalized(); - *u = s * (*HJC - *LFE).cross(*ITB - *LFE).normalized(); - *v = w->cross(*u); + 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); + // 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(); + return true; }; -void _ma_plugingait_construct_thigh_pose_kad(ma::math::Vector* u, ma::math::Vector* v, ma::math::Vector* w, const ma::math::Map* /*ITB*/, const ma::math::Map* LFE, const ma::math::Position* HJC, const ma::math::Position* KJC, double s) -{ - *w = (*HJC - *KJC).normalized(); - *v = -s * (*LFE - *KJC).normalized(); - *u = v->cross(*w); -} -void _ma_plugingait_construct_shank_pose_basic(ma::math::Vector* u, ma::math::Vector* v, ma::math::Vector* w, const ma::math::Map* LS, 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(*LS - *AJC).normalized(); - *v = w->cross(*u); -}; - -void _ma_plugingait_construct_shank_pose_kad(ma::math::Vector* u, ma::math::Vector* v, ma::math::Vector* w, const ma::math::Map* /*LS*/, const ma::math::Map* LTM, const ma::math::Position* KJC, const ma::math::Position* AJC, double s) -{ - *w = (*KJC - *AJC).normalized(); - *v = -s * (*LTM - *AJC).normalized(); - *u = v->cross(*w); -}; namespace ma { @@ -212,9 +242,7 @@ namespace body RightAnkleAbAddOffset(0.0), LeftAnkleAbAddOffset(0.0), CalibrateKneeJointCentre(nullptr), - CalibrateAnkleJointCentre(nullptr), - ConstructThighPose(nullptr), - ConstructShankPose(nullptr) + CalibrateAnkleJointCentre(nullptr) {}; PluginGaitPrivate::~PluginGaitPrivate() _OPENMA_NOEXCEPT = default; @@ -261,12 +289,14 @@ namespace body error("PluginGait - Unknown side for the lower limb. Calibration aborted."); return false; } + std::vector offsets{thighRotationOffset, shankRotationOffset, tibialTorsionOffset, ankleAbAddOffset}; // ----------------------------------------- // Thigh // ----------------------------------------- - math::Position KJC; - if (!this->CalibrateKneeJointCentre(&KJC, offsets, this, landmarks, prefix, s, kneeWidth, HJC)) + 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; // Set the segment length seglength = (KJC - *HJC).norm().mean(); @@ -283,7 +313,8 @@ namespace body // Shank // ----------------------------------------- math::Position AJC; - if (!this->CalibrateAnkleJointCentre(&AJC, offsets, this, landmarks, prefix, s, ankleWidth, &KJC)) + // NOTE: VLFE is only used by the KADMed variant. + if (!this->CalibrateAnkleJointCentre(&AJC, &VLFE, offsets, this, landmarks, prefix, s, ankleWidth, &KJC)) return false; // Set the segment length seglength = (AJC - KJC).norm().mean(); @@ -325,7 +356,7 @@ namespace body } // - Shank axes math::Vector u, v_shank, w; - this->ConstructShankPose(&u, &v_shank, &w, &LS, <M, &KJC, &AJC, s); + _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(); @@ -493,7 +524,7 @@ namespace body } seg = model->segments()->findChild({},{{"side",side},{"part",Part::Thigh}},false); const math::Position KJC = compute_chord((this->MarkerDiameter + kneeWidth) / 2.0, LFE, *HJC, ITB, thighRotationOffset); - this->ConstructThighPose(&u, &v, &w, &ITB, &LFE, HJC, &KJC, s); + _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); @@ -513,7 +544,7 @@ namespace body const math::Position AJC = compute_chord((this->MarkerDiameter + ankleWidth) / 2.0, LTM, KJC, LS, shankRotationOffset); w = (KJC - AJC).normalized(); math::Vector v_shank; - this->ConstructShankPose(&u, &v_shank, &w, &LS, <M, &KJC, &AJC, s); + _ma_plugingait_construct_shank_pose(&u, &v_shank, &w, <M, &KJC, &AJC, s); math::to_timesequence(u, 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); @@ -892,12 +923,12 @@ namespace body */ 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. By default, this property contains the value 0.0. + * [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. By default, this property contains the value 0.0. + * [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; @@ -1000,15 +1031,16 @@ namespace body { optr->CalibrateKneeJointCentre = &_ma_plugingait_calibrate_kjc_basic; optr->CalibrateAnkleJointCentre = &_ma_plugingait_calibrate_ajc_basic; - optr->ConstructThighPose = &_ma_plugingait_construct_thigh_pose_basic; - optr->ConstructShankPose = &_ma_plugingait_construct_shank_pose_basic; } - else + else if (variant == KAD) { optr->CalibrateKneeJointCentre = &_ma_plugingait_calibrate_kjc_kad; optr->CalibrateAnkleJointCentre = &_ma_plugingait_calibrate_ajc_kad; - optr->ConstructThighPose = &_ma_plugingait_construct_thigh_pose_kad; - optr->ConstructShankPose = &_ma_plugingait_construct_shank_pose_kad; + } + else + { + optr->CalibrateKneeJointCentre = &_ma_plugingait_calibrate_kjc_kad; + optr->CalibrateAnkleJointCentre = &_ma_plugingait_calibrate_ajc_kadmed; } }; diff --git a/modules/body/test/c++/plugingaitTest_calibration.cpp b/modules/body/test/c++/plugingaitTest_calibration.cpp index 81ceebe..0e2b699 100644 --- a/modules/body/test/c++/plugingaitTest_calibration.cpp +++ b/modules/body/test/c++/plugingaitTest_calibration.cpp @@ -391,6 +391,40 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) TS_ASSERT_DELTA(helper.rightStaticPlantarFlexionOffset(), 0.12429, 1e-5); TS_ASSERT_DELTA(helper.rightStaticRotationOffset(), 0.0345916, 1e-5); }; + + CXXTEST_TEST(calibrateFullBodyFrameBasicKADMed) + { + 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_Basic.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, 1e-5); + 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-4); + // TS_ASSERT_DELTA(helper.rightShankRotationOffset(), 0.5526 * M_PI / 180.0 , 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-4); + }; }; CXXTEST_SUITE_REGISTRATION(PluginGaitCalibrationTest) @@ -407,4 +441,5 @@ CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrate3BothLowerBodyFF_N CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrate3BothLowerBodynoFF) CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrate2BothUpperBodyHeadOffsetDisabled) CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyFrameBasicKAD) -CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyFrameFootFlatKAD) \ No newline at end of file +CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyFrameFootFlatKAD) +CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyFrameBasicKADMed) \ No newline at end of file diff --git a/modules/body/test/c++/plugingaitTest_reconstruction.cpp b/modules/body/test/c++/plugingaitTest_reconstruction.cpp index 2631c96..f2a57c2 100644 --- a/modules/body/test/c++/plugingaitTest_reconstruction.cpp +++ b/modules/body/test/c++/plugingaitTest_reconstruction.cpp @@ -362,9 +362,9 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) 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"}, {1e-3,2e-5,2e-5}); + 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"}, {7.5e-4,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}); @@ -393,9 +393,9 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) 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"}, {1e-3,2e-5,2e-5}); + 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"}, {7.5e-4,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}); @@ -413,4 +413,4 @@ CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstruct3BothLowerBod CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstruct3BothLowerBodyNoFF) CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstruct2BothUpperBodyHeadOffsetDisabled) CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyFrameBasicKAD) -CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyFrameFootFlatKAD) \ No newline at end of file +CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyFrameFootFlatKAD) \ No newline at end of file From ab5aaeb3da943ef2fdce6a098b745df401c4c67f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Thu, 3 Nov 2016 00:47:55 -0400 Subject: [PATCH 23/35] Computation of the (Left|Right)AnkleAbAddOffset parameters. --- modules/body/src/plugingait.cpp | 7 +++++-- .../body/test/c++/plugingaitTest_calibration.cpp | 14 +++++++------- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/modules/body/src/plugingait.cpp b/modules/body/src/plugingait.cpp index 79cb17f..b78add8 100644 --- a/modules/body/src/plugingait.cpp +++ b/modules/body/src/plugingait.cpp @@ -194,10 +194,13 @@ bool _ma_plugingait_calibrate_ajc_kadmed(ma::math::Position* AJC, ma::math::Posi // 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 { diff --git a/modules/body/test/c++/plugingaitTest_calibration.cpp b/modules/body/test/c++/plugingaitTest_calibration.cpp index 0e2b699..c84cba3 100644 --- a/modules/body/test/c++/plugingaitTest_calibration.cpp +++ b/modules/body/test/c++/plugingaitTest_calibration.cpp @@ -414,16 +414,16 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) TS_ASSERT_DELTA(helper.leftThighRotationOffset(), 8.9585 * M_PI / 180.0, 1e-4); // TS_ASSERT_DELTA(helper.leftShankRotationOffset(), 3.89674 * M_PI / 180.0, 1e-5); 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.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-4); + TS_ASSERT_DELTA(helper.rightThighRotationOffset(), -10.0483 * M_PI / 180.0, 1e-5); // TS_ASSERT_DELTA(helper.rightShankRotationOffset(), 0.5526 * M_PI / 180.0 , 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-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); }; }; From 85d636b813ce70da13ba7140296041cabafe44da Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Thu, 3 Nov 2016 15:06:10 -0400 Subject: [PATCH 24/35] Validation of the computation for the shank offsets using the KADMed variant. --- modules/body/src/plugingait.cpp | 24 +++++++++---------- .../test/c++/plugingaitTest_calibration.cpp | 4 ++-- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/modules/body/src/plugingait.cpp b/modules/body/src/plugingait.cpp index b78add8..684e9f0 100644 --- a/modules/body/src/plugingait.cpp +++ b/modules/body/src/plugingait.cpp @@ -62,16 +62,6 @@ #ifndef DOXYGEN_SHOULD_SKIP_THIS -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) -{ - ma::math::Vector w = (*KJC - *AJC).normalized(); - ma::math::Vector LS_proj_trans_unit = ((*LS - (*LS - *AJC).dot(w).replicate<3>() * w) - *AJC).normalized(); - ma::math::Vector v = (*LTM - *AJC).normalized(); - ma::math::Scalar dot = LS_proj_trans_unit.dot(v); - ma::math::Scalar crossnorm = LS_proj_trans_unit.cross(v).norm(); - *offset = crossnorm.atan2(dot).mean(); -} - 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(); @@ -86,6 +76,16 @@ void _ma_plugingait_construct_shank_pose(ma::math::Vector* u, ma::math::Vector* *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) +{ + ma::math::Vector u, v, w; + _ma_plugingait_construct_shank_pose(&u, &v, &w, LTM, KJC, AJC, -1.0); + ma::math::Vector LS_proj_trans_unit = ((*LS - (*LS - *AJC).dot(w).replicate<3>() * w) - *AJC).normalized(); + ma::math::Scalar dot = LS_proj_trans_unit.dot(v); + ma::math::Scalar crossnorm = LS_proj_trans_unit.cross(v).norm(); + *offset = crossnorm.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"]; @@ -144,7 +144,7 @@ bool _ma_plugingait_calibrate_ajc_basic(ma::math::Position* AJC, ma::math::Posit 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) +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"]; @@ -161,7 +161,7 @@ bool _ma_plugingait_calibrate_ajc_kad(ma::math::Position* AJC, ma::math::Positio 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) +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"]; diff --git a/modules/body/test/c++/plugingaitTest_calibration.cpp b/modules/body/test/c++/plugingaitTest_calibration.cpp index c84cba3..e846fe3 100644 --- a/modules/body/test/c++/plugingaitTest_calibration.cpp +++ b/modules/body/test/c++/plugingaitTest_calibration.cpp @@ -412,14 +412,14 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) 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, 1e-5); + 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 , 1e-4); + 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); From aef515002396044517ff6987c4288175f52c7a18 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Sat, 5 Nov 2016 01:20:34 -0400 Subject: [PATCH 25/35] [WIP] Reconstruction of the untortioned tibia implemented. WARNING: The defintion of the ankle joints is currently wrong with the KADMed variant! --- modules/body/src/plugingait.cpp | 43 +++++++++-- .../test/c++/plugingaitTest_calibration.cpp | 16 ++-- .../test/c++/plugingaitTest_kinematics.cpp | 75 +++++++++++++++---- .../c++/plugingaitTest_reconstruction.cpp | 43 +++++++++-- 4 files changed, 140 insertions(+), 37 deletions(-) diff --git a/modules/body/src/plugingait.cpp b/modules/body/src/plugingait.cpp index 684e9f0..4035749 100644 --- a/modules/body/src/plugingait.cpp +++ b/modules/body/src/plugingait.cpp @@ -481,8 +481,10 @@ namespace body auto pptr = this->pint(); std::string prefix; double s = 0.0, ankleWidth = 0.0, kneeWidth = 0.0, - thighRotationOffset = 0.0, shankRotationOffset = 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."; @@ -491,8 +493,10 @@ namespace body 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) { @@ -502,8 +506,10 @@ namespace body kneeWidth = this->RightKneeWidth; thighRotationOffset = this->RightThighRotationOffset; shankRotationOffset = this->RightShankRotationOffset; + tibialTorsionOffset = this->RightTibialTorsionOffset; staticPlantarFlexionOffset = -this->RightStaticPlantarFlexionOffset; staticRotationOffset = this->RightStaticRotationOffset; + ankleAbAddOffset = -this->RightAnkleAbAddOffset; } else { @@ -544,11 +550,31 @@ 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, shankRotationOffset); - w = (KJC - AJC).normalized(); - math::Vector v_shank; - _ma_plugingait_construct_shank_pose(&u, &v_shank, &w, <M, &KJC, &AJC, s); - 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; + math::Vector v_tibia_tortioned; + _ma_plugingait_construct_shank_pose(&u, &v_tibia_tortioned, &w, <M, &KJC, &AJC, s); + const double ct = cos(tibialTorsionOffset); + const double st = sin(tibialTorsionOffset); + math::Vector u_shank = ct * u - st * v_tibia_tortioned; + math::Vector v_shank = st * u + ct * v_tibia_tortioned; + 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); // ----------------------------------------- @@ -563,7 +589,8 @@ 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 + u = v_tibia_tortioned.cross(w).normalized(); v = w.cross(u); const double cx = cos(staticRotationOffset), sx = sin(staticRotationOffset), cy = cos(staticPlantarFlexionOffset), sy = sin(staticPlantarFlexionOffset); diff --git a/modules/body/test/c++/plugingaitTest_calibration.cpp b/modules/body/test/c++/plugingaitTest_calibration.cpp index e846fe3..ac1525a 100644 --- a/modules/body/test/c++/plugingaitTest_calibration.cpp +++ b/modules/body/test/c++/plugingaitTest_calibration.cpp @@ -331,7 +331,7 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) TS_ASSERT_EQUALS(helper.rightStaticRotationOffset(), 0.0); }; - CXXTEST_TEST(calibrateFullBodyFrameBasicKAD) + CXXTEST_TEST(calibrateFullBodyFrameKAD) { ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KAD); helper.setMarkerDiameter(14.0); // mm @@ -343,7 +343,7 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) helper.setRightAnkleWidth(72.9); // mm ma::Node root("root"); - generate_trial_from_file(&root, OPENMA_TDD_PATH_IN("c3d/plugingait/PiGKad_Calibration_Basic.c3d")); + 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); @@ -360,7 +360,7 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) TS_ASSERT_DELTA(helper.rightStaticRotationOffset(), 2.22081 * M_PI / 180.0, 1e-5); }; - CXXTEST_TEST(calibrateFullBodyFrameFootFlatKAD) + CXXTEST_TEST(calibrateFullBodyFrameKADFF) { ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KAD); helper.setMarkerDiameter(14.0); // mm @@ -392,7 +392,7 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) TS_ASSERT_DELTA(helper.rightStaticRotationOffset(), 0.0345916, 1e-5); }; - CXXTEST_TEST(calibrateFullBodyFrameBasicKADMed) + CXXTEST_TEST(calibrateFullBodyFrameKADMed) { ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KADMed); helper.setMarkerDiameter(14.0); // mm @@ -405,7 +405,7 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) ma::Node root("root"); - generate_trial_from_file(&root, OPENMA_TDD_PATH_IN("c3d/plugingait/PiGKadMed_Calibration_Basic.c3d")); + 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); @@ -440,6 +440,6 @@ CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrate3BothLowerBodyFF) CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrate3BothLowerBodyFF_N18) CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrate3BothLowerBodynoFF) CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrate2BothUpperBodyHeadOffsetDisabled) -CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyFrameBasicKAD) -CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyFrameFootFlatKAD) -CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyFrameBasicKADMed) \ No newline at end of file +CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyFrameKAD) +CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyFrameKADFF) +CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyFrameKADMed) \ 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 c1f4a7e..0cb33ae 100644 --- a/modules/body/test/c++/plugingaitTest_kinematics.cpp +++ b/modules/body/test/c++/plugingaitTest_kinematics.cpp @@ -461,7 +461,7 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) compare_joint_kinematics(kinematics, trial, "L.Head.Progress.Angle", "LViconHeadAngles"); }; - CXXTEST_TEST(kinematicsFullBodyFrameBasicKAD) + CXXTEST_TEST(kinematicsFullBodyFrameKAD) { ma::Subject subject("Anonymous", {{"markerDiameter", 14.0}, @@ -474,10 +474,10 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) 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_Basic.c3d")), true); + 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_Basic.c3d")), true); + 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); @@ -505,7 +505,7 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) compare_joint_kinematics(kinematics, trial, "L.Foot.Progress.Angle", "LFootProgressAngles",{{1.2e-4,3.5e-4}}); }; - CXXTEST_TEST(kinematicsFullBodyFrameFootFlatKAD) + CXXTEST_TEST(kinematicsFullBodyFrameKADFF) { ma::Subject subject("Anonymous", {{"markerDiameter", 14.0}, @@ -550,17 +550,62 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) 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(kinematicsFullBodyFrameKADMed) + { + 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); + // 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, "L.Ankle.Angle", "LAnkleAngles"); + 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.Ankle.Angle", "RAnkleAngles"); + 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"); + // compare_joint_kinematics(kinematics, trial, "L.Foot.Progress.Angle", "LFootProgressAngles"); + }; }; CXXTEST_SUITE_REGISTRATION(PluginGaitKinematicsTest) -CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothLowerBodyOneFrame) -CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothUpperBodyOneFrame) -CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothLowerBodyHoleFrames) -CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothUpperBodyHoleFrames) -CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothFullBodyFullFrames) -CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematics3BothLowerBodyFF) -CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematics3BothLowerBodyFF_N18) -CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematics3BothLowerBodyNoFF) -CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothFullBodyFullFramesHeadOffsetDisabled) -CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsFullBodyFrameBasicKAD) -CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsFullBodyFrameFootFlatKAD) +// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothLowerBodyOneFrame) +// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothUpperBodyOneFrame) +// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothLowerBodyHoleFrames) +// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothUpperBodyHoleFrames) +// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothFullBodyFullFrames) +// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematics3BothLowerBodyFF) +// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematics3BothLowerBodyFF_N18) +// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematics3BothLowerBodyNoFF) +// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothFullBodyFullFramesHeadOffsetDisabled) +// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsFullBodyFrameKAD) +// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsFullBodyFrameKADFF) +CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsFullBodyFrameKADMed) \ No newline at end of file diff --git a/modules/body/test/c++/plugingaitTest_reconstruction.cpp b/modules/body/test/c++/plugingaitTest_reconstruction.cpp index f2a57c2..84dad53 100644 --- a/modules/body/test/c++/plugingaitTest_reconstruction.cpp +++ b/modules/body/test/c++/plugingaitTest_reconstruction.cpp @@ -341,7 +341,7 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) } }; - CXXTEST_TEST(reconstructFullBodyFrameBasicKAD) + CXXTEST_TEST(reconstructFullBodyFrameKAD) { ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KAD); helper.setMarkerDiameter(14.0); // mm @@ -353,10 +353,10 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) 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_Basic.c3d")); + 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_Basic.c3d")); + generate_trial_from_file(&rootDynamic, OPENMA_TDD_PATH_IN("c3d/plugingait/PiGKad_Motion.c3d")); TS_ASSERT(helper.reconstruct(&rootModel, &rootDynamic)); auto trial = rootDynamic.findChild(); @@ -370,7 +370,7 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) compare_segment_motion(model, trial, "L.Foot.SCS", {"LFOO","LFOA","LFOL","LFOP"}, {1e4}); }; - CXXTEST_TEST(reconstructFullBodyFrameFootFlatKAD) + CXXTEST_TEST(reconstructFullBodyFrameKADFF) { ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KAD); helper.setMarkerDiameter(14.0); // mm @@ -400,6 +400,36 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) 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(reconstructFullBodyFrameKADMed) + { + 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 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}); + // NOTE: The feet 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}); + }; }; CXXTEST_SUITE_REGISTRATION(PluginGaitReconstructionTest) @@ -412,5 +442,6 @@ CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstruct3BothLowerBod CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstruct3BothLowerBodyFF_N18) CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstruct3BothLowerBodyNoFF) CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstruct2BothUpperBodyHeadOffsetDisabled) -CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyFrameBasicKAD) -CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyFrameFootFlatKAD) \ No newline at end of file +CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyFrameKAD) +CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyFrameKADFF) +CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyFrameKADMed) \ No newline at end of file From f8c6b45a4e90c5d15e20abf9bbb995c94128fec4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Sat, 5 Nov 2016 10:59:21 -0400 Subject: [PATCH 26/35] Symbol M_PI not defined by default with MSVC. --- modules/body/test/c++/CMakeLists.txt | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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 From 3cb680419e6724984601dfbc7150653082d149b4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Wed, 16 Nov 2016 19:29:56 -0500 Subject: [PATCH 27/35] Fix an error on the computation of the thigh and shank offsets (wrong sign in some cases). --- modules/body/src/plugingait.cpp | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/modules/body/src/plugingait.cpp b/modules/body/src/plugingait.cpp index 4035749..48dc447 100644 --- a/modules/body/src/plugingait.cpp +++ b/modules/body/src/plugingait.cpp @@ -76,14 +76,16 @@ void _ma_plugingait_construct_shank_pose(ma::math::Vector* u, ma::math::Vector* *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) +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, -1.0); + _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); - ma::math::Scalar crossnorm = LS_proj_trans_unit.cross(v).norm(); - *offset = crossnorm.atan2(dot).mean(); + 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) @@ -124,9 +126,10 @@ bool _ma_plugingait_calibrate_kjc_kad(ma::math::Position* KJC, ma::math::Positio 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); - ma::math::Scalar crossnorm = ITB_proj_trans_unit.cross(v).norm(); - *offsets[0] = -s * crossnorm.atan2(dot).mean(); + const double ss = s * sign(static_cast(cross.dot(w).mean())); + *offsets[0] = ss * cross.norm().atan2(dot).mean(); return true; }; @@ -144,7 +147,7 @@ bool _ma_plugingait_calibrate_ajc_basic(ma::math::Position* AJC, ma::math::Posit 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) +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"]; @@ -157,7 +160,7 @@ bool _ma_plugingait_calibrate_ajc_kad(ma::math::Position* AJC, ma::math::Positio // 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); + _ma_plugingait_compute_shank_rotation_offset(offsets[1], AJC, KJC, &LS, <M, s); return true; }; @@ -174,7 +177,7 @@ bool _ma_plugingait_calibrate_ajc_kadmed(ma::math::Position* AJC, ma::math::Posi // 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); + _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 From 95fb04a58ce551e6b3bdbcc894933a61d81c58e7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Wed, 16 Nov 2016 19:30:34 -0500 Subject: [PATCH 28/35] New unit test related to the commit 3cb680419e6724984601dfbc7150653082d149b4. --- .../test/c++/plugingaitTest_calibration.cpp | 37 ++++++++++++++++++- 1 file changed, 35 insertions(+), 2 deletions(-) diff --git a/modules/body/test/c++/plugingaitTest_calibration.cpp b/modules/body/test/c++/plugingaitTest_calibration.cpp index ac1525a..b8c5bfe 100644 --- a/modules/body/test/c++/plugingaitTest_calibration.cpp +++ b/modules/body/test/c++/plugingaitTest_calibration.cpp @@ -403,7 +403,6 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) 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); @@ -425,6 +424,39 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) 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(calibrateFullBodyFrameKADMed2) + { + 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) @@ -442,4 +474,5 @@ CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrate3BothLowerBodynoFF CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrate2BothUpperBodyHeadOffsetDisabled) CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyFrameKAD) CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyFrameKADFF) -CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyFrameKADMed) \ No newline at end of file +CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyFrameKADMed) +CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyFrameKADMed2) \ No newline at end of file From 82a0168ccf02344fbe00decca44944e66146f0c6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Wed, 16 Nov 2016 19:31:06 -0500 Subject: [PATCH 29/35] Unit test for the Python bindings related to calibration of the Plug-in Gait model. --- modules/body/test/python/CMakeLists.txt | 3 +- .../test/python/plugingaitTest_calibration.py | 62 +++++++++++++++++++ 2 files changed, 64 insertions(+), 1 deletion(-) create mode 100644 modules/body/test/python/plugingaitTest_calibration.py diff --git a/modules/body/test/python/CMakeLists.txt b/modules/body/test/python/CMakeLists.txt index 0c10a62..82befbc 100644 --- a/modules/body/test/python/CMakeLists.txt +++ b/modules/body/test/python/CMakeLists.txt @@ -2,4 +2,5 @@ SET(PACKAGE_DIRECTORY "${EXECUTABLE_OUTPUT_PATH}/swig/python/openma") SET(TESTS_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}") SET(PTD_DIRS ${PACKAGE_DIRECTORY} ${TESTS_DIRECTORY}) -ADD_PYTHON_TESTDRIVER(python_body_landmarkstranslator landmarkstranslatorTest INCLUDES ${PTD_DIRS}) \ No newline at end of file +ADD_PYTHON_TESTDRIVER(python_body_landmarkstranslator landmarkstranslatorTest INCLUDES ${PTD_DIRS}) +ADD_PYTHON_TESTDRIVER(python_body_plugingait_calibration plugingaitTest_calibration INCLUDES ${PTD_DIRS} "${CMAKE_BINARY_DIR}/modules/io/test/python") \ 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 From 13947de66b935ce229b742538dfa5c3cddcbc961 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Wed, 16 Nov 2016 19:33:48 -0500 Subject: [PATCH 30/35] Unit tests *FullBodyFrameKAD* renamed *FullBodyKAD*. --- .../body/test/c++/plugingaitTest_calibration.cpp | 16 ++++++++-------- .../body/test/c++/plugingaitTest_kinematics.cpp | 12 ++++++------ .../test/c++/plugingaitTest_reconstruction.cpp | 12 ++++++------ 3 files changed, 20 insertions(+), 20 deletions(-) diff --git a/modules/body/test/c++/plugingaitTest_calibration.cpp b/modules/body/test/c++/plugingaitTest_calibration.cpp index b8c5bfe..17cfa51 100644 --- a/modules/body/test/c++/plugingaitTest_calibration.cpp +++ b/modules/body/test/c++/plugingaitTest_calibration.cpp @@ -331,7 +331,7 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) TS_ASSERT_EQUALS(helper.rightStaticRotationOffset(), 0.0); }; - CXXTEST_TEST(calibrateFullBodyFrameKAD) + CXXTEST_TEST(calibrateFullBodyKAD) { ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KAD); helper.setMarkerDiameter(14.0); // mm @@ -360,7 +360,7 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) TS_ASSERT_DELTA(helper.rightStaticRotationOffset(), 2.22081 * M_PI / 180.0, 1e-5); }; - CXXTEST_TEST(calibrateFullBodyFrameKADFF) + CXXTEST_TEST(calibrateFullBodyKADFF) { ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KAD); helper.setMarkerDiameter(14.0); // mm @@ -392,7 +392,7 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) TS_ASSERT_DELTA(helper.rightStaticRotationOffset(), 0.0345916, 1e-5); }; - CXXTEST_TEST(calibrateFullBodyFrameKADMed) + CXXTEST_TEST(calibrateFullBodyKADMed) { ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KADMed); helper.setMarkerDiameter(14.0); // mm @@ -425,7 +425,7 @@ CXXTEST_SUITE(PluginGaitCalibrationTest) TS_ASSERT_DELTA(helper.rightAnkleAbAddOffset(), 8.70843 * M_PI / 180.0, 1e-5); }; - CXXTEST_TEST(calibrateFullBodyFrameKADMed2) + CXXTEST_TEST(calibrateFullBodyKADMed2) { ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KADMed); helper.setMarkerDiameter(25.0); // mm @@ -472,7 +472,7 @@ CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrate3BothLowerBodyFF) CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrate3BothLowerBodyFF_N18) CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrate3BothLowerBodynoFF) CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrate2BothUpperBodyHeadOffsetDisabled) -CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyFrameKAD) -CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyFrameKADFF) -CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyFrameKADMed) -CXXTEST_TEST_REGISTRATION(PluginGaitCalibrationTest, calibrateFullBodyFrameKADMed2) \ No newline at end of file +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 0cb33ae..86e66db 100644 --- a/modules/body/test/c++/plugingaitTest_kinematics.cpp +++ b/modules/body/test/c++/plugingaitTest_kinematics.cpp @@ -461,7 +461,7 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) compare_joint_kinematics(kinematics, trial, "L.Head.Progress.Angle", "LViconHeadAngles"); }; - CXXTEST_TEST(kinematicsFullBodyFrameKAD) + CXXTEST_TEST(kinematicsFullBodyKAD) { ma::Subject subject("Anonymous", {{"markerDiameter", 14.0}, @@ -505,7 +505,7 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) compare_joint_kinematics(kinematics, trial, "L.Foot.Progress.Angle", "LFootProgressAngles",{{1.2e-4,3.5e-4}}); }; - CXXTEST_TEST(kinematicsFullBodyFrameKADFF) + CXXTEST_TEST(kinematicsFullBodyKADFF) { ma::Subject subject("Anonymous", {{"markerDiameter", 14.0}, @@ -551,7 +551,7 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) compare_joint_kinematics(kinematics, trial, "L.Foot.Progress.Angle", "LFootProgressAngles",{{1.2e-4,3.4e-4}}); }; - CXXTEST_TEST(kinematicsFullBodyFrameKADMed) + CXXTEST_TEST(kinematicsFullBodyKADMed) { ma::Subject subject("Anonymous", {{"markerDiameter", 14.0}, @@ -606,6 +606,6 @@ CXXTEST_SUITE_REGISTRATION(PluginGaitKinematicsTest) // CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematics3BothLowerBodyFF_N18) // CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematics3BothLowerBodyNoFF) // CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothFullBodyFullFramesHeadOffsetDisabled) -// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsFullBodyFrameKAD) -// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsFullBodyFrameKADFF) -CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsFullBodyFrameKADMed) \ No newline at end of file +// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsFullBodyKAD) +// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsFullBodyKADFF) +CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsFullBodyKADMed) \ No newline at end of file diff --git a/modules/body/test/c++/plugingaitTest_reconstruction.cpp b/modules/body/test/c++/plugingaitTest_reconstruction.cpp index 84dad53..34335f6 100644 --- a/modules/body/test/c++/plugingaitTest_reconstruction.cpp +++ b/modules/body/test/c++/plugingaitTest_reconstruction.cpp @@ -341,7 +341,7 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) } }; - CXXTEST_TEST(reconstructFullBodyFrameKAD) + CXXTEST_TEST(reconstructFullBodyKAD) { ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KAD); helper.setMarkerDiameter(14.0); // mm @@ -370,7 +370,7 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) compare_segment_motion(model, trial, "L.Foot.SCS", {"LFOO","LFOA","LFOL","LFOP"}, {1e4}); }; - CXXTEST_TEST(reconstructFullBodyFrameKADFF) + CXXTEST_TEST(reconstructFullBodyKADFF) { ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KAD); helper.setMarkerDiameter(14.0); // mm @@ -401,7 +401,7 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) compare_segment_motion(model, trial, "L.Foot.SCS", {"LFOO","LFOA","LFOL","LFOP"}, {1e4}); }; - CXXTEST_TEST(reconstructFullBodyFrameKADMed) + CXXTEST_TEST(reconstructFullBodyKADMed) { ma::body::PluginGait helper(ma::body::Region::Lower, ma::body::Side::Both, ma::body::PluginGait::KADMed); helper.setMarkerDiameter(14.0); // mm @@ -442,6 +442,6 @@ CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstruct3BothLowerBod CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstruct3BothLowerBodyFF_N18) CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstruct3BothLowerBodyNoFF) CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstruct2BothUpperBodyHeadOffsetDisabled) -CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyFrameKAD) -CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyFrameKADFF) -CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyFrameKADMed) \ No newline at end of file +CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyKAD) +CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyKADFF) +CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyKADMed) \ No newline at end of file From 08c5500ee3927b37f5733102fc9041ef3c2b31f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Wed, 16 Nov 2016 23:13:03 -0500 Subject: [PATCH 31/35] New unit test for the KADMed variant. --- .../c++/plugingaitTest_reconstruction.cpp | 33 ++++++++++++++++++- 1 file changed, 32 insertions(+), 1 deletion(-) diff --git a/modules/body/test/c++/plugingaitTest_reconstruction.cpp b/modules/body/test/c++/plugingaitTest_reconstruction.cpp index 34335f6..79095fe 100644 --- a/modules/body/test/c++/plugingaitTest_reconstruction.cpp +++ b/modules/body/test/c++/plugingaitTest_reconstruction.cpp @@ -430,6 +430,36 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) // 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(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 + + 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}); + // NOTE: The feet 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}); + }; }; CXXTEST_SUITE_REGISTRATION(PluginGaitReconstructionTest) @@ -444,4 +474,5 @@ CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstruct3BothLowerBod CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstruct2BothUpperBodyHeadOffsetDisabled) CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyKAD) CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyKADFF) -CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyKADMed) \ No newline at end of file +CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyKADMed) +CXXTEST_TEST_REGISTRATION(PluginGaitReconstructionTest, reconstructFullBodyKADMed2) \ No newline at end of file From 92e388c5da84b5689f5b05d5cc17cff97b8d89b7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Wed, 16 Nov 2016 23:30:12 -0500 Subject: [PATCH 32/35] Hidden way to compare the reconstruction of the Foot reference frame of the original PiG KADMed variant. --- modules/body/src/plugingait.cpp | 7 +++++++ .../test/c++/plugingaitTest_reconstruction.cpp | 17 +++++++++++------ 2 files changed, 18 insertions(+), 6 deletions(-) diff --git a/modules/body/src/plugingait.cpp b/modules/body/src/plugingait.cpp index 48dc447..2d2ddb0 100644 --- a/modules/body/src/plugingait.cpp +++ b/modules/body/src/plugingait.cpp @@ -593,7 +593,14 @@ namespace body seg = model->segments()->findChild({},{{"side",side},{"part",Part::Foot}},false); w = (AJC - MTH2).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_tibia_tortioned.cross(w).normalized(); +#else + if (pptr->property("_ma_debug_vicon_original_foot_frame").cast()) + u = v_shank.cross(w).normalized(); + else + u = v_tibia_tortioned.cross(w).normalized(); +#endif v = w.cross(u); const double cx = cos(staticRotationOffset), sx = sin(staticRotationOffset), cy = cos(staticPlantarFlexionOffset), sy = sin(staticPlantarFlexionOffset); diff --git a/modules/body/test/c++/plugingaitTest_reconstruction.cpp b/modules/body/test/c++/plugingaitTest_reconstruction.cpp index 79095fe..9ceb195 100644 --- a/modules/body/test/c++/plugingaitTest_reconstruction.cpp +++ b/modules/body/test/c++/plugingaitTest_reconstruction.cpp @@ -411,6 +411,7 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) 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")); @@ -426,9 +427,11 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) 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}); - // NOTE: The feet 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}); +#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) @@ -441,6 +444,7 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) 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")); @@ -456,9 +460,10 @@ CXXTEST_SUITE(PluginGaitReconstructionTest) 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}); - // NOTE: The feet 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}); +#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 }; }; From 5f62aaf98109d00cfa10c67f111346523b6abb82 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Wed, 16 Nov 2016 23:37:04 -0500 Subject: [PATCH 33/35] Supplementary test to compute the joint angles for the KADMed variant. --- .../test/c++/plugingaitTest_kinematics.cpp | 69 +++++++++++++++---- 1 file changed, 57 insertions(+), 12 deletions(-) diff --git a/modules/body/test/c++/plugingaitTest_kinematics.cpp b/modules/body/test/c++/plugingaitTest_kinematics.cpp index 86e66db..5a0e034 100644 --- a/modules/body/test/c++/plugingaitTest_kinematics.cpp +++ b/modules/body/test/c++/plugingaitTest_kinematics.cpp @@ -594,18 +594,63 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) // compare_joint_kinematics(kinematics, trial, "R.Foot.Progress.Angle", "RFootProgressAngles"); // compare_joint_kinematics(kinematics, trial, "L.Foot.Progress.Angle", "LFootProgressAngles"); }; + + 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); + // 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, "L.Ankle.Angle", "LAnkleAngles"); + 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.Ankle.Angle", "RAnkleAngles"); + 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"); + // compare_joint_kinematics(kinematics, trial, "L.Foot.Progress.Angle", "LFootProgressAngles"); + }; }; CXXTEST_SUITE_REGISTRATION(PluginGaitKinematicsTest) -// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothLowerBodyOneFrame) -// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothUpperBodyOneFrame) -// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothLowerBodyHoleFrames) -// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothUpperBodyHoleFrames) -// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothFullBodyFullFrames) -// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematics3BothLowerBodyFF) -// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematics3BothLowerBodyFF_N18) -// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematics3BothLowerBodyNoFF) -// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothFullBodyFullFramesHeadOffsetDisabled) -// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsFullBodyKAD) -// CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsFullBodyKADFF) -CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsFullBodyKADMed) \ No newline at end of file +CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothLowerBodyOneFrame) +CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothUpperBodyOneFrame) +CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothLowerBodyHoleFrames) +CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothUpperBodyHoleFrames) +CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematicsBothFullBodyFullFrames) +CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematics3BothLowerBodyFF) +CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematics3BothLowerBodyFF_N18) +CXXTEST_TEST_REGISTRATION(PluginGaitKinematicsTest, kinematics3BothLowerBodyNoFF) +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) \ No newline at end of file From 7fe919b24f07e0f9f030ab5fd9d925024e8fbe4f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Sat, 19 Nov 2016 18:17:03 -0500 Subject: [PATCH 34/35] Variable v_tibia_tortioned renamed v_shank_torsioned to stay consistent with the rest of the code. --- modules/body/src/plugingait.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/modules/body/src/plugingait.cpp b/modules/body/src/plugingait.cpp index 2d2ddb0..f266c37 100644 --- a/modules/body/src/plugingait.cpp +++ b/modules/body/src/plugingait.cpp @@ -571,12 +571,12 @@ namespace body 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; - math::Vector v_tibia_tortioned; - _ma_plugingait_construct_shank_pose(&u, &v_tibia_tortioned, &w, <M, &KJC, &AJC, s); + math::Vector v_shank_torsioned; + _ma_plugingait_construct_shank_pose(&u, &v_shank_torsioned, &w, <M, &KJC, &AJC, s); const double ct = cos(tibialTorsionOffset); const double st = sin(tibialTorsionOffset); - math::Vector u_shank = ct * u - st * v_tibia_tortioned; - math::Vector v_shank = st * u + ct * v_tibia_tortioned; + 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); @@ -594,12 +594,12 @@ namespace body w = (AJC - MTH2).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_tibia_tortioned.cross(w).normalized(); + 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_tibia_tortioned.cross(w).normalized(); + u = v_shank_torsioned.cross(w).normalized(); #endif v = w.cross(u); const double cx = cos(staticRotationOffset), sx = sin(staticRotationOffset), From aff993781fbc3258347fce51b7cd0b6c11eba242 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Arnaud=20Barre=CC=81?= Date: Sat, 26 Nov 2016 15:17:56 -0500 Subject: [PATCH 35/35] A new segment is created to represent the torsioned shank in the plug-in gait model. IMPORTANT: This is a workaround solution as the shank should be represented only by a single segment. The 'body' module must be improved to offer this possibility (ie. refactor the way an anchor is defined). --- modules/body/src/plugingait.cpp | 20 ++++++++++++---- .../test/c++/plugingaitTest_kinematics.cpp | 24 ++++++++++++------- 2 files changed, 31 insertions(+), 13 deletions(-) diff --git a/modules/body/src/plugingait.cpp b/modules/body/src/plugingait.cpp index f266c37..84aebae 100644 --- a/modules/body/src/plugingait.cpp +++ b/modules/body/src/plugingait.cpp @@ -62,6 +62,8 @@ #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(); @@ -571,8 +573,12 @@ namespace body 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; @@ -1083,7 +1089,7 @@ namespace body optr->CalibrateAnkleJointCentre = &_ma_plugingait_calibrate_ajc_kadmed; } }; - + /** * Create segments and joints according to the region and side given to the constructor. */ @@ -1164,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); @@ -1174,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"); @@ -1187,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; @@ -1197,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"); diff --git a/modules/body/test/c++/plugingaitTest_kinematics.cpp b/modules/body/test/c++/plugingaitTest_kinematics.cpp index 5a0e034..2850a49 100644 --- a/modules/body/test/c++/plugingaitTest_kinematics.cpp +++ b/modules/body/test/c++/plugingaitTest_kinematics.cpp @@ -571,6 +571,7 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) 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 @@ -585,14 +586,16 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) 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, "L.Ankle.Angle", "LAnkleAngles"); 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.Ankle.Angle", "RAnkleAngles"); 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"); - // compare_joint_kinematics(kinematics, trial, "L.Foot.Progress.Angle", "LFootProgressAngles"); + 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) @@ -615,6 +618,7 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) 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 @@ -629,14 +633,16 @@ CXXTEST_SUITE(PluginGaitKinematicsTest) 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, "L.Ankle.Angle", "LAnkleAngles"); 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.Ankle.Angle", "RAnkleAngles"); 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"); - // compare_joint_kinematics(kinematics, trial, "L.Foot.Progress.Angle", "LFootProgressAngles"); +#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 }; };