From 597f6c46bf1c7952055b022e6f95ec484f35ef7a Mon Sep 17 00:00:00 2001 From: SergeiNikolaev Date: Tue, 30 Nov 2021 16:18:03 +0100 Subject: [PATCH 1/9] Correct headers and namespaces for compatibility with SOFA v21.06 --- src/genericComponents/AddNoiseEngine.cpp | 4 +- src/genericComponents/AddNoiseEngine.h | 8 +- .../CorrectionForceField.cpp | 6 +- src/genericComponents/CorrectionForceField.h | 30 +- .../CorrectionForceField.inl | 42 +- src/genericComponents/FilterEvents.cpp | 8 +- src/genericComponents/FilterEvents.h | 20 +- .../FilteringUpdateEngine.cpp | 2 + src/genericComponents/FilteringUpdateEngine.h | 17 +- .../Indices2ValuesTransformer.cpp | 10 +- .../Indices2ValuesTransformer.h | 40 +- .../Indices2ValuesTransformer.inl | 10 +- src/genericComponents/MatrixCovariance.cpp | 5 +- src/genericComponents/MatrixCovariance.h | 18 +- src/genericComponents/MatrixCovariance.inl | 5 +- src/genericComponents/ObservationSource.h | 6 +- src/genericComponents/OptimMonitor.cpp | 2 + src/genericComponents/OptimMonitor.h | 88 ++-- src/genericComponents/OptimMonitor.inl | 36 +- src/genericComponents/OptimParams.cpp | 482 ++++++++++-------- src/genericComponents/OptimParams.h | 63 +-- src/genericComponents/OptimParams.inl | 17 +- src/genericComponents/PointProjection.cpp | 7 +- src/genericComponents/PointProjection.h | 287 +++++------ src/genericComponents/PointProjection.inl | 102 ++-- src/genericComponents/ShowSpheres.cpp | 4 + src/genericComponents/ShowSpheres.h | 16 +- src/genericComponents/ShowSpheres.inl | 10 +- .../SigmaPointsVTKExporter.cpp | 177 ++++--- .../SigmaPointsVTKExporter.h | 54 +- .../SimulatedStateObservationSource.cpp | 8 +- .../SimulatedStateObservationSource.h | 26 +- .../SimulatedStateObservationSource.inl | 83 +-- .../SimulatedStateObservationStreamer.cpp | 2 + .../SimulatedStateObservationStreamer.h | 15 +- .../SimulatedStateObservationStreamer.inl | 6 +- src/genericComponents/StepPCGLinearSolver.cpp | 47 +- src/genericComponents/StepPCGLinearSolver.h | 56 +- .../StochasticPositionHandler.cpp | 30 +- .../StochasticPositionHandler.h | 58 ++- .../StochasticPositionHandler.inl | 66 +-- src/genericComponents/TimeProfiling.h | 17 +- .../TransformStochasticEngine.h | 35 +- .../TransformStochasticEngine.inl | 183 +++---- src/genericComponents/VTKExporterDA.cpp | 2 + src/genericComponents/VTKExporterDA.h | 3 +- src/initOptimusPlugin.cpp | 44 +- src/initOptimusPlugin.h | 18 +- 48 files changed, 1189 insertions(+), 1086 deletions(-) diff --git a/src/genericComponents/AddNoiseEngine.cpp b/src/genericComponents/AddNoiseEngine.cpp index e7f3ae6c..14612d5c 100644 --- a/src/genericComponents/AddNoiseEngine.cpp +++ b/src/genericComponents/AddNoiseEngine.cpp @@ -27,6 +27,7 @@ //#include + namespace sofa { @@ -36,8 +37,6 @@ namespace component namespace engine { -using namespace defaulttype; - SOFA_DECL_CLASS(AddNoiseEngine) @@ -50,6 +49,7 @@ int AddNoiseEngineClass = core::RegisterObject("AddNoiseEngine") template class AddNoiseEngine; + } // namespace engine } // namespace component diff --git a/src/genericComponents/AddNoiseEngine.h b/src/genericComponents/AddNoiseEngine.h index 7fce9d0b..2a0675d0 100644 --- a/src/genericComponents/AddNoiseEngine.h +++ b/src/genericComponents/AddNoiseEngine.h @@ -30,12 +30,12 @@ #include #include -#include -#include +#include +#include #include #include -#include +#include #include #include @@ -64,7 +64,7 @@ class AddNoiseEngine : public sofa::core::DataEngine typedef typename DataTypes::Coord Coord; typedef typename DataTypes::Deriv Deriv; typedef typename DataTypes::Real Real; - typedef typename sofa::defaulttype::Vec3d Vector3; + typedef typename sofa::type::Vec3d Vector3; typedef sofa::core::topology::Topology::Edge Edge; AddNoiseEngine(); diff --git a/src/genericComponents/CorrectionForceField.cpp b/src/genericComponents/CorrectionForceField.cpp index 2741c6f3..e24c46a8 100644 --- a/src/genericComponents/CorrectionForceField.cpp +++ b/src/genericComponents/CorrectionForceField.cpp @@ -21,9 +21,11 @@ ******************************************************************************/ #include "genericComponents/CorrectionForceField.inl" -#include +#include #include + + namespace sofa { @@ -36,13 +38,13 @@ namespace forcefield SOFA_DECL_CLASS(CorrectionForceField) - int CorrectionForceFieldClass = core::RegisterObject("Simple elastic springs applied to given degrees of freedom between their current and rest shape position") .add< CorrectionForceField >() .add< CorrectionForceField >() ; + } // namespace forcefield } // namespace component diff --git a/src/genericComponents/CorrectionForceField.h b/src/genericComponents/CorrectionForceField.h index 59b8781f..e9d0c6e4 100644 --- a/src/genericComponents/CorrectionForceField.h +++ b/src/genericComponents/CorrectionForceField.h @@ -65,8 +65,8 @@ class CorrectionForceField : public core::behavior::ForceField typedef typename DataTypes::CPos CPos; typedef typename DataTypes::Deriv Deriv; typedef typename DataTypes::Real Real; - typedef helper::vector< unsigned int > VecIndex; - typedef helper::vector< Real > VecReal; + typedef type::vector< unsigned int > VecIndex; + typedef type::vector< Real > VecReal; typedef sofa::component::topology::PointSubsetData< VecIndex > SetIndex; typedef core::objectmodel::Data DataVecCoord; @@ -74,15 +74,18 @@ class CorrectionForceField : public core::behavior::ForceField Data< Deriv > d_force; - SetIndex d_indices; - Data< VecDeriv > d_forces; - Data> d_Optimforces; - Data d_delta; - Data d_paramF; + SetIndex d_indices; + Data< VecDeriv > d_forces; + Data< type::vector > d_Optimforces; + Data< double > d_delta; + Data< double > d_paramF; + type::vector m_active; + + +protected: + CorrectionForceField(); - protected: - CorrectionForceField(); public: /// BaseObject initialization method. void bwdInit() override; @@ -101,15 +104,10 @@ class CorrectionForceField : public core::behavior::ForceField return 0.0; } - virtual void draw(const core::visual::VisualParams* /* vparams */) override; - void plusF(); - void minusF(); + void plusF(); + void minusF(); void handleEvent(sofa::core::objectmodel::Event *event) override; - - helper::vector m_active; - - }; diff --git a/src/genericComponents/CorrectionForceField.inl b/src/genericComponents/CorrectionForceField.inl index 19fae552..6f18f691 100644 --- a/src/genericComponents/CorrectionForceField.inl +++ b/src/genericComponents/CorrectionForceField.inl @@ -26,7 +26,7 @@ #include #include #include -#include +#include #include #include #include @@ -49,20 +49,12 @@ namespace forcefield template CorrectionForceField::CorrectionForceField() : d_force(initData(&d_force, "force","applied forces at each point")) - - , d_indices(initData(&d_indices, "indices", - "indices where the forces are applied")) - - , d_forces(initData(&d_forces, "forces", - "applied forces at each point")) - , d_Optimforces(initData(&d_Optimforces, "optimForces", - "applied forces at each point")) - , d_delta(initData(&d_delta, "delta","translation rotation vector")) - - , d_paramF(initData(&d_paramF, "paramF", - "parameter modeling force uncertainty")) -{ -} + , d_indices(initData(&d_indices, "indices", "indices where the forces are applied")) + , d_forces(initData(&d_forces, "forces", "applied forces at each point")) + , d_Optimforces(initData(&d_Optimforces, "optimForces", "applied forces at each point")) + , d_delta(initData(&d_delta, "delta", "translation rotation vector")) + , d_paramF(initData(&d_paramF, "paramF", "parameter modeling force uncertainty")) +{ } template @@ -73,6 +65,7 @@ void CorrectionForceField::bwdInit() } + template void CorrectionForceField::plusF() { @@ -90,23 +83,25 @@ void CorrectionForceField::plusF() { std::cout<<"newForc: "<< d_Optimforces.getValue() < void CorrectionForceField::minusF() { double temp = d_Optimforces.getValue()[0]; - temp-= d_delta.getValue(); - helper::vector T; + temp -= d_delta.getValue(); + type::vector T; T.resize(d_Optimforces.getValue().size()); - T[0]=d_Optimforces.getValue()[0]; + T[0] = d_Optimforces.getValue()[0]; - T[1]=temp; - for (unsigned int i=2; i void CorrectionForceField::handleEvent(sofa::core::objectmodel::Event *event) { @@ -119,6 +114,7 @@ void CorrectionForceField::handleEvent(sofa::core::objectmodel::Event } } + template void CorrectionForceField::addForce(const core::MechanicalParams* /*params*/, DataVecDeriv& f1, const DataVecCoord& p1, const DataVecDeriv&) @@ -184,6 +180,7 @@ void CorrectionForceField::addKToMatrix(const core::MechanicalParams* // } } + template void CorrectionForceField::draw(const core::visual::VisualParams* /* vparams */) { @@ -210,7 +207,6 @@ void CorrectionForceField::draw(const core::visual::VisualParams* /* // glPointSize(1); // glLineWidth(1); - } diff --git a/src/genericComponents/FilterEvents.cpp b/src/genericComponents/FilterEvents.cpp index 6c347e61..39c75c5b 100644 --- a/src/genericComponents/FilterEvents.cpp +++ b/src/genericComponents/FilterEvents.cpp @@ -26,6 +26,7 @@ #include "FilterEvents.h" + namespace sofa { @@ -36,6 +37,7 @@ namespace stochastic { + SOFA_EVENT_CPP( PredictionEndEvent ) PredictionEndEvent::PredictionEndEvent(SReal dt) @@ -64,9 +66,9 @@ CorrectionEndEvent::~CorrectionEndEvent() -} /// sofa +} // namespace stochastic -} /// component +} // namespace component -} /// stochastic +} // namespace sofa diff --git a/src/genericComponents/FilterEvents.h b/src/genericComponents/FilterEvents.h index 84c308c2..96041b4d 100644 --- a/src/genericComponents/FilterEvents.h +++ b/src/genericComponents/FilterEvents.h @@ -38,8 +38,10 @@ namespace stochastic class SOFA_STOCHASTIC_API PredictionEndEvent : public sofa::core::objectmodel::Event { -public: +protected: + SReal dt; +public: SOFA_EVENT_H( PredictionEndEvent ) PredictionEndEvent( SReal dt ); @@ -49,16 +51,15 @@ class SOFA_STOCHASTIC_API PredictionEndEvent : public sofa::core::objectmodel:: SReal getDt() const { return dt; } inline static const char* GetClassName() { return "AssimilationEndEvent"; } - -protected: - SReal dt; }; class SOFA_STOCHASTIC_API CorrectionEndEvent : public sofa::core::objectmodel::Event { -public: +protected: + SReal dt; +public: SOFA_EVENT_H( CorrectionEndEvent ) CorrectionEndEvent( SReal dt ); @@ -68,16 +69,13 @@ class SOFA_STOCHASTIC_API CorrectionEndEvent : public sofa::core::objectmodel::E SReal getDt() const { return dt; } inline static const char* GetClassName() { return "AssimilationEndEvent"; } - -protected: - SReal dt; }; -} /// stochastic +} // stochastic -} /// components +} // components -} /// sofa +} // sofa diff --git a/src/genericComponents/FilteringUpdateEngine.cpp b/src/genericComponents/FilteringUpdateEngine.cpp index 34bbb6c2..2bb8d173 100644 --- a/src/genericComponents/FilteringUpdateEngine.cpp +++ b/src/genericComponents/FilteringUpdateEngine.cpp @@ -35,6 +35,7 @@ namespace engine { + SOFA_DECL_CLASS(FilteringUpdateEngine) // Register in the Factory @@ -55,6 +56,7 @@ template class FilteringUpdateEngine; #endif + } // namespace engine } // namespace component diff --git a/src/genericComponents/FilteringUpdateEngine.h b/src/genericComponents/FilteringUpdateEngine.h index 56eea28b..518d84f8 100644 --- a/src/genericComponents/FilteringUpdateEngine.h +++ b/src/genericComponents/FilteringUpdateEngine.h @@ -26,12 +26,12 @@ #include //#include -#include -#include +#include +#include #include -#include "../stochasticFiltering/UKFilterClassic.h" +#include "../stochasticFiltering/StochasticFilterBase.h" #include "../stochasticFiltering/StochasticStateWrapperBase.h" #include "OptimParams.h" @@ -59,7 +59,7 @@ class FilteringUpdateEngine : public core::objectmodel::BaseObject SOFA_CLASS(FilteringUpdateEngine, core::objectmodel::BaseObject); typedef core::objectmodel::BaseObject Inherit; - typedef typename sofa::defaulttype::Vec3d Vector3; + typedef typename sofa::type::Vec3d Vector3; FilteringUpdateEngine(); ~FilteringUpdateEngine() {} @@ -67,9 +67,9 @@ class FilteringUpdateEngine : public core::objectmodel::BaseObject Data d_addData; Data d_updateData; - component::stochastic::UKFilterClassic* filter; - helper::vector*> stateWrappers; - helper::vector vecOptimParams; + component::stochastic::StochasticFilterBase* filter; + type::vector*> stateWrappers; + type::vector vecOptimParams; // found planes @@ -94,8 +94,7 @@ class FilteringUpdateEngine : public core::objectmodel::BaseObject //{ // return "double"; //} - -}; /// class +}; diff --git a/src/genericComponents/Indices2ValuesTransformer.cpp b/src/genericComponents/Indices2ValuesTransformer.cpp index c93f7e18..af9ece8f 100644 --- a/src/genericComponents/Indices2ValuesTransformer.cpp +++ b/src/genericComponents/Indices2ValuesTransformer.cpp @@ -26,6 +26,7 @@ #include + namespace sofa { @@ -35,17 +36,17 @@ namespace component namespace engine { -using namespace sofa; -using namespace sofa::defaulttype; + SOFA_DECL_CLASS(Indices2ValuesTransformer) int Indices2ValuesTransformerClass = core::RegisterObject("?") - .add< Indices2ValuesTransformer >(true) + .add< Indices2ValuesTransformer >(true) ; -template class SOFA_OPTIMUSPLUGIN_API Indices2ValuesTransformer; +template class SOFA_OPTIMUSPLUGIN_API Indices2ValuesTransformer; + } // namespace engine @@ -53,3 +54,4 @@ template class SOFA_OPTIMUSPLUGIN_API Indices2ValuesTransformer; } // namespace component } // namespace sofa + diff --git a/src/genericComponents/Indices2ValuesTransformer.h b/src/genericComponents/Indices2ValuesTransformer.h index 8356d013..22b1388e 100644 --- a/src/genericComponents/Indices2ValuesTransformer.h +++ b/src/genericComponents/Indices2ValuesTransformer.h @@ -27,7 +27,7 @@ #include #include -#include +#include #include "../initOptimusPlugin.h" @@ -51,13 +51,26 @@ class Indices2ValuesTransformer : public sofa::core::DataEngine typedef typename DataTypes::Coord Coord; typedef typename DataTypes::VecCoord VecCoord; typedef typename DataTypes::Real Real; - typedef sofa::defaulttype::Vec<3,Real> Vec3; + typedef sofa::type::Vec<3,Real> Vec3; typedef unsigned int Index; -protected: + //Input + Data > f_inputValues; ///< Already existing values (can be empty) + Data > f_indices; ///< Indices to map value on + Data > f_values1, f_values2; ///< Values to map indices on + + //Output + Data > f_outputValues; ///< New map between indices and values + //Parameter + Data p_defaultValue; ///< Default value for indices without any value + Data d_transformation; + + +protected: Indices2ValuesTransformer(); ~Indices2ValuesTransformer() {} + public: void init() override; void reinit() override; @@ -72,28 +85,11 @@ class Indices2ValuesTransformer : public sofa::core::DataEngine { return DataTypes::Name(); } - - //Input - Data > f_inputValues; ///< Already existing values (can be empty) - Data > f_indices; ///< Indices to map value on - Data > f_values1, f_values2; ///< Values to map indices on - - //Output - Data > f_outputValues; ///< New map between indices and values - - //Parameter - Data p_defaultValue; ///< Default value for indices without any value - Data d_transformation; - }; + #if defined(SOFA_EXTERN_TEMPLATE) && !defined(SOFA_COMPONENT_ENGINE_INDICES2VALUESTRANSFORMER_CPP) -#ifndef SOFA_FLOAT -extern template class SOFA_GENERAL_ENGINE_API Indices2ValuesTransformer; -#endif //SOFA_FLOAT -#ifndef SOFA_DOUBLE -extern template class SOFA_GENERAL_ENGINE_API Indices2ValuesTransformer; -#endif //SOFA_DOUBLE +extern template class SOFA_OPTIMUSPLUGIN_API Indices2ValuesTransformer; #endif diff --git a/src/genericComponents/Indices2ValuesTransformer.inl b/src/genericComponents/Indices2ValuesTransformer.inl index 6c4b15a6..0671dccf 100644 --- a/src/genericComponents/Indices2ValuesTransformer.inl +++ b/src/genericComponents/Indices2ValuesTransformer.inl @@ -78,12 +78,12 @@ void Indices2ValuesTransformer::doUpdate() { cleanDirty(); - helper::ReadAccessor< Data< helper::vector > > inputValues = f_inputValues; - helper::ReadAccessor< Data< helper::vector > > indices = f_indices; - helper::ReadAccessor< Data< helper::vector > > values1 = f_values1; - helper::ReadAccessor< Data< helper::vector > > values2 = f_values2; + helper::ReadAccessor< Data< type::vector > > inputValues = f_inputValues; + helper::ReadAccessor< Data< type::vector > > indices = f_indices; + helper::ReadAccessor< Data< type::vector > > values1 = f_values1; + helper::ReadAccessor< Data< type::vector > > values2 = f_values2; - helper::WriteAccessor< Data< helper::vector > > outputValues = f_outputValues; + helper::WriteAccessor< Data< type::vector > > outputValues = f_outputValues; const Real& defaultValue = p_defaultValue.getValue(); diff --git a/src/genericComponents/MatrixCovariance.cpp b/src/genericComponents/MatrixCovariance.cpp index b450cfcd..cefcdd2e 100644 --- a/src/genericComponents/MatrixCovariance.cpp +++ b/src/genericComponents/MatrixCovariance.cpp @@ -26,6 +26,8 @@ #include #include "MatrixCovariance.inl" + + namespace sofa { @@ -36,7 +38,6 @@ namespace container { -/// DECLARATIONS SOFA_DECL_CLASS(MatrixCovariance) @@ -49,8 +50,10 @@ int MatrixCovarianceClass = core::RegisterObject("Covariance data") template class SOFA_OPTIMUSPLUGIN_API MatrixCovariance; + } // namespace container } // namespace component } // namespace sofa + diff --git a/src/genericComponents/MatrixCovariance.h b/src/genericComponents/MatrixCovariance.h index 61c5eec5..71272d26 100644 --- a/src/genericComponents/MatrixCovariance.h +++ b/src/genericComponents/MatrixCovariance.h @@ -48,6 +48,14 @@ class MatrixCovariance : public sofa::core::objectmodel::BaseObject public: SOFA_CLASS(SOFA_TEMPLATE(MatrixCovariance, DataTypes), BaseObject); +protected: + Data> d_val; // real actual value of parameters + Data d_rows; // amount of rows for covariance matrix + Data d_columns; // amount of columns for covariance matrix + + virtual void handleEvent(core::objectmodel::Event */*event*/) override {} + +public: MatrixCovariance(); ~MatrixCovariance(); void init() override; @@ -56,15 +64,7 @@ class MatrixCovariance : public sofa::core::objectmodel::BaseObject unsigned int Rows() { return d_rows.getValue(); } unsigned int Columns() { return d_columns.getValue(); } - const helper::vector& GetData() { return d_val.getValue(); } - -protected: - Data> d_val; // real actual value of parameters - Data d_rows; // amount of rows for covariance matrix - Data d_columns; // amount of columns for covariance matrix - - virtual void handleEvent(core::objectmodel::Event */*event*/) override {} - + const type::vector& GetData() { return d_val.getValue(); } }; diff --git a/src/genericComponents/MatrixCovariance.inl b/src/genericComponents/MatrixCovariance.inl index b0452734..fdeac3a2 100644 --- a/src/genericComponents/MatrixCovariance.inl +++ b/src/genericComponents/MatrixCovariance.inl @@ -23,10 +23,9 @@ #include "MatrixCovariance.h" -#include -#include +#include +#include #include -#include #include #include diff --git a/src/genericComponents/ObservationSource.h b/src/genericComponents/ObservationSource.h index ed026a06..7e7b5698 100644 --- a/src/genericComponents/ObservationSource.h +++ b/src/genericComponents/ObservationSource.h @@ -43,12 +43,8 @@ class ObservationSource : public sofa::core::objectmodel::BaseObject public: typedef sofa::core::objectmodel::BaseObject Inherit; - ObservationSource() - : Inherit() - {} - + ObservationSource() : Inherit() {} virtual void getObservation(double _time) = 0; - }; diff --git a/src/genericComponents/OptimMonitor.cpp b/src/genericComponents/OptimMonitor.cpp index 15447a93..b4a42bae 100644 --- a/src/genericComponents/OptimMonitor.cpp +++ b/src/genericComponents/OptimMonitor.cpp @@ -49,11 +49,13 @@ int OptimMonitorClass = core::RegisterObject("OptimMonitoring of particles") .add< OptimMonitor >() ; + template class SOFA_OPTIMUSPLUGIN_API OptimMonitor; template class SOFA_OPTIMUSPLUGIN_API OptimMonitor; template class SOFA_OPTIMUSPLUGIN_API OptimMonitor; + } // namespace misc } // namespace component diff --git a/src/genericComponents/OptimMonitor.h b/src/genericComponents/OptimMonitor.h index affaa072..855d188e 100644 --- a/src/genericComponents/OptimMonitor.h +++ b/src/genericComponents/OptimMonitor.h @@ -25,7 +25,7 @@ #include #include -#include +#include #include #include @@ -40,12 +40,12 @@ namespace component namespace misc { + /** * Class which replaces the default monitor in SOFA. Differences: * 1. Export on special Optimus events (see initialization according to d_exportOnEvent * 2. Reinitialize pointers to mechanical object data everytime before the export (needed for some Python scenes) **/ - template class OptimMonitor : public virtual core::objectmodel::BaseObject { @@ -60,47 +60,9 @@ class OptimMonitor : public virtual core::objectmodel::BaseObject typedef typename DataTypes::Coord Coord; typedef typename DataTypes::Deriv Deriv; -protected: - OptimMonitor (); - ~OptimMonitor (); -public: - /// init data - virtual void init () override; - - /// reset OptimMonitored values - virtual void reset () override; - - /** initialize gnuplot files - * called when ExportGnuplot box is checked - */ - virtual void reinit() override; - - /** function called at every step of simulation; - * store mechanical state vectors (forces, positions, velocities) into - * the OptimMonitorData nested class. The filter (which position(s), velocity(ies) or *force(s) are displayed) is made in the gui - */ - virtual void handleEvent( core::objectmodel::Event* ev ) override; - - virtual void draw (const core::visual::VisualParams* vparams) override; - - /// create gnuplot files - virtual void initGnuplot ( const std::string path ); - - /// write in gnuplot files the OptimMonitored desired data (velocities,positions,forces) - virtual void exportGnuplot ( Real time ); - - virtual std::string getTemplateName() const override - { - return templateName(this); - } - - static std::string templateName(const OptimMonitor* = NULL) - { - return DataTypes::Name(); - } /// Editable Data - Data< helper::vector > d_indices; + Data< type::vector > d_indices; Data< bool > d_saveXToGnuplot; ///< export OptimMonitored positions as gnuplot file Data< bool > d_saveVToGnuplot; ///< export OptimMonitored velocities as gnuplot file @@ -129,7 +91,6 @@ class OptimMonitor : public virtual core::objectmodel::BaseObject protected: - std::ofstream* m_saveGnuplotX; std::ofstream* m_saveGnuplotV; std::ofstream* m_saveGnuplotF; @@ -142,9 +103,50 @@ class OptimMonitor : public virtual core::objectmodel::BaseObject double m_saveDt; ///< use for trajectoriesPrecision (save to file value only if trajectoriesPrecision <= internalDt) double m_internalDt; ///< use for trajectoriesPrecision (save value only if trajectoriesPrecision <= internalDt) - sofa::helper::vector < sofa::helper::vector > m_savedPos; ///< store all the monitored positions, for trajectories display + sofa::type::vector < sofa::type::vector > m_savedPos; ///< store all the monitored positions, for trajectories display + + + OptimMonitor(); + ~OptimMonitor(); + +public: + /// init data + virtual void init() override; + + /// reset OptimMonitored values + virtual void reset() override; + + /** initialize gnuplot files + * called when ExportGnuplot box is checked + */ + virtual void reinit() override; + + /** function called at every step of simulation; + * store mechanical state vectors (forces, positions, velocities) into + * the OptimMonitorData nested class. The filter (which position(s), velocity(ies) or *force(s) are displayed) is made in the gui + */ + virtual void handleEvent(core::objectmodel::Event* ev) override; + + virtual void draw(const core::visual::VisualParams* vparams) override; + + /// create gnuplot files + virtual void initGnuplot(const std::string path); + + /// write in gnuplot files the OptimMonitored desired data (velocities,positions,forces) + virtual void exportGnuplot(Real time); + + virtual std::string getTemplateName() const override + { + return templateName(this); + } + + static std::string templateName(const OptimMonitor* = NULL) + { + return DataTypes::Name(); + } }; + #if defined(SOFA_EXTERN_TEMPLATE) && !defined(SOFA_COMPONENT_MISC_OPTIMMONITOR_CPP) extern template class SOFA_OPTIMUSPLUGIN_API OptimMonitor; extern template class SOFA_OPTIMUSPLUGIN_API OptimMonitor; diff --git a/src/genericComponents/OptimMonitor.inl b/src/genericComponents/OptimMonitor.inl index 4769a269..4892a2ed 100644 --- a/src/genericComponents/OptimMonitor.inl +++ b/src/genericComponents/OptimMonitor.inl @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include "../genericComponents/FilterEvents.h" @@ -77,10 +77,10 @@ OptimMonitor::OptimMonitor() { if (!f_listening.isSet()) f_listening.setValue(true); - d_positionsColor=RGBAColor::yellow(); - d_velocitiesColor=RGBAColor::yellow(); - d_forcesColor=RGBAColor::yellow(); - d_trajectoriesColor=RGBAColor::yellow(); + d_positionsColor = RGBAColor::yellow(); + d_velocitiesColor = RGBAColor::yellow(); + d_forcesColor = RGBAColor::yellow(); + d_trajectoriesColor = RGBAColor::yellow(); } /////////////////////////// end OptimMonitor /////////////////////////////////// @@ -126,7 +126,7 @@ void OptimMonitor::init() /// initial export of the data if (d_saveZeroStep.getValue()) { if ( d_saveXToGnuplot.getValue() || d_saveVToGnuplot.getValue() || d_saveFToGnuplot.getValue() ) { - exportGnuplot ( (Real) this ->getTime() ); + exportGnuplot( (Real)this->getTime() ); } if (d_showTrajectories.getValue()) @@ -147,7 +147,7 @@ template void OptimMonitor::reset() { m_internalDt = 0.0; - for(unsigned int i=0 ; i::draw(const core::visual::VisualParams* vparams) vparams->drawTool()->setLightingEnabled(false); if (d_showPositions.getValue()) { - helper::vector points; + type::vector points; for (unsigned int i=0; i < d_indices.getValue().size(); ++i) { Coord posvertex = (*m_X)[d_indices.getValue()[i]]; - points.push_back(defaulttype::Vector3(posvertex[0],posvertex[1],posvertex[2])); + points.push_back(type::Vector3(posvertex[0], posvertex[1], posvertex[2])); } vparams->drawTool()->drawPoints(points, (float)(d_showSizeFactor.getValue())*2.0f, d_positionsColor.getValue()); @@ -244,9 +244,9 @@ void OptimMonitor::draw(const core::visual::VisualParams* vparams) for (unsigned int i=0; i < d_indices.getValue().size(); ++i) { Coord posVertex = (*m_X)[d_indices.getValue()[i]]; - defaulttype::Vector3 p1(posVertex[0],posVertex[1],posVertex[2]); + type::Vector3 p1(posVertex[0],posVertex[1],posVertex[2]); Deriv velVertex = (*m_V)[d_indices.getValue()[i]]; - defaulttype::Vector3 p2(d_showSizeFactor.getValue()*velVertex[0],d_showSizeFactor.getValue()*velVertex[1],d_showSizeFactor.getValue()*velVertex[2]); + type::Vector3 p2(d_showSizeFactor.getValue()*velVertex[0],d_showSizeFactor.getValue()*velVertex[1],d_showSizeFactor.getValue()*velVertex[2]); if(p2.norm() > d_showMinThreshold.getValue()) vparams->drawTool()->drawArrow(p1, p1+p2, (float)(d_showSizeFactor.getValue()*p2.norm()/20.0), d_velocitiesColor.getValue()); @@ -258,11 +258,11 @@ void OptimMonitor::draw(const core::visual::VisualParams* vparams) for (unsigned int i=0; i < d_indices.getValue().size(); ++i) { Coord posVertex = (*m_X)[d_indices.getValue()[i]]; - defaulttype::Vector3 p1(posVertex[0],posVertex[1],posVertex[2]); + type::Vector3 p1(posVertex[0],posVertex[1],posVertex[2]); Deriv forceVertex = (*m_F)[d_indices.getValue()[i]]; - defaulttype::Vector3 p2(d_showSizeFactor.getValue()*forceVertex[0],d_showSizeFactor.getValue()*forceVertex[1],d_showSizeFactor.getValue()*forceVertex[2]); + type::Vector3 p2(d_showSizeFactor.getValue()*forceVertex[0],d_showSizeFactor.getValue()*forceVertex[1],d_showSizeFactor.getValue()*forceVertex[2]); - if(p2.norm() > d_showMinThreshold.getValue()) + if (p2.norm() > d_showMinThreshold.getValue()) vparams->drawTool()->drawArrow(p1, p1+p2, (float)(d_showSizeFactor.getValue()*p2.norm()/20.0), d_forcesColor.getValue()); } } @@ -272,14 +272,14 @@ void OptimMonitor::draw(const core::visual::VisualParams* vparams) m_internalDt += this->getContext()->getDt(); for (unsigned int i=0; i < d_indices.getValue().size(); ++i) { - helper::vector points; + type::vector points; Coord point; for (unsigned int j=0 ; jdrawTool()->drawLines(points, (float)(d_showSizeFactor.getValue()*0.2), d_trajectoriesColor.getValue()); } @@ -310,7 +310,6 @@ void OptimMonitor::initGnuplot ( const std::string path ) for (unsigned int i = 0; i < d_indices.getValue().size(); i++) ( *m_saveGnuplotX ) << d_indices.getValue()[i] << " "; ( *m_saveGnuplotX ) << std::endl; - } if ( d_saveVToGnuplot.getValue() ) @@ -383,6 +382,7 @@ void OptimMonitor::exportGnuplot ( Real time ) } /////////////////////////////////////////////////////////////////////////// + } // namespace misc } // namespace component diff --git a/src/genericComponents/OptimParams.cpp b/src/genericComponents/OptimParams.cpp index 05418aea..96e19da5 100644 --- a/src/genericComponents/OptimParams.cpp +++ b/src/genericComponents/OptimParams.cpp @@ -39,13 +39,14 @@ namespace container { -using namespace defaulttype; +using namespace defaulttype; /// SPECIALIZATIONS FOR vector template<> -void OptimParams >::getInitVariance(DVec& _variance) { +void OptimParams >::getInitVariance(DVec& _variance) +{ _variance.resize(m_stdev.getValue().size()); switch (transParamType) { @@ -59,38 +60,41 @@ void OptimParams >::getInitVariance(DVec& _variance } } + + template<> -void OptimParams >::vectorToParams(VectorXd& _vector) { - helper::WriteAccessor > > val = m_val; - helper::ReadAccessor > > minVal = m_minVal; - helper::ReadAccessor > > maxVal = m_maxVal; +void OptimParams >::vectorToParams(VectorXd& _vector) +{ + helper::WriteAccessor< Data< type::vector > > val = m_val; + helper::ReadAccessor< Data< type::vector > > minVal = m_minVal; + helper::ReadAccessor< Data< type::vector > > maxVal = m_maxVal; switch (transParamType) { case 1: for (size_t i = 0; i < this->paramIndices.size(); i++) { - val[i]=fabs(_vector[this->paramIndices[i]]); + val[i] = fabs(_vector[this->paramIndices[i]]); } break; case 2: // std::cout << "backward step " << std::endl; for (size_t i = 0; i < this->paramIndices.size(); i++) { - val[i]=sigmoid(_vector[this->paramIndices[i]], maxVal[i], minVal[i]); - //std::cout << val[i] << std::endl; + val[i] = sigmoid(_vector[this->paramIndices[i]], maxVal[i], minVal[i]); + // std::cout << val[i] << std::endl; } break; case 3: for (size_t i = 0; i < this->paramIndices.size(); i++) { - val[i]=exp(_vector[this->paramIndices[i]]); - //std::cout << val[i] << std::endl; + val[i] = exp(_vector[this->paramIndices[i]]); + // std::cout << val[i] << std::endl; } break; case 4: for (size_t i = 0; i < this->paramIndices.size(); i++) { double inVal = _vector[this->paramIndices[i]]; - //std::cout << "HERE: " << inVal << " vs " << minVal[i] << " " <<(inVal < minVal[i]) << std::endl; + // std::cout << "HERE: " << inVal << " vs " << minVal[i] << " " <<(inVal < minVal[i]) << std::endl; val[i] = (inVal < minVal[i]) ? minVal[i] : inVal; val[i] = (val[i] > maxVal[i]) ? maxVal[i] : val[i]; - //std::cout << " val: " << val[i] << std::endl; + // std::cout << " val: " << val[i] << std::endl; } break; default: @@ -98,35 +102,37 @@ void OptimParams >::vectorToParams(VectorXd& _vecto val[i] = _vector[this->paramIndices[i]]; } - /*std::cout <<"Values used in SOFA:" << std::endl; - for (size_t i = 0; i < this->paramIndices.size(); i++) { - std::cout << " " << val[i]; - } - std::cout << std::endl;*/ + // std::cout <<"Values used in SOFA:" << std::endl; + // for (size_t i = 0; i < this->paramIndices.size(); i++) { + // std::cout << " " << val[i]; + // } + // std::cout << std::endl; } + template<> -void OptimParams >::vectorToParams(VectorXf& _vector) { - helper::WriteAccessor > > val = m_val; - helper::ReadAccessor > > minVal = m_minVal; - helper::ReadAccessor > > maxVal = m_maxVal; +void OptimParams >::vectorToParams(VectorXf& _vector) +{ + helper::WriteAccessor< Data< type::vector > > val = m_val; + helper::ReadAccessor< Data< type::vector > > minVal = m_minVal; + helper::ReadAccessor< Data< type::vector > > maxVal = m_maxVal; switch (transParamType) { case 1: for (size_t i = 0; i < this->paramIndices.size(); i++) { - val[i]=double(fabs(_vector[this->paramIndices[i]])); + val[i] = double(fabs(_vector[this->paramIndices[i]])); } break; case 2: for (size_t i = 0; i < this->paramIndices.size(); i++) { - val[i]=sigmoid(_vector[this->paramIndices[i]], maxVal[i], minVal[i]); + val[i] = sigmoid(_vector[this->paramIndices[i]], maxVal[i], minVal[i]); } break; case 3: for (size_t i = 0; i < this->paramIndices.size(); i++) { - val[i]=exp(_vector[this->paramIndices[i]]); - //std::cout << val[i] << std::endl; + val[i] = exp(_vector[this->paramIndices[i]]); + // std::cout << val[i] << std::endl; } break; case 4: @@ -143,9 +149,11 @@ void OptimParams >::vectorToParams(VectorXf& _vecto } + template<> -void OptimParams >::paramsToVector(VectorXd& _vector) { - helper::ReadAccessor > > val = m_val; // real values of parameters +void OptimParams >::paramsToVector(VectorXd& _vector) +{ + helper::ReadAccessor< Data< type::vector > > val = m_val; // real values of parameters switch (transParamType) { case 1: @@ -156,25 +164,28 @@ void OptimParams >::paramsToVector(VectorXd& _vecto // std::cout << "forward step " << std::endl; for (size_t i = 0; i < this->paramIndices.size(); i++) { _vector[paramIndices[i]] = double(logit(val[i], m_maxVal.getValue()[i], m_minVal.getValue()[i])); - //std::cout << val[i] << " " << logit(val[i], m_maxVal.getValue()[i], m_minVal.getValue()[i]) << std::endl; + // std::cout << val[i] << " " << logit(val[i], m_maxVal.getValue()[i], m_minVal.getValue()[i]) << std::endl; } break; case 3: for (size_t i = 0; i < this->paramIndices.size(); i++) { _vector[paramIndices[i]] = double(log(val[i])); - //std::cout << val[i] << std::endl; + // std::cout << val[i] << std::endl; } break; default: for (size_t i = 0; i < paramIndices.size(); i++) _vector[paramIndices[i]] = val[i]; } - //std::cout << "paramsToVector raVal\n" << _vector << std::endl; + // std::cout << "paramsToVector raVal\n" << _vector << std::endl; } + + template<> -void OptimParams >::paramsToVector(VectorXf& _vector) { - helper::ReadAccessor > > val = m_val; // real values of parameters +void OptimParams >::paramsToVector(VectorXf& _vector) +{ + helper::ReadAccessor< Data< type::vector > > val = m_val; // real values of parameters switch (transParamType) { case 1: @@ -189,7 +200,7 @@ void OptimParams >::paramsToVector(VectorXf& _vecto case 3: for (size_t i = 0; i < this->paramIndices.size(); i++) { _vector[paramIndices[i]] = float(log(val[i])); - //std::cout << val[i] << std::endl; + // std::cout << val[i] << std::endl; } break; default: @@ -199,14 +210,17 @@ void OptimParams >::paramsToVector(VectorXf& _vecto } + template<> -void OptimParams >::init() { +void OptimParams >::init() +{ Inherit::init(); m_dim = 1; /// take the initial value and initial stdev - if (!this->m_prescribedParamKeys.getValue().empty()) { - helper::ReadAccessor > >keys = m_prescribedParamKeys; + if ( !this->m_prescribedParamKeys.getValue().empty() ) + { + helper::ReadAccessor< Data< type::vector > >keys = m_prescribedParamKeys; size_t numParams = this->m_dim * this->m_numParams.getValue(); size_t numKeyValues = keys.size(); @@ -222,9 +236,9 @@ void OptimParams >::init() { for (size_t i = 0; i < numKeys; i++) { m_paramKeys[i].first = keys[i*(numParams+1)]; - helper::vector v(numParams); + type::vector v(numParams); for (size_t j = 0; j < numParams; j++) - v[j] = keys[i*(numParams+1)+j+1]; + v[j] = keys[i * (numParams + 1) + j + 1]; m_paramKeys[i].second = v; } @@ -235,14 +249,14 @@ void OptimParams >::init() { std::cout << std::endl; }*/ - helper::WriteAccessor > > initVal = m_initVal; + helper::WriteAccessor< Data< sofa::type::vector > > initVal = m_initVal; initVal.resize(numParams); for (size_t i = 0; i < numParams; i++) initVal[i] = m_paramKeys[0].second[i]; } } - helper::ReadAccessor > > initVal = m_initVal; + helper::ReadAccessor > > initVal = m_initVal; size_t nInitVal = initVal.size(); if ( (m_numParams.getValue() == 0 && nInitVal > 0) || m_numParams.getValue() == nInitVal) { @@ -255,7 +269,7 @@ void OptimParams >::init() { double value = initVal[0]; sout << this->getName() << ": Resizing init value vector to " << nInitVal << sendl; - helper::WriteAccessor > > wInitVal = m_initVal; + helper::WriteAccessor > > wInitVal = m_initVal; wInitVal.wref().resize(nInitVal); for (size_t i = 0; i < nInitVal; i++) wInitVal[i] = value; @@ -268,7 +282,7 @@ void OptimParams >::init() { size_t numParams = m_numParams.getValue(); if (numParams > 1 && m_maxVal.getValue().size() == 1) { const double val = m_maxVal.getValue()[0]; - helper::WriteAccessor > > wMaxVal = m_maxVal; + helper::WriteAccessor > > wMaxVal = m_maxVal; wMaxVal.resize(numParams, val); for (size_t i = 0; i < numParams; i++) wMaxVal[i] = val; @@ -276,35 +290,35 @@ void OptimParams >::init() { if (numParams > 1 && m_minVal.getValue().size() == 1) { const double val = m_minVal.getValue()[0]; - helper::WriteAccessor > > wMinVal = m_minVal; + helper::WriteAccessor > > wMinVal = m_minVal; wMinVal.resize(numParams, val); for (size_t i = 0; i < numParams; i++) wMinVal[i] = val; } if (nInitVal != 0) { - helper::WriteAccessor > > val = m_val; + helper::WriteAccessor > > val = m_val; if (val.size() == 0) { val.resize(nInitVal); for (size_t i = 0; i < nInitVal; i++) val[i] = initVal[i]; } - helper::WriteAccessor > > minVal = m_minVal; + helper::WriteAccessor > > minVal = m_minVal; if (minVal.size() == 0) { minVal.resize(nInitVal); for (size_t i = 0; i < nInitVal; i++) minVal[i] = initVal[i]; } - helper::WriteAccessor > > maxVal = m_maxVal; + helper::WriteAccessor > > maxVal = m_maxVal; if (maxVal.size() == 0) { maxVal.resize(nInitVal); for (size_t i = 0; i < nInitVal; i++) maxVal[i] = initVal[i]; } - helper::WriteAccessor > > stdev = m_stdev; + helper::WriteAccessor > > stdev = m_stdev; if (stdev.size() == 0) { stdev.resize(nInitVal, 0.0); PRNW("Standard deviation not used, setting to 0!") @@ -327,43 +341,44 @@ void OptimParams >::init() { } + template<> -void OptimParams >::appendParameters() +void OptimParams >::appendParameters() { size_t numParams = this->m_dim * this->m_numParams.getValue(); std::cout << "Old number of parameters: " << numParams << std::endl; // read added parameters - helper::ReadAccessor > > addedVal = m_addedVal; - helper::ReadAccessor > > addedMinVal = m_addedMinVal; - helper::ReadAccessor > > addedMaxVal = m_addedMaxVal; - helper::ReadAccessor > > addedStd = m_addedStd; + helper::ReadAccessor< Data< sofa::type::vector > > addedVal = m_addedVal; + helper::ReadAccessor< Data< sofa::type::vector > > addedMinVal = m_addedMinVal; + helper::ReadAccessor< Data< sofa::type::vector > > addedMaxVal = m_addedMaxVal; + helper::ReadAccessor< Data< sofa::type::vector > > addedStd = m_addedStd; size_t newNumParams = addedVal.size() + this->m_dim * this->m_numParams.getValue(); std::cout << "Added values size: " << addedVal.size() << std::endl; std::cout << "New number of parameters: " << newNumParams << std::endl; - helper::WriteAccessor > > val = m_val; + helper::WriteAccessor > > val = m_val; val.resize(newNumParams); for (size_t i = numParams; i < newNumParams; i++) val[i] = addedVal[i - numParams]; - helper::WriteAccessor > > initVal = m_initVal; + helper::WriteAccessor > > initVal = m_initVal; initVal.resize(newNumParams); for (size_t i = numParams; i < newNumParams; i++) initVal[i] = addedVal[i - numParams]; - helper::WriteAccessor > > minVal = m_minVal; + helper::WriteAccessor > > minVal = m_minVal; minVal.resize(newNumParams); for (size_t i = numParams; i < newNumParams; i++) minVal[i] = addedMinVal[i - numParams]; - helper::WriteAccessor > > maxVal = m_maxVal; + helper::WriteAccessor > > maxVal = m_maxVal; maxVal.resize(newNumParams); for (size_t i = numParams; i < newNumParams; i++) maxVal[i] = addedMaxVal[i - numParams]; - helper::WriteAccessor > > stdev = m_stdev; + helper::WriteAccessor > > stdev = m_stdev; stdev.resize(newNumParams); for (size_t i = numParams; i < newNumParams; i++) stdev[i] = addedStd[i - numParams]; @@ -371,22 +386,31 @@ void OptimParams >::appendParameters() m_numParams.setValue(newNumParams); } + + template<> void OptimParams::appendParameters() { } + + template<> -void OptimParams::appendParameters() { } +void OptimParams::appendParameters() { } + + template<> void OptimParams::appendParameters() { } + + template<> void OptimParams::appendParameters() { } template<> -void OptimParams >::handleEvent(core::objectmodel::Event *event) { +void OptimParams >::handleEvent(core::objectmodel::Event *event) +{ if (dynamic_cast(event)) { //if (!this->m_optimize.getValue()) { @@ -396,14 +420,14 @@ void OptimParams >::handleEvent(core::objectmodel:: int timeSlot = -1; for (size_t i = 1; i < m_paramKeys.size() && timeSlot < 0; i++) { - if (m_paramKeys[i-1].first <= actTime && actTime < m_paramKeys[i].first) { - timeSlot = i-1; + if (m_paramKeys[i - 1].first <= actTime && actTime < m_paramKeys[i].first) { + timeSlot = i - 1; } } if (timeSlot == -1) { if (actTime >= m_paramKeys.back().first) { - helper::WriteAccessor > > val = m_val; + helper::WriteAccessor > > val = m_val; std::cout << "Const val: "; for (size_t i = 0; i < val.size(); i++) { val[i] = m_paramKeys.back().second[i]; @@ -416,49 +440,48 @@ void OptimParams >::handleEvent(core::objectmodel:: } else { double t1 = m_paramKeys[timeSlot].first; double t2 = m_paramKeys[timeSlot+1].first; - double r1 = (actTime-t1)/(t2-t1); - double r2 = (t2-actTime)/(t2-t1); + double r1 = (actTime - t1) / (t2 - t1); + double r2 = (t2 - actTime) / (t2 - t1); //std::cout << "Time slot: " << timeSlot << " actual time: " << actTime << " ratii: " << r1 << " " << r2 << std::endl; - helper::WriteAccessor > > val = m_val; + helper::WriteAccessor > > val = m_val; std::cout << "Value: "; for (size_t i = 0; i < val.size(); i++) { - double v1=m_paramKeys[timeSlot].second[i]; - double v2=m_paramKeys[timeSlot+1].second[i]; - - /// (y2-y1)*(tanh(3.5*(a-(t0+t1)/2))+1)/2+y1 - if (this->m_interpolateSmooth.getValue()) - val[i] = (v2-v1)*(tanh(2*(actTime-(t1+t2)/2))+1)/2+v1; - else - val[i] = r2*v1 + r1*v2; + double v1 = m_paramKeys[timeSlot].second[i]; + double v2 = m_paramKeys[timeSlot + 1].second[i]; + + // (y2 - y1) * (tanh(3.5 * (a - (t0 + t1) / 2)) + 1) / 2 + y1 + if (this->m_interpolateSmooth.getValue()) { + val[i] = (v2 - v1) * (tanh(2 * (actTime - (t1 + t2) / 2)) + 1) / 2 + v1; + } else { + val[i] = r2 * v1 + r1 * v2; + } std::cout << " " << val[i]; } std::cout << std::endl; - - //} - - } + } + // } if (this->saveParam) { std::ofstream paramFile(m_exportParamFile.getValue().c_str(), std::ios::app); if (paramFile.is_open()) { - helper::ReadAccessor > > val = m_val; + helper::ReadAccessor< Data< sofa::type::vector > > val = m_val; for (size_t i = 0; i < val.size(); i++) paramFile << val[i] << " "; paramFile << '\n'; paramFile.close(); } } - } } -/// SPECIALIZATIONS FOR VecCoord3D +/// SPECIALIZATIONS FOR VecCoord3D template<> -void OptimParams::init() { +void OptimParams::init() +{ Inherit::init(); this->m_dim = 3; @@ -499,8 +522,10 @@ void OptimParams::init() { } + template<> -void OptimParams::vectorToParams(VectorXd& _vector) { +void OptimParams::vectorToParams(VectorXd& _vector) +{ helper::WriteAccessor > waVal = m_val; size_t numParams = m_numParams.getValue(); @@ -510,8 +535,11 @@ void OptimParams::vectorToParams(VectorXd& _vector) { waVal[i][j] = _vector[paramIndices[i*m_dim+j]]; } + + template<> -void OptimParams::paramsToVector(VectorXd& _vector) { +void OptimParams::paramsToVector(VectorXd& _vector) +{ helper::ReadAccessor > raVal = m_val; size_t k = 0; size_t numParams = this->m_numParams.getValue(); @@ -521,8 +549,11 @@ void OptimParams::paramsToVector(VectorXd& _vector) { _vector[paramIndices[i*m_dim+j]]=raVal[i][j]; } + + template<> -void OptimParams::getInitVariance(DVec& _variance) { +void OptimParams::getInitVariance(DVec& _variance) +{ size_t numParams = this->m_numParams.getValue(); _variance.resize(numParams*m_dim); size_t ij = 0; @@ -534,54 +565,56 @@ void OptimParams::getInitVariance(DVec& _variance) { } + template<> -void OptimParams::handleEvent(core::objectmodel::Event *event) { +void OptimParams::handleEvent(core::objectmodel::Event *event) +{ if (dynamic_cast(event)) { - double actTime = this->getTime() + this->getContext()->getDt(); /// the time has not been increased yet - std::cout << "[" << this->getName() << "] begin event at time: " << actTime << std::endl; + double actTime = this->getTime() + this->getContext()->getDt(); /// the time has not been increased yet + std::cout << "[" << this->getName() << "] begin event at time: " << actTime << std::endl; - int timeSlot = -1; + int timeSlot = -1; - for (size_t i = 1; i < m_paramKeys.size() && timeSlot < 0; i++) { - if (m_paramKeys[i-1].first <= actTime && actTime < m_paramKeys[i].first) { - timeSlot = i-1; - } + for (size_t i = 1; i < m_paramKeys.size() && timeSlot < 0; i++) { + if (m_paramKeys[i-1].first <= actTime && actTime < m_paramKeys[i].first) { + timeSlot = i - 1; } - if (timeSlot == -1) { - if (actTime >= m_paramKeys.back().first) { - helper::WriteAccessor > val = m_val; - std::cout << "[" << this->getName() << "] const val: "; - for (size_t i = 0; i < val.size(); i++) { - val[i] = m_paramKeys.back().second[i]; - std::cout << " " << val[i]; - } - std::cout << std::endl; - } else { - std::cerr << this->getName() << " ERROR: no slot found for time " << actTime << std::endl; - } - } else { - double t1 = m_paramKeys[timeSlot].first; - double t2 = m_paramKeys[timeSlot+1].first; - double r1 = (actTime-t1)/(t2-t1); - double r2 = (t2-actTime)/(t2-t1); - - helper::WriteAccessor > val = m_val; - std::cout << "[" << this->getName() << "] Value: "; + } + if (timeSlot == -1) { + if (actTime >= m_paramKeys.back().first) { + helper::WriteAccessor > val = m_val; + std::cout << "[" << this->getName() << "] const val: "; for (size_t i = 0; i < val.size(); i++) { - Vec3dTypes::Coord v1=m_paramKeys[timeSlot].second[i]; - Vec3dTypes::Coord v2=m_paramKeys[timeSlot+1].second[i]; - - /// (y2-y1)*(tanh(3.5*(a-(t0+t1)/2))+1)/2+y1 - if (this->m_interpolateSmooth.getValue()) - val[i] = (v2-v1)*(tanh(2*(actTime-(t1+t2)/2))+1)/2+v1; - else - val[i] = r2*v1 + r1*v2; + val[i] = m_paramKeys.back().second[i]; std::cout << " " << val[i]; } std::cout << std::endl; - + } else { + std::cerr << this->getName() << " ERROR: no slot found for time " << actTime << std::endl; } + } else { + double t1 = m_paramKeys[timeSlot].first; + double t2 = m_paramKeys[timeSlot + 1].first; + double r1 = (actTime - t1) / (t2 - t1); + double r2 = (t2 - actTime) / (t2 - t1); + + helper::WriteAccessor< Data< Vec3dTypes::VecDeriv > > val = m_val; + std::cout << "[" << this->getName() << "] Value: "; + for (size_t i = 0; i < val.size(); i++) { + Vec3dTypes::Coord v1 = m_paramKeys[timeSlot].second[i]; + Vec3dTypes::Coord v2 = m_paramKeys[timeSlot+1].second[i]; + + // (y2 - y1) * (tanh(3.5 * (a - (t0 + t1) / 2)) + 1) / 2 + y1 + if (this->m_interpolateSmooth.getValue()) { + val[i] = (v2 - v1) * (tanh(2 * (actTime - (t1 + t2) / 2)) + 1) / 2 + v1; + } else { + val[i] = r2 * v1 + r1 * v2; + } + std::cout << " " << val[i]; + } + std::cout << std::endl; + } if (this->saveParam) { std::ofstream paramFile(m_exportParamFile.getValue().c_str(), std::ios::app); @@ -593,31 +626,28 @@ void OptimParams::handleEvent(core::objectmodel::Event *ev paramFile.close(); } } - } } -/// Specialization for RigidDeriv3d +/// Specialization for RigidDeriv3d template<> -void OptimParams::init() { +void OptimParams::init() +{ Inherit1::init(); paramMOrigid = m_paramMOLinkrigid.get(); - if (paramMOrigid == NULL) + if (paramMOrigid == NULL) { std::cerr << "WARNING: cannot find the parametric mechanical state, assuming no mechanical state is associated with the parameters" << std::endl; - else { + } else { std::cout << "Mechanical state associated with the parameters: " << paramMOrigid->getName() << std::endl; - typename MechStateRigid3d::ReadVecCoord moPos = paramMOrigid->readPositions(); mstate_dim = moPos.size(); - } helper::ReadAccessor >initVal = m_initVal; size_t nInitVal = initVal.size(); - if (nInitVal != mstate_dim ){ PRNE ("Wrong values of parameter \"initValue\" in OptimParams. \nRESET it consistent with dimension of "<< paramMOrigid->getName()) return; @@ -634,7 +664,7 @@ void OptimParams::init() { return; } } - if (nInitVal != 0 || nInitVal==mstate_dim) { + if (nInitVal != 0 || nInitVal == mstate_dim) { helper::WriteAccessor > val = m_val; if (val.size() == 0) { @@ -653,25 +683,30 @@ void OptimParams::init() { for (size_t i = 1; i < nInitVal; i++) stdev[i] = stdev[0]; } - }else{ + } else { PRNE ("Wrong initValue. Must be consistent with "<< paramMOrigid->getName() << "dimensions. ") } } + template<> -void OptimParams::vectorToParams(VectorXd& _vector) { +void OptimParams::vectorToParams(VectorXd& _vector) +{ helper::WriteAccessor > waVal = m_val; size_t numParams = mstate_dim; size_t k = 0; for (size_t i = 0; i < numParams; i++) - for (size_t j = 0; k < this->paramIndices.size() ; j++, k++) + for (size_t j = 0; k < this->paramIndices.size(); j++, k++) waVal[i][j] = _vector[paramIndices[k]]; } + + template<> -void OptimParams::paramsToVector(VectorXd& _vector) { +void OptimParams::paramsToVector(VectorXd& _vector) +{ helper::ReadAccessor > raVal = m_val; size_t k = 0; @@ -681,64 +716,68 @@ void OptimParams::paramsToVector(VectorXd& _vector) { } + template<> -void OptimParams::getInitVariance(DVec& _variance) { +void OptimParams::getInitVariance(DVec& _variance) +{ size_t numParams = this->m_numParams.getValue(); _variance.resize(numParams); size_t ij = 0; for (size_t i = 0; i < mstate_dim; i++) for (size_t j = 0; j < 6; j++, ij++) _variance[ij] = helper::SQR(m_stdev.getValue()[i][j]); - } + + template<> -void OptimParams::handleEvent(core::objectmodel::Event *event) { +void OptimParams::handleEvent(core::objectmodel::Event *event) +{ if (dynamic_cast(event)) { - double actTime = this->getTime() + this->getContext()->getDt(); /// the time has not been increased yet - std::cout << "[" << this->getName() << "] begin event at time: " << actTime << std::endl; + double actTime = this->getTime() + this->getContext()->getDt(); /// the time has not been increased yet + std::cout << "[" << this->getName() << "] begin event at time: " << actTime << std::endl; - int timeSlot = -1; + int timeSlot = -1; - for (size_t i = 1; i < m_paramKeys.size() && timeSlot < 0; i++) { - if (m_paramKeys[i-1].first <= actTime && actTime < m_paramKeys[i].first) { - timeSlot = i-1; - } + for (size_t i = 1; i < m_paramKeys.size() && timeSlot < 0; i++) { + if (m_paramKeys[i - 1].first <= actTime && actTime < m_paramKeys[i].first) { + timeSlot = i - 1; } - if (timeSlot == -1) { - if (actTime >= m_paramKeys.back().first) { - helper::WriteAccessor > val = m_val; - std::cout << "[" << this->getName() << "] const val: "; - for (size_t i = 0; i < val.size(); i++) { - val[i] = m_paramKeys.back().second[i]; - std::cout << " " << val[i]; - } - std::cout << std::endl; - } else { - std::cerr << this->getName() << " ERROR: no slot found for time " << actTime << std::endl; - } - } else { - double t1 = m_paramKeys[timeSlot].first; - double t2 = m_paramKeys[timeSlot+1].first; -// double r1 = (actTime-t1)/(t2-t1); -// double r2 = (t2-actTime)/(t2-t1); - - helper::WriteAccessor > val = m_val; - std::cout << "[" << this->getName() << "] Value: "; + } + if (timeSlot == -1) { + if (actTime >= m_paramKeys.back().first) { + helper::WriteAccessor > val = m_val; + std::cout << "[" << this->getName() << "] const val: "; for (size_t i = 0; i < val.size(); i++) { - Rigid3dTypes::Deriv v1=m_paramKeys[timeSlot].second[i]; - Rigid3dTypes::Deriv v2=m_paramKeys[timeSlot+1].second[i]; - - /// (y2-y1)*(tanh(3.5*(a-(t0+t1)/2))+1)/2+y1 - if (this->m_interpolateSmooth.getValue()) - val[i] = (v2-v1)*(tanh(2*(actTime-(t1+t2)/2))+1)/2+v1; -// else -// val[i] = r2*v1 + r1*v2; -// std::cout << " " << val[i]; + val[i] = m_paramKeys.back().second[i]; + std::cout << " " << val[i]; } std::cout << std::endl; + } else { + std::cerr << this->getName() << " ERROR: no slot found for time " << actTime << std::endl; } + } else { + double t1 = m_paramKeys[timeSlot].first; + double t2 = m_paramKeys[timeSlot + 1].first; + // double r1 = (actTime - t1) / (t2 - t1); + // double r2 = (t2 - actTime) / (t2 - t1); + + helper::WriteAccessor > val = m_val; + std::cout << "[" << this->getName() << "] Value: "; + for (size_t i = 0; i < val.size(); i++) { + Rigid3dTypes::Deriv v1 = m_paramKeys[timeSlot].second[i]; + Rigid3dTypes::Deriv v2 = m_paramKeys[timeSlot + 1].second[i]; + + // (y2 - y1) * (tanh(3.5 * (a - (t0 + t1) / 2)) + 1) / 2 + y1 + if (this->m_interpolateSmooth.getValue()) + val[i] = (v2 - v1) * (tanh(2 * (actTime - (t1 + t2) / 2)) + 1) / 2 + v1; + // else + // val[i] = r2 * v1 + r1 * v2; + // std::cout << " " << val[i]; + } + std::cout << std::endl; + } if (this->saveParam) { std::ofstream paramFile(m_exportParamFile.getValue().c_str(), std::ios::app); @@ -754,16 +793,17 @@ void OptimParams::handleEvent(core::objectmodel::Event * } -/// Specialization for double +/// Specialization for double template<> -void OptimParams::init() { +void OptimParams::init() +{ Inherit::init(); m_dim = 1; /// take the initial value and initial stdev if (!this->m_prescribedParamKeys.getValue().empty()) { - helper::ReadAccessor > >keys = m_prescribedParamKeys; + helper::ReadAccessor< Data< type::vector > >keys = m_prescribedParamKeys; size_t numKeyValues = keys.size(); @@ -777,8 +817,8 @@ void OptimParams::init() { m_paramKeys.resize(numKeys); for (size_t i = 0; i < numKeys; i++) { - m_paramKeys[i].first = keys[2*i]; - m_paramKeys[i].second = keys[2*i+1]; + m_paramKeys[i].first = keys[2 * i]; + m_paramKeys[i].second = keys[2 * i + 1]; } /*for (size_t i = 0; i < m_paramKeys.size(); i++) { @@ -794,76 +834,80 @@ void OptimParams::init() { } + template<> -void OptimParams::handleEvent(core::objectmodel::Event *event) { +void OptimParams::handleEvent(core::objectmodel::Event *event) +{ if (dynamic_cast(event)) { - //if (!this->m_optimize.getValue()) { - double actTime = this->getTime() + this->getContext()->getDt(); /// the time has not been increased yet - std::cout << "[" << this->getName() << "] begin event at time: " << actTime << std::endl; + // if (!this->m_optimize.getValue()) { + double actTime = this->getTime() + this->getContext()->getDt(); /// the time has not been increased yet + std::cout << "[" << this->getName() << "] begin event at time: " << actTime << std::endl; - int timeSlot = -1; + int timeSlot = -1; - std::cout << "Keys size: " << m_paramKeys.size() << std::endl; - for (size_t i = 1; i < m_paramKeys.size() && timeSlot < 0; i++) { - if (m_paramKeys[i-1].first <= actTime && actTime < m_paramKeys[i].first) { - timeSlot = i-1; - } + std::cout << "Keys size: " << m_paramKeys.size() << std::endl; + for (size_t i = 1; i < m_paramKeys.size() && timeSlot < 0; i++) { + if (m_paramKeys[i - 1].first <= actTime && actTime < m_paramKeys[i].first) { + timeSlot = i - 1; } - if (timeSlot == -1) { - if (actTime >= m_paramKeys.back().first) { - //helper::WriteAccessor > val = m_val; - double val = m_paramKeys.back().second; - std::cout << "[" << this->getName() << "] const val: " << val << std::endl; - m_val.setValue(val); - } else { - std::cerr << this->getName() << " ERROR: no slot found for time " << actTime << std::endl; - } + } + if (timeSlot == -1) { + if (actTime >= m_paramKeys.back().first) { + // helper::WriteAccessor > val = m_val; + double val = m_paramKeys.back().second; + std::cout << "[" << this->getName() << "] const val: " << val << std::endl; + m_val.setValue(val); } else { - double t1 = m_paramKeys[timeSlot].first; - double t2 = m_paramKeys[timeSlot+1].first; - double r1 = (actTime-t1)/(t2-t1); - double r2 = (t2-actTime)/(t2-t1); + std::cerr << this->getName() << " ERROR: no slot found for time " << actTime << std::endl; + } + } else { + double t1 = m_paramKeys[timeSlot].first; + double t2 = m_paramKeys[timeSlot + 1].first; + double r1 = (actTime - t1) / (t2 - t1); + double r2 = (t2 - actTime) / (t2 - t1); - //std::cout << "Time slot: " << timeSlot << " actual time: " << actTime << " ratii: " << r1 << " " << r2 << std::endl; + // std::cout << "Time slot: " << timeSlot << " actual time: " << actTime << " ratii: " << r1 << " " << r2 << std::endl; - //helper::WriteAccessor > val = m_val; - double val; - double v1=m_paramKeys[timeSlot].second; - double v2=m_paramKeys[timeSlot+1].second; + // helper::WriteAccessor > val = m_val; + double val; + double v1 = m_paramKeys[timeSlot].second; + double v2 = m_paramKeys[timeSlot + 1].second; - /// (y2-y1)*(tanh(3.5*(a-(t0+t1)/2))+1)/2+y1 - if (this->m_interpolateSmooth.getValue()) - val = (v2-v1)*(tanh(2*(actTime-(t1+t2)/2))+1)/2+v1; - else - val = r2*v1 + r1*v2; - std::cout << "[" << this->getName() << "] Value: " << val << std::endl; - m_val.setValue(val); + // (y2 - y1) * (tanh(3.5 * (a - (t0 + t1) / 2)) + 1) / 2 + y1 + if (this->m_interpolateSmooth.getValue()) { + val = (v2 - v1) * (tanh(2 * (actTime - (t1 + t2) / 2)) + 1) / 2 + v1; + } else { + val = r2 * v1 + r1 * v2; } + std::cout << "[" << this->getName() << "] Value: " << val << std::endl; + m_val.setValue(val); + } } } -/// DECLARATIONS +/// DECLARATIONS SOFA_DECL_CLASS(OptimParams) // Register in the Factory int OptimParamsClass = core::RegisterObject("Optimization Parameters") .add< OptimParams >() - .add< OptimParams >() - .add< OptimParams > >() + .add< OptimParams >() + .add< OptimParams > >() .add< OptimParams >() .add< OptimParams >() ; template class SOFA_OPTIMUSPLUGIN_API OptimParams; -template class SOFA_OPTIMUSPLUGIN_API OptimParams; -template class SOFA_OPTIMUSPLUGIN_API OptimParams >; +template class SOFA_OPTIMUSPLUGIN_API OptimParams; +template class SOFA_OPTIMUSPLUGIN_API OptimParams >; template class SOFA_OPTIMUSPLUGIN_API OptimParams; template class SOFA_OPTIMUSPLUGIN_API OptimParams; + } // namespace container } // namespace component diff --git a/src/genericComponents/OptimParams.h b/src/genericComponents/OptimParams.h index 0dabc0ed..6986d5bb 100644 --- a/src/genericComponents/OptimParams.h +++ b/src/genericComponents/OptimParams.h @@ -34,9 +34,6 @@ #include #include -#ifdef Success -#undef Success // dirty workaround to cope with the (dirtier) X11 define. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=253 -#endif #include @@ -57,105 +54,105 @@ using namespace defaulttype; template struct templateName { - std::string operator ()(void) { return("generic"); } + std::string operator()(void) { return("generic"); } }; template<> struct templateName { - std::string operator ()(void) { return("VecDeriv3d"); } + std::string operator()(void) { return("VecDeriv3d"); } }; template<> struct templateName { - std::string operator ()(void) { return("RigidDeriv3d"); } + std::string operator()(void) { return("RigidDeriv3d"); } }; template<> struct templateName { - std::string operator ()(void) { return("double"); } + std::string operator()(void) { return("double"); } }; template<> struct templateName > { - std::string operator ()(void) { return("Rigid3d"); } + std::string operator()(void) { return("Rigid3d"); } }; template<> struct templateName > { - std::string operator ()(void) { return("Rigid2d"); } + std::string operator()(void) { return("Rigid2d"); } }; template<> -struct templateName +struct templateName { - std::string operator ()(void) { return("Vec3d"); } + std::string operator()(void) { return("Vec3d"); } }; template<> -struct templateName +struct templateName { - std::string operator ()(void) { return("Vec2d"); } + std::string operator()(void) { return("Vec2d"); } }; template<> -struct templateName +struct templateName { - std::string operator ()(void) { return("Vec1d"); } + std::string operator()(void) { return("Vec1d"); } }; template<> -struct templateName > +struct templateName > { - std::string operator ()(void) { return("Vector"); } + std::string operator()(void) { return("Vector"); } }; template<> -struct templateName > +struct templateName > { - std::string operator ()(void) { return("Vector"); } + std::string operator()(void) { return("Vector"); } }; /** * Parent class for OptimParams (see below) */ - class OptimParamsBase : public sofa::core::objectmodel::BaseObject { public: SOFA_ABSTRACT_CLASS(OptimParamsBase, BaseObject); - typedef helper::vector DVec; - typedef helper::vector IVec; + typedef type::vector DVec; + typedef type::vector IVec; typedef Eigen::Matrix VectorXf; typedef Eigen::Matrix VectorXd; + protected: size_t m_dim; size_t mstate_dim; - Data< bool > m_optimize; ///if OptimParams component are used in Verdandi optimization + Data< bool > m_optimize; /// if OptimParams component are used in Verdandi optimization Data< size_t > m_numParams; Data< std::string > m_transformParams; - Data< helper::vector > m_prescribedParamKeys; + Data< type::vector > m_prescribedParamKeys; Data< std::string > m_exportParamFile; Data< bool> m_interpolateSmooth; bool saveParam; int transParamType; - IVec paramIndices; /// mapping of parameters stored in m_val to Verdandi state vector + IVec paramIndices; /// mapping of parameters stored in m_val to Verdandi state vector + public: - //virtual void rawVectorToParams(const double* _vector) = 0; /// copy values from a input vector into parameters at correct positions //virtual void paramsToRawVector(double* _vector) = 0; /// copy values from parameters to the output vector at correct positions @@ -205,7 +202,6 @@ class OptimParamsBase : public sofa::core::objectmodel::BaseObject if (std::strcmp(transf.c_str(), "project") == 0) transParamType = 4; - } size_t size() { @@ -223,12 +219,14 @@ class OptimParamsBase : public sofa::core::objectmodel::BaseObject bool isOptimized() { return(m_optimize.getValue()); } - void setOptimize(bool value) - { + + void setOptimize(bool value) { m_optimize.setValue(value); } }; + + /** * Class used as a container of stochastic values. Gaussian distribution is assumed, thus a variable is represented by expected value and standard deviation. */ @@ -272,7 +270,12 @@ class OptimParams : public OptimParamsBase { return templateName(this); } - static std::string templateName(const OptimParams* = NULL) { std::string name = sofa::component::container::templateName()(); return(name); } + + static std::string templateName(const OptimParams* = NULL) + { + std::string name = sofa::component::container::templateName()(); + return(name); + } Data< DataTypes > m_addedVal; /// added parameters Data< DataTypes > m_addedMinVal; /// added minimal bound for parameters diff --git a/src/genericComponents/OptimParams.inl b/src/genericComponents/OptimParams.inl index a34d689e..0b752329 100644 --- a/src/genericComponents/OptimParams.inl +++ b/src/genericComponents/OptimParams.inl @@ -24,10 +24,9 @@ #include "OptimParams.h" -#include -#include +#include +#include #include -#include #include #include @@ -81,7 +80,7 @@ void OptimParams::reinit() template<> -void OptimParams >::bwdInit() +void OptimParams >::bwdInit() { loader_t* myLoader = m_loader.get(); if (myLoader) @@ -91,11 +90,11 @@ void OptimParams >::bwdInit() std::cout<<"Optim params <"< found loader <"<getName()<<"> with "< > > initValues = m_initVal; - sofa::helper::WriteAccessor< Data > > values = m_val; - sofa::helper::WriteAccessor< Data > > min = m_minVal; - sofa::helper::WriteAccessor< Data > > max = m_maxVal; - sofa::helper::WriteAccessor< Data > > stdev = m_stdev; + sofa::helper::WriteAccessor< Data > > initValues = m_initVal; + sofa::helper::WriteAccessor< Data > > values = m_val; + sofa::helper::WriteAccessor< Data > > min = m_minVal; + sofa::helper::WriteAccessor< Data > > max = m_maxVal; + sofa::helper::WriteAccessor< Data > > stdev = m_stdev; initValues.resize(count); diff --git a/src/genericComponents/PointProjection.cpp b/src/genericComponents/PointProjection.cpp index 6606d112..ebe3d0e0 100644 --- a/src/genericComponents/PointProjection.cpp +++ b/src/genericComponents/PointProjection.cpp @@ -27,12 +27,17 @@ #include "PointProjection.inl" //#include + + namespace sofa { + //SOFA_DECL_CLASS(PointProjection) template class PointProjection; //template class PointProjection; -} + +} // namespace sofa + diff --git a/src/genericComponents/PointProjection.h b/src/genericComponents/PointProjection.h index de6118cf..bf366e2d 100644 --- a/src/genericComponents/PointProjection.h +++ b/src/genericComponents/PointProjection.h @@ -29,171 +29,156 @@ #include #include -#include -#include +#include +#include #include namespace sofa { + /** * @brief Projection of points onto 3D triangular surface. * * @tparam Real Real type to use. */ - template class PointProjection { - - public: - typedef sofa::defaulttype::Vec<2, Real> Vec2; - typedef sofa::defaulttype::Vec<3, Real> Vec3; - - typedef sofa::defaulttype::Mat<3,3, Real> Mat33; - - typedef sofa::component::topology::TriangleSetTopologyContainer::Edge Edge; - typedef sofa::component::topology::TriangleSetTopologyContainer::TriangleID Index; - typedef sofa::component::topology::TriangleSetTopologyContainer::Triangle Triangle; - typedef sofa::component::topology::TriangleSetTopologyContainer::TrianglesAroundVertex TrianglesAroundVertex; - typedef sofa::component::topology::TriangleSetTopologyContainer::TrianglesAroundEdge TrianglesAroundEdge; - typedef sofa::component::topology::TriangleSetTopologyContainer::EdgesInTriangle EdgesInTriangle; - typedef sofa::component::topology::TriangleSetTopologyContainer::SeqEdges SeqEdges; - typedef sofa::component::topology::TriangleSetTopologyContainer::SeqTriangles SeqTriangles; - - //typedef sofa::helper::vector VecVec3; - typedef sofa::defaulttype::Vec3dTypes::VecCoord VecVec3; - typedef sofa::defaulttype::Vec3dTypes::Coord Coord; - typedef sofa::helper::vector VecIndex; - - enum { InvalidID = sofa::core::topology::Topology::InvalidID }; - - /** - * @brief Class initialization. - * - * @param _topology Associated triangular topology. - */ - PointProjection(sofa::component::topology::TriangleSetTopologyContainer &_topology) : - topology(_topology) {} - - - /** - * @brief Find a projection of a point on the surface. - * - * @param baryCoords Barycentric coordinates of projected point. - * @param triangleID Triangle on which the point is projected. Or - * InvalidID if projection fails. - * @param point Point to project. - * @param x Current positions of points in the topology. - */ - void ProjectPoint(Vec3 &baryCoords, Coord &projectedCoord, Index &triangleID, - const Vec3 &point, const VecVec3 &x); - - /** - * @brief Find a projection of a point on the surface of defined set of - * triangles. - * - * @param baryCoords Barycentric coordinates of projected point. - * @param triangleID Triangle on which the point is projected. Or - * InvalidID if projection fails. - * @param point Point to project. - * @param x Current positions of points in the topology. - * @param triangleList Indices of triangle to consider for projection. - */ - void ProjectPoint(Vec3 &baryCoords, Coord &projectedCoord, Index &triangleID, - const Vec3 &point, const VecVec3 &x, const VecIndex &triangleList); - - /** - * - * Compute barycentric coordinates of a point. - * - * Compute barycentric coordinates of point p in triangle whose - * vertices are a, b and c. If bConstraint is true constraint the - * coordinates to lie inside the triangle. - * - * @param baryCoords The barycentric coordinates. - * @param a Position of first triangle point. - * @param b Position of second triangle point. - * @param c Position of third triangle point. - * @param bConstraint Constraint coordinates to lie inside triangle. - */ - static void ComputeBaryCoords( - Vec3 &baryCoords, const Vec3 &p, - const Vec3 &a, const Vec3 &b, const Vec3 &c, bool bConstraint=true); - - /** - * - * Compute barycentric coordinates of a point (2D case). - * - * Compute barycentric coordinates of point p in triangle whose - * vertices are a, b and c. If bConstraint is true constraint the - * coordinates to lie inside the triangle. - * - * @param baryCoords The barycentric coordinates. - * @param a Position of first triangle point. - * @param b Position of second triangle point. - * @param c Position of third triangle point. - * @param bConstraint Constraint coordinates to lie inside triangle. - */ - static void ComputeBaryCoords( - Vec3 &baryCoords, const Vec2 &p, - const Vec2 &a, const Vec2 &b, const Vec2 &c, bool bConstraint=true); - - private: - - sofa::component::topology::TriangleSetTopologyContainer &topology; - - public: - - /** - * @brief Finds the closest point to a point. - * - * @param closestVertex Index of the closest point. - * @param point Position of the point. - * @param inVertices Positions of points in input set. - * - * @return Square of the distance to the closest vertex. - */ - Real FindClosestPoint(Index& closestVertex, - const Vec3& point, const VecVec3 &inVertices); - - /** - * @brief Finds the closest edge to a point. - * - * @param closestEdge Index of the closest edge. - * @param point Position of the point. - * @param inVertices Positions of points in input set. - * @param inEdges LIst of edges. - * - * @return Square of the distance to the closest edge. - */ - Real FindClosestEdge(Index& closestEdge, - const Vec3& point, const VecVec3 &inVertices, - const SeqEdges &inEdges); - - /** - * @brief Finds the closest triangle to a point. - * - * @param closestTriangle Index of the closest triangle. - * @param point Position of the point. - * @param inVertices Positions of points in input set. - * @param inEdges LIst of triangles. - * - * @return Square of the distance to the closest triangle. - */ - Real FindClosestTriangle(Index& closestTriangle, - const Vec3& point, const VecVec3 &inVertices, - const SeqTriangles &inTriangles); - - private: - void ProjectPoint( - Vec3 &baryCoords, Index &triangleID, Coord &projectedCoord, - const Vec3 &point, const VecVec3 &x, - Index closestVertex, Index closestEdge, Index closestTriangle, - Real minVertex, Real minEdge, Real minTriangle); - - +public: + typedef sofa::type::Vec<2, Real> Vec2; + typedef sofa::type::Vec<3, Real> Vec3; + + typedef sofa::type::Mat<3,3, Real> Mat33; + + typedef sofa::component::topology::TriangleSetTopologyContainer::Edge Edge; + typedef sofa::component::topology::TriangleSetTopologyContainer::TriangleID Index; + typedef sofa::component::topology::TriangleSetTopologyContainer::Triangle Triangle; + typedef sofa::component::topology::TriangleSetTopologyContainer::TrianglesAroundVertex TrianglesAroundVertex; + typedef sofa::component::topology::TriangleSetTopologyContainer::TrianglesAroundEdge TrianglesAroundEdge; + typedef sofa::component::topology::TriangleSetTopologyContainer::EdgesInTriangle EdgesInTriangle; + typedef sofa::component::topology::TriangleSetTopologyContainer::SeqEdges SeqEdges; + typedef sofa::component::topology::TriangleSetTopologyContainer::SeqTriangles SeqTriangles; + + //typedef sofa::type::vector VecVec3; + typedef sofa::defaulttype::Vec3dTypes::VecCoord VecVec3; + typedef sofa::defaulttype::Vec3dTypes::Coord Coord; + typedef sofa::type::vector VecIndex; + + enum { InvalidID = sofa::core::topology::Topology::InvalidID }; + + /** + * @brief Class initialization. + * + * @param _topology Associated triangular topology. + */ + PointProjection(sofa::component::topology::TriangleSetTopologyContainer &_topology) : topology(_topology) {} + + /** + * @brief Find a projection of a point on the surface. + * + * @param baryCoords Barycentric coordinates of projected point. + * @param triangleID Triangle on which the point is projected. Or + * InvalidID if projection fails. + * @param point Point to project. + * @param x Current positions of points in the topology. + */ + void ProjectPoint(Vec3 &baryCoords, Coord &projectedCoord, Index &triangleID, + const Vec3 &point, const VecVec3 &x); + + /** + * @brief Find a projection of a point on the surface of defined set of + * triangles. + * + * @param baryCoords Barycentric coordinates of projected point. + * @param triangleID Triangle on which the point is projected. Or + * InvalidID if projection fails. + * @param point Point to project. + * @param x Current positions of points in the topology. + * @param triangleList Indices of triangle to consider for projection. + */ + void ProjectPoint(Vec3 &baryCoords, Coord &projectedCoord, Index &triangleID, + const Vec3 &point, const VecVec3 &x, const VecIndex &triangleList); + + /** + * + * Compute barycentric coordinates of a point. + * + * Compute barycentric coordinates of point p in triangle whose + * vertices are a, b and c. If bConstraint is true constraint the + * coordinates to lie inside the triangle. + * + * @param baryCoords The barycentric coordinates. + * @param a Position of first triangle point. + * @param b Position of second triangle point. + * @param c Position of third triangle point. + * @param bConstraint Constraint coordinates to lie inside triangle. + */ + static void ComputeBaryCoords(Vec3 &baryCoords, const Vec3 &p, + const Vec3 &a, const Vec3 &b, const Vec3 &c, bool bConstraint = true); + + /** + * + * Compute barycentric coordinates of a point (2D case). + * + * Compute barycentric coordinates of point p in triangle whose + * vertices are a, b and c. If bConstraint is true constraint the + * coordinates to lie inside the triangle. + * + * @param baryCoords The barycentric coordinates. + * @param a Position of first triangle point. + * @param b Position of second triangle point. + * @param c Position of third triangle point. + * @param bConstraint Constraint coordinates to lie inside triangle. + */ + static void ComputeBaryCoords(Vec3 &baryCoords, const Vec2 &p, + const Vec2 &a, const Vec2 &b, const Vec2 &c, bool bConstraint = true); + + /** + * @brief Finds the closest point to a point. + * + * @param closestVertex Index of the closest point. + * @param point Position of the point. + * @param inVertices Positions of points in input set. + * + * @return Square of the distance to the closest vertex. + */ + Real FindClosestPoint(Index& closestVertex, const Vec3& point, const VecVec3 &inVertices); + + /** + * @brief Finds the closest edge to a point. + * + * @param closestEdge Index of the closest edge. + * @param point Position of the point. + * @param inVertices Positions of points in input set. + * @param inEdges LIst of edges. + * + * @return Square of the distance to the closest edge. + */ + Real FindClosestEdge(Index& closestEdge, const Vec3& point, const VecVec3 &inVertices, + const SeqEdges &inEdges); + + /** + * @brief Finds the closest triangle to a point. + * + * @param closestTriangle Index of the closest triangle. + * @param point Position of the point. + * @param inVertices Positions of points in input set. + * @param inEdges LIst of triangles. + * + * @return Square of the distance to the closest triangle. + */ + Real FindClosestTriangle(Index& closestTriangle, const Vec3& point, + const VecVec3 &inVertices, const SeqTriangles &inTriangles); + + +protected: + sofa::component::topology::TriangleSetTopologyContainer &topology; + + void ProjectPoint(Vec3 &baryCoords, Index &triangleID, Coord &projectedCoord, const Vec3 &point, + const VecVec3 &x, Index closestVertex, Index closestEdge, Index closestTriangle, + Real minVertex, Real minEdge, Real minTriangle); }; diff --git a/src/genericComponents/PointProjection.inl b/src/genericComponents/PointProjection.inl index a685ce0f..9bb515c3 100644 --- a/src/genericComponents/PointProjection.inl +++ b/src/genericComponents/PointProjection.inl @@ -34,9 +34,10 @@ namespace sofa { + template void PointProjection::ProjectPoint(Vec3 &baryCoords, Coord &projectedCoord, Index &triangleID, - const Vec3 &point, const VecVec3 &x) + const Vec3 &point, const VecVec3 &x) { Index closestVertex, closestEdge, closestTriangle; Real minVertex, minEdge, minTriangle; @@ -59,10 +60,12 @@ void PointProjection::ProjectPoint(Vec3 &baryCoords, Coord &projectedCoord minVertex, minEdge, minTriangle); } + + // -------------------------------------------------------------------------------------- template void PointProjection::ProjectPoint(Vec3 &baryCoords, Coord &projectedCoord, Index &triangleID, - const Vec3 &point, const VecVec3 &x, const VecIndex &triangleList) + const Vec3 &point, const VecVec3 &x, const VecIndex &triangleList) { Index closestVertex=InvalidID, closestEdge=InvalidID, closestTriangle=InvalidID; Real minVertex, minEdge=1e12, minTriangle; @@ -125,16 +128,16 @@ void PointProjection::ProjectPoint(Vec3 &baryCoords, Coord &projectedCoord minVertex, minEdge, minTriangle); } -// ----------------------------------------------------------------------------- + +// ----------------------------------------------------------------------------- // Do some magic to constraint the coordinates inside the triangle // the requirements are: // coef_a, coef_b, coef_c ≥ 0 // coef_a + coef_b + coef_c = 1 template //void ConstraintBaryCoords(Real &coef_a, Real &coef_b, Real &coef_c) -void ConstraintBaryCoords(sofa::defaulttype::Vec<3, Real> &baryCoords) - +void ConstraintBaryCoords(sofa::type::Vec<3, Real> &baryCoords) { if (baryCoords[0] < 0.0) baryCoords[0] = 0.0; if (baryCoords[1] < 0.0) baryCoords[1] = 0.0; @@ -161,38 +164,38 @@ void ConstraintBaryCoords(sofa::defaulttype::Vec<3, Real> &baryCoords) } + // -------------------------------------------------------------------------------------- template -void PointProjection::ComputeBaryCoords( - Vec3 &baryCoords, - const Vec3 &p, const Vec3 &a, const Vec3 &b, const Vec3 &c, bool bConstraint) +void PointProjection::ComputeBaryCoords(Vec3 &baryCoords, const Vec3 &p, + const Vec3 &a, const Vec3 &b, const Vec3 &c, bool bConstraint) { const double ZERO = 1e-20; Vec3 M = (Vec3) (b-a).cross(c-a); - double norm2_M = M*(M); + double norm2_M = M * (M); double coef_a, coef_b, coef_c; if(norm2_M < ZERO) // triangle (a,b,c) is flat { - coef_a = (double) (1.0/3.0); - coef_b = (double) (1.0/3.0); + coef_a = (double) (1.0 / 3.0); + coef_b = (double) (1.0 / 3.0); coef_c = (double) (1.0 - (coef_a + coef_b)); } else { - Vec3 N = M/norm2_M; + Vec3 N = M / norm2_M; - coef_a = N*((b-p).cross(c-p)); - coef_b = N*((c-p).cross(a-p)); + coef_a = N * ((b - p).cross(c - p)); + coef_b = N * ((c - p).cross(a - p)); if (bConstraint) { coef_c = 1.0 - (coef_a + coef_b); } else { - coef_c = N*((a-p).cross(b-p)); + coef_c = N * ((a - p).cross(b - p)); } } @@ -205,11 +208,12 @@ void PointProjection::ComputeBaryCoords( } } + + // ----------------------------------------------------------------------------- template -void PointProjection::ComputeBaryCoords( - Vec3 &baryCoords, - const Vec2 &p, const Vec2 &a, const Vec2 &b, const Vec2 &c, bool bConstraint) +void PointProjection::ComputeBaryCoords(Vec3 &baryCoords, const Vec2 &p, + const Vec2 &a, const Vec2 &b, const Vec2 &c, bool bConstraint) { Mat33 m; m(0,0) = 1; m(0,1) = 1; m(0,2) = 1; @@ -225,13 +229,15 @@ void PointProjection::ComputeBaryCoords( } } + + // ----------------------------------------------------------------------------- template Real PointProjection::FindClosestPoint(Index& closestVertex, - const Vec3& point, const VecVec3 &inVertices) + const Vec3& point, const VecVec3 &inVertices) { Real minimumDistance = 10e12; - for (unsigned int v=0; v::FindClosestPoint(Index& closestVertex, } + // ----------------------------------------------------------------------------- template -Real PointProjection::FindClosestEdge(Index& closestEdge, - const Vec3& point, const VecVec3 &inVertices, const SeqEdges &inEdges) +Real PointProjection::FindClosestEdge(Index& closestEdge, const Vec3& point, + const VecVec3 &inVertices, const SeqEdges &inEdges) { Real minimumDistance = 10e12; - for (unsigned int e=0; e= 0 && alpha <= 1) @@ -276,7 +283,7 @@ Real PointProjection::FindClosestEdge(Index& closestEdge, Vec3 P, Q, PQ; P = point; Q = pointEdge1 + AB * alpha; - PQ = Q-P; + PQ = Q - P; Real distance = PQ.norm2(); if (distance < minimumDistance) @@ -287,33 +294,32 @@ Real PointProjection::FindClosestEdge(Index& closestEdge, // Updates the minimum's value minimumDistance = distance; } - } } + } return minimumDistance; } + // ----------------------------------------------------------------------------- template -Real PointProjection::FindClosestTriangle(Index& closestTriangle, - const Vec3& point, const VecVec3 &inVertices, - const SeqTriangles &inTriangles) +Real PointProjection::FindClosestTriangle(Index& closestTriangle, const Vec3& point, + const VecVec3 &inVertices, const SeqTriangles &inTriangles) { Real minimumDistance = 10e12; - for (unsigned int t=0; t 1e-10)) { // Point projected onto the plane of the triangle lies outside @@ -340,12 +346,14 @@ Real PointProjection::FindClosestTriangle(Index& closestTriangle, return minimumDistance; } + + // ----------------------------------------------------------------------------- template void PointProjection::ProjectPoint(Vec3 &baryCoords, Index &triangleID, Coord &projectedCoord, - const Vec3 &point, const VecVec3 &x, - Index closestVertex, Index closestEdge, Index closestTriangle, - Real minVertex, Real minEdge, Real minTriangle) + const Vec3 &point, const VecVec3 &x, + Index closestVertex, Index closestEdge, Index closestTriangle, + Real minVertex, Real minEdge, Real minTriangle) { const SeqTriangles &triangles = topology.getTriangles(); @@ -400,15 +408,15 @@ void PointProjection::ProjectPoint(Vec3 &baryCoords, Index &triangleID, Co if (triangleID != InvalidID) { // Computes barycentric coordinates within the triangle - ComputeBaryCoords(baryCoords, point, - x[ triangles[triangleID][0] ], - x[ triangles[triangleID][1] ], - x[ triangles[triangleID][2] ]); + ComputeBaryCoords(baryCoords, point, x[ triangles[triangleID][0] ], + x[ triangles[triangleID][1] ], x[ triangles[triangleID][2] ]); } - projectedCoord = x[triangles[triangleID][0]]*baryCoords[0] + x[triangles[triangleID][1]]*baryCoords[1] + x[triangles[triangleID][2]]*baryCoords[2]; + projectedCoord = x[triangles[triangleID][0]] * baryCoords[0] + x[triangles[triangleID][1]] * baryCoords[1] + + x[triangles[triangleID][2]] * baryCoords[2]; } + } // namespace sofa diff --git a/src/genericComponents/ShowSpheres.cpp b/src/genericComponents/ShowSpheres.cpp index bc6bafd0..e7f7c8a4 100644 --- a/src/genericComponents/ShowSpheres.cpp +++ b/src/genericComponents/ShowSpheres.cpp @@ -28,6 +28,7 @@ #include + namespace sofa { @@ -37,6 +38,8 @@ namespace component namespace engine { + + SOFA_DECL_CLASS(ShowSpheres) using namespace sofa::defaulttype; @@ -51,6 +54,7 @@ template class SOFA_OPTIMUSPLUGIN_API ShowSpheres; template class SOFA_OPTIMUSPLUGIN_API ShowSpheres; + } // namespace engine } // namespace component diff --git a/src/genericComponents/ShowSpheres.h b/src/genericComponents/ShowSpheres.h index eb32cf78..2434adc9 100644 --- a/src/genericComponents/ShowSpheres.h +++ b/src/genericComponents/ShowSpheres.h @@ -23,7 +23,7 @@ #include "initOptimusPlugin.h" #include -#include +#include #include #include #include @@ -51,34 +51,32 @@ class ShowSpheres : public sofa::core::objectmodel::BaseObject SOFA_CLASS(SOFA_TEMPLATE(ShowSpheres, DataTypes), core::objectmodel::BaseObject); typedef typename DataTypes::VecCoord VecCoord; - typedef defaulttype::Vec<4,float> Vec4f; + typedef type::Vec<4,float> Vec4f; -ShowSpheres(); -~ShowSpheres(); - -protected: -public: Data _positions; - Data > _indices; + Data > _indices; Data _draw; Data _radius; Data _showIndicesSize; Data _color; Data _indexColor; + ShowSpheres(); + ~ShowSpheres(); void draw(const core::visual::VisualParams* vparams) override; void init() override { } void reinit() override { } +}; -}; /// class #if defined(SOFA_EXTERN_TEMPLATE) && !defined(SHOWSPHERES_CPP) extern template class SOFA_OPTIMUSPLUGIN_API ShowSpheres; extern template class SOFA_OPTIMUSPLUGIN_API ShowSpheres; #endif + } // engine } // component diff --git a/src/genericComponents/ShowSpheres.inl b/src/genericComponents/ShowSpheres.inl index a8730c9f..6008a2f8 100644 --- a/src/genericComponents/ShowSpheres.inl +++ b/src/genericComponents/ShowSpheres.inl @@ -57,7 +57,7 @@ template void ShowSpheres::draw(const core::visual::VisualParams* vparams) { if (_draw.getValue() == true && vparams->displayFlags().getShowBehaviorModels()) { helper::ReadAccessor< Data< VecCoord > > pos = _positions; - helper::vector ind = _indices.getValue(); + type::vector ind = _indices.getValue(); // if indices are empty, take all the nodes if (ind.size() == 0) { @@ -77,11 +77,11 @@ void ShowSpheres::draw(const core::visual::VisualParams* vparams) { const unsigned int npoints = ind.size(); const float rad = _radius.getValue(); - helper::vector points(npoints); - helper::vector radii(npoints); + type::vector points(npoints); + type::vector radii(npoints); for (unsigned int i = 0; i < npoints; i++) { - points[i] = defaulttype::Vector3(pos[ind[i]][0], pos[ind[i]][1], pos[ind[i]][2]); + points[i] = type::Vector3(pos[ind[i]][0], pos[ind[i]][1], pos[ind[i]][2]); radii[i] = rad; } @@ -95,7 +95,7 @@ void ShowSpheres::draw(const core::visual::VisualParams* vparams) { float scale = (float)((vparams->sceneBBox().maxBBox() - vparams->sceneBBox().minBBox()).norm() * _showIndicesSize.getValue()); for (size_t i = 0; i < npoints; i++) { - const defaulttype::Vector3 &p = points[i]; + const type::Vector3 &p = points[i]; std::string text = std::to_string(ind[i]); vparams->drawTool()->draw3DText(p, scale, indCol, text.c_str()); } diff --git a/src/genericComponents/SigmaPointsVTKExporter.cpp b/src/genericComponents/SigmaPointsVTKExporter.cpp index 479facd4..2a64ada2 100644 --- a/src/genericComponents/SigmaPointsVTKExporter.cpp +++ b/src/genericComponents/SigmaPointsVTKExporter.cpp @@ -52,9 +52,9 @@ int SigmaPointsVTKExporterClass = core::RegisterObject("Save State vectors from .add< SigmaPointsVTKExporter >(); + SigmaPointsVTKExporter::SigmaPointsVTKExporter() - : stepCounter(0), outfile(NULL) - , vtkFilename( initData(&vtkFilename, "filename", "output VTK file name")) + : vtkFilename( initData(&vtkFilename, "filename", "output VTK file name")) , fileFormat( initData(&fileFormat, (bool) true, "XMLformat", "Set to true to use XML format")) , position( initData(&position, "position", "points position (will use points from topology or mechanical state if this is empty)")) , writeEdges( initData(&writeEdges, (bool) true, "edges", "write edge topology")) @@ -68,8 +68,11 @@ SigmaPointsVTKExporter::SigmaPointsVTKExporter() , exportAtBegin( initData(&exportAtBegin, false, "exportAtBegin", "export file at the initialization")) , exportAtEnd( initData(&exportAtEnd, false, "exportAtEnd", "export file when the simulation is finished")) , overwrite( initData(&overwrite, false, "overwrite", "overwrite the file, otherwise create a new file at each export, with suffix in the filename")) -{ -} + , stepCounter(0) + , outfile(NULL) +{ } + + SigmaPointsVTKExporter::~SigmaPointsVTKExporter() { @@ -77,6 +80,8 @@ SigmaPointsVTKExporter::~SigmaPointsVTKExporter() delete outfile; } + + void SigmaPointsVTKExporter::init() { sofa::core::objectmodel::BaseContext* context = this->getContext(); @@ -104,8 +109,8 @@ void SigmaPointsVTKExporter::init() gnode = dynamic_cast(this->getContext()); - const helper::vector& pointsData = dPointsDataFields.getValue(); - const helper::vector& cellsData = dCellsDataFields.getValue(); + const type::vector& pointsData = dPointsDataFields.getValue(); + const type::vector& cellsData = dCellsDataFields.getValue(); if (!pointsData.empty()) { @@ -115,20 +120,22 @@ void SigmaPointsVTKExporter::init() { fetchDataFields(cellsData, cellsDataObject, cellsDataField, cellsDataName); } - } -void SigmaPointsVTKExporter::fetchDataFields(const helper::vector& strData, helper::vector& objects, helper::vector& fields, helper::vector& names) + + +void SigmaPointsVTKExporter::fetchDataFields(const type::vector &strData, type::vector &objects, + type::vector &fields, type::vector &names) { - for (unsigned int i=0 ; i& } } -void SigmaPointsVTKExporter::writeData(const helper::vector& objects, const helper::vector& fields, const helper::vector& names) + + +void SigmaPointsVTKExporter::writeData(const type::vector& objects, const type::vector& fields, const type::vector& names) { sofa::core::objectmodel::BaseContext* context = this->getContext(); - for (unsigned int i=0 ; iget (objects[i]); core::objectmodel::BaseData* field = NULL; - if (obj) - { + + if (obj) { field = obj->findData(fields[i]); } - if (!obj || !field) - { + if (!obj || !field) { if (!obj) msg_error() << "VTKExporter : error while fetching data field '" << msgendl << fields[i] << "' of object '" << objects[i] << msgendl @@ -174,28 +182,26 @@ void SigmaPointsVTKExporter::writeData(const helper::vector& object msg_error() << "VTKExporter : error while fetching data field " << msgendl << fields[i] << " of object '" << objects[i] << msgendl << "', check field name " << msgendl; - } - else - { + } else { //Scalars std::string line; - unsigned int sizeSeg=0; - if (dynamic_cast >* >(field)) + unsigned int sizeSeg = 0; + if (dynamic_cast >* >(field)) { line = "float 1"; sizeSeg = 1; } - if (dynamic_cast >* >(field)) + if (dynamic_cast >* >(field)) { line = "double 1"; sizeSeg = 1; } - if (dynamic_cast >* > (field)) + if (dynamic_cast >* > (field)) { line = "float 2"; sizeSeg = 2; } - if (dynamic_cast >* >(field)) + if (dynamic_cast >* >(field)) { line = "double 2"; sizeSeg = 2; @@ -211,12 +217,12 @@ void SigmaPointsVTKExporter::writeData(const helper::vector& object else { //Vectors - if (dynamic_cast >* > (field)) + if (dynamic_cast >* > (field)) { line = "float"; sizeSeg = 3; } - if (dynamic_cast >* >(field)) + if (dynamic_cast >* >(field)) { line = "double"; sizeSeg = 3; @@ -227,17 +233,17 @@ void SigmaPointsVTKExporter::writeData(const helper::vector& object *outfile << segmentString(field->getValueString(),sizeSeg) << std::endl; *outfile << std::endl; - - } } } -void SigmaPointsVTKExporter::writeDataArray(const helper::vector& objects, const helper::vector& fields, const helper::vector& names) + + +void SigmaPointsVTKExporter::writeDataArray(const type::vector& objects, const type::vector& fields, const type::vector& names) { sofa::core::objectmodel::BaseContext* context = this->getContext(); - for (unsigned int i=0 ; iget (objects[i]); core::objectmodel::BaseData* field = NULL; @@ -261,23 +267,23 @@ void SigmaPointsVTKExporter::writeDataArray(const helper::vector& o { //Scalars std::string type; - unsigned int sizeSeg=0; - if (dynamic_cast >* >(field)) + unsigned int sizeSeg = 0; + if (dynamic_cast >* >(field)) { type = "Int32"; sizeSeg = 1; } - if (dynamic_cast >* >(field)) + if (dynamic_cast >* >(field)) { type = "UInt32"; sizeSeg = 1; } - if (dynamic_cast >* >(field)) + if (dynamic_cast >* >(field)) { type = "Float32"; sizeSeg = 1; } - if (dynamic_cast >* >(field)) + if (dynamic_cast >* >(field)) { type = "Float64"; sizeSeg = 1; @@ -286,39 +292,40 @@ void SigmaPointsVTKExporter::writeDataArray(const helper::vector& o //Vectors if (type.empty()) { - if (dynamic_cast >* >(field)) + if (dynamic_cast >* >(field)) { type = "Float32"; sizeSeg = 1; } - if (dynamic_cast >* >(field)) + if (dynamic_cast >* >(field)) { type = "Float64"; sizeSeg = 1; } - if (dynamic_cast >* >(field)) + if (dynamic_cast >* >(field)) { type = "Float32"; sizeSeg = 2; } - if (dynamic_cast >* >(field)) + if (dynamic_cast >* >(field)) { type = "Float64"; sizeSeg = 2; } - if (dynamic_cast >* > (field)) + if (dynamic_cast >* > (field)) { type = "Float32"; sizeSeg = 3; } - if (dynamic_cast >* >(field)) + if (dynamic_cast >* >(field)) { type = "Float64"; sizeSeg = 3; } } + *outfile << " 1) *outfile << "\" NumberOfComponents=\"" << sizeSeg; @@ -330,10 +337,11 @@ void SigmaPointsVTKExporter::writeDataArray(const helper::vector& o } + std::string SigmaPointsVTKExporter::segmentString(std::string str, unsigned int n) { std::string::size_type loc = 0; - unsigned int i=0; + unsigned int i = 0; loc = str.find(' ', 0); @@ -352,6 +360,7 @@ std::string SigmaPointsVTKExporter::segmentString(std::string str, unsigned int } + void SigmaPointsVTKExporter::writeVTKSimple() { std::string filename = vtkFilename.getFullPath(); @@ -362,14 +371,14 @@ void SigmaPointsVTKExporter::writeVTKSimple() if (filename.size() > 3) { std::string ext; std::string baseName; - if (filename.substr(filename.size()-4)==".vtu") { + if (filename.substr(filename.size() - 4) == ".vtu") { ext = ".vtu"; - baseName = filename.substr(0, filename.size()-4); + baseName = filename.substr(0, filename.size() - 4); } - if (filename.substr(filename.size()-4)==".vtk") { + if (filename.substr(filename.size() - 4) == ".vtk") { ext = ".vtk"; - baseName = filename.substr(0, filename.size()-4); + baseName = filename.substr(0, filename.size() - 4); } /// no extension given => default "vtu" @@ -378,13 +387,14 @@ void SigmaPointsVTKExporter::writeVTKSimple() baseName = filename; } - if (overwrite.getValue()) + if (overwrite.getValue()) { filename = baseName + ext; - else + } else { filename = baseName + oss.str() + ext; - + } } + /*if ( filename.size() > 3 && filename.substr(filename.size()-4)==".vtu") { if (!overwrite.getValue()) @@ -406,8 +416,8 @@ void SigmaPointsVTKExporter::writeVTKSimple() return; } - const helper::vector& pointsData = dPointsDataFields.getValue(); - const helper::vector& cellsData = dCellsDataFields.getValue(); + const type::vector& pointsData = dPointsDataFields.getValue(); + const type::vector& cellsData = dCellsDataFields.getValue(); helper::ReadAccessor > pointsPos = position; @@ -549,6 +559,8 @@ void SigmaPointsVTKExporter::writeVTKSimple() msg_info() << "Export VTK in file " << filename << " done."; } + + void SigmaPointsVTKExporter::writeVTKXML() { std::string filename = vtkFilename.getFullPath(); @@ -576,12 +588,12 @@ void SigmaPointsVTKExporter::writeVTKXML() outfile = NULL; return; } - const helper::vector& pointsData = dPointsDataFields.getValue(); - const helper::vector& cellsData = dCellsDataFields.getValue(); + const type::vector& pointsData = dPointsDataFields.getValue(); + const type::vector& cellsData = dCellsDataFields.getValue(); - helper::ReadAccessor > pointsPos = position; + helper::ReadAccessor< Data< defaulttype::Vec3Types::VecCoord> > pointsPos = position; - const size_t nbp = (!pointsPos.empty()) ? pointsPos.size() : topology->getNbPoints(); + const size_t nbp = ( !pointsPos.empty() ) ? pointsPos.size() : topology->getNbPoints(); size_t numberOfCells; numberOfCells = ( (writeEdges.getValue()) ? topology->getNbEdges() : 0 ) @@ -600,21 +612,21 @@ void SigmaPointsVTKExporter::writeVTKXML() << "### ###" << msgendl << "Total nb cells: " << numberOfCells << msgendl; - //write header + // write header *outfile << "" << std::endl; *outfile << " " << std::endl; - //write piece + // write piece *outfile << " " << std::endl; - //write point data + // write point data if (!pointsData.empty()) { *outfile << " " << std::endl; writeDataArray(pointsDataObject, pointsDataField, pointsDataName); *outfile << " " << std::endl; } - //write cell data + // write cell data if (!cellsData.empty()) { *outfile << " " << std::endl; @@ -624,12 +636,12 @@ void SigmaPointsVTKExporter::writeVTKXML() - //write points + // write points *outfile << " " << std::endl; *outfile << " " << std::endl; if (!pointsPos.empty()) { - for (size_t i = 0 ; i < nbp; i++) + for (size_t i = 0; i < nbp; i++) { *outfile << "\t" << pointsPos[i] << std::endl; } @@ -647,34 +659,34 @@ void SigmaPointsVTKExporter::writeVTKXML() *outfile << " " << std::endl; *outfile << " " << std::endl; - //write cells + // write cells *outfile << " " << std::endl; - //write connectivity + // write connectivity *outfile << " " << std::endl; if (writeEdges.getValue()) { - for (unsigned int i=0 ; igetNbEdges() ; i++) + for (unsigned int i = 0; i < topology->getNbEdges(); i++) *outfile << " " << topology->getEdge(i) << std::endl; } if (writeTriangles.getValue()) { - for (unsigned int i=0 ; igetNbTriangles() ; i++) + for (unsigned int i = 0; i < topology->getNbTriangles(); i++) *outfile << " " << topology->getTriangle(i) << std::endl; } if (writeQuads.getValue()) { - for (unsigned int i=0 ; igetNbQuads() ; i++) + for (unsigned int i = 0; i < topology->getNbQuads(); i++) *outfile << " " << topology->getQuad(i) << std::endl; } if (writeTetras.getValue()) { - for (unsigned int i=0 ; igetNbTetras() ; i++) + for (unsigned int i = 0; i < topology->getNbTetras(); i++) *outfile << " " << topology->getTetra(i) << std::endl; } if (writeHexas.getValue()) { - for (unsigned int i=0 ; igetNbHexas() ; i++) + for (unsigned int i = 0; i < topology->getNbHexas(); i++) *outfile << " " << topology->getHexa(i) << std::endl; } *outfile << " " << std::endl; @@ -684,7 +696,7 @@ void SigmaPointsVTKExporter::writeVTKXML() *outfile << " "; if (writeEdges.getValue()) { - for (unsigned int i=0 ; igetNbEdges() ; i++) + for (unsigned int i = 0; i < topology->getNbEdges(); i++) { num += 2; *outfile << num << " "; @@ -692,7 +704,7 @@ void SigmaPointsVTKExporter::writeVTKXML() } if (writeTriangles.getValue()) { - for (unsigned int i=0 ; igetNbTriangles() ; i++) + for (unsigned int i = 0; igetNbTriangles(); i++) { num += 3; *outfile << num << " "; @@ -700,7 +712,7 @@ void SigmaPointsVTKExporter::writeVTKXML() } if (writeQuads.getValue()) { - for (unsigned int i=0 ; igetNbQuads() ; i++) + for (unsigned int i = 0; i < topology->getNbQuads(); i++) { num += 4; *outfile << num << " "; @@ -708,7 +720,7 @@ void SigmaPointsVTKExporter::writeVTKXML() } if (writeTetras.getValue()) { - for (unsigned int i=0 ; igetNbTetras() ; i++) + for (unsigned int i = 0; i < topology->getNbTetras(); i++) { num += 4; *outfile << num << " "; @@ -716,7 +728,7 @@ void SigmaPointsVTKExporter::writeVTKXML() } if (writeHexas.getValue()) { - for (unsigned int i=0 ; igetNbHexas() ; i++) + for (unsigned int i = 0; i < topology->getNbHexas(); i++) { num += 8; *outfile << num << " "; @@ -729,27 +741,27 @@ void SigmaPointsVTKExporter::writeVTKXML() *outfile << " "; if (writeEdges.getValue()) { - for (unsigned int i=0 ; igetNbEdges() ; i++) + for (unsigned int i = 0; i < topology->getNbEdges(); i++) *outfile << 3 << " "; } if (writeTriangles.getValue()) { - for (unsigned int i=0 ; igetNbTriangles() ; i++) + for (unsigned int i = 0; i < topology->getNbTriangles(); i++) *outfile << 5 << " "; } if (writeQuads.getValue()) { - for (unsigned int i=0 ; igetNbQuads() ; i++) + for (unsigned int i = 0; i < topology->getNbQuads(); i++) *outfile << 9 << " "; } if (writeTetras.getValue()) { - for (unsigned int i=0 ; igetNbTetras() ; i++) + for (unsigned int i = 0; i < topology->getNbTetras(); i++) *outfile << 10 << " "; } if (writeHexas.getValue()) { - for (unsigned int i=0 ; igetNbHexas() ; i++) + for (unsigned int i = 0; i < topology->getNbHexas(); i++) *outfile << 12 << " "; } *outfile << std::endl; @@ -766,6 +778,7 @@ void SigmaPointsVTKExporter::writeVTKXML() } + void SigmaPointsVTKExporter::handleEvent(sofa::core::objectmodel::Event *event) { if (sofa::core::objectmodel::KeypressedEvent::checkEventType(event)) @@ -786,7 +799,6 @@ void SigmaPointsVTKExporter::handleEvent(sofa::core::objectmodel::Event *event) } } - if ( /*simulation::AnimateEndEvent* ev =*/ simulation::AnimateEndEvent::checkEventType(event)) { unsigned int maxStep = exportEveryNbSteps.getValue(); @@ -815,13 +827,16 @@ void SigmaPointsVTKExporter::handleEvent(sofa::core::objectmodel::Event *event) } } + + void SigmaPointsVTKExporter::cleanup() { if (exportAtEnd.getValue()) (fileFormat.getValue()) ? writeVTKXML() : writeVTKSimple(); - } + + void SigmaPointsVTKExporter::bwdInit() { if (exportAtBegin.getValue()) diff --git a/src/genericComponents/SigmaPointsVTKExporter.h b/src/genericComponents/SigmaPointsVTKExporter.h index f958aefd..75922502 100644 --- a/src/genericComponents/SigmaPointsVTKExporter.h +++ b/src/genericComponents/SigmaPointsVTKExporter.h @@ -22,7 +22,6 @@ /* * SigmaPointsVTKExporter.h */ - #pragma once @@ -54,23 +53,6 @@ class SOFA_OPTIMUSPLUGIN_API SigmaPointsVTKExporter : public core::objectmodel:: public: SOFA_CLASS(SigmaPointsVTKExporter,core::objectmodel::BaseObject); -protected: - sofa::core::topology::BaseMeshTopology* topology; - sofa::core::behavior::BaseMechanicalState* mstate; - unsigned int stepCounter; - - std::ofstream* outfile; - - void fetchDataFields(const helper::vector& strData, helper::vector& objects, helper::vector& fields, helper::vector& names); - void writeVTKSimple(); - void writeVTKXML(); - void writeData(const helper::vector& objects, const helper::vector& fields, const helper::vector& names); - void writeDataArray(const helper::vector& objects, const helper::vector& fields, const helper::vector& names); - std::string segmentString(std::string str, unsigned int n); - - sofa::simulation::Node* gnode; - -public: sofa::core::objectmodel::DataFileName vtkFilename; Data fileFormat; ///< 0 for Simple Legacy Formats, 1 for XML File Format Data position; ///< points position (will use points from topology or mechanical state if this is empty) @@ -79,8 +61,8 @@ class SOFA_OPTIMUSPLUGIN_API SigmaPointsVTKExporter : public core::objectmodel:: Data writeQuads; ///< write quad topology Data writeTetras; ///< write tetra topology Data writeHexas; ///< write hexa topology - Data > dPointsDataFields; ///< Data to visualize (on points) - Data > dCellsDataFields; ///< Data to visualize (on cells) + Data > dPointsDataFields; ///< Data to visualize (on points) + Data > dCellsDataFields; ///< Data to visualize (on cells) Data exportEveryNbSteps; ///< export file only at specified number of steps (0=disable) Data exportAtBegin; ///< export file at the initialization Data exportAtEnd; ///< export file when the simulation is finished @@ -91,21 +73,39 @@ class SOFA_OPTIMUSPLUGIN_API SigmaPointsVTKExporter : public core::objectmodel:: double lastTime; - helper::vector pointsDataObject; - helper::vector pointsDataField; - helper::vector pointsDataName; + type::vector pointsDataObject; + type::vector pointsDataField; + type::vector pointsDataName; + + type::vector cellsDataObject; + type::vector cellsDataField; + type::vector cellsDataName; + - helper::vector cellsDataObject; - helper::vector cellsDataField; - helper::vector cellsDataName; protected: + sofa::core::topology::BaseMeshTopology* topology; + sofa::core::behavior::BaseMechanicalState* mstate; + unsigned int stepCounter; + + std::ofstream* outfile; + + void fetchDataFields(const type::vector& strData, type::vector& objects, type::vector& fields, type::vector& names); + void writeVTKSimple(); + void writeVTKXML(); + void writeData(const type::vector& objects, const type::vector& fields, const type::vector& names); + void writeDataArray(const type::vector& objects, const type::vector& fields, const type::vector& names); + std::string segmentString(std::string str, unsigned int n); + + sofa::simulation::Node* gnode; + SigmaPointsVTKExporter(); virtual ~SigmaPointsVTKExporter(); + + public: void init() override; void cleanup() override; void bwdInit() override; - void handleEvent(sofa::core::objectmodel::Event *) override; }; diff --git a/src/genericComponents/SimulatedStateObservationSource.cpp b/src/genericComponents/SimulatedStateObservationSource.cpp index 49c19a78..0b63170c 100644 --- a/src/genericComponents/SimulatedStateObservationSource.cpp +++ b/src/genericComponents/SimulatedStateObservationSource.cpp @@ -26,9 +26,6 @@ #include -using namespace sofa::defaulttype; - - namespace sofa { @@ -39,6 +36,10 @@ namespace component namespace container { + + +using namespace sofa::defaulttype; + SOFA_DECL_CLASS(SimulatedStateObservationSource) int SimulatedStateObservationSourceClass = core::RegisterObject("Parameters that will be adapted by ROUKF object") @@ -59,3 +60,4 @@ template class SOFA_OPTIMUSPLUGIN_API SimulatedStateObservationSource #include -#include -#include +#include +#include #include #include @@ -53,23 +53,26 @@ namespace container using namespace sofa::core::objectmodel; /** - Class implementing an observation source: it reads observations exported in direct simulation using OptimMonitor component and provides data to the observation manager. -*/ -class SOFA_OPTIMUSPLUGIN_API SimulatedStateObservationSourceBase : public BaseObject // ObservationSource + * Class implementing an observation source: + * it reads observations exported in direct simulation using OptimMonitor component and provides data to the observation manager. + */ +class SOFA_OPTIMUSPLUGIN_API SimulatedStateObservationSourceBase : public BaseObject { public: virtual unsigned int getObsDimention() = 0; }; + + template -class SimulatedStateObservationSource : public SimulatedStateObservationSourceBase // ObservationSource +class SimulatedStateObservationSource : public SimulatedStateObservationSourceBase { public: SOFA_CLASS(SOFA_TEMPLATE(SimulatedStateObservationSource, DataTypes) , SimulatedStateObservationSourceBase); typedef SimulatedStateObservationSourceBase Inherit; typedef typename DataTypes::VecCoord VecCoord; - typedef helper::vector VecIndex; + typedef type::vector VecIndex; typedef typename DataTypes::Coord Coord; typedef typename DataTypes::Real Real; typedef typename std::vector VecVecCoord; @@ -88,8 +91,11 @@ class SimulatedStateObservationSource : public SimulatedStateObservationSourceBa bool m_exportIndices; -public: + static constexpr double ROUND = 10000000.0; + static constexpr unsigned int UNCORRESPONDENT_INDEX = 65535; + +public: SimulatedStateObservationSource(); virtual ~SimulatedStateObservationSource() override; @@ -103,7 +109,6 @@ class SimulatedStateObservationSource : public SimulatedStateObservationSourceBa Data d_trackedObservations; Data< bool > d_asynObs; - /// maps: time + vector std::vector m_positions; @@ -163,9 +168,6 @@ class SimulatedStateObservationSource : public SimulatedStateObservationSourceBa { return DataTypes::Name(); } - -private: - }; diff --git a/src/genericComponents/SimulatedStateObservationSource.inl b/src/genericComponents/SimulatedStateObservationSource.inl index 0471f334..744e491b 100644 --- a/src/genericComponents/SimulatedStateObservationSource.inl +++ b/src/genericComponents/SimulatedStateObservationSource.inl @@ -33,8 +33,6 @@ #include #include #include -#define ROUND 10000000.0 -#define UNCORRESPONDENT_INDEX 65535 @@ -67,12 +65,15 @@ SimulatedStateObservationSource::SimulatedStateObservationSource() m_exportIndices = false; } + + template SimulatedStateObservationSource::~SimulatedStateObservationSource() { } + template void SimulatedStateObservationSource::init() { @@ -126,6 +127,7 @@ void SimulatedStateObservationSource::init() } + /* * File format: * time | indices | positions @@ -147,9 +149,9 @@ void SimulatedStateObservationSource::parseAsynMonitorFileVariable(co m_nObservations = 0; //info to fill - helper::vector vdt; - helper::vector > vindices; - helper::vector > vpositions; + type::vector vdt; + type::vector > vindices; + type::vector > vpositions; unsigned int maxIndex = 0; std::ifstream in(_name); @@ -163,8 +165,8 @@ void SimulatedStateObservationSource::parseAsynMonitorFileVariable(co continue; Real dt; - helper::vector indices; - helper::vector positions; + type::vector indices; + type::vector positions; std::istringstream ss(line); std::string substr; @@ -199,7 +201,7 @@ void SimulatedStateObservationSource::parseAsynMonitorFileVariable(co { VecCoord position(m_nParticles); - const helper::vector& indices = vindices[i]; + const type::vector& indices = vindices[i]; for(unsigned int j=0 ; j::parseAsynMonitorFileVariable(co } } + + template void SimulatedStateObservationSource::parseAsynMonitorFile(const std::string& _name) { m_observationTable.clear(); m_indexTable.clear(); -#ifdef __APPLE__ - setlocale(LC_ALL, "C"); -#else - std::setlocale(LC_ALL, "C"); -#endif + #ifdef __APPLE__ + setlocale(LC_ALL, "C"); + #else + std::setlocale(LC_ALL, "C"); + #endif std::ifstream file(_name.c_str()); @@ -289,26 +293,25 @@ void SimulatedStateObservationSource::parseAsynMonitorFile(const std: { sout << "Valid observations available: #observations: " << m_nObservations << " #particles: " << m_nParticles << std::endl; } -#ifdef __APPLE__ - setlocale(LC_ALL, "C"); -#else - std::setlocale(LC_ALL, "C"); -#endif + #ifdef __APPLE__ + setlocale(LC_ALL, "C"); + #else + std::setlocale(LC_ALL, "C"); + #endif +} -} template void SimulatedStateObservationSource::parseMonitorFile(const std::string &_name) { m_observationTable.clear(); m_indexTable.clear(); -#ifdef __APPLE__ - setlocale(LC_ALL, "C"); -#else - std::setlocale(LC_ALL, "C"); -#endif - + #ifdef __APPLE__ + setlocale(LC_ALL, "C"); + #else + std::setlocale(LC_ALL, "C"); + #endif std::ifstream file(_name.c_str()); @@ -374,10 +377,10 @@ void SimulatedStateObservationSource::parseMonitorFile(const std::str PRNS(" Working with 2D observations" << " dim: " << m_dim); } while (tokens.size() > 1) { -// if (dim != 3) { -// PRNE(" On line " << nLine << " dim: " << dim); -// return; -// } + // if (dim != 3) { + // PRNE(" On line " << nLine << " dim: " << dim); + // return; + // } double lineTime = atof(tokens[0].c_str()) ; if (m_initTime < 0.0) @@ -467,14 +470,14 @@ void SimulatedStateObservationSource::parseMonitorFile(const std::str std::cout << "OBSERVATION FATAL ERROR: no positions and no particles, adding dummy zero observation vector of length 1000!" << std::endl; } }*/ -#ifdef __APPLE__ - setlocale(LC_ALL, "C"); -#else - std::setlocale(LC_ALL, "C"); -#endif + #ifdef __APPLE__ + setlocale(LC_ALL, "C"); + #else + std::setlocale(LC_ALL, "C"); + #endif +} -} template bool SimulatedStateObservationSource::getObservation(double _time, VecCoord& _observation) @@ -483,6 +486,8 @@ bool SimulatedStateObservationSource::getObservation(double _time, Ve return getObservation(_time, _observation, index); } + + template bool SimulatedStateObservationSource::getObservation(double _time, VecCoord& _observation, VecIndex& _index) { @@ -536,7 +541,7 @@ bool SimulatedStateObservationSource::getObservation(double _time, Ve } else { - size_t ix = (fabs(m_dt) < 1e-10) ? 0 : size_t(round(_time/m_dt)); + size_t ix = (fabs(m_dt) < 1e-10) ? 0 : size_t(round(_time / m_dt)); //PRNS("Getting observation for time " << _time << " index: " << ix); if (ix >= size_t(m_positions.size())) { @@ -552,10 +557,12 @@ bool SimulatedStateObservationSource::getObservation(double _time, Ve } } + + template bool SimulatedStateObservationSource::getCorrespondentIndices(double _time, VecIndex &_index) { - size_t ix = (fabs(m_dt) < 1e-10) ? 0 : size_t(round(_time/m_dt)); + size_t ix = (fabs(m_dt) < 1e-10) ? 0 : size_t(round(_time / m_dt)); if (ix >= size_t(m_correspondentIndices.size())) { //PRNE("No observation for time " << _time << " , using the last one from " << m_positions.size()-1); @@ -595,6 +602,8 @@ void SimulatedStateObservationSource::draw(const core::visual::Visual // vparams->drawTool()->drawSpheres(points, float(m_drawSize.getValue()), sofa::defaulttype::Vec<4, float> (0.0f, 0.0f, 1.0f, 1.0f)); } + + template void SimulatedStateObservationSource::handleEvent(core::objectmodel::Event *event) { diff --git a/src/genericComponents/SimulatedStateObservationStreamer.cpp b/src/genericComponents/SimulatedStateObservationStreamer.cpp index a852256b..2a5de6d1 100644 --- a/src/genericComponents/SimulatedStateObservationStreamer.cpp +++ b/src/genericComponents/SimulatedStateObservationStreamer.cpp @@ -37,6 +37,7 @@ namespace container { + using namespace sofa::defaulttype; SOFA_DECL_CLASS(SimulatedStateObservationStreamer) @@ -59,3 +60,4 @@ template class SOFA_OPTIMUSPLUGIN_API SimulatedStateObservationStreamer #include -#include -#include +#include +#include #include #include @@ -53,7 +53,7 @@ namespace container using namespace sofa::core::objectmodel; template -class SOFA_OPTIMUSPLUGIN_API SimulatedStateObservationStreamer : public SimulatedStateObservationSource // ObservationSource +class SOFA_OPTIMUSPLUGIN_API SimulatedStateObservationStreamer : public SimulatedStateObservationSource { public: SOFA_CLASS(SOFA_TEMPLATE(SimulatedStateObservationStreamer, DataTypes) , SOFA_TEMPLATE(SimulatedStateObservationSource, DataTypes)); @@ -64,12 +64,6 @@ class SOFA_OPTIMUSPLUGIN_API SimulatedStateObservationStreamer : public Simulate typedef typename std::vector VecVecCoord; //typedef typename std::map ObservationTable; -protected: - //ObservationTable observationTable; - - unsigned int nParticles, nObservations, dim; - -public: SimulatedStateObservationStreamer(); ~SimulatedStateObservationStreamer(); @@ -147,6 +141,9 @@ class SOFA_OPTIMUSPLUGIN_API SimulatedStateObservationStreamer : public Simulate return DataTypes::Name(); } +protected: + //ObservationTable observationTable; + unsigned int nParticles, nObservations, dim; }; diff --git a/src/genericComponents/SimulatedStateObservationStreamer.inl b/src/genericComponents/SimulatedStateObservationStreamer.inl index 585f85c1..2369db8e 100644 --- a/src/genericComponents/SimulatedStateObservationStreamer.inl +++ b/src/genericComponents/SimulatedStateObservationStreamer.inl @@ -52,15 +52,17 @@ SimulatedStateObservationStreamer::SimulatedStateObservationStreamer( , m_actualTime( initData (&m_actualTime, "actualTime", "actual iteration time") ) , m_prevTime( initData (&m_prevTime, "previousTime", "previous iteration time") ) { - } + + template SimulatedStateObservationStreamer::~SimulatedStateObservationStreamer() { } + template void SimulatedStateObservationStreamer::init() { @@ -104,6 +106,8 @@ void SimulatedStateObservationStreamer::draw(const core::visual::Visu // vparams->drawTool()->drawSpheres(points, float(m_drawSize.getValue()), sofa::defaulttype::Vec<4, float> (0.0f, 0.0f, 1.0f, 1.0f)); } + + template void SimulatedStateObservationStreamer::handleEvent(core::objectmodel::Event *event) { diff --git a/src/genericComponents/StepPCGLinearSolver.cpp b/src/genericComponents/StepPCGLinearSolver.cpp index 6c9af22a..e993dd60 100644 --- a/src/genericComponents/StepPCGLinearSolver.cpp +++ b/src/genericComponents/StepPCGLinearSolver.cpp @@ -19,7 +19,6 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -// // Copyright: See COPYING file that comes with this distribution #include #include @@ -27,7 +26,6 @@ #include #include #include -#include #include #include #include @@ -48,15 +46,16 @@ namespace linearsolver { + using namespace sofa::defaulttype; using namespace sofa::core::behavior; using namespace sofa::simulation; using namespace sofa::core::objectmodel; -using sofa::helper::system::thread::CTime; -using sofa::helper::system::thread::ctime_t; using std::cerr; using std::endl; + + template StepPCGLinearSolver::StepPCGLinearSolver() : f_maxIter( initData(&f_maxIter,(unsigned)25,"iterations","maximum number of iterations of the Conjugate Gradient solution") ) @@ -73,11 +72,13 @@ StepPCGLinearSolver::StepPCGLinearSolver() , verbose( initData(&verbose, false, "verbose", "print tracing informations") ) { f_graph.setWidget("graph"); -// f_graph.setReadOnly(true); + // f_graph.setReadOnly(true); first = true; this->f_listening.setValue(true); } + + template void StepPCGLinearSolver::init() { @@ -101,6 +102,8 @@ void StepPCGLinearSolver::init() f_iterationsNeeded.setValue(100000); } + + template void StepPCGLinearSolver::setSystemMBKMatrix(const core::MechanicalParams* mparams) { @@ -119,7 +122,6 @@ void StepPCGLinearSolver::setSystemMBKMatrix(const core::Mechan f_forceFactorization.getValue() || // forcing in any case (f_precondOnTimeStep.getValue() && firstTemporalInvocation) ) // only in first temporal invocation { - sofa::helper::AdvancedTimer::valSet("PCG::PrecondBuildMBK", 1); sofa::helper::AdvancedTimer::stepBegin("PCG::PrecondSetSystemMBKMatrix"); PRNS("Invoking LDL in first simulation in given time step!"); @@ -149,28 +151,34 @@ void StepPCGLinearSolver::setSystemMBKMatrix(const core::Mechan sofa::helper::AdvancedTimer::stepEnd("PCG::PrecondSetSystemMBKMatrix"); } }*/ - preconditioners->updateSystemMatrix(); } + + template<> inline void StepPCGLinearSolver::cgstep_beta(Vector& p, Vector& r, double beta) { p.eq(r,p,beta); // p = p*beta + r } + + template<> inline void StepPCGLinearSolver::cgstep_alpha(Vector& x, Vector& p, double alpha) { x.peq(p,alpha); // x = x + alpha p } + + template -void StepPCGLinearSolver::handleEvent(sofa::core::objectmodel::Event* event) { +void StepPCGLinearSolver::handleEvent(sofa::core::objectmodel::Event* event) +{ /// this event shoul be launch before the addKToMatrix if (sofa::simulation::AnimateBeginEvent::checkEventType(event)) { newton_iter = 0; - std::map < std::string, sofa::helper::vector >& graph = * f_graph.beginEdit(); + std::map < std::string, sofa::type::vector >& graph = * f_graph.beginEdit(); graph.clear(); double actualTime = gnode->getTime(); @@ -183,24 +191,24 @@ void StepPCGLinearSolver::handleEvent(sofa::core::objectmodel::Ev if (next_refresh_step == 1) firstTemporalInvocation = true; - } } } + template void StepPCGLinearSolver::solve (Matrix& M, Vector& x, Vector& b) { sofa::helper::AdvancedTimer::stepBegin("PCGLinearSolver::solve"); - std::map < std::string, sofa::helper::vector >& graph = * f_graph.beginEdit(); -// sofa::helper::vector& graph_error = graph["Error"]; + std::map < std::string, sofa::type::vector >& graph = *f_graph.beginEdit(); + // sofa::helper::vector& graph_error = graph["Error"]; newton_iter++; char name[256]; - sprintf(name,"Error %d",newton_iter); - sofa::helper::vector& graph_error = graph[std::string(name)]; + sprintf(name, "Error %d", newton_iter); + sofa::type::vector& graph_error = graph[std::string(name)]; const core::ExecParams* params = core::ExecParams::defaultInstance(); typename Inherit::TempVectorContainer vtmp(this, params, M, x, b); @@ -215,7 +223,7 @@ void StepPCGLinearSolver::solve (Matrix& M, Vector& x, Vector& //PRNS("M : " << M.colSize() << " x " << M.rowSize()); r = M * x; - cgstep_beta(r,b,-1);// r = -1 * r + b = b - (M * x) + cgstep_beta(r, b, -1);// r = -1 * r + b = b - (M * x) if (apply_precond) { sofa::helper::AdvancedTimer::stepBegin("PCGLinearSolver::apply Precond"); @@ -238,8 +246,8 @@ void StepPCGLinearSolver::solve (Matrix& M, Vector& x, Vector& double dtq = w.dot(s); double alpha = r_norm / dtq; - cgstep_alpha(x,w,alpha);//for(int i=0; i::solve (Matrix& M, Vector& x, Vector& double deltaOld = r_norm; r_norm = r.dot(s); - graph_error.push_back(r_norm/b_norm); + graph_error.push_back(r_norm / b_norm); double beta = r_norm / deltaOld; - cgstep_beta(w,s,beta);//for (int i=0; i::solve (Matrix& M, Vector& x, Vector& } + SOFA_DECL_CLASS(StepPCGLinearSolver) int StepPCGLinearSolverClass = core::RegisterObject("Linear system solver using the conjugate gradient iterative algorithm") diff --git a/src/genericComponents/StepPCGLinearSolver.h b/src/genericComponents/StepPCGLinearSolver.h index ed2462b8..1b5a1830 100644 --- a/src/genericComponents/StepPCGLinearSolver.h +++ b/src/genericComponents/StepPCGLinearSolver.h @@ -24,9 +24,9 @@ #include #include #include +#include #include "../initOptimusPlugin.h" - #include @@ -43,14 +43,14 @@ namespace linearsolver /// Linear system solver using the conjugate gradient iterative algorithm template -class StepPCGLinearSolver : public sofa::component::linearsolver::MatrixLinearSolver +class StepPCGLinearSolver : public sofa::component::linearsolver::MatrixLinearSolver { public: - SOFA_CLASS(SOFA_TEMPLATE2(StepPCGLinearSolver,TMatrix,TVector),SOFA_TEMPLATE2(sofa::component::linearsolver::MatrixLinearSolver,TMatrix,TVector)); + SOFA_CLASS(SOFA_TEMPLATE2(StepPCGLinearSolver, TMatrix, TVector),SOFA_TEMPLATE2(sofa::component::linearsolver::MatrixLinearSolver, TMatrix, TVector)); typedef TMatrix Matrix; typedef TVector Vector; - typedef sofa::component::linearsolver::MatrixLinearSolver Inherit; + typedef sofa::component::linearsolver::MatrixLinearSolver Inherit; Data f_maxIter; Data f_tolerance; @@ -62,42 +62,43 @@ class StepPCGLinearSolver : public sofa::component::linearsolver::MatrixLinearSo Data f_iterationsNeeded; Data f_numIterationsToRefactorize; Data< std::string > f_preconditioners; - Data > > f_graph; + Data > > f_graph; Data verbose; protected: - StepPCGLinearSolver(); -public: - void solve (Matrix& M, Vector& x, Vector& b) override; - void init() override; - void setSystemMBKMatrix(const core::MechanicalParams* mparams) override; - //void setSystemRHVector(VecId v); - //void setSystemLHVector(VecId v); + StepPCGLinearSolver(); -private : - unsigned next_refresh_step; - sofa::core::behavior::LinearSolver* preconditioners; - bool first; - int newton_iter; - - double lastTime; - -protected: /// This method is separated from the rest to be able to use custom/optimized versions depending on the types of vectors. /// It computes: p = p*beta + r inline void cgstep_beta(Vector& p, Vector& r, double beta); /// This method is separated from the rest to be able to use custom/optimized versions depending on the types of vectors. /// It computes: x += p*alpha, r -= q*alpha - inline void cgstep_alpha(Vector& x,Vector& p,double alpha); + inline void cgstep_alpha(Vector& x, Vector& p, double alpha); void handleEvent(sofa::core::objectmodel::Event* event) override; - sofa::simulation::Node* gnode; double firstTemporalInvocation; + + +private: + unsigned next_refresh_step; + sofa::core::behavior::LinearSolver* preconditioners; + bool first; + int newton_iter; + double lastTime; + +public: + void solve (Matrix& M, Vector& x, Vector& b) override; + void init() override; + void setSystemMBKMatrix(const core::MechanicalParams* mparams) override; + //void setSystemRHVector(VecId v); + //void setSystemLHVector(VecId v); }; + + template inline void StepPCGLinearSolver::cgstep_beta(Vector& p, Vector& r, double beta) { @@ -105,18 +106,23 @@ inline void StepPCGLinearSolver::cgstep_beta(Vector& p, Vector& p += r; //z; } + + template -inline void StepPCGLinearSolver::cgstep_alpha(Vector& x,Vector& p,double alpha) +inline void StepPCGLinearSolver::cgstep_alpha(Vector& x, Vector& p, double alpha) { x.peq(p,alpha); // x = x + alpha p } + template<> inline void StepPCGLinearSolver::cgstep_beta(Vector& p, Vector& r, double beta); + + template<> -inline void StepPCGLinearSolver::cgstep_alpha(Vector& x,Vector& p,double alpha); +inline void StepPCGLinearSolver::cgstep_alpha(Vector& x, Vector& p, double alpha); diff --git a/src/genericComponents/StochasticPositionHandler.cpp b/src/genericComponents/StochasticPositionHandler.cpp index 42280b21..9c3e8778 100644 --- a/src/genericComponents/StochasticPositionHandler.cpp +++ b/src/genericComponents/StochasticPositionHandler.cpp @@ -20,6 +20,8 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #include +#include +#include #include @@ -34,11 +36,10 @@ namespace misc { -SOFA_DECL_CLASS(StochasticPositionHandler) using namespace defaulttype; - +SOFA_DECL_CLASS(StochasticPositionHandler) int StochasticPositionHandlerClass = core::RegisterObject("Write State vectors to file at each timestep") .add< StochasticPositionHandler >(); @@ -51,8 +52,7 @@ StochasticPositionHandlerCreator::StochasticPositionHandlerCreator(const core::E , recordV(true) , createInMapping(false) , counterStochasticPositionHandler(0) -{ -} +{ } StochasticPositionHandlerCreator::StochasticPositionHandlerCreator(const core::ExecParams* params, const std::string &n, bool _recordX, bool _recordV, bool _recordF, bool _createInMapping, int c) :simulation::Visitor(params) @@ -62,18 +62,20 @@ StochasticPositionHandlerCreator::StochasticPositionHandlerCreator(const core::E , recordF(_recordF) , createInMapping(_createInMapping) , counterStochasticPositionHandler(c) -{ -} +{ } + //Create a Write State component each time a mechanical state is found simulation::Visitor::Result StochasticPositionHandlerCreator::processNodeTopDown( simulation::Node* gnode) { - sofa::core::behavior::BaseMechanicalState * mstate=gnode->mechanicalState; - if (!mstate) return simulation::Visitor::RESULT_CONTINUE; - core::behavior::OdeSolver *isSimulated; + sofa::core::behavior::BaseMechanicalState* mstate = gnode->mechanicalState; + if (!mstate) + return simulation::Visitor::RESULT_CONTINUE; + core::behavior::OdeSolver* isSimulated; mstate->getContext()->get(isSimulated); - if (!isSimulated) return simulation::Visitor::RESULT_CONTINUE; + if (!isSimulated) + return simulation::Visitor::RESULT_CONTINUE; //We have a mechanical state addStochasticPositionHandler(mstate, gnode); @@ -81,10 +83,11 @@ simulation::Visitor::Result StochasticPositionHandlerCreator::processNodeTopDown } + void StochasticPositionHandlerCreator::addStochasticPositionHandler(sofa::core::behavior::BaseMechanicalState *ms, simulation::Node* gnode) { sofa::core::objectmodel::BaseContext* context = gnode->getContext(); - sofa::core::BaseMapping *mapping; + sofa::core::BaseMapping* mapping; context->get(mapping); if ( createInMapping || mapping == NULL) { @@ -106,7 +109,6 @@ void StochasticPositionHandlerCreator::addStochasticPositionHandler(sofa::core:: ws->f_filename.setValue(ofilename.str()); ws->init(); ws->f_listening.setValue(true); //Activated at init ++counterStochasticPositionHandler; - } } @@ -115,11 +117,13 @@ void StochasticPositionHandlerCreator::addStochasticPositionHandler(sofa::core:: //if state is true, we activate all the write states present in the scene. simulation::Visitor::Result StochasticPositionHandlerActivator::processNodeTopDown( simulation::Node* gnode) { - sofa::component::misc::StochasticPositionHandler *ws = gnode->get< sofa::component::misc::StochasticPositionHandler >(this->subsetsToManage); + sofa::component::misc::StochasticPositionHandler* ws = gnode->get< sofa::component::misc::StochasticPositionHandler >(this->subsetsToManage); if (ws != NULL) { changeStateWriter(ws);} return simulation::Visitor::RESULT_CONTINUE; } + + void StochasticPositionHandlerActivator::changeStateWriter(sofa::component::misc::StochasticPositionHandler*ws) { if (!state) ws->reset(); diff --git a/src/genericComponents/StochasticPositionHandler.h b/src/genericComponents/StochasticPositionHandler.h index 3fc4b258..322d303f 100644 --- a/src/genericComponents/StochasticPositionHandler.h +++ b/src/genericComponents/StochasticPositionHandler.h @@ -22,10 +22,10 @@ #pragma once #include "initOptimusPlugin.h" -#include #include #include #include +#include #include #include #include @@ -53,7 +53,7 @@ namespace misc class SOFA_OPTIMUSPLUGIN_API StochasticPositionHandler: public core::objectmodel::BaseObject { public: - SOFA_CLASS(StochasticPositionHandler,core::objectmodel::BaseObject); + SOFA_CLASS(StochasticPositionHandler, core::objectmodel::BaseObject); sofa::core::objectmodel::DataFileName f_filename; Data < bool > f_writeX; @@ -61,9 +61,10 @@ class SOFA_OPTIMUSPLUGIN_API StochasticPositionHandler: public core::objectmodel Data < double > f_keperiod; Data < bool > d_groundTruth; Data < bool > d_observations; - Data< helper::vector > d_indices; + Data< type::vector > d_indices; Data < bool > d_finalStochPos; + protected: core::behavior::BaseMechanicalState* mmodel; std::ofstream* outfile; @@ -71,11 +72,10 @@ class SOFA_OPTIMUSPLUGIN_API StochasticPositionHandler: public core::objectmodel unsigned int nextTime; double lastTime; - - StochasticPositionHandler(); - virtual ~StochasticPositionHandler(); + + public: virtual void init() override; @@ -83,7 +83,7 @@ class SOFA_OPTIMUSPLUGIN_API StochasticPositionHandler: public core::objectmodel virtual void reset() override; -// virtual void finalStochasticState(sofa::core::objectmodel::Event* event) ; + // virtual void finalStochasticState(sofa::core::objectmodel::Event* event) ; virtual void handleEvent(sofa::core::objectmodel::Event* event) override; double time_prec; @@ -98,49 +98,53 @@ class SOFA_OPTIMUSPLUGIN_API StochasticPositionHandler: public core::objectmodel return false; return BaseObject::canCreate(obj, context, arg); } - }; + + ///Create StochasticPositionHandler component in the graph each time needed class StochasticPositionHandlerCreator: public simulation::Visitor { -public: - StochasticPositionHandlerCreator(const core::ExecParams* params); - StochasticPositionHandlerCreator(const core::ExecParams* params, const std::string &n, bool _recordX, bool _recordV, bool _recordF, bool _createInMapping, int c=0); - virtual Result processNodeTopDown( simulation::Node* ); - - void setSceneName(std::string &n) { sceneName = n; } - void setRecordX(bool b) {recordX=b;} - void setRecordV(bool b) {recordV=b;} - void setRecordF(bool b) {recordF=b;} - void setCreateInMapping(bool b) { createInMapping=b; } - void setCounter(int c) { counterStochasticPositionHandler = c; } - virtual const char* getClassName() const { return "StochasticPositionHandlerCreator"; } protected: std::string sceneName; std::string extension; - bool recordX,recordV,recordF; + bool recordX, recordV, recordF; bool createInMapping; int counterStochasticPositionHandler; //avoid to have two same files if two mechanical objects has the same name void addStochasticPositionHandler(sofa::core::behavior::BaseMechanicalState*ms, simulation::Node* gnode); + +public: + StochasticPositionHandlerCreator(const core::ExecParams* params); + StochasticPositionHandlerCreator(const core::ExecParams* params, const std::string &n, bool _recordX, bool _recordV, bool _recordF, bool _createInMapping, int c=0); + virtual Result processNodeTopDown( simulation::Node* ); + + void setSceneName(std::string &n) { sceneName = n; } + void setRecordX(bool b) { recordX = b; } + void setRecordV(bool b) { recordV = b; } + void setRecordF(bool b) { recordF = b; } + void setCreateInMapping(bool b) { createInMapping = b; } + void setCounter(int c) { counterStochasticPositionHandler = c; } + virtual const char* getClassName() const { return "StochasticPositionHandlerCreator"; } }; + + class StochasticPositionHandlerActivator: public simulation::Visitor { +protected: + void changeStateWriter(sofa::component::misc::StochasticPositionHandler *ws); + bool state; + public: StochasticPositionHandlerActivator( const core::ExecParams* params, bool active) : Visitor(params), state(active) {} - virtual Result processNodeTopDown( simulation::Node* ); + virtual Result processNodeTopDown( simulation::Node* ); bool getState() const { return state; } - void setState(bool active) { state=active; } + void setState(bool active) { state = active; } virtual const char* getClassName() const { return "StochasticPositionHandlerActivator"; } -protected: - void changeStateWriter(sofa::component::misc::StochasticPositionHandler *ws); - - bool state; }; diff --git a/src/genericComponents/StochasticPositionHandler.inl b/src/genericComponents/StochasticPositionHandler.inl index 9e5f9c9c..c2a664e3 100644 --- a/src/genericComponents/StochasticPositionHandler.inl +++ b/src/genericComponents/StochasticPositionHandler.inl @@ -100,12 +100,15 @@ void StochasticPositionHandler::init() } } + void StochasticPositionHandler::reinit(){ if (outfile) delete outfile; init(); } + + void StochasticPositionHandler::reset() { nextTime = 0; @@ -115,44 +118,43 @@ void StochasticPositionHandler::reset() void StochasticPositionHandler::handleEvent(sofa::core::objectmodel::Event* event) { - if (/* simulation::AnimateBeginEvent* ev = */simulation::AnimateEndEvent::checkEventType(event)) - -// if (sofa::component::stochastic::CorrectionEndEvent::checkEventType(event)) + if (/* simulation::AnimateBeginEvent* ev = */simulation::AnimateEndEvent::checkEventType(event)) + // if (sofa::component::stochastic::CorrectionEndEvent::checkEventType(event)) { - - double time = getContext()->getTime(); - if (d_groundTruth.getValue()) - time=time+this->getContext()->getDt(); - if (!mmodel) return; - if (outfile) - { - if (d_observations.getValue()){ - (*outfile) << time << " "; - if (f_writeX.getValue()) - { - mmodel->writeVec(core::VecId::position(), *outfile); - (*outfile) << "\n"; - } - if (f_writeV.getValue()) - { - mmodel->writeVec(core::VecId::velocity(), *outfile); - (*outfile) << "\n"; - } - } else { - // write the X state - (*outfile) << "T= "<< time << "\n"; - if (f_writeX.getValue()) - { - (*outfile) << " X= "; - mmodel->writeVec(core::VecId::position(), *outfile); - (*outfile) << "\n"; - } + double time = getContext()->getTime(); + if (d_groundTruth.getValue()) + time = time + this->getContext()->getDt(); + if (!mmodel) return; + if (outfile) + { + if (d_observations.getValue()){ + (*outfile) << time << " "; + if (f_writeX.getValue()) + { + mmodel->writeVec(core::VecId::position(), *outfile); + (*outfile) << "\n"; + } + if (f_writeV.getValue()) + { + mmodel->writeVec(core::VecId::velocity(), *outfile); + (*outfile) << "\n"; } - outfile->flush(); + } else { + // write the X state + (*outfile) << "T= "<< time << "\n"; + if (f_writeX.getValue()) + { + (*outfile) << " X= "; + mmodel->writeVec(core::VecId::position(), *outfile); + (*outfile) << "\n"; + } + } + outfile->flush(); } } } + //void StochasticPositionHandler::handleEvent(sofa::core::objectmodel::Event* event) //{ // if (/* simulation::AnimateBeginEvent* ev = */simulation::AnimateEndEvent::checkEventType(event)) diff --git a/src/genericComponents/TimeProfiling.h b/src/genericComponents/TimeProfiling.h index 04e4d572..842e228a 100644 --- a/src/genericComponents/TimeProfiling.h +++ b/src/genericComponents/TimeProfiling.h @@ -21,6 +21,7 @@ ******************************************************************************/ #pragma once +#include #include #include #include @@ -39,8 +40,15 @@ namespace stochastic class TimeProfiling { -public: +protected: + boost::posix_time::time_duration currentMomentMicroseconds; + boost::posix_time::ptime time_t_epoch; + bool m_isActive; + size_t m_stepNumber; + std::string m_timeDataFile; + +public: TimeProfiling( ) : m_isActive(false) { time_t_epoch = boost::posix_time::ptime(boost::gregorian::date(1970,1,1)); } @@ -104,13 +112,6 @@ class TimeProfiling outputFile.close(); } } - -protected: - boost::posix_time::time_duration currentMomentMicroseconds; - boost::posix_time::ptime time_t_epoch; - bool m_isActive; - size_t m_stepNumber; - std::string m_timeDataFile; }; diff --git a/src/genericComponents/TransformStochasticEngine.h b/src/genericComponents/TransformStochasticEngine.h index e7cb383b..96b4ce6d 100644 --- a/src/genericComponents/TransformStochasticEngine.h +++ b/src/genericComponents/TransformStochasticEngine.h @@ -30,7 +30,7 @@ #include #include -#include +#include #include #include @@ -57,17 +57,32 @@ class TransformStochasticEngine : public core::DataEngine typedef typename DataTypes::VecCoord VecCoord; typedef typename DataTypes::Coord Coord; typedef typename DataTypes::Real Real; + typedef sofa::type::Quat Quaternion; typedef sofa::core::topology::BaseMeshTopology::SeqTriangles SeqTriangles; typedef sofa::core::topology::BaseMeshTopology::SeqQuads SeqQuads; typedef typename SeqTriangles::value_type Triangle; typedef typename SeqQuads::value_type Quad; + protected: + Data f_inputX; // input position + Data f_outputX; // ouput position + Data translation; // translation + Data rotation; // rotation + Data quaternion; // quaternion rotation + Data scale; // scale + Data inverse; + Data> d_optimParams; + Data d_triangles; //< input triangles + Data d_quads; //< input quads + Data d_normals; //< ouput normals + Data d_stochEstim; - TransformStochasticEngine(); - ~TransformStochasticEngine() {} public: + TransformStochasticEngine(); + ~TransformStochasticEngine() {} + void init() override; void bwdInit() override; @@ -84,20 +99,6 @@ class TransformStochasticEngine : public core::DataEngine { return DataTypes::Name(); } - -protected: - Data f_inputX; // input position - Data f_outputX; // ouput position - Data translation; // translation - Data rotation; // rotation - Data quaternion; // quaternion rotation - Data scale; // scale - Data inverse; - Data> d_optimParams; - Data d_triangles; //< input triangles - Data d_quads; //< input quads - Data d_normals; //< ouput normals - Data d_stochEstim; }; extern template class SOFA_OPTIMUSPLUGIN_API TransformStochasticEngine; diff --git a/src/genericComponents/TransformStochasticEngine.inl b/src/genericComponents/TransformStochasticEngine.inl index 3ab656d3..1e1c8079 100644 --- a/src/genericComponents/TransformStochasticEngine.inl +++ b/src/genericComponents/TransformStochasticEngine.inl @@ -22,7 +22,7 @@ #pragma once -#include //M_PI +#include #include #include #include @@ -42,26 +42,21 @@ namespace engine { -using helper::ReadAccessor; -using helper::WriteOnlyAccessor; -using helper::vector; -using collision::TriangleOctreeRoot; template TransformStochasticEngine::TransformStochasticEngine() : f_inputX ( initData (&f_inputX, "input_position", "input array of 3d points") ) , f_outputX( initData (&f_outputX, "output_position", "output array of 3d points") ) - , translation(initData(&translation, defaulttype::Vector3(0,0,0),"translation", "translation vector ") ) - , rotation(initData(&rotation, defaulttype::Vector3(0,0,0), "rotation", "rotation vector ") ) - , quaternion(initData(&quaternion, defaulttype::Quaternion(0,0,0,1), "quaternion", "rotation quaternion ") ) - , scale(initData(&scale, defaulttype::Vector3(1,1,1),"scale", "scale factor") ) + , translation(initData(&translation, type::Vector3(0,0,0),"translation", "translation vector ") ) + , rotation(initData(&rotation, type::Vector3(0,0,0), "rotation", "rotation vector ") ) + , quaternion(initData(&quaternion, type::Quat(0.0, 0.0, 0.0, 1.0), "quaternion", "rotation quaternion ") ) + , scale(initData(&scale, type::Vector3(1,1,1),"scale", "scale factor") ) , inverse(initData(&inverse, false, "inverse", "true to apply inverse transformation")) , d_optimParams(initData(&d_optimParams,"optimParams", "build transformation matrix from OptimParams ")) , d_triangles( initData (&d_triangles, "triangles", "input mesh triangles") ) , d_quads( initData (&d_quads, "quads", "input mesh quads") ) , d_normals( initData (&d_normals, "normal", "point normals") ) , d_stochEstim(initData(&d_stochEstim, true, "stochEstim", "set to True to retrieve transformation matrix from optimParams")) - { addInput(&f_inputX); addInput(&d_triangles); @@ -77,67 +72,72 @@ TransformStochasticEngine::TransformStochasticEngine() setDirtyValue(); } + template void TransformStochasticEngine::init() { } + + template void TransformStochasticEngine::bwdInit() { - if((d_triangles.getValue().size()==0) && (d_quads.getValue().size()==0)) + if ((d_triangles.getValue().size() == 0) && (d_quads.getValue().size() == 0)) msg_warning(this) << "No input mesh"; - if(f_inputX.getValue().size()==0) + if (f_inputX.getValue().size() == 0) msg_warning(this) << "No input position"; -// if(d_optimParams.getValue().size()!=7) -// msg_warning(this) << "Wrong Size of Stochastic Params. Needs 3 angles, 3 position and 1 scalar to dilate"; - + // if(d_optimParams.getValue().size()!=7) + // msg_warning(this) << "Wrong Size of Stochastic Params. Needs 3 angles, 3 position and 1 scalar to dilate"; } + template void TransformStochasticEngine::reinit() { doUpdate(); - } + //Declare a TransformOperation class able to do an operation on a Coord template struct TransformOperation { virtual ~TransformOperation() {} - virtual void execute(typename DataTypes::Coord &v) const =0; + virtual void execute(typename DataTypes::Coord &v) const = 0; }; + //***************************************************************** //Scale Operation template struct Scale : public TransformOperation { typedef typename DataTypes::Real Real; - Scale():sx(0),sy(0),sz(0) {} + Scale() : sx(0), sy(0), sz(0) {} void execute(typename DataTypes::Coord &p) const { Real x,y,z; - DataTypes::get(x,y,z,p); - DataTypes::set(p,x*sx,y*sy,z*sz); + DataTypes::get(x, y, z, p); + DataTypes::set(p, x * sx, y * sy, z * sz); } - void configure(const defaulttype::Vector3 &s, bool inverse) + void configure(const type::Vector3 &s, bool inverse) { if (inverse) { - sx=(Real)(1.0/s[0]); sy=(Real)(1.0/s[1]); sz=(Real)(1.0/s[2]); + sx = (Real)(1.0 / s[0]); sy = (Real)(1.0 / s[1]); sz = (Real)(1.0 / s[2]); } else { - sx=(Real)s[0]; sy=(Real)s[1]; sz=(Real)s[2]; + sx = (Real)s[0]; sy = (Real)s[1]; sz = (Real)s[2]; } } + private: - Real sx,sy,sz; + Real sx, sy, sz; }; @@ -150,35 +150,36 @@ struct RotationSpecialized : public TransformOperation void execute(typename DataTypes::Coord &p) const { - defaulttype::Vector3 pos; + type::Vector3 pos; DataTypes::get(pos[0],pos[1],pos[2],p); pos=q.rotate(pos); DataTypes::set(p,pos[0],pos[1],pos[2]); } - void configure(const defaulttype::Vector3 &r, bool inverse) + void configure(const type::Vector3 &r, bool inverse) { - q=helper::Quater::createQuaterFromEuler( r*(M_PI/180.0)); + q = type::Quat::createQuaterFromEuler( r * (M_PI / 180.0)); if (inverse) q = q.inverse(); - } - void configure(const defaulttype::Quaternion &qi, bool inverse, sofa::core::objectmodel::Base*) + void configure(const type::Quat &qi, bool inverse, sofa::core::objectmodel::Base*) { - q=qi; + q = qi; if (inverse) q = q.inverse(); } private: - defaulttype::Quaternion q; + type::Quat q; }; + template -struct Rotation : public RotationSpecialized -{ }; +struct Rotation : public RotationSpecialized { }; + + //***************************************************************** //Translation Operation @@ -186,14 +187,15 @@ template struct Translation : public TransformOperation { typedef typename DataTypes::Real Real; - Translation():tx(0),ty(0),tz(0) {} + Translation() : tx(0), ty(0), tz(0) {} void execute(typename DataTypes::Coord &p) const { Real x,y,z; - DataTypes::get(x,y,z,p); - DataTypes::set(p,x+tx,y+ty,z+tz); + DataTypes::get(x, y, z, p); + DataTypes::set(p, x + tx, y + ty, z + tz); } - void configure(const defaulttype::Vector3 &t, bool inverse) + + void configure(const type::Vector3 &t, bool inverse) { if (inverse) { @@ -204,11 +206,13 @@ struct Translation : public TransformOperation tx=(Real)t[0]; ty=(Real)t[1]; tz=(Real)t[2]; } } + private: Real tx,ty,tz; }; + //***************************************************************** //Functor to apply the operations wanted template @@ -219,7 +223,7 @@ struct Transform template Operation* add(Operation *op, bool inverse) { - // Operation *op=new Operation(); + // Operation *op=new Operation(); if (inverse) list.push_front(op); else @@ -227,79 +231,81 @@ struct Transform return op; } - std::list< Op* > &getOperations() {return list;} + std::list< Op* > &getOperations() { return list; } void operator()(typename DataTypes::Coord &v) const { - for (typename std::list< Op* >::const_iterator it=list.begin(); it != list.end() ; ++it) + for (typename std::list< Op* >::const_iterator it = list.begin(); it != list.end(); ++it) { (*it)->execute(v); } } + private: std::list< Op* > list; }; + template void TransformStochasticEngine::doUpdate() { -// ReadAccessor > inDilate= f_inputX; -// ReadAccessor > triangles = d_triangles; -// ReadAccessor > quads = d_quads; -// Real distance = d_optimParams.getValue()[0]; - -// cleanDirty(); - -// WriteOnlyAccessor > outDilate = f_outputX; - -// const int nbp = inDilate.size(); -// const int nbt = triangles.size(); -// const int nbq = quads.size(); - -// WriteOnlyAccessor > normals = d_normals; -// normals.resize(nbp); -// for (int i=0; i > inDilate= f_inputX; + // helper::ReadAccessor > triangles = d_triangles; + // helper::ReadAccessor > quads = d_quads; + // Real distance = d_optimParams.getValue()[0]; + + // cleanDirty(); + + // helper::WriteOnlyAccessor > outDilate = f_outputX; + + // const int nbp = inDilate.size(); + // const int nbt = triangles.size(); + // const int nbq = quads.size(); + + // helper::WriteOnlyAccessor > normals = d_normals; + // normals.resize(nbp); + // for (int i=0; i transformation; const bool inv = inverse.getValue(); - // if (r != defaulttype::Vector3(0,0,0)) + // if (r != defaulttype::Vector3(0,0,0)) transformation.add(new Rotation, inv)->configure(r, inv); - // if (t != defaulttype::Vector3(0,0,0)) + // if (t != defaulttype::Vector3(0,0,0)) transformation.add(new Translation, inv)->configure(t, inv); @@ -318,7 +324,7 @@ void TransformStochasticEngine::doUpdate() std::for_each(out.begin(), out.end(), transformation); //Deleting operations - std::list< TransformOperation* > operations=transformation.getOperations(); + std::list< TransformOperation* > operations = transformation.getOperations(); while (!operations.empty()) { delete operations.back(); @@ -326,9 +332,6 @@ void TransformStochasticEngine::doUpdate() } f_outputX.endEdit(); - - - } diff --git a/src/genericComponents/VTKExporterDA.cpp b/src/genericComponents/VTKExporterDA.cpp index 8f41247a..dc8e0138 100644 --- a/src/genericComponents/VTKExporterDA.cpp +++ b/src/genericComponents/VTKExporterDA.cpp @@ -35,12 +35,14 @@ namespace misc { + SOFA_DECL_CLASS(VTKExporterDA) int VTKExporterClassDA = core::RegisterObject("Save geometries in VTK, compatible with Optimus data assimilation") .add< VTKExporterDA >(); + void VTKExporterDA::handleEvent(sofa::core::objectmodel::Event *event) { if (sofa::component::stochastic::CorrectionEndEvent::checkEventType(event)) { diff --git a/src/genericComponents/VTKExporterDA.h b/src/genericComponents/VTKExporterDA.h index 853cae3e..9a65532c 100644 --- a/src/genericComponents/VTKExporterDA.h +++ b/src/genericComponents/VTKExporterDA.h @@ -47,10 +47,9 @@ class SOFA_OPTIMUSPLUGIN_API VTKExporterDA : public VTKExporter SOFA_CLASS(VTKExporterDA,VTKExporter); void handleEvent(sofa::core::objectmodel::Event *) override; - - }; + } // namespace misc } // namespace component diff --git a/src/initOptimusPlugin.cpp b/src/initOptimusPlugin.cpp index 446494e4..7b402531 100644 --- a/src/initOptimusPlugin.cpp +++ b/src/initOptimusPlugin.cpp @@ -21,14 +21,15 @@ ******************************************************************************/ #include "initOptimusPlugin.h" + + namespace sofa { namespace component { - //Here are just several convenient functions to help user to know what contains the plugin - + //Here are just several convenient functions to help user to know what contains the plugin extern "C" { SOFA_OPTIMUSPLUGIN_API void initExternalModule(); SOFA_OPTIMUSPLUGIN_API const char* getModuleName(); @@ -37,7 +38,8 @@ namespace component SOFA_OPTIMUSPLUGIN_API const char* getModuleDescription(); SOFA_OPTIMUSPLUGIN_API const char* getModuleComponentList(); } - + + void initExternalModule() { static bool first = true; @@ -47,16 +49,19 @@ namespace component } } + const char* getModuleName() { - return "Extended Kalman filter"; + return "Extended Kalman filter"; } + const char* getModuleVersion() { return "0.1"; } + const char* getModuleLicense() { return "LGPL"; @@ -68,16 +73,18 @@ namespace component return "Bayesian Filtering is a probabilistic technique for data fusion. The technique combines a concise mathematical formulation of a system with observations of that system. Probabilities are used to represent the state of a system, and likelihood functions to represent their relationships"; } + const char* getModuleComponentList() { - return "Filter_exception; "; + return "Filter_exception"; } +} // namespace component + +} // na,espace sofa -} -} #ifdef __APPLE__ int pthread_barrier_init(pthread_barrier_t *barrier, const pthread_barrierattr_t *attr, unsigned int count) @@ -137,17 +144,18 @@ SOFA_LINK_CLASS(OptimParams) //SOFA_LINK_CLASS(TestingParams) //SOFA_LINK_CLASS(BubblePackingForceField) -#ifdef SOFA_HAVE_VERDANDI -SOFA_LINK_CLASS(VerdandiAnimationLoop) -SOFA_LINK_CLASS(SofaModelWrapper) +//#ifdef SOFA_HAVE_VERDANDI +//SOFA_LINK_CLASS(VerdandiAnimationLoop) +//SOFA_LINK_CLASS(SofaModelWrapper) -SOFA_LINK_CLASS(SofaLinearObservationManager) -SOFA_LINK_CLASS(MappedPointsObservationManager) -SOFA_LINK_CLASS(ARObservationManager) +//SOFA_LINK_CLASS(SofaLinearObservationManager) +//SOFA_LINK_CLASS(MappedPointsObservationManager) +//SOFA_LINK_CLASS(ARObservationManager) -SOFA_LINK_CLASS(SimulatedStateObservationSource) +//SOFA_LINK_CLASS(SimulatedStateObservationSource) + +//SOFA_LINK_CLASS(SofaReducedOrderUKF) +//SOFA_LINK_CLASS(SofaUnscentedKalmanFilter) +//SOFA_LINK_CLASS(SofaForwardDriver) +//#endif -SOFA_LINK_CLASS(SofaReducedOrderUKF) -SOFA_LINK_CLASS(SofaUnscentedKalmanFilter) -SOFA_LINK_CLASS(SofaForwardDriver) -#endif diff --git a/src/initOptimusPlugin.h b/src/initOptimusPlugin.h index f6111e48..b1a38227 100644 --- a/src/initOptimusPlugin.h +++ b/src/initOptimusPlugin.h @@ -21,9 +21,12 @@ ******************************************************************************/ #pragma once +/** + * This plugin contains a set of Optimization Methods (initially mainly Bayesian Filtering techniques) + */ #include -#include /* pthread_t, pthread_barrier_t */ +//#include /* pthread_t, pthread_barrier_t */ #include #include @@ -36,7 +39,6 @@ typedef struct int count; int tripCount; } pthread_barrier_t; - #endif //#define SQR(ARG) ((ARG)*(ARG)) @@ -63,14 +65,6 @@ typedef struct //#define PRNS(ARG) if (this->verbose.getValue()) { std::cout << "[" << this->getName() << "]: " << ARG << std::endl; } //#define PRNSC(ARG) if (this->verbose.getValue()) { std::cout << "[" << this->getName() << "]: " << ARG; } -#define TIC this->startTime = double(this->timer->getTime()); -#define TOC(arg) this->stopTime = double(this->timer->getTime()); \ - std::cout << "[" << this->getName() << "] WTIME: " << arg << " " << this->stopTime - this->startTime << std::endl; - -#define TOCTIC(arg) this->stopTime = double(this->timer->getTime()); \ - std::cout << "[" << this->getName() << "] WTIME: " << arg << " " << this->stopTime - this->startTime << std::endl; \ - this->startTime = double(this->timer->getTime()); - #define SHOW_SIZE(name, A) std::cout << "Size of " << name << ": " << A.rows() << " x " << A.cols() << std::endl; #define asumSMat(NAME, MAT) { double sm = 0.0; \ @@ -99,10 +93,6 @@ for (size_t i = 0; i < MAT.rows(); i++) \ std::cout << "@@@ ABS. SUM OF " << NAME << ": " << sm << std::endl; \ } \ -/** \mainpage - This plugin contains a set of Optimization Methods (initially mainly Bayesian Filtering techniques) - */ - typedef enum FilterKind { UNDEF = -1, CLASSIC = 0, From 2f9286cc00a059fd0bc1fd6465e88ced12257404 Mon Sep 17 00:00:00 2001 From: SergeiNikolaev Date: Wed, 1 Dec 2021 15:50:27 +0100 Subject: [PATCH 2/9] UPDATE: correct headers and namespaces in stochastic filtering components for compatibility with SOFA v21.06 --- .../AdaptativeUKFilterClassic.cpp | 6 +- .../AdaptativeUKFilterClassic.h | 49 +- .../AdaptativeUKFilterClassic.inl | 336 ++--- .../BindedSimpleObservationManager.cpp | 6 +- .../BindedSimpleObservationManager.h | 41 +- .../BindedSimpleObservationManager.inl | 148 +- src/stochasticFiltering/EnTKFilter.cpp | 4 +- src/stochasticFiltering/EnTKFilter.h | 39 +- src/stochasticFiltering/EnTKFilter.inl | 98 +- .../FilteringAnimationLoop.cpp | 67 +- .../FilteringAnimationLoop.h | 42 +- .../ForceFieldSensitivity.h | 20 +- .../MappedStateObservationManager.cpp | 11 +- .../MappedStateObservationManager.h | 44 +- .../MappedStateObservationManager.inl | 58 +- .../MappedStateVelocityObservationManager.cpp | 6 +- .../MappedStateVelocityObservationManager.h | 56 +- .../MappedStateVelocityObservationManager.inl | 67 +- .../ObservationManagerBase.h | 65 +- .../PreStochasticWrapper.cpp | 75 +- .../PreStochasticWrapper.h | 18 +- src/stochasticFiltering/ROUKFilter.cpp | 6 +- src/stochasticFiltering/ROUKFilter.h | 66 +- src/stochasticFiltering/ROUKFilter.inl | 250 ++-- .../SimpleObservationManager.cpp | 5 +- .../SimpleObservationManager.h | 48 +- .../SimpleObservationManager.inl | 144 +- ...impleUncorrespondentObservationManager.cpp | 6 +- .../SimpleUncorrespondentObservationManager.h | 49 +- ...impleUncorrespondentObservationManager.inl | 54 +- .../StochasticFilterBase.h | 99 +- .../StochasticStateWrapper.cpp | 7 + .../StochasticStateWrapper.h | 106 +- .../StochasticStateWrapper.inl | 1229 +++++++++-------- .../StochasticStateWrapperBase.h | 132 +- src/stochasticFiltering/UKFilterClassic.cpp | 5 +- src/stochasticFiltering/UKFilterClassic.h | 38 +- src/stochasticFiltering/UKFilterClassic.inl | 239 ++-- .../UKFilterClassicWithSVD.cpp | 6 +- .../UKFilterClassicWithSVD.h | 52 +- .../UKFilterClassicWithSVD.inl | 293 ++-- src/stochasticFiltering/UKFilterSimCorr.cpp | 5 +- src/stochasticFiltering/UKFilterSimCorr.h | 70 +- src/stochasticFiltering/UKFilterSimCorr.inl | 105 +- src/test/GeoEmulator.cpp | 15 +- src/test/GeoEmulator.h | 23 +- src/test/GeoEmulator.inl | 40 +- src/test/GeoListener.cpp | 17 +- src/test/GeoListener.h | 22 +- src/test/GeoListener.inl | 49 +- 50 files changed, 2426 insertions(+), 2010 deletions(-) diff --git a/src/stochasticFiltering/AdaptativeUKFilterClassic.cpp b/src/stochasticFiltering/AdaptativeUKFilterClassic.cpp index 3170b032..7e2fb15b 100644 --- a/src/stochasticFiltering/AdaptativeUKFilterClassic.cpp +++ b/src/stochasticFiltering/AdaptativeUKFilterClassic.cpp @@ -24,6 +24,8 @@ #include "AdaptativeUKFilterClassic.inl" //#include + + namespace sofa { @@ -33,9 +35,9 @@ namespace component namespace stochastic { -using namespace defaulttype; +using namespace defaulttype; SOFA_DECL_CLASS(AdaptativeUKFilterClassic) @@ -50,6 +52,7 @@ int AdaptativeUKFilterClassicClass = core::RegisterObject("AdaptativeUKFilterCla #endif ; + #ifndef SOFA_FLOAT template class SOFA_STOCHASTIC_API AdaptativeUKFilterClassic; #endif @@ -58,6 +61,7 @@ template class SOFA_STOCHASTIC_API AdaptativeUKFilterClassic; #endif + } // namespace stochastic } // namespace component diff --git a/src/stochasticFiltering/AdaptativeUKFilterClassic.h b/src/stochasticFiltering/AdaptativeUKFilterClassic.h index 8a74666e..21daa6df 100644 --- a/src/stochasticFiltering/AdaptativeUKFilterClassic.h +++ b/src/stochasticFiltering/AdaptativeUKFilterClassic.h @@ -33,10 +33,6 @@ #include #include -#ifdef Success -#undef Success // dirty workaround to cope with the (dirtier) X11 define. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=253 -#endif -#include #include #include //#include @@ -46,13 +42,21 @@ #include #include +#include + + + namespace sofa { + namespace component { + namespace stochastic { + + extern "C"{ // product C= alphaA.B + betaC void dgemm_(char* TRANSA, char* TRANSB, const int* M, @@ -66,10 +70,9 @@ void dgemv_(char* TRANS, const int* M, const int* N, } -using namespace defaulttype; template -class AdaptativeUKFilterClassic: public sofa::component::stochastic::StochasticUnscentedFilterBase +class AdaptativeUKFilterClassic : public sofa::component::stochastic::StochasticUnscentedFilterBase { public: SOFA_CLASS(SOFA_TEMPLATE(AdaptativeUKFilterClassic, FilterType), StochasticUnscentedFilterBase); @@ -80,12 +83,10 @@ class AdaptativeUKFilterClassic: public sofa::component::stochastic::StochasticU typedef typename Eigen::Matrix EMatrixX; typedef typename Eigen::Matrix EVectorX; - AdaptativeUKFilterClassic(); - ~AdaptativeUKFilterClassic() {} protected: StochasticStateWrapperBaseT* masterStateWrapper; - helper::vector*> stateWrappers; + type::vector*> stateWrappers; ObservationManager* observationManager; //ObservationSource *observationSource; @@ -98,7 +99,7 @@ class AdaptativeUKFilterClassic: public sofa::component::stochastic::StochasticU size_t sigmaPointsNum; bool alphaConstant; std::vector m_sigmaPointObservationIndexes; - helper::vector d; + type::vector d; EVectorX vecAlpha, vecAlphaVar; EVectorX stateExp, predState, predObsExp; @@ -119,19 +120,20 @@ class AdaptativeUKFilterClassic: public sofa::component::stochastic::StochasticU /// structures for parallel computing: - helper::vector sigmaPoints2WrapperIDs; - helper::vector > wrapper2SigmaPointsIDs; + type::vector sigmaPoints2WrapperIDs; + type::vector > wrapper2SigmaPointsIDs; /// functions_initial void computeSimplexSigmaPoints(EMatrixX& sigmaMat); void computeStarSigmaPoints(EMatrixX& sigmaMat); void computeAdaptiveParameters( EVectorX& _mu, EVectorX& _epsilon, EVectorX& _stateExp, EVectorX& _predState); -public: - Data > d_state; - Data > d_variance; - Data > d_covariance; - Data > d_innovation; + +public: + Data > d_state; + Data > d_variance; + Data > d_covariance; + Data > d_innovation; Data< bool > d_draw; Data< double > d_radius_draw; Data< double > d_MOnodes_draw; @@ -141,14 +143,16 @@ class AdaptativeUKFilterClassic: public sofa::component::stochastic::StochasticU Data< double > d_paramB; Data< double > d_chiSquared; - - double m_omega; bool hasObs; + + AdaptativeUKFilterClassic(); + ~AdaptativeUKFilterClassic() {} + void init() override; void bwdInit() override; - /*virtual std::string getTemplateName() const override + /* virtual std::string getTemplateName() const override { return templateName(this); } @@ -160,7 +164,7 @@ class AdaptativeUKFilterClassic: public sofa::component::stochastic::StochasticU void stabilizeMatrix (EMatrixX& _initial, EMatrixX& _stabilized); void pseudoInverse (EMatrixX& M,EMatrixX& pinvM ); void writeValidationPlot (std::string filename ,EVectorX& state ); -void sqrtMat(EMatrixX& A, EMatrixX& sqrtA); + void sqrtMat(EMatrixX& A, EMatrixX& sqrtA); virtual void computePerturbedStates(); virtual void computePrediction() override; // Compute perturbed state included in computeprediction @@ -170,8 +174,9 @@ void sqrtMat(EMatrixX& A, EMatrixX& sqrtA); void draw(const core::visual::VisualParams* vparams) override; virtual void updateState() override { } +}; + -}; /// class } // stochastic diff --git a/src/stochasticFiltering/AdaptativeUKFilterClassic.inl b/src/stochasticFiltering/AdaptativeUKFilterClassic.inl index 2cf452ee..ce5bae7d 100644 --- a/src/stochasticFiltering/AdaptativeUKFilterClassic.inl +++ b/src/stochasticFiltering/AdaptativeUKFilterClassic.inl @@ -26,13 +26,18 @@ #include #include + + namespace sofa { + namespace component { + namespace stochastic { + template AdaptativeUKFilterClassic::AdaptativeUKFilterClassic() : Inherit() @@ -46,11 +51,9 @@ AdaptativeUKFilterClassic::AdaptativeUKFilterClassic() , d_paramA( initData(&d_paramA, "paramA", "nodes of the mechanical object") ) , d_paramB( initData(&d_paramB, "paramB", "nodes of the mechanical object") ) , d_chiSquared( initData(&d_chiSquared,"chiSquared", "nodes of the mechanical object") ) +{ } -{ - -} template void AdaptativeUKFilterClassic::computePerturbedStates() @@ -66,6 +69,8 @@ void AdaptativeUKFilterClassic::computePerturbedStates() } } + + template void AdaptativeUKFilterClassic::computePrediction() { @@ -78,7 +83,6 @@ void AdaptativeUKFilterClassic::computePrediction() sqrtMat(stateCovar, matPsqrt); - stateExp.fill(FilterType(0.0)); stateExp = masterStateWrapper->getState(); @@ -107,7 +111,7 @@ void AdaptativeUKFilterClassic::computePrediction() masterStateWrapper->setState(stateExp, mechParams); predState=stateExp; - }else{ + } else { // std::cout<<"\n HAS OBS: =" << hasObs << " COMPUTE ONLY PREDICTION" << std::endl; // stateExp.fill(FilterType(0.0)); @@ -118,7 +122,6 @@ void AdaptativeUKFilterClassic::computePrediction() sqrtMat(stateCovar, matPsqrt); - stateExp.fill(FilterType(0.0)); stateExp = masterStateWrapper->getState(); @@ -128,7 +131,7 @@ void AdaptativeUKFilterClassic::computePrediction() } /// Propagate sigma points - genMatXi=matXi.transpose(); + genMatXi = matXi.transpose(); computePerturbedStates(); /// Compute Predicted Mean @@ -138,24 +141,24 @@ void AdaptativeUKFilterClassic::computePrediction() } /// Compute Predicted Covariance - EMatrixX matXiTrans= matXi.transpose(); + EMatrixX matXiTrans = matXi.transpose(); EMatrixX centeredPxx = matXiTrans.rowwise() - matXiTrans.colwise().mean(); EMatrixX weightedCenteredPxx = centeredPxx.array().colwise() * vecAlphaVar.array(); EMatrixX covPxx = (centeredPxx.adjoint() * weightedCenteredPxx); stateCovar = covPxx + modelCovar; masterStateWrapper->setState(stateExp, mechParams); - } } + + template void AdaptativeUKFilterClassic::computeCorrection() { - if (hasObs) { - // std::cout<<"\n HAS OBS: =" << hasObs << " COMPUTE CORRECTION" << std::endl; + // std::cout<<"\n HAS OBS: =" << hasObs << " COMPUTE CORRECTION" << std::endl; PRNS("======= Computing correction, T= " << this->actualTime << " ======"); EVectorX zCol(observationSize); matZmodel.resize(observationSize, sigmaPointsNum); @@ -193,7 +196,7 @@ void AdaptativeUKFilterClassic::computeCorrection() /// Compute Kalman Gain EMatrixX matK(stateSize, observationSize); - matK =matPxz*pinvmatPz; + matK = matPxz * pinvmatPz; /// Compute Innovation EVectorX innovation(observationSize); @@ -201,48 +204,48 @@ void AdaptativeUKFilterClassic::computeCorrection() /// Compute Final State and Final Covariance stateExp = stateExp + matK * innovation; - stateCovar = stateCovar - matK*matPxz.transpose(); + stateCovar = stateCovar - matK * matPxz.transpose(); masterStateWrapper->setState(stateExp, mechParams); - /// ADAPTIVE ADJUSTMENT computeAdaptiveParameters(mu,epsilon,stateExp,predState); double delta; double lambda; - double D =(phi - d_paramA.getValue()*d_chiSquared.getValue())*(1.0/phi); - if (D> d_delta_0.getValue()) delta=D; - else delta=d_delta_0.getValue(); - std::cout<<"delta_0 "<< d_delta_0.getValue()<< " " << " D " << D << " " << " delta " << delta < d_delta_0.getValue()) { + delta = D; + } else { + delta=d_delta_0.getValue(); + } + std::cout<< "delta_0 " << d_delta_0.getValue() << " " << " D " << D << " " << " delta " << delta < d_lambda_0.getValue()) lambda=L; - else lambda=d_lambda_0.getValue(); - std::cout<<"lambda_0 "<< d_lambda_0.getValue()<< " " << " L " << L << " " << " lambda " << lambda < d_lambda_0.getValue()) { + lambda = L; + } else { + lambda = d_lambda_0.getValue(); + } + std::cout << "lambda_0 " << d_lambda_0.getValue() << " " << " L " << L << " " << " lambda " << lambda < d_chiSquared.getValue()){ + if (phi > d_chiSquared.getValue()) { /// Update Noise Covariance Matrices ZHENG 2018 - std::cout<<"Update Noise Covariance Matrices" << std::endl; - - modelCovar=(1-lambda)*modelCovar +lambda*(matK*mu*mu.transpose()*matK.transpose()); - obsCovar=(1-delta)*obsCovar +delta*(epsilon*epsilon.transpose()+covPzz); + std::cout << "Update Noise Covariance Matrices" << std::endl; + modelCovar = (1 - lambda) * modelCovar + lambda * (matK * mu * mu.transpose() * matK.transpose()); + obsCovar = (1 - delta) * obsCovar + delta * (epsilon * epsilon.transpose() + covPzz); /// Correct State Estimations matPz = obsCovar + covPzz; - matK =matPxz*pinvmatPz; + matK = matPxz * pinvmatPz; stateExp = stateExp + matK * innovation; - stateCovar = stateCovar - matK*matPxz.transpose(); + stateCovar = stateCovar - matK * matPxz.transpose(); masterStateWrapper->setState(stateExp, mechParams); - - } - - } /// Write Some File for Validation @@ -256,13 +259,13 @@ void AdaptativeUKFilterClassic::computeCorrection() } writeValidationPlot(d_filenameFinalState.getValue() ,stateExp); - } + + template void AdaptativeUKFilterClassic::computeAdaptiveParameters( EVectorX& _mu, EVectorX& _epsilon, EVectorX& _stateExp, EVectorX& _predState) { - EVectorX _realObs(observationSize); EVectorX _HpredState(observationSize); EVectorX _HstateExp(observationSize); @@ -273,18 +276,19 @@ void AdaptativeUKFilterClassic::computeAdaptiveParameters( EVectorX& _mu=_realObs - _HpredState; /// innovation _epsilon= _realObs - _HstateExp; - } + template -void AdaptativeUKFilterClassic::init() { +void AdaptativeUKFilterClassic::init() +{ Inherit::init(); assert(this->gnode); this->gnode->template get >(&stateWrappers, this->getTags(), sofa::core::objectmodel::BaseContext::SearchDown); PRNS("found " << stateWrappers.size() << " state wrappers"); - masterStateWrapper=NULL; + masterStateWrapper = NULL; size_t numSlaveWrappers = 0; size_t numMasterWrappers = 0; numThreads = 0; @@ -305,26 +309,25 @@ void AdaptativeUKFilterClassic::init() { return; } - /// Init for Adaptive Tuning Parameters - if (d_lambda_0.isSet() == 0){ + if (d_lambda_0.isSet() == 0){ serr << "No parameter *lambda_0* initialized for Adaptive Adjustment of Q" << sendl; return; } - if (d_delta_0.isSet() == 0 ){ + if (d_delta_0.isSet() == 0 ){ serr << "No parameter *delta_0* initialized for Adaptive Adjustment of R" << sendl; return; } - if (d_paramA.isSet() == 0 ){ + if (d_paramA.isSet() == 0 ){ serr << "No parameter *paramA* initialized for Adaptive Adjustment of Q" << sendl; return; } - if (d_paramB.isSet() == 0 ){ + if (d_paramB.isSet() == 0 ){ serr << "No *paramB* initialized for Adaptive Adjustment of R" << sendl; return; } - if (d_chiSquared.isSet() == 0 ){ + if (d_chiSquared.isSet() == 0 ){ serr << "No *chiSquared* initialized for Adaptive Adjustment of R" << sendl; return; } @@ -336,38 +339,38 @@ void AdaptativeUKFilterClassic::init() { PRNS("number of slave wrappers: " << numThreads-1); /// slaves + master } - numThreads=numSlaveWrappers+numMasterWrappers; + numThreads = numSlaveWrappers + numMasterWrappers; this->gnode->get(observationManager, core::objectmodel::BaseContext::SearchDown); if (observationManager) { PRNS("found observation manager: " << observationManager->getName()); - } else + } else { PRNE("no observation manager found!"); - + } /// Init for Write Function - exportPrefix = d_exportPrefix.getFullPath(); + exportPrefix = d_exportPrefix.getFullPath(); this->saveParam = false; if (!d_filenameFinalState.getValue().empty()) { std::ofstream paramFileFinalState(d_filenameFinalState.getValue().c_str()); - if (paramFileFinalState .is_open()) { + if (paramFileFinalState.is_open()) { this->saveParam = true; paramFileFinalState.close(); } } - m_omega= ((double) rand() / (RAND_MAX)); + m_omega = ((double) rand() / (RAND_MAX)); +} -} template -void AdaptativeUKFilterClassic::bwdInit() { +void AdaptativeUKFilterClassic::bwdInit() +{ assert(masterStateWrapper); stateSize = masterStateWrapper->getStateSize(); - std::cout<< "[UKF] stateSize " << stateSize << std::endl; - + std::cout << "[UKF] stateSize " << stateSize << std::endl; /// Initialize Observation's data if (!initialiseObservationsAtFirstStep.getValue()) { @@ -379,7 +382,6 @@ void AdaptativeUKFilterClassic::bwdInit() { obsCovar = observationManager->getErrorVariance(); } - /// Initialize Model's Error Covariance stateCovar = masterStateWrapper->getStateErrorVariance(); modelCovar = masterStateWrapper->getModelErrorVariance(); @@ -397,13 +399,14 @@ void AdaptativeUKFilterClassic::bwdInit() { sigmaPointsNum = matI.rows(); matXi.resize(stateSize, sigmaPointsNum); - genMatXi.resize( sigmaPointsNum,stateSize); + genMatXi.resize( sigmaPointsNum, stateSize); /// Init State Observation Mapping for Sigma Points m_sigmaPointObservationIndexes.resize(sigmaPointsNum); - } + + template void AdaptativeUKFilterClassic::initializeStep(const core::ExecParams* _params, const size_t _step) { Inherit::initializeStep(_params, _step); @@ -424,51 +427,66 @@ void AdaptativeUKFilterClassic::initializeStep(const core::ExecParam observationManager->initializeStep(stepNumber); } + + template -void AdaptativeUKFilterClassic::stabilizeMatrix (EMatrixX& _initial, EMatrixX& _stabilized) { +void AdaptativeUKFilterClassic::stabilizeMatrix(EMatrixX& _initial, EMatrixX& _stabilized) +{ Eigen::JacobiSVD svd(_initial, Eigen::ComputeThinU | Eigen::ComputeThinV); const Eigen::JacobiSVD::SingularValuesType singVals = svd.singularValues(); Eigen::JacobiSVD::SingularValuesType singValsStab = singVals; - for (int i=0; i < singVals.rows(); i++ ){ - if ((singValsStab(i)*singValsStab(i))*(1.0/(singVals(0)*singVals(0)))< 1.0e-6) singValsStab(i)=0; + for (int i = 0; i < singVals.rows(); i++) { + if ((singValsStab(i) * singValsStab(i)) * (1.0 / (singVals(0) * singVals(0))) < 1.0e-6) + singValsStab(i) = 0; } - _stabilized= svd.matrixU()*singValsStab*svd.matrixV().transpose(); - + _stabilized = svd.matrixU() * singValsStab * svd.matrixV().transpose(); } + + template -void AdaptativeUKFilterClassic::pseudoInverse( EMatrixX& M,EMatrixX& pinvM) { - double epsilon= 1e-15; +void AdaptativeUKFilterClassic::pseudoInverse( EMatrixX& M, EMatrixX& pinvM) +{ + double epsilon = 1e-15; Eigen::JacobiSVD svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV); const Eigen::JacobiSVD::SingularValuesType singVals = svd.singularValues(); Eigen::JacobiSVD::SingularValuesType invSingVals = singVals; - for(int i=0; i -void AdaptativeUKFilterClassic::sqrtMat(EMatrixX& A, EMatrixX& sqrtA){ - double epsilon= 1e-15; +void AdaptativeUKFilterClassic::sqrtMat(EMatrixX& A, EMatrixX& sqrtA) +{ + double epsilon = 1e-15; Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV); const Eigen::JacobiSVD::SingularValuesType singVals = svd.singularValues(); Eigen::JacobiSVD::SingularValuesType sqrtSingVals = singVals; - for(int i=0; i -void AdaptativeUKFilterClassic::writeValidationPlot (std::string filename ,EVectorX& state ){ +void AdaptativeUKFilterClassic::writeValidationPlot(std::string filename, EVectorX& state) +{ if (this->saveParam) { std::ofstream paramFile(filename.c_str(), std::ios::app); if (paramFile.is_open()) { @@ -479,35 +497,40 @@ void AdaptativeUKFilterClassic::writeValidationPlot (std::string fil } } + + template -void AdaptativeUKFilterClassic::computeSimplexSigmaPoints(EMatrixX& sigmaMat) { +void AdaptativeUKFilterClassic::computeSimplexSigmaPoints(EMatrixX& sigmaMat) +{ size_t p = stateSize; size_t r = stateSize + 1; EMatrixX workingMatrix = EMatrixX::Zero(p, r); Type scal, beta, sqrt_p; - beta = Type(p) / Type(p+1); + beta = Type(p) / Type(p + 1); sqrt_p = sqrt(Type(p)); - scal = Type(1.0)/sqrt(Type(2) * beta); - workingMatrix(0,0) = scal; - workingMatrix(0,1) = -scal; + scal = Type(1.0) / sqrt(Type(2) * beta); + workingMatrix(0, 0) = scal; + workingMatrix(0, 1) = -scal; for (size_t i = 1; i < p; i++) { scal = Type(1.0) / sqrt(beta * Type(i+1) * Type(i+2)); - for (size_t j = 0; j < i+1; j++) - workingMatrix(i,j) = -scal; - workingMatrix(i,i+1) = Type(i+1) * scal; + for (size_t j = 0; j < i + 1; j++) { + workingMatrix(i, j) = -scal; + } + workingMatrix(i, i+1) = Type(i+1) * scal; } - sigmaMat.resize(r,p); - for (size_t i = 0; i < r; i++) + sigmaMat.resize(r, p); + for (size_t i = 0; i < r; i++) { sigmaMat.row(i) = workingMatrix.col(i) * sqrt_p; + } vecAlpha.resize(r); - vecAlpha.fill(Type(1.0)/Type(r)); + vecAlpha.fill(Type(1.0) / Type(r)); alphaConstant = true; alpha = vecAlpha(0); @@ -516,8 +539,11 @@ void AdaptativeUKFilterClassic::computeSimplexSigmaPoints(EMatrixX& vecAlphaVar.fill(alphaVar); } + + template -void AdaptativeUKFilterClassic::computeStarSigmaPoints(EMatrixX& sigmaMat) { +void AdaptativeUKFilterClassic::computeStarSigmaPoints(EMatrixX& sigmaMat) +{ size_t p = stateSize; size_t r = 2 * stateSize + 1; @@ -529,18 +555,18 @@ void AdaptativeUKFilterClassic::computeStarSigmaPoints(EMatrixX& sig sqrt_vec = sqrt(p + lambda); for (size_t j = 0; j < p; j++) { - workingMatrix(j,j) = sqrt_vec; + workingMatrix(j, j) = sqrt_vec; } for (size_t j = p; j < 2 * p; j++) { - workingMatrix(2*p-j-1,j) = -sqrt_vec; + workingMatrix(2 * p - j - 1, j) = -sqrt_vec; } - sigmaMat.resize(r,p); + sigmaMat.resize(r, p); for (size_t i = 0; i < r; i++) sigmaMat.row(i) = workingMatrix.col(i); vecAlpha.resize(r); - vecAlpha.fill(Type(1.0)/Type(2 * (p + lambda))); + vecAlpha.fill(Type(1.0) / Type(2 * (p + lambda))); vecAlpha(2 * p) = Type(lambda) / Type(p + lambda); alphaConstant = false; alpha = vecAlpha(0); @@ -551,56 +577,58 @@ void AdaptativeUKFilterClassic::computeStarSigmaPoints(EMatrixX& sig vecAlphaVar(2 * p) = Type(lambda) / Type(p + lambda); } + + template -void AdaptativeUKFilterClassic::draw(const core::visual::VisualParams* vparams ) { - if(d_draw.getValue()){ +void AdaptativeUKFilterClassic::draw(const core::visual::VisualParams* vparams) +{ + if( d_draw.getValue() ) { if (vparams->displayFlags().getShowVisualModels()) { - std::vector> predpoints; + std::vector> predpoints; predpoints.resize( sigmaPointsNum ); - for( std::vector>::iterator it = predpoints.begin(); it != predpoints.end(); ++it) - { + for (std::vector>::iterator it = predpoints.begin(); it != predpoints.end(); ++it) { it->resize( d_MOnodes_draw.getValue() ); } - for(unsigned i=0; i < sigmaPointsNum; i++){ + for(unsigned i = 0; i < sigmaPointsNum; i++) { EVectorX coll = genMatXi.row(i); - for (unsigned j=0; j < d_MOnodes_draw.getValue(); j++){ - for (unsigned k=0; k < 3; k++){ - if (masterStateWrapper->estimOnlyXYZ()) - predpoints[i][j][k]=coll(3*j+k); - else - predpoints[i][j][k]=coll(6*j+k); + for (unsigned j = 0; j < d_MOnodes_draw.getValue(); j++){ + for (unsigned k = 0; k < 3; k++){ + if (masterStateWrapper->estimOnlyXYZ()) { + predpoints[i][j][k] = coll(3*j+k); + } else { + predpoints[i][j][k] = coll(6*j+k); + } } } - helper::types::RGBAColor color; + sofa::type::RGBAColor color; switch (i) { - case 0: color = helper::types::RGBAColor(1.0,0.0,0.0,1.0); break; - case 1: color = helper::types::RGBAColor(0.0,1.0,0.0,1.0); break; - case 2: color = helper::types::RGBAColor(0.0,0.0,1.0,1.0); break; - default: color = helper::types::RGBAColor(0.5, 0.5, 0.5, 0.5); + case 0: color = sofa::type::RGBAColor(1.0,0.0,0.0,1.0); break; + case 1: color = sofa::type::RGBAColor(0.0,1.0,0.0,1.0); break; + case 2: color = sofa::type::RGBAColor(0.0,0.0,1.0,1.0); break; + default: color = sofa::type::RGBAColor(0.5, 0.5, 0.5, 0.5); } - helper::vector colorB; + type::vector colorB; colorB.resize(this->stateSize); - for(size_t i =0; i < colorB.size(); i++){ - - colorB[i]= ((double) rand() / (RAND_MAX)) ; + for(size_t i = 0; i < colorB.size(); i++) { + colorB[i] = ((double) rand() / (RAND_MAX)); } - vparams->drawTool()->drawSpheres(predpoints[i], d_radius_draw.getValue(), helper::types::RGBAColor(m_omega,0.0f,0.0f,1.0f)); } - // if (d_MOnodes_draw.getValue()>=2) - // vparams->drawTool()->drawLineStrip(predpoints[i],3.0,sofa::defaulttype::Vec<4, float>(color[i],0.5f,colorB[i],1.0f)); } - + vparams->drawTool()->drawSpheres(predpoints[i], d_radius_draw.getValue(), sofa::type::RGBAColor(m_omega,0.0f,0.0f,1.0f)); + } + // if (d_MOnodes_draw.getValue()>=2) + // vparams->drawTool()->drawLineStrip(predpoints[i],3.0,sofa::defaulttype::Vec<4, float>(color[i],0.5f,colorB[i],1.0f)); + // } } } +} -} - } // stochastic } // component @@ -608,37 +636,37 @@ void AdaptativeUKFilterClassic::draw(const core::visual::VisualParam } // sofa -/*matPxzB.fill(FilterType(0.0)); -matPzB.fill(FilterType(0.0)); - -EVectorX vx(stateSize), z(observationSize), vz(observationSize); -vx.fill(FilterType(0.0)); -vz.fill(FilterType(0.0)); -z.fill(FilterType(0.0)); -for (size_t i = 0; i < sigmaPointsNum; i++) { - vx = matXi.col(i) - stateExp; - z = matZmodel.col(i); - vz = z - predObsExp; - for (size_t x = 0; x < stateSize; x++) - for (size_t y = 0; y < observationSize; y++) - matPxzB(x,y) += vx(x)*vz(y); - - for (size_t x = 0; x < observationSize; x++) - for (size_t y = 0; y < observationSize; y++) - matPzB(x,y) += vz(x)*vz(y); -} -matPxzB = alphaVar * matPxzB; -matPzB = alphaVar * matPzB + obsCovar;*/ -/* - stateCovar.fill(FilterType(0.0)); - EVectorX tmpX(stateSize); - tmpX.fill(FilterType(0.0)); - for (size_t i = 0; i < sigmaPointsNum; i++) { - tmpX = matXi.col(i) - stateExp; - for (size_t x = 0; x < stateSize; x++) - for (size_t y = 0; y < stateSize; y++) - stateCovar(x,y) += tmpX(x)*tmpX(y); - } - stateCovar=alphaVar*stateCovar; -}*/ +// matPxzB.fill(FilterType(0.0)); +// matPzB.fill(FilterType(0.0)); +// +// EVectorX vx(stateSize), z(observationSize), vz(observationSize); +// vx.fill(FilterType(0.0)); +// vz.fill(FilterType(0.0)); +// z.fill(FilterType(0.0)); +// for (size_t i = 0; i < sigmaPointsNum; i++) { +// vx = matXi.col(i) - stateExp; +// z = matZmodel.col(i); +// vz = z - predObsExp; +// for (size_t x = 0; x < stateSize; x++) +// for (size_t y = 0; y < observationSize; y++) +// matPxzB(x, y) += vx(x) * vz(y); +// +// for (size_t x = 0; x < observationSize; x++) +// for (size_t y = 0; y < observationSize; y++) +// matPzB(x, y) += vz(x) * vz(y); +// } +// matPxzB = alphaVar * matPxzB; +// matPzB = alphaVar * matPzB + obsCovar;*/ + +// stateCovar.fill(FilterType(0.0)); +// EVectorX tmpX(stateSize); +// tmpX.fill(FilterType(0.0)); +// for (size_t i = 0; i < sigmaPointsNum; i++) { +// tmpX = matXi.col(i) - stateExp; +// for (size_t x = 0; x < stateSize; x++) +// for (size_t y = 0; y < stateSize; y++) +// stateCovar(x,y) += tmpX(x)*tmpX(y); +// } +// stateCovar = alphaVar * stateCovar; +//} diff --git a/src/stochasticFiltering/BindedSimpleObservationManager.cpp b/src/stochasticFiltering/BindedSimpleObservationManager.cpp index cb789c68..f3b29d71 100644 --- a/src/stochasticFiltering/BindedSimpleObservationManager.cpp +++ b/src/stochasticFiltering/BindedSimpleObservationManager.cpp @@ -26,6 +26,8 @@ #include "BindedSimpleObservationManager.inl" //#include + + namespace sofa { @@ -35,9 +37,10 @@ namespace component namespace stochastic { -using namespace defaulttype; +using namespace defaulttype; + SOFA_DECL_CLASS(BindedSimpleObservationManager) // Register in the Factory @@ -54,6 +57,7 @@ template class SOFA_STOCHASTIC_API BindedSimpleObservationManager; + } // namespace stochastic } // namespace component diff --git a/src/stochasticFiltering/BindedSimpleObservationManager.h b/src/stochasticFiltering/BindedSimpleObservationManager.h index f1e3282a..d2aff968 100644 --- a/src/stochasticFiltering/BindedSimpleObservationManager.h +++ b/src/stochasticFiltering/BindedSimpleObservationManager.h @@ -36,14 +36,18 @@ #include "../genericComponents/SimulatedStateObservationSource.h" #include "StochasticStateWrapper.h" + + namespace sofa { + namespace component { + namespace stochastic { -using namespace defaulttype; + template class BindedSimpleObservationManager: public sofa::component::stochastic::ObservationManager @@ -65,33 +69,33 @@ class BindedSimpleObservationManager: public sofa::component::stochastic::Observ typedef typename DataTypes2::VecCoord VecCoord; typedef typename DataTypes2::VecDeriv VecDeriv; - Data d_use2dObservations; - Data d_projectionMatrix; - Data d_proj_dist; + Data< bool > d_use2dObservations; + Data< sofa::type::Mat3x4d > d_projectionMatrix; + Data< double > d_proj_dist; - Data> d_bindId; + Data< type::vector > d_bindId; SingleLink, StateWrapper, BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> stateWrapperLink; typedef core::behavior::MechanicalState MappState; - Data d_mappedStatePath; + Data < std::string > d_mappedStatePath; - - BindedSimpleObservationManager(); - ~BindedSimpleObservationManager() {} - protected: - MasterState* masterState; - ObservationSource *observationSource; + ObservationSource* observationSource; StateWrapper* stateWrapper; double actualObservationTime; - helper::vector bindId; + type::vector bindId; MappState* mappedState; - public: + +public: + BindedSimpleObservationManager(); + ~BindedSimpleObservationManager() {} + + void init() override; void bwdInit() override; @@ -102,9 +106,7 @@ class BindedSimpleObservationManager: public sofa::component::stochastic::Observ virtual bool obsFunction(EVectorX& /* _state */, EVectorX& /* _predictedObservation */) override; typename DataTypes1::VecCoord realObservations; - typename helper::vector< VecCoord > modelObservations; - - + typename type::vector< VecCoord > modelObservations; /// Pre-construction check method called by ObjectFactory. @@ -112,7 +114,7 @@ class BindedSimpleObservationManager: public sofa::component::stochastic::Observ template static bool canCreate(T*& obj, core::objectmodel::BaseContext* context, core::objectmodel::BaseObjectDescription* arg) { - // if (dynamic_cast(context->getMechanicalState()) == NULL) return false; + // if (dynamic_cast(context->getMechanicalState()) == NULL) return false; return sofa::core::objectmodel::BaseObject::canCreate(obj, context, arg); } @@ -125,8 +127,7 @@ class BindedSimpleObservationManager: public sofa::component::stochastic::Observ { return DataTypes1::Name()+ std::string(",") + DataTypes2::Name(); } - -}; /// class +}; } // stochastic diff --git a/src/stochasticFiltering/BindedSimpleObservationManager.inl b/src/stochasticFiltering/BindedSimpleObservationManager.inl index c12a7af5..c9ce68f5 100644 --- a/src/stochasticFiltering/BindedSimpleObservationManager.inl +++ b/src/stochasticFiltering/BindedSimpleObservationManager.inl @@ -22,10 +22,10 @@ #pragma once #include - #include "BindedSimpleObservationManager.h" + namespace sofa { @@ -35,27 +35,27 @@ namespace component namespace stochastic { + template -BindedSimpleObservationManager::BindedSimpleObservationManager() +BindedSimpleObservationManager::BindedSimpleObservationManager() : Inherit() , d_use2dObservations(initData(&d_use2dObservations, false, "use2dObservation", "Set to True if using 2D observations")) - , d_projectionMatrix( initData(&d_projectionMatrix, Mat3x4d(defaulttype::Vec<4,float>(1.0,0.0,0.0,0.0), - defaulttype::Vec<4,float>(0.0,1.0,0.0,0.0), - defaulttype::Vec<4,float>(0.0,0.0,1.0,0.0)), "projectionMatrix","Projection matrix")) - , d_proj_dist(initData(&d_proj_dist, (double) 0.0, "projDist", "Projection Distance")) + , d_projectionMatrix( initData(&d_projectionMatrix, sofa::type::Mat3x4d(type::Vec<4,float>(1.0,0.0,0.0,0.0), + type::Vec<4,float>(0.0,1.0,0.0,0.0), + type::Vec<4,float>(0.0,0.0,1.0,0.0)), "projectionMatrix","Projection matrix")) + , d_proj_dist(initData(&d_proj_dist, (double)0.0, "projDist", "Projection Distance")) , d_bindId(initData(&d_bindId, "bindId", "Vector of 2D 3D correspondences")) , stateWrapperLink(initLink("stateWrapper", "link to the state wrapper needed to perform apply (perhaps to be changed)")) , d_mappedStatePath(initData(&d_mappedStatePath, "mappedState", "Link to Virtual Mapped Catheter ")) +{ } + -{ -} template -void BindedSimpleObservationManager::init() +void BindedSimpleObservationManager::init() { - Inherit::init(); - bindId=d_bindId.getValue(); + bindId = d_bindId.getValue(); this->gnode->get(observationSource); if (observationSource) { PRNS("Found observation source: " << observationSource->getName()); @@ -66,12 +66,12 @@ void BindedSimpleObservationManager::init() stateWrapper = stateWrapperLink.get(); if (stateWrapper) { - std::cout<< "[BindedSimpleObservationManager] Link to state wrapper: " << stateWrapper->getName()<< std::endl; + std::cout << "[BindedSimpleObservationManager] Link to state wrapper: " << stateWrapper->getName() << std::endl; } else { - std::cout<< "[BindedSimpleObservationManager] Link to state wrapper not initialized!"<< std::endl; - + std::cout << "[BindedSimpleObservationManager] Link to state wrapper not initialized!" << std::endl; } - if(stateWrapper->declaredMapState()==0){ + + if(stateWrapper->declaredMapState() == 0){ return; serr<<"No mapped state declared in the StochasticStateWrapper "<::init() this->gnode->get(masterState); if (masterState != NULL) { - std::cout <<"Found master mechanical state: " << masterState->getName()<< std::endl; - } - else { - std::cout <<"No master mechanical state found!"<< std::endl; + std::cout << "Found master mechanical state: " << masterState->getName() << std::endl; + } else { + std::cout << "No master mechanical state found!" << std::endl; } - this->gnode->get(mappedState, d_mappedStatePath.getValue()); if ( mappedState != NULL) { - std::cout<<"[BindedSimpleObservationManager] Found mapped mechanical state: " << mappedState->getName() << " size: "<< mappedState->getSize() <getName() << " size: " << mappedState->getSize() << std::endl; + } else { + std::cout << "[BindedSimpleObservationManager] No mapped state state found" << std::endl; return; - } +} -} - template -void BindedSimpleObservationManager::bwdInit() +void BindedSimpleObservationManager::bwdInit() { this->observationSize = observationSource->getStateSize() * DataTypes1::spatial_dimensions; Inherit::bwdInit(); +} -} template -bool BindedSimpleObservationManager::hasObservation(double _time) { +bool BindedSimpleObservationManager::hasObservation(double _time) +{ bool hasObservation; - if(this->actualTime==0){ - hasObservation=true; - } else{ - - hasObservation= observationSource->getObservation(this->actualTime, realObservations); + if (this->actualTime == 0) { + hasObservation = true; + } else { + hasObservation = observationSource->getObservation(this->actualTime, realObservations); } + if (!hasObservation) { PRNE("No observation for time " << _time); - return(false); + return (false); } - return(true); + return (true); } + + template -bool BindedSimpleObservationManager::getInnovation(double _time, EVectorX& _state, EVectorX& _innovation) +bool BindedSimpleObservationManager::getInnovation(double _time, EVectorX& _state, EVectorX& _innovation) { if (_time != this->actualTime) { PRNE("Observation for time " << this->actualTime << " not prepared, call hasObservation first!"); @@ -137,7 +135,7 @@ bool BindedSimpleObservationManager::getInnova if (_innovation.rows() != long(this->observationSize)) { PRNE("Wrong innovation size: " << _innovation.rows() << " should be " << this->observationSize); - return(false); + return (false); } if ((stateWrapper->getFilterKind() == SIMCORR) || (stateWrapper->getFilterKind() == CLASSIC) || (stateWrapper->getFilterKind() == LOCENSEMBLE)) { @@ -145,35 +143,38 @@ bool BindedSimpleObservationManager::getInnova _innovation(i) = realObservations[0](i) - _state(i); } - return true; +} + -} template -bool BindedSimpleObservationManager::obsFunction(EVectorX& /* _state */, EVectorX& /* _predictedObservation */) +bool BindedSimpleObservationManager::obsFunction(EVectorX& /* _state */, EVectorX& /* _predictedObservation */) { return 0; } + + template -bool BindedSimpleObservationManager::getRealObservation(double /* _time */, EVectorX& /* _realObs */) +bool BindedSimpleObservationManager::getRealObservation(double /* _time */, EVectorX& /* _realObs */) { return 0; } + + template -bool BindedSimpleObservationManager::getPredictedObservation(int _id, EVectorX& _predictedObservation) +bool BindedSimpleObservationManager::getPredictedObservation(int _id, EVectorX& _predictedObservation) { - - const Mat3x4d & P = d_projectionMatrix.getValue(); + const sofa::type::Mat3x4d & P = d_projectionMatrix.getValue(); _predictedObservation.resize(this->observationSize); Data allPred2DObs; - Data mappPred3DState; + Data mappPred3DState; - typename Vec3dTypes::VecCoord& mappPred3DStateEdit = *mappPred3DState.beginEdit(); + typename defaulttype::Vec3Types::VecCoord& mappPred3DStateEdit = *mappPred3DState.beginEdit(); mappPred3DStateEdit.resize(mappedState->getSize()); stateWrapper->getActualMappedPosition(_id, mappPred3DStateEdit); @@ -185,48 +186,47 @@ bool BindedSimpleObservationManager::getPredic double rx = P[0][0] * mappPred3DStateEdit[i][0] + P[0][1] * mappPred3DStateEdit[i][1] + P[0][2] * mappPred3DStateEdit[i][2] + P[0][3]; double ry = P[1][0] * mappPred3DStateEdit[i][0] + P[1][1] * mappPred3DStateEdit[i][1] + P[1][2] * mappPred3DStateEdit[i][2] + P[1][3]; double rz = P[2][0] * mappPred3DStateEdit[i][0] + P[2][1] * mappPred3DStateEdit[i][1] + P[2][2] * mappPred3DStateEdit[i][2] + P[2][3]; - allPred2DObsEdit[i][0]=rx* (1.0/rz); - allPred2DObsEdit[i][1]=ry* (1.0/rz); + allPred2DObsEdit[i][0] = rx * (1.0 / rz); + allPred2DObsEdit[i][1] = ry * (1.0 / rz); } -// std::cout << "_predictedObservation " << allPred2DObsEdit << std::endl; -// double dist; -// Vector2 trans; -// for (unsigned t=0;t < real2DObsEdit.size();t++) { -// Vector2 real(real2DObsEdit[t][0],real2DObsEdit[t][1]); + // std::cout << "_predictedObservation " << allPred2DObsEdit << std::endl; -// Vector2 proj0 (allPred2DObsEdit[0][0],allPred2DObsEdit[0][1]); -// Vector2 real0(real2DObsEdit[0][0],real2DObsEdit[0][1]); -// trans=(real0-proj0); + // double dist; + // Vector2 trans; + // for (unsigned t=0;t < real2DObsEdit.size();t++) { + // Vector2 real(real2DObsEdit[t][0],real2DObsEdit[t][1]); + // Vector2 proj0 (allPred2DObsEdit[0][0],allPred2DObsEdit[0][1]); + // Vector2 real0(real2DObsEdit[0][0],real2DObsEdit[0][1]); + // trans=(real0-proj0); -// int bind = -1; -// double minDist = 0; + // int bind = -1; + // double minDist = 0; -// for (unsigned i=0;i < allPred2DObsEdit.size();i++) { -// Vector2 proj ((allPred2DObsEdit[i][0]+trans[0]),(allPred2DObsEdit[i][1]+trans[1])); -// dist=(real-proj).norm(); + // for (unsigned i=0;i < allPred2DObsEdit.size();i++) { + // Vector2 proj ((allPred2DObsEdit[i][0]+trans[0]),(allPred2DObsEdit[i][1]+trans[1])); + // dist=(real-proj).norm(); -// if (dist< d_proj_dist.getValue() && ((bind == -1) || (dist< minDist))) { -// minDist = dist; -// bind = i; -// } -// } -// bindId.push_back(bind); -// } + // if (dist< d_proj_dist.getValue() && ((bind == -1) || (dist< minDist))) { + // minDist = dist; + // bind = i; + // } + // } + // bindId.push_back(bind); + // } - for (size_t i = 0; i <(this->observationSize)*0.5; i++){ + for (size_t i = 0; i < (this->observationSize) * 0.5; i++){ for (size_t d = 0; d < 2; d++){ - _predictedObservation(2*i+d) = allPred2DObsEdit[bindId[i]][d]; + _predictedObservation(2 * i + d) = allPred2DObsEdit[bindId[i]][d]; } } - return true; - } + } // stochastic } // component diff --git a/src/stochasticFiltering/EnTKFilter.cpp b/src/stochasticFiltering/EnTKFilter.cpp index 45f898aa..0d5ad5a1 100644 --- a/src/stochasticFiltering/EnTKFilter.cpp +++ b/src/stochasticFiltering/EnTKFilter.cpp @@ -25,6 +25,7 @@ //#include + namespace sofa { @@ -34,9 +35,9 @@ namespace component namespace stochastic { -using namespace defaulttype; +using namespace defaulttype; SOFA_DECL_CLASS(EnTKFilter) @@ -59,6 +60,7 @@ template class SOFA_STOCHASTIC_API EnTKFilter; #endif + } // namespace stochastic } // namespace component diff --git a/src/stochasticFiltering/EnTKFilter.h b/src/stochasticFiltering/EnTKFilter.h index 8cb25a20..40a17cbc 100644 --- a/src/stochasticFiltering/EnTKFilter.h +++ b/src/stochasticFiltering/EnTKFilter.h @@ -34,10 +34,6 @@ #include #include -#ifdef Success -#undef Success // dirty workaround to cope with the (dirtier) X11 define. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=253 -#endif -#include #include #include //#include @@ -47,6 +43,9 @@ #include #include +#include + + namespace sofa { @@ -58,8 +57,9 @@ namespace stochastic { -extern "C"{ - // product C= alphaA.B + betaC + +extern "C" { + // product C = alphaA.B + betaC void dgemm_(char* TRANSA, char* TRANSB, const int* M, const int* N, const int* K, double* alpha, double* A, const int* LDA, double* B, const int* LDB, double* beta, @@ -71,7 +71,6 @@ extern "C"{ } -using namespace defaulttype; template class EnTKFilter : public sofa::component::stochastic::StochasticFilterBase @@ -85,12 +84,10 @@ class EnTKFilter : public sofa::component::stochastic::StochasticFilterBase typedef typename Eigen::Matrix EMatrixX; typedef typename Eigen::Matrix EVectorX; -EnTKFilter(); -~EnTKFilter() {} protected: StochasticStateWrapperBaseT* masterStateWrapper; - helper::vector*> stateWrappers; + type::vector*> stateWrappers; ObservationManager* observationManager; //ObservationSource *observationSource; @@ -123,17 +120,20 @@ EnTKFilter(); bool saveParam; /// structures for parallel computing: - helper::vector sigmaPoints2WrapperIDs; - helper::vector > wrapper2SigmaPointsIDs; + type::vector sigmaPoints2WrapperIDs; + type::vector > wrapper2SigmaPointsIDs; public: - Data d_ensembleMembersNumber; - Data d_additiveNoiseType; + Data< size_t > d_ensembleMembersNumber; + Data< size_t > d_additiveNoiseType; Data< std::string > d_inverseOptionType; - Data > d_state; - Data > d_variance; - Data > d_covariance; - Data > d_innovation; + Data< type::vector > d_state; + Data< type::vector > d_variance; + Data< type::vector > d_covariance; + Data< type::vector > d_innovation; + + EnTKFilter(); + ~EnTKFilter() {} void init() override; void bwdInit() override; @@ -160,8 +160,7 @@ EnTKFilter(); virtual void initializeStep(const core::ExecParams* _params, const size_t _step) override; virtual void updateState() override; - -}; /// class +}; diff --git a/src/stochasticFiltering/EnTKFilter.inl b/src/stochasticFiltering/EnTKFilter.inl index c8bb33fa..7df53011 100644 --- a/src/stochasticFiltering/EnTKFilter.inl +++ b/src/stochasticFiltering/EnTKFilter.inl @@ -27,6 +27,7 @@ #include + namespace sofa { @@ -37,6 +38,7 @@ namespace stochastic { + template EnTKFilter::EnTKFilter() : Inherit() @@ -47,9 +49,7 @@ EnTKFilter::EnTKFilter() , d_variance( initData(&d_variance, "variance", "actual variance of reduced state (parameters) estimated by the filter" ) ) , d_covariance( initData(&d_covariance, "covariance", "actual co-variance of reduced state (parameters) estimated by the filter" ) ) , d_innovation( initData(&d_innovation, "innovation", "innovation value computed by the filter" ) ) -{ - -} +{ } @@ -205,10 +205,10 @@ void EnTKFilter::computeCorrection() masterStateWrapper->setState(stateExp, mechParams); /// Write Some Data for Validation - helper::WriteAccessor > > stat = d_state; - helper::WriteAccessor > > var = d_variance; - helper::WriteAccessor > > covar = d_covariance; - helper::WriteAccessor > > innov = d_innovation; + helper::WriteAccessor< Data< type::vector > > stat = d_state; + helper::WriteAccessor< Data< type::vector > > var = d_variance; + helper::WriteAccessor< Data< type::vector > > covar = d_covariance; + helper::WriteAccessor< Data< type::vector > > innov = d_innovation; stat.resize(stateSize); var.resize(stateSize); @@ -242,7 +242,8 @@ void EnTKFilter::computeCorrection() template -void EnTKFilter::init() { +void EnTKFilter::init() +{ Inherit::init(); assert(this->gnode); @@ -273,22 +274,25 @@ void EnTKFilter::init() { PRNS("number of slave wrappers: " << numThreads-1); /// slaves + master } - numThreads=numSlaveWrappers+numMasterWrappers; + numThreads = numSlaveWrappers + numMasterWrappers; this->gnode->get(observationManager, core::objectmodel::BaseContext::SearchDown); if (observationManager) { PRNS("found observation manager: " << observationManager->getName()); - } else + } else { PRNE("no observation manager found!"); + } } + template -void EnTKFilter::bwdInit() { +void EnTKFilter::bwdInit() +{ assert(masterStateWrapper); stateSize = masterStateWrapper->getStateSize(); - std::cout<< "[LETKF] stateSize " << stateSize << std::endl; + std::cout << "[LETKF] stateSize " << stateSize << std::endl; PRNS("StateSize " << stateSize); /// Initialize inversion type @@ -346,8 +350,11 @@ void EnTKFilter::bwdInit() { m_sigmaPointObservationIndexes.resize(ensembleMembersNum); } + + template -void EnTKFilter::initializeStep(const core::ExecParams* _params, const size_t _step) { +void EnTKFilter::initializeStep(const core::ExecParams* _params, const size_t _step) +{ Inherit::initializeStep(_params, _step); if (initialiseObservationsAtFirstStep.getValue()) { @@ -370,11 +377,11 @@ void EnTKFilter::initializeStep(const core::ExecParams* _params, con template -void EnTKFilter::updateState() { - +void EnTKFilter::updateState() +{ stateSize = masterStateWrapper->getStateSize(); - //std::cout<< "new [UKF] stateSize " << stateSize << std::endl; - //PRNS("StateSize " << stateSize); + // std::cout<< "new [UKF] stateSize " << stateSize << std::endl; + // PRNS("StateSize " << stateSize); /// Initialize Model's Error Covariance stateCovar = masterStateWrapper->getStateErrorVariance(); @@ -383,11 +390,11 @@ void EnTKFilter::updateState() { for (size_t i = 0; i < (size_t)stateCovar.rows(); i++) { diagStateCov(i) = stateCovar(i,i); } - //std::cout<< "INIT COVARIANCE DIAGONAL P(n+1)+n: " << diagStateCov.transpose() << std::endl; + // std::cout<< "INIT COVARIANCE DIAGONAL P(n+1)+n: " << diagStateCov.transpose() << std::endl; PRNS(" INIT COVARIANCE DIAGONAL P(n+1)+n: \n" << diagStateCov.transpose()); modelNoise = masterStateWrapper->getModelElementNoise(); - //std::cout<< "Model covariance: " << modelNoise << std::endl; + // std::cout<< "Model covariance: " << modelNoise << std::endl; PRNS(" INIT COVARIANCE DIAGONAL P(n+1)+n: \n" << modelNoise); stateExp = masterStateWrapper->getState(); @@ -405,48 +412,64 @@ void EnTKFilter::updateState() { template -void EnTKFilter::stabilizeMatrix (EMatrixX& _initial, EMatrixX& _stabilized) { +void EnTKFilter::stabilizeMatrix(EMatrixX& _initial, EMatrixX& _stabilized) +{ Eigen::JacobiSVD svd(_initial, Eigen::ComputeThinU | Eigen::ComputeThinV); const Eigen::JacobiSVD::SingularValuesType singVals = svd.singularValues(); Eigen::JacobiSVD::SingularValuesType singValsStab = singVals; - for (int i=0; i < singVals.rows(); i++ ){ - if ((singValsStab(i)*singValsStab(i))*(1.0/(singVals(0)*singVals(0)))< 1.0e-6) singValsStab(i)=0; + for (int i = 0; i < singVals.rows(); i++ ){ + if ((singValsStab(i) * singValsStab(i)) * (1.0 / (singVals(0) * singVals(0))) < 1.0e-6) { + singValsStab(i) = 0; + } } - _stabilized= svd.matrixU()*singValsStab*svd.matrixV().transpose(); - + _stabilized = svd.matrixU() * singValsStab * svd.matrixV().transpose(); } + + template -void EnTKFilter::pseudoInverse( EMatrixX& M,EMatrixX& pinvM) { +void EnTKFilter::pseudoInverse(EMatrixX& M, EMatrixX& pinvM) +{ double epsilon= 1e-15; Eigen::JacobiSVD svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV); const Eigen::JacobiSVD::SingularValuesType singVals = svd.singularValues(); Eigen::JacobiSVD::SingularValuesType invSingVals = singVals; - for(int i=0; i -void EnTKFilter::sqrtMat(EMatrixX& A, EMatrixX& sqrtA){ - double epsilon= 1e-15; +void EnTKFilter::sqrtMat(EMatrixX& A, EMatrixX& sqrtA) +{ + double epsilon = 1e-15; Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV); const Eigen::JacobiSVD::SingularValuesType singVals = svd.singularValues(); Eigen::JacobiSVD::SingularValuesType sqrtSingVals = singVals; - for(int i=0; i -void EnTKFilter::writeValidationPlot (std::string filename ,EVectorX& state ){ +void EnTKFilter::writeValidationPlot (std::string filename ,EVectorX& state ) +{ if (this->saveParam) { std::ofstream paramFile(filename.c_str(), std::ios::app); if (paramFile.is_open()) { @@ -458,6 +481,7 @@ void EnTKFilter::writeValidationPlot (std::string filename ,EVectorX } + } // stochastic } // component diff --git a/src/stochasticFiltering/FilteringAnimationLoop.cpp b/src/stochasticFiltering/FilteringAnimationLoop.cpp index feaf1471..d531edd3 100644 --- a/src/stochasticFiltering/FilteringAnimationLoop.cpp +++ b/src/stochasticFiltering/FilteringAnimationLoop.cpp @@ -31,6 +31,8 @@ #include #include + + namespace sofa { @@ -40,19 +42,23 @@ namespace component namespace stochastic { + + SOFA_DECL_CLASS(FilteringAnimationLoop) int FilteringAnimationLoopClass = core::RegisterObject("Animation loop for stochastic filtering, requires a filter") .add< FilteringAnimationLoop >() ; + + FilteringAnimationLoop::FilteringAnimationLoop() : Inherit() , gnode(0) , verbose( initData(&verbose, false, "verbose", "print out traces") ) , d_timeDataFile( initData(&d_timeDataFile, std::string(""), "computationTimeFile", "file to save computation results") ) -{ -} +{ } + FilteringAnimationLoop::FilteringAnimationLoop(sofa::simulation::Node* _gnode) @@ -64,7 +70,10 @@ FilteringAnimationLoop::FilteringAnimationLoop(sofa::simulation::Node* _gnode) assert(gnode); } -void FilteringAnimationLoop::init() { + + +void FilteringAnimationLoop::init() +{ Inherit::init(); if (!gnode) @@ -93,16 +102,24 @@ void FilteringAnimationLoop::init() { m_timeProfiler.init(d_timeDataFile); } -void FilteringAnimationLoop::bwdInit() { - //PRNS("bwdInit"); + + +void FilteringAnimationLoop::bwdInit() +{ + // PRNS("bwdInit"); } -/// function called in every step of SOFA loop: calls initialization, prediction and correction which must be implemented by the filter component -void FilteringAnimationLoop::step(const core::ExecParams* _params, SReal /*_dt*/) { + + +/** + * function called in every step of SOFA loop: calls initialization, prediction and correction which must be implemented by the filter component + */ +void FilteringAnimationLoop::step(const core::ExecParams* _params, SReal /*_dt*/) +{ SReal dt = gnode->getDt(); sofa::simulation::AnimateBeginEvent ev(dt); - SingleLevelEventVisitor act ( _params, &ev, gnode ); - gnode->execute ( act ); + SingleLevelEventVisitor act( _params, &ev, gnode ); + gnode->execute( act ); actualStep++; if (verbose.getValue()) { @@ -120,23 +137,20 @@ void FilteringAnimationLoop::step(const core::ExecParams* _params, SReal /*_dt*/ m_timeProfiler.SaveStartTime(); filter->initializeStep(_params, actualStep); - TIC sofa::helper::AdvancedTimer::stepBegin("KalmanFilterPrediction"); //std::cout << "Start filter prediction" << std::endl; filter->computePrediction(); sofa::helper::AdvancedTimer::stepEnd("KalmanFilterPrediction"); //std::cout << "End filter prediction" << std::endl; - PredictionEndEvent predEvent ( dt ); - sofa::simulation::PropagateEventVisitor predEVisitor ( _params, &predEvent ); - gnode->execute ( predEVisitor ); - TOCTIC("== prediction total"); + PredictionEndEvent predEvent( dt ); + sofa::simulation::PropagateEventVisitor predEVisitor( _params, &predEvent ); + gnode->execute( predEVisitor ); sofa::helper::AdvancedTimer::stepBegin("KalmanFilterCorrection"); filter->computeCorrection(); CorrectionEndEvent corrEvent ( dt ); - sofa::simulation::PropagateEventVisitor corrEVisitor ( _params, &corrEvent ); - gnode->execute ( corrEVisitor ); - TOC("== correction total"); + sofa::simulation::PropagateEventVisitor corrEVisitor( _params, &corrEvent ); + gnode->execute( corrEVisitor ); // compute signle iteration step sofa::helper::AdvancedTimer::stepEnd("KalmanFilterCorrection"); @@ -144,11 +158,12 @@ void FilteringAnimationLoop::step(const core::ExecParams* _params, SReal /*_dt*/ /// this should be probably removed: sofa::simulation::AnimateEndEvent ev2(dt); - SingleLevelEventVisitor act2 ( _params, &ev2, gnode ); - gnode->execute ( act2 ); - + SingleLevelEventVisitor act2( _params, &ev2, gnode ); + gnode->execute( act2 ); } + + SingleLevelEventVisitor::SingleLevelEventVisitor(const core::ExecParams* params, sofa::core::objectmodel::Event* e, sofa::simulation::Node* node) : sofa::simulation::Visitor(params) , m_event(e) @@ -156,9 +171,12 @@ SingleLevelEventVisitor::SingleLevelEventVisitor(const core::ExecParams* params, {} + SingleLevelEventVisitor::~SingleLevelEventVisitor() {} + + sofa::simulation::Visitor::Result SingleLevelEventVisitor::processNodeTopDown(sofa::simulation::Node* node) { if (node == m_node) { @@ -168,16 +186,21 @@ sofa::simulation::Visitor::Result SingleLevelEventVisitor::processNodeTopDown(so return Visitor::RESULT_PRUNE; else return Visitor::RESULT_CONTINUE; - } else + } else { return Visitor::RESULT_PRUNE; + } } + + void SingleLevelEventVisitor::processObject(sofa::simulation::Node*, core::objectmodel::BaseObject* obj) { - if( obj->f_listening.getValue()==true ) + if( obj->f_listening.getValue() == true ) obj->handleEvent( m_event ); } + + } // namespace stochastic } // namespace component diff --git a/src/stochasticFiltering/FilteringAnimationLoop.h b/src/stochasticFiltering/FilteringAnimationLoop.h index d77599d0..b7564c19 100644 --- a/src/stochasticFiltering/FilteringAnimationLoop.h +++ b/src/stochasticFiltering/FilteringAnimationLoop.h @@ -44,20 +44,28 @@ #include "../genericComponents/FilterEvents.h" #include "../genericComponents/TimeProfiling.h" + + namespace sofa { + namespace component { + namespace stochastic { -using namespace defaulttype; + class SingleLevelEventVisitor : public sofa::simulation::Visitor { +protected: + sofa::core::objectmodel::Event* m_event; + sofa::simulation::Node* m_node; + + public: SingleLevelEventVisitor(const core::ExecParams* params, sofa::core::objectmodel::Event* e, sofa::simulation::Node* node); - ~SingleLevelEventVisitor(); Visitor::Result processNodeTopDown(sofa::simulation::Node* node); @@ -65,17 +73,15 @@ class SingleLevelEventVisitor : public sofa::simulation::Visitor virtual const char* getClassName() const { return "PropagateEventVisitor"; } virtual std::string getInfos() const { return std::string(m_event->getClassName()); } -protected: - sofa::core::objectmodel::Event* m_event; - sofa::simulation::Node* m_node; }; + + /** Class which implements the main loop of SOFA simulation: requires stochastic filter that will be called * It requires filter component on the same level (only one filter is supposed in the scene for now. * The filter API is defined in StochasticFilterBase. */ - -class FilteringAnimationLoop: public sofa::core::behavior::BaseAnimationLoop +class FilteringAnimationLoop : public sofa::core::behavior::BaseAnimationLoop { public: SOFA_CLASS(FilteringAnimationLoop,sofa::core::behavior::BaseAnimationLoop); @@ -83,21 +89,12 @@ class FilteringAnimationLoop: public sofa::core::behavior::BaseAnimationLoop typedef sofa::core::behavior::BaseAnimationLoop Inherit; typedef sofa::component::stochastic::StochasticFilterBase StochasticFilterBase; - ~FilteringAnimationLoop() {} - - FilteringAnimationLoop(); - FilteringAnimationLoop(sofa::simulation::Node* _gnode); - - -virtual void step(const core::ExecParams* _params, SReal _dt) override; - protected: sofa::helper::system::thread::CTime *timer; double startTime, stopTime; size_t actualStep; - helper::vector preStochasticWrappers; - + type::vector preStochasticWrappers; int numStep; sofa::component::stochastic::TimeProfiling m_timeProfiler; @@ -109,13 +106,16 @@ virtual void step(const core::ExecParams* _params, SReal _dt) override; Data verbose; core::objectmodel::DataFileName d_timeDataFile; - void init() override; - void bwdInit() override; - + FilteringAnimationLoop(); + FilteringAnimationLoop(sofa::simulation::Node* _gnode); + ~FilteringAnimationLoop() {} + virtual void step(const core::ExecParams* _params, SReal _dt) override; + void init() override; + void bwdInit() override; +}; -}; /// class } // stochastic diff --git a/src/stochasticFiltering/ForceFieldSensitivity.h b/src/stochasticFiltering/ForceFieldSensitivity.h index cab80c1b..5979bb48 100644 --- a/src/stochasticFiltering/ForceFieldSensitivity.h +++ b/src/stochasticFiltering/ForceFieldSensitivity.h @@ -30,6 +30,8 @@ #include #include + + namespace sofa { @@ -39,29 +41,31 @@ namespace core namespace behavior { + + /** - * \brief Component to allow for computing of the sensitivity matrix of a model (probably force field) - * + * Component to allow for computing of the sensitivity matrix of a model (probably force field) */ class SOFA_CORE_API ForceFieldSensitivity : public virtual objectmodel::BaseObject { public: SOFA_ABSTRACT_CLASS(ForceFieldSensitivity, objectmodel::BaseObject); SOFA_BASE_CAST_IMPLEMENTATION(ForceFieldSensitivity) + protected: ForceFieldSensitivity(); virtual ~ForceFieldSensitivity() {} private: - BaseForceField(const ForceFieldSensitivity& n) ; - BaseForceField& operator=(const ForceFieldSensitivity& n) ; + BaseForceField(const ForceFieldSensitivity& n); + BaseForceField& operator=(const ForceFieldSensitivity& n); +}; -}; /// class -} /// namespace behaviour +} // namespace behaviour -} /// namespace core +} // namespace core -} /// namespace sofa +} // namespace sofa diff --git a/src/stochasticFiltering/MappedStateObservationManager.cpp b/src/stochasticFiltering/MappedStateObservationManager.cpp index 817b2d10..cd26b28b 100644 --- a/src/stochasticFiltering/MappedStateObservationManager.cpp +++ b/src/stochasticFiltering/MappedStateObservationManager.cpp @@ -26,6 +26,8 @@ #include "MappedStateObservationManager.inl" //#include + + namespace sofa { @@ -35,9 +37,10 @@ namespace component namespace stochastic { -using namespace defaulttype; +using namespace defaulttype; + SOFA_DECL_CLASS(MappedStateObservationManager) // Register in the Factory @@ -50,10 +53,10 @@ template class SOFA_STOCHASTIC_API MappedStateObservationManager; -} // namespace stochastic -} // namespace component -} // namespace sofa +} // namespace stochastic +} // namespace component +} // namespace sofa diff --git a/src/stochasticFiltering/MappedStateObservationManager.h b/src/stochasticFiltering/MappedStateObservationManager.h index be36730f..0baf5c13 100644 --- a/src/stochasticFiltering/MappedStateObservationManager.h +++ b/src/stochasticFiltering/MappedStateObservationManager.h @@ -29,6 +29,7 @@ #include #include #include +#include #include #include @@ -48,7 +49,6 @@ namespace stochastic { -using namespace defaulttype; template class MappedStateObservationManager: public sofa::component::stochastic::ObservationManager @@ -69,8 +69,22 @@ class MappedStateObservationManager: public sofa::component::stochastic::Observa typedef sofa::component::container::SimulatedStateObservationSource ObservationSource; typedef StochasticStateWrapper StateWrapper; - MappedStateObservationManager(); - ~MappedStateObservationManager() {} + + Data< typename DataTypes1::VecCoord > inputObservationData; + Data< typename DataTypes2::VecCoord > mappedObservationData; + Data< double > noiseStdev; + Data< int > abberantIndex; + Data< bool > doNotMapObservations; + Data< type::vector > d_observationIndices; + type::vector observationIndices; + + SingleLink, StateWrapper, BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> stateWrapperLink; + + boost::mt19937* pRandGen; // I don't seed it on purpouse (it's not relevant) + boost::normal_distribution<>* pNormDist; + boost::variate_generator >* pVarNorm; + type::vector noise; + protected: size_t inputVectorSize, masterVectorSize, mappedVectorSize; /// real sizes of vectors @@ -87,6 +101,9 @@ class MappedStateObservationManager: public sofa::component::stochastic::Observa public: + MappedStateObservationManager(); + ~MappedStateObservationManager() {} + void init() override; void bwdInit() override; void initializeObservationData(); @@ -96,30 +113,13 @@ class MappedStateObservationManager: public sofa::component::stochastic::Observa virtual bool getRealObservation(double /* _time */, EVectorX& /* _realObs */) override; virtual bool getPredictedObservation(int _id, EVectorX& _predictedObservation) override; virtual bool obsFunction(EVectorX& /* _state */, EVectorX& /* _predictedObservation */) override; - - Data inputObservationData; - Data mappedObservationData; - Data noiseStdev; - Data abberantIndex; - Data doNotMapObservations; - Data > d_observationIndices; - helper::vector observationIndices; - - - SingleLink, StateWrapper, BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> stateWrapperLink; - - boost::mt19937* pRandGen; // I don't seed it on purpouse (it's not relevant) - boost::normal_distribution<>* pNormDist; - boost::variate_generator >* pVarNorm; - helper::vector noise; +}; -}; /// class - - } // namespace stochastic } // namespace component } // namespace sofa + diff --git a/src/stochasticFiltering/MappedStateObservationManager.inl b/src/stochasticFiltering/MappedStateObservationManager.inl index e820577d..22bae2e2 100644 --- a/src/stochasticFiltering/MappedStateObservationManager.inl +++ b/src/stochasticFiltering/MappedStateObservationManager.inl @@ -22,7 +22,6 @@ #pragma once #include - #include "MappedStateObservationManager.h" @@ -47,8 +46,9 @@ MappedStateObservationManager::MappedStateObse , doNotMapObservations( initData(&doNotMapObservations, false, "doNotMapObservations", "if real observations are read from a file (not the mechanical object)") ) , d_observationIndices( initData(&d_observationIndices, "observationIndices", "take these indices from vector of observations (implies not using mapping)") ) , stateWrapperLink(initLink("stateWrapper", "link to the state wrapper needed to perform apply (perhaps to be changed)")) -{ -} +{ } + + template void MappedStateObservationManager::init() @@ -103,9 +103,10 @@ void MappedStateObservationManager::init() PRNS("Found indices which will be taken as observations: " << observationIndices); doNotMapObservations.setValue(true); } - } + + template void MappedStateObservationManager::bwdInit() { @@ -116,8 +117,10 @@ void MappedStateObservationManager::bwdInit() Inherit::bwdInit(); } + + template -void MappedStateObservationManager::initializeObservationData() +void MappedStateObservationManager::initializeObservationData() { masterStateSize = masterState->getSize(); mappedStateSize = mappedState->getSize(); @@ -171,8 +174,11 @@ void MappedStateObservationManager::initialize Inherit::initializeObservationData(); } + + template -bool MappedStateObservationManager::hasObservation(double _time) { +bool MappedStateObservationManager::hasObservation(double _time) +{ if (Inherit::initialiseObservationsAtFirstStep.getValue()) { initializeObservationData(); Inherit::initialiseObservationsAtFirstStep.setValue(false); @@ -200,7 +206,7 @@ bool MappedStateObservationManager::hasObserva for (size_t d = 0; d < 3; d++) { if (noiseStdev.getValue() != 0.0) mappedObsState[i][d] += (*pVarNorm)(); - actualObservation(3*i+d) = mappedObsState[i][d]; + actualObservation(3 * i + d) = mappedObsState[i][d]; } } } else { @@ -217,17 +223,18 @@ bool MappedStateObservationManager::hasObserva for (size_t d = 0; d < 3; d++) { if (noiseStdev.getValue() != 0.0) mappedObsState[i][d] += (*pVarNorm)(); - actualObservation(3*i+d) = mappedObsState[i][d]; + actualObservation(3 * i + d) = mappedObsState[i][d]; } } } return (true); } -template -bool MappedStateObservationManager::getPredictedObservation(int _id, EVectorX& _predictedObservation) { +template +bool MappedStateObservationManager::getPredictedObservation(int _id, EVectorX& _predictedObservation) +{ Data predictedMasterState; Data predictedMappedState; @@ -252,21 +259,27 @@ bool MappedStateObservationManager::getPredict return true; } + + + template -bool MappedStateObservationManager::obsFunction(EVectorX& /* _state */, EVectorX& /* _predictedObservation */) +bool MappedStateObservationManager::obsFunction(EVectorX& /* _state */, EVectorX& /* _predictedObservation */) { return 0; } + + template -bool MappedStateObservationManager::getRealObservation(double /* _time */, EVectorX& /* _realObs */) +bool MappedStateObservationManager::getRealObservation(double /* _time */, EVectorX& /* _realObs */) { return 0; } + template -bool MappedStateObservationManager::getInnovation(double _time, EVectorX& _state, EVectorX& _innovation) +bool MappedStateObservationManager::getInnovation(double _time, EVectorX& _state, EVectorX& _innovation) { if (_time != this->actualTime) { PRNE("Observation for time " << this->actualTime << " not prepared, call hasObservation first!"); @@ -295,7 +308,7 @@ bool MappedStateObservationManager::getInnovat for (size_t i = 0; i < predictedMappedStateEdit.size(); i++) for (size_t d = 0; d < 3; d++) - _innovation(3*i+d) = actualObservation(3*i+d) - predictedMappedStateEdit[i][d]; + _innovation(3 * i + d) = actualObservation(3 * i + d) - predictedMappedStateEdit[i][d]; } /// TEMPORARY: _state here is the predicted observation computed before @@ -339,21 +352,16 @@ bool MappedStateObservationManager::getInnovat return this->innovation_; */ - } - -//template -//void MappedStateObservationManager::init() -//{ -// -//} +// template +// void MappedStateObservationManager::init() +// { } // -//template -//void MappedStateObservationManager::reinit() -//{ -//} +// template +// void MappedStateObservationManager::reinit() +// { } } // namespace stochastic diff --git a/src/stochasticFiltering/MappedStateVelocityObservationManager.cpp b/src/stochasticFiltering/MappedStateVelocityObservationManager.cpp index b3bde42a..bea57c99 100644 --- a/src/stochasticFiltering/MappedStateVelocityObservationManager.cpp +++ b/src/stochasticFiltering/MappedStateVelocityObservationManager.cpp @@ -26,6 +26,8 @@ #include "MappedStateVelocityObservationManager.inl" //#include + + namespace sofa { @@ -35,9 +37,10 @@ namespace component namespace stochastic { -using namespace defaulttype; +using namespace defaulttype; + SOFA_DECL_CLASS(MappedStateVelocityObservationManager) // Register in the Factory @@ -51,6 +54,7 @@ template class SOFA_STOCHASTIC_API MappedStateVelocityObservationManager; + } // namespace stochastic } // namespace component diff --git a/src/stochasticFiltering/MappedStateVelocityObservationManager.h b/src/stochasticFiltering/MappedStateVelocityObservationManager.h index 68059c5e..af255e90 100644 --- a/src/stochasticFiltering/MappedStateVelocityObservationManager.h +++ b/src/stochasticFiltering/MappedStateVelocityObservationManager.h @@ -29,6 +29,7 @@ #include #include #include +#include #include #include @@ -48,7 +49,6 @@ namespace stochastic { -using namespace defaulttype; template class MappedStateVelocityObservationManager: public sofa::component::stochastic::ObservationManager @@ -69,17 +69,36 @@ class MappedStateVelocityObservationManager: public sofa::component::stochastic: typedef sofa::component::container::SimulatedStateObservationSource ObservationSource; typedef StochasticStateWrapper StateWrapper; - MappedStateVelocityObservationManager(); - ~MappedStateVelocityObservationManager() {} + Data< typename DataTypes1::VecCoord > inputObservationData; + Data< typename DataTypes1::VecDeriv > inputVelocityObservationData; + Data< typename DataTypes2::VecCoord > mappedObservationFullData; + Data< typename DataTypes2::VecCoord > mappedObservationData; + Data< typename DataTypes2::VecDeriv > mappedObservationVelocityData; + Data< double > noiseStdev; + Data< int > abberantIndex; + Data< bool > doNotMapObservations; + Data< bool > d_observePositions; + Data< bool > d_observeVelocities; + Data< FilterType > d_velocityObservationStdev; + Data< type::vector > d_observationIndices; + type::vector observationIndices; + + SingleLink, StateWrapper, BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> stateWrapperLink; + + boost::mt19937* pRandGen; // I don't seed it on purpouse (it's not relevant) + boost::normal_distribution<>* pNormDist; + boost::variate_generator >* pVarNorm; + type::vector noise; + protected: - size_t inputVectorSize, masterVectorSize, mappedVectorSize; /// real sizes of vectors + size_t inputVectorSize, masterVectorSize, mappedVectorSize; /// real sizes of vectors size_t inputStateSize, inputLoadedStateSize, masterStateSize, mappedStateSize; /// number of points in each vector Mapping* mapping; MappedState* mappedState; MasterState* masterState; - helper::vector observationSources; + type::vector observationSources; StateWrapper* stateWrapper; double actualObservationTime; @@ -87,6 +106,9 @@ class MappedStateVelocityObservationManager: public sofa::component::stochastic: public: + MappedStateVelocityObservationManager(); + ~MappedStateVelocityObservationManager() {} + void init() override; void bwdInit() override; void initializeObservationData(); @@ -96,30 +118,8 @@ class MappedStateVelocityObservationManager: public sofa::component::stochastic: virtual bool getRealObservation(double _time, EVectorX& _realObs) override; virtual bool getPredictedObservation(int _id, EVectorX& _predictedObservation) override; virtual bool obsFunction(EVectorX& _state, EVectorX& _predictedObservation) override; +}; - Data inputObservationData; - Data inputVelocityObservationData; - Data mappedObservationFullData; - Data mappedObservationData; - Data mappedObservationVelocityData; - Data noiseStdev; - Data abberantIndex; - Data doNotMapObservations; - Data d_observePositions; - Data d_observeVelocities; - Data d_velocityObservationStdev; - Data > d_observationIndices; - helper::vector observationIndices; - - - SingleLink, StateWrapper, BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> stateWrapperLink; - - boost::mt19937* pRandGen; // I don't seed it on purpouse (it's not relevant) - boost::normal_distribution<>* pNormDist; - boost::variate_generator >* pVarNorm; - helper::vector noise; - -}; /// class } // namespace stochastic diff --git a/src/stochasticFiltering/MappedStateVelocityObservationManager.inl b/src/stochasticFiltering/MappedStateVelocityObservationManager.inl index df74fd0d..824fe108 100644 --- a/src/stochasticFiltering/MappedStateVelocityObservationManager.inl +++ b/src/stochasticFiltering/MappedStateVelocityObservationManager.inl @@ -22,7 +22,6 @@ #pragma once #include - #include "MappedStateVelocityObservationManager.h" @@ -37,8 +36,9 @@ namespace stochastic { + template -MappedStateVelocityObservationManager::MappedStateVelocityObservationManager() +MappedStateVelocityObservationManager::MappedStateVelocityObservationManager() : Inherit() , inputObservationData( initData (&inputObservationData, "observations", "observations read from a file") ) , inputVelocityObservationData( initData (&inputVelocityObservationData, "velocityObservations", "velocity observations read from a file") ) @@ -51,11 +51,12 @@ MappedStateVelocityObservationManager::MappedS , d_velocityObservationStdev( initData(&d_velocityObservationStdev, FilterType(0.0), "velocityObservationStdev", "standard deviation in velocity observations") ) , d_observationIndices( initData(&d_observationIndices, "observationIndices", "take these indices from vector of observations (implies not using mapping)") ) , stateWrapperLink(initLink("stateWrapper", "link to the state wrapper needed to perform apply (perhaps to be changed)")) -{ -} +{ } + + template -void MappedStateVelocityObservationManager::init() +void MappedStateVelocityObservationManager::init() { Inherit::init(); @@ -110,11 +111,12 @@ void MappedStateVelocityObservationManager::in PRNS("Found indices which will be taken as observations: " << observationIndices); doNotMapObservations.setValue(true); } - } + + template -void MappedStateVelocityObservationManager::bwdInit() +void MappedStateVelocityObservationManager::bwdInit() { if (!Inherit::initialiseObservationsAtFirstStep.getValue()) { initializeObservationData(); @@ -123,8 +125,10 @@ void MappedStateVelocityObservationManager::bw Inherit::bwdInit(); } + + template -void MappedStateVelocityObservationManager::initializeObservationData() +void MappedStateVelocityObservationManager::initializeObservationData() { masterStateSize = masterState->getSize(); mappedStateSize = mappedState->getSize(); @@ -214,8 +218,11 @@ void MappedStateVelocityObservationManager::in Inherit::initializeObservationData(); } + + template -bool MappedStateVelocityObservationManager::hasObservation(double _time) { +bool MappedStateVelocityObservationManager::hasObservation(double _time) +{ if (Inherit::initialiseObservationsAtFirstStep.getValue()) { initializeObservationData(); Inherit::initialiseObservationsAtFirstStep.setValue(false); @@ -302,9 +309,11 @@ bool MappedStateVelocityObservationManager::ha return (true); } -template -bool MappedStateVelocityObservationManager::getPredictedObservation(int _id, EVectorX& _predictedObservation) { + +template +bool MappedStateVelocityObservationManager::getPredictedObservation(int _id, EVectorX& _predictedObservation) +{ sofa::core::MechanicalParams mp; size_t mappedStateShift = 0; @@ -323,9 +332,9 @@ bool MappedStateVelocityObservationManager::ge stateWrapper->getActualPosition(_id, predictedMasterStateEdit); std::cout << "Predicted state: " << predictedMasterState << std::endl; - //stateWrapper->setSofaVectorFromFilterVector(_state, predictedMasterStateEdit); - //sofa::helper::WriteAccessor< Data > masterState = predictedMasterState; + // stateWrapper->setSofaVectorFromFilterVector(_state, predictedMasterStateEdit); + // sofa::helper::WriteAccessor< Data > masterState = predictedMasterState; mapping->apply(&mp, predictedMappedState, predictedMasterState); @@ -333,9 +342,10 @@ bool MappedStateVelocityObservationManager::ge for (size_t d = 0; d < 3; d++) _predictedObservation(3*i+d) = predictedMappedStateEdit[i][d]; } + if (d_observeVelocities.getValue()) { - Data predictedMasterVelocity; - Data predictedMappedVelocity; + Data< typename DataTypes1::VecDeriv > predictedMasterVelocity; + Data< typename DataTypes2::VecDeriv > predictedMappedVelocity; typename DataTypes1::VecDeriv& predictedMasterVelocityEdit = *predictedMasterVelocity.beginEdit(); typename DataTypes2::VecDeriv& predictedMappedVelocityEdit = *predictedMappedVelocity.beginEdit(); @@ -360,12 +370,17 @@ bool MappedStateVelocityObservationManager::ge return true; } + + + template bool MappedStateVelocityObservationManager::obsFunction(EVectorX& /* _state */, EVectorX& /* _predictedObservation */) { return false; } + + template bool MappedStateVelocityObservationManager::getRealObservation(double /* _time */, EVectorX& /* _realObs */) { @@ -373,8 +388,9 @@ bool MappedStateVelocityObservationManager::ge } + template -bool MappedStateVelocityObservationManager::getInnovation(double _time, EVectorX& _state, EVectorX& _innovation) +bool MappedStateVelocityObservationManager::getInnovation(double _time, EVectorX& _state, EVectorX& _innovation) { if (_time != this->actualTime) { PRNE("Observation for time " << this->actualTime << " not prepared, call hasObservation first!"); @@ -436,7 +452,6 @@ bool MappedStateVelocityObservationManager::ge return(true); - /* //std::cout << this->getName() << ": size of mapped state: " << mappedState.size() << std::endl; this->innovation_.Reallocate(mappedState.size()*3); @@ -469,21 +484,17 @@ bool MappedStateVelocityObservationManager::ge return this->innovation_; */ - } - -//template -//void MappedStateObservationManager::init() -//{ -// -//} +// template +// void MappedStateObservationManager::init() +// { } // -//template -//void MappedStateObservationManager::reinit() -//{ -//} +// template +// void MappedStateObservationManager::reinit() +// { } + } // namespace stochastic diff --git a/src/stochasticFiltering/ObservationManagerBase.h b/src/stochasticFiltering/ObservationManagerBase.h index 9a7f5896..629ae0fa 100644 --- a/src/stochasticFiltering/ObservationManagerBase.h +++ b/src/stochasticFiltering/ObservationManagerBase.h @@ -22,12 +22,11 @@ #pragma once #include "initOptimusPlugin.h" - #include - #include + namespace sofa { @@ -38,21 +37,12 @@ namespace stochastic { -using namespace defaulttype; - -class ObservationManagerBase: public sofa::core::objectmodel::BaseObject +class ObservationManagerBase : public sofa::core::objectmodel::BaseObject { public: SOFA_ABSTRACT_CLASS(ObservationManagerBase, BaseObject); - typedef sofa::core::objectmodel::BaseObject Inherit; - ObservationManagerBase() - : Inherit() - , verbose( initData(&verbose, false, "verbose", "print tracing informations") ) - {} - - ~ObservationManagerBase() {} protected: sofa::simulation::Node* gnode; @@ -62,6 +52,13 @@ class ObservationManagerBase: public sofa::core::objectmodel::BaseObject public: Data verbose; + ObservationManagerBase() + : Inherit() + , verbose( initData(&verbose, false, "verbose", "print tracing informations") ) + {} + + ~ObservationManagerBase() {} + void init() override { Inherit::init(); @@ -76,10 +73,9 @@ class ObservationManagerBase: public sofa::core::objectmodel::BaseObject stepNumber = _stepNumber; actualTime = double(stepNumber)*gnode->getDt(); } +}; -}; /// class - template class ObservationManager: public sofa::component::stochastic::ObservationManagerBase @@ -91,24 +87,22 @@ class ObservationManager: public sofa::component::stochastic::ObservationManager typedef typename Eigen::Matrix EMatrixX; typedef typename Eigen::Matrix EVectorX; - ObservationManager() - :Inherit() - , observationStdev( initData(&observationStdev, FilterType(0.0), "observationStdev", "standard deviation in observations") ) - , initialiseObservationsAtFirstStep( initData(&initialiseObservationsAtFirstStep, false, "initialiseObservationsAtFirstStep", "if true initialise component during first iteration") ) - {} - - ~ObservationManager() {} + Data observationStdev; + Data initialiseObservationsAtFirstStep; protected: - size_t observationSize; /// size of the observation vector - + size_t observationSize; /// size of the observation vector FilterType errorVarianceValue; EMatrixX errorVariance; EMatrixX errorVarianceInverse; public: - Data observationStdev; - Data initialiseObservationsAtFirstStep; + ObservationManager() + :Inherit() + , observationStdev( initData(&observationStdev, FilterType(0.0), "observationStdev", "standard deviation in observations") ) + , initialiseObservationsAtFirstStep( initData(&initialiseObservationsAtFirstStep, false, "initialiseObservationsAtFirstStep", "if true initialise component during first iteration") ) + {} + ~ObservationManager() {} virtual bool hasObservation(double _time) = 0; virtual bool getInnovation(double _time, EVectorX& _state, EVectorX& _innovation) = 0; @@ -116,19 +110,23 @@ class ObservationManager: public sofa::component::stochastic::ObservationManager virtual bool obsFunction(EVectorX& _state, EVectorX& _predictedObservation) = 0; virtual bool getPredictedObservation(int _id, EVectorX& _predictedObservation) = 0; - size_t getObservationSize() { + size_t getObservationSize() + { return observationSize; } - virtual EMatrixX& getErrorVariance() { + virtual EMatrixX& getErrorVariance() + { return errorVariance; } - virtual EMatrixX& getErrorVarianceInverse() { + virtual EMatrixX& getErrorVarianceInverse() + { return errorVarianceInverse; } - void init() override { + void init() override + { Inherit::init(); gnode = dynamic_cast(this->getContext()); @@ -138,7 +136,8 @@ class ObservationManager: public sofa::component::stochastic::ObservationManager } } - void bwdInit() override { + void bwdInit() override + { Inherit::bwdInit(); if (!initialiseObservationsAtFirstStep.getValue()) { @@ -146,7 +145,8 @@ class ObservationManager: public sofa::component::stochastic::ObservationManager } } - void initializeObservationData() { + void initializeObservationData() + { if (observationSize == 0) { PRNE("No observations available, cannot allocate the structures!"); } @@ -156,8 +156,8 @@ class ObservationManager: public sofa::component::stochastic::ObservationManager errorVariance = EMatrixX::Identity(observationSize, observationSize) * errorVarianceValue; errorVarianceInverse = EMatrixX::Identity(observationSize, observationSize) / errorVarianceValue; } +}; -}; /// class } // stochastic @@ -165,3 +165,4 @@ class ObservationManager: public sofa::component::stochastic::ObservationManager } // component } // sofa + diff --git a/src/stochasticFiltering/PreStochasticWrapper.cpp b/src/stochasticFiltering/PreStochasticWrapper.cpp index d0d9c0a9..c3ef0248 100644 --- a/src/stochasticFiltering/PreStochasticWrapper.cpp +++ b/src/stochasticFiltering/PreStochasticWrapper.cpp @@ -55,6 +55,8 @@ #include "PreStochasticWrapper.h" //#include + + namespace sofa { @@ -64,6 +66,8 @@ namespace component namespace stochastic { + + using namespace defaulttype; SOFA_DECL_CLASS(PreStochasticWrapper) @@ -73,12 +77,15 @@ int PreStochasticWrapperClass = core::RegisterObject("PreStochasticWrapper") .add< PreStochasticWrapper>() ; + PreStochasticWrapper::PreStochasticWrapper() : verbose( initData(&verbose, false, "verbose", "print tracing informations") ) -{ -} +{ } -void PreStochasticWrapper::init() { + + +void PreStochasticWrapper::init() +{ gnode = dynamic_cast(this->getContext()); if (!gnode) { @@ -86,8 +93,10 @@ void PreStochasticWrapper::init() { } } -void PreStochasticWrapper::step(const core::ExecParams* _params, const size_t _step) { + +void PreStochasticWrapper::step(const core::ExecParams* _params, const size_t _step) +{ sofa::helper::AdvancedTimer::stepBegin("PreStochasticWrapper"); PRNS(" calling step in " << _step); @@ -96,11 +105,11 @@ void PreStochasticWrapper::step(const core::ExecParams* _params, const size_t _s //sofa::helper::AdvancedTimer::begin("Animate"); //std::cout<<"step "<gnode->getDt(); + double dt = this->gnode->getDt(); //core::ExecParams* _params = sofa::core::ExecParams::defaultInstance(); //std::cout << "[" << this->getName() << "]: step default begin at time = " << gnode->getTime() << " update time: " << _update_time << std::endl; @@ -110,16 +119,16 @@ void PreStochasticWrapper::step(const core::ExecParams* _params, const size_t _s //sofa::helper::AdvancedTimer::begin("Animate"); //std::cout<<"step "<getName() << "]: animate begin" << std::endl; - sofa::simulation::AnimateBeginEvent ev ( dt ); - sofa::simulation::PropagateEventVisitor act ( _params, &ev ); - this->gnode->execute ( act ); + sofa::simulation::AnimateBeginEvent ev( dt ); + sofa::simulation::PropagateEventVisitor act( _params, &ev ); + this->gnode->execute( act ); //std::cout<<"step "<getName() << "]: behaviour update position" << std::endl; sofa::simulation::BehaviorUpdatePositionVisitor beh(_params , dt); - this->gnode->execute ( beh ); + this->gnode->execute( beh ); //std::cout<<"step "<getName() << "]: update internal data" << std::endl; sofa::simulation::UpdateInternalDataVisitor uid(_params); - this->gnode->execute ( uid ); + this->gnode->execute( uid ); //std::cout<<"step "<getName() << "]: animate" << std::endl; sofa::simulation::AnimateVisitor act(_params, dt); - this->gnode->execute ( act ); + this->gnode->execute( act ); //if (_updateTime) { //std::cout << "[" << this->getName() << "]: update simulation context" << std::endl; - this->gnode->setTime ( startTime + dt ); + this->gnode->setTime( startTime + dt ); this->gnode->execute< sofa::simulation::UpdateSimulationContextVisitor >(_params); //} //std::cout<<"step "<getName() << "]: animate end" << std::endl; - sofa::simulation::AnimateEndEvent ev ( dt ); - sofa::simulation::PropagateEventVisitor act (_params, &ev ); - this->gnode->execute ( act ); + sofa::simulation::AnimateEndEvent ev( dt ); + sofa::simulation::PropagateEventVisitor act(_params, &ev ); + this->gnode->execute( act ); } sofa::helper::AdvancedTimer::stepBegin("UpdateMapping"); @@ -157,26 +166,28 @@ void PreStochasticWrapper::step(const core::ExecParams* _params, const size_t _s //sofa::helper::AdvancedTimer::step("UpdateMappingEndEvent"); { //std::cout << "[" << this->getName() << "]: update mapping end" << std::endl; - sofa::simulation::UpdateMappingEndEvent ev ( dt ); - sofa::simulation::PropagateEventVisitor act ( _params , &ev ); - this->gnode->execute ( act ); + sofa::simulation::UpdateMappingEndEvent ev( dt ); + sofa::simulation::PropagateEventVisitor act( _params , &ev ); + this->gnode->execute( act ); } sofa::helper::AdvancedTimer::stepEnd("UpdateMapping"); -#ifndef SOFA_NO_UPDATE_BBOX - sofa::helper::AdvancedTimer::stepBegin("UpdateBBox"); - this->gnode->execute< sofa::simulation::UpdateBoundingBoxVisitor >(_params); - sofa::helper::AdvancedTimer::stepEnd("UpdateBBox"); -#endif -#ifdef SOFA_DUMP_VISITOR_INFO - simulation::Visitor::printCloseNode("Step"); -#endif + #ifndef SOFA_NO_UPDATE_BBOX + sofa::helper::AdvancedTimer::stepBegin("UpdateBBox"); + this->gnode->execute< sofa::simulation::UpdateBoundingBoxVisitor >(_params); + sofa::helper::AdvancedTimer::stepEnd("UpdateBBox"); + #endif + #ifdef SOFA_DUMP_VISITOR_INFO + simulation::Visitor::printCloseNode("Step"); + #endif //sofa::helper::AdvancedTimer::end("Animate"); sofa::helper::AdvancedTimer::stepEnd("AnimationStep"); sofa::helper::AdvancedTimer::stepEnd("PreStochasticWrapper"); } + + } // namespace stochastic } // namespace component diff --git a/src/stochasticFiltering/PreStochasticWrapper.h b/src/stochasticFiltering/PreStochasticWrapper.h index 26bc55b1..405c4e82 100644 --- a/src/stochasticFiltering/PreStochasticWrapper.h +++ b/src/stochasticFiltering/PreStochasticWrapper.h @@ -29,6 +29,8 @@ #include #include + + namespace sofa { @@ -38,27 +40,23 @@ namespace component namespace stochastic { -using namespace defaulttype; -class PreStochasticWrapper: public sofa::core::objectmodel::BaseObject + +class PreStochasticWrapper : public sofa::core::objectmodel::BaseObject { public: SOFA_CLASS(PreStochasticWrapper,sofa::core::objectmodel::BaseObject); + Data verbose; + sofa::simulation::Node* gnode; + PreStochasticWrapper(); ~PreStochasticWrapper() {} virtual void step(const core::ExecParams* _params, const size_t _step); void init() override; +}; -protected: - -public: - Data verbose; - sofa::simulation::Node* gnode; - - -}; /// class } // stochastic diff --git a/src/stochasticFiltering/ROUKFilter.cpp b/src/stochasticFiltering/ROUKFilter.cpp index eb1f8122..6ae614db 100644 --- a/src/stochasticFiltering/ROUKFilter.cpp +++ b/src/stochasticFiltering/ROUKFilter.cpp @@ -26,6 +26,8 @@ #include "ROUKFilter.inl" //#include + + namespace sofa { @@ -35,9 +37,9 @@ namespace component namespace stochastic { -using namespace defaulttype; +using namespace defaulttype; SOFA_DECL_CLASS(ROUKFilter) @@ -51,6 +53,7 @@ int ROUKFilterClass = core::RegisterObject("ROUKFilter") #endif ; + #ifndef SOFA_FLOAT template class SOFA_STOCHASTIC_API ROUKFilter; #endif @@ -59,6 +62,7 @@ template class SOFA_STOCHASTIC_API ROUKFilter; #endif + } // namespace stochastic } // namespace component diff --git a/src/stochasticFiltering/ROUKFilter.h b/src/stochasticFiltering/ROUKFilter.h index 238ed91b..b3b8a233 100644 --- a/src/stochasticFiltering/ROUKFilter.h +++ b/src/stochasticFiltering/ROUKFilter.h @@ -35,11 +35,6 @@ #include -#ifdef Success -#undef Success // dirty workaround to cope with the (dirtier) X11 define. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=253 -#endif -#include - //#include #include #include @@ -47,6 +42,10 @@ #include #include +#include + + + namespace sofa { @@ -57,8 +56,9 @@ namespace stochastic { + /// to speed up, wrappers for BLAS matrix multiplications created, much faster that Eigen by default -extern "C"{ +extern "C" { // product C= alphaA.B + betaC void dgemm_(char* TRANSA, char* TRANSB, const int* M, const int* N, const int* K, double* alpha, double* A, @@ -68,9 +68,8 @@ extern "C"{ void dgemv_(char* TRANS, const int* M, const int* N, double* alpha, double* A, const int* LDA, double* X, const int* INCX, double* beta, double* C, const int* INCY); - } +} -using namespace defaulttype; /** @@ -80,10 +79,8 @@ using namespace defaulttype; * Naming conventions inspired by Verdandi library, corresponds to symbols used in the paper. * Filter requires StochasticStateWrapper which provides the interface with SOFA. */ - - template -class ROUKFilter: public sofa::component::stochastic::StochasticUnscentedFilterBase +class ROUKFilter : public sofa::component::stochastic::StochasticUnscentedFilterBase { public: SOFA_CLASS(SOFA_TEMPLATE(ROUKFilter, FilterType), StochasticUnscentedFilterBase); @@ -94,12 +91,21 @@ class ROUKFilter: public sofa::component::stochastic::StochasticUnscentedFilterB typedef typename Eigen::Matrix EMatrixX; typedef typename Eigen::Matrix EVectorX; -ROUKFilter(); -~ROUKFilter(); + + Data< std::string > observationErrorVarianceType; + Data< bool > useBlasToMultiply; + Data< type::vector > reducedState; + Data< type::vector > reducedVariance; + Data< type::vector > reducedCovariance; + Data< type::vector > d_reducedInnovation; + Data< type::vector > d_state; + Data< type::vector > d_variance; + Data< type::vector > d_covariance; + Data< bool > d_executeSimulationForCorrectedData; protected: StochasticStateWrapperBaseT* masterStateWrapper; - helper::vector*> stateWrappers; + type::vector*> stateWrappers; ObservationManager* observationManager; /// vector sizes @@ -118,8 +124,8 @@ ROUKFilter(); Type alpha, alphaVar; /// structures for parallel computing: - helper::vector sigmaPoints2WrapperIDs; - helper::vector > wrapper2SigmaPointsIDs; + type::vector sigmaPoints2WrapperIDs; + type::vector > wrapper2SigmaPointsIDs; size_t numThreads; /// functions @@ -134,17 +140,8 @@ ROUKFilter(); void computeStarCorrection(); public: - Data observationErrorVarianceType; - Data useBlasToMultiply; - Data > reducedState; - Data > reducedVariance; - Data > reducedCovariance; - Data > d_reducedInnovation; - Data > d_state; - Data > d_variance; - Data > d_covariance; - Data d_executeSimulationForCorrectedData; - + ROUKFilter(); + ~ROUKFilter(); void init() override; void bwdInit() override; @@ -156,13 +153,14 @@ ROUKFilter(); virtual void computeCorrection() override; virtual void updateState() override { } +}; + + -}; /// class /** * Code necessary for parallelization. Not tested since longer time. */ - template struct WorkerThreadData { @@ -170,12 +168,12 @@ struct WorkerThreadData StochasticStateWrapperBaseT* wrapper; size_t threadID; - helper::vector* sigmaIDs; + type::vector* sigmaIDs; EMatrixX* stateMatrix; const core::ExecParams* execParams; bool saveLog; - void set(size_t _threadID,StochasticStateWrapperBaseT* _wrapper, helper::vector *_sigID, + void set(size_t _threadID,StochasticStateWrapperBaseT* _wrapper, type::vector *_sigID, EMatrixX* _stateMat, const core::ExecParams* _execParams, bool _saveLog) { threadID=_threadID; sigmaIDs=_sigID; @@ -186,12 +184,14 @@ struct WorkerThreadData } }; + + template void* threadFunction(void* inArgs) { char name[100]; std::ofstream fd; WorkerThreadData* threadData = reinterpret_cast* >(inArgs); - helper::vector& sigIDs = *(threadData->sigmaIDs); + type::vector& sigIDs = *(threadData->sigmaIDs); size_t id = threadData->threadID; StochasticStateWrapperBaseT* wrapper = threadData->wrapper; @@ -225,6 +225,8 @@ void* threadFunction(void* inArgs) { return nullptr; } + + } // stochastic } // component diff --git a/src/stochasticFiltering/ROUKFilter.inl b/src/stochasticFiltering/ROUKFilter.inl index d26a2516..74ab5c40 100644 --- a/src/stochasticFiltering/ROUKFilter.inl +++ b/src/stochasticFiltering/ROUKFilter.inl @@ -23,13 +23,19 @@ #include "ROUKFilter.h" + + namespace sofa { + namespace component { + namespace stochastic { + + /// C = alpha*A*B + beta*C; template void ROUKFilter::blasMultAdd(EMatrixX& _a, EMatrixX& _b, EMatrixX& _c, Type _alpha, Type _beta) { @@ -42,6 +48,8 @@ void ROUKFilter::blasMultAdd(EMatrixX& _a, EMatrixX& _b, EMatrixX& _ Type* _cdata = _c.data(); dgemm_(&trans,&trans, &m, &n, &k, &_alpha, _adata, &m, _bdata, &k, &_beta, _cdata, &m); } + + template void ROUKFilter::blasMultAdd(char _trans1, char _trans2, EMatrixX& _a, EMatrixX& _b, EMatrixX& _c, Type _alpha, Type _beta) { int m = (_trans1 == 'N' ? _a.rows() : _a.cols()); @@ -57,6 +65,8 @@ void ROUKFilter::blasMultAdd(char _trans1, char _trans2, EMatrixX& _ dgemm_(&_trans1,&_trans2, &m, &n, &k, &_alpha, _adata, &m1, _bdata, &k1, &_beta, _cdata, &m); } + + template ROUKFilter::ROUKFilter() : Inherit() @@ -74,47 +84,53 @@ ROUKFilter::ROUKFilter() this->reducedOrder.setValue(true); } + + template ROUKFilter::~ROUKFilter() {} + + template -void ROUKFilter::computePerturbedStates(EVectorX& _meanState) { - if (numThreads == 1) { - EVectorX xCol(stateSize); - for (size_t i = 0; i < sigmaPointsNum; i++) { - xCol = matXi.col(i); - //std::cout << "Before simul:" << xCol(stateSize-3) << " " << xCol(stateSize-2) << " " << xCol(stateSize-1) << std::endl; - stateWrappers[sigmaPoints2WrapperIDs[i]]->transformState(xCol, mechParams); - //std::cout << "After simul:" << xCol(stateSize-3) << " " << xCol(stateSize-2) << " " << xCol(stateSize-1) << std::endl; - matXi.col(i) = xCol; - - /*char fileName[100]; - sprintf(fileName, "sigma_%03d_%03d", this->stepNumber, i); - std::ofstream cvmFile; - cvmFile.open(fileName); - cvmFile << xCol << std::endl; - cvmFile.close();*/ - } - _meanState = alpha * matXi.rowwise().sum(); - } else { - pthread_t* tid = new pthread_t[numThreads]; - WorkerThreadData* threadData = new WorkerThreadData[numThreads]; - for (size_t i = 0; i < numThreads; i++) { - threadData[i].set(i, stateWrappers[i], &wrapper2SigmaPointsIDs[i], &matXi, this->execParams, 0); - PRNS("Creating thread " << i); - pthread_create(&tid[i], NULL, threadFunction, (void*)&threadData[i]); - } +void ROUKFilter::computePerturbedStates(EVectorX& _meanState) +{ + // if (numThreads == 1) { + EVectorX xCol(stateSize); + for (size_t i = 0; i < sigmaPointsNum; i++) { + xCol = matXi.col(i); + //std::cout << "Before simul:" << xCol(stateSize-3) << " " << xCol(stateSize-2) << " " << xCol(stateSize-1) << std::endl; + stateWrappers[sigmaPoints2WrapperIDs[i]]->transformState(xCol, mechParams); + //std::cout << "After simul:" << xCol(stateSize-3) << " " << xCol(stateSize-2) << " " << xCol(stateSize-1) << std::endl; + matXi.col(i) = xCol; - for (size_t i = 0; i < numThreads; i++) { - int s = pthread_join(tid[i], NULL); - if (s != 0) - PRNE("Thread " << i << " problem: " << s); - } - _meanState = alpha * matXi.rowwise().sum(); + /*char fileName[100]; + sprintf(fileName, "sigma_%03d_%03d", this->stepNumber, i); + std::ofstream cvmFile; + cvmFile.open(fileName); + cvmFile << xCol << std::endl; + cvmFile.close();*/ } + // } else { + // pthread_t* tid = new pthread_t[numThreads]; + // WorkerThreadData* threadData = new WorkerThreadData[numThreads]; + // for (size_t i = 0; i < numThreads; i++) { + // threadData[i].set(i, stateWrappers[i], &wrapper2SigmaPointsIDs[i], &matXi, this->execParams, 0); + // PRNS("Creating thread " << i); + // pthread_create(&tid[i], NULL, threadFunction, (void*)&threadData[i]); + // } + + // for (size_t i = 0; i < numThreads; i++) { + // int s = pthread_join(tid[i], NULL); + // if (s != 0) + // PRNE("Thread " << i << " problem: " << s); + // } + // } + + _meanState = alpha * matXi.rowwise().sum(); } + template void ROUKFilter::computePrediction() { @@ -128,6 +144,8 @@ void ROUKFilter::computePrediction() } } + + template void ROUKFilter::computeSimplexPrediction() { @@ -136,7 +154,6 @@ void ROUKFilter::computeSimplexPrediction() //std::cout << "Compute star prediction" << std::endl; - //TIC EMatrixX tmpStateVarProj(stateSize, reducedStateSize); EMatrixX tmpStateVarProj2(stateSize, reducedStateSize); @@ -146,7 +163,6 @@ void ROUKFilter::computeSimplexPrediction() //std::cout << "State: " << vecX.transpose() << std::endl; Eigen::LLT lltU(matUinv); matUinv = lltU.matrixL(); - //TOC("== prediction: Cholesky "); sofa::helper::AdvancedTimer::stepEnd("Cholseky_prediction"); tmpStateVarProj = masterStateWrapper->getStateErrorVarianceProjector(); @@ -157,7 +173,6 @@ void ROUKFilter::computeSimplexPrediction() blasMultAdd(tmpStateVarProj, matUinv, tmpStateVarProj2, 1.0, 0.0); else tmpStateVarProj2.noalias() = tmpStateVarProj * matUinv; - //TOC("== prediction multiplication1 == "); //std::cout << "matUinv: " << matUinv << std::endl; //std::cout << "tmpStateVarProj: " << tmpStateVarProj.transpose() << std::endl; @@ -168,26 +183,20 @@ void ROUKFilter::computeSimplexPrediction() for (size_t i = 0; i < sigmaPointsNum; i++) matXi.col(i) = vecX; - //TIC - if (useBlasToMultiply.getValue()) blasMultAdd(tmpStateVarProj2, matI, matXi, 1.0, 1.0); else matXi = matXi + tmpStateVarProj2 * matI; - //TOC("== prediction multiplication2 == "); sofa::helper::AdvancedTimer::stepEnd("prediction_multiplication"); //std::cout << "\n matXi-\n " << matXi.transpose() << std::endl; //std::cout << "\n matI \n " << matI.transpose() << std::endl; //std::cout << "\n tmpStateVarProj2 \n " << tmpStateVarProj2.transpose() << std::endl; - TOCTIC(" =T= pred.pre sx") sofa::helper::AdvancedTimer::stepBegin("transform_state"); computePerturbedStates(vecX); //asumEVec("summPred",vecX); - //TOCTIC("== prediction compute perturbations =="); sofa::helper::AdvancedTimer::stepEnd("transform_state"); - TOCTIC(" =T= pred.sim sx") //std::cout << "\n matXi+\n " << matXi.transpose() << std::endl; @@ -196,7 +205,6 @@ void ROUKFilter::computeSimplexPrediction() blasMultAdd(matXi, matItrans, tmpStateVarProj2, alpha, 0.0); else tmpStateVarProj2 = alpha*matXi * matItrans; - //TOC("== prediction multiplication3 =="); //PRNS("\n errorVarProj \n " << matItrans.transpose()); //std::cout << "\n matItrans \n " << matItrans.transpose() << std::endl; @@ -207,16 +215,16 @@ void ROUKFilter::computeSimplexPrediction() masterStateWrapper->writeState(this->getTime()); sofa::helper::AdvancedTimer::stepEnd("ROUKFSimplexPrediction"); - //TOCTIC(" =T= pred.post sx") } + + template void ROUKFilter::computeStarPrediction() { //PRNS("Computing prediction, T= " << this->actualTime); sofa::helper::AdvancedTimer::stepBegin("ROUKFStarPrediction"); - //TIC EMatrixX tmpStateVarProj(stateSize, reducedStateSize); EMatrixX tmpStateVarProj2(stateSize, reducedStateSize); @@ -226,7 +234,6 @@ void ROUKFilter::computeStarPrediction() //std::cout << "State: " << vecX.transpose() << std::endl; Eigen::LLT lltU(matUinv); matUinv = lltU.matrixL(); - //TOC("== prediction: Cholesky "); sofa::helper::AdvancedTimer::stepEnd("Cholseky_prediction"); tmpStateVarProj = masterStateWrapper->getStateErrorVarianceProjector(); @@ -235,7 +242,6 @@ void ROUKFilter::computeStarPrediction() blasMultAdd(tmpStateVarProj, matUinv, tmpStateVarProj2, 1.0, 0.0); else tmpStateVarProj2.noalias() = tmpStateVarProj * matUinv; - //TOC("== prediction multiplication1 == "); //std::cout << "matUinv: " << matUinv << std::endl; //std::cout << "tmpStateVarProj: " << tmpStateVarProj.transpose() << std::endl; @@ -252,12 +258,10 @@ void ROUKFilter::computeStarPrediction() matXi = matXi + tmpStateVarProj2 * matI; sofa::helper::AdvancedTimer::stepEnd("prediction_multiplication"); - //TOCTIC("=T= pred.pre st") sofa::helper::AdvancedTimer::stepBegin("transform_state"); computePerturbedStates(vecX); //asumEVec("summPred",vecX); sofa::helper::AdvancedTimer::stepEnd("transform_state"); - //TOCTIC("=T= pred.sim st") //std::cout << "\n matItrans \n " << matItrans.transpose() << std::endl; //std::cout << "\n tmpStateVarProj2 \n " << tmpStateVarProj2.transpose() << std::endl; @@ -334,12 +338,12 @@ void ROUKFilter::computeStarPrediction() //std::cout << "\n tmpStateVarProj2_resampled \n " << tmpStateVarProj2.transpose() << std::endl; masterStateWrapper->setStateErrorVarianceProjector(tmpStateVarProj2); - TOC("=T= pred.post st") sofa::helper::AdvancedTimer::stepEnd("ROUKFStarPrediction"); } + template void ROUKFilter::computeCorrection() { @@ -353,6 +357,8 @@ void ROUKFilter::computeCorrection() } } + + template void ROUKFilter::computeSimplexCorrection() { @@ -365,7 +371,6 @@ void ROUKFilter::computeSimplexCorrection() } if (observationManager->hasObservation(this->actualTime)) { - //TIC EVectorX vecXCol; EVectorX vecZCol(observationSize), vecZ(observationSize); EMatrixX matZItrans(sigmaPointsNum, observationSize); @@ -382,7 +387,6 @@ void ROUKFilter::computeSimplexCorrection() vecZ = vecZ + alpha * vecZCol; matZItrans.row(i) = vecZCol; } - //TOCTIC("== an1sx == "); sofa::helper::AdvancedTimer::stepEnd("Innovation"); //asumEVec("correction accumInnov",vecZ); @@ -409,9 +413,7 @@ void ROUKFilter::computeSimplexCorrection() matWorkingPO = matHLtrans * matR.inverse(); } matTemp = EMatrixX::Identity(matUinv.rows(), matUinv.cols()) + matWorkingPO * matHLtrans.transpose(); - //TOCTIC("== an3sx == "); matUinv = matTemp.inverse(); - //TOCTIC("== an4sx == (inv) "); EVectorX reducedInnovation(reducedStateSize); reducedInnovation = Type(-1.0) * matUinv*matWorkingPO*vecZ; @@ -468,13 +470,13 @@ void ROUKFilter::computeSimplexCorrection() EMatrixX covarianceMatrix(stateSize, stateSize); covarianceMatrix = errorVarProj * matUinv * errorVarProj.transpose(); - helper::WriteAccessor > > redState = reducedState; - helper::WriteAccessor > > redVar = reducedVariance; - helper::WriteAccessor > > redCovar = reducedCovariance; - helper::WriteAccessor > > innov = d_reducedInnovation; - helper::WriteAccessor > > mstate = d_state; - helper::WriteAccessor > > var = d_variance; - helper::WriteAccessor > > covar = d_covariance; + helper::WriteAccessor > > redState = reducedState; + helper::WriteAccessor > > redVar = reducedVariance; + helper::WriteAccessor > > redCovar = reducedCovariance; + helper::WriteAccessor > > innov = d_reducedInnovation; + helper::WriteAccessor > > mstate = d_state; + helper::WriteAccessor > > var = d_variance; + helper::WriteAccessor > > covar = d_covariance; redState.resize(reducedStateSize); redVar.resize(reducedStateSize); @@ -505,7 +507,6 @@ void ROUKFilter::computeSimplexCorrection() for (size_t index = 0; index < observationSize; index++) { innov[index] = vecZ[index]; } - TOC( "=T= corr sx") /*char fileName[100]; sprintf(fileName, "outVar/parL_%03d.txt", this->stepNumber); @@ -524,7 +525,6 @@ void ROUKFilter::computeSimplexCorrection() cvmFile << reducedCovarianceMatrix << std::endl; cvmFile.close();*/ - //TOC("== an5sx == "); /*Type maxState = 0.0, minState = 1e10; for (size_t i = stateSize - reducedStateSize; i < stateSize; i++) { maxState = (state(i) > maxState) ? state(i) : maxState; @@ -538,6 +538,8 @@ void ROUKFilter::computeSimplexCorrection() sofa::helper::AdvancedTimer::stepEnd("ROUKFSimplexCorrection"); } + + template void ROUKFilter::computeStarCorrection() { @@ -550,7 +552,6 @@ void ROUKFilter::computeStarCorrection() } if (observationManager->hasObservation(this->actualTime)) { - //TIC EVectorX vecXCol; EVectorX vecZCol(observationSize), vecZ(observationSize); EMatrixX matZItrans(sigmaPointsNum, observationSize); @@ -566,7 +567,6 @@ void ROUKFilter::computeStarCorrection() vecZ = vecZ + alpha * vecZCol; matZItrans.row(i) = vecZCol; } - //TOCTIC("== an1sx == "); sofa::helper::AdvancedTimer::stepEnd("Innovation"); //std::cout << "matZItrans-: " << matZItrans << std::endl; //std::cout << "vecZ: " << vecZ.transpose() << std::endl; @@ -631,9 +631,7 @@ void ROUKFilter::computeStarCorrection() //std::cout << "workingMatrixRP: " << workingMatrixRP << std::endl; matTemp = EMatrixX::Identity(matUinv.rows(), matUinv.cols()) + alphaVar * matItrans.transpose() * workingMatrixRP; - //TOCTIC("== an3sx == "); matUinv = matTemp.inverse(); - //TOCTIC("== an4sx == (inv) "); // Compute {HL}_{n+1} workingMatrixRR2 = EMatrixX::Identity(workingMatrixRR2.rows(), workingMatrixRR2.cols()) + matDv * workingMatrixRR; @@ -693,10 +691,10 @@ void ROUKFilter::computeStarCorrection() EMatrixX reducedCovarianceMatrix(reducedStateSize, reducedStateSize); reducedCovarianceMatrix = parErrorVarProj * matUinv * parErrorVarProj.transpose(); - helper::WriteAccessor > > redState = reducedState; - helper::WriteAccessor > > redVar = reducedVariance; - helper::WriteAccessor > > redCovar = reducedCovariance; - helper::WriteAccessor > > innov = d_reducedInnovation; + helper::WriteAccessor > > redState = reducedState; + helper::WriteAccessor > > redVar = reducedVariance; + helper::WriteAccessor > > redCovar = reducedCovariance; + helper::WriteAccessor > > innov = d_reducedInnovation; redState.resize(reducedStateSize); redVar.resize(reducedStateSize); @@ -715,7 +713,6 @@ void ROUKFilter::computeStarCorrection() for (size_t index = 0; index < observationSize; index++) { innov[index] = vecZ[index]; } - TOC ("=T= corr st") //char fileName[100]; //sprintf(fileName, "outVar/parL_%03d.txt", this->stepNumber); @@ -734,7 +731,6 @@ void ROUKFilter::computeStarCorrection() //cvmFile << reducedCovarianceMatrix << std::endl; //cvmFile.close(); - //TOC("== an5sx == "); //Type maxState = 0.0, minState = 1e10; //for (size_t i = stateSize - reducedStateSize; i < stateSize; i++) { // maxState = (state(i) > maxState) ? state(i) : maxState; @@ -748,8 +744,11 @@ void ROUKFilter::computeStarCorrection() sofa::helper::AdvancedTimer::stepEnd("ROUKFStarCorrection"); } + + template -void ROUKFilter::init() { +void ROUKFilter::init() +{ Inherit::init(); assert(this->gnode); this->gnode->template get >(&stateWrappers, this->getTags(), sofa::core::objectmodel::BaseContext::SearchDown); @@ -779,17 +778,21 @@ void ROUKFilter::init() { PRNS("number of slave wrappers: " << numThreads-1); /// slaves + master } - numThreads=numSlaveWrappers+numMasterWrappers; + numThreads = numSlaveWrappers + numMasterWrappers; this->gnode->get(observationManager, core::objectmodel::BaseContext::SearchDown); if (observationManager) { PRNS("found observation manager: " << observationManager->getName()); - } else + } else { PRNE("no observation manager found!"); + } } + + template -void ROUKFilter::bwdInit() { +void ROUKFilter::bwdInit() +{ PRNS("bwdInit"); assert(masterStateWrapper); @@ -843,7 +846,7 @@ void ROUKFilter::bwdInit() { //std::cout << "Matrix verification: " << matI * matItrans << std::endl; matDv.resize(sigmaPointsNum, sigmaPointsNum); - matDv = alphaVar*alphaVar*matItrans*matI; + matDv = alphaVar * alphaVar * matItrans * matI; matXi.resize(stateSize, sigmaPointsNum); @@ -851,7 +854,7 @@ void ROUKFilter::bwdInit() { sigmaPoints2WrapperIDs.resize(sigmaPointsNum, 0); wrapper2SigmaPointsIDs.resize(numThreads); for (size_t i = 0; i < sigmaPointsNum; i++) { - size_t threadID = i%(numThreads); + size_t threadID = i % (numThreads); sigmaPoints2WrapperIDs[i] = threadID; wrapper2SigmaPointsIDs[threadID].push_back(i); } @@ -860,12 +863,12 @@ void ROUKFilter::bwdInit() { /// export initial stochastic state - helper::WriteAccessor > > redState = reducedState; - helper::WriteAccessor > > redVar = reducedVariance; - helper::WriteAccessor > > redCovar = reducedCovariance; - helper::WriteAccessor > > mstate = d_state; - helper::WriteAccessor > > var = d_variance; - helper::WriteAccessor > > covar = d_covariance; + helper::WriteAccessor > > redState = reducedState; + helper::WriteAccessor > > redVar = reducedVariance; + helper::WriteAccessor > > redCovar = reducedCovariance; + helper::WriteAccessor > > mstate = d_state; + helper::WriteAccessor > > var = d_variance; + helper::WriteAccessor > > covar = d_covariance; EVectorX state = masterStateWrapper->getState(); EMatrixX errorVarProj = masterStateWrapper->getStateErrorVarianceProjector(); @@ -906,13 +909,15 @@ void ROUKFilter::bwdInit() { } } - - //std::cout << "SigmaWrapper: " << sigmaPoints2WrapperIDs << std::endl; - //std::cout << "WrapperSigma: " << wrapper2SigmaPointsIDs << std::endl; + // std::cout << "SigmaWrapper: " << sigmaPoints2WrapperIDs << std::endl; + // std::cout << "WrapperSigma: " << wrapper2SigmaPointsIDs << std::endl; } + + template -void ROUKFilter::initializeStep(const core::ExecParams* _params, const size_t _step) { +void ROUKFilter::initializeStep(const core::ExecParams* _params, const size_t _step) +{ Inherit::initializeStep(_params, _step); if (initialiseObservationsAtFirstStep.getValue()) { @@ -927,8 +932,11 @@ void ROUKFilter::initializeStep(const core::ExecParams* _params, con observationManager->initializeStep(stepNumber); } + + template -void ROUKFilter::computeSimplexSigmaPoints(EMatrixX& sigmaMat) { +void ROUKFilter::computeSimplexSigmaPoints(EMatrixX& sigmaMat) +{ size_t p = reducedStateSize; size_t r = reducedStateSize + 1; @@ -937,16 +945,16 @@ void ROUKFilter::computeSimplexSigmaPoints(EMatrixX& sigmaMat) { Type scal, beta, sqrt_p; beta = Type(p) / Type(p+1); sqrt_p = sqrt(Type(p)); - scal = Type(1.0)/sqrt(Type(2) * beta); - workingMatrix(0,0) = -scal; - workingMatrix(0,1) = scal; + scal = Type(1.0) / sqrt(Type(2) * beta); + workingMatrix(0, 0) = -scal; + workingMatrix(0, 1) = scal; for (size_t i = 1; i < p; i++) { scal = Type(1.0) / sqrt(beta * Type(i+1) * Type(i+2)); - for (size_t j = 0; j < i+1; j++) - workingMatrix(i,j) = scal; - workingMatrix(i,i+1) = -Type(i+1) * scal; + for (size_t j = 0; j < i + 1; j++) + workingMatrix(i, j) = scal; + workingMatrix(i, i+1) = -Type(i+1) * scal; } sigmaMat.resize(r,p); @@ -954,7 +962,7 @@ void ROUKFilter::computeSimplexSigmaPoints(EMatrixX& sigmaMat) { sigmaMat.row(i) = workingMatrix.col(i) * sqrt_p; vecAlpha.resize(r); - vecAlpha.fill(Type(1.0)/Type(r)); + vecAlpha.fill(Type(1.0) / Type(r)); alphaConstant = true; alpha = vecAlpha(0); @@ -962,10 +970,12 @@ void ROUKFilter::computeSimplexSigmaPoints(EMatrixX& sigmaMat) { vecAlphaVar.resize(r); vecAlphaVar.fill(alphaVar); - //PRNS("sigmaMat: \n" << sigmaMat); - //PRNS("vecAlphaVar: \n" << vecAlphaVar); + // PRNS("sigmaMat: \n" << sigmaMat); + // PRNS("vecAlphaVar: \n" << vecAlphaVar); } + + template void ROUKFilter::computeStarSigmaPoints(EMatrixX& sigmaMat) { size_t p = reducedStateSize; @@ -977,17 +987,17 @@ void ROUKFilter::computeStarSigmaPoints(EMatrixX& sigmaMat) { lambda = this->lambdaScale.getValue(); ///// FIX: make coefficients the same for all sigma points - lambda = 0.5; + // lambda = 0.5; ///// FIX // PRNS("lambda: \n" << lambda); sqrt_vec = sqrt(p + lambda); for (size_t j = 0; j < p; j++) { - workingMatrix(j,j) = sqrt_vec; + workingMatrix(j, j) = sqrt_vec; } for (size_t j = p; j < 2 * p; j++) { - workingMatrix(2*p-j-1,j) = -sqrt_vec; + workingMatrix(2 * p - j - 1, j) = -sqrt_vec; } sigmaMat.resize(r,p); @@ -996,45 +1006,45 @@ void ROUKFilter::computeStarSigmaPoints(EMatrixX& sigmaMat) { vecAlpha.resize(r); for (size_t i = 0; i < 2 * p; i++) { - vecAlpha(i) = Type(1.0)/Type(2 * (p + lambda)); + vecAlpha(i) = Type(1.0) / Type(2 * (p + lambda)); } - vecAlpha(2 * p) = Type(lambda)/Type(p + lambda); + vecAlpha(2 * p) = Type(lambda) / Type(p + lambda); alphaConstant = true; alpha = vecAlpha(0); vecAlphaVar.resize(r); if (this->useUnbiasedVariance.getValue()) { for (size_t i = 0; i < 2 * p; i++) { - vecAlphaVar(i) = Type(1.0)/Type(2 * (p + lambda) - 1); + vecAlphaVar(i) = Type(1.0) / Type(2 * (p + lambda) - 1); } // double beta = 2.0; - vecAlphaVar(2 * p) = Type(lambda)/Type(p + lambda); + vecAlphaVar(2 * p) = Type(lambda) / Type(p + lambda); - alphaVar = Type(1.0)/Type(2 * (p + lambda) - 1); + alphaVar = Type(1.0) / Type(2 * (p + lambda) - 1); } else { + for (size_t i = 0; i < 2 * p; i++) { - vecAlphaVar(i) = Type(1.0)/Type(2 * (p + lambda)); + vecAlphaVar(i) = Type(1.0) / Type(2 * (p + lambda)); } // double beta = 2.0; - vecAlphaVar(2 * p) = Type(lambda)/Type(p + lambda); + vecAlphaVar(2 * p) = Type(lambda) / Type(p + lambda); - alphaVar = Type(1.0)/Type(2 * (p + lambda)); + alphaVar = Type(1.0) / Type(2 * (p + lambda)); } - //PRNS("sigmaMat: \n" << sigmaMat); - //PRNS("vecAlphaVar: \n" << vecAlphaVar); + // PRNS("sigmaMat: \n" << sigmaMat); + // PRNS("vecAlphaVar: \n" << vecAlphaVar); } -//template -//void ROUKFilter::init() -//{ +// template +// void ROUKFilter::init() +// { } // -//} -// -//template -//void ROUKFilter::reinit() -//{ -//} +// template +// void ROUKFilter::reinit() +// { } + + } // stochastic diff --git a/src/stochasticFiltering/SimpleObservationManager.cpp b/src/stochasticFiltering/SimpleObservationManager.cpp index 0fb48109..bac2f4ee 100644 --- a/src/stochasticFiltering/SimpleObservationManager.cpp +++ b/src/stochasticFiltering/SimpleObservationManager.cpp @@ -26,6 +26,8 @@ #include "SimpleObservationManager.inl" //#include + + namespace sofa { @@ -35,9 +37,10 @@ namespace component namespace stochastic { -using namespace defaulttype; +using namespace defaulttype; + SOFA_DECL_CLASS(SimpleObservationManager) // Register in the Factory diff --git a/src/stochasticFiltering/SimpleObservationManager.h b/src/stochasticFiltering/SimpleObservationManager.h index bfebfc01..f0ecf810 100644 --- a/src/stochasticFiltering/SimpleObservationManager.h +++ b/src/stochasticFiltering/SimpleObservationManager.h @@ -36,17 +36,21 @@ #include "../genericComponents/SimulatedStateObservationSource.h" #include "StochasticStateWrapper.h" + + namespace sofa { + namespace component { + namespace stochastic { -using namespace defaulttype; + template -class SimpleObservationManager: public sofa::component::stochastic::ObservationManager +class SimpleObservationManager : public sofa::component::stochastic::ObservationManager { public: SOFA_CLASS(SOFA_TEMPLATE3(SimpleObservationManager, FilterType, DataTypes1, DataTypes2), SOFA_TEMPLATE(ObservationManager, FilterType)); @@ -58,44 +62,38 @@ class SimpleObservationManager: public sofa::component::stochastic::ObservationM typedef typename Eigen::Matrix EVectorX; typedef typename DataTypes1::Real Real1; + typedef typename DataTypes2::VecCoord VecCoord; + typedef typename DataTypes2::VecDeriv VecDeriv; + typedef core::behavior::MechanicalState MasterState; typedef sofa::component::container::SimulatedStateObservationSource ObservationSource; typedef StochasticStateWrapper StateWrapper; ///Before it was DataTypes1 + typename DataTypes1::VecCoord realObservations; + typename type::vector< VecCoord > modelObservations; - typedef typename DataTypes2::VecCoord VecCoord; - typedef typename DataTypes2::VecDeriv VecDeriv; + Data< sofa::type::Mat3x4d > d_projectionMatrix; + SingleLink, StateWrapper, BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> stateWrapperLink; - SimpleObservationManager(); - virtual ~SimpleObservationManager() override {} - protected: - MasterState* masterState; - ObservationSource *observationSource; + ObservationSource* observationSource; StateWrapper* stateWrapper; - double actualObservationTime; - - public: + SimpleObservationManager(); + virtual ~SimpleObservationManager() override {} + void init() override; void bwdInit() override; - virtual bool hasObservation(double _time) override; /// TODO + virtual bool hasObservation(double _time) override; virtual bool getInnovation(double _time, EVectorX& _state, EVectorX& _innovation) override; virtual bool getRealObservation(double _time, EVectorX& _realObs) override; virtual bool obsFunction(EVectorX& _state, EVectorX& _predictedObservation) override; virtual bool getPredictedObservation(int _id, EVectorX& _predictedObservation) override; - typename DataTypes1::VecCoord realObservations; - typename helper::vector< VecCoord > modelObservations; - - Data d_projectionMatrix; - - SingleLink, StateWrapper, BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> stateWrapperLink; - /// Pre-construction check method called by ObjectFactory. /// Check that DataTypes matches the MechanicalState. template @@ -114,14 +112,14 @@ class SimpleObservationManager: public sofa::component::stochastic::ObservationM { return DataTypes1::Name()+ std::string(",") + DataTypes2::Name(); } +}; -}; /// class +extern template class SOFA_STOCHASTIC_API SimpleObservationManager; +extern template class SOFA_STOCHASTIC_API SimpleObservationManager; +extern template class SOFA_STOCHASTIC_API SimpleObservationManager; +extern template class SOFA_STOCHASTIC_API SimpleObservationManager; -extern template class SOFA_STOCHASTIC_API SimpleObservationManager; -extern template class SOFA_STOCHASTIC_API SimpleObservationManager; -extern template class SOFA_STOCHASTIC_API SimpleObservationManager; -extern template class SOFA_STOCHASTIC_API SimpleObservationManager; } // stochastic diff --git a/src/stochasticFiltering/SimpleObservationManager.inl b/src/stochasticFiltering/SimpleObservationManager.inl index b1a9aac2..2463b12f 100644 --- a/src/stochasticFiltering/SimpleObservationManager.inl +++ b/src/stochasticFiltering/SimpleObservationManager.inl @@ -22,10 +22,10 @@ #pragma once #include - #include "SimpleObservationManager.h" + namespace sofa { @@ -35,24 +35,21 @@ namespace component namespace stochastic { + + template -SimpleObservationManager::SimpleObservationManager() +SimpleObservationManager::SimpleObservationManager() : Inherit() - , d_projectionMatrix( initData(&d_projectionMatrix, Mat3x4d(defaulttype::Vec<4,float>(1.0,0.0,0.0,0.0), - defaulttype::Vec<4,float>(0.0,1.0,0.0,0.0), - defaulttype::Vec<4,float>(0.0,0.0,1.0,0.0)), "projectionMatrix","Projection matrix")) + , d_projectionMatrix( initData(&d_projectionMatrix, sofa::type::Mat3x4d(type::Vec<4,float>(1.0,0.0,0.0,0.0), + type::Vec<4,float>(0.0,1.0,0.0,0.0), + type::Vec<4,float>(0.0,0.0,1.0,0.0)), "projectionMatrix", "Projection matrix")) , stateWrapperLink(initLink("stateWrapper", "link to the state wrapper needed to perform apply (perhaps to be changed)")) -{ -} - - - - +{ } template -void SimpleObservationManager::init() +void SimpleObservationManager::init() { Inherit::init(); @@ -60,64 +57,67 @@ void SimpleObservationManager::init() if (observationSource) { PRNS("Found observation source: " << observationSource->getName()); } else { - serr <<"No observation source found!"<getName()<getName() << std::endl; } else { - std::cout<<"[SimpleObservationManager] Link to state wrapper not initialized!"<gnode->get(masterState); if (masterState != NULL) { PRNS("Found master mechanical state: " << masterState->getName()); - } - else { + } else { PRNE("No master mechanical state found!"); } - } + + template -void SimpleObservationManager::bwdInit() +void SimpleObservationManager::bwdInit() { this->observationSize = observationSource->getStateSize() * DataTypes1::spatial_dimensions; Inherit::bwdInit(); - } + + template -bool SimpleObservationManager::hasObservation(double _time) { -bool hasObservation; - if(this->actualTime==0){ - hasObservation=true; - } else{ +bool SimpleObservationManager::hasObservation(double _time) +{ + bool hasObservation; + if (this->actualTime == 0) { + hasObservation = true; + } else { + hasObservation = observationSource->getObservation(this->actualTime, realObservations); + } - hasObservation= observationSource->getObservation(this->actualTime, realObservations); -} if (!hasObservation) { PRNE("No observation for time " << _time); - return(false); + return (false); } - return(true); + return (true); } + + template -bool SimpleObservationManager::getInnovation(double _time, EVectorX& _state, EVectorX& _innovation) +bool SimpleObservationManager::getInnovation(double _time, EVectorX& _state, EVectorX& _innovation) { - if (_time != this->actualTime) { PRNE("Observation for time " << this->actualTime << " not prepared, call hasObservation first!"); - return(false); + return (false); } if (_innovation.rows() != long(this->observationSize)) { PRNE("Wrong innovation size: " << _innovation.rows() << " should be " << this->observationSize); - return(false); + return (false); } if ((stateWrapper->getFilterKind() == SIMCORR) || (stateWrapper->getFilterKind() == CLASSIC) || (stateWrapper->getFilterKind() == LOCENSEMBLE)) { @@ -125,43 +125,43 @@ bool SimpleObservationManager::getInnovation(d _innovation(i) = realObservations[0](i) - _state(i); } - return true; + return (true); +} -} template -bool SimpleObservationManager::getRealObservation(double _time, EVectorX& _realObs) +bool SimpleObservationManager::getRealObservation(double _time, EVectorX& _realObs) { - if (_time != this->actualTime) { PRNE("Observation for time " << this->actualTime << " not prepared, call hasObservation first!"); - return(false); + return (false); } if (_realObs.rows() != long(this->observationSize)) { PRNE("Wrong innovation size: " << _realObs.rows() << " should be " << this->observationSize); - return(false); + return (false); } if ((stateWrapper->getFilterKind() == SIMCORR) || (stateWrapper->getFilterKind() == CLASSIC) || (stateWrapper->getFilterKind() == LOCENSEMBLE)) { - for (size_t i = 0; i < this->observationSize; i++) - _realObs(i) = realObservations[0](i); + for (size_t i = 0; i < this->observationSize; i++) { + _realObs(i) = realObservations[0](i); + } } return true; +} + -} template -bool SimpleObservationManager::obsFunction(EVectorX& _state, EVectorX& _predictedObservation) +bool SimpleObservationManager::obsFunction(EVectorX& _state, EVectorX& _predictedObservation) { - const Mat3x4d & P = d_projectionMatrix.getValue(); + const sofa::type::Mat3x4d& P = d_projectionMatrix.getValue(); _predictedObservation.resize(this->observationSize); - Data predicted2DState; - Data predicted3DState; - + Data< typename DataTypes1::VecCoord > predicted2DState; + Data< typename DataTypes2::VecCoord > predicted3DState; typename DataTypes1::VecCoord& predicted2DStateEdit = *predicted2DState.beginEdit(); typename DataTypes2::VecCoord& predicted3DStateEdit = *predicted3DState.beginEdit(); @@ -172,19 +172,20 @@ bool SimpleObservationManager::obsFunction(EVe predicted2DStateEdit.resize(predicted3DStateEdit.size()); //2D observations - if(ObservationSource::Coord::total_size == 2) + if (ObservationSource::Coord::total_size == 2) { for (unsigned i = 0; i < predicted3DStateEdit.size(); i++) { double rx = P[0][0] * predicted3DStateEdit[i][0] + P[0][1] * predicted3DStateEdit[i][1] + P[0][2] * predicted3DStateEdit[i][2] + P[0][3]; double ry = P[1][0] * predicted3DStateEdit[i][0] + P[1][1] * predicted3DStateEdit[i][1] + P[1][2] * predicted3DStateEdit[i][2] + P[1][3]; double rz = P[2][0] * predicted3DStateEdit[i][0] + P[2][1] * predicted3DStateEdit[i][1] + P[2][2] * predicted3DStateEdit[i][2] + P[2][3]; - predicted2DStateEdit[i][0]=rx* (1.0/rz); - predicted2DStateEdit[i][1]=ry* (1.0/rz); + predicted2DStateEdit[i][0] = rx * (1.0 / rz); + predicted2DStateEdit[i][1] = ry * (1.0 / rz); } + for (size_t i = 0; i < predicted3DStateEdit.size(); i++) { - for (size_t d = 0; d < 2; d++){ + for (size_t d = 0; d < 2; d++) { _predictedObservation(2*i+d) = predicted2DStateEdit[i][d]; } } @@ -193,25 +194,25 @@ bool SimpleObservationManager::obsFunction(EVe else // 3D Observations { for (size_t i = 0; i < predicted3DStateEdit.size(); i++) + { for (size_t d = 0; d < 3; d++) _predictedObservation(3*i+d) = predicted3DStateEdit[i][d]; - + } } - return true; - + return (true); } + template -bool SimpleObservationManager::getPredictedObservation(int _id, EVectorX& _predictedObservation) +bool SimpleObservationManager::getPredictedObservation(int _id, EVectorX& _predictedObservation) { - const Mat3x4d & P = d_projectionMatrix.getValue(); + const sofa::type::Mat3x4d & P = d_projectionMatrix.getValue(); _predictedObservation.resize(this->observationSize); - Data predicted2DState; - Data predicted3DState; - + Data< typename DataTypes1::VecCoord > predicted2DState; + Data< typename DataTypes2::VecCoord > predicted3DState; typename DataTypes1::VecCoord& predicted2DStateEdit = *predicted2DState.beginEdit(); typename DataTypes2::VecCoord& predicted3DStateEdit = *predicted3DState.beginEdit(); @@ -229,28 +230,25 @@ bool SimpleObservationManager::getPredictedObs double rx = P[0][0] * predicted3DStateEdit[i][0] + P[0][1] * predicted3DStateEdit[i][1] + P[0][2] * predicted3DStateEdit[i][2] + P[0][3]; double ry = P[1][0] * predicted3DStateEdit[i][0] + P[1][1] * predicted3DStateEdit[i][1] + P[1][2] * predicted3DStateEdit[i][2] + P[1][3]; double rz = P[2][0] * predicted3DStateEdit[i][0] + P[2][1] * predicted3DStateEdit[i][1] + P[2][2] * predicted3DStateEdit[i][2] + P[2][3]; - predicted2DStateEdit[i][0]=rx* (1.0/rz); - predicted2DStateEdit[i][1]=ry* (1.0/rz); + predicted2DStateEdit[i][0] = rx * (1.0 / rz); + predicted2DStateEdit[i][1] = ry * (1.0 / rz); } + for (size_t i = 0; i < predicted3DStateEdit.size(); i++) { - for (size_t d = 0; d < 2; d++) - { - _predictedObservation(2*i+d) = predicted2DStateEdit[i][d]; + for (size_t d = 0; d < 2; d++) { + _predictedObservation(2 * i + d) = predicted2DStateEdit[i][d]; } } - - } - // 3D Observations - else{ + } else { + // 3D Observations for (size_t i = 0; i < predicted3DStateEdit.size(); i++) + { for (size_t d = 0; d < 3; d++) - _predictedObservation(3*i+d) = predicted3DStateEdit[i][d]; - + _predictedObservation(3 * i + d) = predicted3DStateEdit[i][d]; + } } - return true; - - + return (true); } diff --git a/src/stochasticFiltering/SimpleUncorrespondentObservationManager.cpp b/src/stochasticFiltering/SimpleUncorrespondentObservationManager.cpp index 8f89dfc8..c5a4e466 100644 --- a/src/stochasticFiltering/SimpleUncorrespondentObservationManager.cpp +++ b/src/stochasticFiltering/SimpleUncorrespondentObservationManager.cpp @@ -26,6 +26,8 @@ #include "SimpleUncorrespondentObservationManager.inl" //#include + + namespace sofa { @@ -35,9 +37,10 @@ namespace component namespace stochastic { -using namespace defaulttype; +using namespace defaulttype; + SOFA_DECL_CLASS(SimpleUncorrespondentObservationManager) // Register in the Factory @@ -50,6 +53,7 @@ template class SOFA_STOCHASTIC_API SimpleUncorrespondentObservationManager; + } // namespace stochastic } // namespace component diff --git a/src/stochasticFiltering/SimpleUncorrespondentObservationManager.h b/src/stochasticFiltering/SimpleUncorrespondentObservationManager.h index 835e808e..63d648cd 100644 --- a/src/stochasticFiltering/SimpleUncorrespondentObservationManager.h +++ b/src/stochasticFiltering/SimpleUncorrespondentObservationManager.h @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -47,10 +48,9 @@ namespace stochastic { -using namespace defaulttype; template -class SimpleUncorrespondentObservationManager: public sofa::component::stochastic::ObservationManager +class SimpleUncorrespondentObservationManager : public sofa::component::stochastic::ObservationManager { public: SOFA_CLASS(SOFA_TEMPLATE3(SimpleUncorrespondentObservationManager, FilterType, DataTypes1, DataTypes2), SOFA_TEMPLATE(ObservationManager, FilterType)); @@ -62,15 +62,29 @@ class SimpleUncorrespondentObservationManager: public sofa::component::stochasti typedef typename Eigen::Matrix EVectorX; typedef typename DataTypes1::Real Real1; - typedef helper::vector VecIndex; + typedef type::vector VecIndex; typedef core::behavior::MechanicalState MasterState; typedef core::behavior::MechanicalState MappedState; typedef sofa::core::Mapping Mapping; typedef sofa::component::container::SimulatedStateObservationSource ObservationSource; typedef StochasticStateWrapper StateWrapper; - SimpleUncorrespondentObservationManager(); - ~SimpleUncorrespondentObservationManager() {} + Data< typename DataTypes1::VecCoord > inputObservationData; + Data< typename DataTypes2::VecCoord > mappedObservationData; + Data< VecIndex > inputIndices; + Data< VecIndex > mappedMask; + Data< double > noiseStdev; + Data< int > abberantIndex; + Data< type::vector > d_observationIndices; + type::vector observationIndices; + + SingleLink, StateWrapper, BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> stateWrapperLink; + + boost::mt19937* pRandGen; // I don't seed it on purpouse (it's not relevant) + boost::normal_distribution<>* pNormDist; + boost::variate_generator >* pVarNorm; + type::vector noise; + protected: size_t inputVectorSize, masterVectorSize, mappedVectorSize; /// real sizes of vectors @@ -87,34 +101,20 @@ class SimpleUncorrespondentObservationManager: public sofa::component::stochasti public: + SimpleUncorrespondentObservationManager(); + ~SimpleUncorrespondentObservationManager() {} + void init() override; void bwdInit() override; void initializeObservationData(); - virtual bool hasObservation(double _time) override; /// TODO + virtual bool hasObservation(double _time) override; virtual bool getInnovation(double _time, EVectorX& _state, EVectorX& _innovation) override; virtual bool getRealObservation(double /* _time */, EVectorX& /* _realObs */) override; virtual bool getPredictedObservation(int _id, EVectorX& _predictedObservation) override; virtual bool obsFunction(EVectorX& /* _state */, EVectorX& /* _predictedObservation */) override; +}; - Data inputObservationData; - Data inputIndices; - Data mappedObservationData; - Data mappedMask; - Data noiseStdev; - Data abberantIndex; - Data > d_observationIndices; - helper::vector observationIndices; - - - SingleLink, StateWrapper, BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> stateWrapperLink; - - boost::mt19937* pRandGen; // I don't seed it on purpouse (it's not relevant) - boost::normal_distribution<>* pNormDist; - boost::variate_generator >* pVarNorm; - helper::vector noise; - -}; /// class } // namespace stochastic @@ -122,3 +122,4 @@ class SimpleUncorrespondentObservationManager: public sofa::component::stochasti } // namespace component } // namespace sofa + diff --git a/src/stochasticFiltering/SimpleUncorrespondentObservationManager.inl b/src/stochasticFiltering/SimpleUncorrespondentObservationManager.inl index 4ea0d093..0a596e06 100644 --- a/src/stochasticFiltering/SimpleUncorrespondentObservationManager.inl +++ b/src/stochasticFiltering/SimpleUncorrespondentObservationManager.inl @@ -22,10 +22,10 @@ #pragma once #include - #include "SimpleUncorrespondentObservationManager.h" + namespace sofa { @@ -35,21 +35,24 @@ namespace component namespace stochastic { + + template SimpleUncorrespondentObservationManager::SimpleUncorrespondentObservationManager() : Inherit() , inputObservationData( initData (&inputObservationData, "observations", "observations read from a file") ) - , inputIndices( initData (&inputIndices, "indices", "indices read from a file") ) , mappedObservationData( initData (&mappedObservationData, "mappedObservations", "mapped observations") ) + , inputIndices( initData (&inputIndices, "indices", "indices read from a file") ) , noiseStdev( initData(&noiseStdev, double(0.0), "noiseStdev", "standard deviation of generated noise") ) , abberantIndex( initData(&abberantIndex, int(-1), "abberantIndex", "index of an aberrant point") ) , d_observationIndices( initData(&d_observationIndices, "observationIndices", "take these indices from vector of observations (implies not using mapping)") ) , stateWrapperLink(initLink("stateWrapper", "link to the state wrapper needed to perform apply (perhaps to be changed)")) -{ -} +{ } + + template -void SimpleUncorrespondentObservationManager::init() +void SimpleUncorrespondentObservationManager::init() { Inherit::init(); @@ -103,8 +106,9 @@ void SimpleUncorrespondentObservationManager:: } + template -void SimpleUncorrespondentObservationManager::bwdInit() +void SimpleUncorrespondentObservationManager::bwdInit() { if (!Inherit::initialiseObservationsAtFirstStep.getValue()) { initializeObservationData(); @@ -113,6 +117,8 @@ void SimpleUncorrespondentObservationManager:: Inherit::bwdInit(); } + + template void SimpleUncorrespondentObservationManager::initializeObservationData() { @@ -155,6 +161,8 @@ void SimpleUncorrespondentObservationManager Inherit::initializeObservationData(); } + + template bool SimpleUncorrespondentObservationManager::hasObservation(double _time) { if (Inherit::initialiseObservationsAtFirstStep.getValue()) { @@ -204,10 +212,10 @@ bool SimpleUncorrespondentObservationManager } -template -bool SimpleUncorrespondentObservationManager::getPredictedObservation(int _id, EVectorX& _predictedObservation) { - +template +bool SimpleUncorrespondentObservationManager::getPredictedObservation(int _id, EVectorX& _predictedObservation) +{ Data predictedMasterState; Data predictedMappedState; @@ -222,22 +230,27 @@ bool SimpleUncorrespondentObservationManager sofa::core::MechanicalParams mp; //sofa::helper::WriteAccessor< Data > masterState = predictedMasterState; - mapping->apply(&mp, predictedMappedState, predictedMasterState); _predictedObservation.resize(this->observationSize); - for (size_t i = 0; i < predictedMappedStateEdit.size(); i++) + for (size_t i = 0; i < predictedMappedStateEdit.size(); i++) { for (size_t d = 0; d < DataTypes1::spatial_dimensions; d++) _predictedObservation(DataTypes1::spatial_dimensions * i + d) = predictedMappedStateEdit[i][d]; + } return true; } + + + template bool SimpleUncorrespondentObservationManager::obsFunction(EVectorX& /* _state */, EVectorX& /* _predictedObservation */) { return 0; } + + template bool SimpleUncorrespondentObservationManager::getRealObservation(double /* _time */, EVectorX& /* _realObs */) { @@ -245,6 +258,7 @@ bool SimpleUncorrespondentObservationManager } + template bool SimpleUncorrespondentObservationManager::getInnovation(double _time, EVectorX& _state, EVectorX& _innovation) { @@ -287,7 +301,6 @@ bool SimpleUncorrespondentObservationManager return (true); - /* //std::cout << this->getName() << ": size of mapped state: " << mappedState.size() << std::endl; this->innovation_.Reallocate(mappedState.size()*DataTypes1::spatial_dimensions); @@ -320,21 +333,18 @@ bool SimpleUncorrespondentObservationManager return this->innovation_; */ - } -//template -//void MappedStateUncorrespondentObservationManager::init() -//{ -// -//} +// template +// void MappedStateUncorrespondentObservationManager::init() +// { } // -//template -//void MappedStateUncorrespondentObservationManager::reinit() -//{ -//} +// template +// void MappedStateUncorrespondentObservationManager::reinit() +// { } + } // namespace stochastic diff --git a/src/stochasticFiltering/StochasticFilterBase.h b/src/stochasticFiltering/StochasticFilterBase.h index 8dad49b7..2ba547fb 100644 --- a/src/stochasticFiltering/StochasticFilterBase.h +++ b/src/stochasticFiltering/StochasticFilterBase.h @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -36,6 +37,8 @@ #include "StochasticStateWrapperBase.h" #include "ObservationManagerBase.h" + + namespace sofa { @@ -45,27 +48,22 @@ namespace component namespace stochastic { -using namespace defaulttype; -/// ganaeral filter interface + +/** + * general filter interface + */ class StochasticFilterBase : public sofa::core::objectmodel::BaseObject { public: SOFA_ABSTRACT_CLASS(StochasticFilterBase, BaseObject); typedef sofa::core::objectmodel::BaseObject Inherit; - StochasticFilterBase() - : Inherit() - , mechParams(0) - , verbose( initData(&verbose, false, "verbose", "print tracing informations") ) - , useUnbiasedVariance( initData(&useUnbiasedVariance, false, "useUnbiasedVariance", "if true, the unbiased variance is computed (normalization by 1/(n-1) [not activated for UKFClassic!") ) - //, useModelIncertitude( initData(&useModelIncertitude, false, "useModelIncertitude", "if true, the state covariance is computed by adding Q") ) - , initialiseObservationsAtFirstStep( initData(&initialiseObservationsAtFirstStep, false, "initialiseObservationsAtFirstStep", "if true initialise component during first iteration") ) - { + Data< bool > verbose; + Data< bool > useUnbiasedVariance; + // Data< bool > useModelIncertitude; + Data< bool > initialiseObservationsAtFirstStep; - } - - ~StochasticFilterBase() {} protected: sofa::helper::system::thread::CTime *timer; @@ -78,13 +76,19 @@ class StochasticFilterBase : public sofa::core::objectmodel::BaseObject const core::MechanicalParams* mechParams; public: - Data verbose; - Data useUnbiasedVariance; - //Data useModelIncertitude; - Data initialiseObservationsAtFirstStep; + StochasticFilterBase() + : Inherit() + , verbose( initData(&verbose, false, "verbose", "print tracing informations") ) + , useUnbiasedVariance( initData(&useUnbiasedVariance, false, "useUnbiasedVariance", "if true, the unbiased variance is computed (normalization by 1/(n-1) [not activated for UKFClassic!") ) + //, useModelIncertitude( initData(&useModelIncertitude, false, "useModelIncertitude", "if true, the state covariance is computed by adding Q") ) + , initialiseObservationsAtFirstStep( initData(&initialiseObservationsAtFirstStep, false, "initialiseObservationsAtFirstStep", "if true initialise component during first iteration") ) + , mechParams(0) + { } + ~StochasticFilterBase() {} - void init() override { + void init() override + { Inherit::init(); gnode = dynamic_cast(this->getContext()); @@ -96,12 +100,14 @@ class StochasticFilterBase : public sofa::core::objectmodel::BaseObject actualTime = 0.0; } - virtual void initializeStep(const core::ExecParams* _params, const size_t _step) { - if (mechParams==0) + virtual void initializeStep(const core::ExecParams* _params, const size_t _step) + { + if (mechParams == 0) { mechParams = new core::MechanicalParams(*_params); - execParams=_params; - stepNumber=_step; - actualTime = double(stepNumber)*gnode->getDt(); + } + execParams = _params; + stepNumber = _step; + actualTime = double(stepNumber) * gnode->getDt(); this->gnode->setTime(this->actualTime); this->gnode->execute< sofa::simulation::UpdateSimulationContextVisitor >(execParams); } @@ -111,27 +117,23 @@ class StochasticFilterBase : public sofa::core::objectmodel::BaseObject // change filter state virtual void updateState() = 0; +}; -}; /// class -/// ganeral interface for uncertain filter types +/** + * general interface for uncertain filter types + */ class StochasticUnscentedFilterBase : public StochasticFilterBase { public: SOFA_ABSTRACT_CLASS(StochasticUnscentedFilterBase, StochasticFilterBase); typedef StochasticFilterBase Inherit; - StochasticUnscentedFilterBase() - : Inherit() - , reducedOrder( initData(&reducedOrder, false, "reducedOrder", "reduced order type of the filter") ) - , lambdaScale( initData(&lambdaScale, 0.5, "lambdaScale", "scaling for sigma points") ) - , d_sigmaTopologyType( initData(&d_sigmaTopologyType, "sigmaTopology", "sigma topology type") ) - { - - } + Data< bool > reducedOrder; + Data< double > lambdaScale; + Data< std::string > d_sigmaTopologyType; - ~StochasticUnscentedFilterBase() {} protected: typedef enum SigmaTopology { @@ -141,31 +143,40 @@ class StochasticUnscentedFilterBase : public StochasticFilterBase SigmaTopologyType m_sigmaTopology; + public: - Data reducedOrder; - Data lambdaScale; - Data< std::string > d_sigmaTopologyType; + StochasticUnscentedFilterBase() + : Inherit() + , reducedOrder( initData(&reducedOrder, false, "reducedOrder", "reduced order type of the filter") ) + , lambdaScale( initData(&lambdaScale, 0.5, "lambdaScale", "scaling for sigma points") ) + , d_sigmaTopologyType( initData(&d_sigmaTopologyType, "sigmaTopology", "sigma topology type") ) + { } + + ~StochasticUnscentedFilterBase() {} - void init() override { + void init() override + { Inherit::init(); m_sigmaTopology = SIMPLEX; std::string topology = d_sigmaTopologyType.getValue(); - if (std::strcmp(topology.c_str(), "Star") == 0) + if (std::strcmp(topology.c_str(), "Star") == 0) { m_sigmaTopology = STAR; + } } - virtual void initializeStep(const core::ExecParams* _params, const size_t _step) { + virtual void initializeStep(const core::ExecParams* _params, const size_t _step) override + { Inherit::initializeStep(_params, _step); } - virtual void computePrediction() = 0; - virtual void computeCorrection() = 0; + virtual void computePrediction() override = 0; + virtual void computeCorrection() override = 0; // change filter state - virtual void updateState() = 0; + virtual void updateState() override = 0; +}; -}; /// class } // stochastic diff --git a/src/stochasticFiltering/StochasticStateWrapper.cpp b/src/stochasticFiltering/StochasticStateWrapper.cpp index b50a7ce6..879726c3 100644 --- a/src/stochasticFiltering/StochasticStateWrapper.cpp +++ b/src/stochasticFiltering/StochasticStateWrapper.cpp @@ -26,6 +26,8 @@ #include "StochasticStateWrapper.inl" //#include + + namespace sofa { @@ -35,6 +37,8 @@ namespace component namespace stochastic { + + using namespace defaulttype; SOFA_DECL_CLASS(StochasticStateWrapper) @@ -45,9 +49,12 @@ int StochasticStateWrapperClass = core::RegisterObject("StochasticStateWrapper") .add< StochasticStateWrapper >() ; + template class SOFA_STOCHASTIC_API StochasticStateWrapper; template class SOFA_STOCHASTIC_API StochasticStateWrapper; + + } // namespace simulation } // namespace component diff --git a/src/stochasticFiltering/StochasticStateWrapper.h b/src/stochasticFiltering/StochasticStateWrapper.h index 72a325c9..555a4b8e 100644 --- a/src/stochasticFiltering/StochasticStateWrapper.h +++ b/src/stochasticFiltering/StochasticStateWrapper.h @@ -38,9 +38,9 @@ #include #include #include -#include #include #include +#include #include @@ -49,8 +49,6 @@ #include #include - - #include #include "initOptimusPlugin.h" @@ -59,6 +57,7 @@ #include + namespace sofa { @@ -69,30 +68,25 @@ namespace stochastic { + template class InternalCopy { -public : - +public: typedef typename DataTypes::VecCoord VecCoord; - - void copyStateToFilter(helper::vector > & pairs, const VecCoord & ) {} - - void copyFilterToSofa(helper::vector > & pairs, VecCoord & ) {} - + void copyStateToFilter(type::vector >& pairs, const VecCoord& ) {} + void copyFilterToSofa(type::vector >& pairs, VecCoord& ) {} void stateDim() {} - }; -using namespace defaulttype; + /** * Class which implements an interface between a filter (working with Eigen vectors and matrices) and SOFA and its own data types as Vec3d, Rigid3d etc.). * Presence of the component StochasticStateWrapper in a SOFA scene node indicates that the physical simulation is employed for predictions. * It closely interacts with mechanical state (set/get mechanism) and OptimParams which is a container for storing stochastic quantities. */ - template -class StochasticStateWrapper: public sofa::component::stochastic::StochasticStateWrapperBaseT +class StochasticStateWrapper : public sofa::component::stochastic::StochasticStateWrapperBaseT { public: SOFA_CLASS(SOFA_TEMPLATE2(StochasticStateWrapper, DataTypes, FilterType), SOFA_TEMPLATE(StochasticStateWrapperBaseT, FilterType)); @@ -102,6 +96,7 @@ class StochasticStateWrapper: public sofa::component::stochastic::StochasticStat typedef typename DataTypes::VecDeriv VecDeriv; typedef typename DataTypes::Coord Coord; typedef typename DataTypes::Coord Deriv; + typedef typename DataTypes::Real Real; typedef FilterType Type; typedef typename sofa::component::linearsolver::FullMatrix FullMatrix; @@ -110,23 +105,41 @@ class StochasticStateWrapper: public sofa::component::stochastic::StochasticStat typedef typename core::behavior::MechanicalState MechanicalState; - typedef typename core::behavior::MechanicalState MappedMechanicalState; + typedef typename core::behavior::MechanicalState MappedMechanicalState; typedef typename component::projectiveconstraintset::FixedConstraint FixedConstraint; typedef sofa::component::container::OptimParamsBase OptimParamsBase; typedef typename Inherit::EMatrixX EMatrixX; typedef typename Inherit::EVectorX EVectorX; - typedef defaulttype::Vec<4,float> Vec4f; + typedef type::Vec<4,float> Vec4f; + + + Data< bool > d_langrangeMultipliers; + Data< bool > estimatePosition; + Data< bool > estimateOnlyXYZ; + + Data< bool > estimateVelocity; + Data< type::vector > posModelStdev, velModelStdev; + Data< type::vector > paramModelStdev; + Data< type::vector > d_positionStdev; /// standart deviation for positions + Data< type::vector > d_velocityStdev; /// standart deviation for velocities + Data< std::string > d_mappedStatePath; + Data< bool > d_draw; + Data< double > d_radius_draw; + Data< FullMatrix > d_fullMatrix; + Data< bool > m_solveVelocityConstraintFirst; + + EMatrixX modelErrorVariance; + EMatrixX modelErrorVarianceInverse; + FilterType modelErrorVarianceValue; - StochasticStateWrapper(); - ~StochasticStateWrapper(); protected: - MechanicalState *mechanicalState; - MappedMechanicalState *mappedState; + MechanicalState* mechanicalState; + MappedMechanicalState* mappedState; FixedConstraint* fixedConstraint; - helper::vector vecOptimParams; + type::vector vecOptimParams; InternalCopy m_internalCopy; size_t posDim; size_t velDim; @@ -135,42 +148,28 @@ class StochasticStateWrapper: public sofa::component::stochastic::StochasticStat VecCoord beginTimeStepPos; VecDeriv beginTimeStepVel; - Vec3dTypes::VecCoord beginTimeStepMappedPos; + sofa::defaulttype::Vec3dTypes::VecCoord beginTimeStepMappedPos; - helper::vector sigmaStatePos; - helper::vector sigmaStateVel; - helper::vector sigmaMappedStatePos; - helper::vector color,colorB; + type::vector sigmaStatePos; + type::vector sigmaStateVel; + type::vector sigmaMappedStatePos; + type::vector color, colorB; bool valid; - helper::vector fixedNodes, freeNodes; - helper::vector > positionPairs; - helper::vector > velocityPairs; + type::vector< size_t > fixedNodes, freeNodes; + type::vector< std::pair > positionPairs; + type::vector< std::pair > velocityPairs; - void copyStateFilter2Sofa(const core::MechanicalParams *_mechParams, bool _setVelocityFromPosition = false); // copy actual DA state to SOFA state and propagate to mappings + /// copy actual DA state to SOFA state and propagate to mappings + void copyStateFilter2Sofa(const core::MechanicalParams *_mechParams, bool _setVelocityFromPosition = false); void copyStateSofa2Filter(); // copy the actual SOFA state to DA state void computeSofaStep(const core::ExecParams* execParams, bool _updateTime); void computeSofaStepWithLM(const core::ExecParams* params); -public: - Data d_langrangeMultipliers; - Data estimatePosition; - Data estimateOnlyXYZ; - - Data estimateVelocity; - Data> posModelStdev, velModelStdev; - Data> paramModelStdev; - Data> d_positionStdev; /// standart deviation for positions - Data> d_velocityStdev; /// standart deviation for velocities - Data d_mappedStatePath; - Data< bool > d_draw; - Data< double > d_radius_draw; - Data d_fullMatrix; - Data m_solveVelocityConstraintFirst; - EMatrixX modelErrorVariance; - EMatrixX modelErrorVarianceInverse; - FilterType modelErrorVarianceValue; +public: + StochasticStateWrapper(); + ~StochasticStateWrapper(); /// initialization before the data assimilation starts void init() override; @@ -194,12 +193,13 @@ class StochasticStateWrapper: public sofa::component::stochastic::StochasticStat void getPos(EVectorX& _state, VecCoord& actualPos); void getActualVelocity(int _id, VecDeriv& _vel); - void getActualMappedPosition(int _id, Vec3dTypes::VecCoord& _mapPos); + void getActualMappedPosition(int _id, sofa::defaulttype::Vec3dTypes::VecCoord& _mapPos); void setState(EVectorX& _state, const core::MechanicalParams* _mparams) override; void setSofaVectorFromFilterVector(EVectorX& _state, typename DataTypes::VecCoord& _vec); void setSofaVelocityFromFilterVector(EVectorX& _state, typename DataTypes::VecDeriv& _vel); + /// get the variance of error of the state virtual EMatrixX& getStateErrorVariance() override; void updateStateErrorVariance(); @@ -213,8 +213,8 @@ class StochasticStateWrapper: public sofa::component::stochastic::StochasticStat void draw(const core::visual::VisualParams* vparams) override; - /// SOFA-imposed methods for object factory + /// SOFA-imposed methods for object factory template static bool canCreate(T*& obj, core::objectmodel::BaseContext* context, core::objectmodel::BaseObjectDescription* arg) { @@ -231,12 +231,11 @@ class StochasticStateWrapper: public sofa::component::stochastic::StochasticStat return DataTypes::Name(); } -protected : +protected: sofa::core::behavior::ConstraintSolver *constraintSolver; component::constraintset::LCPConstraintSolver::SPtr defaultSolver; - /****** ADDED FOR COLLISIONS *****/ /// Activate collision pipeline virtual void computeCollision(const core::ExecParams* params = core::ExecParams::defaultInstance()); @@ -244,7 +243,6 @@ protected : /// Activate OdeSolvers virtual void integrate(const core::ExecParams* params, SReal dt); - //typedef simulation::Node::Sequence Solvers; //typedef core::collision::Pipeline Pipeline; //const Solvers& getSolverSequence(); @@ -253,8 +251,8 @@ protected : // This pointer is initialized one time at the construction, avoiding dynamic_cast(context) every time step //simulation::Node* gnode; /****** ADDED FOR COLLISIONS *****/ +}; -}; /// class } // simulation diff --git a/src/stochasticFiltering/StochasticStateWrapper.inl b/src/stochasticFiltering/StochasticStateWrapper.inl index aae1a472..04e40e11 100644 --- a/src/stochasticFiltering/StochasticStateWrapper.inl +++ b/src/stochasticFiltering/StochasticStateWrapper.inl @@ -21,15 +21,17 @@ ******************************************************************************/ #pragma once +/// visitors #include #include #include #include +#include +#include +#include #include #include #include -#include -#include #include #include #include @@ -40,23 +42,36 @@ #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/// event handlers +#include +#include #include #include #include -#include -#include -#include + + #include #include #include +#include #include #include - - - #include "StochasticStateWrapper.h" + namespace sofa { @@ -67,33 +82,39 @@ namespace stochastic { + template<> -class InternalCopy { +class InternalCopy +{ public: + typedef typename sofa::defaulttype::Rigid3dTypes::VecCoord VecCoord; + typedef typename sofa::defaulttype::Rigid3dTypes::Real Real; + type::vector> m_ori; - typedef typename Rigid3dTypes::VecCoord VecCoord; - - void copyStateToFilter(helper::vector > & positionPairs, const VecCoord & pos) { + void copyStateToFilter(type::vector > & positionPairs, const VecCoord & pos) + { m_ori.clear(); - for (helper::vector >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++){ - //for (unsigned i=0;i >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) + { + // for (unsigned i = 0; i < pos.size(); i++) m_ori.push_back(pos[it->first].getOrientation()); } std::cout << "[internal stockedQUAT] " << m_ori < > & positionPairs, VecCoord & pos) { - for (helper::vector >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++){ - //for (unsigned i=0;i >& positionPairs, VecCoord& pos) + { + for (type::vector >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) + { + // for (unsigned i = 0; i < pos.size(); i++) pos[it->first].getOrientation() = m_ori[it->first]; std::cout << "[internal injectedQUAT] " << pos[it->first].getOrientation() << std::endl; } } +}; - helper::vector m_ori; -}; using namespace sofa::simulation; @@ -104,41 +125,49 @@ StochasticStateWrapper::StochasticStateWrapper() , estimatePosition( initData(&estimatePosition, false, "estimatePosition", "estimate the position (e.g., if initial conditions with uncertainty") ) , estimateOnlyXYZ( initData(&estimateOnlyXYZ, false, "estimateOnlyXYZ", "estimate only the X Y Z for a Rigid Body") ) , estimateVelocity( initData(&estimateVelocity, false, "estimateVelocity", "estimate the velocity (e.g., if initial conditions with uncertainty") ) - , posModelStdev( initData(&posModelStdev, helper::vector(0.0), "posModelStdev", "standard deviation in observations") ) - , velModelStdev( initData(&velModelStdev, helper::vector(0.0), "velModelStdev", "standard deviation in observations") ) - , paramModelStdev( initData(¶mModelStdev, helper::vector(0.0), "paramModelStdev", "standard deviation in observations") ) - , d_positionStdev( initData(&d_positionStdev, helper::vector(1, 0.0), "positionStdev", "estimate standard deviation for positions")) - , d_velocityStdev( initData(&d_velocityStdev, helper::vector(1, 0.0), "velocityStdev", "estimate standard deviation for velocities")) + , posModelStdev( initData(&posModelStdev, type::vector(0.0), "posModelStdev", "standard deviation in observations") ) + , velModelStdev( initData(&velModelStdev, type::vector(0.0), "velModelStdev", "standard deviation in observations") ) + , paramModelStdev( initData(¶mModelStdev, type::vector(0.0), "paramModelStdev", "standard deviation in observations") ) + , d_positionStdev( initData(&d_positionStdev, type::vector(1, 0.0), "positionStdev", "estimate standard deviation for positions")) + , d_velocityStdev( initData(&d_velocityStdev, type::vector(1, 0.0), "velocityStdev", "estimate standard deviation for velocities")) , d_mappedStatePath(initData(&d_mappedStatePath, "mappedState", "Link to Virtual Mapped Catheter ")) , d_draw(initData(&d_draw, false, "draw","Activation of draw")) , d_radius_draw( initData(&d_radius_draw, "radiusDraw", "radius of the spheres") ) , d_fullMatrix( initData(&d_fullMatrix, "fullMatrix", "testing the full matrix" ) ) -{ -} +{ } + + template StochasticStateWrapper::~StochasticStateWrapper() -{ -} +{ } + + template <> -void StochasticStateWrapper::stateDim(){ - if(!estimateOnlyXYZ.getValue()){ - std::cout<<"[StochasticStateWrapper] Estimate 6DoF Position"<::stateDim() +{ + if ( !estimateOnlyXYZ.getValue() ) { + std::cout << "[StochasticStateWrapper] Estimate 6DoF Position" << std::endl; posDim = 6; - } - else { - std::cout<<"[StochasticStateWrapper] Estimate 3Dof Position"< -void StochasticStateWrapper::stateDim(){ +void StochasticStateWrapper::stateDim() +{ posDim = 3; velDim = 3; } + + + template void StochasticStateWrapper::init() { @@ -146,9 +175,9 @@ void StochasticStateWrapper::init() valid = true; /// get a mechanical state this->gnode->get(mechanicalState, core::objectmodel::BaseContext::SearchDown); - if (!mechanicalState) { + if ( !mechanicalState ) { PRNE("No mechanical state found"); - valid=false; + valid = false; } else { PRNS("Mechanical state found: " << mechanicalState->getName()); } @@ -159,9 +188,9 @@ void StochasticStateWrapper::init() /// get optim params (multiple per node, at least one) vecOptimParams.clear(); this->gnode->template get(&vecOptimParams, core::objectmodel::BaseContext::SearchDown ); - if (vecOptimParams.empty()) { -// PRNW("No OptimParams found"); - //valid=false; + if ( vecOptimParams.empty() ) { + // PRNW("No OptimParams found"); + // valid = false; } else { PRNSC("OptimParams found " << vecOptimParams.size() << "x: "); if (this->verbose.getValue()) { @@ -173,21 +202,22 @@ void StochasticStateWrapper::init() /// get fixed constraints (optional) this->gnode->get(fixedConstraint, core::objectmodel::BaseContext::SearchDown); - if (fixedConstraint) { + if ( fixedConstraint ) { PRNS("Fixed constraint found: " << fixedConstraint->getName()); } /// search for constraint solver - if (this->d_langrangeMultipliers.getValue()) { + if ( this->d_langrangeMultipliers.getValue() ) { PRNS("Looking for constraint solver"); { simulation::common::VectorOperations vop(core::ExecParams::defaultInstance(), this->getContext()); - core::behavior::MultiVecDeriv dx(&vop, core::VecDerivId::dx() ); dx.realloc( &vop, true, true ); - core::behavior::MultiVecDeriv df(&vop, core::VecDerivId::dforce() ); df.realloc( &vop, true, true ); + core::behavior::MultiVecDeriv dx(&vop, core::VecDerivId::dx() ); + dx.realloc( &vop, true, true ); + core::behavior::MultiVecDeriv df(&vop, core::VecDerivId::dforce() ); + df.realloc( &vop, true, true ); } - this->getContext()->get(constraintSolver, core::objectmodel::BaseContext::SearchDown); if (constraintSolver == NULL && defaultSolver != NULL) { @@ -201,38 +231,41 @@ void StochasticStateWrapper::init() defaultSolver.reset(); } } + this->gnode->get(mappedState, d_mappedStatePath.getValue()); if ( mappedState != NULL) { - std::cout<<"Found mapped mechanical state: " << mappedState->getName()<declaredMappedState=1; - } - else { - std::cout<<"[WARNING] No mapped state state found! Necessary for BindedSimpleObservationManager"<declaredMappedState=0; + std::cout << "Found mapped mechanical state: " << mappedState->getName() << std::endl; + this->declaredMappedState = 1; + } else { + std::cout << "[WARNING] No mapped state state found! Necessary for BindedSimpleObservationManager" << std::endl; + this->declaredMappedState = 0; } this->EstimatePOSITION = estimatePosition.getValue(); - this->EstimateONLYXYZ=estimateOnlyXYZ.getValue(); + this->EstimateONLYXYZ = estimateOnlyXYZ.getValue(); } + + template -void StochasticStateWrapper::bwdInit() { - if (!valid) +void StochasticStateWrapper::bwdInit() +{ + if ( !valid ) return; this->mStateSize = mechanicalState->getSize(); - if ( mappedState != NULL) { + if ( mappedState != NULL ) { this->mappedMStateSize = mappedState->getSize(); } /// extract free and fixed nodes (fixed nodes cannot be included in the stochastic state) fixedNodes.clear(); freeNodes.clear(); - if (fixedConstraint) { + if ( fixedConstraint ) { const typename FixedConstraint::SetIndexArray& fix = fixedConstraint-> d_indices.getValue(); for (size_t i = 0; i < fix.size(); i++) fixedNodes.push_back(size_t(fix[i])); for (size_t i = 0; i < mechanicalState->getSize(); i++) { - helper::vector::iterator it = find(fixedNodes.begin(), fixedNodes.end(), i); + type::vector::iterator it = find(fixedNodes.begin(), fixedNodes.end(), i); if (it == fixedNodes.end()) freeNodes.push_back(i); @@ -242,77 +275,77 @@ void StochasticStateWrapper::bwdInit() { freeNodes.push_back(i); } - positionPairs.clear(); velocityPairs.clear(); size_t vsi = 0; if (estimatePosition.getValue()) { - helper::vector posStdev; + type::vector posStdev; posStdev.resize(posDim); if (d_positionStdev.getValue().size() != posDim) { PRNW("Bad initial value of Position Initial Covariance. Resize according to Position DoFs"); - for (size_t i=0 ;i pr(freeNodes[i], vsi); - vsi+=posDim; + vsi += posDim; positionPairs.push_back(pr); } + /// add standart deviation for positions this->positionVariance.resize(posDim * positionPairs.size()); - for (size_t i = 0; i < positionPairs.size(); i++){ - for (size_t j = 0; j < posDim; j ++) - this->positionVariance[i*posDim+j] = posStdev[j%posDim] * posStdev[j%posDim]; + for (size_t i = 0; i < positionPairs.size(); i++) { + for (size_t j = 0; j < posDim; j++) + this->positionVariance[i * posDim + j] = posStdev[j % posDim] * posStdev[j % posDim]; } } - /*PRNS("Mapping stochastic -> mechanical"); for (size_t i = 0; i < positionPairs.size(); i++) { PRNS(positionPairs[i].first << " --> " << positionPairs[i].second); }*/ if (estimateVelocity.getValue()) { - helper::vector velStdev; + type::vector velStdev; velStdev.resize(velDim); - if( d_velocityStdev.getValue().size() != velDim ){ + if ( d_velocityStdev.getValue().size() != velDim ) { PRNW("Bad initial value of Velocity Initial Covariance. Resize according to Velocity DoFs"); - for (size_t i=0 ;i pr(freeNodes[i], vsi); - vsi+=velDim; + vsi += velDim; velocityPairs.push_back(pr); } + /// add standart deviation for velocities this->velocityVariance.resize(velDim * velocityPairs.size()); - for (size_t i = 0; i < velocityPairs.size(); i++ ){ - for (size_t j = 0; j < velDim; j ++) - this->velocityVariance[i*velDim+j] = velStdev[j%velDim] * velStdev[j%velDim]; + for (size_t i = 0; i < velocityPairs.size(); i++) { + for (size_t j = 0; j < velDim; j++) + this->velocityVariance[i * velDim + j] = velStdev[j % velDim] * velStdev[j % velDim]; } } - size_t vpi = 0; this->reducedStateIndex = vsi; - for (size_t pi = 0; pi < vecOptimParams.size(); pi++) { - helper::vector opv; + for (size_t pi = 0; pi < vecOptimParams.size(); pi++) + { + type::vector opv; for (size_t i = 0; i < vecOptimParams[pi]->size(); i++, vpi++) { - opv.push_back(this->reducedStateIndex+vpi); + opv.push_back(this->reducedStateIndex + vpi); } vecOptimParams[pi]->setVStateParamIndices(opv); } @@ -327,18 +360,19 @@ void StochasticStateWrapper::bwdInit() { this->state.resize(this->stateSize); copyStateSofa2Filter(); - color.resize(this->stateSize+1); //TRUE IF USING SIMPLEX SIGMA PTS + color.resize(this->stateSize + 1); //TRUE IF USING SIMPLEX SIGMA PTS colorB.resize(color.size()); - for(size_t i =0; i < color.size(); i++){ - color[i]= ((double) rand() / (RAND_MAX)) ; - colorB[i]= ((double) rand() / (RAND_MAX)) ; + for (size_t i = 0; i < color.size(); i++) { + color[i] = ((double) rand() / (RAND_MAX)); + colorB[i] = ((double) rand() / (RAND_MAX)); } } template -void StochasticStateWrapper::updateState(bool addData) { +void StochasticStateWrapper::updateState(bool addData) +{ if (addData) { size_t vsi = freeNodes.size() * posDim; if (estimateVelocity.getValue()) { @@ -347,8 +381,9 @@ void StochasticStateWrapper::updateState(bool addData) { size_t vpi = 0; this->reducedStateIndex = vsi; - for (size_t pi = 0; pi < vecOptimParams.size(); pi++) { - helper::vector opv; + for (size_t pi = 0; pi < vecOptimParams.size(); pi++) + { + type::vector opv; for (size_t i = 0; i < vecOptimParams[pi]->size(); i++, vpi++) { opv.push_back(this->reducedStateIndex+vpi); } @@ -367,11 +402,11 @@ void StochasticStateWrapper::updateState(bool addData) { updateModelErrorVariance(); copyStateSofa2Filter(); - color.resize(this->stateSize+1); //TRUE IF USING SIMPLEX SIGMA PTS + color.resize(this->stateSize + 1); //TRUE IF USING SIMPLEX SIGMA PTS colorB.resize(color.size()); - for(size_t i =0; i < color.size(); i++){ - color[i]= ((double) rand() / (RAND_MAX)) ; - colorB[i]= ((double) rand() / (RAND_MAX)) ; + for (size_t i = 0; i < color.size(); i++) { + color[i] = ((double) rand() / (RAND_MAX)); + colorB[i] = ((double) rand() / (RAND_MAX)); } } else { PRNS("Update stochastic state with size " << this->stateSize); @@ -383,9 +418,13 @@ void StochasticStateWrapper::updateState(bool addData) { -/// function called from observation manager when it needs to perform mapping (a more elegant solution could/should be found) +/** + * function called from observation manager when it needs to perform mapping + * (a more elegant solution could/should be found) + */ template -void StochasticStateWrapper::setSofaVectorFromFilterVector(EVectorX& _state, typename DataTypes::VecCoord& _vec) { +void StochasticStateWrapper::setSofaVectorFromFilterVector(EVectorX& _state, typename DataTypes::VecCoord& _vec) +{ if (_vec.size() != mechanicalState->getSize()) { PRNE("Input vector not compatible with the actual Sofa state size"); return; @@ -395,10 +434,10 @@ void StochasticStateWrapper::setSofaVectorFromFilterVecto for (size_t fni = 0; fni < fixedNodes.size(); fni++) { size_t fn = fixedNodes[fni]; _vec[fn] = pos[fn]; - //PRNS("Setting fixed[" << fn << "] = " << _vec[fn]); + // PRNS("Setting fixed[" << fn << "] = " << _vec[fn]); } - for (helper::vector >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) { + for (type::vector >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) { if (it->first >= _vec.size()) { PRNE("Accessing Sofa vector out of bounds: " << it->first << " vs. " << _vec.size()); return; @@ -412,11 +451,15 @@ void StochasticStateWrapper::setSofaVectorFromFilterVecto for (size_t d = 0; d < posDim; d++) { _vec[it->first][d] = _state(it->second + d); } - //PRNS("Setting free[" << it->first << "] = " << _vec[it->first]); + // PRNS("Setting free[" << it->first << "] = " << _vec[it->first]); } } + + + template -void StochasticStateWrapper::setSofaVelocityFromFilterVector(EVectorX& _state, typename DataTypes::VecDeriv& _vel) { +void StochasticStateWrapper::setSofaVelocityFromFilterVector(EVectorX& _state, typename DataTypes::VecDeriv& _vel) +{ if (_vel.size() != mechanicalState->getSize()) { PRNE("Input velocity vector not compatible with the actual Sofa state size"); return; @@ -426,10 +469,10 @@ void StochasticStateWrapper::setSofaVelocityFromFilterVec for (size_t fni = 0; fni < fixedNodes.size(); fni++) { size_t fn = fixedNodes[fni]; _vel[fn] = vel[fn]; - //PRNS("Setting fixed[" << fn << "] = " << _vec[fn]); + // PRNS("Setting fixed[" << fn << "] = " << _vec[fn]); } - for (helper::vector >::iterator it = velocityPairs.begin(); it != velocityPairs.end(); it++) { + for (type::vector >::iterator it = velocityPairs.begin(); it != velocityPairs.end(); it++) { if (it->first >= _vel.size()) { PRNE("Accessing Sofa vector out of bounds: " << it->first << " vs. " << _vel.size()); return; @@ -443,337 +486,372 @@ void StochasticStateWrapper::setSofaVelocityFromFilterVec for (size_t d = 0; d < velDim; d++) { _vel[it->first][d] = _state(it->second + d); } - //PRNS("Setting free[" << it->first << "] = " << _vec[it->first]); + // PRNS("Setting free[" << it->first << "] = " << _vec[it->first]); } } -/// function that sets SOFA state (position, velocity, parameters) from this->state (stochastic state, Eigen vector) + +/** + * function that sets SOFA state (position, velocity, parameters) from this->state (stochastic state, Eigen vector) + */ template <> -void StochasticStateWrapper::copyStateFilter2Sofa(const core::MechanicalParams* _mechParams, bool /* _setVelocityFromPosition */) { +void StochasticStateWrapper::copyStateFilter2Sofa(const core::MechanicalParams* _mechParams, bool /* _setVelocityFromPosition */) +{ typename MechanicalState::WriteVecCoord pos = mechanicalState->writePositions(); typename MechanicalState::WriteVecDeriv vel = mechanicalState->writeVelocities(); /// Position and Orientation - if(estimatePosition.getValue() && !estimateOnlyXYZ.getValue()){ - helper::vector EulerPos; + if ( estimatePosition.getValue() && !estimateOnlyXYZ.getValue() ) { + type::vector EulerPos; EulerPos.resize(pos.size()); - defaulttype::Quat q_ori; + type::Quat q_ori; q_ori.normalize(); - Vector3 euler_ori; - Vector3 eu_pos; - for (helper::vector >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) { + type::Vector3 euler_ori; + type::Vector3 eu_pos; + for (type::vector< std::pair >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) + { for (size_t d = 0; d < posDim; d++) { EulerPos[it->first][d] = this->state(it->second + d); - // m_internalCopy.copyFilterToSofa(positionPairs,pos.wref()); + // m_internalCopy.copyFilterToSofa(positionPairs, pos.wref()); } } - for(size_t i= 0; i < pos.size(); i++) { - for(size_t j=0; j <3; j++){ - eu_pos[j]=EulerPos[i][j]; + for (size_t i = 0; i < pos.size(); i++) + { + for (size_t j = 0; j < 3; j++) { + eu_pos[j] = EulerPos[i][j]; } unsigned k=0; - for(size_t j=3; j <6; j++, k++){ - euler_ori[k]=EulerPos[i][j]; + for(size_t j = 3; j < 6; j++, k++) { + euler_ori[k] = EulerPos[i][j]; } - q_ori=defaulttype::Quat::createQuaterFromEuler(euler_ori*M_PI/180); + q_ori = type::Quat::createQuaterFromEuler(euler_ori * M_PI / 180); q_ori.normalize(); - pos[i].getOrientation()= q_ori; - pos[i].getCenter() =eu_pos; + pos[i].getOrientation() = q_ori; + pos[i].getCenter() = eu_pos; q_ori.clear(); } } - if(!estimatePosition.getValue()&& estimateOnlyXYZ.getValue()){ - std::cout<<"ERROR check boolean estimatePosition " < EulerPos; + type::vector EulerPos; EulerPos.resize(pos.size()); - Vector3 eu_pos; + type::Vector3 eu_pos; - for (helper::vector >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) { + for (type::vector >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) + { for (size_t d = 0; d < 3; d++) { EulerPos[it->first][d] = this->state(it->second + d); } } - for(size_t i= 0; i < pos.size(); i++) { - for(size_t j=0; j <3; j++){ - eu_pos[j]=EulerPos[i][j]; + for (size_t i = 0; i < pos.size(); i++) + { + for (size_t j = 0; j < 3; j++) { + eu_pos[j] = EulerPos[i][j]; } - pos[i].getCenter() =eu_pos; + pos[i].getCenter() = eu_pos; } - //Compute Quaternion for First Node + /// Compute Quaternion for First Node { - Vector3 P0(EulerPos[0][0],EulerPos[0][1],EulerPos[0][2]); - Vector3 P1(EulerPos[1][0],EulerPos[1][1],EulerPos[1][2]); + type::Vector3 P0(EulerPos[0][0], EulerPos[0][1], EulerPos[0][2]); + type::Vector3 P1(EulerPos[1][0], EulerPos[1][1], EulerPos[1][2]); - Vector3 X0 = P1-P0; + type::Vector3 X0 = P1 - P0; X0.normalize(); - Vector3 Y0,Z0; + type::Vector3 Y0, Z0; - if (fabs(dot(X0,Vector3(1,0,0))) >= 0.999999999999999) { - Y0 = cross(X0,Vector3(0,1,0)); - }else { - Y0 = cross(X0,Vector3(1,0,0)); + if (fabs(dot(X0, type::Vector3(1, 0, 0))) >= 0.999999999999999) { + Y0 = cross(X0, type::Vector3(0, 1, 0)); + } else { + Y0 = cross(X0, type::Vector3(1, 0, 0)); } Y0.normalize(); - Z0 = cross(X0,Y0); + Z0 = cross(X0, Y0); Z0.normalize(); - pos[0].getOrientation().fromFrame(X0,Y0,Z0); + pos[0].getOrientation().fromFrame(X0, Y0, Z0); } - //Compute Quaternion for Beam Shaft - for(size_t i= 1; i < pos.size()-1; i++) { - Vector3 P0(EulerPos[i][0],EulerPos[i][1],EulerPos[i][2]); - Vector3 P1(EulerPos[i+1][0],EulerPos[i+1][1],EulerPos[i+1][2]); + /// Compute Quaternion for Beam Shaft + for (size_t i = 1; i < pos.size() - 1; i++) + { + type::Vector3 P0(EulerPos[i][0], EulerPos[i][1], EulerPos[i][2]); + type::Vector3 P1(EulerPos[i + 1][0], EulerPos[i + 1][1], EulerPos[i + 1][2]); - Vector3 X = P1-P0; + type::Vector3 X = P1 - P0; X.normalize(); - Vector3 Yprec = pos[i-1].getOrientation().rotate(Vector3(0,1,0)); - Vector3 Z = cross(X,Yprec); + type::Vector3 Yprec = pos[i - 1].getOrientation().rotate(type::Vector3(0, 1, 0)); + type::Vector3 Z = cross(X, Yprec); Z.normalize(); - Vector3 Y = cross(Z,X); + type::Vector3 Y = cross(Z, X); Y.normalize(); - pos[i].getOrientation().fromFrame(X,Y,Z); + pos[i].getOrientation().fromFrame(X, Y, Z); } + /// Compute quaternion for last node + int end = pos.size() - 1; + { + type::Vector3 P0(EulerPos[end - 1][0], EulerPos[end - 1][1], EulerPos[end - 1][2]); + type::Vector3 P1(EulerPos[end][0], EulerPos[end][1], EulerPos[end][2]); - // Compute quaternion for last node - int end =pos.size()-1;{ - - Vector3 P0(EulerPos[end-1][0],EulerPos[end-1][1],EulerPos[end-1][2]); - Vector3 P1(EulerPos[end][0],EulerPos[end][1],EulerPos[end][2]); - - Vector3 X = P1-P0; + type::Vector3 X = P1 - P0; X.normalize(); - Vector3 Yprec = pos[end-1].getOrientation().rotate(Vector3(0,1,0)); + type::Vector3 Yprec = pos[end - 1].getOrientation().rotate(type::Vector3(0, 1, 0)); - Vector3 Z = cross(X,Yprec); + type::Vector3 Z = cross(X, Yprec); Z.normalize(); - Vector3 Y = cross(Z,X); + type::Vector3 Y = cross(Z, X); Y.normalize(); - pos[end].getOrientation().fromFrame(X,Y,Z); + pos[end].getOrientation().fromFrame(X, Y, Z); } } - if(estimateVelocity.getValue()){ - for (helper::vector >::iterator it = velocityPairs.begin(); it != velocityPairs.end(); it++) { + if (estimateVelocity.getValue()) + { + for (type::vector >::iterator it = velocityPairs.begin(); it != velocityPairs.end(); it++) { for (size_t d = 0; d < velDim; d++) { vel[it->first][d] = this->state(it->second + d); } } } - /// if velocity is not estimated, it must be computed from the positions at the beginning and at the end of the time step - if (!estimateVelocity.getValue()) { - if (velocityPairs.empty() && beginTimeStepPos.size() == pos.size()) { - - helper::vector EulerPos; EulerPos.resize(pos.size()); - defaulttype::Quat q_ori; q_ori.normalize(); - Vector3 euler_ori; Vector3 eu_pos; - - - helper::vector begEulerPos; begEulerPos.resize(pos.size()); - defaulttype::Quat begq_ori; begq_ori.normalize(); - Vector3 begeuler_ori; Vector3 begeu_pos; - - for(size_t i= 0; i < pos.size(); i++) { + /// if velocity is not estimated, it must be computed from the positions at the beginning and at the end of the time step + if ( !estimateVelocity.getValue() ) + { + if (velocityPairs.empty() && beginTimeStepPos.size() == pos.size()) + { + type::vector EulerPos; + EulerPos.resize(pos.size()); + type::Quat q_ori; + q_ori.normalize(); + type::Vector3 euler_ori; + type::Vector3 eu_pos; + + type::vector begEulerPos; + begEulerPos.resize(pos.size()); + type::Quat begq_ori; + begq_ori.normalize(); + type::Vector3 begeuler_ori; + type::Vector3 begeu_pos; + + for (size_t i = 0; i < pos.size(); i++) + { q_ori.clear(); - q_ori=pos[i].getOrientation(); + q_ori = pos[i].getOrientation(); q_ori.normalize(); - eu_pos=pos[i].getCenter(); - euler_ori=q_ori.toEulerVector(); + eu_pos = pos[i].getCenter(); + euler_ori = q_ori.toEulerVector(); begq_ori.clear(); - begq_ori=beginTimeStepPos[i].getOrientation(); + begq_ori = beginTimeStepPos[i].getOrientation(); begq_ori.normalize(); - begeu_pos=beginTimeStepPos[i].getCenter(); - begeuler_ori=begq_ori.toEulerVector(); - - for(size_t j=0; j <3; j++){ - EulerPos[i][j]=eu_pos[j]; - begEulerPos[i][j]=begeu_pos[j]; + begeu_pos = beginTimeStepPos[i].getCenter(); + begeuler_ori = begq_ori.toEulerVector(); + for (size_t j = 0; j < 3; j++) + { + EulerPos[i][j] = eu_pos[j]; + begEulerPos[i][j] = begeu_pos[j]; } - unsigned k=0; - for(size_t j=3; j <6; j++, k++){ - EulerPos[i][j]=euler_ori[k]*180/M_PI; - begEulerPos[i][j]=begeuler_ori[k]*180/M_PI; - + unsigned k = 0; + for (size_t j = 3; j < 6; j++, k++) { + EulerPos[i][j] = euler_ori[k] * 180 / M_PI; + begEulerPos[i][j] = begeuler_ori[k] * 180 / M_PI; } - } - - for (helper::vector >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) { + for (type::vector >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) { for (size_t d = 0; d < 6; d++) { - vel[it->first][d] = (EulerPos[it->first][d] - begEulerPos[it->first][d])/this->getContext()->getDt(); + vel[it->first][d] = (EulerPos[it->first][d] - begEulerPos[it->first][d]) / this->getContext()->getDt(); } -// std::cout<<"pos: " <gnode ); + sofa::simulation::mechanicalvisitor::MechanicalPropagateOnlyPositionAndVelocityVisitor(_mechParams).execute( this->gnode ); /// let the OptimParams to extract the actual values of parameters from the verdandi state for (size_t opi = 0; opi < vecOptimParams.size(); opi++) vecOptimParams[opi]->vectorToParams(this->state); } -/// function that compies SOFA state (position, velocity, parameters) to this->state (stochastic state, Eigen vector) + + +/** + * function that compies SOFA state (position, velocity, parameters) to this->state (stochastic state, Eigen vector) + */ template <> -void StochasticStateWrapper::copyStateSofa2Filter() { +void StochasticStateWrapper::copyStateSofa2Filter() +{ typename MechanicalState::ReadVecCoord pos = mechanicalState->readPositions(); typename MechanicalState::ReadVecDeriv vel = mechanicalState->readVelocities(); - - if(!estimatePosition.getValue()&& estimateOnlyXYZ.getValue()){ - std::cout<<"ERROR check boolean estimatePosition " < EulerPos; + if ( estimatePosition.getValue() && !estimateOnlyXYZ.getValue() ) + { + type::vector EulerPos; EulerPos.resize(pos.size()); - defaulttype::Quat q_ori; + type::Quat q_ori; q_ori.normalize(); - Vector3 euler_ori; - Vector3 eu_pos; + type::Vector3 euler_ori; + type::Vector3 eu_pos; - for(size_t i= 0; i < pos.size(); i++) { + for (size_t i = 0; i < pos.size(); i++) + { q_ori.clear(); - q_ori=pos[i].getOrientation(); + q_ori = pos[i].getOrientation(); q_ori.normalize(); - eu_pos=pos[i].getCenter(); - euler_ori=q_ori.toEulerVector(); - // euler_ori.normalize(); + eu_pos = pos[i].getCenter(); + euler_ori = q_ori.toEulerVector(); + // euler_ori.normalize(); - for(size_t j=0; j <3; j++){ - EulerPos[i][j]=eu_pos[j]; + for (size_t j = 0; j < 3; j++) { + EulerPos[i][j] = eu_pos[j]; } - unsigned k=0; - for(size_t j=3; j <6; j++, k++){ - EulerPos[i][j]=euler_ori[k]*180/M_PI; + unsigned k = 0; + for (size_t j = 3; j < 6; j++, k++) { + EulerPos[i][j] = euler_ori[k] * 180 / M_PI; } - } - for (helper::vector >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) + for (type::vector >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) { for (size_t d = 0; d < posDim; d++) { this->state(it->second + d) = EulerPos[it->first][d]; } + } } - if(estimatePosition.getValue()&&estimateOnlyXYZ.getValue()){ - helper::vector EulerPos; + if ( estimatePosition.getValue() && estimateOnlyXYZ.getValue() ) + { + type::vector EulerPos; EulerPos.resize(pos.size()); - Vector3 eu_pos; + type::Vector3 eu_pos; - for(size_t i= 0; i < pos.size(); i++) { - eu_pos=pos[i].getCenter(); - for(size_t j=0; j <3; j++){ - EulerPos[i][j]=eu_pos[j]; + for (size_t i = 0; i < pos.size(); i++) { + eu_pos = pos[i].getCenter(); + for (size_t j = 0; j < 3; j++) { + EulerPos[i][j] = eu_pos[j]; } - } - for (helper::vector >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) + for ( type::vector >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) { for (size_t d = 0; d < 3; d++) { this->state(it->second + d) = EulerPos[it->first][d]; } + } } - if(estimateVelocity.getValue()){ - for (helper::vector >::iterator it = velocityPairs.begin(); it != velocityPairs.end(); it++) + if ( estimateVelocity.getValue() ) { + for (type::vector >::iterator it = velocityPairs.begin(); it != velocityPairs.end(); it++) { for (size_t d = 0; d < velDim; d++) this->state(it->second + d) = vel[it->first][d]; + } } for (size_t opi = 0; opi < vecOptimParams.size(); opi++) vecOptimParams[opi]->paramsToVector(this->state); - } -/// function that sets SOFA state (position, velocity, parameters) from this->state (stochastic state, Eigen vector) + +/** + * function that sets SOFA state (position, velocity, parameters) from this->state (stochastic state, Eigen vector) + */ template -void StochasticStateWrapper::copyStateFilter2Sofa(const core::MechanicalParams* _mechParams, bool _setVelocityFromPosition) { +void StochasticStateWrapper::copyStateFilter2Sofa(const core::MechanicalParams* _mechParams, bool _setVelocityFromPosition) +{ typename MechanicalState::WriteVecCoord pos = mechanicalState->writePositions(); typename MechanicalState::WriteVecDeriv vel = mechanicalState->writeVelocities(); - if(estimatePosition.getValue()){ - for (helper::vector >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) { + if ( estimatePosition.getValue() ) { + for (type::vector >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) { for (size_t d = 0; d < posDim; d++) { pos[it->first][d] = this->state(it->second + d); } } } - if(estimateVelocity.getValue()){ - for (helper::vector >::iterator it = velocityPairs.begin(); it != velocityPairs.end(); it++) { + if ( estimateVelocity.getValue() ) { + for (type::vector >::iterator it = velocityPairs.begin(); it != velocityPairs.end(); it++) { for (size_t d = 0; d < velDim; d++) { vel[it->first][d] = this->state(it->second + d); } } } /// if velocity is not estimated, it must be computed from the positions at the beginning and at the end of the time step - if (_setVelocityFromPosition) { - if (velocityPairs.empty() && beginTimeStepPos.size() == pos.size()) { - for (helper::vector >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) { + if ( _setVelocityFromPosition ) { + if ( velocityPairs.empty() && beginTimeStepPos.size() == pos.size() ) { + for (type::vector >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) { for (size_t d = 0; d < velDim; d++) { - vel[it->first][d] = (pos[it->first][d] - beginTimeStepPos[it->first][d])/this->getContext()->getDt(); + vel[it->first][d] = (pos[it->first][d] - beginTimeStepPos[it->first][d]) / this->getContext()->getDt(); } } } } - sofa::simulation::MechanicalPropagateOnlyPositionAndVelocityVisitor(_mechParams).execute( this->gnode ); + sofa::simulation::mechanicalvisitor::MechanicalPropagateOnlyPositionAndVelocityVisitor(_mechParams).execute( this->gnode ); /// let the OptimParams to extract the actual values of parameters from the verdandi state for (size_t opi = 0; opi < vecOptimParams.size(); opi++) vecOptimParams[opi]->vectorToParams(this->state); } -/// function that compies SOFA state (position, velocity, parameters) to this->state (stochastic state, Eigen vector) + + +/** + * function that compies SOFA state (position, velocity, parameters) to this->state (stochastic state, Eigen vector) + */ template -void StochasticStateWrapper::copyStateSofa2Filter() { +void StochasticStateWrapper::copyStateSofa2Filter() +{ typename MechanicalState::ReadVecCoord pos = mechanicalState->readPositions(); typename MechanicalState::ReadVecDeriv vel = mechanicalState->readVelocities(); - if(estimatePosition.getValue()){ - for (helper::vector >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) + if ( estimatePosition.getValue() ) { + for (type::vector >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) { for (size_t d = 0; d < posDim; d++) { this->state(it->second + d) = pos[it->first][d]; } - + } } - if(estimateVelocity.getValue()){ - for (helper::vector >::iterator it = velocityPairs.begin(); it != velocityPairs.end(); it++) + if ( estimateVelocity.getValue() ) + { + for (type::vector >::iterator it = velocityPairs.begin(); it != velocityPairs.end(); it++) { for (size_t d = 0; d < velDim; d++) this->state(it->second + d) = vel[it->first][d]; + } } for (size_t opi = 0; opi < vecOptimParams.size(); opi++) vecOptimParams[opi]->paramsToVector(this->state); } -/// create an internal copy of SOFA state (positions and velocities); needed to reinitialize before simulation of each sigma point + + +/** + * create an internal copy of SOFA state (positions and velocities); needed to reinitialize before simulation of each sigma point + */ template -void StochasticStateWrapper::storeMState() { +void StochasticStateWrapper::storeMState() +{ /// store the actual mechanical state (position, velocity) beginTimeStepPos.resize(this->mStateSize); beginTimeStepVel.resize(this->mStateSize); @@ -785,7 +863,7 @@ void StochasticStateWrapper::storeMState() { beginTimeStepPos[i] = pos[i]; beginTimeStepVel[i] = vel[i]; } - if ( mappedState != NULL) { + if ( mappedState != NULL) { beginTimeStepMappedPos.resize(this->mappedMStateSize); typename MappedMechanicalState::ReadVecCoord mapPos = mappedState->readPositions(); for (size_t i = 0; i < this->mappedMStateSize; i++) { @@ -794,40 +872,50 @@ void StochasticStateWrapper::storeMState() { } } -/// re-initialize SOFA state (positions and velocities) from back-up create by storeMState + + +/** + * re-initialize SOFA state (positions and velocities) from back-up create by storeMState + */ template -void StochasticStateWrapper::reinitMState(const core::MechanicalParams* _mechParams) { +void StochasticStateWrapper::reinitMState(const core::MechanicalParams* _mechParams) +{ typename MechanicalState::WriteVecCoord pos = mechanicalState->writePositions(); typename MechanicalState::WriteVecDeriv vel = mechanicalState->writeVelocities(); for (size_t i = 0; i < this->mStateSize; i++) { pos[i] = beginTimeStepPos[i]; vel[i] = beginTimeStepVel[i]; } - if ( mappedState != NULL) { + if ( mappedState != NULL) { typename MappedMechanicalState::WriteVecCoord mapPos = mappedState->writePositions(); for (size_t i = 0; i < this->mappedMStateSize; i++) - mapPos[i] = beginTimeStepMappedPos[i] ; + mapPos[i] = beginTimeStepMappedPos[i]; } - sofa::simulation::MechanicalPropagateOnlyPositionAndVelocityVisitor(_mechParams).execute( this->gnode ); - + sofa::simulation::mechanicalvisitor::MechanicalPropagateOnlyPositionAndVelocityVisitor(_mechParams).execute( this->gnode ); } + template -void StochasticStateWrapper::getActualPosition(int _id, VecCoord& _pos) { +void StochasticStateWrapper::getActualPosition(int _id, VecCoord& _pos) +{ _pos = sigmaStatePos[_id]; } + template -void StochasticStateWrapper::getActualVelocity(int _id, VecDeriv& _vel) { +void StochasticStateWrapper::getActualVelocity(int _id, VecDeriv& _vel) +{ _vel = sigmaStateVel[_id]; } + template -void StochasticStateWrapper::getPos(EVectorX& _state, VecCoord& actualPos) { +void StochasticStateWrapper::getPos(EVectorX& _state, VecCoord& actualPos) +{ if (! (this->filterKind == CLASSIC || this->filterKind == REDORD || this->filterKind == LOCENSEMBLE) ) return; @@ -837,8 +925,8 @@ void StochasticStateWrapper::getPos(EVectorX& _state, Vec typename MechanicalState::WriteVecCoord posW = mechanicalState->writePositions(); - if(estimatePosition.getValue()){ - for (helper::vector >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) { + if ( estimatePosition.getValue() ) { + for (type::vector >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) { for (size_t d = 0; d < posDim; d++) { posW[it->first][d] = _state(it->second + d); } @@ -852,49 +940,57 @@ void StochasticStateWrapper::getPos(EVectorX& _state, Vec typename MechanicalState::WriteVecCoord pos = mechanicalState->writePositions(); - if(estimatePosition.getValue()){ - for (helper::vector >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) { + if ( estimatePosition.getValue() ) { + for (type::vector< std::pair >::iterator it = positionPairs.begin(); it != positionPairs.end(); it++) { for (size_t d = 0; d < posDim; d++) { pos[it->first][d] = savedState(it->second + d); } } } - } + + template -void StochasticStateWrapper::getActualMappedPosition(int _id, Vec3dTypes::VecCoord& _mapPos) { +void StochasticStateWrapper::getActualMappedPosition(int _id, sofa::defaulttype::Vec3dTypes::VecCoord& _mapPos) +{ if ( mappedState != NULL) _mapPos = sigmaMappedStatePos[_id]; } + + template -void StochasticStateWrapper::setState(EVectorX& _state, const core::MechanicalParams* _mparams) { - double dt = this->gnode->getDt(); +void StochasticStateWrapper::setState(EVectorX& _state, const core::MechanicalParams* _mparams) +{ + double dt = this->gnode->getDt(); this->state = _state; copyStateFilter2Sofa(_mparams, true); sofa::helper::AdvancedTimer::stepBegin("UpdateMapping"); - //Visual Information update: Ray Pick add a MechanicalMapping used as VisualMapping - //std::cout << "[" << this->getName() << "]: update mapping" << std::endl; + /// Visual Information update: Ray Pick add a MechanicalMapping used as VisualMapping + // std::cout << "[" << this->getName() << "]: update mapping" << std::endl; this->gnode->template execute< sofa::simulation::UpdateMappingVisitor >(_mparams); sofa::helper::AdvancedTimer::step("UpdateMappingEndEvent"); { - //std::cout << "[" << this->getName() << "]: update mapping end" << std::endl; - sofa::simulation::UpdateMappingEndEvent ev ( dt ); - sofa::simulation::PropagateEventVisitor act ( _mparams , &ev ); - this->gnode->execute ( act ); + // std::cout << "[" << this->getName() << "]: update mapping end" << std::endl; + sofa::simulation::UpdateMappingEndEvent ev( dt ); + sofa::simulation::PropagateEventVisitor act( _mparams , &ev ); + this->gnode->execute( act ); } sofa::helper::AdvancedTimer::stepEnd("UpdateMapping"); sofa::simulation::AnimateEndEvent ev ( dt ); - } + + template -typename StochasticStateWrapper::EMatrixX& StochasticStateWrapper::getStateErrorVariance() { - if (this->stateErrorVariance.rows() == 0) { +typename StochasticStateWrapper::EMatrixX& StochasticStateWrapper::getStateErrorVariance() +{ + if (this->stateErrorVariance.rows() == 0) + { PRNS("Constructing state co-variance matrix") - this->stateErrorVariance.resize(this->stateSize, this->stateSize); + this->stateErrorVariance.resize(this->stateSize, this->stateSize); this->stateErrorVariance.setZero(); size_t vsi = 0; @@ -907,26 +1003,27 @@ typename StochasticStateWrapper::EMatrixX& StochasticStat /// vsi continues to increase since velocity is always after position if (estimateVelocity.getValue()) { for (size_t index = 0; index < (size_t)this->velocityVariance.size(); index++, vsi++) { - this->stateErrorVariance(vsi,vsi) = this->velocityVariance[index]; + this->stateErrorVariance(vsi, vsi) = this->velocityVariance[index]; } } - for (size_t opi = 0; opi < this->vecOptimParams.size(); opi++) { - helper::vector variance; + type::vector variance; this->vecOptimParams[opi]->getInitVariance(variance); for (size_t pi = 0; pi < this->vecOptimParams[opi]->size(); pi++, vsi++) - this->stateErrorVariance(vsi,vsi) = variance[pi]; + this->stateErrorVariance(vsi, vsi) = variance[pi]; } } - //std::cout << "P0: " << this->stateErrorVariance << std::endl; + // std::cout << "P0: " << this->stateErrorVariance << std::endl; return this->stateErrorVariance; } + template -void StochasticStateWrapper::updateStateErrorVariance() { +void StochasticStateWrapper::updateStateErrorVariance() +{ PRNS("Constructing state co-variance matrix") int oldStateVarianceSize = this->stateErrorVariance.rows(); this->stateErrorVariance.conservativeResize(this->stateSize, this->stateSize); @@ -940,10 +1037,10 @@ void StochasticStateWrapper::updateStateErrorVariance() { } size_t vsi = 0; - if (estimatePosition.getValue()) { + if ( estimatePosition.getValue() ) { vsi += (size_t)this->positionVariance.size(); // for (size_t index = 0; index < (size_t)this->positionVariance.size(); index++, vsi++) { - // this->stateErrorVariance(vsi,vsi) = this->positionVariance[index]; + // this->stateErrorVariance(vsi, vsi) = this->positionVariance[index]; // } } @@ -951,200 +1048,215 @@ void StochasticStateWrapper::updateStateErrorVariance() { if (estimateVelocity.getValue()) { vsi += (size_t)this->velocityVariance.size(); // for (size_t index = 0; index < (size_t)this->velocityVariance.size(); index++, vsi++) { - // this->stateErrorVariance(vsi,vsi) = this->velocityVariance[index]; + // this->stateErrorVariance(vsi, vsi) = this->velocityVariance[index]; // } } for (size_t opi = 0; opi < this->vecOptimParams.size(); opi++) { - helper::vector variance; + type::vector variance; this->vecOptimParams[opi]->getInitVariance(variance); - for (size_t pi = 0; pi < this->vecOptimParams[opi]->size(); pi++, vsi++) + for (size_t pi = 0; pi < this->vecOptimParams[opi]->size(); pi++, vsi++) { if (vsi >= static_cast(oldStateVarianceSize)) { this->stateErrorVariance(vsi,vsi) = variance[pi]; } + } } std::cout << "stateErrorVariance: " << this->stateErrorVariance << std::endl; - //std::cout << "P0: " << this->stateErrorVariance << std::endl; + // std::cout << "P0: " << this->stateErrorVariance << std::endl; } + template -typename StochasticStateWrapper::EMatrixX& StochasticStateWrapper::getModelErrorVariance() { - if (this->modelErrorVariance.rows() == 0) { - helper::vector velModStDev, posModStDev; +typename StochasticStateWrapper::EMatrixX& StochasticStateWrapper::getModelErrorVariance() +{ + if (this->modelErrorVariance.rows() == 0) + { + type::vector velModStDev, posModStDev; - if (estimatePosition.getValue()) { + if ( estimatePosition.getValue() ) { posModStDev.resize(posDim); - for (size_t i=0 ;i paramModelStDev = paramModelStdev.getValue(); + type::vector paramModelStDev = paramModelStdev.getValue(); modelErrorVariance = EMatrixX::Identity(this->stateSize, this->stateSize); modelErrorVarianceInverse = EMatrixX::Identity(this->stateSize, this->stateSize) ; - size_t kp= 0; - size_t kv= 0; - size_t k= 0; - const size_t N =freeNodes.size(); - const size_t M =posDim; - const size_t V =velDim; + size_t kp = 0; + size_t kv = 0; + size_t k = 0; + const size_t N = freeNodes.size(); + const size_t M = posDim; + const size_t V = velDim; EVectorX diagPosModelStDev; - if (estimatePosition.getValue()) { - diagPosModelStDev.resize(N*M); - for (size_t i = 0; i < N; i++ ){ - for (size_t j = 0; j < M; j ++) - diagPosModelStDev(i*M+j)=posModStDev[j%M]*posModStDev[j%M]; + if ( estimatePosition.getValue() ) { + diagPosModelStDev.resize(N * M); + for (size_t i = 0; i < N; i++) { + for (size_t j = 0; j < M; j++) + diagPosModelStDev(i * M + j) = posModStDev[j % M] * posModStDev[j % M]; } } EVectorX diagVelModelStDev; - if (estimateVelocity.getValue()) { - diagVelModelStDev.resize(N*V); - for (size_t i = 0; i < N; i++){ - for (size_t j = 0; j < V; j ++) - diagVelModelStDev(i*V+j)=velModStDev[j%V]*velModStDev[j%V]; + if ( estimateVelocity.getValue() ) { + diagVelModelStDev.resize(N * V); + for (size_t i = 0; i < N; i++) { + for (size_t j = 0; j < V; j++) + diagVelModelStDev(i * V + j) = velModStDev[j % V] * velModStDev[j % V]; } } - if (estimatePosition.getValue() && estimateVelocity.getValue()){ + if (estimatePosition.getValue() && estimateVelocity.getValue()) { modelErrorVariance = EMatrixX::Identity(this->stateSize, this->stateSize); for (unsigned index = 0; index < this->positionVariance.size(); index++, kp++) - modelErrorVariance(index,index) = diagPosModelStDev(kp) ; - for (size_t index = this->positionVariance.size(); index < this->reducedStateIndex; index++,kv++) - modelErrorVariance(index,index) = diagVelModelStDev[kv] ; - for (size_t pi = this->reducedStateIndex; pi < this->stateSize; pi++,k++) - if (paramModelStdev.isSet()==0){ - modelErrorVariance(pi,pi) = 0; - }else{ - modelErrorVariance(pi,pi) = paramModelStDev[k] *paramModelStDev[k]; /// Non-null Q for Force + modelErrorVariance(index, index) = diagPosModelStDev(kp); + + for (size_t index = this->positionVariance.size(); index < this->reducedStateIndex; index++, kv++) + modelErrorVariance(index, index) = diagVelModelStDev[kv]; + + for (size_t pi = this->reducedStateIndex; pi < this->stateSize; pi++, k++) + if (paramModelStdev.isSet() == 0) { + modelErrorVariance(pi, pi) = 0; + } else { + /// Non-null Q for Force + modelErrorVariance(pi, pi) = paramModelStDev[k] * paramModelStDev[k]; } } - //NORMALLY IS THE CASE OF DATA ASSIMILATION where Parameters have no Q - if (estimatePosition.getValue() && !estimateVelocity.getValue()){ + // NORMALLY IS THE CASE OF DATA ASSIMILATION where Parameters have no Q + if ( estimatePosition.getValue() && !estimateVelocity.getValue() ) { modelErrorVariance = EMatrixX::Identity(this->stateSize, this->stateSize); + for (unsigned index = 0; index < this->positionVariance.size(); index++, kp++) - modelErrorVariance(index,index) = diagPosModelStDev(kp) ; - for (size_t pi = this->reducedStateIndex; pi < this->stateSize; pi++,k++) - modelErrorVariance(pi,pi) = 0; - } + modelErrorVariance(index, index) = diagPosModelStDev(kp); + for (size_t pi = this->reducedStateIndex; pi < this->stateSize; pi++, k++) + modelErrorVariance(pi, pi) = 0; + } } return this->modelErrorVariance; } + + template -typename StochasticStateWrapper::EVectorX& StochasticStateWrapper::getModelElementNoise() { - if (this->modelElementNoise.size() == 0) { - helper::vector velModStDev, posModStDev; +typename StochasticStateWrapper::EVectorX& StochasticStateWrapper::getModelElementNoise() +{ + if (this->modelElementNoise.size() == 0) + { + type::vector velModStDev, posModStDev; - if (estimatePosition.getValue()) { + if ( estimatePosition.getValue() ) { posModStDev.resize(posDim); - for (size_t i=0 ;i paramModelStDev = paramModelStdev.getValue(); + type::vector paramModelStDev = paramModelStdev.getValue(); this->modelElementNoise = EVectorX::Zero(this->stateSize); - size_t kp= 0; - size_t kv= 0; - size_t k= 0; - const size_t N =freeNodes.size(); - const size_t M =posDim; - const size_t V =velDim; + size_t kp = 0; + size_t kv = 0; + size_t k = 0; + const size_t N = freeNodes.size(); + const size_t M = posDim; + const size_t V = velDim; EVectorX vecPosModelStDev; - if (estimatePosition.getValue()) { - vecPosModelStDev.resize(N*M); - for (size_t i = 0; i < N; i++ ){ - for (size_t j = 0; j < M; j ++) - vecPosModelStDev(i*M+j) = posModStDev[j%M]; + if ( estimatePosition.getValue() ) { + vecPosModelStDev.resize(N * M); + for (size_t i = 0; i < N; i++) { + for (size_t j = 0; j < M; j++) + vecPosModelStDev(i * M + j) = posModStDev[j % M]; } } EVectorX vecVelModelStDev; - if (estimateVelocity.getValue()) { - vecVelModelStDev.resize(N*V); - for (size_t i = 0; i < N; i++){ - for (size_t j = 0; j < V; j ++) - vecVelModelStDev(i*V+j) = velModStDev[j%V]; + if ( estimateVelocity.getValue() ) { + vecVelModelStDev.resize(N * V); + for (size_t i = 0; i < N; i++) { + for (size_t j = 0; j < V; j++) + vecVelModelStDev(i * V + j) = velModStDev[j % V]; } } - if (estimatePosition.getValue() && estimateVelocity.getValue()){ + if (estimatePosition.getValue() && estimateVelocity.getValue()) { for (unsigned index = 0; index < this->positionVariance.size(); index++, kp++) - this->modelElementNoise(index) = vecPosModelStDev(kp) ; - for (size_t index = this->positionVariance.size(); index < this->reducedStateIndex; index++,kv++) - this->modelElementNoise(index) = vecVelModelStDev[kv] ; - for (size_t pi = this->reducedStateIndex; pi < this->stateSize; pi++,k++) - if (paramModelStdev.isSet() == 0){ + this->modelElementNoise(index) = vecPosModelStDev(kp); + + for (size_t index = this->positionVariance.size(); index < this->reducedStateIndex; index++, kv++) + this->modelElementNoise(index) = vecVelModelStDev[kv]; + + for (size_t pi = this->reducedStateIndex; pi < this->stateSize; pi++, k++) { + if (paramModelStdev.isSet() == 0) { this->modelElementNoise(pi) = 0; } else { - this->modelElementNoise(pi) = paramModelStDev[k] * paramModelStDev[k]; /// Non-null Q for Force + /// Non-null Q for Force + this->modelElementNoise(pi) = paramModelStDev[k] * paramModelStDev[k]; } + } } - //NORMALLY IS THE CASE OF DATA ASSIMILATION where Parameters have no Q - if (estimatePosition.getValue() && !estimateVelocity.getValue()){ + /// NORMALLY IS THE CASE OF DATA ASSIMILATION where Parameters have no Q + if (estimatePosition.getValue() && !estimateVelocity.getValue()) { for (unsigned index = 0; index < this->positionVariance.size(); index++, kp++) this->modelElementNoise(index) = vecPosModelStDev(kp); - for (size_t pi = this->reducedStateIndex; pi < this->stateSize; pi++,k++) + + for (size_t pi = this->reducedStateIndex; pi < this->stateSize; pi++, k++) this->modelElementNoise(pi) = 0; } - } return this->modelElementNoise; - } + template -void StochasticStateWrapper::updateModelErrorVariance() { +void StochasticStateWrapper::updateModelErrorVariance() +{ PRNS("Constructing state co-variance matrix") int oldModelVarianceSize = this->stateErrorVariance.rows(); modelErrorVariance.conservativeResize(this->stateSize, this->stateSize); - // set zeros to new added elements + /// set zeros to new added elements for (int first_index = 0; first_index < modelErrorVariance.rows(); first_index++) { for (int second_index = oldModelVarianceSize; second_index < modelErrorVariance.rows(); second_index++) { modelErrorVariance(first_index, second_index) = 0.0; @@ -1153,40 +1265,48 @@ void StochasticStateWrapper::updateModelErrorVariance() { } } + + template -typename StochasticStateWrapper::EMatrixX& StochasticStateWrapper::getStateErrorVarianceReduced() { +typename StochasticStateWrapper::EMatrixX& StochasticStateWrapper::getStateErrorVarianceReduced() +{ if (this->stateErrorVarianceReduced.rows() == 0) { this->stateErrorVarianceReduced.resize(this->reducedStateSize,this->reducedStateSize); this->stateErrorVarianceReduced.setZero(); size_t vpi = 0; for (size_t opi = 0; opi < this->vecOptimParams.size(); opi++) { - helper::vector variance; + type::vector variance; this->vecOptimParams[opi]->getInitVariance(variance); for (size_t pi = 0; pi < this->vecOptimParams[opi]->size(); pi++, vpi++) { - this->stateErrorVarianceReduced(vpi,vpi) = Type(Type(1.0) / variance[pi]); + this->stateErrorVarianceReduced(vpi, vpi) = Type(Type(1.0) / variance[pi]); } } } return this->stateErrorVarianceReduced; } + + template -typename StochasticStateWrapper::EMatrixX& StochasticStateWrapper::getStateErrorVarianceProjector() { +typename StochasticStateWrapper::EMatrixX& StochasticStateWrapper::getStateErrorVarianceProjector() +{ if (this->stateErrorVarianceProjector.rows() == 0) { this->stateErrorVarianceProjector.resize(this->stateSize, this->reducedStateSize); this->stateErrorVarianceProjector.setZero(); for (size_t i = 0; i < this->reducedStateSize; i++) - this->stateErrorVarianceProjector(i+this->reducedStateIndex,i) = Type(1.0); + this->stateErrorVarianceProjector(i+this->reducedStateIndex, i) = Type(1.0); } return this->stateErrorVarianceProjector; } + template -void StochasticStateWrapper::initializeStep(size_t _stepNumber) { +void StochasticStateWrapper::initializeStep(size_t _stepNumber) +{ PRNS("Initialize time step" << _stepNumber); Inherit::initializeStep(_stepNumber); /// storing the initial state does not hurt for any type of the filter @@ -1197,22 +1317,25 @@ void StochasticStateWrapper::initializeStep(size_t _stepN sigmaStatePos.clear(); sigmaStateVel.clear(); sigmaMappedStatePos.clear(); - } -template -void StochasticStateWrapper::computeSimulationStep(EVectorX &_state, const core::MechanicalParams *_mparams, int& _stateID) { - if (this->filterKind == SIMCORR) { + +template +void StochasticStateWrapper::computeSimulationStep(EVectorX &_state, const core::MechanicalParams *_mparams, int& _stateID) +{ + if (this->filterKind == SIMCORR) + { reinitMState(_mparams); this->state = _state; copyStateFilter2Sofa(_mparams); - if (this->d_langrangeMultipliers.getValue()) + if (this->d_langrangeMultipliers.getValue()) { computeSofaStepWithLM(_mparams); - else + } else { computeSofaStep(_mparams, false); + } /// store the result of the simulation as a vector VecCoord actualPos(this->mStateSize); @@ -1234,8 +1357,10 @@ void StochasticStateWrapper::computeSimulationStep(EVecto } + template -void StochasticStateWrapper::transformState(EVectorX &_vecX, const core::MechanicalParams *_mparams, int* _stateID) { +void StochasticStateWrapper::transformState(EVectorX &_vecX, const core::MechanicalParams *_mparams, int* _stateID) +{ if (! (this->filterKind == CLASSIC || this->filterKind == REDORD || this->filterKind == LOCENSEMBLE) ) return; @@ -1248,10 +1373,11 @@ void StochasticStateWrapper::transformState(EVectorX &_ve copyStateFilter2Sofa(_mparams); - if (this->d_langrangeMultipliers.getValue()) + if (this->d_langrangeMultipliers.getValue()) { computeSofaStepWithLM(_mparams); - else + } else { computeSofaStep(_mparams, false); + } copyStateSofa2Filter(); _vecX = this->state; @@ -1261,7 +1387,6 @@ void StochasticStateWrapper::transformState(EVectorX &_ve VecCoord actualPos(this->mStateSize); VecDeriv actualVel(this->mStateSize); - typename MechanicalState::ReadVecCoord pos = mechanicalState->readPositions(); typename MechanicalState::ReadVecDeriv vel = mechanicalState->readVelocities(); for (size_t i = 0; i < this->mStateSize; i++) { @@ -1271,8 +1396,8 @@ void StochasticStateWrapper::transformState(EVectorX &_ve sigmaStatePos.push_back(actualPos); sigmaStateVel.push_back(actualVel); - if ( mappedState != NULL) { - Vec3dTypes::VecCoord actualMappedPos(this->mappedMStateSize); + if ( mappedState != NULL) { + sofa::defaulttype::Vec3dTypes::VecCoord actualMappedPos(this->mappedMStateSize); typename MappedMechanicalState::ReadVecCoord mapPos = mappedState->readPositions(); for (size_t i = 0; i < this->mappedMStateSize; i++) { @@ -1300,164 +1425,177 @@ void StochasticStateWrapper::transformState(EVectorX &_ve sigmaStateVel.push_back(actualVel); } - if ((_stateID == nullptr) || (*_stateID >= 0)) { this->state = savedState; copyStateFilter2Sofa(_mparams); } } + + + template -void StochasticStateWrapper::lastApplyOperator(EVectorX& /* _vecX */, const core::MechanicalParams* /* _mparams */) { +void StochasticStateWrapper::lastApplyOperator(EVectorX& /* _vecX */, const core::MechanicalParams* /* _mparams */) +{ if (! (this->filterKind == CLASSIC || this->filterKind == LOCENSEMBLE) ) return; -// this->state = _vecX; -// reinitMState(_mparams); + // this->state = _vecX; + // reinitMState(_mparams); -// copyStateFilter2Sofa(_mparams); + // copyStateFilter2Sofa(_mparams); -// if (this->d_langrangeMultipliers.getValue()) -// computeSofaStepWithLM(_mparams); -// else -// computeSofaStep(_mparams, false); -// copyStateSofa2Filter() + // if (this->d_langrangeMultipliers.getValue()) + // computeSofaStepWithLM(_mparams); + // else + // computeSofaStep(_mparams, false); + // copyStateSofa2Filter() typename MechanicalState::ReadVecCoord Initpos = mechanicalState->readPositions(); typename MechanicalState::ReadVecDeriv vel = mechanicalState->readVelocities(); typename MechanicalState::WriteVecCoord pos = mechanicalState->writePositions(); + for (size_t i = 0; i < this->mStateSize; i++) { - pos[i] = Initpos[i] + vel[i]*this->gnode->getDt() ; + pos[i] = Initpos[i] + vel[i] * this->gnode->getDt(); } - } + template -void StochasticStateWrapper::draw(const core::visual::VisualParams* vparams ) { - if (d_draw.getValue()){ - if (vparams->displayFlags().getShowVisualModels()) { - std::vector points; - for (unsigned i =0; i < sigmaStatePos.size(); i++){ +void StochasticStateWrapper::draw(const core::visual::VisualParams* vparams ) +{ + if (d_draw.getValue()) + { + if ( vparams->displayFlags().getShowVisualModels() ) + { + std::vector points; + for (unsigned i = 0; i < sigmaStatePos.size(); i++) + { VecCoord &pts = sigmaStatePos[i]; points.resize(pts.size()); - for(unsigned j =0; j < pts.size(); j++){ - points[j][0]=pts[j][0]; - points[j][1]=pts[j][1]; - points[j][2]=pts[j][2]; + for(unsigned j = 0; j < pts.size(); j++) { + points[j][0] = pts[j][0]; + points[j][1] = pts[j][1]; + points[j][2] = pts[j][2]; } helper::types::RGBAColor color; - switch (i) { - case 0: color = helper::types::RGBAColor(1.0,0.0,0.0,1.0); break; - case 1: color = helper::types::RGBAColor(0.0,1.0,0.0,1.0); break; - case 2: color = helper::types::RGBAColor(0.0,0.0,1.0,1.0); break; + switch(i) { + case 0: color = helper::types::RGBAColor(1.0, 0.0, 0.0, 1.0); break; + case 1: color = helper::types::RGBAColor(0.0, 1.0, 0.0, 1.0); break; + case 2: color = helper::types::RGBAColor(0.0, 0.0, 1.0, 1.0); break; default: color = helper::types::RGBAColor(0.5, 0.5, 0.5, 0.5); } vparams->drawTool()->setPolygonMode(0,vparams->displayFlags().getShowWireFrame()); vparams->drawTool()->setLightingEnabled(true); //Enable lightning vparams->drawTool()->drawSpheres(points, d_radius_draw.getValue(), color); // sofa::defaulttype::Vec<4, float>(color[i],0.8f,colorB[i],1.0f)); - vparams->drawTool()->setPolygonMode(0,false); - vparams->drawTool()->drawLineStrip(points, 3.0, helper::types::RGBAColor(color[i],0.8f,colorB[i],1.0f)); - + vparams->drawTool()->setPolygonMode(0, false); + vparams->drawTool()->drawLineStrip(points, 3.0, helper::types::RGBAColor(color[i], 0.8f, colorB[i], 1.0f)); } } } - } -/// perform a simulation step, code taken from DefaultAnimationLoop + + +/** + * perform a simulation step, code taken from DefaultAnimationLoop + */ template -void StochasticStateWrapper::computeSofaStep(const core::ExecParams* execParams, bool _updateTime) { - double dt = this->gnode->getDt(); - //core::ExecParams* execParams = sofa::core::ExecParams::defaultInstance(); +void StochasticStateWrapper::computeSofaStep(const core::ExecParams* execParams, bool _updateTime) +{ + double dt = this->gnode->getDt(); + // core::ExecParams* execParams = sofa::core::ExecParams::defaultInstance(); - //std::cout << "[" << this->getName() << "]: step default begin at time = " << gnode->getTime() << " update time: " << _update_time << std::endl; + // std::cout << "[" << this->getName() << "]: step default begin at time = " << gnode->getTime() << " update time: " << _update_time << std::endl; sofa::helper::AdvancedTimer::stepBegin("AnimationStep"); - //std::cout<<"step "<getName() << "]: animate begin" << std::endl; - sofa::simulation::AnimateBeginEvent ev ( dt ); - sofa::simulation::PropagateEventVisitor act ( execParams, &ev ); - this->gnode->execute ( act ); - //std::cout<<"step "<getName() << "]: animate begin" << std::endl; + sofa::simulation::AnimateBeginEvent ev( dt ); + sofa::simulation::PropagateEventVisitor act( execParams, &ev ); + this->gnode->execute( act ); + // std::cout<<"step "<gnode->getTime(); - //std::cout<<"step "<getName() << "]: behaviour update position" << std::endl; + // std::cout<<"step "<getName() << "]: behaviour update position" << std::endl; sofa::simulation::BehaviorUpdatePositionVisitor beh(execParams , dt); - this->gnode->execute ( beh ); - //std::cout<<"step "<getName() << "]: update internal data" << std::endl; + this->gnode->execute( beh ); + // std::cout<<"step "<getName() << "]: update internal data" << std::endl; sofa::simulation::UpdateInternalDataVisitor uid(execParams); - this->gnode->execute ( uid ); - //std::cout<<"step "<getName() << "]: animate" << std::endl; + this->gnode->execute( uid ); + // std::cout<<"step "<getName() << "]: animate" << std::endl; sofa::simulation::AnimateVisitor act(execParams, dt); - this->gnode->execute ( act ); + this->gnode->execute( act ); if (_updateTime) { - //std::cout << "[" << this->getName() << "]: update simulation context" << std::endl; - this->gnode->setTime ( startTime + dt ); - this->gnode->template execute< sofa::simulation::UpdateSimulationContextVisitor >(execParams); + // std::cout << "[" << this->getName() << "]: update simulation context" << std::endl; + this->gnode->setTime( startTime + dt ); + this->gnode->template execute< sofa::simulation::UpdateSimulationContextVisitor >(execParams); } - //std::cout<<"step "<getName() << "]: animate end" << std::endl; - sofa::simulation::AnimateEndEvent ev ( dt ); - sofa::simulation::PropagateEventVisitor act (execParams, &ev ); - this->gnode->execute ( act ); + // std::cout << "[" << this->getName() << "]: animate end" << std::endl; + sofa::simulation::AnimateEndEvent ev( dt ); + sofa::simulation::PropagateEventVisitor act(execParams, &ev ); + this->gnode->execute( act ); } sofa::helper::AdvancedTimer::stepBegin("UpdateMapping"); - //Visual Information update: Ray Pick add a MechanicalMapping used as VisualMapping - //std::cout << "[" << this->getName() << "]: update mapping" << std::endl; - this->gnode->template execute< sofa::simulation::UpdateMappingVisitor >(execParams); + // Visual Information update: Ray Pick add a MechanicalMapping used as VisualMapping + // std::cout << "[" << this->getName() << "]: update mapping" << std::endl; + this->gnode->template execute< sofa::simulation::UpdateMappingVisitor >(execParams); sofa::helper::AdvancedTimer::step("UpdateMappingEndEvent"); { - //std::cout << "[" << this->getName() << "]: update mapping end" << std::endl; - sofa::simulation::UpdateMappingEndEvent ev ( dt ); - sofa::simulation::PropagateEventVisitor act ( execParams , &ev ); - this->gnode->execute ( act ); + // std::cout << "[" << this->getName() << "]: update mapping end" << std::endl; + sofa::simulation::UpdateMappingEndEvent ev( dt ); + sofa::simulation::PropagateEventVisitor act( execParams , &ev ); + this->gnode->execute( act ); } sofa::helper::AdvancedTimer::stepEnd("UpdateMapping"); -#ifndef SOFA_NO_UPDATE_BBOX - sofa::helper::AdvancedTimer::stepBegin("UpdateBBox"); - this->gnode->template execute< sofa::simulation::UpdateBoundingBoxVisitor >(execParams); - sofa::helper::AdvancedTimer::stepEnd("UpdateBBox"); -#endif -#ifdef SOFA_DUMP_VISITOR_INFO - simulation::Visitor::printCloseNode("Step"); -#endif + #ifndef SOFA_NO_UPDATE_BBOX + sofa::helper::AdvancedTimer::stepBegin("UpdateBBox"); + this->gnode->template execute< sofa::simulation::UpdateBoundingBoxVisitor >(execParams); + sofa::helper::AdvancedTimer::stepEnd("UpdateBBox"); + #endif + #ifdef SOFA_DUMP_VISITOR_INFO + simulation::Visitor::printCloseNode("Step"); + #endif //sofa::helper::AdvancedTimer::end("Animate"); sofa::helper::AdvancedTimer::stepEnd("AnimationStep"); - } -/// perform a simulation step with collisions, code taken from FreeMotionAnimationLoop + +/** + * perform a simulation step with collisions, code taken from FreeMotionAnimationLoop + */ template -void StochasticStateWrapper::computeSofaStepWithLM(const core::ExecParams* params) { - //if (dt == 0) +void StochasticStateWrapper::computeSofaStepWithLM(const core::ExecParams* params) +{ + // if (dt == 0) SReal dt = this->gnode->getDt(); - { - sofa::simulation::AnimateBeginEvent ev ( dt ); - sofa::simulation::PropagateEventVisitor act ( params, &ev ); - this->gnode->execute ( act ); + sofa::simulation::AnimateBeginEvent ev( dt ); + sofa::simulation::PropagateEventVisitor act( params, &ev ); + this->gnode->execute( act ); } double startTime = this->gnode->getTime(); @@ -1478,27 +1616,28 @@ void StochasticStateWrapper::computeSofaStepWithLM(const cparams.setOrder(m_solveVelocityConstraintFirst.getValue() ? core::ConstraintParams::VEL : core::ConstraintParams::POS_AND_VEL); { - core::behavior::MultiVecDeriv dx(&vop, core::VecDerivId::dx()); dx.realloc(&vop, true, true); - core::behavior::MultiVecDeriv df(&vop, core::VecDerivId::dforce()); df.realloc(&vop, true, true); + core::behavior::MultiVecDeriv dx(&vop, core::VecDerivId::dx()); + dx.realloc( &vop, true, true ); + core::behavior::MultiVecDeriv df(&vop, core::VecDerivId::dforce()); + df.realloc( &vop, true, true ); } - - // This solver will work in freePosition and freeVelocity vectors. - // We need to initialize them if it's not already done. + /// This solver will work in freePosition and freeVelocity vectors. + /// We need to initialize them if it's not already done. sofa::helper::AdvancedTimer::stepBegin("MechanicalVInitVisitor"); - simulation::MechanicalVInitVisitor< core::V_COORD >(params, core::VecCoordId::freePosition(), core::ConstVecCoordId::position(), true).execute(this->gnode); - simulation::MechanicalVInitVisitor< core::V_DERIV >(params, core::VecDerivId::freeVelocity(), core::ConstVecDerivId::velocity(), true).execute(this->gnode); + sofa::simulation::mechanicalvisitor::MechanicalVInitVisitor< core::V_COORD >(params, core::VecCoordId::freePosition(), core::ConstVecCoordId::position(), true).execute(this->gnode); + sofa::simulation::mechanicalvisitor::MechanicalVInitVisitor< core::V_DERIV >(params, core::VecDerivId::freeVelocity(), core::ConstVecDerivId::velocity(), true).execute(this->gnode); sofa::helper::AdvancedTimer::stepEnd("MechanicalVInitVisitor"); { sofa::helper::AdvancedTimer::stepBegin("AnimateBeginEvent"); - AnimateBeginEvent ev ( dt ); - PropagateEventVisitor act ( params, &ev ); - this->gnode->execute ( act ); + AnimateBeginEvent ev( dt ); + PropagateEventVisitor act( params, &ev ); + this->gnode->execute( act ); sofa::helper::AdvancedTimer::stepEnd("AnimateBeginEvent"); } - sofa::simulation::BehaviorUpdatePositionVisitor beh(params , dt); + sofa::simulation::BehaviorUpdatePositionVisitor beh(params, dt); using helper::system::thread::CTime; using sofa::helper::AdvancedTimer; @@ -1527,8 +1666,7 @@ void StochasticStateWrapper::computeSofaStepWithLM(const dmsg_info() << "updatePos performed - beginVisitor called" ; - - sofa::simulation::MechanicalBeginIntegrationVisitor beginVisitor(params, dt); + sofa::simulation::mechanicalvisitor::MechanicalBeginIntegrationVisitor beginVisitor(params, dt); this->gnode->execute(&beginVisitor); dmsg_info() << "beginVisitor performed - SolveVisitor for freeMotion is called" ; @@ -1536,10 +1674,10 @@ void StochasticStateWrapper::computeSofaStepWithLM(const // Mapping geometric stiffness coming from previous lambda. { - simulation::MechanicalVOpVisitor lambdaMultInvDt(params, cparams.lambda(), sofa::core::ConstMultiVecId::null(), cparams.lambda(), 1.0 / dt); + simulation::mechanicalvisitor::MechanicalVOpVisitor lambdaMultInvDt(params, cparams.lambda(), sofa::core::ConstMultiVecId::null(), cparams.lambda(), 1.0 / dt); lambdaMultInvDt.setMapped(true); this->getContext()->executeVisitor(&lambdaMultInvDt); - simulation::MechanicalComputeGeometricStiffness geometricStiffnessVisitor(&mop.mparams, cparams.lambda()); + simulation::mechanicalvisitor::MechanicalComputeGeometricStiffness geometricStiffnessVisitor(&mop.mparams, cparams.lambda()); this->getContext()->executeVisitor(&geometricStiffnessVisitor); } @@ -1549,27 +1687,25 @@ void StochasticStateWrapper::computeSofaStepWithLM(const this->gnode->execute(&freeMotion); sofa::helper::AdvancedTimer::stepEnd("Step2 FreeMotion"); - mop.projectResponse(freeVel); mop.propagateDx(freeVel, true); - if (cparams.constOrder() == core::ConstraintParams::POS || - cparams.constOrder() == core::ConstraintParams::POS_AND_VEL) + if (cparams.constOrder() == core::ConstraintParams::POS || cparams.constOrder() == core::ConstraintParams::POS_AND_VEL) { // xfree = x + vfree*dt - simulation::MechanicalVOpVisitor freePosEqPosPlusFreeVelDt(params, freePos, pos, freeVel, dt); + simulation::mechanicalvisitor::MechanicalVOpVisitor freePosEqPosPlusFreeVelDt(params, freePos, pos, freeVel, dt); freePosEqPosPlusFreeVelDt.setMapped(true); this->getContext()->executeVisitor(&freePosEqPosPlusFreeVelDt); } - dmsg_info() << " SolveVisitor for freeMotion performed" ; + dmsg_info() << " SolveVisitor for freeMotion performed"; - // Collision detection and response creation + /// Collision detection and response creation sofa::helper::AdvancedTimer::stepBegin("Step3 ComputeCollision"); this->computeCollision(params); sofa::helper::AdvancedTimer::stepEnd("Step3 ComputeCollision"); - // Solve constraints + /// Solve constraints if (constraintSolver) { sofa::helper::AdvancedTimer::stepBegin("ConstraintSolver"); @@ -1580,9 +1716,7 @@ void StochasticStateWrapper::computeSofaStepWithLM(const // x_t+1 = x_t + ( vfree + dv ) * dt pos.eq(pos, vel, dt); - } - else - { + } else { constraintSolver->solveConstraint(&cparams, pos, vel); } @@ -1594,91 +1728,92 @@ void StochasticStateWrapper::computeSofaStepWithLM(const } - simulation::MechanicalEndIntegrationVisitor endVisitor(params, dt); + simulation::mechanicalvisitor::MechanicalEndIntegrationVisitor endVisitor(params, dt); this->gnode->execute(&endVisitor); mop.projectPositionAndVelocity(pos, vel); mop.propagateXAndV(pos, vel); - this->gnode->setTime ( startTime + dt ); + this->gnode->setTime( startTime + dt ); this->gnode-> template execute(params); // propagate time { - AnimateEndEvent ev ( dt ); - PropagateEventVisitor act ( params, &ev ); - this->gnode->execute ( act ); + AnimateEndEvent ev( dt ); + PropagateEventVisitor act( params, &ev ); + this->gnode->execute( act ); } - sofa::helper::AdvancedTimer::stepBegin("UpdateMapping"); - //Visual Information update: Ray Pick add a MechanicalMapping used as VisualMapping + // Visual Information update: Ray Pick add a MechanicalMapping used as VisualMapping this->gnode->template execute(params); -// sofa::helper::AdvancedTimer::step("UpdateMappingEndEvent"); + // sofa::helper::AdvancedTimer::step("UpdateMappingEndEvent"); { - UpdateMappingEndEvent ev ( dt ); - PropagateEventVisitor act ( params , &ev ); - this->gnode->execute ( act ); + UpdateMappingEndEvent ev( dt ); + PropagateEventVisitor act( params , &ev ); + this->gnode->execute( act ); } sofa::helper::AdvancedTimer::stepEnd("UpdateMapping"); - - -#ifndef SOFA_NO_UPDATE_BBOX - this->gnode->template execute(params); -#endif + #ifndef SOFA_NO_UPDATE_BBOX + this->gnode->template execute(params); + #endif sofa::helper::AdvancedTimer::stepEnd("MechanicalVInitVisitor"); - } + + template void StochasticStateWrapper::computeCollision(const core::ExecParams* params) { { CollisionBeginEvent evBegin; - PropagateEventVisitor eventPropagation( params, &evBegin); + PropagateEventVisitor eventPropagation(params, &evBegin); eventPropagation.execute(this->getContext()); } CollisionVisitor act(params); - act.setTags(this->getTags()); + act.setTags( this->getTags() ); act.execute( this->getContext() ); { CollisionEndEvent evEnd; - PropagateEventVisitor eventPropagation( params, &evEnd); + PropagateEventVisitor eventPropagation(params, &evEnd); eventPropagation.execute(this->getContext()); } } + + template void StochasticStateWrapper::integrate(const core::ExecParams* params, SReal dt) { - { IntegrateBeginEvent evBegin; - PropagateEventVisitor eventPropagation( params, &evBegin); + PropagateEventVisitor eventPropagation(params, &evBegin); eventPropagation.execute(this->getContext()); } - MechanicalIntegrationVisitor act( params, dt ); - act.setTags(this->getTags()); + simulation::mechanicalvisitor::MechanicalIntegrationVisitor act( params, dt ); + act.setTags( this->getTags() ); act.execute( this->getContext() ); { IntegrateEndEvent evBegin; - PropagateEventVisitor eventPropagation( params, &evBegin); + PropagateEventVisitor eventPropagation(params, &evBegin); eventPropagation.execute(this->getContext()); } } -/*template -const StochasticStateWrapper::Solvers& StochasticStateWrapper::getSolverSequence() -{ - simulation::Node* gnode = dynamic_cast( getContext() ); - assert( gnode ); - return gnode->solver; -}*/ + + +//template +//const StochasticStateWrapper::Solvers& StochasticStateWrapper::getSolverSequence() +//{ +// simulation::Node* gnode = dynamic_cast( getContext() ); +// assert( gnode ); +// return gnode->solver; +//} diff --git a/src/stochasticFiltering/StochasticStateWrapperBase.h b/src/stochasticFiltering/StochasticStateWrapperBase.h index 272c354c..ce4eeb22 100644 --- a/src/stochasticFiltering/StochasticStateWrapperBase.h +++ b/src/stochasticFiltering/StochasticStateWrapperBase.h @@ -21,18 +21,18 @@ ******************************************************************************/ #pragma once +#include #include +#include +#include #include -#include #include "initOptimusPlugin.h" -#ifdef Success -#undef Success // dirty workaround to cope with the (dirtier) X11 define. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=253 -#endif #include + namespace sofa { @@ -43,19 +43,18 @@ namespace stochastic { -class StochasticStateWrapperBase: public sofa::core::objectmodel::BaseObject + +class StochasticStateWrapperBase : public sofa::core::objectmodel::BaseObject { public: - SOFA_ABSTRACT_CLASS(StochasticStateWrapperBase, BaseObject); + SOFA_ABSTRACT_CLASS(StochasticStateWrapperBase, core::objectmodel::BaseObject); typedef sofa::core::objectmodel::BaseObject Inherit; - StochasticStateWrapperBase() - : Inherit() - , verbose( initData(&verbose, false, "verbose", "print tracing informations") ) - , slave( initData(&slave, false, "slave", "slave wrapper (needed only for parallelization") ) - , writeStateFile( initData(&writeStateFile, "writeStateFile", "write state at the end of the correction (writeState mode)") ) - { - } + Data verbose; + Data slave; + sofa::core::objectmodel::DataFileName writeStateFile; + std::fstream stateFile; + protected: sofa::simulation::Node* gnode; @@ -66,12 +65,15 @@ class StochasticStateWrapperBase: public sofa::core::objectmodel::BaseObject double startTime, stopTime; public: - Data verbose; - Data slave; - sofa::core::objectmodel::DataFileName writeStateFile; - std::fstream stateFile; + StochasticStateWrapperBase() + : Inherit() + , verbose( initData(&verbose, false, "verbose", "print tracing informations") ) + , slave( initData(&slave, false, "slave", "slave wrapper (needed only for parallelization") ) + , writeStateFile( initData(&writeStateFile, "writeStateFile", "write state at the end of the correction (writeState mode)") ) + { } - void init() override { + void init() override + { Inherit::init(); gnode = dynamic_cast(this->getContext()); @@ -88,37 +90,34 @@ class StochasticStateWrapperBase: public sofa::core::objectmodel::BaseObject } } - virtual void initializeStep(size_t _stepNumber) { + virtual void initializeStep(size_t _stepNumber) + { stepNumber = _stepNumber; actualTime = double(stepNumber)*gnode->getDt(); //PRNS("========= Initialize DA step T = " << actualTime); } - bool isSlave() { + bool isSlave() + { return slave.getValue(); } - virtual void writeState(double) { - } -}; /// class + virtual void writeState(double) { } +}; + + template -class StochasticStateWrapperBaseT: public sofa::component::stochastic::StochasticStateWrapperBase +class StochasticStateWrapperBaseT : public sofa::component::stochastic::StochasticStateWrapperBase { public: SOFA_CLASS(SOFA_TEMPLATE(StochasticStateWrapperBaseT, FilterType), StochasticStateWrapperBase); typedef sofa::component::stochastic::StochasticStateWrapperBase Inherit; - typedef typename Eigen::Matrix EMatrixX; typedef typename Eigen::Matrix EVectorX; - StochasticStateWrapperBaseT() - : Inherit() - , filterKind(UNDEF) { - } - protected: /// stochastic state size_t stateSize; @@ -145,15 +144,23 @@ class StochasticStateWrapperBaseT: public sofa::component::stochastic::Stochasti /// size of the associated mechanical state size_t mStateSize; /// size of the mechanical state size_t mappedMStateSize; /// size of the mapped mechanical state + + public: - void init() override { + StochasticStateWrapperBaseT() + : Inherit() + , filterKind(UNDEF) + { } + + void init() override + { Inherit::init(); } virtual void updateState(bool) { } /// function required by classical and reduced-order filters (preform simulation -> compute new sigma state) - virtual void transformState(EVectorX& _vecX, const core::MechanicalParams* mparams, int* _stateID = nullptr) = 0; + virtual void transformState(EVectorX& _vecX, const core::MechanicalParams* mparams, int* _stateID = nullptr) = 0; virtual void lastApplyOperator(EVectorX& _vecX, const core::MechanicalParams* mparams) = 0; /// function required by sim-corr filters (perform simulation -> store results internally to compute the observation) @@ -161,68 +168,89 @@ class StochasticStateWrapperBaseT: public sofa::component::stochastic::Stochasti /// function to be executed at the beginning of the time step - virtual void setFilterKind(FilterKind _kind) { + virtual void setFilterKind(FilterKind _kind) + { filterKind = _kind; } - virtual FilterType getFilterKind() { + virtual FilterType getFilterKind() + { return filterKind; } - virtual EVectorX& getState() { + virtual EVectorX& getState() + { return state; } - virtual void setState(EVectorX& _state, const core::MechanicalParams* /*_mparams*/) { + virtual void setState(EVectorX& _state, const core::MechanicalParams* /*_mparams*/) + { state = _state; } - size_t getStateSize() { + size_t getStateSize() + { return state.rows(); } - virtual bool& estimPosition() { - return EstimatePOSITION ; + + virtual bool& estimPosition() + { + return EstimatePOSITION; } - virtual bool& estimOnlyXYZ() { - return EstimateONLYXYZ ; + + virtual bool& estimOnlyXYZ() + { + return EstimateONLYXYZ; } - virtual bool& declaredMapState() { + virtual bool& declaredMapState() + { return declaredMappedState; } - virtual EMatrixX& getStateErrorVariance() { + virtual EMatrixX& getStateErrorVariance() + { return stateErrorVariance; } - virtual EMatrixX& getModelErrorVariance() { + + virtual EMatrixX& getModelErrorVariance() + { return modelErrorVariance; } - virtual EVectorX& getModelElementNoise() { + + virtual EVectorX& getModelElementNoise() + { return modelElementNoise; } - virtual EMatrixX& getStateErrorVarianceDevUKF() { + virtual EMatrixX& getStateErrorVarianceDevUKF() + { return stateErrorVariance; } - virtual EMatrixX& getStateErrorVarianceUKF() { + virtual EMatrixX& getStateErrorVarianceUKF() + { return stateErrorVariance; } /// for reduced-order filtering - virtual void setStateErrorVarianceProjector(EMatrixX& _mat) { + virtual void setStateErrorVarianceProjector(EMatrixX& _mat) + { stateErrorVarianceProjector = _mat; } - virtual EMatrixX& getStateErrorVarianceProjector() { + virtual EMatrixX& getStateErrorVarianceProjector() + { return stateErrorVarianceProjector; } - virtual EMatrixX& getStateErrorVarianceReduced() { + virtual EMatrixX& getStateErrorVarianceReduced() + { return stateErrorVarianceReduced; } - virtual void writeState(double timeStep) override { + virtual void writeState(double timeStep) override + { if (this->stateFile.is_open()) { this->stateFile << "T= " << timeStep << std::endl << " X= "; for (size_t i = 0; i < reducedStateIndex; i++) @@ -239,8 +267,8 @@ class StochasticStateWrapperBaseT: public sofa::component::stochastic::Stochasti // stateErrorVarianceRow = stateErrorVariance.row(rowIndex); // return stateErrorVarianceRow; //} +}; -}; /// class } // stochastic diff --git a/src/stochasticFiltering/UKFilterClassic.cpp b/src/stochasticFiltering/UKFilterClassic.cpp index d8844a48..a984a692 100644 --- a/src/stochasticFiltering/UKFilterClassic.cpp +++ b/src/stochasticFiltering/UKFilterClassic.cpp @@ -25,6 +25,7 @@ //#include + namespace sofa { @@ -34,9 +35,9 @@ namespace component namespace stochastic { -using namespace defaulttype; +using namespace defaulttype; SOFA_DECL_CLASS(UKFilterClassic) @@ -51,6 +52,7 @@ int UKFilterClassicClass = core::RegisterObject("UKFilterClassicOrig") #endif ; + #ifndef SOFA_FLOAT template class SOFA_STOCHASTIC_API UKFilterClassic; #endif @@ -59,6 +61,7 @@ template class SOFA_STOCHASTIC_API UKFilterClassic; #endif + } // namespace stochastic } // namespace component diff --git a/src/stochasticFiltering/UKFilterClassic.h b/src/stochasticFiltering/UKFilterClassic.h index 52517f98..d772d504 100644 --- a/src/stochasticFiltering/UKFilterClassic.h +++ b/src/stochasticFiltering/UKFilterClassic.h @@ -21,6 +21,7 @@ ******************************************************************************/ #pragma once +/* include files */ #include "initOptimusPlugin.h" #include "StochasticFilterBase.h" #include "StochasticStateWrapper.h" @@ -34,10 +35,6 @@ #include #include -#ifdef Success -#undef Success // dirty workaround to cope with the (dirtier) X11 define. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=253 -#endif -#include #include #include //#include @@ -47,6 +44,9 @@ #include #include +#include + + namespace sofa { @@ -58,7 +58,8 @@ namespace stochastic { -extern "C"{ + +extern "C" { // product C= alphaA.B + betaC void dgemm_(char* TRANSA, char* TRANSB, const int* M, const int* N, const int* K, double* alpha, double* A, @@ -68,10 +69,9 @@ extern "C"{ void dgemv_(char* TRANS, const int* M, const int* N, double* alpha, double* A, const int* LDA, double* X, const int* INCX, double* beta, double* C, const int* INCY); - } +} -using namespace defaulttype; template class UKFilterClassic : public sofa::component::stochastic::StochasticUnscentedFilterBase @@ -81,20 +81,20 @@ class UKFilterClassic : public sofa::component::stochastic::StochasticUnscentedF typedef sofa::component::stochastic::StochasticUnscentedFilterBase Inherit; typedef FilterType Type; - typedef typename Eigen::Matrix EMatrixX; typedef typename Eigen::Matrix EVectorX; -UKFilterClassic(); -~UKFilterClassic() {} + Data > d_state; + Data > d_variance; + Data > d_covariance; + Data > d_innovation; protected: StochasticStateWrapperBaseT* masterStateWrapper; - helper::vector*> stateWrappers; + type::vector*> stateWrappers; ObservationManager* observationManager; //ObservationSource *observationSource; - /// vector sizes size_t observationSize, stateSize, reducedStateSize; size_t numThreads; @@ -122,18 +122,17 @@ UKFilterClassic(); /// structures for parallel computing: - helper::vector sigmaPoints2WrapperIDs; - helper::vector > wrapper2SigmaPointsIDs; + type::vector sigmaPoints2WrapperIDs; + type::vector > wrapper2SigmaPointsIDs; /// functions_initial void computeSimplexSigmaPoints(EMatrixX& sigmaMat); void computeStarSigmaPoints(EMatrixX& sigmaMat); + public: - Data > d_state; - Data > d_variance; - Data > d_covariance; - Data > d_innovation; + UKFilterClassic(); + ~UKFilterClassic() {} void init() override; void bwdInit() override; @@ -160,8 +159,7 @@ UKFilterClassic(); virtual void initializeStep(const core::ExecParams* _params, const size_t _step) override; virtual void updateState() override; - -}; /// class +}; diff --git a/src/stochasticFiltering/UKFilterClassic.inl b/src/stochasticFiltering/UKFilterClassic.inl index 5536c54e..c9efeb4b 100644 --- a/src/stochasticFiltering/UKFilterClassic.inl +++ b/src/stochasticFiltering/UKFilterClassic.inl @@ -26,6 +26,7 @@ #include + namespace sofa { @@ -39,17 +40,17 @@ namespace stochastic template UKFilterClassic::UKFilterClassic() : Inherit() - , d_exportPrefix( initData(&d_exportPrefix, "exportPrefix", "prefix for storing various quantities into files")) - , d_filenameCov( initData(&d_filenameCov, "filenameCov", "output file name")) - , d_filenameInn( initData(&d_filenameInn, "filenameInn", "output file name")) - , d_filenameFinalState( initData(&d_filenameFinalState, "filenameFinalState", "output file name")) , d_state( initData(&d_state, "state", "actual expected value of reduced state (parameters) estimated by the filter" ) ) , d_variance( initData(&d_variance, "variance", "actual variance of reduced state (parameters) estimated by the filter" ) ) , d_covariance( initData(&d_covariance, "covariance", "actual co-variance of reduced state (parameters) estimated by the filter" ) ) , d_innovation( initData(&d_innovation, "innovation", "innovation value computed by the filter" ) ) -{ + , d_exportPrefix( initData(&d_exportPrefix, "exportPrefix", "prefix for storing various quantities into files")) + , d_filenameCov( initData(&d_filenameCov, "filenameCov", "output file name")) + , d_filenameInn( initData(&d_filenameInn, "filenameInn", "output file name")) + , d_filenameFinalState( initData(&d_filenameFinalState, "filenameFinalState", "output file name")) +{ } + -} template void UKFilterClassic::computePerturbedStates() @@ -65,6 +66,8 @@ void UKFilterClassic::computePerturbedStates() } } + + template void UKFilterClassic::computePrediction() { @@ -109,7 +112,7 @@ void UKFilterClassic::computePrediction() /// Compute Predicted Covariance stateCovar.setZero(); - EMatrixX matXiTrans= matXi.transpose(); + EMatrixX matXiTrans = matXi.transpose(); EMatrixX centeredPxx = matXiTrans.rowwise() - matXiTrans.colwise().mean(); EMatrixX weightedCenteredPxx = centeredPxx.array().colwise() * vecAlphaVar.array(); EMatrixX covPxx = (centeredPxx.adjoint() * weightedCenteredPxx); @@ -157,7 +160,6 @@ void UKFilterClassic::computeCorrection() EMatrixX matPz(observationSize, observationSize); EMatrixX pinvmatPz(observationSize, observationSize); - EMatrixX matXiTrans= matXi.transpose(); EMatrixX matZItrans = matZmodel.transpose(); EMatrixX centeredCx = matXiTrans.rowwise() - matXiTrans.colwise().mean(); @@ -168,7 +170,7 @@ void UKFilterClassic::computeCorrection() //std::cout << "covPxz: " << covPxz << std::endl; /// Compute Observation Covariance - matPxz=covPxz; + matPxz = covPxz; //EMatrixX covPzz = (centeredCz.adjoint() * centeredCz) / double(centeredCz.rows() ); EMatrixX covPzz = (centeredCz.adjoint() * weightedCenteredCz); matPz = obsCovar + covPzz; @@ -177,7 +179,7 @@ void UKFilterClassic::computeCorrection() /// Compute Kalman Gain EMatrixX matK(stateSize, observationSize); pseudoInverse(matPz, pinvmatPz); - matK = matPxz*pinvmatPz; + matK = matPxz * pinvmatPz; /// Compute Innovation EVectorX innovation(observationSize); @@ -187,7 +189,7 @@ void UKFilterClassic::computeCorrection() /// Compute Final State and Final Covariance stateExp = stateExp + matK * innovation; //std::cout << "matK: " << matK << std::endl; - stateCovar = stateCovar - matK*matPxz.transpose(); + stateCovar = stateCovar - matK * matPxz.transpose(); for (size_t i = 0; i < (size_t)stateCovar.rows(); i++) { diagStateCov(i) = stateCovar(i,i); @@ -201,37 +203,37 @@ void UKFilterClassic::computeCorrection() masterStateWrapper->setState(stateExp, mechParams); /// Write Some Data for Validation - helper::WriteAccessor > > stat = d_state; - helper::WriteAccessor > > var = d_variance; - helper::WriteAccessor > > covar = d_covariance; - helper::WriteAccessor > > innov = d_innovation; + helper::WriteAccessor > > stat = d_state; + helper::WriteAccessor > > var = d_variance; + helper::WriteAccessor > > covar = d_covariance; + helper::WriteAccessor > > innov = d_innovation; stat.resize(stateSize); var.resize(stateSize); - size_t numCovariances = (stateSize*(stateSize-1))/2; + size_t numCovariances = (stateSize * (stateSize - 1)) / 2; covar.resize(numCovariances); innov.resize(observationSize); size_t gli = 0; for (size_t i = 0; i < stateSize; i++) { stat[i] = stateExp[i]; - var[i] = stateCovar(i,i); - for (size_t j = i+1; j < stateSize; j++) { - covar[gli++] = stateCovar(i,j); + var[i] = stateCovar(i, i); + for (size_t j = i + 1; j < stateSize; j++) { + covar[gli++] = stateCovar(i, j); } } for (size_t index = 0; index < observationSize; index++) { innov[index] = innovation[index]; } - //char nstepc[100]; - //sprintf(nstepc, "%04lu", stepNumber); - //if (! exportPrefix.empty()) { - // std::string fileName = exportPrefix + "/covar_" + nstepc + ".txt"; - // std::ofstream ofs; - // ofs.open(fileName.c_str(), std::ofstream::out); - // ofs << stateCovar << std::endl; - //} + // char nstepc[100]; + // sprintf(nstepc, "%04lu", stepNumber); + // if (! exportPrefix.empty()) { + // std::string fileName = exportPrefix + "/covar_" + nstepc + ".txt"; + // std::ofstream ofs; + // ofs.open(fileName.c_str(), std::ofstream::out); + // ofs << stateCovar << std::endl; + // } writeValidationPlot(d_filenameInn.getValue(),innovation); } @@ -242,13 +244,14 @@ void UKFilterClassic::computeCorrection() template -void UKFilterClassic::init() { +void UKFilterClassic::init() +{ Inherit::init(); assert(this->gnode); this->gnode->template get >(&stateWrappers, this->getTags(), sofa::core::objectmodel::BaseContext::SearchDown); PRNS("found " << stateWrappers.size() << " state wrappers"); - masterStateWrapper=NULL; + masterStateWrapper = NULL; size_t numSlaveWrappers = 0; size_t numMasterWrappers = 0; numThreads = 0; @@ -273,16 +276,17 @@ void UKFilterClassic::init() { PRNS("number of slave wrappers: " << numThreads-1); /// slaves + master } - numThreads=numSlaveWrappers+numMasterWrappers; + numThreads = numSlaveWrappers + numMasterWrappers; this->gnode->get(observationManager, core::objectmodel::BaseContext::SearchDown); if (observationManager) { PRNS("found observation manager: " << observationManager->getName()); - } else + } else { PRNE("no observation manager found!"); + } /// Init for Write Function - exportPrefix = d_exportPrefix.getFullPath(); + exportPrefix = d_exportPrefix.getFullPath(); PRNS("export prefix: " << exportPrefix) this->saveParam = false; @@ -293,16 +297,18 @@ void UKFilterClassic::init() { paramFile.close(); } } + if (!d_filenameInn.getValue().empty()) { std::ofstream paramFileInn(d_filenameInn.getValue().c_str()); - if (paramFileInn .is_open()) { + if (paramFileInn.is_open()) { this->saveParam = true; paramFileInn.close(); } } + if (!d_filenameFinalState.getValue().empty()) { std::ofstream paramFileFinalState(d_filenameFinalState.getValue().c_str()); - if (paramFileFinalState .is_open()) { + if (paramFileFinalState.is_open()) { this->saveParam = true; paramFileFinalState.close(); } @@ -312,7 +318,8 @@ void UKFilterClassic::init() { template -void UKFilterClassic::bwdInit() { +void UKFilterClassic::bwdInit() +{ assert(masterStateWrapper); stateSize = masterStateWrapper->getStateSize(); @@ -329,13 +336,12 @@ void UKFilterClassic::bwdInit() { obsCovar = observationManager->getErrorVariance(); } - /// Initialize Model's Error Covariance stateCovar = masterStateWrapper->getStateErrorVariance(); diagStateCov.resize(stateCovar.rows()); for (size_t i = 0; i < (size_t)stateCovar.rows(); i++) { - diagStateCov(i)=stateCovar(i,i); + diagStateCov(i) = stateCovar(i, i); } PRNS(" INIT COVARIANCE DIAGONAL P(n+1)+n: \n" << diagStateCov.transpose()); @@ -365,7 +371,8 @@ void UKFilterClassic::bwdInit() { template -void UKFilterClassic::initializeStep(const core::ExecParams* _params, const size_t _step) { +void UKFilterClassic::initializeStep(const core::ExecParams* _params, const size_t _step) +{ Inherit::initializeStep(_params, _step); if (initialiseObservationsAtFirstStep.getValue()) { @@ -387,18 +394,18 @@ void UKFilterClassic::initializeStep(const core::ExecParams* _params template -void UKFilterClassic::updateState() { - +void UKFilterClassic::updateState() +{ stateSize = masterStateWrapper->getStateSize(); - //std::cout<< "new [UKF] stateSize " << stateSize << std::endl; - //PRNS("StateSize " << stateSize); + // std::cout<< "new [UKF] stateSize " << stateSize << std::endl; + // PRNS("StateSize " << stateSize); /// Initialize Model's Error Covariance stateCovar = masterStateWrapper->getStateErrorVariance(); diagStateCov.resize(stateCovar.rows()); for (size_t i = 0; i < (size_t)stateCovar.rows(); i++) { - diagStateCov(i)=stateCovar(i,i); + diagStateCov(i) = stateCovar(i,i); } //std::cout<< "INIT COVARIANCE DIAGONAL P(n+1)+n: " << diagStateCov.transpose() << std::endl; PRNS(" INIT COVARIANCE DIAGONAL P(n+1)+n: \n" << diagStateCov.transpose()); @@ -430,49 +437,63 @@ void UKFilterClassic::updateState() { template -void UKFilterClassic::stabilizeMatrix (EMatrixX& _initial, EMatrixX& _stabilized) { +void UKFilterClassic::stabilizeMatrix(EMatrixX& _initial, EMatrixX& _stabilized) +{ Eigen::JacobiSVD svd(_initial, Eigen::ComputeThinU | Eigen::ComputeThinV); const Eigen::JacobiSVD::SingularValuesType singVals = svd.singularValues(); Eigen::JacobiSVD::SingularValuesType singValsStab = singVals; - for (int i=0; i < singVals.rows(); i++ ){ - if ((singValsStab(i)*singValsStab(i))*(1.0/(singVals(0)*singVals(0)))< 1.0e-6) singValsStab(i)=0; + for (int i = 0; i < singVals.rows(); i++) { + if ( (singValsStab(i) * singValsStab(i)) * (1.0 / (singVals(0) * singVals(0))) < 1.0e-6) + singValsStab(i) = 0; } - _stabilized= svd.matrixU()*singValsStab*svd.matrixV().transpose(); + _stabilized = svd.matrixU() * singValsStab * svd.matrixV().transpose(); } + template -void UKFilterClassic::pseudoInverse( EMatrixX& M,EMatrixX& pinvM) { - double epsilon= 1e-15; +void UKFilterClassic::pseudoInverse(EMatrixX& M, EMatrixX& pinvM) +{ + double epsilon = 1e-15; Eigen::JacobiSVD svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV); const Eigen::JacobiSVD::SingularValuesType singVals = svd.singularValues(); Eigen::JacobiSVD::SingularValuesType invSingVals = singVals; - for(int i=0; i -void UKFilterClassic::sqrtMat(EMatrixX& A, EMatrixX& sqrtA){ - double epsilon= 1e-15; +void UKFilterClassic::sqrtMat(EMatrixX& A, EMatrixX& sqrtA) +{ + double epsilon = 1e-15; Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV); const Eigen::JacobiSVD::SingularValuesType singVals = svd.singularValues(); Eigen::JacobiSVD::SingularValuesType sqrtSingVals = singVals; - for(int i=0; i -void UKFilterClassic::writeValidationPlot (std::string filename ,EVectorX& state ){ +void UKFilterClassic::writeValidationPlot (std::string filename ,EVectorX& state ) +{ if (this->saveParam) { std::ofstream paramFile(filename.c_str(), std::ios::app); if (paramFile.is_open()) { @@ -486,34 +507,34 @@ void UKFilterClassic::writeValidationPlot (std::string filename ,EVe template -void UKFilterClassic::computeSimplexSigmaPoints(EMatrixX& sigmaMat) { +void UKFilterClassic::computeSimplexSigmaPoints(EMatrixX& sigmaMat) +{ size_t p = stateSize; size_t r = stateSize + 1; EMatrixX workingMatrix = EMatrixX::Zero(p, r); Type scal, beta, sqrt_p; - beta = Type(p) / Type(p+1); + beta = Type(p) / Type(p + 1); sqrt_p = sqrt(Type(p)); - scal = Type(1.0)/sqrt(Type(2) * beta); - workingMatrix(0,0) = scal; - workingMatrix(0,1) = -scal; + scal = Type(1.0) / sqrt(Type(2) * beta); + workingMatrix(0, 0) = scal; + workingMatrix(0, 1) = -scal; for (size_t i = 1; i < p; i++) { scal = Type(1.0) / sqrt(beta * Type(i+1) * Type(i+2)); - for (size_t j = 0; j < i+1; j++) - workingMatrix(i,j) = -scal; - workingMatrix(i,i+1) = Type(i+1) * scal; + for (size_t j = 0; j < i + 1; j++) + workingMatrix(i, j) = -scal; + workingMatrix(i, i+1) = Type(i + 1) * scal; } - sigmaMat.resize(r,p); for (size_t i = 0; i < r; i++) sigmaMat.row(i) = workingMatrix.col(i) * sqrt_p; vecAlpha.resize(r); - vecAlpha.fill(Type(1.0)/Type(r)); + vecAlpha.fill(Type(1.0) / Type(r)); alphaConstant = true; alpha = vecAlpha(0); @@ -526,8 +547,11 @@ void UKFilterClassic::computeSimplexSigmaPoints(EMatrixX& sigmaMat) //std::cout<< "vecAlphaVar: " << vecAlphaVar << std::endl; } + + template -void UKFilterClassic::computeStarSigmaPoints(EMatrixX& sigmaMat) { +void UKFilterClassic::computeStarSigmaPoints(EMatrixX& sigmaMat) +{ size_t p = stateSize; size_t r = 2 * stateSize + 1; @@ -539,13 +563,13 @@ void UKFilterClassic::computeStarSigmaPoints(EMatrixX& sigmaMat) { sqrt_vec = sqrt(p + lambda); for (size_t j = 0; j < p; j++) { - workingMatrix(j,j) = sqrt_vec; + workingMatrix(j, j) = sqrt_vec; } for (size_t j = p; j < 2 * p; j++) { - workingMatrix(2*p-j-1,j) = -sqrt_vec; + workingMatrix(2 * p - j - 1, j) = -sqrt_vec; } - sigmaMat.resize(r,p); + sigmaMat.resize(r, p); for (size_t i = 0; i < r; i++) sigmaMat.row(i) = workingMatrix.col(i); @@ -575,38 +599,37 @@ void UKFilterClassic::computeStarSigmaPoints(EMatrixX& sigmaMat) { -/*matPxzB.fill(FilterType(0.0)); -matPzB.fill(FilterType(0.0)); - -EVectorX vx(stateSize), z(observationSize), vz(observationSize); -vx.fill(FilterType(0.0)); -vz.fill(FilterType(0.0)); -z.fill(FilterType(0.0)); -for (size_t i = 0; i < sigmaPointsNum; i++) { - vx = matXi.col(i) - stateExp; - z = matZmodel.col(i); - vz = z - predObsExp; - for (size_t x = 0; x < stateSize; x++) - for (size_t y = 0; y < observationSize; y++) - matPxzB(x,y) += vx(x)*vz(y); - - for (size_t x = 0; x < observationSize; x++) - for (size_t y = 0; y < observationSize; y++) - matPzB(x,y) += vz(x)*vz(y); -} -matPxzB = alphaVar * matPxzB; -matPzB = alphaVar * matPzB + obsCovar;*/ - -/* - stateCovar.fill(FilterType(0.0)); - EVectorX tmpX(stateSize); - tmpX.fill(FilterType(0.0)); - for (size_t i = 0; i < sigmaPointsNum; i++) { - tmpX = matXi.col(i) - stateExp; - for (size_t x = 0; x < stateSize; x++) - for (size_t y = 0; y < stateSize; y++) - stateCovar(x,y) += tmpX(x)*tmpX(y); - } - stateCovar=alphaVar*stateCovar; -}*/ +// matPxzB.fill(FilterType(0.0)); +// matPzB.fill(FilterType(0.0)); + +// EVectorX vx(stateSize), z(observationSize), vz(observationSize); +// vx.fill(FilterType(0.0)); +// vz.fill(FilterType(0.0)); +// z.fill(FilterType(0.0)); +// for (size_t i = 0; i < sigmaPointsNum; i++) { +// vx = matXi.col(i) - stateExp; +// z = matZmodel.col(i); +// vz = z - predObsExp; +// for (size_t x = 0; x < stateSize; x++) +// for (size_t y = 0; y < observationSize; y++) +// matPxzB(x,y) += vx(x)*vz(y); + +// for (size_t x = 0; x < observationSize; x++) +// for (size_t y = 0; y < observationSize; y++) +// matPzB(x,y) += vz(x)*vz(y); +// } +// matPxzB = alphaVar * matPxzB; +// matPzB = alphaVar * matPzB + obsCovar;*/ + +// stateCovar.fill(FilterType(0.0)); +// EVectorX tmpX(stateSize); +// tmpX.fill(FilterType(0.0)); +// for (size_t i = 0; i < sigmaPointsNum; i++) { +// tmpX = matXi.col(i) - stateExp; +// for (size_t x = 0; x < stateSize; x++) +// for (size_t y = 0; y < stateSize; y++) +// stateCovar(x,y) += tmpX(x)*tmpX(y); +// } +// stateCovar=alphaVar*stateCovar; +// } diff --git a/src/stochasticFiltering/UKFilterClassicWithSVD.cpp b/src/stochasticFiltering/UKFilterClassicWithSVD.cpp index 3bd4ed18..7c37eb70 100644 --- a/src/stochasticFiltering/UKFilterClassicWithSVD.cpp +++ b/src/stochasticFiltering/UKFilterClassicWithSVD.cpp @@ -25,6 +25,7 @@ //#include + namespace sofa { @@ -35,9 +36,8 @@ namespace stochastic { -using namespace defaulttype; - +using namespace defaulttype; SOFA_DECL_CLASS(UKFilterClassicWithSVD) @@ -51,6 +51,7 @@ int UKFilterClassicWithSVDClass = core::RegisterObject("UKFilterClassic") #endif ; + #ifndef SOFA_FLOAT template class SOFA_STOCHASTIC_API UKFilterClassicWithSVD; #endif @@ -59,6 +60,7 @@ template class SOFA_STOCHASTIC_API UKFilterClassicWithSVD; #endif + } // namespace stochastic } // namespace component diff --git a/src/stochasticFiltering/UKFilterClassicWithSVD.h b/src/stochasticFiltering/UKFilterClassicWithSVD.h index 8bcb0a22..aa841ab0 100644 --- a/src/stochasticFiltering/UKFilterClassicWithSVD.h +++ b/src/stochasticFiltering/UKFilterClassicWithSVD.h @@ -34,10 +34,6 @@ #include #include -#ifdef Success -#undef Success // dirty workaround to cope with the (dirtier) X11 define. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=253 -#endif -#include #include #include //#include @@ -47,6 +43,9 @@ #include #include +#include + + namespace sofa { @@ -58,7 +57,8 @@ namespace stochastic { -extern "C"{ + +extern "C" { // product C= alphaA.B + betaC void dgemm_(char* TRANSA, char* TRANSB, const int* M, const int* N, const int* K, double* alpha, double* A, @@ -71,7 +71,6 @@ void dgemv_(char* TRANS, const int* M, const int* N, } -using namespace defaulttype; template class UKFilterClassicWithSVD : public sofa::component::stochastic::StochasticUnscentedFilterBase @@ -81,20 +80,26 @@ class UKFilterClassicWithSVD : public sofa::component::stochastic::StochasticUns typedef sofa::component::stochastic::StochasticUnscentedFilterBase Inherit; typedef FilterType Type; - typedef typename Eigen::Matrix EMatrixX; typedef typename Eigen::Matrix EVectorX; - UKFilterClassicWithSVD(); - ~UKFilterClassicWithSVD() {} + Data< type::vector > d_state; + Data< type::vector > d_variance; + Data< type::vector > d_covariance; + Data< type::vector > d_innovation; + Data< bool > d_draw; + Data< double > d_radius_draw; + Data< double > d_MOnodes_draw; + double m_omega; + bool hasObs; + protected: StochasticStateWrapperBaseT* masterStateWrapper; - helper::vector*> stateWrappers; + type::vector*> stateWrappers; ObservationManager* observationManager; //ObservationSource *observationSource; - /// vector sizes size_t observationSize, stateSize, reducedStateSize; size_t numThreads; @@ -118,25 +123,19 @@ class UKFilterClassicWithSVD : public sofa::component::stochastic::StochasticUns bool saveParam; Type alpha, alphaVar; - /// structures for parallel computing: - helper::vector sigmaPoints2WrapperIDs; - helper::vector > wrapper2SigmaPointsIDs; + type::vector sigmaPoints2WrapperIDs; + type::vector > wrapper2SigmaPointsIDs; /// functions_initial void computeSimplexSigmaPoints(EMatrixX& sigmaMat); void computeStarSigmaPoints(EMatrixX& sigmaMat); -public: - Data > d_state; - Data > d_variance; - Data > d_covariance; - Data > d_innovation; - Data< bool > d_draw; - Data< double > d_radius_draw; - Data< double > d_MOnodes_draw; - double m_omega; - bool hasObs; + +public: + UKFilterClassicWithSVD(); + ~UKFilterClassicWithSVD() {} + void init() override; void bwdInit() override; @@ -152,7 +151,7 @@ class UKFilterClassicWithSVD : public sofa::component::stochastic::StochasticUns void stabilizeMatrix (EMatrixX& _initial, EMatrixX& _stabilized); void pseudoInverse (EMatrixX& M,EMatrixX& pinvM ); void writeValidationPlot (std::string filename ,EVectorX& state ); -void sqrtMat(EMatrixX& A, EMatrixX& sqrtA); + void sqrtMat(EMatrixX& A, EMatrixX& sqrtA); virtual void computePerturbedStates(); virtual void computePrediction() override; // Compute perturbed state included in computeprediction @@ -162,8 +161,7 @@ void sqrtMat(EMatrixX& A, EMatrixX& sqrtA); void draw(const core::visual::VisualParams* vparams) override; virtual void updateState() override { } - -}; /// class +}; diff --git a/src/stochasticFiltering/UKFilterClassicWithSVD.inl b/src/stochasticFiltering/UKFilterClassicWithSVD.inl index dda1e4c2..4ea096a3 100644 --- a/src/stochasticFiltering/UKFilterClassicWithSVD.inl +++ b/src/stochasticFiltering/UKFilterClassicWithSVD.inl @@ -24,19 +24,25 @@ #include "UKFilterClassicWithSVD.h" #include #include +#include +#include + + namespace sofa { + namespace component { + namespace stochastic { + + template UKFilterClassicWithSVD::UKFilterClassicWithSVD() : Inherit() - , d_exportPrefix( initData(&d_exportPrefix, "exportPrefix", "prefix for storing various quantities into files")) - , d_filenameFinalState( initData(&d_filenameFinalState, "filenameFinalState", "output file name")) , d_state( initData(&d_state, "state", "actual expected value of reduced state (parameters) estimated by the filter" ) ) , d_variance( initData(&d_variance, "variance", "actual variance of reduced state (parameters) estimated by the filter" ) ) , d_covariance( initData(&d_covariance, "covariance", "actual co-variance of reduced state (parameters) estimated by the filter" ) ) @@ -44,11 +50,11 @@ UKFilterClassicWithSVD::UKFilterClassicWithSVD() , d_draw(initData(&d_draw, false, "draw","Activation of draw")) , d_radius_draw( initData(&d_radius_draw, (double) 0.005,"radiusDraw", "radius of the spheres") ) , d_MOnodes_draw( initData(&d_MOnodes_draw,(double) 1.0, "MOnodesDraw", "nodes of the mechanical object") ) + , d_exportPrefix( initData(&d_exportPrefix, "exportPrefix", "prefix for storing various quantities into files")) + , d_filenameFinalState( initData(&d_filenameFinalState, "filenameFinalState", "output file name")) +{ } -{ - -} template void UKFilterClassicWithSVD::computePerturbedStates() @@ -64,12 +70,15 @@ void UKFilterClassicWithSVD::computePerturbedStates() } } + + template void UKFilterClassicWithSVD::computePrediction() { hasObs = observationManager->hasObservation(this->actualTime); - if (hasObs) { -// std::cout<<"\n HAS OBS: =" << hasObs << " COMPUTE PREDICTION" << std::endl; + if (hasObs) + { + // std::cout << "\n HAS OBS: =" << hasObs << " COMPUTE PREDICTION" << std::endl; PRNS("Computing prediction, T= " << this->actualTime << " ======"); // /// Computes background error variance Cholesky factorization. @@ -108,19 +117,16 @@ void UKFilterClassicWithSVD::computePrediction() stateCovar = covPxx + modelCovar; masterStateWrapper->setState(stateExp, mechParams); + } else { + // std::cout << "\n HAS OBS: =" << hasObs << " COMPUTE ONLY PREDICTION" << std::endl; - }else{ -// std::cout<<"\n HAS OBS: =" << hasObs << " COMPUTE ONLY PREDICTION" << std::endl; + // stateExp.fill(FilterType(0.0)); + // stateExp = masterStateWrapper->getState(); + // masterStateWrapper->lastApplyOperator(stateExp, mechParams); -// stateExp.fill(FilterType(0.0)); -// stateExp = masterStateWrapper->getState(); -// masterStateWrapper->lastApplyOperator(stateExp, mechParams); - - EMatrixX matPsqrt(stateSize,stateSize); + EMatrixX matPsqrt(stateSize, stateSize); sqrtMat(stateCovar, matPsqrt); - - stateExp.fill(FilterType(0.0)); stateExp = masterStateWrapper->getState(); @@ -147,16 +153,17 @@ void UKFilterClassicWithSVD::computePrediction() stateCovar = covPxx + modelCovar; masterStateWrapper->setState(stateExp, mechParams); - } } + + template void UKFilterClassicWithSVD::computeCorrection() { - - if (hasObs) { -// std::cout<<"\n HAS OBS: =" << hasObs << " COMPUTE CORRECTION" << std::endl; + if (hasObs) + { + // std::cout << "\n HAS OBS: =" << hasObs << " COMPUTE CORRECTION" << std::endl; PRNS("======= Computing correction, T= " << this->actualTime << " ======"); EVectorX zCol(observationSize); @@ -166,7 +173,7 @@ void UKFilterClassicWithSVD::computeCorrection() /// Compute Predicted Observation for (size_t i = 0; i < sigmaPointsNum; i++) { - observationManager->getPredictedObservation(m_sigmaPointObservationIndexes[i], zCol); + observationManager->getPredictedObservation(m_sigmaPointObservationIndexes[i], zCol); matZmodel.col(i) = zCol; predObsExp = predObsExp + zCol * vecAlpha(i); } @@ -176,7 +183,6 @@ void UKFilterClassicWithSVD::computeCorrection() EMatrixX matPz(observationSize, observationSize); EMatrixX pinvmatPz(observationSize, observationSize); - EMatrixX matXiTrans= matXi.transpose(); EMatrixX matZItrans = matZmodel.transpose(); EMatrixX centeredCx = matXiTrans.rowwise() - matXiTrans.colwise().mean(); @@ -185,14 +191,14 @@ void UKFilterClassicWithSVD::computeCorrection() EMatrixX covPxz = (centeredCx.adjoint() * weightedCenteredCz); /// Compute Observation Covariance - matPxz=covPxz; + matPxz = covPxz; EMatrixX covPzz = (centeredCz.adjoint() * weightedCenteredCz); matPz = obsCovar + covPzz; /// Compute Kalman Gain EMatrixX matK(stateSize, observationSize); pseudoInverse(matPz, pinvmatPz); - matK =matPxz*pinvmatPz; + matK = matPxz * pinvmatPz; /// Compute Innovation EVectorX innovation(observationSize); @@ -204,9 +210,8 @@ void UKFilterClassicWithSVD::computeCorrection() masterStateWrapper->setState(stateExp, mechParams); - /// Write Some File for Validation - helper::WriteAccessor > > stat = d_state; + helper::WriteAccessor > > stat = d_state; stat.resize(stateSize); for (size_t i = 0; i < stateSize; i++) stat[i] = stateExp[i]; @@ -219,22 +224,21 @@ void UKFilterClassicWithSVD::computeCorrection() ofs.open(fileName.c_str(), std::ofstream::out); ofs << stateCovar << std::endl; } - } writeValidationPlot(d_filenameFinalState.getValue() ,stateExp); - } template -void UKFilterClassicWithSVD::init() { +void UKFilterClassicWithSVD::init() +{ Inherit::init(); assert(this->gnode); this->gnode->template get >(&stateWrappers, this->getTags(), sofa::core::objectmodel::BaseContext::SearchDown); PRNS("found " << stateWrappers.size() << " state wrappers"); - masterStateWrapper=NULL; + masterStateWrapper = NULL; size_t numSlaveWrappers = 0; size_t numMasterWrappers = 0; numThreads = 0; @@ -259,39 +263,40 @@ void UKFilterClassicWithSVD::init() { PRNS("number of slave wrappers: " << numThreads-1); /// slaves + master } - numThreads=numSlaveWrappers+numMasterWrappers; + numThreads = numSlaveWrappers+numMasterWrappers; this->gnode->get(observationManager, core::objectmodel::BaseContext::SearchDown); if (observationManager) { PRNS("found observation manager: " << observationManager->getName()); - } else + } else { PRNE("no observation manager found!"); + } /// Init for Write Function - exportPrefix = d_exportPrefix.getFullPath(); + exportPrefix = d_exportPrefix.getFullPath(); this->saveParam = false; if (!d_filenameFinalState.getValue().empty()) { std::ofstream paramFileFinalState(d_filenameFinalState.getValue().c_str()); - if (paramFileFinalState .is_open()) { + if (paramFileFinalState.is_open()) { this->saveParam = true; paramFileFinalState.close(); } } - m_omega= ((double) rand() / (RAND_MAX)); + m_omega = ((double) rand() / (RAND_MAX)); +} -} template -void UKFilterClassicWithSVD::bwdInit() { +void UKFilterClassicWithSVD::bwdInit() +{ assert(masterStateWrapper); stateSize = masterStateWrapper->getStateSize(); std::cout<< "[UKF] stateSize " << stateSize << std::endl; - /// Initialize Observation's data if (!initialiseObservationsAtFirstStep.getValue()) { observationSize = this->observationManager->getObservationSize(); @@ -302,7 +307,6 @@ void UKFilterClassicWithSVD::bwdInit() { obsCovar = observationManager->getErrorVariance(); } - /// Initialize Model's Error Covariance stateCovar = masterStateWrapper->getStateErrorVariance(); modelCovar = masterStateWrapper->getModelErrorVariance(); @@ -324,11 +328,13 @@ void UKFilterClassicWithSVD::bwdInit() { /// Init State Observation Mapping for Sigma Points m_sigmaPointObservationIndexes.resize(sigmaPointsNum); - } + + template -void UKFilterClassicWithSVD::initializeStep(const core::ExecParams* _params, const size_t _step) { +void UKFilterClassicWithSVD::initializeStep(const core::ExecParams* _params, const size_t _step) +{ Inherit::initializeStep(_params, _step); if (initialiseObservationsAtFirstStep.getValue()) { @@ -347,53 +353,65 @@ void UKFilterClassicWithSVD::initializeStep(const core::ExecParams* observationManager->initializeStep(stepNumber); } + + template -void UKFilterClassicWithSVD::stabilizeMatrix (EMatrixX& _initial, EMatrixX& _stabilized) { +void UKFilterClassicWithSVD::stabilizeMatrix (EMatrixX& _initial, EMatrixX& _stabilized) +{ Eigen::JacobiSVD svd(_initial, Eigen::ComputeThinU | Eigen::ComputeThinV); const Eigen::JacobiSVD::SingularValuesType singVals = svd.singularValues(); Eigen::JacobiSVD::SingularValuesType singValsStab = singVals; - for (int i=0; i < singVals.rows(); i++ ){ - if ((singValsStab(i)*singValsStab(i))*(1.0/(singVals(0)*singVals(0)))< 1.0e-6) singValsStab(i)=0; + for (int i = 0; i < singVals.rows(); i++) { + if ( (singValsStab(i) * singValsStab(i)) * (1.0 / (singVals(0) * singVals(0))) < 1.0e-6) + singValsStab(i) = 0; } - _stabilized= svd.matrixU()*singValsStab*svd.matrixV().transpose(); - + _stabilized = svd.matrixU() * singValsStab * svd.matrixV().transpose(); } + + template -void UKFilterClassicWithSVD::pseudoInverse( EMatrixX& M,EMatrixX& pinvM) { - double epsilon= 1e-15; +void UKFilterClassicWithSVD::pseudoInverse(EMatrixX& M, EMatrixX& pinvM) +{ + double epsilon = 1e-15; Eigen::JacobiSVD svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV); const Eigen::JacobiSVD::SingularValuesType singVals = svd.singularValues(); Eigen::JacobiSVD::SingularValuesType invSingVals = singVals; - for(int i=0; i -void UKFilterClassicWithSVD::sqrtMat(EMatrixX& A, EMatrixX& sqrtA){ - double epsilon= 1e-15; +void UKFilterClassicWithSVD::sqrtMat(EMatrixX& A, EMatrixX& sqrtA) +{ + double epsilon = 1e-15; Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV); const Eigen::JacobiSVD::SingularValuesType singVals = svd.singularValues(); Eigen::JacobiSVD::SingularValuesType sqrtSingVals = singVals; - for(int i=0; i -void UKFilterClassicWithSVD::writeValidationPlot (std::string filename ,EVectorX& state ){ +void UKFilterClassicWithSVD::writeValidationPlot(std::string filename, EVectorX& state) +{ if (this->saveParam) { std::ofstream paramFile(filename.c_str(), std::ios::app); if (paramFile.is_open()) { @@ -404,35 +422,37 @@ void UKFilterClassicWithSVD::writeValidationPlot (std::string filena } } + + template -void UKFilterClassicWithSVD::computeSimplexSigmaPoints(EMatrixX& sigmaMat) { +void UKFilterClassicWithSVD::computeSimplexSigmaPoints(EMatrixX& sigmaMat) +{ size_t p = stateSize; size_t r = stateSize + 1; EMatrixX workingMatrix = EMatrixX::Zero(p, r); Type scal, beta, sqrt_p; - beta = Type(p) / Type(p+1); + beta = Type(p) / Type(p + 1); sqrt_p = sqrt(Type(p)); - scal = Type(1.0)/sqrt(Type(2) * beta); - workingMatrix(0,0) = scal; - workingMatrix(0,1) = -scal; + scal = Type(1.0) / sqrt(Type(2) * beta); + workingMatrix(0, 0) = scal; + workingMatrix(0, 1) = -scal; for (size_t i = 1; i < p; i++) { - scal = Type(1.0) / sqrt(beta * Type(i+1) * Type(i+2)); + scal = Type(1.0) / sqrt(beta * Type(i + 1) * Type(i + 2)); - for (size_t j = 0; j < i+1; j++) - workingMatrix(i,j) = -scal; - workingMatrix(i,i+1) = Type(i+1) * scal; + for (size_t j = 0; j < i + 1; j++) + workingMatrix(i, j) = -scal; + workingMatrix(i, i+1) = Type(i + 1) * scal; } - - sigmaMat.resize(r,p); + sigmaMat.resize(r, p); for (size_t i = 0; i < r; i++) sigmaMat.row(i) = workingMatrix.col(i) * sqrt_p; vecAlpha.resize(r); - vecAlpha.fill(Type(1.0)/Type(r)); + vecAlpha.fill(Type(1.0) / Type(r)); alphaConstant = true; alpha = vecAlpha(0); @@ -441,8 +461,11 @@ void UKFilterClassicWithSVD::computeSimplexSigmaPoints(EMatrixX& sig vecAlphaVar.fill(alphaVar); } + + template -void UKFilterClassicWithSVD::computeStarSigmaPoints(EMatrixX& sigmaMat) { +void UKFilterClassicWithSVD::computeStarSigmaPoints(EMatrixX& sigmaMat) +{ size_t p = stateSize; size_t r = 2 * stateSize + 1; @@ -454,18 +477,18 @@ void UKFilterClassicWithSVD::computeStarSigmaPoints(EMatrixX& sigmaM sqrt_vec = sqrt(p + lambda); for (size_t j = 0; j < p; j++) { - workingMatrix(j,j) = sqrt_vec; + workingMatrix(j, j) = sqrt_vec; } for (size_t j = p; j < 2 * p; j++) { - workingMatrix(2*p-j-1,j) = -sqrt_vec; + workingMatrix(2 * p - j - 1, j) = -sqrt_vec; } - sigmaMat.resize(r,p); + sigmaMat.resize(r, p); for (size_t i = 0; i < r; i++) sigmaMat.row(i) = workingMatrix.col(i); vecAlpha.resize(r); - vecAlpha.fill(Type(1.0)/Type(2 * (p + lambda))); + vecAlpha.fill(Type(1.0) / Type(2 * (p + lambda))); vecAlpha(2 * p) = Type(lambda) / Type(p + lambda); alphaConstant = false; alpha = vecAlpha(0); @@ -476,30 +499,33 @@ void UKFilterClassicWithSVD::computeStarSigmaPoints(EMatrixX& sigmaM vecAlphaVar(2 * p) = Type(lambda) / Type(p + lambda); } + + template -void UKFilterClassicWithSVD::draw(const core::visual::VisualParams* vparams ) { - if(d_draw.getValue()){ +void UKFilterClassicWithSVD::draw(const core::visual::VisualParams* vparams) +{ + if ( d_draw.getValue() ) + { if (vparams->displayFlags().getShowVisualModels()) { - std::vector> predpoints; + std::vector> predpoints; predpoints.resize( sigmaPointsNum ); - for( std::vector>::iterator it = predpoints.begin(); it != predpoints.end(); ++it) - { + for (std::vector>::iterator it = predpoints.begin(); it != predpoints.end(); ++it) { it->resize( d_MOnodes_draw.getValue() ); } - for(unsigned i=0; i < sigmaPointsNum; i++){ + for (unsigned i = 0; i < sigmaPointsNum; i++) { EVectorX coll = genMatXi.row(i); - for (unsigned j=0; j < d_MOnodes_draw.getValue(); j++){ - for (unsigned k=0; k < 3; k++){ - if (masterStateWrapper->estimOnlyXYZ()) - predpoints[i][j][k]=coll(3*j+k); - else - predpoints[i][j][k]=coll(6*j+k); + for (unsigned j = 0; j < d_MOnodes_draw.getValue(); j++) { + for (unsigned k = 0; k < 3; k++) { + if (masterStateWrapper->estimOnlyXYZ()) { + predpoints[i][j][k] = coll(3 * j + k); + } else { + predpoints[i][j][k] = coll(6 * j + k); + } } } - helper::types::RGBAColor color; switch (i) { @@ -508,22 +534,21 @@ void UKFilterClassicWithSVD::draw(const core::visual::VisualParams* case 2: color = helper::types::RGBAColor(0.0,0.0,1.0,1.0); break; default: color = helper::types::RGBAColor(0.5, 0.5, 0.5, 0.5); } - helper::vector colorB; - colorB.resize(this->stateSize); - for(size_t i =0; i < colorB.size(); i++){ - colorB[i]= ((double) rand() / (RAND_MAX)) ; + type::vector colorB; + colorB.resize(this->stateSize); + for (size_t i = 0; i < colorB.size(); i++) { + colorB[i] = ((double) rand() / (RAND_MAX)); } - vparams->drawTool()->drawSpheres(predpoints[i], d_radius_draw.getValue(), helper::types::RGBAColor(m_omega,0.0f,0.0f,1.0f)); } -// if (d_MOnodes_draw.getValue()>=2) -// vparams->drawTool()->drawLineStrip(predpoints[i],3.0,sofa::defaulttype::Vec<4, float>(color[i],0.5f,colorB[i],1.0f)); } + vparams->drawTool()->drawSpheres(predpoints[i], d_radius_draw.getValue(), helper::types::RGBAColor(m_omega,0.0f,0.0f,1.0f)); + } + // if (d_MOnodes_draw.getValue() >= 2) + // vparams->drawTool()->drawLineStrip(predpoints[i],3.0,sofa::defaulttype::Vec<4, float>(color[i],0.5f,colorB[i],1.0f)); + // } } } - - - } @@ -536,37 +561,37 @@ void UKFilterClassicWithSVD::draw(const core::visual::VisualParams* -/*matPxzB.fill(FilterType(0.0)); -matPzB.fill(FilterType(0.0)); - -EVectorX vx(stateSize), z(observationSize), vz(observationSize); -vx.fill(FilterType(0.0)); -vz.fill(FilterType(0.0)); -z.fill(FilterType(0.0)); -for (size_t i = 0; i < sigmaPointsNum; i++) { - vx = matXi.col(i) - stateExp; - z = matZmodel.col(i); - vz = z - predObsExp; - for (size_t x = 0; x < stateSize; x++) - for (size_t y = 0; y < observationSize; y++) - matPxzB(x,y) += vx(x)*vz(y); +// matPxzB.fill(FilterType(0.0)); +// matPzB.fill(FilterType(0.0)); + +// EVectorX vx(stateSize), z(observationSize), vz(observationSize); +// vx.fill(FilterType(0.0)); +// vz.fill(FilterType(0.0)); +// z.fill(FilterType(0.0)); +// for (size_t i = 0; i < sigmaPointsNum; i++) { +// vx = matXi.col(i) - stateExp; +// z = matZmodel.col(i); +// vz = z - predObsExp; +// for (size_t x = 0; x < stateSize; x++) +// for (size_t y = 0; y < observationSize; y++) +// matPxzB(x,y) += vx(x) * vz(y); +// +// for (size_t x = 0; x < observationSize; x++) +// for (size_t y = 0; y < observationSize; y++) +// matPzB(x,y) += vz(x) * vz(y); +// } +// matPxzB = alphaVar * matPxzB; +// matPzB = alphaVar * matPzB + obsCovar;*/ + +// stateCovar.fill(FilterType(0.0)); +// EVectorX tmpX(stateSize); +// tmpX.fill(FilterType(0.0)); +// for (size_t i = 0; i < sigmaPointsNum; i++) { +// tmpX = matXi.col(i) - stateExp; +// for (size_t x = 0; x < stateSize; x++) +// for (size_t y = 0; y < stateSize; y++) +// stateCovar(x,y) += tmpX(x) * tmpX(y); +// } +// stateCovar=alphaVar*stateCovar; +// } - for (size_t x = 0; x < observationSize; x++) - for (size_t y = 0; y < observationSize; y++) - matPzB(x,y) += vz(x)*vz(y); -} -matPxzB = alphaVar * matPxzB; -matPzB = alphaVar * matPzB + obsCovar;*/ - -/* - stateCovar.fill(FilterType(0.0)); - EVectorX tmpX(stateSize); - tmpX.fill(FilterType(0.0)); - for (size_t i = 0; i < sigmaPointsNum; i++) { - tmpX = matXi.col(i) - stateExp; - for (size_t x = 0; x < stateSize; x++) - for (size_t y = 0; y < stateSize; y++) - stateCovar(x,y) += tmpX(x)*tmpX(y); - } - stateCovar=alphaVar*stateCovar; -}*/ diff --git a/src/stochasticFiltering/UKFilterSimCorr.cpp b/src/stochasticFiltering/UKFilterSimCorr.cpp index fce6870e..66e7aced 100644 --- a/src/stochasticFiltering/UKFilterSimCorr.cpp +++ b/src/stochasticFiltering/UKFilterSimCorr.cpp @@ -25,6 +25,7 @@ //#include + namespace sofa { @@ -34,9 +35,9 @@ namespace component namespace stochastic { -using namespace defaulttype; +using namespace defaulttype; SOFA_DECL_CLASS(UKFilterSimCorr) @@ -51,6 +52,7 @@ int UKFilterSimCorrClass = core::RegisterObject("UKFilterSimCorr") #endif ; + #ifndef SOFA_FLOAT template class SOFA_STOCHASTIC_API UKFilterSimCorr; #endif @@ -59,6 +61,7 @@ template class SOFA_STOCHASTIC_API UKFilterSimCorr; #endif + } // namespace stochastic } // namespace component diff --git a/src/stochasticFiltering/UKFilterSimCorr.h b/src/stochasticFiltering/UKFilterSimCorr.h index e33b474b..8b3ff517 100644 --- a/src/stochasticFiltering/UKFilterSimCorr.h +++ b/src/stochasticFiltering/UKFilterSimCorr.h @@ -34,10 +34,6 @@ #include #include -#ifdef Success -#undef Success // dirty workaround to cope with the (dirtier) X11 define. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=253 -#endif -#include #include #include //#include @@ -47,6 +43,9 @@ #include #include +#include + + namespace sofa { @@ -58,18 +57,21 @@ namespace stochastic { + /// to speed up, wrappers for BLAS matrix multiplications created, much faster that Eigen by default -extern "C"{ +extern "C" { // product C= alphaA.B + betaC - void dgemm_(char* TRANSA, char* TRANSB, const int* M, + void dgemm_(char* TRANSA, char* TRANSB, const int* M, const int* N, const int* K, double* alpha, double* A, const int* LDA, double* B, const int* LDB, double* beta, double* C, const int* LDC); // product Y= alphaA.X + betaY - void dgemv_(char* TRANS, const int* M, const int* N, + void dgemv_(char* TRANS, const int* M, const int* N, double* alpha, double* A, const int* LDA, double* X, const int* INCX, double* beta, double* C, const int* INCY); - } +} + + /** * Class implementing a special version of the unscented Kalman filter for data assimilation. @@ -78,27 +80,26 @@ extern "C"{ * This version keeps _only_ the parameters in the stochastic state. * Filter requires StochasticStateWrapper which provides the interface with SOFA. */ - -using namespace defaulttype; - template -class UKFilterSimCorr: public sofa::component::stochastic::StochasticUnscentedFilterBase +class UKFilterSimCorr : public sofa::component::stochastic::StochasticUnscentedFilterBase { public: SOFA_CLASS(SOFA_TEMPLATE(UKFilterSimCorr, FilterType), StochasticUnscentedFilterBase); typedef sofa::component::stochastic::StochasticUnscentedFilterBase Inherit; typedef FilterType Type; - typedef typename Eigen::Matrix EMatrixX; typedef typename Eigen::Matrix EVectorX; -UKFilterSimCorr(); -~UKFilterSimCorr() {} + Data > d_state; + Data > d_variance; + Data > d_covariance; + Data > d_innovation; + protected: StochasticStateWrapperBaseT* masterStateWrapper; - helper::vector*> stateWrappers; + type::vector*> stateWrappers; ObservationManager* observationManager; /// vector sizes @@ -124,18 +125,17 @@ UKFilterSimCorr(); /// structures for parallel computing: - helper::vector sigmaPoints2WrapperIDs; - helper::vector > wrapper2SigmaPointsIDs; + type::vector sigmaPoints2WrapperIDs; + type::vector< type::vector > wrapper2SigmaPointsIDs; /// functions void computeSimplexSigmaPoints(EMatrixX& sigmaMat); void computeStarSigmaPoints(EMatrixX& sigmaMat); -public: - Data > d_state; - Data > d_variance; - Data > d_covariance; - Data > d_innovation; + +public: + UKFilterSimCorr(); + ~UKFilterSimCorr() {} void init() override; void bwdInit() override; @@ -156,30 +156,6 @@ UKFilterSimCorr(); virtual void initializeStep(const core::ExecParams* _params, const size_t _step) override; virtual void updateState() override { } - -}; /// class - -template -struct WorkerThreadData -{ - typedef typename Eigen::Matrix EMatrixX; - - StochasticStateWrapperBaseT* wrapper; - size_t threadID; - helper::vector* sigmaIDs; - EMatrixX* stateMatrix; - const core::ExecParams* execParams; - bool saveLog; - - void set(size_t _threadID,StochasticStateWrapperBaseT* _wrapper, helper::vector *_sigID, - EMatrixX* _stateMat, const core::ExecParams* _execParams, bool _saveLog) { - threadID=_threadID; - sigmaIDs=_sigID; - stateMatrix=_stateMat; - saveLog = _saveLog; - wrapper = _wrapper; - execParams = _execParams; - } }; diff --git a/src/stochasticFiltering/UKFilterSimCorr.inl b/src/stochasticFiltering/UKFilterSimCorr.inl index 13102071..1d0cf3db 100644 --- a/src/stochasticFiltering/UKFilterSimCorr.inl +++ b/src/stochasticFiltering/UKFilterSimCorr.inl @@ -26,6 +26,7 @@ #include + namespace sofa { @@ -36,39 +37,39 @@ namespace stochastic { + template UKFilterSimCorr::UKFilterSimCorr() : Inherit() - , d_filename( initData(&d_filename, "filename", "output file name")) - , outfile(NULL) , d_state( initData(&d_state, "state", "actual expected value of reduced state (parameters) estimated by the filter" ) ) , d_variance( initData(&d_variance, "variance", "actual variance of reduced state (parameters) estimated by the filter" ) ) , d_covariance( initData(&d_covariance, "covariance", "actual co-variance of reduced state (parameters) estimated by the filter" ) ) - , d_innovation( initData(&d_innovation, "innovation", "innovation value computed by the filter" ) ) -{ + , d_innovation( initData(&d_innovation, "innovation", "innovation value computed by the filter" ) ) + , d_filename( initData(&d_filename, "filename", "output file name")) + , outfile(NULL) +{ } -} template void UKFilterSimCorr::computePrediction() { - //PRNS("Computing prediction, T= " << this->actualTime << " ======"); + // PRNS("Computing prediction, T= " << this->actualTime << " ======"); /// Computes background error variance Cholesky factorization. Eigen::LLT lltU(stateCovar); EMatrixX matPsqrt = lltU.matrixL(); - //PRNS("matPsqrt: " << matPsqrt); + // PRNS("matPsqrt: " << matPsqrt); stateExp = masterStateWrapper->getState(); - //PRNS("X(n): \n" << stateExp.transpose()); - //PRNS("P(n): \n" << stateCovar); + // PRNS("X(n): \n" << stateExp.transpose()); + // PRNS("P(n): \n" << stateCovar); /// Computes X_{n}^{(i)-} sigma points for (size_t i = 0; i < sigmaPointsNum; i++) { matXi.col(i) = stateExp + matPsqrt * matI.row(i).transpose(); } - //PRNS("MatXi: \n" << matXi); + // PRNS("MatXi: \n" << matXi); /// There's no dynamics => the state is not changed @@ -87,22 +88,25 @@ void UKFilterSimCorr::computePrediction() //// for (size_t y = 0; y < stateSize; y++) //// stateCovar(x,y) += vecAlphaVar(i) * tmpX(x)*tmpX(y); //// } - EMatrixX matXiTrans= matXi.transpose(); + EMatrixX matXiTrans = matXi.transpose(); EMatrixX centeredPxx = matXiTrans.rowwise() - matXiTrans.colwise().mean(); EMatrixX weightedCenteredPxx = centeredPxx.array().colwise() * vecAlphaVar.array(); EMatrixX covPxx = (centeredPxx.adjoint() * weightedCenteredPxx); - //EMatrixX covPxx = (centeredPxx.adjoint() * centeredPxx) / double(centeredPxx.rows() ) + // EMatrixX covPxx = (centeredPxx.adjoint() * centeredPxx) / double(centeredPxx.rows() ) stateCovar = covPxx; // stateCovar = alphaVar*stateCovar; - //PRNS("X(n+1)-: " << stateExp.transpose()); - //PRNS("P(n+1)-: \n" << stateCovar); + // PRNS("X(n+1)-: " << stateExp.transpose()); + // PRNS("P(n+1)-: \n" << stateCovar); } + + template void UKFilterSimCorr::computeCorrection() { - if (observationManager->hasObservation(this->actualTime)) { + if (observationManager->hasObservation(this->actualTime)) + { //PRNS("======= Computing correction, T= " << this->actualTime << " ======"); /// Compute variance factorization @@ -147,7 +151,7 @@ void UKFilterSimCorr::computeCorrection() EMatrixX centeredCz = matZItrans.rowwise() - matZItrans.colwise().mean(); EMatrixX weightedCenteredCz = centeredCz.array().colwise() * vecAlphaVar.array(); EMatrixX covPxz = (centeredCx.adjoint() * weightedCenteredCz); - matPxz=covPxz; + matPxz = covPxz; EMatrixX covPzz = (centeredCz.adjoint() * weightedCenteredCz); matPz = obsCovar + covPzz; @@ -186,10 +190,10 @@ void UKFilterSimCorr::computeCorrection() //PRNS("X(n+1)+: \n" << stateExp.transpose()); //PRNS("P(n+1)+n: \n" << stateCovar); - helper::WriteAccessor > > stat = d_state; - helper::WriteAccessor > > var = d_variance; - helper::WriteAccessor > > covar = d_covariance; - helper::WriteAccessor > > innov = d_innovation; + helper::WriteAccessor > > stat = d_state; + helper::WriteAccessor > > var = d_variance; + helper::WriteAccessor > > covar = d_covariance; + helper::WriteAccessor > > innov = d_innovation; stat.resize(stateSize); var.resize(stateSize); @@ -201,19 +205,21 @@ void UKFilterSimCorr::computeCorrection() for (size_t i = 0; i < stateSize; i++) { stat[i] = stateExp[i]; var[i] = stateCovar(i,i); - for (size_t j = i+1; j < stateSize; j++) { + for (size_t j = i + 1; j < stateSize; j++) { covar[gli++] = stateCovar(i,j); } } for (size_t index = 0; index < observationSize; index++) { innov[index] = innovation[index]; } - } } + + template -void UKFilterSimCorr::init() { +void UKFilterSimCorr::init() +{ Inherit::init(); assert(this->gnode); @@ -244,7 +250,7 @@ void UKFilterSimCorr::init() { PRNS("number of slave wrappers: " << numThreads-1); /// slaves + master } - numThreads=numSlaveWrappers+numMasterWrappers; + numThreads = numSlaveWrappers + numMasterWrappers; this->gnode->get(observationManager, core::objectmodel::BaseContext::SearchDown); if (observationManager) { @@ -256,8 +262,11 @@ void UKFilterSimCorr::init() { outfile = new std::ofstream(filename.c_str()); } + + template -void UKFilterSimCorr::bwdInit() { +void UKFilterSimCorr::bwdInit() +{ assert(masterStateWrapper); stateSize = masterStateWrapper->getStateSize(); @@ -291,9 +300,9 @@ void UKFilterSimCorr::bwdInit() { matXi.setZero(); /// copy state (exp, covariance) to SOFA data for exporting - helper::WriteAccessor > > dstate = this->d_state; - helper::WriteAccessor > > dvar = this->d_variance; - helper::WriteAccessor > > dcovar = this->d_covariance; + helper::WriteAccessor > > dstate = this->d_state; + helper::WriteAccessor > > dvar = this->d_variance; + helper::WriteAccessor > > dcovar = this->d_covariance; dstate.resize(stateSize); dvar.resize(stateSize); @@ -304,12 +313,14 @@ void UKFilterSimCorr::bwdInit() { for (size_t i = 0; i < stateSize; i++) { dstate[i] = stateExp(i); dvar[i] = stateCovar(i); - for (size_t j = i+1; j < stateSize; j++) { + for (size_t j = i + 1; j < stateSize; j++) { dcovar[gli++] = stateCovar(i,j); } } } + + template void UKFilterSimCorr::initializeStep(const core::ExecParams* _params, const size_t _step) { Inherit::initializeStep(_params, _step); @@ -331,8 +342,11 @@ void UKFilterSimCorr::initializeStep(const core::ExecParams* _params observationManager->initializeStep(stepNumber); } + + template -void UKFilterSimCorr::computeSimplexSigmaPoints(EMatrixX& sigmaMat) { +void UKFilterSimCorr::computeSimplexSigmaPoints(EMatrixX& sigmaMat) +{ size_t p = stateSize; size_t r = stateSize + 1; @@ -341,38 +355,39 @@ void UKFilterSimCorr::computeSimplexSigmaPoints(EMatrixX& sigmaMat) Type scal, beta, sqrt_p; beta = Type(p) / Type(p+1); sqrt_p = sqrt(Type(p)); - scal = Type(1.0)/sqrt(Type(2) * beta); - workingMatrix(0,0) = -scal; - workingMatrix(0,1) = scal; + scal = Type(1.0) / sqrt(Type(2) * beta); + workingMatrix(0, 0) = -scal; + workingMatrix(0, 1) = scal; for (size_t i = 1; i < p; i++) { - scal = Type(1.0) / sqrt(beta * Type(i+1) * Type(i+2)); + scal = Type(1.0) / sqrt(beta * Type(i + 1) * Type(i + 2)); for (size_t j = 0; j < i+1; j++) - workingMatrix(i,j) = scal; - workingMatrix(i,i+1) = -Type(i+1) * scal; + workingMatrix(i, j) = scal; + workingMatrix(i, i + 1) = -Type(i+1) * scal; } - - sigmaMat.resize(r,p); + sigmaMat.resize(r, p); for (size_t i = 0; i < r; i++) sigmaMat.row(i) = workingMatrix.col(i) * sqrt_p; vecAlpha.resize(r); - vecAlpha.fill(Type(1.0)/Type(r)); + vecAlpha.fill(Type(1.0) / Type(r)); alphaConstant = true; alpha = vecAlpha(0); alphaVar = (this->useUnbiasedVariance.getValue()) ? Type(1.0)/Type(r-1) : Type(1.0)/Type(r); vecAlphaVar.resize(r); vecAlphaVar.fill(alphaVar); - //PRNS("sigmaMat: \n" << sigmaMat); - //PRNS("vecAlphaVar: \n" << vecAlphaVar); + // PRNS("sigmaMat: \n" << sigmaMat); + // PRNS("vecAlphaVar: \n" << vecAlphaVar); } + template -void UKFilterSimCorr::computeStarSigmaPoints(EMatrixX& sigmaMat) { +void UKFilterSimCorr::computeStarSigmaPoints(EMatrixX& sigmaMat) +{ size_t p = stateSize; size_t r = 2 * stateSize + 1; @@ -384,13 +399,13 @@ void UKFilterSimCorr::computeStarSigmaPoints(EMatrixX& sigmaMat) { sqrt_vec = sqrt(p + lambda); for (size_t j = 0; j < p; j++) { - workingMatrix(j,j) = sqrt_vec; + workingMatrix(j, j) = sqrt_vec; } for (size_t j = p; j < 2 * p; j++) { - workingMatrix(2*p-j-1,j) = -sqrt_vec; + workingMatrix(2 * p - j - 1, j) = -sqrt_vec; } - sigmaMat.resize(r,p); + sigmaMat.resize(r, p); for (size_t i = 0; i < r; i++) sigmaMat.row(i) = workingMatrix.col(i); diff --git a/src/test/GeoEmulator.cpp b/src/test/GeoEmulator.cpp index e5b00d38..5969ed28 100644 --- a/src/test/GeoEmulator.cpp +++ b/src/test/GeoEmulator.cpp @@ -30,14 +30,19 @@ #include -namespace sofa { -namespace component { +namespace sofa +{ + +namespace component +{ + +namespace behavior +{ -namespace behavior { -using namespace sofa::defaulttype; +using namespace sofa::defaulttype; SOFA_DECL_CLASS(GeoEmulator) @@ -45,8 +50,10 @@ int GeoEmulatorClass = core::RegisterObject("Geomagic device manipulating object .add(); + } // namespace bahavior } // namespace component } // namespace sofa + diff --git a/src/test/GeoEmulator.h b/src/test/GeoEmulator.h index 49a1285f..e3e10e3b 100644 --- a/src/test/GeoEmulator.h +++ b/src/test/GeoEmulator.h @@ -37,11 +37,15 @@ -namespace sofa { +namespace sofa +{ -namespace component { +namespace component +{ + +namespace behavior +{ -namespace behavior { /** @@ -51,8 +55,8 @@ namespace behavior { * class to emulate button * pressing on geomagic device */ -class GeoEmulator : public core::objectmodel::BaseObject { - +class GeoEmulator : public core::objectmodel::BaseObject +{ public: SOFA_CLASS(GeoEmulator, core::objectmodel::BaseObject); @@ -79,9 +83,9 @@ class GeoEmulator : public core::objectmodel::BaseObject { sofa::core::objectmodel::DataFileName d_positionSourceFilename; sofa::core::objectmodel::DataFileName d_buttonSourceFilename; - Data> timeData; - helper::vector m_deviceSecondButton; - helper::vector< helper::vector > m_devicePositions; + Data< type::vector > timeData; + type::vector m_deviceSecondButton; + type::vector< type::vector > m_devicePositions; size_t m_currentIndex; @@ -90,8 +94,11 @@ class GeoEmulator : public core::objectmodel::BaseObject { void parseSourceButtonFile(); }; + + } // namespace behavior } // namespace component } // namespace sofa + diff --git a/src/test/GeoEmulator.inl b/src/test/GeoEmulator.inl index 3e476e15..a54b7b0d 100644 --- a/src/test/GeoEmulator.inl +++ b/src/test/GeoEmulator.inl @@ -30,12 +30,15 @@ +namespace sofa +{ -namespace sofa { +namespace component +{ -namespace component { +namespace behavior +{ -namespace behavior { /** @@ -126,13 +129,13 @@ void GeoEmulator::bwdInit() */ void GeoEmulator::parseSourcePositionsFile() { - helper::WriteAccessor>> time = timeData; + helper::WriteAccessor> > time = timeData; -#ifdef __APPLE__ - setlocale(LC_ALL, "C"); -#else - std::setlocale(LC_ALL, "C"); -#endif + #ifdef __APPLE__ + setlocale(LC_ALL, "C"); + #else + std::setlocale(LC_ALL, "C"); + #endif time.clear(); m_devicePositions.clear(); @@ -173,14 +176,13 @@ void GeoEmulator::parseSourcePositionsFile() dataSize = tokens.size(); dimension = (tokens.size() - 1) / nParticles; - while (dataSize > 1) { double dt; dt = atof(tokens[0].c_str()); time.push_back(dt); - helper::vector position(nParticles); + type::vector position(nParticles); for (size_t i = 0; i < nParticles; i++) { for (size_t d = 0; d < dimension; d++) { position[i][d] = atof(tokens[dimension*i+d+1].c_str()); @@ -221,13 +223,13 @@ void GeoEmulator::parseSourcePositionsFile() */ void GeoEmulator::parseSourceButtonFile() { - helper::ReadAccessor>> time = timeData; + helper::ReadAccessor> > time = timeData; -#ifdef __APPLE__ - setlocale(LC_ALL, "C"); -#else - std::setlocale(LC_ALL, "C"); -#endif + #ifdef __APPLE__ + setlocale(LC_ALL, "C"); + #else + std::setlocale(LC_ALL, "C"); + #endif m_deviceSecondButton.clear(); @@ -263,7 +265,6 @@ void GeoEmulator::parseSourceButtonFile() timePosition++; } - std::getline(buttonFile, line); nLine++; @@ -298,7 +299,7 @@ void GeoEmulator::parseSourceButtonFile() void GeoEmulator::handleEvent(sofa::core::objectmodel::Event* event) { if (sofa::simulation::AnimateEndEvent::checkEventType(event)) { - helper::ReadAccessor>> time = timeData; + helper::ReadAccessor> > time = timeData; sofa::core::objectmodel::BaseContext* currentContext = this->getContext(); double currentTime = currentContext->getTime(); @@ -370,3 +371,4 @@ GeoEmulator::~GeoEmulator() } // namespace component } // namespace sofa + diff --git a/src/test/GeoListener.cpp b/src/test/GeoListener.cpp index 76ba7112..6ab8e90d 100644 --- a/src/test/GeoListener.cpp +++ b/src/test/GeoListener.cpp @@ -32,15 +32,20 @@ #include -namespace sofa { -namespace component { +namespace sofa +{ -namespace behavior { +namespace component +{ + +namespace behavior +{ -using namespace sofa::defaulttype; +using namespace sofa::defaulttype; + SOFA_DECL_CLASS(GeomagicDeviceListener) int GeomagicDeviceListenerClass = core::RegisterObject("Geomagic device manipulating object") @@ -49,12 +54,16 @@ int GeomagicDeviceListenerClass = core::RegisterObject("Geomagic device manipula .add< GeoListener >() ; + template class GeoListener; template class GeoListener; template class GeoListener; + + } // namespace bahavior } // namespace component } // namespace sofa + diff --git a/src/test/GeoListener.h b/src/test/GeoListener.h index 106736af..3e2499bb 100644 --- a/src/test/GeoListener.h +++ b/src/test/GeoListener.h @@ -25,8 +25,8 @@ #pragma once /* include files */ -#include #include +#include #include "../initOptimusPlugin.h" #include #include @@ -42,11 +42,15 @@ -namespace sofa { +namespace sofa +{ + +namespace component +{ -namespace component { +namespace behavior +{ -namespace behavior { /** @@ -57,15 +61,14 @@ namespace behavior { * events from geomagic device */ template -class GeoListener : public core::objectmodel::BaseObject { - +class GeoListener : public core::objectmodel::BaseObject +{ public: SOFA_CLASS(SOFA_TEMPLATE(GeoListener, DataTypes), core::objectmodel::BaseObject); // updating and accumulating Von Mises stress void handleEvent(sofa::core::objectmodel::Event* event) override; - // constructor and destructor GeoListener(); virtual ~GeoListener(); @@ -97,21 +100,24 @@ class GeoListener : public core::objectmodel::BaseObject { Data d_savePression; sofa::core::objectmodel::DataFileName d_pressFilename; - // geometry data for visualisation core::behavior::MechanicalState* m_mstate; forcefield::RestShapeSpringsForceField* m_spring; component::mapping::BarycentricMapping* m_mapping; }; + #if defined(SOFA_EXTERN_TEMPLATE) && !defined(SOFA_COMPONENT_GEOMAGIC_DEVICE_LISTENER_CPP) extern template class GeoListener; extern template class GeoListener; extern template class GeoListener; #endif + + } // namespace behavior } // namespace component } // namespace sofa + diff --git a/src/test/GeoListener.inl b/src/test/GeoListener.inl index 4bfadc9a..3eedb9d1 100644 --- a/src/test/GeoListener.inl +++ b/src/test/GeoListener.inl @@ -29,12 +29,15 @@ +namespace sofa +{ -namespace sofa { +namespace component +{ -namespace component { +namespace behavior +{ -namespace behavior { /** @@ -59,7 +62,6 @@ GeoListener::GeoListener() , d_detachStiffSpring( initData(&d_detachStiffSpring, "detachSpring", "spring detached") ) , d_savePression( initData(&d_savePression, (bool) false, "saveAttachmentData", "set to true to save attachment button pressing")) , d_pressFilename( initData(&d_pressFilename, "filename", "output attachment file name")) - { this->f_listening.setValue(true); } @@ -125,31 +127,30 @@ void GeoListener::handleEvent(sofa::core::objectmodel::Event* event) reinit(); } + // if (d_geomagicButtonPressed.getValue() && !m_buttonWasPressed) { - //if (d_geomagicButtonPressed.getValue() && !m_buttonWasPressed) { + // // rewrite sphere position + // helper::ReadAccessor< Data> > x_rA = m_mstate->read(core::VecCoordId::position()); + // defaulttype::Vec3d center = x_rA[0]; + // for (unsigned int i = 1; i < x_rA.size(); i++) + // { + // center += x_rA[i]; + // } + // center /= x_rA.size(); -// // rewrite sphere position -// helper::ReadAccessor< Data> > x_rA = m_mstate->read(core::VecCoordId::position()); -// defaulttype::Vec3d center = x_rA[0]; -// for (unsigned int i = 1; i < x_rA.size(); i++) -// { -// center += x_rA[i]; -// } -// center /= x_rA.size(); + // // recalculate geomagic position + // defaulttype::Vec3d difference = defaulttype::Vec3d(d_devicePosition.getValue().getCenter().x(), + // d_devicePosition.getValue().getCenter().y(), d_devicePosition.getValue().getCenter().z()) - center; -// // recalculate geomagic position -// defaulttype::Vec3d difference = defaulttype::Vec3d(d_devicePosition.getValue().getCenter().x(), -// d_devicePosition.getValue().getCenter().y(), d_devicePosition.getValue().getCenter().z()) - center; - -// helper::WriteAccessor< Data> > x_wA = m_mstate->write(core::VecCoordId::position()); -// for (unsigned int i = 0; i < x_wA.size(); i++) -// { -// x_wA[i] += difference; -// } + // helper::WriteAccessor< Data> > x_wA = m_mstate->write(core::VecCoordId::position()); + // for (unsigned int i = 0; i < x_wA.size(); i++) + // { + // x_wA[i] += difference; + // } // reinit(); // m_mapping->reinit(); - //} + // } if (d_savePression.getValue()) { sofa::core::objectmodel::BaseContext* currentContext = this->getContext(); @@ -209,8 +210,10 @@ GeoListener::~GeoListener() } + } // namespace bahavior } // namespace compoinent } // namespace sofa + From edfe0cb025cf1825daee20a5b4216a8b0c2c561f Mon Sep 17 00:00:00 2001 From: SergeiNikolaev Date: Fri, 4 Feb 2022 19:26:50 +0100 Subject: [PATCH 3/9] FIX: remove pthread dependence from CMake file --- CMakeLists.txt | 4 ++-- ReadMe.md | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index cc8a6667..751c0e25 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -36,7 +36,7 @@ find_package(Eigen3 3.3 REQUIRED NO_MODULE) include_directories(${EIGEN3_INCLUDE_DIR}) -set(LINKER_DEPENDENCIES SofaCore SofaBase SofaGeneralAnimationLoop SofaGeneralEngine SofaImplicitOdeSolver SofaGeneralLinearSolver SofaGeneralRigid SofaBoundaryCondition SofaBaseTopology SofaExporter SofaUserInteraction SofaConstraint SofaSimpleFem SofaGeneralTopology SofaTopologyMapping SofaUserInteraction SofaConstraint SofaGeneralLoader SofaMiscEngine SofaMiscFem SofaMiscForceField SofaMiscMapping SofaMiscSolver SofaMiscTopology SofaGuiCommon blas pthread Eigen3::Eigen) +set(LINKER_DEPENDENCIES SofaCore SofaBase SofaGeneralAnimationLoop SofaGeneralEngine SofaImplicitOdeSolver SofaGeneralLinearSolver SofaGeneralRigid SofaBoundaryCondition SofaBaseTopology SofaExporter SofaUserInteraction SofaConstraint SofaSimpleFem SofaGeneralTopology SofaTopologyMapping SofaUserInteraction SofaConstraint SofaGeneralLoader SofaMiscEngine SofaMiscFem SofaMiscForceField SofaMiscMapping SofaMiscSolver SofaMiscTopology SofaGuiCommon blas Eigen3::Eigen) set(COMPILATION_FLAGS "-DSOFA_BUILD_OPTIMUS -Wno-unused-local-typedef -Wno-deprecated") set(STOCHASTIC_FILTERING "1" CACHE BOOL "Set to activate the compilation of the filtering module" FORCE) @@ -168,7 +168,7 @@ add_library(${PROJECT_NAME} SHARED ${HEADER_FILES} ${SOURCE_FILES} ${README_FILE if(SofaPardisoSolver_FOUND) set(LINKER_DEPENDENCIES ${LINKER_DEPENDENCIES} SofaPardisoSolver) else() - message("SofaPardisoSolver not found, some scenes may not work") + message("SofaPardisoSolver not found, PardisoSolver component will not work") endif() target_include_directories(${PROJECT_NAME} PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}") diff --git a/ReadMe.md b/ReadMe.md index 92183141..6ba30c4d 100644 --- a/ReadMe.md +++ b/ReadMe.md @@ -24,7 +24,7 @@ In order to obtain the plugin, it is necessary to perform git clone of the plugi Requirements ------------ -Except for SOFA, the plugin dependencies are BLAS, Eigen, pthread. It is highly recommended to use Pardiso solver with Optimus, as other solvers in SOFA are less reliable and might impact the estimation. +Except for SOFA, the plugin dependencies are Eigen and BLAS. It is highly recommended to use Pardiso solver with Optimus, as other solvers in SOFA are less reliable and might impact the estimation. In-tree build From fd3602d552167c954e61cb7b403b3aad50de586e Mon Sep 17 00:00:00 2001 From: SergeiNikolaev Date: Fri, 11 Feb 2022 16:20:49 +0100 Subject: [PATCH 4/9] FIX: correct environment variables to execute benchmark tests --- .github/workflows/ubuntu.yml | 10 +++++----- .../assimBC_synthBrick/assimBC_synthBrick_GenObs.py | 1 + .../cyl3PartsGeomagic_GenObs_bench.py | 2 +- .../cyl3PartsConstForce_GenObs_bench.py | 11 ++++++++++- .../cyl3PartsConstForce_SDA_bench.py | 10 +++++++++- .../AppliedForces_SDA_bench.py | 1 + benchmarks/crontask/runOptimusTests.sh | 5 +++-- .../crontask/runOptimusTests_without_compiling.sh | 5 +++-- src/initOptimusPlugin.cpp | 6 +++--- 9 files changed, 36 insertions(+), 15 deletions(-) diff --git a/.github/workflows/ubuntu.yml b/.github/workflows/ubuntu.yml index 2c8bc6d8..0ec420f3 100644 --- a/.github/workflows/ubuntu.yml +++ b/.github/workflows/ubuntu.yml @@ -12,8 +12,8 @@ jobs: runs-on: ${{ matrix.os }} strategy: matrix: - os: [ubuntu-18.04] - python-version: [3.7] + os: [ubuntu-20.04] + python-version: [3.10] env: SOFA_ROOT_DIRECTORY: /opt/sofa SOFA_BUILD_DIRECTORY: /opt/sofa/build_release @@ -37,7 +37,7 @@ jobs: - name: Build pybind11 run: | - git clone -b v2.4 --depth 1 https://github.com/pybind/pybind11.git /tmp/pybind11 + git clone -b v2.8 --depth 1 https://github.com/pybind/pybind11.git /tmp/pybind11 cd /tmp/pybind11 cmake -DPYBIND11_TEST=OFF . cmake -DPYBIND11_TEST=OFF . @@ -50,7 +50,7 @@ jobs: - name: Build SOFA run: | - git clone --branch v20.12 https://github.com/sofa-framework/sofa.git $SOFA_ROOT_DIRECTORY + git clone --branch v21.06 https://github.com/sofa-framework/sofa.git $SOFA_ROOT_DIRECTORY mkdir $SOFA_BUILD_DIRECTORY cd $SOFA_BUILD_DIRECTORY cmake -DSOFAGUI_QGLVIEWER=OFF -DSOFAGUI_QT=OFF -DSOFAGUI_QTVIEWER=OFF .. @@ -63,7 +63,7 @@ jobs: - name: Build python3 plugin run: | - git clone --branch v20.12 https://github.com/sofa-framework/SofaPython3.git $SOFA_PYTHON3_DIRECTORY + git clone --branch v21.06 https://github.com/sofa-framework/SofaPython3.git $SOFA_PYTHON3_DIRECTORY mkdir $SOFA_PYTHON3_BUILD_DIRECTORY cd $SOFA_PYTHON3_BUILD_DIRECTORY cmake -DCMAKE_PREFIX_PATH=$SOFA_BUILD_DIRECTORY/install .. diff --git a/benchmarks/assimBC_synthBrick/assimBC_synthBrick_GenObs.py b/benchmarks/assimBC_synthBrick/assimBC_synthBrick_GenObs.py index 7801a645..a08dcaa8 100644 --- a/benchmarks/assimBC_synthBrick/assimBC_synthBrick_GenObs.py +++ b/benchmarks/assimBC_synthBrick/assimBC_synthBrick_GenObs.py @@ -15,6 +15,7 @@ def createScene(rootNode): rootNode.addObject('RequiredPlugin', name='Deformable', pluginName='SofaDeformable') rootNode.addObject('RequiredPlugin', name='MeshCollision', pluginName='SofaMeshCollision') rootNode.addObject('RequiredPlugin', name='Loader', pluginName='SofaLoader') + rootNode.addObject('RequiredPlugin', name='Exporter', pluginName='SofaExporter') rootNode.addObject('RequiredPlugin', name='Visual', pluginName='SofaOpenglVisual') # rootNode.addObject('RequiredPlugin', name='Python3', pluginName='SofaPython3') rootNode.addObject('RequiredPlugin', name='Optimus', pluginName='Optimus') diff --git a/benchmarks/assimStiffness_cylinder_geomagic_yaml/cyl3PartsGeomagic_GenObs_bench.py b/benchmarks/assimStiffness_cylinder_geomagic_yaml/cyl3PartsGeomagic_GenObs_bench.py index a6959b5e..09c4db0c 100644 --- a/benchmarks/assimStiffness_cylinder_geomagic_yaml/cyl3PartsGeomagic_GenObs_bench.py +++ b/benchmarks/assimStiffness_cylinder_geomagic_yaml/cyl3PartsGeomagic_GenObs_bench.py @@ -17,7 +17,7 @@ def createScene(rootNode): rootNode.addObject('RequiredPlugin', name='GeneralImplicitOdeSolver', pluginName='SofaGeneralImplicitOdeSolver') rootNode.addObject('RequiredPlugin', name='SparseSolver', pluginName='SofaSparseSolver') rootNode.addObject('RequiredPlugin', name='BoundaryCondition', pluginName='SofaBoundaryCondition') - rootNode.addObject('RequiredPlugin', name='Loader', pluginName='SofaLoader') + rootNode.addObject('RequiredPlugin', name='DataLoader', pluginName='SofaLoader') rootNode.addObject('RequiredPlugin', name='MiscForceField', pluginName='SofaMiscForceField') rootNode.addObject('RequiredPlugin', name='Rigid', pluginName='SofaRigid') rootNode.addObject('RequiredPlugin', name='SimpleFem', pluginName='SofaSimpleFem') diff --git a/benchmarks/assimStiffness_cylinder_python3_yaml/cyl3PartsConstForce_GenObs_bench.py b/benchmarks/assimStiffness_cylinder_python3_yaml/cyl3PartsConstForce_GenObs_bench.py index 7850d253..3424f251 100644 --- a/benchmarks/assimStiffness_cylinder_python3_yaml/cyl3PartsConstForce_GenObs_bench.py +++ b/benchmarks/assimStiffness_cylinder_python3_yaml/cyl3PartsConstForce_GenObs_bench.py @@ -12,7 +12,16 @@ def createScene(rootNode): - rootNode.addObject('RequiredPlugin', name='SofaGeneralEngine') + rootNode.addObject('RequiredPlugin', name='GeneralEngine', pluginName='SofaGeneralEngine') + rootNode.addObject('RequiredPlugin', name='Engine', pluginName='SofaEngine') + rootNode.addObject('RequiredPlugin', name='BoundaryCondition', pluginName='SofaBoundaryCondition') + rootNode.addObject('RequiredPlugin', name='ImplicitOdeSolver', pluginName='SofaImplicitOdeSolver') + rootNode.addObject('RequiredPlugin', name='SparseSolver', pluginName='SofaSparseSolver') + rootNode.addObject('RequiredPlugin', name='MiscForceField', pluginName='SofaMiscForceField') + rootNode.addObject('RequiredPlugin', name='SimpleFem', pluginName='SofaSimpleFem') + rootNode.addObject('RequiredPlugin', name='Deformable', pluginName='SofaDeformable') + rootNode.addObject('RequiredPlugin', name='Loader', pluginName='SofaLoader') + rootNode.addObject('RequiredPlugin', name='Visual', pluginName='SofaOpenglVisual') rootNode.addObject('RequiredPlugin', name='Optimus', pluginName='Optimus') # rootNode.addObject('RequiredPlugin', name='Python3', pluginName='SofaPython3') diff --git a/benchmarks/assimStiffness_cylinder_python3_yaml/cyl3PartsConstForce_SDA_bench.py b/benchmarks/assimStiffness_cylinder_python3_yaml/cyl3PartsConstForce_SDA_bench.py index 4101287f..c378c1b1 100644 --- a/benchmarks/assimStiffness_cylinder_python3_yaml/cyl3PartsConstForce_SDA_bench.py +++ b/benchmarks/assimStiffness_cylinder_python3_yaml/cyl3PartsConstForce_SDA_bench.py @@ -13,7 +13,15 @@ def createScene(rootNode): - rootNode.addObject('RequiredPlugin', name='SofaGeneralEngine') + rootNode.addObject('RequiredPlugin', name='GeneralEngine', pluginName='SofaGeneralEngine') + rootNode.addObject('RequiredPlugin', name='Engine', pluginName='SofaEngine') + rootNode.addObject('RequiredPlugin', name='BoundaryCondition', pluginName='SofaBoundaryCondition') + rootNode.addObject('RequiredPlugin', name='ImplicitOdeSolver', pluginName='SofaImplicitOdeSolver') + rootNode.addObject('RequiredPlugin', name='SparseSolver', pluginName='SofaSparseSolver') + rootNode.addObject('RequiredPlugin', name='MiscForceField', pluginName='SofaMiscForceField') + rootNode.addObject('RequiredPlugin', name='SimpleFem', pluginName='SofaSimpleFem') + rootNode.addObject('RequiredPlugin', name='Deformable', pluginName='SofaDeformable') + rootNode.addObject('RequiredPlugin', name='Loader', pluginName='SofaLoader') rootNode.addObject('RequiredPlugin', name='Optimus', pluginName='Optimus') # rootNode.addObject('RequiredPlugin', name='Python3', pluginName='SofaPython3') diff --git a/benchmarks/assimStiffness_cylinder_yaml/AppliedForces_SDA_bench.py b/benchmarks/assimStiffness_cylinder_yaml/AppliedForces_SDA_bench.py index d9c5e327..12b80f97 100644 --- a/benchmarks/assimStiffness_cylinder_yaml/AppliedForces_SDA_bench.py +++ b/benchmarks/assimStiffness_cylinder_yaml/AppliedForces_SDA_bench.py @@ -17,6 +17,7 @@ def createScene(rootNode): rootNode.addObject('RequiredPlugin', name='BoundaryCondition', pluginName='SofaBoundaryCondition') rootNode.addObject('RequiredPlugin', name='SLoader', pluginName='SofaLoader') rootNode.addObject('RequiredPlugin', name='SimpleFem', pluginName='SofaSimpleFem') + rootNode.addObject('RequiredPlugin', name='Exporter', pluginName='SofaExporter') rootNode.addObject('RequiredPlugin', name='GraphComponent', pluginName='SofaGraphComponent') # rootNode.addObject('RequiredPlugin', name='Python3', pluginName='SofaPython3') rootNode.addObject('RequiredPlugin', name='Optimus', pluginName='Optimus') diff --git a/benchmarks/crontask/runOptimusTests.sh b/benchmarks/crontask/runOptimusTests.sh index 1b5b8f28..099a5a9d 100755 --- a/benchmarks/crontask/runOptimusTests.sh +++ b/benchmarks/crontask/runOptimusTests.sh @@ -22,7 +22,8 @@ SOFA_PYTHON3_BUILD_DIRECTORY=$SOFA_PYTHON3_DIRECTORY/build_release ### export pardiso license export PARDISO_LIC_PATH=$HOME_DIRECTORY/External_libraries/Pardiso export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$HOME_DIRECTORY/External_libraries/Pardiso -export PYTHONPATH=$SOFA_PYTHON3_BUILD_DIRECTORY/lib/site-packages +export SOFA_ROOT=$BUILD_DIRECTORY/install +export PYTHONPATH=$SOFA_PYTHON3_BUILD_DIRECTORY/lib/python3/site-packages export SOFA_PLUGIN_PATH=$OPTIMUS_BUILD_DIRECTORY @@ -106,7 +107,7 @@ do cd $FOLDER if [ -f $FOLDER/verify.sh ]; then echo "Perform test: $FOLDER" - $FOLDER/verify.sh $BUILD_DIRECTORY/bin/runSofa $SOFA_PYTHON3_BUILD_DIRECTORY/lib >> $GENERAL_DIRECTORY/log_`/bin/date +"%Y_%m_%d"`.txt + $FOLDER/verify.sh $BUILD_DIRECTORY/install/bin/runSofa $SOFA_PYTHON3_BUILD_DIRECTORY/lib >> $GENERAL_DIRECTORY/log_`/bin/date +"%Y_%m_%d"`.txt fi done echo "All tests have been executed" diff --git a/benchmarks/crontask/runOptimusTests_without_compiling.sh b/benchmarks/crontask/runOptimusTests_without_compiling.sh index 6620d0e1..d27abe14 100755 --- a/benchmarks/crontask/runOptimusTests_without_compiling.sh +++ b/benchmarks/crontask/runOptimusTests_without_compiling.sh @@ -22,7 +22,8 @@ SOFA_PYTHON3_BUILD_DIRECTORY=$SOFA_PYTHON3_DIRECTORY/build_release ### export pardiso license export PARDISO_LIC_PATH=$HOME_DIRECTORY/External_libraries/Pardiso export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$HOME_DIRECTORY/External_libraries/Pardiso -export PYTHONPATH=$SOFA_PYTHON3_BUILD_DIRECTORY/lib/site-packages +export SOFA_ROOT=$BUILD_DIRECTORY/install +export PYTHONPATH=$SOFA_PYTHON3_BUILD_DIRECTORY/lib/python3/site-packages export SOFA_PLUGIN_PATH=$OPTIMUS_BUILD_DIRECTORY @@ -35,7 +36,7 @@ do cd $FOLDER if [ -f $FOLDER/verify.sh ]; then echo "Perform test: $FOLDER" - $FOLDER/verify.sh $BUILD_DIRECTORY/bin/runSofa $SOFA_PYTHON3_BUILD_DIRECTORY/lib >> $GENERAL_DIRECTORY/log_`/bin/date +"%Y_%m_%d"`.txt + $FOLDER/verify.sh $BUILD_DIRECTORY/install/bin/runSofa $SOFA_PYTHON3_BUILD_DIRECTORY/lib >> $GENERAL_DIRECTORY/log_`/bin/date +"%Y_%m_%d"`.txt fi done echo "All tests have been executed" diff --git a/src/initOptimusPlugin.cpp b/src/initOptimusPlugin.cpp index 7b402531..0a7663fa 100644 --- a/src/initOptimusPlugin.cpp +++ b/src/initOptimusPlugin.cpp @@ -52,19 +52,19 @@ namespace component const char* getModuleName() { - return "Extended Kalman filter"; + return "Optimus"; } const char* getModuleVersion() { - return "0.1"; + return "1.0"; } const char* getModuleLicense() { - return "LGPL"; + return "GPL"; } From cce5f4866200a93b86d2e8bf07f4d119359b0daa Mon Sep 17 00:00:00 2001 From: SergeiNikolaev Date: Fri, 11 Feb 2022 16:31:09 +0100 Subject: [PATCH 5/9] FIX: correct github script to run benchmark tests --- .github/workflows/ubuntu.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ubuntu.yml b/.github/workflows/ubuntu.yml index 0ec420f3..96006964 100644 --- a/.github/workflows/ubuntu.yml +++ b/.github/workflows/ubuntu.yml @@ -13,7 +13,7 @@ jobs: strategy: matrix: os: [ubuntu-20.04] - python-version: [3.10] + python-version: ['3.10'] env: SOFA_ROOT_DIRECTORY: /opt/sofa SOFA_BUILD_DIRECTORY: /opt/sofa/build_release From 7011c579604fb6e0b2afefa8ebde2d5e48f9d1bf Mon Sep 17 00:00:00 2001 From: SergeiNikolaev Date: Mon, 14 Feb 2022 12:32:18 +0100 Subject: [PATCH 6/9] FIX: remove libpthread dependence from Optimus --- .github/workflows/ubuntu.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ubuntu.yml b/.github/workflows/ubuntu.yml index 96006964..0d362df9 100644 --- a/.github/workflows/ubuntu.yml +++ b/.github/workflows/ubuntu.yml @@ -31,7 +31,7 @@ jobs: - name: Install requirements run: | - sudo apt install -qq libeigen3-dev libboost-all-dev libpthread-stubs0-dev libblas-dev freeglut3 freeglut3-dev libglew-dev + sudo apt install -qq libeigen3-dev libboost-all-dev libblas-dev freeglut3 freeglut3-dev libglew-dev python3 -m pip install --upgrade pip python3 -m pip install numpy pyyaml From 2c416ce0a2d76b562473604c322bffd9abb4a96c Mon Sep 17 00:00:00 2001 From: SergeiNikolaev Date: Mon, 14 Feb 2022 13:02:34 +0100 Subject: [PATCH 7/9] FIX: temporary replace ubuntu version --- .github/workflows/ubuntu.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ubuntu.yml b/.github/workflows/ubuntu.yml index 0d362df9..a4673242 100644 --- a/.github/workflows/ubuntu.yml +++ b/.github/workflows/ubuntu.yml @@ -12,7 +12,7 @@ jobs: runs-on: ${{ matrix.os }} strategy: matrix: - os: [ubuntu-20.04] + os: [ubuntu-18.04] python-version: ['3.10'] env: SOFA_ROOT_DIRECTORY: /opt/sofa From 38c8736b8f749c02a894f38f327b38a992f708cd Mon Sep 17 00:00:00 2001 From: SergeiNikolaev Date: Wed, 16 Feb 2022 00:29:42 +0100 Subject: [PATCH 8/9] FIX: remove python2 dependences from benchmark scripts --- benchmarks/crontask/plugin_list.conf.default | 47 -------------------- benchmarks/crontask/runOptimusTests.sh | 4 +- 2 files changed, 2 insertions(+), 49 deletions(-) delete mode 100644 benchmarks/crontask/plugin_list.conf.default diff --git a/benchmarks/crontask/plugin_list.conf.default b/benchmarks/crontask/plugin_list.conf.default deleted file mode 100644 index 06d00ee5..00000000 --- a/benchmarks/crontask/plugin_list.conf.default +++ /dev/null @@ -1,47 +0,0 @@ - -SofaSimpleFem 20.12.00 -SofaRigid 20.12.00 -SofaDeformable 20.12.00 -SofaObjectInteraction 20.12.00 -SofaMeshCollision 20.12.00 -SofaEngine 20.12.00 -SofaExplicitOdeSolver 20.12.00 -SofaImplicitOdeSolver 20.12.00 -SofaLoader 20.12.00 -SofaGeneralVisual 20.12.00 -SofaGraphComponent 20.12.00 -SofaGeneralMeshCollision 20.12.00 -SofaBoundaryCondition 20.12.00 -SofaGeneralAnimationLoop 20.12.00 -SofaGeneralDeformable 20.12.00 -SofaGeneralEngine 20.12.00 -SofaGeneralExplicitOdeSolver 20.12.00 -SofaGeneralImplicitOdeSolver 20.12.00 -SofaGeneralLinearSolver 20.12.00 -SofaGeneralRigid 20.12.00 -SofaGeneralObjectInteraction 20.12.00 -SofaGeneralSimpleFem 20.12.00 -SofaGeneralTopology 20.12.00 -SofaTopologyMapping 20.12.00 -SofaUserInteraction 20.12.00 -SofaConstraint 20.12.00 -SofaGeneralLoader 20.12.00 -SofaSparseSolver 20.12.00 -SofaPreconditioner 20.12.00 -SofaHaptics 20.12.00 -SofaValidation 20.12.00 -SofaDenseSolver 20.12.00 -SofaNonUniformFem 20.12.00 -SofaOpenglVisual 20.12.00 -SofaMiscTopology 20.12.00 -SofaMiscExtra 20.12.00 -SofaMiscForceField 20.12.00 -SofaMiscEngine 20.12.00 -SofaMiscSolver 20.12.00 -SofaMiscFem 20.12.00 -SofaMiscMapping 20.12.00 -CImgPlugin 0.1 -SofaMiscCollision 1.0 -DiffusionSolver 0.1 -image 0.1 -CGALPlugin 0.1 diff --git a/benchmarks/crontask/runOptimusTests.sh b/benchmarks/crontask/runOptimusTests.sh index 099a5a9d..a25eb952 100755 --- a/benchmarks/crontask/runOptimusTests.sh +++ b/benchmarks/crontask/runOptimusTests.sh @@ -72,8 +72,8 @@ cd $BUILD_DIRECTORY /usr/bin/make install 2>&1 >> $GENERAL_DIRECTORY/log_`/bin/date +"%Y_%m_%d"`.txt ### remove SofaPython plugin with python2 from the plugins default list (use local version of list) -cp $OPTIMUS_DIRECTORY/benchmarks/crontask/plugin_list.conf.default $BUILD_DIRECTORY/lib/plugin_list.conf.default -cp $OPTIMUS_DIRECTORY/benchmarks/crontask/plugin_list.conf.default $BUILD_DIRECTORY/install/lib/plugin_list.conf.default +# cp $OPTIMUS_DIRECTORY/benchmarks/crontask/plugin_list.conf.default $BUILD_DIRECTORY/lib/plugin_list.conf.default +# cp $OPTIMUS_DIRECTORY/benchmarks/crontask/plugin_list.conf.default $BUILD_DIRECTORY/install/lib/plugin_list.conf.default echo "Recompile python3 plugin" From 5cf701e48a5b113852b4ac3e1f6d3a29c6c5bf54 Mon Sep 17 00:00:00 2001 From: SergeiNikolaev Date: Thu, 17 Feb 2022 11:59:54 +0100 Subject: [PATCH 9/9] FIX: remove old depepndences from configuration parameters --- benchmarks/crontask/runOptimusTests.sh | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/benchmarks/crontask/runOptimusTests.sh b/benchmarks/crontask/runOptimusTests.sh index a25eb952..7bef8c1c 100755 --- a/benchmarks/crontask/runOptimusTests.sh +++ b/benchmarks/crontask/runOptimusTests.sh @@ -67,15 +67,10 @@ fi echo "Recompile sofa sources" cd $BUILD_DIRECTORY /usr/bin/make clean -/usr/local/bin/cmake -DSOFA_BUILD_TESTS=ON -DSOFAGUI_BUILD_TESTS=ON -DSOFA_EXTERNAL_DIRECTORIES=/home/sergei/Optimus_test/sofaconfig/sergei .. 2>&1 >> $GENERAL_DIRECTORY/log_`/bin/date +"%Y_%m_%d"`.txt +/usr/local/bin/cmake .. 2>&1 >> $GENERAL_DIRECTORY/log_`/bin/date +"%Y_%m_%d"`.txt /usr/bin/make -B -j 8 2>&1 >> $GENERAL_DIRECTORY/log_`/bin/date +"%Y_%m_%d"`.txt /usr/bin/make install 2>&1 >> $GENERAL_DIRECTORY/log_`/bin/date +"%Y_%m_%d"`.txt -### remove SofaPython plugin with python2 from the plugins default list (use local version of list) -# cp $OPTIMUS_DIRECTORY/benchmarks/crontask/plugin_list.conf.default $BUILD_DIRECTORY/lib/plugin_list.conf.default -# cp $OPTIMUS_DIRECTORY/benchmarks/crontask/plugin_list.conf.default $BUILD_DIRECTORY/install/lib/plugin_list.conf.default - - echo "Recompile python3 plugin" if ! [ -d "$SOFA_PYTHON3_BUILD_DIRECTORY" ]; then mkdir $SOFA_PYTHON3_BUILD_DIRECTORY