Convert rotation matrix expressions in Feature::h() to quaternion.

Since the native representation of the attitude is in quaternion, it seems more
consistent to use quaternion operations instead of converting to rotation
matrices. This was also done to double-check my hand derived expressions that I
intent to use in publication.

1 files changed, 12 insertions, 13 deletions

diff --git a/src/feature.cpp b/src/feature.cpp
index 82ccc63..2c5dbb0 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -232,22 +232,21 @@ Feature::h ( const Vector3d &x, const Quaterniond &q )
     xib = p2x(X);
 
     // Predict initial view
-    Matrix3d Rbb0, Rbw, Rb0w;
-    Rbw = q.toRotationMatrix();
-    Rb0w = q0.toRotationMatrix();
-    Rbb0 = Rb0w.transpose()*Rbw;
-    xib0 = Rbb0*xib + Rbw.transpose()*(x-xb0w);
-    pib0 = x2p(xib0);
+    Quaterniond qbb0, qib, qw, qb0w;
+    qib = Quaterniond(0, xib[0], xib[1], xib[2]);
+    qw = Quaterniond(0,x[0],x[1],x[2]);
+    qb0w = Quaterniond(0,xb0w[0],xb0w[1],xb0w[2]);
 
-    // Predict reflection view
-    Matrix<double,3,1> n;
-    n << 0, 0, 1;
-    Matrix3d S;
-    S = Matrix3d::Identity() - 2*n*n.transpose();
-    Vector3d xpb, ppb;
+    qbb0 = q0.inverse()*q;
+    xib0 = (qbb0*qib*qbb0.inverse()).vec() + (q.inverse()*qw*q).vec() - (q.inverse()*qb0w*q).vec();
+    pib0 = x2p(xib0);
 
-    xpb = Rbw.transpose()*(S*Rbw*xib-2*n*n.transpose()*x);
+    // Predict reflection using quaternions with rotation to world
+    Vector3d ppb,xpb;
+    Quaterniond qn;
+    qn = Quaterniond(0.,0.,0.,-1.); // plane of water in world frame
 
+    xpb = (q.inverse()*qn*q*qib*q.inverse()*qn*q).vec()+(q.inverse()*qn*qw*qn*q).vec()-(q.inverse()*qw*q).vec();
     ppb = x2p(xpb);
 
     h[0] = X[0];