From 728cc9930cbea478ca732b6c19bab275ef50c6d0 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Fri, 7 Apr 2017 17:28:23 -0500
Subject: Begin Vision class.

This class can display measurements and featuers and find new features.
When complete it will also perform measurements.
---
 src/state.cpp | 48 +++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 47 insertions(+), 1 deletion(-)

(limited to 'src/state.cpp')

diff --git a/src/state.cpp b/src/state.cpp
index 937a653..8a31535 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -204,7 +204,7 @@ State::partialUpdate( const MatrixXd &h, const MatrixXd &S,
     // Get the update
     Matrix<double,Dynamic,1> dx;
     dx = K*y;
-    if (dx.segment<3>(3).norm()>0.04) {
+    if (dx.segment<3>(3).norm()>1) {
         cerr << "dx: " << dx.head<STATESIZE>().transpose() << endl;
         return K;
     }
@@ -1035,3 +1035,49 @@ State::quaternion_covariance ( const Matrix<double,4,4> &covq )
 }		/* -----  end of method State::quaternion_covariance  ----- */
 
 #endif
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  State
+ *      Method:  State :: featuresAsMeasurements
+ * Description:  Writes the feature locations to a measurement_t vector in body
+ * coordinates.
+ *--------------------------------------------------------------------------------------
+ */
+void
+#if STATESIZE==13
+State::featuresAsMeasurements ( std::vector<measurement_t> &yk )
+#else
+State::featuresAsMeasurements ( std::vector<measurement_t> &yk,
+        const Quaterniond &q)
+#endif
+{
+#if STATESIZE==13
+    Quaterniond q = body->qhat();
+#endif
+    for (auto i=features.begin(); i!=features.end(); ++i) {
+        // Get the predicted measurement
+        Matrix<double,6,1> h;
+        h = (*i)->h(body->ned(), q);
+
+        Vector3d pbs, pbr;
+        pbs << h(0), h(1), 1;
+        pbr << h(4), h(5), 1;
+
+        measurement_t z;
+        z.id = 0;
+        z.source = (*i)->p2x(pbs);
+        z.reflection = (*i)->p2x(pbr);
+        z.z_type = REFLECTION;
+
+        // Get ellipse
+        Matrix<double,6,6> s;
+        s = (*i)->S(Pxx(), Pxy((*i)->id()), Pyy((*i)->id()), body->ned(), q);
+        z.Ssrc << s(0,0), s(1,1);
+        z.Sref << s(4,4), s(5,5);
+
+        yk.push_back(z);
+    }
+    return ;
+}		/* -----  end of method State::featuresAsMeasurements  ----- */
+
-- 
cgit v1.1