From 0ebd419f71a639f4c8b552981c180613485b5ee6 Mon Sep 17 00:00:00 2001 From: Martin Miller Date: Mon, 31 Jul 2017 22:51:13 -0500 Subject: 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. --- src/feature.cpp | 25 ++++++++++++------------- 1 file 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 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]; -- cgit v1.1