diff options
Diffstat (limited to 'src/feature.cpp')
| -rw-r--r-- | src/feature.cpp | 35 |
1 files changed, 23 insertions, 12 deletions
diff --git a/src/feature.cpp b/src/feature.cpp index 004fcb3..93ff7bb 100644 --- a/src/feature.cpp +++ b/src/feature.cpp @@ -37,12 +37,6 @@ Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xr, q0 = q; } /* ----- end of method Feature::Feature (constructor) ----- */ -Vector2d -Feature::initialView ( ) -{ - return X0.segment<2>(0); -} /* ----- end of method Feature::initialView ----- */ - /* *-------------------------------------------------------------------------------------- @@ -139,7 +133,7 @@ Feature::R ( ) { Matrix<double,6,6> R; R = Matrix<double,6,6>::Identity(); - R *= 1e-9; + R *= 1e-7; return R; } /* ----- end of method Feature::R ----- */ @@ -199,7 +193,11 @@ Feature::h ( const Vector3d &x, const Quaterniond &q ) xib = p2x(X); // Predict initial view - xib0 = q0._transformVector(q._transformVector(xib) + x - xb0w); + Matrix3d Rbb0, Rbw, Rb0w; + Rbw = q.toRotationMatrix(); + Rb0w = q0.toRotationMatrix(); + Rbb0 = Rb0w.transpose()*Rbw; + xib0 = Rbb0*xib + Rbw.transpose()*(x-xb0w); pib0 = x2p(xib0); // Predict reflection view @@ -209,9 +207,6 @@ Feature::h ( const Vector3d &x, const Quaterniond &q ) S = Matrix3d::Identity() - 2*n*n.transpose(); Vector3d xpb, ppb; - Matrix3d Rbw; - Rbw = q.toRotationMatrix(); - xpb = Rbw.transpose()*(S*Rbw*xib-2*n*n.transpose()*x); ppb = x2p(xpb); @@ -226,6 +221,22 @@ Feature::h ( const Vector3d &x, const Quaterniond &q ) return h; } /* ----- end of method Feature::h ----- */ +Matrix<double,6,1> +Feature::measurement_vector ( const Vector3d &xs, const Vector3d &xr ) +{ + Matrix<double,6,1> z; + Vector3d ps,p0,pr; + ps = x2p(xs); + pr = x2p(xr); + z[0] = ps[0]; + z[1] = ps[1]; + z[2] = X0[0]; + z[3] = X0[1]; + z[4] = pr[0]; + z[5] = pr[1]; + return z; +} /* ----- end of method Feature::measurement_vector ----- */ + /* *-------------------------------------------------------------------------------------- * Class: Feature @@ -457,7 +468,7 @@ Feature::P0 ( ) { Matrix<double,3,3> P; double p0 = 1e-4; - double p1 = 1e-2; + double p1 = 1e-4; P << p0, 0., 0., 0., p0, 0., 0., 0., p1; |