From c51745a84d6240e68e6a429a8b9c80341a0198d7 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Mon, 20 Mar 2017 00:03:13 -0500
Subject: Use hardcoded block sizes. Update skewSymmetric.

We can hardcode block sizes using templates for a possible performance
increase. Body::skewSymmetric() was modified to return matrix instead of
modifying one in place.
---
 src/body.cpp | 31 ++++++++++++++++---------------
 1 file changed, 16 insertions(+), 15 deletions(-)

(limited to 'src/body.cpp')

diff --git a/src/body.cpp b/src/body.cpp
index 643d91c..f27a62f 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -100,12 +100,12 @@ Body::Q (double dt)
 {
     Matrix<double,9,9> Q;
     Q = Matrix<double,9,9>::Zero();
-    Q.block(0,0,3,3) = 0.25*dt*dt*dt*dt*Matrix<double,3,3>::Identity();
-    Q.block(3,0,3,3) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity();
-    Q.block(0,3,3,3) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity();
-    Q.block(3,3,3,3) = dt*dt*Matrix<double,3,3>::Identity();
+    Q.block<3,3>(0,0) = 0.25*dt*dt*dt*dt*Matrix<double,3,3>::Identity();
+    Q.block<3,3>(3,0) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity();
+    Q.block<3,3>(0,3) = 0.5*dt*dt*dt*Matrix<double,3,3>::Identity();
+    Q.block<3,3>(3,3) = dt*dt*Matrix<double,3,3>::Identity();
     Q *= 800e-6;
-    Q.block(6,6,3,3) = dt*dt*5e-8*Matrix<double,3,3>::Identity();
+    Q.block<3,3>(6,6) = dt*dt*5e-8*Matrix<double,3,3>::Identity();
     return Q;
 }		/* -----  end of method Body::q  ----- */
 
@@ -154,10 +154,10 @@ Body::F ( const Vector3d &ang, const Quaterniond &q )
     Matrix<double,9,9> F = Matrix<double,9,9>::Zero();
     Matrix<double,3,3> Rbw(q.toRotationMatrix());
     Matrix<double,3,3> W;
-    skewSymmetric(ang,W);
-    F.block(0,3,3,3) = Rbw;
-    F.block(3,3,3,3) = -W;
-    F.block(3,6,3,3) = -Matrix<double,3,3>::Identity();
+    W = skewSymmetric(ang);
+    F.block<3,3>(0,3) = Rbw;
+    F.block<3,3>(3,3) = -W;
+    F.block<3,3>(3,6) = -Matrix<double,3,3>::Identity();
     return F;
 }		/* -----  end of method Body::F  ----- */
 
@@ -180,10 +180,10 @@ Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond
     b = Matrix<double,6,1>::Zero();
 
     Matrix<double,3,3> Rbw(q.toRotationMatrix());
-    A.block(0,0,3,3) = Rbw;
+    A.block<3,3>(0,0) = Rbw;
     Matrix<double,3,3> W;
-    skewSymmetric(ang,W);
-    A.block(3,0,3,3) = -W;
+    W = skewSymmetric(ang);
+    A.block<3,3>(3,0) = -W;
     b.segment(3,3) = acc-bias+Rbw.transpose()*gravity_world;
 
     X.segment(0,6) += (A*X.segment(3,3)+b)*dt;
@@ -197,11 +197,12 @@ Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond
  * Description:  Create the skew symmetric matrix y from the vector x.
  *--------------------------------------------------------------------------------------
  */
-void
-Body::skewSymmetric ( const Vector3d &x, Matrix<double,3,3> &y )
+Matrix<double,3,3>
+Body::skewSymmetric ( const Vector3d &x )
 {
+    Matrix<double,3,3> y;
     y << 0.,-x[2], x[1], x[2],0.,-x[0],-x[1],x[0],0.;
-    return ;
+    return y;
 }		/* -----  end of method Body::skewSymmetric  ----- */
 
 
-- 
cgit v1.1