Resolve "Add loss function API"

Closes #270 (closed)

include/core/factor/factor_analytic.h

@@ -18,43 +18,24 @@ class FactorAnalytic: public FactorBase
 
     public:
 
-        /** \brief Constructor of category FAC_ABSOLUTE
-         *
-         * Constructor of category FAC_ABSOLUTE
-         *
-         **/
         FactorAnalytic(const std::string&  _tp,
-                           bool _apply_loss_function,
-                           FactorStatus _status,
-                           StateBlockPtr _state0Ptr,
-                           StateBlockPtr _state1Ptr = nullptr,
-                           StateBlockPtr _state2Ptr = nullptr,
-                           StateBlockPtr _state3Ptr = nullptr,
-                           StateBlockPtr _state4Ptr = nullptr,
-                           StateBlockPtr _state5Ptr = nullptr,
-                           StateBlockPtr _state6Ptr = nullptr,
-                           StateBlockPtr _state7Ptr = nullptr,
-                           StateBlockPtr _state8Ptr = nullptr,
-                           StateBlockPtr _state9Ptr = nullptr ) ;
-
-        FactorAnalytic(const std::string&  _tp,
-                           const FrameBasePtr& _frame_other_ptr,
-                           const CaptureBasePtr& _capture_other_ptr,
-                           const FeatureBasePtr& _feature_other_ptr,
-                           const LandmarkBasePtr& _landmark_other_ptr,
-                           const ProcessorBasePtr& _processor_ptr,
-                           bool _apply_loss_function,
-                           FactorStatus _status,
-                           StateBlockPtr _state0Ptr,
-                           StateBlockPtr _state1Ptr = nullptr,
-                           StateBlockPtr _state2Ptr = nullptr,
-                           StateBlockPtr _state3Ptr = nullptr,
-                           StateBlockPtr _state4Ptr = nullptr,
-                           StateBlockPtr _state5Ptr = nullptr,
-                           StateBlockPtr _state6Ptr = nullptr,
-                           StateBlockPtr _state7Ptr = nullptr,
-                           StateBlockPtr _state8Ptr = nullptr,
-                           StateBlockPtr _state9Ptr = nullptr );
+                       const FrameBasePtr& _frame_other_ptr,
+                       const CaptureBasePtr& _capture_other_ptr,
+                       const FeatureBasePtr& _feature_other_ptr,
+                       const LandmarkBasePtr& _landmark_other_ptr,
+                       const ProcessorBasePtr& _processor_ptr,
+                       bool _apply_loss_function,
+                       FactorStatus _status,
+                       StateBlockPtr _state0Ptr,
+                       StateBlockPtr _state1Ptr = nullptr,
+                       StateBlockPtr _state2Ptr = nullptr,
+                       StateBlockPtr _state3Ptr = nullptr,
+                       StateBlockPtr _state4Ptr = nullptr,
+                       StateBlockPtr _state5Ptr = nullptr,
+                       StateBlockPtr _state6Ptr = nullptr,
+                       StateBlockPtr _state7Ptr = nullptr,
+                       StateBlockPtr _state8Ptr = nullptr,
+                       StateBlockPtr _state9Ptr = nullptr );
 
         virtual ~FactorAnalytic() = default;
 
@@ -86,7 +67,7 @@ class FactorAnalytic: public FactorBase
             // load parameters evaluation value
             std::vector<Eigen::Map<const Eigen::VectorXd>> state_blocks_map_;
             for (unsigned int i = 0; i < state_block_sizes_vector_.size(); i++)
-                state_blocks_map_.push_back(Eigen::Map<const Eigen::VectorXd>((double*)parameters[i], state_block_sizes_vector_[i]));
+                state_blocks_map_.emplace_back((double*)parameters[i], state_block_sizes_vector_[i]);
 
             // residuals
             Eigen::Map<Eigen::VectorXd> residuals_map((double*)residuals, getSize());
@@ -102,24 +83,34 @@ class FactorAnalytic: public FactorBase
                 {
                     compute_jacobians_[i] = (jacobians[i] != nullptr);
                     if (jacobians[i] != nullptr)
-                        jacobians_map_.push_back(Eigen::Map<Eigen::MatrixRowXd>((double*)jacobians[i], getSize(), state_block_sizes_vector_[i]));
+                        jacobians_map_.emplace_back((double*)jacobians[i], getSize(), state_block_sizes_vector_[i]);
                     else
-                        jacobians_map_.push_back(Eigen::Map<Eigen::MatrixRowXd>(nullptr, 0, 0)); //TODO: check if it can be done
+                        jacobians_map_.emplace_back(nullptr, 0, 0); //TODO: check if it can be done
                 }
 
                 // evaluate jacobians
                 evaluateJacobians(state_blocks_map_, jacobians_map_, compute_jacobians_);
             }
             return true;
-
-            return true;
         };
 
         /** Returns the residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
          **/
-        // TODO
         virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
         {
+            assert(_states_ptr.size() == state_block_sizes_vector_.size());
+
+            // load parameters evaluation value
+            std::vector<Eigen::Map<const Eigen::VectorXd>> state_blocks_map_;
+            for (unsigned int i = 0; i < state_block_sizes_vector_.size(); i++)
+                state_blocks_map_.emplace_back(_states_ptr[i], state_block_sizes_vector_[i]);
+
+            // residuals
+            residual_ = evaluateResiduals(state_blocks_map_);
+
+            // compute jacobians
+            jacobians_.resize(state_block_sizes_vector_.size());
+            evaluateJacobians(state_blocks_map_, jacobians_, std::vector<bool>(state_block_sizes_vector_.size(),true));
         };
 
         /** \brief Returns the residual evaluated in the states provided
@@ -139,7 +130,12 @@ class FactorAnalytic: public FactorBase
          * \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
          *
          **/
-        virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector, std::vector<Eigen::Map<Eigen::MatrixRowXd>>& jacobians, const std::vector<bool>& _compute_jacobian) const = 0;
+        virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+                                       std::vector<Eigen::Map<Eigen::MatrixRowXd>>& jacobians,
+                                       const std::vector<bool>& _compute_jacobian) const = 0;
+        virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+                                       std::vector<Eigen::MatrixXd>& jacobians,
+                                       const std::vector<bool>& _compute_jacobian) const = 0;
 
         /** \brief Returns the jacobians (without measurement noise normalization) evaluated in the current state blocks values
          *