Skip to content
Snippets Groups Projects

WIP: Resolve "New factorRelativePose2DWithExtrinsics"

Closed WIP: Resolve "New factorRelativePose2DWithExtrinsics"
237-factorodom2d-extrinsics-are-not-considered-when-computing-the-factor into devel
Closed Pep Martí Saumell requested to merge 237-factorodom2d-extrinsics-are-not-considered-when-computing-the-factor into devel
Viewing commit 7d8ac7dd
Prev Next
Show latest version
1 file
+ 4
− 1
Compare changes
  • Side-by-side
  • Inline
include/core/factor/factor_odom_2D.h
+ 4
− 1
@@ -56,13 +56,16 @@ inline bool FactorOdom2D::operator ()(const T* const _p1, const T* const _o1, co
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> > sp(_sp);
T o1 = _o1[0];
T o2 = _o2[0];
T so = _so[0];
Eigen::Matrix<T, 3, 1> expected_measurement;
Eigen::Matrix<T, 3, 1> er; // error
// Expected measurement
expected_measurement.head(2) = Eigen::Rotation2D<T>(-o1) * (p2 - p1);
expected_measurement.head(2) = Eigen::Rotation2D<T>(-so)*(Eigen::Rotation2D<T>(-o1)*(p2 + Eigen::Rotation2D<T>(o2)*sp - p1) - sp);
expected_measurement(2) = o2 - o1;
// Error