Use householder transformation for ppbhat

1 files changed, 10 insertions, 9 deletions

diff --git a/src/feature.cpp b/src/feature.cpp
index fe62bbf..004fcb3 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -203,16 +203,17 @@ Feature::h ( const Vector3d &x, const Quaterniond &q )
     pib0 = x2p(xib0);
 
     // Predict reflection view
+    Matrix<double,3,1> n;
+    n << 0, 0, 1;
+    Matrix3d S;
+    S = Matrix3d::Identity() - 2*n*n.transpose();
     Vector3d xpb, ppb;
-    Quaterniond qxpb, qrb;
-    const Quaterniond qrw(0,0,0,-1); // the reflecting plane in world frame
-    Quaterniond qxib(0,xib[0],xib[1],xib[2]);
-
-    // Compute reflection plane in body frame
-    qrb = q.conjugate()*qrw;
-    // Reflect xib about qrb
-    qxpb = qrb*qxib*qrb;
-    xpb = qxpb.vec();
+
+    Matrix3d Rbw;
+    Rbw = q.toRotationMatrix();
+
+    xpb = Rbw.transpose()*(S*Rbw*xib-2*n*n.transpose()*x);
+    
     ppb = x2p(xpb);
 
     h[0] = X[0];