Tweak main

2 files changed, 56 insertions, 11 deletions

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<measurement_t> 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<double,9,9> &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<double,9,1> &X, Quaterniond &q);