From 7d8ac7dd4441ef95adb9dea6321e31f70d214d97 Mon Sep 17 00:00:00 2001
From: PepMS <jmarti@iri.upc.edu>
Date: Tue, 2 Jul 2019 14:45:12 +0200
Subject: [PATCH] FactorOdom2D including extrinsics
---
include/core/factor/factor_odom_2D.h | 5 ++++-
1 file changed, 4 insertions(+), 1 deletion(-)
diff --git a/include/core/factor/factor_odom_2D.h b/include/core/factor/factor_odom_2D.h
index ed3d0da69..0589fbcd4 100644
--- a/include/core/factor/factor_odom_2D.h
+++ b/include/core/factor/factor_odom_2D.h
@@ -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
--
GitLab