summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/body.cpp2
-rw-r--r--src/feature.cpp158
-rw-r--r--src/feature.h26
-rw-r--r--src/main.cpp19
-rw-r--r--src/state.cpp45
5 files changed, 178 insertions, 72 deletions
diff --git a/src/body.cpp b/src/body.cpp
index 7ded581..691a209 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -216,7 +216,7 @@ Body::skewSymmetric ( const Vector3d &x )
void
Body::unicsv ( )
{
- printf("%d,%c,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", utm_i, utm_c,
+ printf("%d,%c,%f,%f,%f,%f,%f,%f,%f,%f,%f", utm_i, utm_c,
X[0], X[1], -X[2],
X[3], X[4], X[5],
X[6], X[7], X[8]);
diff --git a/src/feature.cpp b/src/feature.cpp
index fc08ee7..c8aa888 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -22,7 +22,7 @@
*--------------------------------------------------------------------------------------
* Class: Feature
* Method: Feature
- * Description: constructor
+ * Description: constructor for reflection type features
*--------------------------------------------------------------------------------------
*/
Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xr,
@@ -32,18 +32,34 @@ Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xr,
Vector3d xib;
xib = findDepth(q,z,xs,xr);
X = x2p(xib);
- /*
- Vector3d pib;
+ X0 = X;
+ xb0w = xbw;
+ q0 = q;
+ fType = REFLECTION;
+} /* ----- end of method Feature::Feature (constructor) ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ * Class: Feature
+ * Method: Feature :: Feature
+ * Description: Constructor for mono type features
+ *--------------------------------------------------------------------------------------
+ */
+Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xbw,
+ const Quaterniond &q)
+{
+ _id = id;
+ Vector3d xib, pib;
xib = xs/xs[0];
pib[0] = xib[1];
pib[1] = xib[2];
- pib[2] = .1;
- X = pib;
- */
+ pib[2] = RHO_0;
+
X0 = X;
xb0w = xbw;
q0 = q;
-} /* ----- end of method Feature::Feature (constructor) ----- */
+ fType = MONO;
+}
/*
@@ -113,15 +129,15 @@ Feature::findDepth ( const Quaterniond &qbw, double z, const Vector3d &xs,
* with other measurements outside of the instance and compute S.
*--------------------------------------------------------------------------------------
*/
-Matrix<double,6,6>
+Matrix<double,Dynamic,Dynamic,0,6,6>
Feature::S ( const Matrix<double,9,9> &Pxx, const Matrix<double,9,3> &Pxy,
const Matrix<double,3,3> &Pyy, const Vector3d &pos, const Quaterniond &q)
{
- Matrix<double,6,9> hx;
- Matrix<double,6,3> hy;
+ Matrix<double,Dynamic,9,0,6,9> hx;
+ Matrix<double,Dynamic,3,0,6,3> hy;
hx = Hx( pos, q);
hy = Hy( pos, q);
- Matrix<double,6,6> S;
+ Matrix<double,Dynamic,Dynamic,0,6,6> S;
S = hx*Pxx*hx.transpose() + 2*hx*Pxy*hy.transpose() + hy*Pyy*hy.transpose() + R();
return S;
@@ -136,14 +152,21 @@ Feature::Q ( const double dt )
return Q;
} /* ----- end of method Feature::q ----- */
-Matrix<double,6,6>
+Matrix<double,Dynamic,Dynamic,0,6,6>
Feature::R ( )
{
- Matrix<double,6,6> R;
+ Matrix<double,Dynamic,Dynamic,0,6,6> R;
+ if (fType==REFLECTION) {
+ R = MatrixXd::Identity(6,6);
+ } else {
+ R = MatrixXd::Identity(4,4);
+ }
R = Matrix<double,6,6>::Identity();
R.block<2,2>(0,0) *= VIEW_NOISE;
R.block<2,2>(2,2) *= INITIAL_VIEW_NOISE;
- R.block<2,2>(4,4) *= REFLECTION_VIEW_NOISE;
+ if (fType==REFLECTION) {
+ R.block<2,2>(4,4) *= REFLECTION_VIEW_NOISE;
+ }
return R;
} /* ----- end of method Feature::R ----- */
@@ -200,10 +223,15 @@ Feature::motionModel ( const Vector3d &ang, const Vector3d &vel, const double d
* Description: Returns the predicted measurement vector.
*--------------------------------------------------------------------------------------
*/
-Matrix<double,6,1>
+Matrix<double,Dynamic,1,0,6,1>
Feature::h ( const Vector3d &x, const Quaterniond &q )
{
- Matrix<double,6,1> h;
+ Matrix<double,Dynamic,1,0,6,1> h;
+ if (fType==REFLECTION) {
+ h = MatrixXd::Identity(6,1);
+ } else {
+ h = MatrixXd::Identity(4,1);
+ }
Vector3d xib, xib0;
Vector3d pib0;
@@ -217,23 +245,26 @@ Feature::h ( const Vector3d &x, const Quaterniond &q )
xib0 = Rbb0*xib + Rbw.transpose()*(x-xb0w);
pib0 = x2p(xib0);
- // Predict reflection view
- Matrix<double,3,1> n;
- n << 0, 0, 1;
- Matrix3d S;
- S = Matrix3d::Identity() - 2*n*n.transpose();
- Vector3d xpb, ppb;
-
- xpb = Rbw.transpose()*(S*Rbw*xib-2*n*n.transpose()*x);
-
- ppb = x2p(xpb);
-
h[0] = X[0];
h[1] = X[1];
h[2] = pib0[0];
h[3] = pib0[1];
- h[4] = ppb[0];
- h[5] = ppb[1];
+
+ // Predict reflection view
+ if (fType==REFLECTION) {
+ Matrix<double,3,1> n;
+ n << 0, 0, 1;
+ Matrix3d S;
+ S = Matrix3d::Identity() - 2*n*n.transpose();
+ Vector3d xpb, ppb;
+
+ xpb = Rbw.transpose()*(S*Rbw*xib-2*n*n.transpose()*x);
+
+ ppb = x2p(xpb);
+
+ h[4] = ppb[0];
+ h[5] = ppb[1];
+ }
return h;
} /* ----- end of method Feature::h ----- */
@@ -246,19 +277,30 @@ Feature::h ( const Vector3d &x, const Quaterniond &q )
* coordinates.
*--------------------------------------------------------------------------------------
*/
-Matrix<double,6,1>
+Matrix<double,Dynamic,1,0,6,1>
Feature::measurement_vector ( const Vector3d &xs, const Vector3d &xr )
{
Matrix<double,6,1> z;
- Vector3d ps,p0,pr;
+ if (fType==REFLECTION) {
+ z = MatrixXd::Identity(6,1);
+ } else {
+ z = MatrixXd::Identity(4,1);
+ }
+
+ Vector3d ps,p0;
ps = x2p(xs);
- pr = x2p(xr);
z[0] = ps[0];
z[1] = ps[1];
z[2] = X0[0];
z[3] = X0[1];
- z[4] = pr[0];
- z[5] = pr[1];
+
+ if (fType==REFLECTION) {
+ Vector3d pr;
+ pr = x2p(xr);
+ z[4] = pr[0];
+ z[5] = pr[1];
+ }
+
return z;
} /* ----- end of method Feature::measurement_vector ----- */
@@ -355,7 +397,7 @@ Feature::Fy ( const Vector3d &v, const Vector3d &w, double dt )
return F;
} /* ----- end of method Feature::Fy ----- */
-Matrix<double,Dynamic,9>
+Matrix<double,Dynamic,9,0,6,9>
Feature::Hx ( const Vector3d &pos, const Quaterniond &q )
{
double xbw1,xbw2,xbw3;
@@ -387,8 +429,12 @@ Feature::Hx ( const Vector3d &pos, const Quaterniond &q )
qb0w3=q0.z();
qb0w4=q0.w();
- Matrix<double,Dynamic,9> H;
- H = Matrix<double,Dynamic,9>::Zero(6,9);
+ Matrix<double,Dynamic,9,0,6,9> H;
+ if (fType==REFLECTION) {
+ H = Matrix<double,Dynamic,9>::Zero(6,9);
+ } else {
+ H = Matrix<double,Dynamic,9>::Zero(4,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);
@@ -405,11 +451,15 @@ Feature::Hx ( const Vector3d &pos, const Quaterniond &q )
hx43 = ((4 * qbw1*qbw3 - 4 * qbw2*qbw4)*((2 * qbw1*qbw3 + 2 * qbw2*qbw4)*((pow(qbw1 , 2) - pow(qbw2 , 2) - pow(qbw3 , 2) + pow(qbw4 , 2)) / pib3 + (pib1*(2 * qbw1*qbw2 - 2 * qbw3*qbw4)) / pib3 + (pib2*(2 * qbw1*qbw3 + 2 * qbw2*qbw4)) / pib3) + (2 * qbw1*qbw4 - 2 * qbw2*qbw3)*((pib1*(pow(qbw1 , 2) - pow(qbw2 , 2) + pow(qbw3 , 2) - pow(qbw4 , 2))) / pib3 - (2 * qbw1*qbw2 + 2 * qbw3*qbw4) / pib3 + (pib2*(2 * qbw1*qbw4 - 2 * qbw2*qbw3)) / pib3) + (pow(qbw1 , 2) + pow(qbw2 , 2) - pow(qbw3 , 2) - pow(qbw4 , 2))*(2 * xbw3 + (2 * qbw1*qbw3 - 2 * qbw2*qbw4) / pib3 - (pib2*(pow(qbw1 , 2) + pow(qbw2 , 2) - pow(qbw3 , 2) - pow(qbw4 , 2))) / pib3 + (pib1*(2 * qbw1*qbw4 + 2 * qbw2*qbw3)) / pib3))) / pow(((2 * qbw1*qbw3 - 2 * qbw2*qbw4)*(2 * xbw3 + (2 * qbw1*qbw3 - 2 * qbw2*qbw4) / pib3 - (pib2*(pow(qbw1 , 2) + pow(qbw2 , 2) - pow(qbw3 , 2) - pow(qbw4 , 2))) / pib3 + (pib1*(2 * qbw1*qbw4 + 2 * qbw2*qbw3)) / pib3) + (2 * qbw1*qbw2 + 2 * qbw3*qbw4)*((pib1*(pow(qbw1 , 2) - pow(qbw2 , 2) + pow(qbw3 , 2) - pow(qbw4 , 2))) / pib3 - (2 * qbw1*qbw2 + 2 * qbw3*qbw4) / pib3 + (pib2*(2 * qbw1*qbw4 - 2 * qbw2*qbw3)) / pib3) - ((pow(qbw1 , 2) - pow(qbw2 , 2) - pow(qbw3 , 2) + pow(qbw4 , 2)) / pib3 + (pib1*(2 * qbw1*qbw2 - 2 * qbw3*qbw4)) / pib3 + (pib2*(2 * qbw1*qbw3 + 2 * qbw2*qbw4)) / pib3)*(pow(qbw1 , 2) - pow(qbw2 , 2) - pow(qbw3 , 2) + pow(qbw4 , 2))) , 2) - (2 * pow(qbw1 , 2) + 2 * pow(qbw2 , 2) - 2 * pow(qbw3 , 2) - 2 * pow(qbw4 , 2)) / ((2 * qbw1*qbw3 - 2 * qbw2*qbw4)*(2 * xbw3 + (2 * qbw1*qbw3 - 2 * qbw2*qbw4) / pib3 - (pib2*(pow(qbw1 , 2) + pow(qbw2 , 2) - pow(qbw3 , 2) - pow(qbw4 , 2))) / pib3 + (pib1*(2 * qbw1*qbw4 + 2 * qbw2*qbw3)) / pib3) + (2 * qbw1*qbw2 + 2 * qbw3*qbw4)*((pib1*(pow(qbw1 , 2) - pow(qbw2 , 2) + pow(qbw3 , 2) - pow(qbw4 , 2))) / pib3 - (2 * qbw1*qbw2 + 2 * qbw3*qbw4) / pib3 + (pib2*(2 * qbw1*qbw4 - 2 * qbw2*qbw3)) / pib3) - ((pow(qbw1 , 2) - pow(qbw2 , 2) - pow(qbw3 , 2) + pow(qbw4 , 2)) / pib3 + (pib1*(2 * qbw1*qbw2 - 2 * qbw3*qbw4)) / pib3 + (pib2*(2 * qbw1*qbw3 + 2 * qbw2*qbw4)) / pib3)*(pow(qbw1 , 2) - pow(qbw2 , 2) - pow(qbw3 , 2) + pow(qbw4 , 2)));
- H.block<4,3>(2,0) << hx11, hx12, hx13, hx21, hx22, hx23,0,0,hx33,0,0,hx43;
+ if (fType==REFLECTION) {
+ H.block<4,3>(2,0) << hx11, hx12, hx13, hx21, hx22, hx23,0,0,hx33,0,0,hx43;
+ } else {
+ H.block<2,3>(2,0) << hx11, hx12, hx13, hx21, hx22, hx23;
+ }
return H;
} /* ----- end of method Feature::Hx ----- */
-Matrix<double,6,3>
+Matrix<double,Dynamic,3,0,6,3>
Feature::Hy ( const Vector3d &pos, const Quaterniond &q )
{
double xbw1,xbw2,xbw3;
@@ -441,8 +491,12 @@ Feature::Hy ( const Vector3d &pos, const Quaterniond &q )
qb0w3=q0.z();
qb0w4=q0.w();
- Matrix<double,6,3> H;
- H = Matrix<double,6,3>::Zero();
+ Matrix<double,Dynamic,3,0,6,3> H;
+ if (fType==REFLECTION) {
+ H = MatrixXd::Zero(6,3);
+ } else {
+ H = MatrixXd::Zero(4,3);
+ }
hy11 = -((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*((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 * 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))*(((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)) / (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));
@@ -468,12 +522,19 @@ Feature::Hy ( const Vector3d &pos, const Quaterniond &q )
hy43 = ((2 * qbw1*qbw3 + 2 * qbw2*qbw4)*((pow(qbw1, 2) - pow(qbw2, 2) - pow(qbw3, 2) + pow(qbw4, 2)) / pow(pib3, 2) + (pib1*(2 * qbw1*qbw2 - 2 * qbw3*qbw4)) / pow(pib3, 2) + (pib2*(2 * qbw1*qbw3 + 2 * qbw2*qbw4)) / pow(pib3, 2)) + (2 * qbw1*qbw4 - 2 * qbw2*qbw3)*((pib1*(pow(qbw1, 2) - pow(qbw2, 2) + pow(qbw3, 2) - pow(qbw4, 2))) / pow(pib3, 2) - (2 * qbw1*qbw2 + 2 * qbw3*qbw4) / pow(pib3, 2) + (pib2*(2 * qbw1*qbw4 - 2 * qbw2*qbw3)) / pow(pib3, 2)) + ((2 * qbw1*qbw3 - 2 * qbw2*qbw4) / pow(pib3, 2) - (pib2*(pow(qbw1, 2) + pow(qbw2, 2) - pow(qbw3, 2) - pow(qbw4, 2))) / pow(pib3, 2) + (pib1*(2 * qbw1*qbw4 + 2 * qbw2*qbw3)) / pow(pib3, 2))*(pow(qbw1, 2) + pow(qbw2, 2) - pow(qbw3, 2) - pow(qbw4, 2))) / ((2 * qbw1*qbw3 - 2 * qbw2*qbw4)*(2 * xbw3 + (2 * qbw1*qbw3 - 2 * qbw2*qbw4) / pib3 - (pib2*(pow(qbw1, 2) + pow(qbw2, 2) - pow(qbw3, 2) - pow(qbw4, 2))) / pib3 + (pib1*(2 * qbw1*qbw4 + 2 * qbw2*qbw3)) / pib3) + (2 * qbw1*qbw2 + 2 * qbw3*qbw4)*((pib1*(pow(qbw1, 2) - pow(qbw2, 2) + pow(qbw3, 2) - pow(qbw4, 2))) / pib3 - (2 * qbw1*qbw2 + 2 * qbw3*qbw4) / pib3 + (pib2*(2 * qbw1*qbw4 - 2 * qbw2*qbw3)) / pib3) - ((pow(qbw1, 2) - pow(qbw2, 2) - pow(qbw3, 2) + pow(qbw4, 2)) / pib3 + (pib1*(2 * qbw1*qbw2 - 2 * qbw3*qbw4)) / pib3 + (pib2*(2 * qbw1*qbw3 + 2 * qbw2*qbw4)) / pib3)*(pow(qbw1, 2) - pow(qbw2, 2) - pow(qbw3, 2) + pow(qbw4, 2))) - (((2 * qbw1*qbw3 + 2 * qbw2*qbw4)*((pow(qbw1, 2) - pow(qbw2, 2) - pow(qbw3, 2) + pow(qbw4, 2)) / pib3 + (pib1*(2 * qbw1*qbw2 - 2 * qbw3*qbw4)) / pib3 + (pib2*(2 * qbw1*qbw3 + 2 * qbw2*qbw4)) / pib3) + (2 * qbw1*qbw4 - 2 * qbw2*qbw3)*((pib1*(pow(qbw1, 2) - pow(qbw2, 2) + pow(qbw3, 2) - pow(qbw4, 2))) / pib3 - (2 * qbw1*qbw2 + 2 * qbw3*qbw4) / pib3 + (pib2*(2 * qbw1*qbw4 - 2 * qbw2*qbw3)) / pib3) + (pow(qbw1, 2) + pow(qbw2, 2) - pow(qbw3, 2) - pow(qbw4, 2))*(2 * xbw3 + (2 * qbw1*qbw3 - 2 * qbw2*qbw4) / pib3 - (pib2*(pow(qbw1, 2) + pow(qbw2, 2) - pow(qbw3, 2) - pow(qbw4, 2))) / pib3 + (pib1*(2 * qbw1*qbw4 + 2 * qbw2*qbw3)) / pib3))*((2 * qbw1*qbw2 + 2 * qbw3*qbw4)*((pib1*(pow(qbw1, 2) - pow(qbw2, 2) + pow(qbw3, 2) - pow(qbw4, 2))) / pow(pib3, 2) - (2 * qbw1*qbw2 + 2 * qbw3*qbw4) / pow(pib3, 2) + (pib2*(2 * qbw1*qbw4 - 2 * qbw2*qbw3)) / pow(pib3, 2)) + (2 * qbw1*qbw3 - 2 * qbw2*qbw4)*((2 * qbw1*qbw3 - 2 * qbw2*qbw4) / pow(pib3, 2) - (pib2*(pow(qbw1, 2) + pow(qbw2, 2) - pow(qbw3, 2) - pow(qbw4, 2))) / pow(pib3, 2) + (pib1*(2 * qbw1*qbw4 + 2 * qbw2*qbw3)) / pow(pib3, 2)) - ((pow(qbw1, 2) - pow(qbw2, 2) - pow(qbw3, 2) + pow(qbw4, 2)) / pow(pib3, 2) + (pib1*(2 * qbw1*qbw2 - 2 * qbw3*qbw4)) / pow(pib3, 2) + (pib2*(2 * qbw1*qbw3 + 2 * qbw2*qbw4)) / pow(pib3, 2))*(pow(qbw1, 2) - pow(qbw2, 2) - pow(qbw3, 2) + pow(qbw4, 2)))) / pow(((2 * qbw1*qbw3 - 2 * qbw2*qbw4)*(2 * xbw3 + (2 * qbw1*qbw3 - 2 * qbw2*qbw4) / pib3 - (pib2*(pow(qbw1, 2) + pow(qbw2, 2) - pow(qbw3, 2) - pow(qbw4, 2))) / pib3 + (pib1*(2 * qbw1*qbw4 + 2 * qbw2*qbw3)) / pib3) + (2 * qbw1*qbw2 + 2 * qbw3*qbw4)*((pib1*(pow(qbw1, 2) - pow(qbw2, 2) + pow(qbw3, 2) - pow(qbw4, 2))) / pib3 - (2 * qbw1*qbw2 + 2 * qbw3*qbw4) / pib3 + (pib2*(2 * qbw1*qbw4 - 2 * qbw2*qbw3)) / pib3) - ((pow(qbw1, 2) - pow(qbw2, 2) - pow(qbw3, 2) + pow(qbw4, 2)) / pib3 + (pib1*(2 * qbw1*qbw2 - 2 * qbw3*qbw4)) / pib3 + (pib2*(2 * qbw1*qbw3 + 2 * qbw2*qbw4)) / pib3)*(pow(qbw1, 2) - pow(qbw2, 2) - pow(qbw3, 2) + pow(qbw4, 2))), 2);
- H << 1,0,0,
- 0,1,0,
- hy11,hy12,hy13,
- hy21,hy22,hy23,
- hy31,hy32,hy33,
- hy41,hy42,hy43;
+ if (fType==REFLECTION) {
+ H << 1,0,0,
+ 0,1,0,
+ hy11,hy12,hy13,
+ hy21,hy22,hy23,
+ hy31,hy32,hy33,
+ hy41,hy42,hy43;
+ } else {
+ H << 1,0,0,
+ 0,1,0,
+ hy11,hy12,hy13,
+ hy21,hy22,hy23;
+ }
return H ;
} /* ----- end of method Feature::Hy ----- */
@@ -497,6 +558,9 @@ Feature::P0 ( )
double p0 = FEATURECOVX;
double p1 = FEATURECOVY;
double p2 = FEATURECOVRHO;
+ if (fType==MONO) {
+ p2 = FEATURECOVRHO_MONO;
+ }
P << p0, 0., 0.,
0., p1, 0.,
0., 0., p2;
diff --git a/src/feature.h b/src/feature.h
index bddb50a..e9d1060 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -8,9 +8,11 @@
#define VIEW_NOISE 1e-3 /* */
#define INITIAL_VIEW_NOISE 1e-3 /* */
#define REFLECTION_VIEW_NOISE 1e-2 /* */
-#define FEATURECOVX 1e-3 /* */
-#define FEATURECOVY 1e-3 /* */
-#define FEATURECOVRHO 1e-2 /* */
+#define FEATURECOVX 1e-4 /* */
+#define FEATURECOVY 1e-4 /* */
+#define FEATURECOVRHO 5e-3 /* */
+#define FEATURECOVRHO_MONO 0.5 /* */
+#define RHO_0 1./10. /* */
using Eigen::Dynamic;
using Eigen::Matrix;
@@ -35,6 +37,8 @@ class Feature
/* ==================== LIFECYCLE ======================================= */
Feature ( int id, const Vector3d &xs, const Vector3d &xr,
const Vector3d &xbw, const Quaterniond &q, double z);
+ Feature ( int id, const Vector3d &xs, const Vector3d &xbw,
+ const Quaterniond &q);
/* ==================== ACCESSORS ======================================= */
int id();
@@ -52,16 +56,17 @@ 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,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,Dynamic,9,0,6,9> Hx( const Vector3d &pos, const Quaterniond &q);
+ Matrix<double,Dynamic,3,0,6,3> Hy( const Vector3d &pos, const Quaterniond &q);
+ Matrix<double,Dynamic,1,0,6,1> h( const Vector3d &x, const Quaterniond &q);
Matrix<double,3,6> L();
- Matrix<double,6,1> measurement_vector(const Vector3d &xs, const Vector3d &xr);
+ Matrix<double,Dynamic,1,0,6,1> measurement_vector(const Vector3d &xs, const Vector3d &xr);
Matrix<double,3,3> Q(const double dt);
- Matrix<double,6,6> R();
- Matrix<double,6,6> S ( const Matrix<double,9,9> &Pxx, const Matrix<double,9,3> &Pxy,
- const Matrix<double,3,3> &Pyy, const Vector3d &pos, const Quaterniond &q);
+ Matrix<double,Dynamic,Dynamic,0,6,6> R();
+ Matrix<double,Dynamic,Dynamic,0,6,6> S ( const Matrix<double,9,9> &Pxx,
+ const Matrix<double,9,3> &Pxy, const Matrix<double,3,3> &Pyy,
+ const Vector3d &pos, const Quaterniond &q);
Vector3d p2x(const Vector3d &p);
Vector3d x2p(const Vector3d &x);
@@ -79,6 +84,7 @@ class Feature
Vector3d X0;
Quaterniond q0;
Vector3d xb0w;
+ measurement_type fType;
}; /* ----- end of class Feature ----- */
diff --git a/src/main.cpp b/src/main.cpp
index eb1cdc1..fc08e7a 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -86,17 +86,22 @@ imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q)
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) {
+ int Nmatched;
+ while ((Nmatched=fscanf(fin,"%d,%d,%d,%d,%d", &id, &sx, &sy, &rx, &ry))!=EOF) {
measurement_t m;
m.id = id;
// Points in the image frame
Vector3d xi, xir;
xi << sx, sy, 1.;
- xir << rx, ry, 1.;
- // Points in the camera frame
m.source = cam.img2body(xi);
- m.reflection = cam.img2body(xir);
- m.z_type = REFLECTION;
+ if (Nmatched==5) {
+ xir << rx, ry, 1.;
+ m.reflection = cam.img2body(xir);
+ m.z_type = REFLECTION;
+ } else {
+ m.z_type = MONO;
+ }
+ // Points in the camera frame
z.push_back(m);
}
if (fclose(fin)==EOF) {
@@ -184,7 +189,7 @@ utmCallback(const message &msg, State &mu, const Quaterniond &q)
} else {
/*
i+=1;
- if (i%18==0) {
+ if (i%12==0) {
UTM utm_water;
utm_water.northing = msg.utm.northing;
utm_water.easting = msg.utm.easting;
@@ -347,7 +352,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\n");
+ printf("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
char line[MAXLINE];
diff --git a/src/state.cpp b/src/state.cpp
index 8ad1d9c..ab79086 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -146,17 +146,18 @@ State::R ( const std::vector<measurement_t> &z )
r = MatrixXd::Zero(rows,rows);
for (int i=0,row=0; i<z.size(); ++i) {
+ Feature *fi;
switch ( z[i].z_type ) {
case REFLECTION:
- Feature *fi;
fi = featureById(z[i].id);
r.block<6,6>(row,row) = fi->R();
row += 6;
break;
case MONO:
- fprintf(stderr, "mono points not supported.\n");
- exit(1);
+ fi = featureById(z[i].id);
+ r.block<4,4>(row,row) = fi->R();
+ row += 4;
break;
case HEIGHT:
@@ -224,9 +225,9 @@ State::innovation( const std::vector<measurement_t> &z, const Quaterniond &q)
y = Matrix<double,Dynamic,1>::Zero(rows,1);
for (int i=0,row=0; i<z.size(); ++i) {
+ Feature *fi;
measurement_type mt=z[i].z_type;
if (mt==REFLECTION) {
- Feature *fi;
fi = featureById(z[i].id);
Matrix<double,6,1> hi, zi;
hi = fi->h(body->ned(), q);
@@ -234,8 +235,12 @@ State::innovation( const std::vector<measurement_t> &z, const Quaterniond &q)
y.segment<6>(row) = zi-hi;
row += 6;
} else if (mt==MONO) {
- fprintf(stderr, "mono points not supported.\n");
- exit(1);
+ fi = featureById(z[i].id);
+ Matrix<double,4,1> hi, zi;
+ hi = fi->h(body->ned(), q);
+ zi = fi->measurement_vector(z[i].source, z[i].reflection);
+ y.segment<4>(row) = zi-hi;
+ row += 4;
} else if (mt==HEIGHT) {
Matrix<double,1,1> zi, hi;
zi[0] = z[i].height;
@@ -276,6 +281,26 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni
featuresToAdd.push_back(*i);
}
}
+ // Remove features that don't have measurements in this timestep.
+ auto i=features.begin();
+ int feati=0;
+ while (i!=features.end()) {
+ bool measured=false;
+ for (auto j=z.begin(); j!=z.end(); ++j) {
+ if (j->z_type!=REFLECTION) continue;
+ if (j->id==(*i)->id()) {
+ measured=true;
+ break;
+ }
+ }
+ if (measured==true) {
+ ++i;
+ ++feati;
+ } else {
+ i=removeFeature(i,feati);
+ }
+ }
+
if (featuresToUpdate.size()>1) {
#ifdef FULLS
MatrixXd h;
@@ -300,7 +325,12 @@ State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q, double
for (auto i=F.begin(); i!=F.end(); ++i) {
if (features.size()>MAXFEATURES) break;
// Create feature
- Feature *f = new Feature(i->id, i->source, i->reflection, pos, q, z);
+ Feature *f;
+ if (i->z_type==REFLECTION) {
+ f = new Feature(i->id, i->source, i->reflection, pos, q, z);
+ } else {
+ f = new Feature(i->id, i->source, pos, q);
+ }
if (!f->sane()) {
delete f;
continue;
@@ -517,6 +547,7 @@ void
State::unicsv ( )
{
body->unicsv();
+ printf(",%d\n", features.size());
return ;
} /* ----- end of method State::unicsv ----- */