segfault not fix yet

CMakeLists.txt

@@ -13,12 +13,13 @@ 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} -O0 -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 "Debug")
+# set(CMAKE_BUILD_TYPE "RelWithDebInfo")
 # set(CMAKE_BUILD_TYPE "Release")
 add_definitions(-DNDEBUG)
 

src/optimizer.cpp

@@ -101,10 +101,8 @@ Edge* Optimizer::CreateEdge(FeatureVertex *fv, GroupVertex *gv, const Vec2 &xp,
   return e;
 }
 
-void Optimizer::AddFeature(const FeatureAdapter &f, const std::vector<ObsAdapterG> &obs) {
+void Optimizer::AddFeature(const FeatureAdapter &f, const VectorObsAdapterG &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);
@@ -122,7 +120,7 @@ void Optimizer::AddFeature(const FeatureAdapter &f, const std::vector<ObsAdapter
   }
 }
 
-void Optimizer::AddGroup(const GroupAdapter &g, const std::vector<ObsAdapterF> &obs) {
+void Optimizer::AddGroup(const GroupAdapter &g, const VectorObsAdapterF &obs) {
   if (!gvertices_.count(g.id)) {
     CreateGroupVertex(g);
   }

src/optimizer.h

@@ -24,8 +24,8 @@ class Optimizer {
   static OptimizerPtr instance();
   void Solve(int iters=1);
 
-  void AddFeature(const FeatureAdapter &f, const std::vector<ObsAdapterG> &obs);
-  void AddGroup(const GroupAdapter &g, const std::vector<ObsAdapterF> &obs);
+  void AddFeature(const FeatureAdapter &f, const VectorObsAdapterG &obs);
+  void AddGroup(const GroupAdapter &g, const VectorObsAdapterF &obs);
 
 private:
   Optimizer() = delete;

src/optimizer_adapters.cpp

@@ -12,20 +12,21 @@ void AddFeature(FeaturePtr f) {
     return;
   }
   // adapt feature
-  auto adapter_f = FeatureAdapter{f->id(), f->Xs()};
+  FeatureAdapter adapter_f{f->id(), f->Xs()};
+  VectorObsAdapterG adapter_obs;
+
   // 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;
+  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
+    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()});
+  }
+  std::cout << "adding feature #" << adapter_f.id << 
+    " with " << adapter_obs.size() << " groups" << std::endl;
   Optimizer::instance()->AddFeature(adapter_f, adapter_obs);
 }
 
@@ -36,19 +37,19 @@ void AddGroup(GroupPtr g) {
   }
 
   // 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;
+  GroupAdapter adapter_g{g->id(), g->gsb()};
+  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);
 }
 

src/optimizer_types.h

@@ -116,7 +116,10 @@ struct GroupAdapter {
 };
 
 using ObsAdapterG = std::tuple<GroupAdapter, Vec2, Mat2>;
-// using ObsAdaptersG = std::vector<ObsAdapterG, Eigen::aligned_allocator<
+using VectorObsAdapterG = std::vector<ObsAdapterG, 
+      Eigen::aligned_allocator<ObsAdapterG>>;
 using ObsAdapterF = std::tuple<FeatureAdapter, Vec2, Mat2>;
+using VectorObsAdapterF = std::vector<ObsAdapterF,
+      Eigen::aligned_allocator<ObsAdapterF>>;
 
 } // namespace xivo

src/test/test_optimizer.cpp

@@ -62,7 +62,7 @@ int main() {
       Sample::gaussian(1),
       Sample::gaussian(1)};
 
-    vector<ObsAdapterG> obs;
+    VectorObsAdapterG obs;
     for (int j = 0; j < gsb.size(); ++j) {
       Vec3 Xb = gsb[j].inv() * pt;
       Vec2 xp = Xb.head<2>() / Xb(2);
@@ -72,7 +72,7 @@ int main() {
             GroupAdapter{j, gsb[j]},
             noisy_xp,  Mat2::Identity() /(PIXEL_NOISE * PIXEL_NOISE)));
     }
-    optimizer->AddFeature(FeatureAdapter{gsb.size() + i, noisy_pt}, obs);
+    optimizer->AddFeature(FeatureAdapter{10000 + i, noisy_pt}, obs);
   }
   optimizer->Solve(10);
 }