Skip to content
Snippets Groups Projects

Resolve "New factor FactorKfLmkPose3dWithExtrinsicsPtr"

Merged Resolve "New factor FactorKfLmkPose3dWithExtrinsicsPtr"
402-new-factor-factorkflmkpose3dwithextrinsicsptr into devel
Merged Mederic Fourmy requested to merge 402-new-factor-factorkflmkpose3dwithextrinsicsptr into devel
Compare
and
5 files
+ 585
− 39
Compare changes
  • Side-by-side
  • Inline
Files
5
include/core/factor/factor_relative_pose_2d_with_extrinsics.h
+ 82
− 39
@@ -16,61 +16,104 @@ WOLF_PTR_TYPEDEFS(FactorRelativePose2dWithExtrinsics);
class FactorRelativePose2dWithExtrinsics : public FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>
{
public:
/** \brief Class constructor Frame-Frame
*/
FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>("FactorRelativePose2dWithExtrinsics",
_top,
_ftr_ptr,
_frame_other_ptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function,
_status,
_frame_other_ptr->getP(),
_frame_other_ptr->getO(),
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_ftr_ptr->getCapture()->getSensorP(),
_ftr_ptr->getCapture()->getSensorO())
{
assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
//
}
FactorStatus _status = FAC_ACTIVE);
/** \brief Class constructor Frame-Landmark
*/
FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
const LandmarkBasePtr& _lmk_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top = TOP_LMK,
FactorStatus _status = FAC_ACTIVE);
~FactorRelativePose2dWithExtrinsics() override = default;
template<typename T>
bool operator ()(const T* const _p1,
const T* const _o1,
const T* const _p2,
const T* const _o2,
const T* const _sp,
const T* const _so,
bool operator ()(const T* const _p_ref,
const T* const _o_ref,
const T* const _p_target,
const T* const _o_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const;
};
FactorRelativePose2dWithExtrinsics::FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status) :
FactorAutodiff("FactorRelativePose2dWithExtrinsics",
_top,
_ftr_ptr,
_frame_other_ptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function,
_status,
_frame_other_ptr->getP(),
_frame_other_ptr->getO(),
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_ftr_ptr->getCapture()->getSensorP(),
_ftr_ptr->getCapture()->getSensorO())
{
assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
//
}
FactorRelativePose2dWithExtrinsics::FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
const LandmarkBasePtr& _lmk_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
const FactorTopology& _top,
FactorStatus _status) :
FactorAutodiff("FactorRelativePose2dWithExtrinsics",
_top,
_ftr_ptr,
nullptr, nullptr, nullptr, _lmk_other_ptr,
_processor_ptr,
_apply_loss_function,
_status,
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_lmk_other_ptr->getP(),
_lmk_other_ptr->getO(),
_ftr_ptr->getCapture()->getSensorP(),
_ftr_ptr->getCapture()->getSensorO())
{
assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
//
}
template<typename T>
inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p1,
const T* const _o1,
const T* const _p2,
const T* const _o2,
const T* const _ps,
const T* const _os,
inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p_ref,
const T* const _o_ref,
const T* const _p_target,
const T* const _o_target,
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const
{
// MAPS
Eigen::Map<Eigen::Matrix<T,3,1> > res(_residuals);
Eigen::Map<const Eigen::Matrix<T,2,1> > p1(_p1);
Eigen::Map<const Eigen::Matrix<T,2,1> > p2(_p2);
Eigen::Map<const Eigen::Matrix<T,2,1> > ps(_ps);
T o1 = _o1[0];
T o2 = _o2[0];
T os = _os[0];
Eigen::Map<const Eigen::Matrix<T,2,1> > p_ref(_p_ref);
Eigen::Map<const Eigen::Matrix<T,2,1> > p_target(_p_target);
Eigen::Map<const Eigen::Matrix<T,2,1> > p_sensor(_p_sensor);
T o_ref = _o_ref[0];
T o_target = _o_target[0];
T o_sensor = _o_sensor[0];
Eigen::Matrix<T, 3, 1> expected_measurement;
Eigen::Matrix<T, 3, 1> er; // error
@@ -91,8 +134,8 @@ inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p1,
// s1_p_s1_s2 = s1_R_r1*(r1_p_s1_r1 + r1_R_w*(w_p_r1_w + w_p_w_r2 + w_R_r2*r2_p_r2_s2))
expected_measurement.head(2) = Eigen::Rotation2D<T>(-os)*(-ps + Eigen::Rotation2D<T>(-o1)*(-p1 + p2 + Eigen::Rotation2D<T>(o2)*ps));
expected_measurement(2) = o2 - o1;
expected_measurement.head(2) = Eigen::Rotation2D<T>(-o_sensor)*(-p_sensor + Eigen::Rotation2D<T>(-o_ref)*(-p_ref + p_target + Eigen::Rotation2D<T>(o_target)*p_sensor));
expected_measurement(2) = o_target - o_ref;
// Error
er = expected_measurement - getMeasurement().cast<T>();