summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorMartin Miller2017-03-26 15:21:00 -0500
committerMartin Miller2017-03-26 15:21:00 -0500
commitc7af11e6a252a7dfbacd9a702bbe3ee0a9127b17 (patch)
tree3e5e0f02dd23bd073ffaef9e9e510761b4cbd16a
parent8685dabd1fc9614eeece5134352d09055b2d80f2 (diff)
downloadrefslam-c7af11e6a252a7dfbacd9a702bbe3ee0a9127b17.zip
refslam-c7af11e6a252a7dfbacd9a702bbe3ee0a9127b17.tar.gz
Set height above water.
-rw-r--r--src/main.cpp75
-rw-r--r--src/main.h2
-rw-r--r--src/state.cpp21
-rw-r--r--src/state.h4
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:
diff --git a/src/main.h b/src/main.h
index 0698e68..bf28c02 100644
--- a/src/main.h
+++ b/src/main.h
@@ -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);