summaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/feature.cpp6
-rw-r--r--src/feature.h7
-rw-r--r--src/main.cpp84
-rw-r--r--src/main.h7
-rw-r--r--src/state.cpp11
-rw-r--r--src/state.h2
6 files changed, 68 insertions, 49 deletions
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 );