From ab1f6562c802bf815d42dc327e7d704b1b475d1e Mon Sep 17 00:00:00 2001 From: = Date: Sat, 6 Aug 2016 00:40:04 -0400 Subject: [PATCH 01/35] Fixes compile errors when using BOOST version 1.61.0 --- examples/TimeTBB.cpp | 1 + gtsam/base/GenericValue.h | 1 + gtsam/base/Matrix.cpp | 1 + gtsam/base/Vector.cpp | 1 + gtsam/base/deprecated/LieScalar.h | 1 + gtsam/geometry/BearingRange.h | 1 + gtsam/geometry/Cal3_S2Stereo.h | 1 + gtsam/geometry/Point2.cpp | 1 + gtsam/geometry/Point3.cpp | 1 + gtsam/geometry/Quaternion.h | 1 + gtsam/geometry/Rot2.cpp | 1 + gtsam/geometry/SO3.h | 1 + gtsam/linear/JacobianFactor.cpp | 20 +++++++++---------- gtsam/linear/JacobianFactor.h | 2 +- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 1 + gtsam/nonlinear/internal/ExecutionTrace.h | 1 + gtsam/nonlinear/tests/testCallRecord.cpp | 1 + gtsam_unstable/geometry/Event.h | 1 + 18 files changed, 26 insertions(+), 12 deletions(-) diff --git a/examples/TimeTBB.cpp b/examples/TimeTBB.cpp index aa2195078..178fa513f 100644 --- a/examples/TimeTBB.cpp +++ b/examples/TimeTBB.cpp @@ -20,6 +20,7 @@ #include #include +#include using namespace std; using namespace gtsam; diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 17783c5b9..7c4d0398d 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -28,6 +28,7 @@ #include #include #include // operator typeid +#include namespace gtsam { diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index d74e1c122..0c37b405e 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -32,6 +32,7 @@ #include #include #include +#include using namespace std; diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 8f17e4c6e..6dc9800ca 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include diff --git a/gtsam/base/deprecated/LieScalar.h b/gtsam/base/deprecated/LieScalar.h index f9c424e95..68632addc 100644 --- a/gtsam/base/deprecated/LieScalar.h +++ b/gtsam/base/deprecated/LieScalar.h @@ -19,6 +19,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h index 27fe2f197..b1e003864 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -22,6 +22,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 365a6c40e..29ccd194d 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -18,6 +18,7 @@ #pragma once #include +#include namespace gtsam { diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 2152a7c39..74b9a2bec 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -17,6 +17,7 @@ #include #include +#include using namespace std; diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 091906d5f..8df5f5607 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -16,6 +16,7 @@ #include #include +#include using namespace std; diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index 0ab4a5ee6..af9481181 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -21,6 +21,7 @@ #include #include // Logmap/Expmap derivatives #include +#include #define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 9780cb49a..4cabe7140 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -17,6 +17,7 @@ */ #include +#include using namespace std; diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 3152fa2fb..0396fbce0 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -24,6 +24,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 06c8f3f64..4597759e3 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -219,15 +219,13 @@ FastVector _convertOrCastToJacobians( /* ************************************************************************* */ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, boost::optional ordering, - boost::optional variableSlots) { + boost::optional p_variableSlots) { gttic(JacobianFactor_combine_constructor); // Compute VariableSlots if one was not provided - boost::optional computedVariableSlots; - if (!variableSlots) { - computedVariableSlots = VariableSlots(graph); - variableSlots = computedVariableSlots; // Binds reference, does not copy VariableSlots - } + // Binds reference, does not copy VariableSlots + const VariableSlots & variableSlots = + p_variableSlots ? p_variableSlots.get() : VariableSlots(graph); // Cast or convert to Jacobians FastVector jacobians = _convertOrCastToJacobians( @@ -238,15 +236,15 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, // 'unorderedSlots' of any variables discovered that are not in the ordering. Those will then // be added after all of the ordered variables. FastVector orderedSlots; - orderedSlots.reserve(variableSlots->size()); + orderedSlots.reserve(variableSlots.size()); if (ordering) { // If an ordering is provided, arrange the slots first that ordering FastList unorderedSlots; size_t nOrderingSlotsUsed = 0; orderedSlots.resize(ordering->size()); FastMap inverseOrdering = ordering->invert(); - for (VariableSlots::const_iterator item = variableSlots->begin(); - item != variableSlots->end(); ++item) { + for (VariableSlots::const_iterator item = variableSlots.begin(); + item != variableSlots.end(); ++item) { FastMap::const_iterator orderingPosition = inverseOrdering.find(item->first); if (orderingPosition == inverseOrdering.end()) { @@ -267,8 +265,8 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, } else { // If no ordering is provided, arrange the slots as they were, which will be sorted // numerically since VariableSlots uses a map sorting on Key. - for (VariableSlots::const_iterator item = variableSlots->begin(); - item != variableSlots->end(); ++item) + for (VariableSlots::const_iterator item = variableSlots.begin(); + item != variableSlots.end(); ++item) orderedSlots.push_back(item); } gttoc(Order_slots); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 14ff46241..d7814f541 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -154,7 +154,7 @@ namespace gtsam { explicit JacobianFactor( const GaussianFactorGraph& graph, boost::optional ordering = boost::none, - boost::optional variableSlots = boost::none); + boost::optional p_variableSlots = boost::none); /** Virtual destructor */ virtual ~JacobianFactor() {} diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 904e08770..f1135d2f0 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -23,6 +23,7 @@ #include #include #include +#include class NonlinearOptimizerMoreOptimizationTest; diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h index ed811764a..a147f505e 100644 --- a/gtsam/nonlinear/internal/ExecutionTrace.h +++ b/gtsam/nonlinear/internal/ExecutionTrace.h @@ -25,6 +25,7 @@ #include #include +#include namespace gtsam { namespace internal { diff --git a/gtsam/nonlinear/tests/testCallRecord.cpp b/gtsam/nonlinear/tests/testCallRecord.cpp index 208f0b284..5fc4e208d 100644 --- a/gtsam/nonlinear/tests/testCallRecord.cpp +++ b/gtsam/nonlinear/tests/testCallRecord.cpp @@ -23,6 +23,7 @@ #include #include +#include using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index 40c70696b..5bd37d2d2 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -21,6 +21,7 @@ #include #include +#include namespace gtsam { From f39cbf736d215ef43de259232affd4ce575576bc Mon Sep 17 00:00:00 2001 From: Yao Chen Date: Wed, 10 Aug 2016 22:06:53 -0400 Subject: [PATCH 02/35] Test the newly-added function emplace_shared --- gtsam/inference/tests/testFactorGraph.cpp | 44 +++++++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 gtsam/inference/tests/testFactorGraph.cpp diff --git a/gtsam/inference/tests/testFactorGraph.cpp b/gtsam/inference/tests/testFactorGraph.cpp new file mode 100644 index 000000000..a3a69e72d --- /dev/null +++ b/gtsam/inference/tests/testFactorGraph.cpp @@ -0,0 +1,44 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 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 testFactorGraph.cpp + * @author Yao Chen + */ + +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; + + +TEST(FactorGraph, testEmplaceShared) { + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); + Point2 measurement(10.0, 10.0); + + NonlinearFactorGraph graph; + graph.push_back(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', 0), Symbol('l', 0), K)); + graph.emplace_shared >(measurement, measurementNoise, Symbol('x', 0), Symbol('l', 0), K); + + EXPECT_LONGS_EQUAL(2, graph.size()); + EXPECT(graph[0]->equals(*(graph[1]))); +} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ From 3d33b7e2cd4f2aa5b224d6eb4723388b4cffeb8f Mon Sep 17 00:00:00 2001 From: Yao Chen Date: Wed, 10 Aug 2016 22:07:05 -0400 Subject: [PATCH 03/35] Added one function emplace_shared --- gtsam/inference/FactorGraph.h | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 38f30f5a3..604de632c 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -33,6 +33,8 @@ #include #include +#include + namespace gtsam { // Forward declarations @@ -159,6 +161,12 @@ namespace gtsam { void push_back(const sharedFactor& factor) { factors_.push_back(factor); } + /** Emplace a factor */ + template + void emplace_shared(Args&&... args) { + factors_.push_back(boost::make_shared(std::forward(args)...)); + } + /** push back many factors with an iterator over shared_ptr (factors are not copied) */ template typename boost::enable_if >::type @@ -355,4 +363,4 @@ namespace gtsam { } // namespace gtsam -#include \ No newline at end of file +#include From 01b3bf40389152830ff53d22a4d6a1a1255e8a5a Mon Sep 17 00:00:00 2001 From: Carl Morgan Date: Thu, 11 Aug 2016 14:23:26 +1200 Subject: [PATCH 04/35] boost::spirit assign_a fixes to use non-literials --- wrap/Module.cpp | 2 +- wrap/tests/testSpirit.cpp | 18 ++++++++++++++---- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/wrap/Module.cpp b/wrap/Module.cpp index a115c5e10..61d2a29e0 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -152,7 +152,7 @@ void Module::parseMarkup(const std::string& data) { // parse forward declaration ForwardDeclaration fwDec0, fwDec; Rule forward_declaration_p = - !(str_p("virtual")[assign_a(fwDec.isVirtual, true)]) + !(str_p("virtual")[assign_a(fwDec.isVirtual, T)]) >> str_p("class") >> (*(basic.namespace_p >> str_p("::")) >> basic.className_p)[assign_a(fwDec.name)] >> ch_p(';') diff --git a/wrap/tests/testSpirit.cpp b/wrap/tests/testSpirit.cpp index 2a525e08e..27c9be6d6 100644 --- a/wrap/tests/testSpirit.cpp +++ b/wrap/tests/testSpirit.cpp @@ -102,9 +102,19 @@ TEST( spirit, constMethod_p ) { EXPECT(parse("double norm() const;", constMethod_p, space_p).full); } + /* ************************************************************************* */ +/* 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. + GCC versions 4.7.2 -> 5.4 inclusive */ + TEST( spirit, return_value_p ) { - bool isEigen = true; + static const bool T = true; + static const bool F = false; + + bool isEigen = T; + string actual_return_type; string actual_function_name; @@ -119,9 +129,9 @@ TEST( spirit, return_value_p ) { Rule funcName_p = lexeme_d[lower_p >> *(alnum_p | '_')]; Rule returnType_p = - (basisType_p[assign_a(actual_return_type)][assign_a(isEigen, true)]) | - (className_p[assign_a(actual_return_type)][assign_a(isEigen,false)]) | - (eigenType_p[assign_a(actual_return_type)][assign_a(isEigen, true)]); + (basisType_p[assign_a(actual_return_type)][assign_a(isEigen, T)]) | + (className_p[assign_a(actual_return_type)][assign_a(isEigen, F)]) | + (eigenType_p[assign_a(actual_return_type)][assign_a(isEigen, T)]); Rule testFunc_p = returnType_p >> funcName_p[assign_a(actual_function_name)] >> str_p("();"); From d1cdafa3f57a74c5dfb9baa21dbb505f0cce6001 Mon Sep 17 00:00:00 2001 From: Ryan Estep Date: Mon, 29 Aug 2016 09:07:03 +1200 Subject: [PATCH 05/35] Removed the boost::regex include (not used) from the matlab wrapper & removed any linking to boost::regex --- CMakeLists.txt | 6 +++--- cmake/GtsamMatlabWrap.cmake | 3 +-- wrap/Argument.cpp | 1 - wrap/CMakeLists.txt | 2 +- 4 files changed, 5 insertions(+), 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3c93264f3..77434d135 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -116,11 +116,11 @@ if(MSVC) endif() endif() -find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time regex timer chrono) +find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time timer chrono) # Required components if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR - NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY OR NOT Boost_REGEX_LIBRARY) + NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY) message(FATAL_ERROR "Missing required Boost components >= v1.43, please install/upgrade Boost or configure your search paths.") endif() @@ -128,7 +128,7 @@ 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) set(GTSAM_BOOST_LIBRARIES ${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} - ${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY} ${Boost_REGEX_LIBRARY}) + ${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY}) if (GTSAM_DISABLE_NEW_TIMERS) message("WARNING: GTSAM timing instrumentation manually disabled") add_definitions(-DGTSAM_DISABLE_NEW_TIMERS) diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake index d1d3d93dd..a9e04a01a 100644 --- a/cmake/GtsamMatlabWrap.cmake +++ b/cmake/GtsamMatlabWrap.cmake @@ -128,8 +128,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex ## This needs to be fixed!! if(UNIX AND NOT APPLE) list(APPEND automaticDependencies ${Boost_SERIALIZATION_LIBRARY_RELEASE} ${Boost_FILESYSTEM_LIBRARY_RELEASE} - ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE} ${Boost_DATE_TIME_LIBRARY_RELEASE} - ${Boost_REGEX_LIBRARY_RELEASE}) + ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE} ${Boost_DATE_TIME_LIBRARY_RELEASE}) if(Boost_TIMER_LIBRARY_RELEASE AND NOT GTSAM_DISABLE_NEW_TIMERS) # Only present in Boost >= 1.48.0 list(APPEND automaticDependencies ${Boost_TIMER_LIBRARY_RELEASE} ${Boost_CHRONO_LIBRARY_RELEASE}) if(GTSAM_MEX_BUILD_STATIC_MODULE) diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index cde04afe0..52d9ca0b5 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -18,7 +18,6 @@ #include "Argument.h" -#include #include #include diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index 03915a662..04d471b39 100644 --- a/wrap/CMakeLists.txt +++ b/wrap/CMakeLists.txt @@ -1,6 +1,6 @@ # Build/install Wrap -set(WRAP_BOOST_LIBRARIES ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_THREAD_LIBRARY} ${Boost_REGEX_LIBRARY}) +set(WRAP_BOOST_LIBRARIES ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_THREAD_LIBRARY}) # Allow for disabling serialization to handle errors related to Clang's linker option(GTSAM_WRAP_SERIALIZATION "If enabled, allows for wrapped objects to be saved via boost.serialization" ON) From 95c75b8bae1a31a11497604b2bdf5fefe6d3018f Mon Sep 17 00:00:00 2001 From: Yao Chen Date: Fri, 9 Sep 2016 08:33:51 -0400 Subject: [PATCH 06/35] Updated functions --- examples/CameraResectioning.cpp | 20 ++++++++------------ examples/Pose2SLAMExample_graphviz.cpp | 12 ++++++------ examples/Pose2SLAMwSPCG.cpp | 12 ++++++------ examples/SFMExample.cpp | 6 +++--- examples/SFMExample_SmartFactor.cpp | 4 ++-- examples/SFMExample_SmartFactorPCG.cpp | 4 ++-- examples/SFMExample_bal.cpp | 6 +++--- examples/SFMExample_bal_COLAMD_METIS.cpp | 1 + 8 files changed, 31 insertions(+), 34 deletions(-) diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 676dd42ec..204af1fea 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -71,18 +71,14 @@ int main(int argc, char* argv[]) { // add measurement factors SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector2(0.5, 0.5)); boost::shared_ptr factor; - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(55, 45), Point3(10, 10, 0))); - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(45, 45), Point3(-10, 10, 0))); - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(45, 55), Point3(-10, -10, 0))); - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(55, 55), Point3(10, -10, 0))); + graph.emplace_shared(measurementNoise, X(1), calib, + Point2(55, 45), Point3(10, 10, 0)); + graph.emplace_shared(measurementNoise, X(1), calib, + Point2(45, 45), Point3(-10, 10, 0)); + graph.emplace_shared(measurementNoise, X(1), calib, + Point2(45, 55), Point3(-10, -10, 0)); + graph.emplace_shared(measurementNoise, X(1), calib, + Point2(55, 55), Point3(10, -10, 0)); /* 3. Create an initial estimate for the camera pose */ Values initial; diff --git a/examples/Pose2SLAMExample_graphviz.cpp b/examples/Pose2SLAMExample_graphviz.cpp index 314ccbdb4..99711da2d 100644 --- a/examples/Pose2SLAMExample_graphviz.cpp +++ b/examples/Pose2SLAMExample_graphviz.cpp @@ -33,19 +33,19 @@ int main (int argc, char** argv) { // 2a. Add a prior on the first pose, setting it to the origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.push_back(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); + graph.emplace_shared >(1, Pose2(0, 0, 0), priorNoise); // For simplicity, we will use the same noise model for odometry and loop closures noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 2b. Add odometry factors - graph.push_back(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); - graph.push_back(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); - graph.push_back(BetweenFactor(3, 4, Pose2(2, 0, M_PI_2), model)); - graph.push_back(BetweenFactor(4, 5, Pose2(2, 0, M_PI_2), model)); + graph.emplace_shared >(1, 2, Pose2(2, 0, 0 ), model); + graph.emplace_shared >(2, 3, Pose2(2, 0, M_PI_2), model); + graph.emplace_shared >(3, 4, Pose2(2, 0, M_PI_2), model); + graph.emplace_shared >(4, 5, Pose2(2, 0, M_PI_2), model); // 2c. Add the loop closure constraint - graph.push_back(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); + graph.emplace_shared >(5, 2, Pose2(2, 0, M_PI_2), model); // 3. Create the data structure to hold the initial estimate to the solution // For illustrative purposes, these have been deliberately set to incorrect values diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 2c25d2f8e..9b459d7b8 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -36,18 +36,18 @@ int main(int argc, char** argv) { // 2a. Add a prior on the first pose, setting it to the origin Pose2 prior(0.0, 0.0, 0.0); // prior at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.push_back(PriorFactor(1, prior, priorNoise)); + graph.emplace_shared >(1, prior, priorNoise); // 2b. Add odometry factors noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - graph.push_back(BetweenFactor(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.push_back(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.push_back(BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.push_back(BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.emplace_shared >(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.emplace_shared >(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.emplace_shared >(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.emplace_shared >(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); // 2c. Add the loop closure constraint noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - graph.push_back(BetweenFactor(5, 1, Pose2(0.0, 0.0, 0.0), model)); + graph.emplace_shared >(5, 1, Pose2(0.0, 0.0, 0.0), model); graph.print("\nFactor Graph:\n"); // print diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 893454936..0e48bb892 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -75,14 +75,14 @@ int main(int argc, char* argv[]) { // Add a prior on pose x1. This indirectly specifies where the origin is. noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph + graph.emplace_shared >(Symbol('x', 0), poses[0], poseNoise); // add directly to graph // Simulated measurements from each camera pose, adding them to the factor graph for (size_t i = 0; i < poses.size(); ++i) { SimpleCamera camera(poses[i], *K); for (size_t j = 0; j < points.size(); ++j) { Point2 measurement = camera.project(points[j]); - graph.push_back(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + graph.emplace_shared >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); } } @@ -90,7 +90,7 @@ int main(int argc, char* argv[]) { // Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance // between the first camera and the first landmark. All other landmark positions are interpreted using this scale. noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.push_back(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + graph.emplace_shared >(Symbol('l', 0), points[0], pointNoise); // add directly to graph graph.print("Factor Graph:\n"); // Create the data structure to hold the initial estimate to the solution diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index 4112afcad..e9d02b15c 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -82,13 +82,13 @@ int main(int argc, char* argv[]) { // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); - graph.push_back(PriorFactor(0, poses[0], noise)); + graph.emplace_shared >(0, poses[0], noise); // Because the structure-from-motion problem has a scale ambiguity, the problem is // still under-constrained. Here we add a prior on the second pose x1, so this will // fix the scale by indicating the distance between x0 and x1. // Because these two are fixed, the rest of the poses will be also be fixed. - graph.push_back(PriorFactor(1, poses[1], noise)); // add directly to graph + graph.emplace_shared >(1, poses[1], noise); // add directly to graph graph.print("Factor Graph:\n"); diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp index 7f6c0ba71..da7c5ba9b 100644 --- a/examples/SFMExample_SmartFactorPCG.cpp +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -74,10 +74,10 @@ int main(int argc, char* argv[]) { // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); - graph.push_back(PriorFactor(0, poses[0], noise)); + graph.emplace_shared >(0, poses[0], noise); // Fix the scale ambiguity by adding a prior - graph.push_back(PriorFactor(1, poses[0], noise)); + graph.emplace_shared >(1, poses[0], noise); // Create the initial estimate to the solution Values initialEstimate; diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index 4c655fb7a..a3a416eb3 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -59,15 +59,15 @@ int main (int argc, char* argv[]) { for(const SfM_Measurement& m: track.measurements) { size_t i = m.first; Point2 uv = m.second; - graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P + graph.emplace_shared(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P } j += 1; } // Add a prior on pose x1. This indirectly specifies where the origin is. // and a prior on the position of the first landmark to fix the scale - graph.push_back(PriorFactor(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1))); - graph.push_back(PriorFactor (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1))); + graph.emplace_shared >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); + graph.emplace_shared > (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)); // Create initial estimate Values initial; diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index 44a402b33..71eb1ac8f 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -82,6 +82,7 @@ int main (int argc, char* argv[]) { /** --------------- COMPARISON -----------------------**/ /** ----------------------------------------------------**/ +M\] LevenbergMarquardtParams params_using_COLAMD, params_using_METIS; try { From a7fbce3e4c3b0fba45a64ab87ab14a8c5df734d3 Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Mon, 26 Sep 2016 17:01:41 -0400 Subject: [PATCH 07/35] fix dogleg compile issue in CMake Timing mode --- gtsam/nonlinear/DoglegOptimizerImpl.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index cead7f9db..9a7067878 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -243,7 +243,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( stay = false; } } - gttoc(adjust_Delta); + gttoc(adjust_delta); } // dx_d and f_error have already been filled in during the loop From 249d6b0b1b180d43e59d1898904d8cc76a209320 Mon Sep 17 00:00:00 2001 From: Yao Chen Date: Sat, 1 Oct 2016 11:17:41 -0400 Subject: [PATCH 08/35] Replaced graph.push_back with graph.emplace_shared if needed. --- examples/SFMExample_bal_COLAMD_METIS.cpp | 7 +- examples/SelfCalibrationExample.cpp | 8 +- examples/StereoVOExample.cpp | 14 +-- examples/StereoVOExample_large.cpp | 8 +- examples/VisualISAM2Example.cpp | 6 +- gtsam/geometry/tests/testTriangulation.cpp | 12 +-- gtsam/geometry/triangulation.h | 8 +- gtsam/slam/tests/testAntiFactor.cpp | 4 +- gtsam/slam/tests/testGeneralSFMFactor.cpp | 36 ++++---- .../testGeneralSFMFactor_Cal3Bundler.cpp | 6 +- gtsam/slam/tests/testReferenceFrameFactor.cpp | 24 ++--- .../tests/testSmartProjectionCameraFactor.cpp | 20 ++--- .../tests/testSmartProjectionPoseFactor.cpp | 87 ++++++++----------- gtsam/slam/tests/testStereoFactor.cpp | 4 +- .../examples/ConcurrentCalibration.cpp | 10 +-- .../examples/SmartProjectionFactorExample.cpp | 2 +- .../SmartStereoProjectionFactorExample.cpp | 2 +- gtsam_unstable/linear/LPSolver.h | 7 +- .../tests/testConcurrentBatchFilter.cpp | 58 ++++++------- .../tests/testConcurrentBatchSmoother.cpp | 48 +++++----- .../tests/testConcurrentIncrementalFilter.cpp | 52 +++++------ .../slam/tests/testSmartRangeFactor.cpp | 4 +- .../testSmartStereoProjectionPoseFactor.cpp | 16 ++-- tests/testGeneralSFMFactorB.cpp | 2 +- timing/timeSFMBAL.cpp | 2 +- 25 files changed, 215 insertions(+), 232 deletions(-) diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index 71eb1ac8f..13a07dda5 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -64,15 +64,15 @@ int main (int argc, char* argv[]) { for(const SfM_Measurement& m: track.measurements) { size_t i = m.first; Point2 uv = m.second; - graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P + graph.emplace_shared(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P } j += 1; } // Add a prior on pose x1. This indirectly specifies where the origin is. // and a prior on the position of the first landmark to fix the scale - graph.push_back(PriorFactor(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1))); - graph.push_back(PriorFactor (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1))); + graph.emplace_shared >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); + graph.emplace_shared >(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)); // Create initial estimate Values initial; @@ -82,7 +82,6 @@ int main (int argc, char* argv[]) { /** --------------- COMPARISON -----------------------**/ /** ----------------------------------------------------**/ -M\] LevenbergMarquardtParams params_using_COLAMD, params_using_METIS; try { diff --git a/examples/SelfCalibrationExample.cpp b/examples/SelfCalibrationExample.cpp index 1c5d155dc..93e010543 100644 --- a/examples/SelfCalibrationExample.cpp +++ b/examples/SelfCalibrationExample.cpp @@ -54,7 +54,7 @@ int main(int argc, char* argv[]) { // Add a prior on pose x1. noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); + graph.emplace_shared >(Symbol('x', 0), poses[0], poseNoise); // Simulated measurements from each camera pose, adding them to the factor graph Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0); @@ -65,17 +65,17 @@ int main(int argc, char* argv[]) { Point2 measurement = camera.project(points[j]); // The only real difference with the Visual SLAM example is that here we use a // different factor type, that also calculates the Jacobian with respect to calibration - graph.push_back(GeneralSFMFactor2(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0))); + graph.emplace_shared >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0)); } } // Add a prior on the position of the first landmark. noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.push_back(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + graph.emplace_shared >(Symbol('l', 0), points[0], pointNoise); // add directly to graph // Add a prior on the calibration. noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100).finished()); - graph.push_back(PriorFactor(Symbol('K', 0), K, calNoise)); + graph.emplace_shared >(Symbol('K', 0), K, calNoise); // Create the initial estimate to the solution // now including an estimate on the camera calibration parameters diff --git a/examples/StereoVOExample.cpp b/examples/StereoVOExample.cpp index 35d6747bf..27c10deb3 100644 --- a/examples/StereoVOExample.cpp +++ b/examples/StereoVOExample.cpp @@ -39,7 +39,7 @@ int main(int argc, char** argv){ //create graph object, add first pose at origin with key '1' NonlinearFactorGraph graph; Pose3 first_pose; - graph.push_back(NonlinearEquality(1, Pose3())); + 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); @@ -47,14 +47,14 @@ int main(int argc, char** argv){ const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)); //create and add stereo factors between first pose (key value 1) and the three landmarks - graph.push_back(GenericStereoFactor(StereoPoint2(520, 480, 440), model, 1, 3, K)); - graph.push_back(GenericStereoFactor(StereoPoint2(120, 80, 440), model, 1, 4, K)); - graph.push_back(GenericStereoFactor(StereoPoint2(320, 280, 140), model, 1, 5, K)); + graph.emplace_shared >(StereoPoint2(520, 480, 440), model, 1, 3, K); + graph.emplace_shared >(StereoPoint2(120, 80, 440), model, 1, 4, K); + graph.emplace_shared >(StereoPoint2(320, 280, 140), model, 1, 5, K); //create and add stereo factors between second pose and the three landmarks - graph.push_back(GenericStereoFactor(StereoPoint2(570, 520, 490), model, 2, 3, K)); - graph.push_back(GenericStereoFactor(StereoPoint2(70, 20, 490), model, 2, 4, K)); - graph.push_back(GenericStereoFactor(StereoPoint2(320, 270, 115), model, 2, 5, K)); + graph.emplace_shared >(StereoPoint2(570, 520, 490), model, 2, 3, K); + graph.emplace_shared >(StereoPoint2(70, 20, 490), model, 2, 4, K); + graph.emplace_shared >(StereoPoint2(320, 270, 115), model, 2, 5, K); //create Values object to contain initial estimates of camera poses and landmark locations Values initial_estimate; diff --git a/examples/StereoVOExample_large.cpp b/examples/StereoVOExample_large.cpp index 8b88c772d..80831bd21 100644 --- a/examples/StereoVOExample_large.cpp +++ b/examples/StereoVOExample_large.cpp @@ -83,9 +83,9 @@ int main(int argc, char** argv){ cout << "Reading stereo factors" << endl; //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { - graph.push_back( - GenericStereoFactor(StereoPoint2(uL, uR, v), model, - Symbol('x', x), Symbol('l', l), K)); + graph.emplace_shared< + GenericStereoFactor >(StereoPoint2(uL, uR, v), model, + Symbol('x', x), Symbol('l', l), K); //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)); @@ -99,7 +99,7 @@ int main(int argc, char** argv){ //constrain the first pose such that it cannot change from its original value during optimization // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky // QR is much slower than Cholesky, but numerically more stable - graph.push_back(NonlinearEquality(Symbol('x',1),first_pose)); + graph.emplace_shared >(Symbol('x',1),first_pose); cout << "Optimizing" << endl; //create Levenberg-Marquardt optimizer to optimize the factor graph diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp index 78bc463ec..157768be7 100644 --- a/examples/VisualISAM2Example.cpp +++ b/examples/VisualISAM2Example.cpp @@ -90,7 +90,7 @@ int main(int argc, char* argv[]) { for (size_t j = 0; j < points.size(); ++j) { SimpleCamera camera(poses[i], *K); Point2 measurement = camera.project(points[j]); - graph.push_back(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + graph.emplace_shared >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); } // Add an initial guess for the current pose @@ -104,11 +104,11 @@ int main(int argc, char* argv[]) { if( i == 0) { // Add a prior on pose x0 noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3),Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); + graph.emplace_shared >(Symbol('x', 0), poses[0], poseNoise); // Add a prior on landmark l0 noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.push_back(PriorFactor(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + graph.emplace_shared >(Symbol('l', 0), points[0], pointNoise); // add directly to graph // Add initial guesses to all observed landmarks // Intentionally initialize the variables off from the ground truth diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index f57ca60d3..9e599f3f5 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -378,12 +378,12 @@ TEST( triangulation, StereotriangulateNonlinear ) { NonlinearFactorGraph graph; static SharedNoiseModel unit(noiseModel::Unit::Create(3)); - graph.push_back(StereoFactor::shared_ptr(new StereoFactor(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK))); - graph.push_back(StereoFactor::shared_ptr(new StereoFactor(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK))); + graph.emplace_shared(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK); + graph.emplace_shared(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK); const SharedDiagonal posePrior = noiseModel::Isotropic::Sigma(6, 1e-9); - graph.push_back(PriorFactor(Symbol('x',1), Pose3(m1), posePrior)); - graph.push_back(PriorFactor(Symbol('x',2), Pose3(m2), posePrior)); + graph.emplace_shared >(Symbol('x',1), Pose3(m1), posePrior); + graph.emplace_shared >(Symbol('x',2), Pose3(m2), posePrior); LevenbergMarquardtOptimizer optimizer(graph, values); Values result = optimizer.optimize(); @@ -399,8 +399,8 @@ TEST( triangulation, StereotriangulateNonlinear ) { NonlinearFactorGraph graph; static SharedNoiseModel unit(noiseModel::Unit::Create(3)); - graph.push_back(TriangulationFactor(cameras[0], measurements[0], unit, Symbol('l',1))); - graph.push_back(TriangulationFactor(cameras[1], measurements[1], unit, Symbol('l',1))); + graph.emplace_shared >(cameras[0], measurements[0], unit, Symbol('l',1)); + graph.emplace_shared >(cameras[1], measurements[1], unit, Symbol('l',1)); LevenbergMarquardtOptimizer optimizer(graph, values); Values result = optimizer.optimize(); diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 81c8ed2f3..fdfe32e8b 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -89,8 +89,8 @@ std::pair triangulationGraph( const Pose3& pose_i = poses[i]; typedef PinholePose Camera; Camera camera_i(pose_i, sharedCal); - graph.push_back(TriangulationFactor // - (camera_i, measurements[i], unit2, landmarkKey)); + graph.emplace_shared > // + (camera_i, measurements[i], unit2, landmarkKey); } return std::make_pair(graph, values); } @@ -116,8 +116,8 @@ std::pair triangulationGraph( traits::dimension)); for (size_t i = 0; i < measurements.size(); i++) { const CAMERA& camera_i = cameras[i]; - graph.push_back(TriangulationFactor // - (camera_i, measurements[i], unit, landmarkKey)); + graph.emplace_shared > // + (camera_i, measurements[i], unit, landmarkKey); } return std::make_pair(graph, values); } diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index 7a95e0fa9..e34e13279 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -90,8 +90,8 @@ TEST( AntiFactor, EquivalentBayesNet) SharedNoiseModel sigma(noiseModel::Unit::Create(6)); NonlinearFactorGraph graph; - graph.push_back(PriorFactor(1, pose1, sigma)); - graph.push_back(BetweenFactor(1, 2, pose1.between(pose2), sigma)); + graph.emplace_shared >(1, pose1, sigma); + graph.emplace_shared >(1, 2, pose1.between(pose2), sigma); // Create a configuration corresponding to the ground truth Values values; diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index b99cb5d9c..8e0b5ad8f 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -368,9 +368,9 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { graph.addCameraConstraint(0, cameras[0]); // Constrain the scale of the problem with a soft range factor of 1m between the cameras - graph.push_back( - RangeFactor(X(0), X(1), 2., - noiseModel::Isotropic::Sigma(1, 10.))); + graph.emplace_shared< + RangeFactor >(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 10.)); const double reproj_error = 1e-5; @@ -385,12 +385,12 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { // Tests range factor between a GeneralCamera and a Pose3 Graph graph; graph.addCameraConstraint(0, GeneralCamera()); - graph.push_back( - RangeFactor(X(0), X(1), 2., - noiseModel::Isotropic::Sigma(1, 1.))); - graph.push_back( - PriorFactor(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), - noiseModel::Isotropic::Sigma(6, 1.))); + graph.emplace_shared< + RangeFactor >(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 1.)); + graph.emplace_shared< + PriorFactor >(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), + noiseModel::Isotropic::Sigma(6, 1.)); Values init; init.insert(X(0), GeneralCamera()); @@ -413,15 +413,15 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { // Tests range factor between a CalibratedCamera and a Pose3 NonlinearFactorGraph graph; - graph.push_back( - PriorFactor(X(0), CalibratedCamera(), - noiseModel::Isotropic::Sigma(6, 1.))); - graph.push_back( - RangeFactor(X(0), X(1), 2., - noiseModel::Isotropic::Sigma(1, 1.))); - graph.push_back( - PriorFactor(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), - noiseModel::Isotropic::Sigma(6, 1.))); + graph.emplace_shared< + PriorFactor >(X(0), CalibratedCamera(), + noiseModel::Isotropic::Sigma(6, 1.)); + graph.emplace_shared< + RangeFactor >(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 1.)); + graph.emplace_shared< + PriorFactor >(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), + noiseModel::Isotropic::Sigma(6, 1.)); Values init; init.insert(X(0), CalibratedCamera()); diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 9fda7d745..d8ac0aa5b 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -369,9 +369,9 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) { graph.addCameraConstraint(0, cameras[0]); // Constrain the scale of the problem with a soft range factor of 1m between the cameras - graph.push_back( - RangeFactor(X(0), X(1), 2., - noiseModel::Isotropic::Sigma(1, 10.))); + graph.emplace_shared< + RangeFactor >(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 10.)); const double reproj_error = 1e-5; diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp index f955aa191..24b818835 100644 --- a/gtsam/slam/tests/testReferenceFrameFactor.cpp +++ b/gtsam/slam/tests/testReferenceFrameFactor.cpp @@ -136,15 +136,15 @@ TEST( ReferenceFrameFactor, converge_trans ) { // Pose2 trans = transIdeal * Pose2(-200.0, 100.0, 2.0); // beyond pi/2 - fails NonlinearFactorGraph graph; - graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1)); - graph.push_back(PointReferenceFrameFactor(lB2, tA1, lA2)); + graph.emplace_shared(lB1, tA1, lA1); + graph.emplace_shared(lB2, tA1, lA2); // hard constraints on points double error_gain = 1000.0; - graph.push_back(NonlinearEquality(lA1, local1, error_gain)); - graph.push_back(NonlinearEquality(lA2, local2, error_gain)); - graph.push_back(NonlinearEquality(lB1, global1, error_gain)); - graph.push_back(NonlinearEquality(lB2, global2, error_gain)); + graph.emplace_shared >(lA1, local1, error_gain); + graph.emplace_shared >(lA2, local2, error_gain); + graph.emplace_shared >(lB1, global1, error_gain); + graph.emplace_shared >(lB2, global2, error_gain); // create initial estimate Values init; @@ -186,9 +186,9 @@ TEST( ReferenceFrameFactor, converge_local ) { NonlinearFactorGraph graph; double error_gain = 1000.0; - graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1)); - graph.push_back(NonlinearEquality(lB1, global, error_gain)); - graph.push_back(NonlinearEquality(tA1, trans, error_gain)); + graph.emplace_shared(lB1, tA1, lA1); + graph.emplace_shared >(lB1, global, error_gain); + graph.emplace_shared >(tA1, trans, error_gain); // create initial estimate Values init; @@ -222,9 +222,9 @@ TEST( ReferenceFrameFactor, converge_global ) { NonlinearFactorGraph graph; double error_gain = 1000.0; - graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1)); - graph.push_back(NonlinearEquality(lA1, local, error_gain)); - graph.push_back(NonlinearEquality(tA1, trans, error_gain)); + graph.emplace_shared(lB1, tA1, lA1); + graph.emplace_shared >(lA1, local, error_gain); + graph.emplace_shared >(tA1, trans, error_gain); // create initial estimate Values init; diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index 0db8149eb..4feeadc86 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -209,8 +209,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); - graph.push_back(PriorFactor(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(c2, cam2, noisePrior); // Create initial estimate Values initial; @@ -321,8 +321,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); @@ -398,8 +398,8 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) { graph.push_back(smartFactor3); graph.push_back(smartFactor4); graph.push_back(smartFactor5); - graph.push_back(PriorFactor(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); @@ -476,8 +476,8 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); @@ -552,8 +552,8 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index f0a864fdd..20b4e7cab 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -262,8 +262,8 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, bodyPose1, noisePrior)); - graph.push_back(PriorFactor(x2, bodyPose2, noisePrior)); + graph.emplace_shared >(x1, bodyPose1, noisePrior); + graph.emplace_shared >(x2, bodyPose2, noisePrior); // Check errors at ground truth poses Values gtValues; @@ -319,8 +319,8 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, cam2.pose(), noisePrior); Values groundTruth; groundTruth.insert(x1, cam1.pose()); @@ -547,8 +547,8 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, cam2.pose(), noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -614,8 +614,8 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, cam2.pose(), noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -675,8 +675,8 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, cam2.pose(), noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -747,8 +747,8 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.push_back(smartFactor4); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, cam2.pose(), noisePrior); Values values; values.insert(x1, cam1.pose()); @@ -800,8 +800,8 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, cam2.pose(), noisePrior); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise @@ -830,30 +830,21 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { NonlinearFactorGraph graph; // Project three landmarks into three cameras - graph.push_back( - ProjectionFactor(cam1.project(landmark1), model, x1, L(1), sharedK2)); - graph.push_back( - ProjectionFactor(cam2.project(landmark1), model, x2, L(1), sharedK2)); - graph.push_back( - ProjectionFactor(cam3.project(landmark1), model, x3, L(1), sharedK2)); + graph.emplace_shared(cam1.project(landmark1), model, x1, L(1), sharedK2); + graph.emplace_shared(cam2.project(landmark1), model, x2, L(1), sharedK2); + graph.emplace_shared(cam3.project(landmark1), model, x3, L(1), sharedK2); - graph.push_back( - ProjectionFactor(cam1.project(landmark2), model, x1, L(2), sharedK2)); - graph.push_back( - ProjectionFactor(cam2.project(landmark2), model, x2, L(2), sharedK2)); - graph.push_back( - ProjectionFactor(cam3.project(landmark2), model, x3, L(2), sharedK2)); + graph.emplace_shared(cam1.project(landmark2), model, x1, L(2), sharedK2); + graph.emplace_shared(cam2.project(landmark2), model, x2, L(2), sharedK2); + graph.emplace_shared(cam3.project(landmark2), model, x3, L(2), sharedK2); - graph.push_back( - ProjectionFactor(cam1.project(landmark3), model, x1, L(3), sharedK2)); - graph.push_back( - ProjectionFactor(cam2.project(landmark3), model, x2, L(3), sharedK2)); - graph.push_back( - ProjectionFactor(cam3.project(landmark3), model, x3, L(3), sharedK2)); + graph.emplace_shared(cam1.project(landmark3), model, x1, L(3), sharedK2); + graph.emplace_shared(cam2.project(landmark3), model, x2, L(3), sharedK2); + graph.emplace_shared(cam3.project(landmark3), model, x3, L(3), sharedK2); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - graph.push_back(PriorFactor(x1, level_pose, noisePrior)); - graph.push_back(PriorFactor(x2, pose_right, noisePrior)); + graph.emplace_shared >(x1, level_pose, noisePrior); + graph.emplace_shared >(x2, pose_right, noisePrior); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); @@ -1000,11 +991,9 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back( - PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back( - PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); + graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise @@ -1068,11 +1057,9 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back( - PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back( - PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); + graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1286,8 +1273,8 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, cam2.pose(), noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1362,11 +1349,9 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back( - PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back( - PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); + graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 45b978c27..c802603db 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -185,11 +185,11 @@ TEST( StereoFactor, singlePoint) { NonlinearFactorGraph graph; - graph.push_back(NonlinearEquality(X(1), camera1)); + graph.emplace_shared >(X(1), camera1); StereoPoint2 measurement(320, 320.0-50, 240); // arguments: measurement, sigma, cam#, measurement #, K, baseline (m) - graph.push_back(GenericStereoFactor(measurement, model, X(1), L(1), K)); + graph.emplace_shared >(measurement, model, X(1), L(1), K); // Create a configuration corresponding to the ground truth Values values; diff --git a/gtsam_unstable/examples/ConcurrentCalibration.cpp b/gtsam_unstable/examples/ConcurrentCalibration.cpp index 28f7e8444..fe1c83ce3 100644 --- a/gtsam_unstable/examples/ConcurrentCalibration.cpp +++ b/gtsam_unstable/examples/ConcurrentCalibration.cpp @@ -61,7 +61,7 @@ int main(int argc, char** argv){ 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()); - graph.push_back(PriorFactor(Symbol('K', 0), *noisy_K, calNoise)); + graph.emplace_shared >(Symbol('K', 0), *noisy_K, calNoise); ifstream pose_file(pose_loc.c_str()); @@ -77,7 +77,7 @@ int main(int argc, char** argv){ } noiseModel::Isotropic::shared_ptr poseNoise = noiseModel::Isotropic::Sigma(6, 0.01); - graph.push_back(PriorFactor(Symbol('x', pose_id), Pose3(m), poseNoise)); + graph.emplace_shared >(Symbol('x', pose_id), Pose3(m), poseNoise); // camera and landmark keys size_t x, l; @@ -89,9 +89,9 @@ int main(int argc, char** argv){ cout << "Reading stereo factors" << endl; //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { -// graph.push_back( GenericStereoFactor(StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K)); +// graph.emplace_shared >(StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K); - graph.push_back(GeneralSFMFactor2(Point2(uL,v), model, Symbol('x', x), Symbol('l', l), Symbol('K', 0))); + graph.emplace_shared >(Point2(uL,v), model, Symbol('x', x), Symbol('l', l), Symbol('K', 0)); //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it @@ -107,7 +107,7 @@ int main(int argc, char** argv){ //constrain the first pose such that it cannot change from its original value during optimization // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky // QR is much slower than Cholesky, but numerically more stable - graph.push_back(NonlinearEquality(Symbol('x',1),first_pose)); + graph.emplace_shared >(Symbol('x',1),first_pose); cout << "Optimizing" << endl; LevenbergMarquardtParams params; diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index c5ba616ed..94a70470a 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -104,7 +104,7 @@ int main(int argc, char** argv){ //constrain the first pose such that it cannot change from its original value during optimization // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky // QR is much slower than Cholesky, but numerically more stable - graph.push_back(NonlinearEquality(1,firstPose)); + graph.emplace_shared >(1,firstPose); LevenbergMarquardtParams params; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index 40ad0cb52..c8023b23c 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -126,7 +126,7 @@ int main(int argc, char** argv){ //constrain the first pose such that it cannot change from its original value during optimization // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky // QR is much slower than Cholesky, but numerically more stable - graph.push_back(NonlinearEquality(Symbol('x',1),first_pose)); + graph.emplace_shared >(Symbol('x',1),first_pose); LevenbergMarquardtParams params; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h index 91cee3941..153aa7fda 100644 --- a/gtsam_unstable/linear/LPSolver.h +++ b/gtsam_unstable/linear/LPSolver.h @@ -51,7 +51,7 @@ struct LPPolicy { ++it) { size_t dim = lp.cost.getDim(it); Vector b = xk.at(*it) - lp.cost.getA(it).transpose(); // b = xk-g - graph.push_back(JacobianFactor(*it, Matrix::Identity(dim, dim), b)); + graph.emplace_shared(*it, Matrix::Identity(dim, dim), b); } KeySet allKeys = lp.inequalities.keys(); @@ -67,8 +67,7 @@ struct LPPolicy { std::inserter(difference, difference.end())); for (Key k : difference) { size_t dim = lp.constrainedKeyDimMap().at(k); - graph.push_back( - JacobianFactor(k, Matrix::Identity(dim, dim), xk.at(k))); + graph.emplace_shared(k, Matrix::Identity(dim, dim), xk.at(k)); } } return graph; @@ -77,4 +76,4 @@ struct LPPolicy { using LPSolver = ActiveSetSolver; -} \ No newline at end of file +} diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index 74ee23fe5..4aff1b465 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -403,9 +403,9 @@ TEST( ConcurrentBatchFilter, update_and_marginalize ) NonlinearFactorGraph partialGraph; - partialGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - partialGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - partialGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + partialGraph.emplace_shared >(1, poseInitial, noisePrior); + partialGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + partialGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); Values partialValues; partialValues.insert(1, optimalValues.at(1)); @@ -437,10 +437,10 @@ TEST( ConcurrentBatchFilter, update_and_marginalize ) expectedGraph.push_back(NonlinearFactor::shared_ptr()); expectedGraph.push_back(NonlinearFactor::shared_ptr()); // ========================================================== - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); for(const GaussianFactor::shared_ptr& factor: result) { - expectedGraph.push_back(LinearContainerFactor(factor, partialValues)); + expectedGraph.emplace_shared(factor, partialValues); } @@ -1126,13 +1126,13 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_1 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + // we removed this one: expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -1181,11 +1181,11 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_2 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // we removed this one: expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); @@ -1238,10 +1238,10 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_3 ) NonlinearFactorGraph expectedGraph; // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -1258,11 +1258,11 @@ TEST( ConcurrentBatchFilter, removeFactors_values ) // Add some factors to the filter NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); - newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.emplace_shared >(1, poseInitial, noisePrior); + newFactors.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(1, 2, poseOdometry, noiseOdometery); Values newValues; newValues.insert(1, Pose3().compose(poseError)); @@ -1291,11 +1291,11 @@ TEST( ConcurrentBatchFilter, removeFactors_values ) // note: factors are removed before the optimization NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // we removed this one: expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index dc316e2ac..a0b6e2c1b 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -617,13 +617,13 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_1 ) NonlinearFactorGraph actualGraph = smoother.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + // we removed this one: expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -670,11 +670,11 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_2 ) NonlinearFactorGraph actualGraph = smoother.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // we removed this one: expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); @@ -724,10 +724,10 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_3 ) NonlinearFactorGraph expectedGraph; // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -744,11 +744,11 @@ TEST( ConcurrentBatchSmoother, removeFactors_values ) // Add some factors to the Smoother NonlinearFactorGraph newFactors; - newFactors.push_back(PriorFactor(1, poseInitial, noisePrior)); - newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.emplace_shared >(1, poseInitial, noisePrior); + newFactors.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(1, 2, poseOdometry, noiseOdometery); Values newValues; newValues.insert(1, Pose3().compose(poseError)); @@ -774,11 +774,11 @@ TEST( ConcurrentBatchSmoother, removeFactors_values ) // note: factors are removed before the optimization NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // we removed this one: expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp index 0c922fb9e..d6b7ab851 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -423,9 +423,9 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 ) // ---------------------------------------------------------------------------------------------- NonlinearFactorGraph partialGraph; - partialGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - partialGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - partialGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + partialGraph.emplace_shared >(1, poseInitial, noisePrior); + partialGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + partialGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); GaussianFactorGraph linearGraph = *partialGraph.linearize(newValues); @@ -441,7 +441,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 ) expectedGraph.push_back(NonlinearFactor::shared_ptr()); expectedGraph.push_back(NonlinearFactor::shared_ptr()); // ========================================================== - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); for(const GaussianFactor::shared_ptr& factor: marginal) { // the linearization point for the linear container is optional, but it is not used in the filter, @@ -507,9 +507,9 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 ) // ---------------------------------------------------------------------------------------------- NonlinearFactorGraph partialGraph; - partialGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - partialGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - partialGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + partialGraph.emplace_shared >(1, poseInitial, noisePrior); + partialGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + partialGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); GaussianFactorGraph linearGraph = *partialGraph.linearize(optimalValues); @@ -525,7 +525,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 ) expectedGraph.push_back(NonlinearFactor::shared_ptr()); expectedGraph.push_back(NonlinearFactor::shared_ptr()); // ========================================================== - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); for(const GaussianFactor::shared_ptr& factor: marginal) { // the linearization point for the linear container is optional, but it is not used in the filter, @@ -1231,13 +1231,13 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)) // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -1289,11 +1289,11 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // we removed this one: expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); @@ -1350,10 +1350,10 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 ) NonlinearFactorGraph expectedGraph; // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } @@ -1406,11 +1406,11 @@ TEST( ConcurrentIncrementalFilter, removeFactors_values ) Values actualValues = filter.getLinearizationPoint(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // we removed this one: expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index 9ae4aa928..e18505af6 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -116,8 +116,8 @@ TEST( SmartRangeFactor, optimization ) { graph.push_back(f); const noiseModel::Base::shared_ptr // priorNoise = noiseModel::Diagonal::Sigmas(Vector3(1, 1, M_PI)); - graph.push_back(PriorFactor(1, pose1, priorNoise)); - graph.push_back(PriorFactor(2, pose2, priorNoise)); + graph.emplace_shared >(1, pose1, priorNoise); + graph.emplace_shared >(2, pose2, priorNoise); // Try optimizing LevenbergMarquardtParams params; diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 8051e238a..8bd874711 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -254,8 +254,8 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.emplace_shared >(x1, pose1, noisePrior); + graph.emplace_shared >(x2, pose2, noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -394,8 +394,8 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.emplace_shared >(x1, pose1, noisePrior); + graph.emplace_shared >(x2, pose2, noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -463,8 +463,8 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.emplace_shared >(x1, pose1, noisePrior); + graph.emplace_shared >(x2, pose2, noisePrior); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -545,8 +545,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.push_back(smartFactor4); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.emplace_shared >(x1, pose1, noisePrior); + graph.emplace_shared >(x2, pose2, noisePrior); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise diff --git a/tests/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp index 410617cbf..95caaaf9c 100644 --- a/tests/testGeneralSFMFactorB.cpp +++ b/tests/testGeneralSFMFactorB.cpp @@ -51,7 +51,7 @@ TEST(PinholeCamera, BAL) { for (size_t j = 0; j < db.number_tracks(); j++) { for (const SfM_Measurement& m: db.tracks[j].measurements) - graph.push_back(sfmFactor(m.second, unit2, m.first, P(j))); + graph.emplace_shared(m.second, unit2, m.first, P(j)); } Values initial = initialCamerasAndPointsEstimate(db); diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index c1e695b3a..be1104b1a 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -40,7 +40,7 @@ int main(int argc, char* argv[]) { for (const SfM_Measurement& m: db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; - graph.push_back(SfmFactor(z, gNoiseModel, C(i), P(j))); + graph.emplace_shared(z, gNoiseModel, C(i), P(j)); } } From 3c1a0a8801ed02d6f3073730db4332f03f78221e Mon Sep 17 00:00:00 2001 From: Yao Chen Date: Sat, 1 Oct 2016 11:41:37 -0400 Subject: [PATCH 09/35] Replaced graph.add with graph.emplace_shared if needed. --- examples/LocalizationExample.cpp | 10 +-- examples/METISOrderingExample.cpp | 6 +- examples/OdometryExample.cpp | 6 +- examples/PlanarSLAMExample.cpp | 12 +-- examples/Pose2SLAMExample.cpp | 12 +-- examples/VisualISAMExample.cpp | 9 +- gtsam/geometry/tests/testUnit3.cpp | 2 +- gtsam/navigation/tests/testImuFactor.cpp | 4 +- gtsam/slam/InitializePose3.cpp | 7 +- gtsam/slam/tests/testDataset.cpp | 86 +++++++++---------- .../slam/tests/testEssentialMatrixFactor.cpp | 10 +-- gtsam/slam/tests/testRotateFactor.cpp | 12 +-- .../tests/testNonlinearClusterTree.cpp | 12 +-- 13 files changed, 93 insertions(+), 95 deletions(-) diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index 84bca65f3..17f921fd1 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -120,15 +120,15 @@ int main(int argc, char** argv) { // For simplicity, we will use the same noise model for each odometry factor noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses - graph.add(BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)); - graph.add(BetweenFactor(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise)); + graph.emplace_shared >(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise); + graph.emplace_shared >(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise); // 2b. Add "GPS-like" measurements // We will use our custom UnaryFactor for this. 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)); - graph.add(boost::make_shared(3, 4.0, 0.0, unaryNoise)); + graph.emplace_shared(1, 0.0, 0.0, unaryNoise); + graph.emplace_shared(2, 2.0, 0.0, unaryNoise); + graph.emplace_shared(3, 4.0, 0.0, unaryNoise); graph.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution diff --git a/examples/METISOrderingExample.cpp b/examples/METISOrderingExample.cpp index eb6bdd49d..3416eb6b7 100644 --- a/examples/METISOrderingExample.cpp +++ b/examples/METISOrderingExample.cpp @@ -37,12 +37,12 @@ int main(int argc, char** argv) { 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)); - graph.add(PriorFactor(1, priorMean, priorNoise)); + graph.emplace_shared >(1, priorMean, priorNoise); Pose2 odometry(2.0, 0.0, 0.0); 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)); + graph.emplace_shared >(1, 2, odometry, odometryNoise); + graph.emplace_shared >(2, 3, odometry, odometryNoise); graph.print("\nFactor Graph:\n"); // print Values initial; diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index b5cd681e5..3b547e70c 100644 --- a/examples/OdometryExample.cpp +++ b/examples/OdometryExample.cpp @@ -65,15 +65,15 @@ int main(int argc, char** argv) { // A prior factor consists of a mean and a noise model (covariance matrix) 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)); - graph.add(PriorFactor(1, priorMean, priorNoise)); + graph.emplace_shared >(1, priorMean, priorNoise); // Add odometry factors Pose2 odometry(2.0, 0.0, 0.0); // For simplicity, we will use the same noise model for each odometry factor noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses - graph.add(BetweenFactor(1, 2, odometry, odometryNoise)); - graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); + graph.emplace_shared >(1, 2, odometry, odometryNoise); + graph.emplace_shared >(2, 3, odometry, odometryNoise); graph.print("\nFactor Graph:\n"); // print // Create the data structure to hold the initialEstimate estimate to the solution diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index a716f9cd8..7c43c22e2 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -81,13 +81,13 @@ int main(int argc, char** argv) { // Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix) Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - graph.add(PriorFactor(x1, prior, priorNoise)); // add directly to graph + graph.emplace_shared >(x1, prior, priorNoise); // add directly to graph // Add two odometry factors Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta - graph.add(BetweenFactor(x1, x2, odometry, odometryNoise)); - graph.add(BetweenFactor(x2, x3, odometry, odometryNoise)); + graph.emplace_shared >(x1, x2, odometry, odometryNoise); + graph.emplace_shared >(x2, x3, odometry, odometryNoise); // Add Range-Bearing measurements to two different landmarks // create a noise model for the landmark measurements @@ -101,9 +101,9 @@ int main(int argc, char** argv) { range32 = 2.0; // Add Bearing-Range factors - graph.add(BearingRangeFactor(x1, l1, bearing11, range11, measurementNoise)); - graph.add(BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise)); - graph.add(BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise)); + graph.emplace_shared >(x1, l1, bearing11, range11, measurementNoise); + graph.emplace_shared >(x2, l1, bearing21, range21, measurementNoise); + graph.emplace_shared >(x3, l2, bearing32, range32, measurementNoise); // Print graph.print("Factor Graph:\n"); diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index 7991f7fbf..f977a08af 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -72,23 +72,23 @@ int main(int argc, char** argv) { // 2a. Add a prior on the first pose, setting it to the origin // A prior factor consists of a mean and a noise model (covariance matrix) noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.add(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); + graph.emplace_shared >(1, Pose2(0, 0, 0), priorNoise); // For simplicity, we will use the same noise model for odometry and loop closures noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 2b. Add odometry factors // Create odometry (Between) factors between consecutive poses - graph.add(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); - graph.add(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); - graph.add(BetweenFactor(3, 4, Pose2(2, 0, M_PI_2), model)); - graph.add(BetweenFactor(4, 5, Pose2(2, 0, M_PI_2), model)); + graph.emplace_shared >(1, 2, Pose2(2, 0, 0 ), model); + graph.emplace_shared >(2, 3, Pose2(2, 0, M_PI_2), model); + graph.emplace_shared >(3, 4, Pose2(2, 0, M_PI_2), model); + graph.emplace_shared >(4, 5, Pose2(2, 0, M_PI_2), model); // 2c. Add the loop closure constraint // This factor encodes the fact that we have returned to the same pose. In real systems, // these constraints may be identified in many ways, such as appearance-based techniques // with camera images. We will use another Between Factor to enforce this constraint: - graph.add(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); + graph.emplace_shared >(5, 2, Pose2(2, 0, M_PI_2), model); graph.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index a839289c2..8ca1be596 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -89,9 +89,8 @@ int main(int argc, char* argv[]) { SimpleCamera camera(poses[i], *K); Point2 measurement = camera.project(points[j]); // Add measurement - graph.add( - GenericProjectionFactor(measurement, noise, - Symbol('x', i), Symbol('l', j), K)); + graph.emplace_shared >(measurement, noise, + Symbol('x', i), Symbol('l', j), K); } // Intentionally initialize the variables off from the ground truth @@ -109,12 +108,12 @@ int main(int argc, char* argv[]) { // Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); - graph.add(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); + graph.emplace_shared >(Symbol('x', 0), poses[0], poseNoise); // Add a prior on landmark l0 noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.add(PriorFactor(Symbol('l', 0), points[0], pointNoise)); + graph.emplace_shared >(Symbol('l', 0), points[0], pointNoise); // Add initial guesses to all observed landmarks Point3 noise(-0.25, 0.20, 0.15); diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 912b26262..71093d668 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -426,7 +426,7 @@ TEST(Unit3, ErrorBetweenFactor) { // Add prior factors. SharedNoiseModel R_prior = noiseModel::Unit::Create(2); for (size_t i = 0; i < data.size(); i++) { - graph.add(PriorFactor(U(i), data[i], R_prior)); + graph.emplace_shared >(U(i), data[i], R_prior); } // Add process factors using the dot product error function. diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index d1dc6316d..6f2d60f2f 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -781,8 +781,8 @@ TEST(ImuFactor, bodyPSensorWithBias) { pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factors - graph.add(ImuFactor(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim)); - graph.add(BetweenFactor(B(i - 1), B(i), zeroBias, biasNoiseModel)); + graph.emplace_shared(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim); + graph.emplace_shared >(B(i - 1), B(i), zeroBias, biasNoiseModel); values.insert(X(i), Pose3()); values.insert(V(i), zeroVel); diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index f049e9d62..58408e7e3 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -120,9 +120,8 @@ NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) { boost::shared_ptr > pose3Prior = boost::dynamic_pointer_cast >(factor); if (pose3Prior) - pose3Graph.add( - BetweenFactor(keyAnchor, pose3Prior->keys()[0], - pose3Prior->prior(), pose3Prior->noiseModel())); + pose3Graph.emplace_shared >(keyAnchor, pose3Prior->keys()[0], + pose3Prior->prior(), pose3Prior->noiseModel()); } return pose3Graph; } @@ -330,7 +329,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { // add prior noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6); initialPose.insert(keyAnchor, Pose3()); - pose3graph.add(PriorFactor(keyAnchor, Pose3(), priorModel)); + pose3graph.emplace_shared >(keyAnchor, Pose3(), priorModel); // Create optimizer GaussNewtonParams params; diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 1dd4e8abc..b3e208b91 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -112,18 +112,18 @@ TEST( dataSet, readG2o) noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(44.721360, 44.721360, 30.901699)); NonlinearFactorGraph expectedGraph; - expectedGraph.add(BetweenFactor(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); - expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); - expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); - expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); - expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); - expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); - expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); - expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); - expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); - expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); - expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); - expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); + expectedGraph.emplace_shared >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model); + expectedGraph.emplace_shared >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model); + expectedGraph.emplace_shared >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model); + expectedGraph.emplace_shared >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model); + expectedGraph.emplace_shared >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model); + expectedGraph.emplace_shared >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model); + expectedGraph.emplace_shared >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model); + expectedGraph.emplace_shared >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model); + expectedGraph.emplace_shared >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model); + expectedGraph.emplace_shared >(9,10, Pose2(1.003350, 0.022250, -0.195918), model); + expectedGraph.emplace_shared >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model); + expectedGraph.emplace_shared >(3,10, Pose2(0.044020, 0.988477, -1.553511), model); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); } @@ -164,27 +164,27 @@ TEST( dataSet, readG2o3D) Point3 p01 = Point3(1.001367, 0.015390, 0.004948); Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); - expectedGraph.add(BetweenFactor(0, 1, Pose3(R01,p01), model)); + expectedGraph.emplace_shared >(0, 1, Pose3(R01,p01), model); Point3 p12 = Point3(0.523923, 0.776654, 0.326659); Rot3 R12 = Rot3::Quaternion(0.105373 , 0.311512, 0.656877, -0.678505 ); - expectedGraph.add(BetweenFactor(1, 2, Pose3(R12,p12), model)); + expectedGraph.emplace_shared >(1, 2, Pose3(R12,p12), model); Point3 p23 = Point3(0.910927, 0.055169, -0.411761); Rot3 R23 = Rot3::Quaternion(0.568551 , 0.595795, -0.561677, 0.079353 ); - expectedGraph.add(BetweenFactor(2, 3, Pose3(R23,p23), model)); + expectedGraph.emplace_shared >(2, 3, Pose3(R23,p23), model); Point3 p34 = Point3(0.775288, 0.228798, -0.596923); Rot3 R34 = Rot3::Quaternion(0.542221 , -0.592077, 0.303380, -0.513226 ); - expectedGraph.add(BetweenFactor(3, 4, Pose3(R34,p34), model)); + expectedGraph.emplace_shared >(3, 4, Pose3(R34,p34), model); Point3 p14 = Point3(-0.577841, 0.628016, -0.543592); Rot3 R14 = Rot3::Quaternion(0.327419 , -0.125250, -0.534379, 0.769122 ); - expectedGraph.add(BetweenFactor(1, 4, Pose3(R14,p14), model)); + expectedGraph.emplace_shared >(1, 4, Pose3(R14,p14), model); Point3 p30 = Point3(-0.623267, 0.086928, 0.773222); Rot3 R30 = Rot3::Quaternion(0.083672 , 0.104639, 0.627755, 0.766795 ); - expectedGraph.add(BetweenFactor(3, 0, Pose3(R30,p30), model)); + expectedGraph.emplace_shared >(3, 0, Pose3(R30,p30), model); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); } @@ -224,7 +224,7 @@ TEST( dataSet, readG2o3DNonDiagonalNoise) NonlinearFactorGraph expectedGraph; Point3 p01 = Point3(1.001367, 0.015390, 0.004948); Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); - expectedGraph.add(BetweenFactor(0, 1, Pose3(R01,p01), model)); + expectedGraph.emplace_shared >(0, 1, Pose3(R01,p01), model); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-2)); } @@ -242,18 +242,18 @@ TEST( dataSet, readG2oHuber) SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel); NonlinearFactorGraph expectedGraph; - expectedGraph.add(BetweenFactor(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); - expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); - expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); - expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); - expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); - expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); - expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); - expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); - expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); - expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); - expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); - expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); + expectedGraph.emplace_shared >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model); + expectedGraph.emplace_shared >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model); + expectedGraph.emplace_shared >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model); + expectedGraph.emplace_shared >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model); + expectedGraph.emplace_shared >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model); + expectedGraph.emplace_shared >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model); + expectedGraph.emplace_shared >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model); + expectedGraph.emplace_shared >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model); + expectedGraph.emplace_shared >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model); + expectedGraph.emplace_shared >(9,10, Pose2(1.003350, 0.022250, -0.195918), model); + expectedGraph.emplace_shared >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model); + expectedGraph.emplace_shared >(3,10, Pose2(0.044020, 0.988477, -1.553511), model); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); } @@ -270,18 +270,18 @@ TEST( dataSet, readG2oTukey) SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel); NonlinearFactorGraph expectedGraph; - expectedGraph.add(BetweenFactor(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); - expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); - expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); - expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); - expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); - expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); - expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); - expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); - expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); - expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); - expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); - expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); + expectedGraph.emplace_shared >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model); + expectedGraph.emplace_shared >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model); + expectedGraph.emplace_shared >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model); + expectedGraph.emplace_shared >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model); + expectedGraph.emplace_shared >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model); + expectedGraph.emplace_shared >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model); + expectedGraph.emplace_shared >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model); + expectedGraph.emplace_shared >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model); + expectedGraph.emplace_shared >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model); + expectedGraph.emplace_shared >(9,10, Pose2(1.003350, 0.022250, -0.195918), model); + expectedGraph.emplace_shared >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model); + expectedGraph.emplace_shared >(3,10, Pose2(0.044020, 0.988477, -1.553511), model); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); } diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 2e3d613d6..d33551edf 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -178,7 +178,7 @@ TEST (EssentialMatrixFactor, minimization) { // Noise sigma is 1cm, assuming metric measurements NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) - graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1)); + graph.emplace_shared(1, pA(i), pB(i), model1); // Check error at ground truth Values truth; @@ -251,7 +251,7 @@ TEST (EssentialMatrixFactor2, minimization) { // Noise sigma is 1cm, assuming metric measurements NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) - graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2)); + graph.emplace_shared(100, i, pA(i), pB(i), model2); // Check error at ground truth Values truth; @@ -323,7 +323,7 @@ TEST (EssentialMatrixFactor3, minimization) { NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) // but now we specify the rotation bRc - graph.add(EssentialMatrixFactor3(100, i, pA(i), pB(i), cRb, model2)); + graph.emplace_shared(100, i, pA(i), pB(i), cRb, model2); // Check error at ground truth Values truth; @@ -391,7 +391,7 @@ TEST (EssentialMatrixFactor, extraMinimization) { NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) - graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1, K)); + graph.emplace_shared(1, pA(i), pB(i), model1, K); // Check error at ground truth Values truth; @@ -465,7 +465,7 @@ TEST (EssentialMatrixFactor2, extraMinimization) { // Noise sigma is 1, assuming pixel measurements NonlinearFactorGraph graph; for (size_t i = 0; i < data.number_tracks(); i++) - graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2, K)); + graph.emplace_shared(100, i, pA(i), pB(i), model2, K); // Check error at ground truth Values truth; diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index 9bb296444..caf3d31df 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -89,9 +89,9 @@ TEST (RotateFactor, minimization) { // Let's try to recover the correct iRc by minimizing NonlinearFactorGraph graph; Model model = noiseModel::Isotropic::Sigma(3, 0.01); - graph.add(RotateFactor(1, i1Ri2, c1Zc2, model)); - graph.add(RotateFactor(1, i2Ri3, c2Zc3, model)); - graph.add(RotateFactor(1, i3Ri4, c3Zc4, model)); + graph.emplace_shared(1, i1Ri2, c1Zc2, model); + graph.emplace_shared(1, i2Ri3, c2Zc3, model); + graph.emplace_shared(1, i3Ri4, c3Zc4, model); // Check error at ground truth Values truth; @@ -162,9 +162,9 @@ TEST (RotateDirectionsFactor, minimization) { // Let's try to recover the correct iRc by minimizing NonlinearFactorGraph graph; Model model = noiseModel::Isotropic::Sigma(2, 0.01); - graph.add(RotateDirectionsFactor(1, p1, z1, model)); - graph.add(RotateDirectionsFactor(1, p2, z2, model)); - graph.add(RotateDirectionsFactor(1, p3, z3, model)); + graph.emplace_shared(1, p1, z1, model); + graph.emplace_shared(1, p2, z2, model); + graph.emplace_shared(1, p3, z3, model); // Check error at ground truth Values truth; diff --git a/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp index 9391814d4..2240034af 100644 --- a/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp +++ b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp @@ -27,22 +27,22 @@ NonlinearFactorGraph planarSLAMGraph() { // Prior on pose x1 at the origin. Pose2 prior(0.0, 0.0, 0.0); auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.add(PriorFactor(x1, prior, priorNoise)); + graph.emplace_shared >(x1, prior, priorNoise); // Two odometry factors Pose2 odometry(2.0, 0.0, 0.0); auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - graph.add(BetweenFactor(x1, x2, odometry, odometryNoise)); - graph.add(BetweenFactor(x2, x3, odometry, odometryNoise)); + graph.emplace_shared >(x1, x2, odometry, odometryNoise); + graph.emplace_shared >(x2, x3, odometry, odometryNoise); // Add Range-Bearing measurements to two different landmarks auto measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); Rot2 bearing11 = Rot2::fromDegrees(45), bearing21 = Rot2::fromDegrees(90), bearing32 = Rot2::fromDegrees(90); double range11 = std::sqrt(4.0 + 4.0), range21 = 2.0, range32 = 2.0; - graph.add(BearingRangeFactor(x1, l1, bearing11, range11, measurementNoise)); - graph.add(BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise)); - graph.add(BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise)); + graph.emplace_shared >(x1, l1, bearing11, range11, measurementNoise); + graph.emplace_shared >(x2, l1, bearing21, range21, measurementNoise); + graph.emplace_shared >(x3, l2, bearing32, range32, measurementNoise); return graph; } From 60f556e513f4574bd3746b9003d0a0e1c5cca667 Mon Sep 17 00:00:00 2001 From: Yao Chen Date: Sat, 1 Oct 2016 11:45:44 -0400 Subject: [PATCH 10/35] Replaced graph.push_back with graph.emplace_shared if needed. --- gtsam_unstable/linear/tests/testLPSolver.cpp | 12 ++++++------ .../tests/testConcurrentIncrementalSmootherGN.cpp | 8 ++++---- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/gtsam_unstable/linear/tests/testLPSolver.cpp b/gtsam_unstable/linear/tests/testLPSolver.cpp index 8bf6be56b..a105a39f0 100644 --- a/gtsam_unstable/linear/tests/testLPSolver.cpp +++ b/gtsam_unstable/linear/tests/testLPSolver.cpp @@ -189,12 +189,12 @@ TEST(LPSolver, overConstrainedLinearSystem) { TEST(LPSolver, overConstrainedLinearSystem2) { GaussianFactorGraph graph; - graph.push_back(JacobianFactor(1, I_1x1, 2, I_1x1, kOne, - noiseModel::Constrained::All(1))); - graph.push_back(JacobianFactor(1, I_1x1, 2, -I_1x1, 5 * kOne, - noiseModel::Constrained::All(1))); - graph.push_back(JacobianFactor(1, I_1x1, 2, 2 * I_1x1, 6 * kOne, - noiseModel::Constrained::All(1))); + graph.emplace_shared(1, I_1x1, 2, I_1x1, kOne, + noiseModel::Constrained::All(1)); + graph.emplace_shared(1, I_1x1, 2, -I_1x1, 5 * kOne, + noiseModel::Constrained::All(1)); + graph.emplace_shared(1, I_1x1, 2, 2 * I_1x1, 6 * kOne, + noiseModel::Constrained::All(1)); VectorValues x = graph.optimize(); // This check confirms that gtsam linear constraint solver can't handle // over-constrained system diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index 65f74dc3c..fdf9f08b5 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -639,13 +639,13 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 ) actualGraph.print("actual graph: \n"); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } From d7d35876f1ee67dc928414c298304f3e14e34a92 Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Mon, 3 Oct 2016 19:11:44 -0400 Subject: [PATCH 11/35] jacobians for logmap and expmap --- gtsam/base/ProductLieGroup.h | 22 ++++++++++++++++------ tests/testLie.cpp | 27 +++++++++++++++++++++++++++ 2 files changed, 43 insertions(+), 6 deletions(-) diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 463b5f5d9..87ead88f0 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -138,17 +138,27 @@ public: return ProductLieGroup(g,h); } static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { - if (Hv) throw std::runtime_error("ProductLieGroup::Expmap derivatives not implemented yet"); - G g = traits::Expmap(v.template head()); - H h = traits::Expmap(v.template tail()); + Jacobian1 D_g_first; Jacobian2 D_h_second; + G g = traits::Expmap(v.template head(), Hv ? &D_g_first : 0); + H h = traits::Expmap(v.template tail(), Hv ? &D_h_second : 0); + if (Hv) { + Hv->setZero(); + Hv->template topLeftCorner() = D_g_first; + Hv->template bottomRightCorner() = D_h_second; + } return ProductLieGroup(g,h); } static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) { - if (Hp) throw std::runtime_error("ProductLieGroup::Logmap derivatives not implemented yet"); - typename traits::TangentVector v1 = traits::Logmap(p.first); - typename traits::TangentVector v2 = traits::Logmap(p.second); + Jacobian1 D_g_first; Jacobian2 D_h_second; + typename traits::TangentVector v1 = traits::Logmap(p.first, Hp ? &D_g_first : 0); + typename traits::TangentVector v2 = traits::Logmap(p.second, Hp ? &D_h_second : 0); TangentVector v; v << v1, v2; + if (Hp) { + Hp->setZero(); + Hp->template topLeftCorner() = D_g_first; + Hp->template bottomRightCorner() = D_h_second; + } return v; } ProductLieGroup expmap(const TangentVector& v) const { diff --git a/tests/testLie.cpp b/tests/testLie.cpp index a134a899c..2d8a0b975 100644 --- a/tests/testLie.cpp +++ b/tests/testLie.cpp @@ -102,6 +102,33 @@ TEST( testProduct, inverse ) { EXPECT(assert_equal(numericH1, actH1, tol)); } +/* ************************************************************************* */ +Product expmap_proxy(const Vector5& vec) { + return Product::Expmap(vec); +} +TEST( testProduct, Expmap ) { + Vector5 vec; + vec << 1, 2, 0.1, 0.2, 0.3; + + Matrix actH; + Product::Expmap(vec, actH); + Matrix numericH = numericalDerivative11(expmap_proxy, vec); + EXPECT(assert_equal(numericH, actH, tol)); +} + +/* ************************************************************************* */ +Vector5 logmap_proxy(const Product& p) { + return Product::Logmap(p); +} +TEST( testProduct, Logmap ) { + Product state(Point2(1, 2), Pose2(3, 4, 5)); + + Matrix actH; + Product::Logmap(state, actH); + Matrix numericH = numericalDerivative11(logmap_proxy, state); + EXPECT(assert_equal(numericH, actH, tol)); +} + //****************************************************************************** int main() { TestResult tr; From f9de023caff5e7dab0daf94acd06a389b1a46629 Mon Sep 17 00:00:00 2001 From: chrisbeall Date: Thu, 6 Oct 2016 14:25:40 -0700 Subject: [PATCH 12/35] Only add custom all.tests target when GTSAM_BUILD_TESTS is true --- cmake/GtsamTesting.cmake | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/cmake/GtsamTesting.cmake b/cmake/GtsamTesting.cmake index 62a129010..15d4219e6 100644 --- a/cmake/GtsamTesting.cmake +++ b/cmake/GtsamTesting.cmake @@ -101,6 +101,9 @@ mark_as_advanced(GTSAM_SINGLE_TEST_EXE) # Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck) if(GTSAM_BUILD_TESTS) add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} -C $ --output-on-failure) + + # Add target to build tests without running + add_custom_target(all.tests) endif() # Add examples target @@ -109,8 +112,6 @@ add_custom_target(examples) # Add timing target add_custom_target(timing) -# Add target to build tests without running -add_custom_target(all.tests) # Implementations of this file's macros: From 9ba6b80f9ed49c212a22b589c47aeb95265bb1bb Mon Sep 17 00:00:00 2001 From: Yao Chen Date: Sat, 8 Oct 2016 10:55:22 -0400 Subject: [PATCH 13/35] 1. Replace boost enable_if and is_base_of with those in stl 2. Removed boost header files 3. Removed testFactorGraph.cpp --- gtsam/inference/FactorGraph.h | 36 ++++++++++--------- gtsam/inference/tests/testFactorGraph.cpp | 44 ----------------------- 2 files changed, 19 insertions(+), 61 deletions(-) delete mode 100644 gtsam/inference/tests/testFactorGraph.cpp diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 604de632c..86e130cc7 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -22,17 +22,16 @@ #pragma once -#include -#include -#include -#include -#include -#include - #include #include #include +#include +#include +#include +#include + +#include #include namespace gtsam { @@ -153,7 +152,7 @@ namespace gtsam { /** Add a factor directly using a shared_ptr */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(boost::shared_ptr factor) { factors_.push_back(boost::shared_ptr(factor)); } @@ -163,19 +162,20 @@ namespace gtsam { /** Emplace a factor */ template - void emplace_shared(Args&&... args) { + typename std::enable_if::value>::type + emplace_shared(Args&&... args) { factors_.push_back(boost::make_shared(std::forward(args)...)); } /** push back many factors with an iterator over shared_ptr (factors are not copied) */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(ITERATOR firstFactor, ITERATOR lastFactor) { factors_.insert(end(), firstFactor, lastFactor); } /** push back many factors as shared_ptr's in a container (factors are not copied) */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(const CONTAINER& container) { push_back(container.begin(), container.end()); } @@ -183,22 +183,24 @@ namespace gtsam { /** push back a BayesTree as a collection of factors. NOTE: This should be hidden in derived * classes in favor of a type-specialized version that calls this templated function. */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(const BayesTree& bayesTree) { bayesTree.addFactorsToGraph(*this); } +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /** Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid * the copy). */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(const DERIVEDFACTOR& factor) { factors_.push_back(boost::make_shared(factor)); } +#endif /** push back many factors with an iterator over plain factors (factors are copied) */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(ITERATOR firstFactor, ITERATOR lastFactor) { for (ITERATOR f = firstFactor; f != lastFactor; ++f) push_back(*f); @@ -206,14 +208,14 @@ namespace gtsam { /** push back many factors as non-pointer objects in a container (factors are copied) */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(const CONTAINER& container) { push_back(container.begin(), container.end()); } /** Add a factor directly using a shared_ptr */ template - typename boost::enable_if, + typename std::enable_if::value, boost::assign::list_inserter > >::type operator+=(boost::shared_ptr factor) { return boost::assign::make_list_inserter(RefCallPushBack(*this))(factor); @@ -234,7 +236,7 @@ namespace gtsam { /** Add a factor directly using a shared_ptr */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type add(boost::shared_ptr factor) { push_back(factor); } diff --git a/gtsam/inference/tests/testFactorGraph.cpp b/gtsam/inference/tests/testFactorGraph.cpp deleted file mode 100644 index a3a69e72d..000000000 --- a/gtsam/inference/tests/testFactorGraph.cpp +++ /dev/null @@ -1,44 +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 testFactorGraph.cpp - * @author Yao Chen - */ - -#include -#include -#include - -#include -#include - -using namespace std; -using namespace gtsam; - - -TEST(FactorGraph, testEmplaceShared) { - Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); - noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); - Point2 measurement(10.0, 10.0); - - NonlinearFactorGraph graph; - graph.push_back(GenericProjectionFactor(measurement, measurementNoise, Symbol('x', 0), Symbol('l', 0), K)); - graph.emplace_shared >(measurement, measurementNoise, Symbol('x', 0), Symbol('l', 0), K); - - EXPECT_LONGS_EQUAL(2, graph.size()); - EXPECT(graph[0]->equals(*(graph[1]))); -} - - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ From e64bc1007ca12cbd3fb282fd2bf769e4cdfb5a2f Mon Sep 17 00:00:00 2001 From: chrisbeall Date: Sun, 9 Oct 2016 22:18:11 -0700 Subject: [PATCH 14/35] Upgrade to Eigen 3.2.10 - http://eigen.tuxfamily.org/index.php?title=ChangeLog#Eigen_3.2.10 --- gtsam/3rdparty/Eigen/.hg_archival.txt | 4 +- gtsam/3rdparty/Eigen/.hgtags | 2 + gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h | 5 +- .../Eigen/Eigen/src/Core/CommaInitializer.h | 25 +- .../Eigen/Eigen/src/Core/DiagonalMatrix.h | 6 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h | 2 +- .../3rdparty/Eigen/Eigen/src/Core/Functors.h | 3 + .../Eigen/Eigen/src/Core/GeneralProduct.h | 5 - .../Eigen/Eigen/src/Core/GenericPacketMath.h | 4 +- .../Eigen/Eigen/src/Core/MathFunctions.h | 40 +- .../Eigen/Eigen/src/Core/PermutationMatrix.h | 3 +- .../Eigen/Eigen/src/Core/PlainObjectBase.h | 6 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Reverse.h | 20 +- .../Eigen/Eigen/src/Core/SolveTriangular.h | 3 +- .../3rdparty/Eigen/Eigen/src/Core/Transpose.h | 9 +- .../Eigen/Eigen/src/Core/Transpositions.h | 3 +- .../Core/products/TriangularSolverMatrix.h | 2 +- .../Eigen/Eigen/src/Core/util/BlasUtil.h | 57 +- .../Eigen/Eigen/src/Core/util/Constants.h | 16 +- .../src/Core/util/DisableStupidWarnings.h | 8 + .../Eigen/Eigen/src/Core/util/Macros.h | 2 +- .../Eigen/Eigen/src/Core/util/Memory.h | 133 +- .../src/Core/util/ReenableStupidWarnings.h | 3 + .../src/Eigenvalues/GeneralizedEigenSolver.h | 32 +- .../src/Eigenvalues/Tridiagonalization.h | 4 +- .../Eigen/Eigen/src/Geometry/Homogeneous.h | 2 +- .../Eigen/Eigen/src/Geometry/Quaternion.h | 2 +- .../Eigen/Eigen/src/Geometry/Transform.h | 8 +- .../Eigen/Eigen/src/Geometry/Translation.h | 6 +- .../Eigen/Eigen/src/Householder/Householder.h | 3 +- .../src/Householder/HouseholderSequence.h | 3 +- gtsam/3rdparty/Eigen/Eigen/src/LU/Inverse.h | 2 +- .../Eigen/Eigen/src/LU/arch/Inverse_SSE.h | 28 +- .../Eigen/src/PaStiXSupport/PaStiXSupport.h | 12 +- .../Eigen/src/PardisoSupport/PardisoSupport.h | 21 +- .../Eigen/Eigen/src/QR/ColPivHouseholderQR.h | 3 - .../Eigen/Eigen/src/QR/FullPivHouseholderQR.h | 3 - .../Eigen/Eigen/src/QR/HouseholderQR.h | 3 - .../3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h | 90 +- .../Eigen/src/SparseCore/CompressedStorage.h | 11 +- .../Eigen/Eigen/src/SparseCore/SparseBlock.h | 1162 +++++++++-------- .../Eigen/Eigen/src/SparseCore/SparseMatrix.h | 12 +- .../Eigen/src/SparseCore/SparseMatrixBase.h | 1 + .../Eigen/Eigen/src/SparseCore/SparseRedux.h | 7 +- .../SparseSparseProductWithPruning.h | 2 +- .../Eigen/Eigen/src/SparseCore/SparseVector.h | 10 +- .../Eigen/src/UmfPackSupport/UmfPackSupport.h | 3 +- .../Eigen/bench/btl/generic_bench/btl.hh | 13 +- .../Eigen/cmake/EigenConfigureTesting.cmake | 11 +- .../Eigen/cmake/EigenDetermineOSVersion.cmake | 2 +- .../Eigen/doc/SparseLinearSystems.dox | 3 + gtsam/3rdparty/Eigen/doc/TopicAssertions.dox | 2 +- gtsam/3rdparty/Eigen/test/CMakeLists.txt | 1 + .../3rdparty/Eigen/test/commainitializer.cpp | 62 +- gtsam/3rdparty/Eigen/test/cwiseop.cpp | 3 + .../Eigen/test/eigen2/eigen2_cwiseop.cpp | 3 + gtsam/3rdparty/Eigen/test/geo_homogeneous.cpp | 2 + .../Eigen/test/geo_transformations.cpp | 79 ++ gtsam/3rdparty/Eigen/test/packetmath.cpp | 1 + .../3rdparty/Eigen/test/prec_inverse_4x4.cpp | 15 + gtsam/3rdparty/Eigen/test/product.h | 8 + gtsam/3rdparty/Eigen/test/sparse_basic.cpp | 8 + .../src/MatrixFunctions/MatrixExponential.h | 8 +- .../unsupported/Eigen/src/Splines/SplineFwd.h | 8 +- .../Eigen/unsupported/test/autodiff.cpp | 11 + 65 files changed, 1201 insertions(+), 830 deletions(-) diff --git a/gtsam/3rdparty/Eigen/.hg_archival.txt b/gtsam/3rdparty/Eigen/.hg_archival.txt index c4115d7f6..4dd5bd180 100644 --- a/gtsam/3rdparty/Eigen/.hg_archival.txt +++ b/gtsam/3rdparty/Eigen/.hg_archival.txt @@ -1,4 +1,4 @@ repo: 8a21fd850624c931e448cbcfb38168cb2717c790 -node: 07105f7124f9aef00a68c85e0fc606e65d3d6c15 +node: b9cd8366d4e8f49471c7afafc4c2a1b00e54a54d branch: 3.2 -tag: 3.2.8 +tag: 3.2.10 diff --git a/gtsam/3rdparty/Eigen/.hgtags b/gtsam/3rdparty/Eigen/.hgtags index 8f0097f20..c83128570 100644 --- a/gtsam/3rdparty/Eigen/.hgtags +++ b/gtsam/3rdparty/Eigen/.hgtags @@ -31,3 +31,5 @@ ffa86ffb557094721ca71dcea6aed2651b9fd610 3.2.0 bdd17ee3b1b3a166cd5ec36dcad4fc1f3faf774a 3.2.5 c58038c56923e0fd86de3ded18e03df442e66dfb 3.2.6 b30b87236a1b1552af32ac34075ee5696a9b5a33 3.2.7 +07105f7124f9aef00a68c85e0fc606e65d3d6c15 3.2.8 +dc6cfdf9bcec5efc7b6593bddbbb3d675de53524 3.2.9 diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h index 827894443..87bedfa46 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h @@ -66,9 +66,8 @@ struct traits > : traits::MaxColsAtCompileTime), XprTypeIsRowMajor = (int(traits::Flags)&RowMajorBit) != 0, - IsDense = is_same::value, - IsRowMajor = (IsDense&&MaxRowsAtCompileTime==1&&MaxColsAtCompileTime!=1) ? 1 - : (IsDense&&MaxColsAtCompileTime==1&&MaxRowsAtCompileTime!=1) ? 0 + IsRowMajor = (MaxRowsAtCompileTime==1&&MaxColsAtCompileTime!=1) ? 1 + : (MaxColsAtCompileTime==1&&MaxRowsAtCompileTime!=1) ? 0 : XprTypeIsRowMajor, HasSameStorageOrderAsXprType = (IsRowMajor == XprTypeIsRowMajor), InnerSize = IsRowMajor ? int(ColsAtCompileTime) : int(RowsAtCompileTime), diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h index a036d8c3b..5dd3adeae 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h @@ -76,9 +76,7 @@ struct CommaInitializer template CommaInitializer& operator,(const DenseBase& other) { - if(other.cols()==0 || other.rows()==0) - return *this; - if (m_col==m_xpr.cols()) + if (m_col==m_xpr.cols() && (other.cols()!=0 || other.rows()!=m_currentBlockRows)) { m_row+=m_currentBlockRows; m_col = 0; @@ -86,24 +84,18 @@ struct CommaInitializer eigen_assert(m_row+m_currentBlockRows<=m_xpr.rows() && "Too many rows passed to comma initializer (operator<<)"); } - eigen_assert(m_col - (m_row, m_col) = other; - else - m_xpr.block(m_row, m_col, other.rows(), other.cols()) = other; + m_xpr.template block + (m_row, m_col, other.rows(), other.cols()) = other; m_col += other.cols(); return *this; } inline ~CommaInitializer() { - eigen_assert((m_row+m_currentBlockRows) == m_xpr.rows() - && m_col == m_xpr.cols() - && "Too few coefficients passed to comma initializer (operator<<)"); + finished(); } /** \returns the built matrix once all its coefficients have been set. @@ -113,7 +105,12 @@ struct CommaInitializer * quaternion.fromRotationMatrix((Matrix3f() << axis0, axis1, axis2).finished()); * \endcode */ - inline XprType& finished() { return m_xpr; } + inline XprType& finished() { + eigen_assert(((m_row+m_currentBlockRows) == m_xpr.rows() || m_xpr.cols() == 0) + && m_col == m_xpr.cols() + && "Too few coefficients passed to comma initializer (operator<<)"); + return m_xpr; + } XprType& m_xpr; // target expression Index m_row; // current row id diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalMatrix.h index e6c220f41..53c757bef 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalMatrix.h @@ -44,10 +44,10 @@ class DiagonalBase : public EigenBase template void evalTo(MatrixBase &other) const; template - void addTo(MatrixBase &other) const + inline void addTo(MatrixBase &other) const { other.diagonal() += diagonal(); } template - void subTo(MatrixBase &other) const + inline void subTo(MatrixBase &other) const { other.diagonal() -= diagonal(); } inline const DiagonalVectorType& diagonal() const { return derived().diagonal(); } @@ -98,7 +98,7 @@ class DiagonalBase : public EigenBase template template -void DiagonalBase::evalTo(MatrixBase &other) const +inline void DiagonalBase::evalTo(MatrixBase &other) const { other.setZero(); other.diagonal() = diagonal(); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h index 9d7651f1f..23aab831b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h @@ -59,7 +59,7 @@ struct dot_nocheck */ template template -typename internal::scalar_product_traits::Scalar,typename internal::traits::Scalar>::ReturnType +inline typename internal::scalar_product_traits::Scalar,typename internal::traits::Scalar>::ReturnType MatrixBase::dot(const MatrixBase& other) const { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h index 5f14c6587..5a1b2f28a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h @@ -969,6 +969,8 @@ template struct functor_traits > { enum { Cost = 1, PacketAccess = false }; }; +#if(__cplusplus < 201103L) +// std::binder* are deprecated since c++11 and will be removed in c++17 template struct functor_traits > { enum { Cost = functor_traits::Cost, PacketAccess = false }; }; @@ -976,6 +978,7 @@ struct functor_traits > template struct functor_traits > { enum { Cost = functor_traits::Cost, PacketAccess = false }; }; +#endif template struct functor_traits > diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h index 29ac522d2..5744eb71e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h @@ -205,9 +205,6 @@ class GeneralProduct public: GeneralProduct(const Lhs& lhs, const Rhs& rhs) { - EIGEN_STATIC_ASSERT((internal::is_same::value), - YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) - Base::coeffRef(0,0) = (lhs.transpose().cwiseProduct(rhs)).sum(); } @@ -264,8 +261,6 @@ class GeneralProduct GeneralProduct(const Lhs& lhs, const Rhs& rhs) : Base(lhs,rhs) { - EIGEN_STATIC_ASSERT((internal::is_same::value), - YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) } struct set { template void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived() = src; } }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h index 5f783ebee..c6e93bbb0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h @@ -183,8 +183,8 @@ template inline void pstoreu(Scalar* to, const /** \internal tries to do cache prefetching of \a addr */ template inline void prefetch(const Scalar* addr) { -#if !defined(_MSC_VER) -__builtin_prefetch(addr); +#if (!EIGEN_COMP_MSVC) && (EIGEN_COMP_GNUC || EIGEN_COMP_CLANG || EIGEN_COMP_ICC) + __builtin_prefetch(addr); #endif } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h index 4e17ecd4b..f707aa41e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h @@ -218,8 +218,8 @@ struct conj_retval * Implementation of abs2 * ****************************************************************************/ -template -struct abs2_impl +template +struct abs2_impl_default { typedef typename NumTraits::Real RealScalar; static inline RealScalar run(const Scalar& x) @@ -228,15 +228,26 @@ struct abs2_impl } }; -template -struct abs2_impl > +template +struct abs2_impl_default // IsComplex { - static inline RealScalar run(const std::complex& x) + typedef typename NumTraits::Real RealScalar; + static inline RealScalar run(const Scalar& x) { return real(x)*real(x) + imag(x)*imag(x); } }; +template +struct abs2_impl +{ + typedef typename NumTraits::Real RealScalar; + static inline RealScalar run(const Scalar& x) + { + return abs2_impl_default::IsComplex>::run(x); + } +}; + template struct abs2_retval { @@ -496,11 +507,24 @@ struct floor_log2 template struct random_default_impl { - typedef typename NumTraits::NonInteger NonInteger; - static inline Scalar run(const Scalar& x, const Scalar& y) { - return x + Scalar((NonInteger(y)-x+1) * std::rand() / (RAND_MAX + NonInteger(1))); + typedef typename conditional::IsSigned,std::ptrdiff_t,std::size_t>::type ScalarX; + if(y=x the result converted to an unsigned long is still correct. + std::size_t range = ScalarX(y)-ScalarX(x); + std::size_t offset = 0; + // rejection sampling + std::size_t divisor = 1; + std::size_t multiplier = 1; + if(range range); + return Scalar(ScalarX(x) + offset); } static inline Scalar run() diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h index 85ffae265..bda79fa04 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h @@ -584,10 +584,11 @@ struct permut_matrix_product_retval const Index n = Side==OnTheLeft ? rows() : cols(); // FIXME we need an is_same for expression that is not sensitive to constness. For instance // is_same_xpr, Block >::value should be true. + const typename Dest::Scalar *dst_data = internal::extract_data(dst); if( is_same::value && blas_traits::HasUsableDirectAccess && blas_traits::HasUsableDirectAccess - && extract_data(dst) == extract_data(m_matrix)) + && dst_data!=0 && dst_data == extract_data(m_matrix)) { // apply the permutation inplace Matrix mask(m_permutation.size()); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h index a4e4af4a7..9f71956ff 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h @@ -315,8 +315,8 @@ class PlainObjectBase : public internal::dense_xpr_base::type EIGEN_STRONG_INLINE void resizeLike(const EigenBase& _other) { const OtherDerived& other = _other.derived(); - internal::check_rows_cols_for_overflow::run(other.rows(), other.cols()); - const Index othersize = other.rows()*other.cols(); + internal::check_rows_cols_for_overflow::run(Index(other.rows()), Index(other.cols())); + const Index othersize = Index(other.rows())*Index(other.cols()); if(RowsAtCompileTime == 1) { eigen_assert(other.rows() == 1 || other.cols() == 1); @@ -487,7 +487,7 @@ class PlainObjectBase : public internal::dense_xpr_base::type /** \sa MatrixBase::operator=(const EigenBase&) */ template EIGEN_STRONG_INLINE PlainObjectBase(const EigenBase &other) - : m_storage(other.derived().rows() * other.derived().cols(), other.derived().rows(), other.derived().cols()) + : m_storage(Index(other.derived().rows()) * Index(other.derived().cols()), other.derived().rows(), other.derived().cols()) { _check_template_params(); internal::check_rows_cols_for_overflow::run(other.derived().rows(), other.derived().cols()); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Reverse.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Reverse.h index e30ae3d28..041f8222a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Reverse.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Reverse.h @@ -76,9 +76,23 @@ template class Reverse EIGEN_DENSE_PUBLIC_INTERFACE(Reverse) using Base::IsRowMajor; - // next line is necessary because otherwise const version of operator() - // is hidden by non-const version defined in this file - using Base::operator(); + // The following two operators are provided to worarkound + // a MSVC 2013 issue. In theory, we could simply do: + // using Base::operator(); + // to make const version of operator() visible. + // Otheriwse, they would be hidden by the non-const versions defined in this file + + inline CoeffReturnType operator()(Index row, Index col) const + { + eigen_assert(row >= 0 && row < rows() && col >= 0 && col < cols()); + return coeff(row, col); + } + + inline CoeffReturnType operator()(Index index) const + { + eigen_assert(index >= 0 && index < m_matrix.size()); + return coeff(index); + } protected: enum { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h index 83565ddd8..30c9c38ec 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h @@ -243,7 +243,8 @@ template struct triangular_solv template inline void evalTo(Dest& dst) const { - if(!(is_same::value && extract_data(dst) == extract_data(m_rhs))) + const typename Dest::Scalar *dst_data = internal::extract_data(dst); + if(!(is_same::value && dst_data!=0 && extract_data(dst) == extract_data(m_rhs))) dst = m_rhs; m_triangularMatrix.template solveInPlace(dst); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h index 22096ea2f..2abce3c66 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h @@ -331,11 +331,11 @@ inline void MatrixBase::adjointInPlace() namespace internal { -template -struct blas_traits > - : blas_traits +template +struct blas_traits > + : blas_traits::type> { - typedef SelfCwiseBinaryOp XprType; + typedef SelfCwiseBinaryOp XprType; static inline const XprType extract(const XprType& x) { return x; } }; @@ -392,7 +392,6 @@ struct checkTransposeAliasing_impl ::run(extract_data(dst), other)) && "aliasing detected during transposition, use transposeInPlace() " "or evaluate the rhs into a temporary using .eval()"); - } }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h index e4ba0756f..16bc1ce15 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h @@ -376,7 +376,8 @@ struct transposition_matrix_product_retval const int size = m_transpositions.size(); Index j = 0; - if(!(is_same::value && extract_data(dst) == extract_data(m_matrix))) + const typename Dest::Scalar *dst_data = internal::extract_data(dst); + if(!(is_same::value && dst_data!=0 && dst_data == extract_data(m_matrix))) dst = m_matrix; for(int k=(Transposed?size-1:0) ; Transposed?k>=0:k0 ? l2/(4 * sizeof(Scalar) * otherStride) : 0; + Index subcols = cols>0 ? l2/(4 * sizeof(Scalar) * std::max(otherStride,size)) : 0; subcols = std::max((subcols/Traits::nr)*Traits::nr, Traits::nr); for(Index k2=IsLower ? 0 : size; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/BlasUtil.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/BlasUtil.h index a28f16fa0..e74381432 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/BlasUtil.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/BlasUtil.h @@ -42,16 +42,29 @@ template struct conj_if; template<> struct conj_if { template - inline T operator()(const T& x) { return numext::conj(x); } + inline T operator()(const T& x) const { return numext::conj(x); } template - inline T pconj(const T& x) { return internal::pconj(x); } + inline T pconj(const T& x) const { return internal::pconj(x); } }; template<> struct conj_if { template - inline const T& operator()(const T& x) { return x; } + inline const T& operator()(const T& x) const { return x; } template - inline const T& pconj(const T& x) { return x; } + inline const T& pconj(const T& x) const { return x; } +}; + +// Generic implementation for custom complex types. +template +struct conj_helper +{ + typedef typename scalar_product_traits::ReturnType Scalar; + + EIGEN_STRONG_INLINE Scalar pmadd(const LhsScalar& x, const RhsScalar& y, const Scalar& c) const + { return padd(c, pmul(x,y)); } + + EIGEN_STRONG_INLINE Scalar pmul(const LhsScalar& x, const RhsScalar& y) const + { return conj_if()(x) * conj_if()(y); } }; template struct conj_helper @@ -171,12 +184,13 @@ template struct blas_traits }; // pop conjugate -template -struct blas_traits, NestedXpr> > - : blas_traits +template +struct blas_traits, Xpr> > + : blas_traits::type> { + typedef typename internal::remove_all::type NestedXpr; typedef blas_traits Base; - typedef CwiseUnaryOp, NestedXpr> XprType; + typedef CwiseUnaryOp, Xpr> XprType; typedef typename Base::ExtractType ExtractType; enum { @@ -188,12 +202,13 @@ struct blas_traits, NestedXpr> > }; // pop scalar multiple -template -struct blas_traits, NestedXpr> > - : blas_traits +template +struct blas_traits, Xpr> > + : blas_traits::type> { + typedef typename internal::remove_all::type NestedXpr; typedef blas_traits Base; - typedef CwiseUnaryOp, NestedXpr> XprType; + typedef CwiseUnaryOp, Xpr> XprType; typedef typename Base::ExtractType ExtractType; static inline ExtractType extract(const XprType& x) { return Base::extract(x.nestedExpression()); } static inline Scalar extractScalarFactor(const XprType& x) @@ -201,12 +216,13 @@ struct blas_traits, NestedXpr> > }; // pop opposite -template -struct blas_traits, NestedXpr> > - : blas_traits +template +struct blas_traits, Xpr> > + : blas_traits::type> { + typedef typename internal::remove_all::type NestedXpr; typedef blas_traits Base; - typedef CwiseUnaryOp, NestedXpr> XprType; + typedef CwiseUnaryOp, Xpr> XprType; typedef typename Base::ExtractType ExtractType; static inline ExtractType extract(const XprType& x) { return Base::extract(x.nestedExpression()); } static inline Scalar extractScalarFactor(const XprType& x) @@ -214,13 +230,14 @@ struct blas_traits, NestedXpr> > }; // pop/push transpose -template -struct blas_traits > - : blas_traits +template +struct blas_traits > + : blas_traits::type> { + typedef typename internal::remove_all::type NestedXpr; typedef typename NestedXpr::Scalar Scalar; typedef blas_traits Base; - typedef Transpose XprType; + typedef Transpose XprType; typedef Transpose ExtractType; // const to get rid of a compile error; anyway blas traits are only used on the RHS typedef Transpose _ExtractType; typedef typename conditional=6 + + #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS + #pragma GCC diagnostic push + #endif + #pragma GCC diagnostic ignored "-Wignored-attributes" + #endif #endif // not EIGEN_WARNINGS_DISABLED diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h index e0d90eb82..7d9c89fa1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h @@ -13,7 +13,7 @@ #define EIGEN_WORLD_VERSION 3 #define EIGEN_MAJOR_VERSION 2 -#define EIGEN_MINOR_VERSION 8 +#define EIGEN_MINOR_VERSION 10 #define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \ (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h index bc1ea69ed..ffa7e34f3 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h @@ -507,7 +507,12 @@ template void smart_copy(const T* start, const T* end, T* target) template struct smart_copy_helper { static inline void run(const T* start, const T* end, T* target) - { memcpy(target, start, std::ptrdiff_t(end)-std::ptrdiff_t(start)); } + { + std::ptrdiff_t size = std::ptrdiff_t(end)-std::ptrdiff_t(start); + if(size==0) return; + eigen_internal_assert(start!=0 && end!=0 && target!=0); + memcpy(target, start, size); + } }; template struct smart_copy_helper { @@ -515,7 +520,6 @@ template struct smart_copy_helper { { std::copy(start, end, target); } }; - /***************************************************************************** *** Implementation of runtime stack allocation (falling back to malloc) *** *****************************************************************************/ @@ -655,99 +659,60 @@ template class aligned_stack_memory_handler /****************************************************************************/ + /** \class aligned_allocator -* \ingroup Core_Module -* -* \brief STL compatible allocator to use with with 16 byte aligned types -* -* Example: -* \code -* // Matrix4f requires 16 bytes alignment: -* std::map< int, Matrix4f, std::less, -* aligned_allocator > > my_map_mat4; -* // Vector3f does not require 16 bytes alignment, no need to use Eigen's allocator: -* std::map< int, Vector3f > my_map_vec3; -* \endcode -* -* \sa \ref TopicStlContainers. -*/ + * \ingroup Core_Module + * + * \brief STL compatible allocator to use with with 16 byte aligned types + * + * Example: + * \code + * // Matrix4f requires 16 bytes alignment: + * std::map< int, Matrix4f, std::less, + * aligned_allocator > > my_map_mat4; + * // Vector3f does not require 16 bytes alignment, no need to use Eigen's allocator: + * std::map< int, Vector3f > my_map_vec3; + * \endcode + * + * \sa \blank \ref TopicStlContainers. + */ template -class aligned_allocator +class aligned_allocator : public std::allocator { public: - typedef size_t size_type; - typedef std::ptrdiff_t difference_type; - typedef T* pointer; - typedef const T* const_pointer; - typedef T& reference; - typedef const T& const_reference; - typedef T value_type; + typedef size_t size_type; + typedef std::ptrdiff_t difference_type; + typedef T* pointer; + typedef const T* const_pointer; + typedef T& reference; + typedef const T& const_reference; + typedef T value_type; - template - struct rebind - { - typedef aligned_allocator other; - }; + template + struct rebind + { + typedef aligned_allocator other; + }; - pointer address( reference value ) const - { - return &value; - } + aligned_allocator() : std::allocator() {} - const_pointer address( const_reference value ) const - { - return &value; - } + aligned_allocator(const aligned_allocator& other) : std::allocator(other) {} - aligned_allocator() - { - } + template + aligned_allocator(const aligned_allocator& other) : std::allocator(other) {} - aligned_allocator( const aligned_allocator& ) - { - } + ~aligned_allocator() {} - template - aligned_allocator( const aligned_allocator& ) - { - } + pointer allocate(size_type num, const void* /*hint*/ = 0) + { + internal::check_size_for_overflow(num); + return static_cast( internal::aligned_malloc(num * sizeof(T)) ); + } - ~aligned_allocator() - { - } - - size_type max_size() const - { - return (std::numeric_limits::max)(); - } - - pointer allocate( size_type num, const void* hint = 0 ) - { - EIGEN_UNUSED_VARIABLE(hint); - internal::check_size_for_overflow(num); - return static_cast( internal::aligned_malloc( num * sizeof(T) ) ); - } - - void construct( pointer p, const T& value ) - { - ::new( p ) T( value ); - } - - void destroy( pointer p ) - { - p->~T(); - } - - void deallocate( pointer p, size_type /*num*/ ) - { - internal::aligned_free( p ); - } - - bool operator!=(const aligned_allocator& ) const - { return false; } - - bool operator==(const aligned_allocator& ) const - { return true; } + void deallocate(pointer p, size_type /*num*/) + { + internal::aligned_free(p); + } }; //---------- Cache sizes ---------- diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h index 5ddfbd4aa..d573bbd4a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h @@ -8,7 +8,10 @@ #pragma warning pop #elif defined __clang__ #pragma clang diagnostic pop + #elif defined __GNUC__ && __GNUC__>=6 + #pragma GCC diagnostic pop #endif + #endif #endif // EIGEN_WARNINGS_DISABLED diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h index 956e80d9e..e5131d20b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h @@ -327,13 +327,33 @@ GeneralizedEigenSolver::compute(const MatrixType& A, const MatrixTyp } else { - Scalar p = Scalar(0.5) * (m_matS.coeff(i, i) - m_matS.coeff(i+1, i+1)); - Scalar z = sqrt(abs(p * p + m_matS.coeff(i+1, i) * m_matS.coeff(i, i+1))); - m_alphas.coeffRef(i) = ComplexScalar(m_matS.coeff(i+1, i+1) + p, z); - m_alphas.coeffRef(i+1) = ComplexScalar(m_matS.coeff(i+1, i+1) + p, -z); + // We need to extract the generalized eigenvalues of the pair of a general 2x2 block S and a triangular 2x2 block T + // From the eigen decomposition of T = U * E * U^-1, + // we can extract the eigenvalues of (U^-1 * S * U) / E + // Here, we can take advantage that E = diag(T), and U = [ 1 T_01 ; 0 T_11-T_00], and U^-1 = [1 -T_11/(T_11-T_00) ; 0 1/(T_11-T_00)]. + // Then taking beta=T_00*T_11*(T_11-T_00), we can avoid any division, and alpha is the eigenvalues of A = (U^-1 * S * U) * diag(T_11,T_00) * (T_11-T_00): + + // T = [a b ; 0 c] + // S = [e f ; g h] + RealScalar a = m_realQZ.matrixT().coeff(i, i), b = m_realQZ.matrixT().coeff(i, i+1), c = m_realQZ.matrixT().coeff(i+1, i+1); + RealScalar e = m_matS.coeff(i, i), f = m_matS.coeff(i, i+1), g = m_matS.coeff(i+1, i), h = m_matS.coeff(i+1, i+1); + RealScalar d = c-a; + RealScalar gb = g*b; + Matrix A; + A << (e*d-gb)*c, ((e*b+f*d-h*b)*d-gb*b)*a, + g*c , (gb+h*d)*a; + + // NOTE, we could also compute the SVD of T's block during the QZ factorization so that the respective T block is guaranteed to be diagonal, + // and then we could directly apply the formula below (while taking care of scaling S columns by T11,T00): + + Scalar p = Scalar(0.5) * (A.coeff(i, i) - A.coeff(i+1, i+1)); + Scalar z = sqrt(abs(p * p + A.coeff(i+1, i) * A.coeff(i, i+1))); + m_alphas.coeffRef(i) = ComplexScalar(A.coeff(i+1, i+1) + p, z); + m_alphas.coeffRef(i+1) = ComplexScalar(A.coeff(i+1, i+1) + p, -z); + + m_betas.coeffRef(i) = + m_betas.coeffRef(i+1) = a*c*d; - m_betas.coeffRef(i) = m_realQZ.matrixT().coeff(i,i); - m_betas.coeffRef(i+1) = m_realQZ.matrixT().coeff(i,i); i += 2; } } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/Tridiagonalization.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/Tridiagonalization.h index 192278d68..a63c08aee 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/Tridiagonalization.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/Tridiagonalization.h @@ -367,10 +367,10 @@ void tridiagonalization_inplace(MatrixType& matA, CoeffVectorType& hCoeffs) hCoeffs.tail(n-i-1).noalias() = (matA.bottomRightCorner(remainingSize,remainingSize).template selfadjointView() * (conj(h) * matA.col(i).tail(remainingSize))); - hCoeffs.tail(n-i-1) += (conj(h)*Scalar(-0.5)*(hCoeffs.tail(remainingSize).dot(matA.col(i).tail(remainingSize)))) * matA.col(i).tail(n-i-1); + hCoeffs.tail(n-i-1) += (conj(h)*RealScalar(-0.5)*(hCoeffs.tail(remainingSize).dot(matA.col(i).tail(remainingSize)))) * matA.col(i).tail(n-i-1); matA.bottomRightCorner(remainingSize, remainingSize).template selfadjointView() - .rankUpdate(matA.col(i).tail(remainingSize), hCoeffs.tail(remainingSize), -1); + .rankUpdate(matA.col(i).tail(remainingSize), hCoeffs.tail(remainingSize), Scalar(-1)); matA.col(i).coeffRef(i+1) = beta; hCoeffs.coeffRef(i) = h; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Homogeneous.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Homogeneous.h index 372e422b9..820ac96fe 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Homogeneous.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Homogeneous.h @@ -75,7 +75,7 @@ template class Homogeneous inline Index rows() const { return m_matrix.rows() + (int(Direction)==Vertical ? 1 : 0); } inline Index cols() const { return m_matrix.cols() + (int(Direction)==Horizontal ? 1 : 0); } - inline Scalar coeff(Index row, Index col) const + inline Scalar coeff(Index row, Index col=0) const { if( (int(Direction)==Vertical && row==m_matrix.rows()) || (int(Direction)==Horizontal && col==m_matrix.cols())) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h index 25ed17bb6..e89ba80b2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h @@ -276,7 +276,7 @@ public: inline Coefficients& coeffs() { return m_coeffs;} inline const Coefficients& coeffs() const { return m_coeffs;} - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(IsAligned) + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(bool(IsAligned)) protected: Coefficients m_coeffs; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h index 0186f3b82..e87eec201 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h @@ -443,7 +443,7 @@ public: operator * (const DiagonalBase &b) const { TransformTimeDiagonalReturnType res(*this); - res.linear() *= b; + res.linearExt() *= b; return res; } @@ -557,7 +557,7 @@ public: return res; } - inline Transform& operator*=(const DiagonalMatrix& s) { linear() *= s; return *this; } + inline Transform& operator*=(const DiagonalMatrix& s) { linearExt() *= s; return *this; } template inline Transform& operator=(const RotationBase& r); @@ -828,7 +828,7 @@ Transform::prescale(const MatrixBase &oth { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim)) EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS) - m_matrix.template block(0,0).noalias() = (other.asDiagonal() * m_matrix.template block(0,0)); + affine().noalias() = (other.asDiagonal() * affine()); return *this; } @@ -1048,7 +1048,7 @@ void Transform::computeRotationScaling(RotationMatrixTy } } -/** decomposes the linear part of the transformation as a product rotation x scaling, the scaling being +/** decomposes the linear part of the transformation as a product scaling x rotation, the scaling being * not necessarily positive. * * If either pointer is zero, the corresponding computation is skipped. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h index 2e7798686..84f6a0d12 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h @@ -130,8 +130,10 @@ public: } /** Applies translation to vector */ - inline VectorType operator* (const VectorType& other) const - { return m_coeffs + other; } + template + inline typename internal::enable_if::type + operator* (const MatrixBase& vec) const + { return m_coeffs + vec.derived(); } /** \returns the inverse translation (opposite) */ Translation inverse() const { return Translation(-m_coeffs); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Householder/Householder.h b/gtsam/3rdparty/Eigen/Eigen/src/Householder/Householder.h index 32112af9b..4c1f499a1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Householder/Householder.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Householder/Householder.h @@ -75,8 +75,9 @@ void MatrixBase::makeHouseholder( RealScalar tailSqNorm = size()==1 ? RealScalar(0) : tail.squaredNorm(); Scalar c0 = coeff(0); + const RealScalar tol = (std::numeric_limits::min)(); - if(tailSqNorm == RealScalar(0) && numext::imag(c0)==RealScalar(0)) + if(tailSqNorm <= tol && numext::abs2(numext::imag(c0))<=tol) { tau = RealScalar(0); beta = numext::real(c0); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Householder/HouseholderSequence.h b/gtsam/3rdparty/Eigen/Eigen/src/Householder/HouseholderSequence.h index d800ca1fa..aea2439a9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Householder/HouseholderSequence.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Householder/HouseholderSequence.h @@ -237,8 +237,9 @@ template class HouseholderS { workspace.resize(rows()); Index vecs = m_length; + const typename Dest::Scalar *dst_data = internal::extract_data(dst); if( internal::is_same::type,Dest>::value - && internal::extract_data(dst) == internal::extract_data(m_vectors)) + && dst_data!=0 && dst_data == internal::extract_data(m_vectors)) { // in-place dst.diagonal().setOnes(); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/LU/Inverse.h b/gtsam/3rdparty/Eigen/Eigen/src/LU/Inverse.h index 3cf887193..e836fd696 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/LU/Inverse.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/LU/Inverse.h @@ -290,7 +290,7 @@ struct inverse_impl : public ReturnByValue > { const int Size = EIGEN_PLAIN_ENUM_MIN(MatrixType::ColsAtCompileTime,Dest::ColsAtCompileTime); EIGEN_ONLY_USED_FOR_DEBUG(Size); - eigen_assert(( (Size<=1) || (Size>4) || (extract_data(m_matrix)!=extract_data(dst))) + eigen_assert(( (Size<=1) || (Size>4) || (extract_data(m_matrix)!=0 && extract_data(m_matrix)!=extract_data(dst))) && "Aliasing problem detected in inverse(), you need to do inverse().eval() here."); compute_inverse::run(m_matrix, dst); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/LU/arch/Inverse_SSE.h b/gtsam/3rdparty/Eigen/Eigen/src/LU/arch/Inverse_SSE.h index 60b7a2376..80ec674d6 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/LU/arch/Inverse_SSE.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/LU/arch/Inverse_SSE.h @@ -151,10 +151,12 @@ struct compute_inverse_size4 iC = _mm_mul_ps(rd,iC); iD = _mm_mul_ps(rd,iD); - result.template writePacket( 0, _mm_shuffle_ps(iA,iB,0x77)); - result.template writePacket( 4, _mm_shuffle_ps(iA,iB,0x22)); - result.template writePacket( 8, _mm_shuffle_ps(iC,iD,0x77)); - result.template writePacket(12, _mm_shuffle_ps(iC,iD,0x22)); + DenseIndex res_stride = result.outerStride(); + float* res = result.data(); + pstoret(res+0, _mm_shuffle_ps(iA,iB,0x77)); + pstoret(res+res_stride, _mm_shuffle_ps(iA,iB,0x22)); + pstoret(res+2*res_stride, _mm_shuffle_ps(iC,iD,0x77)); + pstoret(res+3*res_stride, _mm_shuffle_ps(iC,iD,0x22)); } }; @@ -311,14 +313,16 @@ struct compute_inverse_size4 iC1 = _mm_sub_pd(_mm_mul_pd(B1, dC), iC1); iC2 = _mm_sub_pd(_mm_mul_pd(B2, dC), iC2); - result.template writePacket( 0, _mm_mul_pd(_mm_shuffle_pd(iA2, iA1, 3), d1)); // iA# / det - result.template writePacket( 4, _mm_mul_pd(_mm_shuffle_pd(iA2, iA1, 0), d2)); - result.template writePacket( 2, _mm_mul_pd(_mm_shuffle_pd(iB2, iB1, 3), d1)); // iB# / det - result.template writePacket( 6, _mm_mul_pd(_mm_shuffle_pd(iB2, iB1, 0), d2)); - result.template writePacket( 8, _mm_mul_pd(_mm_shuffle_pd(iC2, iC1, 3), d1)); // iC# / det - result.template writePacket(12, _mm_mul_pd(_mm_shuffle_pd(iC2, iC1, 0), d2)); - result.template writePacket(10, _mm_mul_pd(_mm_shuffle_pd(iD2, iD1, 3), d1)); // iD# / det - result.template writePacket(14, _mm_mul_pd(_mm_shuffle_pd(iD2, iD1, 0), d2)); + DenseIndex res_stride = result.outerStride(); + double* res = result.data(); + pstoret(res+0, _mm_mul_pd(_mm_shuffle_pd(iA2, iA1, 3), d1)); + pstoret(res+res_stride, _mm_mul_pd(_mm_shuffle_pd(iA2, iA1, 0), d2)); + pstoret(res+2, _mm_mul_pd(_mm_shuffle_pd(iB2, iB1, 3), d1)); + pstoret(res+res_stride+2, _mm_mul_pd(_mm_shuffle_pd(iB2, iB1, 0), d2)); + pstoret(res+2*res_stride, _mm_mul_pd(_mm_shuffle_pd(iC2, iC1, 3), d1)); + pstoret(res+3*res_stride, _mm_mul_pd(_mm_shuffle_pd(iC2, iC1, 0), d2)); + pstoret(res+2*res_stride+2,_mm_mul_pd(_mm_shuffle_pd(iD2, iD1, 3), d1)); + pstoret(res+3*res_stride+2,_mm_mul_pd(_mm_shuffle_pd(iD2, iD1, 0), d2)); } }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/PaStiXSupport/PaStiXSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/PaStiXSupport/PaStiXSupport.h index a955287d1..20acc0226 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/PaStiXSupport/PaStiXSupport.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/PaStiXSupport/PaStiXSupport.h @@ -10,6 +10,14 @@ #ifndef EIGEN_PASTIXSUPPORT_H #define EIGEN_PASTIXSUPPORT_H +#if defined(DCOMPLEX) + #define PASTIX_COMPLEX COMPLEX + #define PASTIX_DCOMPLEX DCOMPLEX +#else + #define PASTIX_COMPLEX std::complex + #define PASTIX_DCOMPLEX std::complex +#endif + namespace Eigen { /** \ingroup PaStiXSupport_Module @@ -74,14 +82,14 @@ namespace internal { if (n == 0) { ptr = NULL; idx = NULL; vals = NULL; } if (nbrhs == 0) {x = NULL; nbrhs=1;} - c_pastix(pastix_data, pastix_comm, n, ptr, idx, reinterpret_cast(vals), perm, invp, reinterpret_cast(x), nbrhs, iparm, dparm); + c_pastix(pastix_data, pastix_comm, n, ptr, idx, reinterpret_cast(vals), perm, invp, reinterpret_cast(x), nbrhs, iparm, dparm); } void eigen_pastix(pastix_data_t **pastix_data, int pastix_comm, int n, int *ptr, int *idx, std::complex *vals, int *perm, int * invp, std::complex *x, int nbrhs, int *iparm, double *dparm) { if (n == 0) { ptr = NULL; idx = NULL; vals = NULL; } if (nbrhs == 0) {x = NULL; nbrhs=1;} - z_pastix(pastix_data, pastix_comm, n, ptr, idx, reinterpret_cast(vals), perm, invp, reinterpret_cast(x), nbrhs, iparm, dparm); + z_pastix(pastix_data, pastix_comm, n, ptr, idx, reinterpret_cast(vals), perm, invp, reinterpret_cast(x), nbrhs, iparm, dparm); } // Convert the matrix to Fortran-style Numbering diff --git a/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h b/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h index 18cd7d88a..0faacc5f5 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h @@ -221,11 +221,11 @@ class PardisoImpl m_type = type; bool symmetric = std::abs(m_type) < 10; m_iparm[0] = 1; // No solver default - m_iparm[1] = 3; // use Metis for the ordering - m_iparm[2] = 1; // Numbers of processors, value of OMP_NUM_THREADS + m_iparm[1] = 2; // use Metis for the ordering + m_iparm[2] = 0; // Reserved. Set to zero. (??Numbers of processors, value of OMP_NUM_THREADS??) m_iparm[3] = 0; // No iterative-direct algorithm m_iparm[4] = 0; // No user fill-in reducing permutation - m_iparm[5] = 0; // Write solution into x + m_iparm[5] = 0; // Write solution into x, b is left unchanged m_iparm[6] = 0; // Not in use m_iparm[7] = 2; // Max numbers of iterative refinement steps m_iparm[8] = 0; // Not in use @@ -246,7 +246,10 @@ class PardisoImpl m_iparm[26] = 0; // No matrix checker m_iparm[27] = (sizeof(RealScalar) == 4) ? 1 : 0; m_iparm[34] = 1; // C indexing - m_iparm[59] = 1; // Automatic switch between In-Core and Out-of-Core modes + m_iparm[36] = 0; // CSR + m_iparm[59] = 0; // 0 - In-Core ; 1 - Automatic switch between In-Core and Out-of-Core modes ; 2 - Out-of-Core + + memset(m_pt, 0, sizeof(m_pt)); } protected: @@ -384,7 +387,6 @@ bool PardisoImpl::_solve(const MatrixBase &b, MatrixBase::_solve(const MatrixBase &b, MatrixBase * * \sa \ref TutorialSparseDirectSolvers @@ -447,6 +452,9 @@ class PardisoLU : public PardisoImpl< PardisoLU > * using the Intel MKL PARDISO library. The sparse matrix A must be selfajoint and positive definite. * The vectors or matrices X and B can be either dense or sparse. * + * By default, it runs in in-core mode. To enable PARDISO's out-of-core feature, set: + * \code solver.pardisoParameterArray()[59] = 1; \endcode + * * \tparam MatrixType the type of the sparse matrix A, it must be a SparseMatrix<> * \tparam UpLo can be any bitwise combination of Upper, Lower. The default is Upper, meaning only the upper triangular part has to be used. * Upper|Lower can be used to tell both triangular parts can be used as input. @@ -507,6 +515,9 @@ class PardisoLLT : public PardisoImpl< PardisoLLT > * For complex matrices, A can also be symmetric only, see the \a Options template parameter. * The vectors or matrices X and B can be either dense or sparse. * + * By default, it runs in in-core mode. To enable PARDISO's out-of-core feature, set: + * \code solver.pardisoParameterArray()[59] = 1; \endcode + * * \tparam MatrixType the type of the sparse matrix A, it must be a SparseMatrix<> * \tparam Options can be any bitwise combination of Upper, Lower, and Symmetric. The default is Upper, meaning only the upper triangular part has to be used. * Symmetric can be used for symmetric, non-selfadjoint complex matrices, the default being to assume a selfadjoint matrix. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h index 567eab7cd..1ef77ae32 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h @@ -127,9 +127,6 @@ template class ColPivHouseholderQR * * \returns a solution. * - * \note The case where b is a matrix is not yet implemented. Also, this - * code is space inefficient. - * * \note_about_checking_solutions * * \note_about_arbitrary_choice_of_solution diff --git a/gtsam/3rdparty/Eigen/Eigen/src/QR/FullPivHouseholderQR.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/FullPivHouseholderQR.h index 0b39966e1..bdd994795 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/FullPivHouseholderQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/FullPivHouseholderQR.h @@ -134,9 +134,6 @@ template class FullPivHouseholderQR * \returns the exact or least-square solution if the rank is greater or equal to the number of columns of A, * and an arbitrary solution otherwise. * - * \note The case where b is a matrix is not yet implemented. Also, this - * code is space inefficient. - * * \note_about_checking_solutions * * \note_about_arbitrary_choice_of_solution diff --git a/gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR.h index 343a66499..e4a3a639f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR.h @@ -107,9 +107,6 @@ template class HouseholderQR * * \returns a solution. * - * \note The case where b is a matrix is not yet implemented. Also, this - * code is space inefficient. - * * \note_about_checking_solutions * * \note_about_arbitrary_choice_of_solution diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h index 89ace381e..7a5821d4f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h @@ -359,29 +359,42 @@ struct svd_precondition_2x2_block_to_be_real SVD; typedef typename SVD::Index Index; - static void run(typename SVD::WorkMatrixType&, SVD&, Index, Index) {} + typedef typename MatrixType::RealScalar RealScalar; + static bool run(typename SVD::WorkMatrixType&, SVD&, Index, Index, RealScalar&) { return true; } }; template struct svd_precondition_2x2_block_to_be_real { typedef JacobiSVD SVD; + typedef typename SVD::Index Index; typedef typename MatrixType::Scalar Scalar; typedef typename MatrixType::RealScalar RealScalar; - typedef typename SVD::Index Index; - static void run(typename SVD::WorkMatrixType& work_matrix, SVD& svd, Index p, Index q) + static bool run(typename SVD::WorkMatrixType& work_matrix, SVD& svd, Index p, Index q, RealScalar& maxDiagEntry) { using std::sqrt; + using std::abs; + using std::max; Scalar z; JacobiRotation rot; RealScalar n = sqrt(numext::abs2(work_matrix.coeff(p,p)) + numext::abs2(work_matrix.coeff(q,p))); - + + const RealScalar considerAsZero = (std::numeric_limits::min)(); + const RealScalar precision = NumTraits::epsilon(); + if(n==0) { - z = abs(work_matrix.coeff(p,q)) / work_matrix.coeff(p,q); - work_matrix.row(p) *= z; - if(svd.computeU()) svd.m_matrixU.col(p) *= conj(z); - if(work_matrix.coeff(q,q)!=Scalar(0)) + // make sure first column is zero + work_matrix.coeffRef(p,p) = work_matrix.coeffRef(q,p) = Scalar(0); + + if(abs(numext::imag(work_matrix.coeff(p,q)))>considerAsZero) + { + // work_matrix.coeff(p,q) can be zero if work_matrix.coeff(q,p) is not zero but small enough to underflow when computing n + z = abs(work_matrix.coeff(p,q)) / work_matrix.coeff(p,q); + work_matrix.row(p) *= z; + if(svd.computeU()) svd.m_matrixU.col(p) *= conj(z); + } + if(abs(numext::imag(work_matrix.coeff(q,q)))>considerAsZero) { z = abs(work_matrix.coeff(q,q)) / work_matrix.coeff(q,q); work_matrix.row(q) *= z; @@ -395,19 +408,25 @@ struct svd_precondition_2x2_block_to_be_real rot.s() = work_matrix.coeff(q,p) / n; work_matrix.applyOnTheLeft(p,q,rot); if(svd.computeU()) svd.m_matrixU.applyOnTheRight(p,q,rot.adjoint()); - if(work_matrix.coeff(p,q) != Scalar(0)) + if(abs(numext::imag(work_matrix.coeff(p,q)))>considerAsZero) { - Scalar z = abs(work_matrix.coeff(p,q)) / work_matrix.coeff(p,q); + z = abs(work_matrix.coeff(p,q)) / work_matrix.coeff(p,q); work_matrix.col(q) *= z; if(svd.computeV()) svd.m_matrixV.col(q) *= z; } - if(work_matrix.coeff(q,q) != Scalar(0)) + if(abs(numext::imag(work_matrix.coeff(q,q)))>considerAsZero) { z = abs(work_matrix.coeff(q,q)) / work_matrix.coeff(q,q); work_matrix.row(q) *= z; if(svd.computeU()) svd.m_matrixU.col(q) *= conj(z); } } + + // update largest diagonal entry + maxDiagEntry = max EIGEN_EMPTY (maxDiagEntry,max EIGEN_EMPTY (abs(work_matrix.coeff(p,p)), abs(work_matrix.coeff(q,q)))); + // and check whether the 2x2 block is already diagonal + RealScalar threshold = max EIGEN_EMPTY (considerAsZero, precision * maxDiagEntry); + return abs(work_matrix.coeff(p,q))>threshold || abs(work_matrix.coeff(q,p)) > threshold; } }; @@ -424,22 +443,23 @@ void real_2x2_jacobi_svd(const MatrixType& matrix, Index p, Index q, JacobiRotation rot1; RealScalar t = m.coeff(0,0) + m.coeff(1,1); RealScalar d = m.coeff(1,0) - m.coeff(0,1); - if(t == RealScalar(0)) + if(d == RealScalar(0)) { - rot1.c() = RealScalar(0); - rot1.s() = d > RealScalar(0) ? RealScalar(1) : RealScalar(-1); + rot1.s() = RealScalar(0); + rot1.c() = RealScalar(1); } else { - RealScalar t2d2 = numext::hypot(t,d); - rot1.c() = abs(t)/t2d2; - rot1.s() = d/t2d2; - if(tmakeJacobi(m,0,1); - *j_left = rot1 * j_right->transpose(); + *j_left = rot1 * j_right->transpose(); } } // end namespace internal @@ -826,6 +846,7 @@ JacobiSVD::compute(const MatrixType& matrix, unsig check_template_parameters(); using std::abs; + using std::max; allocate(matrix.rows(), matrix.cols(), computationOptions); // currently we stop when we reach precision 2*epsilon as the last bit of precision can require an unreasonable number of iterations, @@ -857,6 +878,7 @@ JacobiSVD::compute(const MatrixType& matrix, unsig } /*** step 2. The main Jacobi SVD iteration. ***/ + RealScalar maxDiagEntry = m_workMatrix.cwiseAbs().diagonal().maxCoeff(); bool finished = false; while(!finished) @@ -872,25 +894,27 @@ JacobiSVD::compute(const MatrixType& matrix, unsig // if this 2x2 sub-matrix is not diagonal already... // notice that this comparison will evaluate to false if any NaN is involved, ensuring that NaN's don't // keep us iterating forever. Similarly, small denormal numbers are considered zero. - using std::max; - RealScalar threshold = (max)(considerAsZero, precision * (max)(abs(m_workMatrix.coeff(p,p)), - abs(m_workMatrix.coeff(q,q)))); - // We compare both values to threshold instead of calling max to be robust to NaN (See bug 791) + RealScalar threshold = max EIGEN_EMPTY (considerAsZero, precision * maxDiagEntry); if(abs(m_workMatrix.coeff(p,q))>threshold || abs(m_workMatrix.coeff(q,p)) > threshold) { finished = false; - // perform SVD decomposition of 2x2 sub-matrix corresponding to indices p,q to make it diagonal - internal::svd_precondition_2x2_block_to_be_real::run(m_workMatrix, *this, p, q); - JacobiRotation j_left, j_right; - internal::real_2x2_jacobi_svd(m_workMatrix, p, q, &j_left, &j_right); + // the complex to real operation returns true is the updated 2x2 block is not already diagonal + if(internal::svd_precondition_2x2_block_to_be_real::run(m_workMatrix, *this, p, q, maxDiagEntry)) + { + JacobiRotation j_left, j_right; + internal::real_2x2_jacobi_svd(m_workMatrix, p, q, &j_left, &j_right); - // accumulate resulting Jacobi rotations - m_workMatrix.applyOnTheLeft(p,q,j_left); - if(computeU()) m_matrixU.applyOnTheRight(p,q,j_left.transpose()); + // accumulate resulting Jacobi rotations + m_workMatrix.applyOnTheLeft(p,q,j_left); + if(computeU()) m_matrixU.applyOnTheRight(p,q,j_left.transpose()); - m_workMatrix.applyOnTheRight(p,q,j_right); - if(computeV()) m_matrixV.applyOnTheRight(p,q,j_right); + m_workMatrix.applyOnTheRight(p,q,j_right); + if(computeV()) m_matrixV.applyOnTheRight(p,q,j_right); + + // keep track of the largest diagonal coefficient + maxDiagEntry = max EIGEN_EMPTY (maxDiagEntry,max EIGEN_EMPTY (abs(m_workMatrix.coeff(p,p)), abs(m_workMatrix.coeff(q,q)))); + } } } } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/CompressedStorage.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/CompressedStorage.h index a667cb56e..34cad3df7 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/CompressedStorage.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/CompressedStorage.h @@ -102,6 +102,11 @@ class CompressedStorage inline size_t allocatedSize() const { return m_allocatedSize; } inline void clear() { m_size = 0; } + const Scalar* valuePtr() const { return m_values; } + Scalar* valuePtr() { return m_values; } + const Index* indexPtr() const { return m_indices; } + Index* indexPtr() { return m_indices; } + inline Scalar& value(size_t i) { return m_values[i]; } inline const Scalar& value(size_t i) const { return m_values[i]; } @@ -208,8 +213,10 @@ class CompressedStorage Index* newIndices = new Index[size]; size_t copySize = (std::min)(size, m_size); // copy - internal::smart_copy(m_values, m_values+copySize, newValues); - internal::smart_copy(m_indices, m_indices+copySize, newIndices); + if (copySize>0) { + internal::smart_copy(m_values, m_values+copySize, newValues); + internal::smart_copy(m_indices, m_indices+copySize, newIndices); + } // delete old stuff delete[] m_values; delete[] m_indices; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h index 4f4983508..99886079d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h @@ -1,539 +1,623 @@ -// This file is part of Eigen, a lightweight C++ template library -// for linear algebra. -// -// Copyright (C) 2008-2009 Gael Guennebaud -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#ifndef EIGEN_SPARSE_BLOCK_H -#define EIGEN_SPARSE_BLOCK_H - -namespace Eigen { - -template -class BlockImpl - : public SparseMatrixBase > -{ - typedef typename internal::remove_all::type _MatrixTypeNested; - typedef Block BlockType; -public: - enum { IsRowMajor = internal::traits::IsRowMajor }; -protected: - enum { OuterSize = IsRowMajor ? BlockRows : BlockCols }; -public: - EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) - - class InnerIterator: public XprType::InnerIterator - { - typedef typename BlockImpl::Index Index; - public: - inline InnerIterator(const BlockType& xpr, Index outer) - : XprType::InnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) - {} - inline Index row() const { return IsRowMajor ? m_outer : this->index(); } - inline Index col() const { return IsRowMajor ? this->index() : m_outer; } - protected: - Index m_outer; - }; - class ReverseInnerIterator: public XprType::ReverseInnerIterator - { - typedef typename BlockImpl::Index Index; - public: - inline ReverseInnerIterator(const BlockType& xpr, Index outer) - : XprType::ReverseInnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) - {} - inline Index row() const { return IsRowMajor ? m_outer : this->index(); } - inline Index col() const { return IsRowMajor ? this->index() : m_outer; } - protected: - Index m_outer; - }; - - inline BlockImpl(const XprType& xpr, int i) - : m_matrix(xpr), m_outerStart(i), m_outerSize(OuterSize) - {} - - inline BlockImpl(const XprType& xpr, int startRow, int startCol, int blockRows, int blockCols) - : m_matrix(xpr), m_outerStart(IsRowMajor ? startRow : startCol), m_outerSize(IsRowMajor ? blockRows : blockCols) - {} - - inline const Scalar coeff(int row, int col) const - { - return m_matrix.coeff(row + IsRowMajor ? m_outerStart : 0, col +IsRowMajor ? 0 : m_outerStart); - } - - inline const Scalar coeff(int index) const - { - return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index : m_outerStart); - } - - EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); } - EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); } - - protected: - - typename XprType::Nested m_matrix; - Index m_outerStart; - const internal::variable_if_dynamic m_outerSize; - - EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) - private: - Index nonZeros() const; -}; - - -/*************************************************************************** -* specialisation for SparseMatrix -***************************************************************************/ - -template -class BlockImpl,BlockRows,BlockCols,true,Sparse> - : public SparseMatrixBase,BlockRows,BlockCols,true> > -{ - typedef SparseMatrix<_Scalar, _Options, _Index> SparseMatrixType; - typedef typename internal::remove_all::type _MatrixTypeNested; - typedef Block BlockType; - typedef Block ConstBlockType; -public: - enum { IsRowMajor = internal::traits::IsRowMajor }; - EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) -protected: - enum { OuterSize = IsRowMajor ? BlockRows : BlockCols }; -public: - - class InnerIterator: public SparseMatrixType::InnerIterator - { - public: - inline InnerIterator(const BlockType& xpr, Index outer) - : SparseMatrixType::InnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) - {} - inline Index row() const { return IsRowMajor ? m_outer : this->index(); } - inline Index col() const { return IsRowMajor ? this->index() : m_outer; } - protected: - Index m_outer; - }; - class ReverseInnerIterator: public SparseMatrixType::ReverseInnerIterator - { - public: - inline ReverseInnerIterator(const BlockType& xpr, Index outer) - : SparseMatrixType::ReverseInnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) - {} - inline Index row() const { return IsRowMajor ? m_outer : this->index(); } - inline Index col() const { return IsRowMajor ? this->index() : m_outer; } - protected: - Index m_outer; - }; - - inline BlockImpl(const SparseMatrixType& xpr, int i) - : m_matrix(xpr), m_outerStart(i), m_outerSize(OuterSize) - {} - - inline BlockImpl(const SparseMatrixType& xpr, int startRow, int startCol, int blockRows, int blockCols) - : m_matrix(xpr), m_outerStart(IsRowMajor ? startRow : startCol), m_outerSize(IsRowMajor ? blockRows : blockCols) - {} - - template - inline BlockType& operator=(const SparseMatrixBase& other) - { - typedef typename internal::remove_all::type _NestedMatrixType; - _NestedMatrixType& matrix = const_cast<_NestedMatrixType&>(m_matrix);; - // This assignement is slow if this vector set is not empty - // and/or it is not at the end of the nonzeros of the underlying matrix. - - // 1 - eval to a temporary to avoid transposition and/or aliasing issues - SparseMatrix tmp(other); - - // 2 - let's check whether there is enough allocated memory - Index nnz = tmp.nonZeros(); - Index start = m_outerStart==0 ? 0 : matrix.outerIndexPtr()[m_outerStart]; // starting position of the current block - Index end = m_matrix.outerIndexPtr()[m_outerStart+m_outerSize.value()]; // ending posiiton of the current block - Index block_size = end - start; // available room in the current block - Index tail_size = m_matrix.outerIndexPtr()[m_matrix.outerSize()] - end; - - Index free_size = m_matrix.isCompressed() - ? Index(matrix.data().allocatedSize()) + block_size - : block_size; - - if(nnz>free_size) - { - // realloc manually to reduce copies - typename SparseMatrixType::Storage newdata(m_matrix.data().allocatedSize() - block_size + nnz); - - std::memcpy(&newdata.value(0), &m_matrix.data().value(0), start*sizeof(Scalar)); - std::memcpy(&newdata.index(0), &m_matrix.data().index(0), start*sizeof(Index)); - - std::memcpy(&newdata.value(start), &tmp.data().value(0), nnz*sizeof(Scalar)); - std::memcpy(&newdata.index(start), &tmp.data().index(0), nnz*sizeof(Index)); - - std::memcpy(&newdata.value(start+nnz), &matrix.data().value(end), tail_size*sizeof(Scalar)); - std::memcpy(&newdata.index(start+nnz), &matrix.data().index(end), tail_size*sizeof(Index)); - - newdata.resize(m_matrix.outerIndexPtr()[m_matrix.outerSize()] - block_size + nnz); - - matrix.data().swap(newdata); - } - else - { - // no need to realloc, simply copy the tail at its respective position and insert tmp - matrix.data().resize(start + nnz + tail_size); - - std::memmove(&matrix.data().value(start+nnz), &matrix.data().value(end), tail_size*sizeof(Scalar)); - std::memmove(&matrix.data().index(start+nnz), &matrix.data().index(end), tail_size*sizeof(Index)); - - std::memcpy(&matrix.data().value(start), &tmp.data().value(0), nnz*sizeof(Scalar)); - std::memcpy(&matrix.data().index(start), &tmp.data().index(0), nnz*sizeof(Index)); - } - - // update innerNonZeros - if(!m_matrix.isCompressed()) - for(Index j=0; j(other); - } - - inline const Scalar* valuePtr() const - { return m_matrix.valuePtr() + m_matrix.outerIndexPtr()[m_outerStart]; } - inline Scalar* valuePtr() - { return m_matrix.const_cast_derived().valuePtr() + m_matrix.outerIndexPtr()[m_outerStart]; } - - inline const Index* innerIndexPtr() const - { return m_matrix.innerIndexPtr() + m_matrix.outerIndexPtr()[m_outerStart]; } - inline Index* innerIndexPtr() - { return m_matrix.const_cast_derived().innerIndexPtr() + m_matrix.outerIndexPtr()[m_outerStart]; } - - inline const Index* outerIndexPtr() const - { return m_matrix.outerIndexPtr() + m_outerStart; } - inline Index* outerIndexPtr() - { return m_matrix.const_cast_derived().outerIndexPtr() + m_outerStart; } - - Index nonZeros() const - { - if(m_matrix.isCompressed()) - return std::size_t(m_matrix.outerIndexPtr()[m_outerStart+m_outerSize.value()]) - - std::size_t(m_matrix.outerIndexPtr()[m_outerStart]); - else if(m_outerSize.value()==0) - return 0; - else - return Map >(m_matrix.innerNonZeroPtr()+m_outerStart, m_outerSize.value()).sum(); - } - - inline Scalar& coeffRef(int row, int col) - { - return m_matrix.const_cast_derived().coeffRef(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart)); - } - - inline const Scalar coeff(int row, int col) const - { - return m_matrix.coeff(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart)); - } - - inline const Scalar coeff(int index) const - { - return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index : m_outerStart); - } - - const Scalar& lastCoeff() const - { - EIGEN_STATIC_ASSERT_VECTOR_ONLY(BlockImpl); - eigen_assert(nonZeros()>0); - if(m_matrix.isCompressed()) - return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart+1]-1]; - else - return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart]+m_matrix.innerNonZeroPtr()[m_outerStart]-1]; - } - - EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); } - EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); } - - protected: - - typename SparseMatrixType::Nested m_matrix; - Index m_outerStart; - const internal::variable_if_dynamic m_outerSize; - -}; - - -template -class BlockImpl,BlockRows,BlockCols,true,Sparse> - : public SparseMatrixBase,BlockRows,BlockCols,true> > -{ - typedef SparseMatrix<_Scalar, _Options, _Index> SparseMatrixType; - typedef typename internal::remove_all::type _MatrixTypeNested; - typedef Block BlockType; -public: - enum { IsRowMajor = internal::traits::IsRowMajor }; - EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) -protected: - enum { OuterSize = IsRowMajor ? BlockRows : BlockCols }; -public: - - class InnerIterator: public SparseMatrixType::InnerIterator - { - public: - inline InnerIterator(const BlockType& xpr, Index outer) - : SparseMatrixType::InnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) - {} - inline Index row() const { return IsRowMajor ? m_outer : this->index(); } - inline Index col() const { return IsRowMajor ? this->index() : m_outer; } - protected: - Index m_outer; - }; - class ReverseInnerIterator: public SparseMatrixType::ReverseInnerIterator - { - public: - inline ReverseInnerIterator(const BlockType& xpr, Index outer) - : SparseMatrixType::ReverseInnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) - {} - inline Index row() const { return IsRowMajor ? m_outer : this->index(); } - inline Index col() const { return IsRowMajor ? this->index() : m_outer; } - protected: - Index m_outer; - }; - - inline BlockImpl(const SparseMatrixType& xpr, int i) - : m_matrix(xpr), m_outerStart(i), m_outerSize(OuterSize) - {} - - inline BlockImpl(const SparseMatrixType& xpr, int startRow, int startCol, int blockRows, int blockCols) - : m_matrix(xpr), m_outerStart(IsRowMajor ? startRow : startCol), m_outerSize(IsRowMajor ? blockRows : blockCols) - {} - - inline const Scalar* valuePtr() const - { return m_matrix.valuePtr() + m_matrix.outerIndexPtr()[m_outerStart]; } - - inline const Index* innerIndexPtr() const - { return m_matrix.innerIndexPtr() + m_matrix.outerIndexPtr()[m_outerStart]; } - - inline const Index* outerIndexPtr() const - { return m_matrix.outerIndexPtr() + m_outerStart; } - - Index nonZeros() const - { - if(m_matrix.isCompressed()) - return std::size_t(m_matrix.outerIndexPtr()[m_outerStart+m_outerSize.value()]) - - std::size_t(m_matrix.outerIndexPtr()[m_outerStart]); - else if(m_outerSize.value()==0) - return 0; - else - return Map >(m_matrix.innerNonZeroPtr()+m_outerStart, m_outerSize.value()).sum(); - } - - inline const Scalar coeff(int row, int col) const - { - return m_matrix.coeff(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart)); - } - - inline const Scalar coeff(int index) const - { - return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index : m_outerStart); - } - - const Scalar& lastCoeff() const - { - EIGEN_STATIC_ASSERT_VECTOR_ONLY(BlockImpl); - eigen_assert(nonZeros()>0); - if(m_matrix.isCompressed()) - return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart+1]-1]; - else - return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart]+m_matrix.innerNonZeroPtr()[m_outerStart]-1]; - } - - EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); } - EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); } - - protected: - - EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) - - typename SparseMatrixType::Nested m_matrix; - Index m_outerStart; - const internal::variable_if_dynamic m_outerSize; -}; - -//---------- - -/** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this - * is col-major (resp. row-major). - */ -template -typename SparseMatrixBase::InnerVectorReturnType SparseMatrixBase::innerVector(Index outer) -{ return InnerVectorReturnType(derived(), outer); } - -/** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this - * is col-major (resp. row-major). Read-only. - */ -template -const typename SparseMatrixBase::ConstInnerVectorReturnType SparseMatrixBase::innerVector(Index outer) const -{ return ConstInnerVectorReturnType(derived(), outer); } - -/** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this - * is col-major (resp. row-major). - */ -template -typename SparseMatrixBase::InnerVectorsReturnType -SparseMatrixBase::innerVectors(Index outerStart, Index outerSize) -{ - return Block(derived(), - IsRowMajor ? outerStart : 0, IsRowMajor ? 0 : outerStart, - IsRowMajor ? outerSize : rows(), IsRowMajor ? cols() : outerSize); - -} - -/** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this - * is col-major (resp. row-major). Read-only. - */ -template -const typename SparseMatrixBase::ConstInnerVectorsReturnType -SparseMatrixBase::innerVectors(Index outerStart, Index outerSize) const -{ - return Block(derived(), - IsRowMajor ? outerStart : 0, IsRowMajor ? 0 : outerStart, - IsRowMajor ? outerSize : rows(), IsRowMajor ? cols() : outerSize); - -} - -/** Generic implementation of sparse Block expression. - * Real-only. - */ -template -class BlockImpl - : public SparseMatrixBase >, internal::no_assignment_operator -{ - typedef typename internal::remove_all::type _MatrixTypeNested; - typedef Block BlockType; -public: - enum { IsRowMajor = internal::traits::IsRowMajor }; - EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) - - /** Column or Row constructor - */ - inline BlockImpl(const XprType& xpr, int i) - : m_matrix(xpr), - m_startRow( (BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) ? i : 0), - m_startCol( (BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) ? i : 0), - m_blockRows(BlockRows==1 ? 1 : xpr.rows()), - m_blockCols(BlockCols==1 ? 1 : xpr.cols()) - {} - - /** Dynamic-size constructor - */ - inline BlockImpl(const XprType& xpr, int startRow, int startCol, int blockRows, int blockCols) - : m_matrix(xpr), m_startRow(startRow), m_startCol(startCol), m_blockRows(blockRows), m_blockCols(blockCols) - {} - - inline int rows() const { return m_blockRows.value(); } - inline int cols() const { return m_blockCols.value(); } - - inline Scalar& coeffRef(int row, int col) - { - return m_matrix.const_cast_derived() - .coeffRef(row + m_startRow.value(), col + m_startCol.value()); - } - - inline const Scalar coeff(int row, int col) const - { - return m_matrix.coeff(row + m_startRow.value(), col + m_startCol.value()); - } - - inline Scalar& coeffRef(int index) - { - return m_matrix.const_cast_derived() - .coeffRef(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index), - m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0)); - } - - inline const Scalar coeff(int index) const - { - return m_matrix - .coeff(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index), - m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0)); - } - - inline const _MatrixTypeNested& nestedExpression() const { return m_matrix; } - - class InnerIterator : public _MatrixTypeNested::InnerIterator - { - typedef typename _MatrixTypeNested::InnerIterator Base; - const BlockType& m_block; - Index m_end; - public: - - EIGEN_STRONG_INLINE InnerIterator(const BlockType& block, Index outer) - : Base(block.derived().nestedExpression(), outer + (IsRowMajor ? block.m_startRow.value() : block.m_startCol.value())), - m_block(block), - m_end(IsRowMajor ? block.m_startCol.value()+block.m_blockCols.value() : block.m_startRow.value()+block.m_blockRows.value()) - { - while( (Base::operator bool()) && (Base::index() < (IsRowMajor ? m_block.m_startCol.value() : m_block.m_startRow.value())) ) - Base::operator++(); - } - - inline Index index() const { return Base::index() - (IsRowMajor ? m_block.m_startCol.value() : m_block.m_startRow.value()); } - inline Index outer() const { return Base::outer() - (IsRowMajor ? m_block.m_startRow.value() : m_block.m_startCol.value()); } - inline Index row() const { return Base::row() - m_block.m_startRow.value(); } - inline Index col() const { return Base::col() - m_block.m_startCol.value(); } - - inline operator bool() const { return Base::operator bool() && Base::index() < m_end; } - }; - class ReverseInnerIterator : public _MatrixTypeNested::ReverseInnerIterator - { - typedef typename _MatrixTypeNested::ReverseInnerIterator Base; - const BlockType& m_block; - Index m_begin; - public: - - EIGEN_STRONG_INLINE ReverseInnerIterator(const BlockType& block, Index outer) - : Base(block.derived().nestedExpression(), outer + (IsRowMajor ? block.m_startRow.value() : block.m_startCol.value())), - m_block(block), - m_begin(IsRowMajor ? block.m_startCol.value() : block.m_startRow.value()) - { - while( (Base::operator bool()) && (Base::index() >= (IsRowMajor ? m_block.m_startCol.value()+block.m_blockCols.value() : m_block.m_startRow.value()+block.m_blockRows.value())) ) - Base::operator--(); - } - - inline Index index() const { return Base::index() - (IsRowMajor ? m_block.m_startCol.value() : m_block.m_startRow.value()); } - inline Index outer() const { return Base::outer() - (IsRowMajor ? m_block.m_startRow.value() : m_block.m_startCol.value()); } - inline Index row() const { return Base::row() - m_block.m_startRow.value(); } - inline Index col() const { return Base::col() - m_block.m_startCol.value(); } - - inline operator bool() const { return Base::operator bool() && Base::index() >= m_begin; } - }; - protected: - friend class InnerIterator; - friend class ReverseInnerIterator; - - EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) - - typename XprType::Nested m_matrix; - const internal::variable_if_dynamic m_startRow; - const internal::variable_if_dynamic m_startCol; - const internal::variable_if_dynamic m_blockRows; - const internal::variable_if_dynamic m_blockCols; - private: - Index nonZeros() const; -}; - -} // end namespace Eigen - -#endif // EIGEN_SPARSE_BLOCK_H - +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008-2009 Gael Guennebaud +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_SPARSE_BLOCK_H +#define EIGEN_SPARSE_BLOCK_H + +namespace Eigen { + +template +class BlockImpl + : public SparseMatrixBase > +{ +public: + typedef Block BlockType; + enum { IsRowMajor = internal::traits::IsRowMajor }; +protected: + typedef typename internal::remove_all::type _MatrixTypeNested; + enum { OuterSize = IsRowMajor ? BlockRows : BlockCols }; +public: + EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) + + class InnerIterator: public XprType::InnerIterator + { + typedef typename BlockImpl::Index Index; + public: + inline InnerIterator(const Block& xpr, Index outer) + : XprType::InnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) + {} + inline Index row() const { return IsRowMajor ? m_outer : this->index(); } + inline Index col() const { return IsRowMajor ? this->index() : m_outer; } + protected: + Index m_outer; + }; + class ReverseInnerIterator: public XprType::ReverseInnerIterator + { + typedef typename BlockImpl::Index Index; + public: + inline ReverseInnerIterator(const BlockType& xpr, Index outer) + : XprType::ReverseInnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) + {} + inline Index row() const { return IsRowMajor ? m_outer : this->index(); } + inline Index col() const { return IsRowMajor ? this->index() : m_outer; } + protected: + Index m_outer; + }; + + inline BlockImpl(const XprType& xpr, int i) + : m_matrix(xpr), m_outerStart(i), m_outerSize(OuterSize) + {} + + inline BlockImpl(const XprType& xpr, int startRow, int startCol, int blockRows, int blockCols) + : m_matrix(xpr), m_outerStart(IsRowMajor ? startRow : startCol), m_outerSize(IsRowMajor ? blockRows : blockCols) + {} + + inline const Scalar coeff(int row, int col) const + { + return m_matrix.coeff(row + IsRowMajor ? m_outerStart : 0, col +IsRowMajor ? 0 : m_outerStart); + } + + inline const Scalar coeff(int index) const + { + return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index : m_outerStart); + } + + EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); } + EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); } + + protected: + + typename XprType::Nested m_matrix; + Index m_outerStart; + const internal::variable_if_dynamic m_outerSize; + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) + private: + Index nonZeros() const; +}; + + +/*************************************************************************** +* specialisation for SparseMatrix +***************************************************************************/ + +template +class BlockImpl,BlockRows,BlockCols,true,Sparse> + : public SparseMatrixBase,BlockRows,BlockCols,true> > +{ + typedef SparseMatrix<_Scalar, _Options, _Index> SparseMatrixType; + typedef typename internal::remove_all::type _MatrixTypeNested; + typedef Block ConstBlockType; +public: + typedef Block BlockType; + enum { IsRowMajor = internal::traits::IsRowMajor }; + EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) +protected: + enum { OuterSize = IsRowMajor ? BlockRows : BlockCols }; +public: + + class InnerIterator: public SparseMatrixType::InnerIterator + { + public: + inline InnerIterator(const BlockType& xpr, Index outer) + : SparseMatrixType::InnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) + {} + inline Index row() const { return IsRowMajor ? m_outer : this->index(); } + inline Index col() const { return IsRowMajor ? this->index() : m_outer; } + protected: + Index m_outer; + }; + class ReverseInnerIterator: public SparseMatrixType::ReverseInnerIterator + { + public: + inline ReverseInnerIterator(const BlockType& xpr, Index outer) + : SparseMatrixType::ReverseInnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) + {} + inline Index row() const { return IsRowMajor ? m_outer : this->index(); } + inline Index col() const { return IsRowMajor ? this->index() : m_outer; } + protected: + Index m_outer; + }; + + inline BlockImpl(const SparseMatrixType& xpr, int i) + : m_matrix(xpr), m_outerStart(i), m_outerSize(OuterSize) + {} + + inline BlockImpl(const SparseMatrixType& xpr, int startRow, int startCol, int blockRows, int blockCols) + : m_matrix(xpr), m_outerStart(IsRowMajor ? startRow : startCol), m_outerSize(IsRowMajor ? blockRows : blockCols) + {} + + template + inline BlockType& operator=(const SparseMatrixBase& other) + { + typedef typename internal::remove_all::type _NestedMatrixType; + _NestedMatrixType& matrix = const_cast<_NestedMatrixType&>(m_matrix);; + // This assignement is slow if this vector set is not empty + // and/or it is not at the end of the nonzeros of the underlying matrix. + + // 1 - eval to a temporary to avoid transposition and/or aliasing issues + SparseMatrix tmp(other); + + // 2 - let's check whether there is enough allocated memory + Index nnz = tmp.nonZeros(); + Index start = m_outerStart==0 ? 0 : matrix.outerIndexPtr()[m_outerStart]; // starting position of the current block + Index end = m_matrix.outerIndexPtr()[m_outerStart+m_outerSize.value()]; // ending posiiton of the current block + Index block_size = end - start; // available room in the current block + Index tail_size = m_matrix.outerIndexPtr()[m_matrix.outerSize()] - end; + + Index free_size = m_matrix.isCompressed() + ? Index(matrix.data().allocatedSize()) + block_size + : block_size; + + if(nnz>free_size) + { + // realloc manually to reduce copies + typename SparseMatrixType::Storage newdata(m_matrix.data().allocatedSize() - block_size + nnz); + + std::memcpy(newdata.valuePtr(), m_matrix.data().valuePtr(), start*sizeof(Scalar)); + std::memcpy(newdata.indexPtr(), m_matrix.data().indexPtr(), start*sizeof(Index)); + + std::memcpy(newdata.valuePtr() + start, tmp.data().valuePtr(), nnz*sizeof(Scalar)); + std::memcpy(newdata.indexPtr() + start, tmp.data().indexPtr(), nnz*sizeof(Index)); + + std::memcpy(newdata.valuePtr()+start+nnz, matrix.data().valuePtr()+end, tail_size*sizeof(Scalar)); + std::memcpy(newdata.indexPtr()+start+nnz, matrix.data().indexPtr()+end, tail_size*sizeof(Index)); + + newdata.resize(m_matrix.outerIndexPtr()[m_matrix.outerSize()] - block_size + nnz); + + matrix.data().swap(newdata); + } + else + { + // no need to realloc, simply copy the tail at its respective position and insert tmp + matrix.data().resize(start + nnz + tail_size); + + std::memmove(matrix.data().valuePtr()+start+nnz, matrix.data().valuePtr()+end, tail_size*sizeof(Scalar)); + std::memmove(matrix.data().indexPtr()+start+nnz, matrix.data().indexPtr()+end, tail_size*sizeof(Index)); + + std::memcpy(matrix.data().valuePtr()+start, tmp.data().valuePtr(), nnz*sizeof(Scalar)); + std::memcpy(matrix.data().indexPtr()+start, tmp.data().indexPtr(), nnz*sizeof(Index)); + } + + // update innerNonZeros + if(!m_matrix.isCompressed()) + for(Index j=0; j(other); + } + + inline const Scalar* valuePtr() const + { return m_matrix.valuePtr() + m_matrix.outerIndexPtr()[m_outerStart]; } + inline Scalar* valuePtr() + { return m_matrix.const_cast_derived().valuePtr() + m_matrix.outerIndexPtr()[m_outerStart]; } + + inline const Index* innerIndexPtr() const + { return m_matrix.innerIndexPtr() + m_matrix.outerIndexPtr()[m_outerStart]; } + inline Index* innerIndexPtr() + { return m_matrix.const_cast_derived().innerIndexPtr() + m_matrix.outerIndexPtr()[m_outerStart]; } + + inline const Index* outerIndexPtr() const + { return m_matrix.outerIndexPtr() + m_outerStart; } + inline Index* outerIndexPtr() + { return m_matrix.const_cast_derived().outerIndexPtr() + m_outerStart; } + + Index nonZeros() const + { + if(m_matrix.isCompressed()) + return std::size_t(m_matrix.outerIndexPtr()[m_outerStart+m_outerSize.value()]) + - std::size_t(m_matrix.outerIndexPtr()[m_outerStart]); + else if(m_outerSize.value()==0) + return 0; + else + return Map >(m_matrix.innerNonZeroPtr()+m_outerStart, m_outerSize.value()).sum(); + } + + inline Scalar& coeffRef(int row, int col) + { + return m_matrix.const_cast_derived().coeffRef(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart)); + } + + inline const Scalar coeff(int row, int col) const + { + return m_matrix.coeff(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart)); + } + + inline const Scalar coeff(int index) const + { + return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index : m_outerStart); + } + + const Scalar& lastCoeff() const + { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(BlockImpl); + eigen_assert(nonZeros()>0); + if(m_matrix.isCompressed()) + return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart+1]-1]; + else + return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart]+m_matrix.innerNonZeroPtr()[m_outerStart]-1]; + } + + EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); } + EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); } + + protected: + + typename SparseMatrixType::Nested m_matrix; + Index m_outerStart; + const internal::variable_if_dynamic m_outerSize; + +}; + + +template +class BlockImpl,BlockRows,BlockCols,true,Sparse> + : public SparseMatrixBase,BlockRows,BlockCols,true> > +{ + typedef SparseMatrix<_Scalar, _Options, _Index> SparseMatrixType; + typedef typename internal::remove_all::type _MatrixTypeNested; +public: + typedef Block BlockType; + enum { IsRowMajor = internal::traits::IsRowMajor }; + EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) +protected: + enum { OuterSize = IsRowMajor ? BlockRows : BlockCols }; +public: + + class InnerIterator: public SparseMatrixType::InnerIterator + { + public: + inline InnerIterator(const BlockType& xpr, Index outer) + : SparseMatrixType::InnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) + {} + inline Index row() const { return IsRowMajor ? m_outer : this->index(); } + inline Index col() const { return IsRowMajor ? this->index() : m_outer; } + protected: + Index m_outer; + }; + class ReverseInnerIterator: public SparseMatrixType::ReverseInnerIterator + { + public: + inline ReverseInnerIterator(const BlockType& xpr, Index outer) + : SparseMatrixType::ReverseInnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer) + {} + inline Index row() const { return IsRowMajor ? m_outer : this->index(); } + inline Index col() const { return IsRowMajor ? this->index() : m_outer; } + protected: + Index m_outer; + }; + + inline BlockImpl(const SparseMatrixType& xpr, int i) + : m_matrix(xpr), m_outerStart(i), m_outerSize(OuterSize) + {} + + inline BlockImpl(const SparseMatrixType& xpr, int startRow, int startCol, int blockRows, int blockCols) + : m_matrix(xpr), m_outerStart(IsRowMajor ? startRow : startCol), m_outerSize(IsRowMajor ? blockRows : blockCols) + {} + + inline const Scalar* valuePtr() const + { return m_matrix.valuePtr() + m_matrix.outerIndexPtr()[m_outerStart]; } + + inline const Index* innerIndexPtr() const + { return m_matrix.innerIndexPtr() + m_matrix.outerIndexPtr()[m_outerStart]; } + + inline const Index* outerIndexPtr() const + { return m_matrix.outerIndexPtr() + m_outerStart; } + + Index nonZeros() const + { + if(m_matrix.isCompressed()) + return std::size_t(m_matrix.outerIndexPtr()[m_outerStart+m_outerSize.value()]) + - std::size_t(m_matrix.outerIndexPtr()[m_outerStart]); + else if(m_outerSize.value()==0) + return 0; + else + return Map >(m_matrix.innerNonZeroPtr()+m_outerStart, m_outerSize.value()).sum(); + } + + inline const Scalar coeff(int row, int col) const + { + return m_matrix.coeff(row + (IsRowMajor ? m_outerStart : 0), col + (IsRowMajor ? 0 : m_outerStart)); + } + + inline const Scalar coeff(int index) const + { + return m_matrix.coeff(IsRowMajor ? m_outerStart : index, IsRowMajor ? index : m_outerStart); + } + + const Scalar& lastCoeff() const + { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(BlockImpl); + eigen_assert(nonZeros()>0); + if(m_matrix.isCompressed()) + return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart+1]-1]; + else + return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart]+m_matrix.innerNonZeroPtr()[m_outerStart]-1]; + } + + EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); } + EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); } + + protected: + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) + + typename SparseMatrixType::Nested m_matrix; + Index m_outerStart; + const internal::variable_if_dynamic m_outerSize; +}; + +//---------- + +/** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this + * is col-major (resp. row-major). + */ +template +typename SparseMatrixBase::InnerVectorReturnType SparseMatrixBase::innerVector(Index outer) +{ return InnerVectorReturnType(derived(), outer); } + +/** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this + * is col-major (resp. row-major). Read-only. + */ +template +const typename SparseMatrixBase::ConstInnerVectorReturnType SparseMatrixBase::innerVector(Index outer) const +{ return ConstInnerVectorReturnType(derived(), outer); } + +/** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this + * is col-major (resp. row-major). + */ +template +typename SparseMatrixBase::InnerVectorsReturnType +SparseMatrixBase::innerVectors(Index outerStart, Index outerSize) +{ + return Block(derived(), + IsRowMajor ? outerStart : 0, IsRowMajor ? 0 : outerStart, + IsRowMajor ? outerSize : rows(), IsRowMajor ? cols() : outerSize); + +} + +/** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this + * is col-major (resp. row-major). Read-only. + */ +template +const typename SparseMatrixBase::ConstInnerVectorsReturnType +SparseMatrixBase::innerVectors(Index outerStart, Index outerSize) const +{ + return Block(derived(), + IsRowMajor ? outerStart : 0, IsRowMajor ? 0 : outerStart, + IsRowMajor ? outerSize : rows(), IsRowMajor ? cols() : outerSize); + +} + +namespace internal { + +template< typename XprType, int BlockRows, int BlockCols, bool InnerPanel, + bool OuterVector = (BlockCols==1 && XprType::IsRowMajor) || (BlockRows==1 && !XprType::IsRowMajor)> +class GenericSparseBlockInnerIteratorImpl; + +} + +/** Generic implementation of sparse Block expression. + * Real-only. + */ +template +class BlockImpl + : public SparseMatrixBase >, internal::no_assignment_operator +{ + typedef typename internal::remove_all::type _MatrixTypeNested; + public: + typedef Block BlockType; + enum { IsRowMajor = internal::traits::IsRowMajor }; + EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType) + + /** Column or Row constructor + */ + inline BlockImpl(const XprType& xpr, int i) + : m_matrix(xpr), + m_startRow( (BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) ? i : 0), + m_startCol( (BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) ? i : 0), + m_blockRows(BlockRows==1 ? 1 : xpr.rows()), + m_blockCols(BlockCols==1 ? 1 : xpr.cols()) + {} + + /** Dynamic-size constructor + */ + inline BlockImpl(const XprType& xpr, int startRow, int startCol, int blockRows, int blockCols) + : m_matrix(xpr), m_startRow(startRow), m_startCol(startCol), m_blockRows(blockRows), m_blockCols(blockCols) + {} + + inline int rows() const { return m_blockRows.value(); } + inline int cols() const { return m_blockCols.value(); } + + inline Scalar& coeffRef(int row, int col) + { + return m_matrix.const_cast_derived() + .coeffRef(row + m_startRow.value(), col + m_startCol.value()); + } + + inline const Scalar coeff(int row, int col) const + { + return m_matrix.coeff(row + m_startRow.value(), col + m_startCol.value()); + } + + inline Scalar& coeffRef(int index) + { + return m_matrix.const_cast_derived() + .coeffRef(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index), + m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0)); + } + + inline const Scalar coeff(int index) const + { + return m_matrix + .coeff(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index), + m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0)); + } + + inline const _MatrixTypeNested& nestedExpression() const { return m_matrix; } + + typedef internal::GenericSparseBlockInnerIteratorImpl InnerIterator; + + class ReverseInnerIterator : public _MatrixTypeNested::ReverseInnerIterator + { + typedef typename _MatrixTypeNested::ReverseInnerIterator Base; + const BlockType& m_block; + Index m_begin; + public: + + EIGEN_STRONG_INLINE ReverseInnerIterator(const BlockType& block, Index outer) + : Base(block.derived().nestedExpression(), outer + (IsRowMajor ? block.m_startRow.value() : block.m_startCol.value())), + m_block(block), + m_begin(IsRowMajor ? block.m_startCol.value() : block.m_startRow.value()) + { + while( (Base::operator bool()) && (Base::index() >= (IsRowMajor ? m_block.m_startCol.value()+block.m_blockCols.value() : m_block.m_startRow.value()+block.m_blockRows.value())) ) + Base::operator--(); + } + + inline Index index() const { return Base::index() - (IsRowMajor ? m_block.m_startCol.value() : m_block.m_startRow.value()); } + inline Index outer() const { return Base::outer() - (IsRowMajor ? m_block.m_startRow.value() : m_block.m_startCol.value()); } + inline Index row() const { return Base::row() - m_block.m_startRow.value(); } + inline Index col() const { return Base::col() - m_block.m_startCol.value(); } + + inline operator bool() const { return Base::operator bool() && Base::index() >= m_begin; } + }; + protected: + friend class internal::GenericSparseBlockInnerIteratorImpl; + friend class ReverseInnerIterator; + + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) + + typename XprType::Nested m_matrix; + const internal::variable_if_dynamic m_startRow; + const internal::variable_if_dynamic m_startCol; + const internal::variable_if_dynamic m_blockRows; + const internal::variable_if_dynamic m_blockCols; + private: + Index nonZeros() const; +}; + +namespace internal { + template + class GenericSparseBlockInnerIteratorImpl : public internal::remove_all::type::InnerIterator + { + public: + typedef Block BlockType; + enum { + IsRowMajor = BlockType::IsRowMajor + }; + typedef typename BlockType::Index Index; + protected: + typedef typename internal::remove_all::type _MatrixTypeNested; + typedef typename _MatrixTypeNested::InnerIterator Base; + const BlockType& m_block; + Index m_end; + public: + + EIGEN_STRONG_INLINE GenericSparseBlockInnerIteratorImpl(const BlockType& block, Index outer) + : Base(block.derived().nestedExpression(), outer + (IsRowMajor ? block.m_startRow.value() : block.m_startCol.value())), + m_block(block), + m_end(IsRowMajor ? block.m_startCol.value()+block.m_blockCols.value() : block.m_startRow.value()+block.m_blockRows.value()) + { + while( (Base::operator bool()) && (Base::index() < (IsRowMajor ? m_block.m_startCol.value() : m_block.m_startRow.value())) ) + Base::operator++(); + } + + inline Index index() const { return Base::index() - (IsRowMajor ? m_block.m_startCol.value() : m_block.m_startRow.value()); } + inline Index outer() const { return Base::outer() - (IsRowMajor ? m_block.m_startRow.value() : m_block.m_startCol.value()); } + inline Index row() const { return Base::row() - m_block.m_startRow.value(); } + inline Index col() const { return Base::col() - m_block.m_startCol.value(); } + + inline operator bool() const { return Base::operator bool() && Base::index() < m_end; } + }; + + // Row vector of a column-major sparse matrix or column of a row-major one. + template + class GenericSparseBlockInnerIteratorImpl + { + public: + typedef Block BlockType; + enum { + IsRowMajor = BlockType::IsRowMajor + }; + typedef typename BlockType::Index Index; + typedef typename BlockType::Scalar Scalar; + protected: + typedef typename internal::remove_all::type _MatrixTypeNested; + const BlockType& m_block; + Index m_outerPos; + Index m_innerIndex; + Scalar m_value; + Index m_end; + public: + + EIGEN_STRONG_INLINE GenericSparseBlockInnerIteratorImpl(const BlockType& block, Index outer = 0) + : + m_block(block), + m_outerPos( (IsRowMajor ? block.m_startCol.value() : block.m_startRow.value()) - 1), // -1 so that operator++ finds the first non-zero entry + m_innerIndex(IsRowMajor ? block.m_startRow.value() : block.m_startCol.value()), + m_end(IsRowMajor ? block.m_startCol.value()+block.m_blockCols.value() : block.m_startRow.value()+block.m_blockRows.value()) + { + EIGEN_UNUSED_VARIABLE(outer); + eigen_assert(outer==0); + + ++(*this); + } + + inline Index index() const { return m_outerPos - (IsRowMajor ? m_block.m_startCol.value() : m_block.m_startRow.value()); } + inline Index outer() const { return 0; } + inline Index row() const { return IsRowMajor ? 0 : index(); } + inline Index col() const { return IsRowMajor ? index() : 0; } + + inline Scalar value() const { return m_value; } + + inline GenericSparseBlockInnerIteratorImpl& operator++() + { + // search next non-zero entry + while(m_outerPosm_data.resize(rows()); - Eigen::Map >(&this->m_data.index(0), rows()).setLinSpaced(0, rows()-1); - Eigen::Map >(&this->m_data.value(0), rows()).setOnes(); + Eigen::Map >(this->m_data.indexPtr(), rows()).setLinSpaced(0, rows()-1); + Eigen::Map >(this->m_data.valuePtr(), rows()).setOnes(); Eigen::Map >(this->m_outerIndex, rows()+1).setLinSpaced(0, rows()); std::free(m_innerNonZeros); m_innerNonZeros = 0; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h index 9341d9ad2..6f4a47cf5 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h @@ -38,6 +38,7 @@ template class SparseMatrixBase typedef typename internal::packet_traits::type PacketScalar; typedef typename internal::traits::StorageKind StorageKind; typedef typename internal::traits::Index Index; + typedef typename internal::traits::Index StorageIndex; typedef typename internal::add_const_on_value_type_if_arithmetic< typename internal::packet_traits::type >::type PacketReturnType; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseRedux.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseRedux.h index f3da93a71..51ed9aeb1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseRedux.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseRedux.h @@ -29,7 +29,10 @@ typename internal::traits >::Scalar SparseMatrix<_Scalar,_Options,_Index>::sum() const { eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); - return Matrix::Map(&m_data.value(0), m_data.size()).sum(); + if(this->isCompressed()) + return Matrix::Map(m_data.valuePtr(), m_data.size()).sum(); + else + return Base::sum(); } template @@ -37,7 +40,7 @@ typename internal::traits >::Scalar SparseVector<_Scalar,_Options,_Index>::sum() const { eigen_assert(rows()>0 && cols()>0 && "you are using a non initialized matrix"); - return Matrix::Map(&m_data.value(0), m_data.size()).sum(); + return Matrix::Map(m_data.valuePtr(), m_data.size()).sum(); } } // end namespace Eigen diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h index fcc18f5c9..55b84a4eb 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h @@ -48,7 +48,7 @@ static void sparse_sparse_product_with_pruning_impl(const Lhs& lhs, const Rhs& r res.resize(rows, cols); res.reserve(estimated_nnz_prod); - double ratioColRes = double(estimated_nnz_prod)/double(lhs.rows()*rhs.cols()); + double ratioColRes = double(estimated_nnz_prod)/(double(lhs.rows())*double(rhs.cols())); for (Index j=0; j + UmfPackLU(const InputMatrixType& matrix) { init(); compute(matrix); diff --git a/gtsam/3rdparty/Eigen/bench/btl/generic_bench/btl.hh b/gtsam/3rdparty/Eigen/bench/btl/generic_bench/btl.hh index f1a88ff74..86a8438cd 100644 --- a/gtsam/3rdparty/Eigen/bench/btl/generic_bench/btl.hh +++ b/gtsam/3rdparty/Eigen/bench/btl/generic_bench/btl.hh @@ -44,15 +44,10 @@ #define BTL_ASM_COMMENT(X) #endif -#if (defined __GNUC__) && (!defined __INTEL_COMPILER) && !defined(__arm__) && !defined(__powerpc__) -#define BTL_DISABLE_SSE_EXCEPTIONS() { \ - int aux; \ - asm( \ - "stmxcsr %[aux] \n\t" \ - "orl $32832, %[aux] \n\t" \ - "ldmxcsr %[aux] \n\t" \ - : : [aux] "m" (aux)); \ -} +#ifdef __SSE__ +#include "xmmintrin.h" +// This enables flush to zero (FTZ) and denormals are zero (DAZ) modes: +#define BTL_DISABLE_SSE_EXCEPTIONS() { _mm_setcsr(_mm_getcsr() | 0x8040); } #else #define BTL_DISABLE_SSE_EXCEPTIONS() #endif diff --git a/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake b/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake index 2b11d8360..b5b62aee6 100644 --- a/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake +++ b/gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake @@ -1,7 +1,7 @@ include(EigenTesting) include(CheckCXXSourceCompiles) -# configure the "site" and "buildname" +# configure the "site" and "buildname" ei_set_sitename() # retrieve and store the build string @@ -14,17 +14,10 @@ add_dependencies(check buildtests) # check whether /bin/bash exists find_file(EIGEN_BIN_BASH_EXISTS "/bin/bash" PATHS "/" NO_DEFAULT_PATH) -# CMake/Ctest does not allow us to change the build command, -# so we have to workaround by directly editing the generated DartConfiguration.tcl file -# save CMAKE_MAKE_PROGRAM -set(CMAKE_MAKE_PROGRAM_SAVE ${CMAKE_MAKE_PROGRAM}) -# and set a fake one -set(CMAKE_MAKE_PROGRAM "@EIGEN_MAKECOMMAND_PLACEHOLDER@") - # This call activates testing and generates the DartConfiguration.tcl include(CTest) -set(EIGEN_TEST_BUILD_FLAGS " " CACHE STRING "Options passed to the build command of unit tests") +set(EIGEN_TEST_BUILD_FLAGS "" CACHE STRING "Options passed to the build command of unit tests") # Overwrite default DartConfiguration.tcl such that ctest can build our unit tests. # Recall that our unit tests are not in the "all" target, so we have to explicitely ask ctest to build our custom 'buildtests' target. diff --git a/gtsam/3rdparty/Eigen/cmake/EigenDetermineOSVersion.cmake b/gtsam/3rdparty/Eigen/cmake/EigenDetermineOSVersion.cmake index 3c48d4c37..9246fa67c 100644 --- a/gtsam/3rdparty/Eigen/cmake/EigenDetermineOSVersion.cmake +++ b/gtsam/3rdparty/Eigen/cmake/EigenDetermineOSVersion.cmake @@ -26,7 +26,7 @@ function(DetermineShortWindowsName WIN_VERSION win_num_version) endfunction() function(DetermineOSVersion OS_VERSION) - if (WIN32) + if (WIN32 AND CMAKE_HOST_SYSTEM_NAME MATCHES Windows) file (TO_NATIVE_PATH "$ENV{COMSPEC}" SHELL) exec_program( ${SHELL} ARGS "/c" "ver" OUTPUT_VARIABLE ver_output) diff --git a/gtsam/3rdparty/Eigen/doc/SparseLinearSystems.dox b/gtsam/3rdparty/Eigen/doc/SparseLinearSystems.dox index 051a02ed9..b66e2ba2b 100644 --- a/gtsam/3rdparty/Eigen/doc/SparseLinearSystems.dox +++ b/gtsam/3rdparty/Eigen/doc/SparseLinearSystems.dox @@ -46,6 +46,9 @@ They are summarized in the following table: SPQR\link SPQRSupport_Module SPQRSupport \endlink QR factorization Any, rectangularfill-in reducing, multithreaded, fast dense algebra requires the SuiteSparse package, \b GPL recommended for linear least-squares problems, has a rank-revealing feature +PardisoLLT \n PardisoLDLT \n PardisoLU\link PardisoSupport_Module PardisoSupport \endlinkDirect LLt, LDLt, LU factorizationsSPD \n SPD \n SquareFill-in reducing, Leverage fast dense algebra, Multithreading + Requires the Intel MKL package, \b Proprietary + optimized for tough problems patterns, see also \link TopicUsingIntelMKL using MKL with Eigen \endlink Here \c SPD means symmetric positive definite. diff --git a/gtsam/3rdparty/Eigen/doc/TopicAssertions.dox b/gtsam/3rdparty/Eigen/doc/TopicAssertions.dox index 4ead40174..c8b4d84f2 100644 --- a/gtsam/3rdparty/Eigen/doc/TopicAssertions.dox +++ b/gtsam/3rdparty/Eigen/doc/TopicAssertions.dox @@ -16,7 +16,7 @@ Both eigen_assert and eigen_plain_assert are defined in Macros.h. Defining eigen #include #undef eigen_assert #define eigen_assert(x) \ - if (!x) { throw (std::runtime_error("Put your message here")); } + if (!(x)) { throw (std::runtime_error("Put your message here")); } \endcode \subsection DisableAssert Disabling assertions diff --git a/gtsam/3rdparty/Eigen/test/CMakeLists.txt b/gtsam/3rdparty/Eigen/test/CMakeLists.txt index c0d8a4e28..40c8f669d 100644 --- a/gtsam/3rdparty/Eigen/test/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/test/CMakeLists.txt @@ -125,6 +125,7 @@ endif(TEST_LIB) set_property(GLOBAL PROPERTY EIGEN_CURRENT_SUBPROJECT "Official") add_custom_target(BuildOfficial) +ei_add_test(rand) ei_add_test(meta) ei_add_test(sizeof) ei_add_test(dynalloc) diff --git a/gtsam/3rdparty/Eigen/test/commainitializer.cpp b/gtsam/3rdparty/Eigen/test/commainitializer.cpp index 99102b966..296592340 100644 --- a/gtsam/3rdparty/Eigen/test/commainitializer.cpp +++ b/gtsam/3rdparty/Eigen/test/commainitializer.cpp @@ -9,14 +9,69 @@ #include "main.h" + +template +void test_blocks() +{ + Matrix m_fixed; + MatrixXi m_dynamic(M1+M2, N1+N2); + + Matrix mat11; mat11.setRandom(); + Matrix mat12; mat12.setRandom(); + Matrix mat21; mat21.setRandom(); + Matrix mat22; mat22.setRandom(); + + MatrixXi matx11 = mat11, matx12 = mat12, matx21 = mat21, matx22 = mat22; + + { + VERIFY_IS_EQUAL((m_fixed << mat11, mat12, mat21, matx22).finished(), (m_dynamic << mat11, matx12, mat21, matx22).finished()); + VERIFY_IS_EQUAL((m_fixed.template topLeftCorner()), mat11); + VERIFY_IS_EQUAL((m_fixed.template topRightCorner()), mat12); + VERIFY_IS_EQUAL((m_fixed.template bottomLeftCorner()), mat21); + VERIFY_IS_EQUAL((m_fixed.template bottomRightCorner()), mat22); + VERIFY_IS_EQUAL((m_fixed << mat12, mat11, matx21, mat22).finished(), (m_dynamic << mat12, matx11, matx21, mat22).finished()); + } + + if(N1 > 0) + { + VERIFY_RAISES_ASSERT((m_fixed << mat11, mat12, mat11, mat21, mat22)); + VERIFY_RAISES_ASSERT((m_fixed << mat11, mat12, mat21, mat21, mat22)); + } + else + { + // allow insertion of zero-column blocks: + VERIFY_IS_EQUAL((m_fixed << mat11, mat12, mat11, mat11, mat21, mat21, mat22).finished(), (m_dynamic << mat12, mat22).finished()); + } + if(M1 != M2) + { + VERIFY_RAISES_ASSERT((m_fixed << mat11, mat21, mat12, mat22)); + } +} + + +template +struct test_block_recursion +{ + static void run() + { + test_blocks<(N>>6)&3, (N>>4)&3, (N>>2)&3, N & 3>(); + test_block_recursion::run(); + } +}; + +template<> +struct test_block_recursion<-1> +{ + static void run() { } +}; + void test_commainitializer() { Matrix3d m3; Matrix4d m4; - VERIFY_RAISES_ASSERT( (m3 << 1, 2, 3, 4, 5, 6, 7, 8) ); - #ifndef _MSC_VER + VERIFY_RAISES_ASSERT( (m3 << 1, 2, 3, 4, 5, 6, 7, 8) ); VERIFY_RAISES_ASSERT( (m3 << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10) ); #endif @@ -43,4 +98,7 @@ void test_commainitializer() 4, 5, 6, vec[2].transpose(); VERIFY_IS_APPROX(m3, ref); + + // recursively test all block-sizes from 0 to 3: + test_block_recursion<(1<<8) - 1>(); } diff --git a/gtsam/3rdparty/Eigen/test/cwiseop.cpp b/gtsam/3rdparty/Eigen/test/cwiseop.cpp index e3361da17..d13002cae 100644 --- a/gtsam/3rdparty/Eigen/test/cwiseop.cpp +++ b/gtsam/3rdparty/Eigen/test/cwiseop.cpp @@ -164,9 +164,12 @@ template void cwiseops(const MatrixType& m) VERIFY( (m1.cwise().min(m2).cwise() < (m1+mones)).all() ); VERIFY( (m1.cwise().max(m2).cwise() > (m1-mones)).all() ); +#if(__cplusplus < 201103L) +// std::binder* are deprecated since c++11 and will be removed in c++17 VERIFY( (m1.cwise()(), Scalar(1)))).all() ); VERIFY( !(m1.cwise()(), Scalar(1)))).all() ); VERIFY( !(m1.cwise()>m1bis.unaryExpr(bind2nd(plus(), Scalar(1)))).any() ); +#endif cwiseops_real_only(m1, m2, m3, mones); } diff --git a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp index 22e1cc342..a36edd473 100644 --- a/gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp +++ b/gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp @@ -137,9 +137,12 @@ template void cwiseops(const MatrixType& m) VERIFY( (m1.cwise().min(m2).cwise() < (m1+mones)).all() ); VERIFY( (m1.cwise().max(m2).cwise() > (m1-mones)).all() ); +#if(__cplusplus < 201103L) +// std::binder* are deprecated since c++11 and will be removed in c++17 VERIFY( (m1.cwise()(), Scalar(1)))).all() ); VERIFY( !(m1.cwise()(), Scalar(1)))).all() ); VERIFY( !(m1.cwise()>m1.unaryExpr(bind2nd(plus(), Scalar(1)))).any() ); +#endif } void test_eigen2_cwiseop() diff --git a/gtsam/3rdparty/Eigen/test/geo_homogeneous.cpp b/gtsam/3rdparty/Eigen/test/geo_homogeneous.cpp index c91bde819..01330308a 100644 --- a/gtsam/3rdparty/Eigen/test/geo_homogeneous.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_homogeneous.cpp @@ -54,6 +54,8 @@ template void homogeneous(void) T2MatrixType t2 = T2MatrixType::Random(); VERIFY_IS_APPROX(t2 * (v0.homogeneous().eval()), t2 * v0.homogeneous()); VERIFY_IS_APPROX(t2 * (m0.colwise().homogeneous().eval()), t2 * m0.colwise().homogeneous()); + VERIFY_IS_APPROX(t2 * (v0.homogeneous().asDiagonal()), t2 * hv0.asDiagonal()); + VERIFY_IS_APPROX((v0.homogeneous().asDiagonal()) * t2, hv0.asDiagonal() * t2); VERIFY_IS_APPROX((v0.transpose().rowwise().homogeneous().eval()) * t2, v0.transpose().rowwise().homogeneous() * t2); diff --git a/gtsam/3rdparty/Eigen/test/geo_transformations.cpp b/gtsam/3rdparty/Eigen/test/geo_transformations.cpp index 547765714..383c42bad 100644 --- a/gtsam/3rdparty/Eigen/test/geo_transformations.cpp +++ b/gtsam/3rdparty/Eigen/test/geo_transformations.cpp @@ -320,6 +320,9 @@ template void transformations() t0.scale(v0); t1 *= AlignedScaling3(v0); VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); + t1 = AlignedScaling3(v0) * (Translation3(v0) * Transform3(q1)); + t1 = t1 * v0.asDiagonal(); + VERIFY_IS_APPROX(t0.matrix(), t1.matrix()); // transformation * translation t0.translate(v0); t1 = t1 * Translation3(v0); @@ -437,6 +440,79 @@ template void transformations() Rotation2D r2(r1); // copy ctor VERIFY_IS_APPROX(r2.angle(),s0); } + + { + Transform3 t32(Matrix4::Random()), t33, t34; + t34 = t33 = t32; + t32.scale(v0); + t33*=AlignedScaling3(v0); + VERIFY_IS_APPROX(t32.matrix(), t33.matrix()); + t33 = t34 * AlignedScaling3(v0); + VERIFY_IS_APPROX(t32.matrix(), t33.matrix()); + } + +} + +template +void transform_associativity_left(const A1& a1, const A2& a2, const P& p, const Q& q, const V& v, const H& h) +{ + VERIFY_IS_APPROX( q*(a1*v), (q*a1)*v ); + VERIFY_IS_APPROX( q*(a2*v), (q*a2)*v ); + VERIFY_IS_APPROX( q*(p*h).hnormalized(), ((q*p)*h).hnormalized() ); +} + +template +void transform_associativity2(const A1& a1, const A2& a2, const P& p, const Q& q, const V& v, const H& h) +{ + VERIFY_IS_APPROX( a1*(q*v), (a1*q)*v ); + VERIFY_IS_APPROX( a2*(q*v), (a2*q)*v ); + VERIFY_IS_APPROX( p *(q*v).homogeneous(), (p *q)*v.homogeneous() ); + + transform_associativity_left(a1, a2,p, q, v, h); +} + +template +void transform_associativity(const RotationType& R) +{ + typedef Matrix VectorType; + typedef Matrix HVectorType; + typedef Matrix LinearType; + typedef Matrix MatrixType; + typedef Transform AffineCompactType; + typedef Transform AffineType; + typedef Transform ProjectiveType; + typedef DiagonalMatrix ScalingType; + typedef Translation TranslationType; + + AffineCompactType A1c; A1c.matrix().setRandom(); + AffineCompactType A2c; A2c.matrix().setRandom(); + AffineType A1(A1c); + AffineType A2(A2c); + ProjectiveType P1; P1.matrix().setRandom(); + VectorType v1 = VectorType::Random(); + VectorType v2 = VectorType::Random(); + HVectorType h1 = HVectorType::Random(); + Scalar s1 = internal::random(); + LinearType L = LinearType::Random(); + MatrixType M = MatrixType::Random(); + + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, A2, v2, h1) ); + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, A2c, v2, h1) ); + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, v1.asDiagonal(), v2, h1) ); + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, ScalingType(v1), v2, h1) ); + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, Scaling(v1), v2, h1) ); + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, Scaling(s1), v2, h1) ); + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, TranslationType(v1), v2, h1) ); + CALL_SUBTEST( transform_associativity_left(A1c, A1, P1, L, v2, h1) ); + CALL_SUBTEST( transform_associativity2(A1c, A1, P1, R, v2, h1) ); + + VERIFY_IS_APPROX( A1*(M*h1), (A1*M)*h1 ); + VERIFY_IS_APPROX( A1c*(M*h1), (A1c*M)*h1 ); + VERIFY_IS_APPROX( P1*(M*h1), (P1*M)*h1 ); + + VERIFY_IS_APPROX( M*(A1*h1), (M*A1)*h1 ); + VERIFY_IS_APPROX( M*(A1c*h1), (M*A1c)*h1 ); + VERIFY_IS_APPROX( M*(P1*h1), ((M*P1)*h1) ); } template void transform_alignment() @@ -517,5 +593,8 @@ void test_geo_transformations() CALL_SUBTEST_7(( transform_products() )); CALL_SUBTEST_7(( transform_products() )); + + CALL_SUBTEST_8(( transform_associativity(Rotation2D(internal::random()*double(3.14))) )); + CALL_SUBTEST_8(( transform_associativity(Quaterniond(Vector4d::Random().normalized())) )); } } diff --git a/gtsam/3rdparty/Eigen/test/packetmath.cpp b/gtsam/3rdparty/Eigen/test/packetmath.cpp index 38aa256ce..bac7b02d1 100644 --- a/gtsam/3rdparty/Eigen/test/packetmath.cpp +++ b/gtsam/3rdparty/Eigen/test/packetmath.cpp @@ -327,6 +327,7 @@ template void test_conj_helper(Scalar ref[i] += cj0(data1[i]) * cj1(data2[i]); VERIFY(internal::isApprox(ref[i], cj.pmadd(data1[i],data2[i],tmp)) && "conj_helper pmadd"); } + *pval += 0; // Workaround msvc 2013 issue (bad code generation) internal::pstore(pval,pcj.pmadd(internal::pload(data1),internal::pload(data2),internal::pload(pval))); VERIFY(areApprox(ref, pval, PacketSize) && "conj_helper pmadd"); } diff --git a/gtsam/3rdparty/Eigen/test/prec_inverse_4x4.cpp b/gtsam/3rdparty/Eigen/test/prec_inverse_4x4.cpp index c4ef2d4bd..eb6ad18c9 100644 --- a/gtsam/3rdparty/Eigen/test/prec_inverse_4x4.cpp +++ b/gtsam/3rdparty/Eigen/test/prec_inverse_4x4.cpp @@ -53,14 +53,29 @@ template void inverse_general_4x4(int repeat) // FIXME that 1.25 used to be 1.2 until we tested gcc 4.1 on 30 June 2010 and got 1.21. VERIFY(error_avg < (NumTraits::IsComplex ? 8.0 : 1.25)); VERIFY(error_max < (NumTraits::IsComplex ? 64.0 : 20.0)); + + { + int s = 5;//internal::random(4,10); + int i = 0;//internal::random(0,s-4); + int j = 0;//internal::random(0,s-4); + Matrix mat(s,s); + mat.setRandom(); + MatrixType submat = mat.template block<4,4>(i,j); + MatrixType mat_inv = mat.template block<4,4>(i,j).inverse(); + VERIFY_IS_APPROX(mat_inv, submat.inverse()); + mat.template block<4,4>(i,j) = submat.inverse(); + VERIFY_IS_APPROX(mat_inv, (mat.template block<4,4>(i,j))); + } } void test_prec_inverse_4x4() { CALL_SUBTEST_1((inverse_permutation_4x4())); CALL_SUBTEST_1(( inverse_general_4x4(200000 * g_repeat) )); + CALL_SUBTEST_1(( inverse_general_4x4 >(200000 * g_repeat) )); CALL_SUBTEST_2((inverse_permutation_4x4 >())); + CALL_SUBTEST_2(( inverse_general_4x4 >(200000 * g_repeat) )); CALL_SUBTEST_2(( inverse_general_4x4 >(200000 * g_repeat) )); CALL_SUBTEST_3((inverse_permutation_4x4())); diff --git a/gtsam/3rdparty/Eigen/test/product.h b/gtsam/3rdparty/Eigen/test/product.h index 0d054ff46..397af24ae 100644 --- a/gtsam/3rdparty/Eigen/test/product.h +++ b/gtsam/3rdparty/Eigen/test/product.h @@ -178,4 +178,12 @@ template void product(const MatrixType& m) // CwiseUnaryOp VERIFY_IS_APPROX(x = Scalar(1.)*(A*x), A*z); } + + // regression for blas_trais + { + VERIFY_IS_APPROX(square * (square*square).transpose(), square * square.transpose() * square.transpose()); + VERIFY_IS_APPROX(square * (-(square*square)), -square * square * square); + VERIFY_IS_APPROX(square * (s1*(square*square)), s1 * square * square * square); + VERIFY_IS_APPROX(square * (square*square).conjugate(), square * square.conjugate() * square.conjugate()); + } } diff --git a/gtsam/3rdparty/Eigen/test/sparse_basic.cpp b/gtsam/3rdparty/Eigen/test/sparse_basic.cpp index abe6a9d14..2df7b63a7 100644 --- a/gtsam/3rdparty/Eigen/test/sparse_basic.cpp +++ b/gtsam/3rdparty/Eigen/test/sparse_basic.cpp @@ -299,6 +299,14 @@ template void sparse_basic(const SparseMatrixType& re VERIFY_IS_APPROX(m1.innerVector(0).dot(refM2.row(0)), refM1.row(0).dot(refM2.row(0))); else VERIFY_IS_APPROX(m1.innerVector(0).dot(refM2.row(0)), refM1.col(0).dot(refM2.row(0))); + + DenseVector rv = DenseVector::Random(m1.cols()); + DenseVector cv = DenseVector::Random(m1.rows()); + Index r = internal::random(0,m1.rows()-2); + Index c = internal::random(0,m1.cols()-1); + VERIFY_IS_APPROX(( m1.template block<1,Dynamic>(r,0,1,m1.cols()).dot(rv)) , refM1.row(r).dot(rv)); + VERIFY_IS_APPROX(m1.row(r).dot(rv), refM1.row(r).dot(rv)); + VERIFY_IS_APPROX(m1.col(c).dot(cv), refM1.col(c).dot(cv)); VERIFY_IS_APPROX(m1.conjugate(), refM1.conjugate()); VERIFY_IS_APPROX(m1.real(), refM1.real()); diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h index 6825a7882..88dba54f5 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h @@ -293,7 +293,7 @@ void MatrixExponential::computeUV(float) const float maxnorm = 3.925724783138660f; frexp(m_l1norm / maxnorm, &m_squarings); if (m_squarings < 0) m_squarings = 0; - MatrixType A = m_M / pow(Scalar(2), m_squarings); + MatrixType A = m_M / Scalar(pow(2, m_squarings)); pade7(A); } } @@ -315,7 +315,7 @@ void MatrixExponential::computeUV(double) const double maxnorm = 5.371920351148152; frexp(m_l1norm / maxnorm, &m_squarings); if (m_squarings < 0) m_squarings = 0; - MatrixType A = m_M / pow(Scalar(2), m_squarings); + MatrixType A = m_M / Scalar(pow(2, m_squarings)); pade13(A); } } @@ -340,7 +340,7 @@ void MatrixExponential::computeUV(long double) const long double maxnorm = 4.0246098906697353063L; frexp(m_l1norm / maxnorm, &m_squarings); if (m_squarings < 0) m_squarings = 0; - MatrixType A = m_M / pow(Scalar(2), m_squarings); + MatrixType A = m_M / Scalar(pow(2, m_squarings)); pade13(A); } #elif LDBL_MANT_DIG <= 106 // double-double @@ -376,7 +376,7 @@ void MatrixExponential::computeUV(long double) const long double maxnorm = 2.884233277829519311757165057717815L; frexp(m_l1norm / maxnorm, &m_squarings); if (m_squarings < 0) m_squarings = 0; - MatrixType A = m_M / pow(Scalar(2), m_squarings); + MatrixType A = m_M / Scalar(pow(2, m_squarings)); pade17(A); } #else diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Splines/SplineFwd.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Splines/SplineFwd.h index 49db8d35d..9ea23a9a1 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Splines/SplineFwd.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Splines/SplineFwd.h @@ -31,6 +31,8 @@ namespace Eigen enum { OrderAtCompileTime = _Degree==Dynamic ? Dynamic : _Degree+1 /*!< The spline curve's order at compile-time. */ }; enum { NumOfDerivativesAtCompileTime = OrderAtCompileTime /*!< The number of derivatives defined for the current spline. */ }; + + enum { DerivativeMemoryLayout = Dimension==1 ? RowMajor : ColMajor /*!< The derivative type's memory layout. */ }; /** \brief The data type used to store non-zero basis functions. */ typedef Array BasisVectorType; @@ -39,7 +41,7 @@ namespace Eigen typedef Array BasisDerivativeType; /** \brief The data type used to store the spline's derivative values. */ - typedef Array DerivativeType; + typedef Array DerivativeType; /** \brief The point type the spline is representing. */ typedef Array PointType; @@ -62,12 +64,14 @@ namespace Eigen { enum { OrderAtCompileTime = _Degree==Dynamic ? Dynamic : _Degree+1 /*!< The spline curve's order at compile-time. */ }; enum { NumOfDerivativesAtCompileTime = _DerivativeOrder==Dynamic ? Dynamic : _DerivativeOrder+1 /*!< The number of derivatives defined for the current spline. */ }; + + enum { DerivativeMemoryLayout = _Dim==1 ? RowMajor : ColMajor /*!< The derivative type's memory layout. */ }; /** \brief The data type used to store the values of the basis function derivatives. */ typedef Array<_Scalar,Dynamic,Dynamic,RowMajor,NumOfDerivativesAtCompileTime,OrderAtCompileTime> BasisDerivativeType; /** \brief The data type used to store the spline's derivative values. */ - typedef Array<_Scalar,_Dim,Dynamic,ColMajor,_Dim,NumOfDerivativesAtCompileTime> DerivativeType; + typedef Array<_Scalar,_Dim,Dynamic,DerivativeMemoryLayout,_Dim,NumOfDerivativesAtCompileTime> DerivativeType; }; /** \brief 2D float B-spline with dynamic degree. */ diff --git a/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp b/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp index 087e7c542..7c112a1f0 100644 --- a/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp +++ b/gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp @@ -162,6 +162,15 @@ void test_autodiff_jacobian() CALL_SUBTEST(( forward_jacobian(TestFunc1(3,3)) )); } +double bug_1222() { + typedef Eigen::AutoDiffScalar AD; + const double _cv1_3 = 1.0; + const AD chi_3 = 1.0; + // this line did not work, because operator+ returns ADS, which then cannot be converted to ADS + const AD denom = chi_3 + _cv1_3; + return denom.value(); +} + void test_autodiff() { for(int i = 0; i < g_repeat; i++) { @@ -169,5 +178,7 @@ void test_autodiff() CALL_SUBTEST_2( test_autodiff_vector() ); CALL_SUBTEST_3( test_autodiff_jacobian() ); } + + bug_1222(); } From b0c57d34dbe03a8365b0c2446656839e203bccfc Mon Sep 17 00:00:00 2001 From: chrisbeall Date: Fri, 14 Oct 2016 16:30:03 -0700 Subject: [PATCH 15/35] fix typename --- gtsam/slam/BetweenFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index bfd7d4e34..e6aab45da 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -92,7 +92,7 @@ namespace gtsam { T hx = traits::Between(p1, p2, H1, H2); // h(x) // manifold equivalent of h(x)-z -> log(z,h(x)) #ifdef SLOW_BUT_CORRECT_BETWEENFACTOR - typename traits::ChartJacobian::Fixed Hlocal; + typename traits::ChartJacobian::Jacobian Hlocal; Vector rval = traits::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0); if (H1) *H1 = Hlocal * (*H1); if (H1) *H2 = Hlocal * (*H2); From 799c5d8bb90d929da369d9b1caaef0081458d8bd Mon Sep 17 00:00:00 2001 From: = Date: Tue, 18 Oct 2016 14:00:12 -0400 Subject: [PATCH 16/35] Replace iostream with iosfwd. --- gtsam/geometry/Quaternion.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index af9481181..4f5495263 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -21,7 +21,7 @@ #include #include // Logmap/Expmap derivatives #include -#include +#include #define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> From 80562c8ae222acfdc5e79181f1c75b6ac3b987e8 Mon Sep 17 00:00:00 2001 From: Yao Chen Date: Wed, 19 Oct 2016 09:16:47 -0400 Subject: [PATCH 17/35] Fixed the issue of compilation error. --- gtsam/inference/FactorGraph.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 86e130cc7..016488701 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -188,7 +188,7 @@ namespace gtsam { bayesTree.addFactorsToGraph(*this); } -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +//#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /** Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid * the copy). */ template @@ -196,7 +196,7 @@ namespace gtsam { push_back(const DERIVEDFACTOR& factor) { factors_.push_back(boost::make_shared(factor)); } -#endif +//#endif /** push back many factors with an iterator over plain factors (factors are copied) */ template From 08c9493b6dd8922ae2dc753150ff38d24ab93ff9 Mon Sep 17 00:00:00 2001 From: chrisbeall Date: Thu, 20 Oct 2016 18:32:22 -0700 Subject: [PATCH 18/35] fix include --- gtsam/base/SymmetricBlockMatrix.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index da3a9a8b8..53a8912f6 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -24,7 +24,7 @@ #include #include #include -#include +#include namespace boost { namespace serialization { From efd966b45adc9bd2c9a49b436889cb0010b440ef Mon Sep 17 00:00:00 2001 From: chrisbeall Date: Tue, 1 Nov 2016 15:11:57 -0400 Subject: [PATCH 19/35] Move print methods to cpp files wherever possible --- gtsam/base/GenericValue.h | 3 +-- gtsam/geometry/Cal3_S2Stereo.cpp | 39 +++++++++++++++++++++++++++++++ gtsam/geometry/Cal3_S2Stereo.h | 12 +++------- gtsam/geometry/Quaternion.h | 2 +- gtsam/geometry/SO3.cpp | 7 ++++++ gtsam/geometry/SO3.h | 6 ++--- gtsam_unstable/geometry/Event.cpp | 14 +++++++++++ gtsam_unstable/geometry/Event.h | 11 +++------ 8 files changed, 70 insertions(+), 24 deletions(-) create mode 100644 gtsam/geometry/Cal3_S2Stereo.cpp diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 7c4d0398d..5b59f4872 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -26,9 +26,8 @@ #include #include -#include -#include // operator typeid #include +#include // operator typeid namespace gtsam { diff --git a/gtsam/geometry/Cal3_S2Stereo.cpp b/gtsam/geometry/Cal3_S2Stereo.cpp new file mode 100644 index 000000000..22966ee37 --- /dev/null +++ b/gtsam/geometry/Cal3_S2Stereo.cpp @@ -0,0 +1,39 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 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 Cal3_S2Stereo.cpp + * @brief The most common 5DOF 3D->2D calibration + Stereo baseline + * @author Chris Beall + */ + +#include + +#include + +namespace gtsam { +using namespace std; + +/* ************************************************************************* */ +void Cal3_S2Stereo::print(const std::string& s) const { + K_.print(s+"K: "); + std::cout << s << "Baseline: " << b_ << std::endl; + } + +/* ************************************************************************* */ +bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const { + if (fabs(b_ - other.b_) > tol) return false; + return K_.equals(other.K_,tol); +} + +/* ************************************************************************* */ + +} // namespace gtsam diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 29ccd194d..db49448ec 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include namespace gtsam { @@ -63,16 +63,10 @@ namespace gtsam { /// @name Testable /// @{ - void print(const std::string& s = "") const { - K_.print(s+"K: "); - std::cout << s << "Baseline: " << b_ << std::endl; - } + void print(const std::string& s = "") const; /// Check if equal up to specified tolerance - bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const { - if (fabs(b_ - other.b_) > tol) return false; - return K_.equals(other.K_,tol); - } + bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const; /// @} /// @name Standard Interface diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index 4f5495263..af9481181 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -21,7 +21,7 @@ #include #include // Logmap/Expmap derivatives #include -#include +#include #define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index bd111d9b1..07c03ab49 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace gtsam { @@ -116,6 +117,12 @@ SO3 SO3::AxisAngle(const Vector3& axis, double theta) { return so3::ExpmapFunctor(axis, theta).expmap(); } +/* ************************************************************************* */ +void SO3::print(const std::string& s) const { + std::cout << s << *this << std::endl; + } + +/* ************************************************************************* */ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { if (H) { so3::DexpFunctor impl(omega); diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 0396fbce0..53f2c2130 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -24,7 +24,7 @@ #include #include -#include +#include namespace gtsam { @@ -68,9 +68,7 @@ public: /// @name Testable /// @{ - void print(const std::string& s) const { - std::cout << s << *this << std::endl; - } + void print(const std::string& s) const; bool equals(const SO3 & R, double tol) const { return equal_with_abs_tol(*this, R, tol); diff --git a/gtsam_unstable/geometry/Event.cpp b/gtsam_unstable/geometry/Event.cpp index dda2c32e6..c503514a6 100644 --- a/gtsam_unstable/geometry/Event.cpp +++ b/gtsam_unstable/geometry/Event.cpp @@ -18,7 +18,21 @@ */ #include +#include namespace gtsam { +/* ************************************************************************* */ +void Event::print(const std::string& s) const { + std::cout << s << "time = " << time_ << "location = " << location_.transpose(); +} + +/* ************************************************************************* */ +bool Event::equals(const Event& other, double tol) const { + return std::abs(time_ - other.time_) < tol + && traits::Equals(location_, other.location_, tol); +} + +/* ************************************************************************* */ + } //\ namespace gtsam diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index 5bd37d2d2..d9bacd106 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -21,7 +21,7 @@ #include #include -#include +#include namespace gtsam { @@ -60,15 +60,10 @@ public: } /** print with optional string */ - void print(const std::string& s = "") const { - std::cout << s << "time = " << time_ << "location = " << location_.transpose(); - } + void print(const std::string& s = "") const; /** equals with an tolerance */ - bool equals(const Event& other, double tol = 1e-9) const { - return std::abs(time_ - other.time_) < tol - && traits::Equals(location_, other.location_, tol); - } + bool equals(const Event& other, double tol = 1e-9) const; /// Updates a with tangent space delta inline Event retract(const Vector4& v) const { From 5fa4abf99cb89c0c9af9dc198b49f5e936a4f6f4 Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Thu, 8 Dec 2016 15:19:18 -0500 Subject: [PATCH 20/35] fix optional jacobians --- gtsam/base/VectorSpace.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index 5456ae7f5..43644b5c4 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -138,14 +138,14 @@ struct VectorSpaceImpl { } static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1, - ChartJacobian H2) { + ChartJacobian H2 = boost::none) { if (H1) *H1 = Eye(v1); if (H2) *H2 = Eye(v2); return v1 + v2; } static Class Between(const Class& v1, const Class& v2, ChartJacobian H1, - ChartJacobian H2) { + ChartJacobian H2 = boost::none) { if (H1) *H1 = - Eye(v1); if (H2) *H2 = Eye(v2); return v2 - v1; From 21aa7a2e856c535b0aeaf3643032e94b12e5465d Mon Sep 17 00:00:00 2001 From: Simon Julier Date: Tue, 17 Jan 2017 10:12:00 +0000 Subject: [PATCH 21/35] Fixed unrwapping of scalar references. --- wrap/Argument.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 52d9ca0b5..a47c6711c 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -75,16 +75,17 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { string cppType = type.qualifiedName("::"); string matlabUniqueType = type.qualifiedName(); + bool isNotScalar = !Argument::isScalar(); if (is_ptr && type.category != Qualified::EIGEN) // A pointer: emit an "unwrap_shared_ptr" call which returns a pointer file.oss << "boost::shared_ptr<" << cppType << "> " << name << " = unwrap_shared_ptr< "; - else if (is_ref && type.category != Qualified::EIGEN) + else if (is_ref && isNotScalar && type.category != Qualified::EIGEN) // A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer file.oss << cppType << "& " << name << " = *unwrap_shared_ptr< "; else - // Not a pointer or a reference: emit an "unwrap" call + // Not a pointer, or a reference to a scalar type. Therefore, emit an "unwrap" call // unwrap is specified in matlab.h as a series of template specializations // that know how to unpack the expected MATLAB object // example: double tol = unwrap< double >(in[2]); @@ -92,7 +93,7 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << cppType << " " << name << " = unwrap< "; file.oss << cppType << " >(" << matlabName; - if( (is_ptr || is_ref) && type.category != Qualified::EIGEN) + if( (is_ptr || is_ref) && isNotScalar && type.category != Qualified::EIGEN) file.oss << ", \"ptr_" << matlabUniqueType << "\""; file.oss << ");" << endl; } From f50c3c0d51fa7f4d015ec84e5a536a0cc4ffd918 Mon Sep 17 00:00:00 2001 From: Simon Julier Date: Tue, 17 Jan 2017 16:53:28 +0000 Subject: [PATCH 22/35] Use INSTALL_NAME_DIR to embed names in the dylibs and avoid linker errors.y --- cmake/GtsamMatlabWrap.cmake | 3 ++- gtsam/CMakeLists.txt | 4 ++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake index a9e04a01a..3165a307a 100644 --- a/cmake/GtsamMatlabWrap.cmake +++ b/cmake/GtsamMatlabWrap.cmake @@ -70,7 +70,8 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex set(mexModuleExt mexglx) endif() elseif(APPLE) - set(mexModuleExt mexmaci64) + set(mexModuleExt mexmaci64) + set(CMAKE_INSTALL_DIR_NAME ${GTSAM_TOOLBOX_INSTALL_PATH}) elseif(MSVC) if(CMAKE_CL_64) set(mexModuleExt mexw64) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 8c1d8bb43..63528d3b4 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -113,6 +113,10 @@ if (GTSAM_BUILD_STATIC_LIBRARY) PREFIX "lib" COMPILE_DEFINITIONS GTSAM_IMPORT_STATIC) endif() + if(APPLE) # Set the + set_target_properties(gtsam PROPERTIES + INSTALL_DIR_NAME ${CMAKE_INSTALL_PREFIX}/lib) + endif() install(TARGETS gtsam EXPORT GTSAM-exports ARCHIVE DESTINATION lib) list(APPEND GTSAM_EXPORTED_TARGETS gtsam) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) From 2a4aa76e7e1d8693c43a10fc59b2ba0873838b1a Mon Sep 17 00:00:00 2001 From: Simon Julier Date: Wed, 18 Jan 2017 18:58:48 +0000 Subject: [PATCH 23/35] Added instructions on the iostream and the wrapper. Also added instructions on how to enable the toolbox.y --- matlab/README-gtsam-toolbox.txt | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/matlab/README-gtsam-toolbox.txt b/matlab/README-gtsam-toolbox.txt index 6f74f9806..127eb57ed 100644 --- a/matlab/README-gtsam-toolbox.txt +++ b/matlab/README-gtsam-toolbox.txt @@ -7,8 +7,12 @@ http://borg.cc.gatech.edu/projects/gtsam ================================================================================ This is the GTSAM MATLAB toolbox, a MATLAB wrapper around the GTSAM C++ -library. +library. To build it, enable GTSAM_INSTALL_MATLAB_TOOLBOX in CMake. +The interface is created automatically by the wrap tool, which +directly parses C++ header files and generates matlab proxy objects +and wrapping and unwrapping code. The wrap tool also redirects the +standard "cout" stream to matlab's console. ---------------------------------------- Note about newer Ubuntu versions unsupported by MATLAB (later than 10.04) From f802099bfdcbeec4239ad9283e507324ee83d9e2 Mon Sep 17 00:00:00 2001 From: Simon Julier Date: Wed, 18 Jan 2017 19:13:25 +0000 Subject: [PATCH 24/35] Tidied up the text to make it a bit clearer / less ambiguous.y --- matlab/README-gtsam-toolbox.txt | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/matlab/README-gtsam-toolbox.txt b/matlab/README-gtsam-toolbox.txt index 127eb57ed..a1691be32 100644 --- a/matlab/README-gtsam-toolbox.txt +++ b/matlab/README-gtsam-toolbox.txt @@ -9,10 +9,12 @@ 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 is created automatically by the wrap tool, which -directly parses C++ header files and generates matlab proxy objects -and wrapping and unwrapping code. The wrap tool also redirects the -standard "cout" stream to matlab's console. +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) From d8d7c5618a11076a1a56a7e3232d4e195f70b1c8 Mon Sep 17 00:00:00 2001 From: Simon Julier Date: Thu, 19 Jan 2017 01:49:12 +0000 Subject: [PATCH 25/35] Generate an error and exit if trying to wrap a non-const scalar reference. --- wrap/Argument.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index a47c6711c..6badf7794 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -77,6 +77,12 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { string matlabUniqueType = type.qualifiedName(); bool isNotScalar = !Argument::isScalar(); + // We cannot handle scalar non const references + if (!isNotScalar && is_ref && !is_const) { + cerr << "Cannot wrap a scalar non-const reference" << endl; + exit(-1); + } + if (is_ptr && type.category != Qualified::EIGEN) // A pointer: emit an "unwrap_shared_ptr" call which returns a pointer file.oss << "boost::shared_ptr<" << cppType << "> " << name From d1422ac9216af6fa53b49932cb258a195c1b22ba Mon Sep 17 00:00:00 2001 From: Simon Julier Date: Thu, 19 Jan 2017 09:28:04 +0000 Subject: [PATCH 26/35] Reverted change to make files to make the pull request clean. --- cmake/GtsamMatlabWrap.cmake | 3 +-- gtsam/CMakeLists.txt | 4 ---- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake index 3165a307a..a9e04a01a 100644 --- a/cmake/GtsamMatlabWrap.cmake +++ b/cmake/GtsamMatlabWrap.cmake @@ -70,8 +70,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex set(mexModuleExt mexglx) endif() elseif(APPLE) - set(mexModuleExt mexmaci64) - set(CMAKE_INSTALL_DIR_NAME ${GTSAM_TOOLBOX_INSTALL_PATH}) + set(mexModuleExt mexmaci64) elseif(MSVC) if(CMAKE_CL_64) set(mexModuleExt mexw64) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 63528d3b4..8c1d8bb43 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -113,10 +113,6 @@ if (GTSAM_BUILD_STATIC_LIBRARY) PREFIX "lib" COMPILE_DEFINITIONS GTSAM_IMPORT_STATIC) endif() - if(APPLE) # Set the - set_target_properties(gtsam PROPERTIES - INSTALL_DIR_NAME ${CMAKE_INSTALL_PREFIX}/lib) - endif() install(TARGETS gtsam EXPORT GTSAM-exports ARCHIVE DESTINATION lib) list(APPEND GTSAM_EXPORTED_TARGETS gtsam) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) From 6a109aca9bae0b858099f59d084a0254841397ac Mon Sep 17 00:00:00 2001 From: Simon Julier Date: Fri, 20 Jan 2017 01:58:59 +0000 Subject: [PATCH 27/35] Throw an exception rather than call exit. --- wrap/Argument.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 6badf7794..01da3a756 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -79,8 +79,7 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { // We cannot handle scalar non const references if (!isNotScalar && is_ref && !is_const) { - cerr << "Cannot wrap a scalar non-const reference" << endl; - exit(-1); + throw std::runtime_error("Cannot unwrap a scalar non-const reference"); } if (is_ptr && type.category != Qualified::EIGEN) From 5482f1f5eb720c9668373910ca6a78289fffc12a Mon Sep 17 00:00:00 2001 From: Ellon Mendes Date: Thu, 9 Jun 2016 13:55:36 +0200 Subject: [PATCH 28/35] [python] Make python library hidden by renaming gtsampy.so to _gtsampy.so This commit also fixes a naming problem of the python .so module (_libgtsam_python.so -> _gtsampy.so) --- python/gtsam/__init__.py | 2 +- python/handwritten/CMakeLists.txt | 6 +++--- python/handwritten/exportgtsam.cpp | 2 +- python/setup.py.in | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py index 2e95ea33a..2d7ac72f7 100644 --- a/python/gtsam/__init__.py +++ b/python/gtsam/__init__.py @@ -1 +1 @@ -from gtsampy import * +from _gtsampy import * diff --git a/python/handwritten/CMakeLists.txt b/python/handwritten/CMakeLists.txt index fd022dd17..689354b4e 100644 --- a/python/handwritten/CMakeLists.txt +++ b/python/handwritten/CMakeLists.txt @@ -12,7 +12,7 @@ endforeach() add_library(gtsam_python SHARED exportgtsam.cpp ${gtsam_python_srcs}) string(TOUPPER "${CMAKE_BUILD_TYPE}" build_type_toupper) set_target_properties(gtsam_python PROPERTIES - OUTPUT_NAME gtsampy + OUTPUT_NAME _gtsampy PREFIX "" ${build_type_toupper}_POSTFIX "" SKIP_BUILD_RPATH TRUE @@ -31,11 +31,11 @@ target_link_libraries(gtsam_python # Cause the library to be output in the correct directory. # TODO: Change below to work on different systems (currently works only with Linux) -set(output_path ${CMAKE_CURRENT_BINARY_DIR}/../gtsam/_libgtsam_python.so) +set(output_path ${CMAKE_CURRENT_BINARY_DIR}/../gtsam/_gtsampy.so) add_custom_command( OUTPUT ${output_path} DEPENDS gtsam_python COMMAND ${CMAKE_COMMAND} -E copy $ ${output_path} - COMMENT "Copying extension module to python/gtsam/_libgtsam_python.so" + COMMENT "Copying extension module to python/gtsam/_gtsampy.so" ) add_custom_target(copy_gtsam_python_module ALL DEPENDS ${output_path}) \ No newline at end of file diff --git a/python/handwritten/exportgtsam.cpp b/python/handwritten/exportgtsam.cpp index 94dc10e56..8fa7e0fdd 100644 --- a/python/handwritten/exportgtsam.cpp +++ b/python/handwritten/exportgtsam.cpp @@ -62,7 +62,7 @@ void registerNumpyEigenConversions(); //-----------------------------------// -BOOST_PYTHON_MODULE(gtsampy){ +BOOST_PYTHON_MODULE(_gtsampy){ // NOTE: We need to call import_array1() instead of import_array() to support both python 2 // and 3. The reason is that BOOST_PYTHON_MODULE puts all its contents in a function diff --git a/python/setup.py.in b/python/setup.py.in index d3b5fcde4..8b2de7352 100644 --- a/python/setup.py.in +++ b/python/setup.py.in @@ -11,5 +11,5 @@ setup(name='gtsam', package_dir={ '': '${CMAKE_CURRENT_SOURCE_DIR}' }, packages=['gtsam', 'gtsam_utils', 'gtsam_examples', 'gtsam_tests'], #package_data={'gtsam' : ['_libgtsam_python.so']}, # location of .so file is relative to package_dir - data_files=[('${PY_INSTALL_FOLDER}/gtsam/', ['gtsam/_libgtsam_python.so'])], # location of .so file relative to setup.py + data_files=[('${PY_INSTALL_FOLDER}/gtsam/', ['gtsam/_gtsampy.so'])], # location of .so file relative to setup.py ) From 68d26ff279d2a2ca275929bac2b1205397134181 Mon Sep 17 00:00:00 2001 From: Haldean Brown Date: Thu, 9 Mar 2017 16:57:29 -0800 Subject: [PATCH 29/35] Support bearing factors between Pose3 values I am modelling a system in which there are two bodies, and one can observe the other but cannot estimate the other's pose. This is perfectly modeled by a BearingRangeFactor, but without this patch, you cannot make a BearingRangeFactor between two Pose3 values. This adds support for that by extending the Pose3 class to support calling bearing() on another Pose3. --- gtsam/geometry/Pose3.cpp | 9 +++++++++ gtsam/geometry/Pose3.h | 12 ++++++++++++ 2 files changed, 21 insertions(+) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 25cd661e8..529453bc9 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -378,6 +378,15 @@ Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1, } } +/* ************************************************************************* */ +Unit3 Pose3::bearing(const Pose3& point, OptionalJacobian<2, 6> H1, + OptionalJacobian<2, 6> H2) const { + if (H2) { + return bearing(point.translation(), H1, H2.cols<3>(3)); + } + return bearing(point.translation(), H1, boost::none); +} + /* ************************************************************************* */ boost::optional Pose3::Align(const std::vector& abPointPairs) { const size_t n = abPointPairs.size(); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index ee1206119..111f226bb 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -282,6 +282,15 @@ public: Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> H1 = boost::none, OptionalJacobian<2, 3> H2 = boost::none) const; + /** + * Calculate bearing to another pose + * @param other 3D location and orientation of other body. The orientation + * information is ignored. + * @return bearing (Unit3) + */ + Unit3 bearing(const Pose3& point, OptionalJacobian<2, 6> H1 = boost::none, + OptionalJacobian<2, 6> H2 = boost::none) const; + /// @} /// @name Advanced Interface /// @{ @@ -353,6 +362,9 @@ struct traits : public internal::LieGroup {}; template <> struct Bearing : HasBearing {}; +template<> +struct Bearing : HasBearing {}; + template struct Range : HasRange {}; From 42e7e313402eb5ed5d65db870ab20ba55567529b Mon Sep 17 00:00:00 2001 From: Haldean Brown Date: Mon, 13 Mar 2017 10:31:37 -0700 Subject: [PATCH 30/35] Add new Pose3::bearing overload to Python wrapper --- python/handwritten/geometry/Pose3.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/python/handwritten/geometry/Pose3.cpp b/python/handwritten/geometry/Pose3.cpp index f778ea4e0..551f2f60c 100644 --- a/python/handwritten/geometry/Pose3.cpp +++ b/python/handwritten/geometry/Pose3.cpp @@ -53,6 +53,9 @@ void exportPose3(){ // function pointers to desambiguate range() calls double (Pose3::*range1)(const Point3 &, OptionalJacobian<1,6>, OptionalJacobian<1,3>) const = &Pose3::range; double (Pose3::*range2)(const Pose3 &, OptionalJacobian<1,6>, OptionalJacobian<1,6>) const = &Pose3::range; + // function pointers to desambiguate bearing() calls + Unit3 (Pose3::*bearing1)(const Point3 &, OptionalJacobian<2,6>, OptionalJacobian<2,3>) const = &Pose3::bearing; + Unit3 (Pose3::*bearing2)(const Pose3 &, OptionalJacobian<2,6>, OptionalJacobian<2,6>) const = &Pose3::bearing; class_("Pose3") .def(init<>()) @@ -65,7 +68,6 @@ void exportPose3(){ .def("equals", &Pose3::equals, equals_overloads(args("pose", "tol"))) .def("identity", &Pose3::identity) .staticmethod("identity") - .def("bearing", &Pose3::bearing) .def("matrix", &Pose3::matrix) .def("transform_from", &Pose3::transform_from, transform_from_overloads(args("point", "H1", "H2"))) @@ -88,5 +90,6 @@ void exportPose3(){ .def("between", between2, between_overloads()) .def("range", range1, range_overloads()) .def("range", range2, range_overloads()) - .def("bearing", &Pose3::bearing, bearing_overloads()); + .def("bearing", bearing1, bearing_overloads()) + .def("bearing", bearing2, bearing_overloads()); } From 53898778962ee4579f77b5f2875ecdee80db7625 Mon Sep 17 00:00:00 2001 From: Haldean Brown Date: Wed, 15 Mar 2017 10:36:10 -0700 Subject: [PATCH 31/35] set attitude jacobian to zero in Pose3-to-Pose3 bearing --- gtsam/geometry/Pose3.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 529453bc9..8554b7f46 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -382,6 +382,7 @@ Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1, Unit3 Pose3::bearing(const Pose3& point, OptionalJacobian<2, 6> H1, OptionalJacobian<2, 6> H2) const { if (H2) { + H2->setZero(); return bearing(point.translation(), H1, H2.cols<3>(3)); } return bearing(point.translation(), H1, boost::none); From 5ae331ad041235e80b6a645d6dbe6062c9783435 Mon Sep 17 00:00:00 2001 From: Haldean Brown Date: Thu, 30 Mar 2017 12:52:48 -0700 Subject: [PATCH 32/35] add test for pose-to-pose bearing --- gtsam/geometry/tests/testPose3.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 97ccbcb34..98d3e11ee 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -708,6 +708,27 @@ TEST(Pose3, Bearing2) { EXPECT(assert_equal(expectedH2, actualH2)); } +TEST(Pose3, PoseToPoseBearing) { + Matrix expectedH1, actualH1, expectedH2, actualH2, H2block; + EXPECT(assert_equal(Unit3(0,1,0), xl1.bearing(xl2, actualH1, actualH2), 1e-9)); + + // Check numerical derivatives + expectedH1 = numericalDerivative21(bearing_proxy, xl1, l2); + + // Since the second pose is treated as a point, the value calculated by + // numericalDerivative22 only depends on the position of the pose. Here, we + // calculate the Jacobian w.r.t. the second pose's position, and then augment + // that with zeroes in the block that is w.r.t. the second pose's + // orientation. + H2block = numericalDerivative22(bearing_proxy, xl1, l2); + expectedH2 = Matrix(2, 6); + expectedH2.setZero(); + expectedH2.block<2, 3>(0, 3) = H2block; + + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); +} + /* ************************************************************************* */ TEST( Pose3, unicycle ) { From 2058b928826abbe121ebffa85fa8f989c67d5f46 Mon Sep 17 00:00:00 2001 From: Haldean Brown Date: Thu, 30 Mar 2017 12:53:01 -0700 Subject: [PATCH 33/35] rename Pose3 parameter from point to pose --- gtsam/geometry/Pose3.cpp | 6 +++--- gtsam/geometry/Pose3.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 8554b7f46..50e487e75 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -379,13 +379,13 @@ Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1, } /* ************************************************************************* */ -Unit3 Pose3::bearing(const Pose3& point, OptionalJacobian<2, 6> H1, +Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> H1, OptionalJacobian<2, 6> H2) const { if (H2) { H2->setZero(); - return bearing(point.translation(), H1, H2.cols<3>(3)); + return bearing(pose.translation(), H1, H2.cols<3>(3)); } - return bearing(point.translation(), H1, boost::none); + return bearing(pose.translation(), H1, boost::none); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 111f226bb..6df62485b 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -288,7 +288,7 @@ public: * information is ignored. * @return bearing (Unit3) */ - Unit3 bearing(const Pose3& point, OptionalJacobian<2, 6> H1 = boost::none, + Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> H1 = boost::none, OptionalJacobian<2, 6> H2 = boost::none) const; /// @} From 5cc5c82b26ef768b8181ea1dd72533095cc21664 Mon Sep 17 00:00:00 2001 From: chrisbeall Date: Mon, 22 May 2017 18:11:35 -0400 Subject: [PATCH 34/35] -Add virtual destructor to PreintegratedRotationParams -Add namespace so GTSAM_VALUE_EXPORT macro can be used in projects outside of gtsam --- gtsam/base/GenericValue.h | 2 +- gtsam/navigation/PreintegratedRotation.h | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 5b59f4872..93a7d0db5 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -190,7 +190,7 @@ public: } /// use this macro instead of BOOST_CLASS_EXPORT for GenericValues -#define GTSAM_VALUE_EXPORT(Type) BOOST_CLASS_EXPORT(GenericValue) +#define GTSAM_VALUE_EXPORT(Type) BOOST_CLASS_EXPORT(gtsam::GenericValue) }; diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 88d9c6437..cb40774f4 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -35,6 +35,8 @@ struct PreintegratedRotationParams { PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {} + virtual ~PreintegratedRotationParams() {} + virtual void print(const std::string& s) const; virtual bool equals(const PreintegratedRotationParams& other, double tol=1e-9) const; From c82fe1fde2fc988b6bde5e4798b66129bbb5da19 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 5 Aug 2017 20:20:16 +0000 Subject: [PATCH 35/35] Initial Bitbucket Pipelines configuration --- bitbucket-pipelines.yml | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 bitbucket-pipelines.yml diff --git a/bitbucket-pipelines.yml b/bitbucket-pipelines.yml new file mode 100644 index 000000000..2d8b7f061 --- /dev/null +++ b/bitbucket-pipelines.yml @@ -0,0 +1,16 @@ +# This is a sample build configuration for C++ – Make. +# Check our guides at https://confluence.atlassian.com/x/5Q4SMw for more examples. +# Only use spaces to indent your .yml configuration. +# ----- +# You can specify a custom docker image from Docker Hub as your build environment. +image: ilssim/cmake-boost-qt5 + +pipelines: + default: + - step: + script: # Modify the commands below to build your repository. + - mkdir build + - cd build + - cmake .. + - make + - make check \ No newline at end of file