diff --git a/include/core/ceres_wrapper/cost_function_wrapper.h b/include/core/ceres_wrapper/cost_function_wrapper.h
index 357b706b6ded7413e98677f199dbf42602544d5f..021eeebc7bd4163d07c82d7b8bdc2b031dc65e3b 100644
--- a/include/core/ceres_wrapper/cost_function_wrapper.h
+++ b/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
diff --git a/include/core/factor/factor_analytic.h b/include/core/factor/factor_analytic.h
index e3e2592e46875ba091f690b0867b889de6e9d008..bc61b157e553b6bafcf61b522850d22b48713828 100644
--- a/include/core/factor/factor_analytic.h
+++ b/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;
diff --git a/include/core/factor/factor_autodiff.h b/include/core/factor/factor_autodiff.h
index 6dde3d183951282ab10f3683ad1957d55e48eaf9..6b2b1d2be2cfe75c326e342fcbaa367a7ef29afe 100644
--- a/include/core/factor/factor_autodiff.h
+++ b/include/core/factor/factor_autodiff.h
@@ -138,22 +138,22 @@ class FactorAutodiff : public FactorBase
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<FacT const*>(this))(params[0],
- params[1],
- params[2],
- params[3],
- params[4],
- params[5],
- params[6],
- params[7],
- params[8],
- params[9],
- params[10],
- params[11],
- params[12],
- params[13],
- params[14],
- residuals);
+ return (*static_cast<FacT const*>(this))(params[0],
+ params[1],
+ params[2],
+ params[3],
+ params[4],
+ params[5],
+ params[6],
+ params[7],
+ params[8],
+ params[9],
+ params[10],
+ params[11],
+ params[12],
+ params[13],
+ params[14],
+ residuals);
}
// also compute jacobians
else
@@ -164,22 +164,23 @@ class FactorAutodiff : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- jets_9_.data(),
- jets_10_.data(),
- jets_11_.data(),
- jets_12_.data(),
- jets_13_.data(),
- jets_14_.data(),
- residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ jets_12_.data(),
+ jets_13_.data(),
+ jets_14_.data(),
+ residuals_jets_.data());
+ if (not res) return false;
// fill the residual array
for (unsigned int i = 0; i < RES; i++) residuals[i] = residuals_jets_[i].a;
@@ -222,7 +223,7 @@ class FactorAutodiff : public FactorBase
*_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
{
@@ -235,22 +236,23 @@ class FactorAutodiff : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- jets_9_.data(),
- jets_10_.data(),
- jets_11_.data(),
- jets_12_.data(),
- jets_13_.data(),
- jets_14_.data(),
- residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ jets_12_.data(),
+ jets_13_.data(),
+ jets_14_.data(),
+ residuals_jets_.data());
+ if (not res) return false;
// fill the residual vector
for (unsigned int i = 0; i < RES; i++) residual_(i) = residuals_jets_[i].a;
@@ -269,6 +271,7 @@ class FactorAutodiff : public FactorBase
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
+ return true;
}
/** \brief Returns the residual size
@@ -383,21 +386,21 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<FacT const*>(this))(params[0],
- params[1],
- params[2],
- params[3],
- params[4],
- params[5],
- params[6],
- params[7],
- params[8],
- params[9],
- params[10],
- params[11],
- params[12],
- params[13],
- residuals);
+ return (*static_cast<FacT const*>(this))(params[0],
+ params[1],
+ params[2],
+ params[3],
+ params[4],
+ params[5],
+ params[6],
+ params[7],
+ params[8],
+ params[9],
+ params[10],
+ params[11],
+ params[12],
+ params[13],
+ residuals);
}
// also compute jacobians
else
@@ -408,21 +411,22 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- jets_9_.data(),
- jets_10_.data(),
- jets_11_.data(),
- jets_12_.data(),
- jets_13_.data(),
- residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ jets_12_.data(),
+ jets_13_.data(),
+ residuals_jets_.data());
+ if (not res) return false;
// fill the residual array
for (unsigned int i = 0; i < RES; i++) residuals[i] = residuals_jets_[i].a;
@@ -457,7 +461,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11
for (unsigned int i = 0; i < B13; i++) jets_13_[i].a = params[13][i];
}
- 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
{
@@ -470,21 +474,23 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- jets_9_.data(),
- jets_10_.data(),
- jets_11_.data(),
- jets_12_.data(),
- jets_13_.data(),
- residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ jets_12_.data(),
+ jets_13_.data(),
+ residuals_jets_.data());
+
+ if (not res) return false;
// fill the residual vector
for (unsigned int i = 0; i < RES; i++) residual_(i) = residuals_jets_[i].a;
@@ -503,6 +509,8 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
+
+ return true;
}
unsigned int getSize() const override
@@ -609,20 +617,20 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<FacT const*>(this))(params[0],
- params[1],
- params[2],
- params[3],
- params[4],
- params[5],
- params[6],
- params[7],
- params[8],
- params[9],
- params[10],
- params[11],
- params[12],
- residuals);
+ return (*static_cast<FacT const*>(this))(params[0],
+ params[1],
+ params[2],
+ params[3],
+ params[4],
+ params[5],
+ params[6],
+ params[7],
+ params[8],
+ params[9],
+ params[10],
+ params[11],
+ params[12],
+ residuals);
}
// also compute jacobians
else
@@ -633,20 +641,21 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- jets_9_.data(),
- jets_10_.data(),
- jets_11_.data(),
- jets_12_.data(),
- residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ jets_12_.data(),
+ residuals_jets_.data());
+ if (not res) return false;
// fill the residual array
for (unsigned int i = 0; i < RES; i++) residuals[i] = residuals_jets_[i].a;
@@ -680,7 +689,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11
for (unsigned int i = 0; i < B12; i++) jets_12_[i].a = params[12][i];
}
- 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
{
@@ -693,20 +702,21 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- jets_9_.data(),
- jets_10_.data(),
- jets_11_.data(),
- jets_12_.data(),
- residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ jets_12_.data(),
+ residuals_jets_.data());
+ if (not res) return false;
// fill the residual vector
for (unsigned int i = 0; i < RES; i++) residual_(i) = residuals_jets_[i].a;
@@ -725,6 +735,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
+ return true;
}
unsigned int getSize() const override
@@ -828,19 +839,19 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<FacT const*>(this))(params[0],
- params[1],
- params[2],
- params[3],
- params[4],
- params[5],
- params[6],
- params[7],
- params[8],
- params[9],
- params[10],
- params[11],
- residuals);
+ return (*static_cast<FacT const*>(this))(params[0],
+ params[1],
+ params[2],
+ params[3],
+ params[4],
+ params[5],
+ params[6],
+ params[7],
+ params[8],
+ params[9],
+ params[10],
+ params[11],
+ residuals);
}
// also compute jacobians
else
@@ -851,19 +862,20 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- jets_9_.data(),
- jets_10_.data(),
- jets_11_.data(),
- residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ residuals_jets_.data());
+ if (not res) return false;
// fill the residual array
for (unsigned int i = 0; i < RES; i++) residuals[i] = residuals_jets_[i].a;
@@ -896,7 +908,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11
for (unsigned int i = 0; i < B11; i++) jets_11_[i].a = params[11][i];
}
- 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
{
@@ -909,19 +921,20 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- jets_9_.data(),
- jets_10_.data(),
- jets_11_.data(),
- residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ residuals_jets_.data());
+ if (not res) return false;
// fill the residual vector
for (unsigned int i = 0; i < RES; i++) residual_(i) = residuals_jets_[i].a;
@@ -940,6 +953,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
+ return true;
}
unsigned int getSize() const override
@@ -1040,18 +1054,18 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, 0,
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<FacT const*>(this))(params[0],
- params[1],
- params[2],
- params[3],
- params[4],
- params[5],
- params[6],
- params[7],
- params[8],
- params[9],
- params[10],
- residuals);
+ return (*static_cast<FacT const*>(this))(params[0],
+ params[1],
+ params[2],
+ params[3],
+ params[4],
+ params[5],
+ params[6],
+ params[7],
+ params[8],
+ params[9],
+ params[10],
+ residuals);
}
// also compute jacobians
else
@@ -1062,18 +1076,19 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, 0,
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- jets_9_.data(),
- jets_10_.data(),
- residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ residuals_jets_.data());
+ if (not res) return false;
// fill the residual array
for (unsigned int i = 0; i < RES; i++) residuals[i] = residuals_jets_[i].a;
@@ -1105,7 +1120,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, 0,
for (unsigned int i = 0; i < B10; i++) jets_10_[i].a = params[10][i];
}
- 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
{
@@ -1118,18 +1133,19 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, 0,
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- jets_9_.data(),
- jets_10_.data(),
- residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ residuals_jets_.data());
+ if (not res) return false;
// fill the residual vector
for (unsigned int i = 0; i < RES; i++) residual_(i) = residuals_jets_[i].a;
@@ -1148,6 +1164,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, 0,
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
+ return true;
}
unsigned int getSize() const override
@@ -1245,17 +1262,17 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, 0, 0, 0,
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<FacT const*>(this))(params[0],
- params[1],
- params[2],
- params[3],
- params[4],
- params[5],
- params[6],
- params[7],
- params[8],
- params[9],
- residuals);
+ return (*static_cast<FacT const*>(this))(params[0],
+ params[1],
+ params[2],
+ params[3],
+ params[4],
+ params[5],
+ params[6],
+ params[7],
+ params[8],
+ params[9],
+ residuals);
}
// also compute jacobians
else
@@ -1266,17 +1283,18 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, 0, 0, 0,
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- jets_9_.data(),
- residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ residuals_jets_.data());
+ if (not res) return false;
// fill the residual array
for (unsigned int i = 0; i < RES; i++) residuals[i] = residuals_jets_[i].a;
@@ -1307,7 +1325,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, 0, 0, 0,
for (unsigned int i = 0; i < B9; i++) jets_9_[i].a = params[9][i];
}
- 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
{
@@ -1320,17 +1338,18 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, 0, 0, 0,
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- jets_9_.data(),
- residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ residuals_jets_.data());
+ if (not res) return false;
// fill the residual vector
for (unsigned int i = 0; i < RES; i++) residual_(i) = residuals_jets_[i].a;
@@ -1349,6 +1368,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, 0, 0, 0,
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
+ return true;
}
unsigned int getSize() const override
@@ -1443,16 +1463,16 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, 0, 0, 0, 0,
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<FacT const*>(this))(params[0],
- params[1],
- params[2],
- params[3],
- params[4],
- params[5],
- params[6],
- params[7],
- params[8],
- residuals);
+ return (*static_cast<FacT const*>(this))(params[0],
+ params[1],
+ params[2],
+ params[3],
+ params[4],
+ params[5],
+ params[6],
+ params[7],
+ params[8],
+ residuals);
}
// also compute jacobians
else
@@ -1463,16 +1483,17 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, 0, 0, 0, 0,
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ residuals_jets_.data());
+ if (not res) return false;
// fill the residual array
for (unsigned int i = 0; i < RES; i++) residuals[i] = residuals_jets_[i].a;
@@ -1502,7 +1523,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, 0, 0, 0, 0,
for (unsigned int i = 0; i < B8; i++) jets_8_[i].a = params[8][i];
}
- 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
{
@@ -1515,16 +1536,17 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, 0, 0, 0, 0,
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- jets_8_.data(),
- residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ residuals_jets_.data());
+ if (not res) return false;
// fill the residual vector
for (unsigned int i = 0; i < RES; i++) residual_(i) = residuals_jets_[i].a;
@@ -1543,6 +1565,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, B8, 0, 0, 0, 0,
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
+ return true;
}
unsigned int getSize() const override
@@ -1634,7 +1657,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, 0, 0, 0, 0, 0, 0
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<FacT const*>(this))(
+ return (*static_cast<FacT const*>(this))(
params[0], params[1], params[2], params[3], params[4], params[5], params[6], params[7], residuals);
}
// also compute jacobians
@@ -1646,15 +1669,16 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, 0, 0, 0, 0, 0, 0
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ residuals_jets_.data());
+ if (not res) return false;
// fill the residual array
for (unsigned int i = 0; i < RES; i++) residuals[i] = residuals_jets_[i].a;
@@ -1683,7 +1707,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, 0, 0, 0, 0, 0, 0
for (unsigned int i = 0; i < B7; i++) jets_7_[i].a = params[7][i];
}
- 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
{
@@ -1696,15 +1720,16 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, 0, 0, 0, 0, 0, 0
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- jets_7_.data(),
- residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ residuals_jets_.data());
+ if (not res) return false;
// fill the residual vector
for (unsigned int i = 0; i < RES; i++) residual_(i) = residuals_jets_[i].a;
@@ -1723,6 +1748,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, B7, 0, 0, 0, 0, 0, 0
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
+ return true;
}
unsigned int getSize() const override
@@ -1810,7 +1836,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, 0, 0, 0, 0, 0, 0, 0,
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<FacT const*>(this))(
+ return (*static_cast<FacT const*>(this))(
params[0], params[1], params[2], params[3], params[4], params[5], params[6], residuals);
}
// also compute jacobians
@@ -1822,14 +1848,15 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, 0, 0, 0, 0, 0, 0, 0,
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ residuals_jets_.data());
+ if (not res) return false;
// fill the residual array
for (unsigned int i = 0; i < RES; i++) residuals[i] = residuals_jets_[i].a;
@@ -1857,7 +1884,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, 0, 0, 0, 0, 0, 0, 0,
for (unsigned int i = 0; i < B6; i++) jets_6_[i].a = params[6][i];
}
- 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
{
@@ -1870,14 +1897,15 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, 0, 0, 0, 0, 0, 0, 0,
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ residuals_jets_.data());
+ if (not res) return false;
// fill the residual vector
for (unsigned int i = 0; i < RES; i++) residual_(i) = residuals_jets_[i].a;
@@ -1896,6 +1924,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, B6, 0, 0, 0, 0, 0, 0, 0,
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
+ return true;
}
unsigned int getSize() const override
@@ -1980,7 +2009,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, 0, 0, 0, 0, 0, 0, 0, 0,
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<FacT const*>(this))(
+ return (*static_cast<FacT const*>(this))(
params[0], params[1], params[2], params[3], params[4], params[5], residuals);
}
// also compute jacobians
@@ -1992,13 +2021,14 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, 0, 0, 0, 0, 0, 0, 0, 0,
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ residuals_jets_.data());
+ if (not res) return false;
// fill the residual array
for (unsigned int i = 0; i < RES; i++) residuals[i] = residuals_jets_[i].a;
@@ -2025,7 +2055,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, 0, 0, 0, 0, 0, 0, 0, 0,
for (unsigned int i = 0; i < B5; i++) jets_5_[i].a = params[5][i];
}
- 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
{
@@ -2038,13 +2068,14 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, 0, 0, 0, 0, 0, 0, 0, 0,
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ residuals_jets_.data());
+ if (not res) return false;
// fill the residual vector
for (unsigned int i = 0; i < RES; i++) residual_(i) = residuals_jets_[i].a;
@@ -2063,6 +2094,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, B5, 0, 0, 0, 0, 0, 0, 0, 0,
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
+ return true;
}
unsigned int getSize() const override
@@ -2144,7 +2176,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<FacT const*>(this))(params[0], params[1], params[2], params[3], params[4], residuals);
+ return (*static_cast<FacT const*>(this))(params[0], params[1], params[2], params[3], params[4], residuals);
}
// also compute jacobians
else
@@ -2155,12 +2187,13 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ residuals_jets_.data());
+ if (not res) return false;
// fill the residual array
for (unsigned int i = 0; i < RES; i++) residuals[i] = residuals_jets_[i].a;
@@ -2186,7 +2219,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
for (unsigned int i = 0; i < B4; i++) jets_4_[i].a = params[4][i];
}
- 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
{
@@ -2199,8 +2232,9 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(
+ bool res = (*static_cast<FacT const*>(this))(
jets_0_.data(), jets_1_.data(), jets_2_.data(), jets_3_.data(), jets_4_.data(), residuals_jets_.data());
+ if (not res) return false;
// fill the residual vector
for (unsigned int i = 0; i < RES; i++) residual_(i) = residuals_jets_[i].a;
@@ -2219,6 +2253,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, B4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
+ return true;
}
unsigned int getSize() const override
@@ -2292,7 +2327,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0>
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<FacT const*>(this))(params[0], params[1], params[2], params[3], residuals);
+ return (*static_cast<FacT const*>(this))(params[0], params[1], params[2], params[3], residuals);
}
// also compute jacobians
else
@@ -2303,8 +2338,9 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0>
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(
+ bool res = (*static_cast<FacT const*>(this))(
jets_0_.data(), jets_1_.data(), jets_2_.data(), jets_3_.data(), residuals_jets_.data());
+ if (not res) return false;
// fill the residual array
for (unsigned int i = 0; i < RES; i++) residuals[i] = residuals_jets_[i].a;
@@ -2329,7 +2365,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0>
for (unsigned int i = 0; i < B3; i++) jets_3_[i].a = params[3][i];
}
- 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
{
@@ -2342,8 +2378,9 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0>
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(
+ bool res = (*static_cast<FacT const*>(this))(
jets_0_.data(), jets_1_.data(), jets_2_.data(), jets_3_.data(), residuals_jets_.data());
+ if (not res) return false;
// fill the residual vector
for (unsigned int i = 0; i < RES; i++) residual_(i) = residuals_jets_[i].a;
@@ -2362,6 +2399,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, B3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0>
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
+ return true;
}
unsigned int getSize() const override
@@ -2433,7 +2471,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0>
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<FacT const*>(this))(params[0], params[1], params[2], residuals);
+ return (*static_cast<FacT const*>(this))(params[0], params[1], params[2], residuals);
}
// also compute jacobians
else
@@ -2444,7 +2482,9 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0>
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(), jets_1_.data(), jets_2_.data(), residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(
+ jets_0_.data(), jets_1_.data(), jets_2_.data(), residuals_jets_.data());
+ if (not res) return false;
// fill the residual array
for (unsigned int i = 0; i < RES; i++) residuals[i] = residuals_jets_[i].a;
@@ -2468,7 +2508,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0>
for (unsigned int i = 0; i < B2; i++) jets_2_[i].a = params[2][i];
}
- 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
{
@@ -2481,7 +2521,9 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0>
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(), jets_1_.data(), jets_2_.data(), residuals_jets_.data());
+ bool res =
+ (*static_cast<FacT const*>(this))(jets_0_.data(), jets_1_.data(), jets_2_.data(), residuals_jets_.data());
+ if (not res) return false;
// fill the residual vector
for (unsigned int i = 0; i < RES; i++) residual_(i) = residuals_jets_[i].a;
@@ -2500,6 +2542,7 @@ class FactorAutodiff<FacT, RES, B0, B1, B2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0>
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
+ return true;
}
unsigned int getSize() const override
@@ -2569,7 +2612,7 @@ class FactorAutodiff<FacT, RES, B0, B1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0> :
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<FacT const*>(this))(params[0], params[1], residuals);
+ return (*static_cast<FacT const*>(this))(params[0], params[1], residuals);
}
// also compute jacobians
else
@@ -2580,7 +2623,8 @@ class FactorAutodiff<FacT, RES, B0, B1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0> :
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(), jets_1_.data(), residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(), jets_1_.data(), residuals_jets_.data());
+ if (not res) return false;
// fill the residual array
for (unsigned int i = 0; i < RES; i++) residuals[i] = residuals_jets_[i].a;
@@ -2603,7 +2647,7 @@ class FactorAutodiff<FacT, RES, B0, B1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0> :
for (unsigned int i = 0; i < B1; i++) jets_1_[i].a = params[1][i];
}
- 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
{
@@ -2616,7 +2660,8 @@ class FactorAutodiff<FacT, RES, B0, B1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0> :
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(), jets_1_.data(), residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(), jets_1_.data(), residuals_jets_.data());
+ if (not res) return false;
// fill the residual vector
for (unsigned int i = 0; i < RES; i++) residual_(i) = residuals_jets_[i].a;
@@ -2635,6 +2680,7 @@ class FactorAutodiff<FacT, RES, B0, B1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0> :
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
+ return true;
}
unsigned int getSize() const override
@@ -2702,7 +2748,7 @@ class FactorAutodiff<FacT, RES, B0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0> :
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<FacT const*>(this))(params[0], residuals);
+ return (*static_cast<FacT const*>(this))(params[0], residuals);
}
// also compute jacobians
else
@@ -2713,7 +2759,9 @@ class FactorAutodiff<FacT, RES, B0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0> :
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(), residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(), residuals_jets_.data());
+
+ if (not res) return false;
// fill the residual array
for (unsigned int i = 0; i < RES; i++) residuals[i] = residuals_jets_[i].a;
@@ -2735,7 +2783,7 @@ class FactorAutodiff<FacT, RES, B0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0> :
for (unsigned int i = 0; i < B0; i++) jets_0_[i].a = params[0][i];
}
- 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
{
@@ -2748,7 +2796,9 @@ class FactorAutodiff<FacT, RES, B0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0> :
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_.data(), residuals_jets_.data());
+ bool res = (*static_cast<FacT const*>(this))(jets_0_.data(), residuals_jets_.data());
+
+ if (not res) return false;
// fill the residual vector
for (unsigned int i = 0; i < RES; i++) residual_(i) = residuals_jets_[i].a;
@@ -2767,6 +2817,8 @@ class FactorAutodiff<FacT, RES, B0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0> :
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
+
+ return true;
}
unsigned int getSize() const override
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index 8b4d704791f625d2bd7fd2ea33956075962a3ebb..0e2f95ce7d5199f103c2919b6aa23046f710b1e4 100644
--- a/include/core/factor/factor_base.h
+++ b/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;
diff --git a/src/factor/factor_analytic.cpp b/src/factor/factor_analytic.cpp
index 5dce1fcd1754602ee11a90a140f5bd1a7ad373d3..77cca6cb1aac0545129638fd3172f9998f9f484f 100644
--- a/src/factor/factor_analytic.cpp
+++ b/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
diff --git a/test/dummy/factor_dummy.h b/test/dummy/factor_dummy.h
index 80b4afe46625b387184f6a4be0403cb91ae11a21..7ebd261c649ef5db30492598e798e9bd95520219 100644
--- a/test/dummy/factor_dummy.h
+++ b/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
diff --git a/test/dummy/factor_feature_dummy.h b/test/dummy/factor_feature_dummy.h
index 8fde5b87169cd8c3ec7760c9c67735bd391d94e0..39498a77e18a20b73067dbc994eaba92da135650 100644
--- a/test/dummy/factor_feature_dummy.h
+++ b/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,
diff --git a/test/dummy/factor_landmark_dummy.h b/test/dummy/factor_landmark_dummy.h
index d6af7753217d0c2d2ec1e694080429f04ec4c32b..b53e28a63fcba8bb9ae85c57b7c85b0745985095 100644
--- a/test/dummy/factor_landmark_dummy.h
+++ b/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,