Changes

7 files changed, 71 insertions, 52 deletions

diff --git a/Makefile b/Makefile
index 657ff46..4710ab9 100644
--- a/Makefile
+++ b/Makefile
@@ -1,7 +1,7 @@
-OBJECT=src/main.o src/body.o src/state.o src/feature.o src/camera.o src/ourerr.o src/types.o
+OBJECT=src/main.o src/body.o src/state.o src/feature.o src/camera.o src/ourerr.o 
 SRCS=$(patsubst %.o,%.cpp,$(OBJECT))
-CXXFLAGS+=-O2 -march=native -std=c++11 -pedantic-errors
-CXXFLAGS+=-g
+CXXFLAGS+=-O3 -march=native -std=c++11
+#CXXFLAGS+=-g
 CXXFLAGS+=$(shell pkg-config --cflags eigen3 yaml-cpp)
 #CXXFLAGS+=$(shell pkg-config --cflags opencv)
 LIBS+=$(shell pkg-config --libs eigen3 yaml-cpp)
diff --git a/src/feature.cpp b/src/feature.cpp
index ffce145..9dce1d1 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -353,7 +353,7 @@ Feature::Fy ( const Vector3d &v, const Vector3d &w, double dt )
     return F;
 }		/* -----  end of method Feature::Fy  ----- */
 
