wip

cfg/tumvi_cam0.json

@@ -127,7 +127,7 @@
 
   // pre-subfilter triangulation options
   "triangulation": {
-    "method": 1,
+    "method": 2,
     "zmin": 0.05,
     "zmax": 5.0
   },

src/estimator.h

@@ -91,7 +91,7 @@ class Estimator : public Component<Estimator, State> {
   SE3 gbc() const { return SE3{X_.Rbc, X_.Tbc}; }
   SE3 gsb() const { return SE3{X_.Rsb, X_.Tsb}; }
   SE3 gsc() const { return gsb() * gbc(); }
-  State X() const { return X_; }
+  const State& X() const { return X_; }
   const timestamp_t &ts() const { return curr_time_; }
 
 private:

src/factory.cpp

@@ -1,4 +1,5 @@
 // factory method to create a system
+// Author: Xiaohan Fei ([email protected])
 #include "param.h"
 #include "camera_manager.h"
 #include "mm.h"

src/optimizer_adapters.cpp

@@ -1,29 +1,28 @@
 #include "optimizer_adapters.h"
 #include "optimizer.h"
 #include "graph.h"
+#include "estimator.h"
 
 namespace xivo {
 
 namespace adapter {
 
 void AddFeature(FeaturePtr f) {
   if (!Graph::instance()->HasFeature(f)) {
-    // std::cout << "feature #" << f->id() << " NOT found in graph" << std::endl;
     return;
   }
   // adapt feature
   FeatureAdapter adapter_f{f->id(), f->Xs()};
   VectorObsAdapterG adapter_obs;
 
   // adapt observations
+  auto gbc{Estimator::instance()->gbc()};
   std::vector<Obs> vobs = Graph::instance()->GetObservationsOf(f);
   for (const auto& obs : vobs) {
     GroupPtr g = obs.g;
-    GroupAdapter adapter_g{g->id(), g->gsb()};
-    // FIXME (xfei): convert xp to bearing vector in body frame
+    GroupAdapter adapter_g{g->id(), g->gsb() * gbc};
     Vec2 xc = Camera::instance()->UnProject(obs.xp);
-    // adapter_obs.push_back(std::make_tuple(adapter_g, xc, Mat2::Identity()));
-    adapter_obs.push_back(ObsAdapterG{adapter_g, xc, Mat2::Identity()});
+    adapter_obs.push_back(std::make_tuple(adapter_g, xc, Mat2::Identity()));
   }
   std::cout << "adding feature #" << adapter_f.id << 
     " with " << adapter_obs.size() << " groups" << std::endl;
@@ -32,22 +31,23 @@ void AddFeature(FeaturePtr f) {
 
 void AddGroup(GroupPtr g) {
   if (!Graph::instance()->HasGroup(g)) {
-    // std::cout << "group #" << g->id() << " NOT found in graph" << std::endl;
     return;
   }
 
   // adapt group
-  GroupAdapter adapter_g{g->id(), g->gsb()};
+
+  auto gbc{Estimator::instance()->gbc()};
+  GroupAdapter adapter_g{g->id(), g->gsb() * gbc};
   VectorObsAdapterF adapter_obs;
 
   std::vector<FeaturePtr> vf = Graph::instance()->GetFeaturesOf(g);
   for (FeaturePtr f : vf) {
     Vec2 xp = Graph::instance()->GetFeatureAdj(f).at(g->id());
     Vec2 xc = Camera::instance()->UnProject(xp);
-    // FIXME (xfei): convert xp to bearing vector in body frame
     FeatureAdapter adapter_f{f->id(), f->Xs()};
     adapter_obs.push_back(std::make_tuple(adapter_f, xc, Mat2::Identity()));
   }
+
   std::cout << "adding group #" << adapter_g.id << 
     " with " << adapter_obs.size() << " features" << std::endl;
   Optimizer::instance()->AddGroup(adapter_g, adapter_obs);