need to fix segfault in unordered_map

CMakeLists.txt

@@ -9,13 +9,17 @@ if (USE_GPERFTOOLS)
   add_definitions(-DUSE_GPERFTOOLS)
 endif (USE_GPERFTOOLS)
 
+if (BUILD_G2O)
+  add_definitions(-DUSE_G2O)
+endif (BUILD_G2O)
+
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -std=c++17 -Wno-narrowing -Wno-register -fPIC -g")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mtune=native -march=native")
 
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -funroll-loops")
 
-# set(CMAKE_BUILD_TYPE "RelWithDebInfo")
-set(CMAKE_BUILD_TYPE "Release")
+set(CMAKE_BUILD_TYPE "RelWithDebInfo")
+# set(CMAKE_BUILD_TYPE "Release")
 add_definitions(-DNDEBUG)
 
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin)

common/project.h

@@ -1,5 +1,4 @@
 // Project and Unproject functions.
-//
 // Author: Xiaohan Fei ([email protected])
 #pragma once
 

src/CMakeLists.txt

@@ -47,7 +47,7 @@ if (USE_GPERFTOOLS)
   list(APPEND deps profiler)
 endif (USE_GPERFTOOLS)
 
-add_library(xapp SHARED
+add_library(xapp STATIC
         estimator_process.cpp
         loader.cpp
         geometry.cpp
@@ -56,7 +56,7 @@ add_library(xapp SHARED
         viewer.cpp)
 target_link_libraries(xapp ${deps})
 
-add_library(xest SHARED
+add_library(xest STATIC
         estimator.cpp
         princedormand.cpp
         rk4.cpp
@@ -79,8 +79,9 @@ target_link_libraries(xest ${deps})
 set(libxivo xest xapp)
 
 if (BUILD_G2O)
-  add_library(xopt
+  add_library(xopt STATIC
     optimizer.cpp
+    optimizer_adapters.cpp
   )
   target_link_libraries(xopt xest ${deps})
   list(APPEND libxivo xopt)

src/estimator.cpp

@@ -362,14 +362,6 @@ void Estimator::Run() {
   });
 }
 
-bool Estimator::Finished() {
-#ifndef NDEBUG
-  CHECK(async_run_);
-#endif
-  std::scoped_lock lck(buf_.mtx);
-  return buf_.empty();
-}
-
 bool Estimator::InitializeGravity() {
   VLOG(0) << "attempt to initialize gravity";
   if (!simulation_) {
@@ -865,21 +857,24 @@ void Estimator::VisualMeas(const timestamp_t &ts_raw, const cv::Mat &img) {
     ts -= timestamp_t(uint64_t(-X_.td * 1e9)); // seconds -> nanoseconds
   }
 #endif
-  {
+  if (async_run_) {
     std::scoped_lock lck(buf_.mtx);
     buf_.push_back(std::make_unique<internal::Visual>(ts, img));
-    // std::cout << "visual pushed\n";
     MaintainBuffer();
-    // std::cout << "buffer.size=" << buf_.size() << std::endl;
+  } else {
+    buf_.push_back(std::make_unique<internal::Visual>(ts, img));
+    MaintainBuffer();
   }
 }
 
 void Estimator::InertialMeas(const timestamp_t &ts, const Vec3 &gyro,
                              const Vec3 &accel) {
-  {
+  if (async_run_) {
     std::scoped_lock lck(buf_.mtx);
     buf_.push_back(std::make_unique<internal::Inertial>(ts, gyro, accel));
-    // std::cout << "inertial pushed\n";
+    MaintainBuffer();
+  } else {
+    buf_.push_back(std::make_unique<internal::Inertial>(ts, gyro, accel));
     MaintainBuffer();
   }
 }

src/estimator.h

@@ -83,7 +83,6 @@ class Estimator : public Component<Estimator, State> {
   SE3 gsc() const { return gsb() * gbc(); }
   State X() const { return X_; }
   const timestamp_t &ts() const { return curr_time_; }
-  bool Finished();
 
 private:
   void UpdateState(const State::Tangent &dX) { X_ += dX; }

src/graph.cpp

@@ -3,6 +3,10 @@
 #include "feature.h"
 #include "group.h"
 
+#ifdef USE_G2O
+#include "optimizer_adapters.h"
+#endif
+
 namespace xivo {
 
 void FeatureAdj::Add(const Observation &obs) { insert({obs.g->id(), obs.xp}); }
@@ -54,6 +58,10 @@ const GroupAdj &Graph::GetGroupAdj(GroupPtr g) const {
 void Graph::RemoveFeature(const FeaturePtr f) {
   CHECK(HasFeature(f)) << "feature #" << f->id() << " not exists";
 
+#ifdef USE_G2O
+  adapter::AddFeature(f);
+#endif
+
   int fid = f->id();
   features_.erase(fid);
   for (const auto &obs : feature_adj_.at(fid)) {
@@ -73,6 +81,10 @@ void Graph::RemoveFeatures(const std::vector<FeaturePtr> &features) {
 void Graph::RemoveGroup(const GroupPtr g) {
   CHECK(HasGroup(g)) << "group #" << g->id() << " not exists";
 
+#ifdef USE_G2O
+  adapter::AddGroup(g);
+#endif
+
   int gid = g->id();
   groups_.erase(gid);
   for (auto fid : group_adj_.at(gid)) {

src/group.cpp

@@ -18,7 +18,9 @@ GroupPtr Group::Create(const SO3 &Rsb, const Vec3 &Tsb) {
   return g;
 }
 
-void Group::Delete(GroupPtr g) { MemoryManager::instance()->ReturnGroup(g); }
+void Group::Delete(GroupPtr g) { 
+  MemoryManager::instance()->ReturnGroup(g); 
+}
 
 void Group::Reset(const SO3 &Rsb, const Vec3 &Tsb) {
   id_ = counter_++;

src/optimizer.cpp

@@ -104,6 +104,7 @@ Edge* Optimizer::CreateEdge(FeatureVertex *fv, GroupVertex *gv, const Vec2 &xp,
 void Optimizer::AddFeature(const FeatureAdapter &f, const std::vector<ObsAdapterG> &obs) {
   // CHECK(!fvertices_.count(f->id()) << "Feature #" << f->id() << " already in optimization graph";
 
+  std::cout << "adding feature #" << f.id << std::endl;
   if (!fvertices_.count(f.id)) {
     // feature vertex not exist, create one
     CreateFeatureVertex(f);

src/optimizer.h

@@ -18,9 +18,6 @@ using OptimizerPtr = Optimizer*;
 
 
 class Optimizer {
-public:
-
-
 public:
   ~Optimizer();
   static OptimizerPtr Create(const Json::Value &cfg);
@@ -56,7 +53,6 @@ class Optimizer {
 
   // g2o variables
   g2o::SparseOptimizer optimizer_;
-
 };
 
 } // namespace xivo

src/optimizer_adapters.cpp

@@ -0,0 +1,57 @@
+#include "optimizer_adapters.h"
+#include "optimizer.h"
+#include "graph.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
+  auto adapter_f = FeatureAdapter{f->id(), f->Xs()};
+  // adapt observations
+  auto vobs = Graph::instance()->GetObservationsOf(f);
+  std::vector<ObsAdapterG> adapter_obs;
+
+  // for (const auto& obs : vobs) {
+  //   auto g = obs.g;
+  //   auto adapter_g = GroupAdapter{g->id(), g->gsb()};
+  //   // FIXME (xfei): convert xp to bearing vector in body frame
+  //   auto xc = Camera::instance()->UnProject(obs.xp);
+  //   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;
+  Optimizer::instance()->AddFeature(adapter_f, adapter_obs);
+}
+
+void AddGroup(GroupPtr g) {
+  if (!Graph::instance()->HasGroup(g)) {
+    // std::cout << "group #" << g->id() << " NOT found in graph" << std::endl;
+    return;
+  }
+
+  // adapt group
+  auto adapter_g = GroupAdapter{g->id(), g->gsb()};
+
+  std::vector<ObsAdapterF> adapter_obs;
+  // auto vf = Graph::instance()->GetFeaturesOf(g);
+  // for (auto f : vf) {
+  //   auto xp = Graph::instance()->GetFeatureAdj(f).at(g->id());
+  //   auto 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);
+}
+
+} // namespace adapter
+
+} // namespace xivo

src/optimizer_adapters.h

@@ -0,0 +1,19 @@
+// Adapt feature and group objects to objects compatible with 
+// the optimizer.
+#pragma once
+#include "optimizer_types.h"
+#include "feature.h"
+#include "group.h"
+
+namespace xivo {
+
+namespace adapter {
+
+void AddFeature(FeaturePtr f);
+void AddGroup(GroupPtr g);
+
+} // namespace adapter
+
+} // namespace xivo
+
+

src/optimizer_types.h

@@ -104,16 +104,19 @@ class Edge: public g2o::BaseBinaryEdge<2, Vec2,  FeatureVertex, GroupVertex> {
 };
 
 struct FeatureAdapter {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   int id;
   Vec3 Xs;  // 3D coordinates in spatial frame
 };
 
 struct GroupAdapter {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   int id;
   SE3 gsb;  // body to spatial transformation
 };
 
 using ObsAdapterG = std::tuple<GroupAdapter, Vec2, Mat2>;
+// using ObsAdaptersG = std::vector<ObsAdapterG, Eigen::aligned_allocator<
 using ObsAdapterF = std::tuple<FeatureAdapter, Vec2, Mat2>;
 
 } // namespace xivo