-Matrix<double,6,9>
+Matrix<double,Dynamic,9>
 Feature::Hx ( const Vector3d &pos, const Quaterniond &q )
 {
     double xbw1,xbw2,xbw3;
@@ -385,8 +385,8 @@ Feature::Hx ( const Vector3d &pos, const Quaterniond &q )
     qb0w3=q0.z();
     qb0w4=q0.w();
 
-    Matrix<double,6,9> H;
-    H = Matrix<double,6,9>::Zero();
+    Matrix<double,Dynamic,9> H;
+    H = Matrix<double,Dynamic,9>::Zero(6,9);
     hx11 = -(2 * (qb0w1)*(qb0w2)-2 * (qb0w3)*(qb0w4)) / ((2 * (qb0w1)*(qb0w2)+2 * (qb0w3)*(qb0w4))*(xb0w2 - xbw2) + (2 * (qb0w1)*(qb0w3)-2 * (qb0w2)*(qb0w4))*(xb0w3 - xbw3) - ((pow(qbw1 , 2) - pow(qbw2 , 2) - pow(qbw3 , 2) + pow(qbw4 , 2))*(pow((qb0w1) , 2) - pow((qb0w2) , 2) - pow((qb0w3) , 2) + pow((qb0w4) , 2)) + (2 * (qb0w1)*(qb0w2)+2 * (qb0w3)*(qb0w4))*(2 * qbw1*qbw2 + 2 * qbw3*qbw4) + (2 * (qb0w1)*(qb0w3)-2 * (qb0w2)*(qb0w4))*(2 * qbw1*qbw3 - 2 * qbw2*qbw4)) / pib3 + (xb0w1 - xbw1)*(pow((qb0w1) , 2) - pow((qb0w2) , 2) - pow((qb0w3) , 2) + pow((qb0w4) , 2)) - (pib1*((2 * qbw1*qbw2 - 2 * qbw3*qbw4)*(pow((qb0w1) , 2) - pow((qb0w2) , 2) - pow((qb0w3) , 2) + pow((qb0w4) , 2)) - (2 * (qb0w1)*(qb0w2)+2 * (qb0w3)*(qb0w4))*(pow(qbw1 , 2) - pow(qbw2 , 2) + pow(qbw3 , 2) - pow(qbw4 , 2)) + (2 * (qb0w1)*(qb0w3)-2 * (qb0w2)*(qb0w4))*(2 * qbw1*qbw4 + 2 * qbw2*qbw3))) / pib3 + (pib2*((2 * (qb0w1)*(qb0w3)-2 * (qb0w2)*(qb0w4))*(pow(qbw1 , 2) + pow(qbw2 , 2) - pow(qbw3 , 2) - pow(qbw4 , 2)) - (2 * qbw1*qbw3 + 2 * qbw2*qbw4)*(pow((qb0w1) , 2) - pow((qb0w2) , 2) - pow((qb0w3) , 2) + pow((qb0w4) , 2)) + (2 * (qb0w1)*(qb0w2)+2 * (qb0w3)*(qb0w4))*(2 * qbw1*qbw4 - 2 * qbw2*qbw3))) / pib3) - ((pow((qb0w1) , 2) - pow((qb0w2) , 2) - pow((qb0w3) , 2) + pow((qb0w4) , 2))*(((2 * (qb0w1)*(qb0w2)-2 * (qb0w3)*(qb0w4))*(pow(qbw1 , 2) - pow(qbw2 , 2) - pow(qbw3 , 2) + pow(qbw4 , 2)) - (2 * qbw1*qbw2 + 2 * qbw3*qbw4)*(pow((qb0w1) , 2) - pow((qb0w2) , 2) + pow((qb0w3) , 2) - pow((qb0w4) , 2)) + (2 * (qb0w1)*(qb0w4)+2 * (qb0w2)*(qb0w3))*(2 * qbw1*qbw3 - 2 * qbw2*qbw4)) / pib3 - (2 * (qb0w1)*(qb0w4)+2 * (qb0w2)*(qb0w3))*(xb0w3 - xbw3) - (2 * (qb0w1)*(qb0w2)-2 * (qb0w3)*(qb0w4))*(xb0w1 - xbw1) + (xb0w2 - xbw2)*(pow((qb0w1) , 2) - pow((qb0w2) , 2) + pow((qb0w3) , 2) - pow((qb0w4) , 2)) + (pib1*((pow(qbw1 , 2) - pow(qbw2 , 2) + pow(qbw3 , 2) - pow(qbw4 , 2))*(pow((qb0w1) , 2) - pow((qb0w2) , 2) + pow((qb0w3) , 2) - pow((qb0w4) , 2)) + (2 * (qb0w1)*(qb0w2)-2 * (qb0w3)*(qb0w4))*(2 * qbw1*qbw2 - 2 * qbw3*qbw4) + (2 * (qb0w1)*(qb0w4)+2 * (qb0w2)*(qb0w3))*(2 * qbw1*qbw4 + 2 * qbw2*qbw3))) / pib3 + (pib2*((2 * qbw1*qbw4 - 2 * qbw2*qbw3)*(pow((qb0w1) , 2) - pow((qb0w2) , 2) + pow((qb0w3) , 2) - pow((qb0w4) , 2)) - (2 * (qb0w1)*(qb0w4)+2 * (qb0w2)*(qb0w3))*(pow(qbw1 , 2) + pow(qbw2 , 2) - pow(qbw3 , 2) - pow(qbw4 , 2)) + (2 * (qb0w1)*(qb0w2)-2 * (qb0w3)*(qb0w4))*(2 * qbw1*qbw3 + 2 * qbw2*qbw4))) / pib3)) / pow(((2 * (qb0w1)*(qb0w2)+2 * (qb0w3)*(qb0w4))*(xb0w2 - xbw2) + (2 * (qb0w1)*(qb0w3)-2 * (qb0w2)*(qb0w4))*(xb0w3 - xbw3) - ((pow(qbw1 , 2) - pow(qbw2 , 2) - pow(qbw3 , 2) + pow(qbw4 , 2))*(pow((qb0w1) , 2) - pow((qb0w2) , 2) - pow((qb0w3) , 2) + pow((qb0w4) , 2)) + (2 * (qb0w1)*(qb0w2)+2 * (qb0w3)*(qb0w4))*(2 * qbw1*qbw2 + 2 * qbw3*qbw4) + (2 * (qb0w1)*(qb0w3)-2 * (qb0w2)*(qb0w4))*(2 * qbw1*qbw3 - 2 * qbw2*qbw4)) / pib3 + (xb0w1 - xbw1)*(pow((qb0w1) , 2) - pow((qb0w2) , 2) - pow((qb0w3) , 2) + pow((qb0w4) , 2)) - (pib1*((2 * qbw1*qbw2 - 2 * qbw3*qbw4)*(pow((qb0w1) , 2) - pow((qb0w2) , 2) - pow((qb0w3) , 2) + pow((qb0w4) , 2)) - (2 * (qb0w1)*(qb0w2)+2 * (qb0w3)*(qb0w4))*(pow(qbw1 , 2) - pow(qbw2 , 2) + pow(qbw3 , 2) - pow(qbw4 , 2)) + (2 * (qb0w1)*(qb0w3)-2 * (qb0w2)*(qb0w4))*(2 * qbw1*qbw4 + 2 * qbw2*qbw3))) / pib3 + (pib2*((2 * (qb0w1)*(qb0w3)-2 * (qb0w2)*(qb0w4))*(pow(qbw1 , 2) + pow(qbw2 , 2) - pow(qbw3 , 2) - pow(qbw4 , 2)) - (2 * qbw1*qbw3 + 2 * qbw2*qbw4)*(pow((qb0w1) , 2) - pow((qb0w2) , 2) - pow((qb0w3) , 2) + pow((qb0w4) , 2)) + (2 * (qb0w1)*(qb0w2)+2 * (qb0w3)*(qb0w4))*(2 * qbw1*qbw4 - 2 * qbw2*qbw3))) / pib3) , 2);
 
     hx12 = (pow((qb0w1) , 2) - pow((qb0w2) , 2) + pow((qb0w3) , 2) - pow((qb0w4) , 2)) / ((2 * (qb0w1)*(qb0w2)+2 * (qb0w3)*(qb0w4))*(xb0w2 - xbw2) + (2 * (qb0w1)*(qb0w3)-2 * (qb0w2)*(qb0w4))*(xb0w3 - xbw3) - ((pow(qbw1 , 2) - pow(qbw2 , 2) - pow(qbw3 , 2) + pow(qbw4 , 2))*(pow((qb0w1) , 2) - pow((qb0w2) , 2) - pow((qb0w3) , 2) + pow((qb0w4) , 2)) + (2 * (qb0w1)*(qb0w2)+2 * (qb0w3)*(qb0w4))*(2 * qbw1*qbw2 + 2 * qbw3*qbw4) + (2 * (qb0w1)*(qb0w3)-2 * (qb0w2)*(qb0w4))*(2 * qbw1*qbw3 - 2 * qbw2*qbw4)) / pib3 + (xb0w1 - xbw1)*(pow((qb0w1) , 2) - pow((qb0w2) , 2) - pow((qb0w3) , 2) + pow((qb0w4) , 2)) - (pib1*((2 * qbw1*qbw2 - 2 * qbw3*qbw4)*(pow((qb0w1) , 2) - pow((qb0w2) , 2) - pow((qb0w3) , 2) + pow((qb0w4) , 2)) - (2 * (qb0w1)*(qb0w2)+2 * (qb0w3)*(qb0w4))*(pow(qbw1 , 2) - pow(qbw2 , 2) + pow(qbw3 , 2) - pow(qbw4 , 2)) + (2 * (qb0w1)*(qb0w3)-2 * (qb0w2)*(qb0w4))*(2 * qbw1*qbw4 + 2 * qbw2*qbw3))) / pib3 + (pib2*((2 * (qb0w1)*(qb0w3)-2 * (qb0w2)*(qb0w4))*(pow(qbw1 , 2) + pow(qbw2 , 2) - pow(qbw3 , 2) - pow(qbw4 , 2)) - (2 * qbw1*qbw3 + 2 * qbw2*qbw4)*(pow((qb0w1) , 2) - pow((qb0w2) , 2) - pow((qb0w3) , 2) + pow((qb0w4) , 2)) + (2 * (qb0w1)*(qb0w2)+2 * (qb0w3)*(qb0w4))*(2 * qbw1*qbw4 - 2 * qbw2*qbw3))) / pib3) - ((2 * (qb0w1)*(qb0w2)+2 * (qb0w3)*(qb0w4))*(((2 * (qb0w1)*(qb0w2)-2 * (qb0w3)*(qb0w4))*(pow(qbw1 , 2) - pow(qbw2 , 2) - pow(qbw3 , 2) + pow(qbw4 , 2)) - (2 * qbw1*qbw2 + 2 * qbw3*qbw4)*(pow((qb0w1) , 2) - pow((qb0w2) , 2) + pow((qb0w3) , 2) - pow((qb0w4) , 2)) + (2 * (qb0w1)*(qb0w4)+2 * (qb0w2)*(qb0w3))*(2 * qbw1*qbw3 - 2 * qbw2*qbw4)) / pib3 - (2 * (qb0w1)*(qb0w4)+2 * (qb0w2)*(qb0w3))*(xb0w3 - xbw3) - (2 * (qb0w1)*(qb0w2)-2 * (qb0w3)*(qb0w4))*(xb0w1 - xbw1) + (xb0w2 - xbw2)*(pow((qb0w1) , 2) - pow((qb0w2) , 2) + pow((qb0w3) , 2) - pow((qb0w4) , 2)) + (pib1*((pow(qbw1 , 2) - pow(qbw2 , 2) + pow(qbw3 , 2) - pow(qbw4 , 2))*(pow((qb0w1) , 2) - pow((qb0w2) , 2) + pow((qb0w3) , 2) - pow((qb0w4) , 2)) + (2 * (qb0w1)*(qb0w2)-2 * (qb0w3)*(qb0w4))*(2 * qbw1*qbw2 - 2 * qbw3*qbw4) + (2 * (qb0w1)*(qb0w4)+2 * (qb0w2)*(qb0w3))*(2 * qbw1*qbw4 + 2 * qbw2*qbw3))) / pib3 + (pib2*((2 * qbw1*qbw4 - 2 * qbw2*qbw3)*(pow((qb0w1) , 2) - pow((qb0w2) , 2) + pow((qb0w3) , 2) - pow((qb0w4) , 2)) - (2 * (qb0w1)*(qb0w4)+2 * (qb0w2)*(qb0w3))*(pow(qbw1 , 2) + pow(qbw2 , 2) - pow(qbw3 , 2) - pow(qbw4 , 2)) + (2 * (qb0w1)*(qb0w2)-2 * (qb0w3)*(qb0w4))*(2 * qbw1*qbw3 + 2 * qbw2*qbw4))) / pib3)) / pow(((2 * (qb0w1)*(qb0w2)+2 * (qb0w3)*(qb0w4))*(xb0w2 - xbw2) + (2 * (qb0w1)*(qb0w3)-2 * (qb0w2)*(qb0w4))*(xb0w3 - xbw3) - ((pow(qbw1 , 2) - pow(qbw2 , 2) - pow(qbw3 , 2) + pow(qbw4 , 2))*(pow((qb0w1) , 2) - pow((qb0w2) , 2) - pow((qb0w3) , 2) + pow((qb0w4) , 2)) + (2 * (qb0w1)*(qb0w2)+2 * (qb0w3)*(qb0w4))*(2 * qbw1*qbw2 + 2 * qbw3*qbw4) + (2 * (qb0w1)*(qb0w3)-2 * (qb0w2)*(qb0w4))*(2 * qbw1*qbw3 - 2 * qbw2*qbw4)) / pib3 + (xb0w1 - xbw1)*(pow((qb0w1) , 2) - pow((qb0w2) , 2) - pow((qb0w3) , 2) + pow((qb0w4) , 2)) - (pib1*((2 * qbw1*qbw2 - 2 * qbw3*qbw4)*(pow((qb0w1) , 2) - pow((qb0w2) , 2) - pow((qb0w3) , 2) + pow((qb0w4) , 2)) - (2 * (qb0w1)*(qb0w2)+2 * (qb0w3)*(qb0w4))*(pow(qbw1 , 2) - pow(qbw2 , 2) + pow(qbw3 , 2) - pow(qbw4 , 2)) + (2 * (qb0w1)*(qb0w3)-2 * (qb0w2)*(qb0w4))*(2 * qbw1*qbw4 + 2 * qbw2*qbw3))) / pib3 + (pib2*((2 * (qb0w1)*(qb0w3)-2 * (qb0w2)*(qb0w4))*(pow(qbw1 , 2) + pow(qbw2 , 2) - pow(qbw3 , 2) - pow(qbw4 , 2)) - (2 * qbw1*qbw3 + 2 * qbw2*qbw4)*(pow((qb0w1) , 2) - pow((qb0w2) , 2) - pow((qb0w3) , 2) + pow((qb0w4) , 2)) + (2 * (qb0w1)*(qb0w2)+2 * (qb0w3)*(qb0w4))*(2 * qbw1*qbw4 - 2 * qbw2*qbw3))) / pib3) , 2);
diff --git a/src/feature.h b/src/feature.h
index c8b835e..a8af84e 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -4,11 +4,12 @@
 #include <iostream>
 #include "types.h"
 
-using Eigen::Vector2d;
-using Eigen::Vector3d;
+using Eigen::Dynamic;
 using Eigen::Matrix;
 using Eigen::MatrixXd;
 using Eigen::Quaterniond;
+using Eigen::Vector2d;
+using Eigen::Vector3d;
 using std::cout;
 using std::cerr;
 using std::endl;
@@ -43,7 +44,7 @@ class Feature
                 const Vector3d &xr);
         Matrix<double,3,9> Fx( double dt );
         Matrix<double,3,3> Fy( const Vector3d &vel, const Vector3d &ang, double dt);
