diff options
-rw-r--r-- | src/main.cpp | 75 | ||||
-rw-r--r-- | src/main.h | 2 | ||||
-rw-r--r-- | src/state.cpp | 21 | ||||
-rw-r--r-- | src/state.h | 4 |
4 files changed, 61 insertions, 41 deletions
diff --git a/src/main.cpp b/src/main.cpp index 390a57e..bcf6b37 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -45,29 +45,32 @@ covCallback(const message &msg, State &mu, const Quaterniond &q) void imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q) { - std::vector<measurement_t> z; - int id,sx,sy,rx,ry; - strcat(msg.image_names[0],".txt"); - FILE *fin; - if ((fin=fopen(msg.image_names[0], "r"))==NULL) { - err_sys("fopen: %s", msg.image_names[0]); + if (seenutm && seenpva && seencov) { + std::vector<measurement_t> z; + int id,sx,sy,rx,ry; + strcat(msg.image_names[0],".txt"); + FILE *fin; + if ((fin=fopen(msg.image_names[0], "r"))==NULL) { + err_sys("fopen: %s", msg.image_names[0]); + } + while (fscanf(fin,"%d,%d,%d,%d,%d", &id, &sx, &sy, &rx, &ry)!=EOF) { + measurement_t m; + m.id = id; + // Points in the image frame + Vector3d xi, xir; + xi << sx, sy, 1.; + xir << rx, ry, 1.; + // Points in the camera frame + m.source = cam.img2body(xi); + m.reflection = cam.img2body(xir); + m.z_type = REFLECTION; + z.push_back(m); + } + if (fclose(fin)==EOF) { + err_sys("fclose"); + } + mu.feature_update(z,q); } - while (fscanf(fin,"%d,%d,%d,%d,%d", &id, &sx, &sy, &rx, &ry)!=EOF) { - measurement_t m; - m.id = id; - // Points in the image frame - Vector3d xi, xir; - xi << sx, sy, 1.; - xir << rx, ry, 1.; - // Points in the camera frame - m.source = cam.img2body(xi); - m.reflection = cam.img2body(xir); - z.push_back(m); - } - if (fclose(fin)==EOF) { - err_sys("fclose"); - } - mu.feature_update(z,q); return; } @@ -112,15 +115,27 @@ pvaCallback(const message &msg, State &mu, Quaterniond &q) } void -utmCallback(const message &msg, State &mu) +utmCallback(const message &msg, State &mu, const Quaterniond &q) { - if (!seenutm || !seencov || !seenpva) { + Vector3d tip; + tip << 0.6*3.43, 0, -0.34; + tip = q._transformVector(tip); + if ((!seenutm || !seencov) && seenpva) { seenutm=true; - mu.enu(msg.utm); - } else { // Perform Z measurement - Matrix<double,1,1> z; - z << -msg.utm.up; - mu.update(z); + UTM utm_water; + utm_water.northing = msg.utm.northing; + utm_water.easting = msg.utm.easting; + utm_water.up = -tip[2]; + utm_water.zone_i = msg.utm.zone_i; + utm_water.zone_c = msg.utm.zone_c; + mu.enu(utm_water); + } else if (seenutm) { // Perform Z measurement + std::vector<measurement_t> z; + measurement_t height; + height.z_type = HEIGHT; + height.height = tip[2]; + z.push_back(height); + //mu.update(q,z); } return; } @@ -268,7 +283,7 @@ while (scanf("%s", line)!=EOF) { switch ( msg.msg_type ) { case BESTUTM: - utmCallback(msg, mu); + utmCallback(msg, mu,qbw); break; case IMG: @@ -28,6 +28,6 @@ void covCallback(const message &msg, Matrix<double,9,9> &P, const Quaterniond &q void imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q); void imuCallback(const message &msg, State &mu, const Quaterniond &q, const timestamp dt); void pvaCallback(const message &msg, Matrix<double,9,1> &X, Quaterniond &q); -void utmCallback(const message &msg, State &mu); +void utmCallback(const message &msg, State &mu, const Quaterniond &q); #endif /* ----- #ifndef main_INC ----- */ diff --git a/src/state.cpp b/src/state.cpp index eac80d8..57ebbfa 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -49,8 +49,10 @@ State::enu ( const UTM &utm ) *-------------------------------------------------------------------------------------- */ MatrixXd -State::H ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z ) +State::H ( const Quaterniond &q, const std::vector<measurement_t> &z ) { + Vector3d pos; + pos = body->ned(); MatrixXd h; // Determine the size of H int cols = 9 + 3*features.size(); @@ -161,7 +163,6 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, const std::vector<measurement_t> &z, const Quaterniond &q) { MatrixXd K; - K = P*h.transpose()*S; // P^T is implied here since P is symmetric K = S.partialPivLu().solve(h*P).transpose(); P = (MatrixXd::Identity(P.rows(),P.rows())-K*h)*P; @@ -229,12 +230,11 @@ State::innovation( const std::vector<measurement_t> &z, const Quaterniond &q) } void -State::update ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z ) +State::update ( const Quaterniond &q, const std::vector<measurement_t> &z ) { - #ifdef FULLS MatrixXd h; - h = H(pos,q,z); + h = H(q,z); MatrixXd S; S = h*P*h.transpose() + R(z); kalmanUpdate(h,S,z,q); @@ -255,14 +255,19 @@ void State::feature_update ( const std::vector<measurement_t> &z, const Quaterniond &q) { std::vector<measurement_t> featuresToAdd; + std::vector<measurement_t> featuresToUpdate; for (auto i=z.begin(); i!=z.end(); ++i) { - if (i->z_type==HEIGHT) continue; - if (exists(i->id)) { - ; + if (i->z_type==HEIGHT) { + featuresToUpdate.push_back(*i); + } else if (exists(i->id)) { + featuresToUpdate.push_back(*i); } else { featuresToAdd.push_back(*i); } } + if (featuresToUpdate.size()>0) { + update(q, featuresToUpdate); + } addFeatures( featuresToAdd, q); return ; diff --git a/src/state.h b/src/state.h index 3a90217..7a5835d 100644 --- a/src/state.h +++ b/src/state.h @@ -38,7 +38,7 @@ class State int rowById(int id); Feature *featureById(int id); MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt); - MatrixXd H ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z ); + MatrixXd H ( const Quaterniond &q, const std::vector<measurement_t> &z ); int Hrows( const std::vector<measurement_t> &z ); MatrixXd Q(double dt); MatrixXd R( const std::vector<measurement_t> &z ); @@ -59,7 +59,7 @@ class State std::list<Feature *>::iterator removeFeature(std::list<Feature *>::iterator &i, int j); void velocity_covariance(const Matrix<double,3,3> &p); void vel(const Matrix<double,3,1> &v); - void update ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z ); + void update ( const Quaterniond &q, const std::vector<measurement_t> &z ); /* ==================== OPERATORS ======================================= */ Matrix<double,Dynamic,1> innovation( const std::vector<measurement_t> &z, const Quaterniond &q); |