diff --git a/src/three_D_tools.h b/src/SE3.h
similarity index 84%
rename from src/three_D_tools.h
rename to src/SE3.h
index 8ed81869500d4fbcab5c0091e78f972eb1e2a85b..f6eb550dd676e1506952a0c93ae4e4b637516a2f 100644
--- a/src/three_D_tools.h
+++ b/src/SE3.h
@@ -1,12 +1,12 @@
/*
- * three_D_tools.h
+ * SE3.h
*
* Created on: Mar 15, 2018
* Author: jsola
*/
-#ifndef THREE_D_TOOLS_H_
-#define THREE_D_TOOLS_H_
+#ifndef SE3_H_
+#define SE3_H_
#include "wolf.h"
@@ -232,8 +232,11 @@ Matrix<typename Derived::Scalar, 6, 1> log_SE3(const MatrixBase<Derived>& delta_
Map<Matrix<typename Derived::Scalar, 3, 1> > dp_ret ( & ret(0) );
Map<Matrix<typename Derived::Scalar, 3, 1> > do_ret ( & ret(3) );
- dp_ret = dp_in;
- do_ret = log_q(dq_in);
+ Matrix<typename Derived::Scalar, 3, 3> V_inv;
+
+ do_ret = log_q(dq_in);
+ V_inv = jac_SO3_left_inv(do_ret);
+ dp_ret = V_inv * dp_in;
return ret;
}
@@ -245,12 +248,16 @@ Matrix<typename Derived::Scalar, 7, 1> exp_SE3(const MatrixBase<Derived>& d_in)
Matrix<typename Derived::Scalar, 7, 1> ret;
+ Matrix<typename Derived::Scalar, 3, 3> V;
+
+ V = jac_SO3_left(d_in.template tail<3>());
+
Map<const Matrix<typename Derived::Scalar, 3, 1> > dp_in ( & d_in(0) );
Map<const Matrix<typename Derived::Scalar, 3, 1> > do_in ( & d_in(3) );
Map<Matrix<typename Derived::Scalar, 3, 1> > dp ( & ret(0) );
Map<Quaternion<typename Derived::Scalar> > dq ( & ret(3) );
- dp = dp_in;
+ dp = V * dp_in;
dq = exp_q(do_in);
return ret;
@@ -379,22 +386,88 @@ inline Matrix<typename D1::Scalar, 6, 1> minus(const MatrixBase<D1>& d1,
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)
+ const typename D1::Scalar& t,
+ MatrixBase<D3>& ret)
{
- Matrix<typename D1::Scalar, 6, 1> dd;
- Matrix<typename D1::Scalar, 7, 1> ret;
+ Matrix<typename D1::Scalar, 7, 1> dd = between(d1, d2);
- minus(d1, d2, dd);
- plus(d1, dd * t, ret);
+ Matrix<typename D1::Scalar, 6, 1> tau = t * log_SE3(dd);
- return ret;
+ ret = compose(d1, exp_SE3(tau));
+}
+
+template<typename D1, typename D2>
+inline void toSE3(const MatrixBase<D1>& pose,
+ MatrixBase<D2>& SE3)
+{
+ MatrixSizeCheck<4,4>::check(SE3);
+
+ typedef typename D1::Scalar T;
+
+ SE3.template block<3,1>(0,3) = pose.template head<3>();
+ SE3.template block<3,3>(0,0) = q2R(pose.template tail<4>());
+ SE3.template block<1,3>(3,0).setZero();
+ SE3(3,3) = (T)1.0;
}
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 6, 6> Q_helper(const MatrixBase<D1>& v, const MatrixBase<D1>& w)
+{
+ typedef typename D1::Scalar T;
+
+ Matrix<T, 3, 3> vx = skew(v);
+ Matrix<T, 3, 3> wx = skew(w);
+ Matrix<T, 3, 3> wxvx = wx*vx;
+ Matrix<T, 3, 3> vxwx = wxvx.transpose();
+ Matrix<T, 3, 3> wxwx = wx*wx;
+ Matrix<T, 3, 3> wxvxwx = wx*vx*wx;
+
+ T th = wx.norm();
+ T th_2 = th*th;
+ T th_3 = th_2*th;
+ T th_4 = th_2*th_2;
+ T th_5 = th_3*th_2;
+ T sin_th = sin(th);
+ T cos_th = cos(th);
+ T A = (th-sin_th)/th_3;
+ T B = (T(1.0) - th_2/T(2.0) - cos_th)/th_4;
+ T C = (th - sin_th - th_3/T(6.0))/th_5;
+
+ Matrix<T, 3, 3> Q
+ = T(0.5)*vx
+ + A * (wxvx + vxwx + wxvxwx)
+ - B * (wxwx*vx + vxwx*wx - T(3.0)*wxvxwx)
+ - (B - T(3.0)*C) * wxvx*wxwx;
+ // - T(0.5) * (B - T(3.0)*C) * (wxvx*wxwx + wxwx*vxwx);
+
+ return Q;
+}
+template<typename D1>
+inline Matrix<typename D1::Scalar, 6, 6> jac_SE3_left(const MatrixBase<D1>& tangent)
+{
+ typedef typename D1::Scalar T;
+ Map<Matrix<T, 3, 1>> v(tangent.data() + 0); // linear
+ Map<Matrix<T, 3, 1>> w(tangent.data() + 3); // angular
+
+ Matrix<T, 3, 3> Jl(jac_SO3_left(w));
+ Matrix<T, 3, 3> Q = Q_helper(v,w);
+
+ Matrix<T, 6, 6> Jl_SE3;
+ Jl_SE3.topleft(3,3) = Jl;
+ Jl_SE3.bottomright(3,3) = Jl;
+ Jl_SE3.topright(3,3) = Q;
+ Jl_SE3.bottomleft(3,3) .setZero();
+}
+
+template<typename D1>
+inline Matrix<typename D1::Scalar, 6, 6> jac_SE3_right(const MatrixBase<D1>& tangent)
+{
+ return jac_SE3_left(-tangent);
+}
} // namespace three_d
} // namespace wolf
-#endif /* THREE_D_TOOLS_H_ */
+#endif /* SE3_H_ */
diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
index bac392115b91c812e8b079493ebe85b95ae3a8d2..cd99f0476dfe9eacbf0b7b63566f9306def8e554 100644
--- a/src/test/CMakeLists.txt
+++ b/src/test/CMakeLists.txt
@@ -94,7 +94,9 @@ target_link_libraries(gtest_processor_motion ${PROJECT_NAME})
# Rotation test
wolf_add_gtest(gtest_rotation gtest_rotation.cpp)
-#target_link_libraries(gtest_rotation ${PROJECT_NAME})
+
+# SE3 test
+wolf_add_gtest(gtest_SE3 gtest_SE3.cpp)
# SensorBase test
wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)
diff --git a/src/test/gtest_SE3.cpp b/src/test/gtest_SE3.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b734385b4a997e78d8b0de73573bbbe331009715
--- /dev/null
+++ b/src/test/gtest_SE3.cpp
@@ -0,0 +1,236 @@
+/**
+ * \file gtest_SE3.cpp
+ *
+ * Created on: Feb 2, 2019
+ * \author: jsola
+ */
+
+
+#include "../SE3.h"
+#include "utils_gtest.h"
+
+
+
+using namespace Eigen;
+using namespace wolf;
+using namespace three_D;
+
+TEST(SE3, exp_0)
+{
+ Vector6s tau = Vector6s::Zero();
+ Vector7s pose; pose << 0,0,0, 0,0,0,1;
+
+ ASSERT_MATRIX_APPROX(pose, exp_SE3(tau), 1e-8);
+}
+
+TEST(SE3, log_I)
+{
+ Vector7s pose; pose << 0,0,0, 0,0,0,1;
+ Vector6s tau = Vector6s::Zero();
+
+ ASSERT_MATRIX_APPROX(tau, log_SE3(pose), 1e-8);
+}
+
+TEST(SE3, exp_log)
+{
+ Vector6s tau = Vector6s::Random() / 5.0;
+ Vector7s pose = exp_SE3(tau);
+
+ ASSERT_MATRIX_APPROX(tau, log_SE3(exp_SE3(tau)), 1e-8);
+ ASSERT_MATRIX_APPROX(pose, exp_SE3(log_SE3(pose)), 1e-8);
+}
+
+TEST(SE3, exp_of_multiple)
+{
+ Vector6s tau, tau2, tau3;
+ Vector7s pose, pose2, pose3;
+ tau = Vector6s::Random() / 5;
+ pose << exp_SE3(tau);
+ tau2 = 2*tau;
+ tau3 = 3*tau;
+ pose2 = compose(pose, pose);
+ pose3 = compose(pose2, pose);
+
+ // 1
+ ASSERT_MATRIX_APPROX(exp_SE3(tau), pose, 1e-8);
+
+ // 2
+ ASSERT_MATRIX_APPROX(exp_SE3(tau2), pose2, 1e-8);
+
+ // 3
+ ASSERT_MATRIX_APPROX(exp_SE3(tau3), pose3, 1e-8);
+}
+
+TEST(SE3, log_of_power)
+{
+ Vector6s tau, tau2, tau3;
+ Vector7s pose, pose2, pose3;
+ tau = Vector6s::Random() / 5;
+ pose << exp_SE3(tau);
+ tau2 = 2*tau;
+ tau3 = 3*tau;
+ pose2 = compose(pose, pose);
+ pose3 = compose(pose2, pose);
+
+ // 1
+ ASSERT_MATRIX_APPROX(tau, log_SE3(pose), 1e-8);
+
+ // 2
+ ASSERT_MATRIX_APPROX(tau2, log_SE3(pose2), 1e-8);
+
+ // 3
+ ASSERT_MATRIX_APPROX(tau3, log_SE3(pose3), 1e-8);
+}
+
+TEST(SE3, interpolate_I_xyz)
+{
+ Vector7s p1, p2, p_pre;
+
+ p1 << 0,0,0, 0,0,0,1;
+ p2 << 1,2,3, 0,0,0,1;
+ Scalar t = 0.2;
+
+ interpolate(p1, p2, t, p_pre);
+
+ Vector7s p_gt; p_gt << 0.2,0.4,0.6, 0,0,0,1;
+
+ ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, interpolate_xyz_xyz)
+{
+ Vector7s p1, p2, p_pre;
+
+ p1 << 1,2,3, 0,0,0,1;
+ p2 << 2,4,6, 0,0,0,1;
+ Scalar t = 0.2;
+
+ interpolate(p1, p2, t, p_pre);
+
+ Vector7s p_gt; p_gt << 1.2,2.4,3.6, 0,0,0,1;
+
+ ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, interpolate_I_qx)
+{
+ Vector7s p1, p2, p_pre;
+
+ p1 << 0,0,0, 0,0,0,1;
+ p2 << 0,0,0, 1,0,0,0;
+ Scalar t = 0.5;
+
+ interpolate(p1, p2, t, p_pre);
+
+ Vector7s p_gt; p_gt << 0,0,0, sqrt(2)/2,0,0,sqrt(2)/2;
+
+ ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, interpolate_I_qy)
+{
+ Vector7s p1, p2, p_pre;
+
+ p1 << 0,0,0, 0,0,0,1;
+ p2 << 0,0,0, 0,1,0,0;
+ Scalar t = 0.5;
+
+ interpolate(p1, p2, t, p_pre);
+
+ Vector7s p_gt; p_gt << 0,0,0, 0,sqrt(2)/2,0,sqrt(2)/2;
+
+ ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, interpolate_I_qz)
+{
+ Vector7s p1, p2, p_pre;
+
+ p1 << 0,0,0, 0,0,0,1;
+ p2 << 0,0,0, 0,0,1,0;
+ Scalar t = 0.5;
+
+ interpolate(p1, p2, t, p_pre);
+
+ Vector7s p_gt; p_gt << 0,0,0, 0,0,sqrt(2)/2,sqrt(2)/2;
+
+ ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, interpolate_I_qxyz)
+{
+ Vector7s p1, p2, dp, p_pre, p_gt;
+ Vector3s da;
+
+ da.setRandom(); da /= 5;
+
+ p1 << 0,0,0, 0,0,0,1;
+ dp << 0,0,0, exp_q(da).coeffs();
+ p_gt = compose(p1, dp);
+ p2 = compose(p_gt, dp);
+ Scalar t = 0.5;
+
+ interpolate(p1, p2, t, p_pre);
+
+ ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, interpolate_half)
+{
+ Vector7s p1, p2, dp, p_pre, p_gt;
+ Vector6s da;
+
+ p1.setRandom(); p1.tail(4).normalize();
+
+ da.setRandom(); da /= 5; // small rotation
+ dp << exp_SE3(da);
+
+ // compose double, then interpolate 1/2
+
+ p_gt = compose(p1, dp);
+ p2 = compose(p_gt, dp);
+
+ Scalar t = 0.5;
+ interpolate(p1, p2, t, p_pre);
+
+ ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, interpolate_third)
+{
+ Vector7s p1, p2, dp, dp2, dp3, p_pre, p_gt;
+ Vector6s da;
+
+ p1.setRandom(); p1.tail(4).normalize();
+
+ da.setRandom(); da /= 5; // small rotation
+ dp << exp_SE3(da);
+ dp2 = compose(dp, dp);
+ dp3 = compose(dp2, dp);
+
+ // compose triple, then interpolate 1/3
+
+ p_gt = compose(p1, dp);
+ p2 = compose(p1, dp3);
+
+ Scalar t = 1.0/3.0;
+ interpolate(p1, p2, t, p_pre);
+
+ ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+}
+
+TEST(SE3, toSE3_I)
+{
+ Vector7s pose; pose << 0,0,0, 0,0,0,1;
+ Matrix4s R;
+ toSE3(pose, R);
+
+ ASSERT_MATRIX_APPROX(R, Matrix4s::Identity(), 1e-8);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+