summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--Makefile8
-rw-r--r--src/camera.h4
-rw-r--r--src/feature.cpp8
-rw-r--r--src/feature.h1
-rw-r--r--src/main.cpp6
-rw-r--r--src/main.h21
-rw-r--r--src/state.h3
-rw-r--r--tests/Makefile2
-rw-r--r--tests/test_feature.cpp67
9 files changed, 93 insertions, 27 deletions
diff --git a/Makefile b/Makefile
index af2e0ee..e6b364e 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/filter.o
SRCS=$(patsubst %.o,%.cpp,$(OBJECT))
-CXXFLAGS+=-O3 -march=native -std=c++11
-CXXFLAGS+=-g
+CXXFLAGS+=-O2 -march=native -std=c++11 -pipe -msse3
+#CXXFLAGS+=-g
CXXFLAGS+=$(shell pkg-config --cflags eigen3 yaml-cpp)
#CXXFLAGS+=$(shell pkg-config --cflags opencv)
LIBS+=$(shell pkg-config --libs eigen3 yaml-cpp)
@@ -9,12 +9,12 @@ LIBS+=$(shell pkg-config --libs eigen3 yaml-cpp)
slam: ${OBJECT}
- $(CXX) -o $@ $^ $(CXXFLAGS) ${LIBS}
+ $(CXX) -o $@ $^ $(CXXFLAGS) $(CPPFLAGS) ${LIBS}
#.cpp.o:
%.o: %.cpp %.h
- $(CXX) $(CXXFLAGS) ${LIBS} -c $< -o $@
+ $(CXX) $(CXXFLAGS) $(CPPFLAGS) ${LIBS} -c $< -o $@
.PHONY: clean
clean:
diff --git a/src/camera.h b/src/camera.h
index a08eb0c..ea006ec 100644
--- a/src/camera.h
+++ b/src/camera.h
@@ -5,8 +5,8 @@
#include <yaml-cpp/yaml.h>
#define BINNING 0.5 // set the binning factor
-#define YAWCORRECT 7.5
-//#define DOYAWCORRECT
+#define YAWCORRECT 7.0
+#define DOYAWCORRECT
using Eigen::Matrix;
using Eigen::Vector4d;
using Eigen::Vector3d;
diff --git a/src/feature.cpp b/src/feature.cpp
index caa56f0..a75fdf0 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -54,6 +54,8 @@ Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xbw,
pib[1] = xib[2];
pib[2] = RHO_0;
+ X = pib;
+
X0 = X;
xb0w = xbw;
q0 = q;
@@ -531,11 +533,11 @@ Feature::P0 ( measurement_type t )
double p0 = FEATURECOVX;
double p1 = FEATURECOVY;
double p2;
- if (t==REFLECTION) {
+#ifdef INITDEPTH
p2 = FEATURECOVRHO;
- } else {
+#else
p2 = FEATURECOVRHO_MONO;
- }
+#endif
P << p0, 0., 0.,
0., p1, 0.,
0., 0., p2;
diff --git a/src/feature.h b/src/feature.h
index 2754f89..db4f05e 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -16,6 +16,7 @@
#define INLIER_THRESHOLD 5.9915 /* */
#define RANSAC_LI_THRESHOLD 4e-5 /* */
#define RANSAC_HI_THRESHOLD 5e-2 /* */
+#define INITDEPTH
using Eigen::Dynamic;
diff --git a/src/main.cpp b/src/main.cpp
index c7ed177..914aa26 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -357,7 +357,7 @@ main(int argc, char **argv)
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,Nfeats\n");
+ printf("GPS,UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Bias-X,Bias-Y,Bias-Z,Nfeats\n");
#endif /* ----- not ROS_PUBLISH ----- */
// Read sensors from file
int i=0;
@@ -369,6 +369,8 @@ main(int argc, char **argv)
switch ( msg.msg_type ) {
case BESTUTM:
utmCallback(msg, mu,qbw);
+ printf("%f,", msg.stamp.secs+msg.stamp.nsecs*1e-9);
+ mu.unicsv();
break;
case IMG:
@@ -388,7 +390,7 @@ main(int argc, char **argv)
geometry_msgs::PoseStamped msg;
ros::spinOnce();
#else
- mu.unicsv();
+ //mu.unicsv();
#endif
}
}
diff --git a/src/main.h b/src/main.h
index 746e187..6e11875 100644
--- a/src/main.h
+++ b/src/main.h
@@ -22,23 +22,24 @@
#define ANGBIASY 6.984255690000021506e-03
#define ANGBIASZ -1.418145565750002614e-03
-#define ACCBIASX 0.95*-0.03713532
-#define ACCBIASY 0.65*0.01465135
-#define ACCBIASZ -1*-0.00709229
+//#define ACCBIASX 0.95*-0.03713532
+//#define ACCBIASY 0.65*0.01465135
+//#define ACCBIASZ -1*-0.00709229
//#define ACCBIASX 0.99*-0.03713532
//#define ACCBIASY 0.2*0.01465135
//#define ACCBIASZ -1*-0.00709229
-//#define ACCBIASX -0.03713532
-//#define ACCBIASY 0.01465135
-//#define ACCBIASZ -1*-0.00709229
+#define ACCBIASX -0.03713532
+#define ACCBIASY 0.01465135
+#define ACCBIASZ -1*-0.00709229
-#define CANOECENTER 0.65 /* center of gravity of canoe */
-#define CANOEHEIGHT -0.55
+#define CANOECENTER 0.88 /* center of gravity of canoe */
+#define CANOEHEIGHT -0.50
+//#define CANOEHEIGHT -0.75
#define DOWNSAMPLE 1 /* */
-//#define HEIGHT_FROM_ATTITUDE /* use the attitude to measure the height */
-//#define MEASURE_HEIGHT
+#define HEIGHT_FROM_ATTITUDE /* use the attitude to measure the height */
+#define MEASURE_HEIGHT
#if ROS_PUBLISH
#include <ros/ros.h>
diff --git a/src/state.h b/src/state.h
index 19e807b..a7f7df5 100644
--- a/src/state.h
+++ b/src/state.h
@@ -13,8 +13,7 @@
#define MAXFEATURES 50
#define COVBIAS 2e-5
//#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once
-//#define DORANSAC /* */
-//#define INITDEPTH /* */
+#define DORANSAC /* */
//#define INLIERTEST /* */
using Eigen::Dynamic;
diff --git a/tests/Makefile b/tests/Makefile
index 06e42ae..d27c965 100644
--- a/tests/Makefile
+++ b/tests/Makefile
@@ -1,4 +1,4 @@
-OBJECT=test_feature.o ../src/feature.o ../src/camera.o
+OBJECT=test_feature.o ../src/feature.o ../src/camera.o ../src/camera.h
CXXFLAGS+=-O2 -march=native -std=c++11 -pedantic-errors
CXXFLAGS+=-g -I../src
CXXFLAGS+=$(shell pkg-config --cflags eigen3 yaml-cpp)
diff --git a/tests/test_feature.cpp b/tests/test_feature.cpp
index 13b4e0a..4e6acc2 100644
--- a/tests/test_feature.cpp
+++ b/tests/test_feature.cpp
@@ -31,14 +31,24 @@ test_findDepth ( Camera &cam )
{
bool success;
UTM pos;
+ /* 16.bmp
pos.northing = 4451490.899494356;
pos.easting = 381887.311030777;
pos.up = 206.256890383;
+ */
+ // 9494.bmp
+ pos.northing = 4451693.139997130;
+ pos.easting = 382292.412884794;
+ pos.up = 203.230833591;
pos.zone_i = 16;
pos.zone_c = 'T';
+ /*
// Quat for 16.bmp
attitude_t att{ -1.073641623878,2.685395530695,16.357370508605};
+ */
+ // Quat for 9494.bmp
+ attitude_t att{ 1.669725078950,4.920310214269,49.251704514034};
double roll, pitch, yaw;
roll = M_PI*att.roll/180.;
@@ -51,7 +61,56 @@ test_findDepth ( Camera &cam )
* AngleAxisd(pitch, Vector3d::UnitY());
q = Quaterniond(R);
- Matrix<double,26,5> meas;
+ Matrix<double,48,5> meas;
+ meas << 94,201,258,192,509,
+ 184,210,265,200,480,
+ 588,297,265,296,386,
+ 614,483,267,487,316,
+ 810,352,251,355,415,
+ 996,147,267,142,384,
+ 1062,182,250,179,510,
+ 1070,82,246,73,416,
+ 1072,280,278,282,373,
+ 1146,274,257,272,402,
+ 1152,156,282,149,382,
+ 1274,249,263,246,393,
+ 1370,134,253,123,419,
+ 1432,238,265,242,381,
+ 1474,235,239,242,431,
+ 1520,270,276,271,368,
+ 1588,479,217,463,444,
+ 1640,249,273,252,363,
+ 1692,158,236,142,463,
+ 1724,271,255,272,387,
+ 1784,187,260,185,383,
+ 1792,335,266,338,348,
+ 1812,111,242,96,422,
+ 1820,59,226,49,437,
+ 1826,193,208,193,507,
+ 1834,160,245,154,412,
+ 1856,184,169,191,502,
+ 1862,288,219,293,422,
+ 1870,220,235,216,432,
+ 1872,309,283,309,348,
+ 1938,89,237,81,414,
+ 1976,161,189,168,496,
+ 1984,192,187,196,506,
+ 2002,156,226,153,421,
+ 2004,273,208,273,471,
+ 2008,63,267,59,390,
+ 2014,306,267,308,363,
+ 2034,205,235,201,400,
+ 2084,149,167,148,518,
+ 2114,243,295,239,330,
+ 2120,143,223,141,417,
+ 2134,176,188,163,470,
+ 2142,194,229,189,400,
+ 2168,55,237,47,399,
+ 2192,110,94,115,491,
+ 2194,221,268,220,353,
+ 2200,197,118,201,506,
+ 2220,212,245,212,381;
+/*
meas << 162,493,260,493,331,
66,370,229,369,352,
268,763,235,764,364,
@@ -78,10 +137,11 @@ test_findDepth ( Camera &cam )
1262,547,219,546,360,
1274,314,218,313,354,
1278,469,217,469,365;
+ */
double z;
- z = -0.34;
- z -= 0.6*3.43*sin(pitch);
+ z = -0.55;
+ z -= 0.65*3.43*sin(pitch);
Vector3d xbw;
xbw << pos.northing, pos.easting, -pos.up;
@@ -100,6 +160,7 @@ test_findDepth ( Camera &cam )
xr = cam.img2body(xri);
Feature f(id,xs,xr,xbw,q,z);
+ //Feature f(id,xs,xbw,q);
if (f.sane())
{
UTM utmi;