From 1404a28bfcec93f2a510a862d07367cbb8a632cc Mon Sep 17 00:00:00 2001 From: Xiaohan Fei Date: Fri, 6 Sep 2019 23:16:04 -0700 Subject: [PATCH] refactor: make the estimator a singleton so that other modules can access it without being fed with the estimator --- pybind11/pyxivo.cpp | 14 +++------ src/CMakeLists.txt | 1 + src/app/vio.cpp | 4 ++- src/estimator.cpp | 53 +++++++++++-------------------- src/estimator.h | 22 ++++++++++--- src/estimator_process.cpp | 5 +-- src/estimator_process.h | 3 +- src/factory.cpp | 65 +++++++++++++++++++++++++++++++++++++++ 8 files changed, 115 insertions(+), 52 deletions(-) create mode 100644 src/factory.cpp diff --git a/pybind11/pyxivo.cpp b/pybind11/pyxivo.cpp index 066dec52..907c1ca3 100644 --- a/pybind11/pyxivo.cpp +++ b/pybind11/pyxivo.cpp @@ -25,7 +25,8 @@ class EstimatorWrapper { } auto cfg = LoadJson(cfg_path); - estimator_ = std::unique_ptr(new Estimator{cfg}); + // estimator_ = std::unique_ptr(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_; + // std::unique_ptr estimator_; + EstimatorPtr estimator_; std::unique_ptr 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); } diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ef8c14e3..c4b4cb54 100644 --- a/src/CMakeLists.txt +++ b/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 diff --git a/src/app/vio.cpp b/src/app/vio.cpp index 884bc260..7b920bd5 100644 --- a/src/app/vio.cpp +++ b/src/app/vio.cpp @@ -40,7 +40,9 @@ int main(int argc, char **argv) { std::unique_ptr loader(new DataLoader{image_dir, imu_dir}); // create estimator - auto est = std::make_unique( + // auto est = std::make_unique( + // LoadJson(cfg["estimator_cfg"].asString())); + auto est = CreateSystem( LoadJson(cfg["estimator_cfg"].asString())); // create viewer diff --git a/src/estimator.cpp b/src/estimator.cpp index 25bf449e..58145fec 100644 --- a/src/estimator.cpp +++ b/src/estimator.cpp @@ -21,6 +21,24 @@ namespace xivo { +std::unique_ptr Estimator::instance_{nullptr}; + +EstimatorPtr Estimator::Create(const Json::Value &cfg) { + if (instance_) { + LOG(WARNING) << "Estimator already exists!"; + } else { + instance_ = std::unique_ptr(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(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 // ///////////////////////////// diff --git a/src/estimator.h b/src/estimator.h index c471d975..54c52940 100644 --- a/src/estimator.h +++ b/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,7 +59,7 @@ 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_; @@ -62,13 +67,18 @@ class Inertial : public Message { } // namespace internal + class Estimator : public Component { 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 { void PrintErrorState(); void PrintNominalState(); +private: + Estimator(const Json::Value &cfg); + static std::unique_ptr instance_; + private: std::vector instate_features_; // in-state features std::vector oos_features_; // out-of-state features diff --git a/src/estimator_process.cpp b/src/estimator_process.cpp index a9c5a2aa..a563dcbd 100644 --- a/src/estimator_process.cpp +++ b/src/estimator_process.cpp @@ -15,7 +15,8 @@ bool operator<(const std::unique_ptr &m1, void EstimatorProcess::Initialize(const std::string &config_path) { // TODO (xfei): error handling? auto est_cfg = LoadJson(config_path); - estimator_ = std::unique_ptr(new Estimator{est_cfg}); + // estimator_ = std::unique_ptr(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(message)) { // FIXME: instead of drawing on the canvas in the estimator diff --git a/src/estimator_process.h b/src/estimator_process.h index ecde3dee..768d140e 100644 --- a/src/estimator_process.h +++ b/src/estimator_process.h @@ -92,7 +92,8 @@ class EstimatorProcess : public Process { private: std::string name_; - std::unique_ptr estimator_; // owned + // std::unique_ptr estimator_; // owned + EstimatorPtr estimator_; // owned // results publisher for asynchronized communication Publisher *publisher_; // non-owned }; // EstimatorProcess diff --git a/src/factory.cpp b/src/factory.cpp new file mode 100644 index 00000000..57624f6e --- /dev/null +++ b/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