factor redirect output of derived factor evaluation

include/core/ceres_wrapper/cost_function_wrapper.h

@@ -58,7 +58,11 @@ inline CostFunctionWrapper::~CostFunctionWrapper() {}
 
 inline bool CostFunctionWrapper::Evaluate(const double* const* parameters, double* residuals, double** jacobians) const
 {
-    return factor_ptr_->evaluate(parameters, residuals, jacobians);
+    auto res = factor_ptr_->evaluate(parameters, residuals, jacobians);
+
+    WOLF_ERROR_COND(not res, "Error evaluating factor ", factor_ptr_->getType(), ": ", factor_ptr_->id());
+
+    return res;
 }
 
 inline FactorBasePtr CostFunctionWrapper::getFactor() const

include/core/factor/factor_analytic.h

@@ -47,7 +47,7 @@ class FactorAnalytic : public FactorBase
     /** Returns the residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the
      *point provided in _states_ptr
      **/
-    void evaluate(const std::vector<const double*>& _states_ptr,
+    bool evaluate(const std::vector<const double*>& _states_ptr,
                   Eigen::VectorXd&                  _residual,
                   std::vector<Eigen::MatrixXd>&     _jacobians) const override;
 

include/core/factor/factor_base.h

@@ -169,7 +169,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
     /** Returns a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in
      *_states_ptr and the residual vector
      **/
-    virtual void evaluate(const std::vector<const double*>& _states_ptr,
+    virtual bool evaluate(const std::vector<const double*>& _states_ptr,
                           Eigen::VectorXd&                  _residual,
                           std::vector<Eigen::MatrixXd>&     _jacobians) const = 0;
 

src/factor/factor_analytic.cpp

@@ -75,7 +75,7 @@ bool FactorAnalytic::evaluate(double const* const* parameters, double* residuals
     return true;
 };
 
-void FactorAnalytic::evaluate(const std::vector<const double*>& _states_ptr,
+bool FactorAnalytic::evaluate(const std::vector<const double*>& _states_ptr,
                               Eigen::VectorXd&                  residual_,
                               std::vector<Eigen::MatrixXd>&     jacobians_) const
 {
@@ -92,6 +92,8 @@ void FactorAnalytic::evaluate(const std::vector<const double*>& _states_ptr,
     // compute jacobians
     jacobians_.resize(state_block_sizes_.size());
     evaluateJacobians(state_blocks_map_, jacobians_, std::vector<bool>(state_block_sizes_.size(), true));
+    
+    return true;
 };
 
 }  // namespace wolf

test/dummy/factor_dummy.h

@@ -51,10 +51,11 @@ class FactorDummy : public FactorBase
         return true;
     }
 
-    void evaluate(const std::vector<const double*>& _states_ptr,
+    bool evaluate(const std::vector<const double*>& _states_ptr,
                   Eigen::VectorXd&                  _residual,
                   std::vector<Eigen::MatrixXd>&     _jacobians) const override
     {
+        return true;
     }
 
     unsigned int getSize() const override

test/dummy/factor_feature_dummy.h

@@ -48,9 +48,12 @@ class FactorFeatureDummy : public FactorBase
     /** Returns a residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the
      *point provided in _states_ptr
      **/
-    void evaluate(const std::vector<const double*>& _states_ptr,
+    bool evaluate(const std::vector<const double*>& _states_ptr,
                   Eigen::VectorXd&                  residual_,
-                  std::vector<Eigen::MatrixXd>&     jacobians_) const override{};
+                  std::vector<Eigen::MatrixXd>&     jacobians_) const override
+    {
+        return true;
+    };
 
   public:
     static FactorBasePtr create(const FeatureBasePtr&   _feature_ptr,

test/dummy/factor_landmark_dummy.h

@@ -46,9 +46,12 @@ class FactorLandmarkDummy : public FactorBase
     /** Returns a residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the
      *point provided in _states_ptr
      **/
-    void evaluate(const std::vector<const double*>& _states_ptr,
+    bool evaluate(const std::vector<const double*>& _states_ptr,
                   Eigen::VectorXd&                  residual_,
-                  std::vector<Eigen::MatrixXd>&     jacobians_) const override{};
+                  std::vector<Eigen::MatrixXd>&     jacobians_) const override
+    {
+        return true;
+    };
 
   public:
     static FactorBasePtr create(const FeatureBasePtr&   _feature_ptr,