Cleanup

include/core/problem/problem.h

@@ -79,17 +79,29 @@ class Problem : public std::enable_shared_from_this<Problem>
         static ProblemPtr autoSetup(ParamsServer &_server);
         virtual ~Problem();
 
+
+
         // Properties -----------------------------------------
+    public:
         SizeEigen getFrameStructureSize() const;
         void getFrameStructureSize(SizeEigen& _x_size, SizeEigen& _cov_size) const;
         SizeEigen getDim() const;
         StateStructure getFrameStructure() const;
+
+    protected:
         void appendToStructure(const StateStructure& _structure);
 
+
+
+
         // Tree manager --------------------------------------
+    public:
         void setTreeManager(TreeManagerBasePtr _gm);
         TreeManagerBasePtr getTreeManager() const;
 
+
+
+
         // Hardware branch ------------------------------------
         HardwareBasePtr getHardware() const;
 
@@ -172,6 +184,9 @@ class Problem : public std::enable_shared_from_this<Problem>
         IsMotionPtr getProcessorIsMotion();
         std::list<IsMotionPtr> getProcessorIsMotionList();
         
+
+
+
         // Trajectory branch ----------------------------------
         TrajectoryBasePtr getTrajectory() const;
 
@@ -312,14 +327,14 @@ class Problem : public std::enable_shared_from_this<Problem>
         // State getters
         TimeStamp       getTimeStamp    ( ) const;
         VectorComposite getState        ( const StateStructure& _structure = "" ) const;
-        VectorComposite getState        (const TimeStamp& _ts, const StateStructure& _structure = "" ) const;
-        VectorComposite stateZero       ( const StateStructure& _structure = "" ) const;
+        VectorComposite getState        ( const TimeStamp& _ts, const StateStructure& _structure = "" ) const;
 
         // Zero state provider
-        Eigen::VectorXd zeroState ( ) const;
+        VectorComposite stateZero       ( const StateStructure& _structure = "" ) const;
+        Eigen::VectorXd zeroState ( ) const; // todo deprecated
+
+
 
-        // Perturb state
-        void            perturb(double amplitude = 0.01);
 
         // Map branch -----------------------------------------
         MapBasePtr getMap() const;
@@ -327,7 +342,13 @@ class Problem : public std::enable_shared_from_this<Problem>
         void saveMap(const std::string& _filename_dot_yaml, //
                      const std::string& _map_name = "Map automatically saved by Wolf");
 
-        // Covariances -------------------------------------------
+
+
+        // All branches -------------------------------------------
+        // perturb states
+        void perturb(double amplitude = 0.01);
+
+        // Covariances
         void clearCovariance();
         void addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXd& _cov);
         void addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXd& _cov);
@@ -339,6 +360,8 @@ class Problem : public std::enable_shared_from_this<Problem>
         bool getLastKeyOrAuxFrameCovariance(Eigen::MatrixXd& _covariance) const;
         bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXd& _covariance) const;
 
+
+
         // Solver management ----------------------------------------
 
         /** \brief Notifies a new/removed state block to update the solver manager
@@ -388,10 +411,6 @@ class Problem : public std::enable_shared_from_this<Problem>
                    bool metric = true, //
                    bool state_blocks = false,
                    std::ostream& stream = std::cout) const;
-        void print(const std::string& depth, //
-                   bool constr_by = false, //
-                   bool metric = true, //
-                   bool state_blocks = false) const;
         bool check(int verbose_level = 0) const;
         bool check(bool verbose, std::ostream& stream) const;
 

src/problem/problem.cpp

@@ -1735,21 +1735,6 @@ bool Problem::check(int _verbose_level) const
 {
     return check((_verbose_level > 0), std::cout);
 }
-void Problem::print(const std::string& depth, bool constr_by, bool metric, bool state_blocks) const
-{
-    if (depth.compare("T") == 0)
-        print(0, constr_by, metric, state_blocks);
-    else if (depth.compare("F") == 0)
-        print(1, constr_by, metric, state_blocks);
-    else if (depth.compare("C") == 0)
-        print(2, constr_by, metric, state_blocks);
-    else if (depth.compare("f") == 0)
-        print(3, constr_by, metric, state_blocks);
-    else if (depth.compare("c") == 0)
-        print(4, constr_by, metric, state_blocks);
-    else
-        print(0, constr_by, metric, state_blocks);
-}
 
 void Problem::perturb(double amplitude)
 {