diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index a03d0da..aa3b3aa 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -9,7 +9,7 @@ repos: - id: check-yaml - id: check-added-large-files args: ['--maxkb=3000'] -- repo: https://gitlab.com/pycqa/flake8 +- repo: https://github.com/pycqa/flake8 rev: 3.8.2 hooks: - id: flake8 diff --git a/CMakeLists.txt b/CMakeLists.txt index 15e5f7c..c315dec 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,58 +1,36 @@ -cmake_minimum_required(VERSION 3.10) +cmake_minimum_required(VERSION 3.15) project(dcsam) # Set some compilation options. set(CMAKE_CXX_STANDARD 17) -add_compile_options(-Wall -Wpedantic -Wextra) +if (NOT CMAKE_BUILD_TYPE) + # Options: Debug, Release, MinSizeRel, RelWithDebInfo + message(STATUS "No build type selected, default to Release") + set(CMAKE_BUILD_TYPE "Release") +endif() # Add option to enable testing -option(ENABLE_TESTS "Enable tests" OFF) +option(DCSAM_ENABLE_TESTS "Enable tests" OFF) # External package dependencies. -find_package(GTSAM 4.1 REQUIRED) +find_package(GTSAM 4.2 REQUIRED) find_package(Eigen3 3.3 REQUIRED) # add_definitions(-march=native) # add_definitions(-std=c++1z) -set(DCSAM_HDRS - include/dcsam/DCSAM_types.h - include/dcsam/DCSAM_utils.h - include/dcsam/DCSAM.h - include/dcsam/DCFactor.h - include/dcsam/DCContinuousFactor.h - include/dcsam/DCDiscreteFactor.h - include/dcsam/DCMixtureFactor.h - include/dcsam/DiscretePriorFactor.h - include/dcsam/SmartDiscretePriorFactor.h - include/dcsam/HybridFactorGraph.h - include/dcsam/SemanticBearingRangeFactor.h - include/dcsam/SmartSemanticBearingRangeFactor.h - include/dcsam/DCMaxMixtureFactor.h - include/dcsam/DCEMFactor.h - include/dcsam/DiscreteMarginalsOrdered.h - ) - -set(DCSAM_SRCS - src/DCSAM.cpp - src/HybridFactorGraph.cpp - ) - -set(DCSAM_LIBS - gtsam - Eigen3::Eigen - ) - -add_library(dcsam STATIC ${DCSAM_SRCS} ${DCSAM_HDRS}) +add_library(dcsam SHARED) +target_sources(dcsam PRIVATE src/DCSAM.cpp src/HybridFactorGraph.cpp) target_include_directories(dcsam PUBLIC include) -target_link_libraries(dcsam PUBLIC ${DCSAM_LIBS}) +target_link_libraries(dcsam PUBLIC Eigen3::Eigen gtsam) +target_compile_options(dcsam PRIVATE -Wall -Wpedantic -Wextra) # Make library accessible to other cmake projects export(PACKAGE dcsam) export(TARGETS dcsam FILE dcsamConfig.cmake) # Include unit tests directory to the project. -if (${ENABLE_TESTS}) +if(DCSAM_ENABLE_TESTS) message(STATUS "Testing enabled. Building tests.") enable_testing() add_subdirectory(tests) diff --git a/README.md b/README.md index 5380bc0..882152e 100644 --- a/README.md +++ b/README.md @@ -10,8 +10,8 @@ A technical report describing this library and our solver can be found [here](ht ```bibtex @article{doherty2022discrete, author={Doherty, Kevin J. and Lu, Ziqi and Singh, Kurran and Leonard, John J.}, - journal={IEEE Robotics and Automation Letters}, - title={Discrete-{C}ontinuous {S}moothing and {M}apping}, + journal={IEEE Robotics and Automation Letters}, + title={Discrete-{C}ontinuous {S}moothing and {M}apping}, year={2022}, volume={7}, number={4}, @@ -22,13 +22,13 @@ A technical report describing this library and our solver can be found [here](ht ## Prerequisites -- [GTSAM](https://github.com/borglab/gtsam) @ `caa14bc` +- [GTSAM](https://github.com/borglab/gtsam) @ `4.2a8` To retrieve the appropriate version of GTSAM: ```sh -~/$ git clone https://github.com/borglab/gtsam -~/$ cd gtsam -~/gtsam/$ git checkout caa14bc +~ $ git clone https://github.com/borglab/gtsam +~ $ cd gtsam +~/gtsam $ git checkout 4.2a8 ``` Follow instructions in the GTSAM repository to build and install with your desired configuration. @@ -44,26 +44,26 @@ Follow instructions in the GTSAM repository to build and install with your desir To build using `cmake`: ```bash -~/dcsam/$ mkdir build -~/dcsam/$ cd build -~/dcsam/build$ cmake .. -~/dcsam/build$ make -j +~/dcsam $ mkdir build +~/dcsam $ cd build +~/dcsam/build $ cmake .. +~/dcsam/build $ make -j ``` ### Run tests To run unit tests, first build with testing enabled: ```bash -~/$ mkdir build -~/$ cd build -~/build$ cmake .. -DENABLE_TESTS=ON -~/build$ make -j +~/dcsam $ mkdir build +~/dcsam $ cd build +~/dcsam/build $ cmake .. -DDCSAM_ENABLE_TESTS=ON +~/dcsam/build $ make -j ``` Now you can run the tests as follows: ```bash -~/build$ make test +~/dcsam/build $ make test ``` ### Examples diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 6520fee..b11816f 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -1,25 +1,13 @@ -option(ENABLE_PLOTTING "Enable plotting (requires matplotlibcpp)" OFF) - -if (ENABLE_PLOTTING) - message(STATUS "Plotting enabled. Building tests with plotting.") - find_package(matplotlibcpp REQUIRED) -endif() +option(DCSAM_ENABLE_PLOTTING "Enable plotting (requires matplotlib_cpp)" OFF) # The gtest/gtest_main targets carry header search path # dependencies automatically when using CMake 2.8.11 or # later. Otherwise we have to add them here ourselves. -if (CMAKE_VERSION VERSION_LESS 2.8.11) +if(CMAKE_VERSION VERSION_LESS 2.8.11) include_directories("${gtest_SOURCE_DIR}/include") endif() -if (ENABLE_PLOTTING) - add_definitions(-DENABLE_PLOTTING=1) -endif() - -# Include tests here... -add_executable(testDCSAM testDCSAM.cpp) - -if (APPLE) +if(APPLE) set(GTEST_LIBS /usr/local/lib/libgtest.a /usr/local/lib/libgtest_main.a @@ -28,16 +16,41 @@ else() set(GTEST_LIBS gtest gtest_main -) + ) endif() +add_executable(testDCSAM testDCSAM.cpp) +target_link_libraries(testDCSAM dcsam gtsam ${GTEST_LIBS}) +# If plotting is enabled, we need to link to matplotlib_cpp +if(DCSAM_ENABLE_PLOTTING) + message(STATUS "Plotting enabled. Building tests with plotting.") + include(FetchContent) + FetchContent_Declare(matplotlibcpp + GIT_REPOSITORY https://github.com/lava/matplotlib-cpp.git + GIT_TAG ef0383f1315d32e0156335e10b82e90b334f6d9f + ) -# If plotting is enabled, we need to link to matplotlibcpp -if (ENABLE_PLOTTING) - target_link_libraries(testDCSAM dcsam matplotlibcpp gtsam ${GTEST_LIBS}) -else() - target_link_libraries(testDCSAM dcsam gtsam ${GTEST_LIBS}) + FetchContent_GetProperties(matplotlibcpp) + if(NOT matplotlibcpp_POPULATED) + FetchContent_Populate(matplotlibcpp) + + find_package(Python3 COMPONENTS Interpreter Development REQUIRED) + find_package(Python3 COMPONENTS NumPy) + + add_library(matplotlibcpp INTERFACE) + target_include_directories(matplotlibcpp INTERFACE ${matplotlibcpp_SOURCE_DIR}) + target_compile_features(matplotlibcpp INTERFACE cxx_std_11) + target_link_libraries(matplotlibcpp INTERFACE Python3::Python Python3::Module) + if(Python3_NumPy_FOUND) + target_link_libraries(matplotlibcpp INTERFACE Python3::NumPy) + else() + target_compile_definitions(matplotlibcpp INTERFACE WITHOUT_NUMPY) + endif() + endif() + + target_compile_definitions(testDCSAM PRIVATE ENABLE_PLOTTING=1) + target_link_libraries(testDCSAM matplotlibcpp) endif() add_test(TestDCSAM testDCSAM) diff --git a/tests/testDCSAM.cpp b/tests/testDCSAM.cpp index 6a8592c..f1f30ec 100644 --- a/tests/testDCSAM.cpp +++ b/tests/testDCSAM.cpp @@ -16,14 +16,14 @@ #include #include #include -#include +#include #include #include #include #ifdef ENABLE_PLOTTING -#include "matplotlibcpp.h" +#include #endif // Our custom DCSAM includes @@ -39,16 +39,11 @@ const double tol = 1e-7; -using namespace std; -using namespace dcsam; +/******************************************************************************/ #ifdef ENABLE_PLOTTING namespace plt = matplotlibcpp; -#endif -/******************************************************************************/ - -// TEMP just for plotting // thx @ github gist template std::vector linspace(T a, T b, size_t N) { @@ -59,6 +54,7 @@ std::vector linspace(T a, T b, size_t N) { for (x = xs.begin(), val = a; x != xs.end(); ++x, val += h) *x = val; return xs; } +#endif /* * Test simple DiscretePriorFactor. We construct a gtsam::DiscreteFactor factor @@ -80,14 +76,14 @@ TEST(TestSuite, discrete_prior_factor) { const std::vector probs{0.1, 0.9}; // Make a discrete prior factor and add it to the graph - DiscretePriorFactor dpf(dk, probs); + dcsam::DiscretePriorFactor dpf(dk, probs); dfg.push_back(dpf); // Solve dcsam::DiscreteValues mostProbableEstimate = dfg.optimize(); // Get the most probable estimate - size_t mpeD = mostProbableEstimate.at(dk.first); + const size_t mpeD = mostProbableEstimate.at(dk.first); // Get the marginals gtsam::DiscreteMarginals discreteMarginals(dfg); @@ -104,7 +100,7 @@ TEST(TestSuite, discrete_prior_factor) { } /* - * Test update-able SmartDiscretePriorFactor/ We construct a + * Test update-able SmartDiscretePriorFactor. We construct a * gtsam::DiscreteFactor factor graph with a single binary variable d1 and a * single custom SmartDiscretePriorFactor p(d1) specifying the distribution: * p(d1 = 0) = 0.1, p(d1 = 1) = 0.9. @@ -127,7 +123,7 @@ TEST(TestSuite, smart_discrete_prior_factor) { const std::vector probs{0.1, 0.9}; // Make a discrete prior factor and add it to the graph - SmartDiscretePriorFactor dpf(dk, probs); + dcsam::SmartDiscretePriorFactor dpf(dk, probs); dfg.push_back(dpf); // Solve @@ -145,8 +141,8 @@ TEST(TestSuite, smart_discrete_prior_factor) { // Update the factor const std::vector newProbs{0.9, 0.1}; - boost::shared_ptr smart = - boost::dynamic_pointer_cast(dfg[0]); + boost::shared_ptr smart = + boost::dynamic_pointer_cast(dfg[0]); if (smart) smart->updateProbs(newProbs); // Solve @@ -181,7 +177,7 @@ TEST(TestSuite, smart_discrete_prior_factor) { */ TEST(TestSuite, dcdiscrete_mixture) { // Make an empty discrete factor graph - DCFactorGraph dcfg; + dcsam::DCFactorGraph dcfg; // We'll make a variable with 2 possible assignments const size_t cardinality = 2; @@ -203,12 +199,12 @@ TEST(TestSuite, dcdiscrete_mixture) { const double sigmaNullHypo = 8.0; gtsam::noiseModel::Isotropic::shared_ptr prior_noiseNullHypo = gtsam::noiseModel::Isotropic::Sigma(1, sigmaNullHypo); - gtsam::PriorFactor fNullHypo(x1, loc, prior_noiseNullHypo); + std::vector> factorComponents{f1, fNullHypo}; - DCMixtureFactor> dcMixture(keys, dk, - factorComponents); + dcsam::DCMixtureFactor> dcMixture(keys, dk, + factorComponents); dcfg.push_back(dcMixture); gtsam::DiscreteKey dkTest = dcMixture.discreteKeys()[0]; @@ -222,7 +218,7 @@ TEST(TestSuite, dcdiscrete_mixture) { // We also need an initial guess for the discrete variables (this will only be // used if it is needed by your factors), here it is ignored. - DiscreteValues initialGuessDiscrete; + dcsam::DiscreteValues initialGuessDiscrete; initialGuessDiscrete[dk.first] = 0; // Let's make a discrete factor graph @@ -230,14 +226,14 @@ TEST(TestSuite, dcdiscrete_mixture) { // Pack DCMixture into a DCDiscreteFactor for (auto& it : dcfg) { - DCDiscreteFactor dcDiscrete(it->discreteKeys(), it); + dcsam::DCDiscreteFactor dcDiscrete(it->discreteKeys(), it); dfg.push_back(dcDiscrete); } // Update continuous info for (size_t j = 0; j < dfg.size(); j++) { - boost::shared_ptr dcDiscreteFactor = - boost::dynamic_pointer_cast(dfg[j]); + boost::shared_ptr dcDiscreteFactor = + boost::dynamic_pointer_cast(dfg[j]); if (dcDiscreteFactor) { dcDiscreteFactor->updateContinuous(initialGuess); dcDiscreteFactor->updateDiscrete(initialGuessDiscrete); @@ -248,7 +244,7 @@ TEST(TestSuite, dcdiscrete_mixture) { dcsam::DiscreteValues mostProbableEstimate = dfg.optimize(); // Get the most probable estimate - size_t mpeD = mostProbableEstimate.at(dk.first); + const size_t mpeD = mostProbableEstimate.at(dk.first); // Get the marginals gtsam::DiscreteMarginals newDiscreteMarginals(dfg); @@ -275,7 +271,7 @@ TEST(TestSuite, dcdiscrete_mixture) { */ TEST(TestSuite, dccontinuous_mixture) { // Make an empty discrete factor graph - DCFactorGraph dcfg; + dcsam::DCFactorGraph dcfg; // We'll make a variable with 2 possible assignments const size_t cardinality = 2; @@ -301,8 +297,8 @@ TEST(TestSuite, dccontinuous_mixture) { gtsam::PriorFactor fNullHypo(x1, loc, prior_noiseNullHypo); std::vector> factorComponents{f1, fNullHypo}; - DCMixtureFactor> dcMixture(keys, dk, - factorComponents); + dcsam::DCMixtureFactor> dcMixture(keys, dk, + factorComponents); dcfg.push_back(dcMixture); gtsam::DiscreteKey dkTest = dcMixture.discreteKeys()[0]; @@ -313,7 +309,7 @@ TEST(TestSuite, dccontinuous_mixture) { #ifdef ENABLE_PLOTTING // Query cost function std::vector xs = linspace(-5.0, 5.0, 50); - DiscreteValues dv1, dvNH; + dcsam::DiscreteValues dv1, dvNH; dv1[dk.first] = 0; dvNH[dk.first] = 1; std::vector errors1; @@ -326,8 +322,12 @@ TEST(TestSuite, dccontinuous_mixture) { xvals.clear(); } - plt::plot(xs, errors1); - plt::plot(xs, errorsNH); + plt::plot(xs, errors1, {{"label", "-log(p(x|d=0))"}}); + plt::plot(xs, errorsNH, {{"label", "-log(p(x|d=1))"}}); + plt::xlabel("Value of r.v. X"); + plt::ylabel("Negative Log-Likelihood"); + plt::grid(true); + plt::legend(); #endif // Let's make an initial guess for the continuous values @@ -341,13 +341,13 @@ TEST(TestSuite, dccontinuous_mixture) { std::vector initError1{dcMixture.error(initialGuess, dv1)}; std::vector initErrorNH{dcMixture.error(initialGuess, dvNH)}; - plt::scatter(initVec, initError1, {{"color", "r"}}); - plt::scatter(initVec, initErrorNH, {{"color", "r"}}); + plt::scatter(initVec, initError1, 15, {{"color", "r"}}); + plt::scatter(initVec, initErrorNH, 15, {{"color", "r"}}); #endif // We also need an initial guess for the discrete variables (this will only be // used if it is needed by your factors), here it is ignored. - DiscreteValues initialGuessDiscrete; + dcsam::DiscreteValues initialGuessDiscrete; initialGuessDiscrete[dk.first] = 0; // Let's make some factor graphs @@ -356,16 +356,16 @@ TEST(TestSuite, dccontinuous_mixture) { // Pack DCMixture into a DCContinuousFactor for (auto& it : dcfg) { - DCDiscreteFactor dcDiscrete(it); - DCContinuousFactor dcContinuous(it); + dcsam::DCDiscreteFactor dcDiscrete(it); + dcsam::DCContinuousFactor dcContinuous(it); dfg.push_back(dcDiscrete); graph.push_back(dcContinuous); } // Update continuous info inside DCDiscreteFactor for (size_t j = 0; j < dfg.size(); j++) { - boost::shared_ptr dcDiscreteFactor = - boost::dynamic_pointer_cast(dfg[j]); + boost::shared_ptr dcDiscreteFactor = + boost::dynamic_pointer_cast(dfg[j]); if (dcDiscreteFactor) { dcDiscreteFactor->updateContinuous(initialGuess); dcDiscreteFactor->updateDiscrete(initialGuessDiscrete); @@ -387,8 +387,8 @@ TEST(TestSuite, dccontinuous_mixture) { // Update discrete info inside DCContinuousFactor for (size_t j = 0; j < graph.size(); j++) { - boost::shared_ptr dcContinuousFactor = - boost::dynamic_pointer_cast(graph[j]); + boost::shared_ptr dcContinuousFactor = + boost::dynamic_pointer_cast(graph[j]); if (dcContinuousFactor) dcContinuousFactor->updateDiscrete(mostProbableEstimate); } @@ -410,15 +410,15 @@ TEST(TestSuite, dccontinuous_mixture) { std::vector updatedError1{dcMixture.error(values, dv1)}; std::vector updatedErrorNH{dcMixture.error(values, dvNH)}; - plt::scatter(updatedVec, updatedError1, {{"color", "b"}}); - plt::scatter(updatedVec, updatedErrorNH, {{"color", "b"}}); + plt::scatter(updatedVec, updatedError1, 15, {{"color", "b"}}); + plt::scatter(updatedVec, updatedErrorNH, 15, {{"color", "b"}}); plt::show(); #endif // Now update the continuous info in the discrete solver for (size_t j = 0; j < dfg.size(); j++) { - boost::shared_ptr dcDiscreteFactor = - boost::dynamic_pointer_cast(dfg[j]); + boost::shared_ptr dcDiscreteFactor = + boost::dynamic_pointer_cast(dfg[j]); if (dcDiscreteFactor) dcDiscreteFactor->updateContinuous(values); // NOTE: we won't updateDiscrete explicitly here anymore, because we don't // need to. @@ -464,11 +464,11 @@ TEST(TestSuite, simple_mixture_factor) { gtsam::PriorFactor fNullHypo(x1, loc, prior_noiseNullHypo); std::vector> factorComponents{f1, fNullHypo}; - DCMixtureFactor> dcMixture(keys, dk, - factorComponents); + dcsam::DCMixtureFactor> dcMixture(keys, dk, + factorComponents); // Make an empty hybrid factor graph - HybridFactorGraph hfg; + dcsam::HybridFactorGraph hfg; hfg.push_dc(dcMixture); @@ -479,7 +479,7 @@ TEST(TestSuite, simple_mixture_factor) { // Plot the cost functions for each hypothesis #ifdef ENABLE_PLOTTING std::vector xs = linspace(-5.0, 5.0, 50); - DiscreteValues dv1, dvNH; + dcsam::DiscreteValues dv1, dvNH; dv1[dk.first] = 0; dvNH[dk.first] = 1; std::vector errors1; @@ -492,8 +492,12 @@ TEST(TestSuite, simple_mixture_factor) { xvals.clear(); } - plt::plot(xs, errors1); - plt::plot(xs, errorsNH); + plt::plot(xs, errors1, {{"label", "-log(p(x|d=0))"}}); + plt::plot(xs, errorsNH, {{"label", "-log(p(x|d=1))"}}); + plt::xlabel("Value of r.v. X"); + plt::ylabel("Negative Log-Likelihood"); + plt::grid(true); + plt::legend(); #endif // Let's make an initial guess @@ -507,23 +511,23 @@ TEST(TestSuite, simple_mixture_factor) { std::vector initError1{dcMixture.error(initialGuess, dv1)}; std::vector initErrorNH{dcMixture.error(initialGuess, dvNH)}; - plt::scatter(initVec, initError1, {{"color", "r"}}); - plt::scatter(initVec, initErrorNH, {{"color", "r"}}); + plt::scatter(initVec, initError1, 15, {{"color", "r"}}); + plt::scatter(initVec, initErrorNH, 15, {{"color", "r"}}); #endif // We also need an initial guess for the discrete variables (this will only be // used if it is needed by your factors), here it is ignored. - DiscreteValues initialGuessDiscrete; + dcsam::DiscreteValues initialGuessDiscrete; initialGuessDiscrete[dk.first] = 0; // Let's make a solver - DCSAM dcsam; + dcsam::DCSAM dcsam; // Add the HybridFactorGraph to DCSAM dcsam.update(hfg, initialGuess, initialGuessDiscrete); // Solve - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); // Run another iteration dcsam.update(); @@ -537,17 +541,13 @@ TEST(TestSuite, simple_mixture_factor) { std::vector updatedError1{dcMixture.error(dcvals.continuous, dv1)}; std::vector updatedErrorNH{dcMixture.error(dcvals.continuous, dvNH)}; - plt::scatter(updatedVec, updatedError1, {{"color", "b"}}); - plt::scatter(updatedVec, updatedErrorNH, {{"color", "b"}}); - plt::show(); -#endif - -#ifdef ENABLE_PLOTTING + plt::scatter(updatedVec, updatedError1, 15, {{"color", "b"}}); + plt::scatter(updatedVec, updatedErrorNH, 15, {{"color", "b"}}); plt::show(); #endif // Ensure that the prediction is correct - size_t mpeD = dcvals.discrete.at(dk.first); + const size_t mpeD = dcvals.discrete.at(dk.first); EXPECT_EQ(mpeD, 0); } @@ -557,7 +557,7 @@ TEST(TestSuite, simple_mixture_factor) { */ TEST(TestSuite, simple_slam_batch) { // Make a hybrid factor graph - HybridFactorGraph graph; + dcsam::HybridFactorGraph graph; // Values for initial guess gtsam::Values initialGuess; @@ -565,8 +565,8 @@ TEST(TestSuite, simple_slam_batch) { gtsam::Symbol x0('x', 0); gtsam::Pose2 pose0(0, 0, 0); gtsam::Pose2 dx(1, 0, 0.78539816); - double prior_sigma = 0.1; - double meas_sigma = 1.0; + const double prior_sigma = 0.1; + const double meas_sigma = 1.0; gtsam::noiseModel::Isotropic::shared_ptr prior_noise = gtsam::noiseModel::Isotropic::Sigma(3, prior_sigma); @@ -579,7 +579,7 @@ TEST(TestSuite, simple_slam_batch) { graph.push_nonlinear(p0); // Setup dcsam - DCSAM dcsam; + dcsam::DCSAM dcsam; gtsam::Pose2 odom(pose0); gtsam::Pose2 noise(0.01, 0.01, 0.01); @@ -600,7 +600,7 @@ TEST(TestSuite, simple_slam_batch) { gtsam::BetweenFactor bw(x0, x7, dx * noise, meas_noise); dcsam.update(graph, initialGuess); - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); // Plot the robot positions #ifdef ENABLE_PLOTTING @@ -611,6 +611,7 @@ TEST(TestSuite, simple_slam_batch) { } plt::plot(xs, ys); + plt::grid(true); plt::show(); #endif @@ -623,7 +624,7 @@ TEST(TestSuite, simple_slam_batch) { */ TEST(TestSuite, simple_slam_incremental) { // Make a factor graph - HybridFactorGraph graph; + dcsam::HybridFactorGraph graph; // Values for initial guess gtsam::Values initialGuess; @@ -631,8 +632,8 @@ TEST(TestSuite, simple_slam_incremental) { gtsam::Symbol x0('x', 0); gtsam::Pose2 pose0(0, 0, 0); gtsam::Pose2 dx(1, 0, 0.78539816); - double prior_sigma = 0.1; - double meas_sigma = 1.0; + const double prior_sigma = 0.1; + const double meas_sigma = 1.0; gtsam::noiseModel::Isotropic::shared_ptr prior_noise = gtsam::noiseModel::Isotropic::Sigma(3, prior_sigma); @@ -645,7 +646,7 @@ TEST(TestSuite, simple_slam_incremental) { graph.push_nonlinear(p0); // Setup dcsam - DCSAM dcsam; + dcsam::DCSAM dcsam; dcsam.update(graph, initialGuess); graph.clear(); @@ -674,7 +675,7 @@ TEST(TestSuite, simple_slam_incremental) { gtsam::BetweenFactor bw(x0, x7, dx * noise, meas_noise); dcsam.update(graph, initialGuess); - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); // Plot the robot positions #ifdef ENABLE_PLOTTING @@ -684,6 +685,7 @@ TEST(TestSuite, simple_slam_incremental) { ys.push_back(dcvals.continuous.at(gtsam::Symbol('x', i)).y()); } plt::plot(xs, ys); + plt::grid(true); plt::show(); #endif @@ -697,10 +699,10 @@ TEST(TestSuite, simple_slam_incremental) { */ TEST(TestSuite, simple_discrete_dcsam) { // Create a DCSAM instance - DCSAM dcsam; + dcsam::DCSAM dcsam; // Make an empty hybrid factor graph - HybridFactorGraph hfg; + dcsam::HybridFactorGraph hfg; // We'll make a variable with 2 possible assignments const size_t cardinality = 2; @@ -708,21 +710,21 @@ TEST(TestSuite, simple_discrete_dcsam) { const std::vector probs{0.1, 0.9}; // Make a discrete prior factor and add it to the graph - DiscretePriorFactor dpf(dk, probs); + dcsam::DiscretePriorFactor dpf(dk, probs); hfg.push_discrete(dpf); // Initial guess for discrete values (only used in certain circumstances) - DiscreteValues initialGuessDiscrete; + dcsam::DiscreteValues initialGuessDiscrete; initialGuessDiscrete[dk.first] = 1; // Update DCSAM with the new factor dcsam.update(hfg, initialGuessDiscrete); // Solve - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); // Get the most probable estimate - size_t mpeD = dcvals.discrete.at(dk.first); + const size_t mpeD = dcvals.discrete.at(dk.first); // Ensure that the prediction is correct EXPECT_EQ(mpeD, 1); @@ -735,12 +737,12 @@ TEST(TestSuite, simple_discrete_dcsam) { */ TEST(TestSuite, simple_semantic_slam) { // Make a factor graph - HybridFactorGraph hfg; + dcsam::HybridFactorGraph hfg; // Values for initial guess gtsam::Values initialGuess; // Initial guess for discrete values (only used in certain circumstances) - DiscreteValues initialGuessDiscrete; + dcsam::DiscreteValues initialGuessDiscrete; gtsam::Symbol x0('x', 0); gtsam::Symbol l1('l', 1); @@ -749,9 +751,9 @@ TEST(TestSuite, simple_semantic_slam) { gtsam::DiscreteKey lm1_class(lc1, 2); gtsam::Pose2 pose0(0, 0, 0); gtsam::Pose2 dx(1, 0, 0.78539816); - double prior_sigma = 0.1; - double meas_sigma = 1.0; - double circumradius = (std::sqrt(4 + 2 * std::sqrt(2))) / 2.0; + const double prior_sigma = 0.1; + const double meas_sigma = 1.0; + const double circumradius = (std::sqrt(4 + 2 * std::sqrt(2))) / 2.0; gtsam::Point2 landmark1(circumradius, circumradius); gtsam::noiseModel::Isotropic::shared_ptr prior_noise = @@ -770,8 +772,8 @@ TEST(TestSuite, simple_semantic_slam) { prior_lm1_class.push_back(0.1); gtsam::PriorFactor p0(x0, pose0, prior_noise); - gtsam::PriorFactor pl1(l1, landmark1, prior_lm_noise); - DiscretePriorFactor plc1(lm1_class, prior_lm1_class); + // gtsam::PriorFactor pl1(l1, landmark1, prior_lm_noise); + dcsam::DiscretePriorFactor plc1(lm1_class, prior_lm1_class); initialGuess.insert(x0, pose0); initialGuess.insert(l1, landmark1); @@ -784,10 +786,10 @@ TEST(TestSuite, simple_semantic_slam) { hfg.push_discrete(plc1); // Setup dcsam - DCSAM dcsam; + dcsam::DCSAM dcsam; dcsam.update(hfg, initialGuess, initialGuessDiscrete); - DCValues dcval_start = dcsam.calculateEstimate(); + dcsam::DCValues dcval_start = dcsam.calculateEstimate(); hfg.clear(); initialGuess.clear(); @@ -806,7 +808,7 @@ TEST(TestSuite, simple_semantic_slam) { // Add bearing range measurement to landmark in center gtsam::Rot2 bearing1 = gtsam::Rot2::fromDegrees(67.5); - double range1 = circumradius; + const double range1 = circumradius; hfg.push_nonlinear(gtsam::BearingRangeFactor( xi, l1, bearing1, range1, br_noise)); @@ -820,16 +822,16 @@ TEST(TestSuite, simple_semantic_slam) { semantic_meas.push_back(0.1); semantic_meas.push_back(0.9); } - DiscretePriorFactor dpf(lm1_class, semantic_meas); + dcsam::DiscretePriorFactor dpf(lm1_class, semantic_meas); hfg.push_discrete(dpf); odom = odom * meas; initialGuess.insert(xj, odom); dcsam.update(hfg, initialGuess); - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); - size_t mpeClassL1 = dcvals.discrete.at(lc1); + const size_t mpeClassL1 = dcvals.discrete.at(lc1); // Plot poses and landmarks #ifdef ENABLE_PLOTTING @@ -847,10 +849,11 @@ TEST(TestSuite, simple_semantic_slam) { lmys.push_back( dcvals.continuous.at(gtsam::Symbol('l', 1)).y()); - string color = (mpeClassL1 == 0) ? "b" : "orange"; + std::string color = (mpeClassL1 == 0) ? "b" : "orange"; plt::plot(xs, ys); - plt::scatter(lmxs, lmys, {{"color", color}}); + plt::scatter(lmxs, lmys, 30, {{"color", color}}); + plt::grid(true); plt::show(); #endif @@ -864,9 +867,9 @@ TEST(TestSuite, simple_semantic_slam) { hfg.push_nonlinear(bw); dcsam.update(hfg, initialGuess); - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); - size_t mpeClassL1 = dcvals.discrete.at(lc1); + const size_t mpeClassL1 = dcvals.discrete.at(lc1); // Plot the poses and landmarks #ifdef ENABLE_PLOTTING @@ -882,10 +885,11 @@ TEST(TestSuite, simple_semantic_slam) { lmys.push_back( dcvals.continuous.at(gtsam::Symbol('l', 1)).y()); - string color = (mpeClassL1 == 0) ? "b" : "orange"; + std::string color = (mpeClassL1 == 0) ? "b" : "orange"; plt::plot(xs, ys); - plt::scatter(lmxs, lmys, {{"color", color}}); + plt::scatter(lmxs, lmys, 30, {{"color", color}}); + plt::grid(true); plt::show(); #endif @@ -902,12 +906,12 @@ TEST(TestSuite, simple_semantic_slam) { */ TEST(TestSuite, dcsam_initialization) { // Make a factor graph. - HybridFactorGraph hfg; + dcsam::HybridFactorGraph hfg; // Values for initial guess. gtsam::Values initialGuess; // Initial guess for discrete variables. - DiscreteValues initialGuessDiscrete; + dcsam::DiscreteValues initialGuessDiscrete; gtsam::Symbol x0('x', 0); gtsam::Symbol l1('l', 1); @@ -919,8 +923,8 @@ TEST(TestSuite, dcsam_initialization) { // Set up initial pose gtsam::Pose2 pose0(0, 0, 0); - double prior_sigma = 0.1; - double meas_sigma = 1.0; + const double prior_sigma = 0.1; + const double meas_sigma = 1.0; gtsam::Point2 landmark1(1.0, 1.0); /// Noise models for measurements and priors @@ -941,8 +945,8 @@ TEST(TestSuite, dcsam_initialization) { // Add prior factors to the graph and solve. gtsam::PriorFactor p0(x0, pose0, prior_noise); - gtsam::PriorFactor pl1(l1, landmark1, prior_lm_noise); - DiscretePriorFactor plc1(lm1_class, prior_lm1_class); + // gtsam::PriorFactor pl1(l1, landmark1, prior_lm_noise); + dcsam::DiscretePriorFactor plc1(lm1_class, prior_lm1_class); initialGuess.insert(x0, pose0); initialGuess.insert(l1, landmark1); @@ -953,10 +957,10 @@ TEST(TestSuite, dcsam_initialization) { hfg.push_discrete(plc1); // Setup dcsam - DCSAM dcsam; + dcsam::DCSAM dcsam; dcsam.update(hfg, initialGuess, initialGuessDiscrete); - DCValues dcval_start = dcsam.calculateEstimate(); + dcsam::DCValues dcval_start = dcsam.calculateEstimate(); std::cout << "Printing first values" << std::endl; dcval_start.discrete.print(); @@ -974,13 +978,13 @@ TEST(TestSuite, dcsam_initialization) { // landmark (x, y) = (1, 1) gtsam::Rot2 bearing = gtsam::Rot2::fromDegrees(45); double range = sqrt(2); - gtsam::BearingRangeFactor brfactor( - x0, l1, bearing, range, br_noise); + // gtsam::BearingRangeFactor brfactor( + // x0, l1, bearing, range, br_noise); // Set a semantic bearing-range factor up with BR measurement above and // semantic measurement equal to the landmark class prior. gtsam::KeyVector lm_keys{x0, l1}; - hfg.push_dc(SemanticBearingRangeFactor( + hfg.push_dc(dcsam::SemanticBearingRangeFactor( x0, l1, lm1_class, prior_lm1_class, bearing, range, br_noise)); // If DCSAM isn't initializing `x0` and `l1` properly for the new factor, this @@ -988,7 +992,7 @@ TEST(TestSuite, dcsam_initialization) { dcsam.update(hfg); // Attempt a solve if we made it this far. - DCValues dcval_final = dcsam.calculateEstimate(); + dcsam::DCValues dcval_final = dcsam.calculateEstimate(); // If we made it here without an AssertionError, the test passed. EXPECT_EQ(true, true); @@ -1002,11 +1006,11 @@ TEST(TestSuite, dcsam_initialization) { */ TEST(TestSuite, bearing_range_semantic_slam) { // Make a factor graph - HybridFactorGraph hfg; + dcsam::HybridFactorGraph hfg; // Values for initial guess gtsam::Values initialGuess; - DiscreteValues initialGuessDiscrete; + dcsam::DiscreteValues initialGuessDiscrete; gtsam::Symbol x0('x', 0); gtsam::Symbol l1('l', 1); @@ -1015,9 +1019,9 @@ TEST(TestSuite, bearing_range_semantic_slam) { gtsam::DiscreteKey lm1_class(lc1, 2); gtsam::Pose2 pose0(0, 0, 0); gtsam::Pose2 dx(1, 0, 0.78539816); - double prior_sigma = 0.1; - double meas_sigma = 1.0; - double circumradius = (std::sqrt(4 + 2 * std::sqrt(2))) / 2.0; + const double prior_sigma = 0.1; + const double meas_sigma = 1.0; + const double circumradius = (std::sqrt(4 + 2 * std::sqrt(2))) / 2.0; gtsam::Point2 landmark1(circumradius, circumradius); gtsam::noiseModel::Isotropic::shared_ptr prior_noise = @@ -1037,7 +1041,7 @@ TEST(TestSuite, bearing_range_semantic_slam) { gtsam::PriorFactor p0(x0, pose0, prior_noise); gtsam::PriorFactor pl1(l1, landmark1, prior_lm_noise); - DiscretePriorFactor plc1(lm1_class, prior_lm1_class); + dcsam::DiscretePriorFactor plc1(lm1_class, prior_lm1_class); initialGuess.insert(x0, pose0); initialGuess.insert(l1, landmark1); @@ -1048,10 +1052,10 @@ TEST(TestSuite, bearing_range_semantic_slam) { hfg.push_discrete(plc1); // Setup dcsam - DCSAM dcsam; + dcsam::DCSAM dcsam; dcsam.update(hfg, initialGuess, initialGuessDiscrete); - DCValues dcval_start = dcsam.calculateEstimate(); + dcsam::DCValues dcval_start = dcsam.calculateEstimate(); std::cout << "Printing first values" << std::endl; dcval_start.discrete.print(); @@ -1072,7 +1076,7 @@ TEST(TestSuite, bearing_range_semantic_slam) { // Add semantic bearing-range measurement to landmark in center gtsam::Rot2 bearing1 = gtsam::Rot2::fromDegrees(67.5); - double range1 = circumradius; + const double range1 = circumradius; // For the first couple measurements, pick class=0, later pick class=1 std::vector semantic_meas; @@ -1084,16 +1088,16 @@ TEST(TestSuite, bearing_range_semantic_slam) { semantic_meas.push_back(0.9); } - hfg.push_dc(SemanticBearingRangeFactor( + hfg.push_dc(dcsam::SemanticBearingRangeFactor( xi, l1, lm1_class, semantic_meas, bearing1, range1, br_noise)); odom = odom * meas; initialGuess.insert(xj, odom); dcsam.update(hfg, initialGuess); - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); - size_t mpeClassL1 = dcvals.discrete.at(lc1); + const size_t mpeClassL1 = dcvals.discrete.at(lc1); // Plot poses and landmarks #ifdef ENABLE_PLOTTING @@ -1111,10 +1115,11 @@ TEST(TestSuite, bearing_range_semantic_slam) { lmys.push_back( dcvals.continuous.at(gtsam::Symbol('l', 1)).y()); - string color = (mpeClassL1 == 0) ? "b" : "orange"; + std::string color = (mpeClassL1 == 0) ? "b" : "orange"; plt::plot(xs, ys); - plt::scatter(lmxs, lmys, {{"color", color}}); + plt::scatter(lmxs, lmys, 30, {{"color", color}}); + plt::grid(true); plt::show(); #endif @@ -1128,9 +1133,9 @@ TEST(TestSuite, bearing_range_semantic_slam) { hfg.push_nonlinear(bw); dcsam.update(hfg, initialGuess); - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); - size_t mpeClassL1 = dcvals.discrete.at(lc1); + const size_t mpeClassL1 = dcvals.discrete.at(lc1); // Plot the poses and landmarks #ifdef ENABLE_PLOTTING @@ -1146,10 +1151,11 @@ TEST(TestSuite, bearing_range_semantic_slam) { lmys.push_back( dcvals.continuous.at(gtsam::Symbol('l', 1)).y()); - string color = (mpeClassL1 == 0) ? "b" : "orange"; + std::string color = (mpeClassL1 == 0) ? "b" : "orange"; plt::plot(xs, ys); - plt::scatter(lmxs, lmys, {{"color", color}}); + plt::scatter(lmxs, lmys, 30, {{"color", color}}); + plt::grid(true); plt::show(); #endif @@ -1163,11 +1169,11 @@ TEST(TestSuite, bearing_range_semantic_slam) { */ TEST(TestSuite, dcMaxMixture_semantic_slam) { // Make a factor graph - HybridFactorGraph hfg; + dcsam::HybridFactorGraph hfg; // Values for initial guess gtsam::Values initialGuess; - DiscreteValues initialGuessDiscrete; + dcsam::DiscreteValues initialGuessDiscrete; gtsam::Symbol x0('x', 0); gtsam::Symbol l1('l', 1); @@ -1176,9 +1182,9 @@ TEST(TestSuite, dcMaxMixture_semantic_slam) { gtsam::DiscreteKey lm1_class(lc1, 2); gtsam::Pose2 pose0(0, 0, 0); gtsam::Pose2 dx(1, 0, 0.78539816); - double prior_sigma = 0.1; - double meas_sigma = 1.0; - double circumradius = (std::sqrt(4 + 2 * std::sqrt(2))) / 2.0; + const double prior_sigma = 0.1; + const double meas_sigma = 1.0; + const double circumradius = (std::sqrt(4 + 2 * std::sqrt(2))) / 2.0; gtsam::Point2 landmark1(circumradius, circumradius); gtsam::noiseModel::Isotropic::shared_ptr prior_noise = @@ -1198,7 +1204,7 @@ TEST(TestSuite, dcMaxMixture_semantic_slam) { gtsam::PriorFactor p0(x0, pose0, prior_noise); gtsam::PriorFactor pl1(l1, landmark1, prior_lm_noise); - DiscretePriorFactor plc1(lm1_class, prior_lm1_class); + dcsam::DiscretePriorFactor plc1(lm1_class, prior_lm1_class); initialGuess.insert(x0, pose0); initialGuess.insert(l1, landmark1); @@ -1220,7 +1226,7 @@ TEST(TestSuite, dcMaxMixture_semantic_slam) { prior_lm2_class.push_back(0.9); gtsam::PriorFactor pl2(l2, landmark2, prior_lm_noise); - DiscretePriorFactor plc2(lm2_class, prior_lm2_class); + dcsam::DiscretePriorFactor plc2(lm2_class, prior_lm2_class); initialGuess.insert(l2, landmark2); initialGuessDiscrete[lm2_class.first] = 1; @@ -1229,10 +1235,10 @@ TEST(TestSuite, dcMaxMixture_semantic_slam) { hfg.push_discrete(plc2); // Setup dcsam - DCSAM dcsam; + dcsam::DCSAM dcsam; dcsam.update(hfg, initialGuess, initialGuessDiscrete); - DCValues dcval_start = dcsam.calculateEstimate(); + dcsam::DCValues dcval_start = dcsam.calculateEstimate(); std::cout << "Printing first values" << std::endl; dcval_start.discrete.print(); @@ -1268,20 +1274,21 @@ TEST(TestSuite, dcMaxMixture_semantic_slam) { gtsam::DiscreteKeys dks({lm1_class, lm2_class}); // build mixture: dcmaxmixture should be picking the component for lm1 - SemanticBearingRangeFactor sbr1( + dcsam::SemanticBearingRangeFactor sbr1( xi, l1, lm1_class, semantic_meas, bearing1, range1, br_noise); - SemanticBearingRangeFactor sbr2( + dcsam::SemanticBearingRangeFactor sbr2( xi, l2, lm2_class, semantic_meas, bearing1, range1, br_noise); - DCMaxMixtureFactor> + dcsam::DCMaxMixtureFactor< + dcsam::SemanticBearingRangeFactor> dcmmf({xi, l1, l2}, dks, {sbr1, sbr2}, {.5, .5}, false); hfg.push_dc(dcmmf); odom = odom * meas; initialGuess.insert(xj, odom); dcsam.update(hfg, initialGuess); - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); - size_t mpeClassL1 = dcvals.discrete.at(lc1); + const size_t mpeClassL1 = dcvals.discrete.at(lc1); // Plot poses and landmarks #ifdef ENABLE_PLOTTING @@ -1299,10 +1306,11 @@ TEST(TestSuite, dcMaxMixture_semantic_slam) { lmys.push_back( dcvals.continuous.at(gtsam::Symbol('l', 1)).y()); - string color = (mpeClassL1 == 0) ? "b" : "orange"; + std::string color = (mpeClassL1 == 0) ? "b" : "orange"; plt::plot(xs, ys); - plt::scatter(lmxs, lmys, {{"color", color}}); + plt::scatter(lmxs, lmys, 30, {{"color", color}}); + plt::grid(true); plt::show(); #endif @@ -1316,9 +1324,9 @@ TEST(TestSuite, dcMaxMixture_semantic_slam) { hfg.push_nonlinear(bw); dcsam.update(hfg, initialGuess); - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); - size_t mpeClassL1 = dcvals.discrete.at(lc1); + const size_t mpeClassL1 = dcvals.discrete.at(lc1); // Plot the poses and landmarks #ifdef ENABLE_PLOTTING @@ -1334,10 +1342,11 @@ TEST(TestSuite, dcMaxMixture_semantic_slam) { lmys.push_back( dcvals.continuous.at(gtsam::Symbol('l', 1)).y()); - string color = (mpeClassL1 == 0) ? "b" : "orange"; + std::string color = (mpeClassL1 == 0) ? "b" : "orange"; plt::plot(xs, ys); - plt::scatter(lmxs, lmys, {{"color", color}}); + plt::scatter(lmxs, lmys, 30, {{"color", color}}); + plt::grid(true); plt::show(); #endif @@ -1346,11 +1355,11 @@ TEST(TestSuite, dcMaxMixture_semantic_slam) { TEST(TestSuite, simple_dcemfactor) { // Make a factor graph - HybridFactorGraph hfg; + dcsam::HybridFactorGraph hfg; // Values for initial guess gtsam::Values initialGuess; - DiscreteValues initialGuessDiscrete; + dcsam::DiscreteValues initialGuessDiscrete; gtsam::Symbol x0('x', 0); gtsam::Symbol l1('l', 1); @@ -1359,9 +1368,9 @@ TEST(TestSuite, simple_dcemfactor) { gtsam::DiscreteKey lm1_class(lc1, 2); gtsam::Pose2 pose0(0, 0, 0); gtsam::Pose2 dx(1, 0, 0.78539816); - double prior_sigma = 0.1; - double meas_sigma = 1.0; - double circumradius = (std::sqrt(4 + 2 * std::sqrt(2))) / 2.0; + const double prior_sigma = 0.1; + const double meas_sigma = 1.0; + const double circumradius = (std::sqrt(4 + 2 * std::sqrt(2))) / 2.0; gtsam::Point2 landmark1(circumradius, circumradius); gtsam::noiseModel::Isotropic::shared_ptr prior_noise = @@ -1381,7 +1390,7 @@ TEST(TestSuite, simple_dcemfactor) { gtsam::PriorFactor p0(x0, pose0, prior_noise); gtsam::PriorFactor pl1(l1, landmark1, prior_lm_noise); - DiscretePriorFactor plc1(lm1_class, prior_lm1_class); + dcsam::DiscretePriorFactor plc1(lm1_class, prior_lm1_class); initialGuess.insert(x0, pose0); initialGuess.insert(l1, landmark1); @@ -1403,7 +1412,7 @@ TEST(TestSuite, simple_dcemfactor) { prior_lm2_class.push_back(0.9); gtsam::PriorFactor pl2(l2, landmark2, prior_lm_noise); - DiscretePriorFactor plc2(lm2_class, prior_lm2_class); + dcsam::DiscretePriorFactor plc2(lm2_class, prior_lm2_class); initialGuess.insert(l2, landmark2); initialGuessDiscrete[lm2_class.first] = 1; @@ -1412,10 +1421,10 @@ TEST(TestSuite, simple_dcemfactor) { hfg.push_discrete(plc2); // Setup dcsam - DCSAM dcsam; + dcsam::DCSAM dcsam; dcsam.update(hfg, initialGuess, initialGuessDiscrete); - DCValues dcval_start = dcsam.calculateEstimate(); + dcsam::DCValues dcval_start = dcsam.calculateEstimate(); std::cout << "Printing first values" << std::endl; dcval_start.discrete.print(); @@ -1436,7 +1445,7 @@ TEST(TestSuite, simple_dcemfactor) { // Add semantic bearing-range measurement to landmark in center gtsam::Rot2 bearing1 = gtsam::Rot2::fromDegrees(67.5); - double range1 = circumradius; + const double range1 = circumradius; // For the first couple measurements, pick class=0, later pick class=1 std::vector semantic_meas; @@ -1451,20 +1460,21 @@ TEST(TestSuite, simple_dcemfactor) { gtsam::DiscreteKeys dks({lm1_class, lm2_class}); // build mixture: dcemfactor should be picking the component for lm1 - SemanticBearingRangeFactor sbr1( + dcsam::SemanticBearingRangeFactor sbr1( xi, l1, lm1_class, semantic_meas, bearing1, range1, br_noise); - SemanticBearingRangeFactor sbr2( + dcsam::SemanticBearingRangeFactor sbr2( xi, l2, lm2_class, semantic_meas, bearing1, range1, br_noise); - DCEMFactor> dcemf( - {xi, l1, l2}, dks, {sbr1, sbr2}, {.5, .5}, false); + dcsam::DCEMFactor< + dcsam::SemanticBearingRangeFactor> + dcemf({xi, l1, l2}, dks, {sbr1, sbr2}, {.5, .5}, false); hfg.push_dc(dcemf); odom = odom * meas; initialGuess.insert(xj, odom); dcsam.update(hfg, initialGuess); - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); - size_t mpeClassL1 = dcvals.discrete.at(lc1); + const size_t mpeClassL1 = dcvals.discrete.at(lc1); // Plot poses and landmarks #ifdef ENABLE_PLOTTING @@ -1482,10 +1492,11 @@ TEST(TestSuite, simple_dcemfactor) { lmys.push_back( dcvals.continuous.at(gtsam::Symbol('l', 1)).y()); - string color = (mpeClassL1 == 0) ? "b" : "orange"; + std::string color = (mpeClassL1 == 0) ? "b" : "orange"; plt::plot(xs, ys); - plt::scatter(lmxs, lmys, {{"color", color}}); + plt::scatter(lmxs, lmys, 30, {{"color", color}}); + plt::grid(true); plt::show(); #endif @@ -1499,9 +1510,9 @@ TEST(TestSuite, simple_dcemfactor) { hfg.push_nonlinear(bw); dcsam.update(hfg, initialGuess); - DCValues dcvals = dcsam.calculateEstimate(); + dcsam::DCValues dcvals = dcsam.calculateEstimate(); - size_t mpeClassL1 = dcvals.discrete.at(lc1); + const size_t mpeClassL1 = dcvals.discrete.at(lc1); // Plot the poses and landmarks #ifdef ENABLE_PLOTTING @@ -1517,10 +1528,11 @@ TEST(TestSuite, simple_dcemfactor) { lmys.push_back( dcvals.continuous.at(gtsam::Symbol('l', 1)).y()); - string color = (mpeClassL1 == 0) ? "b" : "orange"; + std::string color = (mpeClassL1 == 0) ? "b" : "orange"; plt::plot(xs, ys); - plt::scatter(lmxs, lmys, {{"color", color}}); + plt::scatter(lmxs, lmys, 30, {{"color", color}}); + plt::grid(true); plt::show(); #endif