Cosmetics in Problem .h and .cpp

src/problem.cpp

@@ -36,9 +36,9 @@ Problem::Problem(const std::string& _frame_structure) :
         trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
         map_ptr_(std::make_shared<MapBase>()),
         processor_motion_ptr_(),
-        prior_is_set_(false),
         state_size_(0),
-        state_cov_size_(0)
+        state_cov_size_(0),
+        prior_is_set_(false)
 {
     if (_frame_structure == "PO 2D")
     {
@@ -63,9 +63,9 @@ Problem::Problem(const std::string& _frame_structure) :
 
 void Problem::setup()
 {
-    hardware_ptr_->setProblem(shared_from_this());
-    trajectory_ptr_->setProblem(shared_from_this());
-    map_ptr_->setProblem(shared_from_this());
+    hardware_ptr_  -> setProblem(shared_from_this());
+    trajectory_ptr_-> setProblem(shared_from_this());
+    map_ptr_       -> setProblem(shared_from_this());
 }
 
 ProblemPtr Problem::create(const std::string& _frame_structure)
@@ -287,18 +287,15 @@ void Problem::getState(const TimeStamp& _ts, Eigen::VectorXs& state)
     assert(state.size() == getFrameStructureSize() && "Problem::getStateAtTimeStamp: bad state size");
 
     if (processor_motion_ptr_ != nullptr)
-    {
         processor_motion_ptr_->getState(_ts, state);
-    }
+
     else
     {
         FrameBasePtr closest_frame = trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
         if (closest_frame != nullptr)
-        {
             closest_frame->getState(state);
-        }else{
+        else
             state = zeroState();
-        }
     }
 }
 
@@ -316,7 +313,7 @@ Size Problem::getFrameStructureSize() const
 
 void Problem::getFrameStructureSize(Size& _x_size, Size& _cov_size) const
 {
-    _x_size = state_size_;
+    _x_size   = state_size_;
     _cov_size = state_cov_size_;
 }
 
@@ -334,6 +331,11 @@ Eigen::VectorXs Problem::zeroState()
 
 bool Problem::permitKeyFrame(ProcessorBasePtr _processor_ptr)
 {
+    // This should implement a black list of processors that have forbidden keyframe creation
+    // This decision is made at problem level, not at processor configuration level.
+    // If you want to configure a processor for not creating keyframes, see Processor::voting_active_ and its accessors.
+
+    // Currently allowing all processors to vote:
     return true;
 }
 

src/problem.h

@@ -55,8 +55,8 @@ class Problem : public std::enable_shared_from_this<Problem>
         std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXs> covariances_;
         std::list<StateBlockNotification> state_block_notification_list_;
         std::list<ConstraintNotification> constraint_notification_list_;
-        bool prior_is_set_;
         Size state_size_, state_cov_size_;
+        bool prior_is_set_;
 
     private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
         Problem(const std::string& _frame_structure); // USE create() below !!
@@ -203,18 +203,17 @@ class Problem : public std::enable_shared_from_this<Problem>
         FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
                                   const TimeStamp& _time_stamp);
 
-        // State getters
-        Eigen::VectorXs getCurrentState();
-        void getCurrentState(Eigen::VectorXs& state);
-        void getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& _ts);
-        Eigen::VectorXs getState(const TimeStamp& _ts);
-        void getState(const TimeStamp& _ts, Eigen::VectorXs& state);
-        FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts);
+        // Frame getters
+        FrameBasePtr    getLastFramePtr         ( );
+        FrameBasePtr    getLastKeyFramePtr      ( );
+        FrameBasePtr    closestKeyFrameToTimeStamp(const TimeStamp& _ts);
 
-        // Zero state provider
-        Eigen::VectorXs zeroState();
 
         /** \brief Give the permission to a processor to create a new keyFrame
+         *
+         * This should implement a black list of processors that have forbidden keyframe creation.
+         *   - This decision is made at problem level, not at processor configuration level.
+         *   - Therefore, if you want to permanently configure a processor for not creating keyframes, see Processor::voting_active_ and its accessors.
          */
         bool permitKeyFrame(ProcessorBasePtr _processor_ptr);
 
@@ -226,14 +225,15 @@ class Problem : public std::enable_shared_from_this<Problem>
                               ProcessorBasePtr _processor_ptr, //
                               const Scalar& _time_tolerance);
 
-        /** \brief Returns a pointer to last frame
-         **/
-        FrameBasePtr getLastFramePtr();
-
-        /** \brief Returns a pointer to last key frame
-         */
-        FrameBasePtr getLastKeyFramePtr();
-
+        // State getters
+        Eigen::VectorXs getCurrentState         ( );
+        void            getCurrentState         (Eigen::VectorXs& state);
+        void            getCurrentStateAndStamp (Eigen::VectorXs& state, TimeStamp& _ts);
+        Eigen::VectorXs getState                (const TimeStamp& _ts);
+        void            getState                (const TimeStamp& _ts, Eigen::VectorXs& state);
+        // Zero state provider
+        Eigen::VectorXs zeroState ( );
+        bool priorIsSet() const;
 
 
 
@@ -313,10 +313,6 @@ class Problem : public std::enable_shared_from_this<Problem>
                    bool state_blocks = false);
         bool check(int verbose_level = 0);
 
-        bool priorIsSet() const
-        {
-            return prior_is_set_;
-        }
 };
 
 } // namespace wolf
@@ -326,6 +322,11 @@ class Problem : public std::enable_shared_from_this<Problem>
 namespace wolf
 {
 
+inline bool Problem::priorIsSet() const
+{
+    return prior_is_set_;
+}
+
 inline ProcessorMotionPtr& Problem::getProcessorMotionPtr()
 {
     return processor_motion_ptr_;