Skip to content
Snippets Groups Projects
Commit 7d8ac7dd authored by Pep Martí Saumell's avatar Pep Martí Saumell
Browse files

FactorOdom2D including extrinsics

parent 543f1146
No related branches found
No related tags found
1 merge request!298WIP: Resolve "New factorRelativePose2DWithExtrinsics"
Pipeline #3848 failed
......@@ -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
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment