From 80db4deddec37dae4860f0603e92183c21ed4031 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Mon, 5 Sep 2016 11:04:14 +0200
Subject: [PATCH] [Minor][C++] Minor optimization of exp6
---
src/spatial/explog.hpp | 5 +++--
1 file changed, 3 insertions(+), 2 deletions(-)
diff --git a/src/spatial/explog.hpp b/src/spatial/explog.hpp
index 7794b146b..af9966690 100644
--- a/src/spatial/explog.hpp
+++ b/src/spatial/explog.hpp
@@ -83,9 +83,10 @@ namespace se3
Scalar t = w.norm();
if (t > 1e-15)
{
- Matrix3 S(alphaSkew(1./t, w));
+ const double inv_t = 1./t;
+ Matrix3 S(alphaSkew(inv_t, w));
double ct,st; SINCOS (t,&st,&ct);
- Matrix3 V((1. - ct)/t * S + (1. - st/t) * S * S);
+ Matrix3 V((1. - ct) * inv_t * S + (1. - st * inv_t) * S * S);
Vector3 p(v + V * v);
return SE3Tpl<_Scalar, _Options>(exp3(w), p);
}
--
GitLab