diff options
| -rw-r--r-- | src/body.cpp | 7 | ||||
| -rw-r--r-- | src/feature.cpp | 35 | ||||
| -rw-r--r-- | src/feature.h | 2 | ||||
| -rw-r--r-- | src/main.cpp | 10 | ||||
| -rw-r--r-- | src/state.cpp | 10 |
5 files changed, 45 insertions, 19 deletions
diff --git a/src/body.cpp b/src/body.cpp index 83838a5..ae43e58 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -28,6 +28,11 @@ void Body::update ( const Matrix<double,9,1> &dx ) { X += dx; + if (X[2]<-1.) { + X[2]=-1.; + } else if (X[2]>-0.2) { + X[2]=-0.2; + } return ; } /* ----- end of method Body::update ----- */ @@ -84,7 +89,7 @@ Matrix<double,1,1> Body::R() { Matrix<double,1,1> R; - R << 1e-3; + R << 1e-6; return R; } 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; diff --git a/src/feature.h b/src/feature.h index a405e8f..a169287 100644 --- a/src/feature.h +++ b/src/feature.h @@ -29,7 +29,6 @@ class Feature /* ==================== ACCESSORS ======================================= */ int id(); - Vector2d initialView(); Matrix<double,3,3> P0(); UTM utm(Vector3d &xbw, Quaterniond &q); @@ -48,6 +47,7 @@ class Feature Matrix<double,6,3> Hy( const Vector3d &pos, const Quaterniond &q); Matrix<double,6,1> h( const Vector3d &x, const Quaterniond &q); Matrix<double,3,6> L(); + Matrix<double,6,1> measurement_vector(const Vector3d &xs, const Vector3d &xr); Matrix<double,3,3> Q(const double dt); Matrix<double,6,6> R(); diff --git a/src/main.cpp b/src/main.cpp index bcf6b37..85bf09f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -47,6 +47,16 @@ imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q) { if (seenutm && seenpva && seencov) { std::vector<measurement_t> z; + + Vector3d tip; + tip << 0.6*3.43, 0, -0.34; + tip = q._transformVector(tip); + + measurement_t height; + height.z_type = HEIGHT; + height.height = tip[2]; + z.push_back(height); + int id,sx,sy,rx,ry; strcat(msg.image_names[0],".txt"); FILE *fin; diff --git a/src/state.cpp b/src/state.cpp index 57ebbfa..998be20 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -21,7 +21,7 @@ State::State ( ) { body = new Body; P = Matrix<double,Dynamic,Dynamic>::Zero(9,9); - P.block(6,6,3,3) = 1e-6*Matrix3d::Identity(); + P.block(6,6,3,3) = 1e-9*Matrix3d::Identity(); return ; } /* ----- end of method State::State ----- */ @@ -204,9 +204,7 @@ State::innovation( const std::vector<measurement_t> &z, const Quaterniond &q) fi = featureById(z[i].id); Matrix<double,6,1> hi, zi; hi = fi->h(body->ned(), q); - zi.segment<2>(0) = z[i].source.segment<2>(0); - zi.segment<2>(2) = fi->initialView(); - zi.segment<2>(4) = z[i].reflection.segment<2>(0); + zi = fi->measurement_vector(z[i].source, z[i].reflection); y.segment<6>(row) = zi-hi; row += 6; } else if (mt==MONO) { @@ -238,6 +236,7 @@ State::update ( const Quaterniond &q, const std::vector<measurement_t> &z ) MatrixXd S; S = h*P*h.transpose() + R(z); kalmanUpdate(h,S,z,q); + cout << features.size() << endl; #else /* ----- not FULLS ----- */ @@ -280,7 +279,8 @@ State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q) for (auto i=F.begin(); i!=F.end(); ++i) { if (features.size()>MAXFEATURES) break; // Create feature - Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q, -0.44); + Vector3d pos = body->ned(); + Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q, pos[2]); if (!f->sane()) { delete f; continue; |