[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[Toon-members] TooN sim2.h sim3.h
From: |
Gerhard Reitmayr |
Subject: |
[Toon-members] TooN sim2.h sim3.h |
Date: |
Thu, 30 Jun 2011 17:52:27 +0000 |
CVSROOT: /cvsroot/toon
Module name: TooN
Changes by: Gerhard Reitmayr <gerhard> 11/06/30 17:52:27
Modified files:
. : sim2.h sim3.h
Log message:
fixed sim{2,3} with Taylor series expansions to avoid cancelling poles
at 0 in various terms. The adjoints are still missing
CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/TooN/sim2.h?cvsroot=toon&r1=1.1&r2=1.2
http://cvs.savannah.gnu.org/viewcvs/TooN/sim3.h?cvsroot=toon&r1=1.1&r2=1.2
Patches:
Index: sim2.h
===================================================================
RCS file: /cvsroot/toon/TooN/sim2.h,v
retrieving revision 1.1
retrieving revision 1.2
diff -u -b -r1.1 -r1.2
--- sim2.h 29 Jun 2011 20:20:00 -0000 1.1
+++ sim2.h 30 Jun 2011 17:52:26 -0000 1.2
@@ -34,6 +34,7 @@
#define TOON_INCLUDE_SIM2_H
#include <TooN/se2.h>
+#include <TooN/sim3.h>
namespace TooN {
@@ -340,22 +341,12 @@
result.get_rotation() = SO2<Precision>::exp(theta);
// scale
- const Precision s = mu[3];
- const Precision es = exp(s);
- result.get_scale() = es;
+ result.get_scale() = exp(mu[3]);
// translation
- const Precision a = es * sin(theta);
- const Precision b = es * cos(theta);
- const Precision c = (fabs(s) < 1e-6) ? (1 + s/2 + s*s/6) : expm1(s)/s;
-
- const Precision inv_s_theta = 1/(s*s + theta * theta);
-
- const Precision CC = (a*s + (1-b)*theta) * inv_s_theta / theta;
- const Precision CCC = ((b-1)*s + a*theta) * inv_s_theta;
-
+ const Vector<3, Precision> coeff =
Internal::compute_rodrigues_coefficients_sim3(mu[3], theta);
const Vector<2, Precision> cross = makeVector( -theta * mu[1], theta *
mu[0]);
- result.get_translation() = CCC * mu.template slice<0,2>() +
TooN::operator*(CC, cross);
+ result.get_translation() = coeff[2] * mu.template slice<0,2>() +
TooN::operator*(coeff[1], cross);
return result;
}
@@ -371,22 +362,12 @@
result[2] = theta;
// scale
- const Precision es = sim2.get_scale();
- const Precision s = log(sim2.get_scale());
- result[3] = s;
+ result[3] = log(sim2.get_scale());
// translation
- const Precision a = es * sin(theta);
- const Precision b = es * cos(theta);
- const Precision c = (fabs(s) < 1e-6) ? (1 + s/2 + s*s/6) : expm1(s)/s;
-
- const Precision inv_s_theta = 1/(s*s + theta * theta);
-
- const Precision CC = (a*s + (1-b)*theta) * inv_s_theta / theta;
- const Precision CCC = ((b-1)*s + a*theta) * inv_s_theta;
-
+ const Vector<3, Precision> coeff =
Internal::compute_rodrigues_coefficients_sim3(result[3], theta);
Matrix<2,2, Precision> cross = Zeros; cross(0,1) = -theta; cross(1,0) =
theta;
- const Matrix<2,2, Precision> W = CCC * Identity + CC * cross;
+ const Matrix<2,2, Precision> W = coeff[2] * Identity + coeff[1] * cross;
result.template slice<0,2>() = gaussian_elimination(W,
sim2.get_translation());
return result;
Index: sim3.h
===================================================================
RCS file: /cvsroot/toon/TooN/sim3.h,v
retrieving revision 1.1
retrieving revision 1.2
diff -u -b -r1.1 -r1.2
--- sim3.h 29 Jun 2011 20:20:00 -0000 1.1
+++ sim3.h 30 Jun 2011 17:52:26 -0000 1.2
@@ -39,7 +39,7 @@
/// This can be represented by a \f$3\times\f$4 matrix operating on a
homogeneous co-ordinate,
/// so that a vector \f$\underline{x}\f$ is transformed to a new location
\f$\underline{x}'\f$
/// by
-/// \f[\begin{aligned}\underline{x}' &= E\times\underline{x}\\
\begin{bmatrix}x'\\y'\\z'\end{bmatrix} &= \begin{pmatrix}r_{11} & r_{12} &
r_{13} & t_1\\r_{21} & r_{22} & r_{23} & t_2\\r_{31} & r_{32} & r_{33} &
t_3\end{pmatrix}\begin{bmatrix}x\\y\\z\\1\end{bmatrix}\end{aligned}\f]
+/// \f[\begin{aligned}\underline{x}' &= E\times\underline{x}\\
\begin{bmatrix}x'\\y'\\z'\end{bmatrix} &= \begin{pmatrix}s r_{11} & s r_{12} &
s r_{13} & t_1\\s r_{21} & s r_{22} & s r_{23} & t_2\\s r_{31} & s r_{32} & s
r_{33} & t_3\end{pmatrix}\begin{bmatrix}x\\y\\z\\1\end{bmatrix}\end{aligned}\f]
///
/// This transformation is a member of the Lie group SIM3. These can be
parameterised with
/// seven numbers (in the space of the Lie Algebra). In this class, the first
three parameters are a
@@ -404,56 +404,68 @@
return Matrix<Rows,4,typename
Internal::MultiplyType<PM,P>::type>(Operator<Internal::MSIM3Mult<Rows, C, PM,
A, P> >(lhs,rhs));
}
+namespace Internal {
+
+/// internal function that calculates the coefficients for the Rodrigues
formula for SIM3 translation
+template <typename Precision>
+inline Vector<3, Precision> compute_rodrigues_coefficients_sim3( const
Precision & s, const Precision & t ){
+ using std::exp;
+
+ Vector<3, Precision> coeff;
+ const Precision es = exp(s);
+
+ // 4 cases for s -> 0 and/or theta -> 0
+ // the Taylor expansions were calculated with Maple 12 and truncated at
the 3rd power,
+ // such that eps^3 < 1e-18 which results in approximately 1 + eps^3 = 1
+ static const Precision eps = 1e-6;
+
+ if(fabs(s) < eps && fabs(t) < eps){
+ coeff[0] = 1 + s/2 + s*s/6;
+ coeff[1] = 1/2 + s/3 - t*t/24 + s*s/8;
+ coeff[2] = 1/6 + s/8 - t*t/120 + s*s/20;
+ } else if(fabs(s) < eps) {
+ coeff[0] = 1 + s/2 + s*s/6;
+ coeff[1] = (1-cos(t))/(t*t) +
(sin(t)-cos(t)*t)*s/(t*t*t)+(2*sin(t)*t-t*t*cos(t)-2+2*cos(t))*s*s/(2*t*t*t*t);
+ coeff[2] = (t-sin(t))/(t*t*t) -
(-t*t-2+2*cos(t)+2*sin(t)*t)*s/(2*t*t*t*t) -
(-t*t*t+6*cos(t)*t+3*sin(t)*t*t-6*sin(t))*s*s/(6*t*t*t*t*t);
+ } else if(fabs(t) < eps) {
+ coeff[0] = (es - 1)/s;
+ coeff[1] = (s*es+1-es)/(s*s) -
(6*s*es+6-6*es+es*s*s*s-3*es*s*s)*t*t/(6*s*s*s*s);
+ coeff[2] = (es*s*s-2*s*es+2*es-2)/(2*s*s*s) -
(es*s*s*s*s-4*es*s*s*s+12*es*s*s-24*s*es+24*es-24)*t*t/(24*s*s*s*s*s);
+ } else {
+ const Precision a = es * sin(t);
+ const Precision b = es * cos(t);
+ const Precision inv_s_theta = 1/(s*s + t*t);
+
+ coeff[0] = (es - 1)/s;
+ coeff[1] = (a*s + (1-b)*t) * inv_s_theta / t;
+ coeff[2] = (coeff[0] - ((b-1)*s + a*t) * inv_s_theta) / (t*t);
+ }
+
+ return coeff;
+}
+
+}
+
template <typename Precision>
template <int S, typename P, typename VA>
inline SIM3<Precision> SIM3<Precision>::exp(const Vector<S, P, VA>& mu){
SizeMismatch<7,S>::test(7, mu.size());
- static const Precision one_6th = 1.0/6.0;
- static const Precision one_20th = 1.0/20.0;
-
- using std::sqrt;
using std::exp;
SIM3<Precision> result;
- const Vector<3,Precision> w = mu.template slice<3,3>();
- const Precision theta_sq = w*w;
- const Precision theta = sqrt(theta_sq);
-
// scale
- const Precision s = mu[6];
- const Precision es = exp(s);
- result.get_scale() = es;
+ result.get_scale() = exp(mu[6]);
// rotation
- Precision A, B;
- if (theta_sq < 1e-8) {
- A = 1.0 - one_6th * theta_sq;
- B = 0.5;
- } else {
- if (theta_sq < 1e-6) {
- A = 1.0 - theta_sq * one_6th*(1.0 - one_20th *
theta_sq);
- B = 0.5 - 0.25 * one_6th * theta_sq;
- } else {
- const Precision inv_theta = 1.0/theta;
- A = sin(theta) * inv_theta;
- B = (1 - cos(theta)) * (inv_theta * inv_theta);
- }
- }
- rodrigues_so3_exp(w, A, B, result.get_rotation().my_matrix);
+ const Vector<3,Precision> w = mu.template slice<3,3>();
+ const Precision t = norm(w);
+ result.get_rotation() = SO3<>::exp(w);
// translation
- const Precision a = es * sin(theta);
- const Precision b = es * cos(theta);
- const Precision c = (fabs(s) < 1e-6) ? (1 + s/2 + s*s/6) : expm1(s)/s;
-
- const Precision inv_s_theta = 1/(s*s + theta_sq);
-
- const Precision CC = (a*s + (1-b)*theta) * inv_s_theta / theta;
- const Precision CCC = (c - ((b-1)*s + a*theta) * inv_s_theta) / (theta
* theta);
-
+ const Vector<3, Precision> coeff =
Internal::compute_rodrigues_coefficients_sim3(mu[6],t);
const Vector<3,Precision> cross = w ^ mu.template slice<0,3>();
- result.get_translation() = c * mu.template slice<0,3>() +
TooN::operator*(CC, cross) + TooN::operator*(CCC, (w ^ cross));
+ result.get_translation() = coeff[0] * mu.template slice<0,3>() +
TooN::operator*(coeff[1], cross) + TooN::operator*(coeff[2], (w ^ cross));
return result;
}
@@ -467,8 +479,7 @@
// rotation
result.template slice<3,3>() = sim3.get_rotation().ln();
- const Precision theta_sq = norm_sq(result.template slice<3,3>());
- const Precision theta = sqrt(theta_sq);
+ const Precision theta = norm(result.template slice<3,3>());
// scale
const Precision es = sim3.get_scale();
@@ -476,15 +487,9 @@
result[6] = s;
// Translation
- const Precision a = es * sin(theta);
- const Precision b = es * cos(theta);
- const Precision c = (fabs(s) < 1e-6) ? (1 + s/2 + s*s/6) : expm1(s)/s;
- const Precision inv_s_theta = 1/(s*s + theta_sq);
-
- const Precision CC = (a*s + (1-b)*theta) * inv_s_theta / theta;
- const Precision CCC = (c - ((b-1)*s + a*theta) * inv_s_theta) / (theta
* theta);
+ const Vector<3, Precision> coeff =
Internal::compute_rodrigues_coefficients_sim3(s, theta);
const Matrix<3,3, Precision> cross =
cross_product_matrix(result.template slice<3,3>());
- const Matrix<3,3, Precision> W = Identity * c + cross * CC + cross *
cross * CCC;
+ const Matrix<3,3, Precision> W = Identity * coeff[0] + cross * coeff[1]
+ cross * cross * coeff[2];
result.template slice<0,3>() = gaussian_elimination(W,
sim3.get_translation());
[Prev in Thread] |
Current Thread |
[Next in Thread] |
- [Toon-members] TooN sim2.h sim3.h,
Gerhard Reitmayr <=