-        Matrix<double,6,9> Hx( const Vector3d &pos, const Quaterniond &q);
+        Matrix<double,Dynamic,9> Hx( const Vector3d &pos, const Quaterniond &q);
         Matrix<double,6,3> Hy( const Vector3d &pos, const Quaterniond &q);
         Matrix<double,6,1> h( const Vector3d &x, const Quaterniond &q);
         Matrix<double,3,6> L();
diff --git a/src/main.cpp b/src/main.cpp
index 62d2560..f4594fc 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -36,10 +36,6 @@ aboveWater(const Quaterniond &q)
     return tip[2];
 }
 
-#ifdef  USE_ROS
-
-#else      /* -----  not USE_ROS  ----- */
-
 /* 
  * ===  FUNCTION  ======================================================================
  *         Name:  covCallback
@@ -299,7 +295,7 @@ parseLine(char *line, message *data)
         exit(1);
     }
 }
-#endif     /* -----  not USE_ROS  ----- */
+
 /* 
  * ===  FUNCTION  ======================================================================
  *         Name:  update_dt
@@ -345,45 +341,55 @@ main(int argc, char **argv)
     // Open camera YAML
     Camera cam(argv[1]);
 
-#ifdef  USE_ROS
-    
-#else      /* -----  not USE_ROS  ----- */
-printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Bias-X,Bias-Y,Bias-Z\n");
-// Read sensors from file
-char line[MAXLINE];
-while (scanf("%s", line)!=EOF) {
-    message msg;
-    parseLine(line, &msg);
-    
-    switch ( msg.msg_type ) {
-        case BESTUTM:	
-            utmCallback(msg, mu,qbw);
-            break;
+#ifdef  ROS_PUBLISH
+    // Create ROS publisher
+    ros::init(argc, argv, "refslam");
+    ros::NodeHandle n;
+    ros::Publisher ekfPose = n.advertise<geometry_msgs::PoseStamped>("ekfPose", 100);
+#else      /* -----  not ROS_PUBLISH  ----- */
+    // Print unicsv header
+    printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Bias-X,Bias-Y,Bias-Z\n");
+#endif     /* -----  not ROS_PUBLISH  ----- */
+    // Read sensors from file
+    char line[MAXLINE];
+    while (scanf("%s", line)!=EOF) {
+        message msg;
+        parseLine(line, &msg);
+        
+        switch ( msg.msg_type ) {
+            case BESTUTM:	
+                utmCallback(msg, mu,qbw);
+                break;
 
-        case IMG:	
-            imgCallback(msg, mu, cam, qbw);
-            break;
+            case IMG:	
+                imgCallback(msg, mu, cam, qbw);
+                break;
 
-        case INSPVAS:	
-            pvaCallback(msg, mu, qbw);
-            break;
+            case INSPVAS:	
+                pvaCallback(msg, mu, qbw);
+                break;
 
-        case RAWIMUS:	
-            dt = update_dt(msg.stamp, &t_old);
-            imuCallback(msg, mu, qbw, dt);
-            if (seenutm && seencov && seenpva)
-                mu.unicsv();
-            break;
+            case RAWIMUS:	
+                dt = update_dt(msg.stamp, &t_old);
+                imuCallback(msg, mu, qbw, dt);
+                if (seenutm && seencov && seenpva) {
+#ifdef  ROS_PUBLISH
+                    geometry_msgs::PoseStamped msg;
+                    ros::spinOnce();
+#else
+                    mu.unicsv();
+#endif
+                }
+                break;
 
-        case INSCOVS:
-            covCallback(msg, mu, qbw);
-            break;
+            case INSCOVS:
+                covCallback(msg, mu, qbw);
+                break;
 
-        default:	
-            break;
-    }				/* -----  end switch  ----- */
-}
-#endif     /* -----  not USE_ROS  ----- */
+            default:	
+                break;
+        }				/* -----  end switch  ----- */
+    }
 	return 0;
 }
 
