diff options
| -rw-r--r-- | src/feature.cpp | 19 |
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]; |