From e62b25f26ebd68f4550c2f1f9c6411040fe3443d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Fri, 1 Feb 2019 12:46:51 +0100
Subject: [PATCH] Add interpolate() method for 3D tools
---
src/three_D_tools.h | 17 +++++++++++++++++
1 file changed, 17 insertions(+)
diff --git a/src/three_D_tools.h b/src/three_D_tools.h
index 44512070f..8ed818695 100644
--- a/src/three_D_tools.h
+++ b/src/three_D_tools.h
@@ -31,6 +31,7 @@
* - exp_SE3: go from tangent space to delta manifold (equivalent to exp() in rotations)
* - plus: D2 = D1 * exp_SE3(d)
* - minus: d = log_SE3( D1.inv() * D2 )
+ * - interpolate: dd = D1 * exp ( log( D1.inv() * D2 ) * t ) = D1 (+) ( (D2 (-) D1) * t)
*/
@@ -375,6 +376,22 @@ inline Matrix<typename D1::Scalar, 6, 1> minus(const MatrixBase<D1>& d1,
return ret;
}
+template<typename D1, typename D2, typename D3>
+inline void interpolate(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ const typename D1::Scalar t,
+ MatrixBase<D3>& sum)
+{
+ Matrix<typename D1::Scalar, 6, 1> dd;
+ Matrix<typename D1::Scalar, 7, 1> ret;
+
+ minus(d1, d2, dd);
+ plus(d1, dd * t, ret);
+
+ return ret;
+}
+
+
} // namespace three_d
} // namespace wolf
--
GitLab