diff --git a/src/main.h b/src/main.h
index aeb4949..3c9b82d 100644
--- a/src/main.h
+++ b/src/main.h
@@ -15,6 +15,13 @@
 #include "state.h"
 #include "types.h"
 
+//#define ROS_PUBLISH          /* Uncomment to publish ROS node */
+//#define ROS_SUBSCRIBE            /* Uncomment to subscribe to ROS */
+
+#if  ROS_PUBLISH
+#include <ros/ros.h>
+#include <ros/geometry_msgs.h>
+#endif     /* -----  ROS_PUBLISH  ----- */
 
 using Eigen::Matrix;
 using Eigen::Matrix3d;
diff --git a/src/state.cpp b/src/state.cpp
index aaf9cb9..8ad1d9c 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -61,6 +61,7 @@ State::H ( const Quaterniond &q, const std::vector<measurement_t> &z )
     for (int i=0,row=0; i<z.size(); ++i) {
         switch ( z[i].z_type ) {
             int col;
+            Feature *fi;
             case REFLECTION:	
                 col = rowById(z[i].id);
                 if (col==-1) {
@@ -68,7 +69,6 @@ State::H ( const Quaterniond &q, const std::vector<measurement_t> &z )
                     exit(1);
                 }
 
-                Feature *fi;
                 fi = featureById(z[i].id);
 
                 h.block<6,9>(row,0) = fi->Hx(pos,q);
@@ -77,8 +77,13 @@ State::H ( const Quaterniond &q, const std::vector<measurement_t> &z )
                 break;
 
             case MONO:	
-                fprintf(stderr, "mono points not supported.\n");
-                exit(1);
+                col = rowById(z[i].id);
+                if (col==-1) {
+                    fprintf(stderr, "Feature %d not found, quitting.\n", z[i].id);
+                    exit(1);
+                }
+                fi = featureById( z[i].id );
+                h.block<4,9>(row,0) = fi->Hx(pos,q);
                 break;
 
             case HEIGHT:	
diff --git a/src/state.h b/src/state.h
index 89676f3..0e1fe90 100644
--- a/src/state.h
+++ b/src/state.h
@@ -71,7 +71,7 @@ class State
 
     private:
         /* ====================  METHODS       ======================================= */
-        void addFeatures(std::vector<measurement_t> &F, const Quaterniond &q);
+        void addFeatures(std::vector<measurement_t> &F, const Quaterniond &q, double z);
         void expandP ( const Matrix3d &Pi);
         void shrinkP( int i );