diff --git a/include/core/factor/factor_analytic.h b/include/core/factor/factor_analytic.h
index 7b52572a64a6dc8efd08e27c3debde46d1d1207b..f1f9f02e617be2046471b7cecb26959bfe610143 100644
--- a/include/core/factor/factor_analytic.h
+++ b/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
*
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index a79e0640337809e2c9a22e3c6525d2b65b6386b9..6f215396071b757b980faac31be8d0250716ea71 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -57,12 +57,6 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
virtual void setProblem(ProblemPtr) final;
public:
- /** \brief Constructor not involving nodes of other frames, only feature, capture and/or frame of this factor
- **/
- FactorBase(const std::string& _tp,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE);
-
/** \brief Default constructor.
*
* IMPORTANT: "other" means "of another branch of the wolf tree".
@@ -74,8 +68,8 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
virtual ~FactorBase() = default;
diff --git a/include/core/factor/factor_block_absolute.h b/include/core/factor/factor_block_absolute.h
index 48256ba25a260b7e89f4927b7062b482957236e6..eb55faae757f7fe1b02d2fe29c75a049e0b04299 100644
--- a/include/core/factor/factor_block_absolute.h
+++ b/include/core/factor/factor_block_absolute.h
@@ -38,9 +38,15 @@ class FactorBlockAbsolute : public FactorAnalytic
FactorBlockAbsolute(StateBlockPtr _sb_ptr,
unsigned int _start_idx = 0,
int _size = -1,
+ ProcessorBasePtr _processor_ptr = nullptr,
bool _apply_loss_function = false,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("BLOCK ABS",
+ nullptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
_apply_loss_function,
_status,
_sb_ptr),
@@ -84,9 +90,12 @@ class FactorBlockAbsolute : public FactorAnalytic
* \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,
+ 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 override;
+ 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 override;
/** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
*
@@ -114,9 +123,24 @@ inline Eigen::VectorXd FactorBlockAbsolute::evaluateResiduals(const std::vector<
return getMeasurementSquareRootInformationUpper() * (_st_vector.front().segment(sb_constrained_start_,sb_constrained_size_) - getMeasurement());
}
-inline void FactorBlockAbsolute::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
+inline void FactorBlockAbsolute::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
+{
+ assert(_st_vector.size() == 1 && "Wrong number of state blocks!");
+ assert(jacobians.size() == 1 && "Wrong number of jacobians!");
+ assert(_compute_jacobian.size() == 1 && "Wrong number of _compute_jacobian booleans!");
+ assert(_st_vector.front().size() == sb_size_ && "Wrong StateBlock size");
+ assert(_st_vector.front().size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
+
+ // normalized jacobian
+ if (_compute_jacobian.front())
+ jacobians.front() = getMeasurementSquareRootInformationUpper() * J_;
+}
+
+inline void FactorBlockAbsolute::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::MatrixXd>& jacobians,
+ const std::vector<bool>& _compute_jacobian) const
{
assert(_st_vector.size() == 1 && "Wrong number of state blocks!");
assert(jacobians.size() == 1 && "Wrong number of jacobians!");
diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index a57ca27393cb1a05f87923755d7e6695e5f76fcf..5f6e149883ff1499757af2db05a9f8fc2a956b16 100644
--- a/include/core/factor/factor_diff_drive.h
+++ b/include/core/factor/factor_diff_drive.h
@@ -50,8 +50,8 @@ class FactorDiffDrive : public FactorAutodiff<FactorDiffDrive,
FactorDiffDrive(const FeatureMotionPtr& _ftr_ptr,
const CaptureBasePtr& _capture_origin_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- const bool _apply_loss_function = false,
+ const ProcessorBasePtr& _processor_ptr,
+ const bool _apply_loss_function,
const FactorStatus _status = FAC_ACTIVE) :
Base("FactorDiffDrive",
_capture_origin_ptr->getFrame(),
diff --git a/include/core/factor/factor_odom_2D.h b/include/core/factor/factor_odom_2D.h
index 23be00e288af137aafbac87025fc066de6fd51a3..2f1a011c19ae11ae78aaea9a17fd1c5b0d23fe5c 100644
--- a/include/core/factor/factor_odom_2D.h
+++ b/include/core/factor/factor_odom_2D.h
@@ -17,8 +17,9 @@ class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>
public:
FactorOdom2D(const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_other_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>("FactorOdom2D",
_frame_other_ptr, nullptr, nullptr, nullptr,
_processor_ptr,
@@ -43,11 +44,11 @@ class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>
bool operator ()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2,
T* _residuals) const;
- public:
- static FactorBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr)
- {
- return std::make_shared<FactorOdom2D>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
- }
+// public:
+// static FactorBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr)
+// {
+// return std::make_shared<FactorOdom2D>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
+// }
};
diff --git a/include/core/factor/factor_odom_2D_analytic.h b/include/core/factor/factor_odom_2D_analytic.h
index f705a76edb9dd87ad8aa5c7cb0a253fcb4859754..96b3688fd01dada34da94ac21f7bef178c2e5d42 100644
--- a/include/core/factor/factor_odom_2D_analytic.h
+++ b/include/core/factor/factor_odom_2D_analytic.h
@@ -14,12 +14,16 @@ class FactorOdom2DAnalytic : public FactorRelative2DAnalytic
{
public:
FactorOdom2DAnalytic(const FeatureBasePtr& _ftr_ptr,
- const FrameBasePtr& _frame_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE) :
- FactorRelative2DAnalytic("ODOM_2D", _ftr_ptr,
- _frame_ptr, _processor_ptr, _apply_loss_function, _status)
+ const FrameBasePtr& _frame_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorRelative2DAnalytic("ODOM_2D",
+ _ftr_ptr,
+ _frame_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status)
{
//
}
@@ -31,82 +35,15 @@ class FactorOdom2DAnalytic : public FactorRelative2DAnalytic
return std::string("MOTION");
}
-
-// /** \brief Returns the factor residual size
-// *
-// * Returns the factor residual size
-// *
-// **/
-// virtual unsigned int getSize() const
-// {
-// return 3;
-// }
-//
-// /** \brief Returns the residual evaluated in the states provided
-// *
-// * Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXd
-// *
-// **/
-// virtual Eigen::VectorXd evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector) const
-// {
-// Eigen::VectorXd residual(3);
-// Eigen::VectorXd expected_measurement(3);
-//
-// // Expected measurement
-// Eigen::Matrix2d R = Eigen::Rotation2D<double>(-_st_vector[1](0)).matrix();
-// expected_measurement.head(2) = R * (_st_vector[2]-_st_vector[0]); // rotar menys l'angle de primer (-_o1)
-// expected_measurement(2) = _st_vector[3](0) - _st_vector[1](0);
-//
-// // Residual
-// residual = expected_measurement - getMeasurement();
-// while (residual(2) > M_PI)
-// residual(2) = residual(2) - 2*M_PI;
-// while (residual(2) <= -M_PI)
-// residual(2) = residual(2) + 2*M_PI;
-// residual = getMeasurementSquareRootInformationUpper() * residual;
-//
-// return residual;
-// }
-//
-// /** \brief Returns the jacobians evaluated in the states provided
-// *
-// * Returns the jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXd.
-// * IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
-// *
-// * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
-// * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
-// * \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
+ public:
+// static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
+// const NodeBasePtr& _correspondant_ptr,
+// const ProcessorBasePtr& _processor_ptr = nullptr)
// {
-// jacobians[0] << -cos(_st_vector[1](0)), -sin(_st_vector[1](0)),
-// sin(_st_vector[1](0)), -cos(_st_vector[1](0)),
-// 0, 0;
-// jacobians[0] = getMeasurementSquareRootInformationUpper() * jacobians[0];
-//
-// jacobians[1] << -(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0)) + (_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)),
-// -(_st_vector[2](0) - _st_vector[0](0)) * cos(_st_vector[1](0)) - (_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0)),
-// -1;
-// jacobians[1] = getMeasurementSquareRootInformationUpper() * jacobians[0];
-//
-// jacobians[2] << cos(_st_vector[1](0)), sin(_st_vector[1](0)),
-// -sin(_st_vector[1](0)),cos(_st_vector[1](0)),
-// 0, 0;
-// jacobians[2] = getMeasurementSquareRootInformationUpper() * jacobians[0];
-//
-// jacobians[3] << 0, 0, 1;
-// jacobians[3] = getMeasurementSquareRootInformationUpper() * jacobians[0];
+// return std::make_shared<FactorOdom2DAnalytic>(_feature_ptr,
+// std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
// }
- public:
- static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
- const NodeBasePtr& _correspondant_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr)
- {
- return std::make_shared<FactorOdom2DAnalytic>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
- }
-
};
} // namespace wolf
diff --git a/include/core/factor/factor_odom_3D.h b/include/core/factor/factor_odom_3D.h
index 347bf50685cc05d93ccb55f0a80f58417b31ae6b..d359adbbd275a3faca43d1a154590830cd9425a6 100644
--- a/include/core/factor/factor_odom_3D.h
+++ b/include/core/factor/factor_odom_3D.h
@@ -22,8 +22,8 @@ class FactorOdom3D : public FactorAutodiff<FactorOdom3D,6,3,4,3,4>
public:
FactorOdom3D(const FeatureBasePtr& _ftr_current_ptr,
const FrameBasePtr& _frame_past_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
virtual ~FactorOdom3D() = default;
@@ -77,10 +77,10 @@ inline void FactorOdom3D::printRes (const Eigen::Matrix<double,6,1> & r) const
}
inline FactorOdom3D::FactorOdom3D(const FeatureBasePtr& _ftr_current_ptr,
- const FrameBasePtr& _frame_past_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) :
+ const FrameBasePtr& _frame_past_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status) :
FactorAutodiff<FactorOdom3D, 6, 3, 4, 3, 4>("FactorOdom3D", // type
_frame_past_ptr, // frame other
nullptr, // capture other
diff --git a/include/core/factor/factor_pose_2D.h b/include/core/factor/factor_pose_2D.h
index 5c47ee4bb272531fa17fa65caed367e8f3eadf49..91229e4c83614df3a92d03ff9e24a407a719eadb 100644
--- a/include/core/factor/factor_pose_2D.h
+++ b/include/core/factor/factor_pose_2D.h
@@ -17,8 +17,16 @@ WOLF_PTR_TYPEDEFS(FactorPose2D);
class FactorPose2D: public FactorAutodiff<FactorPose2D,3,2,1>
{
public:
- FactorPose2D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorPose2D, 3, 2, 1>("FactorPose2D", nullptr, nullptr, nullptr, nullptr, nullptr,_apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
+ FactorPose2D(FeatureBasePtr _ftr_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorPose2D, 3, 2, 1>("FactorPose2D",
+ nullptr, nullptr, nullptr, nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
{
// std::cout << "created FactorPose2D " << std::endl;
}
@@ -30,7 +38,6 @@ class FactorPose2D: public FactorAutodiff<FactorPose2D,3,2,1>
return std::string("ABS");
}
-
template<typename T>
bool operator ()(const T* const _p, const T* const _o, T* _residuals) const;
diff --git a/include/core/factor/factor_pose_3D.h b/include/core/factor/factor_pose_3D.h
index f8032f3b3bbfdaa6dcc490385f6880650fa63e12..81b1837969a3ef62aa069aece3864713d6b31e76 100644
--- a/include/core/factor/factor_pose_3D.h
+++ b/include/core/factor/factor_pose_3D.h
@@ -16,8 +16,16 @@ class FactorPose3D: public FactorAutodiff<FactorPose3D,6,3,4>
{
public:
- FactorPose3D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorPose3D,6,3,4>("FactorPose3D", nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
+ FactorPose3D(FeatureBasePtr _ftr_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorPose3D,6,3,4>("FactorPose3D",
+ nullptr, nullptr, nullptr, nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
{
//
}
diff --git a/include/core/factor/factor_quaternion_absolute.h b/include/core/factor/factor_quaternion_absolute.h
index 1d9793c7253f9ceb700d6320544d85c16d1110fc..4c8c7bf1a735b76b9ac3cbcb12d246a728fd806a 100644
--- a/include/core/factor/factor_quaternion_absolute.h
+++ b/include/core/factor/factor_quaternion_absolute.h
@@ -23,9 +23,19 @@ class FactorQuaternionAbsolute: public FactorAutodiff<FactorQuaternionAbsolute,3
{
public:
- FactorQuaternionAbsolute(StateBlockPtr _sb_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+ FactorQuaternionAbsolute(StateBlockPtr _sb_ptr,
+ ProcessorBasePtr _processor_ptr = nullptr,
+ bool _apply_loss_function = false,
+ FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorQuaternionAbsolute,3,4>("QUATERNION ABS",
- nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _sb_ptr)
+ nullptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _sb_ptr)
{
//
}
diff --git a/include/core/factor/factor_relative_2D_analytic.h b/include/core/factor/factor_relative_2D_analytic.h
index 79da205abd9a3b703c0daba65424da7725d5aefe..be2b7f16a3d5ce01f4c15ea9f4dbd52d7848a956 100644
--- a/include/core/factor/factor_relative_2D_analytic.h
+++ b/include/core/factor/factor_relative_2D_analytic.h
@@ -20,8 +20,8 @@ class FactorRelative2DAnalytic : public FactorAnalytic
FactorRelative2DAnalytic(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic(_tp, _frame_ptr, nullptr, nullptr, nullptr, _processor_ptr, _apply_loss_function, _status, _frame_ptr->getP(), _frame_ptr->getO(), _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
{
@@ -33,8 +33,8 @@ class FactorRelative2DAnalytic : public FactorAnalytic
FactorRelative2DAnalytic(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
const FeatureBasePtr& _ftr_other_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic(_tp, nullptr, nullptr, _ftr_other_ptr, nullptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _ftr_other_ptr->getFrame()->getP(), _ftr_other_ptr->getFrame()->getO() )
{
@@ -46,8 +46,8 @@ class FactorRelative2DAnalytic : public FactorAnalytic
FactorRelative2DAnalytic(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
const LandmarkBasePtr& _landmark_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic(_tp, nullptr, nullptr, nullptr, _landmark_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _landmark_ptr->getP(), _landmark_ptr->getO())
{
@@ -85,9 +85,12 @@ class FactorRelative2DAnalytic : public FactorAnalytic
* \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,
+ 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 override;
+ 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 override;
/** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
@@ -118,44 +121,104 @@ inline Eigen::VectorXd FactorRelative2DAnalytic::evaluateResiduals(
return residual;
}
-inline void FactorRelative2DAnalytic::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
+inline void FactorRelative2DAnalytic::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
+{
+ assert(jacobians.size() == 4);
+ assert(_st_vector.size() == 4);
+ assert(_compute_jacobian.size() == 4);
+
+ if (_compute_jacobian[0])
+ {
+ jacobians[0] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(3,2) <<
+ -cos(_st_vector[1](0)), -sin(_st_vector[1](0)),
+ sin(_st_vector[1](0)), -cos(_st_vector[1](0)),
+ 0, 0).finished();
+ }
+ if (_compute_jacobian[1])
+ {
+ jacobians[1] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(3,1) <<
+ -(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0)) + (_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)),
+ -(_st_vector[2](0) - _st_vector[0](0)) * cos(_st_vector[1](0)) - (_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0)),
+ -1).finished();
+ }
+ if (_compute_jacobian[2])
+ {
+ jacobians[2] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(3,2) <<
+ cos(_st_vector[1](0)), sin(_st_vector[1](0)),
+ -sin(_st_vector[1](0)), cos(_st_vector[1](0)),
+ 0, 0).finished();
+ }
+ if (_compute_jacobian[3])
+ {
+ jacobians[3] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(3,1) <<
+ 0, 0, 1).finished();
+ }
+}
+
+inline void FactorRelative2DAnalytic::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::MatrixXd>& jacobians,
+ const std::vector<bool>& _compute_jacobian) const
{
- jacobians[0] << -cos(_st_vector[1](0)), -sin(_st_vector[1](0)), sin(_st_vector[1](0)), -cos(_st_vector[1](0)), 0, 0;
- jacobians[0] = getMeasurementSquareRootInformationUpper() * jacobians[0];
- jacobians[1]
- << -(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0))
- + (_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)), -(_st_vector[2](0)
- - _st_vector[0](0)) * cos(_st_vector[1](0)) - (_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0)), -1;
- jacobians[1] = getMeasurementSquareRootInformationUpper() * jacobians[1];
- jacobians[2] << cos(_st_vector[1](0)), sin(_st_vector[1](0)), -sin(_st_vector[1](0)), cos(_st_vector[1](0)), 0, 0;
- jacobians[2] = getMeasurementSquareRootInformationUpper() * jacobians[2];
- jacobians[3] << 0, 0, 1;
- jacobians[3] = getMeasurementSquareRootInformationUpper() * jacobians[3];
+ assert(jacobians.size() == 4);
+ assert(_st_vector.size() == 4);
+ assert(_compute_jacobian.size() == 4);
+
+ if (_compute_jacobian[0])
+ {
+ jacobians[0] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixXd(3,2) <<
+ -cos(_st_vector[1](0)), -sin(_st_vector[1](0)),
+ sin(_st_vector[1](0)), -cos(_st_vector[1](0)),
+ 0, 0).finished();
+ }
+ if (_compute_jacobian[1])
+ {
+ jacobians[1] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixXd(3,1) <<
+ -(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0)) + (_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)),
+ -(_st_vector[2](0) - _st_vector[0](0)) * cos(_st_vector[1](0)) - (_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0)),
+ -1).finished();
+ }
+ if (_compute_jacobian[2])
+ {
+ jacobians[2] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixXd(3,2) <<
+ cos(_st_vector[1](0)), sin(_st_vector[1](0)),
+ -sin(_st_vector[1](0)), cos(_st_vector[1](0)),
+ 0, 0).finished();
+ }
+ if (_compute_jacobian[3])
+ {
+ jacobians[3] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixXd(3,1) <<
+ 0, 0, 1).finished();
+ }
}
inline void FactorRelative2DAnalytic::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
{
- jacobians[0]
- << -cos(getStateBlockPtrVector()[1]->getState()(0)), -sin(getStateBlockPtrVector()[1]->getState()(0)), sin(
- getStateBlockPtrVector()[1]->getState()(0)), -cos(getStateBlockPtrVector()[1]->getState()(0)), 0, 0;
-
- jacobians[1]
- << -(getStateBlockPtrVector()[2]->getState()(0) - getStateBlockPtrVector()[0]->getState()(0))
- * sin(getStateBlockPtrVector()[1]->getState()(0))
- + (getStateBlockPtrVector()[2]->getState()(1) - getStateBlockPtrVector()[0]->getState()(1))
- * cos(getStateBlockPtrVector()[1]->getState()(0)), -(getStateBlockPtrVector()[2]->getState()(0)
- - getStateBlockPtrVector()[0]->getState()(0)) * cos(getStateBlockPtrVector()[1]->getState()(0))
- - (getStateBlockPtrVector()[2]->getState()(1) - getStateBlockPtrVector()[0]->getState()(1))
- * sin(getStateBlockPtrVector()[1]->getState()(0)), -1;
-
- jacobians[2]
- << cos(getStateBlockPtrVector()[1]->getState()(0)), sin(getStateBlockPtrVector()[1]->getState()(0)), -sin(
- getStateBlockPtrVector()[1]->getState()(0)), cos(getStateBlockPtrVector()[1]->getState()(0)), 0, 0;
-
- jacobians[3]
- << 0, 0, 1;
+ assert(jacobians.size() == 4);
+
+ jacobians[0] = (Eigen::MatrixXd(3,2) <<
+ -cos(getStateBlockPtrVector()[1]->getState()(0)), -sin(getStateBlockPtrVector()[1]->getState()(0)),
+ sin(getStateBlockPtrVector()[1]->getState()(0)), -cos(getStateBlockPtrVector()[1]->getState()(0)),
+ 0, 0).finished();
+
+ jacobians[1] = (Eigen::MatrixXd(3,1) <<
+ -(getStateBlockPtrVector()[2]->getState()(0)
+ - getStateBlockPtrVector()[0]->getState()(0)) * sin(getStateBlockPtrVector()[1]->getState()(0))
+ + (getStateBlockPtrVector()[2]->getState()(1)
+ - getStateBlockPtrVector()[0]->getState()(1)) * cos(getStateBlockPtrVector()[1]->getState()(0)),
+ -(getStateBlockPtrVector()[2]->getState()(0)
+ - getStateBlockPtrVector()[0]->getState()(0)) * cos(getStateBlockPtrVector()[1]->getState()(0))
+ - (getStateBlockPtrVector()[2]->getState()(1)
+ - getStateBlockPtrVector()[0]->getState()(1)) * sin(getStateBlockPtrVector()[1]->getState()(0)),
+ -1).finished();
+
+ jacobians[2] = (Eigen::MatrixXd(3,2) <<
+ cos(getStateBlockPtrVector()[1]->getState()(0)), sin(getStateBlockPtrVector()[1]->getState()(0)),
+ -sin(getStateBlockPtrVector()[1]->getState()(0)), cos(getStateBlockPtrVector()[1]->getState()(0)),
+ 0, 0).finished();
+
+ jacobians[3] = (Eigen::MatrixXd(3,1) << 0, 0, 1).finished();
}
} // namespace wolf
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 9cf04d22f8f8a94894bf05fb044ff14f816219bb..c4c325aac18b0435357c73af53e44f7a208186ae 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -212,6 +212,7 @@ struct ProcessorParamsBase : public ParamsBase
time_tolerance = _server.getParam<double>(prefix + _unique_name + "/time_tolerance");
voting_active = _server.getParam<bool>(prefix + _unique_name + "/keyframe_vote/voting_active");
voting_aux_active = _server.getParam<bool>(prefix + _unique_name + "/keyframe_vote/voting_aux_active");
+ apply_loss_function = _server.getParam<bool>(prefix + _unique_name + "/apply_loss_function");
}
virtual ~ProcessorParamsBase() = default;
@@ -220,17 +221,18 @@ struct ProcessorParamsBase : public ParamsBase
* a particular Capture of this processor to allow assigning
* this Capture to the Keyframe.
*/
- double time_tolerance = double(0);
- bool voting_active = false; ///< Whether this processor is allowed to vote for a Key Frame or not
- bool voting_aux_active = false; ///< Whether this processor is allowed to vote for an Auxiliary Frame or not
-
+ double time_tolerance;
+ bool voting_active; ///< Whether this processor is allowed to vote for a Key Frame or not
+ bool voting_aux_active; ///< Whether this processor is allowed to vote for an Auxiliary Frame or not
+ bool apply_loss_function; ///< Whether this processor emplaces factors with loss function or not
std::string print() const
{
return ParamsBase::print() + "\n"
- + "voting_active: " + std::to_string(voting_active) + "\n"
- + "voting_aux_active: " + std::to_string(voting_aux_active) + "\n"
- + "time_tolerance: " + std::to_string(time_tolerance) + "\n";
+ + "voting_active: " + std::to_string(voting_active) + "\n"
+ + "voting_aux_active: " + std::to_string(voting_aux_active) + "\n"
+ + "time_tolerance: " + std::to_string(time_tolerance) + "\n"
+ + "apply_loss_function: " + std::to_string(apply_loss_function) + "\n";
}
};
diff --git a/include/core/processor/processor_odom_2D.h b/include/core/processor/processor_odom_2D.h
index 6770f5e2464065169e86a9def4bf67b8a2af8636..491df7ada9f4b9c540740b4a9f06bb209e74f31c 100644
--- a/include/core/processor/processor_odom_2D.h
+++ b/include/core/processor/processor_odom_2D.h
@@ -83,7 +83,7 @@ class ProcessorOdom2D : public ProcessorMotion
const CaptureBasePtr& _capture_origin_ptr) override;
virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature,
- CaptureBasePtr _capture_origin) override;
+ CaptureBasePtr _capture_origin) override;
protected:
ProcessorParamsOdom2DPtr params_odom_2D_;
diff --git a/src/capture/capture_pose.cpp b/src/capture/capture_pose.cpp
index 494bbe789200c5efb97bed8a3c2c1c27bc4d027a..4f2a85c751a5a7945fcdcfe12aa84a6f37265e10 100644
--- a/src/capture/capture_pose.cpp
+++ b/src/capture/capture_pose.cpp
@@ -25,9 +25,9 @@ void CapturePose::emplaceFeatureAndFactor()
// std::cout << data_.size() << " ~~ " << data_covariance_.rows() << "x" << data_covariance_.cols() << std::endl;
// Emplace factor
if (data_.size() == 3 && data_covariance_.rows() == 3 && data_covariance_.cols() == 3 )
- FactorBase::emplace<FactorPose2D>(feature_pose, feature_pose);
+ FactorBase::emplace<FactorPose2D>(feature_pose, feature_pose, nullptr, false);
else if (data_.size() == 7 && data_covariance_.rows() == 6 && data_covariance_.cols() == 6 )
- FactorBase::emplace<FactorPose3D>(feature_pose, feature_pose);
+ FactorBase::emplace<FactorPose3D>(feature_pose, feature_pose, nullptr, false);
else
throw std::runtime_error("Wrong data size in CapturePose. Use 3 for 2D. Use 7 for 3D.");
}
diff --git a/src/factor/factor_analytic.cpp b/src/factor/factor_analytic.cpp
index 9c8ba1ab334629db2e3dd78652884d0e54df9860..73bc391c2c038ec27da7e8b2fc936e054795a415 100644
--- a/src/factor/factor_analytic.cpp
+++ b/src/factor/factor_analytic.cpp
@@ -4,35 +4,14 @@
namespace wolf {
FactorAnalytic::FactorAnalytic(const std::string& _tp,
- bool _apply_loss_function, FactorStatus _status,
- StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
- FactorBase(_tp, _apply_loss_function, _status),
- state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
- _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}),
- state_block_sizes_vector_({_state0Ptr ? (unsigned int) _state0Ptr->getSize() : 0,
- _state1Ptr ? (unsigned int) _state1Ptr->getSize() : 0,
- _state2Ptr ? (unsigned int) _state2Ptr->getSize() : 0,
- _state3Ptr ? (unsigned int) _state3Ptr->getSize() : 0,
- _state4Ptr ? (unsigned int) _state4Ptr->getSize() : 0,
- _state5Ptr ? (unsigned int) _state5Ptr->getSize() : 0,
- _state6Ptr ? (unsigned int) _state6Ptr->getSize() : 0,
- _state7Ptr ? (unsigned int) _state7Ptr->getSize() : 0,
- _state8Ptr ? (unsigned int) _state8Ptr->getSize() : 0,
- _state9Ptr ? (unsigned int) _state9Ptr->getSize() : 0})
-{
- resizeVectors();
-}
-
-FactorAnalytic::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, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
+ 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, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
_state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}),
@@ -49,18 +28,7 @@ FactorAnalytic::FactorAnalytic(const std::string& _tp,
{
resizeVectors();
}
-/*
-FactorAnalytic::FactorAnalytic(FactorType _tp, const LandmarkBasePtr& _landmark_ptr, const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, FactorStatus _status,
- StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
- FactorBase( _tp, nullptr, nullptr, _landmark_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
- _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr})
-{
- resizeVectors();
-}
-*/
+
std::vector<StateBlockPtr> FactorAnalytic::getStateBlockPtrVector() const
{
return state_ptr_vector_;
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index d501b32fba6773848d94a50a6ae5524ff2632661..46b244d9e83f9c01f4fca0bf84af35d81d36d53f 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -6,22 +6,6 @@ namespace wolf {
unsigned int FactorBase::factor_id_count_ = 0;
-FactorBase::FactorBase(const std::string& _tp,
- bool _apply_loss_function,
- FactorStatus _status) :
- NodeBase("FACTOR", _tp),
- feature_ptr_(), // nullptr
- factor_id_(++factor_id_count_),
- status_(_status),
- apply_loss_function_(_apply_loss_function),
- frame_other_ptr_(), // nullptr
- feature_other_ptr_(), // nullptr
- landmark_other_ptr_(), // nullptr
- processor_ptr_() // nullptr
-{
-// std::cout << "constructed +c" << id() << std::endl;
-}
-
FactorBase::FactorBase(const std::string& _tp,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index 269b57d454dcf7239786e25f5931f6c3a9b0ae84..e8318e8cdd00696ec87dcf9cf5451ca57b98e754 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -163,7 +163,8 @@ FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature,
auto fac_odom = FactorBase::emplace<FactorDiffDrive>(ftr_motion,
ftr_motion,
_capture_origin,
- shared_from_this());
+ shared_from_this(),
+ params_prc_diff_drv_selfcal_->apply_loss_function);
return fac_odom;
}
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index 7ba298ec6b8fd1220c81e12c184f8158b65a1fd9..e56f9cdd9654e160d5d53196a11e3f4445aa004f 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/processor_odom_2D.cpp
@@ -141,7 +141,8 @@ FactorBasePtr ProcessorOdom2D::emplaceFactor(FeatureBasePtr _feature, CaptureBas
auto fac_odom = FactorBase::emplace<FactorOdom2D>(_feature,
_feature,
_capture_origin->getFrame(),
- shared_from_this());
+ shared_from_this(),
+ params_->apply_loss_function);
return fac_odom;
}
diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp
index a7836911f571d0e5b3c47e21a7e64281b702da0f..f0241f7e1ce62ee203948fb6f37394b4bcd4e3ce 100644
--- a/src/processor/processor_odom_3D.cpp
+++ b/src/processor/processor_odom_3D.cpp
@@ -235,8 +235,11 @@ FeatureBasePtr ProcessorOdom3D::emplaceFeature(CaptureMotionPtr _capture_motion)
FactorBasePtr ProcessorOdom3D::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
{
- auto fac_odom = FactorBase::emplace<FactorOdom3D>(_feature_motion, _feature_motion, _capture_origin->getFrame(),
- shared_from_this());
+ auto fac_odom = FactorBase::emplace<FactorOdom3D>(_feature_motion,
+ _feature_motion,
+ _capture_origin->getFrame(),
+ shared_from_this(),
+ params_->apply_loss_function);
return fac_odom;
}
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index b65e3f6ad7544c4bcd00713d223f6915a79297e5..4ac66fb4b90181dde9a369c1be7f8b9e46e03c76 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -321,7 +321,7 @@ TEST(CeresManager, AddFactor)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f);
+ FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f,nullptr,false);
// update solver
ceres_manager_ptr->update();
@@ -343,7 +343,7 @@ TEST(CeresManager, DoubleAddFactor)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f);
+ FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f,nullptr,false);
// add factor again
P->notifyFactor(c,ADD);
@@ -368,7 +368,7 @@ TEST(CeresManager, RemoveFactor)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f);
+ FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f,nullptr,false);
// update solver
ceres_manager_ptr->update();
@@ -396,7 +396,7 @@ TEST(CeresManager, AddRemoveFactor)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f);
+ FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f,nullptr,false);
// remove factor
P->notifyFactor(c,REMOVE);
@@ -423,7 +423,7 @@ TEST(CeresManager, DoubleRemoveFactor)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f);
+ FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f,nullptr,false);
// update solver
ceres_manager_ptr->update();
@@ -550,8 +550,8 @@ TEST(CeresManager, FactorsRemoveLocalParam)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3d::Zero(), Matrix3d::Identity());
- FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO());
- FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO());
+ FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO(),nullptr,false);
+ FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO(),nullptr,false);
// update solver
ceres_manager_ptr->update();
@@ -592,8 +592,8 @@ TEST(CeresManager, FactorsUpdateLocalParam)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3d::Zero(), Matrix3d::Identity());
- FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO());
- FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO());
+ FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO(),nullptr,false);
+ FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO(),nullptr,false);
// update solver
ceres_manager_ptr->update();
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index 7febd06735de0ee2cd6488a27621aa93e7fb7dde..93bcb43444f2ad303b5e544cf482ecbad7195fad 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -111,7 +111,7 @@ TEST(Emplace, Factor)
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture());
- auto cnt = FactorBase::emplace<FactorOdom2D>(ftr, ftr, frm);
+ auto cnt = FactorBase::emplace<FactorOdom2D>(ftr, ftr, frm, nullptr, false);
ASSERT_NE(nullptr, ftr->getFactorList().front().get());
}
@@ -120,11 +120,9 @@ TEST(Emplace, EmplaceDerived)
ProblemPtr P = Problem::create("POV", 3);
auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
- // LandmarkBase::emplace<LandmarkBase>(MapBaseWPtr(P->getMap()),"Dummy", nullptr, nullptr);
auto sen = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Eigen::VectorXd(3), IntrinsicsOdom2D());
auto cov = Eigen::MatrixXd::Identity(2,2);
auto cpt = CaptureBase::emplace<CaptureOdom2D>(frm, TimeStamp(0), sen, Eigen::VectorXd(2), cov, nullptr);
- // auto cpt2 = std::static_pointer_cast<CaptureOdom2D>(cpt);
auto m = Eigen::Matrix<double,9,6>();
for(int i = 0; i < 9; i++)
for(int j = 0; j < 6; j++)
@@ -147,9 +145,9 @@ TEST(Emplace, ReturnDerived)
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
auto cov = Eigen::MatrixXd::Identity(2,2);
auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXd(2), cov);
- auto cnt = FactorBase::emplace<FactorOdom2D>(ftr, ftr, frm);
+ auto cnt = FactorBase::emplace<FactorOdom2D>(ftr, ftr, frm, nullptr, false);
- FactorOdom2DPtr fac = FactorBase::emplace<FactorOdom2D>(ftr, ftr, frm);
+ FactorOdom2DPtr fac = FactorBase::emplace<FactorOdom2D>(ftr, ftr, frm, nullptr, false);
}
int main(int argc, char **argv)
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index bec954adbd00091690b6d8b12a2b9b462d2afb69..f9c9212921ae9b780fc17c463beaef43967a0e0a 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -35,7 +35,7 @@ TEST(CaptureAutodiff, ConstructorOdom2d)
auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, Eigen::Vector3d::Zero(), Eigen::Matrix3d::Identity());
// FACTOR
- auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr);
+ auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
ASSERT_TRUE(factor_ptr->getFeature());
ASSERT_TRUE(factor_ptr->getFeature()->getCapture());
@@ -69,7 +69,7 @@ TEST(CaptureAutodiff, ResidualOdom2d)
auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3d::Identity());
// FACTOR
- auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr);
+ auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
// EVALUATE
@@ -112,7 +112,7 @@ TEST(CaptureAutodiff, JacobianOdom2d)
auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3d::Identity());
// FACTOR
- auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr);
+ auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
// COMPUTE JACOBIANS
@@ -127,7 +127,7 @@ TEST(CaptureAutodiff, JacobianOdom2d)
Eigen::VectorXd residuals(factor_ptr->getSize());
factor_ptr->evaluate(states_ptr, residuals, Jauto);
- std::cout << Jauto.size() << std::endl;
+ ASSERT_EQ(Jauto.size(),4);
// ANALYTIC JACOBIANS
Eigen::MatrixXd J0(3,2);
@@ -190,8 +190,8 @@ TEST(CaptureAutodiff, AutodiffVsAnalytic)
auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3d::Identity());
// FACTOR
- auto fac_autodiff_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr);
- auto fac_analytic_ptr = FactorBase::emplace<FactorOdom2DAnalytic>(feature_ptr, feature_ptr, fr1_ptr);
+ auto fac_autodiff_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
+ auto fac_analytic_ptr = FactorBase::emplace<FactorOdom2DAnalytic>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
// COMPUTE JACOBIANS
@@ -208,12 +208,12 @@ TEST(CaptureAutodiff, AutodiffVsAnalytic)
fac_autodiff_ptr->evaluate(states_ptr, residuals, Jautodiff);
std::cout << "autodiff evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl;
t = clock();
- //TODO FactorAnalytic::evaluate
-// fac_analytic_ptr->evaluate(states_ptr, residuals, Janalytic);
-// std::cout << "analytic evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl;
-//
-// for (auto i = 0; i < Jautodiff.size(); i++)
-// ASSERT_MATRIX_APPROX(Jautodiff[i], Janalytic[i], wolf::Constants::EPS);
+ fac_analytic_ptr->evaluate(states_ptr, residuals, Janalytic);
+ std::cout << "analytic evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl;
+
+ ASSERT_EQ(Janalytic.size(), Jautodiff.size());
+ for (auto i = 0; i < Jautodiff.size(); i++)
+ ASSERT_MATRIX_APPROX(Jautodiff[i], Janalytic[i], wolf::Constants::EPS);
}
int main(int argc, char **argv)
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index be9a44b10c6087a9d9056061b8843cc14baadb39..e92ae4b558a34f1019c493d2a5c2bba6685f4d14 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -233,13 +233,14 @@ class FactorDiffDriveTest : public testing::Test
TEST_F(FactorDiffDriveTest, constructor)
{
// plain constructor
- auto c1_obj = FactorDiffDrive(f1, C0, processor);
+ auto c1_obj = FactorDiffDrive(f1, C0, processor, false);
ASSERT_EQ(c1_obj.getType(), "FactorDiffDrive");
// std: make_shared
c1 = std::make_shared<FactorDiffDrive>(f1,
C0,
- processor);
+ processor,
+ false);
ASSERT_NE(c1, nullptr);
ASSERT_EQ(c1->getType(), "FactorDiffDrive");
@@ -248,7 +249,8 @@ TEST_F(FactorDiffDriveTest, constructor)
c1 = FactorBase::emplace<FactorDiffDrive>(f1,
f1,
C0,
- processor);
+ processor,
+ false);
ASSERT_NE(c1, nullptr);
ASSERT_EQ(c1->getType(), "FactorDiffDrive");
@@ -262,7 +264,8 @@ TEST_F(FactorDiffDriveTest, residual_zeros)
c1 = FactorBase::emplace<FactorDiffDrive>(f1,
f1,
C0,
- processor);
+ processor,
+ false);
residual = c1->residual();
@@ -291,7 +294,7 @@ TEST_F(FactorDiffDriveTest, residual_right_turn_90_deg)
F1->setState(Vector3d(1.5, -1.5, -M_PI_2));
// wolf: emplace
- c1 = FactorBase::emplace<FactorDiffDrive>(f1, f1, C0, processor);
+ c1 = FactorBase::emplace<FactorDiffDrive>(f1, f1, C0, processor, false);
residual = c1->residual();
@@ -318,7 +321,7 @@ TEST_F(FactorDiffDriveTest, solve_F1)
F1->setState(Vector3d(1.5, -1.5, -M_PI_2));
// wolf: emplace
- c1 = FactorBase::emplace<FactorDiffDrive>(f1, f1, C0, processor);
+ c1 = FactorBase::emplace<FactorDiffDrive>(f1, f1, C0, processor, false);
residual = c1->residual();
@@ -356,7 +359,7 @@ TEST_F(FactorDiffDriveTest, solve_sensor_intrinsics)
F1->setState(Vector3d(1.5, -1.5, -M_PI_2));
// wolf: emplace
- c1 = FactorBase::emplace<FactorDiffDrive>(f1, f1, C0, processor);
+ c1 = FactorBase::emplace<FactorDiffDrive>(f1, f1, C0, processor, false);
residual = c1->residual();
diff --git a/test/gtest_factor_odom_3D.cpp b/test/gtest_factor_odom_3D.cpp
index e5404bdc370339d19e9ac2e5b9cd421fd5df7c3d..d1b88599bfee17681e8e8187f6ed4eac982b40e1 100644
--- a/test/gtest_factor_odom_3D.cpp
+++ b/test/gtest_factor_odom_3D.cpp
@@ -43,7 +43,7 @@ FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, delta, TimeStamp(1));
// Capture, feature and factor from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "CaptureOdom3D", 1, nullptr, delta, 7, 6, nullptr);
auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom3D", delta, data_cov);
-FactorOdom3DPtr ctr1 = FactorBase::emplace<FactorOdom3D>(fea1, fea1, frm0, nullptr); // create and add
+FactorOdom3DPtr ctr1 = FactorBase::emplace<FactorOdom3D>(fea1, fea1, frm0, nullptr, false); // create and add
////////////////////////////////////////////////////////
diff --git a/test/gtest_factor_pose_2D.cpp b/test/gtest_factor_pose_2D.cpp
index a8b0a2e928e745506ac74c478262d0b989198329..71c7b7d5ba9169be5c433f714be1d7c97f44bcb8 100644
--- a/test/gtest_factor_pose_2D.cpp
+++ b/test/gtest_factor_pose_2D.cpp
@@ -33,12 +33,9 @@ CeresManager ceres_mgr(problem);
FrameBasePtr frm0 = problem->emplaceFrame(KEY, problem->zeroState(), TimeStamp(0));
// Capture, feature and factor from frm1 to frm0
-// CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("CaptureOdom2D", 0, nullptr, pose, 3, 3, nullptr));
auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom2D", 0, nullptr, pose, 3, 3, nullptr);
-// FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("FeatureOdom2D", pose, data_cov));
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureOdom2D", pose, data_cov);
-// FactorPose2DPtr ctr0 = std::static_pointer_cast<FactorPose2D>(fea0->addFactor(std::make_shared<FactorPose2D>(fea0)));
-FactorPose2DPtr ctr0 = FactorBase::emplace<FactorPose2D>(fea0, fea0);
+auto ctr0 = FactorBase::emplace<FactorPose2D>(fea0, fea0, nullptr, false);
////////////////////////////////////////////////////////
diff --git a/test/gtest_factor_pose_3D.cpp b/test/gtest_factor_pose_3D.cpp
index 10f7567094c9d281a2a70010361566c3df489353..d0a6258a01f834b05c91771986740baa2a65b743 100644
--- a/test/gtest_factor_pose_3D.cpp
+++ b/test/gtest_factor_pose_3D.cpp
@@ -41,7 +41,7 @@ FrameBasePtr frm0 = problem->emplaceFrame(KEY, problem->zeroState(), TimeStamp(0
// Capture, feature and factor
auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom3D", 0, nullptr, pose7, 7, 6, nullptr);
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureOdom3D", pose7, data_cov);
-FactorPose3DPtr ctr0 = FactorBase::emplace<FactorPose3D>(fea0, fea0);
+FactorPose3DPtr ctr0 = FactorBase::emplace<FactorPose3D>(fea0, fea0, nullptr, false);
////////////////////////////////////////////////////////
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index e4913f36bc25c90769ee26d10daaad0b9f47485c..10dcda6825250e8f949a7b43391ce2d3676f6f82 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -72,14 +72,11 @@ TEST(FrameBase, LinksToTree)
auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto F2 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), 3, 3, nullptr);
- /// @todo link sensor & proccessor
- ProcessorBasePtr p = std::make_shared<ProcessorOdom2D>(make_shared<ProcessorParamsOdom2D>());
+ auto p = ProcessorBase::emplace<ProcessorOdom2D>(S, std::make_shared<ProcessorParamsOdom2D>());
auto f = FeatureBase::emplace<FeatureBase>(C, "f", Vector1d(1), Matrix<double,1,1>::Identity()*.01);
- auto c = FactorBase::emplace<FactorOdom2D>(f, f, F2, p);
+ auto c = FactorBase::emplace<FactorOdom2D>(f, f, F2, p, false);
//TODO: WARNING! I dropped this comprovations since the emplacing operation is now atomic.
- // c-by link F2 -> c not yet established
- // ASSERT_TRUE(F2->getConstrainedByList().empty());
ASSERT_FALSE(F2->getConstrainedByList().empty());
// tree is inconsistent since we are missing the constrained_by link
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
index ab883fe821ca59a0f868c8c01ac9532570884af5..8679baccf74f02221917063f91cb685af48f3566 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2D.cpp
@@ -135,14 +135,14 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D)
FrameBasePtr F1 = Pr->emplaceFrame(KEY, Vector3d::Zero(), t);
auto C1 = CaptureBase::emplace<CaptureBase>(F1, "CaptureOdom2D", t);
auto f1 = FeatureBase::emplace<FeatureBase>(C1, "FeatureOdom2D", delta, delta_cov);
- auto c1 = FactorBase::emplace<FactorOdom2D>(f1, f1, F0, nullptr);
+ auto c1 = FactorBase::emplace<FactorOdom2D>(f1, f1, F0, nullptr, false);
// KF2 and motion from KF1
t += dt;
FrameBasePtr F2 = Pr->emplaceFrame(KEY, Vector3d::Zero(), t);
auto C2 = CaptureBase::emplace<CaptureBase>(F2, "CaptureOdom2D", t);
auto f2 = FeatureBase::emplace<FeatureBase>(C2, "FeatureOdom2D", delta, delta_cov);
- auto c2 = FactorBase::emplace<FactorOdom2D>(f2, f2, F1, nullptr);
+ auto c2 = FactorBase::emplace<FactorOdom2D>(f2, f2, F1, nullptr, false);
ASSERT_TRUE(Pr->check(0));
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 261158505abc5edeec6eaef8e5e2ec97d52c9eab..b9472ba1995d7e60444048942c48f5085fcf253e 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -518,7 +518,7 @@ TEST(SolverManager, AddFactor)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f);
+ FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f, nullptr, false);
// notification
Notification notif;
@@ -545,7 +545,7 @@ TEST(SolverManager, RemoveFactor)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f);
+ FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f, nullptr, false);
// update solver
solver_manager_ptr->update();
@@ -579,7 +579,7 @@ TEST(SolverManager, AddRemoveFactor)
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f);
+ FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f, nullptr, false);
// notification
Notification notif;
@@ -614,7 +614,7 @@ TEST(SolverManager, DoubleRemoveFactor)
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f);
+ FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f, nullptr, false);
// update solver
solver_manager_ptr->update();