From fa14732398e14503f9eb49d06e16707f15e49a41 Mon Sep 17 00:00:00 2001
From: PepMS <jmarti@iri.upc.edu>
Date: Wed, 3 Jul 2019 12:36:46 +0200
Subject: [PATCH] Added comments to FactorOdom2D
---
include/core/factor/factor_odom_2D.h | 23 +++++++++++++++++++----
1 file changed, 19 insertions(+), 4 deletions(-)
diff --git a/include/core/factor/factor_odom_2D.h b/include/core/factor/factor_odom_2D.h
index 0589fbcd4..f0eba5ca3 100644
--- a/include/core/factor/factor_odom_2D.h
+++ b/include/core/factor/factor_odom_2D.h
@@ -49,23 +49,38 @@ class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1, 2, 1>
template<typename T>
inline bool FactorOdom2D::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, T* _residuals) const
+ const T* const _o2, const T* const _ps, const T* const _os, 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> > sp(_sp);
+ Eigen::Map<const Eigen::Matrix<T,2,1> > ps(_ps);
T o1 = _o1[0];
T o2 = _o2[0];
- T so = _so[0];
+ T os = _os[0];
Eigen::Matrix<T, 3, 1> expected_measurement;
Eigen::Matrix<T, 3, 1> er; // error
// Expected measurement
- expected_measurement.head(2) = Eigen::Rotation2D<T>(-so)*(Eigen::Rotation2D<T>(-o1)*(p2 + Eigen::Rotation2D<T>(o2)*sp - p1) - sp);
+ // r1_p_r1_s1 = ps
+ // r2_p_r2_s2 = ps
+ // r1_R_s1 = R(os)
+ // r2_R_s2 = R(os)
+ // w_R_r1 = R(o1)
+ // w_R_r2 = R(o2)
+ // ----------------------------------------
+
+ // s1_p_s1_s2 = s1_R_r1*r1_p_s1_r1 +
+ // s1_R_r1*r1_R_w*w_p_r1_w +
+ // s1_R_r1*r1_R_w*w_p_w_r2 +
+ // s1_R_r1*r1_R_w*w_R_r2*r2_p_r2_s2
+
+ // 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;
// Error
--
GitLab