From fde220c19346797c4d55c2f479f7efb2f8c9073a Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Sat, 18 Mar 2017 14:32:52 -0500
Subject: Add support for INSCOVS.

The position and velocity covariances are rotated into the correct
frames: NED and FRD.
---
 src/main.cpp | 55 ++++++++++++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 52 insertions(+), 3 deletions(-)

(limited to 'src/main.cpp')

diff --git a/src/main.cpp b/src/main.cpp
index 86d87f4..a30ca6c 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -22,15 +22,27 @@
 #include "main.h"
 bool seenutm=false;
 bool seenpva=false;
+Eigen::Matrix<double,9,9> P;
 
 #ifdef  USE_ROS
 
 #else      /* -----  not USE_ROS  ----- */
 void
+covCallback(const message &msg, Eigen::Matrix<double,9,9> &P, const Eigen::Quaternion<double> &q)
+{
+    Eigen::Matrix3d Rbw(q.toRotationMatrix());
+    Eigen::Matrix3d Renuned;
+    Renuned << 0.,1.,0.,1.,0.,0.,0.,0.,-1;
+    P.block(0,0,3,3) = Renuned*msg.covariance.position*Renuned.transpose();
+    P.block(3,3,3,3) = Rbw.transpose()*msg.covariance.velocity*Rbw;
+}
+
+void
 imgCallback(const message *msg)
 {
     return;
 }
+
 void 
 imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, const Eigen::Quaternion<double> &q, const timestamp dt)
 {
@@ -113,7 +125,39 @@ parseLine(char *line, message *data)
         rfn = strsep(&line, ",");
         sscanf(rfn, "%s", &data->image_names+1);
     } else if (!strcmp(msg_type,"inscovs")) {
+        using Eigen::Map;
+        using Eigen::Matrix3d;
+        double pos[9];
+        double att[9];
+        double vel[9];
         data->msg_type = INSCOVS;
+        sscanf(line,"%lf,%lf,%lf,\
+                     %lf,%lf,%lf,\
+                     %lf,%lf,%lf,\
+                     %lf,%lf,%lf,\
+                     %lf,%lf,%lf,\
+                     %lf,%lf,%lf,\
+                     %lf,%lf,%lf,\
+                     %lf,%lf,%lf,\
+                     %lf,%lf,%lf",
+                     pos, pos+1, pos+2,
+                     pos+3, pos+4, pos+5,
+                     pos+6, pos+7, pos+8,
+                     att, att+1, att+2,
+                     att+3, att+4, att+5,
+                     att+6, att+7, att+8,
+                     vel, vel+1, vel+2,
+                     vel+3, vel+4, vel+5,
+                     vel+6, vel+7, vel+8);
+        data->covariance.position << pos[0], pos[1], pos[2],
+                                     pos[3], pos[4], pos[5],
+                                     pos[6], pos[7], pos[8];
+        data->covariance.attitude << att[0], att[1], att[2],
+                                     att[3], att[4], att[5],
+                                     att[6], att[7], att[8];
+        data->covariance.velocity << vel[0], vel[1], vel[2],
+                                     vel[3], vel[4], vel[5],
+                                     vel[6], vel[7], vel[8];
     } else if (!strcmp(msg_type,"inspvas")) {
         data->msg_type = INSPVAS;
         sscanf(line,"%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf",
@@ -168,6 +212,7 @@ main(int argc, char **argv)
 {
     Eigen::Quaternion<double> qbw;
     Eigen::Matrix<double,9,1> bodyStateVector;
+    Eigen::Matrix<double,9,9> P;
     // bias in FRD coordinates
     bodyStateVector.segment(6,3) << 0.975*-0.03713532, 0.9*0.01465135, -0.00709229;
     timestamp dt{0,0};
@@ -176,9 +221,9 @@ main(int argc, char **argv)
 #ifdef  USE_ROS
     
 #else      /* -----  not USE_ROS  ----- */
-    using std::cout;
-    using std::endl;
-    using std::cerr;
+using std::cout;
+using std::endl;
+using std::cerr;
 printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East\n");
 // Read sensors from file
 char line[MAXLINE];
@@ -209,6 +254,10 @@ while (scanf("%s", line)!=EOF) {
             imuCallback(msg, bodyStateVector, qbw, dt);
             break;
 
+        case INSCOVS:
+            covCallback(msg, P, qbw);
+            cout << P << endl;
+
         default:	
             break;
     }				/* -----  end switch  ----- */
-- 
cgit v1.1