refactor: make the estimator a singleton so that other modules can ac…

…cess it without being fed with the estimator

pybind11/pyxivo.cpp

@@ -25,7 +25,8 @@ class EstimatorWrapper {
     }
 
     auto cfg = LoadJson(cfg_path);
-    estimator_ = std::unique_ptr<Estimator>(new Estimator{cfg});
+    // estimator_ = std::unique_ptr<Estimator>(new Estimator{cfg});
+    estimator_ = CreateSystem(cfg);
 
     if (!viewer_cfg_path.empty()) {
       auto viewer_cfg = LoadJson(viewer_cfg_path);
@@ -77,13 +78,9 @@ class EstimatorWrapper {
       viewer_->Refresh();
   }
 
-  void Release() {
-    viewer_.reset();
-    estimator_.reset();
-  }
-
 private:
-  std::unique_ptr<Estimator> estimator_;
+  // std::unique_ptr<Estimator> estimator_;
+  EstimatorPtr estimator_;
   std::unique_ptr<Viewer> viewer_;
   static bool glog_init_;
   std::string name_;
@@ -101,6 +98,5 @@ PYBIND11_MODULE(pyxivo, m) {
       .def("VisualMeas", &EstimatorWrapper::VisualMeas)
       .def("gsb", &EstimatorWrapper::gsb)
       .def("now", &EstimatorWrapper::now)
-      .def("Visualize", &EstimatorWrapper::Visualize)
-      .def("Release", &EstimatorWrapper::Release);
+      .def("Visualize", &EstimatorWrapper::Visualize);
 }

src/CMakeLists.txt

@@ -57,6 +57,7 @@ add_library(xapp STATIC
 target_link_libraries(xapp ${deps})
 
 add_library(xest STATIC
+        factory.cpp
         estimator.cpp
         princedormand.cpp
         rk4.cpp

src/app/vio.cpp

@@ -40,7 +40,9 @@ int main(int argc, char **argv) {
   std::unique_ptr<DataLoader> loader(new DataLoader{image_dir, imu_dir});
 
   // create estimator
-  auto est = std::make_unique<Estimator>(
+  // auto est = std::make_unique<Estimator>(
+  //     LoadJson(cfg["estimator_cfg"].asString()));
+  auto est = CreateSystem(
       LoadJson(cfg["estimator_cfg"].asString()));
 
   // create viewer

src/estimator.cpp

@@ -21,6 +21,24 @@
 
 namespace xivo {
 
+std::unique_ptr<Estimator> Estimator::instance_{nullptr};
+
+EstimatorPtr Estimator::Create(const Json::Value &cfg) {
+  if (instance_) {
+    LOG(WARNING) << "Estimator already exists!";
+  } else {
+    instance_ = std::unique_ptr<Estimator>(new Estimator{cfg});
+  }
+  return instance_.get();
+}
+
+EstimatorPtr Estimator::instance() {
+  if (!instance_) {
+    LOG(FATAL) << "Estimator NOT created yet!";
+  }
+  return instance_.get();
+}
+
 static const Mat3 I3{Mat3::Identity()};
 static const Mat3 nI3{-I3};
 
@@ -60,11 +78,6 @@ Estimator::~Estimator() {
 Estimator::Estimator(const Json::Value &cfg)
     : cfg_{cfg}, gauge_group_{-1}, worker_{nullptr}, timer_{"estimator"} {
 
-  // initialize paramter server
-  // so that other modules can use parameters right away
-  ParameterServer::Create(cfg_);
-  LOG(INFO) << "Parameter server created";
-
   // /////////////////////////////
   // Component flags
   // /////////////////////////////
@@ -132,13 +145,6 @@ Estimator::Estimator(const Json::Value &cfg)
   imu_ = IMU{Ca, Cg};
   LOG(INFO) << "Imu calibration loaded";
 
-  // load camera parameters
-  auto cam_cfg = cfg_["camera_cfg"].isString()
-                     ? LoadJson(cfg_["camera_cfg"].asString())
-                     : cfg_["camera_cfg"];
-  Camera::Create(cam_cfg);
-  LOG(INFO) << "Camera created";
-
   g_ = GetMatrixFromJson<number_t, 3, 1>(cfg_, "gravity");
   LOG(INFO) << "gravity loaded:" << g_.transpose();
 
@@ -267,21 +273,6 @@ Estimator::Estimator(const Json::Value &cfg)
   Qimu_ *= Qimu_;
   LOG(INFO) << "Covariance of IMU measurement noise loaded";
 
-  // /////////////////////////////
-  // initialize memory manager
-  // /////////////////////////////
-  MemoryManager::Create(cfg_["memory"].get("max_features", 256).asInt(),
-                        cfg_["memory"].get("max_groups", 128).asInt());
-  LOG(INFO) << "Memory management unit created";
-
-  // /////////////////////////////
-  // initialize tracker
-  // /////////////////////////////
-  auto tracker_cfg = cfg_["tracker_cfg"].isString()
-                         ? LoadJson(cfg_["tracker_cfg"].asString())
-                         : cfg_["tracker_cfg"];
-  Tracker::Create(tracker_cfg);
-  LOG(INFO) << "Tracker created";
 
   R_ = cfg_["visual_meas_std"].asDouble();
   R_ *= R_;
@@ -291,14 +282,6 @@ Estimator::Estimator(const Json::Value &cfg)
 
   LOG(INFO) << "R=" << R_ << " ;Roos=" << Roos_;
 
-  // /////////////////////////////
-  // Initialize the visibility graph
-  // /////////////////////////////
-  Graph::Create();
-#ifdef USE_G2O
-  Optimizer::Create(cfg_["optimizer"]);
-#endif
-
   // /////////////////////////////
   // Load initial std on feature state
   // /////////////////////////////

src/estimator.h

@@ -29,13 +29,18 @@
 
 namespace xivo {
 
+class Estimator;
+using EstimatorPtr = Estimator*;
+EstimatorPtr CreateSystem(const Json::Value &cfg);
+
+
 namespace internal {
 class Message {
 public:
   Message(const timestamp_t &ts) : ts_{ts} {}
   const timestamp_t &ts() const { return ts_; }
   virtual ~Message() = default;
-  virtual void Execute(Estimator *) {}
+  virtual void Execute(EstimatorPtr) {}
 
 protected:
   timestamp_t ts_;
@@ -44,7 +49,7 @@ class Message {
 class Visual : public Message {
 public:
   Visual(const timestamp_t &ts, const cv::Mat &img) : Message{ts}, img_{img} {}
-  void Execute(Estimator *est);
+  void Execute(EstimatorPtr est);
 
 private:
   cv::Mat img_;
@@ -54,21 +59,26 @@ class Inertial : public Message {
 public:
   Inertial(const timestamp_t &ts, const Vec3 &gyro, const Vec3 &accel)
       : Message{ts}, gyro_{gyro}, accel_{accel} {}
-  void Execute(Estimator *est);
+  void Execute(EstimatorPtr est);
 
 private:
   Vec3 gyro_, accel_;
 };
 
 } // namespace internal
 
+
 class Estimator : public Component<Estimator, State> {
   friend class internal::Visual;
   friend class internal::Inertial;
+
+public:
+  static EstimatorPtr Create(const Json::Value &cfg);
+  static EstimatorPtr instance();
+
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-  Estimator(const Json::Value &cfg);
   ~Estimator();
 
   void Run();
@@ -145,6 +155,10 @@ class Estimator : public Component<Estimator, State> {
   void PrintErrorState();
   void PrintNominalState();
 
+private:
+  Estimator(const Json::Value &cfg);
+  static std::unique_ptr<Estimator> instance_;
+
 private:
   std::vector<FeaturePtr> instate_features_; // in-state features
   std::vector<FeaturePtr> oos_features_;     // out-of-state features

src/estimator_process.cpp

@@ -15,7 +15,8 @@ bool operator<(const std::unique_ptr<EstimatorMessage> &m1,
 void EstimatorProcess::Initialize(const std::string &config_path) {
   // TODO (xfei): error handling?
   auto est_cfg = LoadJson(config_path);
-  estimator_ = std::unique_ptr<Estimator>(new Estimator{est_cfg});
+  // estimator_ = std::unique_ptr<Estimator>(new Estimator{est_cfg});
+  estimator_ = CreateSystem(est_cfg);
 }
 
 /*
@@ -32,7 +33,7 @@ bool EstimatorProcess::Handle(EstimatorMessage *message) {
   if (Process::Handle(message))
     return true;
 
-  message->Execute(estimator_.get());
+  message->Execute(estimator_);
 
   if (auto msg = dynamic_cast<VisualMeas *>(message)) {
     // FIXME: instead of drawing on the canvas in the estimator

src/estimator_process.h

@@ -92,7 +92,8 @@ class EstimatorProcess : public Process<EstimatorMessage> {
 
 private:
   std::string name_;
-  std::unique_ptr<Estimator> estimator_; // owned
+  // std::unique_ptr<Estimator> estimator_; // owned
+  EstimatorPtr estimator_; // owned
   // results publisher for asynchronized communication
   Publisher *publisher_; // non-owned
 };                       // EstimatorProcess

src/factory.cpp

@@ -0,0 +1,65 @@
+// factory method to create a system
+#include "param.h"
+#include "camera_manager.h"
+#include "mm.h"
+#include "tracker.h"
+#include "graph.h"
+#include "estimator.h"
+
+#ifdef USE_G2O
+#include "optimizer.h"
+#endif
+
+namespace xivo {
+
+EstimatorPtr CreateSystem(const Json::Value &cfg) {
+  static bool system_created{false};
+
+  if (system_created) {
+    return Estimator::instance();
+  }
+
+  // Initialize paramter server
+  ParameterServer::Create(cfg);
+  LOG(INFO) << "Parameter server created";
+
+  // Load camera parameters
+  auto cam_cfg = cfg["camera_cfg"].isString()
+                     ? LoadJson(cfg["camera_cfg"].asString())
+                     : cfg["camera_cfg"];
+  Camera::Create(cam_cfg);
+  LOG(INFO) << "Camera created";
+
+  // Initialize memory manager
+  MemoryManager::Create(cfg["memory"].get("max_features", 256).asInt(),
+                        cfg["memory"].get("max_groups", 128).asInt());
+  LOG(INFO) << "Memory management unit created";
+
+  // Initialize tracker
+  auto tracker_cfg = cfg["tracker_cfg"].isString()
+                         ? LoadJson(cfg["tracker_cfg"].asString())
+                         : cfg["tracker_cfg"];
+  Tracker::Create(tracker_cfg);
+  LOG(INFO) << "Tracker created";
+
+  // Initialize the visibility graph
+  Graph::Create();
+  LOG(INFO) << "Visibility graph created";
+
+#ifdef USE_G2O
+  // Initialize the optimizer
+  Optimizer::Create(cfg["optimizer"]);
+  LOG(INFO) << "Optimizer created";
+#endif
+
+  // Initialize the estimator
+  Estimator::Create(cfg);
+  LOG(INFO) << "Estimator created";
+
+  system_created = true;
+
+  return Estimator::instance();
+}
+
+
+} // namespace xivo