diff --git a/.gitignore b/.gitignore index ad0e08aa1..03ce578e7 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ /build* +/debug* .idea *.pyc *.DS_Store diff --git a/CMakeLists.txt b/CMakeLists.txt index 186dabaf4..29b31f979 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -84,6 +84,12 @@ set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of python to build if((GTSAM_INSTALL_MATLAB_TOOLBOX OR GTSAM_INSTALL_CYTHON_TOOLBOX) AND NOT GTSAM_BUILD_WRAP) message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX or GTSAM_INSTALL_CYTHON_TOOLBOX is enabled, please also enable GTSAM_BUILD_WRAP") endif() +if((GTSAM_INSTALL_MATLAB_TOOLBOX OR GTSAM_INSTALL_CYTHON_TOOLBOX) AND GTSAM_BUILD_TYPE_POSTFIXES) + set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE}_POSTFIX}) + if(NOT "${CURRENT_POSTFIX}" STREQUAL "") + message(FATAL_ERROR "Cannot use executable postfixes with the matlab or cython wrappers. Please disable GTSAM_BUILD_TYPE_POSTFIXES") + endif() +endif() if(GTSAM_INSTALL_WRAP AND NOT GTSAM_BUILD_WRAP) message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP") endif() @@ -130,6 +136,14 @@ if(MSVC) endif() endif() +# If building DLLs in MSVC, we need to avoid EIGEN_STATIC_ASSERT() +# or explicit instantiation will generate build errors. +# See: https://bitbucket.org/gtborg/gtsam/issues/417/fail-to-build-on-msvc-2017 +# +if(MSVC AND BUILD_SHARED_LIBS) + list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT) +endif() + # Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such. set(BOOST_FIND_MINIMUM_VERSION 1.43) set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex) @@ -142,22 +156,43 @@ if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILE message(FATAL_ERROR "Missing required Boost components >= v1.43, please install/upgrade Boost or configure your search paths.") endif() -option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF) # Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library) +option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF) + +# JLBC: This was once updated to target-based names (Boost::xxx), but it caused +# problems with Boost versions newer than FindBoost.cmake was prepared to handle, +# so we downgraded this to classic filenames-based variables, and manually adding +# the target_include_directories(xxx ${Boost_INCLUDE_DIR}) set(GTSAM_BOOST_LIBRARIES - Boost::serialization - Boost::system - Boost::filesystem - Boost::thread - Boost::date_time - Boost::regex + optimized + ${Boost_SERIALIZATION_LIBRARY_RELEASE} + ${Boost_SYSTEM_LIBRARY_RELEASE} + ${Boost_FILESYSTEM_LIBRARY_RELEASE} + ${Boost_THREAD_LIBRARY_RELEASE} + ${Boost_DATE_TIME_LIBRARY_RELEASE} + ${Boost_REGEX_LIBRARY_RELEASE} + debug + ${Boost_SERIALIZATION_LIBRARY_DEBUG} + ${Boost_SYSTEM_LIBRARY_DEBUG} + ${Boost_FILESYSTEM_LIBRARY_DEBUG} + ${Boost_THREAD_LIBRARY_DEBUG} + ${Boost_DATE_TIME_LIBRARY_DEBUG} + ${Boost_REGEX_LIBRARY_DEBUG} ) +message(STATUS "GTSAM_BOOST_LIBRARIES: ${GTSAM_BOOST_LIBRARIES}") if (GTSAM_DISABLE_NEW_TIMERS) message("WARNING: GTSAM timing instrumentation manually disabled") list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS) else() if(Boost_TIMER_LIBRARY) - list(APPEND GTSAM_BOOST_LIBRARIES Boost::timer Boost::chrono) + list(APPEND GTSAM_BOOST_LIBRARIES + optimized + ${Boost_TIMER_LIBRARY_RELEASE} + ${Boost_CHRONO_LIBRARY_RELEASE} + debug + ${Boost_TIMER_LIBRARY_DEBUG} + ${Boost_CHRONO_LIBRARY_DEBUG} + ) else() list(APPEND GTSAM_BOOST_LIBRARIES rt) # When using the header-only boost timer library, need -lrt message("WARNING: GTSAM timing instrumentation will use the older, less accurate, Boost timer library because boost older than 1.48 was found.") diff --git a/CppUnitLite/CMakeLists.txt b/CppUnitLite/CMakeLists.txt index f52841274..72651aca9 100644 --- a/CppUnitLite/CMakeLists.txt +++ b/CppUnitLite/CMakeLists.txt @@ -6,7 +6,7 @@ file(GLOB cppunitlite_src "*.cpp") add_library(CppUnitLite STATIC ${cppunitlite_src} ${cppunitlite_headers}) list(APPEND GTSAM_EXPORTED_TARGETS CppUnitLite) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) -target_link_libraries(CppUnitLite PUBLIC Boost::boost) # boost/lexical_cast.h +target_include_directories(CppUnitLite PUBLIC ${Boost_INCLUDE_DIR}) # boost/lexical_cast.h gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC project structure diff --git a/CppUnitLite/Failure.h b/CppUnitLite/Failure.h index 67ac5ba38..c3aa702e9 100644 --- a/CppUnitLite/Failure.h +++ b/CppUnitLite/Failure.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/CppUnitLite/Test.cpp b/CppUnitLite/Test.cpp index 481fceb07..78995a219 100644 --- a/CppUnitLite/Test.cpp +++ b/CppUnitLite/Test.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -36,7 +36,7 @@ Test *Test::getNext() const } void Test::setNext(Test *test) -{ +{ next_ = test; } @@ -46,9 +46,9 @@ bool Test::check(long expected, long actual, TestResult& result, const std::stri return true; result.addFailure ( Failure ( - name_, + name_, boost::lexical_cast (__FILE__), - __LINE__, + __LINE__, boost::lexical_cast (expected), boost::lexical_cast (actual))); @@ -63,10 +63,10 @@ bool Test::check(const std::string& expected, const std::string& actual, TestRes return true; result.addFailure ( Failure ( - name_, + name_, boost::lexical_cast (__FILE__), - __LINE__, - expected, + __LINE__, + expected, actual)); return false; diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h index 23d1e2573..b1fb0cf08 100644 --- a/CppUnitLite/Test.h +++ b/CppUnitLite/Test.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -12,7 +12,7 @@ /////////////////////////////////////////////////////////////////////////////// // // TEST.H -// +// // This file contains the Test class along with the macros which make effective // in the harness. // @@ -66,7 +66,7 @@ protected: virtual ~testGroup##testName##Test () {};\ void run (TestResult& result_);} \ testGroup##testName##Instance; \ - void testGroup##testName##Test::run (TestResult& result_) + void testGroup##testName##Test::run (TestResult& result_) /** * Declare friend in a class to test its private methods diff --git a/CppUnitLite/TestHarness.h b/CppUnitLite/TestHarness.h index 273309aa5..65dc0d3f9 100644 --- a/CppUnitLite/TestHarness.h +++ b/CppUnitLite/TestHarness.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -12,7 +12,7 @@ /////////////////////////////////////////////////////////////////////////////// // // TESTHARNESS.H -// +// // The primary include file for the framework. // /////////////////////////////////////////////////////////////////////////////// diff --git a/CppUnitLite/TestRegistry.cpp b/CppUnitLite/TestRegistry.cpp index b493e81a6..2a123c88f 100644 --- a/CppUnitLite/TestRegistry.cpp +++ b/CppUnitLite/TestRegistry.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -18,26 +18,26 @@ #include "TestResult.h" #include "TestRegistry.h" -void TestRegistry::addTest (Test *test) +void TestRegistry::addTest (Test *test) { instance ().add (test); } -int TestRegistry::runAllTests (TestResult& result) +int TestRegistry::runAllTests (TestResult& result) { return instance ().run (result); } -TestRegistry& TestRegistry::instance () +TestRegistry& TestRegistry::instance () { static TestRegistry registry; return registry; } -void TestRegistry::add (Test *test) +void TestRegistry::add (Test *test) { if (tests == 0) { test->setNext(0); @@ -52,7 +52,7 @@ void TestRegistry::add (Test *test) } -int TestRegistry::run (TestResult& result) +int TestRegistry::run (TestResult& result) { result.testsStarted (); diff --git a/CppUnitLite/TestRegistry.h b/CppUnitLite/TestRegistry.h index 56db991ad..5c3ebc280 100644 --- a/CppUnitLite/TestRegistry.h +++ b/CppUnitLite/TestRegistry.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -12,9 +12,9 @@ /////////////////////////////////////////////////////////////////////////////// // // TESTREGISTRY.H -// -// TestRegistry is a singleton collection of all the tests to run in a system. -// +// +// TestRegistry is a singleton collection of all the tests to run in a system. +// /////////////////////////////////////////////////////////////////////////////// #ifndef TESTREGISTRY_H @@ -38,7 +38,7 @@ private: void add (Test *test); int run (TestResult& result); - + Test *tests; Test *lastTest; diff --git a/CppUnitLite/TestResult.cpp b/CppUnitLite/TestResult.cpp index 2519c94a9..594590d9e 100644 --- a/CppUnitLite/TestResult.cpp +++ b/CppUnitLite/TestResult.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -22,12 +22,12 @@ TestResult::TestResult () } -void TestResult::testsStarted () +void TestResult::testsStarted () { } -void TestResult::addFailure (const Failure& failure) +void TestResult::addFailure (const Failure& failure) { if (failure.lineNumber < 0) // allow for no line number fprintf (stdout, "%s%s%s%s\n", @@ -48,7 +48,7 @@ void TestResult::addFailure (const Failure& failure) } -void TestResult::testsEnded () +void TestResult::testsEnded () { if (failureCount > 0) fprintf (stdout, "There were %d failures\n", failureCount); diff --git a/CppUnitLite/TestResult.h b/CppUnitLite/TestResult.h index b64b40d75..3897d2990 100644 --- a/CppUnitLite/TestResult.h +++ b/CppUnitLite/TestResult.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -12,10 +12,10 @@ /////////////////////////////////////////////////////////////////////////////// // // TESTRESULT.H -// +// // A TestResult is a collection of the history of some test runs. Right now // it just collects failures. -// +// /////////////////////////////////////////////////////////////////////////////// #ifndef TESTRESULT_H @@ -26,12 +26,12 @@ class Failure; class TestResult { public: - TestResult (); + TestResult (); virtual ~TestResult() {}; virtual void testsStarted (); virtual void addFailure (const Failure& failure); virtual void testsEnded (); - + int getFailureCount() {return failureCount;} private: diff --git a/INSTALL.md b/INSTALL.md index 3437d074d..b8f73f153 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -166,3 +166,30 @@ Another useful debugging symbol is _GLIBCXX_DEBUG, which enables debug checks an NOTE: The native Snow Leopard g++ compiler/library contains a bug that makes it impossible to use _GLIBCXX_DEBUG. MacPorts g++ compilers do work with it though. NOTE: If _GLIBCXX_DEBUG is used to compile gtsam, anything that links against gtsam will need to be compiled with _GLIBCXX_DEBUG as well, due to the use of header-only Eigen. + + +## Installing MKL on Linux + +Intel has a guide for installing MKL on Linux through APT repositories at . + +After following the instructions, add the following to your `~/.bashrc` (and afterwards, open a new terminal before compiling GTSAM): +`LD_PRELOAD` need only be set if you are building the cython wrapper to use GTSAM from python. +```sh +source /opt/intel/mkl/bin/mklvars.sh intel64 +export LD_PRELOAD="$LD_PRELOAD:/opt/intel/mkl/lib/intel64/libmkl_core.so:/opt/intel/mkl/lib/intel64/libmkl_sequential.so" +``` +To use MKL in GTSAM pass the flag `-DGTSAM_WITH_EIGEN_MKL=ON` to cmake. + + +The `LD_PRELOAD` fix seems to be related to a well known problem with MKL which leads to lots of undefined symbol errors, for example: +- +- +- + +Failing to specify `LD_PRELOAD` may lead to errors such as: +`ImportError: /opt/intel/mkl/lib/intel64/libmkl_vml_avx2.so: undefined symbol: mkl_serv_getenv` +or +`Intel MKL FATAL ERROR: Cannot load libmkl_avx2.so or libmkl_def.so.` +when importing GTSAM using the cython wrapper in python. + + diff --git a/README.md b/README.md index 7e03c81f2..720517669 100644 --- a/README.md +++ b/README.md @@ -36,7 +36,9 @@ Prerequisites: Optional prerequisites - used automatically if findable by CMake: - [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`) -- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) +- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) (Ubuntu: [installing using APT](https://software.intel.com/en-us/articles/installing-intel-free-libs-and-python-apt-repo)) + - See [INSTALL.md](INSTALL.md) for more installation information + - Note that MKL may not provide a speedup in all cases. Make sure to benchmark your problem with and without MKL. GTSAM 4 Compatibility --------------------- diff --git a/cmake/example_project/CMakeLists.txt b/cmake/example_project/CMakeLists.txt index 2afc6edc2..620ad4811 100644 --- a/cmake/example_project/CMakeLists.txt +++ b/cmake/example_project/CMakeLists.txt @@ -30,9 +30,12 @@ find_package(GTSAM REQUIRED) # Uses installed package ################################################################################### # Build static library from common sources set(CONVENIENCE_LIB_NAME ${PROJECT_NAME}) -add_library(${CONVENIENCE_LIB_NAME} STATIC example/PrintExamples.h example/PrintExamples.cpp) +add_library(${CONVENIENCE_LIB_NAME} SHARED example/PrintExamples.h example/PrintExamples.cpp) target_link_libraries(${CONVENIENCE_LIB_NAME} gtsam) +# Install library +install(TARGETS ${CONVENIENCE_LIB_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) + ################################################################################### # Build tests (CMake tracks the dependecy to link with GTSAM through our project's static library) gtsamAddTestsGlob("example" "tests/test*.cpp" "" "${CONVENIENCE_LIB_NAME}") diff --git a/cmake/example_project/README.md b/cmake/example_project/README.md new file mode 100644 index 000000000..a1269cf19 --- /dev/null +++ b/cmake/example_project/README.md @@ -0,0 +1,32 @@ +# MATLAB Wrapper Example Project + +This project serves as a lightweight example for demonstrating how to wrap C++ code in MATLAB using GTSAM. + +## Compiling + +We follow the regular build procedure inside the `example_project` directory: + +```sh +mkdir build && cd build +cmake .. +make -j8 +sudo make install + +sudo ldconfig # ensures the shared object file generated is correctly loaded +``` + +## Usage + +Now you can open MATLAB and add the `gtsam_toolbox` to the MATLAB path + +```matlab +addpath('/usr/local/gtsam_toolbox') +``` + +At this point you are ready to run the example project. Starting from the `example_project` directory inside MATLAB, simply run code like regular MATLAB, e.g. + +```matlab +pe = example.PrintExamples(); +pe.sayHello(); +pe.sayGoodbye(); +``` \ No newline at end of file diff --git a/cmake/example_project/example.h b/cmake/example_project/example.h index 47b9a0aa2..b0d732e14 100644 --- a/cmake/example_project/example.h +++ b/cmake/example_project/example.h @@ -18,11 +18,14 @@ // This is an interface file for automatic MATLAB wrapper generation. See // gtsam.h for full documentation and more examples. +#include + namespace example { class PrintExamples { - void sayHello() const; - void sayGoodbye() const; + PrintExamples(); + void sayHello() const; + void sayGoodbye() const; }; } diff --git a/cmake/example_project/example/PrintExamples.h b/cmake/example_project/example/PrintExamples.h index b151dfbc7..25d4dd8cb 100644 --- a/cmake/example_project/example/PrintExamples.h +++ b/cmake/example_project/example/PrintExamples.h @@ -17,6 +17,7 @@ #pragma once +#include #include namespace example { diff --git a/cython/README.md b/cython/README.md index b9ce2f012..cb23b0d4a 100644 --- a/cython/README.md +++ b/cython/README.md @@ -62,7 +62,7 @@ Examples: noiseModel_Gaussian, noiseModel_mEstimator_Tukey - Casting from a base class to a derive class must be done explicitly. Examples: ```Python - noiseBase = factor.get_noiseModel() + noiseBase = factor.noiseModel() noiseGaussian = dynamic_cast_noiseModel_Gaussian_noiseModel_Base(noiseBase) ``` diff --git a/cython/gtsam/examples/DogLegOptimizerExample.py b/cython/gtsam/examples/DogLegOptimizerExample.py new file mode 100644 index 000000000..776ceedc4 --- /dev/null +++ b/cython/gtsam/examples/DogLegOptimizerExample.py @@ -0,0 +1,118 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Example comparing DoglegOptimizer with Levenberg-Marquardt. +Author: Frank Dellaert +""" +# pylint: disable=no-member, invalid-name + +import math +import argparse + +import gtsam +import matplotlib.pyplot as plt +import numpy as np + + +def run(args): + """Test Dogleg vs LM, inspired by issue #452.""" + + # print parameters + print("num samples = {}, deltaInitial = {}".format( + args.num_samples, args.delta)) + + # Ground truth solution + T11 = gtsam.Pose2(0, 0, 0) + T12 = gtsam.Pose2(1, 0, 0) + T21 = gtsam.Pose2(0, 1, 0) + T22 = gtsam.Pose2(1, 1, 0) + + # Factor graph + graph = gtsam.NonlinearFactorGraph() + + # Priors + prior = gtsam.noiseModel_Isotropic.Sigma(3, 1) + graph.add(gtsam.PriorFactorPose2(11, T11, prior)) + graph.add(gtsam.PriorFactorPose2(21, T21, prior)) + + # Odometry + model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.01, 0.01, 0.3])) + graph.add(gtsam.BetweenFactorPose2(11, 12, T11.between(T12), model)) + graph.add(gtsam.BetweenFactorPose2(21, 22, T21.between(T22), model)) + + # Range + model_rho = gtsam.noiseModel_Isotropic.Sigma(1, 0.01) + graph.add(gtsam.RangeFactorPose2(12, 22, 1.0, model_rho)) + + params = gtsam.DoglegParams() + params.setDeltaInitial(args.delta) # default is 10 + + # Add progressively more noise to ground truth + sigmas = [0.01, 0.1, 0.2, 0.5, 1, 2, 5, 10, 20] + n = len(sigmas) + p_dl, s_dl, p_lm, s_lm = [0]*n, [0]*n, [0]*n, [0]*n + for i, sigma in enumerate(sigmas): + dl_fails, lm_fails = 0, 0 + # Attempt num_samples optimizations for both DL and LM + for _attempt in range(args.num_samples): + initial = gtsam.Values() + initial.insert(11, T11.retract(np.random.normal(0, sigma, 3))) + initial.insert(12, T12.retract(np.random.normal(0, sigma, 3))) + initial.insert(21, T21.retract(np.random.normal(0, sigma, 3))) + initial.insert(22, T22.retract(np.random.normal(0, sigma, 3))) + + # Run dogleg optimizer + dl = gtsam.DoglegOptimizer(graph, initial, params) + result = dl.optimize() + dl_fails += graph.error(result) > 1e-9 + + # Run + lm = gtsam.LevenbergMarquardtOptimizer(graph, initial) + result = lm.optimize() + lm_fails += graph.error(result) > 1e-9 + + # Calculate Bayes estimate of success probability + # using a beta prior of alpha=0.5, beta=0.5 + alpha, beta = 0.5, 0.5 + v = args.num_samples+alpha+beta + p_dl[i] = (args.num_samples-dl_fails+alpha)/v + p_lm[i] = (args.num_samples-lm_fails+alpha)/v + + def stddev(p): + """Calculate standard deviation.""" + return math.sqrt(p*(1-p)/(1+v)) + + s_dl[i] = stddev(p_dl[i]) + s_lm[i] = stddev(p_lm[i]) + + fmt = "sigma= {}:\tDL success {:.2f}% +/- {:.2f}%, LM success {:.2f}% +/- {:.2f}%" + print(fmt.format(sigma, + 100*p_dl[i], 100*s_dl[i], + 100*p_lm[i], 100*s_lm[i])) + + if args.plot: + fig, ax = plt.subplots() + dl_plot = plt.errorbar(sigmas, p_dl, yerr=s_dl, label="Dogleg") + lm_plot = plt.errorbar(sigmas, p_lm, yerr=s_lm, label="LM") + plt.title("Dogleg emprical success vs. LM") + plt.legend(handles=[dl_plot, lm_plot]) + ax.set_xlim(0, sigmas[-1]+1) + ax.set_ylim(0, 1) + plt.show() + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description="Compare Dogleg and LM success rates") + parser.add_argument("-n", "--num_samples", type=int, default=1000, + help="Number of samples for each sigma") + parser.add_argument("-d", "--delta", type=float, default=10.0, + help="Initial delta for dogleg") + parser.add_argument("-p", "--plot", action="store_true", + help="Flag to plot results") + args = parser.parse_args() + run(args) diff --git a/cython/gtsam/examples/PlanarManipulatorExample.py b/cython/gtsam/examples/PlanarManipulatorExample.py index 73959b6c5..e42ae09d7 100644 --- a/cython/gtsam/examples/PlanarManipulatorExample.py +++ b/cython/gtsam/examples/PlanarManipulatorExample.py @@ -115,7 +115,7 @@ class ThreeLinkArm(object): l1Zl1 = expmap(0.0, 0.0, q[0]) l2Zl2 = expmap(0.0, self.L1, q[1]) - l3Zl3 = expmap(0.0, 7.0, q[2]) + l3Zl3 = expmap(0.0, self.L1+self.L2, q[2]) return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0) def ik(self, sTt_desired, e=1e-9): @@ -297,12 +297,18 @@ def run_example(): """ Use trajectory interpolation and then trajectory tracking a la Murray to move a 3-link arm on a straight line. """ + # Create arm arm = ThreeLinkArm() + + # Get initial pose using forward kinematics q = np.radians(vector3(30, -30, 45)) sTt_initial = arm.fk(q) + + # Create interpolated trajectory in task space to desired goal pose sTt_goal = Pose2(2.4, 4.3, math.radians(0)) poses = trajectory(sTt_initial, sTt_goal, 50) + # Setup figure and plot initial pose fignum = 0 fig = plt.figure(fignum) axes = fig.gca() @@ -310,6 +316,8 @@ def run_example(): axes.set_ylim(0, 10) gtsam_plot.plot_pose2(fignum, arm.fk(q)) + # For all poses in interpolated trajectory, calculate dq to move to next pose. + # We do this by calculating the local Jacobian J and doing dq = inv(J)*delta(sTt, pose). for pose in poses: sTt = arm.fk(q) error = delta(sTt, pose) diff --git a/cython/gtsam/tests/test_Pose3.py b/cython/gtsam/tests/test_Pose3.py index 3eb4067c9..577a13304 100644 --- a/cython/gtsam/tests/test_Pose3.py +++ b/cython/gtsam/tests/test_Pose3.py @@ -30,9 +30,9 @@ class TestPose3(GtsamTestCase): self.gtsamAssertEquals(actual, expected, 1e-6) def test_transform_to(self): - """Test transform_to method.""" + """Test transformTo method.""" transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0)) - actual = transform.transform_to(Point3(3, 2, 10)) + actual = transform.transformTo(Point3(3, 2, 10)) expected = Point3(2, 1, 10) self.gtsamAssertEquals(actual, expected, 1e-6) diff --git a/cython/gtsam/tests/test_dsf_map.py b/cython/gtsam/tests/test_dsf_map.py new file mode 100644 index 000000000..6eb551034 --- /dev/null +++ b/cython/gtsam/tests/test_dsf_map.py @@ -0,0 +1,38 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for Disjoint Set Forest. +Author: Frank Dellaert & Varun Agrawal +""" +# pylint: disable=invalid-name, no-name-in-module, no-member + +from __future__ import print_function + +import unittest + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestDSFMap(GtsamTestCase): + """Tests for DSFMap.""" + + def test_all(self): + """Test everything in DFSMap.""" + def key(index_pair): + return index_pair.i(), index_pair.j() + + dsf = gtsam.DSFMapIndexPair() + pair1 = gtsam.IndexPair(1, 18) + self.assertEqual(key(dsf.find(pair1)), key(pair1)) + pair2 = gtsam.IndexPair(2, 2) + dsf.merge(pair1, pair2) + self.assertTrue(dsf.find(pair1), dsf.find(pair1)) + + +if __name__ == '__main__': + unittest.main() diff --git a/cython/gtsam_eigency/eigency_cpp.h b/cython/gtsam_eigency/eigency_cpp.h index 923ca7c94..ce303182e 100644 --- a/cython/gtsam_eigency/eigency_cpp.h +++ b/cython/gtsam_eigency/eigency_cpp.h @@ -35,7 +35,7 @@ inline PyArrayObject *_ndarray_view(double *data, long rows, long cols, return ndarray_double_C(data, rows, cols, outer_stride>0?outer_stride:cols, inner_stride>0?inner_stride:1); } else { // Eigen column-major mode: row_stride=outer_stride, and col_stride=inner_stride - // If no stride is given, the cow_stride is set to the number of rows. + // If no stride is given, the cow_stride is set to the number of rows. return ndarray_double_F(data, rows, cols, inner_stride>0?inner_stride:1, outer_stride>0?outer_stride:rows); } } @@ -355,7 +355,7 @@ public: FlattenedMap() : Base(NULL, 0, 0), object_(NULL) {} - + FlattenedMap(Scalar *data, long rows, long cols, long outer_stride=0, long inner_stride=0) : Base(data, rows, cols, Eigen::Stride<_StrideOuter, _StrideInner>(outer_stride, inner_stride)), @@ -363,7 +363,7 @@ public: } FlattenedMap(PyArrayObject *object) - : Base((Scalar *)((PyArrayObject*)object)->data, + : Base((Scalar *)((PyArrayObject*)object)->data, // : Base(_from_numpy((PyArrayObject*)object), (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[0] : 1, (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0], @@ -390,7 +390,7 @@ public: return *this; } - + operator Base() const { return static_cast(*this); } @@ -429,7 +429,7 @@ public: (ColsAtCompileTime == Eigen::Dynamic) ? 0 : ColsAtCompileTime), object_(NULL) { } - + Map(Scalar *data, long rows, long cols) : Base(data, rows, cols), object_(NULL) {} @@ -456,7 +456,7 @@ public: throw std::invalid_argument("Numpy array must be a in one contiguous segment to be able to be transferred to a Eigen Map."); Py_XINCREF(object_); } - + Map &operator=(const Map &other) { if (other.object_) { new (this) Map(other.object_); @@ -474,7 +474,7 @@ public: MapBase::operator=(other); return *this; } - + operator Base() const { return static_cast(*this); } diff --git a/doc/Code/LocalizationExample2.cpp b/doc/Code/LocalizationExample2.cpp index 7fce69566..d22180314 100644 --- a/doc/Code/LocalizationExample2.cpp +++ b/doc/Code/LocalizationExample2.cpp @@ -1,5 +1,5 @@ // add unary measurement factors, like GPS, on all three poses -noiseModel::Diagonal::shared_ptr unaryNoise = +noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y graph.add(boost::make_shared(1, 0.0, 0.0, unaryNoise)); graph.add(boost::make_shared(2, 2.0, 0.0, unaryNoise)); diff --git a/doc/Code/LocalizationFactor.cpp b/doc/Code/LocalizationFactor.cpp index 8b80f2b2a..79a54903e 100644 --- a/doc/Code/LocalizationFactor.cpp +++ b/doc/Code/LocalizationFactor.cpp @@ -5,7 +5,7 @@ public: UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model): NoiseModelFactor1(model, j), mx_(x), my_(y) {} - Vector evaluateError(const Pose2& q, + Vector evaluateError(const Pose2& q, boost::optional H = boost::none) const { if (H) (*H) = (Matrix(2,3)<< 1.0,0.0,0.0, 0.0,1.0,0.0).finished(); diff --git a/doc/Code/OdometryExample.cpp b/doc/Code/OdometryExample.cpp index ef880384c..6e736e2d7 100644 --- a/doc/Code/OdometryExample.cpp +++ b/doc/Code/OdometryExample.cpp @@ -3,13 +3,13 @@ NonlinearFactorGraph graph; // Add a Gaussian prior on pose x_1 Pose2 priorMean(0.0, 0.0, 0.0); -noiseModel::Diagonal::shared_ptr priorNoise = +noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.add(PriorFactor(1, priorMean, priorNoise)); // Add two odometry factors Pose2 odometry(2.0, 0.0, 0.0); -noiseModel::Diagonal::shared_ptr odometryNoise = +noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); graph.add(BetweenFactor(1, 2, odometry, odometryNoise)); graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); diff --git a/doc/Code/Pose2SLAMExample.cpp b/doc/Code/Pose2SLAMExample.cpp index 2e2b41704..1738aabc5 100644 --- a/doc/Code/Pose2SLAMExample.cpp +++ b/doc/Code/Pose2SLAMExample.cpp @@ -1,10 +1,10 @@ NonlinearFactorGraph graph; -noiseModel::Diagonal::shared_ptr priorNoise = +noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.add(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); // Add odometry factors -noiseModel::Diagonal::shared_ptr model = +noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); graph.add(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); graph.add(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 204af1fea..e3408b67b 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/examples/Data/HS21.QPS b/examples/Data/HS21.QPS new file mode 100644 index 000000000..c71305c6e --- /dev/null +++ b/examples/Data/HS21.QPS @@ -0,0 +1,20 @@ +NAME HS21 +ROWS + N OBJ.FUNC + G R------1 +COLUMNS + C------1 R------1 0.100000e+02 + C------2 R------1 -.100000e+01 +RHS + RHS OBJ.FUNC 0.100000e+03 + RHS R------1 0.100000e+02 +RANGES +BOUNDS + LO BOUNDS C------1 0.200000e+01 + UP BOUNDS C------1 0.500000e+02 + LO BOUNDS C------2 -.500000e+02 + UP BOUNDS C------2 0.500000e+02 +QUADOBJ + C------1 C------1 0.200000e-01 + C------2 C------2 0.200000e+01 +ENDATA diff --git a/examples/Data/HS268.QPS b/examples/Data/HS268.QPS new file mode 100644 index 000000000..53de89421 --- /dev/null +++ b/examples/Data/HS268.QPS @@ -0,0 +1,55 @@ +NAME HS268 +ROWS + N OBJ.FUNC + G R------1 + G R------2 + G R------3 + G R------4 + G R------5 +COLUMNS + C------1 OBJ.FUNC 0.183400e+05 R------1 -.100000e+01 + C------1 R------2 0.100000e+02 R------3 -.800000e+01 + C------1 R------4 0.800000e+01 R------5 -.400000e+01 + C------2 OBJ.FUNC -.341980e+05 R------1 -.100000e+01 + C------2 R------2 0.100000e+02 R------3 0.100000e+01 + C------2 R------4 -.100000e+01 R------5 -.200000e+01 + C------3 OBJ.FUNC 0.454200e+04 R------1 -.100000e+01 + C------3 R------2 -.300000e+01 R------3 -.200000e+01 + C------3 R------4 0.200000e+01 R------5 0.300000e+01 + C------4 OBJ.FUNC 0.867200e+04 R------1 -.100000e+01 + C------4 R------2 0.500000e+01 R------3 -.500000e+01 + C------4 R------4 0.500000e+01 R------5 -.500000e+01 + C------5 OBJ.FUNC 0.860000e+02 R------1 -.100000e+01 + C------5 R------2 0.400000e+01 R------3 0.300000e+01 + C------5 R------4 -.300000e+01 R------5 0.100000e+01 +RHS + RHS OBJ.FUNC -.144630e+05 + RHS R------1 -.500000e+01 + RHS R------2 0.200000e+02 + RHS R------3 -.400000e+02 + RHS R------4 0.110000e+02 + RHS R------5 -.300000e+02 +RANGES +BOUNDS + FR BOUNDS C------1 + FR BOUNDS C------2 + FR BOUNDS C------3 + FR BOUNDS C------4 + FR BOUNDS C------5 +QUADOBJ + C------1 C------1 0.203940e+05 + C------1 C------2 -.249080e+05 + C------1 C------3 -.202600e+04 + C------1 C------4 0.389600e+04 + C------1 C------5 0.658000e+03 + C------2 C------2 0.418180e+05 + C------2 C------3 -.346600e+04 + C------2 C------4 -.982800e+04 + C------2 C------5 -.372000e+03 + C------3 C------3 0.351000e+04 + C------3 C------4 0.217800e+04 + C------3 C------5 -.348000e+03 + C------4 C------4 0.303000e+04 + C------4 C------5 -.440000e+02 + C------5 C------5 0.540000e+02 +ENDATA diff --git a/examples/Data/HS35.QPS b/examples/Data/HS35.QPS new file mode 100644 index 000000000..1ee230e39 --- /dev/null +++ b/examples/Data/HS35.QPS @@ -0,0 +1,20 @@ +NAME HS35 +ROWS + N OBJ.FUNC + G R------1 +COLUMNS + C------1 OBJ.FUNC -.800000e+01 R------1 -.100000e+01 + C------2 OBJ.FUNC -.600000e+01 R------1 -.100000e+01 + C------3 OBJ.FUNC -.400000e+01 R------1 -.200000e+01 +RHS + RHS OBJ.FUNC -.900000e+01 + RHS R------1 -.300000e+01 +RANGES +BOUNDS +QUADOBJ + C------1 C------1 0.400000e+01 + C------1 C------2 0.200000e+01 + C------1 C------3 0.200000e+01 + C------2 C------2 0.400000e+01 + C------3 C------3 0.200000e+01 +ENDATA diff --git a/examples/Data/HS35MOD.QPS b/examples/Data/HS35MOD.QPS new file mode 100644 index 000000000..2fe75ef96 --- /dev/null +++ b/examples/Data/HS35MOD.QPS @@ -0,0 +1,21 @@ +NAME HS35MOD +ROWS + N OBJ.FUNC + G R------1 +COLUMNS + C------1 OBJ.FUNC -.800000e+01 R------1 -.100000e+01 + C------2 OBJ.FUNC -.600000e+01 R------1 -.100000e+01 + C------3 OBJ.FUNC -.400000e+01 R------1 -.200000e+01 +RHS + RHS OBJ.FUNC -.900000e+01 + RHS R------1 -.300000e+01 +RANGES +BOUNDS + FX BOUNDS C------2 0.500000e+00 +QUADOBJ + C------1 C------1 0.400000e+01 + C------1 C------2 0.200000e+01 + C------1 C------3 0.200000e+01 + C------2 C------2 0.400000e+01 + C------3 C------3 0.200000e+01 +ENDATA diff --git a/examples/Data/HS51.QPS b/examples/Data/HS51.QPS new file mode 100644 index 000000000..46416af03 --- /dev/null +++ b/examples/Data/HS51.QPS @@ -0,0 +1,33 @@ +NAME HS51 +ROWS + N OBJ.FUNC + E R------1 + E R------2 + E R------3 +COLUMNS + C------1 R------1 0.100000e+01 + C------2 OBJ.FUNC -.400000e+01 R------1 0.300000e+01 + C------2 R------3 0.100000e+01 + C------3 OBJ.FUNC -.400000e+01 R------2 0.100000e+01 + C------4 OBJ.FUNC -.200000e+01 R------2 0.100000e+01 + C------5 OBJ.FUNC -.200000e+01 R------2 -.200000e+01 + C------5 R------3 -.100000e+01 +RHS + RHS OBJ.FUNC -.600000e+01 + RHS R------1 0.400000e+01 +RANGES +BOUNDS + FR BOUNDS C------1 + FR BOUNDS C------2 + FR BOUNDS C------3 + FR BOUNDS C------4 + FR BOUNDS C------5 +QUADOBJ + C------1 C------1 0.200000e+01 + C------1 C------2 -.200000e+01 + C------2 C------2 0.400000e+01 + C------2 C------3 0.200000e+01 + C------3 C------3 0.200000e+01 + C------4 C------4 0.200000e+01 + C------5 C------5 0.200000e+01 +ENDATA diff --git a/examples/Data/HS52.QPS b/examples/Data/HS52.QPS new file mode 100644 index 000000000..06add5ac3 --- /dev/null +++ b/examples/Data/HS52.QPS @@ -0,0 +1,32 @@ +NAME HS52 +ROWS + N OBJ.FUNC + E R------1 + E R------2 + E R------3 +COLUMNS + C------1 R------1 0.100000e+01 + C------2 OBJ.FUNC -.400000e+01 R------1 0.300000e+01 + C------2 R------3 0.100000e+01 + C------3 OBJ.FUNC -.400000e+01 R------2 0.100000e+01 + C------4 OBJ.FUNC -.200000e+01 R------2 0.100000e+01 + C------5 OBJ.FUNC -.200000e+01 R------2 -.200000e+01 + C------5 R------3 -.100000e+01 +RHS + RHS OBJ.FUNC -.600000e+01 +RANGES +BOUNDS + FR BOUNDS C------1 + FR BOUNDS C------2 + FR BOUNDS C------3 + FR BOUNDS C------4 + FR BOUNDS C------5 +QUADOBJ + C------1 C------1 0.320000e+02 + C------1 C------2 -.800000e+01 + C------2 C------2 0.400000e+01 + C------2 C------3 0.200000e+01 + C------3 C------3 0.200000e+01 + C------4 C------4 0.200000e+01 + C------5 C------5 0.200000e+01 +ENDATA diff --git a/examples/Data/QPTEST.QPS b/examples/Data/QPTEST.QPS new file mode 100644 index 000000000..8f4f17f05 --- /dev/null +++ b/examples/Data/QPTEST.QPS @@ -0,0 +1,19 @@ +NAME QP example +ROWS + N obj + G r1 + L r2 +COLUMNS + c1 r1 2.0 r2 -1.0 + c1 obj 1.5 + c2 r1 1.0 r2 2.0 + c2 obj -2.0 +RHS + rhs1 r1 2.0 r2 6.0 +BOUNDS + UP bnd1 c1 20.0 +QUADOBJ + c1 c1 8.0 + c1 c2 2.0 + c2 c2 10.0 +ENDATA diff --git a/examples/Data/randomGrid3D.xml b/examples/Data/randomGrid3D.xml new file mode 100644 index 000000000..6a82ce31c --- /dev/null +++ b/examples/Data/randomGrid3D.xml @@ -0,0 +1,3414 @@ + + + + + + + 32 + 1 + + + + + + 2 + 0 + 0 + 1 + + + + + + 9 + 7 + + 3.20776022311033415e+01 + -3.21030367555546334e+01 + -4.10921918858809718e+01 + -2.97297166857905353e+01 + 5.12273135695288744e+01 + 5.32024088136580744e+01 + 7.91786461660617107e+01 + 6.02804880595302208e+01 + -3.20503372222358784e+00 + -4.49785699465720157e+00 + 9.06604158034029126e+01 + 5.65784169718906416e+00 + 3.12765298180226239e+01 + 2.69796747523965372e+01 + 6.45574939874757661e+01 + -3.41086208590283135e+01 + 3.16243899688174857e+01 + -6.07548743284260979e+01 + 5.22759578856884062e+01 + 2.73875651690404389e+01 + -6.70630095670253041e+01 + 8.09592862312814248e+01 + -2.90562646005293850e+01 + 1.97217242898365228e+01 + 1.92063557124931243e+01 + -3.42017680808024593e+01 + 5.18983240742203904e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + -8.81008786560076906e+00 + 9.93170954106413291e+01 + 7.64832733308121160e+00 + 9.74088247837876793e+01 + 6.98423466470551624e+00 + 2.15114230210294117e+01 + 8.81008786560076906e+00 + -9.93170954106413291e+01 + -7.64832733308121160e+00 + -0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 2.08303433971152465e+01 + 9.34532104006540187e+00 + -9.73589326595991906e+01 + -9.74088247837876793e+01 + -6.98423466470551624e+00 + -2.15114230210294117e+01 + -2.08303433971152465e+01 + -9.34532104006540187e+00 + 9.73589326595991906e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -9.96847734946016431e+01 + -9.41956956915240795e+00 + 3.58611803229205250e+01 + 1.37202268725982691e+02 + 8.31655615217660085e+01 + -2.96050013164699770e+01 + 5.56978428429618404e+01 + -1.64096733189184050e+02 + -6.76911341847846018e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 1 + 2 + + + + + + 9 + 7 + + -5.23677913323444741e+01 + 2.23054746283748031e+01 + -9.06323879014932388e+00 + 5.52805622243718773e+01 + 7.94539068269289146e+01 + 1.94541508392102571e+01 + -6.13821741431047201e+01 + 5.56974973159738553e+01 + -7.78665862090978944e+00 + -3.50785425178010168e+00 + -8.59643527732278869e+01 + 4.37949256925254957e+01 + 1.46448346762102268e+01 + -9.84525875940883211e+00 + -6.31187688571179351e+01 + -1.68743787040301001e+01 + 4.96429783126596220e+01 + 6.02933235638437850e+01 + -8.50324227920073668e+01 + -8.14408668091479448e+00 + 7.80614049299559465e+00 + -3.12168328896096590e+01 + -8.16862254520412101e+00 + 7.01147846710003364e+01 + 4.14413157080187986e+01 + -1.74987783157454646e+00 + 7.04591203724149580e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + -2.60374020501451531e+00 + -5.42127786775135831e+01 + 8.39892562474970532e+01 + -9.97226081614345645e+01 + 7.26909685044996312e+00 + 1.60051631037066500e+00 + 2.60374020501451531e+00 + 5.42127786775135831e+01 + -8.39892562474970532e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -6.97294474564191535e+00 + -8.37146036187337330e+01 + -5.42516652512392241e+01 + 9.97226081614345645e+01 + -7.26909685044996312e+00 + -1.60051631037066500e+00 + 6.97294474564191535e+00 + 8.37146036187337330e+01 + 5.42516652512392241e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.08200476037924709e+01 + 1.28950357649989712e+02 + 1.43352339174516089e+02 + -2.38602247867744630e+01 + -5.20893463997723600e+01 + 2.84608516229487805e+01 + 6.76429765595175212e+01 + 1.20773294728176111e+02 + -1.20589752280510609e+02 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 2 + 3 + + + + + + 9 + 7 + + -1.96993691143317093e+01 + -2.72709220997507202e+01 + 4.46131383257576886e+01 + 9.30649268771392428e+01 + 3.22308546546604946e+00 + -1.69350730671129561e+01 + -3.00372292195983448e+01 + 4.73051200126789553e+01 + -6.91348627543176519e+01 + 8.77985890195871921e-01 + 2.95766095687473936e+01 + -7.96244285285575017e+01 + 7.31430977105361357e+00 + 9.52212779955403192e+01 + 2.32608409272044945e+01 + -1.06369613385613393e+00 + -2.30114188850419632e+00 + -5.58240668503478901e+01 + 8.63825257313862096e+01 + 3.81456041339674954e+01 + 2.72998827723357493e+01 + 3.09037953339841458e+01 + -1.77447464898715950e+01 + -1.04957062263738177e+01 + 3.97018822008379715e+01 + -7.27393475106917862e+01 + -4.57204758371371796e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -4.59114589009974594e+01 + 8.09444187944725400e+01 + 3.66079090854293696e+01 + -7.62990082722604797e+01 + -1.48208463505055157e+01 + -6.29190261377608593e+01 + 4.59114589009974594e+01 + -8.09444187944725400e+01 + -3.66079090854293696e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -4.55038380606685564e+01 + -5.68185144075392898e+01 + 6.85642555729200751e+01 + 7.62990082722604797e+01 + 1.48208463505055157e+01 + 6.29190261377608593e+01 + 4.55038380606685564e+01 + 5.68185144075392898e+01 + -6.85642555729200751e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -8.55922050845758786e-01 + 1.40043738778271234e+02 + -3.81613152068798769e+01 + -5.81227180496655009e+01 + -3.94713178138164338e+01 + 3.22753989340650662e+01 + 1.32631681079235051e+02 + -3.12863815406522541e+01 + -4.03072840384836510e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 3 + 4 + + + + + + 9 + 7 + + 8.66048172555662319e+01 + -1.52328612569721304e+01 + 4.48534445123967629e+01 + 2.02981055828854124e+01 + -4.93746572823205057e+01 + -2.74451016941264996e+01 + 4.12224960002507945e+00 + -6.40449795031895519e+01 + -5.03376341440287618e+01 + 4.92248012617162161e+01 + 3.93527186553859920e+01 + -6.89623533878909143e+01 + -4.08605085624496382e+01 + 5.95700970246627293e+01 + 3.55176671176527492e+01 + 9.26276585311693879e+00 + -6.84332455006064038e+01 + 4.88721436512163976e+00 + -4.03054194818879719e+00 + -1.78980733327891492e+01 + 3.45198923956961394e+01 + -8.80228615455582002e+01 + -4.55655125601066899e+01 + -9.64228954249413128e+00 + 1.13078887631363152e+01 + -3.24259783798813004e+01 + 8.58793556515557270e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + -8.08548316492842929e+01 + 5.81668844249688917e+01 + 8.89436648998715995e+00 + 4.55868302729299728e+01 + 5.23625800953892124e+01 + 7.19722245829668879e+01 + 8.08548316492842929e+01 + -5.81668844249688917e+01 + -8.89436648998715995e+00 + -0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 3.72066809139564114e+01 + 6.22476807764455131e+01 + -6.88541148612105047e+01 + -4.55868302729299728e+01 + -5.23625800953892124e+01 + -7.19722245829668879e+01 + -3.72066809139564114e+01 + -6.22476807764455131e+01 + 6.88541148612105047e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -4.49706199136485765e+01 + 2.66285359051983370e+01 + 1.14028652024075640e+02 + 5.86398529753103119e+01 + 8.34746189227890056e+00 + 1.60810773504427289e+02 + 1.79694799386287997e+02 + -4.53728709677613509e+01 + -1.70780906392258842e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 4 + 5 + + + + + + 9 + 7 + + 6.43003424386522084e+01 + 1.19796180071396279e+01 + 4.55761542738693137e+01 + -6.52039759607597773e+01 + 5.89636417155482349e+01 + 1.80719252805888360e+01 + -1.51516354244859457e+01 + -5.00500232613797280e+01 + -5.34352949392747476e+01 + 7.42583468150113362e+01 + 7.45315542640838657e+00 + -5.24573072801267415e+01 + 4.09274245687232323e+01 + -1.68412174720425938e+01 + -1.36705786744805522e+01 + 2.70644906205976383e+01 + -8.31804320954470597e+01 + 4.33744783589924054e+01 + -1.22788002717957223e+01 + -6.11064170046755617e+00 + -7.16592171005377452e+01 + -5.30800076837930419e+01 + -7.82610172559931669e+01 + 2.92797087670289500e+01 + -2.22315273959740871e+01 + -2.13187310855278938e+01 + -6.26781275987031279e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 6.86610695050494400e+01 + 1.26119199040840346e+01 + 7.16002584545316836e+01 + -7.26277336162735736e+01 + 1.63646703530006263e+01 + 6.67638365734117087e+01 + -6.86610695050494400e+01 + -1.26119199040840346e+01 + -7.16002584545316836e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -3.29694467444833483e+00 + -9.78424092128285139e+01 + 2.03959092771819215e+01 + 7.26277336162735736e+01 + -1.63646703530006263e+01 + -6.67638365734117087e+01 + 3.29694467444833483e+00 + 9.78424092128285139e+01 + -2.03959092771819215e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.74531092467603024e+01 + -9.68528064078233442e-01 + -1.43955992851650620e+01 + -3.71890171910416072e+01 + 2.70827825121624883e+01 + 1.59657441575989878e+02 + -1.61093211332558155e+02 + -2.36356649401689936e+01 + -3.50656643781457547e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 5 + 6 + + + + + + 9 + 7 + + -6.40637543398504619e+01 + -1.35919926214779476e+01 + -7.55586787939104454e+01 + 7.49591051894634859e+01 + -1.51203828101076443e+01 + -6.04179319959996945e+01 + 1.63126149967003826e+01 + -3.67352169146467711e+00 + -1.49855747686980401e+01 + -7.64033153697704415e-01 + 5.83204655642280656e+00 + -2.26064773215979020e+00 + -1.21801830224353154e+01 + 9.23282518419687364e+01 + -3.58808093806811215e+01 + 6.76481543691339766e+01 + 3.42590164955414807e+01 + 6.51923982648987277e+01 + -2.26801968652302843e+00 + -9.78783400225560598e+01 + 1.94155506146273105e+01 + 1.64832141235940597e+01 + 2.49817026599325320e+00 + -1.62466150125265507e+01 + -7.07730069025061255e+01 + 1.59540577889256383e+01 + 6.50575730811547004e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + -2.60428350122129437e+01 + -6.97694761102178944e+01 + 6.67382270354284231e+01 + -9.26426416957655618e+01 + 3.75218214997053465e+01 + 3.07471152699694139e+00 + 2.60428350122129437e+01 + 6.97694761102178944e+01 + -6.67382270354284231e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -2.71866085445878198e+01 + -6.10273144964611944e+01 + -7.44080318325472234e+01 + 9.26426416957655618e+01 + -3.75218214997053465e+01 + -3.07471152699694139e+00 + 2.71866085445878198e+01 + 6.10273144964611944e+01 + 7.44080318325472234e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.03933661487596595e+02 + 4.68471191704998802e+01 + 1.18875204735324314e+01 + -2.97055153316788463e+01 + 7.27443939906840598e+01 + 7.23444094058463492e+01 + 3.82415703375670404e+01 + -2.27415768204949451e+01 + -3.07813391103898937e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 6 + 7 + + + + + + 9 + 7 + + 1.85630374271583953e+01 + -5.29313735130414926e+01 + 3.66303674355102942e+01 + 1.04645716047590440e+01 + 5.73485858047936077e+01 + -5.08591115829850366e+01 + 9.38445241664948213e+01 + -1.36042785531324792e+01 + -2.31303222840427054e+01 + -9.56612084104992455e+00 + -2.98701442261074703e+01 + 7.62840716168778670e+01 + -1.92910486636706899e+01 + -5.04699434611332478e+01 + 3.46538201809487560e+01 + 3.08818026153629788e+01 + 7.17821170583860777e+01 + 5.45013702935509130e+01 + -5.34080385938858839e+01 + 6.23631140795577465e+01 + 4.43545654716690905e+01 + -7.95485560661077074e+01 + -2.30201810552558968e+01 + -5.51074857191607634e+01 + 1.18903119380668851e+01 + -2.67045605220385767e+01 + -2.32776001751549053e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.79872266803328138e+01 + 8.64209977399809475e+01 + -4.69879859748907549e+01 + 9.82455450157955568e+01 + -1.33902787243874393e+01 + 1.29812680517931049e+01 + -1.79872266803328138e+01 + -8.64209977399809475e+01 + 4.69879859748907549e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 4.92671908064704667e+00 + -4.84985730234346519e+01 + -8.73133200250165942e+01 + -9.82455450157955568e+01 + 1.33902787243874393e+01 + -1.29812680517931049e+01 + -4.92671908064704667e+00 + 4.84985730234346519e+01 + 8.73133200250165942e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -8.68503697620646733e+01 + -6.63265206223623283e-01 + 5.77899517416623070e+01 + 1.54728954524297080e+02 + -7.36729123490597857e+01 + -4.33714485898333280e+01 + -8.08065933920789270e+00 + -1.49263004123200204e+02 + 1.24142002075972883e+02 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 7 + 8 + + + + + + 9 + 7 + + 4.74062871911341759e+01 + 5.71222009314007693e+01 + -2.90538462259461063e+01 + 4.09276699085499303e+01 + 3.99974137866047528e+01 + -1.99073676332006535e+01 + -7.78029399881401531e+01 + 5.27745236895511312e+01 + -3.37039333639074385e+01 + -9.12848490867447815e+00 + -6.92290536396862421e+01 + -2.32360574632871630e+00 + 1.58993825041621264e+01 + 5.31010116114126660e+01 + -6.55564733827918502e+01 + -3.41955054720470475e+00 + 4.69927897084473187e+01 + 7.43534572740371971e+01 + -6.28440645552295862e+01 + 4.27829075558255525e+01 + 5.46299457034644860e+01 + 7.56316908352858093e+01 + 1.42185369226067397e+01 + 5.51031175481571864e+01 + 2.63284733063378651e+00 + 2.23353419791525312e+01 + 4.20873574259948739e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.80585028832817862e+01 + -9.52829540657210430e+01 + -2.43936290478516966e+01 + -9.74947022342857252e+01 + -2.06160114466111395e+01 + 8.35243127964282905e+00 + -1.80585028832817862e+01 + 9.52829540657210430e+01 + 2.43936290478516966e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -1.29874366163019737e+01 + 2.22741719608807998e+01 + -9.66187753679099757e+01 + 9.74947022342857252e+01 + 2.06160114466111395e+01 + -8.35243127964282905e+00 + 1.29874366163019737e+01 + -2.22741719608807998e+01 + 9.66187753679099757e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 7.39787443001634273e+01 + -3.29520218737291373e+01 + 1.75142945360573606e+02 + -4.89974435425370416e+01 + -9.39530685963477481e+01 + -3.92888663032043226e+01 + 4.46158732311312889e+01 + 1.62422063134678211e+02 + -1.51571321993876627e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 8 + 9 + + + + + + 9 + 7 + + 9.41130157063151671e+01 + 1.30096002577236085e+01 + -9.65142385029295902e+00 + -2.97023439769390656e+01 + 3.10157410489098062e-01 + 4.06407354840771795e+00 + -9.58311576289570155e+00 + 9.66152903465384156e+01 + 2.35615250878309794e+01 + 1.77277266667175297e+01 + -4.94856229862016690e+01 + 8.47852845508831336e+01 + 1.34310978254269742e+01 + -8.43883523083151488e+01 + -5.15167787166412623e+01 + 3.41073399110957187e-01 + 2.18139939555895035e+00 + 9.36548913434189423e+00 + -2.80240621096084510e+01 + -7.66577929709818839e+00 + 9.19688865464188510e+00 + -9.38736839860108603e+01 + -1.81862826844923511e+01 + 1.54239226370280336e+00 + -8.73344590429244683e+00 + 2.30576115914954443e+01 + -9.65297250180462356e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -9.99929178544290664e+01 + 6.62091181731783163e-01 + -9.88946016968861308e-01 + -6.23564336177379053e-01 + 4.16306145486879302e+01 + 9.09203118110426516e+01 + 9.99929178544290664e+01 + -6.62091181731783163e-01 + 9.88946016968861308e-01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.01367967132287018e+00 + 9.09200394168724984e+01 + -4.16235376434812636e+01 + 6.23564336177379053e-01 + -4.16306145486879302e+01 + -9.09203118110426516e+01 + -1.01367967132287018e+00 + -9.09200394168724984e+01 + 4.16235376434812636e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 5.55282235936336566e+00 + -1.76495493086246057e+02 + -9.69626169859593645e+00 + -1.18086675146736617e+01 + 9.21060305839002780e+01 + 5.32168712759193419e+00 + 1.99148216329777483e+02 + 1.06989143861811957e+01 + -5.26829527913709494e+00 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 9 + 10 + + + + + + 9 + 7 + + -8.63087793249897430e+01 + -1.92823330270961684e+01 + -4.42211315553459627e+01 + 2.21505354932965801e+01 + -2.63320996942306031e+01 + -5.69788664986630025e+01 + -4.53775288609686314e+01 + 2.58526272351369890e+01 + 5.53658756361871127e+01 + -1.54315640909267664e+01 + -4.32654755327350813e+01 + 1.97045712499729326e+01 + 7.39853470327932428e+01 + -5.76879953686396973e+01 + 2.69216116215563304e+01 + 6.54801163688819514e+01 + 5.53812651216026666e+01 + -2.49089008945160231e+01 + -9.71189061792836705e-01 + -7.25245368327360609e+01 + 4.96429238183381116e+01 + 6.85930292060070612e-01 + -1.12841816293779988e+01 + -7.69094846357958062e+01 + -5.16757922033419734e-02 + -6.79147932987382035e+01 + -4.02434226181160639e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 7.83449748282976799e+01 + 6.13201754047114846e+01 + 1.00946028891096997e+01 + -1.98027611608243852e+01 + 9.23644092983748521e+00 + 9.75834966034573341e+01 + -7.83449748282976799e+01 + -6.13201754047114846e+01 + -1.00946028891096997e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 5.89059892503364324e+01 + -7.84507759508154265e+01 + 1.93793752003053505e+01 + 1.98027611608243852e+01 + -9.23644092983748521e+00 + -9.75834966034573341e+01 + -5.89059892503364324e+01 + 7.84507759508154265e+01 + -1.93793752003053505e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -1.06986312175636783e+02 + 1.28415573927117009e+02 + 5.26747761738054123e+01 + -8.33248123208498015e+01 + -6.72578872464476092e+01 + 1.08240234163658471e+02 + -1.79030185090507175e+01 + -1.01966657528716965e+02 + 5.84227812301104876e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 10 + 11 + + + + + + 9 + 7 + + 1.17302291731448682e+00 + 2.23736436785092039e+01 + 8.70065012231049764e+01 + 7.67707833816357947e+01 + 5.73162887867323292e+01 + -1.32881532104113664e+00 + -2.51982778355239070e+01 + -7.72901736529863381e+00 + 4.53049731782644329e+01 + 8.54937155952362531e+01 + -4.11223504877864485e+01 + 2.12416634370425328e+01 + -2.90259802009985357e+01 + -8.76138896821732693e+00 + -5.06100435935214055e+00 + -3.81597643193563130e+01 + -9.02600433698408864e+01 + 7.99447271884073940e-01 + -2.24916749275050378e+01 + 2.24299023473424732e+01 + 3.83080111263447165e+01 + -5.03856743318980023e+01 + 7.20086537445330919e+01 + -4.63731853719149143e+01 + 2.85914723565250490e+01 + -2.34837711903294171e+01 + -7.92472197577816644e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -2.63903093070078647e+01 + -2.67183596992454575e+01 + 9.26805310173727577e+01 + -8.93547271763020774e+01 + 4.29554589851731663e+01 + -1.30599109729635039e+01 + 2.63903093070078647e+01 + 2.67183596992454575e+01 + -9.26805310173727577e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -3.63219534982506076e+01 + -8.62609865371063478e+01 + -3.52101959056759028e+01 + 8.93547271763020774e+01 + -4.29554589851731663e+01 + 1.30599109729635039e+01 + 3.63219534982506076e+01 + 8.62609865371063478e+01 + 3.52101959056759028e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -1.04067559180773088e+01 + 7.90108728350369383e-01 + 5.78189525430708571e+01 + -1.16280923178694167e+02 + 8.10714898076704600e+01 + 7.53829806570444134e+01 + -5.78201683295110769e+01 + 6.19589188064910701e+01 + -1.33505720595716895e+02 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 11 + 12 + + + + + + 9 + 7 + + 3.68344254692879192e+00 + 4.86691203808377892e+00 + -1.57231292941544805e+01 + 3.72290369400325147e+01 + 2.06331817836064921e+01 + -8.89534944072540839e+01 + -8.53296212948073389e+01 + 4.59306709423131920e+01 + -2.45010409318042690e+01 + 2.57025147685637876e+01 + 2.56000347344376031e+01 + -9.16529948262070491e+01 + 2.63288801817711970e-01 + 6.59382843634221683e+00 + 1.98864164108950128e+01 + -3.68109153857383404e+01 + -8.63480489179755466e+01 + -3.21511205713796500e+01 + 8.19562719339757138e+01 + -5.68704775649636858e+01 + 6.94175446640136773e+00 + -5.03482257069523769e+01 + -7.72158009035328661e+01 + -3.65965187643956114e+01 + -7.19547325119217618e+00 + -9.46558534641098959e+00 + -4.77765582369805042e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -7.29991262018860425e+01 + 6.43134564832916169e+01 + -2.31280541536218962e+01 + -5.24720785637083011e+01 + -7.44213220616246360e+01 + -4.13297446617319864e+01 + 7.29991262018860425e+01 + -6.43134564832916169e+01 + 2.31280541536218962e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -4.37927910159324512e+01 + -1.80345817187895037e+01 + 8.80735222258265793e+01 + 5.24720785637083011e+01 + 7.44213220616246360e+01 + 4.13297446617319864e+01 + 4.37927910159324512e+01 + 1.80345817187895037e+01 + -8.80735222258265793e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 9.48709591330575535e+01 + 9.60518679589807789e+01 + -5.19581436253384297e+01 + 2.54957713470891250e+01 + -1.34158356561916207e+02 + -2.25547332522903439e+01 + 1.09221138500000791e+02 + -4.57450052559468858e+01 + -6.82129290695598485e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 12 + 13 + + + + + + 9 + 7 + + 1.93941690967042213e+01 + -3.90998618366849584e+01 + 1.63698620253740224e+00 + 1.66123034210985914e+01 + 8.85555525420248557e+01 + 2.63934202043697148e+01 + -8.61979615512473032e+01 + 1.74318161103396001e+01 + -3.92907393347052789e+01 + -7.36585675537442341e+01 + 5.45643959900256945e+01 + -4.73596788713465688e+00 + 1.23230795012408407e+01 + 3.75671217770436385e+01 + -8.32914041112130690e+01 + -4.08563087068878872e+01 + 8.53843323058891990e+00 + 3.64960868565984242e+01 + -6.45808132649689668e+01 + -7.41043072657268738e+01 + -2.22937919750649804e+00 + -1.10797951271104100e+00 + -1.86389067743133623e+01 + -4.83331185591061043e+01 + 2.23431972515867372e+01 + -4.99220528272985842e+00 + -8.44039814228390526e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + 5.32207681440972067e+01 + -3.06680603945050478e+01 + 7.89114688102511224e+01 + -5.56684880542022569e+01 + -8.29014582098623265e+01 + 5.32613034365356164e+00 + -5.32207681440972067e+01 + 3.06680603945050478e+01 + -7.89114688102511224e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 6.37853374680371630e+01 + -4.67634290692784660e+01 + -6.11932383991170568e+01 + 5.56684880542022569e+01 + 8.29014582098623265e+01 + -5.32613034365356164e+00 + -6.37853374680371630e+01 + 4.67634290692784660e+01 + 6.11932383991170568e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -6.90389218165145877e+01 + 4.83379444724982577e+01 + 1.61042728694415416e+02 + -1.53499694264126390e+02 + -6.29210660700233788e+01 + -1.36328982272427429e-01 + -7.32570951739669738e+01 + -6.73028752573054874e+01 + -7.84207879822722163e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 13 + 14 + + + + + + 9 + 7 + + 6.06804300720567937e+01 + 6.78223389316968621e+01 + 1.14214673416742691e+01 + 2.85045083366314387e+01 + 2.67345752695483867e+01 + 9.28156907783291274e+00 + -3.79052729172203868e+01 + 4.99857449838935750e+01 + -7.77096553357260120e+01 + -7.82277784247720263e+01 + 5.40246939742984793e+01 + 2.32633737948870305e+01 + 2.47956201341556586e+01 + -1.05317240685613065e+01 + 9.52360095902563302e+01 + -1.29078256939381077e+01 + 9.88755369559818398e+00 + 1.89881719971782701e+01 + 9.09983932556493968e+00 + 4.26215664457260388e+01 + 1.04271324229186870e+01 + -6.00988012459201499e+01 + -6.92347566083080466e+01 + 1.36255057217382767e+01 + 5.89311254734215737e+01 + -4.94916274785966195e+01 + -5.89796648826666967e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -2.89030917873941853e+01 + 8.70749520313458305e+01 + -3.97814531392241904e+01 + -9.56630827209552450e+01 + -2.78467078053438328e+01 + 8.55192777842975183e+00 + 2.89030917873941853e+01 + -8.70749520313458305e+01 + 3.97814531392241904e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -3.63123800557649812e+00 + 4.05279359595652906e+01 + 9.13471429077244466e+01 + 9.56630827209552450e+01 + 2.78467078053438328e+01 + -8.55192777842975183e+00 + 3.63123800557649812e+00 + -4.05279359595652906e+01 + -9.13471429077244466e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.43780450624784670e+01 + -6.63129258801445189e+01 + 4.67182022743824987e+00 + -2.52301003257763057e+01 + -9.40350333946463337e+01 + -1.71054540098886392e+01 + 9.90721820600476235e+01 + -1.66885836907954435e+01 + 5.08294565998758543e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 14 + 15 + + + + + + 9 + 7 + + -2.76355843137293995e+01 + -7.18186699571359952e+01 + 3.07651775764515065e+01 + -6.40640399324160086e+01 + -2.68919032866484322e+01 + 9.38443754617159698e+00 + -7.15464009647350991e+01 + 4.97633461308791567e+01 + -2.49225935590679519e+01 + 1.43932895637872207e+01 + -2.88438285303894943e+01 + 6.70843814351506182e+01 + -2.31361564910724091e+01 + 9.02877315480068603e+01 + 3.51897296575981642e+01 + 1.03017517985169054e+01 + -1.55151545070516619e+01 + 6.47165615401468699e+01 + 5.11107200952757168e+01 + 4.26513944039867638e+01 + 5.62224381230135037e+01 + -6.85270728206629229e+01 + -1.63576632701424352e+01 + 1.39839419419296167e+01 + 4.30853333607984084e+01 + -1.81999495152501289e+01 + -7.11401633803200042e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -8.15059339771809164e+01 + -5.54598536753216109e+01 + -1.67626774955357369e+01 + -1.04808841952049807e+01 + 4.25686446848321935e+01 + -8.98780371179923492e+01 + 8.15059339771809164e+01 + 5.54598536753216109e+01 + 1.67626774955357369e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 5.69818724946287603e+01 + -7.14990567770542356e+01 + -4.05086544703406517e+01 + 1.04808841952049807e+01 + -4.25686446848321935e+01 + 8.98780371179923492e+01 + -5.69818724946287603e+01 + 7.14990567770542356e+01 + 4.05086544703406517e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + 2.31230716791378725e+01 + 2.46912491883496621e+01 + 3.19609408881894597e+00 + 1.52981265769411010e+01 + 1.32876273309615431e+01 + 2.19836964146123970e+00 + 2.74814898754146313e+01 + -2.79167095337952915e+01 + 5.37372498127196874e+00 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 15 + 16 + + + + + + 9 + 7 + + -5.10421334268497020e+01 + -3.97471595810313332e+01 + -1.64389363316277826e+00 + 2.54089618298921245e+01 + -3.15327493370988705e+01 + 9.13980507822707153e+01 + -5.91789668556331776e+01 + -4.81012757100621684e+01 + 1.65563384387473489e+00 + -6.78890977332195860e+01 + -3.50650650222022904e+01 + -8.69105008709039062e+00 + -5.80472933502369273e+01 + 6.88255140605843536e+01 + 3.93666342323419940e+01 + 4.37233513120340049e+01 + 4.71317258377345070e+01 + 1.72241668652535651e+01 + -4.44141172746378885e+01 + 7.81566561420669927e+01 + -4.26269969902096406e+01 + 1.02994128500864690e+01 + -1.21754201166700025e+01 + -9.79654030213998794e+00 + -3.57503904294827279e+01 + 2.58360247551576450e+01 + 8.83514468333149239e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -8.11187605223852870e+01 + -5.83599337897738124e+01 + 3.72354929672284474e+00 + 4.13366904132922599e+01 + -5.27200270517053724e+01 + 7.42420148793328991e+01 + 8.11187605223852870e+01 + 5.83599337897738124e+01 + -3.72354929672284474e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -4.13645345312568224e+01 + 6.17633943021323262e+01 + 6.68898976474907556e+01 + -4.13366904132922599e+01 + 5.27200270517053724e+01 + -7.42420148793328991e+01 + 4.13645345312568224e+01 + -6.17633943021323262e+01 + -6.68898976474907556e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 6.98804353612945732e+01 + -9.46593060564561881e+01 + -1.56916014852066439e+02 + -3.53367495425767473e+01 + -1.16911080827186709e+02 + 7.34112761651981316e+01 + 2.36033204137547230e+01 + 1.27622949564988858e+02 + -4.72506285651327431e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 16 + 17 + + + + + + 9 + 7 + + -5.85435867217505290e+01 + 2.33746012476837990e+01 + -5.77863483723881544e+01 + -5.63329646716383934e+01 + -7.47638541726440593e+01 + 3.41977282497739097e+01 + -4.10879444630993547e+01 + 7.01966859196727366e+00 + -3.18903501380708931e+01 + 7.49027535040175820e+01 + -1.88037812404022127e+01 + -5.50573390958028739e+01 + -1.05105126900153234e+01 + 3.04370847103721403e+01 + 2.74657650627104459e+01 + -5.06989372188159564e+01 + -7.70752893724180126e+01 + -2.65036593071036783e+01 + -8.31624785461406368e+00 + 7.75118867469858053e+00 + -5.96862946091213260e+01 + 3.40815941189988152e+01 + -5.16283223754141716e+01 + -6.67021661737265248e+01 + 4.67870803895238083e+01 + -6.21976850537821235e+01 + 4.44353424269327562e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -9.98522291990096846e+01 + -2.17344032867269910e+00 + 4.98081129196222250e+00 + -1.69510675143138556e+00 + -7.46243396331473150e+01 + -6.65457327513788073e+01 + 9.98522291990096846e+01 + 2.17344032867269910e+00 + -4.98081129196222250e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 5.16322932762926534e+00 + -6.65318276575533361e+01 + 7.44772245149880234e+01 + 1.69510675143138556e+00 + 7.46243396331473150e+01 + 6.65457327513788073e+01 + -5.16322932762926534e+00 + 6.65318276575533361e+01 + -7.44772245149880234e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -3.50474015754750923e+01 + -2.85468905697825797e+01 + -8.26608268609589487e+01 + 7.28338697761986396e+01 + -1.03233140690458100e+02 + -6.32144076335772187e+00 + 4.02504937228882511e+01 + 1.40718088483059116e+01 + 7.44299004116042227e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 17 + 18 + + + + + + 9 + 7 + + -8.82393493416751795e+01 + -6.12481728289156635e+00 + 6.45898243198292366e-01 + 3.86841248560220095e+01 + -4.52281770219861770e+01 + -4.30849606407744687e+01 + -2.62811256127213326e+01 + -5.90797411700203270e+01 + -5.09550094510600857e+01 + 4.68685179819303457e+01 + -2.73625956906887646e+00 + 1.87456681275038339e+00 + 7.09242852066529537e+01 + -3.96201491012311280e+01 + 4.26233675831482230e+01 + -5.26328438238840803e+01 + -5.34225198350244597e+01 + 6.12486839444391720e+01 + -3.76709521788862034e+00 + 9.35400534400023815e+01 + -3.47030072986988287e+01 + 3.20432748703922687e+00 + 2.41487264929615755e+01 + 7.47759826419143678e+01 + -2.25246616168526836e+00 + -2.57366654666450962e+01 + -5.63865719112063317e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 9.03046090553630876e+01 + 2.33581936525576914e+01 + -3.60481951371722431e+01 + 1.31196185024049097e+01 + -9.49105504326207523e+01 + -2.86332503730943557e+01 + -9.03046090553630876e+01 + -2.33581936525576914e+01 + 3.60481951371722431e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -4.09017504968844747e+01 + 2.11277591302666750e+01 + -8.87731074167399186e+01 + -1.31196185024049097e+01 + 9.49105504326207523e+01 + 2.86332503730943557e+01 + 4.09017504968844747e+01 + -2.11277591302666750e+01 + 8.87731074167399186e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + 4.26251548517658989e+01 + -5.58446106332349572e+01 + -4.99134201995332649e+00 + 7.19670254876823492e+01 + -1.87430011171921045e+01 + -5.57531304221817550e+01 + -9.47135556308831283e+00 + -7.80687492596315167e+01 + 5.77908380638228465e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 18 + 19 + + + + + + 9 + 7 + + 6.21423423624779545e+01 + 1.02374321697767794e+01 + -2.61952819865811541e+01 + 1.64810134236028816e+01 + -9.33026901138422886e+01 + -2.97993293115291316e+01 + 6.47591857034336442e+01 + 2.72657426895055011e+01 + -2.33482872492159856e+01 + -6.84639855508772825e+01 + -1.58264297171629540e+01 + 2.10899487969025330e+01 + -3.01026938649666285e+01 + 2.36828763275138563e+01 + -9.22944706954510821e+01 + 6.50721593326321113e+01 + 1.34399733570446998e+01 + -1.47336875153178202e+01 + 1.81996160352434408e+01 + 5.50471142576061965e+01 + 8.12609439867173649e+01 + 3.09138639218085887e+00 + -1.18364488818499112e+01 + 7.45772273447166401e-02 + 3.87852402909780736e+01 + -7.92723675773540748e+01 + 4.57949875755143552e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -7.14353163221378225e+01 + -6.97621833385175307e+01 + 5.49848686450887225e+00 + 4.54649938592219129e+01 + -4.02948505639650421e+01 + 7.94308463470498509e+01 + 7.14353163221378225e+01 + 6.97621833385175307e+01 + -5.49848686450887225e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -5.31970855906319784e+01 + 5.92415630606653920e+01 + 6.05021263328172836e+01 + -4.54649938592219129e+01 + 4.02948505639650421e+01 + -7.94308463470498509e+01 + 5.31970855906319784e+01 + -5.92415630606653920e+01 + -6.05021263328172836e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 8.66602846584249278e+01 + -1.40571348898168708e+02 + -1.29029667681538402e+01 + -4.84107069228954998e+01 + -6.46593270163667029e+01 + 1.03797317950936275e+02 + 6.32154043923236131e+01 + 1.69243370521808174e+01 + -9.00005277714220426e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 19 + 20 + + + + + + 9 + 7 + + 5.40325063921370585e+01 + 5.12983919091620777e+00 + 4.24856501928409855e+01 + -6.01366381168498805e+01 + -6.66348312341534665e+01 + 1.23707663998909716e+01 + 2.51821654645419386e+01 + -4.49879933093015580e+01 + 6.61923323512717872e+01 + -8.16765559697753787e+01 + -1.72226454079742588e+01 + 3.80134460532522880e+01 + -3.51922609512735107e+01 + -2.39876166901732191e+01 + 7.97471671594631992e+00 + -4.83518374187914013e+00 + 8.66273522162050824e+01 + 4.67131848907273408e+01 + 1.17456223558336159e+01 + -5.59162876565777989e+00 + 8.16530126090797523e+01 + 6.71811935630704937e+01 + -7.04411581080473894e+01 + -2.09183319463410733e+01 + -1.56755267575532145e+01 + 1.18327312907135145e+01 + -5.35254607305620027e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + 1.72947781738215802e+01 + -7.37458142645349284e+01 + 6.52874071041194242e+01 + -9.78660995310737718e+01 + -5.39934004805816503e+00 + 1.98260860892763304e+01 + -1.72947781738215802e+01 + 7.37458142645349284e+01 + -6.52874071041194242e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -1.10958195052130399e+01 + -6.73231164274660614e+01 + -7.31059558722925402e+01 + 9.78660995310737718e+01 + 5.39934004805816503e+00 + -1.98260860892763304e+01 + 1.10958195052130399e+01 + 6.73231164274660614e+01 + 7.31059558722925402e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -5.38048032045895752e+00 + 1.65536142881929180e+02 + 8.22017033006773943e+01 + -7.27309788858385105e+01 + -1.01358249880792499e+01 + 1.16499706864703938e+02 + 7.80821322304104655e+01 + 9.19602704161340796e+01 + -8.91929751904364565e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 20 + 21 + + + + + + 9 + 7 + + 6.79396504738739537e+01 + 4.82037868237773779e+01 + -5.47024548652807283e+01 + -7.00832895591236564e+01 + 5.14309272143008727e+01 + -3.67255818969308834e+01 + 1.86928047352814559e+01 + -2.23350198922307115e+01 + 1.77311293144151279e+01 + 7.44105813075135192e+00 + 5.99087969415584212e+01 + 5.30460372996668283e+01 + -1.22158131494639797e+01 + -1.50072994727732638e+01 + -6.43870652455934760e+01 + -1.47523300546991756e+01 + 7.84637907473050689e+01 + -5.14524303561770466e+01 + 1.48130048314395335e+00 + -3.96193494527523526e+01 + -4.51446929817367320e+01 + 2.41623483145540554e+01 + -4.80504534286710054e+01 + -6.06763607845337773e+01 + 9.54718801387472098e+01 + 2.61345793187026096e+01 + 4.22977195343479639e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -4.31638784413171734e+01 + 8.58566719453584426e+01 + 2.76678781327787284e+01 + -6.66070179567136051e+01 + -9.65155343163612223e+00 + -7.39618325575448523e+01 + 4.31638784413171734e+01 + -8.58566719453584426e+01 + -2.76678781327787284e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -6.08307879023215037e+01 + -5.03535440542508113e+01 + 6.13525536906117850e+01 + 6.66070179567136051e+01 + 9.65155343163612223e+00 + 7.39618325575448523e+01 + 6.08307879023215037e+01 + 5.03535440542508113e+01 + -6.13525536906117850e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.33814642652391399e+02 + 1.77445244940366287e-01 + -1.49228346303260491e+01 + -6.12411970511728132e-01 + 5.97803936488639280e+01 + -1.02665793382055853e+02 + 2.53293581625552058e+01 + -3.42663008134674527e+01 + 5.61197444293587395e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 21 + 22 + + + + + + 9 + 7 + + -6.74405298173737435e+01 + 7.32759993494661614e+01 + -6.72758148030300518e+00 + 2.84952832715093294e+01 + 3.71106482095568992e+01 + 5.87105666468560798e+01 + 3.06497832963929113e+01 + 2.67945705145493136e+01 + 5.23800862467740629e+01 + -2.34193589858697777e+01 + -2.67958038635302600e+01 + 2.45872057837964846e+01 + 1.16252649553510334e+01 + 8.99424898505579051e+01 + -2.22061693363508326e+01 + 6.98562239082597500e+01 + -3.31539117547968303e+01 + -5.85832970823880999e+01 + 5.89049685210982901e+01 + 4.59874265404548481e+01 + -5.08083954437895216e+01 + -2.22191873368612889e+01 + 1.16539331220696187e+01 + -7.08270509506904347e+01 + 6.46018011815524602e+01 + 1.94415681363770645e+01 + 4.04539860386468320e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 9.78145711723120286e+01 + 1.76232674770143696e+01 + -1.10331368980137903e+01 + -1.40616464377447681e+01 + 1.69815554662718142e+01 + -9.75392068524539155e+01 + -9.78145711723120286e+01 + -1.76232674770143696e+01 + 1.10331368980137903e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -1.53159970565603700e+01 + 9.69589976091932186e+01 + 1.90885572211179095e+01 + 1.40616464377447681e+01 + -1.69815554662718142e+01 + 9.75392068524539155e+01 + 1.53159970565603700e+01 + -9.69589976091932186e+01 + -1.90885572211179095e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -2.25457773322750015e+01 + -1.39359117049240723e+02 + -1.01360767749937452e+02 + -1.06577486560693373e+02 + 3.69148207609075527e+01 + -6.52361857149522422e+01 + -9.51115531141346224e+01 + 7.07220482565039248e+01 + -3.57407782543718326e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 22 + 23 + + + + + + 9 + 7 + + 9.41929589378244003e+01 + 8.94097901372627391e+00 + 3.01620867599012286e+01 + 2.46922319059140740e+01 + 1.35465056881596215e+01 + -4.89963411381141611e+01 + -1.68344538935716344e+01 + -1.83167626535859007e+01 + 7.95313381974681306e+01 + -5.18828216803281350e+00 + -4.19655627938833504e+01 + -6.55823195034947481e+00 + 7.35562337314085681e+01 + 5.97403143502788865e+01 + -2.03387857576245445e-01 + 6.60668686562018337e+01 + -6.61937508072900442e+01 + -2.10487850516630495e+01 + 1.54947760148805180e+01 + -8.96286058833042887e+01 + -5.79472767879112993e+00 + -6.70795355915261116e+00 + -1.69646265262306706e+01 + -8.65671984840189168e+01 + -1.21336256996353509e+01 + 3.69909891189522924e+01 + -4.84851501469951671e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + 8.51395966446430350e+01 + -5.19584580090174413e+01 + 7.18106708732509347e+00 + -5.24346283807042397e+01 + -8.39535649250525182e+01 + 1.42270405549584336e+01 + -8.51395966446430350e+01 + 5.19584580090174413e+01 + -7.18106708732509347e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -1.36338907320492919e+00 + -1.58782107839693651e+01 + -9.87219509153607788e+01 + 5.24346283807042397e+01 + 8.39535649250525182e+01 + -1.42270405549584336e+01 + 1.36338907320492919e+00 + 1.58782107839693651e+01 + 9.87219509153607788e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 3.07007154914777551e+01 + 2.70881185233511914e+01 + 3.78166527462940882e+00 + -1.15162420127751133e+02 + -6.75390943713141123e+00 + 3.95885323960980529e+00 + -1.29970649317129876e+01 + 1.14524822858325365e+02 + 2.24989739182521227e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 23 + 24 + + + + + + 9 + 7 + + -1.83421298069983294e+01 + -8.40178887379047552e+01 + 1.37665553988889080e+01 + 3.46271545199608042e+01 + 4.05922653756383482e+01 + -7.00698095538007770e+00 + 9.19927590625682683e+01 + -3.22667773518094236e+01 + 3.91925773350330608e+00 + 4.07207141973007651e+01 + -3.85719388369921674e+01 + -7.75337815930943464e+01 + 7.24865255322073381e+01 + -3.53413936296266158e+01 + 5.86044610642822335e+01 + -1.83884977004002117e+01 + 1.46625501311897892e+01 + 1.87515674799128185e+01 + -2.53722481762123628e+01 + -3.66484706705800178e+01 + 3.56472023092901580e+01 + -4.40875208799477321e+01 + -6.95087704456724254e+01 + 1.97777311636326232e+01 + 1.27979930474377834e+01 + 3.35520358632383733e+01 + 9.10304934695741821e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -2.81762066375282245e+01 + 9.59328037613137070e+01 + -1.73162987170684679e+00 + -8.31278175295505264e+01 + -2.53085961788951153e+01 + -4.94898061445724977e+01 + 2.81762066375282245e+01 + -9.59328037613137070e+01 + 1.73162987170684679e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -4.79152098220707003e+01 + -1.25048839237672293e+01 + 8.68778484181097070e+01 + 8.31278175295505264e+01 + 2.53085961788951153e+01 + 4.94898061445724977e+01 + 4.79152098220707003e+01 + 1.25048839237672293e+01 + -8.68778484181097070e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -3.78847831737257366e+01 + 2.29972416224487830e+01 + -1.37159770597291157e+02 + -4.30909041970757869e+01 + -7.29718405033518991e+01 + -1.27753851859623239e+02 + -4.00153271264082644e+00 + -1.83214678716897197e+02 + 3.84258732608477231e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 24 + 25 + + + + + + 9 + 7 + + -1.42022591781322873e+01 + -3.56662961537267762e+01 + -1.29665633112741379e+01 + -4.42547548664715933e+01 + 8.53484085500931968e+01 + -1.21228131477824768e+01 + 7.44576769541145751e+01 + 3.58775048496875613e+01 + 4.62292669635554603e+01 + 1.27026107710752729e+01 + -9.11117710856913021e+01 + 2.52571349253896464e+01 + -3.19164772916096373e+00 + -2.63470878340458832e+01 + 1.88346366483448016e+01 + 5.40205809637968812e+01 + -1.90775864349462978e+01 + -8.09820522299282857e+01 + 6.16280452912953720e+01 + 1.93876135715390099e+01 + 7.12990641830701719e+01 + 6.58414093447228908e+01 + 1.80759831933341069e+01 + -6.99233759491107918e+01 + 3.27566373625739402e+01 + 9.70961890249048842e+00 + 4.91628253265810322e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 9.34197183252336458e+01 + 2.76233972145661149e+01 + 2.25766284985226449e+01 + -2.39170403241009595e+01 + 1.53702168542746431e+00 + 9.70855949483436831e+01 + -9.34197183252336458e+01 + -2.76233972145661149e+01 + -2.25766284985226449e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 2.64713318548450189e+01 + -9.60967506769340645e+01 + 8.04258037981846208e+00 + 2.39170403241009595e+01 + -1.53702168542746431e+00 + -9.70855949483436831e+01 + -2.64713318548450189e+01 + 9.60967506769340645e+01 + -8.04258037981846208e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 4.99415394471549448e+01 + 8.89774612805008616e+01 + -7.21549206200566857e+01 + -8.47137695131298614e+01 + -3.96302269514171712e+01 + 2.91958404364136328e+01 + -1.14979727399682560e+02 + 6.32312875848932592e+01 + -5.83619937525406982e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 25 + 26 + + + + + + 9 + 7 + + 4.75279810851286726e+01 + 1.27698906927141866e+01 + -3.85248964418578721e+00 + 8.38265009034030300e+01 + 2.34263445942633055e+01 + 4.00325151654156208e+00 + -3.67133537961906153e+00 + 7.31784749045807636e+00 + 9.95211070535517166e+01 + 7.72966987607180016e+01 + 3.83523138848579492e+01 + 1.84348658933893006e+01 + -5.00282378767150675e+01 + 1.62934700342947103e+00 + -1.23502536597363957e+01 + 3.08280910111897235e+01 + -9.23259671614170685e+01 + 9.05617058795025009e+00 + -3.32567607414421573e+01 + 4.65581163028938505e+01 + 8.06472888898161528e+01 + -1.09449242551094059e+01 + 8.15866317932763252e+01 + -5.45355209481987444e+01 + -6.85221032691171050e+00 + 2.03202505385987919e+01 + 3.49109728776780415e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + 5.75486223700384372e+01 + -3.56976774925786202e+01 + 7.35787461495951334e+01 + -7.77278254550152923e+01 + 4.09823526552186745e+00 + 6.27820803871861983e+01 + -5.75486223700384372e+01 + 3.56976774925786202e+01 + -7.35787461495951334e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -2.54271747023806824e+01 + -9.33213817372218273e+01 + -2.53885505161348775e+01 + 7.77278254550152923e+01 + -4.09823526552186745e+00 + -6.27820803871861983e+01 + 2.54271747023806824e+01 + 9.33213817372218273e+01 + 2.53885505161348775e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 5.11234651369603128e+01 + 1.45916426283882128e+01 + 8.14361101436695947e+01 + -9.64495575814352151e+01 + 5.69392664765279974e+01 + 1.45591038998347216e+02 + -1.52360008121073719e+02 + 3.92577375478538881e+00 + -7.47401324352360774e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 7 + 10 + + + + + + 9 + 7 + + -5.40766118135344485e+01 + -6.55945677789347137e+00 + 5.92166444746720977e-01 + 8.33981325801565987e+01 + -1.15536936327200639e+01 + 1.11233523720273890e+01 + -9.80808079024424018e+00 + -8.64371084278765380e+01 + 4.74584491239462309e+01 + 1.25135196935038806e+01 + 9.77748374151099995e+01 + 6.14999725352584115e+00 + 1.36807989121553284e+01 + 4.43989849701706429e+00 + -9.89599830977766572e+01 + -2.01505456436379626e+00 + -1.55263904030224698e+01 + -7.65145679760131925e-01 + 8.31600037168701789e+01 + -1.88305696684158264e+01 + -2.79941305069657931e+00 + 5.18811705043763141e+01 + -7.17174238817171439e+00 + 6.66998670151083850e+00 + -8.31970807941664781e+00 + -4.61019176373218968e+01 + -8.79761519740697651e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 7.83449748282976799e+01 + 6.13201754047114846e+01 + 1.00946028891096997e+01 + -1.98027611608243852e+01 + 9.23644092983748521e+00 + 9.75834966034573341e+01 + -7.83449748282976799e+01 + -6.13201754047114846e+01 + -1.00946028891096997e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 5.89059892503364324e+01 + -7.84507759508154265e+01 + 1.93793752003053505e+01 + 1.98027611608243852e+01 + -9.23644092983748521e+00 + -9.75834966034573341e+01 + -5.89059892503364324e+01 + 7.84507759508154265e+01 + -1.93793752003053505e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -6.07889604414481113e+01 + 8.49672366649234476e+01 + -1.19149060367207383e+02 + -3.26841768394553398e+01 + -8.97350957633814517e+01 + 9.13622785379021281e+01 + -1.77493968880864855e+02 + -4.85855715361152676e+01 + -7.39158595946127228e+00 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 9 + 14 + + + + + + 9 + 7 + + 4.43615564152903019e+01 + -3.09502672537490433e+01 + -6.65256070847414378e+01 + 4.96695435786911474e+01 + 2.78172662190901541e+01 + 6.19720498005987466e+01 + -7.45919697090238145e+01 + 1.35179597700267684e+00 + 1.13620760877569249e+00 + 5.07131897166802617e+01 + -6.73003419642306540e+01 + 3.11633063652204427e+01 + 5.47056695452382584e+01 + 6.11187671649742512e+01 + -2.76098847773204135e+01 + 6.65963623398869657e+01 + 1.43297991149407444e+00 + -1.99391285797603790e-01 + 7.65967691022320984e-01 + 1.84943157433388130e+01 + -6.50656556956834748e+01 + 8.44674932355512387e-01 + 2.30555803308049860e+01 + -6.99700625824593629e+01 + -3.40876610014427706e-01 + -9.55299072188111609e+01 + -2.94902469873441930e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -2.89030917873941853e+01 + 8.70749520313458305e+01 + -3.97814531392241904e+01 + -9.56630827209552450e+01 + -2.78467078053438328e+01 + 8.55192777842975183e+00 + 2.89030917873941853e+01 + -8.70749520313458305e+01 + 3.97814531392241904e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -3.63123800557649812e+00 + 4.05279359595652906e+01 + 9.13471429077244466e+01 + 9.56630827209552450e+01 + 2.78467078053438328e+01 + -8.55192777842975183e+00 + 3.63123800557649812e+00 + -4.05279359595652906e+01 + -9.13471429077244466e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -7.02581401004128878e+01 + -1.05108950320020767e+02 + -1.10573546354685661e+02 + -2.82871715200345584e+01 + -9.82678476821147342e+01 + -1.38389269150812240e+01 + 2.98238876860104902e+01 + -1.16573384649075678e+02 + 1.35327233223631623e+02 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 10 + 25 + + + + + + 9 + 7 + + 3.95766223652825175e+01 + 2.36448299245490254e+01 + -2.45799893899839930e+01 + -7.00228044208398188e+01 + -4.93985238797199173e+01 + 1.28698697829822883e+01 + 7.78345939301136092e+00 + 2.90892349948341078e+01 + 9.40990441114148695e+01 + -8.42616415682074660e+01 + -2.90334966132265606e+01 + -1.43512687624874875e+01 + -2.62231810185785967e+01 + -4.77560189769582593e+01 + -8.01321381020614343e-01 + 4.26634609899314583e+01 + -8.24081707666526171e+01 + 1.64578293010562966e+01 + -8.45857606147821528e+00 + 2.58771125144124774e+01 + -9.15441805767733570e+01 + 5.92769857321209486e+01 + -7.03919737513036381e+01 + -3.24535577243972782e+01 + -1.66359962010244793e+01 + 2.48028458121687763e+01 + -2.15529183777999371e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 9.34197183252336458e+01 + 2.76233972145661149e+01 + 2.25766284985226449e+01 + -2.39170403241009595e+01 + 1.53702168542746431e+00 + 9.70855949483436831e+01 + -9.34197183252336458e+01 + -2.76233972145661149e+01 + -2.25766284985226449e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 2.64713318548450189e+01 + -9.60967506769340645e+01 + 8.04258037981846208e+00 + 2.39170403241009595e+01 + -1.53702168542746431e+00 + -9.70855949483436831e+01 + -2.64713318548450189e+01 + 9.60967506769340645e+01 + -8.04258037981846208e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -6.19976450903283478e+01 + 1.85138049290959714e+02 + 2.04096238885501400e+01 + -5.38401179783636792e+01 + 1.95403129638118465e+01 + 3.38132128740523674e+00 + -1.81977553628765463e+02 + -6.94268461743989320e+01 + -2.32863678513998718e+00 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 13 + 22 + + + + + + 9 + 7 + + 1.61359876726555882e+01 + 1.08711432986265653e+01 + 8.51184696478907732e+00 + 6.31122752697449414e+01 + -1.20771880436756209e+01 + 7.50149628126125521e+01 + -4.10840851091186110e+01 + -8.68858010361213076e+01 + 2.35731234098279678e+01 + 4.08177879353251924e+01 + -2.17654374458011439e+01 + 8.78457730201184290e+01 + -2.17842465512247614e+01 + 1.46672234096332783e+01 + 6.07972286227193015e-01 + 6.89954693463132855e+01 + -4.93810369540159471e+01 + -4.74803457645439977e+01 + -6.20341282977744939e+01 + -7.54126088063309794e+01 + 1.25298075512294904e+01 + -5.28542109714301915e+01 + 5.80638264069757923e+01 + 5.82126617561848008e+01 + -2.29292298928228675e+01 + -1.00045379878072604e+00 + 1.50754924253946800e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 9.78145711723120286e+01 + 1.76232674770143696e+01 + -1.10331368980137903e+01 + -1.40616464377447681e+01 + 1.69815554662718142e+01 + -9.75392068524539155e+01 + -9.78145711723120286e+01 + -1.76232674770143696e+01 + 1.10331368980137903e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -1.53159970565603700e+01 + 9.69589976091932186e+01 + 1.90885572211179095e+01 + 1.40616464377447681e+01 + -1.69815554662718142e+01 + 9.75392068524539155e+01 + 1.53159970565603700e+01 + -9.69589976091932186e+01 + -1.90885572211179095e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 8.03183434009733901e+01 + -1.57959181406014523e+02 + -6.44060214101481705e+01 + -6.64898917378079801e+01 + -6.21866664385735888e+01 + -6.61756460634393022e+01 + -1.52823432141161049e+02 + -1.42511660560011357e+01 + -7.24093903591090111e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 20 + 15 + + + + + + 9 + 7 + + 4.71343070271561331e+01 + -6.72091990499062604e+01 + 5.47388912655424136e+01 + 8.35500845890857278e+01 + 2.23841738875626213e+01 + -3.32945376880477681e+01 + -2.59734415135038539e+01 + -2.12006909898996589e+01 + 2.34658474313030787e+01 + 6.32652845860905977e+00 + 9.30238234940692976e+00 + 3.37625583972403689e+01 + 3.44554933454442613e+00 + 8.79955033997688929e+01 + 4.09722885299349642e+01 + -1.92439510927881194e+01 + 4.62720459594682580e+01 + -8.23910435376807584e+01 + -5.55999617765192156e+00 + 6.20368695338309522e+01 + 7.15470037167418553e+01 + 3.41059120969163914e+01 + -1.39920442875337869e+01 + -2.48395357584865195e+01 + 9.22321060162172301e+01 + 2.27421056426905714e+01 + 1.25600461547882558e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -8.15059339771809164e+01 + -5.54598536753216109e+01 + -1.67626774955357369e+01 + -1.04808841952049807e+01 + 4.25686446848321935e+01 + -8.98780371179923492e+01 + 8.15059339771809164e+01 + 5.54598536753216109e+01 + 1.67626774955357369e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 5.69818724946287603e+01 + -7.14990567770542356e+01 + -4.05086544703406517e+01 + 1.04808841952049807e+01 + -4.25686446848321935e+01 + 8.98780371179923492e+01 + -5.69818724946287603e+01 + 7.14990567770542356e+01 + 4.05086544703406517e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + -1.44773765128431023e+02 + 3.21567907629271019e+01 + 6.77990836285083986e+01 + 3.24643872643349098e+01 + 3.07230481384375675e+00 + -8.66387073171390654e+00 + 6.03331707496851593e+01 + 1.38479259212844596e+02 + 6.83329111783498746e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 27 + 0 + 0 + 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12 + 13 + 14 + 15 + 16 + 17 + 18 + 19 + 20 + 21 + 22 + 23 + 24 + 25 + 26 + + + + + + 3 + 82 + + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + + + + 29 + 0 + 0 + 3 + 6 + 9 + 12 + 15 + 18 + 21 + 24 + 27 + 30 + 33 + 36 + 39 + 42 + 45 + 48 + 51 + 54 + 57 + 60 + 63 + 66 + 69 + 72 + 75 + 78 + 81 + 82 + + 0 + 3 + 0 + + + + + + + + + + + diff --git a/examples/Data/toy3D.xml b/examples/Data/toy3D.xml new file mode 100644 index 000000000..13dbcbe6c --- /dev/null +++ b/examples/Data/toy3D.xml @@ -0,0 +1,169 @@ + + + + + + + 2 + 1 + + + + + + 2 + 0 + 0 + 1 + + + + + + 9 + 7 + + 1.22427709071730959e+01 + 1.51514613104920048e+01 + 3.60934366857813060e+00 + -1.18407259026383116e+01 + 7.84826117220921216e+00 + 1.23509165494819051e+01 + -6.09875015991639735e+00 + 6.16547190708139126e-01 + 3.94972084922329048e+00 + -4.89208482920378174e+00 + 3.02091647632478866e+00 + -8.95328692238917512e+00 + 7.89831607220345955e+00 + -2.36793602009719084e+00 + 1.48517612051941725e+01 + -3.97284286249233731e-01 + -1.95744531643153863e+01 + -3.85954855417462017e+00 + 4.79268277145419042e+00 + -9.01707953629520453e+00 + 1.37848069005841385e+01 + 1.04829326688375950e+01 + -5.00630568442241675e+00 + 4.70463561852773182e+00 + -1.59179134598689274e+01 + -2.04767784956723942e+00 + 9.54135497908261954e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + -1.76201757312015372e+00 + 1.98634190821282672e+01 + 1.52966546661624236e+00 + 1.94817649567575373e+01 + 1.39684693294110307e+00 + 4.30228460420588288e+00 + 1.76201757312015372e+00 + -1.98634190821282672e+01 + -1.52966546661624236e+00 + -0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 4.16606867942304948e+00 + 1.86906420801308037e+00 + -1.94717865319198360e+01 + -1.94817649567575373e+01 + -1.39684693294110307e+00 + -4.30228460420588288e+00 + -4.16606867942304948e+00 + -1.86906420801308037e+00 + 1.94717865319198360e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00891462904330531e+01 + -1.08132497987816869e+01 + 8.66487736568128497e+00 + 2.88370015604634311e+01 + 1.89391698948574643e+01 + 2.12398960190661290e+00 + 1.22150946112124039e+01 + -2.33658532501596561e+01 + 1.51576204760307363e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 0 + 1 + + + + + + 3 + 7 + + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 3 + 0 + + + + + + + + + + + diff --git a/examples/DiscreteBayesNet_FG.cpp b/examples/DiscreteBayesNet_FG.cpp index 64c17c3db..6eb08c12e 100644 --- a/examples/DiscreteBayesNet_FG.cpp +++ b/examples/DiscreteBayesNet_FG.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index a8e50e1f1..f04b73f6b 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -91,7 +91,7 @@ int main(int argc, char* argv[]) cout << "initial state:\n" << initial_state.transpose() << "\n\n"; // Assemble initial quaternion through gtsam constructor ::quaternion(w,x,y,z); - Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3), + Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3), initial_state(4), initial_state(5)); Point3 prior_point(initial_state.head<3>()); Pose3 prior_pose(prior_rotation, prior_point); @@ -102,7 +102,7 @@ int main(int argc, char* argv[]) int correction_count = 0; initial_values.insert(X(correction_count), prior_pose); initial_values.insert(V(correction_count), prior_velocity); - initial_values.insert(B(correction_count), prior_imu_bias); + initial_values.insert(B(correction_count), prior_imu_bias); // Assemble prior noise model and add it the graph. noiseModel::Diagonal::shared_ptr pose_noise_model = noiseModel::Diagonal::Sigmas((Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5).finished()); // rad,rad,rad,m, m, m @@ -138,7 +138,7 @@ int main(int argc, char* argv[]) p->biasAccCovariance = bias_acc_cov; // acc bias in continuous p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous p->biasAccOmegaInt = bias_acc_omega_int; - + #ifdef USE_COMBINED imu_preintegrated_ = new PreintegratedCombinedMeasurements(p, prior_imu_bias); #else @@ -154,7 +154,7 @@ int main(int argc, char* argv[]) double current_position_error = 0.0, current_orientation_error = 0.0; double output_time = 0.0; - double dt = 0.005; // The real system has noise, but here, results are nearly + double dt = 0.005; // The real system has noise, but here, results are nearly // exactly the same, so keeping this for simplicity. // All priors have been set up, now iterate through the data file. @@ -203,8 +203,8 @@ int main(int argc, char* argv[]) *preint_imu); graph->add(imu_factor); imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); - graph->add(BetweenFactor(B(correction_count-1), - B(correction_count ), + graph->add(BetweenFactor(B(correction_count-1), + B(correction_count ), zero_bias, bias_noise_model)); #endif @@ -215,7 +215,7 @@ int main(int argc, char* argv[]) gps(2)), // D, correction_noise); graph->add(gps_factor); - + // Now optimize and compare results. prop_state = imu_preintegrated_->predict(prev_state, prev_bias); initial_values.insert(X(correction_count), prop_state.pose()); @@ -250,10 +250,10 @@ int main(int argc, char* argv[]) // display statistics cout << "Position error:" << current_position_error << "\t " << "Angular error:" << current_orientation_error << "\n"; - fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", + fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", output_time, gtsam_position(0), gtsam_position(1), gtsam_position(2), gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(), gtsam_quat.w(), - gps(0), gps(1), gps(2), + gps(0), gps(1), gps(2), gps_quat.x(), gps_quat.y(), gps_quat.z(), gps_quat.w()); output_time += 1.0; diff --git a/examples/InverseKinematicsExampleExpressions.cpp b/examples/InverseKinematicsExampleExpressions.cpp new file mode 100644 index 000000000..9e86886e7 --- /dev/null +++ b/examples/InverseKinematicsExampleExpressions.cpp @@ -0,0 +1,92 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file InverseKinematicsExampleExpressions.cpp + * @brief Implement inverse kinematics on a three-link arm using expressions. + * @date April 15, 2019 + * @author Frank Dellaert + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +// Scalar multiplication of a vector, with derivatives. +inline Vector3 scalarMultiply(const double& s, const Vector3& v, + OptionalJacobian<3, 1> Hs, + OptionalJacobian<3, 3> Hv) { + if (Hs) *Hs = v; + if (Hv) *Hv = s * I_3x3; + return s * v; +} + +// Expression version of scalar product, using above function. +inline Vector3_ operator*(const Double_& s, const Vector3_& v) { + return Vector3_(&scalarMultiply, s, v); +} + +// Expression version of Pose2::Expmap +inline Pose2_ Expmap(const Vector3_& xi) { return Pose2_(&Pose2::Expmap, xi); } + +// Main function +int main(int argc, char** argv) { + // Three-link planar manipulator specification. + const double L1 = 3.5, L2 = 3.5, L3 = 2.5; // link lengths + const Pose2 sXt0(0, L1 + L2 + L3, M_PI / 2); // end-effector pose at rest + const Vector3 xi1(0, 0, 1), xi2(L1, 0, 1), + xi3(L1 + L2, 0, 1); // screw axes at rest + + // Create Expressions for unknowns + using symbol_shorthand::Q; + Double_ q1(Q(1)), q2(Q(2)), q3(Q(3)); + + // Forward kinematics expression as product of exponentials + Pose2_ l1Zl1 = Expmap(q1 * Vector3_(xi1)); + Pose2_ l2Zl2 = Expmap(q2 * Vector3_(xi2)); + Pose2_ l3Zl3 = Expmap(q3 * Vector3_(xi3)); + Pose2_ forward = compose(compose(l1Zl1, l2Zl2), compose(l3Zl3, Pose2_(sXt0))); + + // Create a factor graph with a a single expression factor. + ExpressionFactorGraph graph; + Pose2 desiredEndEffectorPose(3, 2, 0); + auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); + graph.addExpressionFactor(forward, desiredEndEffectorPose, model); + + // Create initial estimate + Values initial; + initial.insert(Q(1), 0.1); + initial.insert(Q(2), 0.2); + initial.insert(Q(3), 0.3); + initial.print("\nInitial Estimate:\n"); // print + GTSAM_PRINT(forward.value(initial)); + + // Optimize the initial values using a Levenberg-Marquardt nonlinear optimizer + LevenbergMarquardtParams params; + params.setlambdaInitial(1e6); + LevenbergMarquardtOptimizer optimizer(graph, initial, params); + Values result = optimizer.optimize(); + result.print("Final Result:\n"); + + GTSAM_PRINT(forward.value(result)); + + return 0; +} diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index 17f921fd1..3c07fd4e3 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/examples/METISOrderingExample.cpp b/examples/METISOrderingExample.cpp index 3416eb6b7..76b5098f6 100644 --- a/examples/METISOrderingExample.cpp +++ b/examples/METISOrderingExample.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index 3b547e70c..6dc0d9a93 100644 --- a/examples/OdometryExample.cpp +++ b/examples/OdometryExample.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index 7c43c22e2..52638fdca 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp b/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp index 68a3fd7e7..551814a6b 100644 --- a/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp +++ b/examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp @@ -34,47 +34,47 @@ int main(int argc, char* argv[]) { // Concatenate poses to create trajectory poses.insert( poses.end(), poses2.begin(), poses2.end() ); poses.insert( poses.end(), poses3.begin(), poses3.end() ); // std::vector of Pose3 - auto points = createPoints(); // std::vector of Point3 + auto points = createPoints(); // std::vector of Point3 // (ground-truth) sensor pose in body frame, further an unknown variable Pose3 body_T_sensor_gt(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); - + // The graph ExpressionFactorGraph graph; - + // Specify uncertainty on first pose prior and also for between factor (simplicity reasons) auto poseNoise = noiseModel::Diagonal::Sigmas((Vector(6)<<0.3,0.3,0.3,0.1,0.1,0.1).finished()); - + // Uncertainty bearing range measurement; auto bearingRangeNoise = noiseModel::Diagonal::Sigmas((Vector(3)<<0.01,0.03,0.05).finished()); - + // Expressions for body-frame at key 0 and sensor-tf Pose3_ x_('x', 0); Pose3_ body_T_sensor_('T', 0); - + // Add a prior on the body-pose - graph.addExpressionFactor(x_, poses[0], poseNoise); - + graph.addExpressionFactor(x_, poses[0], poseNoise); + // Simulated measurements from pose for (size_t i = 0; i < poses.size(); ++i) { auto world_T_sensor = poses[i].compose(body_T_sensor_gt); for (size_t j = 0; j < points.size(); ++j) { - + // This expression is the key feature of this example: it creates a differentiable expression of the measurement after being displaced by sensor transform. auto prediction_ = Expression( BearingRange3D::Measure, Pose3_('x',i)*body_T_sensor_, Point3_('l',j)); - + // Create a *perfect* measurement auto measurement = BearingRange3D(world_T_sensor.bearing(points[j]), world_T_sensor.range(points[j])); - + // Add factor graph.addExpressionFactor(prediction_, measurement, bearingRangeNoise); } - + // and add a between factor to the graph if (i > 0) { // And also we have a *perfect* measurement for the between factor. - graph.addExpressionFactor(between(Pose3_('x', i-1),Pose3_('x', i)), poses[i-1].between(poses[i]), poseNoise); + graph.addExpressionFactor(between(Pose3_('x', i-1),Pose3_('x', i)), poses[i-1].between(poses[i]), poseNoise); } } @@ -82,12 +82,12 @@ int main(int argc, char* argv[]) { Values initial; Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); for (size_t i = 0; i < poses.size(); ++i) - initial.insert(Symbol('x', i), poses[i].compose(delta)); + initial.insert(Symbol('x', i), poses[i].compose(delta)); for (size_t j = 0; j < points.size(); ++j) initial.insert(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15)); - + // Initialize body_T_sensor wrongly (because we do not know!) - initial.insert(Symbol('T',0), Pose3()); + initial.insert(Symbol('T',0), Pose3()); std::cout << "initial error: " << graph.error(initial) << std::endl; Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); diff --git a/examples/Pose3SLAMExample_changeKeys.cpp b/examples/Pose3SLAMExample_changeKeys.cpp index 9cab73822..cceaac4ee 100644 --- a/examples/Pose3SLAMExample_changeKeys.cpp +++ b/examples/Pose3SLAMExample_changeKeys.cpp @@ -79,7 +79,7 @@ int main(const int argc, const char *argv[]) { key2 = pose3Between->key2() - firstKey; } NonlinearFactor::shared_ptr simpleFactor( - new BetweenFactor(key1, key2, pose3Between->measured(), pose3Between->get_noiseModel())); + new BetweenFactor(key1, key2, pose3Between->measured(), pose3Between->noiseModel())); simpleGraph.add(simpleFactor); } } diff --git a/examples/SFMExampleExpressions.cpp b/examples/SFMExampleExpressions.cpp index 30db9144d..191664ef6 100644 --- a/examples/SFMExampleExpressions.cpp +++ b/examples/SFMExampleExpressions.cpp @@ -72,7 +72,7 @@ int main(int argc, char* argv[]) { Point2 measurement = camera.project(points[j]); // Below an expression for the prediction of the measurement: Point3_ p('l', j); - Point2_ prediction = uncalibrate(cK, project(transform_to(x, p))); + Point2_ prediction = uncalibrate(cK, project(transformTo(x, p))); // Again, here we use an ExpressionFactor graph.addExpressionFactor(prediction, measurement, measurementNoise); } diff --git a/examples/SFMdata.h b/examples/SFMdata.h index af1f761ee..5462868c9 100644 --- a/examples/SFMdata.h +++ b/examples/SFMdata.h @@ -52,9 +52,9 @@ std::vector createPoints() { /* ************************************************************************* */ std::vector createPoses( const gtsam::Pose3& init = gtsam::Pose3(gtsam::Rot3::Ypr(M_PI/2,0,-M_PI/2), gtsam::Point3(30, 0, 0)), - const gtsam::Pose3& delta = gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4,0), gtsam::Point3(sin(M_PI/4)*30, 0, 30*(1-sin(M_PI/4)))), + const gtsam::Pose3& delta = gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4,0), gtsam::Point3(sin(M_PI/4)*30, 0, 30*(1-sin(M_PI/4)))), int steps = 8) { - + // Create the set of ground-truth poses // Default values give a circular trajectory, radius 30 at pi/4 intervals, always facing the circle center std::vector poses; @@ -63,6 +63,6 @@ std::vector createPoses( for(; i < steps; ++i) { poses.push_back(poses[i-1].compose(delta)); } - + return poses; } \ No newline at end of file diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index 1f40d8c60..f70a44eec 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index f3915ce22..80ac08e03 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -361,8 +361,8 @@ void runIncremental() const auto& measured = factor->measured(); Rot2 measuredBearing = measured.bearing(); double measuredRange = measured.range(); - newVariables.insert(lmKey, - pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0)))); + newVariables.insert(lmKey, + pose.transformFrom(measuredBearing.rotate(Point2(measuredRange, 0.0)))); } } else diff --git a/examples/StereoVOExample.cpp b/examples/StereoVOExample.cpp index 27c10deb3..65ab5422d 100644 --- a/examples/StereoVOExample.cpp +++ b/examples/StereoVOExample.cpp @@ -22,7 +22,7 @@ * -moves forward 1 meter * -takes stereo readings on three landmarks */ - + #include #include #include @@ -40,7 +40,7 @@ int main(int argc, char** argv){ NonlinearFactorGraph graph; Pose3 first_pose; graph.emplace_shared >(1, Pose3()); - + //create factor noise model with 3 sigmas of value 1 const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1); //create stereo camera calibration object with .2m between cameras diff --git a/examples/StereoVOExample_large.cpp b/examples/StereoVOExample_large.cpp index 80831bd21..5eeb6ac3c 100644 --- a/examples/StereoVOExample_large.cpp +++ b/examples/StereoVOExample_large.cpp @@ -50,7 +50,7 @@ int main(int argc, char** argv){ string calibration_loc = findExampleDataFile("VO_calibration.txt"); string pose_loc = findExampleDataFile("VO_camera_poses_large.txt"); string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt"); - + //read camera calibration info from file // focal lengths fx, fy, skew s, principal point u0, v0, baseline b double fx, fy, s, u0, v0, b; @@ -60,7 +60,7 @@ int main(int argc, char** argv){ //create stereo camera calibration object const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b)); - + ifstream pose_file(pose_loc.c_str()); cout << "Reading camera poses" << endl; int pose_id; @@ -72,7 +72,7 @@ int main(int argc, char** argv){ } initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); } - + // camera and landmark keys size_t x, l; @@ -89,8 +89,8 @@ int main(int argc, char** argv){ //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it if (!initial_estimate.exists(Symbol('l', l))) { Pose3 camPose = initial_estimate.at(Symbol('x', x)); - //transform_from() transforms the input Point3 from the camera pose space, camPose, to the global space - Point3 worldPoint = camPose.transform_from(Point3(X, Y, Z)); + //transformFrom() transforms the input Point3 from the camera pose space, camPose, to the global space + Point3 worldPoint = camPose.transformFrom(Point3(X, Y, Z)); initial_estimate.insert(Symbol('l', l), worldPoint); } } diff --git a/examples/UGM_chain.cpp b/examples/UGM_chain.cpp index 0461c1869..4ce4e7af4 100644 --- a/examples/UGM_chain.cpp +++ b/examples/UGM_chain.cpp @@ -36,7 +36,7 @@ int main(int argc, char** argv) { // Each node takes 1 of 7 possible states denoted by 0-6 in following order: // ["VideoGames" "Industry" "GradSchool" "VideoGames(with PhD)" - // "Industry(with PhD)" "Academia" "Deceased"] + // "Industry(with PhD)" "Academia" "Deceased"] const size_t nrStates = 7; // define variables diff --git a/examples/UGM_small.cpp b/examples/UGM_small.cpp index 43cd9b34c..f5338bc67 100644 --- a/examples/UGM_small.cpp +++ b/examples/UGM_small.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp index d68cedb74..49b99a5b6 100644 --- a/examples/easyPoint2KalmanFilter.cpp +++ b/examples/easyPoint2KalmanFilter.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index a0300adf9..1dde7efb5 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -140,7 +140,7 @@ int main() { const GaussianConditional::shared_ptr& cg0 = linearBayesNet->back(); assert(cg0->nrFrontals() == 1); assert(cg0->nrParents() == 0); - linearFactorGraph->add(0, cg0->get_R(), cg0->get_d() - cg0->get_R()*result[ordering->at(x1)], noiseModel::Diagonal::Sigmas(cg0->get_sigmas(), true)); + linearFactorGraph->add(0, cg0->R(), cg0->d() - cg0->R()*result[ordering->at(x1)], noiseModel::Diagonal::Sigmas(cg0->get_sigmas(), true)); // Create the desired ordering ordering = Ordering::shared_ptr(new Ordering); diff --git a/gtsam.h b/gtsam.h index 014907037..686203748 100644 --- a/gtsam.h +++ b/gtsam.h @@ -209,11 +209,60 @@ class KeyGroupMap { bool insert2(size_t key, int val); }; +// Actually a FastSet +class FactorIndexSet { + FactorIndexSet(); + FactorIndexSet(const gtsam::FactorIndexSet& set); + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + void insert(size_t factorIndex); + bool erase(size_t factorIndex); // returns true if value was removed + bool count(size_t factorIndex) const; // returns true if value exists +}; + +// Actually a vector +class FactorIndices { + FactorIndices(); + FactorIndices(const gtsam::FactorIndices& other); + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + size_t at(size_t i) const; + size_t front() const; + size_t back() const; + void push_back(size_t factorIndex) const; +}; + //************************************************************************* // base //************************************************************************* /** gtsam namespace functions */ + +#include +class IndexPair { + IndexPair(); + IndexPair(size_t i, size_t j); + size_t i() const; + size_t j() const; +}; + +template +class DSFMap { + DSFMap(); + KEY find(const KEY& key) const; + void merge(const KEY& x, const KEY& y); +}; + #include bool linear_independent(Matrix A, Matrix B, double tol); @@ -583,8 +632,8 @@ class Pose2 { static Matrix wedge(double vx, double vy, double w); // Group Actions on Point2 - gtsam::Point2 transform_from(const gtsam::Point2& p) const; - gtsam::Point2 transform_to(const gtsam::Point2& p) const; + gtsam::Point2 transformFrom(const gtsam::Point2& p) const; + gtsam::Point2 transformTo(const gtsam::Point2& p) const; // Standard Interface double x() const; @@ -636,8 +685,8 @@ class Pose3 { static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); // Group Action on Point3 - gtsam::Point3 transform_from(const gtsam::Point3& point) const; - gtsam::Point3 transform_to(const gtsam::Point3& point) const; + gtsam::Point3 transformFrom(const gtsam::Point3& point) const; + gtsam::Point3 transformTo(const gtsam::Point3& point) const; // Standard Interface gtsam::Rot3 rotation() const; @@ -646,7 +695,8 @@ class Pose3 { double y() const; double z() const; Matrix matrix() const; - gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to() + gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose) const; + gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose) const; double range(const gtsam::Point3& point); double range(const gtsam::Pose3& pose); @@ -1219,7 +1269,7 @@ namespace noiseModel { #include virtual class Base { void print(string s) const; - // Methods below are available for all noise models. However, can't add them + // Methods below are available for all noise models. However, can't add them // because wrap (incorrectly) thinks robust classes derive from this Base as well. // bool isConstrained() const; // bool isUnit() const; @@ -1257,7 +1307,7 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian { Vector sigmas() const; Vector invsigmas() const; Vector precisions() const; - + // enabling serialization functionality void serializable() const; }; @@ -1730,7 +1780,7 @@ virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { virtual class SubgraphSolver { SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); - SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); + SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph* Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); gtsam::VectorValues optimize() const; }; @@ -1867,7 +1917,6 @@ virtual class NonlinearFactor { #include virtual class NoiseModelFactor: gtsam::NonlinearFactor { bool equals(const gtsam::NoiseModelFactor& other, double tol) const; - gtsam::noiseModel::Base* get_noiseModel() const; // deprecated by below gtsam::noiseModel::Base* noiseModel() const; Vector unwhitenedError(const gtsam::Values& x) const; Vector whitenedError(const gtsam::Values& x) const; @@ -2053,7 +2102,7 @@ virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { bool getUseFixedLambdaFactor(); string getLogFile() const; string getVerbosityLM() const; - + void setDiagonalDamping(bool flag); void setlambdaFactor(double value); void setlambdaInitial(double value); @@ -2091,7 +2140,7 @@ virtual class NonlinearOptimizer { double error() const; int iterations() const; gtsam::Values values() const; - void iterate() const; + gtsam::GaussianFactorGraph* iterate() const; }; #include @@ -2209,8 +2258,6 @@ class ISAM2Result { size_t getCliques() const; }; -class FactorIndices {}; - class ISAM2 { ISAM2(); ISAM2(const gtsam::ISAM2Params& params); diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index b4adc9978..60915eead 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -101,7 +101,10 @@ message(STATUS "Building GTSAM - shared: ${BUILD_SHARED_LIBS}") # BUILD_SHARED_LIBS automatically defines static/shared libs: add_library(gtsam ${gtsam_srcs}) +# Boost: target_link_libraries(gtsam PUBLIC ${GTSAM_BOOST_LIBRARIES}) +target_include_directories(gtsam PUBLIC ${Boost_INCLUDE_DIR}) +# Other deps: target_link_libraries(gtsam PUBLIC ${GTSAM_ADDITIONAL_LIBRARIES}) target_compile_definitions(gtsam PRIVATE ${GTSAM_COMPILE_DEFINITIONS_PRIVATE}) target_compile_definitions(gtsam PUBLIC ${GTSAM_COMPILE_DEFINITIONS_PUBLIC}) diff --git a/gtsam_unstable/base/DSFMap.h b/gtsam/base/DSFMap.h similarity index 80% rename from gtsam_unstable/base/DSFMap.h rename to gtsam/base/DSFMap.h index 6ddb74cfd..dfce185dc 100644 --- a/gtsam_unstable/base/DSFMap.h +++ b/gtsam/base/DSFMap.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -18,9 +18,9 @@ #pragma once +#include // Provides size_t #include #include -#include // Provides size_t namespace gtsam { @@ -29,11 +29,9 @@ namespace gtsam { * Uses rank compression and union by rank, iterator version * @addtogroup base */ -template +template class DSFMap { - -protected: - + protected: /// We store the forest in an STL map, but parents are done with pointers struct Entry { typename std::map::iterator parent_; @@ -41,7 +39,7 @@ protected: Entry() {} }; - typedef typename std::map Map; + typedef typename std::map Map; typedef typename Map::iterator iterator; mutable Map entries_; @@ -62,8 +60,7 @@ protected: iterator find_(const iterator& it) const { // follow parent pointers until we reach set representative iterator& parent = it->second.parent_; - if (parent != it) - parent = find_(parent); // not yet, recurse! + if (parent != it) parent = find_(parent); // not yet, recurse! return parent; } @@ -73,13 +70,11 @@ protected: return find_(initial); } -public: - + public: typedef std::set Set; /// constructor - DSFMap() { - } + DSFMap() {} /// Given key, find the representative key for the set in which it lives inline KEY find(const KEY& key) const { @@ -89,12 +84,10 @@ public: /// Merge two sets void merge(const KEY& x, const KEY& y) { - // straight from http://en.wikipedia.org/wiki/Disjoint-set_data_structure iterator xRoot = find_(x); iterator yRoot = find_(y); - if (xRoot == yRoot) - return; + if (xRoot == yRoot) return; // Merge sets if (xRoot->second.rank_ < yRoot->second.rank_) @@ -117,7 +110,14 @@ public: } return sets; } - }; -} +/// Small utility class for representing a wrappable pairs of ints. +class IndexPair : public std::pair { + public: + IndexPair(): std::pair(0,0) {} + IndexPair(size_t i, size_t j) : std::pair(i,j) {} + inline size_t i() const { return first; }; + inline size_t j() const { return second; }; +}; +} // namespace gtsam diff --git a/gtsam/base/DSFVector.cpp b/gtsam/base/DSFVector.cpp index bdf0543ff..95e8832e0 100644 --- a/gtsam/base/DSFVector.cpp +++ b/gtsam/base/DSFVector.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/DSFVector.h b/gtsam/base/DSFVector.h index d73accf11..fdbd96dc8 100644 --- a/gtsam/base/DSFVector.h +++ b/gtsam/base/DSFVector.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 77edd5fd5..210bdcc73 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 417570604..e159ffa87 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index 310abcf02..a7491d804 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 7bfdd7ea5..21df90513 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -170,7 +170,7 @@ struct FixedDimension { #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// Helper class to construct the product manifold of two other manifolds, M1 and M2 -/// Deprecated because of limited usefulness, maximum obfuscation +/// Deprecated because of limited usefulness, maximum obfuscation template class ProductManifold: public std::pair { BOOST_CONCEPT_ASSERT((IsManifold)); diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 0c37b405e..705d84d25 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -540,7 +540,7 @@ Matrix cholesky_inverse(const Matrix &A) } /* ************************************************************************* */ -// Semantics: +// Semantics: // if B = inverse_square_root(A), then all of the following are true: // inv(B) * inv(B)' == A // inv(B' * B) == A diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index eb1c1bbcc..166297d5f 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 87ead88f0..458538003 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/SymmetricBlockMatrix.cpp b/gtsam/base/SymmetricBlockMatrix.cpp index 5eaa2df1c..7f41da137 100644 --- a/gtsam/base/SymmetricBlockMatrix.cpp +++ b/gtsam/base/SymmetricBlockMatrix.cpp @@ -65,8 +65,9 @@ Matrix SymmetricBlockMatrix::block(DenseIndex I, DenseIndex J) const { void SymmetricBlockMatrix::choleskyPartial(DenseIndex nFrontals) { gttic(VerticalBlockMatrix_choleskyPartial); DenseIndex topleft = variableColOffsets_[blockStart_]; - if (!gtsam::choleskyPartial(matrix_, offset(nFrontals) - topleft, topleft)) + if (!gtsam::choleskyPartial(matrix_, offset(nFrontals) - topleft, topleft)) { throw CholeskyFailed(); + } } /* ************************************************************************* */ diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index 8792be0e6..1a31aa284 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/ThreadsafeException.h b/gtsam/base/ThreadsafeException.h index ca13047a8..ff82ff27c 100644 --- a/gtsam/base/ThreadsafeException.h +++ b/gtsam/base/ThreadsafeException.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index 1cccf0319..a19fbe176 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 7caf490ef..856b559c3 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/VerticalBlockMatrix.cpp b/gtsam/base/VerticalBlockMatrix.cpp index 9f54b9c97..344081c8a 100644 --- a/gtsam/base/VerticalBlockMatrix.cpp +++ b/gtsam/base/VerticalBlockMatrix.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index 9cd566ecf..92031db2b 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/deprecated/LieMatrix.h b/gtsam/base/deprecated/LieMatrix.h index 9ee20a596..953537bf7 100644 --- a/gtsam/base/deprecated/LieMatrix.h +++ b/gtsam/base/deprecated/LieMatrix.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/deprecated/LieScalar.h b/gtsam/base/deprecated/LieScalar.h index 68632addc..50ea9d695 100644 --- a/gtsam/base/deprecated/LieScalar.h +++ b/gtsam/base/deprecated/LieScalar.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/deprecated/LieVector.h b/gtsam/base/deprecated/LieVector.h index b271275c3..60c8103e2 100644 --- a/gtsam/base/deprecated/LieVector.h +++ b/gtsam/base/deprecated/LieVector.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -36,7 +36,7 @@ struct GTSAM_EXPORT LieVector : public Vector { /** initialize from a normal vector */ LieVector(const Vector& v) : Vector(v) {} - + template LieVector(const V& v) : Vector(v) {} diff --git a/gtsam/base/lieProxies.h b/gtsam/base/lieProxies.h index ce224496e..820fc7333 100644 --- a/gtsam/base/lieProxies.h +++ b/gtsam/base/lieProxies.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index cc1cbdb51..a9a088108 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -67,28 +67,29 @@ struct FixedSizeMatrix { } /** - * Numerically compute gradient of scalar function + * @brief Numerically compute gradient of scalar function + * @return n-dimensional gradient computed via central differencing * Class X is the input argument * The class X needs to have dim, expmap, logmap + * int N is the dimension of the X input value if variable dimension type but known at test time */ -template -typename internal::FixedSizeMatrix::type numericalGradient(boost::function h, const X& x, - double delta = 1e-5) { + +template ::dimension> +typename Eigen::Matrix numericalGradient( + boost::function h, const X& x, double delta = 1e-5) { double factor = 1.0 / (2.0 * delta); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X must be a manifold type."); - static const int N = traits::dimension; - BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type."); - - typedef typename traits::TangentVector TangentX; + BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type or N must be specified."); // Prepare a tangent vector to perturb x with, only works for fixed size - TangentX d; + typename traits::TangentVector d; d.setZero(); - Eigen::Matrix g; g.setZero(); // Can be fixed size + Eigen::Matrix g; + g.setZero(); for (int j = 0; j < N; j++) { d(j) = delta; double hxplus = h(traits::Retract(x, d)); @@ -108,37 +109,34 @@ typename internal::FixedSizeMatrix::type numericalGradient(boost::function +template ::dimension> // TODO Should compute fixed-size matrix -typename internal::FixedSizeMatrix::type numericalDerivative11(boost::function h, const X& x, - double delta = 1e-5) { - +typename internal::FixedSizeMatrix::type numericalDerivative11( + boost::function h, const X& x, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Matrix; BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); typedef traits TraitsY; - typedef typename TraitsY::TangentVector TangentY; BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X must be a manifold type."); - static const int N = traits::dimension; - BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type."); + BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type or N must be specified."); typedef traits TraitsX; - typedef typename TraitsX::TangentVector TangentX; // get value at x, and corresponding chart const Y hx = h(x); // Bit of a hack for now to find number of rows - const TangentY zeroY = TraitsY::Local(hx, hx); + const typename TraitsY::TangentVector zeroY = TraitsY::Local(hx, hx); const size_t m = zeroY.size(); // Prepare a tangent vector to perturb x with, only works for fixed size - TangentX dx; + Eigen::Matrix dx; dx.setZero(); // Fill in Jacobian H @@ -146,9 +144,9 @@ typename internal::FixedSizeMatrix::type numericalDerivative11(boost::funct const double factor = 1.0 / (2.0 * delta); for (int j = 0; j < N; j++) { dx(j) = delta; - const TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); + const auto dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); dx(j) = -delta; - const TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); + const auto dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); dx(j) = 0; H.col(j) << (dy1 - dy2) * factor; } diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h index 954d3e86b..e475465de 100644 --- a/gtsam/base/serialization.h +++ b/gtsam/base/serialization.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/base/tests/testDSFMap.cpp b/gtsam/base/tests/testDSFMap.cpp similarity index 92% rename from gtsam_unstable/base/tests/testDSFMap.cpp rename to gtsam/base/tests/testDSFMap.cpp index 9c9143a15..816498ce8 100644 --- a/gtsam_unstable/base/tests/testDSFMap.cpp +++ b/gtsam/base/tests/testDSFMap.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -16,7 +16,7 @@ * @brief unit tests for DSFMap */ -#include +#include #include #include @@ -115,7 +115,6 @@ TEST(DSFMap, mergePairwiseMatches2) { TEST(DSFMap, sets){ // Create some "matches" typedef pair Match; - typedef pair > key_pair; list matches; matches += Match(1,2), Match(2,3), Match(4,5), Match(4,6); @@ -128,18 +127,24 @@ TEST(DSFMap, sets){ set s1, s2; s1 += 1,2,3; s2 += 4,5,6; - + /*for(key_pair st: sets){ cout << "Set " << st.first << " :{"; for(const size_t s: st.second) cout << s << ", "; cout << "}" << endl; }*/ - + EXPECT(s1 == sets[1]); EXPECT(s2 == sets[4]); } +/* ************************************************************************* */ +TEST(DSFMap, findIndexPair) { + DSFMap dsf; + EXPECT(dsf.find(IndexPair(1,2))==IndexPair(1,2)); + EXPECT(dsf.find(IndexPair(1,2)) != dsf.find(IndexPair(1,3))); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam/base/tests/testDSFVector.cpp b/gtsam/base/tests/testDSFVector.cpp index cf32f1c3b..88cea8c01 100644 --- a/gtsam/base/tests/testDSFVector.cpp +++ b/gtsam/base/tests/testDSFVector.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/tests/testGroup.cpp b/gtsam/base/tests/testGroup.cpp index f405bdaf1..976dee697 100644 --- a/gtsam/base/tests/testGroup.cpp +++ b/gtsam/base/tests/testGroup.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp index 5e18e1495..8c68bf8a0 100644 --- a/gtsam/base/tests/testLieMatrix.cpp +++ b/gtsam/base/tests/testLieMatrix.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp index bacb9dd1e..74f5e0d41 100644 --- a/gtsam/base/tests/testLieScalar.cpp +++ b/gtsam/base/tests/testLieScalar.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp index 3c2885c18..76c4fc490 100644 --- a/gtsam/base/tests/testLieVector.cpp +++ b/gtsam/base/tests/testLieVector.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 88a6fa825..468e842f2 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -275,7 +275,7 @@ TEST(Matrix, stream_read ) { -0.3, -8e-2, 5.1, 9.0, 1.2, 3.4, 4.5, 6.7).finished(); - string matrixAsString = + string matrixAsString = "1.1 2.3 4.2 7.6\n" "-0.3 -8e-2 5.1 9.0\n\r" // Test extra spaces and windows newlines "1.2 \t 3.4 4.5 6.7"; // Test tab as separator diff --git a/gtsam/base/tests/testOptionalJacobian.cpp b/gtsam/base/tests/testOptionalJacobian.cpp index de1c07dcf..128576107 100644 --- a/gtsam/base/tests/testOptionalJacobian.cpp +++ b/gtsam/base/tests/testOptionalJacobian.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 8a54d5469..c315b9c27 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index ac58fcca7..b33f06045 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h index b89e15637..ec80baad3 100644 --- a/gtsam/base/timing.h +++ b/gtsam/base/timing.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/base/types.h b/gtsam/base/types.h index c4775b672..3b6c9f337 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -56,6 +56,9 @@ namespace gtsam { /// Integer nonlinear key type typedef std::uint64_t Key; + /// Integer nonlinear factor index type + typedef std::uint64_t FactorIndex; + /// The index type for Eigen objects typedef ptrdiff_t DenseIndex; diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index ecc8533c1..2ad9fc1e3 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -355,7 +355,7 @@ namespace gtsam { NodePtr choose(const L& label, size_t index) const { if (label_ == label) return branches_[index]; // choose branch - + // second case, not label of interest, just recurse boost::shared_ptr r(new Choice(label_, branches_.size())); for(const NodePtr& branch: branches_) diff --git a/gtsam/discrete/DiscreteBayesTree.cpp b/gtsam/discrete/DiscreteBayesTree.cpp index c7a81aa60..bed50a470 100644 --- a/gtsam/discrete/DiscreteBayesTree.cpp +++ b/gtsam/discrete/DiscreteBayesTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/discrete/DiscreteEliminationTree.cpp b/gtsam/discrete/DiscreteEliminationTree.cpp index df59681c2..a2953644d 100644 --- a/gtsam/discrete/DiscreteEliminationTree.cpp +++ b/gtsam/discrete/DiscreteEliminationTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/discrete/DiscreteEliminationTree.h b/gtsam/discrete/DiscreteEliminationTree.h index 1ba33006e..1ea8a5695 100644 --- a/gtsam/discrete/DiscreteEliminationTree.h +++ b/gtsam/discrete/DiscreteEliminationTree.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -31,7 +31,7 @@ namespace gtsam { typedef EliminationTree Base; ///< Base class typedef DiscreteEliminationTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - + /** * Build the elimination tree of a factor graph using pre-computed column structure. * @param factorGraph The factor graph for which to build the elimination tree diff --git a/gtsam/discrete/DiscreteJunctionTree.cpp b/gtsam/discrete/DiscreteJunctionTree.cpp index 0e6e2b73e..dc24860eb 100644 --- a/gtsam/discrete/DiscreteJunctionTree.cpp +++ b/gtsam/discrete/DiscreteJunctionTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/discrete/DiscreteJunctionTree.h b/gtsam/discrete/DiscreteJunctionTree.h index 8c00065d9..f5bc9be1d 100644 --- a/gtsam/discrete/DiscreteJunctionTree.h +++ b/gtsam/discrete/DiscreteJunctionTree.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -51,7 +51,7 @@ namespace gtsam { typedef JunctionTree Base; ///< Base class typedef DiscreteJunctionTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - + /** * Build the elimination tree of a factor graph using precomputed column structure. * @param factorGraph The factor graph for which to build the elimination tree diff --git a/gtsam/discrete/DiscreteMarginals.h b/gtsam/discrete/DiscreteMarginals.h index 2158973a9..c2a188e08 100644 --- a/gtsam/discrete/DiscreteMarginals.h +++ b/gtsam/discrete/DiscreteMarginals.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index 54deedfdc..ca4e88eb6 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -44,8 +44,7 @@ Cal3_S2::Cal3_S2(const std::string &path) : if (infile) infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_; else { - printf("Unable to load the calibration\n"); - exit(0); + throw std::runtime_error("Cal3_S2: Unable to load the calibration"); } infile.close(); diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 6ad7aeb86..747f53fe1 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/Cal3_S2Stereo.cpp b/gtsam/geometry/Cal3_S2Stereo.cpp index 22966ee37..414fe6711 100644 --- a/gtsam/geometry/Cal3_S2Stereo.cpp +++ b/gtsam/geometry/Cal3_S2Stereo.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 3a0f56c30..51692dc82 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 3c9fb1563..dd88f9f69 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -107,7 +107,7 @@ Point2 PinholeBase::Project(const Unit3& pc, OptionalJacobian<2, 2> Dpoint) { /* ************************************************************************* */ pair PinholeBase::projectSafe(const Point3& pw) const { - const Point3 pc = pose().transform_to(pw); + const Point3 pc = pose().transformTo(pw); const Point2 pn = Project(pc); return make_pair(pn, pc.z() > 0); } @@ -116,8 +116,8 @@ pair PinholeBase::projectSafe(const Point3& pw) const { Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint) const { - Matrix3 Rt; // calculated by transform_to if needed - const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0); + Matrix3 Rt; // calculated by transformTo if needed + const Point3 q = pose().transformTo(point, boost::none, Dpoint ? &Rt : 0); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION if (q.z() <= 0) throw CheiralityException(); diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 0de6c6991..1f791935d 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -359,7 +359,7 @@ public: Dresult_ddepth ? &Dpoint_ddepth : 0); Matrix33 Dresult_dpoint; - const Point3 result = pose().transform_from(point, Dresult_dpose, + const Point3 result = pose().transformFrom(point, Dresult_dpose, (Dresult_ddepth || Dresult_dp) ? &Dresult_dpoint : 0); diff --git a/gtsam/geometry/Cyclic.cpp b/gtsam/geometry/Cyclic.cpp index 7242459a6..70580c38f 100644 --- a/gtsam/geometry/Cyclic.cpp +++ b/gtsam/geometry/Cyclic.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h index 15d8154b8..0b81ff0f9 100644 --- a/gtsam/geometry/Cyclic.h +++ b/gtsam/geometry/Cyclic.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 02ede9228..020c94fdb 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -52,13 +52,13 @@ void EssentialMatrix::print(const string& s) const { } /* ************************************************************************* */ -Point3 EssentialMatrix::transform_to(const Point3& p, OptionalJacobian<3, 5> DE, +Point3 EssentialMatrix::transformTo(const Point3& p, OptionalJacobian<3, 5> DE, OptionalJacobian<3, 3> Dpoint) const { Pose3 pose(rotation(), direction().point3()); Matrix36 DE_; - Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint); + Point3 q = pose.transformTo(p, DE ? &DE_ : 0, Dpoint); if (DE) { - // DE returned by pose.transform_to is 3*6, but we need it to be 3*5 + // DE returned by pose.transformTo is 3*6, but we need it to be 3*5 // The last 3 columns are derivative with respect to change in translation // The derivative of translation with respect to a 2D sphere delta is 3*2 direction().basis() // Duy made an educated guess that this needs to be rotated to the local frame diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 308250033..891902da7 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -138,7 +138,7 @@ class GTSAM_EXPORT EssentialMatrix { * @param Dpoint optional 3*3 Jacobian wrpt point * @return point in pose coordinates */ - Point3 transform_to(const Point3& p, + Point3 transformTo(const Point3& p, OptionalJacobian<3, 5> DE = boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const; @@ -176,6 +176,17 @@ class GTSAM_EXPORT EssentialMatrix { /// @} +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + Point3 transform_to(const Point3& p, + OptionalJacobian<3, 5> DE = boost::none, + OptionalJacobian<3, 3> Dpoint = boost::none) const { + return transformTo(p, DE, Dpoint); + }; + /// @} +#endif + private: /// @name Advanced Interface /// @{ diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 2670a63f7..935962423 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -145,7 +145,7 @@ public: (Dresult_dp || Dresult_dcal) ? &Dpoint_dpn : 0, Dresult_ddepth ? &Dpoint_ddepth : 0); Matrix33 Dresult_dpoint; - const Point3 result = pose().transform_from(point, Dresult_dpose, + const Point3 result = pose().transformFrom(point, Dresult_dpose, (Dresult_ddepth || Dresult_dp || Dresult_dcal) ? &Dresult_dpoint : 0); @@ -220,7 +220,7 @@ private: & boost::serialization::make_nvp("PinholeBase", boost::serialization::base_object(*this)); } - + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/gtsam/geometry/PinholeSet.h b/gtsam/geometry/PinholeSet.h index 7869704ca..a2896ca8d 100644 --- a/gtsam/geometry/PinholeSet.h +++ b/gtsam/geometry/PinholeSet.h @@ -1,12 +1,12 @@ /* ---------------------------------------------------------------------------- - + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - + * See LICENSE for the license information - + * -------------------------------------------------------------------------- */ /** diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 2a52e98ba..4e5085cfe 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -198,7 +198,7 @@ Pose2 Pose2::inverse() const { /* ************************************************************************* */ // see doc/math.lyx, SE(2) section -Point2 Pose2::transform_to(const Point2& point, +Point2 Pose2::transformTo(const Point2& point, OptionalJacobian<2, 3> Hpose, OptionalJacobian<2, 2> Hpoint) const { OptionalJacobian<2, 2> Htranslation = Hpose.cols<2>(0); OptionalJacobian<2, 1> Hrotation = Hpose.cols<1>(2); @@ -209,7 +209,7 @@ Point2 Pose2::transform_to(const Point2& point, /* ************************************************************************* */ // see doc/math.lyx, SE(2) section -Point2 Pose2::transform_from(const Point2& point, +Point2 Pose2::transformFrom(const Point2& point, OptionalJacobian<2, 3> Hpose, OptionalJacobian<2, 2> Hpoint) const { OptionalJacobian<2, 2> Htranslation = Hpose.cols<2>(0); OptionalJacobian<2, 1> Hrotation = Hpose.cols<1>(2); @@ -223,7 +223,7 @@ Rot2 Pose2::bearing(const Point2& point, OptionalJacobian<1, 3> Hpose, OptionalJacobian<1, 2> Hpoint) const { // make temporary matrices Matrix23 D_d_pose; Matrix2 D_d_point; - Point2 d = transform_to(point, Hpose ? &D_d_pose : 0, Hpoint ? &D_d_point : 0); + Point2 d = transformTo(point, Hpose ? &D_d_pose : 0, Hpoint ? &D_d_point : 0); if (!Hpose && !Hpoint) return Rot2::relativeBearing(d); Matrix12 D_result_d; Rot2 result = Rot2::relativeBearing(d, Hpose || Hpoint ? &D_result_d : 0); @@ -288,7 +288,7 @@ double Pose2::range(const Pose2& pose, /* ************************************************************************* * New explanation, from scan.ml * It finds the angle using a linear method: - * q = Pose2::transform_from(p) = t + R*p + * q = Pose2::transformFrom(p) = t + R*p * We need to remove the centroids from the data to find the rotation * using dp=[dpx;dpy] and q=[dqx;dqy] we have * |dqx| |c -s| |dpx| |dpx -dpy| |c| diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 6937ff78d..efd6a7f88 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -195,17 +195,17 @@ public: /// @{ /** Return point coordinates in pose coordinate frame */ - Point2 transform_to(const Point2& point, - OptionalJacobian<2, 3> H1 = boost::none, - OptionalJacobian<2, 2> H2 = boost::none) const; + Point2 transformTo(const Point2& point, + OptionalJacobian<2, 3> Dpose = boost::none, + OptionalJacobian<2, 2> Dpoint = boost::none) const; /** Return point coordinates in global frame */ - Point2 transform_from(const Point2& point, - OptionalJacobian<2, 3> H1 = boost::none, - OptionalJacobian<2, 2> H2 = boost::none) const; + Point2 transformFrom(const Point2& point, + OptionalJacobian<2, 3> Dpose = boost::none, + OptionalJacobian<2, 2> Dpoint = boost::none) const; - /** syntactic sugar for transform_from */ - inline Point2 operator*(const Point2& point) const { return transform_from(point);} + /** syntactic sugar for transformFrom */ + inline Point2 operator*(const Point2& point) const { return transformFrom(point);} /// @} /// @name Standard Interface @@ -289,7 +289,23 @@ public: /// @} -private: +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + Point2 transform_from(const Point2& point, + OptionalJacobian<2, 3> Dpose = boost::none, + OptionalJacobian<2, 2> Dpoint = boost::none) const { + return transformFrom(point, Dpose, Dpoint); + } + Point2 transform_to(const Point2& point, + OptionalJacobian<2, 3> Dpose = boost::none, + OptionalJacobian<2, 2> Dpoint = boost::none) const { + return transformTo(point, Dpose, Dpoint); + } + /// @} +#endif + + private: // Serialization function friend class boost::serialization::access; @@ -313,7 +329,7 @@ inline Matrix wedge(const Vector& xi) { /** * Calculate pose between a vector of 2D point correspondences (p,q) - * where q = Pose2::transform_from(p) = t + R*p + * where q = Pose2::transformFrom(p) = t + R*p */ typedef std::pair Point2Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 50e487e75..e720fe0b9 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -285,22 +285,31 @@ Matrix4 Pose3::matrix() const { } /* ************************************************************************* */ +Pose3 Pose3::transformPoseFrom(const Pose3& aTb) const { + const Pose3& wTa = *this; + return wTa * aTb; +} + +/* ************************************************************************* */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 Pose3 Pose3::transform_to(const Pose3& pose) const { Rot3 cRv = R_ * Rot3(pose.R_.inverse()); Point3 t = pose.transform_to(t_); return Pose3(cRv, t); } +#endif /* ************************************************************************* */ -Pose3 Pose3::transform_pose_to(const Pose3& pose, OptionalJacobian<6, 6> H1, - OptionalJacobian<6, 6> H2) const { - if (H1) *H1 = -pose.inverse().AdjointMap() * AdjointMap(); +Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> H1, + OptionalJacobian<6, 6> H2) const { + if (H1) *H1 = -wTb.inverse().AdjointMap() * AdjointMap(); if (H2) *H2 = I_6x6; - return inverse() * pose; + const Pose3& wTa = *this; + return wTa.inverse() * wTb; } /* ************************************************************************* */ -Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose, +Point3 Pose3::transformFrom(const Point3& p, OptionalJacobian<3,6> Dpose, OptionalJacobian<3,3> Dpoint) const { // Only get matrix once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case @@ -316,7 +325,7 @@ Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose, } /* ************************************************************************* */ -Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose, +Point3 Pose3::transformTo(const Point3& p, OptionalJacobian<3,6> Dpose, OptionalJacobian<3,3> Dpoint) const { // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case @@ -340,7 +349,7 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, OptionalJacobian<1, 3> H2) const { Matrix36 D_local_pose; Matrix3 D_local_point; - Point3 local = transform_to(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0); + Point3 local = transformTo(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0); if (!H1 && !H2) { return local.norm(); } else { @@ -366,7 +375,7 @@ Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1, OptionalJacobian<2, 3> H2) const { Matrix36 D_local_pose; Matrix3 D_local_point; - Point3 local = transform_to(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0); + Point3 local = transformTo(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0); if (!H1 && !H2) { return Unit3(local); } else { diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 757c4fdd4..e90f91127 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -81,7 +81,6 @@ public: /** * Create Pose3 by aligning two point pairs * A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point - * Meant to replace the deprecated function 'align', which orders the pairs the opposite way. * Note this allows for noise on the points but in that case the mapping will not be exact. */ static boost::optional Align(const std::vector& abPointPairs); @@ -207,12 +206,12 @@ public: * @param Dpoint optional 3*3 Jacobian wrpt point * @return point in world coordinates */ - Point3 transform_from(const Point3& p, OptionalJacobian<3, 6> Dpose = + Point3 transformFrom(const Point3& p, OptionalJacobian<3, 6> Dpose = boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const; - /** syntactic sugar for transform_from */ + /** syntactic sugar for transformFrom */ inline Point3 operator*(const Point3& p) const { - return transform_from(p); + return transformFrom(p); } /** @@ -222,7 +221,7 @@ public: * @param Dpoint optional 3*3 Jacobian wrpt point * @return point in Pose coordinates */ - Point3 transform_to(const Point3& p, OptionalJacobian<3, 6> Dpose = + Point3 transformTo(const Point3& p, OptionalJacobian<3, 6> Dpose = boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const; /// @} @@ -253,14 +252,12 @@ public: /** convert to 4*4 matrix */ Matrix4 matrix() const; - /** receives a pose in local coordinates and transforms it to world coordinates - * @deprecated: This is actually equivalent to transform_from, so it is WRONG! Use - * transform_pose_to instead. */ - Pose3 transform_to(const Pose3& pose) const; + /** receives a pose in local coordinates and transforms it to world coordinates */ + Pose3 transformPoseFrom(const Pose3& pose) const; /** receives a pose in world coordinates and transforms it to local coordinates */ - Pose3 transform_pose_to(const Pose3& pose, OptionalJacobian<6, 6> H1 = boost::none, - OptionalJacobian<6, 6> H2 = boost::none) const; + Pose3 transformPoseTo(const Pose3& pose, OptionalJacobian<6, 6> H1 = boost::none, + OptionalJacobian<6, 6> H2 = boost::none) const; /** * Calculate range to a landmark @@ -321,6 +318,30 @@ public: GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Pose3& p); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + Point3 transform_from(const Point3& p, + OptionalJacobian<3, 6> Dpose = boost::none, + OptionalJacobian<3, 3> Dpoint = boost::none) const { + return transformFrom(p, Dpose, Dpoint); + } + Point3 transform_to(const Point3& p, + OptionalJacobian<3, 6> Dpose = boost::none, + OptionalJacobian<3, 3> Dpoint = boost::none) const { + return transformTo(p, Dpose, Dpoint); + } + Pose3 transform_pose_to(const Pose3& pose, + OptionalJacobian<6, 6> H1 = boost::none, + OptionalJacobian<6, 6> H2 = boost::none) const { + return transformPoseTo(pose, H1, H2); + } + /** + * @deprecated: this function is neither here not there. */ + Pose3 transform_to(const Pose3& pose) const; + /// @} +#endif + private: /** Serialization function */ friend class boost::serialization::access; @@ -351,12 +372,10 @@ inline Matrix wedge(const Vector& xi) { return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5)); } -/** - * Calculate pose between a vector of 3D point correspondences (b_point, a_point) - * where a_point = Pose3::transform_from(b_point) = t + R*b_point - * @deprecated: use Pose3::Align with point pairs ordered the opposite way - */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +// deprecated: use Pose3::Align with point pairs ordered the opposite way GTSAM_EXPORT boost::optional align(const std::vector& baPointPairs); +#endif // For MATLAB wrapper typedef std::vector Pose3Vector; diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index af9481181..1557a09db 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 4cabe7140..9f3ed35b0 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index eaec62b48..f46f12540 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 07c03ab49..034956e99 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -31,7 +31,6 @@ namespace so3 { void ExpmapFunctor::init(bool nearZeroApprox) { nearZero = nearZeroApprox || (theta2 <= std::numeric_limits::epsilon()); if (!nearZero) { - theta = std::sqrt(theta2); // rotation angle sin_theta = std::sin(theta); const double s2 = std::sin(theta / 2.0); one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] @@ -39,7 +38,7 @@ void ExpmapFunctor::init(bool nearZeroApprox) { } ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) - : theta2(omega.dot(omega)) { + : theta2(omega.dot(omega)), theta(std::sqrt(theta2)) { const double wx = omega.x(), wy = omega.y(), wz = omega.z(); W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; init(nearZeroApprox); @@ -50,7 +49,7 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) } ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox) - : theta2(angle * angle) { + : theta2(angle * angle), theta(angle) { const double ax = axis.x(), ay = axis.y(), az = axis.z(); K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0; W = K * angle; diff --git a/gtsam/geometry/SimpleCamera.cpp b/gtsam/geometry/SimpleCamera.cpp index d92c5bdd7..6134ae3d4 100644 --- a/gtsam/geometry/SimpleCamera.cpp +++ b/gtsam/geometry/SimpleCamera.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index a7e021394..82f26aee2 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index c34d4d8f3..54cdee210 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -37,7 +37,7 @@ namespace gtsam { StereoPoint2 StereoCamera::project2(const Point3& point, OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2) const { - const Point3 q = leftCamPose_.transform_to(point); + const Point3 q = leftCamPose_.transformTo(point); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION if (q.z() <= 0) @@ -94,7 +94,7 @@ namespace gtsam { double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]); double X = Z * (measured[0] - K_->px()) / K_->fx(); double Y = Z * (measured[2] - K_->py()) / K_->fy(); - Point3 point = leftCamPose_.transform_from(Point3(X, Y, Z)); + Point3 point = leftCamPose_.transformFrom(Point3(X, Y, Z)); return point; } @@ -120,7 +120,7 @@ namespace gtsam { -z_partial_uR, z_partial_uR, 0; Matrix3 D_point_local; - const Point3 point = leftCamPose_.transform_from(local, H1, D_point_local); + const Point3 point = leftCamPose_.transformFrom(local, H1, D_point_local); if(H2) { *H2 = D_point_local * D_local_z; @@ -129,7 +129,7 @@ namespace gtsam { return point; } - return leftCamPose_.transform_from(local); + return leftCamPose_.transformFrom(local); } } diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 104ce73c2..3b5bdaefc 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/StereoPoint2.cpp b/gtsam/geometry/StereoPoint2.cpp index cd6f09507..2254d8ac8 100644 --- a/gtsam/geometry/StereoPoint2.cpp +++ b/gtsam/geometry/StereoPoint2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index dd9bc9dbd..e0f3c3424 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index 4ccb6075b..ccecb09c3 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 33a88db1f..f5f824d05 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -134,7 +134,7 @@ TEST( CalibratedCamera, Dproject_point_pose) Point2 result = camera.project(point1, Dpose, Dpoint); Matrix numerical_pose = numericalDerivative21(project2, camera, point1); Matrix numerical_point = numericalDerivative22(project2, camera, point1); - CHECK(assert_equal(Point3(-0.08, 0.08, 0.5), camera.pose().transform_to(point1))); + CHECK(assert_equal(Point3(-0.08, 0.08, 0.5), camera.pose().transformTo(point1))); CHECK(assert_equal(Point2(-.16, .16), result)); CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index 7becfc75f..abcef9e5b 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 75ee9427d..7ba885115 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -115,9 +115,9 @@ TEST (EssentialMatrix, RoundTrip) { //************************************************************************* Point3 transform_to_(const EssentialMatrix& E, const Point3& point) { - return E.transform_to(point); + return E.transformTo(point); } -TEST (EssentialMatrix, transform_to) { +TEST (EssentialMatrix, transformTo) { // test with a more complicated EssentialMatrix Rot3 aRb2 = Rot3::Yaw(M_PI / 3.0) * Rot3::Pitch(M_PI_4) * Rot3::Roll(M_PI / 6.0); @@ -126,7 +126,7 @@ TEST (EssentialMatrix, transform_to) { //EssentialMatrix E(aRb, Unit3(aTb).retract(Vector2(0.1, 0))); static Point3 P(0.2, 0.7, -2); Matrix actH1, actH2; - E.transform_to(P, actH1, actH2); + E.transformTo(P, actH1, actH2); Matrix expH1 = numericalDerivative21(transform_to_, E, P), // expH2 = numericalDerivative22(transform_to_, E, P); EXPECT(assert_equal(expH1, actH1, 1e-8)); diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 88976697a..b8b3fc52c 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -266,45 +266,44 @@ TEST( Pose2, LogmapDerivative2) { } /* ************************************************************************* */ -static Point2 transform_to_proxy(const Pose2& pose, const Point2& point) { - return pose.transform_to(point); +static Point2 transformTo_(const Pose2& pose, const Point2& point) { + return pose.transformTo(point); } -TEST( Pose2, transform_to ) -{ - Pose2 pose(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y - Point2 point(-1,4); // landmark at (-1,4) +TEST(Pose2, transformTo) { + Pose2 pose(M_PI / 2.0, Point2(1, 2)); // robot at (1,2) looking towards y + Point2 point(-1, 4); // landmark at (-1,4) // expected - Point2 expected(2,2); - Matrix expectedH1 = (Matrix(2,3) << -1.0, 0.0, 2.0, 0.0, -1.0, -2.0).finished(); - Matrix expectedH2 = (Matrix(2,2) << 0.0, 1.0, -1.0, 0.0).finished(); + Point2 expected(2, 2); + Matrix expectedH1 = + (Matrix(2, 3) << -1.0, 0.0, 2.0, 0.0, -1.0, -2.0).finished(); + Matrix expectedH2 = (Matrix(2, 2) << 0.0, 1.0, -1.0, 0.0).finished(); // actual Matrix actualH1, actualH2; - Point2 actual = pose.transform_to(point, actualH1, actualH2); - EXPECT(assert_equal(expected,actual)); + Point2 actual = pose.transformTo(point, actualH1, actualH2); + EXPECT(assert_equal(expected, actual)); - EXPECT(assert_equal(expectedH1,actualH1)); - Matrix numericalH1 = numericalDerivative21(transform_to_proxy, pose, point); - EXPECT(assert_equal(numericalH1,actualH1)); + EXPECT(assert_equal(expectedH1, actualH1)); + Matrix numericalH1 = numericalDerivative21(transformTo_, pose, point); + EXPECT(assert_equal(numericalH1, actualH1)); - EXPECT(assert_equal(expectedH2,actualH2)); - Matrix numericalH2 = numericalDerivative22(transform_to_proxy, pose, point); - EXPECT(assert_equal(numericalH2,actualH2)); + EXPECT(assert_equal(expectedH2, actualH2)); + Matrix numericalH2 = numericalDerivative22(transformTo_, pose, point); + EXPECT(assert_equal(numericalH2, actualH2)); } /* ************************************************************************* */ -static Point2 transform_from_proxy(const Pose2& pose, const Point2& point) { - return pose.transform_from(point); +static Point2 transformFrom_(const Pose2& pose, const Point2& point) { + return pose.transformFrom(point); } -TEST (Pose2, transform_from) -{ - Pose2 pose(1., 0., M_PI/2.0); +TEST(Pose2, transformFrom) { + Pose2 pose(1., 0., M_PI / 2.0); Point2 pt(2., 1.); Matrix H1, H2; - Point2 actual = pose.transform_from(pt, H1, H2); + Point2 actual = pose.transformFrom(pt, H1, H2); Point2 expected(0., 2.); EXPECT(assert_equal(expected, actual)); @@ -312,11 +311,11 @@ TEST (Pose2, transform_from) Matrix H1_expected = (Matrix(2, 3) << 0., -1., -2., 1., 0., -1.).finished(); Matrix H2_expected = (Matrix(2, 2) << 0., -1., 1., 0.).finished(); - Matrix numericalH1 = numericalDerivative21(transform_from_proxy, pose, pt); + Matrix numericalH1 = numericalDerivative21(transformFrom_, pose, pt); EXPECT(assert_equal(H1_expected, H1)); EXPECT(assert_equal(H1_expected, numericalH1)); - Matrix numericalH2 = numericalDerivative22(transform_from_proxy, pose, pt); + Matrix numericalH2 = numericalDerivative22(transformFrom_, pose, pt); EXPECT(assert_equal(H2_expected, H2)); EXPECT(assert_equal(H2_expected, numericalH2)); } @@ -349,8 +348,8 @@ TEST(Pose2, compose_a) Point2 point(sqrt(0.5), 3.0*sqrt(0.5)); Point2 expected_point(-1.0, -1.0); - Point2 actual_point1 = (pose1 * pose2).transform_to(point); - Point2 actual_point2 = pose2.transform_to(pose1.transform_to(point)); + Point2 actual_point1 = (pose1 * pose2).transformTo(point); + Point2 actual_point2 = pose2.transformTo(pose1.transformTo(point)); EXPECT(assert_equal(expected_point, actual_point1)); EXPECT(assert_equal(expected_point, actual_point2)); } @@ -735,7 +734,7 @@ TEST(Pose2, align_2) { vector correspondences; Point2 p1(0,0), p2(10,0); - Point2 q1 = expected.transform_from(p1), q2 = expected.transform_from(p2); + Point2 q1 = expected.transformFrom(p1), q2 = expected.transformFrom(p2); EXPECT(assert_equal(Point2(20,10),q1)); EXPECT(assert_equal(Point2(20,20),q2)); Point2Pair pq1(make_pair(p1, q1)); @@ -750,7 +749,7 @@ namespace align_3 { Point2 t(10,10); Pose2 expected(Rot2::fromAngle(2*M_PI/3), t); Point2 p1(0,0), p2(10,0), p3(10,10); - Point2 q1 = expected.transform_from(p1), q2 = expected.transform_from(p2), q3 = expected.transform_from(p3); + Point2 q1 = expected.transformFrom(p1), q2 = expected.transformFrom(p2), q3 = expected.transformFrom(p3); } TEST(Pose2, align_3) { diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index d516ddc8b..0b2f59773 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -316,224 +316,162 @@ TEST( Pose3, compose_inverse) } /* ************************************************************************* */ -Point3 transform_from_(const Pose3& pose, const Point3& point) { return pose.transform_from(point); } -TEST( Pose3, Dtransform_from1_a) -{ +Point3 transformFrom_(const Pose3& pose, const Point3& point) { + return pose.transformFrom(point); +} +TEST(Pose3, Dtransform_from1_a) { Matrix actualDtransform_from1; - T.transform_from(P, actualDtransform_from1, boost::none); - Matrix numerical = numericalDerivative21(transform_from_,T,P); - EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8)); + T.transformFrom(P, actualDtransform_from1, boost::none); + Matrix numerical = numericalDerivative21(transformFrom_, T, P); + EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8)); } -TEST( Pose3, Dtransform_from1_b) -{ +TEST(Pose3, Dtransform_from1_b) { Pose3 origin; Matrix actualDtransform_from1; - origin.transform_from(P, actualDtransform_from1, boost::none); - Matrix numerical = numericalDerivative21(transform_from_,origin,P); - EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8)); + origin.transformFrom(P, actualDtransform_from1, boost::none); + Matrix numerical = numericalDerivative21(transformFrom_, origin, P); + EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8)); } -TEST( Pose3, Dtransform_from1_c) -{ - Point3 origin(0,0,0); - Pose3 T0(R,origin); +TEST(Pose3, Dtransform_from1_c) { + Point3 origin(0, 0, 0); + Pose3 T0(R, origin); Matrix actualDtransform_from1; - T0.transform_from(P, actualDtransform_from1, boost::none); - Matrix numerical = numericalDerivative21(transform_from_,T0,P); - EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8)); + T0.transformFrom(P, actualDtransform_from1, boost::none); + Matrix numerical = numericalDerivative21(transformFrom_, T0, P); + EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8)); } -TEST( Pose3, Dtransform_from1_d) -{ +TEST(Pose3, Dtransform_from1_d) { Rot3 I; - Point3 t0(100,0,0); - Pose3 T0(I,t0); + Point3 t0(100, 0, 0); + Pose3 T0(I, t0); Matrix actualDtransform_from1; - T0.transform_from(P, actualDtransform_from1, boost::none); - //print(computed, "Dtransform_from1_d computed:"); - Matrix numerical = numericalDerivative21(transform_from_,T0,P); - //print(numerical, "Dtransform_from1_d numerical:"); - EXPECT(assert_equal(numerical,actualDtransform_from1,1e-8)); + T0.transformFrom(P, actualDtransform_from1, boost::none); + // print(computed, "Dtransform_from1_d computed:"); + Matrix numerical = numericalDerivative21(transformFrom_, T0, P); + // print(numerical, "Dtransform_from1_d numerical:"); + EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8)); } /* ************************************************************************* */ -TEST( Pose3, Dtransform_from2) -{ +TEST(Pose3, Dtransform_from2) { Matrix actualDtransform_from2; - T.transform_from(P, boost::none, actualDtransform_from2); - Matrix numerical = numericalDerivative22(transform_from_,T,P); - EXPECT(assert_equal(numerical,actualDtransform_from2,1e-8)); + T.transformFrom(P, boost::none, actualDtransform_from2); + Matrix numerical = numericalDerivative22(transformFrom_, T, P); + EXPECT(assert_equal(numerical, actualDtransform_from2, 1e-8)); } /* ************************************************************************* */ -Point3 transform_to_(const Pose3& pose, const Point3& point) { return pose.transform_to(point); } -TEST( Pose3, Dtransform_to1) -{ +Point3 transform_to_(const Pose3& pose, const Point3& point) { + return pose.transformTo(point); +} +TEST(Pose3, Dtransform_to1) { Matrix computed; - T.transform_to(P, computed, boost::none); - Matrix numerical = numericalDerivative21(transform_to_,T,P); - EXPECT(assert_equal(numerical,computed,1e-8)); + T.transformTo(P, computed, boost::none); + Matrix numerical = numericalDerivative21(transform_to_, T, P); + EXPECT(assert_equal(numerical, computed, 1e-8)); } /* ************************************************************************* */ -TEST( Pose3, Dtransform_to2) -{ +TEST(Pose3, Dtransform_to2) { Matrix computed; - T.transform_to(P, boost::none, computed); - Matrix numerical = numericalDerivative22(transform_to_,T,P); - EXPECT(assert_equal(numerical,computed,1e-8)); + T.transformTo(P, boost::none, computed); + Matrix numerical = numericalDerivative22(transform_to_, T, P); + EXPECT(assert_equal(numerical, computed, 1e-8)); } /* ************************************************************************* */ -TEST( Pose3, transform_to_with_derivatives) -{ +TEST(Pose3, transform_to_with_derivatives) { Matrix actH1, actH2; - T.transform_to(P,actH1,actH2); - Matrix expH1 = numericalDerivative21(transform_to_, T,P), - expH2 = numericalDerivative22(transform_to_, T,P); + T.transformTo(P, actH1, actH2); + Matrix expH1 = numericalDerivative21(transform_to_, T, P), + expH2 = numericalDerivative22(transform_to_, T, P); EXPECT(assert_equal(expH1, actH1, 1e-8)); EXPECT(assert_equal(expH2, actH2, 1e-8)); } /* ************************************************************************* */ -TEST( Pose3, transform_from_with_derivatives) -{ +TEST(Pose3, transform_from_with_derivatives) { Matrix actH1, actH2; - T.transform_from(P,actH1,actH2); - Matrix expH1 = numericalDerivative21(transform_from_, T,P), - expH2 = numericalDerivative22(transform_from_, T,P); + T.transformFrom(P, actH1, actH2); + Matrix expH1 = numericalDerivative21(transformFrom_, T, P), + expH2 = numericalDerivative22(transformFrom_, T, P); EXPECT(assert_equal(expH1, actH1, 1e-8)); EXPECT(assert_equal(expH2, actH2, 1e-8)); } /* ************************************************************************* */ -TEST( Pose3, transform_to_translate) -{ - Point3 actual = Pose3(Rot3(), Point3(1, 2, 3)).transform_to(Point3(10.,20.,30.)); - Point3 expected(9.,18.,27.); - EXPECT(assert_equal(expected, actual)); +TEST(Pose3, transform_to_translate) { + Point3 actual = + Pose3(Rot3(), Point3(1, 2, 3)).transformTo(Point3(10., 20., 30.)); + Point3 expected(9., 18., 27.); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ -TEST( Pose3, transform_to_rotate) -{ - Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3(0,0,0)); - Point3 actual = transform.transform_to(Point3(2,1,10)); - Point3 expected(-1,2,10); - EXPECT(assert_equal(expected, actual, 0.001)); +TEST(Pose3, transform_to_rotate) { + Pose3 transform(Rot3::Rodrigues(0, 0, -1.570796), Point3(0, 0, 0)); + Point3 actual = transform.transformTo(Point3(2, 1, 10)); + Point3 expected(-1, 2, 10); + EXPECT(assert_equal(expected, actual, 0.001)); } /* ************************************************************************* */ -TEST( Pose3, transform_to) -{ - Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3(2,4, 0)); - Point3 actual = transform.transform_to(Point3(3,2,10)); - Point3 expected(2,1,10); - EXPECT(assert_equal(expected, actual, 0.001)); +TEST(Pose3, transformTo) { + Pose3 transform(Rot3::Rodrigues(0, 0, -1.570796), Point3(2, 4, 0)); + Point3 actual = transform.transformTo(Point3(3, 2, 10)); + Point3 expected(2, 1, 10); + EXPECT(assert_equal(expected, actual, 0.001)); } -Pose3 transform_pose_to_(const Pose3& pose, const Pose3& pose2) { return pose.transform_pose_to(pose2); } +Pose3 transformPoseTo_(const Pose3& pose, const Pose3& pose2) { + return pose.transformPoseTo(pose2); +} /* ************************************************************************* */ -TEST( Pose3, transform_pose_to) -{ - Pose3 origin = T.transform_pose_to(T); +TEST(Pose3, transformPoseTo) { + Pose3 origin = T.transformPoseTo(T); EXPECT(assert_equal(Pose3{}, origin)); } /* ************************************************************************* */ -TEST( Pose3, transform_pose_to_with_derivatives) -{ +TEST(Pose3, transformPoseTo_with_derivatives) { Matrix actH1, actH2; - Pose3 res = T.transform_pose_to(T2,actH1,actH2); + Pose3 res = T.transformPoseTo(T2, actH1, actH2); EXPECT(assert_equal(res, T.inverse().compose(T2))); - Matrix expH1 = numericalDerivative21(transform_pose_to_, T, T2), - expH2 = numericalDerivative22(transform_pose_to_, T, T2); + Matrix expH1 = numericalDerivative21(transformPoseTo_, T, T2), + expH2 = numericalDerivative22(transformPoseTo_, T, T2); EXPECT(assert_equal(expH1, actH1, 1e-8)); EXPECT(assert_equal(expH2, actH2, 1e-8)); } /* ************************************************************************* */ -TEST( Pose3, transform_pose_to_with_derivatives2) -{ +TEST(Pose3, transformPoseTo_with_derivatives2) { Matrix actH1, actH2; - Pose3 res = T.transform_pose_to(T3,actH1,actH2); + Pose3 res = T.transformPoseTo(T3, actH1, actH2); EXPECT(assert_equal(res, T.inverse().compose(T3))); - Matrix expH1 = numericalDerivative21(transform_pose_to_, T, T3), - expH2 = numericalDerivative22(transform_pose_to_, T, T3); + Matrix expH1 = numericalDerivative21(transformPoseTo_, T, T3), + expH2 = numericalDerivative22(transformPoseTo_, T, T3); EXPECT(assert_equal(expH1, actH1, 1e-8)); EXPECT(assert_equal(expH2, actH2, 1e-8)); } /* ************************************************************************* */ -TEST( Pose3, transform_from) -{ - Point3 actual = T3.transform_from(Point3(0,0,0)); - Point3 expected = Point3(1.,2.,3.); - EXPECT(assert_equal(expected, actual)); +TEST(Pose3, transformFrom) { + Point3 actual = T3.transformFrom(Point3(0, 0, 0)); + Point3 expected = Point3(1., 2., 3.); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ -TEST( Pose3, transform_roundtrip) -{ - Point3 actual = T3.transform_from(T3.transform_to(Point3(12., -0.11,7.0))); - Point3 expected(12., -0.11,7.0); - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST( Pose3, transformPose_to_origin) -{ - // transform to origin - Pose3 actual = T3.transform_to(Pose3()); - EXPECT(assert_equal(T3, actual, 1e-8)); -} - -/* ************************************************************************* */ -TEST( Pose3, transformPose_to_itself) -{ - // transform to itself - Pose3 actual = T3.transform_to(T3); - EXPECT(assert_equal(Pose3(), actual, 1e-8)); -} - -/* ************************************************************************* */ -TEST( Pose3, transformPose_to_translation) -{ - // transform translation only - Rot3 r = Rot3::Rodrigues(-1.570796,0,0); - Pose3 pose2(r, Point3(21.,32.,13.)); - Pose3 actual = pose2.transform_to(Pose3(Rot3(), Point3(1,2,3))); - Pose3 expected(r, Point3(20.,30.,10.)); - EXPECT(assert_equal(expected, actual, 1e-8)); -} - -/* ************************************************************************* */ -TEST( Pose3, transformPose_to_simple_rotate) -{ - // transform translation only - Rot3 r = Rot3::Rodrigues(0,0,-1.570796); - Pose3 pose2(r, Point3(21.,32.,13.)); - Pose3 transform(r, Point3(1,2,3)); - Pose3 actual = pose2.transform_to(transform); - Pose3 expected(Rot3(), Point3(-30.,20.,10.)); - EXPECT(assert_equal(expected, actual, 0.001)); -} - -/* ************************************************************************* */ -TEST( Pose3, transformPose_to) -{ - // transform to - Rot3 r = Rot3::Rodrigues(0,0,-1.570796); //-90 degree yaw - Rot3 r2 = Rot3::Rodrigues(0,0,0.698131701); //40 degree yaw - Pose3 pose2(r2, Point3(21.,32.,13.)); - Pose3 transform(r, Point3(1,2,3)); - Pose3 actual = pose2.transform_to(transform); - Pose3 expected(Rot3::Rodrigues(0,0,2.26892803), Point3(-30.,20.,10.)); - EXPECT(assert_equal(expected, actual, 0.001)); +TEST(Pose3, transform_roundtrip) { + Point3 actual = T3.transformFrom(T3.transformTo(Point3(12., -0.11, 7.0))); + Point3 expected(12., -0.11, 7.0); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ @@ -789,9 +727,9 @@ TEST(Pose3, Align2) { vector correspondences; Point3 p1(0,0,1), p2(10,0,2), p3(20,-10,30); - Point3 q1 = expected.transform_from(p1), - q2 = expected.transform_from(p2), - q3 = expected.transform_from(p3); + Point3 q1 = expected.transformFrom(p1), + q2 = expected.transformFrom(p2), + q3 = expected.transformFrom(p3); Point3Pair ab1(make_pair(q1, p1)); Point3Pair ab2(make_pair(q2, p2)); Point3Pair ab3(make_pair(q3, p3)); diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 0421e1e44..843dc6cc1 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index d6d1f3149..7cd27a9da 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 6f0af3828..e468eaf55 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -103,6 +103,20 @@ Rot3 slow_but_correct_Rodrigues(const Vector& w) { return Rot3(R); } +/* ************************************************************************* */ +TEST( Rot3, AxisAngle) +{ + Vector axis = Vector3(0., 1., 0.); // rotation around Y + double angle = 3.14 / 4.0; + Rot3 expected(0.707388, 0, 0.706825, + 0, 1, 0, + -0.706825, 0, 0.707388); + Rot3 actual = Rot3::AxisAngle(axis, angle); + CHECK(assert_equal(expected,actual,1e-5)); + Rot3 actual2 = Rot3::AxisAngle(axis, angle-2*M_PI); + CHECK(assert_equal(expected,actual2,1e-5)); +} + /* ************************************************************************* */ TEST( Rot3, Rodrigues) { diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index d05356271..7944db6bf 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/tests/testRot3Q.cpp b/gtsam/geometry/tests/testRot3Q.cpp index 9ae8eef13..be159ed58 100644 --- a/gtsam/geometry/tests/testRot3Q.cpp +++ b/gtsam/geometry/tests/testRot3Q.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 68e87d5ba..56751fa06 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -82,6 +82,34 @@ TEST(SO3, ChartDerivatives) { CHECK_CHART_DERIVATIVES(R2, R1); } +/* ************************************************************************* */ +TEST(SO3, Expmap) { + Vector axis = Vector3(0., 1., 0.); // rotation around Y + double angle = 3.14 / 4.0; + Matrix expected(3,3); + expected << 0.707388, 0, 0.706825, 0, 1, 0, -0.706825, 0, 0.707388; + + // axis angle version + so3::ExpmapFunctor f1(axis, angle); + SO3 actual1 = f1.expmap(); + CHECK(assert_equal(expected, actual1.matrix(), 1e-5)); + + // axis angle version, negative angle + so3::ExpmapFunctor f2(axis, angle - 2*M_PI); + SO3 actual2 = f2.expmap(); + CHECK(assert_equal(expected, actual2.matrix(), 1e-5)); + + // omega version + so3::ExpmapFunctor f3(axis * angle); + SO3 actual3 = f3.expmap(); + CHECK(assert_equal(expected, actual3.matrix(), 1e-5)); + + // omega version, negative angle + so3::ExpmapFunctor f4(axis * (angle - 2*M_PI)); + SO3 actual4 = f4.expmap(); + CHECK(assert_equal(expected, actual4.matrix(), 1e-5)); +} + /* ************************************************************************* */ namespace exmap_derivative { static const Vector3 w(0.1, 0.27, -0.2); diff --git a/gtsam/geometry/tests/testSerializationGeometry.cpp b/gtsam/geometry/tests/testSerializationGeometry.cpp index dfef0a9c5..f7ff881eb 100644 --- a/gtsam/geometry/tests/testSerializationGeometry.cpp +++ b/gtsam/geometry/tests/testSerializationGeometry.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -11,7 +11,7 @@ /** * @file testSerializationGeometry.cpp - * @brief + * @brief * @author Richard Roberts * @date Feb 7, 2012 */ diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 1fae470f9..851c5e4d3 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -65,7 +65,7 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( */ GTSAM_EXPORT Point3 triangulateDLT( const std::vector>& projection_matrices, - const Point2Vector& measurements, + const Point2Vector& measurements, double rank_tol = 1e-9); /** @@ -256,7 +256,7 @@ Point3 triangulatePoint3(const std::vector& poses, #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras for(const Pose3& pose: poses) { - const Point3& p_local = pose.transform_to(point); + const Point3& p_local = pose.transformTo(point); if (p_local.z() <= 0) throw(TriangulationCheiralityException()); } @@ -304,7 +304,7 @@ Point3 triangulatePoint3( #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras for(const CAMERA& camera: cameras) { - const Point3& p_local = camera.pose().transform_to(point); + const Point3& p_local = camera.pose().transformTo(point); if (p_local.z() <= 0) throw(TriangulationCheiralityException()); } @@ -484,7 +484,7 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras // Only needed if this was not yet handled by exception - const Point3& p_local = pose.transform_to(point); + const Point3& p_local = pose.transformTo(point); if (p_local.z() <= 0) return TriangulationResult::BehindCamera(); #endif diff --git a/gtsam/global_includes.h b/gtsam/global_includes.h index 9eba7c7fc..c04da2e5c 100644 --- a/gtsam/global_includes.h +++ b/gtsam/global_includes.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index 1934c58ed..a69fb9b8c 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -65,7 +65,7 @@ namespace gtsam { /// @name Standard Interface /// @{ - + void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; }; diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 33a23056b..9935776a5 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/BayesTree.cpp b/gtsam/inference/BayesTree.cpp index 2fb0227b6..e61c5d5a1 100644 --- a/gtsam/inference/BayesTree.cpp +++ b/gtsam/inference/BayesTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index ee0f3c7b5..892ac5f31 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -96,7 +96,7 @@ namespace gtsam { /** Root cliques */ typedef FastVector Roots; - + /** Root cliques */ Roots roots_; @@ -190,7 +190,7 @@ namespace gtsam { /// @} /// @name Advanced Interface /// @{ - + /** * Find parent clique of a conditional. It will look at all parents and * return the one with the lowest index in the ordering. diff --git a/gtsam/inference/Conditional-inst.h b/gtsam/inference/Conditional-inst.h index 39b60079c..9879a582c 100644 --- a/gtsam/inference/Conditional-inst.h +++ b/gtsam/inference/Conditional-inst.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index 39c738a19..1d486030c 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -75,7 +75,7 @@ namespace gtsam { bool equals(const This& c, double tol = 1e-9) const; /// @} - + public: /// @name Standard Interface /// @{ @@ -102,10 +102,10 @@ namespace gtsam { /** Iterator pointing to first frontal key. */ typename FACTOR::const_iterator beginFrontals() const { return asFactor().begin(); } - + /** Iterator pointing past the last frontal key. */ typename FACTOR::const_iterator endFrontals() const { return asFactor().begin() + nrFrontals_; } - + /** Iterator pointing to the first parent key. */ typename FACTOR::const_iterator beginParents() const { return endFrontals(); } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index a8f68ca2e..8084aa75b 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -99,7 +99,7 @@ namespace gtsam { /** Do sequential elimination of all variables to produce a Bayes net. If an ordering is not * provided, the ordering provided by COLAMD will be used. - * + * * Example - Full Cholesky elimination in COLAMD order: * \code * boost::shared_ptr result = graph.eliminateSequential(EliminateCholesky); @@ -108,12 +108,12 @@ namespace gtsam { * Example - METIS ordering for elimination * \code * boost::shared_ptr result = graph.eliminateSequential(OrderingType::METIS); - * + * * Example - Full QR elimination in specified order: * \code * boost::shared_ptr result = graph.eliminateSequential(EliminateQR, myOrdering); * \endcode - * + * * Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: * \code * VariableIndex varIndex(graph); // Build variable index @@ -130,17 +130,17 @@ namespace gtsam { /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not * provided, the ordering will be computed using either COLAMD or METIS, dependeing on * the parameter orderingType (Ordering::COLAMD or Ordering::METIS) - * + * * Example - Full Cholesky elimination in COLAMD order: * \code * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateCholesky); * \endcode - * + * * Example - Full QR elimination in specified order: * \code * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateQR, myOrdering); * \endcode - * + * * Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: * \code * VariableIndex varIndex(graph); // Build variable index @@ -183,7 +183,7 @@ namespace gtsam { const Ordering& ordering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; - + /** Do multifrontal elimination of the given \c variables in an ordering computed by COLAMD to * produce a Bayes net and a remaining factor graph. This computes the factorization \f$ p(X) * = p(A|B) p(B) \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h index 333f898b8..33ba6e18d 100644 --- a/gtsam/inference/EliminationTree-inst.h +++ b/gtsam/inference/EliminationTree-inst.h @@ -49,8 +49,7 @@ namespace gtsam { // Do dense elimination step KeyVector keyAsVector(1); keyAsVector[0] = key; - std::pair, boost::shared_ptr > eliminationResult = - function(gatheredFactors, Ordering(keyAsVector)); + auto eliminationResult = function(gatheredFactors, Ordering(keyAsVector)); // Add conditional to BayesNet output->push_back(eliminationResult.first); @@ -190,13 +189,13 @@ namespace gtsam { { gttic(EliminationTree_eliminate); // Allocate result - boost::shared_ptr result = boost::make_shared(); + auto result = boost::make_shared(); // Run tree elimination algorithm FastVector remainingFactors = inference::EliminateTree(result, *this, function); // Add remaining factors that were not involved with eliminated variables - boost::shared_ptr allRemainingFactors = boost::make_shared(); + auto allRemainingFactors = boost::make_shared(); allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end()); allRemainingFactors->push_back(remainingFactors.begin(), remainingFactors.end()); diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h index fcea1284e..e4a64c589 100644 --- a/gtsam/inference/EliminationTree.h +++ b/gtsam/inference/EliminationTree.h @@ -116,7 +116,7 @@ namespace gtsam { This& operator=(const This& other); /// @} - + public: /// @name Standard Interface /// @{ @@ -142,11 +142,11 @@ namespace gtsam { bool equals(const This& other, double tol = 1e-9) const; /// @} - + public: /// @name Advanced Interface /// @{ - + /** Return the set of roots (one for a tree, multiple for a forest) */ const FastVector& roots() const { return roots_; } diff --git a/gtsam/inference/Factor.cpp b/gtsam/inference/Factor.cpp index 49f34fb2b..58448edbb 100644 --- a/gtsam/inference/Factor.cpp +++ b/gtsam/inference/Factor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index d44d82954..1aaaff0e4 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -28,6 +28,9 @@ #include namespace gtsam { +/// Define collection types: +typedef FastVector FactorIndices; +typedef FastSet FactorIndexSet; /** * This is the base class for all factor types. It is templated on a KEY type, diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index 785b2507d..8ed41826e 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/ISAM.h b/gtsam/inference/ISAM.h index 36edb423c..d6a40b539 100644 --- a/gtsam/inference/ISAM.h +++ b/gtsam/inference/ISAM.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/LabeledSymbol.cpp b/gtsam/inference/LabeledSymbol.cpp index c82062246..0d9c35d2c 100644 --- a/gtsam/inference/LabeledSymbol.cpp +++ b/gtsam/inference/LabeledSymbol.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 6b5bfcc03..8e22202ed 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -27,7 +27,7 @@ namespace gtsam { /** * Character and index key used to refer to variables. Will simply cast to a Key, - * i.e., a large integer. Keys are used to retrieve values from Values, + * i.e., a large integer. Keys are used to retrieve values from Values, * specify what variables factors depend on, etc. */ class GTSAM_EXPORT Symbol { diff --git a/gtsam/inference/VariableIndex-inl.h b/gtsam/inference/VariableIndex-inl.h index 352ea166f..bc8100e4a 100644 --- a/gtsam/inference/VariableIndex-inl.h +++ b/gtsam/inference/VariableIndex-inl.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -25,7 +25,7 @@ namespace gtsam { /* ************************************************************************* */ template void VariableIndex::augment(const FG& factors, - boost::optional&> newFactorIndices) { + boost::optional newFactorIndices) { gttic(VariableIndex_augment); // Augment index for each factor diff --git a/gtsam/inference/VariableIndex.cpp b/gtsam/inference/VariableIndex.cpp index 72c56be5b..b71c81988 100644 --- a/gtsam/inference/VariableIndex.cpp +++ b/gtsam/inference/VariableIndex.cpp @@ -35,8 +35,8 @@ void VariableIndex::print(const string& str, const KeyFormatter& keyFormatter) c cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n"; for(KeyMap::value_type key_factors: index_) { cout << "var " << keyFormatter(key_factors.first) << ":"; - for(const size_t factor: key_factors.second) - cout << " " << factor; + for(const auto index: key_factors.second) + cout << " " << index; cout << "\n"; } cout.flush(); @@ -48,8 +48,8 @@ void VariableIndex::outputMetisFormat(ostream& os) const { // run over variables, which will be hyper-edges. for(KeyMap::value_type key_factors: index_) { // every variable is a hyper-edge covering its factors - for(const size_t factor: key_factors.second) - os << (factor+1) << " "; // base 1 + for(const auto index: key_factors.second) + os << (index+1) << " "; // base 1 os << "\n"; } os << flush; diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index ad8d6720b..5f9e05cb0 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -17,6 +17,7 @@ #pragma once +#include #include #include #include @@ -43,7 +44,7 @@ class GTSAM_EXPORT VariableIndex { public: typedef boost::shared_ptr shared_ptr; - typedef FastVector Factors; + typedef FactorIndices Factors; typedef Factors::iterator Factor_iterator; typedef Factors::const_iterator Factor_const_iterator; @@ -63,7 +64,7 @@ public: /// @name Standard Constructors /// @{ - /** Default constructor, creates an empty VariableIndex */ + /// Default constructor, creates an empty VariableIndex VariableIndex() : nFactors_(0), nEntries_(0) {} /** @@ -77,19 +78,16 @@ public: /// @name Standard Interface /// @{ - /** - * The number of variable entries. This is one greater than the variable - * with the highest index. - */ + /// The number of variable entries. This is equal to the number of unique variable Keys. size_t size() const { return index_.size(); } - /** The number of factors in the original factor graph */ + /// The number of factors in the original factor graph size_t nFactors() const { return nFactors_; } - /** The number of nonzero blocks, i.e. the number of variable-factor entries */ + /// The number of nonzero blocks, i.e. the number of variable-factor entries size_t nEntries() const { return nEntries_; } - /** Access a list of factors by variable */ + /// Access a list of factors by variable const Factors& operator[](Key variable) const { KeyMap::const_iterator item = index_.find(variable); if(item == index_.end()) @@ -102,10 +100,10 @@ public: /// @name Testable /// @{ - /** Test for equality (for unit tests and debug assertions). */ + /// Test for equality (for unit tests and debug assertions). bool equals(const VariableIndex& other, double tol=0.0) const; - /** Print the variable index (for unit tests and debugging). */ + /// Print the variable index (for unit tests and debugging). void print(const std::string& str = "VariableIndex: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; @@ -125,7 +123,7 @@ public: * solving problems incrementally. */ template - void augment(const FG& factors, boost::optional&> newFactorIndices = boost::none); + void augment(const FG& factors, boost::optional newFactorIndices = boost::none); /** * Remove entries corresponding to the specified factors. NOTE: We intentionally do not decrement @@ -140,17 +138,17 @@ public: template void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors); - /** Remove unused empty variables (in debug mode verifies they are empty). */ + /// Remove unused empty variables (in debug mode verifies they are empty). template void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey); - /** Iterator to the first variable entry */ + /// Iterator to the first variable entry const_iterator begin() const { return index_.begin(); } - /** Iterator to the first variable entry */ + /// Iterator to the first variable entry const_iterator end() const { return index_.end(); } - /** Find the iterator for the requested variable entry */ + /// Find the iterator for the requested variable entry const_iterator find(Key key) const { return index_.find(key); } protected: diff --git a/gtsam/inference/VariableSlots.cpp b/gtsam/inference/VariableSlots.cpp index d89ad2ced..64883fe49 100644 --- a/gtsam/inference/VariableSlots.cpp +++ b/gtsam/inference/VariableSlots.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/VariableSlots.h b/gtsam/inference/VariableSlots.h index 8ebd5c13c..f9297d300 100644 --- a/gtsam/inference/VariableSlots.h +++ b/gtsam/inference/VariableSlots.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -35,18 +35,18 @@ namespace gtsam { * factor. In each row-block (factor), some of the column-blocks (variables) * may be empty since factors involving different sets of variables are * interleaved. -* +* * VariableSlots describes the 2D block structure of the combined factor. It * is a map >. The Key is the real * variable index of the combined factor slot. The vector tells, for * each row-block (factor), which column-block (variable slot) from the * component factor appears in this block of the combined factor. -* +* * As an example, if the combined factor contains variables 1, 3, and 5, then * "variableSlots[3][2] == 0" indicates that column-block 1 (corresponding to * variable index 3), row-block 2 (also meaning component factor 2), comes from * column-block 0 of component factor 2. -* +* * \nosubgrouping */ class VariableSlots : public FastMap > { diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index de614c2b4..3ea66405b 100644 --- a/gtsam/inference/graph-inl.h +++ b/gtsam/inference/graph-inl.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/graph.h b/gtsam/inference/graph.h index deeb511db..1e172e5c7 100644 --- a/gtsam/inference/graph.h +++ b/gtsam/inference/graph.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/inference-inst.h b/gtsam/inference/inference-inst.h index 1e3f4cdfc..cc5dc4b88 100644 --- a/gtsam/inference/inference-inst.h +++ b/gtsam/inference/inference-inst.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- -* GTSAM Copyright 2010, Georgia Tech Research Corporation, +* GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/tests/testKey.cpp b/gtsam/inference/tests/testKey.cpp index b0708e660..93a161ccd 100644 --- a/gtsam/inference/tests/testKey.cpp +++ b/gtsam/inference/tests/testKey.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/tests/testLabeledSymbol.cpp b/gtsam/inference/tests/testLabeledSymbol.cpp index 18216453d..b463f4131 100644 --- a/gtsam/inference/tests/testLabeledSymbol.cpp +++ b/gtsam/inference/tests/testLabeledSymbol.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/inference/tests/testVariableSlots.cpp b/gtsam/inference/tests/testVariableSlots.cpp index f61b49bdd..a7a0945e7 100644 --- a/gtsam/inference/tests/testVariableSlots.cpp +++ b/gtsam/inference/tests/testVariableSlots.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -11,7 +11,7 @@ /** * @file testVariableSlots.cpp - * @brief + * @brief * @author Richard Roberts * @date Oct 5, 2010 */ diff --git a/gtsam/linear/Errors.cpp b/gtsam/linear/Errors.cpp index 41e2d8c84..42164d540 100644 --- a/gtsam/linear/Errors.cpp +++ b/gtsam/linear/Errors.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/Errors.h b/gtsam/linear/Errors.h index 44d68be83..eb844e04d 100644 --- a/gtsam/linear/Errors.h +++ b/gtsam/linear/Errors.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index c9ef922be..bc96452b9 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -138,23 +138,34 @@ namespace gtsam { //} /* ************************************************************************* */ - pair GaussianBayesNet::matrix() const { + Ordering GaussianBayesNet::ordering() const { GaussianFactorGraph factorGraph(*this); - KeySet keys = factorGraph.keys(); + auto keys = factorGraph.keys(); // add frontal keys in order Ordering ordering; - for (const sharedConditional& cg: *this) + for (const sharedConditional& cg : *this) if (cg) { - for (Key key: cg->frontals()) { + for (Key key : cg->frontals()) { ordering.push_back(key); keys.erase(key); } } // add remaining keys in case Bayes net is incomplete - for (Key key: keys) - ordering.push_back(key); - // return matrix and RHS - return factorGraph.jacobian(ordering); + for (Key key : keys) ordering.push_back(key); + return ordering; + } + + /* ************************************************************************* */ + pair GaussianBayesNet::matrix(boost::optional ordering) const { + if (ordering) { + // Convert to a GaussianFactorGraph and use its machinery + GaussianFactorGraph factorGraph(*this); + return factorGraph.jacobian(ordering); + } else { + // recursively call with default ordering + const auto defaultOrdering = this->ordering(); + return matrix(defaultOrdering); + } } ///* ************************************************************************* */ @@ -181,11 +192,11 @@ namespace gtsam { double logDet = 0.0; for(const sharedConditional& cg: *this) { if(cg->get_model()) { - Vector diag = cg->get_R().diagonal(); + Vector diag = cg->R().diagonal(); cg->get_model()->whitenInPlace(diag); logDet += diag.unaryExpr(ptr_fun(log)).sum(); } else { - logDet += cg->get_R().diagonal().unaryExpr(ptr_fun(log)).sum(); + logDet += cg->R().diagonal().unaryExpr(ptr_fun(log)).sum(); } } return logDet; diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 401583bbf..669c8a964 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -34,7 +34,7 @@ namespace gtsam { typedef FactorGraph Base; typedef GaussianBayesNet This; typedef GaussianConditional ConditionalType; - typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; typedef boost::shared_ptr sharedConditional; /// @name Standard Constructors @@ -74,15 +74,25 @@ namespace gtsam { /// Version of optimize for incomplete BayesNet, needs solution for missing variables VectorValues optimize(const VectorValues& solutionForMissing) const; + /** + * Return ordering corresponding to a topological sort. + * There are many topological sorts of a Bayes net. This one + * corresponds to the one that makes 'matrix' below upper-triangular. + * In case Bayes net is incomplete any non-frontal are added to the end. + */ + Ordering ordering() const; + ///@} ///@name Linear Algebra ///@{ - + /** * Return (dense) upper-triangular matrix representation + * Will return upper-triangular matrix only when using 'ordering' above. + * In case Bayes net is incomplete zero columns are added to the end. */ - std::pair matrix() const; + std::pair matrix(boost::optional ordering = boost::none) const; /** * Optimize along the gradient direction, with a closed-form computation to perform the line @@ -113,7 +123,7 @@ namespace gtsam { /** Compute the gradient of the energy function, \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - * d \right\Vert^2 \f$, centered around \f$ x = x_0 \f$. The gradient is \f$ R^T(Rx-d) \f$. - * + * * @param x0 The center about which to compute the gradient * @return The gradient as a VectorValues */ VectorValues gradient(const VectorValues& x0) const; @@ -121,7 +131,7 @@ namespace gtsam { /** Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d * \right\Vert^2 \f$, centered around zero. The gradient about zero is \f$ -R^T d \f$. See also * gradient(const GaussianBayesNet&, const VectorValues&). - * + * * @param [output] g A VectorValues to store the gradient, which must be preallocated, see * allocateVectorValues */ VectorValues gradientAtZero() const; diff --git a/gtsam/linear/GaussianBayesTree-inl.h b/gtsam/linear/GaussianBayesTree-inl.h index ab311fdf2..d781533e6 100644 --- a/gtsam/linear/GaussianBayesTree-inl.h +++ b/gtsam/linear/GaussianBayesTree-inl.h @@ -43,7 +43,7 @@ double logDeterminant(const typename BAYESTREE::sharedClique& clique) { double result = 0.0; // this clique - result += clique->conditional()->get_R().diagonal().unaryExpr(std::ptr_fun(log)).sum(); + result += clique->conditional()->R().diagonal().unaryExpr(std::ptr_fun(log)).sum(); // sum of children for(const typename BAYESTREE::sharedClique& child: clique->children_) diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp index 0ceecdfd2..2365388d6 100644 --- a/gtsam/linear/GaussianBayesTree.cpp +++ b/gtsam/linear/GaussianBayesTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -37,7 +37,7 @@ namespace gtsam { /* ************************************************************************* */ double logDeterminant(const GaussianBayesTreeClique::shared_ptr& clique, double& parentSum) { - parentSum += clique->conditional()->get_R().diagonal().unaryExpr(std::ptr_fun(log)).sum(); + parentSum += clique->conditional()->R().diagonal().unaryExpr(std::ptr_fun(log)).sum(); assert(false); return 0; } diff --git a/gtsam/linear/GaussianBayesTree.h b/gtsam/linear/GaussianBayesTree.h index 1b2ad47e0..fcc73f80e 100644 --- a/gtsam/linear/GaussianBayesTree.h +++ b/gtsam/linear/GaussianBayesTree.h @@ -94,7 +94,7 @@ namespace gtsam { /** Compute the gradient of the energy function, \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - * d \right\Vert^2 \f$, centered around \f$ x = x_0 \f$. The gradient is \f$ R^T(Rx-d) \f$. - * + * * @param x0 The center about which to compute the gradient * @return The gradient as a VectorValues */ VectorValues gradient(const VectorValues& x0) const; @@ -102,7 +102,7 @@ namespace gtsam { /** Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d * \right\Vert^2 \f$, centered around zero. The gradient about zero is \f$ -R^T d \f$. See also * gradient(const GaussianBayesNet&, const VectorValues&). - * + * * @return A VectorValues storing the gradient. */ VectorValues gradientAtZero() const; diff --git a/gtsam/linear/GaussianConditional-inl.h b/gtsam/linear/GaussianConditional-inl.h index 45d7ecc3b..fe5b1e0d7 100644 --- a/gtsam/linear/GaussianConditional-inl.h +++ b/gtsam/linear/GaussianConditional-inl.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 93217c027..9297d6461 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -62,7 +62,7 @@ namespace gtsam { cout << (boost::format("[%1%]")%(formatter(*it))).str() << " "; } cout << endl; - cout << formatMatrixIndented(" R = ", get_R()) << endl; + cout << formatMatrixIndented(" R = ", R()) << endl; for (const_iterator it = beginParents() ; it != endParents() ; ++it) { cout << formatMatrixIndented((boost::format(" S[%1%] = ")%(formatter(*it))).str(), getA(it)) << endl; @@ -84,8 +84,8 @@ namespace gtsam { // check if R_ and d_ are linear independent for (DenseIndex i = 0; i < Ab_.rows(); i++) { list rows1, rows2; - rows1.push_back(Vector(get_R().row(i))); - rows2.push_back(Vector(c->get_R().row(i))); + rows1.push_back(Vector(R().row(i))); + rows2.push_back(Vector(c->R().row(i))); // check if the matrices are the same // iterate over the parents_ map @@ -120,10 +120,10 @@ namespace gtsam { const Vector xS = x.vector(KeyVector(beginParents(), endParents())); // Update right-hand-side - const Vector rhs = get_d() - get_S() * xS; + const Vector rhs = d() - S() * xS; // Solve matrix - const Vector solution = get_R().triangularView().solve(rhs); + const Vector solution = R().triangularView().solve(rhs); // Check for indeterminant solution if (solution.hasNaN()) { @@ -134,7 +134,7 @@ namespace gtsam { VectorValues result; DenseIndex vectorPosition = 0; for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { - result.insert(*frontal, solution.segment(vectorPosition, getDim(frontal))); + result.emplace(*frontal, solution.segment(vectorPosition, getDim(frontal))); vectorPosition += getDim(frontal); } @@ -149,10 +149,10 @@ namespace gtsam { // Instead of updating getb(), update the right-hand-side from the given rhs const Vector rhsR = rhs.vector(KeyVector(beginFrontals(), endFrontals())); - xS = rhsR - get_S() * xS; + xS = rhsR - S() * xS; // Solve Matrix - Vector soln = get_R().triangularView().solve(xS); + Vector soln = R().triangularView().solve(xS); // Scale by sigmas if (model_) @@ -162,7 +162,7 @@ namespace gtsam { VectorValues result; DenseIndex vectorPosition = 0; for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { - result.insert(*frontal, soln.segment(vectorPosition, getDim(frontal))); + result.emplace(*frontal, soln.segment(vectorPosition, getDim(frontal))); vectorPosition += getDim(frontal); } @@ -172,13 +172,13 @@ namespace gtsam { /* ************************************************************************* */ void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const { Vector frontalVec = gy.vector(KeyVector(beginFrontals(), endFrontals())); - frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R())); + frontalVec = R().transpose().triangularView().solve(frontalVec); // Check for indeterminant solution if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front()); for (const_iterator it = beginParents(); it!= endParents(); it++) - gy[*it] += -1.0 * Matrix(getA(it)).transpose() * frontalVec; + gy[*it].noalias() += -1.0 * getA(it).transpose() * frontalVec; // Scale by sigmas if (model_) diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index d9b75c69f..8b41a4def 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -94,16 +94,16 @@ namespace gtsam { bool equals(const GaussianFactor&cg, double tol = 1e-9) const; /** Return a view of the upper-triangular R block of the conditional */ - constABlock get_R() const { return Ab_.range(0, nrFrontals()); } + constABlock R() const { return Ab_.range(0, nrFrontals()); } /** Get a view of the parent blocks. */ - constABlock get_S() const { return Ab_.range(nrFrontals(), size()); } + constABlock S() const { return Ab_.range(nrFrontals(), size()); } /** Get a view of the S matrix for the variable pointed to by the given key iterator */ - constABlock get_S(const_iterator variable) const { return BaseFactor::getA(variable); } + constABlock S(const_iterator it) const { return BaseFactor::getA(it); } /** Get a view of the r.h.s. vector d */ - const constBVector get_d() const { return BaseFactor::getb(); } + const constBVector d() const { return BaseFactor::getb(); } /** * Solves a conditional Gaussian and writes the solution into the entries of @@ -130,8 +130,17 @@ namespace gtsam { void scaleFrontalsBySigma(VectorValues& gy) const; // __declspec(deprecated) void scaleFrontalsBySigma(VectorValues& gy) const; // FIXME: depreciated flag doesn't appear to exist? - private: +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + constABlock get_R() const { return R(); } + constABlock get_S() const { return S(); } + constABlock get_S(const_iterator it) const { return S(it); } + const constBVector get_d() const { return d(); } + /// @} +#endif + private: /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/linear/GaussianDensity.cpp b/gtsam/linear/GaussianDensity.cpp index 3e35d5e65..d9cde9b91 100644 --- a/gtsam/linear/GaussianDensity.cpp +++ b/gtsam/linear/GaussianDensity.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -35,8 +35,8 @@ namespace gtsam { for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) cout << (boost::format("[%1%]")%(formatter(*it))).str() << " "; cout << endl; - gtsam::print(Matrix(get_R()), "R: "); - gtsam::print(Vector(get_d()), "d: "); + gtsam::print(Matrix(R()), "R: "); + gtsam::print(Vector(d()), "d: "); if(model_) model_->print("Noise model: "); } diff --git a/gtsam/linear/GaussianDensity.h b/gtsam/linear/GaussianDensity.h index 4b4fc1665..03ffe9326 100644 --- a/gtsam/linear/GaussianDensity.h +++ b/gtsam/linear/GaussianDensity.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/GaussianEliminationTree.cpp b/gtsam/linear/GaussianEliminationTree.cpp index 0daaf0707..87469c8c8 100644 --- a/gtsam/linear/GaussianEliminationTree.cpp +++ b/gtsam/linear/GaussianEliminationTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/GaussianEliminationTree.h b/gtsam/linear/GaussianEliminationTree.h index 1a4ba7868..44bd099e9 100644 --- a/gtsam/linear/GaussianEliminationTree.h +++ b/gtsam/linear/GaussianEliminationTree.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -31,7 +31,7 @@ namespace gtsam { typedef EliminationTree Base; ///< Base class typedef GaussianEliminationTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - + /** * Build the elimination tree of a factor graph using pre-computed column structure. * @param factorGraph The factor graph for which to build the elimination tree diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 8c72ee734..a80b4dfd4 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -44,7 +44,7 @@ namespace gtsam { /** Default constructor creates empty factor */ GaussianFactor() {} - + /** Construct from container of keys. This constructor is used internally from derived factor * constructors, either from a container of keys or from a boost::assign::list_of. */ template diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index a4df04cf9..9281c7e7a 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -61,7 +61,7 @@ namespace gtsam { for (GaussianFactor::const_iterator it = gf->begin(); it != gf->end(); it++) { map::iterator it2 = spec.find(*it); if ( it2 == spec.end() ) { - spec.insert(make_pair(*it, gf->getDim(it))); + spec.emplace(*it, gf->getDim(it)); } } } @@ -246,7 +246,7 @@ namespace gtsam { if (blocks.count(j)) blocks[j] += Bj; else - blocks.insert(make_pair(j,Bj)); + blocks.emplace(j,Bj); } } return blocks; diff --git a/gtsam/linear/GaussianISAM.cpp b/gtsam/linear/GaussianISAM.cpp index ddbc5c1b3..aea12e8ee 100644 --- a/gtsam/linear/GaussianISAM.cpp +++ b/gtsam/linear/GaussianISAM.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/GaussianISAM.h b/gtsam/linear/GaussianISAM.h index 24cf2a95d..55208d4d1 100644 --- a/gtsam/linear/GaussianISAM.h +++ b/gtsam/linear/GaussianISAM.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/GaussianJunctionTree.cpp b/gtsam/linear/GaussianJunctionTree.cpp index b038e5452..016811e83 100644 --- a/gtsam/linear/GaussianJunctionTree.cpp +++ b/gtsam/linear/GaussianJunctionTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index cded6d2ee..d16373c78 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -305,7 +305,7 @@ Matrix HessianFactor::information() const { VectorValues HessianFactor::hessianDiagonal() const { VectorValues d; for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { - d.insert(keys_[j], info_.diagonal(j)); + d.emplace(keys_[j], info_.diagonal(j)); } return d; } @@ -436,7 +436,7 @@ VectorValues HessianFactor::gradientAtZero() const { VectorValues g; size_t n = size(); for (size_t j = 0; j < n; ++j) - g.insert(keys_[j], -info_.aboveDiagonalBlock(j, n)); + g.emplace(keys_[j], -info_.aboveDiagonalBlock(j, n)); return g; } @@ -487,6 +487,11 @@ boost::shared_ptr HessianFactor::eliminateCholesky(const Or // Erase the eliminated keys in this factor keys_.erase(begin(), begin() + nFrontals); } catch (const CholeskyFailed&) { +#ifndef NDEBUG + cout << "Partial Cholesky on HessianFactor failed." << endl; + keys.print("Frontal keys "); + print("HessianFactor:"); +#endif throw IndeterminantLinearSystemException(keys.front()); } @@ -513,7 +518,7 @@ VectorValues HessianFactor::solve() { std::size_t offset = 0; for (DenseIndex j = 0; j < (DenseIndex)n; ++j) { const DenseIndex dim = info_.getDim(j); - delta.insert(keys_[j], solution.segment(offset, dim)); + delta.emplace(keys_[j], solution.segment(offset, dim)); offset += dim; } diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index 411feb7a9..c7d4e5405 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -117,7 +117,7 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg) { for (size_t i = 0; i < n; ++i) { const size_t key = ordering_[i]; const size_t dim = colspec.find(key)->second; - insert(make_pair(key, KeyInfoEntry(i, dim, start))); + this->emplace(key, KeyInfoEntry(i, dim, start)); start += dim; } numCols_ = start; @@ -126,8 +126,8 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg) { /****************************************************************************/ vector KeyInfo::colSpec() const { std::vector result(size(), 0); - for ( const KeyInfo::value_type &item: *this ) { - result[item.second.index()] = item.second.dim(); + for ( const auto &item: *this ) { + result[item.second.index] = item.second.dim; } return result; } @@ -135,8 +135,8 @@ vector KeyInfo::colSpec() const { /****************************************************************************/ VectorValues KeyInfo::x0() const { VectorValues result; - for ( const KeyInfo::value_type &item: *this ) { - result.insert(item.first, Vector::Zero(item.second.dim())); + for ( const auto &item: *this ) { + result.emplace(item.first, Vector::Zero(item.second.dim)); } return result; } diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 92dcbfa07..f0fbfbfd2 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -32,8 +32,8 @@ namespace gtsam { // Forward declarations +struct KeyInfoEntry; class KeyInfo; -class KeyInfoEntry; class GaussianFactorGraph; class Values; class VectorValues; @@ -109,27 +109,14 @@ public: /** * Handy data structure for iterative solvers - * key to (index, dimension, colstart) + * key to (index, dimension, start) */ -class GTSAM_EXPORT KeyInfoEntry: public boost::tuple { - -public: - - typedef boost::tuple Base; - +struct GTSAM_EXPORT KeyInfoEntry { + size_t index, dim, start; KeyInfoEntry() { } KeyInfoEntry(size_t idx, size_t d, Key start) : - Base(idx, d, start) { - } - size_t index() const { - return this->get<0>(); - } - size_t dim() const { - return this->get<1>(); - } - size_t colstart() const { - return this->get<2>(); + index(idx), dim(d), start(start) { } }; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 1f5e5557c..06f6b3bed 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -102,10 +102,9 @@ JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2, } /* ************************************************************************* */ -JacobianFactor::JacobianFactor(const HessianFactor& factor) : - Base(factor), Ab_( - VerticalBlockMatrix::LikeActiveViewOf(factor.info(), - factor.rows())) { +JacobianFactor::JacobianFactor(const HessianFactor& factor) + : Base(factor), + Ab_(VerticalBlockMatrix::LikeActiveViewOf(factor.info(), factor.rows())) { // Copy Hessian into our matrix and then do in-place Cholesky Ab_.full() = factor.info().selfadjointView(); @@ -114,16 +113,19 @@ JacobianFactor::JacobianFactor(const HessianFactor& factor) : bool success; boost::tie(maxrank, success) = choleskyCareful(Ab_.matrix()); - // Check for indefinite system - if (!success) + // Check that Cholesky succeeded OR it managed to factor the full Hessian. + // THe latter case occurs with non-positive definite matrices arising from QP. + if (success || maxrank == factor.rows() - 1) { + // Zero out lower triangle + Ab_.matrix().topRows(maxrank).triangularView() = + Matrix::Zero(maxrank, Ab_.matrix().cols()); + // FIXME: replace with triangular system + Ab_.rowEnd() = maxrank; + model_ = SharedDiagonal(); // is equivalent to Unit::Create(maxrank) + } else { + // indefinite system throw IndeterminantLinearSystemException(factor.keys().front()); - - // Zero out lower triangle - Ab_.matrix().topRows(maxrank).triangularView() = - Matrix::Zero(maxrank, Ab_.matrix().cols()); - // FIXME: replace with triangular system - Ab_.rowEnd() = maxrank; - model_ = SharedDiagonal(); // should be same as Unit::Create(maxrank); + } } /* ************************************************************************* */ @@ -164,9 +166,10 @@ boost::tuple, DenseIndex, DenseIndex> _countDims( n += vardim; } else { if(!(varDims[jointVarpos] == vardim)) { - cout << "Factor " << sourceFactorI << " variable " << DefaultKeyFormatter(sourceFactor.keys()[sourceVarpos]) << - " has different dimensionality of " << vardim << " instead of " << varDims[jointVarpos] << endl; - exit(1); + std::stringstream ss; + ss << "Factor " << sourceFactorI << " variable " << DefaultKeyFormatter(sourceFactor.keys()[sourceVarpos]) << + " has different dimensionality of " << vardim << " instead of " << varDims[jointVarpos]; + throw std::runtime_error(ss.str()); } } #else @@ -429,10 +432,9 @@ Vector JacobianFactor::unweighted_error(const VectorValues& c) const { /* ************************************************************************* */ Vector JacobianFactor::error_vector(const VectorValues& c) const { - if (model_) - return model_->whiten(unweighted_error(c)); - else - return unweighted_error(c); + Vector e = unweighted_error(c); + if (model_) model_->whitenInPlace(e); + return e; } /* ************************************************************************* */ @@ -473,10 +475,10 @@ VectorValues JacobianFactor::hessianDiagonal() const { for (size_t k = 0; k < nj; ++k) { Vector column_k = Ab_(pos).col(k); if (model_) - column_k = model_->whiten(column_k); + model_->whitenInPlace(column_k); dj(k) = dot(column_k, column_k); } - d.insert(j, dj); + d.emplace(j, dj); } return d; } @@ -495,7 +497,7 @@ map JacobianFactor::hessianBlockDiagonal() const { Matrix Aj = Ab_(pos); if (model_) Aj = model_->Whiten(Aj); - blocks.insert(make_pair(j, Aj.transpose() * Aj)); + blocks.emplace(j, Aj.transpose() * Aj); } return blocks; } @@ -540,29 +542,38 @@ void JacobianFactor::updateHessian(const KeyVector& infoKeys, /* ************************************************************************* */ Vector JacobianFactor::operator*(const VectorValues& x) const { - Vector Ax = Vector::Zero(Ab_.rows()); + Vector Ax(Ab_.rows()); + Ax.setZero(); if (empty()) return Ax; // Just iterate over all A matrices and multiply in correct config part - for (size_t pos = 0; pos < size(); ++pos) - Ax += Ab_(pos) * x[keys_[pos]]; + for (size_t pos = 0; pos < size(); ++pos) { + // http://eigen.tuxfamily.org/dox/TopicWritingEfficientProductExpression.html + Ax.noalias() += Ab_(pos) * x[keys_[pos]]; + } - return model_ ? model_->whiten(Ax) : Ax; + if (model_) model_->whitenInPlace(Ax); + return Ax; } /* ************************************************************************* */ void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e, - VectorValues& x) const { - Vector E = alpha * (model_ ? model_->whiten(e) : e); + VectorValues& x) const { + Vector E(e.size()); + E.noalias() = alpha * e; + if (model_) model_->whitenInPlace(E); // Just iterate over all A matrices and insert Ai^e into VectorValues for (size_t pos = 0; pos < size(); ++pos) { - Key j = keys_[pos]; - // Create the value as a zero vector if it does not exist. - pair xi = x.tryInsert(j, Vector()); - if (xi.second) - xi.first->second = Vector::Zero(getDim(begin() + pos)); - xi.first->second += Ab_(pos).transpose()*E; + const Key j = keys_[pos]; + // To avoid another malloc if key exists, we explicitly check + auto it = x.find(j); + if (it != x.end()) { + // http://eigen.tuxfamily.org/dox/TopicWritingEfficientProductExpression.html + it->second.noalias() += Ab_(pos).transpose() * E; + } else { + x.emplace(j, Ab_(pos).transpose() * E); + } } } @@ -624,8 +635,8 @@ VectorValues JacobianFactor::gradientAtZero() const { Vector b = getb(); // Gradient is really -A'*b / sigma^2 // transposeMultiplyAdd will divide by sigma once, so we need one more - Vector b_sigma = model_ ? model_->whiten(b) : b; - this->transposeMultiplyAdd(-1.0, b_sigma, g); // g -= A'*b/sigma^2 + if (model_) model_->whitenInPlace(b); + this->transposeMultiplyAdd(-1.0, b, g); // g -= A'*b/sigma^2 return g; } diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index c0d294adf..8a1b145c3 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index 7db29d861..65fac877a 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index f9ef756f1..08307c5ab 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -89,7 +89,7 @@ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const { VectorValues vvX = buildVectorValues(x, keyInfo_); // VectorValues form of A'Ax for multiplyHessianAdd - VectorValues vvAtAx; + VectorValues vvAtAx = keyInfo_.x0(); // crucial for performance // vvAtAx += 1.0 * A'Ax for each factor gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx); @@ -132,14 +132,14 @@ VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, DenseIndex offset = 0; for (size_t i = 0; i < ordering.size(); ++i) { - const Key &key = ordering[i]; + const Key key = ordering[i]; map::const_iterator it = dimensions.find(key); if (it == dimensions.end()) { throw invalid_argument( "buildVectorValues: inconsistent ordering and dimensions"); } const size_t dim = it->second; - result.insert(key, v.segment(offset, dim)); + result.emplace(key, v.segment(offset, dim)); offset += dim; } @@ -150,8 +150,7 @@ VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) { VectorValues result; for ( const KeyInfo::value_type &item: keyInfo ) { - result.insert(item.first, - v.segment(item.second.colstart(), item.second.dim())); + result.emplace(item.first, v.segment(item.second.start, item.second.dim)); } return result; } diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h index 623b29863..31901db3f 100644 --- a/gtsam/linear/Preconditioner.h +++ b/gtsam/linear/Preconditioner.h @@ -70,21 +70,20 @@ public: Preconditioner() {} virtual ~Preconditioner() {} - /* Computation Interfaces */ + /* + * Abstract interface for raw vectors. VectorValues is a speed bottleneck + * and Yong-Dian has profiled preconditioners (outside GTSAM) with the the + * three methods below. In GTSAM, unfortunately, we are still using the + * VectorValues methods called in iterative-inl.h + */ - /* implement x = L^{-1} y */ + /// implement x = L^{-1} y virtual void solve(const Vector& y, Vector &x) const = 0; -// virtual void solve(const VectorValues& y, VectorValues &x) const = 0; - /* implement x = L^{-T} y */ + /// implement x = L^{-T} y virtual void transposeSolve(const Vector& y, Vector& x) const = 0; -// virtual void transposeSolve(const VectorValues& y, VectorValues &x) const = 0; -// /* implement x = L^{-1} L^{-T} y */ -// virtual void fullSolve(const Vector& y, Vector &x) const = 0; -// virtual void fullSolve(const VectorValues& y, VectorValues &x) const = 0; - - /* build/factorize the preconditioner */ + /// build/factorize the preconditioner virtual void build( const GaussianFactorGraph &gfg, const KeyInfo &info, @@ -113,14 +112,7 @@ public: /* Computation Interfaces for raw vector */ virtual void solve(const Vector& y, Vector &x) const { x = y; } -// virtual void solve(const VectorValues& y, VectorValues& x) const { x = y; } - virtual void transposeSolve(const Vector& y, Vector& x) const { x = y; } -// virtual void transposeSolve(const VectorValues& y, VectorValues& x) const { x = y; } - -// virtual void fullSolve(const Vector& y, Vector &x) const { x = y; } -// virtual void fullSolve(const VectorValues& y, VectorValues& x) const { x = y; } - virtual void build( const GaussianFactorGraph &gfg, const KeyInfo &info, @@ -145,8 +137,6 @@ public: /* Computation Interfaces for raw vector */ virtual void solve(const Vector& y, Vector &x) const; virtual void transposeSolve(const Vector& y, Vector& x) const ; -// virtual void fullSolve(const Vector& y, Vector &x) const ; - virtual void build( const GaussianFactorGraph &gfg, const KeyInfo &info, diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp new file mode 100644 index 000000000..22ad89cd8 --- /dev/null +++ b/gtsam/linear/SubgraphBuilder.cpp @@ -0,0 +1,545 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SubgraphBuilder.cpp + * @date Dec 31, 2009 + * @author Frank Dellaert, Yong-Dian Jian + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include // accumulate +#include +#include +#include +#include + +using std::cout; +using std::endl; +using std::ostream; +using std::vector; + +namespace gtsam { + +/*****************************************************************************/ +/* sort the container and return permutation index with default comparator */ +template +static vector sort_idx(const Container &src) { + typedef typename Container::value_type T; + const size_t n = src.size(); + vector > tmp; + tmp.reserve(n); + for (size_t i = 0; i < n; i++) tmp.emplace_back(i, src[i]); + + /* sort */ + std::stable_sort(tmp.begin(), tmp.end()); + + /* copy back */ + vector idx; + idx.reserve(n); + for (size_t i = 0; i < n; i++) { + idx.push_back(tmp[i].first); + } + return idx; +} + +/*****************************************************************************/ +static vector iidSampler(const vector &weight, const size_t n) { + /* compute the sum of the weights */ + const double sum = std::accumulate(weight.begin(), weight.end(), 0.0); + if (sum == 0.0) { + throw std::runtime_error("Weight vector has no non-zero weights"); + } + + /* make a normalized and accumulated version of the weight vector */ + const size_t m = weight.size(); + vector cdf; + cdf.reserve(m); + for (size_t i = 0; i < m; ++i) { + cdf.push_back(weight[i] / sum); + } + + vector acc(m); + std::partial_sum(cdf.begin(), cdf.end(), acc.begin()); + + /* iid sample n times */ + vector samples; + samples.reserve(n); + const double denominator = (double)RAND_MAX; + for (size_t i = 0; i < n; ++i) { + const double value = rand() / denominator; + /* binary search the interval containing "value" */ + const auto it = std::lower_bound(acc.begin(), acc.end(), value); + const size_t index = it - acc.begin(); + samples.push_back(index); + } + return samples; +} + +/*****************************************************************************/ +static vector UniqueSampler(const vector &weight, + const size_t n) { + const size_t m = weight.size(); + if (n > m) throw std::invalid_argument("UniqueSampler: invalid input size"); + + vector samples; + + size_t count = 0; + vector touched(m, false); + while (count < n) { + vector localIndices; + localIndices.reserve(n - count); + vector localWeights; + localWeights.reserve(n - count); + + /* collect data */ + for (size_t i = 0; i < m; ++i) { + if (!touched[i]) { + localIndices.push_back(i); + localWeights.push_back(weight[i]); + } + } + + /* sampling and cache results */ + vector samples = iidSampler(localWeights, n - count); + for (const size_t &index : samples) { + if (touched[index] == false) { + touched[index] = true; + samples.push_back(index); + if (++count >= n) break; + } + } + } + return samples; +} + +/****************************************************************************/ +Subgraph::Subgraph(const vector &indices) { + edges_.reserve(indices.size()); + for (const size_t &index : indices) { + const Edge e {index,1.0}; + edges_.push_back(e); + } +} + +/****************************************************************************/ +vector Subgraph::edgeIndices() const { + vector eid; + eid.reserve(size()); + for (const Edge &edge : edges_) { + eid.push_back(edge.index); + } + return eid; +} + +/****************************************************************************/ +void Subgraph::save(const std::string &fn) const { + std::ofstream os(fn.c_str()); + boost::archive::text_oarchive oa(os); + oa << *this; + os.close(); +} + +/****************************************************************************/ +Subgraph Subgraph::load(const std::string &fn) { + std::ifstream is(fn.c_str()); + boost::archive::text_iarchive ia(is); + Subgraph subgraph; + ia >> subgraph; + is.close(); + return subgraph; +} + +/****************************************************************************/ +ostream &operator<<(ostream &os, const Subgraph::Edge &edge) { + if (edge.weight != 1.0) + os << edge.index << "(" << std::setprecision(2) << edge.weight << ")"; + else + os << edge.index; + return os; +} + +/****************************************************************************/ +ostream &operator<<(ostream &os, const Subgraph &subgraph) { + os << "Subgraph" << endl; + for (const auto &e : subgraph.edges()) { + os << e << ", "; + } + return os; +} + +/*****************************************************************************/ +void SubgraphBuilderParameters::print() const { print(cout); } + +/***************************************************************************************/ +void SubgraphBuilderParameters::print(ostream &os) const { + os << "SubgraphBuilderParameters" << endl + << "skeleton: " << skeletonTranslator(skeletonType) << endl + << "skeleton weight: " << skeletonWeightTranslator(skeletonWeight) + << endl + << "augmentation weight: " + << augmentationWeightTranslator(augmentationWeight) << endl; +} + +/*****************************************************************************/ +ostream &operator<<(ostream &os, const SubgraphBuilderParameters &p) { + p.print(os); + return os; +} + +/*****************************************************************************/ +SubgraphBuilderParameters::Skeleton +SubgraphBuilderParameters::skeletonTranslator(const std::string &src) { + std::string s = src; + boost::algorithm::to_upper(s); + if (s == "NATURALCHAIN") + return NATURALCHAIN; + else if (s == "BFS") + return BFS; + else if (s == "KRUSKAL") + return KRUSKAL; + throw std::invalid_argument( + "SubgraphBuilderParameters::skeletonTranslator undefined string " + s); + return KRUSKAL; +} + +/****************************************************************/ +std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton s) { + if (s == NATURALCHAIN) + return "NATURALCHAIN"; + else if (s == BFS) + return "BFS"; + else if (s == KRUSKAL) + return "KRUSKAL"; + else + return "UNKNOWN"; +} + +/****************************************************************/ +SubgraphBuilderParameters::SkeletonWeight +SubgraphBuilderParameters::skeletonWeightTranslator(const std::string &src) { + std::string s = src; + boost::algorithm::to_upper(s); + if (s == "EQUAL") + return EQUAL; + else if (s == "RHS") + return RHS_2NORM; + else if (s == "LHS") + return LHS_FNORM; + else if (s == "RANDOM") + return RANDOM; + throw std::invalid_argument( + "SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + + s); + return EQUAL; +} + +/****************************************************************/ +std::string SubgraphBuilderParameters::skeletonWeightTranslator( + SkeletonWeight w) { + if (w == EQUAL) + return "EQUAL"; + else if (w == RHS_2NORM) + return "RHS"; + else if (w == LHS_FNORM) + return "LHS"; + else if (w == RANDOM) + return "RANDOM"; + else + return "UNKNOWN"; +} + +/****************************************************************/ +SubgraphBuilderParameters::AugmentationWeight +SubgraphBuilderParameters::augmentationWeightTranslator( + const std::string &src) { + std::string s = src; + boost::algorithm::to_upper(s); + if (s == "SKELETON") return SKELETON; + // else if (s == "STRETCH") return STRETCH; + // else if (s == "GENERALIZED_STRETCH") return GENERALIZED_STRETCH; + throw std::invalid_argument( + "SubgraphBuilder::Parameters::augmentationWeightTranslator undefined " + "string " + + s); + return SKELETON; +} + +/****************************************************************/ +std::string SubgraphBuilderParameters::augmentationWeightTranslator( + AugmentationWeight w) { + if (w == SKELETON) return "SKELETON"; + // else if ( w == STRETCH ) return "STRETCH"; + // else if ( w == GENERALIZED_STRETCH ) return "GENERALIZED_STRETCH"; + else + return "UNKNOWN"; +} + +/****************************************************************/ +vector SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, + const FastMap &ordering, + const vector &weights) const { + const SubgraphBuilderParameters &p = parameters_; + switch (p.skeletonType) { + case SubgraphBuilderParameters::NATURALCHAIN: + return natural_chain(gfg); + break; + case SubgraphBuilderParameters::BFS: + return bfs(gfg); + break; + case SubgraphBuilderParameters::KRUSKAL: + return kruskal(gfg, ordering, weights); + break; + default: + std::cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl; + break; + } + return vector(); +} + +/****************************************************************/ +vector SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const { + vector unaryFactorIndices; + size_t index = 0; + for (const auto &factor : gfg) { + if (factor->size() == 1) { + unaryFactorIndices.push_back(index); + } + index++; + } + return unaryFactorIndices; +} + +/****************************************************************/ +vector SubgraphBuilder::natural_chain( + const GaussianFactorGraph &gfg) const { + vector chainFactorIndices; + size_t index = 0; + for (const GaussianFactor::shared_ptr &gf : gfg) { + if (gf->size() == 2) { + const Key k0 = gf->keys()[0], k1 = gf->keys()[1]; + if ((k1 - k0) == 1 || (k0 - k1) == 1) chainFactorIndices.push_back(index); + } + index++; + } + return chainFactorIndices; +} + +/****************************************************************/ +vector SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const { + const VariableIndex variableIndex(gfg); + /* start from the first key of the first factor */ + Key seed = gfg[0]->keys()[0]; + + const size_t n = variableIndex.size(); + + /* each vertex has self as the predecessor */ + vector bfsFactorIndices; + bfsFactorIndices.reserve(n - 1); + + /* Initialize */ + std::queue q; + q.push(seed); + + std::set flags; + flags.insert(seed); + + /* traversal */ + while (!q.empty()) { + const size_t head = q.front(); + q.pop(); + for (const size_t index : variableIndex[head]) { + const GaussianFactor &gf = *gfg[index]; + for (const size_t key : gf.keys()) { + if (flags.count(key) == 0) { + q.push(key); + flags.insert(key); + bfsFactorIndices.push_back(index); + } + } + } + } + return bfsFactorIndices; +} + +/****************************************************************/ +vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, + const FastMap &ordering, + const vector &weights) const { + const VariableIndex variableIndex(gfg); + const size_t n = variableIndex.size(); + const vector sortedIndices = sort_idx(weights); + + /* initialize buffer */ + vector treeIndices; + treeIndices.reserve(n - 1); + + // container for acsendingly sorted edges + DSFVector dsf(n); + + size_t count = 0; + double sum = 0.0; + for (const size_t index : sortedIndices) { + const GaussianFactor &gf = *gfg[index]; + const auto keys = gf.keys(); + if (keys.size() != 2) continue; + const size_t u = ordering.find(keys[0])->second, + v = ordering.find(keys[1])->second; + if (dsf.find(u) != dsf.find(v)) { + dsf.merge(u, v); + treeIndices.push_back(index); + sum += weights[index]; + if (++count == n - 1) break; + } + } + return treeIndices; +} + +/****************************************************************/ +vector SubgraphBuilder::sample(const vector &weights, + const size_t t) const { + return UniqueSampler(weights, t); +} + +/****************************************************************/ +Subgraph SubgraphBuilder::operator()( + const GaussianFactorGraph &gfg) const { + const auto &p = parameters_; + const auto inverse_ordering = Ordering::Natural(gfg); + const FastMap forward_ordering = inverse_ordering.invert(); + const size_t n = inverse_ordering.size(), m = gfg.size(); + + // Make sure the subgraph preconditioner does not include more than half of + // the edges beyond the spanning tree, or we might as well solve the whole + // thing. + size_t numExtraEdges = n * p.augmentationFactor; + const size_t numRemaining = m - (n - 1); + numExtraEdges = std::min(numExtraEdges, numRemaining / 2); + + // Calculate weights + vector weights = this->weights(gfg); + + // Build spanning tree. + const vector tree = buildTree(gfg, forward_ordering, weights); + if (tree.size() != n - 1) { + throw std::runtime_error( + "SubgraphBuilder::operator() failure: tree.size() != n-1"); + } + + // Downweight the tree edges to zero. + for (const size_t index : tree) { + weights[index] = 0.0; + } + + /* decide how many edges to augment */ + vector offTree = sample(weights, numExtraEdges); + + vector subgraph = unary(gfg); + subgraph.insert(subgraph.end(), tree.begin(), tree.end()); + subgraph.insert(subgraph.end(), offTree.begin(), offTree.end()); + + return Subgraph(subgraph); +} + +/****************************************************************/ +SubgraphBuilder::Weights SubgraphBuilder::weights( + const GaussianFactorGraph &gfg) const { + const size_t m = gfg.size(); + Weights weight; + weight.reserve(m); + + for (const GaussianFactor::shared_ptr &gf : gfg) { + switch (parameters_.skeletonWeight) { + case SubgraphBuilderParameters::EQUAL: + weight.push_back(1.0); + break; + case SubgraphBuilderParameters::RHS_2NORM: { + if (JacobianFactor::shared_ptr jf = + boost::dynamic_pointer_cast(gf)) { + weight.push_back(jf->getb().norm()); + } else if (HessianFactor::shared_ptr hf = + boost::dynamic_pointer_cast(gf)) { + weight.push_back(hf->linearTerm().norm()); + } + } break; + case SubgraphBuilderParameters::LHS_FNORM: { + if (JacobianFactor::shared_ptr jf = + boost::dynamic_pointer_cast(gf)) { + weight.push_back(std::sqrt(jf->getA().squaredNorm())); + } else if (HessianFactor::shared_ptr hf = + boost::dynamic_pointer_cast(gf)) { + weight.push_back(std::sqrt(hf->information().squaredNorm())); + } + } break; + + case SubgraphBuilderParameters::RANDOM: + weight.push_back(std::rand() % 100 + 1.0); + break; + + default: + throw std::invalid_argument( + "SubgraphBuilder::weights: undefined weight scheme "); + break; + } + } + return weight; +} + +/*****************************************************************************/ +GaussianFactorGraph::shared_ptr buildFactorSubgraph( + const GaussianFactorGraph &gfg, const Subgraph &subgraph, + const bool clone) { + auto subgraphFactors = boost::make_shared(); + subgraphFactors->reserve(subgraph.size()); + for (const auto &e : subgraph) { + const auto factor = gfg[e.index]; + subgraphFactors->push_back(clone ? factor->clone(): factor); + } + return subgraphFactors; +} + +/**************************************************************************************************/ +std::pair // +splitFactorGraph(const GaussianFactorGraph &factorGraph, const Subgraph &subgraph) { + + // Get the subgraph by calling cheaper method + auto subgraphFactors = buildFactorSubgraph(factorGraph, subgraph, false); + + // Now, copy all factors then set subGraph factors to zero + auto remaining = boost::make_shared(factorGraph); + + for (const auto &e : subgraph) { + remaining->remove(e.index); + } + + return std::make_pair(subgraphFactors, remaining); +} + +/*****************************************************************************/ + +} // namespace gtsam diff --git a/gtsam/linear/SubgraphBuilder.h b/gtsam/linear/SubgraphBuilder.h new file mode 100644 index 000000000..5247ea2a2 --- /dev/null +++ b/gtsam/linear/SubgraphBuilder.h @@ -0,0 +1,182 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SubgraphBuilder.h + * @date Dec 31, 2009 + * @author Frank Dellaert, Yong-Dian Jian + */ + +#pragma once + +#include +#include +#include + +#include +#include + +#include + +namespace boost { +namespace serialization { +class access; +} /* namespace serialization */ +} /* namespace boost */ + +namespace gtsam { + +// Forward declarations +class GaussianFactorGraph; +struct PreconditionerParameters; + +/**************************************************************************/ +class GTSAM_EXPORT Subgraph { + public: + struct GTSAM_EXPORT Edge { + size_t index; /* edge id */ + double weight; /* edge weight */ + inline bool isUnitWeight() const { return (weight == 1.0); } + friend std::ostream &operator<<(std::ostream &os, const Edge &edge); + + private: + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_NVP(index); + ar &BOOST_SERIALIZATION_NVP(weight); + } + }; + + typedef std::vector Edges; + typedef std::vector EdgeIndices; + typedef Edges::iterator iterator; + typedef Edges::const_iterator const_iterator; + + protected: + Edges edges_; /* index to the factors */ + + public: + Subgraph() {} + Subgraph(const Subgraph &subgraph) : edges_(subgraph.edges()) {} + Subgraph(const Edges &edges) : edges_(edges) {} + Subgraph(const std::vector &indices); + + inline const Edges &edges() const { return edges_; } + inline size_t size() const { return edges_.size(); } + EdgeIndices edgeIndices() const; + + iterator begin() { return edges_.begin(); } + const_iterator begin() const { return edges_.begin(); } + iterator end() { return edges_.end(); } + const_iterator end() const { return edges_.end(); } + + void save(const std::string &fn) const; + static Subgraph load(const std::string &fn); + friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph); + + private: + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_NVP(edges_); + } +}; + +/****************************************************************************/ +struct GTSAM_EXPORT SubgraphBuilderParameters { + typedef boost::shared_ptr shared_ptr; + + enum Skeleton { + /* augmented tree */ + NATURALCHAIN = 0, /* natural ordering of the graph */ + BFS, /* breadth-first search tree */ + KRUSKAL, /* maximum weighted spanning tree */ + } skeletonType; + + enum SkeletonWeight { /* how to weigh the graph edges */ + EQUAL = 0, /* every block edge has equal weight */ + RHS_2NORM, /* use the 2-norm of the rhs */ + LHS_FNORM, /* use the frobenius norm of the lhs */ + RANDOM, /* bounded random edge weight */ + } skeletonWeight; + + enum AugmentationWeight { /* how to weigh the graph edges */ + SKELETON = 0, /* use the same weights in building + the skeleton */ + // STRETCH, /* stretch in the + // laplacian sense */ GENERALIZED_STRETCH /* + // the generalized stretch defined in + // jian2013iros */ + } augmentationWeight; + + /// factor multiplied with n, yields number of extra edges. + double augmentationFactor; + + SubgraphBuilderParameters() + : skeletonType(KRUSKAL), + skeletonWeight(RANDOM), + augmentationWeight(SKELETON), + augmentationFactor(1.0) {} + virtual ~SubgraphBuilderParameters() {} + + /* for serialization */ + void print() const; + virtual void print(std::ostream &os) const; + friend std::ostream &operator<<(std::ostream &os, + const PreconditionerParameters &p); + + static Skeleton skeletonTranslator(const std::string &s); + static std::string skeletonTranslator(Skeleton s); + static SkeletonWeight skeletonWeightTranslator(const std::string &s); + static std::string skeletonWeightTranslator(SkeletonWeight w); + static AugmentationWeight augmentationWeightTranslator(const std::string &s); + static std::string augmentationWeightTranslator(AugmentationWeight w); +}; + +/*****************************************************************************/ +class GTSAM_EXPORT SubgraphBuilder { + public: + typedef SubgraphBuilder Base; + typedef std::vector Weights; + + SubgraphBuilder( + const SubgraphBuilderParameters &p = SubgraphBuilderParameters()) + : parameters_(p) {} + virtual ~SubgraphBuilder() {} + virtual Subgraph operator()(const GaussianFactorGraph &jfg) const; + + private: + std::vector buildTree(const GaussianFactorGraph &gfg, + const FastMap &ordering, + const std::vector &weights) const; + std::vector unary(const GaussianFactorGraph &gfg) const; + std::vector natural_chain(const GaussianFactorGraph &gfg) const; + std::vector bfs(const GaussianFactorGraph &gfg) const; + std::vector kruskal(const GaussianFactorGraph &gfg, + const FastMap &ordering, + const std::vector &weights) const; + std::vector sample(const std::vector &weights, + const size_t t) const; + Weights weights(const GaussianFactorGraph &gfg) const; + SubgraphBuilderParameters parameters_; +}; + +/** Select the factors in a factor graph according to the subgraph. */ +boost::shared_ptr buildFactorSubgraph( + const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone); + +/** Split the graph into a subgraph and the remaining edges. + * Note that the remaining factorgraph has null factors. */ +std::pair, boost::shared_ptr > +splitFactorGraph(const GaussianFactorGraph &factorGraph, const Subgraph &subgraph); + +} // namespace gtsam diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index d796e28b7..fdcb4f7ac 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -12,451 +12,85 @@ /** * @file SubgraphPreconditioner.cpp * @date Dec 31, 2009 - * @author: Frank Dellaert + * @author Frank Dellaert, Yong-Dian Jian */ #include -#include -#include + +#include +#include +#include #include -#include -#include -#include -#include -#include -#include #include #include #include -#include -#include -#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include // accumulate -#include -#include #include -#include -#include -#include -using namespace std; +using std::cout; +using std::endl; +using std::vector; +using std::ostream; namespace gtsam { /* ************************************************************************* */ -static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { - GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); - for(const GaussianFactor::shared_ptr &gf: gfg) { - JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf); - if( !jf ) { - jf = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) - } - result->push_back(jf); +static Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, + const KeyVector &keys) { + /* a cache of starting index and dim */ + vector > cache; + cache.reserve(3); + + /* figure out dimension by traversing the keys */ + size_t dim = 0; + for (const Key &key : keys) { + const KeyInfoEntry &entry = keyInfo.find(key)->second; + cache.emplace_back(entry.start, entry.dim); + dim += entry.dim; } + + /* use the cache to fill the result */ + Vector result(dim); + size_t index = 0; + for (const auto &p : cache) { + result.segment(index, p.second) = src.segment(p.first, p.second); + index += p.second; + } + return result; } /*****************************************************************************/ -static std::vector iidSampler(const vector &weight, const size_t n) { - - /* compute the sum of the weights */ - const double sum = std::accumulate(weight.begin(), weight.end(), 0.0); - - /* make a normalized and accumulated version of the weight vector */ - const size_t m = weight.size(); - vector w; w.reserve(m); - for ( size_t i = 0 ; i < m ; ++i ) { - w.push_back(weight[i]/sum); +static void setSubvector(const Vector &src, const KeyInfo &keyInfo, + const KeyVector &keys, Vector &dst) { + size_t index = 0; + for (const Key &key : keys) { + const KeyInfoEntry &entry = keyInfo.find(key)->second; + const size_t keyDim = entry.dim; + dst.segment(entry.start, keyDim) = src.segment(index, keyDim); + index += keyDim; } +} - vector acc(m); - std::partial_sum(w.begin(),w.end(),acc.begin()); - - /* iid sample n times */ - vector result; result.reserve(n); - const double denominator = (double)RAND_MAX; - for ( size_t i = 0 ; i < n ; ++i ) { - const double value = rand() / denominator; - /* binary search the interval containing "value" */ - vector::iterator it = std::lower_bound(acc.begin(), acc.end(), value); - size_t idx = it - acc.begin(); - result.push_back(idx); - } +/* ************************************************************************* */ +// Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with +// Cholesky) +static GaussianFactorGraph::shared_ptr convertToJacobianFactors( + const GaussianFactorGraph &gfg) { + auto result = boost::make_shared(); + for (const auto &factor : gfg) + if (factor) { + auto jf = boost::dynamic_pointer_cast(factor); + if (!jf) { + jf = boost::make_shared(*factor); + } + result->push_back(jf); + } return result; } -/*****************************************************************************/ -vector uniqueSampler(const vector &weight, const size_t n) { - - const size_t m = weight.size(); - if ( n > m ) throw std::invalid_argument("uniqueSampler: invalid input size"); - - vector result; - - size_t count = 0; - std::vector touched(m, false); - while ( count < n ) { - std::vector localIndices; localIndices.reserve(n-count); - std::vector localWeights; localWeights.reserve(n-count); - - /* collect data */ - for ( size_t i = 0 ; i < m ; ++i ) { - if ( !touched[i] ) { - localIndices.push_back(i); - localWeights.push_back(weight[i]); - } - } - - /* sampling and cache results */ - vector samples = iidSampler(localWeights, n-count); - for ( const size_t &id: samples ) { - if ( touched[id] == false ) { - touched[id] = true ; - result.push_back(id); - if ( ++count >= n ) break; - } - } - } - return result; -} - -/****************************************************************************/ -Subgraph::Subgraph(const std::vector &indices) { - edges_.reserve(indices.size()); - for ( const size_t &idx: indices ) { - edges_.push_back(SubgraphEdge(idx, 1.0)); - } -} - -/****************************************************************************/ -std::vector Subgraph::edgeIndices() const { - std::vector eid; eid.reserve(size()); - for ( const SubgraphEdge &edge: edges_ ) { - eid.push_back(edge.index_); - } - return eid; -} - -/****************************************************************************/ -void Subgraph::save(const std::string &fn) const { - std::ofstream os(fn.c_str()); - boost::archive::text_oarchive oa(os); - oa << *this; - os.close(); -} - -/****************************************************************************/ -Subgraph::shared_ptr Subgraph::load(const std::string &fn) { - std::ifstream is(fn.c_str()); - boost::archive::text_iarchive ia(is); - Subgraph::shared_ptr subgraph(new Subgraph()); - ia >> *subgraph; - is.close(); - return subgraph; -} - -/****************************************************************************/ -std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge) { - if ( edge.weight() != 1.0 ) - os << edge.index() << "(" << std::setprecision(2) << edge.weight() << ")"; - else - os << edge.index() ; - return os; -} - -/****************************************************************************/ -std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph) { - os << "Subgraph" << endl; - for ( const SubgraphEdge &e: subgraph.edges() ) { - os << e << ", " ; - } - return os; -} - -/*****************************************************************************/ -void SubgraphBuilderParameters::print() const { - print(cout); -} - -/***************************************************************************************/ -void SubgraphBuilderParameters::print(ostream &os) const { - os << "SubgraphBuilderParameters" << endl - << "skeleton: " << skeletonTranslator(skeleton_) << endl - << "skeleton weight: " << skeletonWeightTranslator(skeletonWeight_) << endl - << "augmentation weight: " << augmentationWeightTranslator(augmentationWeight_) << endl - ; -} - -/*****************************************************************************/ -ostream& operator<<(ostream &os, const SubgraphBuilderParameters &p) { - p.print(os); - return os; -} - -/*****************************************************************************/ -SubgraphBuilderParameters::Skeleton SubgraphBuilderParameters::skeletonTranslator(const std::string &src){ - std::string s = src; boost::algorithm::to_upper(s); - if (s == "NATURALCHAIN") return NATURALCHAIN; - else if (s == "BFS") return BFS; - else if (s == "KRUSKAL") return KRUSKAL; - throw invalid_argument("SubgraphBuilderParameters::skeletonTranslator undefined string " + s); - return KRUSKAL; -} - -/****************************************************************/ -std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton w) { - if ( w == NATURALCHAIN )return "NATURALCHAIN"; - else if ( w == BFS ) return "BFS"; - else if ( w == KRUSKAL )return "KRUSKAL"; - else return "UNKNOWN"; -} - -/****************************************************************/ -SubgraphBuilderParameters::SkeletonWeight SubgraphBuilderParameters::skeletonWeightTranslator(const std::string &src) { - std::string s = src; boost::algorithm::to_upper(s); - if (s == "EQUAL") return EQUAL; - else if (s == "RHS") return RHS_2NORM; - else if (s == "LHS") return LHS_FNORM; - else if (s == "RANDOM") return RANDOM; - throw invalid_argument("SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + s); - return EQUAL; -} - -/****************************************************************/ -std::string SubgraphBuilderParameters::skeletonWeightTranslator(SkeletonWeight w) { - if ( w == EQUAL ) return "EQUAL"; - else if ( w == RHS_2NORM ) return "RHS"; - else if ( w == LHS_FNORM ) return "LHS"; - else if ( w == RANDOM ) return "RANDOM"; - else return "UNKNOWN"; -} - -/****************************************************************/ -SubgraphBuilderParameters::AugmentationWeight SubgraphBuilderParameters::augmentationWeightTranslator(const std::string &src) { - std::string s = src; boost::algorithm::to_upper(s); - if (s == "SKELETON") return SKELETON; -// else if (s == "STRETCH") return STRETCH; -// else if (s == "GENERALIZED_STRETCH") return GENERALIZED_STRETCH; - throw invalid_argument("SubgraphBuilder::Parameters::augmentationWeightTranslator undefined string " + s); - return SKELETON; -} - -/****************************************************************/ -std::string SubgraphBuilderParameters::augmentationWeightTranslator(AugmentationWeight w) { - if ( w == SKELETON ) return "SKELETON"; -// else if ( w == STRETCH ) return "STRETCH"; -// else if ( w == GENERALIZED_STRETCH ) return "GENERALIZED_STRETCH"; - else return "UNKNOWN"; -} - -/****************************************************************/ -std::vector SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &w) const { - const SubgraphBuilderParameters &p = parameters_; - switch (p.skeleton_) { - case SubgraphBuilderParameters::NATURALCHAIN: - return natural_chain(gfg); - break; - case SubgraphBuilderParameters::BFS: - return bfs(gfg); - break; - case SubgraphBuilderParameters::KRUSKAL: - return kruskal(gfg, ordering, w); - break; - default: - cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl; - break; - } - return vector(); -} - -/****************************************************************/ -std::vector SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const { - std::vector result ; - size_t idx = 0; - for ( const GaussianFactor::shared_ptr &gf: gfg ) { - if ( gf->size() == 1 ) { - result.push_back(idx); - } - idx++; - } - return result; -} - -/****************************************************************/ -std::vector SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const { - std::vector result ; - size_t idx = 0; - for ( const GaussianFactor::shared_ptr &gf: gfg ) { - if ( gf->size() == 2 ) { - const Key k0 = gf->keys()[0], k1 = gf->keys()[1]; - if ( (k1-k0) == 1 || (k0-k1) == 1 ) - result.push_back(idx); - } - idx++; - } - return result; -} - -/****************************************************************/ -std::vector SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const { - const VariableIndex variableIndex(gfg); - /* start from the first key of the first factor */ - Key seed = gfg[0]->keys()[0]; - - const size_t n = variableIndex.size(); - - /* each vertex has self as the predecessor */ - std::vector result; - result.reserve(n-1); - - /* Initialize */ - std::queue q; - q.push(seed); - - std::set flags; - flags.insert(seed); - - /* traversal */ - while ( !q.empty() ) { - const size_t head = q.front(); q.pop(); - for ( const size_t id: variableIndex[head] ) { - const GaussianFactor &gf = *gfg[id]; - for ( const size_t key: gf.keys() ) { - if ( flags.count(key) == 0 ) { - q.push(key); - flags.insert(key); - result.push_back(id); - } - } - } - } - return result; -} - -/****************************************************************/ -std::vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &w) const { - const VariableIndex variableIndex(gfg); - const size_t n = variableIndex.size(); - const vector idx = sort_idx(w) ; - - /* initialize buffer */ - vector result; - result.reserve(n-1); - - // container for acsendingly sorted edges - DSFVector D(n) ; - - size_t count = 0 ; double sum = 0.0 ; - for (const size_t id: idx) { - const GaussianFactor &gf = *gfg[id]; - if ( gf.keys().size() != 2 ) continue; - const size_t u = ordering.find(gf.keys()[0])->second, - u_root = D.find(u), - v = ordering.find(gf.keys()[1])->second, - v_root = D.find(v) ; - if ( u_root != v_root ) { - D.merge(u_root, v_root) ; - result.push_back(id) ; - sum += w[id] ; - if ( ++count == n-1 ) break ; - } - } - return result; -} - -/****************************************************************/ -std::vector SubgraphBuilder::sample(const std::vector &weights, const size_t t) const { - return uniqueSampler(weights, t); -} - -/****************************************************************/ -Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg) const { - - const SubgraphBuilderParameters &p = parameters_; - const Ordering inverse_ordering = Ordering::Natural(gfg); - const FastMap forward_ordering = inverse_ordering.invert(); - const size_t n = inverse_ordering.size(), t = n * p.complexity_ ; - - vector w = weights(gfg); - const vector tree = buildTree(gfg, forward_ordering, w); - - /* sanity check */ - if ( tree.size() != n-1 ) { - throw runtime_error("SubgraphBuilder::operator() tree.size() != n-1 failed "); - } - - /* down weight the tree edges to zero */ - for ( const size_t id: tree ) { - w[id] = 0.0; - } - - /* decide how many edges to augment */ - std::vector offTree = sample(w, t); - - vector subgraph = unary(gfg); - subgraph.insert(subgraph.end(), tree.begin(), tree.end()); - subgraph.insert(subgraph.end(), offTree.begin(), offTree.end()); - - return boost::make_shared(subgraph); -} - -/****************************************************************/ -SubgraphBuilder::Weights SubgraphBuilder::weights(const GaussianFactorGraph &gfg) const -{ - const size_t m = gfg.size() ; - Weights weight; weight.reserve(m); - - for(const GaussianFactor::shared_ptr &gf: gfg ) { - switch ( parameters_.skeletonWeight_ ) { - case SubgraphBuilderParameters::EQUAL: - weight.push_back(1.0); - break; - case SubgraphBuilderParameters::RHS_2NORM: - { - if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { - weight.push_back(jf->getb().norm()); - } - else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { - weight.push_back(hf->linearTerm().norm()); - } - } - break; - case SubgraphBuilderParameters::LHS_FNORM: - { - if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { - weight.push_back(std::sqrt(jf->getA().squaredNorm())); - } - else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { - weight.push_back(std::sqrt(hf->information().squaredNorm())); - } - } - break; - - case SubgraphBuilderParameters::RANDOM: - weight.push_back(std::rand()%100 + 1.0); - break; - - default: - throw invalid_argument("SubgraphBuilder::weights: undefined weight scheme "); - break; - } - } - return weight; -} - /* ************************************************************************* */ SubgraphPreconditioner::SubgraphPreconditioner(const SubgraphPreconditionerParameters &p) : parameters_(p) {} @@ -484,21 +118,20 @@ double SubgraphPreconditioner::error(const VectorValues& y) const { /* ************************************************************************* */ // gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar), -VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const { +VectorValues SubgraphPreconditioner::gradient(const VectorValues &y) const { VectorValues x = Rc1()->backSubstitute(y); /* inv(R1)*y */ - Errors e = (*Ab2()*x - *b2bar()); /* (A2*inv(R1)*y-b2bar) */ + Errors e = (*Ab2() * x - *b2bar()); /* (A2*inv(R1)*y-b2bar) */ VectorValues v = VectorValues::Zero(x); - Ab2()->transposeMultiplyAdd(1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */ + Ab2()->transposeMultiplyAdd(1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */ return y + Rc1()->backSubstituteTranspose(v); } /* ************************************************************************* */ // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y] Errors SubgraphPreconditioner::operator*(const VectorValues& y) const { - Errors e(y); VectorValues x = Rc1()->backSubstitute(y); /* x=inv(R1)*y */ - Errors e2 = *Ab2() * x; /* A2*x */ + Errors e2 = *Ab2() * x; /* A2*x */ e.splice(e.end(), e2); return e; } @@ -508,8 +141,10 @@ Errors SubgraphPreconditioner::operator*(const VectorValues& y) const { void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const { Errors::iterator ei = e.begin(); - for (size_t i = 0; i < y.size(); ++i, ++ei) - *ei = y[i]; + for(const auto& key_value: y) { + *ei = key_value.second; + ++ei; + } // Add A2 contribution VectorValues x = Rc1()->backSubstitute(y); // x=inv(R1)*y @@ -522,8 +157,10 @@ VectorValues SubgraphPreconditioner::operator^(const Errors& e) const { Errors::const_iterator it = e.begin(); VectorValues y = zero(); - for (size_t i = 0; i < y.size(); ++i, ++it) - y[i] = *it; + for(auto& key_value: y) { + key_value.second = *it; + ++it; + } transposeMultiplyAdd2(1.0, it, e.end(), y); return y; } @@ -534,9 +171,10 @@ void SubgraphPreconditioner::transposeMultiplyAdd (double alpha, const Errors& e, VectorValues& y) const { Errors::const_iterator it = e.begin(); - for (size_t i = 0; i < y.size(); ++i, ++it) { + for(auto& key_value: y) { const Vector& ei = *it; - axpy(alpha, ei, y[i]); + axpy(alpha, ei, key_value.second); + ++it; } transposeMultiplyAdd2(alpha, it, e.end(), y); } @@ -563,47 +201,51 @@ void SubgraphPreconditioner::print(const std::string& s) const { } /*****************************************************************************/ -void SubgraphPreconditioner::solve(const Vector& y, Vector &x) const -{ - /* copy first */ - std::copy(y.data(), y.data() + y.rows(), x.data()); +void SubgraphPreconditioner::solve(const Vector &y, Vector &x) const { + assert(x.size() == y.size()); - /* in place back substitute */ - for (auto cg: boost::adaptors::reverse(*Rc1_)) { + /* back substitute */ + for (const auto &cg : boost::adaptors::reverse(*Rc1_)) { /* collect a subvector of x that consists of the parents of cg (S) */ - const Vector xParent = getSubvector(x, keyInfo_, KeyVector(cg->beginParents(), cg->endParents())); - const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals())); + const KeyVector parentKeys(cg->beginParents(), cg->endParents()); + const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals()); + const Vector xParent = getSubvector(x, keyInfo_, parentKeys); + const Vector rhsFrontal = getSubvector(y, keyInfo_, frontalKeys); /* compute the solution for the current pivot */ - const Vector solFrontal = cg->get_R().triangularView().solve(rhsFrontal - cg->get_S() * xParent); + const Vector solFrontal = cg->R().triangularView().solve( + rhsFrontal - cg->S() * xParent); /* assign subvector of sol to the frontal variables */ - setSubvector(solFrontal, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()), x); + setSubvector(solFrontal, keyInfo_, frontalKeys, x); } } /*****************************************************************************/ -void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const -{ +void SubgraphPreconditioner::transposeSolve(const Vector &y, Vector &x) const { /* copy first */ + assert(x.size() == y.size()); std::copy(y.data(), y.data() + y.rows(), x.data()); /* in place back substitute */ - for(const boost::shared_ptr & cg: *Rc1_) { - const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals())); -// const Vector solFrontal = cg->get_R().triangularView().transpose().solve(rhsFrontal); - const Vector solFrontal = cg->get_R().transpose().triangularView().solve(rhsFrontal); + for (const auto &cg : *Rc1_) { + const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals()); + const Vector rhsFrontal = getSubvector(x, keyInfo_, frontalKeys); + const Vector solFrontal = + cg->R().transpose().triangularView().solve( + rhsFrontal); // Check for indeterminant solution - if ( solFrontal.hasNaN()) throw IndeterminantLinearSystemException(cg->keys().front()); + if (solFrontal.hasNaN()) + throw IndeterminantLinearSystemException(cg->keys().front()); /* assign subvector of sol to the frontal variables */ - setSubvector(solFrontal, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()), x); + setSubvector(solFrontal, keyInfo_, frontalKeys, x); /* substract from parent variables */ - for (GaussianConditional::const_iterator it = cg->beginParents(); it != cg->endParents(); it++) { - KeyInfo::const_iterator it2 = keyInfo_.find(*it); - Eigen::Map rhsParent(x.data()+it2->second.colstart(), it2->second.dim(), 1); + for (auto it = cg->beginParents(); it != cg->endParents(); it++) { + const KeyInfoEntry &entry = keyInfo_.find(*it)->second; + Eigen::Map rhsParent(x.data() + entry.start, entry.dim, 1); rhsParent -= Matrix(cg->getA(it)).transpose() * solFrontal; } } @@ -613,67 +255,18 @@ void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda) { /* identify the subgraph structure */ - const SubgraphBuilder builder(parameters_.builderParams_); - Subgraph::shared_ptr subgraph = builder(gfg); + const SubgraphBuilder builder(parameters_.builderParams); + auto subgraph = builder(gfg); keyInfo_ = keyInfo; /* build factor subgraph */ - GaussianFactorGraph::shared_ptr gfg_subgraph = buildFactorSubgraph(gfg, *subgraph, true); + GaussianFactorGraph::shared_ptr gfg_subgraph = buildFactorSubgraph(gfg, subgraph, true); /* factorize and cache BayesNet */ Rc1_ = gfg_subgraph->eliminateSequential(); } /*****************************************************************************/ -Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys) { - - /* a cache of starting index and dim */ - typedef vector > Cache; - Cache cache; - - /* figure out dimension by traversing the keys */ - size_t d = 0; - for ( const Key &key: keys ) { - const KeyInfoEntry &entry = keyInfo.find(key)->second; - cache.push_back(make_pair(entry.colstart(), entry.dim())); - d += entry.dim(); - } - - /* use the cache to fill the result */ - Vector result = Vector::Zero(d, 1); - size_t idx = 0; - for ( const Cache::value_type &p: cache ) { - result.segment(idx, p.second) = src.segment(p.first, p.second) ; - idx += p.second; - } - - return result; -} - -/*****************************************************************************/ -void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst) { - /* use the cache */ - size_t idx = 0; - for ( const Key &key: keys ) { - const KeyInfoEntry &entry = keyInfo.find(key)->second; - dst.segment(entry.colstart(), entry.dim()) = src.segment(idx, entry.dim()) ; - idx += entry.dim(); - } -} - -/*****************************************************************************/ -boost::shared_ptr -buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone) { - - GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); - result->reserve(subgraph.size()); - for ( const SubgraphEdge &e: subgraph ) { - const size_t idx = e.index(); - if ( clone ) result->push_back(gfg[idx]->clone()); - else result->push_back(gfg[idx]); - } - return result; -} } // nsamespace gtsam diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index e440f32e4..8906337a9 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -17,30 +17,16 @@ #pragma once +#include #include -#include -#include #include #include #include -#include -#include -#include -#include #include -#include #include #include -#include -#include - -namespace boost { -namespace serialization { -class access; -} /* namespace serialization */ -} /* namespace boost */ namespace gtsam { @@ -49,142 +35,11 @@ namespace gtsam { class GaussianFactorGraph; class VectorValues; - struct GTSAM_EXPORT SubgraphEdge { - size_t index_; /* edge id */ - double weight_; /* edge weight */ - SubgraphEdge() : index_(0), weight_(1.0) {} - SubgraphEdge(const SubgraphEdge &e) : index_(e.index()), weight_(e.weight()) {} - SubgraphEdge(const size_t index, const double weight = 1.0): index_(index), weight_(weight) {} - inline size_t index() const { return index_; } - inline double weight() const { return weight_; } - inline bool isUnitWeight() const { return (weight_ == 1.0); } - friend std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge); - private: - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(index_); - ar & BOOST_SERIALIZATION_NVP(weight_); - } - }; - - /**************************************************************************/ - class GTSAM_EXPORT Subgraph { - public: - typedef boost::shared_ptr shared_ptr; - typedef std::vector vector_shared_ptr; - typedef std::vector Edges; - typedef std::vector EdgeIndices; - typedef Edges::iterator iterator; - typedef Edges::const_iterator const_iterator; - - protected: - Edges edges_; /* index to the factors */ - - public: - Subgraph() {} - Subgraph(const Subgraph &subgraph) : edges_(subgraph.edges()) {} - Subgraph(const Edges &edges) : edges_(edges) {} - Subgraph(const std::vector &indices) ; - - inline const Edges& edges() const { return edges_; } - inline size_t size() const { return edges_.size(); } - EdgeIndices edgeIndices() const; - - iterator begin() { return edges_.begin(); } - const_iterator begin() const { return edges_.begin(); } - iterator end() { return edges_.end(); } - const_iterator end() const { return edges_.end(); } - - void save(const std::string &fn) const; - static shared_ptr load(const std::string &fn); - friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph); - - private: - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(edges_); - } - }; - - /****************************************************************************/ - struct GTSAM_EXPORT SubgraphBuilderParameters { - public: - typedef boost::shared_ptr shared_ptr; - - enum Skeleton { - /* augmented tree */ - NATURALCHAIN = 0, /* natural ordering of the graph */ - BFS, /* breadth-first search tree */ - KRUSKAL, /* maximum weighted spanning tree */ - } skeleton_ ; - - enum SkeletonWeight { /* how to weigh the graph edges */ - EQUAL = 0, /* every block edge has equal weight */ - RHS_2NORM, /* use the 2-norm of the rhs */ - LHS_FNORM, /* use the frobenius norm of the lhs */ - RANDOM, /* bounded random edge weight */ - } skeletonWeight_ ; - - enum AugmentationWeight { /* how to weigh the graph edges */ - SKELETON = 0, /* use the same weights in building the skeleton */ -// STRETCH, /* stretch in the laplacian sense */ -// GENERALIZED_STRETCH /* the generalized stretch defined in jian2013iros */ - } augmentationWeight_ ; - - double complexity_; - - SubgraphBuilderParameters() - : skeleton_(KRUSKAL), skeletonWeight_(RANDOM), augmentationWeight_(SKELETON), complexity_(1.0) {} - virtual ~SubgraphBuilderParameters() {} - - /* for serialization */ - void print() const ; - virtual void print(std::ostream &os) const ; - friend std::ostream& operator<<(std::ostream &os, const PreconditionerParameters &p); - - static Skeleton skeletonTranslator(const std::string &s); - static std::string skeletonTranslator(Skeleton w); - static SkeletonWeight skeletonWeightTranslator(const std::string &s); - static std::string skeletonWeightTranslator(SkeletonWeight w); - static AugmentationWeight augmentationWeightTranslator(const std::string &s); - static std::string augmentationWeightTranslator(AugmentationWeight w); - }; - - /*****************************************************************************/ - class GTSAM_EXPORT SubgraphBuilder { - - public: - typedef SubgraphBuilder Base; - typedef boost::shared_ptr shared_ptr; - typedef std::vector Weights; - - SubgraphBuilder(const SubgraphBuilderParameters &p = SubgraphBuilderParameters()) - : parameters_(p) {} - virtual ~SubgraphBuilder() {} - virtual boost::shared_ptr operator() (const GaussianFactorGraph &jfg) const ; - - private: - std::vector buildTree(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &weights) const ; - std::vector unary(const GaussianFactorGraph &gfg) const ; - std::vector natural_chain(const GaussianFactorGraph &gfg) const ; - std::vector bfs(const GaussianFactorGraph &gfg) const ; - std::vector kruskal(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &w) const ; - std::vector sample(const std::vector &weights, const size_t t) const ; - Weights weights(const GaussianFactorGraph &gfg) const; - SubgraphBuilderParameters parameters_; - - }; - - /*******************************************************************************************/ struct GTSAM_EXPORT SubgraphPreconditionerParameters : public PreconditionerParameters { - typedef PreconditionerParameters Base; typedef boost::shared_ptr shared_ptr; SubgraphPreconditionerParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters()) - : Base(), builderParams_(p) {} - virtual ~SubgraphPreconditionerParameters() {} - SubgraphBuilderParameters builderParams_; + : builderParams(p) {} + SubgraphBuilderParameters builderParams; }; /** @@ -249,8 +104,8 @@ namespace gtsam { /* A zero VectorValues with the structure of xbar */ VectorValues zero() const { - VectorValues V(VectorValues::Zero(*xbar_)); - return V ; + assert(xbar_); + return VectorValues::Zero(*xbar_); } /** @@ -285,50 +140,19 @@ namespace gtsam { /*****************************************************************************/ /* implement virtual functions of Preconditioner */ - /* Computation Interfaces for Vector */ - virtual void solve(const Vector& y, Vector &x) const; - virtual void transposeSolve(const Vector& y, Vector& x) const ; + /// implement x = R^{-1} y + void solve(const Vector& y, Vector &x) const override; - virtual void build( + /// implement x = R^{-T} y + void transposeSolve(const Vector& y, Vector& x) const override; + + /// build/factorize the preconditioner + void build( const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map &lambda - ) ; + ) override; /*****************************************************************************/ }; - /* get subvectors */ - Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys); - - /* set subvectors */ - void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst); - - - /* build a factor subgraph, which is defined as a set of weighted edges (factors) */ - boost::shared_ptr - buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone); - - - /* sort the container and return permutation index with default comparator */ - template - std::vector sort_idx(const Container &src) - { - typedef typename Container::value_type T; - const size_t n = src.size() ; - std::vector > tmp; - tmp.reserve(n); - for ( size_t i = 0 ; i < n ; i++ ) - tmp.push_back(std::make_pair(i, src[i])); - - /* sort */ - std::stable_sort(tmp.begin(), tmp.end()) ; - - /* copy back */ - std::vector idx; idx.reserve(n); - for ( size_t i = 0 ; i < n ; i++ ) { - idx.push_back(tmp[i].first) ; - } - return idx; - } - } // namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 22d39a7f2..56b843e8d 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -18,153 +18,94 @@ */ #include + +#include #include #include #include #include -#include using namespace std; namespace gtsam { /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, +// Just taking system [A|b] +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab, const Parameters ¶meters, const Ordering& ordering) : - parameters_(parameters), ordering_(ordering) { - initialize(gfg); + parameters_(parameters) { + GaussianFactorGraph::shared_ptr Ab1,Ab2; + std::tie(Ab1, Ab2) = splitGraph(Ab); + if (parameters_.verbosity()) + cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() + << " factors" << endl; + + auto Rc1 = Ab1->eliminateSequential(ordering, EliminateQR); + auto xbar = boost::make_shared(Rc1->optimize()); + pc_ = boost::make_shared(Ab2, Rc1, xbar); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, - const Parameters ¶meters, const Ordering& ordering) : - parameters_(parameters), ordering_(ordering) { - initialize(*jfg); +// Taking eliminated tree [R1|c] and constraint graph [A2|b2] +SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, + const GaussianFactorGraph::shared_ptr &Ab2, + const Parameters ¶meters) + : parameters_(parameters) { + auto xbar = boost::make_shared(Rc1->optimize()); + pc_ = boost::make_shared(Ab2, Rc1, xbar); } /**************************************************************************************************/ +// Taking subgraphs [A1|b1] and [A2|b2] +// delegate up SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, - const GaussianFactorGraph &Ab2, const Parameters ¶meters, - const Ordering& ordering) : - parameters_(parameters), ordering_(ordering) { - - GaussianBayesNet::shared_ptr Rc1 = Ab1.eliminateSequential(ordering_, - EliminateQR); - initialize(Rc1, boost::make_shared(Ab2)); -} - -/**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, - const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, - const Ordering& ordering) : - parameters_(parameters), ordering_(ordering) { - - GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, - EliminateQR); - initialize(Rc1, Ab2); -} + const GaussianFactorGraph::shared_ptr &Ab2, + const Parameters ¶meters, + const Ordering &ordering) + : SubgraphSolver(Ab1.eliminateSequential(ordering, EliminateQR), Ab2, + parameters) {} /**************************************************************************************************/ +// deprecated variants +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, - const GaussianFactorGraph &Ab2, const Parameters ¶meters, - const Ordering& ordering) : - parameters_(parameters), ordering_(ordering) { - initialize(Rc1, boost::make_shared(Ab2)); -} + const GaussianFactorGraph &Ab2, + const Parameters ¶meters) + : SubgraphSolver(Rc1, boost::make_shared(Ab2), + parameters) {} + +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, + const GaussianFactorGraph &Ab2, + const Parameters ¶meters, + const Ordering &ordering) + : SubgraphSolver(Ab1, boost::make_shared(Ab2), + parameters, ordering) {} +#endif /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, - const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, - const Ordering& ordering) : - parameters_(parameters), ordering_(ordering) { - initialize(Rc1, Ab2); -} - -/**************************************************************************************************/ -VectorValues SubgraphSolver::optimize() { +VectorValues SubgraphSolver::optimize() const { VectorValues ybar = conjugateGradients(*pc_, pc_->zero(), parameters_); return pc_->x(ybar); } -/**************************************************************************************************/ -VectorValues SubgraphSolver::optimize(const VectorValues &initial) { - // the initial is ignored in this case ... - return optimize(); -} - -/**************************************************************************************************/ -void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) { - GaussianFactorGraph::shared_ptr Ab1 = - boost::make_shared(), Ab2 = boost::make_shared< - GaussianFactorGraph>(); - - boost::tie(Ab1, Ab2) = splitGraph(jfg); - if (parameters_.verbosity()) - cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() - << " factors" << endl; - - GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, - EliminateQR); - VectorValues::shared_ptr xbar = boost::make_shared( - Rc1->optimize()); - pc_ = boost::make_shared(Ab2, Rc1, xbar); -} - -/**************************************************************************************************/ -void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, - const GaussianFactorGraph::shared_ptr &Ab2) { - VectorValues::shared_ptr xbar = boost::make_shared( - Rc1->optimize()); - pc_ = boost::make_shared(Ab2, Rc1, xbar); -} - -/**************************************************************************************************/ -boost::tuple // -SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { - - const VariableIndex index(jfg); - const size_t n = index.size(); - DSFVector D(n); - - GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph()); - GaussianFactorGraph::shared_ptr Ac(new GaussianFactorGraph()); - - size_t t = 0; - for ( const GaussianFactor::shared_ptr &gf: jfg ) { - - if (gf->keys().size() > 2) { - throw runtime_error( - "SubgraphSolver::splitGraph the graph is not simple, sanity check failed "); - } - - bool augment = false; - - /* check whether this factor should be augmented to the "tree" graph */ - if (gf->keys().size() == 1) - augment = true; - else { - const Key u = gf->keys()[0], v = gf->keys()[1], u_root = D.find(u), - v_root = D.find(v); - if (u_root != v_root) { - t++; - augment = true; - D.merge(u_root, v_root); - } - } - if (augment) - At->push_back(gf); - else - Ac->push_back(gf); - } - - return boost::tie(At, Ac); -} - -/****************************************************************************/ VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, const std::map &lambda, + const KeyInfo &keyInfo, const map &lambda, const VectorValues &initial) { return VectorValues(); } +/**************************************************************************************************/ +pair // +SubgraphSolver::splitGraph(const GaussianFactorGraph &factorGraph) { + + /* identify the subgraph structure */ + const SubgraphBuilder builder(parameters_.builderParams); + auto subgraph = builder(factorGraph); + + /* build factor subgraph */ + return splitFactorGraph(factorGraph, subgraph); +} + +/****************************************************************************/ + } // \namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 318c029f3..5ab1a8520 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -20,6 +20,10 @@ #pragma once #include +#include + +#include +#include // pair namespace gtsam { @@ -28,30 +32,36 @@ class GaussianFactorGraph; class GaussianBayesNet; class SubgraphPreconditioner; -class GTSAM_EXPORT SubgraphSolverParameters: public ConjugateGradientParameters { -public: - typedef ConjugateGradientParameters Base; - SubgraphSolverParameters() : - Base() { - } - void print() const { - Base::print(); - } +struct GTSAM_EXPORT SubgraphSolverParameters + : public ConjugateGradientParameters { + SubgraphBuilderParameters builderParams; + explicit SubgraphSolverParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters()) + : builderParams(p) {} + void print() const { Base::print(); } virtual void print(std::ostream &os) const { Base::print(os); } }; /** - * This class implements the SPCG solver presented in Dellaert et al in IROS'10. + * This class implements the linear SPCG solver presented in Dellaert et al in + * IROS'10. * - * Given a linear least-squares problem \f$ f(x) = |A x - b|^2 \f$. We split the problem into - * \f$ f(x) = |A_t - b_t|^2 + |A_c - b_c|^2 \f$ where \f$ A_t \f$ denotes the "tree" part, and \f$ A_c \f$ denotes the "constraint" part. - * \f$ A_t \f$ is factorized into \f$ Q_t R_t \f$, and we compute \f$ c_t = Q_t^{-1} b_t \f$, and \f$ x_t = R_t^{-1} c_t \f$ accordingly. - * Then we solve a reparametrized problem \f$ f(y) = |y|^2 + |A_c R_t^{-1} y - \bar{b_y}|^2 \f$, where \f$ y = R_t(x - x_t) \f$, and \f$ \bar{b_y} = (b_c - A_c x_t) \f$ + * Given a linear least-squares problem \f$ f(x) = |A x - b|^2 \f$. We split the + * problem into \f$ f(x) = |A_t - b_t|^2 + |A_c - b_c|^2 \f$ where \f$ A_t \f$ + * denotes the "tree" part, and \f$ A_c \f$ denotes the "constraint" part. * - * In the matrix form, it is equivalent to solving \f$ [A_c R_t^{-1} ; I ] y = [\bar{b_y} ; 0] \f$. We can solve it - * with the least-squares variation of the conjugate gradient method. + * \f$A_t \f$ is factorized into \f$ Q_t R_t \f$, and we compute + * \f$ c_t = Q_t^{-1} b_t \f$, and \f$ x_t = R_t^{-1} c_t \f$ accordingly. + * + * Then we solve a reparametrized problem + * \f$ f(y) = |y|^2 + |A_c R_t^{-1} y -\bar{b_y}|^2 \f$, + * where \f$ y = R_t(x - x_t) \f$, and \f$ \bar{b_y} = (b_c - A_c x_t) \f$ + * + * In the matrix form, it is equivalent to solving + * \f$ [A_c R_t^{-1} ; I ] y = [\bar{b_y} ; 0] \f$. + * We can solve it with the least-squares variation of the conjugate gradient + * method. * * To use it in nonlinear optimization, please see the following example * @@ -63,72 +73,86 @@ public: * * \nosubgrouping */ -class GTSAM_EXPORT SubgraphSolver: public IterativeSolver { - -public: +class GTSAM_EXPORT SubgraphSolver : public IterativeSolver { + public: typedef SubgraphSolverParameters Parameters; -protected: + protected: Parameters parameters_; - Ordering ordering_; - boost::shared_ptr pc_; ///< preconditioner object + boost::shared_ptr pc_; ///< preconditioner object -public: - - /// Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG + public: + /// @name Constructors + /// @{ + /** + * Given a gaussian factor graph, split it into a spanning tree (A1) + others + * (A2) for SPCG Will throw exception if there are ternary factors or higher + * arity, as we use Kruskal's algorithm to split the graph, treating binary + * factors as edges. + */ SubgraphSolver(const GaussianFactorGraph &A, const Parameters ¶meters, - const Ordering& ordering); - - /// Shared pointer version - SubgraphSolver(const boost::shared_ptr &A, - const Parameters ¶meters, const Ordering& ordering); + const Ordering &ordering); /** - * The user specify the subgraph part and the constraint part - * may throw exception if A1 is underdetermined + * The user specifies the subgraph part and the constraints part. + * May throw exception if A1 is underdetermined. An ordering is required to + * eliminate Ab1. We take Ab1 as a const reference, as it will be transformed + * into Rc1, but take Ab2 as a shared pointer as we need to keep it around. + */ + SubgraphSolver(const GaussianFactorGraph &Ab1, + const boost::shared_ptr &Ab2, + const Parameters ¶meters, const Ordering &ordering); + /** + * The same as above, but we assume A1 was solved by caller. + * We take two shared pointers as we keep both around. */ - SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, - const Parameters ¶meters, const Ordering& ordering); - - /// Shared pointer version - SubgraphSolver(const boost::shared_ptr &Ab1, - const boost::shared_ptr &Ab2, - const Parameters ¶meters, const Ordering& ordering); - - /* The same as above, but the A1 is solved before */ SubgraphSolver(const boost::shared_ptr &Rc1, - const GaussianFactorGraph &Ab2, const Parameters ¶meters, - const Ordering& ordering); - - /// Shared pointer version - SubgraphSolver(const boost::shared_ptr &Rc1, - const boost::shared_ptr &Ab2, - const Parameters ¶meters, const Ordering& ordering); + const boost::shared_ptr &Ab2, + const Parameters ¶meters); /// Destructor - virtual ~SubgraphSolver() { - } + virtual ~SubgraphSolver() {} + + /// @} + /// @name Implement interface + /// @{ /// Optimize from zero - VectorValues optimize(); + VectorValues optimize() const; - /// Optimize from given initial values - VectorValues optimize(const VectorValues &initial); + /// Interface that IterativeSolver subclasses have to implement + VectorValues optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, + const std::map &lambda, + const VectorValues &initial) override; - /** Interface that IterativeSolver subclasses have to implement */ - virtual VectorValues optimize(const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, const std::map &lambda, - const VectorValues &initial); + /// @} + /// @name Implement interface + /// @{ -protected: + /// Split graph using Kruskal algorithm, treating binary factors as edges. + std::pair < boost::shared_ptr, + boost::shared_ptr > splitGraph( + const GaussianFactorGraph &gfg); - void initialize(const GaussianFactorGraph &jfg); - void initialize(const boost::shared_ptr &Rc1, - const boost::shared_ptr &Ab2); + /// @} - boost::tuple, - boost::shared_ptr > - splitGraph(const GaussianFactorGraph &gfg); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + SubgraphSolver(const boost::shared_ptr &A, + const Parameters ¶meters, const Ordering &ordering) + : SubgraphSolver(*A, parameters, ordering) {} + SubgraphSolver(const GaussianFactorGraph &, const GaussianFactorGraph &, + const Parameters &, const Ordering &); + SubgraphSolver(const boost::shared_ptr &Ab1, + const boost::shared_ptr &Ab2, + const Parameters ¶meters, const Ordering &ordering) + : SubgraphSolver(*Ab1, Ab2, parameters, ordering) {} + SubgraphSolver(const boost::shared_ptr &, + const GaussianFactorGraph &, const Parameters &); + /// @} +#endif }; -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index b037aad92..e8304e6e7 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -51,7 +51,7 @@ namespace gtsam { Key key; size_t n; boost::tie(key, n) = v; - values_.insert(make_pair(key, x.segment(j, n))); + values_.emplace(key, x.segment(j, n)); j += n; } } @@ -60,7 +60,7 @@ namespace gtsam { VectorValues::VectorValues(const Vector& x, const Scatter& scatter) { size_t j = 0; for (const SlotEntry& v : scatter) { - values_.insert(make_pair(v.key, x.segment(j, v.dimension))); + values_.emplace(v.key, x.segment(j, v.dimension)); j += v.dimension; } } @@ -70,14 +70,12 @@ namespace gtsam { { VectorValues result; for(const KeyValuePair& v: other) - result.values_.insert(make_pair(v.first, Vector::Zero(v.second.size()))); + result.values_.emplace(v.first, Vector::Zero(v.second.size())); return result; } /* ************************************************************************* */ VectorValues::iterator VectorValues::insert(const std::pair& key_value) { - // Note that here we accept a pair with a reference to the Vector, but the Vector is copied as - // it is inserted into the values_ map. std::pair result = values_.insert(key_value); if(!result.second) throw std::invalid_argument( @@ -86,6 +84,16 @@ namespace gtsam { return result.first; } + /* ************************************************************************* */ + VectorValues::iterator VectorValues::emplace(Key j, const Vector& value) { + std::pair result = values_.emplace(j, value); + if(!result.second) + throw std::invalid_argument( + "Requested to emplace variable '" + DefaultKeyFormatter(j) + + "' already in this VectorValues."); + return result.first; + } + /* ************************************************************************* */ void VectorValues::update(const VectorValues& values) { @@ -132,8 +140,7 @@ namespace gtsam { bool VectorValues::equals(const VectorValues& x, double tol) const { if(this->size() != x.size()) return false; - typedef boost::tuple ValuePair; - for(const ValuePair& values: boost::combine(*this, x)) { + for(const auto& values: boost::combine(*this, x)) { if(values.get<0>().first != values.get<1>().first || !equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol)) return false; @@ -142,17 +149,15 @@ namespace gtsam { } /* ************************************************************************* */ - Vector VectorValues::vector() const - { + Vector VectorValues::vector() const { // Count dimensions DenseIndex totalDim = 0; - for(const Vector& v: *this | map_values) - totalDim += v.size(); + for (const Vector& v : *this | map_values) totalDim += v.size(); // Copy vectors Vector result(totalDim); DenseIndex pos = 0; - for(const Vector& v: *this | map_values) { + for (const Vector& v : *this | map_values) { result.segment(pos, v.size()) = v; pos += v.size(); } @@ -242,7 +247,7 @@ namespace gtsam { VectorValues result; // The result.end() hint here should result in constant-time inserts for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) - result.values_.insert(result.end(), make_pair(j1->first, j1->second + j2->second)); + result.values_.emplace(j1->first, j1->second + j2->second); return result; } @@ -300,7 +305,7 @@ namespace gtsam { VectorValues result; // The result.end() hint here should result in constant-time inserts for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) - result.values_.insert(result.end(), make_pair(j1->first, j1->second - j2->second)); + result.values_.emplace(j1->first, j1->second - j2->second); return result; } @@ -316,7 +321,7 @@ namespace gtsam { { VectorValues result; for(const VectorValues::KeyValuePair& key_v: v) - result.values_.insert(result.values_.end(), make_pair(key_v.first, a * key_v.second)); + result.values_.emplace(key_v.first, a * key_v.second); return result; } diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 39abe1b56..b1d9de585 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -50,9 +50,9 @@ namespace gtsam { * Example: * \code VectorValues values; - values.insert(3, Vector3(1.0, 2.0, 3.0)); - values.insert(4, Vector2(4.0, 5.0)); - values.insert(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0).finished()); + values.emplace(3, Vector3(1.0, 2.0, 3.0)); + values.emplace(4, Vector2(4.0, 5.0)); + values.emplace(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0).finished()); // Prints [ 3.0 4.0 ] gtsam::print(values[1]); @@ -64,18 +64,7 @@ namespace gtsam { * *

Advanced Interface and Performance Information

* - * For advanced usage, or where speed is important: - * - Allocate space ahead of time using a pre-allocating constructor - * (\ref AdvancedConstructors "Advanced Constructors"), Zero(), - * SameStructure(), resize(), or append(). Do not use - * insert(Key, const Vector&), which always has to re-allocate the - * internal vector. - * - The vector() function permits access to the underlying Vector, for - * doing mathematical or other operations that require all values. - * - operator[]() returns a SubVector view of the underlying Vector, - * without copying any data. - * - * Access is through the variable index j, and returns a SubVector, + * Access is through the variable Key j, and returns a SubVector, * which is a view on the underlying data structure. * * This class is additionally used in gradient descent and dog leg to store the gradient. @@ -140,7 +129,7 @@ namespace gtsam { /** Check whether a variable with key \c j exists. */ bool exists(Key j) const { return find(j) != end(); } - /** + /** * Read/write access to the vector value with key \c j, throws * std::out_of_range if \c j does not exist, identical to operator[](Key). */ @@ -153,7 +142,7 @@ namespace gtsam { return item->second; } - /** + /** * Access the vector value with key \c j (const version), throws * std::out_of_range if \c j does not exist, identical to operator[](Key). */ @@ -183,15 +172,21 @@ namespace gtsam { * j is already used. * @param value The vector to be inserted. * @param j The index with which the value will be associated. */ - iterator insert(Key j, const Vector& value) { - return insert(std::make_pair(j, value)); - } + iterator insert(const std::pair& key_value); + + /** Emplace a vector \c value with key \c j. Throws an invalid_argument exception if the key \c + * j is already used. + * @param value The vector to be inserted. + * @param j The index with which the value will be associated. */ + iterator emplace(Key j, const Vector& value); /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c * j is already used. * @param value The vector to be inserted. * @param j The index with which the value will be associated. */ - iterator insert(const std::pair& key_value); + iterator insert(Key j, const Vector& value) { + return insert(std::make_pair(j, value)); + } /** Insert all values from \c values. Throws an invalid_argument exception if any keys to be * inserted are already used. */ @@ -202,7 +197,8 @@ namespace gtsam { * exist, it is inserted and an iterator pointing to the new element, along with 'true', is * returned. */ std::pair tryInsert(Key j, const Vector& value) { - return values_.insert(std::make_pair(j, value)); } + return values_.emplace(j, value); + } /** Erase the vector with the given key, or throw std::out_of_range if it does not exist */ void erase(Key var) { @@ -220,15 +216,15 @@ namespace gtsam { iterator end() { return values_.end(); } ///< Iterator over variables const_iterator end() const { return values_.end(); } ///< Iterator over variables - /** + /** * Return the iterator corresponding to the requested key, or end() if no - * variable is present with this key. + * variable is present with this key. */ iterator find(Key j) { return values_.find(j); } - /** + /** * Return the iterator corresponding to the requested key, or end() if no - * variable is present with this key. + * variable is present with this key. */ const_iterator find(Key j) const { return values_.find(j); } diff --git a/gtsam/linear/iterative-inl.h b/gtsam/linear/iterative-inl.h index e61b67f73..58ef7d733 100644 --- a/gtsam/linear/iterative-inl.h +++ b/gtsam/linear/iterative-inl.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/iterative.cpp b/gtsam/linear/iterative.cpp index 07d4de865..08d1fb5b9 100644 --- a/gtsam/linear/iterative.cpp +++ b/gtsam/linear/iterative.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h index ac8c2d7c7..1afaf79d1 100644 --- a/gtsam/linear/linearAlgorithms-inst.h +++ b/gtsam/linear/linearAlgorithms-inst.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -56,7 +56,7 @@ namespace gtsam myData.parentData = parentData; // Take any ancestor results we'll need for(Key parent: clique->conditional_->parents()) - myData.cliqueResults.insert(std::make_pair(parent, myData.parentData->cliqueResults.at(parent))); + myData.cliqueResults.emplace(parent, myData.parentData->cliqueResults.at(parent)); // Solve and store in our results { @@ -87,10 +87,10 @@ namespace gtsam // This is because Eigen (as of 3.3) no longer evaluates S * xS into // a temporary, and the operation trashes valus in xS. // See: http://eigen.tuxfamily.org/index.php?title=3.3 - const Vector rhs = c.getb() - c.get_S() * xS; + const Vector rhs = c.getb() - c.S() * xS; // TODO(gareth): Inline instantiation of Eigen::Solve and check flag - const Vector solution = c.get_R().triangularView().solve(rhs); + const Vector solution = c.R().triangularView().solve(rhs); // Check for indeterminant solution if(solution.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front()); @@ -99,8 +99,8 @@ namespace gtsam DenseIndex vectorPosition = 0; for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) { VectorValues::const_iterator r = - collectedResult.insert(*frontal, solution.segment(vectorPosition, c.getDim(frontal))); - myData.cliqueResults.insert(make_pair(r->first, r)); + collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal))); + myData.cliqueResults.emplace(r->first, r); vectorPosition += c.getDim(frontal); } } diff --git a/gtsam/linear/tests/testErrors.cpp b/gtsam/linear/tests/testErrors.cpp index 9c8eb468d..74eef9a2c 100644 --- a/gtsam/linear/tests/testErrors.cpp +++ b/gtsam/linear/tests/testErrors.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index 13601844c..488368c72 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -21,6 +21,8 @@ #include #include +#include +#include #include #include // for operator += using namespace boost::assign; @@ -28,13 +30,11 @@ using namespace boost::assign; // STL/C++ #include #include -#include -#include using namespace std; using namespace gtsam; -static const Key _x_=0, _y_=1; +static const Key _x_ = 11, _y_ = 22, _z_ = 33; static GaussianBayesNet smallBayesNet = list_of(GaussianConditional(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1))( @@ -42,9 +42,9 @@ static GaussianBayesNet smallBayesNet = static GaussianBayesNet noisyBayesNet = list_of(GaussianConditional(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1, - noiseModel::Diagonal::Sigmas(Vector1::Constant(2))))( + noiseModel::Isotropic::Sigma(1, 2.0)))( GaussianConditional(_y_, Vector1::Constant(5), I_1x1, - noiseModel::Diagonal::Sigmas(Vector1::Constant(3)))); + noiseModel::Isotropic::Sigma(1, 3.0))); /* ************************************************************************* */ TEST( GaussianBayesNet, Matrix ) @@ -120,7 +120,7 @@ TEST( GaussianBayesNet, optimizeIncomplete ) TEST( GaussianBayesNet, optimize3 ) { // y = R*x, x=inv(R)*y - // 4 = 1 1 -1 + // 4 = 1 1 -1 // 5 1 5 // NOTE: we are supplying a new RHS here @@ -136,6 +136,37 @@ TEST( GaussianBayesNet, optimize3 ) EXPECT(assert_equal(expected, actual)); } +/* ************************************************************************* */ +TEST(GaussianBayesNet, ordering) +{ + Ordering expected; + expected += _x_, _y_; + const auto actual = noisyBayesNet.ordering(); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST( GaussianBayesNet, MatrixStress ) +{ + GaussianBayesNet bn; + using GC = GaussianConditional; + bn.emplace_shared(_x_, Vector2(1, 2), 1 * I_2x2, _y_, 2 * I_2x2, _z_, 3 * I_2x2); + bn.emplace_shared(_y_, Vector2(3, 4), 4 * I_2x2, _z_, 5 * I_2x2); + bn.emplace_shared(_z_, Vector2(5, 6), 6 * I_2x2); + + const VectorValues expected = bn.optimize(); + for (const auto keys : + {KeyVector({_x_, _y_, _z_}), KeyVector({_x_, _z_, _y_}), + KeyVector({_y_, _x_, _z_}), KeyVector({_y_, _z_, _x_}), + KeyVector({_z_, _x_, _y_}), KeyVector({_z_, _y_, _x_})}) { + const Ordering ordering(keys); + Matrix R; + Vector d; + boost::tie(R, d) = bn.matrix(ordering); + EXPECT(assert_equal(expected.vector(ordering), R.inverse() * d)); + } +} + /* ************************************************************************* */ TEST( GaussianBayesNet, backSubstituteTranspose ) { @@ -152,6 +183,34 @@ TEST( GaussianBayesNet, backSubstituteTranspose ) VectorValues actual = smallBayesNet.backSubstituteTranspose(x); EXPECT(assert_equal(expected, actual)); + + const auto ordering = noisyBayesNet.ordering(); + const Matrix R = smallBayesNet.matrix(ordering).first; + const Vector expected_vector = R.transpose().inverse() * x.vector(ordering); + EXPECT(assert_equal(expected_vector, actual.vector(ordering))); +} + +/* ************************************************************************* */ +TEST( GaussianBayesNet, backSubstituteTransposeNoisy ) +{ + // x=R'*y, expected=inv(R')*x + // 2 = 1 2 + // 5 1 1 3 + VectorValues + x = map_list_of + (_x_, Vector1::Constant(2)) + (_y_, Vector1::Constant(5)), + expected = map_list_of + (_x_, Vector1::Constant(4)) + (_y_, Vector1::Constant(9)); + + VectorValues actual = noisyBayesNet.backSubstituteTranspose(x); + EXPECT(assert_equal(expected, actual)); + + const auto ordering = noisyBayesNet.ordering(); + const Matrix R = noisyBayesNet.matrix(ordering).first; + const Vector expected_vector = R.transpose().inverse() * x.vector(ordering); + EXPECT(assert_equal(expected_vector, actual.vector(ordering))); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index 60387694e..fae00e1e4 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -71,33 +71,33 @@ TEST(GaussianConditional, constructor) GaussianConditional::const_iterator it = actual.beginFrontals(); EXPECT(assert_equal(Key(1), *it)); - EXPECT(assert_equal(R, actual.get_R())); + EXPECT(assert_equal(R, actual.R())); ++ it; EXPECT(it == actual.endFrontals()); it = actual.beginParents(); EXPECT(assert_equal(Key(3), *it)); - EXPECT(assert_equal(S1, actual.get_S(it))); + EXPECT(assert_equal(S1, actual.S(it))); ++ it; EXPECT(assert_equal(Key(5), *it)); - EXPECT(assert_equal(S2, actual.get_S(it))); + EXPECT(assert_equal(S2, actual.S(it))); ++ it; EXPECT(assert_equal(Key(7), *it)); - EXPECT(assert_equal(S3, actual.get_S(it))); + EXPECT(assert_equal(S3, actual.S(it))); ++it; EXPECT(it == actual.endParents()); - EXPECT(assert_equal(d, actual.get_d())); + EXPECT(assert_equal(d, actual.d())); EXPECT(assert_equal(*s, *actual.get_model())); // test copy constructor GaussianConditional copied(actual); - EXPECT(assert_equal(d, copied.get_d())); + EXPECT(assert_equal(d, copied.d())); EXPECT(assert_equal(*s, *copied.get_model())); - EXPECT(assert_equal(R, copied.get_R())); + EXPECT(assert_equal(R, copied.R())); } /* ************************************************************************* */ @@ -212,7 +212,7 @@ TEST( GaussianConditional, solve_multifrontal ) // 3 variables, all dim=2 GaussianConditional cg(list_of(1)(2)(10), 2, blockMatrix); - EXPECT(assert_equal(Vector(blockMatrix.full().rightCols(1)), cg.get_d())); + EXPECT(assert_equal(Vector(blockMatrix.full().rightCols(1)), cg.d())); // partial solution Vector sl1 = Vector2(9.0, 10.0); diff --git a/gtsam/linear/tests/testGaussianDensity.cpp b/gtsam/linear/tests/testGaussianDensity.cpp index 081b1d3d1..29dc49591 100644 --- a/gtsam/linear/tests/testGaussianDensity.cpp +++ b/gtsam/linear/tests/testGaussianDensity.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -33,7 +33,7 @@ TEST(GaussianDensity, constructor) GaussianConditional conditional(1, d, R, noiseModel::Diagonal::Sigmas(s)); GaussianDensity copied(conditional); - EXPECT(assert_equal(d, copied.get_d())); + EXPECT(assert_equal(d, copied.d())); EXPECT(assert_equal(s, copied.get_model()->sigmas())); } diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 2ea1b15bd..110a1223a 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -146,7 +146,7 @@ TEST(JacobianFactor, constructors_and_accessors) /* ************************************************************************* */ TEST(JabobianFactor, Hessian_conversion) { - HessianFactor hessian(0, (Matrix(4,4) << + HessianFactor hessian(0, (Matrix(4, 4) << 1.57, 2.695, -1.1, -2.35, 2.695, 11.3125, -0.65, -10.225, -1.1, -0.65, 1, 0.5, @@ -154,7 +154,7 @@ TEST(JabobianFactor, Hessian_conversion) { (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(), 73.1725); - JacobianFactor expected(0, (Matrix(2,4) << + JacobianFactor expected(0, (Matrix(2, 4) << 1.2530, 2.1508, -0.8779, -1.8755, 0, 2.5858, 0.4789, -2.3943).finished(), Vector2(-6.2929, -5.7941)); @@ -162,6 +162,27 @@ TEST(JabobianFactor, Hessian_conversion) { EXPECT(assert_equal(expected, JacobianFactor(hessian), 1e-3)); } +/* ************************************************************************* */ +TEST(JabobianFactor, Hessian_conversion2) { + JacobianFactor jf(0, (Matrix(3, 3) << + 1, 2, 3, + 0, 2, 3, + 0, 0, 3).finished(), + Vector3(1, 2, 2)); + HessianFactor hessian(jf); + EXPECT(assert_equal(jf, JacobianFactor(hessian), 1e-9)); +} + +/* ************************************************************************* */ +TEST(JabobianFactor, Hessian_conversion3) { + JacobianFactor jf(0, (Matrix(2, 4) << + 1, 2, 3, 0, + 0, 3, 2, 1).finished(), + Vector2(1, 2)); + HessianFactor hessian(jf); + EXPECT(assert_equal(jf, JacobianFactor(hessian), 1e-9)); +} + /* ************************************************************************* */ namespace simple_graph { @@ -322,27 +343,30 @@ TEST(JacobianFactor, matrices) /* ************************************************************************* */ TEST(JacobianFactor, operators ) { - SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); + const double sigma = 0.1; + SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2, sigma); Matrix I = I_2x2; Vector b = Vector2(0.2,-0.1); JacobianFactor lf(1, -I, 2, I, b, sigma0_1); - VectorValues c; - c.insert(1, Vector2(10.,20.)); - c.insert(2, Vector2(30.,60.)); + VectorValues x; + Vector2 x1(10,20), x2(30,60); + x.insert(1, x1); + x.insert(2, x2); // test A*x - Vector expectedE = Vector2(200.,400.); - Vector actualE = lf * c; + Vector expectedE = (x2 - x1)/sigma; + Vector actualE = lf * x; EXPECT(assert_equal(expectedE, actualE)); // test A^e VectorValues expectedX; - expectedX.insert(1, Vector2(-2000.,-4000.)); - expectedX.insert(2, Vector2(2000., 4000.)); + const double alpha = 0.5; + expectedX.insert(1, - alpha * expectedE /sigma); + expectedX.insert(2, alpha * expectedE /sigma); VectorValues actualX = VectorValues::Zero(expectedX); - lf.transposeMultiplyAdd(1.0, actualE, actualX); + lf.transposeMultiplyAdd(alpha, actualE, actualX); EXPECT(assert_equal(expectedX, actualX)); // test gradient at zero diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index c4da59496..5e40aa4a7 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp index 20fe3ac40..c5b3dab37 100644 --- a/gtsam/linear/tests/testSerializationLinear.cpp +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -11,7 +11,7 @@ /** * @file testSerializationLinear.cpp - * @brief + * @brief * @author Richard Roberts * @date Feb 7, 2012 */ diff --git a/gtsam/navigation/AttitudeFactor.cpp b/gtsam/navigation/AttitudeFactor.cpp index 089747dc6..3c6af9332 100644 --- a/gtsam/navigation/AttitudeFactor.cpp +++ b/gtsam/navigation/AttitudeFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 23cd5c477..19c1f8139 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index cc1dc087e..54d4ce0ed 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -52,8 +52,13 @@ void PreintegratedImuMeasurements::resetIntegration() { //------------------------------------------------------------------------------ void PreintegratedImuMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { + if (dt <= 0) { + throw std::runtime_error( + "PreintegratedImuMeasurements::integrateMeasurement: dt <=0"); + } + // Update preintegrated measurements (also get Jacobian) - Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix93 B, C; PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C); diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index e0792f873..9926d207a 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -151,7 +151,7 @@ class GTSAM_EXPORT PreintegrationBase { /// @name Main functionality /// @{ - /** + /** * Subtract estimate and correct for sensor pose * Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc) * Ignore D_correctedOmega_measuredAcc as it is trivially zero @@ -190,8 +190,8 @@ class GTSAM_EXPORT PreintegrationBase { OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2, OptionalJacobian<9, 6> H3) const; - /** - * Compute errors w.r.t. preintegrated measurements and jacobians + /** + * Compute errors w.r.t. preintegrated measurements and jacobians * wrt pose_i, vel_i, bias_i, pose_j, bias_j */ Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, diff --git a/gtsam/navigation/ScenarioRunner.cpp b/gtsam/navigation/ScenarioRunner.cpp index 79f3f42cc..fcba92f60 100644 --- a/gtsam/navigation/ScenarioRunner.cpp +++ b/gtsam/navigation/ScenarioRunner.cpp @@ -49,7 +49,7 @@ PreintegratedImuMeasurements ScenarioRunner::integrate( NavState ScenarioRunner::predict(const PreintegratedImuMeasurements& pim, const Bias& estimatedBias) const { - const NavState state_i(scenario_->pose(0), scenario_->velocity_n(0)); + const NavState state_i(scenario_.pose(0), scenario_.velocity_n(0)); return pim.predict(state_i, estimatedBias); } diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 537785e06..aa544542d 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -42,7 +42,7 @@ class ScenarioRunner { typedef boost::shared_ptr SharedParams; private: - const Scenario* scenario_; + const Scenario& scenario_; const SharedParams p_; const double imuSampleTime_, sqrt_dt_; const Bias estimatedBias_; @@ -51,7 +51,7 @@ class ScenarioRunner { mutable Sampler gyroSampler_, accSampler_; public: - ScenarioRunner(const Scenario* scenario, const SharedParams& p, + ScenarioRunner(const Scenario& scenario, const SharedParams& p, double imuSampleTime = 1.0 / 100.0, const Bias& bias = Bias()) : scenario_(scenario), p_(p), @@ -68,13 +68,13 @@ class ScenarioRunner { // A gyro simply measures angular velocity in body frame Vector3 actualAngularVelocity(double t) const { - return scenario_->omega_b(t); + return scenario_.omega_b(t); } // An accelerometer measures acceleration in body, but not gravity Vector3 actualSpecificForce(double t) const { - Rot3 bRn(scenario_->rotation(t).transpose()); - return scenario_->acceleration_b(t) - bRn * gravity_n(); + Rot3 bRn(scenario_.rotation(t).transpose()); + return scenario_.acceleration_b(t) - bRn * gravity_n(); } // versions corrupted by bias and noise @@ -104,6 +104,15 @@ class ScenarioRunner { /// Estimate covariance of sampled noise for sanity-check Matrix6 estimateNoiseCovariance(size_t N = 1000) const; + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + ScenarioRunner(const Scenario* scenario, const SharedParams& p, + double imuSampleTime = 1.0 / 100.0, const Bias& bias = Bias()) + : ScenarioRunner(*scenario, p, imuSampleTime, bias) {} + /// @} +#endif }; } // namespace gtsam diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h index 2c0811ae6..11945e53a 100644 --- a/gtsam/navigation/TangentPreintegration.h +++ b/gtsam/navigation/TangentPreintegration.h @@ -139,7 +139,7 @@ private: ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size())); ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size())); } - + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/gtsam/navigation/tests/testScenarios.cpp b/gtsam/navigation/tests/testScenarios.cpp index 5097e9e66..e790c45ba 100644 --- a/gtsam/navigation/tests/testScenarios.cpp +++ b/gtsam/navigation/tests/testScenarios.cpp @@ -52,7 +52,7 @@ TEST(ScenarioRunner, Spin) { const ConstantTwistScenario scenario(W, V); auto p = defaultParams(); - ScenarioRunner runner(&scenario, p, kDt); + ScenarioRunner runner(scenario, p, kDt); const double T = 2 * kDt; // seconds auto pim = runner.integrate(T); @@ -80,7 +80,7 @@ ConstantTwistScenario scenario(Z_3x1, Vector3(v, 0, 0)); /* ************************************************************************* */ TEST(ScenarioRunner, Forward) { using namespace forward; - ScenarioRunner runner(&scenario, defaultParams(), kDt); + ScenarioRunner runner(scenario, defaultParams(), kDt); const double T = 0.1; // seconds auto pim = runner.integrate(T); @@ -94,7 +94,7 @@ TEST(ScenarioRunner, Forward) { /* ************************************************************************* */ TEST(ScenarioRunner, ForwardWithBias) { using namespace forward; - ScenarioRunner runner(&scenario, defaultParams(), kDt); + ScenarioRunner runner(scenario, defaultParams(), kDt); const double T = 0.1; // seconds auto pim = runner.integrate(T, kNonZeroBias); @@ -108,7 +108,7 @@ TEST(ScenarioRunner, Circle) { const double v = 2, w = 6 * kDegree; ConstantTwistScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0)); - ScenarioRunner runner(&scenario, defaultParams(), kDt); + ScenarioRunner runner(scenario, defaultParams(), kDt); const double T = 0.1; // seconds auto pim = runner.integrate(T); @@ -126,7 +126,7 @@ TEST(ScenarioRunner, Loop) { const double v = 2, w = 6 * kDegree; ConstantTwistScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0)); - ScenarioRunner runner(&scenario, defaultParams(), kDt); + ScenarioRunner runner(scenario, defaultParams(), kDt); const double T = 0.1; // seconds auto pim = runner.integrate(T); @@ -157,7 +157,7 @@ const double T = 3; // seconds /* ************************************************************************* */ TEST(ScenarioRunner, Accelerating) { using namespace accelerating; - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); @@ -170,7 +170,7 @@ TEST(ScenarioRunner, Accelerating) { /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingWithBias) { using namespace accelerating; - ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); + ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias); Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); @@ -185,7 +185,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating) { const AcceleratingScenario scenario(nRb, P0, V0, A, W); const double T = 10; // seconds - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); @@ -216,7 +216,7 @@ const double T = 3; // seconds /* ************************************************************************* */ TEST(ScenarioRunner, Accelerating2) { using namespace accelerating2; - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); @@ -229,7 +229,7 @@ TEST(ScenarioRunner, Accelerating2) { /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingWithBias2) { using namespace accelerating2; - ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); + ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias); Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); @@ -244,7 +244,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating2) { const AcceleratingScenario scenario(nRb, P0, V0, A, W); const double T = 10; // seconds - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); @@ -276,7 +276,7 @@ const double T = 3; // seconds /* ************************************************************************* */ TEST(ScenarioRunner, Accelerating3) { using namespace accelerating3; - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); @@ -289,7 +289,7 @@ TEST(ScenarioRunner, Accelerating3) { /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingWithBias3) { using namespace accelerating3; - ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); + ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias); Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); @@ -304,7 +304,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating3) { const AcceleratingScenario scenario(nRb, P0, V0, A, W); const double T = 10; // seconds - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); @@ -337,7 +337,7 @@ const double T = 3; // seconds /* ************************************************************************* */ TEST(ScenarioRunner, Accelerating4) { using namespace accelerating4; - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); @@ -350,7 +350,7 @@ TEST(ScenarioRunner, Accelerating4) { /* ************************************************************************* */ TEST(ScenarioRunner, AcceleratingWithBias4) { using namespace accelerating4; - ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias); + ScenarioRunner runner(scenario, defaultParams(), T / 10, kNonZeroBias); auto pim = runner.integrate(T, kNonZeroBias); Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias); @@ -365,7 +365,7 @@ TEST(ScenarioRunner, AcceleratingAndRotating4) { const AcceleratingScenario scenario(nRb, P0, V0, A, W); const double T = 10; // seconds - ScenarioRunner runner(&scenario, defaultParams(), T / 10); + ScenarioRunner runner(scenario, defaultParams(), T / 10); auto pim = runner.integrate(T); EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9)); diff --git a/gtsam/nonlinear/DoglegOptimizer.cpp b/gtsam/nonlinear/DoglegOptimizer.cpp index 97326f06c..654c1ad33 100644 --- a/gtsam/nonlinear/DoglegOptimizer.cpp +++ b/gtsam/nonlinear/DoglegOptimizer.cpp @@ -11,7 +11,7 @@ /** * @file DoglegOptimizer.cpp - * @brief + * @brief * @author Richard Roberts * @date Feb 26, 2012 */ diff --git a/gtsam/nonlinear/DoglegOptimizer.h b/gtsam/nonlinear/DoglegOptimizer.h index 7013908e5..1fa25fd60 100644 --- a/gtsam/nonlinear/DoglegOptimizer.h +++ b/gtsam/nonlinear/DoglegOptimizer.h @@ -37,11 +37,11 @@ public: VERBOSE }; - double deltaInitial; ///< The initial trust region radius (default: 1.0) + double deltaInitial; ///< The initial trust region radius (default: 10.0) VerbosityDL verbosityDL; ///< The verbosity level for Dogleg (default: SILENT), see also NonlinearOptimizerParams::verbosity DoglegParams() : - deltaInitial(1.0), verbosityDL(SILENT) {} + deltaInitial(10.0), verbosityDL(SILENT) {} virtual ~DoglegParams() {} @@ -105,9 +105,9 @@ public: /** Virtual destructor */ virtual ~DoglegOptimizer() {} - /** Perform a single iteration, returning a new NonlinearOptimizer class - * containing the updated variable assignments, which may be retrieved with - * values(). + /** + * Perform a single iteration, returning GaussianFactorGraph corresponding to + * the linearized factor graph. */ GaussianFactorGraph::shared_ptr iterate() override; diff --git a/gtsam/nonlinear/ExpressionFactorGraph.h b/gtsam/nonlinear/ExpressionFactorGraph.h index 665f887e2..6c09066c2 100644 --- a/gtsam/nonlinear/ExpressionFactorGraph.h +++ b/gtsam/nonlinear/ExpressionFactorGraph.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.h b/gtsam/nonlinear/GaussNewtonOptimizer.h index 3e75fc23d..a436597e5 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.h +++ b/gtsam/nonlinear/GaussNewtonOptimizer.h @@ -11,7 +11,7 @@ /** * @file GaussNewtonOptimizer.h - * @brief + * @brief * @author Richard Roberts * @date Feb 26, 2012 */ @@ -70,9 +70,9 @@ public: /** Virtual destructor */ virtual ~GaussNewtonOptimizer() {} - /** Perform a single iteration, returning a new NonlinearOptimizer class - * containing the updated variable assignments, which may be retrieved with - * values(). + /** + * Perform a single iteration, returning GaussianFactorGraph corresponding to + * the linearized factor graph. */ GaussianFactorGraph::shared_ptr iterate() override; diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 2cedeea9f..11be14c44 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -246,8 +246,8 @@ void updateRgProd(const ISAM2::sharedClique& clique, const KeySet& replacedKeys, clique->conditional()->endParents())); // Compute R*g and S*g for this clique - Vector RSgProd = clique->conditional()->get_R() * gR + - clique->conditional()->get_S() * gS; + Vector RSgProd = clique->conditional()->R() * gR + + clique->conditional()->S() * gS; // Write into RgProd vector DenseIndex vectorPosition = 0; diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 32a24b51d..c0b2d0757 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -96,7 +96,7 @@ bool ISAM2::equals(const ISAM2& other, double tol) const { } /* ************************************************************************* */ -KeySet ISAM2::getAffectedFactors(const KeyList& keys) const { +FactorIndexSet ISAM2::getAffectedFactors(const KeyList& keys) const { static const bool debug = false; if (debug) cout << "Getting affected factors for "; if (debug) { @@ -106,15 +106,14 @@ KeySet ISAM2::getAffectedFactors(const KeyList& keys) const { } if (debug) cout << endl; - NonlinearFactorGraph allAffected; - KeySet indices; + FactorIndexSet indices; for (const Key key : keys) { const VariableIndex::Factors& factors(variableIndex_[key]); indices.insert(factors.begin(), factors.end()); } if (debug) cout << "Affected factors are: "; if (debug) { - for (const size_t index : indices) { + for (const auto index : indices) { cout << index << " "; } } @@ -132,8 +131,6 @@ GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors( KeySet candidates = getAffectedFactors(affectedKeys); gttoc(getAffectedFactors); - NonlinearFactorGraph nonlinearAffectedFactors; - gttic(affectedKeysSet); // for fast lookup below KeySet affectedKeysSet; @@ -589,7 +586,7 @@ ISAM2Result ISAM2::update( // Remove the removed factors NonlinearFactorGraph removeFactors; removeFactors.reserve(removeFactorIndices.size()); - for (size_t index : removeFactorIndices) { + for (const auto index : removeFactorIndices) { removeFactors.push_back(nonlinearFactors_[index]); nonlinearFactors_.remove(index); if (params_.cacheLinearizedFactors) linearFactors_.remove(index); @@ -823,7 +820,7 @@ void ISAM2::marginalizeLeaves( KeySet leafKeysRemoved; // Keep track of factors that get summarized by removing cliques - KeySet factorIndicesToRemove; + FactorIndexSet factorIndicesToRemove; // Remove the subtree and throw away the cliques auto trackingRemoveSubtree = [&](const sharedClique& subtreeRoot) { @@ -937,8 +934,8 @@ void ISAM2::marginalizeLeaves( } } // Create factor graph from factor indices - for (size_t i : factorsFromMarginalizedInClique_step1) { - graph.push_back(nonlinearFactors_[i]->linearize(theta_)); + for (const auto index: factorsFromMarginalizedInClique_step1) { + graph.push_back(nonlinearFactors_[index]->linearize(theta_)); } // Reeliminate the linear graph to get the marginal and discard the @@ -1011,10 +1008,10 @@ void ISAM2::marginalizeLeaves( // Remove the factors to remove that have been summarized in the newly-added // marginal factors NonlinearFactorGraph removedFactors; - for (size_t i : factorIndicesToRemove) { - removedFactors.push_back(nonlinearFactors_[i]); - nonlinearFactors_.remove(i); - if (params_.cacheLinearizedFactors) linearFactors_.remove(i); + for (const auto index: factorIndicesToRemove) { + removedFactors.push_back(nonlinearFactors_[index]); + nonlinearFactors_.remove(index); + if (params_.cacheLinearizedFactors) linearFactors_.remove(index); } variableIndex_.remove(factorIndicesToRemove.begin(), factorIndicesToRemove.end(), removedFactors); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 04bd3d3eb..4b8b1398b 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -274,7 +274,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { * @param newTheta Initial values for new variables * @param theta Current solution to be augmented with new initialization * @param delta Current linear delta to be augmented with zeros - * @param deltaNewton + * @param deltaNewton * @param RgProd * @param keyFormatter Formatter for printing nonlinear keys during debugging */ @@ -292,7 +292,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { */ void expmapMasked(const KeySet& mask); - FastSet getAffectedFactors(const FastList& keys) const; + FactorIndexSet getAffectedFactors(const FastList& keys) const; GaussianFactorGraph::shared_ptr relinearizeAffectedFactors( const FastList& affectedKeys, const KeySet& relinKeys) const; GaussianFactorGraph getCachedBoundaryFactors(const Cliques& orphans); diff --git a/gtsam/nonlinear/ISAM2Clique.cpp b/gtsam/nonlinear/ISAM2Clique.cpp index 48295f338..96212c923 100644 --- a/gtsam/nonlinear/ISAM2Clique.cpp +++ b/gtsam/nonlinear/ISAM2Clique.cpp @@ -37,9 +37,9 @@ void ISAM2Clique::setEliminationResult( gradientContribution_.resize(conditional_->cols() - 1); // Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed // reasons - gradientContribution_ << -conditional_->get_R().transpose() * - conditional_->get_d(), - -conditional_->get_S().transpose() * conditional_->get_d(); + gradientContribution_ << -conditional_->R().transpose() * + conditional_->d(), + -conditional_->S().transpose() * conditional_->d(); } /* ************************************************************************* */ @@ -141,8 +141,8 @@ void ISAM2Clique::fastBackSubstitute(VectorValues* delta) const { // This is because Eigen (as of 3.3) no longer evaluates S * xS into // a temporary, and the operation trashes valus in xS. // See: http://eigen.tuxfamily.org/index.php?title=3.3 - const Vector rhs = c.getb() - c.get_S() * xS; - const Vector solution = c.get_R().triangularView().solve(rhs); + const Vector rhs = c.getb() - c.S() * xS; + const Vector solution = c.R().triangularView().solve(rhs); // Check for indeterminant solution if (solution.hasNaN()) @@ -284,7 +284,7 @@ size_t optimizeWildfireNonRecursive(const ISAM2Clique::shared_ptr& root, /* ************************************************************************* */ void ISAM2Clique::nnz_internal(size_t* result) const { size_t dimR = conditional_->rows(); - size_t dimSep = conditional_->get_S().cols(); + size_t dimSep = conditional_->S().cols(); *result += ((dimR + 1) * dimR) / 2 + dimSep * dimR; // traverse the children for (const auto& child : children) { diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h index 1539d90c4..763a5e198 100644 --- a/gtsam/nonlinear/ISAM2Result.h +++ b/gtsam/nonlinear/ISAM2Result.h @@ -31,8 +31,6 @@ namespace gtsam { -typedef FastVector FactorIndices; - /** * @addtogroup ISAM2 * This struct is returned from ISAM2::update() and contains information about diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index f1135d2f0..2f09f234d 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -94,9 +94,9 @@ public: /// @name Advanced interface /// @{ - /** Perform a single iteration, returning a new NonlinearOptimizer class - * containing the updated variable assignments, which may be retrieved with - * values(). + /** + * Perform a single iteration, returning GaussianFactorGraph corresponding to + * the linearized factor graph. */ GaussianFactorGraph::shared_ptr iterate() override; diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index fa39d2097..0486c1f29 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -70,7 +70,16 @@ public: virtual ~NonlinearConjugateGradientOptimizer() { } + /** + * Perform a single iteration, returning GaussianFactorGraph corresponding to + * the linearized factor graph. + */ GaussianFactorGraph::shared_ptr iterate() override; + + /** + * Optimize for the maximum-likelihood estimate, returning a the optimized + * variable assignments. + */ const Values& optimize() override; }; diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index c6aa52ab2..4d928482e 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index c705e3c8f..5c89425c8 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 4cddc7dda..1942734c5 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -29,14 +29,12 @@ #include #include -/** - * Macro to add a standard clone function to a derived factor - * @deprecated: will go away shortly - just add the clone function directly - */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #define ADD_CLONE_NONLINEAR_FACTOR(Derived) \ virtual gtsam::NonlinearFactor::shared_ptr clone() const { \ return boost::static_pointer_cast( \ gtsam::NonlinearFactor::shared_ptr(new Derived(*this))); } +#endif namespace gtsam { @@ -213,11 +211,6 @@ public: return noiseModel_; } - /// @deprecated access to the noise model - SharedNoiseModel get_noiseModel() const { - return noiseModel_; - } - /** * Error function *without* the NoiseModel, \f$ z-h(x) \f$. * Override this method to finish implementing an N-way factor. @@ -248,6 +241,13 @@ public: */ boost::shared_ptr linearize(const Values& x) const; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + SharedNoiseModel get_noiseModel() const { return noiseModel_; } + /// @} +#endif + private: /** Serialization function */ diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h index edc62ae48..103c231be 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -86,9 +86,9 @@ public: /// @name Standard interface /// @{ - /** Optimize for the maximum-likelihood estimate, returning a new - * NonlinearOptimizer class containing the optimized variable assignments, - * which may be retrieved with values(). + /** + * Optimize for the maximum-likelihood estimate, returning a the optimized + * variable assignments. * * This function simply calls iterate() in a loop, checking for convergence * with check_convergence(). For fine-grain control over the optimization @@ -126,9 +126,9 @@ public: virtual VectorValues solve(const GaussianFactorGraph &gfg, const NonlinearOptimizerParams& params) const; - /** Perform a single iteration, returning a new NonlinearOptimizer class - * containing the updated variable assignments, which may be retrieved with - * values(). + /** + * Perform a single iteration, returning GaussianFactorGraph corresponding to + * the linearized factor graph. */ virtual GaussianFactorGraph::shared_ptr iterate() = 0; diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 8e4678590..3018a14b9 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/nonlinear/Symbol.h b/gtsam/nonlinear/Symbol.h index caac14bf7..0ccac6cc7 100644 --- a/gtsam/nonlinear/Symbol.h +++ b/gtsam/nonlinear/Symbol.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 16f8eba16..1048cd73a 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -415,7 +415,7 @@ namespace gtsam { static ConstKeyValuePair make_const_deref_pair(const KeyValueMap::const_iterator::value_type& key_value) { return ConstKeyValuePair(key_value.first, *key_value.second); } - + static KeyValuePair make_deref_pair(const KeyValueMap::iterator::value_type& key_value) { return KeyValuePair(key_value.first, *key_value.second); } diff --git a/gtsam/nonlinear/WhiteNoiseFactor.h b/gtsam/nonlinear/WhiteNoiseFactor.h index 5ce7fa7b3..634736800 100644 --- a/gtsam/nonlinear/WhiteNoiseFactor.h +++ b/gtsam/nonlinear/WhiteNoiseFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/nonlinear/internal/CallRecord.h b/gtsam/nonlinear/internal/CallRecord.h index e58c40203..8bdc0652e 100644 --- a/gtsam/nonlinear/internal/CallRecord.h +++ b/gtsam/nonlinear/internal/CallRecord.h @@ -127,7 +127,7 @@ private: /** * CallRecordMaxVirtualStaticRows tells which separate virtual reverseAD with specific - * static rows (1..CallRecordMaxVirtualStaticRows) methods are part of the CallRecord + * static rows (1..CallRecordMaxVirtualStaticRows) methods are part of the CallRecord * interface. It is used to keep the testCallRecord unit test in sync. */ const int CallRecordMaxVirtualStaticRows = 5; diff --git a/gtsam/nonlinear/internal/JacobianMap.h b/gtsam/nonlinear/internal/JacobianMap.h index c99f05b7d..95fc6d022 100644 --- a/gtsam/nonlinear/internal/JacobianMap.h +++ b/gtsam/nonlinear/internal/JacobianMap.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/nonlinear/nonlinearExceptions.h b/gtsam/nonlinear/nonlinearExceptions.h index 413610f82..8b32b6fdc 100644 --- a/gtsam/nonlinear/nonlinearExceptions.h +++ b/gtsam/nonlinear/nonlinearExceptions.h @@ -41,7 +41,7 @@ namespace gtsam { Key key() const { return key_; } virtual const char* what() const throw() { if(what_.empty()) - what_ = + what_ = "\nRequested to marginalize out variable " + formatter_(key_) + ", but this variable\n\ is not a leaf. To make the variables you would like to marginalize be leaves,\n\ their ordering should be constrained using the constrainedKeys argument to\n\ diff --git a/gtsam/nonlinear/tests/testCallRecord.cpp b/gtsam/nonlinear/tests/testCallRecord.cpp index 5fc4e208d..494a59fff 100644 --- a/gtsam/nonlinear/tests/testCallRecord.cpp +++ b/gtsam/nonlinear/tests/testCallRecord.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/nonlinear/tests/testExecutionTrace.cpp b/gtsam/nonlinear/tests/testExecutionTrace.cpp index 731f69816..c2b245780 100644 --- a/gtsam/nonlinear/tests/testExecutionTrace.cpp +++ b/gtsam/nonlinear/tests/testExecutionTrace.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 7a06179b4..67e0d1f24 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -166,7 +166,7 @@ double doubleF(const Pose3& pose, // } Pose3_ x(1); Point3_ p(2); -Point3_ p_cam(x, &Pose3::transform_to, p); +Point3_ p_cam(x, &Pose3::transformTo, p); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index 6dc3e3d2f..90ebcbbba 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -11,7 +11,7 @@ /** * @file testSerializationNonlinear.cpp - * @brief + * @brief * @author Richard Roberts * @date Feb 7, 2012 */ diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index b3c557b32..b8eee540d 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/nonlinear/tests/testWhiteNoiseFactor.cpp b/gtsam/nonlinear/tests/testWhiteNoiseFactor.cpp index 1cd042428..877c9adbc 100644 --- a/gtsam/nonlinear/tests/testWhiteNoiseFactor.cpp +++ b/gtsam/nonlinear/tests/testWhiteNoiseFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index 58e7e78af..867db70e0 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -241,7 +241,7 @@ Values localToWorld(const Values& local, const Pose2& base, try { // if value is a Point2, transform it from base pose Point2 point = local.at(key); - world.insert(key, base.transform_from(point)); + world.insert(key, base.transformFrom(point)); } catch (std::exception e2) { // if not Pose2 or Point2, do nothing } diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h index b3fd76c67..b52e115fd 100644 --- a/gtsam/slam/AntiFactor.h +++ b/gtsam/slam/AntiFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h index 1a1fac922..7383690f4 100644 --- a/gtsam/slam/BearingFactor.h +++ b/gtsam/slam/BearingFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 89291fac5..9c412eb19 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -56,7 +56,7 @@ namespace gtsam { /** Constructor */ BetweenFactor(Key key1, Key key2, const VALUE& measured, - const SharedNoiseModel& model) : + const SharedNoiseModel& model = nullptr) : Base(model, key1, key2), measured_(measured) { } @@ -122,7 +122,7 @@ namespace gtsam { boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); } - + // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 }; public: diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index 44a5ee5f2..f6561807e 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 9356aceb2..0eb489f35 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -153,7 +153,7 @@ public: } // Whiten the system if needed - const SharedNoiseModel& noiseModel = this->get_noiseModel(); + const SharedNoiseModel& noiseModel = this->noiseModel(); if (noiseModel && !noiseModel->isUnit()) { // TODO: implement WhitenSystem for fixed size matrices and include // above diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 2f247811d..a1baab5fa 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -82,7 +82,11 @@ Values InitializePose3::normalizeRelaxedRotations( for(const auto& it: relaxedRot3) { Key key = it.first; if (key != kAnchorKey) { - Matrix3 R; R << it.second; + Matrix3 R; + R << Eigen::Map(it.second.data()); // Recover M from vectorized + + // ClosestTo finds rotation matrix closest to H in Frobenius sense + // Rot3 initRot = Rot3::ClosestTo(M.transpose()); Matrix U, V; Vector s; svd(R.transpose(), U, s, V); diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index 44f317a49..78030ff34 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -2,7 +2,7 @@ * @file PoseRotationPrior.h * * @brief Implements a prior on the rotation component of a pose - * + * * @date Jun 14, 2012 * @author Alex Cunningham */ diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index 0b1c0cf63..c38d13b58 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -2,7 +2,7 @@ * @file PoseTranslationPrior.h * * @brief Implements a prior on the translation component of a pose - * + * * @date Jun 14, 2012 * @author Alex Cunningham */ diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 3c5c42ccc..8bb7a3ab8 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -53,7 +53,7 @@ namespace gtsam { virtual ~PriorFactor() {} /** Constructor */ - PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model) : + PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model = nullptr) : Base(model, key), prior_(prior) { } @@ -104,7 +104,7 @@ namespace gtsam { boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(prior_); } - + // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; public: diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index d50674d75..3fa5c1a21 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 68b42f210..30f761934 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -31,7 +31,7 @@ P transform_point( const T& trans, const P& global, boost::optional Dtrans, boost::optional Dglobal) { - return trans.transform_from(global, Dtrans, Dglobal); + return trans.transformFrom(global, Dtrans, Dglobal); } /** @@ -44,7 +44,7 @@ P transform_point( * l = lTg * g * * The Point and Transform concepts must be Lie types, and the transform - * relationship "Point = transform_from(Transform, Point)" must exist. + * relationship "Point = transformFrom(Transform, Point)" must exist. * * To implement this function in new domains, specialize a new version of * Point transform_point(transform, global, Dtrans, Dglobal) diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index e554e0c85..cbeed77c5 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -183,12 +183,9 @@ public: std::vector Gs(numKeys * (numKeys + 1) / 2); std::vector gs(numKeys); - if (this->measured_.size() != cameras.size()) { - std::cout - << "SmartProjectionHessianFactor: this->measured_.size() inconsistent with input" - << std::endl; - exit(1); - } + if (this->measured_.size() != cameras.size()) + throw std::runtime_error("SmartProjectionHessianFactor: this->measured_" + ".size() inconsistent with input"); triangulateSafe(cameras); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index c32a049e2..8a5c87ecf 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -66,6 +66,7 @@ string findExampleDataFile(const string& name) { namesToSearch.push_back(name + ".graph"); namesToSearch.push_back(name + ".txt"); namesToSearch.push_back(name + ".out"); + namesToSearch.push_back(name + ".xml"); // Find first name that exists for(const fs::path& root: rootsToSearch) { @@ -358,7 +359,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, if (!initial->exists(L(id2))) { Pose2 pose = initial->at(id1); Point2 local(cos(bearing) * range, sin(bearing) * range); - Point2 global = pose.transform_from(local); + Point2 global = pose.transformFrom(local); initial->insert(L(id2), global); } } diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index f9f7125cc..fc2f60f35 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -21,8 +21,8 @@ typedef Expression Point2_; typedef Expression Rot2_; typedef Expression Pose2_; -inline Point2_ transform_to(const Pose2_& x, const Point2_& p) { - return Point2_(x, &Pose2::transform_to, p); +inline Point2_ transformTo(const Pose2_& x, const Point2_& p) { + return Point2_(x, &Pose2::transformTo, p); } // 3D Geometry @@ -32,12 +32,12 @@ typedef Expression Unit3_; typedef Expression Rot3_; typedef Expression Pose3_; -inline Point3_ transform_to(const Pose3_& x, const Point3_& p) { - return Point3_(x, &Pose3::transform_to, p); +inline Point3_ transformTo(const Pose3_& x, const Point3_& p) { + return Point3_(x, &Pose3::transformTo, p); } -inline Point3_ transform_from(const Pose3_& x, const Point3_& p) { - return Point3_(x, &Pose3::transform_from, p); +inline Point3_ transformFrom(const Pose3_& x, const Point3_& p) { + return Point3_(x, &Pose3::transformFrom, p); } inline Point3_ rotate(const Rot3_& x, const Point3_& p) { diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index 1e8b330c3..d995bf8e7 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -1,6 +1,6 @@ /** * @file testBetweenFactor.cpp - * @brief + * @brief * @author Duy-Nguyen Ta * @date Aug 2, 2013 */ diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 9434280d4..08de83eb1 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -505,7 +505,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ } for(size_t j=0; j < readData.number_tracks(); j++){ // for each point Key pointKey = P(j); - Point3 point = poseChange.transform_from( readData.tracks[j].p ); + Point3 point = poseChange.transformFrom( readData.tracks[j].p ); value.insert(pointKey, point); } diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index b2150dd86..16197ab03 100644 --- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index d33551edf..b2d368b67 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -220,7 +220,7 @@ TEST (EssentialMatrixFactor2, factor) { EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2); // Check evaluation - Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1); + Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transformTo(P1); const Point2 pi = PinholeBase::Project(P2); Point2 expected(pi - pB(i)); diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index 1c8f3b8ed..63c50e34b 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp index 24b818835..217ff2178 100644 --- a/gtsam/slam/tests/testReferenceFrameFactor.cpp +++ b/gtsam/slam/tests/testReferenceFrameFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -89,7 +89,7 @@ TEST( ReferenceFrameFactor, jacobians_zero ) { // get values that are ideal Pose2 trans(2.0, 3.0, 0.0); Point2 global(5.0, 6.0); - Point2 local = trans.transform_from(global); + Point2 local = trans.transformFrom(global); PointReferenceFrameFactor tc(lA1, tA1, lB1); Vector actCost = tc.evaluateError(global, trans, local), @@ -124,8 +124,8 @@ TEST( ReferenceFrameFactor, converge_trans ) { Pose2 transIdeal(7.0, 3.0, M_PI/2); // verify direction - EXPECT(assert_equal(local1, transIdeal.transform_from(global1))); - EXPECT(assert_equal(local2, transIdeal.transform_from(global2))); + EXPECT(assert_equal(local1, transIdeal.transformFrom(global1))); + EXPECT(assert_equal(local2, transIdeal.transformFrom(global2))); // choose transform // Pose2 trans = transIdeal; // ideal - works @@ -177,7 +177,7 @@ TEST( ReferenceFrameFactor, converge_local ) { // Pose2 trans(1.5, 2.5, 1.0); // larger rotation Pose2 trans(1.5, 2.5, 3.1); // significant rotation - Point2 idealLocal = trans.transform_from(global); + Point2 idealLocal = trans.transformFrom(global); // perturb the initial estimate // Point2 local = idealLocal; // Ideal case - works @@ -213,7 +213,7 @@ TEST( ReferenceFrameFactor, converge_global ) { // Pose2 trans(1.5, 2.5, 1.0); // larger rotation Pose2 trans(1.5, 2.5, 3.1); // significant rotation - Point2 idealForeign = trans.inverse().transform_from(local); + Point2 idealForeign = trans.inverse().transformFrom(local); // perturb the initial estimate // Point2 global = idealForeign; // Ideal - works diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index aaffbf0e6..ddb4d8adb 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -46,7 +46,6 @@ static double rankTol = 1.0; template PinholeCamera perturbCameraPoseAndCalibration( const PinholeCamera& camera) { - GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Pose3 cameraPose = camera.pose(); diff --git a/gtsam/symbolic/SymbolicBayesNet.cpp b/gtsam/symbolic/SymbolicBayesNet.cpp index 038ccecbf..5bc20ad12 100644 --- a/gtsam/symbolic/SymbolicBayesNet.cpp +++ b/gtsam/symbolic/SymbolicBayesNet.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h index 4db265036..ab89a4dba 100644 --- a/gtsam/symbolic/SymbolicBayesNet.h +++ b/gtsam/symbolic/SymbolicBayesNet.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -34,7 +34,7 @@ namespace gtsam { typedef FactorGraph Base; typedef SymbolicBayesNet This; typedef SymbolicConditional ConditionalType; - typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; typedef boost::shared_ptr sharedConditional; /// @name Standard Constructors @@ -54,22 +54,22 @@ namespace gtsam { /** Implicit copy/downcast constructor to override explicit template container constructor */ template SymbolicBayesNet(const FactorGraph& graph) : Base(graph) {} - + /// @} /// @name Testable /// @{ - + /** Check equality */ bool equals(const This& bn, double tol = 1e-9) const; /// @} - + /// @name Standard Interface /// @{ - + void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - + /// @} private: diff --git a/gtsam/symbolic/SymbolicBayesTree.cpp b/gtsam/symbolic/SymbolicBayesTree.cpp index aab626837..07cb8a012 100644 --- a/gtsam/symbolic/SymbolicBayesTree.cpp +++ b/gtsam/symbolic/SymbolicBayesTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/SymbolicBayesTree.h b/gtsam/symbolic/SymbolicBayesTree.h index c4f09c71c..e28f28764 100644 --- a/gtsam/symbolic/SymbolicBayesTree.h +++ b/gtsam/symbolic/SymbolicBayesTree.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/SymbolicEliminationTree.cpp b/gtsam/symbolic/SymbolicEliminationTree.cpp index bd11274bd..7a2e9b831 100644 --- a/gtsam/symbolic/SymbolicEliminationTree.cpp +++ b/gtsam/symbolic/SymbolicEliminationTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/SymbolicEliminationTree.h b/gtsam/symbolic/SymbolicEliminationTree.h index e20fb67bd..5ec886653 100644 --- a/gtsam/symbolic/SymbolicEliminationTree.h +++ b/gtsam/symbolic/SymbolicEliminationTree.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -31,7 +31,7 @@ namespace gtsam { typedef EliminationTree Base; ///< Base class typedef SymbolicEliminationTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - + /** Build the elimination tree of a factor graph using pre-computed column structure. * @param factorGraph The factor graph for which to build the elimination tree * @param structure The set of factors involving each variable. If this is not precomputed, diff --git a/gtsam/symbolic/SymbolicFactor.h b/gtsam/symbolic/SymbolicFactor.h index aca1ba043..66657aa7d 100644 --- a/gtsam/symbolic/SymbolicFactor.h +++ b/gtsam/symbolic/SymbolicFactor.h @@ -89,11 +89,11 @@ namespace gtsam { /// @name Testable /// @{ - + bool equals(const This& other, double tol = 1e-9) const; /// @} - + /// @name Advanced Constructors /// @{ public: @@ -129,7 +129,7 @@ namespace gtsam { /// @name Standard Interface /// @{ - + /** Whether the factor is empty (involves zero variables). */ bool empty() const { return keys_.empty(); } diff --git a/gtsam/symbolic/SymbolicFactorGraph.cpp b/gtsam/symbolic/SymbolicFactorGraph.cpp index 574268472..2824a799c 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.cpp +++ b/gtsam/symbolic/SymbolicFactorGraph.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h index 0a5427ba3..b6f0de2ae 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.h +++ b/gtsam/symbolic/SymbolicFactorGraph.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -82,10 +82,10 @@ namespace gtsam { SymbolicFactorGraph(const FactorGraph& graph) : Base(graph) {} /// @} - + /// @name Testable /// @{ - + bool equals(const This& fg, double tol = 1e-9) const; /// @} diff --git a/gtsam/symbolic/SymbolicISAM.cpp b/gtsam/symbolic/SymbolicISAM.cpp index d7af82e5c..ff724878b 100644 --- a/gtsam/symbolic/SymbolicISAM.cpp +++ b/gtsam/symbolic/SymbolicISAM.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/SymbolicISAM.h b/gtsam/symbolic/SymbolicISAM.h index 3f85facbf..874acd582 100644 --- a/gtsam/symbolic/SymbolicISAM.h +++ b/gtsam/symbolic/SymbolicISAM.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/SymbolicJunctionTree.cpp b/gtsam/symbolic/SymbolicJunctionTree.cpp index 261f682b0..b26b02d84 100644 --- a/gtsam/symbolic/SymbolicJunctionTree.cpp +++ b/gtsam/symbolic/SymbolicJunctionTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/SymbolicJunctionTree.h b/gtsam/symbolic/SymbolicJunctionTree.h index fdc5450b3..7a152e532 100644 --- a/gtsam/symbolic/SymbolicJunctionTree.h +++ b/gtsam/symbolic/SymbolicJunctionTree.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -51,7 +51,7 @@ namespace gtsam { typedef JunctionTree Base; ///< Base class typedef SymbolicJunctionTree This; ///< This class typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - + /** * Build the elimination tree of a factor graph using pre-computed column structure. * @param factorGraph The factor graph for which to build the elimination tree diff --git a/gtsam/symbolic/tests/testSerializationSymbolic.cpp b/gtsam/symbolic/tests/testSerializationSymbolic.cpp index 64b06deec..e6f00b087 100644 --- a/gtsam/symbolic/tests/testSerializationSymbolic.cpp +++ b/gtsam/symbolic/tests/testSerializationSymbolic.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -11,7 +11,7 @@ /** * @file testSerializationInference.cpp - * @brief + * @brief * @author Richard Roberts * @date Feb 7, 2012 */ diff --git a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp index 309b8f22e..a92d66f68 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 1b84e291f..fdc28c5c8 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -612,7 +612,7 @@ TEST(SymbolicBayesTree, complicatedMarginal) SymbolicBayesTreeClique::shared_ptr cur; SymbolicBayesTreeClique::shared_ptr root = MakeClique(list_of(11)(12), 2); cur = root; - + root->children += MakeClique(list_of(9)(10)(11)(12), 2); root->children.back()->parent_ = root; diff --git a/gtsam/symbolic/tests/testSymbolicConditional.cpp b/gtsam/symbolic/tests/testSymbolicConditional.cpp index ddc602443..963d0dfef 100644 --- a/gtsam/symbolic/tests/testSymbolicConditional.cpp +++ b/gtsam/symbolic/tests/testSymbolicConditional.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp index d22703b14..78bb2182c 100644 --- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -11,7 +11,7 @@ /** * @file testSymbolicEliminationTree.cpp - * @brief + * @brief * @author Richard Roberts * @date Oct 14, 2010 */ @@ -145,7 +145,7 @@ TEST(EliminationTree, Create2) Ordering order = list_of(X(1)) (L(3)) (L(1)) (X(5)) (X(2)) (L(2)) (X(4)) (X(3)); SymbolicEliminationTree actual(graph, order); - + EXPECT(assert_equal(expected, actual)); } diff --git a/gtsam/symbolic/tests/testSymbolicFactor.cpp b/gtsam/symbolic/tests/testSymbolicFactor.cpp index 4e2a7b8c9..378e780cd 100644 --- a/gtsam/symbolic/tests/testSymbolicFactor.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/tests/testSymbolicISAM.cpp b/gtsam/symbolic/tests/testSymbolicISAM.cpp index 994e9b107..e3ab36c94 100644 --- a/gtsam/symbolic/tests/testSymbolicISAM.cpp +++ b/gtsam/symbolic/tests/testSymbolicISAM.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp index f2d891827..c5b1f4ff1 100644 --- a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam/symbolic/tests/testVariableIndex.cpp b/gtsam/symbolic/tests/testVariableIndex.cpp index 56ce1f9a5..6afb47e26 100644 --- a/gtsam/symbolic/tests/testVariableIndex.cpp +++ b/gtsam/symbolic/tests/testVariableIndex.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -11,60 +11,66 @@ /** * @file testVariableIndex.cpp - * @brief + * @brief Unit tests for VariableIndex class * @author Richard Roberts - * @date Sep 26, 2010 + * @date Sep 26, 2010 */ +#include +#include +#include + +#include + #include #include using namespace boost::assign; -#include -#include - -#include -#include - using namespace std; using namespace gtsam; /* ************************************************************************* */ -TEST(VariableIndex, augment) { +// 2 small symbolic graphs shared by all tests - SymbolicFactorGraph fg1, fg2; +SymbolicFactorGraph testGraph1() { + SymbolicFactorGraph fg1; fg1.push_factor(0, 1); fg1.push_factor(0, 2); fg1.push_factor(5, 9); fg1.push_factor(2, 3); + return fg1; +} + +SymbolicFactorGraph testGraph2() { + SymbolicFactorGraph fg2; fg2.push_factor(1, 3); fg2.push_factor(2, 4); fg2.push_factor(3, 5); fg2.push_factor(5, 6); + return fg2; +} - SymbolicFactorGraph fgCombined; fgCombined.push_back(fg1); fgCombined.push_back(fg2); +/* ************************************************************************* */ +TEST(VariableIndex, augment) { + auto fg1 = testGraph1(), fg2 = testGraph2(); + SymbolicFactorGraph fgCombined; + fgCombined.push_back(fg1); + fgCombined.push_back(fg2); VariableIndex expected(fgCombined); VariableIndex actual(fg1); actual.augment(fg2); - LONGS_EQUAL(16, (long)actual.nEntries()); - LONGS_EQUAL(8, (long)actual.nFactors()); + LONGS_EQUAL(8, actual.size()); + LONGS_EQUAL(16, actual.nEntries()); + LONGS_EQUAL(8, actual.nFactors()); EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ TEST(VariableIndex, augment2) { - SymbolicFactorGraph fg1, fg2; - fg1.push_factor(0, 1); - fg1.push_factor(0, 2); - fg1.push_factor(5, 9); - fg1.push_factor(2, 3); - fg2.push_factor(1, 3); - fg2.push_factor(2, 4); - fg2.push_factor(3, 5); - fg2.push_factor(5, 6); + auto fg1 = testGraph1(), fg2 = testGraph2(); SymbolicFactorGraph fgCombined; fgCombined.push_back(fg1); @@ -73,27 +79,20 @@ TEST(VariableIndex, augment2) { VariableIndex expected(fgCombined); - FastVector newIndices = list_of(5)(6)(7)(8); + FactorIndices newIndices = list_of(5)(6)(7)(8); VariableIndex actual(fg1); actual.augment(fg2, newIndices); - LONGS_EQUAL(16, (long) actual.nEntries()); - LONGS_EQUAL(9, (long) actual.nFactors()); + LONGS_EQUAL(8, actual.size()); + LONGS_EQUAL(16, actual.nEntries()); + LONGS_EQUAL(9, actual.nFactors()); EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ TEST(VariableIndex, remove) { - SymbolicFactorGraph fg1, fg2; - fg1.push_factor(0, 1); - fg1.push_factor(0, 2); - fg1.push_factor(5, 9); - fg1.push_factor(2, 3); - fg2.push_factor(1, 3); - fg2.push_factor(2, 4); - fg2.push_factor(3, 5); - fg2.push_factor(5, 6); + auto fg1 = testGraph1(), fg2 = testGraph2(); SymbolicFactorGraph fgCombined; fgCombined.push_back(fg1); fgCombined.push_back(fg2); @@ -118,15 +117,7 @@ TEST(VariableIndex, remove) { /* ************************************************************************* */ TEST(VariableIndex, deep_copy) { - SymbolicFactorGraph fg1, fg2; - fg1.push_factor(0, 1); - fg1.push_factor(0, 2); - fg1.push_factor(5, 9); - fg1.push_factor(2, 3); - fg2.push_factor(1, 3); - fg2.push_factor(2, 4); - fg2.push_factor(3, 5); - fg2.push_factor(5, 6); + auto fg1 = testGraph1(), fg2 = testGraph2(); // Create original graph and VariableIndex SymbolicFactorGraph fgOriginal; fgOriginal.push_back(fg1); fgOriginal.push_back(fg2); diff --git a/gtsam_unstable/base/BTree.h b/gtsam_unstable/base/BTree.h index f544366b2..fd675d0d5 100644 --- a/gtsam_unstable/base/BTree.h +++ b/gtsam_unstable/base/BTree.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -244,7 +244,7 @@ namespace gtsam { else if (key < k) node = node->right.root_.get(); else return node->value(); } - + throw std::invalid_argument("BTree::find: key not found"); } diff --git a/gtsam_unstable/base/DSF.h b/gtsam_unstable/base/DSF.h index ba545de35..c7b8cd417 100644 --- a/gtsam_unstable/base/DSF.h +++ b/gtsam_unstable/base/DSF.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/base/Dummy.h b/gtsam_unstable/base/Dummy.h index ff9f3a8a6..14b4b4854 100644 --- a/gtsam_unstable/base/Dummy.h +++ b/gtsam_unstable/base/Dummy.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/base/FixedVector.h b/gtsam_unstable/base/FixedVector.h index dd3668087..b1fff90ef 100644 --- a/gtsam_unstable/base/FixedVector.h +++ b/gtsam_unstable/base/FixedVector.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/base/dllexport.h b/gtsam_unstable/base/dllexport.h index f49199c70..903996db4 100644 --- a/gtsam_unstable/base/dllexport.h +++ b/gtsam_unstable/base/dllexport.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/base/tests/testBTree.cpp b/gtsam_unstable/base/tests/testBTree.cpp index bbc22c8e5..fcdbd0393 100644 --- a/gtsam_unstable/base/tests/testBTree.cpp +++ b/gtsam_unstable/base/tests/testBTree.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/base/tests/testDSF.cpp b/gtsam_unstable/base/tests/testDSF.cpp index 298d439bc..c3a59aada 100644 --- a/gtsam_unstable/base/tests/testDSF.cpp +++ b/gtsam_unstable/base/tests/testDSF.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/base/tests/testFixedVector.cpp b/gtsam_unstable/base/tests/testFixedVector.cpp index 1bae52a80..5c4ca4df3 100644 --- a/gtsam_unstable/base/tests/testFixedVector.cpp +++ b/gtsam_unstable/base/tests/testFixedVector.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp index d2efc3a2d..9929938d5 100644 --- a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp +++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp @@ -1,6 +1,6 @@ /** * @file testLoopyBelief.cpp - * @brief + * @brief * @author Duy-Nguyen Ta * @date Oct 11, 2013 */ @@ -175,19 +175,19 @@ private: // collect all factors involving this key in the original graph DiscreteFactorGraph::shared_ptr star(new DiscreteFactorGraph()); - for(size_t factorIdx: varIndex[key]) { - star->push_back(graph.at(factorIdx)); + for(size_t factorIndex: varIndex[key]) { + star->push_back(graph.at(factorIndex)); // accumulate unary factors - if (graph.at(factorIdx)->size() == 1) { + if (graph.at(factorIndex)->size() == 1) { if (!prodOfUnaries) prodOfUnaries = boost::dynamic_pointer_cast( - graph.at(factorIdx)); + graph.at(factorIndex)); else prodOfUnaries = boost::make_shared( *prodOfUnaries * (*boost::dynamic_pointer_cast( - graph.at(factorIdx)))); + graph.at(factorIndex)))); } } diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index bf92ab9c4..0c00e5d95 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -132,7 +132,7 @@ public: /** * Apply transform to this pose, with optional derivatives * equivalent to: - * local = trans.transform_from(global, Dtrans, Dglobal) + * local = trans.transformFrom(global, Dtrans, Dglobal) * * Note: the transform jacobian convention is flipped */ diff --git a/gtsam_unstable/examples/ConcurrentCalibration.cpp b/gtsam_unstable/examples/ConcurrentCalibration.cpp index fe1c83ce3..41e75432f 100644 --- a/gtsam_unstable/examples/ConcurrentCalibration.cpp +++ b/gtsam_unstable/examples/ConcurrentCalibration.cpp @@ -46,7 +46,7 @@ int main(int argc, char** argv){ string calibration_loc = findExampleDataFile("VO_calibration00s.txt"); string pose_loc = findExampleDataFile("VO_camera_poses00s.txt"); string factor_loc = findExampleDataFile("VO_stereo_factors00s.txt"); - + //read camera calibration info from file // focal lengths fx, fy, skew s, principal point u0, v0, baseline b double fx, fy, s, u0, v0, b; @@ -57,7 +57,7 @@ int main(int argc, char** argv){ //create stereo camera calibration object const Cal3_S2::shared_ptr K(new Cal3_S2(fx,fy,s,u0,v0)); const Cal3_S2::shared_ptr noisy_K(new Cal3_S2(fx*1.2,fy*1.2,s,u0-10,v0+10)); - + initial_estimate.insert(Symbol('K', 0), *noisy_K); noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 1e-5, 100, 100).finished()); @@ -75,7 +75,7 @@ int main(int argc, char** argv){ } initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); } - + noiseModel::Isotropic::shared_ptr poseNoise = noiseModel::Isotropic::Sigma(6, 0.01); graph.emplace_shared >(Symbol('x', pose_id), Pose3(m), poseNoise); @@ -97,8 +97,8 @@ int main(int argc, char** argv){ //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it if (!initial_estimate.exists(Symbol('l', l))) { Pose3 camPose = initial_estimate.at(Symbol('x', x)); - //transform_from() transforms the input Point3 from the camera pose space, camPose, to the global space - Point3 worldPoint = camPose.transform_from(Point3(X, Y, Z)); + //transformFrom() transforms the input Point3 from the camera pose space, camPose, to the global space + Point3 worldPoint = camPose.transformFrom(Point3(X, Y, Z)); initial_estimate.insert(Symbol('l', l), worldPoint); } } diff --git a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp index 8df720cd1..429a2c2b2 100644 --- a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp +++ b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -84,7 +84,7 @@ int main(int argc, char** argv) { // Create a prior on the first pose, placing it at the origin Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); + auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); Key priorKey = 0; newFactors.push_back(PriorFactor(priorKey, priorMean, priorNoise)); newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior @@ -110,11 +110,11 @@ int main(int argc, char** argv) { // Add odometry factors from two different sources with different error stats Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); - noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05)); + auto odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); - noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05)); + auto odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); // Unlike the fixed-lag versions, the concurrent filter implementation @@ -159,7 +159,7 @@ int main(int argc, char** argv) { Key loopKey1(1000 * (0.0)); Key loopKey2(1000 * (5.0)); Pose2 loopMeasurement = Pose2(9.5, 1.00, 0.00); - noiseModel::Diagonal::shared_ptr loopNoise = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.25)); + auto loopNoise = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.25)); NonlinearFactor::shared_ptr loopFactor(new BetweenFactor(loopKey1, loopKey2, loopMeasurement, loopNoise)); // This measurement cannot be added directly to the concurrent filter because it connects a filter state to a smoother state @@ -189,11 +189,11 @@ int main(int argc, char** argv) { // Add odometry factors from two different sources with different error stats Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); - noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05)); + auto odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); - noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05)); + auto odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); // Unlike the fixed-lag versions, the concurrent filter implementation @@ -262,11 +262,11 @@ int main(int argc, char** argv) { // Add odometry factors from two different sources with different error stats Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); - noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05)); + auto odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); - noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05)); + auto odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); // Unlike the fixed-lag versions, the concurrent filter implementation @@ -309,19 +309,19 @@ int main(int argc, char** argv) { // And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds cout << "After 15.0 seconds, each version contains to the following keys:" << endl; cout << " Concurrent Filter Keys: " << endl; - for(const Values::ConstKeyValuePair& key_value: concurrentFilter.getLinearizationPoint()) { + for(const auto& key_value: concurrentFilter.getLinearizationPoint()) { cout << setprecision(5) << " Key: " << key_value.key << endl; } cout << " Concurrent Smoother Keys: " << endl; - for(const Values::ConstKeyValuePair& key_value: concurrentSmoother.getLinearizationPoint()) { + for(const auto& key_value: concurrentSmoother.getLinearizationPoint()) { cout << setprecision(5) << " Key: " << key_value.key << endl; } cout << " Fixed-Lag Smoother Keys: " << endl; - for(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: fixedlagSmoother.timestamps()) { + for(const auto& key_timestamp: fixedlagSmoother.timestamps()) { cout << setprecision(5) << " Key: " << key_timestamp.first << endl; } cout << " Batch Smoother Keys: " << endl; - for(const FixedLagSmoother::KeyTimestampMap::value_type& key_timestamp: batchSmoother.timestamps()) { + for(const auto& key_timestamp: batchSmoother.timestamps()) { cout << setprecision(5) << " Key: " << key_timestamp.first << endl; } diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp index 1376aca40..71153ee31 100644 --- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp +++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index 94a70470a..e290cef7e 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -43,8 +43,6 @@ using namespace std; using namespace gtsam; int main(int argc, char** argv){ - - typedef PinholePose Camera; typedef SmartProjectionPoseFactor SmartFactor; Values initial_estimate; diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index c8023b23c..2426ca201 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -61,7 +61,7 @@ int main(int argc, char** argv){ string calibration_loc = findExampleDataFile("VO_calibration.txt"); string pose_loc = findExampleDataFile("VO_camera_poses_large.txt"); string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt"); - + //read camera calibration info from file // focal lengths fx, fy, skew s, principal point u0, v0, baseline b cout << "Reading calibration info" << endl; @@ -97,7 +97,7 @@ int main(int argc, char** argv){ Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl; } } - + // camera and landmark keys size_t x, l; diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 4f1b42610..70ec6f513 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -157,7 +157,7 @@ public: const gtsam::Point2 pn = k_->calibrate(pi); gtsam::Point3 pc(pn.x(), pn.y(), 1.0); pc = pc/pc.norm(); - gtsam::Point3 pw = pose_.transform_from(pc); + gtsam::Point3 pw = pose_.transformFrom(pc); const gtsam::Point3& pt = pose_.translation(); gtsam::Point3 ray = pw - pt; double theta = atan2(ray.y(), ray.x()); // longitude diff --git a/gtsam_unstable/geometry/SimWall2D.cpp b/gtsam_unstable/geometry/SimWall2D.cpp index 111d23b91..99545a5be 100644 --- a/gtsam_unstable/geometry/SimWall2D.cpp +++ b/gtsam_unstable/geometry/SimWall2D.cpp @@ -36,8 +36,8 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional pt) cons // normalized points, Aa at origin, Ab at (length, 0.0) double len = A.length(); if (debug) cout << "len: " << len << endl; - Point2 Ba = transform.transform_to(B.a()), - Bb = transform.transform_to(B.b()); + Point2 Ba = transform.transformTo(B.a()), + Bb = transform.transformTo(B.b()); if (debug) traits::Print(Ba, "Ba"); if (debug) traits::Print(Bb, "Bb"); @@ -51,11 +51,11 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional pt) cons // check conditions for exactly on the same line if (Ba.y() == 0.0 && Ba.x() > 0.0 && Ba.x() < len) { - if (pt) *pt = transform.transform_from(Ba); + if (pt) *pt = transform.transformFrom(Ba); if (debug) cout << "Ba on the line" << endl; return true; } else if (Bb.y() == 0.0 && Bb.x() > 0.0 && Bb.x() < len) { - if (pt) *pt = transform.transform_from(Bb); + if (pt) *pt = transform.transformFrom(Bb); if (debug) cout << "Bb on the line" << endl; return true; } @@ -64,7 +64,7 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional pt) cons if (fabs(Ba.x() - Bb.x()) < 1e-5) { if (debug) cout << "vertical line" << endl; if (Ba.x() < len && Ba.x() > 0.0) { - if (pt) *pt = transform.transform_from(Point2(Ba.x(), 0.0)); + if (pt) *pt = transform.transformFrom(Point2(Ba.x(), 0.0)); if (debug) cout << " within range" << endl; return true; } else { @@ -91,7 +91,7 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional pt) cons double xint = (low.x() < high.x()) ? low.x() + fabs(low.y())/slope : high.x() - fabs(high.y())/slope; if (debug) cout << "xintercept " << xint << endl; if (xint > 0.0 && xint < len) { - if (pt) *pt = transform.transform_from(Point2(xint, 0.0)); + if (pt) *pt = transform.transformFrom(Point2(xint, 0.0)); return true; } else { if (debug) cout << "xintercept out of range" << endl; @@ -116,7 +116,7 @@ Rot2 SimWall2D::reflection(const Point2& init, const Point2& intersection) const // translate to put the intersection at the origin and the wall along the x axis Rot2 wallAngle = Rot2::relativeBearing(b_ - a_); Pose2 transform(wallAngle, intersection); - Point2 t_init = transform.transform_to(init); + Point2 t_init = transform.transformTo(init); Point2 t_goal(-t_init.x(), t_init.y()); return Rot2::relativeBearing(wallAngle.rotate(t_goal)); } diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 9768f4fa4..d40fb0b59 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -71,7 +71,7 @@ Similarity3 Similarity3::inverse() const { return Similarity3(Rt, sRt, 1.0 / s_); } -Point3 Similarity3::transform_from(const Point3& p, // +Point3 Similarity3::transformFrom(const Point3& p, // OptionalJacobian<3, 7> H1, OptionalJacobian<3, 3> H2) const { const Point3 q = R_ * p + t_; if (H1) { @@ -86,7 +86,7 @@ Point3 Similarity3::transform_from(const Point3& p, // } Point3 Similarity3::operator*(const Point3& p) const { - return transform_from(p); + return transformFrom(p); } Matrix4 Similarity3::wedge(const Vector7& xi) { diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 853261fc2..f7ba53d87 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -96,11 +96,11 @@ public: /// @{ /// Action on a point p is s*(R*p+t) - Point3 transform_from(const Point3& p, // + Point3 transformFrom(const Point3& p, // OptionalJacobian<3, 7> H1 = boost::none, // OptionalJacobian<3, 3> H2 = boost::none) const; - /** syntactic sugar for transform_from */ + /** syntactic sugar for transformFrom */ Point3 operator*(const Point3& p) const; /// @} diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index d2fbd7579..aaeb0854d 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -1,415 +1,415 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testSimilarity3.cpp - * @brief Unit tests for Similarity3 class - * @author Paul Drews - * @author Zhaoyang Lv - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include - -using namespace gtsam; -using namespace std; -using symbol_shorthand::X; - -GTSAM_CONCEPT_TESTABLE_INST(Similarity3) - -static const Point3 P(0.2, 0.7, -2); -static const Rot3 R = Rot3::Rodrigues(0.3, 0, 0); -static const double s = 4; -static const Similarity3 id; -static const Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1); -static const Similarity3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), - Point3(3.5, -8.2, 4.2), 1); -static const Similarity3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3), 1); -static const Similarity3 T4(R, P, s); -static const Similarity3 T5(R, P, 10); -static const Similarity3 T6(Rot3(), Point3(1, 1, 0), 2); // Simpler transform - -//****************************************************************************** -TEST(Similarity3, Concepts) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); -} - -//****************************************************************************** -TEST(Similarity3, Constructors) { - Similarity3 sim3_Construct1; - Similarity3 sim3_Construct2(s); - Similarity3 sim3_Construct3(R, P, s); - Similarity3 sim4_Construct4(R.matrix(), P, s); -} - -//****************************************************************************** -TEST(Similarity3, Getters) { - Similarity3 sim3_default; - EXPECT(assert_equal(Rot3(), sim3_default.rotation())); - EXPECT(assert_equal(Point3(0,0,0), sim3_default.translation())); - EXPECT_DOUBLES_EQUAL(1.0, sim3_default.scale(), 1e-9); - - Similarity3 sim3(Rot3::Ypr(1, 2, 3), Point3(4, 5, 6), 7); - EXPECT(assert_equal(Rot3::Ypr(1, 2, 3), sim3.rotation())); - EXPECT(assert_equal(Point3(4, 5, 6), sim3.translation())); - EXPECT_DOUBLES_EQUAL(7.0, sim3.scale(), 1e-9); -} - -//****************************************************************************** -TEST(Similarity3, AdjointMap) { - const Matrix4 T = T2.matrix(); - // Check Ad with actual definition - Vector7 delta; - delta << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7; - Matrix4 W = Similarity3::wedge(delta); - Matrix4 TW = Similarity3::wedge(T2.AdjointMap() * delta); - EXPECT(assert_equal(TW, Matrix4(T * W * T.inverse()), 1e-9)); -} - -//****************************************************************************** -TEST(Similarity3, inverse) { - Similarity3 sim3(Rot3::Ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7); - Matrix3 Re; // some values from matlab - Re << -0.2248, 0.9024, -0.3676, -0.3502, -0.4269, -0.8337, -0.9093, -0.0587, 0.4120; - Vector3 te(-9.8472, 59.7640, 10.2125); - Similarity3 expected(Re, te, 1.0 / 7.0); - EXPECT(assert_equal(expected, sim3.inverse(), 1e-4)); - EXPECT(assert_equal(sim3, sim3.inverse().inverse(), 1e-8)); - - // test lie group inverse - Matrix H1, H2; - EXPECT(assert_equal(expected, sim3.inverse(H1), 1e-4)); - EXPECT(assert_equal(sim3, sim3.inverse().inverse(H2), 1e-8)); -} - -//****************************************************************************** -TEST(Similarity3, Multiplication) { - Similarity3 test1(Rot3::Ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7); - Similarity3 test2(Rot3::Ypr(1, 2, 3).inverse(), Point3(8, 9, 10), 11); - Matrix3 re; - re << 0.0688, 0.9863, -0.1496, -0.5665, -0.0848, -0.8197, -0.8211, 0.1412, 0.5530; - Vector3 te(-13.6797, 3.2441, -5.7794); - Similarity3 expected(re, te, 77); - EXPECT(assert_equal(expected, test1 * test2, 1e-2)); -} - -//****************************************************************************** -TEST(Similarity3, Manifold) { - EXPECT_LONGS_EQUAL(7, Similarity3::Dim()); - Vector z = Vector7::Zero(); - Similarity3 sim; - EXPECT(sim.retract(z) == sim); - - Vector7 v = Vector7::Zero(); - v(6) = 2; - Similarity3 sim2; - EXPECT(sim2.retract(z) == sim2); - - EXPECT(assert_equal(z, sim2.localCoordinates(sim))); - - Similarity3 sim3 = Similarity3(Rot3(), Point3(1, 2, 3), 1); - Vector v3(7); - v3 << 0, 0, 0, 1, 2, 3, 0; - EXPECT(assert_equal(v3, sim2.localCoordinates(sim3))); - - Similarity3 other = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(4, 5, 6), 1); - - Vector vlocal = sim.localCoordinates(other); - - EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2)); - - Similarity3 other2 = Similarity3(Rot3::Ypr(0.3, 0, 0), Point3(4, 5, 6), 1); - Rot3 R = Rot3::Rodrigues(0.3, 0, 0); - - Vector vlocal2 = sim.localCoordinates(other2); - - EXPECT(assert_equal(sim.retract(vlocal2), other2, 1e-2)); - - // TODO add unit tests for retract and localCoordinates -} - -//****************************************************************************** -TEST( Similarity3, retract_first_order) { - Similarity3 id; - Vector v = Z_7x1; - v(0) = 0.3; - EXPECT(assert_equal(Similarity3(R, Point3(0,0,0), 1), id.retract(v), 1e-2)); -// v(3) = 0.2; -// v(4) = 0.7; -// v(5) = -2; -// EXPECT(assert_equal(Similarity3(R, P, 1), id.retract(v), 1e-2)); -} - -//****************************************************************************** -TEST(Similarity3, localCoordinates_first_order) { - Vector7 d12 = Vector7::Constant(0.1); - d12(6) = 1.0; - Similarity3 t1 = T1, t2 = t1.retract(d12); - EXPECT(assert_equal(d12, t1.localCoordinates(t2))); -} - -//****************************************************************************** -TEST(Similarity3, manifold_first_order) { - Similarity3 t1 = T1; - Similarity3 t2 = T3; - Similarity3 origin; - Vector d12 = t1.localCoordinates(t2); - EXPECT(assert_equal(t2, t1.retract(d12))); - Vector d21 = t2.localCoordinates(t1); - EXPECT(assert_equal(t1, t2.retract(d21))); -} - -//****************************************************************************** -// Return as a 4*4 Matrix -TEST(Similarity3, Matrix) { - Matrix4 expected; - expected << 1, 0, 0, 1, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0.5; - Matrix4 actual = T6.matrix(); - EXPECT(assert_equal(expected, actual)); -} - -//***************************************************************************** -// Exponential and log maps -TEST(Similarity3, ExpLogMap) { - Vector7 delta; - delta << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7; - Vector7 actual = Similarity3::Logmap(Similarity3::Expmap(delta)); - EXPECT(assert_equal(delta, actual)); - - Vector7 zeros; - zeros << 0, 0, 0, 0, 0, 0, 0; - Vector7 logIdentity = Similarity3::Logmap(Similarity3::identity()); - EXPECT(assert_equal(zeros, logIdentity)); - - Similarity3 expZero = Similarity3::Expmap(zeros); - Similarity3 ident = Similarity3::identity(); - EXPECT(assert_equal(expZero, ident)); - - // Compare to matrix exponential, using expm in Lie.h - EXPECT( - assert_equal(expm(delta), Similarity3::Expmap(delta), 1e-3)); -} - -//****************************************************************************** -// Group action on Point3 (with simpler transform) -TEST(Similarity3, GroupAction) { - EXPECT(assert_equal(Point3(2, 2, 0), T6 * Point3(0, 0, 0))); - EXPECT(assert_equal(Point3(4, 2, 0), T6 * Point3(1, 0, 0))); - - // Test group action on R^4 via matrix representation - Vector4 qh; - qh << 1, 0, 0, 1; - Vector4 ph; - ph << 2, 1, 0, 0.5; // equivalent to Point3(4, 2, 0) - EXPECT(assert_equal((Vector )ph, T6.matrix() * qh)); - - // Test some more... - Point3 pa = Point3(1, 0, 0); - Similarity3 Ta(Rot3(), Point3(1, 2, 3), 1.0); - Similarity3 Tb(Rot3(), Point3(1, 2, 3), 2.0); - EXPECT(assert_equal(Point3(2, 2, 3), Ta.transform_from(pa))); - EXPECT(assert_equal(Point3(4, 4, 6), Tb.transform_from(pa))); - - Similarity3 Tc(Rot3::Rz(M_PI / 2.0), Point3(1, 2, 3), 1.0); - Similarity3 Td(Rot3::Rz(M_PI / 2.0), Point3(1, 2, 3), 2.0); - EXPECT(assert_equal(Point3(1, 3, 3), Tc.transform_from(pa))); - EXPECT(assert_equal(Point3(2, 6, 6), Td.transform_from(pa))); - - // Test derivative - boost::function f = boost::bind( - &Similarity3::transform_from, _1, _2, boost::none, boost::none); - - Point3 q(1, 2, 3); - for (const auto T : { T1, T2, T3, T4, T5, T6 }) { - Point3 q(1, 0, 0); - Matrix H1 = numericalDerivative21(f, T, q); - Matrix H2 = numericalDerivative22(f, T, q); - Matrix actualH1, actualH2; - T.transform_from(q, actualH1, actualH2); - EXPECT(assert_equal(H1, actualH1)); - EXPECT(assert_equal(H2, actualH2)); - } -} - -//****************************************************************************** -// Test very simple prior optimization example -TEST(Similarity3, Optimization) { - // Create a PriorFactor with a Sim3 prior - Similarity3 prior = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4); - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1); - Symbol key('x', 1); - PriorFactor factor(key, prior, model); - - // Create graph - NonlinearFactorGraph graph; - graph.push_back(factor); - - // Create initial estimate with identity transform - Values initial; - initial.insert(key, Similarity3()); - - // Optimize - Values result; - LevenbergMarquardtParams params; - params.setVerbosityLM("TRYCONFIG"); - result = LevenbergMarquardtOptimizer(graph, initial).optimize(); - - // After optimization, result should be prior - EXPECT(assert_equal(prior, result.at(key), 1e-4)); -} - -//****************************************************************************** -// Test optimization with both Prior and BetweenFactors -TEST(Similarity3, Optimization2) { - Similarity3 prior = Similarity3(); - Similarity3 m1 = Similarity3(Rot3::Ypr(M_PI / 4.0, 0, 0), Point3(2.0, 0, 0), - 1.0); - Similarity3 m2 = Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0), - Point3(sqrt(8) * 0.9, 0, 0), 1.0); - Similarity3 m3 = Similarity3(Rot3::Ypr(3 * M_PI / 4.0, 0, 0), - Point3(sqrt(32) * 0.8, 0, 0), 1.0); - Similarity3 m4 = Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0), - Point3(6 * 0.7, 0, 0), 1.0); - Similarity3 loop = Similarity3(1.42); - - //prior.print("Goal Transform"); - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, - 0.01); - SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas( - (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished()); - SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas( - (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished()); - PriorFactor factor(X(1), prior, model); // Prior ! - BetweenFactor b1(X(1), X(2), m1, betweenNoise); - BetweenFactor b2(X(2), X(3), m2, betweenNoise); - BetweenFactor b3(X(3), X(4), m3, betweenNoise); - BetweenFactor b4(X(4), X(5), m4, betweenNoise); - BetweenFactor lc(X(5), X(1), loop, betweenNoise2); - - // Create graph - NonlinearFactorGraph graph; - graph.push_back(factor); - graph.push_back(b1); - graph.push_back(b2); - graph.push_back(b3); - graph.push_back(b4); - graph.push_back(lc); - - //graph.print("Full Graph\n"); - Values initial; - initial.insert(X(1), Similarity3()); - initial.insert(X(2), - Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0), Point3(1, 0, 0), 1.1)); - initial.insert(X(3), - Similarity3(Rot3::Ypr(2.0 * M_PI / 2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2)); - initial.insert(X(4), - Similarity3(Rot3::Ypr(3.0 * M_PI / 2.0, 0, 0), Point3(0, 1, 0), 1.3)); - initial.insert(X(5), - Similarity3(Rot3::Ypr(4.0 * M_PI / 2.0, 0, 0), Point3(0, 0, 0), 1.0)); - - //initial.print("Initial Estimate\n"); - - Values result; - result = LevenbergMarquardtOptimizer(graph, initial).optimize(); - //result.print("Optimized Estimate\n"); - Pose3 p1, p2, p3, p4, p5; - p1 = Pose3(result.at(X(1))); - p2 = Pose3(result.at(X(2))); - p3 = Pose3(result.at(X(3))); - p4 = Pose3(result.at(X(4))); - p5 = Pose3(result.at(X(5))); - - //p1.print("Pose1"); - //p2.print("Pose2"); - //p3.print("Pose3"); - //p4.print("Pose4"); - //p5.print("Pose5"); - - Similarity3 expected(0.7); - EXPECT(assert_equal(expected, result.at(X(5)), 0.4)); -} - -//****************************************************************************** -// Align points (p,q) assuming that p = T*q + noise -TEST(Similarity3, AlignScaledPointClouds) { -// Create ground truth - Point3 q1(0, 0, 0), q2(1, 0, 0), q3(0, 1, 0); - - // Create transformed cloud (noiseless) -// Point3 p1 = T4 * q1, p2 = T4 * q2, p3 = T4 * q3; - - // Create an unknown expression - Expression unknownT(0); // use key 0 - - // Create constant expressions for the ground truth points - Expression q1_(q1), q2_(q2), q3_(q3); - - // Create prediction expressions - Expression predict1(unknownT, &Similarity3::transform_from, q1_); - Expression predict2(unknownT, &Similarity3::transform_from, q2_); - Expression predict3(unknownT, &Similarity3::transform_from, q3_); - -//// Create Expression factor graph -// ExpressionFactorGraph graph; -// graph.addExpressionFactor(predict1, p1, R); // |T*q1 - p1| -// graph.addExpressionFactor(predict2, p2, R); // |T*q2 - p2| -// graph.addExpressionFactor(predict3, p3, R); // |T*q3 - p3| -} - -//****************************************************************************** -TEST(Similarity3 , Invariants) { - Similarity3 id; - - EXPECT(check_group_invariants(id, id)); - EXPECT(check_group_invariants(id, T3)); - EXPECT(check_group_invariants(T2, id)); - EXPECT(check_group_invariants(T2, T3)); - - EXPECT(check_manifold_invariants(id, id)); - EXPECT(check_manifold_invariants(id, T3)); - EXPECT(check_manifold_invariants(T2, id)); - EXPECT(check_manifold_invariants(T2, T3)); -} - -//****************************************************************************** -TEST(Similarity3 , LieGroupDerivatives) { - Similarity3 id; - - CHECK_LIE_GROUP_DERIVATIVES(id, id); - CHECK_LIE_GROUP_DERIVATIVES(id, T2); - CHECK_LIE_GROUP_DERIVATIVES(T2, id); - CHECK_LIE_GROUP_DERIVATIVES(T2, T3); -} - -//****************************************************************************** -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -//****************************************************************************** - +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSimilarity3.cpp + * @brief Unit tests for Similarity3 class + * @author Paul Drews + * @author Zhaoyang Lv + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +using namespace gtsam; +using namespace std; +using symbol_shorthand::X; + +GTSAM_CONCEPT_TESTABLE_INST(Similarity3) + +static const Point3 P(0.2, 0.7, -2); +static const Rot3 R = Rot3::Rodrigues(0.3, 0, 0); +static const double s = 4; +static const Similarity3 id; +static const Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1); +static const Similarity3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), + Point3(3.5, -8.2, 4.2), 1); +static const Similarity3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3), 1); +static const Similarity3 T4(R, P, s); +static const Similarity3 T5(R, P, 10); +static const Similarity3 T6(Rot3(), Point3(1, 1, 0), 2); // Simpler transform + +//****************************************************************************** +TEST(Similarity3, Concepts) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + +//****************************************************************************** +TEST(Similarity3, Constructors) { + Similarity3 sim3_Construct1; + Similarity3 sim3_Construct2(s); + Similarity3 sim3_Construct3(R, P, s); + Similarity3 sim4_Construct4(R.matrix(), P, s); +} + +//****************************************************************************** +TEST(Similarity3, Getters) { + Similarity3 sim3_default; + EXPECT(assert_equal(Rot3(), sim3_default.rotation())); + EXPECT(assert_equal(Point3(0,0,0), sim3_default.translation())); + EXPECT_DOUBLES_EQUAL(1.0, sim3_default.scale(), 1e-9); + + Similarity3 sim3(Rot3::Ypr(1, 2, 3), Point3(4, 5, 6), 7); + EXPECT(assert_equal(Rot3::Ypr(1, 2, 3), sim3.rotation())); + EXPECT(assert_equal(Point3(4, 5, 6), sim3.translation())); + EXPECT_DOUBLES_EQUAL(7.0, sim3.scale(), 1e-9); +} + +//****************************************************************************** +TEST(Similarity3, AdjointMap) { + const Matrix4 T = T2.matrix(); + // Check Ad with actual definition + Vector7 delta; + delta << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7; + Matrix4 W = Similarity3::wedge(delta); + Matrix4 TW = Similarity3::wedge(T2.AdjointMap() * delta); + EXPECT(assert_equal(TW, Matrix4(T * W * T.inverse()), 1e-9)); +} + +//****************************************************************************** +TEST(Similarity3, inverse) { + Similarity3 sim3(Rot3::Ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7); + Matrix3 Re; // some values from matlab + Re << -0.2248, 0.9024, -0.3676, -0.3502, -0.4269, -0.8337, -0.9093, -0.0587, 0.4120; + Vector3 te(-9.8472, 59.7640, 10.2125); + Similarity3 expected(Re, te, 1.0 / 7.0); + EXPECT(assert_equal(expected, sim3.inverse(), 1e-4)); + EXPECT(assert_equal(sim3, sim3.inverse().inverse(), 1e-8)); + + // test lie group inverse + Matrix H1, H2; + EXPECT(assert_equal(expected, sim3.inverse(H1), 1e-4)); + EXPECT(assert_equal(sim3, sim3.inverse().inverse(H2), 1e-8)); +} + +//****************************************************************************** +TEST(Similarity3, Multiplication) { + Similarity3 test1(Rot3::Ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7); + Similarity3 test2(Rot3::Ypr(1, 2, 3).inverse(), Point3(8, 9, 10), 11); + Matrix3 re; + re << 0.0688, 0.9863, -0.1496, -0.5665, -0.0848, -0.8197, -0.8211, 0.1412, 0.5530; + Vector3 te(-13.6797, 3.2441, -5.7794); + Similarity3 expected(re, te, 77); + EXPECT(assert_equal(expected, test1 * test2, 1e-2)); +} + +//****************************************************************************** +TEST(Similarity3, Manifold) { + EXPECT_LONGS_EQUAL(7, Similarity3::Dim()); + Vector z = Vector7::Zero(); + Similarity3 sim; + EXPECT(sim.retract(z) == sim); + + Vector7 v = Vector7::Zero(); + v(6) = 2; + Similarity3 sim2; + EXPECT(sim2.retract(z) == sim2); + + EXPECT(assert_equal(z, sim2.localCoordinates(sim))); + + Similarity3 sim3 = Similarity3(Rot3(), Point3(1, 2, 3), 1); + Vector v3(7); + v3 << 0, 0, 0, 1, 2, 3, 0; + EXPECT(assert_equal(v3, sim2.localCoordinates(sim3))); + + Similarity3 other = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(4, 5, 6), 1); + + Vector vlocal = sim.localCoordinates(other); + + EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2)); + + Similarity3 other2 = Similarity3(Rot3::Ypr(0.3, 0, 0), Point3(4, 5, 6), 1); + Rot3 R = Rot3::Rodrigues(0.3, 0, 0); + + Vector vlocal2 = sim.localCoordinates(other2); + + EXPECT(assert_equal(sim.retract(vlocal2), other2, 1e-2)); + + // TODO add unit tests for retract and localCoordinates +} + +//****************************************************************************** +TEST( Similarity3, retract_first_order) { + Similarity3 id; + Vector v = Z_7x1; + v(0) = 0.3; + EXPECT(assert_equal(Similarity3(R, Point3(0,0,0), 1), id.retract(v), 1e-2)); +// v(3) = 0.2; +// v(4) = 0.7; +// v(5) = -2; +// EXPECT(assert_equal(Similarity3(R, P, 1), id.retract(v), 1e-2)); +} + +//****************************************************************************** +TEST(Similarity3, localCoordinates_first_order) { + Vector7 d12 = Vector7::Constant(0.1); + d12(6) = 1.0; + Similarity3 t1 = T1, t2 = t1.retract(d12); + EXPECT(assert_equal(d12, t1.localCoordinates(t2))); +} + +//****************************************************************************** +TEST(Similarity3, manifold_first_order) { + Similarity3 t1 = T1; + Similarity3 t2 = T3; + Similarity3 origin; + Vector d12 = t1.localCoordinates(t2); + EXPECT(assert_equal(t2, t1.retract(d12))); + Vector d21 = t2.localCoordinates(t1); + EXPECT(assert_equal(t1, t2.retract(d21))); +} + +//****************************************************************************** +// Return as a 4*4 Matrix +TEST(Similarity3, Matrix) { + Matrix4 expected; + expected << 1, 0, 0, 1, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0.5; + Matrix4 actual = T6.matrix(); + EXPECT(assert_equal(expected, actual)); +} + +//***************************************************************************** +// Exponential and log maps +TEST(Similarity3, ExpLogMap) { + Vector7 delta; + delta << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7; + Vector7 actual = Similarity3::Logmap(Similarity3::Expmap(delta)); + EXPECT(assert_equal(delta, actual)); + + Vector7 zeros; + zeros << 0, 0, 0, 0, 0, 0, 0; + Vector7 logIdentity = Similarity3::Logmap(Similarity3::identity()); + EXPECT(assert_equal(zeros, logIdentity)); + + Similarity3 expZero = Similarity3::Expmap(zeros); + Similarity3 ident = Similarity3::identity(); + EXPECT(assert_equal(expZero, ident)); + + // Compare to matrix exponential, using expm in Lie.h + EXPECT( + assert_equal(expm(delta), Similarity3::Expmap(delta), 1e-3)); +} + +//****************************************************************************** +// Group action on Point3 (with simpler transform) +TEST(Similarity3, GroupAction) { + EXPECT(assert_equal(Point3(2, 2, 0), T6 * Point3(0, 0, 0))); + EXPECT(assert_equal(Point3(4, 2, 0), T6 * Point3(1, 0, 0))); + + // Test group action on R^4 via matrix representation + Vector4 qh; + qh << 1, 0, 0, 1; + Vector4 ph; + ph << 2, 1, 0, 0.5; // equivalent to Point3(4, 2, 0) + EXPECT(assert_equal((Vector )ph, T6.matrix() * qh)); + + // Test some more... + Point3 pa = Point3(1, 0, 0); + Similarity3 Ta(Rot3(), Point3(1, 2, 3), 1.0); + Similarity3 Tb(Rot3(), Point3(1, 2, 3), 2.0); + EXPECT(assert_equal(Point3(2, 2, 3), Ta.transformFrom(pa))); + EXPECT(assert_equal(Point3(4, 4, 6), Tb.transformFrom(pa))); + + Similarity3 Tc(Rot3::Rz(M_PI / 2.0), Point3(1, 2, 3), 1.0); + Similarity3 Td(Rot3::Rz(M_PI / 2.0), Point3(1, 2, 3), 2.0); + EXPECT(assert_equal(Point3(1, 3, 3), Tc.transformFrom(pa))); + EXPECT(assert_equal(Point3(2, 6, 6), Td.transformFrom(pa))); + + // Test derivative + boost::function f = boost::bind( + &Similarity3::transformFrom, _1, _2, boost::none, boost::none); + + Point3 q(1, 2, 3); + for (const auto T : { T1, T2, T3, T4, T5, T6 }) { + Point3 q(1, 0, 0); + Matrix H1 = numericalDerivative21(f, T, q); + Matrix H2 = numericalDerivative22(f, T, q); + Matrix actualH1, actualH2; + T.transformFrom(q, actualH1, actualH2); + EXPECT(assert_equal(H1, actualH1)); + EXPECT(assert_equal(H2, actualH2)); + } +} + +//****************************************************************************** +// Test very simple prior optimization example +TEST(Similarity3, Optimization) { + // Create a PriorFactor with a Sim3 prior + Similarity3 prior = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4); + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1); + Symbol key('x', 1); + PriorFactor factor(key, prior, model); + + // Create graph + NonlinearFactorGraph graph; + graph.push_back(factor); + + // Create initial estimate with identity transform + Values initial; + initial.insert(key, Similarity3()); + + // Optimize + Values result; + LevenbergMarquardtParams params; + params.setVerbosityLM("TRYCONFIG"); + result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + + // After optimization, result should be prior + EXPECT(assert_equal(prior, result.at(key), 1e-4)); +} + +//****************************************************************************** +// Test optimization with both Prior and BetweenFactors +TEST(Similarity3, Optimization2) { + Similarity3 prior = Similarity3(); + Similarity3 m1 = Similarity3(Rot3::Ypr(M_PI / 4.0, 0, 0), Point3(2.0, 0, 0), + 1.0); + Similarity3 m2 = Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0), + Point3(sqrt(8) * 0.9, 0, 0), 1.0); + Similarity3 m3 = Similarity3(Rot3::Ypr(3 * M_PI / 4.0, 0, 0), + Point3(sqrt(32) * 0.8, 0, 0), 1.0); + Similarity3 m4 = Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0), + Point3(6 * 0.7, 0, 0), 1.0); + Similarity3 loop = Similarity3(1.42); + + //prior.print("Goal Transform"); + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, + 0.01); + SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas( + (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished()); + SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas( + (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished()); + PriorFactor factor(X(1), prior, model); // Prior ! + BetweenFactor b1(X(1), X(2), m1, betweenNoise); + BetweenFactor b2(X(2), X(3), m2, betweenNoise); + BetweenFactor b3(X(3), X(4), m3, betweenNoise); + BetweenFactor b4(X(4), X(5), m4, betweenNoise); + BetweenFactor lc(X(5), X(1), loop, betweenNoise2); + + // Create graph + NonlinearFactorGraph graph; + graph.push_back(factor); + graph.push_back(b1); + graph.push_back(b2); + graph.push_back(b3); + graph.push_back(b4); + graph.push_back(lc); + + //graph.print("Full Graph\n"); + Values initial; + initial.insert(X(1), Similarity3()); + initial.insert(X(2), + Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0), Point3(1, 0, 0), 1.1)); + initial.insert(X(3), + Similarity3(Rot3::Ypr(2.0 * M_PI / 2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2)); + initial.insert(X(4), + Similarity3(Rot3::Ypr(3.0 * M_PI / 2.0, 0, 0), Point3(0, 1, 0), 1.3)); + initial.insert(X(5), + Similarity3(Rot3::Ypr(4.0 * M_PI / 2.0, 0, 0), Point3(0, 0, 0), 1.0)); + + //initial.print("Initial Estimate\n"); + + Values result; + result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + //result.print("Optimized Estimate\n"); + Pose3 p1, p2, p3, p4, p5; + p1 = Pose3(result.at(X(1))); + p2 = Pose3(result.at(X(2))); + p3 = Pose3(result.at(X(3))); + p4 = Pose3(result.at(X(4))); + p5 = Pose3(result.at(X(5))); + + //p1.print("Pose1"); + //p2.print("Pose2"); + //p3.print("Pose3"); + //p4.print("Pose4"); + //p5.print("Pose5"); + + Similarity3 expected(0.7); + EXPECT(assert_equal(expected, result.at(X(5)), 0.4)); +} + +//****************************************************************************** +// Align points (p,q) assuming that p = T*q + noise +TEST(Similarity3, AlignScaledPointClouds) { +// Create ground truth + Point3 q1(0, 0, 0), q2(1, 0, 0), q3(0, 1, 0); + + // Create transformed cloud (noiseless) +// Point3 p1 = T4 * q1, p2 = T4 * q2, p3 = T4 * q3; + + // Create an unknown expression + Expression unknownT(0); // use key 0 + + // Create constant expressions for the ground truth points + Expression q1_(q1), q2_(q2), q3_(q3); + + // Create prediction expressions + Expression predict1(unknownT, &Similarity3::transformFrom, q1_); + Expression predict2(unknownT, &Similarity3::transformFrom, q2_); + Expression predict3(unknownT, &Similarity3::transformFrom, q3_); + +//// Create Expression factor graph +// ExpressionFactorGraph graph; +// graph.addExpressionFactor(predict1, p1, R); // |T*q1 - p1| +// graph.addExpressionFactor(predict2, p2, R); // |T*q2 - p2| +// graph.addExpressionFactor(predict3, p3, R); // |T*q3 - p3| +} + +//****************************************************************************** +TEST(Similarity3 , Invariants) { + Similarity3 id; + + EXPECT(check_group_invariants(id, id)); + EXPECT(check_group_invariants(id, T3)); + EXPECT(check_group_invariants(T2, id)); + EXPECT(check_group_invariants(T2, T3)); + + EXPECT(check_manifold_invariants(id, id)); + EXPECT(check_manifold_invariants(id, T3)); + EXPECT(check_manifold_invariants(T2, id)); + EXPECT(check_manifold_invariants(T2, T3)); +} + +//****************************************************************************** +TEST(Similarity3 , LieGroupDerivatives) { + Similarity3 id; + + CHECK_LIE_GROUP_DERIVATIVES(id, id); + CHECK_LIE_GROUP_DERIVATIVES(id, T2); + CHECK_LIE_GROUP_DERIVATIVES(T2, id); + CHECK_LIE_GROUP_DERIVATIVES(T2, T3); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** + diff --git a/gtsam_unstable/linear/ActiveSetSolver.h b/gtsam_unstable/linear/ActiveSetSolver.h index a5c60f311..8c3c5a7e5 100644 --- a/gtsam_unstable/linear/ActiveSetSolver.h +++ b/gtsam_unstable/linear/ActiveSetSolver.h @@ -28,7 +28,7 @@ namespace gtsam { * This class implements the active set algorithm for solving convex * Programming problems. * - * @tparam PROBLEM Type of the problem to solve, e.g. LP (linear program) or + * @tparam PROBLEM Type of the problem to solve, e.g. LP (linear program) or * QP (quadratic program). * @tparam POLICY specific detail policy tailored for the particular program * @tparam INITSOLVER Solver for an initial feasible solution of this problem. @@ -70,7 +70,7 @@ protected: dual graphs */ /// Vector of key matrix pairs. Matrices are usually the A term for a factor. - typedef std::vector > TermsContainer; + typedef std::vector > TermsContainer; public: /// Constructor @@ -100,18 +100,18 @@ public: protected: /** - * Compute minimum step size alpha to move from the current point @p xk to the - * next feasible point along a direction @p p: x' = xk + alpha*p, - * where alpha \in [0,maxAlpha]. - * + * Compute minimum step size alpha to move from the current point @p xk to the + * next feasible point along a direction @p p: x' = xk + alpha*p, + * where alpha \in [0,maxAlpha]. + * * For QP, maxAlpha = 1. For LP: maxAlpha = Inf. * * @return a tuple of (minAlpha, closestFactorIndex) where closestFactorIndex - * is the closest inactive inequality constraint that blocks xk to move - * further and that has the minimum alpha, or (-1, maxAlpha) if there is no + * is the closest inactive inequality constraint that blocks xk to move + * further and that has the minimum alpha, or (-1, maxAlpha) if there is no * such inactive blocking constraint. - * - * If there is a blocking constraint, the closest one will be added to the + * + * If there is a blocking constraint, the closest one will be added to the * working set and become active in the next iteration. */ boost::tuple computeStepSize( @@ -119,15 +119,15 @@ protected: const VectorValues& p, const double& maxAlpha) const; /** - * Finds the active constraints in the given factor graph and returns the + * Finds the active constraints in the given factor graph and returns the * Dual Jacobians used to build a dual factor graph. */ template TermsContainer collectDualJacobians(Key key, const FactorGraph& graph, const VariableIndex& variableIndex) const { /* - * Iterates through each factor in the factor graph and checks - * whether it's active. If the factor is active it reutrns the A + * Iterates through each factor in the factor graph and checks + * whether it's active. If the factor is active it reutrns the A * term of the factor. */ TermsContainer Aterms; @@ -167,7 +167,7 @@ public: /// Just for testing... GaussianFactorGraph buildWorkingGraph( const InequalityFactorGraph& workingSet, const VectorValues& xk = VectorValues()) const; - + /// Iterate 1 step, return a new state with a new workingSet and values State iterate(const State& state) const; diff --git a/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h b/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h index f7c5a5c79..2184e9f52 100644 --- a/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h +++ b/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h @@ -16,7 +16,7 @@ * @date 1/24/16 */ -#pragma once +#pragma once namespace gtsam { diff --git a/gtsam_unstable/linear/LPSolver.cpp b/gtsam_unstable/linear/LPSolver.cpp index c1319a5ec..59b55cebd 100644 --- a/gtsam_unstable/linear/LPSolver.cpp +++ b/gtsam_unstable/linear/LPSolver.cpp @@ -11,7 +11,7 @@ /** * @file LPSolver.cpp - * @brief + * @brief * @author Duy Nguyen Ta * @author Ivan Dario Jimenez * @date 1/26/16 diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h index 153aa7fda..460b4b7ee 100644 --- a/gtsam_unstable/linear/LPSolver.h +++ b/gtsam_unstable/linear/LPSolver.h @@ -58,7 +58,7 @@ struct LPPolicy { allKeys.merge(lp.equalities.keys()); allKeys.merge(KeySet(lp.cost.keys())); // Add corresponding factors for all variables that are not explicitly in - // the cost function. Gradients of the cost function wrt to these variables + // the cost function. Gradients of the cost function wrt to these variables // are zero (g=0), so b=xk if (lp.cost.keys().size() != allKeys.size()) { KeySet difference; diff --git a/gtsam_unstable/linear/QPSParser.cpp b/gtsam_unstable/linear/QPSParser.cpp index a6fc072c7..3039185f2 100644 --- a/gtsam_unstable/linear/QPSParser.cpp +++ b/gtsam_unstable/linear/QPSParser.cpp @@ -17,56 +17,440 @@ #define BOOST_SPIRIT_USE_PHOENIX_V3 1 +#include +#include +#include +#include #include #include -#include -#include +#include +#include #include #include #include +#include + +#include +#include +#include +#include +#include +#include + +using boost::fusion::at_c; +using namespace std; namespace bf = boost::fusion; namespace qi = boost::spirit::qi; +using Chars = std::vector; + +// Get a string from a fusion vector of Chars +template +static string fromChars(const FusionVector &vars) { + const Chars &chars = at_c(vars); + return string(chars.begin(), chars.end()); +} + namespace gtsam { + +/** + * As the parser reads a file, it call functions in this visitor. This visitor + * in turn stores what the parser has read in a way that can be later used to + * build the full QP problem in the file. + */ +class QPSVisitor { + private: + typedef std::unordered_map coefficient_v; + typedef std::unordered_map constraint_v; + + std::unordered_map + row_to_constraint_v; // Maps QPS ROWS to Variable-Matrix pairs + constraint_v E; // Equalities + constraint_v IG; // Inequalities >= + constraint_v IL; // Inequalities <= + unsigned int numVariables; + std::unordered_map + b; // maps from constraint name to b value for Ax = b equality + // constraints + std::unordered_map + ranges; // Inequalities can be specified as ranges on a variable + std::unordered_map g; // linear term of quadratic cost + std::unordered_map + varname_to_key; // Variable QPS string name to key + std::unordered_map> + H; // H from hessian + double f; // Constant term of quadratic cost + std::string obj_name; // the objective function has a name in the QPS + std::string name_; // the quadratic program has a name in the QPS + std::unordered_map + up; // Upper Bound constraints on variable where X < MAX + std::unordered_map + lo; // Lower Bound constraints on variable where MIN < X + std::unordered_map + fx; // Equalities specified as FX in BOUNDS part of QPS + KeyVector free; // Variables can be specified as free (to which no + // constraints apply) + const bool debug = false; + + public: + QPSVisitor() : numVariables(1) {} + + void setName(boost::fusion::vector const &name) { + name_ = fromChars<1>(name); + if (debug) { + cout << "Parsing file: " << name_ << endl; + } + } + + void addColumn(boost::fusion::vector const &vars) { + string var_ = fromChars<1>(vars); + string row_ = fromChars<3>(vars); + Matrix11 coefficient = at_c<5>(vars) * I_1x1; + if (debug) { + cout << "Added Column for Var: " << var_ << " Row: " << row_ + << " Coefficient: " << coefficient << endl; + } + if (!varname_to_key.count(var_)) + varname_to_key[var_] = Symbol('X', numVariables++); + if (row_ == obj_name) { + g[varname_to_key[var_]] = coefficient; + return; + } + (*row_to_constraint_v[row_])[row_][varname_to_key[var_]] = coefficient; + } + + void addColumnDouble( + boost::fusion::vector const &vars) { + string var_ = fromChars<0>(vars); + string row1_ = fromChars<2>(vars); + string row2_ = fromChars<6>(vars); + Matrix11 coefficient1 = at_c<4>(vars) * I_1x1; + Matrix11 coefficient2 = at_c<8>(vars) * I_1x1; + if (!varname_to_key.count(var_)) + varname_to_key.insert({var_, Symbol('X', numVariables++)}); + if (row1_ == obj_name) + g[varname_to_key[var_]] = coefficient1; + else + (*row_to_constraint_v[row1_])[row1_][varname_to_key[var_]] = coefficient1; + if (row2_ == obj_name) + g[varname_to_key[var_]] = coefficient2; + else + (*row_to_constraint_v[row2_])[row2_][varname_to_key[var_]] = coefficient2; + } + + void addRangeSingle(boost::fusion::vector const &vars) { + string var_ = fromChars<1>(vars); + string row_ = fromChars<3>(vars); + double range = at_c<5>(vars); + ranges[row_] = range; + if (debug) { + cout << "SINGLE RANGE ADDED" << endl; + cout << "VAR:" << var_ << " ROW: " << row_ << " RANGE: " << range << endl; + } + } + void addRangeDouble( + boost::fusion::vector const &vars) { + string var_ = fromChars<1>(vars); + string row1_ = fromChars<3>(vars); + string row2_ = fromChars<7>(vars); + double range1 = at_c<5>(vars); + double range2 = at_c<9>(vars); + ranges[row1_] = range1; + ranges[row2_] = range2; + if (debug) { + cout << "DOUBLE RANGE ADDED" << endl; + cout << "VAR: " << var_ << " ROW1: " << row1_ << " RANGE1: " << range1 + << " ROW2: " << row2_ << " RANGE2: " << range2 << endl; + } + } + + void addRHS(boost::fusion::vector const &vars) { + string var_ = fromChars<1>(vars); + string row_ = fromChars<3>(vars); + double coefficient = at_c<5>(vars); + if (row_ == obj_name) + f = -coefficient; + else + b[row_] = coefficient; + + if (debug) { + cout << "Added RHS for Var: " << var_ << " Row: " << row_ + << " Coefficient: " << coefficient << endl; + } + } + + void addRHSDouble( + boost::fusion::vector const &vars) { + string var_ = fromChars<1>(vars); + string row1_ = fromChars<3>(vars); + string row2_ = fromChars<7>(vars); + double coefficient1 = at_c<5>(vars); + double coefficient2 = at_c<9>(vars); + if (row1_ == obj_name) + f = -coefficient1; + else + b[row1_] = coefficient1; + + if (row2_ == obj_name) + f = -coefficient2; + else + b[row2_] = coefficient2; + + if (debug) { + cout << "Added RHS for Var: " << var_ << " Row: " << row1_ + << " Coefficient: " << coefficient1 << endl; + cout << " " + << "Row: " << row2_ << " Coefficient: " << coefficient2 << endl; + } + } + + void addRow( + boost::fusion::vector const &vars) { + string name_ = fromChars<3>(vars); + char type = at_c<1>(vars); + switch (type) { + case 'N': + obj_name = name_; + break; + case 'L': + row_to_constraint_v[name_] = &IL; + break; + case 'G': + row_to_constraint_v[name_] = &IG; + break; + case 'E': + row_to_constraint_v[name_] = &E; + break; + default: + cout << "invalid type: " << type << endl; + break; + } + if (debug) { + cout << "Added Row Type: " << type << " Name: " << name_ << endl; + } + } + + void addBound(boost::fusion::vector const &vars) { + string type_ = fromChars<1>(vars); + string var_ = fromChars<5>(vars); + double number = at_c<7>(vars); + if (type_.compare(string("UP")) == 0) + up[varname_to_key[var_]] = number; + else if (type_.compare(string("LO")) == 0) + lo[varname_to_key[var_]] = number; + else if (type_.compare(string("FX")) == 0) + fx[varname_to_key[var_]] = number; + else + cout << "Invalid Bound Type: " << type_ << endl; + + if (debug) { + cout << "Added Bound Type: " << type_ << " Var: " << var_ + << " Amount: " << number << endl; + } + } + + void addFreeBound(boost::fusion::vector const &vars) { + string type_ = fromChars<1>(vars); + string var_ = fromChars<5>(vars); + free.push_back(varname_to_key[var_]); + if (debug) { + cout << "Added Free Bound Type: " << type_ << " Var: " << var_ + << " Amount: " << endl; + } + } + + void addQuadTerm(boost::fusion::vector const &vars) { + string var1_ = fromChars<1>(vars); + string var2_ = fromChars<3>(vars); + Matrix11 coefficient = at_c<5>(vars) * I_1x1; + + H[varname_to_key[var1_]][varname_to_key[var2_]] = coefficient; + H[varname_to_key[var2_]][varname_to_key[var1_]] = coefficient; + if (debug) { + cout << "Added QuadTerm for Var: " << var1_ << " Row: " << var2_ + << " Coefficient: " << coefficient << endl; + } + } + + QP makeQP() { + // Create the keys from the variable names + KeyVector keys; + for (auto kv : varname_to_key) { + keys.push_back(kv.second); + } + + // Fill the G matrices and g vectors from + vector Gs; + vector gs; + sort(keys.begin(), keys.end()); + for (size_t i = 0; i < keys.size(); ++i) { + for (size_t j = i; j < keys.size(); ++j) { + if (H.count(keys[i]) > 0 && H[keys[i]].count(keys[j]) > 0) { + Gs.emplace_back(H[keys[i]][keys[j]]); + } else { + Gs.emplace_back(Z_1x1); + } + } + } + for (Key key1 : keys) { + if (g.count(key1) > 0) { + gs.emplace_back(-g[key1]); + } else { + gs.emplace_back(Z_1x1); + } + } + + // Construct the quadratic program + QP madeQP; + auto obj = HessianFactor(keys, Gs, gs, 2 * f); + madeQP.cost.push_back(obj); + + // Add equality and inequality constraints into the QP + size_t dual_key_num = keys.size() + 1; + for (auto kv : E) { + map keyMatrixMapPos; + map keyMatrixMapNeg; + if (ranges.count(kv.first) == 1) { + for (auto km : kv.second) { + keyMatrixMapPos.insert(km); + km.second = -km.second; + keyMatrixMapNeg.insert(km); + } + if (ranges[kv.first] > 0) { + madeQP.inequalities.push_back( + LinearInequality(keyMatrixMapNeg, -b[kv.first], dual_key_num++)); + madeQP.inequalities.push_back(LinearInequality( + keyMatrixMapPos, b[kv.first] + ranges[kv.first], dual_key_num++)); + } else if (ranges[kv.first] < 0) { + madeQP.inequalities.push_back( + LinearInequality(keyMatrixMapPos, b[kv.first], dual_key_num++)); + madeQP.inequalities.push_back(LinearInequality( + keyMatrixMapNeg, ranges[kv.first] - b[kv.first], dual_key_num++)); + } else { + cerr << "ERROR: CANNOT ADD A RANGE OF ZERO" << endl; + throw; + } + continue; + } + map keyMatrixMap; + for (auto km : kv.second) { + keyMatrixMap.insert(km); + } + madeQP.equalities.push_back( + LinearEquality(keyMatrixMap, b[kv.first] * I_1x1, dual_key_num++)); + } + + for (auto kv : IG) { + map keyMatrixMapNeg; + map keyMatrixMapPos; + for (auto km : kv.second) { + keyMatrixMapPos.insert(km); + km.second = -km.second; + keyMatrixMapNeg.insert(km); + } + madeQP.inequalities.push_back( + LinearInequality(keyMatrixMapNeg, -b[kv.first], dual_key_num++)); + if (ranges.count(kv.first) == 1) { + madeQP.inequalities.push_back(LinearInequality( + keyMatrixMapPos, b[kv.first] + ranges[kv.first], dual_key_num++)); + } + } + + for (auto kv : IL) { + map keyMatrixMapPos; + map keyMatrixMapNeg; + for (auto km : kv.second) { + keyMatrixMapPos.insert(km); + km.second = -km.second; + keyMatrixMapNeg.insert(km); + } + madeQP.inequalities.push_back( + LinearInequality(keyMatrixMapPos, b[kv.first], dual_key_num++)); + if (ranges.count(kv.first) == 1) { + madeQP.inequalities.push_back(LinearInequality( + keyMatrixMapNeg, ranges[kv.first] - b[kv.first], dual_key_num++)); + } + } + + for (Key k : keys) { + if (find(free.begin(), free.end(), k) != free.end()) continue; + if (fx.count(k) == 1) + madeQP.equalities.push_back( + LinearEquality(k, I_1x1, fx[k] * I_1x1, dual_key_num++)); + if (up.count(k) == 1) + madeQP.inequalities.push_back( + LinearInequality(k, I_1x1, up[k], dual_key_num++)); + if (lo.count(k) == 1) + madeQP.inequalities.push_back( + LinearInequality(k, -I_1x1, -lo[k], dual_key_num++)); + else + madeQP.inequalities.push_back( + LinearInequality(k, -I_1x1, 0, dual_key_num++)); + } + return madeQP; + } +}; + typedef qi::grammar> base_grammar; -struct QPSParser::MPSGrammar: base_grammar { +struct QPSParser::MPSGrammar : base_grammar { typedef std::vector Chars; - RawQP * rqp_; - boost::function const&)> setName; - boost::function const &)> addRow; - boost::function< - void(bf::vector const &)> rhsSingle; - boost::function< - void( - bf::vector)> rhsDouble; - boost::function< - void(bf::vector)> colSingle; - boost::function< - void( - bf::vector const &)> colDouble; - boost::function< - void(bf::vector const &)> addQuadTerm; - boost::function< - void( - bf::vector const &)> addBound; - boost::function< - void(bf::vector const &)> addBoundFr; - MPSGrammar(RawQP * rqp) : - base_grammar(start), rqp_(rqp), setName( - boost::bind(&RawQP::setName, rqp, ::_1)), addRow( - boost::bind(&RawQP::addRow, rqp, ::_1)), rhsSingle( - boost::bind(&RawQP::addRHS, rqp, ::_1)), rhsDouble( - boost::bind(&RawQP::addRHSDouble, rqp, ::_1)), colSingle( - boost::bind(&RawQP::addColumn, rqp, ::_1)), colDouble( - boost::bind(&RawQP::addColumnDouble, rqp, ::_1)), addQuadTerm( - boost::bind(&RawQP::addQuadTerm, rqp, ::_1)), addBound( - boost::bind(&RawQP::addBound, rqp, ::_1)), addBoundFr( - boost::bind(&RawQP::addBoundFr, rqp, ::_1)) { + QPSVisitor *rqp_; + boost::function const &)> setName; + boost::function const &)> + addRow; + boost::function const &)> + rhsSingle; + boost::function)> + rhsDouble; + boost::function const &)> + rangeSingle; + boost::function)> + rangeDouble; + boost::function)> + colSingle; + boost::function const &)> + colDouble; + boost::function const &)> + addQuadTerm; + boost::function const &)> + addBound; + boost::function const &)> + addFreeBound; + MPSGrammar(QPSVisitor *rqp) + : base_grammar(start), + rqp_(rqp), + setName(boost::bind(&QPSVisitor::setName, rqp, ::_1)), + addRow(boost::bind(&QPSVisitor::addRow, rqp, ::_1)), + rhsSingle(boost::bind(&QPSVisitor::addRHS, rqp, ::_1)), + rhsDouble(boost::bind(&QPSVisitor::addRHSDouble, rqp, ::_1)), + rangeSingle(boost::bind(&QPSVisitor::addRangeSingle, rqp, ::_1)), + rangeDouble(boost::bind(&QPSVisitor::addRangeDouble, rqp, ::_1)), + colSingle(boost::bind(&QPSVisitor::addColumn, rqp, ::_1)), + colDouble(boost::bind(&QPSVisitor::addColumnDouble, rqp, ::_1)), + addQuadTerm(boost::bind(&QPSVisitor::addQuadTerm, rqp, ::_1)), + addBound(boost::bind(&QPSVisitor::addBound, rqp, ::_1)), + addFreeBound(boost::bind(&QPSVisitor::addFreeBound, rqp, ::_1)) { using namespace boost::spirit; using namespace boost::spirit::qi; character = lexeme[alnum | '_' | '-' | '.']; @@ -74,43 +458,54 @@ struct QPSParser::MPSGrammar: base_grammar { word = lexeme[+character]; name = lexeme[lit("NAME") >> *blank >> title >> +space][setName]; row = lexeme[*blank >> character >> +blank >> word >> *blank][addRow]; - rhs_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ - >> *blank][rhsSingle]; - rhs_double = lexeme[(*blank >> word >> +blank >> word >> +blank >> double_ - >> +blank >> word >> +blank >> double_)[rhsDouble] >> *blank]; - col_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ - >> *blank][colSingle]; - col_double = lexeme[*blank - >> (word >> +blank >> word >> +blank >> double_ >> +blank >> word - >> +blank >> double_)[colDouble] >> *blank]; - quad_l = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ - >> *blank][addQuadTerm]; - bound = lexeme[(*blank >> word >> +blank >> word >> +blank >> word >> +blank - >> double_)[addBound] >> *blank]; - bound_fr = lexeme[*blank >> word >> +blank >> word >> +blank >> word - >> *blank][addBoundFr]; + rhs_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ >> + *blank][rhsSingle]; + rhs_double = + lexeme[(*blank >> word >> +blank >> word >> +blank >> double_ >> + +blank >> word >> +blank >> double_)[rhsDouble] >> + *blank]; + range_single = lexeme[*blank >> word >> +blank >> word >> +blank >> + double_ >> *blank][rangeSingle]; + range_double = + lexeme[(*blank >> word >> +blank >> word >> +blank >> double_ >> + +blank >> word >> +blank >> double_)[rangeDouble] >> + *blank]; + col_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ >> + *blank][colSingle]; + col_double = + lexeme[*blank >> (word >> +blank >> word >> +blank >> double_ >> + +blank >> word >> +blank >> double_)[colDouble] >> + *blank]; + quad_l = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ >> + *blank][addQuadTerm]; + bound = lexeme[(*blank >> word >> +blank >> word >> +blank >> word >> + +blank >> double_)[addBound] >> + *blank]; + bound_fr = lexeme[*blank >> word >> +blank >> word >> +blank >> word >> + *blank][addFreeBound]; rows = lexeme[lit("ROWS") >> *blank >> eol >> +(row >> eol)]; - rhs = lexeme[lit("RHS") >> *blank >> eol - >> +((rhs_double | rhs_single) >> eol)]; - cols = lexeme[lit("COLUMNS") >> *blank >> eol - >> +((col_double | col_single) >> eol)]; + rhs = lexeme[lit("RHS") >> *blank >> eol >> + +((rhs_double | rhs_single) >> eol)]; + cols = lexeme[lit("COLUMNS") >> *blank >> eol >> + +((col_double | col_single) >> eol)]; quad = lexeme[lit("QUADOBJ") >> *blank >> eol >> +(quad_l >> eol)]; - bounds = lexeme[lit("BOUNDS") >> +space >> +((bound | bound_fr) >> eol)]; - ranges = lexeme[lit("RANGES") >> +space]; + bounds = lexeme[lit("BOUNDS") >> +space >> *((bound | bound_fr) >> eol)]; + ranges = lexeme[lit("RANGES") >> +space >> + *((range_double | range_single) >> eol)]; end = lexeme[lit("ENDATA") >> *space]; - start = lexeme[name >> rows >> cols >> rhs >> -ranges >> bounds >> quad - >> end]; + start = + lexeme[name >> rows >> cols >> rhs >> -ranges >> bounds >> quad >> end]; } qi::rule, char()> character; qi::rule, Chars()> word, title; - qi::rule > row, end, col_single, - col_double, rhs_single, rhs_double, ranges, bound, bound_fr, bounds, quad, - quad_l, rows, cols, rhs, name, start; + qi::rule> row, end, col_single, + col_double, rhs_single, rhs_double, range_single, range_double, ranges, + bound, bound_fr, bounds, quad, quad_l, rows, cols, rhs, name, start; }; QP QPSParser::Parse() { - RawQP rawData; + QPSVisitor rawData; std::fstream stream(fileName_.c_str()); stream.unsetf(std::ios::skipws); boost::spirit::basic_istream_iterator begin(stream); @@ -123,4 +518,4 @@ QP QPSParser::Parse() { return rawData.makeQP(); } -} +} // namespace gtsam diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index 9efc23a67..3854d2a15 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -32,9 +32,14 @@ struct QPPolicy { static constexpr double maxAlpha = 1.0; /// Simply the cost of the QP problem - static const GaussianFactorGraph& buildCostFunction( - const QP& qp, const VectorValues& xk = VectorValues()) { - return qp.cost; + static const GaussianFactorGraph buildCostFunction(const QP& qp, + const VectorValues& xk = VectorValues()) { + GaussianFactorGraph no_constant_factor; + for (auto factor : qp.cost) { + HessianFactor hf = static_cast(*factor); + no_constant_factor.push_back(hf); + } + return no_constant_factor; } }; diff --git a/gtsam_unstable/linear/RawQP.cpp b/gtsam_unstable/linear/RawQP.cpp deleted file mode 100644 index 47672a947..000000000 --- a/gtsam_unstable/linear/RawQP.cpp +++ /dev/null @@ -1,271 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file RawQP.cpp - * @brief - * @author Ivan Dario Jimenez - * @date 3/5/16 - */ - -#include -#include - -using boost::fusion::at_c; - -namespace gtsam { - -void RawQP::setName( - boost::fusion::vector, std::vector, - std::vector> const &name) { - name_ = std::string(at_c < 1 > (name).begin(), at_c < 1 > (name).end()); - if (debug) { - std::cout << "Parsing file: " << name_ << std::endl; - } -} - -void RawQP::addColumn( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector> const &vars) { - - std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); - std::string row_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); - Matrix11 coefficient = at_c < 5 > (vars) * I_1x1; - - if (!varname_to_key.count(var_)) - varname_to_key[var_] = Symbol('X', varNumber++); - if (row_ == obj_name) { - g[varname_to_key[var_]] = coefficient; - return; - } - (*row_to_constraint_v[row_])[row_][varname_to_key[var_]] = coefficient; - if (debug) { - std::cout << "Added Column for Var: " << var_ << " Row: " << row_ - << " Coefficient: " << coefficient << std::endl; - } - -} - -void RawQP::addColumnDouble( - boost::fusion::vector, std::vector, - std::vector, std::vector, double, std::vector, - std::vector, std::vector, double> const &vars) { - - std::string var_(at_c < 0 > (vars).begin(), at_c < 0 > (vars).end()); - std::string row1_(at_c < 2 > (vars).begin(), at_c < 2 > (vars).end()); - std::string row2_(at_c < 6 > (vars).begin(), at_c < 6 > (vars).end()); - Matrix11 coefficient1 = at_c < 4 > (vars) * I_1x1; - Matrix11 coefficient2 = at_c < 8 > (vars) * I_1x1; - if (!varname_to_key.count(var_)) - varname_to_key.insert( { var_, Symbol('X', varNumber++) }); - if (row1_ == obj_name) - g[varname_to_key[var_]] = coefficient1; - else - (*row_to_constraint_v[row1_])[row1_][varname_to_key[var_]] = coefficient1; - if (row2_ == obj_name) - g[varname_to_key[var_]] = coefficient2; - else - (*row_to_constraint_v[row2_])[row2_][varname_to_key[var_]] = coefficient2; -} - -void RawQP::addRHS( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector> const &vars) { - - std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); - std::string row_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); - double coefficient = at_c < 5 > (vars); - if (row_ == obj_name) - f = -coefficient; - else - b[row_] = coefficient; - - if (debug) { - std::cout << "Added RHS for Var: " << var_ << " Row: " << row_ - << " Coefficient: " << coefficient << std::endl; - } -} - -void RawQP::addRHSDouble( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector, std::vector, std::vector, double> const &vars) { - - std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); - std::string row1_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); - std::string row2_(at_c < 7 > (vars).begin(), at_c < 7 > (vars).end()); - double coefficient1 = at_c < 5 > (vars); - double coefficient2 = at_c < 9 > (vars); - if (row1_ == obj_name) - f = -coefficient1; - else - b[row1_] = coefficient1; - - if (row2_ == obj_name) - f = -coefficient2; - else - b[row2_] = coefficient2; - - if (debug) { - std::cout << "Added RHS for Var: " << var_ << " Row: " << row1_ - << " Coefficient: " << coefficient1 << std::endl; - std::cout << " " << "Row: " << row2_ - << " Coefficient: " << coefficient2 << std::endl; - } -} - -void RawQP::addRow( - boost::fusion::vector, char, std::vector, - std::vector, std::vector> const &vars) { - - std::string name_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); - char type = at_c < 1 > (vars); - switch (type) { - case 'N': - obj_name = name_; - break; - case 'L': - row_to_constraint_v[name_] = &IL; - break; - case 'G': - row_to_constraint_v[name_] = &IG; - break; - case 'E': - row_to_constraint_v[name_] = &E; - break; - default: - std::cout << "invalid type: " << type << std::endl; - break; - } - if (debug) { - std::cout << "Added Row Type: " << type << " Name: " << name_ << std::endl; - } -} - -void RawQP::addBound( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, - std::vector, std::vector, double> const &vars) { - - std::string type_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); - std::string var_(at_c < 5 > (vars).begin(), at_c < 5 > (vars).end()); - double number = at_c < 7 > (vars); - if (type_.compare(std::string("UP")) == 0) - up[varname_to_key[var_]] = number; - else if (type_.compare(std::string("LO")) == 0) - lo[varname_to_key[var_]] = number; - else - std::cout << "Invalid Bound Type: " << type_ << std::endl; - - if (debug) { - std::cout << "Added Bound Type: " << type_ << " Var: " << var_ - << " Amount: " << number << std::endl; - } -} - -void RawQP::addBoundFr( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, - std::vector, std::vector> const &vars) { - std::string type_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); - std::string var_(at_c < 5 > (vars).begin(), at_c < 5 > (vars).end()); - Free.push_back(varname_to_key[var_]); - if (debug) { - std::cout << "Added Free Bound Type: " << type_ << " Var: " << var_ - << " Amount: " << std::endl; - } -} - -void RawQP::addQuadTerm( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector> const &vars) { - std::string var1_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); - std::string var2_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); - Matrix11 coefficient = at_c < 5 > (vars) * I_1x1; - - H[varname_to_key[var1_]][varname_to_key[var2_]] = coefficient; - H[varname_to_key[var2_]][varname_to_key[var1_]] = coefficient; - if (debug) { - std::cout << "Added QuadTerm for Var: " << var1_ << " Row: " << var2_ - << " Coefficient: " << coefficient << std::endl; - } -} - -QP RawQP::makeQP() { - KeyVector keys; - std::vector Gs; - std::vector gs; - for (auto kv : varname_to_key) { - keys.push_back(kv.second); - } - std::sort(keys.begin(), keys.end()); - for (unsigned int i = 0; i < keys.size(); ++i) { - for (unsigned int j = i; j < keys.size(); ++j) { - Gs.push_back(H[keys[i]][keys[j]]); - } - } - for (Key key1 : keys) { - gs.push_back(-g[key1]); - } - int dual_key_num = keys.size() + 1; - QP madeQP; - auto obj = HessianFactor(keys, Gs, gs, f); - - madeQP.cost.push_back(obj); - - for (auto kv : E) { - std::map keyMatrixMap; - for (auto km : kv.second) { - keyMatrixMap.insert(km); - } - madeQP.equalities.push_back( - LinearEquality(keyMatrixMap, b[kv.first] * I_1x1, dual_key_num++)); - } - - for (auto kv : IG) { - std::map keyMatrixMap; - for (auto km : kv.second) { - km.second = -km.second; - keyMatrixMap.insert(km); - } - madeQP.inequalities.push_back( - LinearInequality(keyMatrixMap, -b[kv.first], dual_key_num++)); - } - - for (auto kv : IL) { - std::map keyMatrixMap; - for (auto km : kv.second) { - keyMatrixMap.insert(km); - } - madeQP.inequalities.push_back( - LinearInequality(keyMatrixMap, b[kv.first], dual_key_num++)); - } - - for (Key k : keys) { - if (std::find(Free.begin(), Free.end(), k) != Free.end()) - continue; - if (up.count(k) == 1) - madeQP.inequalities.push_back( - LinearInequality(k, I_1x1, up[k], dual_key_num++)); - if (lo.count(k) == 1) - madeQP.inequalities.push_back( - LinearInequality(k, -I_1x1, lo[k], dual_key_num++)); - else - madeQP.inequalities.push_back( - LinearInequality(k, -I_1x1, 0, dual_key_num++)); - } - return madeQP; -} -} - diff --git a/gtsam_unstable/linear/RawQP.h b/gtsam_unstable/linear/RawQP.h deleted file mode 100644 index 70b2a9d27..000000000 --- a/gtsam_unstable/linear/RawQP.h +++ /dev/null @@ -1,106 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file RawQP.h - * @brief - * @author Ivan Dario Jimenez - * @date 3/5/16 - */ - -#pragma once - -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -namespace gtsam { -class RawQP { -private: - typedef std::unordered_map coefficient_v; - typedef std::unordered_map constraint_v; - - std::unordered_map row_to_constraint_v; - constraint_v E; - constraint_v IG; - constraint_v IL; - unsigned int varNumber; - std::unordered_map b; - std::unordered_map g; - std::unordered_map varname_to_key; - std::unordered_map > H; - double f; - std::string obj_name; - std::string name_; - std::unordered_map up; - std::unordered_map lo; - KeyVector Free; - const bool debug = false; - -public: - RawQP() : - row_to_constraint_v(), E(), IG(), IL(), varNumber(1), b(), g(), varname_to_key(), H(), f(), obj_name(), name_(), up(), lo(), Free() { - } - - void setName( - boost::fusion::vector, std::vector, - std::vector> const & name); - - void addColumn( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector> const & vars); - - void addColumnDouble( - boost::fusion::vector, std::vector, - std::vector, std::vector, double, std::vector, - std::vector, std::vector, double> const & vars); - - void addRHS( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector> const & vars); - - void addRHSDouble( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector, std::vector, std::vector, double> const & vars); - - void addRow( - boost::fusion::vector, char, std::vector, - std::vector, std::vector> const & vars); - - void addBound( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, - std::vector, std::vector, double> const & vars); - - void addBoundFr( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, - std::vector, std::vector> const & vars); - - void addQuadTerm( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector> const & vars); - - QP makeQP(); -} -; -} diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index bc9dd1f98..2292c63d7 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -14,6 +14,7 @@ * @brief Test simple QP solver for a linear inequality constraint * @date Apr 10, 2014 * @author Duy-Nguyen Ta + * @author Ivan Dario Jimenez */ #include @@ -218,7 +219,7 @@ pair testParser(QPSParser parser) { // min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2 expectedqp.cost.push_back( HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, -1.5 * kOne, 10.0 * I_1x1, - 2.0 * kOne, 4.0)); + 2.0 * kOne, 8.0)); // 2x + y >= 2 // -x + 2y <= 6 expectedqp.inequalities.push_back( @@ -269,6 +270,66 @@ TEST(QPSolver, QPExampleTest){ CHECK(assert_equal(error_expected, error_actual)) } +TEST(QPSolver, HS21) { + QP problem = QPSParser("HS21.QPS").Parse(); + VectorValues actualSolution; + VectorValues expectedSolution; + expectedSolution.insert(Symbol('X',1), 2.0*I_1x1); + expectedSolution.insert(Symbol('X',2), 0.0*I_1x1); + boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(-99.9599999, error_actual, 1e-7)) + CHECK(assert_equal(expectedSolution, actualSolution)) +} + +TEST(QPSolver, HS35) { + QP problem = QPSParser("HS35.QPS").Parse(); + VectorValues actualSolution; + boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(1.11111111e-01,error_actual, 1e-7)) +} + +TEST(QPSolver, HS35MOD) { + QP problem = QPSParser("HS35MOD.QPS").Parse(); + VectorValues actualSolution; + boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(2.50000001e-01,error_actual, 1e-7)) +} + +TEST(QPSolver, HS51) { + QP problem = QPSParser("HS51.QPS").Parse(); + VectorValues actualSolution; + boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(8.88178420e-16,error_actual, 1e-7)) +} + +TEST(QPSolver, HS52) { + QP problem = QPSParser("HS52.QPS").Parse(); + VectorValues actualSolution; + boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(5.32664756,error_actual, 1e-7)) +} + +TEST(QPSolver, HS268) { // This test needs an extra order of magnitude of tolerance than the rest + QP problem = QPSParser("HS268.QPS").Parse(); + VectorValues actualSolution; + boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(5.73107049e-07,error_actual, 1e-6)) +} + +TEST(QPSolver, QPTEST) { // REQUIRES Jacobian Fix + QP problem = QPSParser("QPTEST.QPS").Parse(); + VectorValues actualSolution; + boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(0.437187500e01,error_actual, 1e-7)) +} + /* ************************************************************************* */ // Create Matlab's test graph as in http://www.mathworks.com/help/optim/ug/quadprog.html QP createTestMatlabQPEx() { diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index ac521d9c3..4077464a9 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -315,7 +315,7 @@ void BatchFixedLagSmoother::marginalize(const KeyVector& marginalizeKeys) { set removedFactorSlots; const VariableIndex variableIndex(factors_); for(Key key: marginalizeKeys) { - const FastVector& slots = variableIndex[key]; + const auto& slots = variableIndex[key]; removedFactorSlots.insert(slots.begin(), slots.end()); } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 51a959075..97df375d5 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -536,7 +536,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { std::vector removedFactorSlots; VariableIndex variableIndex(factors_); for(Key key: keysToMove) { - const FastVector& slots = variableIndex[key]; + const auto& slots = variableIndex[key]; removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end()); } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index 53dd46fea..d04f579eb 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 0fc975958..775dccbb0 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -270,7 +270,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { while(true) { if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) std::cout << "trying lambda = " << lambda << std::endl; - + // Add prior factors at the current solution gttic(damp); GaussianFactorGraph dampedFactorGraph(linearFactorGraph); @@ -287,7 +287,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { } } gttoc(damp); - if (lmVerbosity >= LevenbergMarquardtParams::DAMPED) + if (lmVerbosity >= LevenbergMarquardtParams::DAMPED) dampedFactorGraph.print("damped"); result.lambdas++; @@ -298,9 +298,9 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { evalpoint = theta_.retract(newDelta); gttoc(solve); - if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) std::cout << "linear delta norm = " << newDelta.norm() << std::endl; - if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) + if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) newDelta.print("delta"); // Evaluate the new error @@ -308,9 +308,9 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { double error = factors_.error(evalpoint); gttoc(compute_error); - if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) std::cout << "next error = " << error << std::endl; - + if(error < result.error) { // Keep this change // Update the error value diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h index f3808686c..67a1ef4f3 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp index 70cb3e268..66d03bfa2 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h index b8a9941ad..42f82f52f 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index e7c8229ef..b70b9efc2 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -291,7 +291,7 @@ FactorIndices ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2& isam FactorIndices removedFactorSlots; const VariableIndex& variableIndex = isam2.getVariableIndex(); for(Key key: keys) { - const FastVector& slots = variableIndex[key]; + const auto& slots = variableIndex[key]; removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end()); } diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h index c28b3bcd1..e01919048 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp index 41a94b876..714d7c8d0 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h index 2d519bf25..ec95e1ec8 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp index e285955a7..1a86adbfa 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h index f9fa6033f..ece8cd2f6 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.h +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp index b673588cf..20a7beff5 100644 --- a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index 54ad22bcb..ff5a096b0 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index 8ecd087c5..47002acb6 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp index d6f6446e8..342e2e79f 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp index d7bddece2..98b4bf8cc 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index 82d8f2153..d4ebc7c0a 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp index 440400fb4..937108a67 100644 --- a/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index aafaecd2c..ab6c1298a 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index 0e22edf0f..2088ee646 100644 --- a/gtsam_unstable/partition/FindSeparator-inl.h +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -60,7 +60,7 @@ namespace gtsam { namespace partition { } // call metis parition routine - METIS_ComputeVertexSeparator(&n, xadj.get(), adjncy.get(), + METIS_ComputeVertexSeparator(&n, xadj.get(), adjncy.get(), vwgt, options, &sepsize, part_.get()); if (verbose) { @@ -95,7 +95,7 @@ namespace gtsam { namespace partition { ncon = graph->ncon; ctrl->ncuts = 1; - + /* determine the weights of the two partitions as a function of the weight of the target partition weights */ @@ -153,7 +153,7 @@ namespace gtsam { namespace partition { modefied_EdgeComputeSeparator(&n, xadj.get(), adjncy.get(), vwgt, adjwgt.get(), options, &edgecut, part_.get()); - + if (verbose) { //stoptimer(TOTALTmr); printf("\nTiming Information --------------------------------------------------\n"); @@ -198,7 +198,7 @@ namespace gtsam { namespace partition { std::cout << "index1: " << index1 << std::endl; std::cout << "index2: " << index2 << std::endl; // if both nodes are in the current graph, i.e. not a joint factor between frontal and separator - if (index1 >= 0 && index2 >= 0) { + if (index1 >= 0 && index2 >= 0) { std::pair& adjacencyMap1 = adjacencyMap[index1]; std::pair& adjacencyMap2 = adjacencyMap[index2]; try{ @@ -239,7 +239,7 @@ namespace gtsam { namespace partition { const std::vector& keys, WorkSpace& workspace, bool verbose) { // create a metis graph size_t numKeys = keys.size(); - if (verbose) + if (verbose) std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl; sharedInts xadj, adjncy, adjwgt; @@ -287,7 +287,7 @@ namespace gtsam { namespace partition { return result; } - /* *************************************************************************/ + /* *************************************************************************/ template boost::optional edgePartitionByMetis(const GenericGraph& graph, const std::vector& keys, WorkSpace& workspace, bool verbose) { @@ -492,7 +492,7 @@ namespace gtsam { namespace partition { const boost::optional >& int2symbol, const bool reduceGraph, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { - boost::optional result = findPartitoning(graph, keys, workspace, + boost::optional result = findPartitoning(graph, keys, workspace, verbose, int2symbol, reduceGraph); // find the island in A and B, and make them separated submaps diff --git a/gtsam_unstable/partition/GenericGraph.cpp b/gtsam_unstable/partition/GenericGraph.cpp index f941407e9..92f0266d0 100644 --- a/gtsam_unstable/partition/GenericGraph.cpp +++ b/gtsam_unstable/partition/GenericGraph.cpp @@ -353,8 +353,6 @@ namespace gtsam { namespace partition { void reduceGenericGraph(const GenericGraph3D& graph, const std::vector& cameraKeys, const std::vector& landmarkKeys, const std::vector& dictionary, GenericGraph3D& reducedGraph) { - typedef size_t CameraKey; - typedef pair CameraPair; typedef size_t LandmarkKey; // get a mapping from each landmark to its connected cameras vector > cameraToLandmarks(dictionary.size()); diff --git a/gtsam_unstable/partition/tests/testFindSeparator.cpp b/gtsam_unstable/partition/tests/testFindSeparator.cpp index 9706e4cc4..fe49de928 100644 --- a/gtsam_unstable/partition/tests/testFindSeparator.cpp +++ b/gtsam_unstable/partition/tests/testFindSeparator.cpp @@ -121,7 +121,7 @@ TEST ( Partition, edgePartitionByMetis2 ) CHECK(A_expected == actual->A); CHECK(B_expected == actual->B); CHECK(C_expected == actual->C); -} +} /* ************************************************************************* */ // x0 - x1 - x2 diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index b009927d6..104b3209d 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h index df1873765..f2bcb7cd7 100644 --- a/gtsam_unstable/slam/BiasedGPSFactor.h +++ b/gtsam_unstable/slam/BiasedGPSFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/DummyFactor.h b/gtsam_unstable/slam/DummyFactor.h index 0b0a1bd6c..9f749e9e5 100644 --- a/gtsam_unstable/slam/DummyFactor.h +++ b/gtsam_unstable/slam/DummyFactor.h @@ -2,7 +2,7 @@ * @file DummyFactor.h * * @brief A simple factor that can be used to trick gtsam solvers into believing a graph is connected. - * + * * @date Sep 10, 2012 * @author Alex Cunningham */ diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index 3041f5f23..83df9e13b 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -88,7 +88,7 @@ public: double theta = landmark(0), phi = landmark(1), rho = landmark(2); Point3 pose_P_landmark(cos(phi)*sin(theta)/rho, sin(phi)/rho, cos(phi)*cos(theta)/rho); // Convert the landmark to world coordinates - Point3 world_P_landmark = pose.transform_from(pose_P_landmark); + Point3 world_P_landmark = pose.transformFrom(pose_P_landmark); // Project landmark into Pose2 PinholeCamera camera(pose, *K_); return camera.project(world_P_landmark) - measured_; @@ -208,7 +208,7 @@ public: double theta = landmark(0), phi = landmark(1), rho = landmark(2); Point3 pose1_P_landmark(cos(phi)*sin(theta)/rho, sin(phi)/rho, cos(phi)*cos(theta)/rho); // Convert the landmark to world coordinates - Point3 world_P_landmark = pose1.transform_from(pose1_P_landmark); + Point3 world_P_landmark = pose1.transformFrom(pose1_P_landmark); // Project landmark into Pose2 PinholeCamera camera(pose2, *K_); return camera.project(world_P_landmark) - measured_; diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index d2a9110d9..03803b5f4 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index 2de060302..d8649a0d5 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/RelativeElevationFactor.h b/gtsam_unstable/slam/RelativeElevationFactor.h index 407652583..3d81fbab3 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.h +++ b/gtsam_unstable/slam/RelativeElevationFactor.h @@ -2,7 +2,7 @@ * @file RelativeElevationFactor.h * * @brief Factor representing a known relative altitude in global frame - * + * * @date Aug 17, 2012 * @author Alex Cunningham */ diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index a844ee823..b3d71d05f 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -2,7 +2,7 @@ * @file SmartRangeFactor.h * * @brief A smart factor for range-only SLAM that does initialization and marginalization - * + * * @date Sep 10, 2012 * @author Alex Cunningham */ diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index f32dd3b3e..db80956fa 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -206,12 +206,9 @@ public: std::vector Gs(numKeys * (numKeys + 1) / 2); std::vector gs(numKeys); - if (this->measured_.size() != cameras.size()) { - std::cout - << "SmartStereoProjectionHessianFactor: this->measured_.size() inconsistent with input" - << std::endl; - exit(1); - } + if (this->measured_.size() != cameras.size()) + throw std::runtime_error("SmartStereoProjectionHessianFactor: this->" + "measured_.size() inconsistent with input"); triangulateSafe(cameras); @@ -360,7 +357,7 @@ public: /// Assumes the point has been computed /// Note E can be 2m*3 or 2m*2, in case point is degenerate void computeJacobiansWithTriangulatedPoint( - FBlocks& Fs, + FBlocks& Fs, Matrix& E, Vector& b, const Cameras& cameras) const { diff --git a/gtsam_unstable/slam/TOAFactor.h b/gtsam_unstable/slam/TOAFactor.h index e37e8ff20..1df14a8de 100644 --- a/gtsam_unstable/slam/TOAFactor.h +++ b/gtsam_unstable/slam/TOAFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/TSAMFactors.h b/gtsam_unstable/slam/TSAMFactors.h index 6c2f55195..0a456e59c 100644 --- a/gtsam_unstable/slam/TSAMFactors.h +++ b/gtsam_unstable/slam/TSAMFactors.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -48,7 +48,7 @@ public: Vector evaluateError(const Pose2& pose, const Point2& point, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - return pose.transform_to(point, H1, H2) - measured_; + return pose.transformTo(point, H1, H2) - measured_; } }; @@ -85,10 +85,10 @@ public: Matrix D_pose_g_base1, D_pose_g_pose; Pose2 pose_g = base1.compose(pose, D_pose_g_base1, D_pose_g_pose); Matrix D_point_g_base2, D_point_g_point; - Point2 point_g = base2.transform_from(point, D_point_g_base2, + Point2 point_g = base2.transformFrom(point, D_point_g_base2, D_point_g_point); Matrix D_e_pose_g, D_e_point_g; - Point2 d = pose_g.transform_to(point_g, D_e_pose_g, D_e_point_g); + Point2 d = pose_g.transformTo(point_g, D_e_pose_g, D_e_point_g); if (H1) *H1 = D_e_pose_g * D_pose_g_base1; if (H2) @@ -100,8 +100,8 @@ public: return d - measured_; } else { Pose2 pose_g = base1.compose(pose); - Point2 point_g = base2.transform_from(point); - Point2 d = pose_g.transform_to(point_g); + Point2 point_g = base2.transformFrom(point); + Point2 d = pose_g.transformTo(point_g); return d - measured_; } } diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h index 156ac242e..c29e3e794 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h index b6d906fd4..48aaa8ed7 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/serialization.h b/gtsam_unstable/slam/serialization.h index 08451fa0c..1752aac80 100644 --- a/gtsam_unstable/slam/serialization.h +++ b/gtsam_unstable/slam/serialization.h @@ -2,7 +2,7 @@ * @file serialization.h * * @brief Global functions for performing serialization, designed for use with matlab - * + * * @date Jun 12, 2013 * @author Alex Cunningham */ diff --git a/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp index c85187d5f..e4112f53d 100644 --- a/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp +++ b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp @@ -1,6 +1,6 @@ /** * @file testBiasedGPSFactor.cpp - * @brief + * @brief * @author Luca Carlone * @date July 30, 2014 */ diff --git a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp index 99f494ad9..a74bfc3c6 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp @@ -56,7 +56,7 @@ TEST( InvDepthFactor, optimize) { LevenbergMarquardtParams lmParams; Values result = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize(); - + // with a single factor the incorrect initialization of 1/4 should not move! EXPECT(assert_equal(initial, result, 1e-9)); @@ -80,12 +80,12 @@ TEST( InvDepthFactor, optimize) { initial.insert(Symbol('x',2), right_pose); Values result2 = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize(); - + Point3 result2_lmk = InvDepthCamera3::invDepthTo3D( result2.at(Symbol('l',1)), result2.at(Symbol('d',1))); EXPECT(assert_equal(landmark, result2_lmk, 1e-9)); - + // TODO: need to add priors to make this work with // Values result2 = optimize(graph, initial, // NonlinearOptimizationParameters(),MULTIFRONTAL, GN); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp index a6eed54d5..951349b0f 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp @@ -38,7 +38,7 @@ TEST( InvDepthFactorVariant3, optimize) { Point2 pixel2 = camera2.project(landmark); // Create expected landmark - Point3 landmark_p1 = pose1.transform_to(landmark); + Point3 landmark_p1 = pose1.transformTo(landmark); // landmark_p1.print("Landmark in Pose1 Frame:\n"); double theta = atan2(landmark_p1.x(), landmark_p1.z()); double phi = atan2(landmark_p1.y(), sqrt(landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z())); @@ -46,7 +46,7 @@ TEST( InvDepthFactorVariant3, optimize) { Vector3 expected((Vector(3) << theta, phi, rho).finished()); - + // Create a factor graph with two inverse depth factors and two pose priors Key poseKey1(1); Key poseKey2(2); @@ -72,7 +72,7 @@ TEST( InvDepthFactorVariant3, optimize) { LevenbergMarquardtParams params; Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); Vector3 actual = result.at(landmarkKey); - + // Test that the correct landmark parameters have been recovered diff --git a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp index b468a2b6e..f589e932f 100644 --- a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp +++ b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp index ddb5cf7a2..166f058e3 100644 --- a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp index 573352e87..4e9fdcb50 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp index e878ea5d9..5eadf5fd6 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index e18505af6..ead807138 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 233dbdceb..4b10d5c0b 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/slam/tests/testTSAMFactors.cpp b/gtsam_unstable/slam/tests/testTSAMFactors.cpp index ebb52d276..098a0ef56 100644 --- a/gtsam_unstable/slam/tests/testTSAMFactors.cpp +++ b/gtsam_unstable/slam/tests/testTSAMFactors.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/timing/timeDSFvariants.cpp b/gtsam_unstable/timing/timeDSFvariants.cpp index f39bdda59..26ed76d02 100644 --- a/gtsam_unstable/timing/timeDSFvariants.cpp +++ b/gtsam_unstable/timing/timeDSFvariants.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include diff --git a/gtsampy.h b/gtsampy.h index 27af74e74..cc5ccb1b4 100644 --- a/gtsampy.h +++ b/gtsampy.h @@ -499,8 +499,8 @@ class Pose2 { static Matrix wedge(double vx, double vy, double w); // Group Actions on Point2 - gtsam::Point2 transform_from(const gtsam::Point2& p) const; - gtsam::Point2 transform_to(const gtsam::Point2& p) const; + gtsam::Point2 transformFrom(const gtsam::Point2& p) const; + gtsam::Point2 transformTo(const gtsam::Point2& p) const; // Standard Interface double x() const; @@ -546,8 +546,8 @@ class Pose3 { static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); // Group Action on Point3 - gtsam::Point3 transform_from(const gtsam::Point3& p) const; - gtsam::Point3 transform_to(const gtsam::Point3& p) const; + gtsam::Point3 transformFrom(const gtsam::Point3& p) const; + gtsam::Point3 transformTo(const gtsam::Point3& p) const; // Standard Interface gtsam::Rot3 rotation() const; @@ -556,7 +556,7 @@ class Pose3 { double y() const; double z() const; Matrix matrix() const; - gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to() + gtsam::Pose3 transformTo(const gtsam::Pose3& pose) const; // FIXME: shadows other transformTo() double range(const gtsam::Point3& point); double range(const gtsam::Pose3& pose); @@ -1742,7 +1742,7 @@ virtual class NonlinearFactor { virtual class NoiseModelFactor: gtsam::NonlinearFactor { void equals(const gtsam::NoiseModelFactor& other, double tol) const; - gtsam::noiseModel::Base* get_noiseModel() const; + gtsam::noiseModel::Base* noiseModel() const; Vector unwhitenedError(const gtsam::Values& x) const; Vector whitenedError(const gtsam::Values& x) const; }; @@ -2045,7 +2045,7 @@ virtual class NonlinearOptimizer { double error() const; int iterations() const; gtsam::Values values() const; - void iterate() const; + gtsam::GaussianFactorGraph* iterate() const; }; virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer { diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m index 0abd9cc3c..71f72f6b9 100644 --- a/matlab/+gtsam/cylinderSampleProjection.m +++ b/matlab/+gtsam/cylinderSampleProjection.m @@ -32,7 +32,7 @@ for i = 1:cylinderNum % Cheirality Exception sampledPoint3 = cylinders{i}.Points{j}; - sampledPoint3local = pose.transform_to(sampledPoint3); + sampledPoint3local = pose.transformTo(sampledPoint3); if sampledPoint3local.z <= 0 continue; end diff --git a/matlab/+gtsam/cylinderSampleProjectionStereo.m b/matlab/+gtsam/cylinderSampleProjectionStereo.m index 6512231e8..10409ac6d 100644 --- a/matlab/+gtsam/cylinderSampleProjectionStereo.m +++ b/matlab/+gtsam/cylinderSampleProjectionStereo.m @@ -24,7 +24,7 @@ for i = 1:cylinderNum % For Cheirality Exception sampledPoint3 = cylinders{i}.Points{j}; - sampledPoint3local = pose.transform_to(sampledPoint3); + sampledPoint3local = pose.transformTo(sampledPoint3); if sampledPoint3local.z < 0 continue; end diff --git a/matlab/README-gtsam-toolbox.md b/matlab/README-gtsam-toolbox.md new file mode 100644 index 000000000..66a02e969 --- /dev/null +++ b/matlab/README-gtsam-toolbox.md @@ -0,0 +1,91 @@ +# GTSAM - Georgia Tech Smoothing and Mapping Library + +## MATLAB wrapper + +http://borg.cc.gatech.edu/projects/gtsam + +This is the GTSAM MATLAB toolbox, a MATLAB wrapper around the GTSAM C++ library. To build it, enable `GTSAM_INSTALL_MATLAB_TOOLBOX=ON` in CMake. + +The interface to the toolbox is generated automatically by the wrap +tool which directly parses C++ header files. The tool generates matlab proxy objects together with all the native functions for wrapping and unwrapping scalar and non scalar types and objects. The interface generated by the wrap tool also redirects the standard output stream (cout) to matlab's console. + +## Ubuntu + +If you have a newer Ubuntu system (later than 10.04), you must make a small modification to your MATLAB installation, due to MATLAB being distributed with an old version of the C++ standard library. Delete or rename all files starting with `libstdc++` in your MATLAB installation directory, in paths: + + /usr/local/MATLAB/[version]/sys/os/[system]/ + /usr/local/MATLAB/[version]/bin/[system]/ + + +## Adding the toolbox to your MATLAB path + +To get started, first add the `toolbox` (or `gtsam_toolbox`) folder to your MATLAB path - in the MATLAB file browser, right-click on the folder and click 'Add to path -> This folder' (**do not add the subfolders to your path**). + +## Final setup on Linux + +MATLAB needs to know where the GTSAM shared object file (`libgtsam.so.4`) is so that it can link to it correctly. + +### System-wide + +If you installed GTSAM system-wide (e.g. with `sudo make install`), then simply run `sudo ldconfig`. + +### Custom Install + +If you have a custom install location, denoted by ``, you need to update your `LD_LIBRARY_PATH` environment variable. + +```sh +export LD_LIBRARY_PATH=/gtsam:$LD_LIBRARY_PATH +``` + +### Linker issues + +If you compile the MATLAB toolbox and everything compiles smoothly, but when you run any MATLAB script, you get following error messages in MATLAB +``` +Invalid MEX-file '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64': +Missing symbol 'mexAtExit' required by '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64' +Missing symbol 'mexCallMATLABWithObject' required by '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64' +... +``` +run following shell line +```sh +export LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6 +``` +before you run MATLAB from the same shell. + +This mainly happens if you have GCC >= 5 and newer version MATLAB like R2017a. + + +## Trying out the examples + +The examples are located in the 'gtsam_examples' subfolder. You may either run them individually at the MATLAB command line, or open the GTSAM example GUI by running 'gtsamExamples'. Example: + +```matlab +>> cd /Users/yourname/toolbox % Change to wherever you installed the toolbox +>> cd gtsam_examples % Change to the examples directory +>> gtsamExamples % Run the GTSAM examples GUI +``` + +## Running the unit tests + +The GTSAM MATLAB toolbox also has a small set of unit tests located in the gtsam_tests directory. Example: + +```matlab +>> cd /Users/yourname/toolbox % Change to wherever you installed the toolbox +>> cd gtsam_tests % Change to the examples directory +>> test_gtsam % Run the unit tests +Starting: testJacobianFactor +Starting: testKalmanFilter +Starting: testLocalizationExample +Starting: testOdometryExample +Starting: testPlanarSLAMExample +Starting: testPose2SLAMExample +Starting: testPose3SLAMExample +Starting: testSFMExample +Starting: testStereoVOExample +Starting: testVisualISAMExample +Tests complete! +``` + +## Writing your own code + +Coding for the GTSAM MATLAB toolbox is straightforward and very fast once you understand a few basic concepts! Please see the manual to get started. diff --git a/matlab/README-gtsam-toolbox.txt b/matlab/README-gtsam-toolbox.txt deleted file mode 100644 index a1691be32..000000000 --- a/matlab/README-gtsam-toolbox.txt +++ /dev/null @@ -1,82 +0,0 @@ -================================================================================ -GTSAM - Georgia Tech Smoothing and Mapping Library - -MATLAB wrapper - -http://borg.cc.gatech.edu/projects/gtsam -================================================================================ - -This is the GTSAM MATLAB toolbox, a MATLAB wrapper around the GTSAM C++ -library. To build it, enable GTSAM_INSTALL_MATLAB_TOOLBOX in CMake. - -The interface to the toolbox is generated automatically by the wrap -tool which directly parses C++ header files. The tool generates matlab -proxy objects together with all the native functions for wrapping and -unwrapping scalar and non scalar types and objects. The interface -generated by the wrap tool also redirects the standard output stream -(cout) to matlab's console. - ----------------------------------------- -Note about newer Ubuntu versions unsupported by MATLAB (later than 10.04) ----------------------------------------- - -If you have a newer Ubuntu system, you must make a small modification to your -MATLAB installation, due to MATLAB being distributed with an old version of -the C++ standard library. Delete or rename all files starting with -'libstdc++' in your MATLAB installation directory, in paths: - - /usr/local/MATLAB/[version]/sys/os/[system]/ - /usr/local/MATLAB/[version]/bin/[system]/ - - ----------------------------------------- -Adding the toolbox to your MATLAB path ----------------------------------------- - -To get started, first add the 'toolbox' folder to your MATLAB path - in the -MATLAB file browser, right-click on the folder and click 'Add to path -> This -folder' (do not add the subfolders to your path). - - ----------------------------------------- -Trying out the examples ----------------------------------------- - -The examples are located in the 'gtsam_examples' subfolder. You may either -run them individually at the MATLAB command line, or open the GTSAM example -GUI by running 'gtsamExamples'. Example: - ->> cd /Users/yourname/toolbox % Change to wherever you installed the toolbox ->> cd gtsam_examples % Change to the examples directory ->> gtsamExamples % Run the GTSAM examples GUI - - ----------------------------------------- -Running the unit tests ----------------------------------------- - -The GTSAM MATLAB toolbox also has a small set of unit tests located in the -gtsam_tests directory. Example: - ->> cd /Users/yourname/toolbox % Change to wherever you installed the toolbox ->> cd gtsam_tests % Change to the examples directory ->> test_gtsam % Run the unit tests -Starting: testJacobianFactor -Starting: testKalmanFilter -Starting: testLocalizationExample -Starting: testOdometryExample -Starting: testPlanarSLAMExample -Starting: testPose2SLAMExample -Starting: testPose3SLAMExample -Starting: testSFMExample -Starting: testStereoVOExample -Starting: testVisualISAMExample -Tests complete! - - ----------------------------------------- -Writing your own code ----------------------------------------- - -Coding for the GTSAM MATLAB toolbox is straightforward and very fast once you -understand a few basic concepts! Please see the manual to get started. diff --git a/matlab/gtsam_examples/StereoVOExample_large.m b/matlab/gtsam_examples/StereoVOExample_large.m index 464448335..fff0ae2d7 100644 --- a/matlab/gtsam_examples/StereoVOExample_large.m +++ b/matlab/gtsam_examples/StereoVOExample_large.m @@ -46,7 +46,7 @@ for i=1:size(measurements,1) % 3D landmarks are stored in camera coordinates: transform % to world coordinates using the respective initial pose pose = initial.atPose3(symbol('x', sf(1))); - world_point = pose.transform_from(Point3(sf(6),sf(7),sf(8))); + world_point = pose.transformFrom(Point3(sf(6),sf(7),sf(8))); initial.insert(symbol('l',sf(2)), world_point); end end diff --git a/matlab/unstable_examples/ConcurrentFilteringAndSmoothingExample.m b/matlab/unstable_examples/ConcurrentFilteringAndSmoothingExample.m index d1d225545..46945648a 100644 --- a/matlab/unstable_examples/ConcurrentFilteringAndSmoothingExample.m +++ b/matlab/unstable_examples/ConcurrentFilteringAndSmoothingExample.m @@ -107,7 +107,7 @@ for time = deltaT : deltaT : 10.0 %% Print the filter optimized poses fprintf(1, 'Timestamp = %5.3f\n', time); filterResult = concurrentFilter.calculateEstimate; - filterResult.at(currentKey).print('Concurrent Estimate: '); + filterResult.atPose2(currentKey).print('Concurrent Estimate: '); %% Plot Covariance Ellipses cla; diff --git a/matlab/unstable_examples/testTSAMFactors.m b/matlab/unstable_examples/testTSAMFactors.m index 341725723..5cfd0aa80 100644 --- a/matlab/unstable_examples/testTSAMFactors.m +++ b/matlab/unstable_examples/testTSAMFactors.m @@ -26,10 +26,10 @@ origin = Pose2; graph.add(gtsam.PriorFactorPose2(10, origin, noisePrior)) graph.add(gtsam.PriorFactorPose2(20, origin, noisePrior)) graph.add(gtsam.PriorFactorPose2(100, origin, noisePrior)) -graph.add(DeltaFactor(10, 1, b1.transform_to(l1), noiseDelta)) -graph.add(DeltaFactor(20, 1, b2.transform_to(l2), noiseDelta)) -graph.add(DeltaFactorBase(100,10, 200,2, b1.transform_to(l2), noiseDelta)) -graph.add(DeltaFactorBase(200,20, 100,1, b2.transform_to(l1), noiseDelta)) +graph.add(DeltaFactor(10, 1, b1.transformTo(l1), noiseDelta)) +graph.add(DeltaFactor(20, 1, b2.transformTo(l2), noiseDelta)) +graph.add(DeltaFactorBase(100,10, 200,2, b1.transformTo(l2), noiseDelta)) +graph.add(DeltaFactorBase(200,20, 100,1, b2.transformTo(l1), noiseDelta)) graph.add(OdometryFactorBase(100,10,200,20, Pose2(2,0,0), noiseOdom)) % Initial values diff --git a/python/README.md b/python/README.md index f1c218cfb..396bcea89 100644 --- a/python/README.md +++ b/python/README.md @@ -1,6 +1,14 @@ Python Wrapper and Packaging ============================ +# Deprecation Notice + +We are in the process of deprecating the old Python bindings and Cython bindings in favor of the new Pybind11 binding system. + +As such, the examples in this directory are no longer guaranteed to work. Please refer to the [cython examples](https://bitbucket.org/gtborg/gtsam/src/develop/cython/gtsam/examples). + +--- + This directory contains the basic setup script and directory structure for the gtsam python module. During the build of gtsam, when GTSAM_BUILD_PYTHON is enabled, the following instructions will run. diff --git a/python/handwritten/base/FastVector.cpp b/python/handwritten/base/FastVector.cpp index 1c3582813..379488631 100644 --- a/python/handwritten/base/FastVector.cpp +++ b/python/handwritten/base/FastVector.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/python/handwritten/common.h b/python/handwritten/common.h index 72d2ae846..2690d52fc 100644 --- a/python/handwritten/common.h +++ b/python/handwritten/common.h @@ -17,7 +17,7 @@ #pragma once /* Fix to avoid registration warnings */ -// Solution taken from https://github.com/BVLC/caffe/pull/4069/commits/673e8cfc0b8f05f9fa3ebbad7cc6202822e5d9c5 +// Solution taken from https://github.com/BVLC/caffe/pull/4069/commits/673e8cfc0b8f05f9fa3ebbad7cc6202822e5d9c5 #define REGISTER_SHARED_PTR_TO_PYTHON(PTR) do { \ const boost::python::type_info info = \ boost::python::type_id >(); \ diff --git a/python/handwritten/geometry/Cal3_S2.cpp b/python/handwritten/geometry/Cal3_S2.cpp index 440b21742..1d59b1780 100644 --- a/python/handwritten/geometry/Cal3_S2.cpp +++ b/python/handwritten/geometry/Cal3_S2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/python/handwritten/geometry/PinholeCamera.cpp b/python/handwritten/geometry/PinholeCamera.cpp index 21ffcdeb7..97f6e09c6 100644 --- a/python/handwritten/geometry/PinholeCamera.cpp +++ b/python/handwritten/geometry/PinholeCamera.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/python/handwritten/geometry/Point2.cpp b/python/handwritten/geometry/Point2.cpp index 99a97adc9..e7590976f 100644 --- a/python/handwritten/geometry/Point2.cpp +++ b/python/handwritten/geometry/Point2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/python/handwritten/geometry/Pose2.cpp b/python/handwritten/geometry/Pose2.cpp index 4f402df7e..aaf17f812 100644 --- a/python/handwritten/geometry/Pose2.cpp +++ b/python/handwritten/geometry/Pose2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -29,12 +29,12 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Pose2::print, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Pose2::equals, 1, 2) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Pose2::compose, 1, 3) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Pose2::between, 1, 3) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose2::transform_to, 1, 3) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose2::transform_from, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose2::transformTo, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose2::transformFrom, 1, 3) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(bearing_overloads, Pose2::bearing, 1, 3) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, Pose2::range, 1, 3) -// Manually wrap +// Manually wrap void exportPose2(){ @@ -61,9 +61,9 @@ void exportPose2(){ // .def("dim", &Pose2::dim) // .def("retract", &Pose2::retract) - .def("transform_to", &Pose2::transform_to, + .def("transformTo", &Pose2::transformTo, transform_to_overloads(args("point", "H1", "H2"))) - .def("transform_from", &Pose2::transform_from, + .def("transformFrom", &Pose2::transformFrom, transform_to_overloads(args("point", "H1", "H2"))) .def("x", &Pose2::x) diff --git a/python/handwritten/geometry/Pose3.cpp b/python/handwritten/geometry/Pose3.cpp index 551f2f60c..cbddab0a9 100644 --- a/python/handwritten/geometry/Pose3.cpp +++ b/python/handwritten/geometry/Pose3.cpp @@ -30,8 +30,8 @@ using namespace gtsam; BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Pose3::print, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Pose3::equals, 1, 2) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose3::transform_to, 1, 3) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose3::transform_from, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose3::transformTo, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose3::transformFrom, 1, 3) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(rotation_overloads, Pose3::rotation, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(translation_overloads, Pose3::translation, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Pose3::compose, 2, 3) @@ -41,9 +41,9 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, Pose3::range, 1, 3) void exportPose3(){ - // function pointers to desambiguate transform_to() calls - Point3 (Pose3::*transform_to1)(const Point3&, OptionalJacobian< 3, 6 >, OptionalJacobian< 3, 3 > ) const = &Pose3::transform_to; - Pose3 (Pose3::*transform_to2)(const Pose3&) const = &Pose3::transform_to; + // function pointers to desambiguate transformTo() calls + Point3 (Pose3::*transform_to1)(const Point3&, OptionalJacobian< 3, 6 >, OptionalJacobian< 3, 3 > ) const = &Pose3::transformTo; + Pose3 (Pose3::*transform_to2)(const Pose3&) const = &Pose3::transformTo; // function pointers to desambiguate compose() calls Pose3 (Pose3::*compose1)(const Pose3 &g) const = &Pose3::compose; Pose3 (Pose3::*compose2)(const Pose3 &g, typename Pose3::ChartJacobian, typename Pose3::ChartJacobian) const = &Pose3::compose; @@ -69,10 +69,10 @@ void exportPose3(){ .def("identity", &Pose3::identity) .staticmethod("identity") .def("matrix", &Pose3::matrix) - .def("transform_from", &Pose3::transform_from, + .def("transformFrom", &Pose3::transformFrom, transform_from_overloads(args("point", "H1", "H2"))) - .def("transform_to", transform_to1, transform_to_overloads(args("point", "H1", "H2"))) - .def("transform_to", transform_to2) + .def("transformTo", transform_to1, transform_to_overloads(args("point", "H1", "H2"))) + .def("transformTo", transform_to2) .def("x", &Pose3::x) .def("y", &Pose3::y) .def("z", &Pose3::z) diff --git a/python/handwritten/geometry/Rot2.cpp b/python/handwritten/geometry/Rot2.cpp index 59b4ce4e8..961ad970f 100644 --- a/python/handwritten/geometry/Rot2.cpp +++ b/python/handwritten/geometry/Rot2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/python/handwritten/inference/Symbol.cpp b/python/handwritten/inference/Symbol.cpp index 9fc5b74e7..0ec3445b8 100644 --- a/python/handwritten/inference/Symbol.cpp +++ b/python/handwritten/inference/Symbol.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/python/handwritten/linear/NoiseModel.cpp b/python/handwritten/linear/NoiseModel.cpp index e918e509d..c4c46fe7f 100644 --- a/python/handwritten/linear/NoiseModel.cpp +++ b/python/handwritten/linear/NoiseModel.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -35,7 +35,7 @@ using namespace gtsam; using namespace gtsam::noiseModel; // Wrap around pure virtual class Base. -// All pure virtual methods should be wrapped. Non-pure may be wrapped if we want to mimic the +// All pure virtual methods should be wrapped. Non-pure may be wrapped if we want to mimic the // overloading through inheritance in Python. // See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/exposing.html#python.class_virtual_functions struct BaseCallback : Base, wrapper @@ -76,7 +76,7 @@ struct BaseCallback : Base, wrapper }; -// Overloads for named constructors. Named constructors are static, so we declare them +// Overloads for named constructors. Named constructors are static, so we declare them // using BOOST_PYTHON_FUNCTION_OVERLOADS instead of BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS // See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/functions.html#python.default_arguments BOOST_PYTHON_FUNCTION_OVERLOADS(Gaussian_SqrtInformation_overloads, Gaussian::SqrtInformation, 1, 2) @@ -97,13 +97,13 @@ void exportNoiseModels(){ object noiseModel_module(handle<>(borrowed(PyImport_AddModule(noiseModel_name.c_str())))); scope().attr("noiseModel") = noiseModel_module; scope noiseModel_scope = noiseModel_module; - + // Then export our classes in the noiseModel scope class_("Base") .def("print", pure_virtual(&Base::print)) ; register_ptr_to_python< boost::shared_ptr >(); - + // NOTE: We should use "Base" in "bases<...>", and not "BaseCallback" (it was not clear at the begining) class_, bases >("Gaussian", no_init) .def("SqrtInformation",&Gaussian::SqrtInformation, Gaussian_SqrtInformation_overloads()) @@ -114,7 +114,7 @@ void exportNoiseModels(){ .staticmethod("Covariance") ; REGISTER_SHARED_PTR_TO_PYTHON(Gaussian); - + class_, bases >("Diagonal", no_init) .def("Sigmas",&Diagonal::Sigmas, Diagonal_Sigmas_overloads()) .staticmethod("Sigmas") @@ -124,7 +124,7 @@ void exportNoiseModels(){ .staticmethod("Precisions") ; REGISTER_SHARED_PTR_TO_PYTHON(Diagonal); - + class_, bases >("Isotropic", no_init) .def("Sigma",&Isotropic::Sigma, Isotropic_Sigma_overloads()) .staticmethod("Sigma") @@ -134,7 +134,7 @@ void exportNoiseModels(){ .staticmethod("Precision") ; REGISTER_SHARED_PTR_TO_PYTHON(Isotropic); - + class_, bases >("Unit", no_init) .def("Create",&Unit::Create) .staticmethod("Create") diff --git a/python/handwritten/nonlinear/ISAM2.cpp b/python/handwritten/nonlinear/ISAM2.cpp index f80c5df99..bd908ce8f 100644 --- a/python/handwritten/nonlinear/ISAM2.cpp +++ b/python/handwritten/nonlinear/ISAM2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -45,7 +45,7 @@ class_("ISAM2Params") // OptimizationParams getOptimizationParams () const // void setKeyFormatter (KeyFormatter keyFormatter) // KeyFormatter getKeyFormatter () const - // GaussianFactorGraph::Eliminate getEliminationFunction () const + // GaussianFactorGraph::Eliminate getEliminationFunction () const ; // TODO(Ellon): Export useful methods/properties of ISAM2Result diff --git a/python/handwritten/nonlinear/NonlinearFactor.cpp b/python/handwritten/nonlinear/NonlinearFactor.cpp index 8ccc123fc..359ac48f3 100644 --- a/python/handwritten/nonlinear/NonlinearFactor.cpp +++ b/python/handwritten/nonlinear/NonlinearFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -25,7 +25,7 @@ using namespace boost::python; using namespace gtsam; // Wrap around pure virtual class NonlinearFactor. -// All pure virtual methods should be wrapped. Non-pure may be wrapped if we want to mimic the +// All pure virtual methods should be wrapped. Non-pure may be wrapped if we want to mimic the // overloading through inheritance in Python. // See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/exposing.html#python.class_virtual_functions struct NonlinearFactorWrap : NonlinearFactor, wrapper diff --git a/python/handwritten/slam/GenericProjectionFactor.cpp b/python/handwritten/slam/GenericProjectionFactor.cpp index dd932ccd4..1aa66b8f3 100644 --- a/python/handwritten/slam/GenericProjectionFactor.cpp +++ b/python/handwritten/slam/GenericProjectionFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/python/handwritten/utils/NumpyEigen.cpp b/python/handwritten/utils/NumpyEigen.cpp index d7cebe7ad..3d3a15727 100644 --- a/python/handwritten/utils/NumpyEigen.cpp +++ b/python/handwritten/utils/NumpyEigen.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -40,7 +40,7 @@ void registerNumpyEigenConversions() NumpyEigenConverter::register_converter(); NumpyEigenConverter::register_converter(); NumpyEigenConverter::register_converter(); - + NumpyEigenConverter::register_converter(); NumpyEigenConverter::register_converter(); NumpyEigenConverter::register_converter(); diff --git a/tests/simulated2D.h b/tests/simulated2D.h index 3245652be..c4930a55b 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/simulated2DConstraints.h b/tests/simulated2DConstraints.h index 7d399dc02..deb9292f7 100644 --- a/tests/simulated2DConstraints.h +++ b/tests/simulated2DConstraints.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/simulated2DOriented.h b/tests/simulated2DOriented.h index 9e604cb3c..948a87ce5 100644 --- a/tests/simulated2DOriented.h +++ b/tests/simulated2DOriented.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/smallExample.h b/tests/smallExample.h index d3a69b0bd..146289dac 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -26,7 +26,6 @@ #include #include #include -#include #include namespace gtsam { @@ -131,7 +130,7 @@ namespace example { * -x11-x21-x31 * with x11 clamped at (1,1), and others related by 2D odometry. */ -// inline boost::tuple planarGraph(size_t N); +// inline std::pair planarGraph(size_t N); /* * Create canonical ordering for planar graph that also works for tree @@ -399,7 +398,7 @@ inline std::pair createNonlinearSmoother(int T) { inline GaussianFactorGraph createSmoother(int T) { NonlinearFactorGraph nlfg; Values poses; - boost::tie(nlfg, poses) = createNonlinearSmoother(T); + std::tie(nlfg, poses) = createNonlinearSmoother(T); return *nlfg.linearize(poses); } @@ -563,7 +562,7 @@ inline Symbol key(size_t x, size_t y) { } // \namespace impl /* ************************************************************************* */ -inline boost::tuple planarGraph(size_t N) { +inline std::pair planarGraph(size_t N) { using namespace impl; // create empty graph @@ -601,7 +600,7 @@ inline boost::tuple planarGraph(size_t N) { // linearize around zero boost::shared_ptr gfg = nlfg.linearize(zeros); - return boost::make_tuple(*gfg, xtrue); + return std::make_pair(*gfg, xtrue); } /* ************************************************************************* */ @@ -614,26 +613,26 @@ inline Ordering planarOrdering(size_t N) { } /* ************************************************************************* */ -inline std::pair splitOffPlanarTree(size_t N, +inline std::pair splitOffPlanarTree(size_t N, const GaussianFactorGraph& original) { - GaussianFactorGraph T, C; + auto T = boost::make_shared(), C= boost::make_shared(); // Add the x11 constraint to the tree - T.push_back(original[0]); + T->push_back(original[0]); // Add all horizontal constraints to the tree size_t i = 1; for (size_t x = 1; x < N; x++) for (size_t y = 1; y <= N; y++, i++) - T.push_back(original[i]); + T->push_back(original[i]); // Add first vertical column of constraints to T, others to C for (size_t x = 1; x <= N; x++) for (size_t y = 1; y < N; y++, i++) if (x == 1) - T.push_back(original[i]); + T->push_back(original[i]); else - C.push_back(original[i]); + C->push_back(original[i]); return std::make_pair(T, C); } diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index a5d1a195c..c67914c8b 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 769b458e4..d9a013a60 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -228,7 +228,7 @@ TEST(ExpressionFactor, Shallow) { Point3_ p_(2); // Construct expression, concise evrsion - Point2_ expression = project(transform_to(x_, p_)); + Point2_ expression = project(transformTo(x_, p_)); // Get and check keys and dims KeyVector keys; @@ -243,7 +243,6 @@ TEST(ExpressionFactor, Shallow) { // traceExecution of shallow tree typedef internal::UnaryExpression Unary; - typedef internal::BinaryExpression Binary; size_t size = expression.traceSize(); internal::ExecutionTraceStorage traceStorage[size]; internal::ExecutionTrace trace; @@ -289,7 +288,7 @@ TEST(ExpressionFactor, tree) { Cal3_S2_ K(3); // Create expression tree - Point3_ p_cam(x, &Pose3::transform_to, p); + Point3_ p_cam(x, &Pose3::transformTo, p); Point2_ xy_hat(Project, p_cam); Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); @@ -302,7 +301,7 @@ TEST(ExpressionFactor, tree) { // Concise version ExpressionFactor f2(model, measured, - uncalibrate(K, project(transform_to(x, p)))); + uncalibrate(K, project(transformTo(x, p)))); EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f2.dim()); boost::shared_ptr gf2 = f2.linearize(values); @@ -462,7 +461,7 @@ TEST(ExpressionFactor, tree_finite_differences) { Cal3_S2_ K(3); // Create expression tree - Point3_ p_cam(x, &Pose3::transform_to, p); + Point3_ p_cam(x, &Pose3::transformTo, p); Point2_ xy_hat(Project, p_cam); Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); diff --git a/tests/testGaussianBayesTreeB.cpp b/tests/testGaussianBayesTreeB.cpp index 984728ebf..b8b6cf284 100644 --- a/tests/testGaussianBayesTreeB.cpp +++ b/tests/testGaussianBayesTreeB.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index eec62ff9c..c4e9d26f5 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -46,6 +46,8 @@ double tol=1e-5; using symbol_shorthand::X; using symbol_shorthand::L; +static auto kUnit2 = noiseModel::Unit::Create(2); + /* ************************************************************************* */ TEST( GaussianFactorGraph, equals ) { @@ -67,364 +69,256 @@ TEST( GaussianFactorGraph, error ) { } /* ************************************************************************* */ -TEST( GaussianFactorGraph, eliminateOne_x1 ) -{ +TEST(GaussianFactorGraph, eliminateOne_x1) { GaussianFactorGraph fg = createGaussianFactorGraph(); GaussianConditional::shared_ptr conditional; - pair result = - fg.eliminatePartialSequential(Ordering(list_of(X(1)))); + auto result = fg.eliminatePartialSequential(Ordering(list_of(X(1)))); conditional = result.first->front(); // create expected Conditional Gaussian - Matrix I = 15*I_2x2, R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; + Matrix I = 15 * I_2x2, R11 = I, S12 = -0.111111 * I, S13 = -0.444444 * I; Vector d = Vector2(-0.133333, -0.0222222); - GaussianConditional expected(X(1),15*d,R11,L(1),S12,X(2),S13); + GaussianConditional expected(X(1), 15 * d, R11, L(1), S12, X(2), S13); - EXPECT(assert_equal(expected,*conditional,tol)); + EXPECT(assert_equal(expected, *conditional, tol)); } -#if 0 - /* ************************************************************************* */ -TEST( GaussianFactorGraph, eliminateOne_x2 ) -{ - Ordering ordering; ordering += X(2),L(1),X(1); - GaussianFactorGraph fg = createGaussianFactorGraph(ordering); - GaussianConditional::shared_ptr actual = fg.eliminateOne(0, EliminateQR).first; +TEST(GaussianFactorGraph, eliminateOne_x2) { + Ordering ordering; + ordering += X(2), L(1), X(1); + GaussianFactorGraph fg = createGaussianFactorGraph(); + auto actual = EliminateQR(fg, Ordering(list_of(X(2)))).first; // create expected Conditional Gaussian - double sig = 0.0894427; - Matrix I = I_2x2/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; - Vector d = Vector2(0.2, -0.14)/sig, sigma = Vector::Ones(2); - GaussianConditional expected(ordering[X(2)],d,R11,ordering[L(1)],S12,ordering[X(1)],S13,sigma); + double sigma = 0.0894427; + Matrix I = I_2x2 / sigma, R11 = I, S12 = -0.2 * I, S13 = -0.8 * I; + Vector d = Vector2(0.2, -0.14) / sigma; + GaussianConditional expected(X(2), d, R11, L(1), S12, X(1), S13, kUnit2); - EXPECT(assert_equal(expected,*actual,tol)); + EXPECT(assert_equal(expected, *actual, tol)); } /* ************************************************************************* */ -TEST( GaussianFactorGraph, eliminateOne_l1 ) -{ - Ordering ordering; ordering += L(1),X(1),X(2); - GaussianFactorGraph fg = createGaussianFactorGraph(ordering); - GaussianConditional::shared_ptr actual = fg.eliminateOne(0, EliminateQR).first; +TEST(GaussianFactorGraph, eliminateOne_l1) { + Ordering ordering; + ordering += L(1), X(1), X(2); + GaussianFactorGraph fg = createGaussianFactorGraph(); + auto actual = EliminateQR(fg, Ordering(list_of(L(1)))).first; // create expected Conditional Gaussian - double sig = sqrt(2.0)/10.; - Matrix I = I_2x2/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; - Vector d = Vector2(-0.1, 0.25)/sig, sigma = Vector::Ones(2); - GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma); + double sigma = sqrt(2.0) / 10.; + Matrix I = I_2x2 / sigma, R11 = I, S12 = -0.5 * I, S13 = -0.5 * I; + Vector d = Vector2(-0.1, 0.25) / sigma; + GaussianConditional expected(L(1), d, R11, X(1), S12, X(2), S13, kUnit2); - EXPECT(assert_equal(expected,*actual,tol)); + EXPECT(assert_equal(expected, *actual, tol)); } /* ************************************************************************* */ -TEST( GaussianFactorGraph, eliminateOne_x1_fast ) -{ - Ordering ordering; ordering += X(1),L(1),X(2); - GaussianFactorGraph fg = createGaussianFactorGraph(ordering); +TEST(GaussianFactorGraph, eliminateOne_x1_fast) { + GaussianFactorGraph fg = createGaussianFactorGraph(); GaussianConditional::shared_ptr conditional; - GaussianFactorGraph remaining; - boost::tie(conditional,remaining) = fg.eliminateOne(ordering[X(1)], EliminateQR); + JacobianFactor::shared_ptr remaining; + boost::tie(conditional, remaining) = EliminateQR(fg, Ordering(list_of(X(1)))); // create expected Conditional Gaussian - Matrix I = 15*I_2x2, R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; - Vector d = Vector2(-0.133333, -0.0222222), sigma = Vector::Ones(2); - GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma); + Matrix I = 15 * I_2x2, R11 = I, S12 = -0.111111 * I, S13 = -0.444444 * I; + Vector d = Vector2(-0.133333, -0.0222222); + GaussianConditional expected(X(1), 15 * d, R11, L(1), S12, X(2), S13, kUnit2); // Create expected remaining new factor - JacobianFactor expectedFactor(1, (Matrix(4,2) << - 4.714045207910318, 0., - 0., 4.714045207910318, - 0., 0., - 0., 0.).finished(), - 2, (Matrix(4,2) << - -2.357022603955159, 0., - 0., -2.357022603955159, - 7.071067811865475, 0., - 0., 7.071067811865475).finished(), - (Vector(4) << -0.707106781186547, 0.942809041582063, 0.707106781186547, -1.414213562373094).finished(), noiseModel::Unit::Create(4)); + JacobianFactor expectedFactor( + L(1), (Matrix(4, 2) << 6.87184, 0, 0, 6.87184, 0, 0, 0, 0).finished(), + X(2), + (Matrix(4, 2) << -5.25494, 0, 0, -5.25494, -7.27607, 0, 0, -7.27607) + .finished(), + (Vector(4) << -1.21268, 1.73817, -0.727607, 1.45521).finished(), + noiseModel::Unit::Create(4)); - EXPECT(assert_equal(expected,*conditional,tol)); - EXPECT(assert_equal((const GaussianFactor&)expectedFactor,*remaining.back(),tol)); + EXPECT(assert_equal(expected, *conditional, tol)); + EXPECT(assert_equal(expectedFactor, *remaining, tol)); } /* ************************************************************************* */ -TEST( GaussianFactorGraph, eliminateOne_x2_fast ) -{ - Ordering ordering; ordering += X(1),L(1),X(2); - GaussianFactorGraph fg = createGaussianFactorGraph(ordering); - GaussianConditional::shared_ptr actual = fg.eliminateOne(ordering[X(2)], EliminateQR).first; +TEST(GaussianFactorGraph, eliminateOne_x2_fast) { + GaussianFactorGraph fg = createGaussianFactorGraph(); + auto actual = EliminateQR(fg, Ordering(list_of(X(2)))).first; // create expected Conditional Gaussian - double sig = 0.0894427; - Matrix I = I_2x2/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; - Vector d = Vector2(0.2, -0.14)/sig, sigma = Vector::Ones(2); - GaussianConditional expected(ordering[X(2)],d,R11,ordering[X(1)],S13,ordering[L(1)],S12,sigma); + double sigma = 0.0894427; + Matrix I = I_2x2 / sigma, R11 = -I, S12 = 0.2 * I, S13 = 0.8 * I; + Vector d = Vector2(-0.2, 0.14) / sigma; + GaussianConditional expected(X(2), d, R11, L(1), S12, X(1), S13, kUnit2); - EXPECT(assert_equal(expected,*actual,tol)); + EXPECT(assert_equal(expected, *actual, tol)); } /* ************************************************************************* */ -TEST( GaussianFactorGraph, eliminateOne_l1_fast ) -{ - Ordering ordering; ordering += X(1),L(1),X(2); - GaussianFactorGraph fg = createGaussianFactorGraph(ordering); - GaussianConditional::shared_ptr actual = fg.eliminateOne(ordering[L(1)], EliminateQR).first; +TEST(GaussianFactorGraph, eliminateOne_l1_fast) { + GaussianFactorGraph fg = createGaussianFactorGraph(); + auto actual = EliminateQR(fg, Ordering(list_of(L(1)))).first; // create expected Conditional Gaussian - double sig = sqrt(2.0)/10.; - Matrix I = I_2x2/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; - Vector d = Vector2(-0.1, 0.25)/sig, sigma = Vector::Ones(2); - GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma); + double sigma = sqrt(2.0) / 10.; + Matrix I = I_2x2 / sigma, R11 = -I, S12 = 0.5 * I, S13 = 0.5 * I; + Vector d = Vector2(0.1, -0.25) / sigma; + GaussianConditional expected(L(1), d, R11, X(1), S12, X(2), S13, kUnit2); - EXPECT(assert_equal(expected,*actual,tol)); + EXPECT(assert_equal(expected, *actual, tol)); } /* ************************************************************************* */ -TEST( GaussianFactorGraph, eliminateAll ) -{ - // create expected Chordal bayes Net - Matrix I = I_2x2; - - Ordering ordering; - ordering += X(2),L(1),X(1); - - Vector d1 = Vector2(-0.1,-0.1); - GaussianBayesNet expected = simpleGaussian(ordering[X(1)],d1,0.1); - - double sig1 = 0.149071; - Vector d2 = Vector2(0.0, 0.2)/sig1, sigma2 = Vector::Ones(2); - push_front(expected,ordering[L(1)],d2, I/sig1,ordering[X(1)], (-1)*I/sig1,sigma2); - - double sig2 = 0.0894427; - Vector d3 = Vector2(0.2, -0.14)/sig2, sigma3 = Vector::Ones(2); - push_front(expected,ordering[X(2)],d3, I/sig2,ordering[L(1)], (-0.2)*I/sig2, ordering[X(1)], (-0.8)*I/sig2, sigma3); - - // Check one ordering - GaussianFactorGraph fg1 = createGaussianFactorGraph(ordering); - GaussianBayesNet actual = *GaussianSequentialSolver(fg1).eliminate(); - EXPECT(assert_equal(expected,actual,tol)); - - GaussianBayesNet actualQR = *GaussianSequentialSolver(fg1, true).eliminate(); - EXPECT(assert_equal(expected,actualQR,tol)); -} - -/* ************************************************************************* */ -TEST( GaussianFactorGraph, copying ) -{ +TEST(GaussianFactorGraph, copying) { // Create a graph - Ordering ordering; ordering += X(2),L(1),X(1); - GaussianFactorGraph actual = createGaussianFactorGraph(ordering); + GaussianFactorGraph actual = createGaussianFactorGraph(); // Copy the graph ! GaussianFactorGraph copy = actual; // now eliminate the copy - GaussianBayesNet actual1 = *GaussianSequentialSolver(copy).eliminate(); + GaussianBayesNet actual1 = *copy.eliminateSequential(); // Create the same graph, but not by copying - GaussianFactorGraph expected = createGaussianFactorGraph(ordering); + GaussianFactorGraph expected = createGaussianFactorGraph(); // and check that original is still the same graph - EXPECT(assert_equal(expected,actual)); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ -TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet ) -{ - Ordering ord; - ord += X(2),L(1),X(1); - GaussianFactorGraph fg = createGaussianFactorGraph(ord); +TEST(GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet) { + GaussianFactorGraph fg = createGaussianFactorGraph(); // render with a given ordering - GaussianBayesNet CBN = *GaussianSequentialSolver(fg).eliminate(); + GaussianBayesNet CBN = *fg.eliminateSequential(); // True GaussianFactorGraph GaussianFactorGraph fg2(CBN); - GaussianBayesNet CBN2 = *GaussianSequentialSolver(fg2).eliminate(); - EXPECT(assert_equal(CBN,CBN2)); + GaussianBayesNet CBN2 = *fg2.eliminateSequential(); + EXPECT(assert_equal(CBN, CBN2)); } /* ************************************************************************* */ -TEST( GaussianFactorGraph, getOrdering) -{ - Ordering original; original += L(1),X(1),X(2); - FactorGraph symbolic(createGaussianFactorGraph(original)); - Permutation perm(*inference::PermutationCOLAMD(VariableIndex(symbolic))); - Ordering actual = original; actual.permuteInPlace(perm); - Ordering expected; expected += L(1),X(2),X(1); - EXPECT(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( GaussianFactorGraph, optimize_Cholesky ) -{ - // create an ordering - Ordering ord; ord += X(2),L(1),X(1); - +TEST(GaussianFactorGraph, optimize_Cholesky) { // create a graph - GaussianFactorGraph fg = createGaussianFactorGraph(ord); + GaussianFactorGraph fg = createGaussianFactorGraph(); // optimize the graph - VectorValues actual = *GaussianSequentialSolver(fg, false).optimize(); + VectorValues actual = fg.optimize(boost::none, EliminateCholesky); // verify - VectorValues expected = createCorrectDelta(ord); - - EXPECT(assert_equal(expected,actual)); + VectorValues expected = createCorrectDelta(); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ TEST( GaussianFactorGraph, optimize_QR ) { - // create an ordering - Ordering ord; ord += X(2),L(1),X(1); - // create a graph - GaussianFactorGraph fg = createGaussianFactorGraph(ord); + GaussianFactorGraph fg = createGaussianFactorGraph(); // optimize the graph - VectorValues actual = *GaussianSequentialSolver(fg, true).optimize(); + VectorValues actual = fg.optimize(boost::none, EliminateQR); // verify - VectorValues expected = createCorrectDelta(ord); - + VectorValues expected = createCorrectDelta(); EXPECT(assert_equal(expected,actual)); } /* ************************************************************************* */ -TEST( GaussianFactorGraph, combine) -{ - // create an ordering - Ordering ord; ord += X(2),L(1),X(1); - +TEST(GaussianFactorGraph, combine) { // create a test graph - GaussianFactorGraph fg1 = createGaussianFactorGraph(ord); + GaussianFactorGraph fg1 = createGaussianFactorGraph(); // create another factor graph - GaussianFactorGraph fg2 = createGaussianFactorGraph(ord); + GaussianFactorGraph fg2 = createGaussianFactorGraph(); // get sizes size_t size1 = fg1.size(); size_t size2 = fg2.size(); // combine them - fg1.combine(fg2); + fg1.push_back(fg2); - EXPECT(size1+size2 == fg1.size()); -} - -/* ************************************************************************* */ -TEST( GaussianFactorGraph, combine2) -{ - // create an ordering - Ordering ord; ord += X(2),L(1),X(1); - - // create a test graph - GaussianFactorGraph fg1 = createGaussianFactorGraph(ord); - - // create another factor graph - GaussianFactorGraph fg2 = createGaussianFactorGraph(ord); - - // get sizes - size_t size1 = fg1.size(); - size_t size2 = fg2.size(); - - // combine them - GaussianFactorGraph fg3 = GaussianFactorGraph::combine2(fg1, fg2); - - EXPECT(size1+size2 == fg3.size()); + EXPECT(size1 + size2 == fg1.size()); } /* ************************************************************************* */ // print a vector of ints if needed for debugging void print(vector v) { - for (size_t k = 0; k < v.size(); k++) - cout << v[k] << " "; + for (size_t k = 0; k < v.size(); k++) cout << v[k] << " "; cout << endl; } /* ************************************************************************* */ -TEST(GaussianFactorGraph, createSmoother) -{ - GaussianFactorGraph fg1 = createSmoother(2).first; - LONGS_EQUAL(3,fg1.size()); - GaussianFactorGraph fg2 = createSmoother(3).first; - LONGS_EQUAL(5,fg2.size()); +TEST(GaussianFactorGraph, createSmoother) { + GaussianFactorGraph fg1 = createSmoother(2); + LONGS_EQUAL(3, fg1.size()); + GaussianFactorGraph fg2 = createSmoother(3); + LONGS_EQUAL(5, fg2.size()); } /* ************************************************************************* */ double error(const VectorValues& x) { - // create an ordering - Ordering ord; ord += X(2),L(1),X(1); - - GaussianFactorGraph fg = createGaussianFactorGraph(ord); + GaussianFactorGraph fg = createGaussianFactorGraph(); return fg.error(x); } /* ************************************************************************* */ -TEST( GaussianFactorGraph, multiplication ) -{ - // create an ordering - Ordering ord; ord += X(2),L(1),X(1); - - GaussianFactorGraph A = createGaussianFactorGraph(ord); - VectorValues x = createCorrectDelta(ord); +TEST(GaussianFactorGraph, multiplication) { + GaussianFactorGraph A = createGaussianFactorGraph(); + VectorValues x = createCorrectDelta(); Errors actual = A * x; Errors expected; - expected += Vector2(-1.0,-1.0); - expected += Vector2(2.0,-1.0); + expected += Vector2(-1.0, -1.0); + expected += Vector2(2.0, -1.0); expected += Vector2(0.0, 1.0); expected += Vector2(-1.0, 1.5); - EXPECT(assert_equal(expected,actual)); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ // Extra test on elimination prompted by Michael's email to Frank 1/4/2010 -TEST( GaussianFactorGraph, elimination ) -{ - Ordering ord; - ord += X(1), X(2); +TEST(GaussianFactorGraph, elimination) { // Create Gaussian Factor Graph GaussianFactorGraph fg; - Matrix Ap = I_2x2, An = I_2x2 * -1; + Matrix Ap = I_1x1, An = I_1x1 * -1; Vector b = (Vector(1) << 0.0).finished(); - SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,2.0); - fg += ord[X(1)], An, ord[X(2)], Ap, b, sigma; - fg += ord[X(1)], Ap, b, sigma; - fg += ord[X(2)], Ap, b, sigma; + SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1, 2.0); + fg += JacobianFactor(X(1), An, X(2), Ap, b, sigma); + fg += JacobianFactor(X(1), Ap, b, sigma); + fg += JacobianFactor(X(2), Ap, b, sigma); // Eliminate - GaussianBayesNet bayesNet = *GaussianSequentialSolver(fg).eliminate(); - - // Check sigma - EXPECT_DOUBLES_EQUAL(1.0,bayesNet[ord[X(2)]]->get_sigmas()(0),1e-5); + Ordering ordering; + ordering += X(1), X(2); + GaussianBayesNet bayesNet = *fg.eliminateSequential(); // Check matrix - Matrix R;Vector d; - boost::tie(R,d) = matrix(bayesNet); - Matrix expected = (Matrix(2, 2) << - 0.707107, -0.353553, - 0.0, 0.612372).finished(); - Matrix expected2 = (Matrix(2, 2) << - 0.707107, -0.353553, - 0.0, -0.612372).finished(); - EXPECT(equal_with_abs_tol(expected, R, 1e-6) || equal_with_abs_tol(expected2, R, 1e-6)); + Matrix R; + Vector d; + boost::tie(R, d) = bayesNet.matrix(); + Matrix expected = + (Matrix(2, 2) << 0.707107, -0.353553, 0.0, 0.612372).finished(); + Matrix expected2 = + (Matrix(2, 2) << 0.707107, -0.353553, 0.0, -0.612372).finished(); + EXPECT(assert_equal(expected, R, 1e-6)); + EXPECT(equal_with_abs_tol(expected, R, 1e-6) || + equal_with_abs_tol(expected2, R, 1e-6)); } - /* ************************************************************************* */ +/* ************************************************************************* */ // Tests ported from ConstrainedGaussianFactorGraph /* ************************************************************************* */ -TEST( GaussianFactorGraph, constrained_simple ) -{ +TEST(GaussianFactorGraph, constrained_simple) { // get a graph with a constraint in it GaussianFactorGraph fg = createSimpleConstraintGraph(); EXPECT(hasConstraints(fg)); - // eliminate and solve - VectorValues actual = *GaussianSequentialSolver(fg).optimize(); + VectorValues actual = fg.eliminateSequential()->optimize(); // verify VectorValues expected = createSimpleConstraintValues(); @@ -432,14 +326,13 @@ TEST( GaussianFactorGraph, constrained_simple ) } /* ************************************************************************* */ -TEST( GaussianFactorGraph, constrained_single ) -{ +TEST(GaussianFactorGraph, constrained_single) { // get a graph with a constraint in it GaussianFactorGraph fg = createSingleConstraintGraph(); EXPECT(hasConstraints(fg)); // eliminate and solve - VectorValues actual = *GaussianSequentialSolver(fg).optimize(); + VectorValues actual = fg.eliminateSequential()->optimize(); // verify VectorValues expected = createSingleConstraintValues(); @@ -447,14 +340,13 @@ TEST( GaussianFactorGraph, constrained_single ) } /* ************************************************************************* */ -TEST( GaussianFactorGraph, constrained_multi1 ) -{ +TEST(GaussianFactorGraph, constrained_multi1) { // get a graph with a constraint in it GaussianFactorGraph fg = createMultiConstraintGraph(); EXPECT(hasConstraints(fg)); // eliminate and solve - VectorValues actual = *GaussianSequentialSolver(fg).optimize(); + VectorValues actual = fg.eliminateSequential()->optimize(); // verify VectorValues expected = createMultiConstraintValues(); @@ -472,13 +364,13 @@ TEST(GaussianFactorGraph, replace) SharedDiagonal noise(noiseModel::Isotropic::Sigma(3, 1.0)); GaussianFactorGraph::sharedFactor f1(new JacobianFactor( - ord[X(1)], I_3x3, ord[X(2)], I_3x3, Z_3x1, noise)); + X(1), I_3x3, X(2), I_3x3, Z_3x1, noise)); GaussianFactorGraph::sharedFactor f2(new JacobianFactor( - ord[X(2)], I_3x3, ord[X(3)], I_3x3, Z_3x1, noise)); + X(2), I_3x3, X(3), I_3x3, Z_3x1, noise)); GaussianFactorGraph::sharedFactor f3(new JacobianFactor( - ord[X(3)], I_3x3, ord[X(4)], I_3x3, Z_3x1, noise)); + X(3), I_3x3, X(4), I_3x3, Z_3x1, noise)); GaussianFactorGraph::sharedFactor f4(new JacobianFactor( - ord[X(5)], I_3x3, ord[X(6)], I_3x3, Z_3x1, noise)); + X(5), I_3x3, X(6), I_3x3, Z_3x1, noise)); GaussianFactorGraph actual; actual.push_back(f1); @@ -494,27 +386,6 @@ TEST(GaussianFactorGraph, replace) EXPECT(assert_equal(expected, actual)); } -/* ************************************************************************* */ -TEST(GaussianFactorGraph, createSmoother2) -{ - using namespace example; - GaussianFactorGraph fg2; - Ordering ordering; - boost::tie(fg2,ordering) = createSmoother(3); - LONGS_EQUAL(5,fg2.size()); - - // eliminate - vector x3var; x3var.push_back(ordering[X(3)]); - vector x1var; x1var.push_back(ordering[X(1)]); - GaussianBayesNet p_x3 = *GaussianSequentialSolver( - *GaussianSequentialSolver(fg2).jointFactorGraph(x3var)).eliminate(); - GaussianBayesNet p_x1 = *GaussianSequentialSolver( - *GaussianSequentialSolver(fg2).jointFactorGraph(x1var)).eliminate(); - CHECK(assert_equal(*p_x1.back(),*p_x3.front())); // should be the same because of symmetry -} - -#endif - /* ************************************************************************* */ TEST(GaussianFactorGraph, hasConstraints) { @@ -544,7 +415,7 @@ TEST( GaussianFactorGraph, conditional_sigma_failure) { // noisemodels at nonlinear level gtsam::SharedNoiseModel priorModel = noiseModel::Diagonal::Sigmas((Vector(6) << 0.05, 0.05, 3.0, 0.2, 0.2, 0.2).finished()); - gtsam::SharedNoiseModel measModel = noiseModel::Unit::Create(2); + gtsam::SharedNoiseModel measModel = kUnit2; gtsam::SharedNoiseModel elevationModel = noiseModel::Isotropic::Sigma(1, 3.0); double fov = 60; // degrees diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index 07323e0fc..309f0e1e0 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index b95f16e76..ad7cabb99 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp index e0c2b7b66..7afb4e178 100644 --- a/tests/testIterative.cpp +++ b/tests/testIterative.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testLie.cpp b/tests/testLie.cpp index 2d8a0b975..0ef12198b 100644 --- a/tests/testLie.cpp +++ b/tests/testLie.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index 195914176..d0b4a1ffa 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp index f44496e27..4bdbd9387 100644 --- a/tests/testMarginals.cpp +++ b/tests/testMarginals.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -11,7 +11,7 @@ /** * @file testMarginals.cpp - * @brief + * @brief * @author Richard Roberts * @date May 14, 2012 */ diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 6e49fa749..e1156ceba 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -11,7 +11,7 @@ /** * @file testNonlinearFactor.cpp - * @brief Unit tests for Non-Linear Factor, + * @brief Unit tests for Non-Linear Factor, * create a non linear factor graph and a values structure for it and * calculate the error for the factor. * @author Christian Potthast diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 9556a8018..290f0138e 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -17,16 +17,6 @@ * @author Christian Potthast */ -/*STL/C++*/ -#include -using namespace std; - -#include -#include -using namespace boost::assign; - -#include - #include #include #include @@ -34,7 +24,21 @@ using namespace boost::assign; #include #include #include +#include +#include +#include +#include +#include + +#include +#include +using namespace boost::assign; + +/*STL/C++*/ +#include + +using namespace std; using namespace gtsam; using namespace example; @@ -197,6 +201,47 @@ TEST(NonlinearFactorGraph, UpdateCholesky) { EXPECT(assert_equal(initial, fg.updateCholesky(initial, boost::none, dampen), 1e-6)); } +/* ************************************************************************* */ +// Example from issue #452 which threw an ILS error. The reason was a very +// weak prior on heading, which was tightened, and the ILS disappeared. +TEST(testNonlinearFactorGraph, eliminate) { + // Linearization point + Pose2 T11(0, 0, 0); + Pose2 T12(1, 0, 0); + Pose2 T21(0, 1, 0); + Pose2 T22(1, 1, 0); + + // Factor graph + auto graph = NonlinearFactorGraph(); + + // Priors + auto prior = noiseModel::Isotropic::Sigma(3, 1); + graph.add(PriorFactor(11, T11, prior)); + graph.add(PriorFactor(21, T21, prior)); + + // Odometry + auto model = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.3)); + graph.add(BetweenFactor(11, 12, T11.between(T12), model)); + graph.add(BetweenFactor(21, 22, T21.between(T22), model)); + + // Range factor + auto model_rho = noiseModel::Isotropic::Sigma(1, 0.01); + graph.add(RangeFactor(12, 22, 1.0, model_rho)); + + Values values; + values.insert(11, T11.retract(Vector3(0.1,0.2,0.3))); + values.insert(12, T12); + values.insert(21, T21); + values.insert(22, T22); + auto linearized = graph.linearize(values); + + // Eliminate + Ordering ordering; + ordering += 11, 21, 12, 22; + auto bn = linearized->eliminateSequential(ordering); + EXPECT_LONGS_EQUAL(4, bn->size()); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 3d30d6880..0e7793552 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -9,7 +9,7 @@ * -------------------------------------------------------------------------- */ -/** +/** * @file testNonlinearOptimizer.cpp * @brief Unit tests for NonlinearOptimizer class * @author Frank Dellaert diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index 6bc155214..64e111e94 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testSimulated2D.cpp b/tests/testSimulated2D.cpp index 25a91c5ce..9c342ad88 100644 --- a/tests/testSimulated2D.cpp +++ b/tests/testSimulated2D.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testSimulated2DOriented.cpp b/tests/testSimulated2DOriented.cpp index d411e7d5e..837fd1689 100644 --- a/tests/testSimulated2DOriented.cpp +++ b/tests/testSimulated2DOriented.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testSimulated3D.cpp b/tests/testSimulated3D.cpp index dcfcc735b..02f6ed762 100644 --- a/tests/testSimulated3D.cpp +++ b/tests/testSimulated3D.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index accf9a65e..fb9f7a5a2 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -15,210 +15,302 @@ * @author Frank Dellaert **/ -#include - -#if 0 - #include + +#include +#include #include -#include +#include #include #include -#include -#include +#include +#include +#include -#include +#include + +#include #include +#include +#include +#include using namespace boost::assign; +#include + using namespace std; using namespace gtsam; using namespace example; // define keys // Create key for simulated planar graph -Symbol key(int x, int y) { - return symbol_shorthand::X(1000*x+y); -} +Symbol key(int x, int y) { return symbol_shorthand::X(1000 * x + y); } /* ************************************************************************* */ -TEST( SubgraphPreconditioner, planarOrdering ) { +TEST(SubgraphPreconditioner, planarOrdering) { // Check canonical ordering Ordering expected, ordering = planarOrdering(3); expected += key(3, 3), key(2, 3), key(1, 3), key(3, 2), key(2, 2), key(1, 2), key(3, 1), key(2, 1), key(1, 1); - CHECK(assert_equal(expected,ordering)); + EXPECT(assert_equal(expected, ordering)); } /* ************************************************************************* */ /** unnormalized error */ static double error(const GaussianFactorGraph& fg, const VectorValues& x) { double total_error = 0.; - for(const GaussianFactor::shared_ptr& factor: fg) + for (const GaussianFactor::shared_ptr& factor : fg) total_error += factor->error(x); return total_error; } /* ************************************************************************* */ -TEST( SubgraphPreconditioner, planarGraph ) - { +TEST(SubgraphPreconditioner, planarGraph) { // Check planar graph construction GaussianFactorGraph A; VectorValues xtrue; boost::tie(A, xtrue) = planarGraph(3); - LONGS_EQUAL(13,A.size()); - LONGS_EQUAL(9,xtrue.size()); - DOUBLES_EQUAL(0,error(A,xtrue),1e-9); // check zero error for xtrue + LONGS_EQUAL(13, A.size()); + LONGS_EQUAL(9, xtrue.size()); + DOUBLES_EQUAL(0, error(A, xtrue), 1e-9); // check zero error for xtrue // Check that xtrue is optimal - GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(A).eliminate(); - VectorValues actual = optimize(*R1); - CHECK(assert_equal(xtrue,actual)); + GaussianBayesNet::shared_ptr R1 = A.eliminateSequential(); + VectorValues actual = R1->optimize(); + EXPECT(assert_equal(xtrue, actual)); } /* ************************************************************************* */ -TEST( SubgraphPreconditioner, splitOffPlanarTree ) -{ +TEST(SubgraphPreconditioner, splitOffPlanarTree) { // Build a planar graph GaussianFactorGraph A; VectorValues xtrue; boost::tie(A, xtrue) = planarGraph(3); // Get the spanning tree and constraints, and check their sizes - GaussianFactorGraph T, C; + GaussianFactorGraph::shared_ptr T, C; boost::tie(T, C) = splitOffPlanarTree(3, A); - LONGS_EQUAL(9,T.size()); - LONGS_EQUAL(4,C.size()); + LONGS_EQUAL(9, T->size()); + LONGS_EQUAL(4, C->size()); // Check that the tree can be solved to give the ground xtrue - GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate(); - VectorValues xbar = optimize(*R1); - CHECK(assert_equal(xtrue,xbar)); + GaussianBayesNet::shared_ptr R1 = T->eliminateSequential(); + VectorValues xbar = R1->optimize(); + EXPECT(assert_equal(xtrue, xbar)); } /* ************************************************************************* */ - -TEST( SubgraphPreconditioner, system ) -{ +TEST(SubgraphPreconditioner, system) { // Build a planar graph GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b - // Get the spanning tree and corresponding ordering - GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); - SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); - SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); + // Get the spanning tree and remaining graph + GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2 + boost::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab); // Eliminate the spanning tree to build a prior - SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 - VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 + const Ordering ord = planarOrdering(N); + auto Rc1 = Ab1->eliminateSequential(ord); // R1*x-c1 + VectorValues xbar = Rc1->optimize(); // xbar = inv(R1)*c1 // Create Subgraph-preconditioned system - VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible - SubgraphPreconditioner system(Ab2, Rc1, xbarShared); + VectorValues::shared_ptr xbarShared( + new VectorValues(xbar)); // TODO: horrible + const SubgraphPreconditioner system(Ab2, Rc1, xbarShared); - // Create zero config - VectorValues zeros = VectorValues::Zero(xbar); + // Get corresponding matrices for tests. Add dummy factors to Ab2 to make + // sure it works with the ordering. + Ordering ordering = Rc1->ordering(); // not ord in general! + Ab2->add(key(1, 1), Z_2x2, Z_2x1); + Ab2->add(key(1, 2), Z_2x2, Z_2x1); + Ab2->add(key(1, 3), Z_2x2, Z_2x1); + Matrix A, A1, A2; + Vector b, b1, b2; + std::tie(A, b) = Ab.jacobian(ordering); + std::tie(A1, b1) = Ab1->jacobian(ordering); + std::tie(A2, b2) = Ab2->jacobian(ordering); + Matrix R1 = Rc1->matrix(ordering).first; + Matrix Abar(13 * 2, 9 * 2); + Abar.topRows(9 * 2) = Matrix::Identity(9 * 2, 9 * 2); + Abar.bottomRows(8) = A2.topRows(8) * R1.inverse(); + + // Helper function to vectorize in correct order, which is the order in which + // we eliminated the spanning tree. + auto vec = [ordering](const VectorValues& x) { return x.vector(ordering); }; // Set up y0 as all zeros - VectorValues y0 = zeros; + const VectorValues y0 = system.zero(); // y1 = perturbed y0 - VectorValues y1 = zeros; - y1[1] = Vector2(1.0, -1.0); + VectorValues y1 = system.zero(); + y1[key(3, 3)] = Vector2(1.0, -1.0); - // Check corresponding x values - VectorValues expected_x1 = xtrue, x1 = system.x(y1); - expected_x1[1] = Vector2(2.01, 2.99); - expected_x1[0] = Vector2(3.01, 2.99); - CHECK(assert_equal(xtrue, system.x(y0))); - CHECK(assert_equal(expected_x1,system.x(y1))); + // Check backSubstituteTranspose works with R1 + VectorValues actual = Rc1->backSubstituteTranspose(y1); + Vector expected = R1.transpose().inverse() * vec(y1); + EXPECT(assert_equal(expected, vec(actual))); + + // Check corresponding x values + // for y = 0, we get xbar: + EXPECT(assert_equal(xbar, system.x(y0))); + // for non-zero y, answer is x = xbar + inv(R1)*y + const Vector expected_x1 = vec(xbar) + R1.inverse() * vec(y1); + const VectorValues x1 = system.x(y1); + EXPECT(assert_equal(expected_x1, vec(x1))); // Check errors - DOUBLES_EQUAL(0,error(Ab,xtrue),1e-9); - DOUBLES_EQUAL(3,error(Ab,x1),1e-9); - DOUBLES_EQUAL(0,error(system,y0),1e-9); - DOUBLES_EQUAL(3,error(system,y1),1e-9); + DOUBLES_EQUAL(0, error(Ab, xbar), 1e-9); + DOUBLES_EQUAL(0, system.error(y0), 1e-9); + DOUBLES_EQUAL(2, error(Ab, x1), 1e-9); + DOUBLES_EQUAL(2, system.error(y1), 1e-9); - // Test gradient in x - VectorValues expected_gx0 = zeros; - VectorValues expected_gx1 = zeros; - CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue))); - expected_gx1[2] = Vector2(-100., 100.); - expected_gx1[4] = Vector2(-100., 100.); - expected_gx1[1] = Vector2(200., -200.); - expected_gx1[3] = Vector2(-100., 100.); - expected_gx1[0] = Vector2(100., -100.); - CHECK(assert_equal(expected_gx1,gradient(Ab,x1))); + // Check that transposeMultiplyAdd <=> y += alpha * Abar' * e + // We check for e1 =[1;0] and e2=[0;1] corresponding to T and C + const double alpha = 0.5; + Errors e1, e2; + for (size_t i = 0; i < 13; i++) { + e1 += i < 9 ? Vector2(1, 1) : Vector2(0, 0); + e2 += i >= 9 ? Vector2(1, 1) : Vector2(0, 0); + } + Vector ee1(13 * 2), ee2(13 * 2); + ee1 << Vector::Ones(9 * 2), Vector::Zero(4 * 2); + ee2 << Vector::Zero(9 * 2), Vector::Ones(4 * 2); + + // Check transposeMultiplyAdd for e1 + VectorValues y = system.zero(); + system.transposeMultiplyAdd(alpha, e1, y); + Vector expected_y = alpha * Abar.transpose() * ee1; + EXPECT(assert_equal(expected_y, vec(y))); + + // Check transposeMultiplyAdd for e2 + y = system.zero(); + system.transposeMultiplyAdd(alpha, e2, y); + expected_y = alpha * Abar.transpose() * ee2; + EXPECT(assert_equal(expected_y, vec(y))); // Test gradient in y - VectorValues expected_gy0 = zeros; - VectorValues expected_gy1 = zeros; - expected_gy1[2] = Vector2(2., -2.); - expected_gy1[4] = Vector2(-2., 2.); - expected_gy1[1] = Vector2(3., -3.); - expected_gy1[3] = Vector2(-1., 1.); - expected_gy1[0] = Vector2(1., -1.); - CHECK(assert_equal(expected_gy0,gradient(system,y0))); - CHECK(assert_equal(expected_gy1,gradient(system,y1))); - - // Check it numerically for good measure - // TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1) - // Vector numerical_g1 = numericalGradient (error, y1, 0.001); - // Vector expected_g1 = (Vector(18) << 0., 0., 0., 0., 2., -2., 0., 0., -2., 2., - // 3., -3., 0., 0., -1., 1., 1., -1.); - // CHECK(assert_equal(expected_g1,numerical_g1)); + auto g = system.gradient(y0); + Vector expected_g = Vector::Zero(18); + EXPECT(assert_equal(expected_g, vec(g))); } /* ************************************************************************* */ -TEST( SubgraphPreconditioner, conjugateGradients ) -{ +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "JacobianFactor"); + +// Read from XML file +static GaussianFactorGraph read(const string& name) { + auto inputFile = findExampleDataFile(name); + ifstream is(inputFile); + if (!is.is_open()) throw runtime_error("Cannot find file " + inputFile); + boost::archive::xml_iarchive in_archive(is); + GaussianFactorGraph Ab; + in_archive >> boost::serialization::make_nvp("graph", Ab); + return Ab; +} + +TEST(SubgraphSolver, Solves) { + // Create preconditioner + SubgraphPreconditioner system; + + // We test on three different graphs + const auto Ab1 = planarGraph(3).first; + const auto Ab2 = read("toy3D"); + const auto Ab3 = read("randomGrid3D"); + + // For all graphs, test solve and solveTranspose + for (const auto& Ab : {Ab1, Ab2, Ab3}) { + // Call build, a non-const method needed to make solve work :-( + KeyInfo keyInfo(Ab); + std::map lambda; + system.build(Ab, keyInfo, lambda); + + // Create a perturbed (non-zero) RHS + const auto xbar = system.Rc1()->optimize(); // merely for use in zero below + auto values_y = VectorValues::Zero(xbar); + auto it = values_y.begin(); + it->second.setConstant(100); + ++it; + it->second.setConstant(-100); + + // Solve the VectorValues way + auto values_x = system.Rc1()->backSubstitute(values_y); + + // Solve the matrix way, this really just checks BN::backSubstitute + // This only works with Rc1 ordering, not with keyInfo ! + // TODO(frank): why does this not work with an arbitrary ordering? + const auto ord = system.Rc1()->ordering(); + const Matrix R1 = system.Rc1()->matrix(ord).first; + auto ord_y = values_y.vector(ord); + auto vector_x = R1.inverse() * ord_y; + EXPECT(assert_equal(vector_x, values_x.vector(ord))); + + // Test that 'solve' does implement x = R^{-1} y + // We do this by asserting it gives same answer as backSubstitute + // Only works with keyInfo ordering: + const auto ordering = keyInfo.ordering(); + auto vector_y = values_y.vector(ordering); + const size_t N = R1.cols(); + Vector solve_x = Vector::Zero(N); + system.solve(vector_y, solve_x); + EXPECT(assert_equal(values_x.vector(ordering), solve_x)); + + // Test that transposeSolve does implement x = R^{-T} y + // We do this by asserting it gives same answer as backSubstituteTranspose + auto values_x2 = system.Rc1()->backSubstituteTranspose(values_y); + Vector solveT_x = Vector::Zero(N); + system.transposeSolve(vector_y, solveT_x); + EXPECT(assert_equal(values_x2.vector(ordering), solveT_x)); + } +} + +/* ************************************************************************* */ +TEST(SubgraphPreconditioner, conjugateGradients) { // Build a planar graph GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b - // Get the spanning tree and corresponding ordering - GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); - SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); - SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); + // Get the spanning tree + GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2 + boost::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab); // Eliminate the spanning tree to build a prior - Ordering ordering = planarOrdering(N); - SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 - VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 + SubgraphPreconditioner::sharedBayesNet Rc1 = + Ab1->eliminateSequential(); // R1*x-c1 + VectorValues xbar = Rc1->optimize(); // xbar = inv(R1)*c1 // Create Subgraph-preconditioned system - VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible + VectorValues::shared_ptr xbarShared( + new VectorValues(xbar)); // TODO: horrible SubgraphPreconditioner system(Ab2, Rc1, xbarShared); // Create zero config y0 and perturbed config y1 VectorValues y0 = VectorValues::Zero(xbar); VectorValues y1 = y0; - y1[1] = Vector2(1.0, -1.0); + y1[key(2, 2)] = Vector2(1.0, -1.0); VectorValues x1 = system.x(y1); // Solve for the remaining constraints using PCG ConjugateGradientParameters parameters; VectorValues actual = conjugateGradients(system, y1, parameters); - CHECK(assert_equal(y0,actual)); + EXPECT(assert_equal(y0,actual)); // Compare with non preconditioned version: VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters); - CHECK(assert_equal(xtrue,actual2,1e-4)); + EXPECT(assert_equal(xtrue, actual2, 1e-4)); } -#endif - /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp index aeeed1b9f..cca13c822 100644 --- a/tests/testSubgraphSolver.cpp +++ b/tests/testSubgraphSolver.cpp @@ -15,26 +15,28 @@ * @author Yong-Dian Jian **/ -#include - -#if 0 +#include #include -#include #include #include #include -#include +#include +#include #include #include -#include +#include + #include using namespace boost::assign; using namespace std; using namespace gtsam; -using namespace example; + +static size_t N = 3; +static SubgraphSolverParameters kParameters; +static auto kOrdering = example::planarOrdering(N); /* ************************************************************************* */ /** unnormalized error */ @@ -45,6 +47,32 @@ static double error(const GaussianFactorGraph& fg, const VectorValues& x) { return total_error; } +/* ************************************************************************* */ +TEST( SubgraphSolver, Parameters ) +{ + LONGS_EQUAL(SubgraphSolverParameters::SILENT, kParameters.verbosity()); + LONGS_EQUAL(500, kParameters.maxIterations()); +} + +/* ************************************************************************* */ +TEST( SubgraphSolver, splitFactorGraph ) +{ + // Build a planar graph + GaussianFactorGraph Ab; + VectorValues xtrue; + std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b + + SubgraphBuilderParameters params; + params.augmentationFactor = 0.0; + SubgraphBuilder builder(params); + auto subgraph = builder(Ab); + EXPECT_LONGS_EQUAL(9, subgraph.size()); + + GaussianFactorGraph::shared_ptr Ab1, Ab2; + std::tie(Ab1, Ab2) = splitFactorGraph(Ab, subgraph); + EXPECT_LONGS_EQUAL(9, Ab1->size()); + EXPECT_LONGS_EQUAL(13, Ab2->size()); +} /* ************************************************************************* */ TEST( SubgraphSolver, constructor1 ) @@ -52,13 +80,11 @@ TEST( SubgraphSolver, constructor1 ) // Build a planar graph GaussianFactorGraph Ab; VectorValues xtrue; - size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b - // The first constructor just takes a factor graph (and parameters) + // The first constructor just takes a factor graph (and kParameters) // and it will split the graph into A1 and A2, where A1 is a spanning tree - SubgraphSolverParameters parameters; - SubgraphSolver solver(Ab, parameters); + SubgraphSolver solver(Ab, kParameters, kOrdering); VectorValues optimized = solver.optimize(); // does PCG optimization DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); } @@ -70,16 +96,15 @@ TEST( SubgraphSolver, constructor2 ) GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b - // Get the spanning tree and corresponding ordering - GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); + // Get the spanning tree + GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2 + std::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab); - // The second constructor takes two factor graphs, - // so the caller can specify the preconditioner (Ab1) and the constraints that are left out (Ab2) - SubgraphSolverParameters parameters; - SubgraphSolver solver(Ab1_, Ab2_, parameters); + // The second constructor takes two factor graphs, so the caller can specify + // the preconditioner (Ab1) and the constraints that are left out (Ab2) + SubgraphSolver solver(*Ab1, Ab2, kParameters, kOrdering); VectorValues optimized = solver.optimize(); DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); } @@ -91,26 +116,22 @@ TEST( SubgraphSolver, constructor3 ) GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b - // Get the spanning tree and corresponding ordering - GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); + // Get the spanning tree and corresponding kOrdering + GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2 + std::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab); - // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2 via QR factorization, where R1 is square UT - GaussianBayesNet::shared_ptr Rc1 = // - EliminationTree::Create(Ab1_)->eliminate(&EliminateQR); + // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2, where R1 is square UT + auto Rc1 = Ab1->eliminateSequential(); // The third constructor allows the caller to pass an already solved preconditioner Rc1_ // as a Bayes net, in addition to the "loop closing constraints" Ab2, as before - SubgraphSolverParameters parameters; - SubgraphSolver solver(Rc1, Ab2_, parameters); + SubgraphSolver solver(Rc1, Ab2, kParameters); VectorValues optimized = solver.optimize(); DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); } -#endif - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/timing/timeAdaptAutoDiff.cpp b/timing/timeAdaptAutoDiff.cpp index 8950c636b..d5431cee7 100644 --- a/timing/timeAdaptAutoDiff.cpp +++ b/timing/timeAdaptAutoDiff.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/timing/timeCameraExpression.cpp b/timing/timeCameraExpression.cpp index 208c991fd..07eeb1bcb 100644 --- a/timing/timeCameraExpression.cpp +++ b/timing/timeCameraExpression.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -68,7 +68,7 @@ int main() { // 20.3 musecs/call NonlinearFactor::shared_ptr f2 = boost::make_shared >(model, z, - uncalibrate(K, project(transform_to(x, p)))); + uncalibrate(K, project(transformTo(x, p)))); time("Bin(Leaf,Un(Bin(Leaf,Leaf))): ", f2, values); // ExpressionFactor ternary @@ -93,7 +93,7 @@ int main() { // 16.0 musecs/call NonlinearFactor::shared_ptr g2 = boost::make_shared >(model, z, - uncalibrate(Cal3_S2_(*fixedK), project(transform_to(x, p)))); + uncalibrate(Cal3_S2_(*fixedK), project(transformTo(x, p)))); time("Bin(Cnst,Un(Bin(Leaf,Leaf))): ", g2, values); // ExpressionFactor, optimized diff --git a/timing/timeCholesky.cpp b/timing/timeCholesky.cpp index 4dd349593..15be64313 100644 --- a/timing/timeCholesky.cpp +++ b/timing/timeCholesky.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/timing/timeFactorOverhead.cpp b/timing/timeFactorOverhead.cpp index 97c40e6f4..d9c19703c 100644 --- a/timing/timeFactorOverhead.cpp +++ b/timing/timeFactorOverhead.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/timing/timeGaussianFactor.cpp b/timing/timeGaussianFactor.cpp index e6ae51aa4..3aecac3c1 100644 --- a/timing/timeGaussianFactor.cpp +++ b/timing/timeGaussianFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -54,7 +54,7 @@ int main() { // create a linear factor Matrix Ax2 = (Matrix(8, 2) << - // x2 + // x2 -5., 0., +0.,-5., 10., 0., @@ -64,9 +64,9 @@ int main() 10., 0., +0.,10. ).finished(); - + Matrix Al1 = (Matrix(8, 10) << - // l1 + // l1 5., 0.,1.,2.,3.,4.,5.,6.,7.,8., 0., 5.,1.,2.,3.,4.,5.,6.,7.,8., 0., 0.,1.,2.,3.,4.,5.,6.,7.,8., @@ -76,7 +76,7 @@ int main() 0., 0.,1.,2.,3.,4.,5.,6.,7.,8., 0., 0.,1.,2.,3.,4.,5.,6.,7.,8. ).finished(); - + Matrix Ax1 = (Matrix(8, 2) << // x1 0.00, 0.,1.,2.,3.,4.,5.,6.,7.,8., @@ -99,7 +99,7 @@ int main() b2(5) = 1.5; b2(6) = 2; b2(7) = -1; - + // time eliminate JacobianFactor combined(_x2_, Ax2, _l1_, Al1, _x1_, Ax1, b2, noiseModel::Isotropic::Sigma(8,1)); long timeLog = clock(); diff --git a/timing/timeGaussianFactorGraph.cpp b/timing/timeGaussianFactorGraph.cpp index cd11f6360..1efdb9542 100644 --- a/timing/timeGaussianFactorGraph.cpp +++ b/timing/timeGaussianFactorGraph.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/timing/timeIncremental.cpp b/timing/timeIncremental.cpp index e09b83232..ec5fc3fa5 100644 --- a/timing/timeIncremental.cpp +++ b/timing/timeIncremental.cpp @@ -155,8 +155,8 @@ int main(int argc, char *argv[]) { Pose pose = isam2.calculateEstimate(poseKey); Rot2 measuredBearing = measurement->measured().bearing(); double measuredRange = measurement->measured().range(); - newVariables.insert(lmKey, - pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0)))); + newVariables.insert(lmKey, + pose.transformFrom(measuredBearing.rotate(Point2(measuredRange, 0.0)))); } } else diff --git a/timing/timeLinearize.h b/timing/timeLinearize.h index dfb63ffa3..a2a77168c 100644 --- a/timing/timeLinearize.h +++ b/timing/timeLinearize.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/timing/timeMatrix.cpp b/timing/timeMatrix.cpp index a094f9628..c3e9f32f4 100644 --- a/timing/timeMatrix.cpp +++ b/timing/timeMatrix.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/timing/timeMatrixOps.cpp b/timing/timeMatrixOps.cpp index a2010891b..f16be40f6 100644 --- a/timing/timeMatrixOps.cpp +++ b/timing/timeMatrixOps.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/timing/timeOneCameraExpression.cpp b/timing/timeOneCameraExpression.cpp index 962e7ee21..73b2ff55f 100644 --- a/timing/timeOneCameraExpression.cpp +++ b/timing/timeOneCameraExpression.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -50,7 +50,7 @@ int main() { #ifdef TERNARY (model, z, project3(x, p, K)); #else - (model, z, uncalibrate(K, project(transform_to(x, p)))); + (model, z, uncalibrate(K, project(transformTo(x, p)))); #endif time("timing:", f, values); diff --git a/timing/timePose2.cpp b/timing/timePose2.cpp index badc03cf8..6af3e0163 100644 --- a/timing/timePose2.cpp +++ b/timing/timePose2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/timing/timeRot2.cpp b/timing/timeRot2.cpp index 8b48d4b16..387d458ea 100644 --- a/timing/timeRot2.cpp +++ b/timing/timeRot2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/timing/timeRot3.cpp b/timing/timeRot3.cpp index 5806149f3..00c02c250 100644 --- a/timing/timeRot3.cpp +++ b/timing/timeRot3.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/timing/timeSFMBALcamTnav.cpp b/timing/timeSFMBALcamTnav.cpp index 33680e813..0475253ec 100644 --- a/timing/timeSFMBALcamTnav.cpp +++ b/timing/timeSFMBALcamTnav.cpp @@ -42,8 +42,8 @@ int main(int argc, char* argv[]) { Point3_ nav_point_(P(j)); graph.addExpressionFactor( gNoiseModel, z, - uncalibrate(calibration_, // now using transform_from !!!: - project(transform_from(camTnav_, nav_point_)))); + uncalibrate(calibration_, // now using transformFrom !!!: + project(transformFrom(camTnav_, nav_point_)))); } } diff --git a/timing/timeSFMBALnavTcam.cpp b/timing/timeSFMBALnavTcam.cpp index dad1edd4e..fc9568626 100644 --- a/timing/timeSFMBALnavTcam.cpp +++ b/timing/timeSFMBALnavTcam.cpp @@ -43,7 +43,7 @@ int main(int argc, char* argv[]) { graph.addExpressionFactor( gNoiseModel, z, uncalibrate(calibration_, - project(transform_to(navTcam_, nav_point_)))); + project(transformTo(navTcam_, nav_point_)))); } } diff --git a/timing/timeSFMExpressions.cpp b/timing/timeSFMExpressions.cpp index cfb8e0dc6..8ab949be7 100644 --- a/timing/timeSFMExpressions.cpp +++ b/timing/timeSFMExpressions.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -61,7 +61,7 @@ int main() { #ifdef TERNARY (model, z, project3(x[i], p[j], K)); #else - (model, z, uncalibrate(K, project(transform_to(x[i], p[j])))); + (model, z, uncalibrate(K, project(transformTo(x[i], p[j])))); #endif graph.push_back(f); } diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 913c11284..f85aed72e 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/Argument.h b/wrap/Argument.h index 0a4ebba9d..c08eb0be9 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -101,7 +101,7 @@ struct ArgumentList: public std::vector { ArgumentList expandTemplate(const TemplateSubstitution& ts) const; bool isSameSignature(const ArgumentList& other) const { - for(size_t i = 0; i #include -#include -#include -#include +#include +#include +#include #include #include // std::ostream_iterator -//#include // on Linux GCC: fails with error regarding needing C++0x std flags -//#include // same failure as above -#include // works on Linux GCC +//#include // on Linux GCC: fails with error regarding needing C++0x std flags +//#include // same failure as above +#include // works on Linux GCC using namespace std; using namespace wrap; @@ -87,20 +87,20 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, vector& functionNames) const { - // Create namespace folders + // Create namespace folders createNamespaceStructure(namespaces(), toolboxPath); - // open destination classFile + // open destination classFile string classFile = matlabName(toolboxPath); FileWriter proxyFile(classFile, verbose_, "%"); - // get the name of actual matlab object + // get the name of actual matlab object const string matlabQualName = qualifiedName("."); const string matlabUniqueName = qualifiedName(); const string cppName = qualifiedName("::"); - // emit class proxy code - // we want our class to inherit the handle class for memory purposes + // emit class proxy code + // we want our class to inherit the handle class for memory purposes const string parent = parentClass ? parentClass->qualifiedName(".") : "handle"; comment_fragment(proxyFile); @@ -110,20 +110,20 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, proxyFile.oss << " end\n"; proxyFile.oss << " methods\n"; - // Constructor + // Constructor proxyFile.oss << " function obj = " << name() << "(varargin)\n"; - // Special pointer constructors - one in MATLAB to create an object and - // assign a pointer returned from a C++ function. In turn this MATLAB - // constructor calls a special C++ function that just adds the object to - // its collector. This allows wrapped functions to return objects in - // other wrap modules - to add these to their collectors the pointer is - // passed from one C++ module into matlab then back into the other C++ - // module. + // Special pointer constructors - one in MATLAB to create an object and + // assign a pointer returned from a C++ function. In turn this MATLAB + // constructor calls a special C++ function that just adds the object to + // its collector. This allows wrapped functions to return objects in + // other wrap modules - to add these to their collectors the pointer is + // passed from one C++ module into matlab then back into the other C++ + // module. pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName, functionNames); wrapperFile.oss << "\n"; - // Regular constructors + // Regular constructors boost::optional cppBaseName = qualifiedParent(); for (size_t i = 0; i < constructor.nrOverloads(); i++) { ArgumentList args = constructor.argumentList(i); @@ -145,7 +145,7 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, proxyFile.oss << " obj.ptr_" << matlabUniqueName << " = my_ptr;\n"; proxyFile.oss << " end\n\n"; - // Deconstructor + // Deconstructor { const int id = (int) functionNames.size(); deconstructor.proxy_fragment(proxyFile, wrapperName, matlabUniqueName, id); @@ -160,7 +160,7 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, proxyFile.oss << " function disp(obj), obj.display; end\n %DISP Calls print on the object\n"; - // Methods + // Methods for(const Methods::value_type& name_m: methods_) { const Method& m = name_m.second; m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, @@ -175,7 +175,7 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, proxyFile.oss << "\n"; proxyFile.oss << " methods(Static = true)\n"; - // Static methods + // Static methods for(const StaticMethods::value_type& name_m: static_methods) { const StaticMethod& m = name_m.second; m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, @@ -190,7 +190,7 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, proxyFile.oss << " end\n"; proxyFile.oss << "end\n"; - // Close file + // Close file proxyFile.emit(true); } @@ -217,8 +217,8 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, functionNames.push_back(upcastFromVoidFunctionName); } - // MATLAB constructor that assigns pointer to matlab object then calls c++ - // function to add the object to the collector. + // MATLAB constructor that assigns pointer to matlab object then calls c++ + // function to add the object to the collector. if (isVirtual) { proxyFile.oss << " if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void')))"; @@ -241,25 +241,25 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, proxyFile.oss << " "; else proxyFile.oss << " base_ptr = "; - proxyFile.oss << wrapperName << "(" << collectorInsertId << ", my_ptr);\n"; // Call collector insert and get base class ptr + proxyFile.oss << wrapperName << "(" << collectorInsertId << ", my_ptr);\n"; // Call collector insert and get base class ptr - // C++ function to add pointer from MATLAB to collector. The pointer always - // comes from a C++ return value; this mechanism allows the object to be added - // to a collector in a different wrap module. If this class has a base class, - // a new pointer to the base class is allocated and returned. + // C++ function to add pointer from MATLAB to collector. The pointer always + // comes from a C++ return value; this mechanism allows the object to be added + // to a collector in a different wrap module. If this class has a base class, + // a new pointer to the base class is allocated and returned. wrapperFile.oss << "void " << collectorInsertFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; wrapperFile.oss << "{\n"; wrapperFile.oss << " mexAtExit(&_deleteAllObjects);\n"; - // Typedef boost::shared_ptr + // Typedef boost::shared_ptr wrapperFile.oss << " typedef boost::shared_ptr<" << cppName << "> Shared;\n"; wrapperFile.oss << "\n"; - // Get self pointer passed in + // Get self pointer passed in wrapperFile.oss << " Shared *self = *reinterpret_cast (mxGetData(in[0]));\n"; - // Add to collector + // Add to collector wrapperFile.oss << " collector_" << matlabUniqueName << ".insert(self);\n"; - // If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy) + // If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy) boost::optional cppBaseName = qualifiedParent(); if (cppBaseName) { wrapperFile.oss << "\n"; @@ -272,10 +272,10 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, } wrapperFile.oss << "}\n"; - // If this is a virtual function, C++ function to dynamic upcast it from a - // shared_ptr. This mechanism allows automatic dynamic creation of the - // real underlying derived-most class when a C++ method returns a virtual - // base class. + // If this is a virtual function, C++ function to dynamic upcast it from a + // shared_ptr. This mechanism allows automatic dynamic creation of the + // real underlying derived-most class when a C++ method returns a virtual + // base class. if (isVirtual) wrapperFile.oss << "\n" "void " << upcastFromVoidFunctionName diff --git a/wrap/Class.h b/wrap/Class.h index 910ecde57..3df37fe67 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -72,9 +72,9 @@ public: // Then the instance variables are set directly by the Module constructor std::vector templateArgs; ///< Template arguments std::string typedefName; ///< The name to typedef *from*, if this class is actually a typedef, i.e. typedef [typedefName] [name] - std::vector templateInstTypeList; ///< the original typelist used to instantiate this class from a template. + std::vector templateInstTypeList; ///< the original typelist used to instantiate this class from a template. ///< Empty if it's not an instantiation. Needed for template classes in Cython pxd. - boost::optional templateClass = boost::none; ///< qualified name of the original template class from which this class was instantiated. + boost::optional templateClass = boost::none; ///< qualified name of the original template class from which this class was instantiated. ///< boost::none if not an instantiation. Needed for template classes in Cython pxd. bool isVirtual; ///< Whether the class is part of a virtual inheritance chain bool isSerializable; ///< Whether we can use boost.serialization to serialize the class - creates exports diff --git a/wrap/Constructor.h b/wrap/Constructor.h index 4292d5645..172cd24a4 100644 --- a/wrap/Constructor.h +++ b/wrap/Constructor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/Deconstructor.cpp b/wrap/Deconstructor.cpp index 9e6495602..7bb366e3f 100644 --- a/wrap/Deconstructor.cpp +++ b/wrap/Deconstructor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -48,11 +48,11 @@ string Deconstructor::wrapper_fragment(FileWriter& file, const string& cppClassName, const string& matlabUniqueName, int id) const { - + const string matlabName = matlab_wrapper_name(matlabUniqueName); const string wrapFunctionName = matlabUniqueName + "_deconstructor_" + boost::lexical_cast(id); - + file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl; file.oss << "{" << endl; file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl; diff --git a/wrap/Deconstructor.h b/wrap/Deconstructor.h index f0dbc2709..ee2f4ea19 100644 --- a/wrap/Deconstructor.h +++ b/wrap/Deconstructor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/ForwardDeclaration.h b/wrap/ForwardDeclaration.h index 127823e82..190387ecc 100644 --- a/wrap/ForwardDeclaration.h +++ b/wrap/ForwardDeclaration.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/FullyOverloadedFunction.h b/wrap/FullyOverloadedFunction.h index 87c5169dd..6b40f6a70 100644 --- a/wrap/FullyOverloadedFunction.h +++ b/wrap/FullyOverloadedFunction.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -79,8 +79,8 @@ public: if (argLCount != nrOverloads() - 1) proxyFile.oss << ", "; else - proxyFile.oss << " : returns " << returnValue(0).return_type(false) - << std::endl; + proxyFile.oss << " : returns " << returnValue(0).returnType() + << std::endl; argLCount++; } } @@ -91,8 +91,8 @@ public: for(ArgumentList argList: argLists_) { proxyFile.oss << "%"; argList.emit_prototype(proxyFile, name); - proxyFile.oss << " : returns " << returnVals_[i++].return_type(false) - << std::endl; + proxyFile.oss << " : returns " << returnVals_[i++].returnType() + << std::endl; } } diff --git a/wrap/Function.cpp b/wrap/Function.cpp index 8da667fae..80b0adbbe 100644 --- a/wrap/Function.cpp +++ b/wrap/Function.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/Function.h b/wrap/Function.h index ad57a28c8..c39b3231c 100644 --- a/wrap/Function.h +++ b/wrap/Function.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp index c8482f2c4..13616d64a 100644 --- a/wrap/GlobalFunction.cpp +++ b/wrap/GlobalFunction.cpp @@ -94,8 +94,6 @@ void GlobalFunction::generateSingleFunction(const string& toolboxPath, // start file.oss << "{\n"; - returnVal.wrapTypeUnwrap(file); - // check arguments // NOTE: for static functions, there is no object passed file.oss << " checkArguments(\"" << matlabUniqueName @@ -136,7 +134,7 @@ void GlobalFunction::python_wrapper(FileWriter& wrapperFile) const { /* ************************************************************************* */ void GlobalFunction::emit_cython_pxd(FileWriter& file) const { file.oss << "cdef extern from \"" << includeFile << "\" namespace \"" - << overloads[0].qualifiedNamespaces("::") + << overloads[0].qualifiedNamespaces("::") << "\":" << endl; for (size_t i = 0; i < nrOverloads(); ++i) { file.oss << " "; diff --git a/wrap/GlobalFunction.h b/wrap/GlobalFunction.h index 473ebadef..9742733f7 100644 --- a/wrap/GlobalFunction.h +++ b/wrap/GlobalFunction.h @@ -2,7 +2,7 @@ * @file GlobalFunction.h * * @brief Implements codegen for a global function wrapped in matlab - * + * * @date Jul 22, 2012 * @author Alex Cunningham */ diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 8f0ef17b9..f7247341c 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -64,8 +64,8 @@ string Method::wrapper_call(FileWriter& wrapperFile, Str cppClassName, << "\",nargout,nargin-1," << args.size() << ");\n"; // get class pointer - // example: shared_ptr = unwrap_shared_ptr< Test >(in[0], "Test"); - wrapperFile.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName + // example: auto obj = unwrap_shared_ptr< Test >(in[0], "Test"); + wrapperFile.oss << " auto obj = unwrap_shared_ptr<" << cppClassName << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; // unwrap arguments, see Argument.cpp, we start at 1 as first is obj diff --git a/wrap/Method.h b/wrap/Method.h index bfa4a65da..88a73cd80 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/MethodBase.cpp b/wrap/MethodBase.cpp index ef169d989..a2ed68780 100644 --- a/wrap/MethodBase.cpp +++ b/wrap/MethodBase.cpp @@ -108,11 +108,6 @@ string MethodBase::wrapper_fragment( // start wrapperFile.oss << "{\n"; - returnVal.wrapTypeUnwrap(wrapperFile); - - wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName - << "> Shared;" << endl; - // get call // for static methods: cppClassName::staticMethod // for instance methods: obj->instanceMethod diff --git a/wrap/Module.h b/wrap/Module.h index 732b66507..2a8344551 100644 --- a/wrap/Module.h +++ b/wrap/Module.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -84,7 +84,7 @@ private: const std::vector instantiations); static std::vector GenerateValidTypes( const std::vector& classes, - const std::vector& forwardDeclarations, + const std::vector& forwardDeclarations, const std::vector& typedefs); static void WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile, const std::string& moduleName, const std::vector& classes); diff --git a/wrap/Qualified.h b/wrap/Qualified.h index 4dc15dda1..416db239d 100644 --- a/wrap/Qualified.h +++ b/wrap/Qualified.h @@ -95,7 +95,7 @@ public: } bool match(const std::vector& templateArgs) const { - for(const std::string& s: templateArgs) + for(const std::string& s: templateArgs) if (match(s)) return true; return false; } diff --git a/wrap/ReturnType.cpp b/wrap/ReturnType.cpp index 506e7d471..fdf86d975 100644 --- a/wrap/ReturnType.cpp +++ b/wrap/ReturnType.cpp @@ -12,11 +12,6 @@ using namespace std; using namespace wrap; -/* ************************************************************************* */ -string ReturnType::str(bool add_ptr) const { - return maybe_shared_ptr(add_ptr && isPtr, qualifiedName("::"), name()); -} - /* ************************************************************************* */ void ReturnType::wrap_result(const string& out, const string& result, FileWriter& wrapperFile, @@ -35,9 +30,10 @@ void ReturnType::wrap_result(const string& out, const string& result, // A virtual class needs to be cloned, so the whole hierarchy is // returned objCopy = result + ".clone()"; - else + else { // ...but a non-virtual class can just be copied - objCopy = "Shared" + name() + "(new " + cppType + "(" + result + "))"; + objCopy = "boost::make_shared<" + cppType + ">(" + result + ")"; + } } // e.g. out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point3", false); wrapperFile.oss << out << " = wrap_shared_ptr(" << objCopy << ",\"" @@ -46,25 +42,18 @@ void ReturnType::wrap_result(const string& out, const string& result, } else if (isPtr) { // Handle shared pointer case for BASIS/EIGEN/VOID - wrapperFile.oss << " {\n Shared" << name() << "* ret = new Shared" - << name() << "(" << result << ");" << endl; - wrapperFile.oss << out << " = wrap_shared_ptr(ret,\"" << matlabType + // This case does not actually occur in GTSAM wrappers, so untested! + wrapperFile.oss << " {\n boost::shared_ptr<" << qualifiedName("::") + << "> shared(" << result << ");" << endl; + wrapperFile.oss << out << " = wrap_shared_ptr(shared,\"" << matlabType << "\");\n }\n"; } else if (matlabType != "void") - // Handle normal case case for BASIS/EIGEN - wrapperFile.oss << out << " = wrap< " << str(false) << " >(" << result + wrapperFile.oss << out << " = wrap< " << qualifiedName("::") << " >(" << result << ");\n"; } -/* ************************************************************************* */ -void ReturnType::wrapTypeUnwrap(FileWriter& wrapperFile) const { - if (category == CLASS) - wrapperFile.oss << " typedef boost::shared_ptr<" << qualifiedName("::") - << "> Shared" << name() << ";" << endl; -} - /* ************************************************************************* */ void ReturnType::emit_cython_pxd( FileWriter& file, const std::string& className, @@ -91,7 +80,7 @@ std::string ReturnType::pyx_returnType(bool addShared) const { /* ************************************************************************* */ std::string ReturnType::pyx_casting(const std::string& var, bool isSharedVar) const { - if (isEigen()) { + if (isEigen()) { string s = "ndarray_copy(" + var + ")"; if (pyxClassName() == "Vector") return s + ".squeeze()"; diff --git a/wrap/ReturnType.h b/wrap/ReturnType.h index de1835f28..ba5086507 100644 --- a/wrap/ReturnType.h +++ b/wrap/ReturnType.h @@ -54,15 +54,10 @@ struct ReturnType : public Qualified { private: friend struct ReturnValue; - std::string str(bool add_ptr) const; - /// Example: out[1] = wrap_shared_ptr(pairResult.second,"Test", false); void wrap_result(const std::string& out, const std::string& result, FileWriter& wrapperFile, const TypeAttributesTable& typeAttributes) const; - - /// Creates typedef - void wrapTypeUnwrap(FileWriter& wrapperFile) const; }; //****************************************************************************** diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index 3f318eddc..e58e85602 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -22,11 +22,12 @@ ReturnValue ReturnValue::expandTemplate(const TemplateSubstitution& ts) const { } /* ************************************************************************* */ -string ReturnValue::return_type(bool add_ptr) const { +string ReturnValue::returnType() const { if (isPair) - return "pair< " + type1.str(add_ptr) + ", " + type2.str(add_ptr) + " >"; + return "pair< " + type1.qualifiedName("::") + ", " + + type2.qualifiedName("::") + " >"; else - return type1.str(add_ptr); + return type1.qualifiedName("::"); } /* ************************************************************************* */ @@ -40,7 +41,7 @@ void ReturnValue::wrap_result(const string& result, FileWriter& wrapperFile, if (isPair) { // For a pair, store the returned pair so we do not evaluate the function // twice - wrapperFile.oss << " " << return_type(true) << " pairResult = " << result + wrapperFile.oss << " auto pairResult = " << result << ";\n"; type1.wrap_result(" out[0]", "pairResult.first", wrapperFile, typeAttributes); @@ -51,12 +52,6 @@ void ReturnValue::wrap_result(const string& result, FileWriter& wrapperFile, } } -/* ************************************************************************* */ -void ReturnValue::wrapTypeUnwrap(FileWriter& wrapperFile) const { - type1.wrapTypeUnwrap(wrapperFile); - if (isPair) type2.wrapTypeUnwrap(wrapperFile); -} - /* ************************************************************************* */ void ReturnValue::emit_matlab(FileWriter& proxyFile) const { string output; diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index 589db5bd6..721132797 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -63,15 +63,13 @@ struct ReturnValue { /// Substitute template argument ReturnValue expandTemplate(const TemplateSubstitution& ts) const; - std::string return_type(bool add_ptr) const; + std::string returnType() const; std::string matlab_returnType() const; void wrap_result(const std::string& result, FileWriter& wrapperFile, const TypeAttributesTable& typeAttributes) const; - void wrapTypeUnwrap(FileWriter& wrapperFile) const; - void emit_matlab(FileWriter& proxyFile) const; /// @param className the actual class name to use when "This" is specified @@ -84,7 +82,7 @@ struct ReturnValue { if (!r.isPair && r.type1.category == ReturnType::VOID) os << "void"; else - os << r.return_type(true); + os << r.returnType(); return os; } diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 4fe273dee..0f812ea61 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index cbcfc8d49..8392a1cc5 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/Template.h b/wrap/Template.h index 991c6c883..32f8e9761 100644 --- a/wrap/Template.h +++ b/wrap/Template.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/TemplateInstantiationTypedef.cpp b/wrap/TemplateInstantiationTypedef.cpp index 6aa35a5bd..3f9b5de10 100644 --- a/wrap/TemplateInstantiationTypedef.cpp +++ b/wrap/TemplateInstantiationTypedef.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/TemplateInstantiationTypedef.h b/wrap/TemplateInstantiationTypedef.h index 2a08d7a94..dfec272e3 100644 --- a/wrap/TemplateInstantiationTypedef.h +++ b/wrap/TemplateInstantiationTypedef.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/TemplateMethod.cpp b/wrap/TemplateMethod.cpp index d683fed50..10d2371c8 100644 --- a/wrap/TemplateMethod.cpp +++ b/wrap/TemplateMethod.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -16,7 +16,7 @@ #include "TemplateMethod.h" #include "Class.h" - + using namespace std; using namespace wrap; diff --git a/wrap/TemplateMethod.h b/wrap/TemplateMethod.h index 896074baa..399eeec5f 100644 --- a/wrap/TemplateMethod.h +++ b/wrap/TemplateMethod.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/TemplateSubstitution.h b/wrap/TemplateSubstitution.h index 7fe00e213..f214bc607 100644 --- a/wrap/TemplateSubstitution.h +++ b/wrap/TemplateSubstitution.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/TypeAttributesTable.cpp b/wrap/TypeAttributesTable.cpp index ee98480c1..539f4b8ec 100644 --- a/wrap/TypeAttributesTable.cpp +++ b/wrap/TypeAttributesTable.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/TypeAttributesTable.h b/wrap/TypeAttributesTable.h index 132e2cda1..4bbc239b4 100644 --- a/wrap/TypeAttributesTable.h +++ b/wrap/TypeAttributesTable.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/matlab.h b/wrap/matlab.h index de3027eac..1af00bd00 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -110,7 +110,7 @@ protected: //***************************************************************************** void checkArguments(const string& name, int nargout, int nargin, int expected) { - stringstream err; + stringstream err; err << name << " expects " << expected << " arguments, not " << nargin; if (nargin!=expected) error(err.str().c_str()); diff --git a/wrap/spirit.h b/wrap/spirit.h index 98113a1f4..202b7e56d 100644 --- a/wrap/spirit.h +++ b/wrap/spirit.h @@ -16,7 +16,7 @@ #include #include #include - + namespace boost { namespace spirit { diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp index dec78b80c..6946a969b 100644 --- a/wrap/tests/expected/geometry_wrapper.cpp +++ b/wrap/tests/expected/geometry_wrapper.cpp @@ -183,35 +183,31 @@ void gtsamPoint2_deconstructor_3(int nargout, mxArray *out[], int nargin, const void gtsamPoint2_argChar_4(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("argChar",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); char a = unwrap< char >(in[1]); obj->argChar(a); } void gtsamPoint2_argUChar_5(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("argUChar",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); unsigned char a = unwrap< unsigned char >(in[1]); obj->argUChar(a); } void gtsamPoint2_dim_6(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("dim",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); out[0] = wrap< int >(obj->dim()); } void gtsamPoint2_eigenArguments_7(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("eigenArguments",nargout,nargin-1,2); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); Vector v = unwrap< Vector >(in[1]); Matrix m = unwrap< Matrix >(in[2]); obj->eigenArguments(v,m); @@ -219,34 +215,29 @@ void gtsamPoint2_eigenArguments_7(int nargout, mxArray *out[], int nargin, const void gtsamPoint2_returnChar_8(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("returnChar",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); out[0] = wrap< char >(obj->returnChar()); } void gtsamPoint2_vectorConfusion_9(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedVectorNotEigen; - typedef boost::shared_ptr Shared; checkArguments("vectorConfusion",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); - out[0] = wrap_shared_ptr(SharedVectorNotEigen(new VectorNotEigen(obj->vectorConfusion())),"VectorNotEigen", false); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + out[0] = wrap_shared_ptr(boost::make_shared(obj->vectorConfusion()),"VectorNotEigen", false); } void gtsamPoint2_x_10(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("x",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); out[0] = wrap< double >(obj->x()); } void gtsamPoint2_y_11(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("y",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); out[0] = wrap< double >(obj->y()); } @@ -288,9 +279,8 @@ void gtsamPoint3_deconstructor_14(int nargout, mxArray *out[], int nargin, const void gtsamPoint3_norm_15(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("norm",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint3"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint3"); out[0] = wrap< double >(obj->norm()); } @@ -306,16 +296,13 @@ void gtsamPoint3_string_serialize_16(int nargout, mxArray *out[], int nargin, co } void gtsamPoint3_StaticFunctionRet_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint3; - typedef boost::shared_ptr Shared; checkArguments("gtsamPoint3.StaticFunctionRet",nargout,nargin,1); double z = unwrap< double >(in[0]); - out[0] = wrap_shared_ptr(SharedPoint3(new gtsam::Point3(gtsam::Point3::StaticFunctionRet(z))),"gtsam.Point3", false); + out[0] = wrap_shared_ptr(boost::make_shared(gtsam::Point3::StaticFunctionRet(z)),"gtsam.Point3", false); } void gtsamPoint3_staticFunction_18(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("gtsamPoint3.staticFunction",nargout,nargin,0); out[0] = wrap< double >(gtsam::Point3::staticFunction()); } @@ -379,187 +366,159 @@ void Test_deconstructor_23(int nargout, mxArray *out[], int nargin, const mxArra void Test_arg_EigenConstRef_24(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("arg_EigenConstRef",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); Matrix value = unwrap< Matrix >(in[1]); obj->arg_EigenConstRef(value); } void Test_create_MixedPtrs_25(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr Shared; checkArguments("create_MixedPtrs",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); - pair< Test, SharedTest > pairResult = obj->create_MixedPtrs(); - out[0] = wrap_shared_ptr(SharedTest(new Test(pairResult.first)),"Test", false); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto pairResult = obj->create_MixedPtrs(); + out[0] = wrap_shared_ptr(boost::make_shared(pairResult.first),"Test", false); out[1] = wrap_shared_ptr(pairResult.second,"Test", false); } void Test_create_ptrs_26(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr Shared; checkArguments("create_ptrs",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); - pair< SharedTest, SharedTest > pairResult = obj->create_ptrs(); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto pairResult = obj->create_ptrs(); out[0] = wrap_shared_ptr(pairResult.first,"Test", false); out[1] = wrap_shared_ptr(pairResult.second,"Test", false); } void Test_print_27(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("print",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); obj->print(); } void Test_return_Point2Ptr_28(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("return_Point2Ptr",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); bool value = unwrap< bool >(in[1]); out[0] = wrap_shared_ptr(obj->return_Point2Ptr(value),"gtsam.Point2", false); } void Test_return_Test_29(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr Shared; checkArguments("return_Test",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); boost::shared_ptr value = unwrap_shared_ptr< Test >(in[1], "ptr_Test"); - out[0] = wrap_shared_ptr(SharedTest(new Test(obj->return_Test(value))),"Test", false); + out[0] = wrap_shared_ptr(boost::make_shared(obj->return_Test(value)),"Test", false); } void Test_return_TestPtr_30(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr Shared; checkArguments("return_TestPtr",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); boost::shared_ptr value = unwrap_shared_ptr< Test >(in[1], "ptr_Test"); out[0] = wrap_shared_ptr(obj->return_TestPtr(value),"Test", false); } void Test_return_bool_31(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_bool",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); bool value = unwrap< bool >(in[1]); out[0] = wrap< bool >(obj->return_bool(value)); } void Test_return_double_32(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_double",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); double value = unwrap< double >(in[1]); out[0] = wrap< double >(obj->return_double(value)); } void Test_return_field_33(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_field",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); Test& t = *unwrap_shared_ptr< Test >(in[1], "ptr_Test"); out[0] = wrap< bool >(obj->return_field(t)); } void Test_return_int_34(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_int",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); int value = unwrap< int >(in[1]); out[0] = wrap< int >(obj->return_int(value)); } void Test_return_matrix1_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_matrix1",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); Matrix value = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->return_matrix1(value)); } void Test_return_matrix2_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_matrix2",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); Matrix value = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->return_matrix2(value)); } void Test_return_pair_37(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_pair",nargout,nargin-1,2); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); Vector v = unwrap< Vector >(in[1]); Matrix A = unwrap< Matrix >(in[2]); - pair< Vector, Matrix > pairResult = obj->return_pair(v,A); + auto pairResult = obj->return_pair(v,A); out[0] = wrap< Vector >(pairResult.first); out[1] = wrap< Matrix >(pairResult.second); } void Test_return_ptrs_38(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr Shared; checkArguments("return_ptrs",nargout,nargin-1,2); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); boost::shared_ptr p1 = unwrap_shared_ptr< Test >(in[1], "ptr_Test"); boost::shared_ptr p2 = unwrap_shared_ptr< Test >(in[2], "ptr_Test"); - pair< SharedTest, SharedTest > pairResult = obj->return_ptrs(p1,p2); + auto pairResult = obj->return_ptrs(p1,p2); out[0] = wrap_shared_ptr(pairResult.first,"Test", false); out[1] = wrap_shared_ptr(pairResult.second,"Test", false); } void Test_return_size_t_39(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_size_t",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); size_t value = unwrap< size_t >(in[1]); out[0] = wrap< size_t >(obj->return_size_t(value)); } void Test_return_string_40(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_string",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); string value = unwrap< string >(in[1]); out[0] = wrap< string >(obj->return_string(value)); } void Test_return_vector1_41(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_vector1",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); Vector value = unwrap< Vector >(in[1]); out[0] = wrap< Vector >(obj->return_vector1(value)); } void Test_return_vector2_42(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_vector2",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); Vector value = unwrap< Vector >(in[1]); out[0] = wrap< Vector >(obj->return_vector2(value)); } @@ -647,114 +606,93 @@ void MyTemplatePoint2_deconstructor_49(int nargout, mxArray *out[], int nargin, void MyTemplatePoint2_accept_T_50(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("accept_T",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); gtsam::Point2& value = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); obj->accept_T(value); } void MyTemplatePoint2_accept_Tptr_51(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("accept_Tptr",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); boost::shared_ptr value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); obj->accept_Tptr(value); } void MyTemplatePoint2_create_MixedPtrs_52(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("create_MixedPtrs",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); - pair< gtsam::Point2, SharedPoint2 > pairResult = obj->create_MixedPtrs(); - out[0] = wrap_shared_ptr(SharedPoint2(new gtsam::Point2(pairResult.first)),"gtsam.Point2", false); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto pairResult = obj->create_MixedPtrs(); + out[0] = wrap_shared_ptr(boost::make_shared(pairResult.first),"gtsam.Point2", false); out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false); } void MyTemplatePoint2_create_ptrs_53(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("create_ptrs",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); - pair< SharedPoint2, SharedPoint2 > pairResult = obj->create_ptrs(); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto pairResult = obj->create_ptrs(); out[0] = wrap_shared_ptr(pairResult.first,"gtsam.Point2", false); out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false); } void MyTemplatePoint2_return_T_54(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("return_T",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); boost::shared_ptr value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); - out[0] = wrap_shared_ptr(SharedPoint2(new gtsam::Point2(obj->return_T(value))),"gtsam.Point2", false); + out[0] = wrap_shared_ptr(boost::make_shared(obj->return_T(value)),"gtsam.Point2", false); } void MyTemplatePoint2_return_Tptr_55(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("return_Tptr",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); boost::shared_ptr value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); out[0] = wrap_shared_ptr(obj->return_Tptr(value),"gtsam.Point2", false); } void MyTemplatePoint2_return_ptrs_56(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("return_ptrs",nargout,nargin-1,2); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); boost::shared_ptr p1 = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); boost::shared_ptr p2 = unwrap_shared_ptr< gtsam::Point2 >(in[2], "ptr_gtsamPoint2"); - pair< SharedPoint2, SharedPoint2 > pairResult = obj->return_ptrs(p1,p2); + auto pairResult = obj->return_ptrs(p1,p2); out[0] = wrap_shared_ptr(pairResult.first,"gtsam.Point2", false); out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false); } void MyTemplatePoint2_templatedMethod_57(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("templatedMethodMatrix",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); Matrix t = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->templatedMethod(t)); } void MyTemplatePoint2_templatedMethod_58(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("templatedMethodPoint2",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); gtsam::Point2& t = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); - out[0] = wrap_shared_ptr(SharedPoint2(new gtsam::Point2(obj->templatedMethod(t))),"gtsam.Point2", false); + out[0] = wrap_shared_ptr(boost::make_shared(obj->templatedMethod(t)),"gtsam.Point2", false); } void MyTemplatePoint2_templatedMethod_59(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint3; - typedef boost::shared_ptr Shared; checkArguments("templatedMethodPoint3",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); gtsam::Point3& t = *unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3"); - out[0] = wrap_shared_ptr(SharedPoint3(new gtsam::Point3(obj->templatedMethod(t))),"gtsam.Point3", false); + out[0] = wrap_shared_ptr(boost::make_shared(obj->templatedMethod(t)),"gtsam.Point3", false); } void MyTemplatePoint2_templatedMethod_60(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("templatedMethodVector",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); Vector t = unwrap< Vector >(in[1]); out[0] = wrap< Vector >(obj->templatedMethod(t)); } @@ -811,124 +749,111 @@ void MyTemplateMatrix_deconstructor_64(int nargout, mxArray *out[], int nargin, void MyTemplateMatrix_accept_T_65(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("accept_T",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); Matrix value = unwrap< Matrix >(in[1]); obj->accept_T(value); } void MyTemplateMatrix_accept_Tptr_66(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("accept_Tptr",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); Matrix value = unwrap< Matrix >(in[1]); obj->accept_Tptr(value); } void MyTemplateMatrix_create_MixedPtrs_67(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("create_MixedPtrs",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - pair< Matrix, SharedMatrix > pairResult = obj->create_MixedPtrs(); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto pairResult = obj->create_MixedPtrs(); out[0] = wrap< Matrix >(pairResult.first); { - SharedMatrix* ret = new SharedMatrix(pairResult.second); - out[1] = wrap_shared_ptr(ret,"Matrix"); + boost::shared_ptr shared(pairResult.second); + out[1] = wrap_shared_ptr(shared,"Matrix"); } } void MyTemplateMatrix_create_ptrs_68(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("create_ptrs",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - pair< SharedMatrix, SharedMatrix > pairResult = obj->create_ptrs(); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto pairResult = obj->create_ptrs(); { - SharedMatrix* ret = new SharedMatrix(pairResult.first); - out[0] = wrap_shared_ptr(ret,"Matrix"); + boost::shared_ptr shared(pairResult.first); + out[0] = wrap_shared_ptr(shared,"Matrix"); } { - SharedMatrix* ret = new SharedMatrix(pairResult.second); - out[1] = wrap_shared_ptr(ret,"Matrix"); + boost::shared_ptr shared(pairResult.second); + out[1] = wrap_shared_ptr(shared,"Matrix"); } } void MyTemplateMatrix_return_T_69(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_T",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); Matrix value = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->return_T(value)); } void MyTemplateMatrix_return_Tptr_70(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_Tptr",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); Matrix value = unwrap< Matrix >(in[1]); { - SharedMatrix* ret = new SharedMatrix(obj->return_Tptr(value)); - out[0] = wrap_shared_ptr(ret,"Matrix"); + boost::shared_ptr shared(obj->return_Tptr(value)); + out[0] = wrap_shared_ptr(shared,"Matrix"); } } void MyTemplateMatrix_return_ptrs_71(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_ptrs",nargout,nargin-1,2); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); Matrix p1 = unwrap< Matrix >(in[1]); Matrix p2 = unwrap< Matrix >(in[2]); - pair< SharedMatrix, SharedMatrix > pairResult = obj->return_ptrs(p1,p2); + auto pairResult = obj->return_ptrs(p1,p2); { - SharedMatrix* ret = new SharedMatrix(pairResult.first); - out[0] = wrap_shared_ptr(ret,"Matrix"); + boost::shared_ptr shared(pairResult.first); + out[0] = wrap_shared_ptr(shared,"Matrix"); } { - SharedMatrix* ret = new SharedMatrix(pairResult.second); - out[1] = wrap_shared_ptr(ret,"Matrix"); + boost::shared_ptr shared(pairResult.second); + out[1] = wrap_shared_ptr(shared,"Matrix"); } } void MyTemplateMatrix_templatedMethod_72(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("templatedMethodMatrix",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); Matrix t = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->templatedMethod(t)); } void MyTemplateMatrix_templatedMethod_73(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("templatedMethodPoint2",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); gtsam::Point2& t = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); - out[0] = wrap_shared_ptr(SharedPoint2(new gtsam::Point2(obj->templatedMethod(t))),"gtsam.Point2", false); + out[0] = wrap_shared_ptr(boost::make_shared(obj->templatedMethod(t)),"gtsam.Point2", false); } void MyTemplateMatrix_templatedMethod_74(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint3; - typedef boost::shared_ptr Shared; checkArguments("templatedMethodPoint3",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); gtsam::Point3& t = *unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3"); - out[0] = wrap_shared_ptr(SharedPoint3(new gtsam::Point3(obj->templatedMethod(t))),"gtsam.Point3", false); + out[0] = wrap_shared_ptr(boost::make_shared(obj->templatedMethod(t)),"gtsam.Point3", false); } void MyTemplateMatrix_templatedMethod_75(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("templatedMethodVector",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); Vector t = unwrap< Vector >(in[1]); out[0] = wrap< Vector >(obj->templatedMethod(t)); } diff --git a/wrap/tests/expected2/geometry_wrapper.cpp b/wrap/tests/expected2/geometry_wrapper.cpp index ba06d1309..3251735ff 100644 --- a/wrap/tests/expected2/geometry_wrapper.cpp +++ b/wrap/tests/expected2/geometry_wrapper.cpp @@ -176,35 +176,31 @@ void gtsamPoint2_deconstructor_3(int nargout, mxArray *out[], int nargin, const void gtsamPoint2_argChar_4(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("argChar",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); char a = unwrap< char >(in[1]); obj->argChar(a); } void gtsamPoint2_argUChar_5(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("argUChar",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); unsigned char a = unwrap< unsigned char >(in[1]); obj->argUChar(a); } void gtsamPoint2_dim_6(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("dim",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); out[0] = wrap< int >(obj->dim()); } void gtsamPoint2_eigenArguments_7(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("eigenArguments",nargout,nargin-1,2); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); Vector v = unwrap< Vector >(in[1]); Matrix m = unwrap< Matrix >(in[2]); obj->eigenArguments(v,m); @@ -212,34 +208,29 @@ void gtsamPoint2_eigenArguments_7(int nargout, mxArray *out[], int nargin, const void gtsamPoint2_returnChar_8(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("returnChar",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); out[0] = wrap< char >(obj->returnChar()); } void gtsamPoint2_vectorConfusion_9(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedVectorNotEigen; - typedef boost::shared_ptr Shared; checkArguments("vectorConfusion",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); - out[0] = wrap_shared_ptr(SharedVectorNotEigen(new VectorNotEigen(obj->vectorConfusion())),"VectorNotEigen", false); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + out[0] = wrap_shared_ptr(boost::make_shared(obj->vectorConfusion()),"VectorNotEigen", false); } void gtsamPoint2_x_10(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("x",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); out[0] = wrap< double >(obj->x()); } void gtsamPoint2_y_11(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("y",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); out[0] = wrap< double >(obj->y()); } @@ -281,24 +272,20 @@ void gtsamPoint3_deconstructor_14(int nargout, mxArray *out[], int nargin, const void gtsamPoint3_norm_15(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("norm",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint3"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint3"); out[0] = wrap< double >(obj->norm()); } void gtsamPoint3_StaticFunctionRet_16(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint3; - typedef boost::shared_ptr Shared; checkArguments("gtsamPoint3.StaticFunctionRet",nargout,nargin,1); double z = unwrap< double >(in[0]); - out[0] = wrap_shared_ptr(SharedPoint3(new gtsam::Point3(gtsam::Point3::StaticFunctionRet(z))),"gtsam.Point3", false); + out[0] = wrap_shared_ptr(boost::make_shared(gtsam::Point3::StaticFunctionRet(z)),"gtsam.Point3", false); } void gtsamPoint3_staticFunction_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("gtsamPoint3.staticFunction",nargout,nargin,0); out[0] = wrap< double >(gtsam::Point3::staticFunction()); } @@ -351,187 +338,159 @@ void Test_deconstructor_21(int nargout, mxArray *out[], int nargin, const mxArra void Test_arg_EigenConstRef_22(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("arg_EigenConstRef",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); Matrix value = unwrap< Matrix >(in[1]); obj->arg_EigenConstRef(value); } void Test_create_MixedPtrs_23(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr Shared; checkArguments("create_MixedPtrs",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); - pair< Test, SharedTest > pairResult = obj->create_MixedPtrs(); - out[0] = wrap_shared_ptr(SharedTest(new Test(pairResult.first)),"Test", false); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto pairResult = obj->create_MixedPtrs(); + out[0] = wrap_shared_ptr(boost::make_shared(pairResult.first),"Test", false); out[1] = wrap_shared_ptr(pairResult.second,"Test", false); } void Test_create_ptrs_24(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr Shared; checkArguments("create_ptrs",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); - pair< SharedTest, SharedTest > pairResult = obj->create_ptrs(); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto pairResult = obj->create_ptrs(); out[0] = wrap_shared_ptr(pairResult.first,"Test", false); out[1] = wrap_shared_ptr(pairResult.second,"Test", false); } void Test_print_25(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("print",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); obj->print(); } void Test_return_Point2Ptr_26(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("return_Point2Ptr",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); bool value = unwrap< bool >(in[1]); out[0] = wrap_shared_ptr(obj->return_Point2Ptr(value),"gtsam.Point2", false); } void Test_return_Test_27(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr Shared; checkArguments("return_Test",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); boost::shared_ptr value = unwrap_shared_ptr< Test >(in[1], "ptr_Test"); - out[0] = wrap_shared_ptr(SharedTest(new Test(obj->return_Test(value))),"Test", false); + out[0] = wrap_shared_ptr(boost::make_shared(obj->return_Test(value)),"Test", false); } void Test_return_TestPtr_28(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr Shared; checkArguments("return_TestPtr",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); boost::shared_ptr value = unwrap_shared_ptr< Test >(in[1], "ptr_Test"); out[0] = wrap_shared_ptr(obj->return_TestPtr(value),"Test", false); } void Test_return_bool_29(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_bool",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); bool value = unwrap< bool >(in[1]); out[0] = wrap< bool >(obj->return_bool(value)); } void Test_return_double_30(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_double",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); double value = unwrap< double >(in[1]); out[0] = wrap< double >(obj->return_double(value)); } void Test_return_field_31(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_field",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); Test& t = *unwrap_shared_ptr< Test >(in[1], "ptr_Test"); out[0] = wrap< bool >(obj->return_field(t)); } void Test_return_int_32(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_int",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); int value = unwrap< int >(in[1]); out[0] = wrap< int >(obj->return_int(value)); } void Test_return_matrix1_33(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_matrix1",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); Matrix value = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->return_matrix1(value)); } void Test_return_matrix2_34(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_matrix2",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); Matrix value = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->return_matrix2(value)); } void Test_return_pair_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_pair",nargout,nargin-1,2); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); Vector v = unwrap< Vector >(in[1]); Matrix A = unwrap< Matrix >(in[2]); - pair< Vector, Matrix > pairResult = obj->return_pair(v,A); + auto pairResult = obj->return_pair(v,A); out[0] = wrap< Vector >(pairResult.first); out[1] = wrap< Matrix >(pairResult.second); } void Test_return_ptrs_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr Shared; checkArguments("return_ptrs",nargout,nargin-1,2); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); boost::shared_ptr p1 = unwrap_shared_ptr< Test >(in[1], "ptr_Test"); boost::shared_ptr p2 = unwrap_shared_ptr< Test >(in[2], "ptr_Test"); - pair< SharedTest, SharedTest > pairResult = obj->return_ptrs(p1,p2); + auto pairResult = obj->return_ptrs(p1,p2); out[0] = wrap_shared_ptr(pairResult.first,"Test", false); out[1] = wrap_shared_ptr(pairResult.second,"Test", false); } void Test_return_size_t_37(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_size_t",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); size_t value = unwrap< size_t >(in[1]); out[0] = wrap< size_t >(obj->return_size_t(value)); } void Test_return_string_38(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_string",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); string value = unwrap< string >(in[1]); out[0] = wrap< string >(obj->return_string(value)); } void Test_return_vector1_39(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_vector1",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); Vector value = unwrap< Vector >(in[1]); out[0] = wrap< Vector >(obj->return_vector1(value)); } void Test_return_vector2_40(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_vector2",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); Vector value = unwrap< Vector >(in[1]); out[0] = wrap< Vector >(obj->return_vector2(value)); } @@ -619,114 +578,93 @@ void MyTemplatePoint2_deconstructor_47(int nargout, mxArray *out[], int nargin, void MyTemplatePoint2_accept_T_48(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("accept_T",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); gtsam::Point2& value = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); obj->accept_T(value); } void MyTemplatePoint2_accept_Tptr_49(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("accept_Tptr",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); boost::shared_ptr value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); obj->accept_Tptr(value); } void MyTemplatePoint2_create_MixedPtrs_50(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("create_MixedPtrs",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); - pair< gtsam::Point2, SharedPoint2 > pairResult = obj->create_MixedPtrs(); - out[0] = wrap_shared_ptr(SharedPoint2(new gtsam::Point2(pairResult.first)),"gtsam.Point2", false); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto pairResult = obj->create_MixedPtrs(); + out[0] = wrap_shared_ptr(boost::make_shared(pairResult.first),"gtsam.Point2", false); out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false); } void MyTemplatePoint2_create_ptrs_51(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("create_ptrs",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); - pair< SharedPoint2, SharedPoint2 > pairResult = obj->create_ptrs(); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto pairResult = obj->create_ptrs(); out[0] = wrap_shared_ptr(pairResult.first,"gtsam.Point2", false); out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false); } void MyTemplatePoint2_return_T_52(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("return_T",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); boost::shared_ptr value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); - out[0] = wrap_shared_ptr(SharedPoint2(new gtsam::Point2(obj->return_T(value))),"gtsam.Point2", false); + out[0] = wrap_shared_ptr(boost::make_shared(obj->return_T(value)),"gtsam.Point2", false); } void MyTemplatePoint2_return_Tptr_53(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("return_Tptr",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); boost::shared_ptr value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); out[0] = wrap_shared_ptr(obj->return_Tptr(value),"gtsam.Point2", false); } void MyTemplatePoint2_return_ptrs_54(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("return_ptrs",nargout,nargin-1,2); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); boost::shared_ptr p1 = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); boost::shared_ptr p2 = unwrap_shared_ptr< gtsam::Point2 >(in[2], "ptr_gtsamPoint2"); - pair< SharedPoint2, SharedPoint2 > pairResult = obj->return_ptrs(p1,p2); + auto pairResult = obj->return_ptrs(p1,p2); out[0] = wrap_shared_ptr(pairResult.first,"gtsam.Point2", false); out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false); } void MyTemplatePoint2_templatedMethod_55(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("templatedMethodMatrix",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); Matrix t = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->templatedMethod(t)); } void MyTemplatePoint2_templatedMethod_56(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("templatedMethodPoint2",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); gtsam::Point2& t = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); - out[0] = wrap_shared_ptr(SharedPoint2(new gtsam::Point2(obj->templatedMethod(t))),"gtsam.Point2", false); + out[0] = wrap_shared_ptr(boost::make_shared(obj->templatedMethod(t)),"gtsam.Point2", false); } void MyTemplatePoint2_templatedMethod_57(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint3; - typedef boost::shared_ptr Shared; checkArguments("templatedMethodPoint3",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); gtsam::Point3& t = *unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3"); - out[0] = wrap_shared_ptr(SharedPoint3(new gtsam::Point3(obj->templatedMethod(t))),"gtsam.Point3", false); + out[0] = wrap_shared_ptr(boost::make_shared(obj->templatedMethod(t)),"gtsam.Point3", false); } void MyTemplatePoint2_templatedMethod_58(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("templatedMethodVector",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); Vector t = unwrap< Vector >(in[1]); out[0] = wrap< Vector >(obj->templatedMethod(t)); } @@ -783,124 +721,111 @@ void MyTemplateMatrix_deconstructor_62(int nargout, mxArray *out[], int nargin, void MyTemplateMatrix_accept_T_63(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("accept_T",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); Matrix value = unwrap< Matrix >(in[1]); obj->accept_T(value); } void MyTemplateMatrix_accept_Tptr_64(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("accept_Tptr",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); Matrix value = unwrap< Matrix >(in[1]); obj->accept_Tptr(value); } void MyTemplateMatrix_create_MixedPtrs_65(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("create_MixedPtrs",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - pair< Matrix, SharedMatrix > pairResult = obj->create_MixedPtrs(); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto pairResult = obj->create_MixedPtrs(); out[0] = wrap< Matrix >(pairResult.first); { - SharedMatrix* ret = new SharedMatrix(pairResult.second); - out[1] = wrap_shared_ptr(ret,"Matrix"); + boost::shared_ptr shared(pairResult.second); + out[1] = wrap_shared_ptr(shared,"Matrix"); } } void MyTemplateMatrix_create_ptrs_66(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("create_ptrs",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - pair< SharedMatrix, SharedMatrix > pairResult = obj->create_ptrs(); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto pairResult = obj->create_ptrs(); { - SharedMatrix* ret = new SharedMatrix(pairResult.first); - out[0] = wrap_shared_ptr(ret,"Matrix"); + boost::shared_ptr shared(pairResult.first); + out[0] = wrap_shared_ptr(shared,"Matrix"); } { - SharedMatrix* ret = new SharedMatrix(pairResult.second); - out[1] = wrap_shared_ptr(ret,"Matrix"); + boost::shared_ptr shared(pairResult.second); + out[1] = wrap_shared_ptr(shared,"Matrix"); } } void MyTemplateMatrix_return_T_67(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_T",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); Matrix value = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->return_T(value)); } void MyTemplateMatrix_return_Tptr_68(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_Tptr",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); Matrix value = unwrap< Matrix >(in[1]); { - SharedMatrix* ret = new SharedMatrix(obj->return_Tptr(value)); - out[0] = wrap_shared_ptr(ret,"Matrix"); + boost::shared_ptr shared(obj->return_Tptr(value)); + out[0] = wrap_shared_ptr(shared,"Matrix"); } } void MyTemplateMatrix_return_ptrs_69(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_ptrs",nargout,nargin-1,2); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); Matrix p1 = unwrap< Matrix >(in[1]); Matrix p2 = unwrap< Matrix >(in[2]); - pair< SharedMatrix, SharedMatrix > pairResult = obj->return_ptrs(p1,p2); + auto pairResult = obj->return_ptrs(p1,p2); { - SharedMatrix* ret = new SharedMatrix(pairResult.first); - out[0] = wrap_shared_ptr(ret,"Matrix"); + boost::shared_ptr shared(pairResult.first); + out[0] = wrap_shared_ptr(shared,"Matrix"); } { - SharedMatrix* ret = new SharedMatrix(pairResult.second); - out[1] = wrap_shared_ptr(ret,"Matrix"); + boost::shared_ptr shared(pairResult.second); + out[1] = wrap_shared_ptr(shared,"Matrix"); } } void MyTemplateMatrix_templatedMethod_70(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("templatedMethodMatrix",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); Matrix t = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->templatedMethod(t)); } void MyTemplateMatrix_templatedMethod_71(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("templatedMethodPoint2",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); gtsam::Point2& t = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); - out[0] = wrap_shared_ptr(SharedPoint2(new gtsam::Point2(obj->templatedMethod(t))),"gtsam.Point2", false); + out[0] = wrap_shared_ptr(boost::make_shared(obj->templatedMethod(t)),"gtsam.Point2", false); } void MyTemplateMatrix_templatedMethod_72(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint3; - typedef boost::shared_ptr Shared; checkArguments("templatedMethodPoint3",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); gtsam::Point3& t = *unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3"); - out[0] = wrap_shared_ptr(SharedPoint3(new gtsam::Point3(obj->templatedMethod(t))),"gtsam.Point3", false); + out[0] = wrap_shared_ptr(boost::make_shared(obj->templatedMethod(t)),"gtsam.Point3", false); } void MyTemplateMatrix_templatedMethod_73(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("templatedMethodVector",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); Vector t = unwrap< Vector >(in[1]); out[0] = wrap< Vector >(obj->templatedMethod(t)); } diff --git a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp index 0f5c70348..6e0c5670d 100644 --- a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp +++ b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp @@ -199,34 +199,29 @@ void ns2ClassA_deconstructor_8(int nargout, mxArray *out[], int nargin, const mx void ns2ClassA_memberFunction_9(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("memberFunction",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_ns2ClassA"); + auto obj = unwrap_shared_ptr(in[0], "ptr_ns2ClassA"); out[0] = wrap< double >(obj->memberFunction()); } void ns2ClassA_nsArg_10(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("nsArg",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_ns2ClassA"); + auto obj = unwrap_shared_ptr(in[0], "ptr_ns2ClassA"); ns1::ClassB& arg = *unwrap_shared_ptr< ns1::ClassB >(in[1], "ptr_ns1ClassB"); out[0] = wrap< int >(obj->nsArg(arg)); } void ns2ClassA_nsReturn_11(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedClassB; - typedef boost::shared_ptr Shared; checkArguments("nsReturn",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_ns2ClassA"); + auto obj = unwrap_shared_ptr(in[0], "ptr_ns2ClassA"); double q = unwrap< double >(in[1]); - out[0] = wrap_shared_ptr(SharedClassB(new ns2::ns3::ClassB(obj->nsReturn(q))),"ns2.ns3.ClassB", false); + out[0] = wrap_shared_ptr(boost::make_shared(obj->nsReturn(q)),"ns2.ns3.ClassB", false); } void ns2ClassA_afunction_12(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("ns2ClassA.afunction",nargout,nargin,0); out[0] = wrap< double >(ns2::ClassA::afunction()); } @@ -343,18 +338,16 @@ void ns2aGlobalFunction_23(int nargout, mxArray *out[], int nargin, const mxArra } void ns2overloadedGlobalFunction_24(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedClassA; checkArguments("ns2overloadedGlobalFunction",nargout,nargin,1); ns1::ClassA& a = *unwrap_shared_ptr< ns1::ClassA >(in[0], "ptr_ns1ClassA"); - out[0] = wrap_shared_ptr(SharedClassA(new ns1::ClassA(ns2::overloadedGlobalFunction(a))),"ns1.ClassA", false); + out[0] = wrap_shared_ptr(boost::make_shared(ns2::overloadedGlobalFunction(a)),"ns1.ClassA", false); } void ns2overloadedGlobalFunction_25(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedClassA; checkArguments("ns2overloadedGlobalFunction",nargout,nargin,2); ns1::ClassA& a = *unwrap_shared_ptr< ns1::ClassA >(in[0], "ptr_ns1ClassA"); double b = unwrap< double >(in[1]); - out[0] = wrap_shared_ptr(SharedClassA(new ns1::ClassA(ns2::overloadedGlobalFunction(a,b))),"ns1.ClassA", false); + out[0] = wrap_shared_ptr(boost::make_shared(ns2::overloadedGlobalFunction(a,b)),"ns1.ClassA", false); } void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) diff --git a/wrap/tests/testArgument.cpp b/wrap/tests/testArgument.cpp index d99f9e4b3..555401309 100644 --- a/wrap/tests/testArgument.cpp +++ b/wrap/tests/testArgument.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/tests/testClass.cpp b/wrap/tests/testClass.cpp index 3d8d79b33..dbf2e8e7a 100644 --- a/wrap/tests/testClass.cpp +++ b/wrap/tests/testClass.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -207,7 +207,7 @@ TEST( Class, TemplateSubstition ) { // check the number of new classes is four EXPECT_LONGS_EQUAL(4, classes.size()); - // check return types + // check return types EXPECT( classes[0].method("myMethod").returnValue(0).type1 == q_void); EXPECT( classes[1].method("myMethod").returnValue(0).type1 == q_double); EXPECT( classes[2].method("myMethod").returnValue(0).type1 == q_Matrix); diff --git a/wrap/tests/testGlobalFunction.cpp b/wrap/tests/testGlobalFunction.cpp index e707b6ee7..c336afd32 100644 --- a/wrap/tests/testGlobalFunction.cpp +++ b/wrap/tests/testGlobalFunction.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/tests/testMethod.cpp b/wrap/tests/testMethod.cpp index d823e9099..f45869728 100644 --- a/wrap/tests/testMethod.cpp +++ b/wrap/tests/testMethod.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/tests/testReturnValue.cpp b/wrap/tests/testReturnValue.cpp index 720b120b4..62a8ff0c7 100644 --- a/wrap/tests/testReturnValue.cpp +++ b/wrap/tests/testReturnValue.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/tests/testSpirit.cpp b/wrap/tests/testSpirit.cpp index 27c9be6d6..b9e01a17e 100644 --- a/wrap/tests/testSpirit.cpp +++ b/wrap/tests/testSpirit.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -68,7 +68,7 @@ TEST( spirit, sequence ) { // parser for interface files // const string reference reference -Rule constStringRef_p = +Rule constStringRef_p = str_p("const") >> "string" >> '&'; // class reference @@ -104,9 +104,9 @@ TEST( spirit, constMethod_p ) { /* ************************************************************************* */ -/* See https://gcc.gnu.org/bugzilla/show_bug.cgi?id=56665 +/* See https://gcc.gnu.org/bugzilla/show_bug.cgi?id=56665 GCC compiler issues with -O2 and -fno-strict-aliasing results in undefined - behaviour when spirit uses assign_a with a literal. + behaviour when spirit uses assign_a with a literal. GCC versions 4.7.2 -> 5.4 inclusive */ TEST( spirit, return_value_p ) { diff --git a/wrap/tests/testTemplate.cpp b/wrap/tests/testTemplate.cpp index eed144677..1c735f3f2 100644 --- a/wrap/tests/testTemplate.cpp +++ b/wrap/tests/testTemplate.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/tests/testType.cpp b/wrap/tests/testType.cpp index f06a88962..229b79728 100644 --- a/wrap/tests/testType.cpp +++ b/wrap/tests/testType.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 8668ec88f..64f28db83 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 32ee85eee..1127fa77b 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -110,7 +110,7 @@ bool files_equal(const string& expected, const string& actual, bool skipheader) /* ************************************************************************* */ string maybe_shared_ptr(bool add, const string& qtype, const string& type) { string str = add? "Shared" : ""; - if (add) str += type; + if (add) str += type; else str += qtype; return str; } diff --git a/wrap/utilities.h b/wrap/utilities.h index d8108d6a5..17c5cba12 100644 --- a/wrap/utilities.h +++ b/wrap/utilities.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -90,7 +90,7 @@ public: ~AttributeError() throw() {} virtual const char* what() const throw() { return what_.c_str(); } }; - + // "Unique" key to signal calling the matlab object constructor with a raw pointer // to a shared pointer of the same C++ object type as the MATLAB type. // Also present in matlab.h diff --git a/wrap/wrap.cpp b/wrap/wrap.cpp index fe17e1c66..5874f52d0 100644 --- a/wrap/wrap.cpp +++ b/wrap/wrap.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list)