From d119b2c7ca16b8375abfeb5c1efd8e98047be5ad Mon Sep 17 00:00:00 2001 From: Martin Miller Date: Mon, 27 Mar 2017 11:23:34 -0500 Subject: Tweak main --- src/main.cpp | 65 ++++++++++++++++++++++++++++++++++++++++++++++++++---------- src/main.h | 2 +- 2 files changed, 56 insertions(+), 11 deletions(-) (limited to 'src') diff --git a/src/main.cpp b/src/main.cpp index 85bf09f..1fa6ec0 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -30,18 +30,37 @@ using std::cerr; #ifdef USE_ROS #else /* ----- not USE_ROS ----- */ + +/* + * === FUNCTION ====================================================================== + * Name: covCallback + * Description: Initializes position and velocity covariance in the proper + * coordinate frames. Rbw is the body to NED rotation. Renuned is the ENU->NED + * rotation. This is only set during the initialization phase and not during + * normal running. + * ===================================================================================== + */ void covCallback(const message &msg, State &mu, const Quaterniond &q) { if (seencov && seenutm && seenpva) return; seencov=true; + // Rotation from body to world Matrix3d Rbw(q.toRotationMatrix()); + // Rotation from ENU to NED Matrix3d Renuned; Renuned << 0.,1.,0.,1.,0.,0.,0.,0.,-1; mu.position_covariance(Renuned*msg.covariance.position*Renuned.transpose()); mu.velocity_covariance(Rbw.transpose()*msg.covariance.velocity*Rbw); } +/* + * === FUNCTION ====================================================================== + * Name: imgCallback + * Description: Handles image measurements and update the filter. Only runs + * after the initialization phase. + * ===================================================================================== + */ void imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q) { @@ -57,12 +76,12 @@ imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q) height.height = tip[2]; z.push_back(height); - 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]); } + int id,sx,sy,rx,ry; while (fscanf(fin,"%d,%d,%d,%d,%d", &id, &sx, &sy, &rx, &ry)!=EOF) { measurement_t m; m.id = id; @@ -85,6 +104,13 @@ imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q) return; } +/* + * === FUNCTION ====================================================================== + * Name: imuCallback + * Description: Only runs after the initialization is complete. Updates the + * motion model, P_k|k-1, and removes features that leave the FOV. + * ===================================================================================== + */ void imuCallback(const message &msg, State &mu, const Quaterniond &q, const timestamp dt) { @@ -99,6 +125,13 @@ imuCallback(const message &msg, State &mu, const Quaterniond &q, const timestamp mu.motionModel(acc,ang,q,dtf); } +/* + * === FUNCTION ====================================================================== + * Name: pvaCallback + * Description: During the initialization phase, updates the velocity and + * quaternion. Afterwards, updates only the quaternion. + * ===================================================================================== + */ void pvaCallback(const message &msg, State &mu, Quaterniond &q) { @@ -124,6 +157,13 @@ pvaCallback(const message &msg, State &mu, Quaterniond &q) return; } +/* + * === FUNCTION ====================================================================== + * Name: utmCallback + * Description: During the initialization, updates the position. Does nothing + * during normal running. + * ===================================================================================== + */ void utmCallback(const message &msg, State &mu, const Quaterniond &q) { @@ -139,17 +179,17 @@ utmCallback(const message &msg, State &mu, const Quaterniond &q) 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 z; - measurement_t height; - height.z_type = HEIGHT; - height.height = tip[2]; - z.push_back(height); - //mu.update(q,z); - } + } return; } +/* + * === FUNCTION ====================================================================== + * Name: parseLine + * Description: Parses a line from regular file input. Gets the time, + * determines the message type and fills in the corresponding data fields. + * ===================================================================================== + */ int parseLine(char *line, message *data) { @@ -241,7 +281,12 @@ parseLine(char *line, message *data) } } #endif /* ----- not USE_ROS ----- */ - +/* + * === FUNCTION ====================================================================== + * Name: update_dt + * Description: Determines dt based on the current and previous timestamp. + * ===================================================================================== + */ timestamp update_dt(const timestamp t, timestamp *t_old) { diff --git a/src/main.h b/src/main.h index bf28c02..0746a99 100644 --- a/src/main.h +++ b/src/main.h @@ -24,7 +24,7 @@ using Eigen::Vector3d; int parseLine(char *line, message *msg); timestamp update_dt(const timestamp t, timestamp *t_old); -void covCallback(const message &msg, Matrix &P, const Quaterniond &q); +void covCallback(const message &msg, State &mu, 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 &X, Quaterniond &q); -- cgit v1.1