diff --git a/CMakeLists.txt b/CMakeLists.txt index ee24e3df..dbb03bfa 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/src/optimizer.cpp b/src/optimizer.cpp index 8864dd6f..eb5e8611 100644 --- a/src/optimizer.cpp +++ b/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 &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 &obs) { +void Optimizer::AddGroup(const GroupAdapter &g, const VectorObsAdapterF &obs) { if (!gvertices_.count(g.id)) { CreateGroupVertex(g); } diff --git a/src/optimizer.h b/src/optimizer.h index aa4f4773..916ebc40 100644 --- a/src/optimizer.h +++ b/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 &obs); - void AddGroup(const GroupAdapter &g, const std::vector &obs); + void AddFeature(const FeatureAdapter &f, const VectorObsAdapterG &obs); + void AddGroup(const GroupAdapter &g, const VectorObsAdapterF &obs); private: Optimizer() = delete; diff --git a/src/optimizer_adapters.cpp b/src/optimizer_adapters.cpp index bf8d6e94..a44e9a4b 100644 --- a/src/optimizer_adapters.cpp +++ b/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 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 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 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 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); } diff --git a/src/optimizer_types.h b/src/optimizer_types.h index 338fd774..238b59ce 100644 --- a/src/optimizer_types.h +++ b/src/optimizer_types.h @@ -116,7 +116,10 @@ struct GroupAdapter { }; using ObsAdapterG = std::tuple; -// using ObsAdaptersG = std::vector>; using ObsAdapterF = std::tuple; +using VectorObsAdapterF = std::vector>; } // namespace xivo diff --git a/src/test/test_optimizer.cpp b/src/test/test_optimizer.cpp index a692e84a..3cb3b102 100644 --- a/src/test/test_optimizer.cpp +++ b/src/test/test_optimizer.cpp @@ -62,7 +62,7 @@ int main() { Sample::gaussian(1), Sample::gaussian(1)}; - vector 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); }