From 012cb01c50932bfebe308238186bff2fcc285614 Mon Sep 17 00:00:00 2001 From: Martin Miller Date: Sun, 26 Mar 2017 15:35:32 -0500 Subject: Use householder transformation for ppbhat --- src/feature.cpp | 19 ++++++++++--------- 1 file 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 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]; -- cgit v1.1