From b925142b987af501fb86d738079e6491940f5e37 Mon Sep 17 00:00:00 2001 From: Martin Brossard Date: Fri, 2 Aug 2019 17:13:02 +0200 Subject: [PATCH 01/79] Create pose3Localizationexample.txt pose3example.txt without loop closure --- examples/Data/pose3Localizationexample.txt | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100644 examples/Data/pose3Localizationexample.txt diff --git a/examples/Data/pose3Localizationexample.txt b/examples/Data/pose3Localizationexample.txt new file mode 100644 index 000000000..a35005aa2 --- /dev/null +++ b/examples/Data/pose3Localizationexample.txt @@ -0,0 +1,9 @@ +VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 +VERTEX_SE3:QUAT 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230 +VERTEX_SE3:QUAT 2 1.993500 0.023275 0.003793 -0.351729 -0.597838 0.584174 0.421446 +VERTEX_SE3:QUAT 3 2.004291 1.024305 0.018047 0.331798 -0.200659 0.919323 0.067024 +VERTEX_SE3:QUAT 4 0.999908 1.055073 0.020212 -0.035697 -0.462490 0.445933 0.765488 +EDGE_SE3:QUAT 0 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000 +EDGE_SE3:QUAT 1 2 0.523923 0.776654 0.326659 0.311512 0.656877 -0.678505 0.105373 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000 +EDGE_SE3:QUAT 2 3 0.910927 0.055169 -0.411761 0.595795 -0.561677 0.079353 0.568551 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000 +EDGE_SE3:QUAT 3 4 0.775288 0.228798 -0.596923 -0.592077 0.303380 -0.513226 0.542221 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000 From 3d7ce45de66bb401cdd7c124cf0b4ef0fd228a57 Mon Sep 17 00:00:00 2001 From: Martin Brossard Date: Fri, 2 Aug 2019 17:14:31 +0200 Subject: [PATCH 02/79] Create Pose3Localization.cpp Pose3Example_g2o with marginal computation. --- examples/Pose3Localization.cpp | 83 ++++++++++++++++++++++++++++++++++ 1 file changed, 83 insertions(+) create mode 100644 examples/Pose3Localization.cpp diff --git a/examples/Pose3Localization.cpp b/examples/Pose3Localization.cpp new file mode 100644 index 000000000..becb9530c --- /dev/null +++ b/examples/Pose3Localization.cpp @@ -0,0 +1,83 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 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 Pose3SLAMExample_initializePose3.cpp + * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3 + * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o + * @date Aug 25, 2014 + * @author Luca Carlone + */ + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +int main(const int argc, const char *argv[]) { + + // Read graph from file + string g2oFile; + if (argc < 2) + g2oFile = findExampleDataFile("pose3Localizationexample.txt"); + else + g2oFile = argv[1]; + + NonlinearFactorGraph::shared_ptr graph; + Values::shared_ptr initial; + bool is3D = true; + boost::tie(graph, initial) = readG2o(g2oFile, is3D); + + // Add prior on the first key + NonlinearFactorGraph graphWithPrior = *graph; + noiseModel::Diagonal::shared_ptr priorModel = // + noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); + Key firstKey = 0; + for(const Values::ConstKeyValuePair& key_value: *initial) { + std::cout << "Adding prior to g2o file " << std::endl; + firstKey = key_value.key; + graphWithPrior.add(PriorFactor(firstKey, Pose3(), priorModel)); + break; + } + + std::cout << "Optimizing the factor graph" << std::endl; + GaussNewtonParams params; + params.setVerbosity("TERMINATION"); // this will show info about stopping conditions + GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params); + Values result = optimizer.optimize(); + std::cout << "Optimization complete" << std::endl; + + std::cout << "initial error=" <error(*initial)<< std::endl; + std::cout << "final error=" <error(result)<< std::endl; + + if (argc < 3) { + result.print("result"); + } else { + const string outputFile = argv[2]; + std::cout << "Writing results to file: " << outputFile << std::endl; + writeG2o(*graph, result, outputFile); + std::cout << "done! " << std::endl; + } + + // Calculate and print marginal covariances for all variables + Marginals marginals(*graph, result); + for(const auto& key_value: result) { + auto p = dynamic_cast*>(&key_value.value); + if (!p) continue; + std::cout << marginals.marginalCovariance(key_value.key) << endl; + } + return 0; +} From c9bcceef2f01e9eb5e22fa172c6d4cbb6171124e Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Sun, 12 Jan 2020 11:34:02 -0500 Subject: [PATCH 03/79] add tbb to travis-ci --- .travis.yml | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/.travis.yml b/.travis.yml index 14fe66ac1..e4e1a3440 100644 --- a/.travis.yml +++ b/.travis.yml @@ -16,6 +16,7 @@ addons: - cmake - libpython-dev python-numpy - libboost-all-dev + - libtbb-dev # before_install: # - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update ; fi @@ -43,7 +44,7 @@ jobs: - stage: compile os: osx compiler: gcc - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF script: bash .travis.sh -b - stage: compile os: osx @@ -54,7 +55,7 @@ jobs: - stage: compile os: osx compiler: clang - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF script: bash .travis.sh -b - stage: compile os: osx @@ -65,7 +66,7 @@ jobs: - stage: compile os: linux compiler: gcc - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF script: bash .travis.sh -b - stage: compile os: linux @@ -76,7 +77,7 @@ jobs: - stage: compile os: linux compiler: clang - env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF script: bash .travis.sh -b - stage: compile os: linux @@ -87,7 +88,13 @@ jobs: - stage: special os: linux compiler: clang - env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON + env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON GTSAM_WITH_TBB=OFF + script: bash .travis.sh -b +# on Linux, with GTSAM_WITH_TBB not off to make sure GTSAM still compiles/tests + - stage: special + os: linux + compiler: gcc + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF script: bash .travis.sh -b # -------- STAGE 2: TESTS ----------- # on Mac, GCC @@ -99,7 +106,7 @@ jobs: - stage: test os: osx compiler: clang - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF script: bash .travis.sh -t - stage: test os: linux @@ -109,7 +116,7 @@ jobs: - stage: test os: linux compiler: gcc - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF script: bash .travis.sh -t - stage: test os: linux From d9923fc3cc36605ce52ab35a71bb4a69713d045f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 21 Feb 2020 19:42:55 -0500 Subject: [PATCH 04/79] replaced/appended all calls to SimpleCamera with PinholeCameraCal3_S2 --- cython/gtsam/examples/ImuFactorExample2.py | 2 +- cython/gtsam/examples/SFMExample.py | 4 +- cython/gtsam/examples/VisualISAMExample.py | 4 +- cython/gtsam/tests/test_SimpleCamera.py | 8 +- cython/gtsam/utils/visual_data_generator.py | 2 +- examples/CameraResectioning.cpp | 4 +- examples/ISAM2Example_SmartFactor.cpp | 2 +- examples/ImuFactorExample2.cpp | 4 +- examples/SFMExample.cpp | 2 +- examples/SFMExampleExpressions.cpp | 4 +- examples/SFMdata.h | 5 +- examples/SelfCalibrationExample.cpp | 2 +- examples/VisualISAM2Example.cpp | 2 +- examples/VisualISAMExample.cpp | 2 +- gtsam.h | 18 ++-- gtsam/geometry/BearingRange.h | 4 +- gtsam/geometry/tests/testTriangulation.cpp | 26 +++--- gtsam/nonlinear/Expression.h | 2 +- gtsam/nonlinear/utilities.h | 4 +- gtsam/sam/tests/testRangeFactor.cpp | 11 +-- gtsam/slam/EssentialMatrixFactor.h | 2 +- gtsam/slam/ProjectionFactor.h | 3 +- .../slam/tests/testEssentialMatrixFactor.cpp | 1 + gtsam/slam/tests/testTriangulationFactor.cpp | 6 +- .../geometry/tests/testInvDepthCamera3.cpp | 5 +- gtsam_unstable/slam/MultiProjectionFactor.h | 2 +- gtsam_unstable/slam/serialization.cpp | 79 +++++++++--------- .../slam/tests/testInvDepthFactor3.cpp | 6 +- .../testSmartStereoProjectionPoseFactor.cpp | 6 +- matlab/+gtsam/Contents.m | 1 + matlab/+gtsam/VisualISAMGenerateData.m | 2 +- matlab/+gtsam/cylinderSampleProjection.m | 2 +- matlab/gtsam_examples/CameraFlyingExample.m | 4 +- matlab/gtsam_examples/SBAExample.m | 4 +- matlab/gtsam_tests/testTriangulation.m | 4 +- .../covarianceAnalysisCreateTrajectory.m | 4 +- .../TransformCalProjectionFactorExampleISAM.m | 2 +- ...ansformCalProjectionFactorIMUExampleISAM.m | 2 +- .../TransformProjectionFactorExample.m | 2 +- .../TransformProjectionFactorExampleISAM.m | 2 +- matlab/unstable_examples/project_landmarks.m | 2 +- tests/testNonlinearEquality.cpp | 6 +- tests/testSerializationSLAM.cpp | 83 ++++++++++--------- tests/testVisualISAM2.cpp | 2 +- 44 files changed, 180 insertions(+), 164 deletions(-) diff --git a/cython/gtsam/examples/ImuFactorExample2.py b/cython/gtsam/examples/ImuFactorExample2.py index 1f6732f49..4b86d4837 100644 --- a/cython/gtsam/examples/ImuFactorExample2.py +++ b/cython/gtsam/examples/ImuFactorExample2.py @@ -81,7 +81,7 @@ def IMU_example(): up = gtsam.Point3(0, 0, 1) target = gtsam.Point3(0, 0, 0) position = gtsam.Point3(radius, 0, 0) - camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2()) + camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, gtsam.Cal3_S2()) pose_0 = camera.pose() # Create the set of ground-truth landmarks and poses diff --git a/cython/gtsam/examples/SFMExample.py b/cython/gtsam/examples/SFMExample.py index bf46f09d5..21b90a60f 100644 --- a/cython/gtsam/examples/SFMExample.py +++ b/cython/gtsam/examples/SFMExample.py @@ -16,7 +16,7 @@ from gtsam.examples import SFMdata from gtsam.gtsam import (Cal3_S2, DoglegOptimizer, GenericProjectionFactorCal3_S2, NonlinearFactorGraph, Point3, Pose3, PriorFactorPoint3, PriorFactorPose3, - Rot3, SimpleCamera, Values) + Rot3, PinholeCameraCal3_S2, Values) def symbol(name: str, index: int) -> int: @@ -75,7 +75,7 @@ def main(): # Simulated measurements from each camera pose, adding them to the factor graph for i, pose in enumerate(poses): - camera = SimpleCamera(pose, K) + camera = PinholeCameraCal3_S2(pose, K) for j, point in enumerate(points): measurement = camera.project(point) factor = GenericProjectionFactorCal3_S2( diff --git a/cython/gtsam/examples/VisualISAMExample.py b/cython/gtsam/examples/VisualISAMExample.py index f4d81eaf7..b7b83e2a1 100644 --- a/cython/gtsam/examples/VisualISAMExample.py +++ b/cython/gtsam/examples/VisualISAMExample.py @@ -18,7 +18,7 @@ from gtsam.examples import SFMdata from gtsam.gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2, NonlinearFactorGraph, NonlinearISAM, Point3, Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, - SimpleCamera, Values) + PinholeCameraCal3_S2, Values) def symbol(name: str, index: int) -> int: @@ -54,7 +54,7 @@ def main(): # Loop over the different poses, adding the observations to iSAM incrementally for i, pose in enumerate(poses): - camera = SimpleCamera(pose, K) + camera = PinholeCameraCal3_S2(pose, K) # Add factors for each landmark observation for j, point in enumerate(points): measurement = camera.project(point) diff --git a/cython/gtsam/tests/test_SimpleCamera.py b/cython/gtsam/tests/test_SimpleCamera.py index efdfec561..a3654a5f1 100644 --- a/cython/gtsam/tests/test_SimpleCamera.py +++ b/cython/gtsam/tests/test_SimpleCamera.py @@ -5,7 +5,7 @@ All Rights Reserved See LICENSE for the license information -SimpleCamera unit tests. +PinholeCameraCal3_S2 unit tests. Author: Frank Dellaert & Duy Nguyen Ta (Python) """ import math @@ -14,7 +14,7 @@ import unittest import numpy as np import gtsam -from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, SimpleCamera +from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, PinholeCameraCal3_S2 from gtsam.utils.test_case import GtsamTestCase K = Cal3_S2(625, 625, 0, 0, 0) @@ -23,14 +23,14 @@ class TestSimpleCamera(GtsamTestCase): def test_constructor(self): pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5)) - camera = SimpleCamera(pose1, K) + camera = PinholeCameraCal3_S2(pose1, K) self.gtsamAssertEquals(camera.calibration(), K, 1e-9) self.gtsamAssertEquals(camera.pose(), pose1, 1e-9) def test_level2(self): # Create a level camera, looking in Y-direction pose2 = Pose2(0.4,0.3,math.pi/2.0) - camera = SimpleCamera.Level(K, pose2, 0.1) + camera = PinholeCameraCal3_S2.Level(K, pose2, 0.1) # expected x = Point3(1,0,0) diff --git a/cython/gtsam/utils/visual_data_generator.py b/cython/gtsam/utils/visual_data_generator.py index 91194c565..7cd885d49 100644 --- a/cython/gtsam/utils/visual_data_generator.py +++ b/cython/gtsam/utils/visual_data_generator.py @@ -99,7 +99,7 @@ def generate_data(options): for i in range(options.nrCameras): theta = i * 2 * pi / options.nrCameras t = gtsam.Point3(r * cos(theta), r * sin(theta), height) - truth.cameras[i] = gtsam.SimpleCamera.Lookat(t, + truth.cameras[i] = gtsam.PinholeCameraCal3_S2.Lookat(t, gtsam.Point3(), gtsam.Point3(0, 0, 1), truth.K) diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index e3408b67b..e93e6ffca 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include using namespace gtsam; @@ -47,7 +47,7 @@ public: /// evaluate the error virtual Vector evaluateError(const Pose3& pose, boost::optional H = boost::none) const { - SimpleCamera camera(pose, *K_); + PinholeCamera camera(pose, *K_); return camera.project(P_, H, boost::none, boost::none) - p_; } }; diff --git a/examples/ISAM2Example_SmartFactor.cpp b/examples/ISAM2Example_SmartFactor.cpp index d8fee1516..46b9fcd47 100644 --- a/examples/ISAM2Example_SmartFactor.cpp +++ b/examples/ISAM2Example_SmartFactor.cpp @@ -4,7 +4,7 @@ * @author Alexander (pumaking on BitBucket) */ -#include +#include #include #include #include diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp index 25a6adf51..4ebc342eb 100644 --- a/examples/ImuFactorExample2.cpp +++ b/examples/ImuFactorExample2.cpp @@ -1,5 +1,5 @@ -#include +#include #include #include #include @@ -34,7 +34,7 @@ int main(int argc, char* argv[]) { double radius = 30; const Point3 up(0, 0, 1), target(0, 0, 0); const Point3 position(radius, 0, 0); - const SimpleCamera camera = SimpleCamera::Lookat(position, target, up); + const PinholeCamera camera = PinholeCamera::Lookat(position, target, up); const auto pose_0 = camera.pose(); // Now, create a constant-twist scenario that makes the camera orbit the diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 8c87fa315..16cd25dba 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -79,7 +79,7 @@ int main(int argc, char* argv[]) { // 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); + PinholeCamera camera(poses[i], *K); for (size_t j = 0; j < points.size(); ++j) { Point2 measurement = camera.project(points[j]); graph.emplace_shared >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); diff --git a/examples/SFMExampleExpressions.cpp b/examples/SFMExampleExpressions.cpp index 191664ef6..73b6f27f7 100644 --- a/examples/SFMExampleExpressions.cpp +++ b/examples/SFMExampleExpressions.cpp @@ -27,7 +27,7 @@ #include // Header order is close to far -#include +#include "SFMdata.h" #include #include #include @@ -67,7 +67,7 @@ int main(int argc, char* argv[]) { // Simulated measurements from each camera pose, adding them to the factor graph for (size_t i = 0; i < poses.size(); ++i) { Pose3_ x('x', i); - SimpleCamera camera(poses[i], K); + PinholeCamera camera(poses[i], K); for (size_t j = 0; j < points.size(); ++j) { Point2 measurement = camera.project(points[j]); // Below an expression for the prediction of the measurement: diff --git a/examples/SFMdata.h b/examples/SFMdata.h index 5462868c9..04d3c9e47 100644 --- a/examples/SFMdata.h +++ b/examples/SFMdata.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file SFMMdata.h + * @file SFMdata.h * @brief Simple example for the structure-from-motion problems * @author Duy-Nguyen Ta */ @@ -30,7 +30,8 @@ #include // We will also need a camera object to hold calibration information and perform projections. -#include +#include +#include /* ************************************************************************* */ std::vector createPoints() { diff --git a/examples/SelfCalibrationExample.cpp b/examples/SelfCalibrationExample.cpp index ba097c074..1d26ea0aa 100644 --- a/examples/SelfCalibrationExample.cpp +++ b/examples/SelfCalibrationExample.cpp @@ -61,7 +61,7 @@ int main(int argc, char* argv[]) { noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); for (size_t i = 0; i < poses.size(); ++i) { for (size_t j = 0; j < points.size(); ++j) { - SimpleCamera camera(poses[i], K); + PinholeCamera camera(poses[i], K); 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 diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp index 35bc9bcf6..db9090de6 100644 --- a/examples/VisualISAM2Example.cpp +++ b/examples/VisualISAM2Example.cpp @@ -89,7 +89,7 @@ int main(int argc, char* argv[]) { for (size_t i = 0; i < poses.size(); ++i) { // Add factors for each landmark observation for (size_t j = 0; j < points.size(); ++j) { - SimpleCamera camera(poses[i], *K); + PinholeCamera camera(poses[i], *K); Point2 measurement = camera.project(points[j]); graph.emplace_shared >( measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index 7a58deeb7..b4086910b 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -86,7 +86,7 @@ int main(int argc, char* argv[]) { // Add factors for each landmark observation for (size_t j = 0; j < points.size(); ++j) { // Create ground truth measurement - SimpleCamera camera(poses[i], *K); + PinholeCamera camera(poses[i], *K); Point2 measurement = camera.project(points[j]); // Add measurement graph.emplace_shared >(measurement, noise, diff --git a/gtsam.h b/gtsam.h index c0ed4d20f..caf377863 100644 --- a/gtsam.h +++ b/gtsam.h @@ -281,7 +281,7 @@ virtual class Value { }; #include -template +template virtual class GenericValue : gtsam::Value { void serializable() const; }; @@ -2133,7 +2133,7 @@ class Values { void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); - void insert(size_t j, const gtsam::SimpleCamera& simpel_camera); + void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert(size_t j, Vector vector); void insert(size_t j, Matrix matrix); @@ -2454,7 +2454,7 @@ class ISAM2 { template + gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, Vector, Matrix}> VALUE calculateEstimate(size_t key) const; gtsam::Values calculateBestEstimate() const; Matrix marginalCovariance(size_t key) const; @@ -2492,7 +2492,7 @@ class NonlinearISAM { #include #include -template +template virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2515,7 +2515,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { #include -template +template virtual class NonlinearEquality : gtsam::NoiseModelFactor { // Constructor - forces exact evaluation NonlinearEquality(size_t j, const T& feasible); @@ -2541,9 +2541,9 @@ typedef gtsam::RangeFactor RangeFactor3D; typedef gtsam::RangeFactor RangeFactorPose2; typedef gtsam::RangeFactor RangeFactorPose3; typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; -typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; +typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; typedef gtsam::RangeFactor RangeFactorCalibratedCamera; -typedef gtsam::RangeFactor RangeFactorSimpleCamera; +typedef gtsam::RangeFactor RangeFactorSimpleCamera; #include @@ -2634,7 +2634,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey); gtsam::Point2 measured() const; }; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; // due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2 //typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; @@ -3134,7 +3134,7 @@ namespace utilities { void perturbPoint2(gtsam::Values& values, double sigma, int seed); void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed); void perturbPoint3(gtsam::Values& values, double sigma, int seed); - void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth); + void insertBackprojections(gtsam::Values& values, const gtsam::PinholeCameraCal3_S2& c, Vector J, Matrix Z, double depth); void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h index 8214b0727..7c73f3cbd 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -187,8 +187,8 @@ struct HasBearing { }; // Similar helper class for to implement Range traits for classes with a range method -// For classes with overloaded range methods, such as SimpleCamera, this can even be templated: -// template struct Range : HasRange {}; +// For classes with overloaded range methods, such as PinholeCamera, this can even be templated: +// template struct Range : HasRange {}; template struct HasRange { typedef RT result_type; diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 97412f94d..d2f0c91df 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include @@ -151,7 +151,7 @@ TEST( triangulation, fourPoses) { // 3. Add a slightly rotated third camera above, again with measurement noise Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); - SimpleCamera camera3(pose3, *sharedCal); + PinholeCamera camera3(pose3, *sharedCal); Point2 z3 = camera3.project(landmark); poses += pose3; @@ -168,7 +168,7 @@ TEST( triangulation, fourPoses) { // 4. Test failure: Add a 4th camera facing the wrong way Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); - SimpleCamera camera4(pose4, *sharedCal); + PinholeCamera camera4(pose4, *sharedCal); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); @@ -185,17 +185,17 @@ TEST( triangulation, fourPoses) { TEST( triangulation, fourPoses_distinct_Ks) { Cal3_S2 K1(1500, 1200, 0, 640, 480); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - SimpleCamera camera1(pose1, K1); + PinholeCamera camera1(pose1, K1); // create second camera 1 meter to the right of first camera Cal3_S2 K2(1600, 1300, 0, 650, 440); - SimpleCamera camera2(pose2, K2); + PinholeCamera camera2(pose2, K2); // 1. Project two landmarks into two cameras and triangulate Point2 z1 = camera1.project(landmark); Point2 z2 = camera2.project(landmark); - CameraSet cameras; + CameraSet > cameras; Point2Vector measurements; cameras += camera1, camera2; @@ -216,7 +216,7 @@ TEST( triangulation, fourPoses_distinct_Ks) { // 3. Add a slightly rotated third camera above, again with measurement noise Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); Cal3_S2 K3(700, 500, 0, 640, 480); - SimpleCamera camera3(pose3, K3); + PinholeCamera camera3(pose3, K3); Point2 z3 = camera3.project(landmark); cameras += camera3; @@ -234,7 +234,7 @@ TEST( triangulation, fourPoses_distinct_Ks) { // 4. Test failure: Add a 4th camera facing the wrong way Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); Cal3_S2 K4(700, 500, 0, 640, 480); - SimpleCamera camera4(pose4, K4); + PinholeCamera camera4(pose4, K4); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); @@ -250,17 +250,17 @@ TEST( triangulation, fourPoses_distinct_Ks) { TEST( triangulation, outliersAndFarLandmarks) { Cal3_S2 K1(1500, 1200, 0, 640, 480); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - SimpleCamera camera1(pose1, K1); + PinholeCamera camera1(pose1, K1); // create second camera 1 meter to the right of first camera Cal3_S2 K2(1600, 1300, 0, 650, 440); - SimpleCamera camera2(pose2, K2); + PinholeCamera camera2(pose2, K2); // 1. Project two landmarks into two cameras and triangulate Point2 z1 = camera1.project(landmark); Point2 z2 = camera2.project(landmark); - CameraSet cameras; + CameraSet > cameras; Point2Vector measurements; cameras += camera1, camera2; @@ -280,7 +280,7 @@ TEST( triangulation, outliersAndFarLandmarks) { // 3. Add a slightly rotated third camera above with a wrong measurement (OUTLIER) Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); Cal3_S2 K3(700, 500, 0, 640, 480); - SimpleCamera camera3(pose3, K3); + PinholeCamera camera3(pose3, K3); Point2 z3 = camera3.project(landmark); cameras += camera3; @@ -302,7 +302,7 @@ TEST( triangulation, outliersAndFarLandmarks) { //****************************************************************************** TEST( triangulation, twoIdenticalPoses) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - SimpleCamera camera1(pose1, *sharedCal); + PinholeCamera camera1(pose1, *sharedCal); // 1. Project two landmarks into two cameras and triangulate Point2 z1 = camera1.project(landmark); diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index 7d02d479c..9e8aa3f29 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -66,7 +66,7 @@ public: // Expressions wrap trees of functions that can evaluate their own derivatives. // The meta-functions below are useful to specify the type of those functions. // Example, a function taking a camera and a 3D point and yielding a 2D point: - // Expression::BinaryFunction::type + // Expression::BinaryFunction,Point3>::type template struct UnaryFunction { typedef boost::function< diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index 867db70e0..cf9fb9482 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -28,7 +28,7 @@ #include #include #include -#include +#include #include @@ -169,7 +169,7 @@ void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { } /// Insert a number of initial point values by backprojecting -void insertBackprojections(Values& values, const SimpleCamera& camera, +void insertBackprojections(Values& values, const PinholeCamera& camera, const Vector& J, const Matrix& Z, double depth) { if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K"); diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp index 5d57886f5..0d19918f6 100644 --- a/gtsam/sam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -19,7 +19,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -353,11 +354,11 @@ TEST(RangeFactor, Point3) { } /* ************************************************************************* */ -// Do tests with SimpleCamera +// Do tests with PinholeCamera TEST( RangeFactor, Camera) { - RangeFactor factor1(poseKey, pointKey, measurement, model); - RangeFactor factor2(poseKey, pointKey, measurement, model); - RangeFactor factor3(poseKey, pointKey, measurement, model); + RangeFactor,Point3> factor1(poseKey, pointKey, measurement, model); + RangeFactor,Pose3> factor2(poseKey, pointKey, measurement, model); + RangeFactor,PinholeCamera> factor3(poseKey, pointKey, measurement, model); } /* ************************************************************************* */ diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 2e921bfef..8bd155a14 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -9,7 +9,7 @@ #include #include -#include +#include #include namespace gtsam { diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 4a8a6b138..856913aae 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -21,7 +21,8 @@ #pragma once #include -#include +#include +#include #include namespace gtsam { diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index b2d368b67..05a94dab9 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -13,6 +13,7 @@ #include #include #include +#include #include #include diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp index 1d2baefee..4bbef6530 100644 --- a/gtsam/slam/tests/testTriangulationFactor.cpp +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include @@ -39,7 +39,7 @@ static const boost::shared_ptr sharedCal = // // Looking along X-axis, 1 meter above ground plane (x-y) static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2); static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); -SimpleCamera camera1(pose1, *sharedCal); +PinholeCamera camera1(pose1, *sharedCal); // landmark ~5 meters infront of camera static const Point3 landmark(5, 0.5, 1.2); @@ -52,7 +52,7 @@ TEST( triangulation, TriangulationFactor ) { Key pointKey(1); SharedNoiseModel model; - typedef TriangulationFactor Factor; + typedef TriangulationFactor > Factor; Factor factor(camera1, z1, model, pointKey); // Use the factor to calculate the Jacobians diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp index 6ac3389ed..4c6251831 100644 --- a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp +++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp @@ -9,7 +9,8 @@ #include #include -#include +#include +#include #include @@ -18,7 +19,7 @@ using namespace gtsam; static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); -SimpleCamera level_camera(level_pose, *K); +PinholeCamera level_camera(level_pose, *K); /* ************************************************************************* */ TEST( InvDepthFactor, Project1) { diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index 3e1263bb9..5bef97bcf 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -21,7 +21,7 @@ #pragma once #include -#include +#include #include namespace gtsam { diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index 17c95e614..5b4bd8929 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -31,20 +31,21 @@ using namespace gtsam; // Creating as many permutations of factors as possible -typedef PriorFactor PriorFactorLieVector; -typedef PriorFactor PriorFactorLieMatrix; -typedef PriorFactor PriorFactorPoint2; -typedef PriorFactor PriorFactorStereoPoint2; -typedef PriorFactor PriorFactorPoint3; -typedef PriorFactor PriorFactorRot2; -typedef PriorFactor PriorFactorRot3; -typedef PriorFactor PriorFactorPose2; -typedef PriorFactor PriorFactorPose3; -typedef PriorFactor PriorFactorCal3_S2; -typedef PriorFactor PriorFactorCal3DS2; -typedef PriorFactor PriorFactorCalibratedCamera; -typedef PriorFactor PriorFactorSimpleCamera; -typedef PriorFactor PriorFactorStereoCamera; +typedef PriorFactor PriorFactorLieVector; +typedef PriorFactor PriorFactorLieMatrix; +typedef PriorFactor PriorFactorPoint2; +typedef PriorFactor PriorFactorStereoPoint2; +typedef PriorFactor PriorFactorPoint3; +typedef PriorFactor PriorFactorRot2; +typedef PriorFactor PriorFactorRot3; +typedef PriorFactor PriorFactorPose2; +typedef PriorFactor PriorFactorPose3; +typedef PriorFactor PriorFactorCal3_S2; +typedef PriorFactor PriorFactorCal3DS2; +typedef PriorFactor PriorFactorCalibratedCamera; +typedef PriorFactor PriorFactorSimpleCamera; +typedef PriorFactor PriorFactorPinholeCameraCal3_S2; +typedef PriorFactor PriorFactorStereoCamera; typedef BetweenFactor BetweenFactorLieVector; typedef BetweenFactor BetweenFactorLieMatrix; @@ -55,29 +56,32 @@ typedef BetweenFactor BetweenFactorRot3; typedef BetweenFactor BetweenFactorPose2; typedef BetweenFactor BetweenFactorPose3; -typedef NonlinearEquality NonlinearEqualityLieVector; -typedef NonlinearEquality NonlinearEqualityLieMatrix; -typedef NonlinearEquality NonlinearEqualityPoint2; -typedef NonlinearEquality NonlinearEqualityStereoPoint2; -typedef NonlinearEquality NonlinearEqualityPoint3; -typedef NonlinearEquality NonlinearEqualityRot2; -typedef NonlinearEquality NonlinearEqualityRot3; -typedef NonlinearEquality NonlinearEqualityPose2; -typedef NonlinearEquality NonlinearEqualityPose3; -typedef NonlinearEquality NonlinearEqualityCal3_S2; -typedef NonlinearEquality NonlinearEqualityCal3DS2; -typedef NonlinearEquality NonlinearEqualityCalibratedCamera; -typedef NonlinearEquality NonlinearEqualitySimpleCamera; -typedef NonlinearEquality NonlinearEqualityStereoCamera; +typedef NonlinearEquality NonlinearEqualityLieVector; +typedef NonlinearEquality NonlinearEqualityLieMatrix; +typedef NonlinearEquality NonlinearEqualityPoint2; +typedef NonlinearEquality NonlinearEqualityStereoPoint2; +typedef NonlinearEquality NonlinearEqualityPoint3; +typedef NonlinearEquality NonlinearEqualityRot2; +typedef NonlinearEquality NonlinearEqualityRot3; +typedef NonlinearEquality NonlinearEqualityPose2; +typedef NonlinearEquality NonlinearEqualityPose3; +typedef NonlinearEquality NonlinearEqualityCal3_S2; +typedef NonlinearEquality NonlinearEqualityCal3DS2; +typedef NonlinearEquality NonlinearEqualityCalibratedCamera; +typedef NonlinearEquality NonlinearEqualitySimpleCamera; +typedef NonlinearEquality NonlinearEqualityPinholeCameraCal3_S2; +typedef NonlinearEquality NonlinearEqualityStereoCamera; -typedef RangeFactor RangeFactor2D; -typedef RangeFactor RangeFactor3D; -typedef RangeFactor RangeFactorPose2; -typedef RangeFactor RangeFactorPose3; -typedef RangeFactor RangeFactorCalibratedCameraPoint; -typedef RangeFactor RangeFactorSimpleCameraPoint; -typedef RangeFactor RangeFactorCalibratedCamera; -typedef RangeFactor RangeFactorSimpleCamera; +typedef RangeFactor RangeFactor2D; +typedef RangeFactor RangeFactor3D; +typedef RangeFactor RangeFactorPose2; +typedef RangeFactor RangeFactorPose3; +typedef RangeFactor RangeFactorCalibratedCameraPoint; +typedef RangeFactor RangeFactorSimpleCameraPoint; +typedef RangeFactor RangeFactorPinholeCameraCal3_S2Point; +typedef RangeFactor RangeFactorCalibratedCamera; +typedef RangeFactor RangeFactorSimpleCamera; +typedef RangeFactor RangeFactorPinholeCameraCal3_S2; typedef BearingRangeFactor BearingRangeFactor2D; typedef BearingRangeFactor BearingRangeFactor3D; @@ -85,7 +89,7 @@ typedef BearingRangeFactor BearingRangeFactor3D; typedef GenericProjectionFactor GenericProjectionFactorCal3_S2; typedef GenericProjectionFactor GenericProjectionFactorCal3DS2; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; //typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; typedef gtsam::GeneralSFMFactor2 GeneralSFMFactor2Cal3_S2; @@ -126,6 +130,7 @@ GTSAM_VALUE_EXPORT(gtsam::Cal3DS2); GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo); GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera); GTSAM_VALUE_EXPORT(gtsam::SimpleCamera); +GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2); GTSAM_VALUE_EXPORT(gtsam::StereoCamera); /* Create GUIDs for factors */ diff --git a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp index a74bfc3c6..14ad43ae2 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp @@ -7,7 +7,7 @@ #include -#include +#include #include #include #include @@ -23,7 +23,7 @@ static SharedNoiseModel sigma(noiseModel::Unit::Create(2)); // camera pose at (0,0,1) looking straight along the x-axis. Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); -SimpleCamera level_camera(level_pose, *K); +PinholeCamera level_camera(level_pose, *K); typedef InvDepthFactor3 InverseDepthFactor; typedef NonlinearEquality PoseConstraint; @@ -64,7 +64,7 @@ TEST( InvDepthFactor, optimize) { // add a camera 2 meters to the right Pose3 right_pose = level_pose * Pose3(Rot3(), Point3(2,0,0)); - SimpleCamera right_camera(right_pose, *K); + PinholeCamera right_camera(right_pose, *K); // projection measurement of landmark into right camera // this measurement disagrees with the depth initialization diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 0ac2c24ee..14ac52c45 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -523,9 +523,9 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){ Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0)); Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0)); - SimpleCamera cam1(cameraPose1, *K); // with camera poses - SimpleCamera cam2(cameraPose2, *K); - SimpleCamera cam3(cameraPose3, *K); + PinholeCamera cam1(cameraPose1, *K); // with camera poses + PinholeCamera cam2(cameraPose2, *K); + PinholeCamera cam3(cameraPose3, *K); // create arbitrary body_Pose_sensor (transforms from sensor to body) Pose3 sensor_to_body = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); // diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m index 10fd5e142..035e7e509 100644 --- a/matlab/+gtsam/Contents.m +++ b/matlab/+gtsam/Contents.m @@ -94,6 +94,7 @@ % Rot2 - class Rot2, see Doxygen page for details % Rot3 - class Rot3, see Doxygen page for details % SimpleCamera - class SimpleCamera, see Doxygen page for details +% PinholeCameraCal3_S2 - class PinholeCameraCal3_S2, see Doxygen page for details % StereoPoint2 - class StereoPoint2, see Doxygen page for details % %% SLAM and SFM diff --git a/matlab/+gtsam/VisualISAMGenerateData.m b/matlab/+gtsam/VisualISAMGenerateData.m index ab47e92db..57f9439a4 100644 --- a/matlab/+gtsam/VisualISAMGenerateData.m +++ b/matlab/+gtsam/VisualISAMGenerateData.m @@ -31,7 +31,7 @@ data.K = truth.K; for i=1:options.nrCameras theta = (i-1)*2*pi/options.nrCameras; t = Point3([r*cos(theta), r*sin(theta), height]'); - truth.cameras{i} = SimpleCamera.Lookat(t, Point3, Point3([0,0,1]'), truth.K); + truth.cameras{i} = PinholeCameraCal3_S2.Lookat(t, Point3, Point3([0,0,1]'), truth.K); % Create measurements for j=1:nrPoints % All landmarks seen in every frame diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m index 71f72f6b9..2b913b52d 100644 --- a/matlab/+gtsam/cylinderSampleProjection.m +++ b/matlab/+gtsam/cylinderSampleProjection.m @@ -13,7 +13,7 @@ function [visiblePoints] = cylinderSampleProjection(K, pose, imageSize, cylinder import gtsam.* -camera = SimpleCamera(pose, K); +camera = PinholeCameraCal3_S2(pose, K); %% memory allocation cylinderNum = length(cylinders); diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 36b7635e2..add2bc75a 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -92,7 +92,7 @@ if options.enableTests cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0)); end - camera = SimpleCamera.Lookat(Point3(10, 50, 10), ... + camera = PinholeCameraCal3_S2.Lookat(Point3(10, 50, 10), ... Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... Point3([0,0,1]'), options.monoK); @@ -154,7 +154,7 @@ while 1 %t = Point3([(i-1)*(options.fieldSize.x - 10)/options.poseNum + 10, ... % 15, 10]'); - camera = SimpleCamera.Lookat(t, ... + camera = PinholeCameraCal3_S2.Lookat(t, ... Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ... Point3([0,0,1]'), options.camera.monoK); cameraPoses{end+1} = camera.pose; diff --git a/matlab/gtsam_examples/SBAExample.m b/matlab/gtsam_examples/SBAExample.m index 7f50f2db8..584ace09a 100644 --- a/matlab/gtsam_examples/SBAExample.m +++ b/matlab/gtsam_examples/SBAExample.m @@ -46,7 +46,7 @@ end %% Add Gaussian priors for a pose and a landmark to constrain the system cameraPriorNoise = noiseModel.Diagonal.Sigmas(cameraNoiseSigmas); -firstCamera = SimpleCamera(truth.cameras{1}.pose, truth.K); +firstCamera = PinholeCameraCal3_S2(truth.cameras{1}.pose, truth.K); graph.add(PriorFactorSimpleCamera(symbol('c',1), firstCamera, cameraPriorNoise)); pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma); @@ -60,7 +60,7 @@ graph.print(sprintf('\nFactor graph:\n')); initialEstimate = Values; for i=1:size(truth.cameras,2) pose_i = truth.cameras{i}.pose.retract(0.1*randn(6,1)); - camera_i = SimpleCamera(pose_i, truth.K); + camera_i = PinholeCameraCal3_S2(pose_i, truth.K); initialEstimate.insert(symbol('c',i), camera_i); end for j=1:size(truth.points,2) diff --git a/matlab/gtsam_tests/testTriangulation.m b/matlab/gtsam_tests/testTriangulation.m index d46493328..7116d3838 100644 --- a/matlab/gtsam_tests/testTriangulation.m +++ b/matlab/gtsam_tests/testTriangulation.m @@ -18,11 +18,11 @@ sharedCal = Cal3_S2(1500, 1200, 0, 640, 480); %% Looking along X-axis, 1 meter above ground plane (x-y) upright = Rot3.Ypr(-pi / 2, 0., -pi / 2); pose1 = Pose3(upright, Point3(0, 0, 1)); -camera1 = SimpleCamera(pose1, sharedCal); +camera1 = PinholeCameraCal3_S2(pose1, sharedCal); %% create second camera 1 meter to the right of first camera pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))); -camera2 = SimpleCamera(pose2, sharedCal); +camera2 = PinholeCameraCal3_S2(pose2, sharedCal); %% landmark ~5 meters infront of camera landmark =Point3 (5, 0.5, 1.2); diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index 2cceea753..195b7ff69 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -97,8 +97,8 @@ if options.useRealData == 1 % Create the camera based on the current pose and the pose of the % camera in the body cameraPose = currentPose.compose(metadata.camera.bodyPoseCamera); - camera = SimpleCamera(cameraPose, metadata.camera.calibration); - %camera = SimpleCamera(currentPose, metadata.camera.calibration); + camera = PinholeCameraCal3_S2(cameraPose, metadata.camera.calibration); + %camera = PinholeCameraCal3_S2(currentPose, metadata.camera.calibration); % Record measurements if the landmark is within visual range for j=1:length(metadata.camera.gtLandmarkPoints) diff --git a/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m index c9e912ea4..129b8c176 100644 --- a/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m +++ b/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m @@ -108,7 +108,7 @@ for i=1:20 % generate some camera measurements cam_pose = initial.atPose3(i).compose(actual_transform); % gtsam.plotPose3(cam_pose); - cam = SimpleCamera(cam_pose,K); + cam = PinholeCameraCal3_S2(cam_pose,K); i % result for j=1:nrPoints diff --git a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m index fd4a17469..4557d711f 100644 --- a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m +++ b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m @@ -182,7 +182,7 @@ for i=1:steps % generate some camera measurements cam_pose = currentIMUPoseGlobal.compose(actual_transform); % gtsam.plotPose3(cam_pose); - cam = SimpleCamera(cam_pose,K); + cam = PinholeCameraCal3_S2(cam_pose,K); i % result for j=1:nrPoints diff --git a/matlab/unstable_examples/TransformProjectionFactorExample.m b/matlab/unstable_examples/TransformProjectionFactorExample.m index 669073e56..79a96209d 100644 --- a/matlab/unstable_examples/TransformProjectionFactorExample.m +++ b/matlab/unstable_examples/TransformProjectionFactorExample.m @@ -73,7 +73,7 @@ for i=1:20 % generate some camera measurements cam_pose = initial.atPose3(i).compose(actual_transform); gtsam.plotPose3(cam_pose); - cam = SimpleCamera(cam_pose,K); + cam = PinholeCameraCal3_S2(cam_pose,K); i for j=1:nrPoints % All landmarks seen in every frame diff --git a/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m index 8edcfade7..ca5b70c62 100644 --- a/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m +++ b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m @@ -98,7 +98,7 @@ for i=1:20 % generate some camera measurements cam_pose = initial.atPose3(i).compose(actual_transform); % gtsam.plotPose3(cam_pose); - cam = SimpleCamera(cam_pose,K); + cam = PinholeCameraCal3_S2(cam_pose,K); i % result for j=1:nrPoints diff --git a/matlab/unstable_examples/project_landmarks.m b/matlab/unstable_examples/project_landmarks.m index 629c6d994..aaccc9248 100644 --- a/matlab/unstable_examples/project_landmarks.m +++ b/matlab/unstable_examples/project_landmarks.m @@ -4,7 +4,7 @@ function [ measurements ] = project_landmarks( pose, landmarks, K ) import gtsam.*; - camera = SimpleCamera(pose,K); + camera = PinholeCameraCal3_S2(pose,K); measurements = Values; for i=1:size(landmarks)-1 diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 95843e5ab..d59666655 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -28,7 +28,7 @@ #include #include #include -#include +#include #include @@ -527,10 +527,10 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { Rot3 faceTowardsY(Point3(1, 0, 0), Point3(0, 0, -1), Point3(0, 1, 0)); Pose3 poseLeft(faceTowardsY, Point3(0, 0, 0)); // origin, left camera - SimpleCamera leftCamera(poseLeft, K); + PinholeCamera leftCamera(poseLeft, K); Pose3 poseRight(faceTowardsY, Point3(2, 0, 0)); // 2 units to the right - SimpleCamera rightCamera(poseRight, K); + PinholeCamera rightCamera(poseRight, K); Point3 landmark(1, 5, 0); //centered between the cameras, 5 units away diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index 64e111e94..9222894a4 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -47,7 +47,7 @@ #include #include #include -#include +#include #include #include @@ -57,20 +57,21 @@ using namespace gtsam; using namespace gtsam::serializationTestHelpers; // Creating as many permutations of factors as possible -typedef PriorFactor PriorFactorLieVector; -typedef PriorFactor PriorFactorLieMatrix; -typedef PriorFactor PriorFactorPoint2; -typedef PriorFactor PriorFactorStereoPoint2; -typedef PriorFactor PriorFactorPoint3; -typedef PriorFactor PriorFactorRot2; -typedef PriorFactor PriorFactorRot3; -typedef PriorFactor PriorFactorPose2; -typedef PriorFactor PriorFactorPose3; -typedef PriorFactor PriorFactorCal3_S2; -typedef PriorFactor PriorFactorCal3DS2; -typedef PriorFactor PriorFactorCalibratedCamera; -typedef PriorFactor PriorFactorSimpleCamera; -typedef PriorFactor PriorFactorStereoCamera; +typedef PriorFactor PriorFactorLieVector; +typedef PriorFactor PriorFactorLieMatrix; +typedef PriorFactor PriorFactorPoint2; +typedef PriorFactor PriorFactorStereoPoint2; +typedef PriorFactor PriorFactorPoint3; +typedef PriorFactor PriorFactorRot2; +typedef PriorFactor PriorFactorRot3; +typedef PriorFactor PriorFactorPose2; +typedef PriorFactor PriorFactorPose3; +typedef PriorFactor PriorFactorCal3_S2; +typedef PriorFactor PriorFactorCal3DS2; +typedef PriorFactor PriorFactorCalibratedCamera; +typedef PriorFactor PriorFactorSimpleCamera; +typedef PriorFactor PriorFactorPinholeCameraCal3_S2; +typedef PriorFactor PriorFactorStereoCamera; typedef BetweenFactor BetweenFactorLieVector; typedef BetweenFactor BetweenFactorLieMatrix; @@ -81,29 +82,32 @@ typedef BetweenFactor BetweenFactorRot3; typedef BetweenFactor BetweenFactorPose2; typedef BetweenFactor BetweenFactorPose3; -typedef NonlinearEquality NonlinearEqualityLieVector; -typedef NonlinearEquality NonlinearEqualityLieMatrix; -typedef NonlinearEquality NonlinearEqualityPoint2; -typedef NonlinearEquality NonlinearEqualityStereoPoint2; -typedef NonlinearEquality NonlinearEqualityPoint3; -typedef NonlinearEquality NonlinearEqualityRot2; -typedef NonlinearEquality NonlinearEqualityRot3; -typedef NonlinearEquality NonlinearEqualityPose2; -typedef NonlinearEquality NonlinearEqualityPose3; -typedef NonlinearEquality NonlinearEqualityCal3_S2; -typedef NonlinearEquality NonlinearEqualityCal3DS2; -typedef NonlinearEquality NonlinearEqualityCalibratedCamera; -typedef NonlinearEquality NonlinearEqualitySimpleCamera; -typedef NonlinearEquality NonlinearEqualityStereoCamera; +typedef NonlinearEquality NonlinearEqualityLieVector; +typedef NonlinearEquality NonlinearEqualityLieMatrix; +typedef NonlinearEquality NonlinearEqualityPoint2; +typedef NonlinearEquality NonlinearEqualityStereoPoint2; +typedef NonlinearEquality NonlinearEqualityPoint3; +typedef NonlinearEquality NonlinearEqualityRot2; +typedef NonlinearEquality NonlinearEqualityRot3; +typedef NonlinearEquality NonlinearEqualityPose2; +typedef NonlinearEquality NonlinearEqualityPose3; +typedef NonlinearEquality NonlinearEqualityCal3_S2; +typedef NonlinearEquality NonlinearEqualityCal3DS2; +typedef NonlinearEquality NonlinearEqualityCalibratedCamera; +typedef NonlinearEquality NonlinearEqualitySimpleCamera; +typedef NonlinearEquality NonlinearEqualityPinholeCameraCal3_S2; +typedef NonlinearEquality NonlinearEqualityStereoCamera; -typedef RangeFactor RangeFactor2D; -typedef RangeFactor RangeFactor3D; -typedef RangeFactor RangeFactorPose2; -typedef RangeFactor RangeFactorPose3; -typedef RangeFactor RangeFactorCalibratedCameraPoint; -typedef RangeFactor RangeFactorSimpleCameraPoint; -typedef RangeFactor RangeFactorCalibratedCamera; -typedef RangeFactor RangeFactorSimpleCamera; +typedef RangeFactor RangeFactor2D; +typedef RangeFactor RangeFactor3D; +typedef RangeFactor RangeFactorPose2; +typedef RangeFactor RangeFactorPose3; +typedef RangeFactor RangeFactorCalibratedCameraPoint; +typedef RangeFactor RangeFactorSimpleCameraPoint; +typedef RangeFactor RangeFactorPinholeCameraCal3_S2Point; +typedef RangeFactor RangeFactorCalibratedCamera; +typedef RangeFactor RangeFactorSimpleCamera; +typedef RangeFactor RangeFactorPinholeCameraCal3_S2; typedef BearingRangeFactor BearingRangeFactor2D; typedef BearingRangeFactor BearingRangeFactor3D; @@ -111,7 +115,7 @@ typedef BearingRangeFactor BearingRangeFactor3D; typedef GenericProjectionFactor GenericProjectionFactorCal3_S2; typedef GenericProjectionFactor GenericProjectionFactorCal3DS2; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; //typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; typedef gtsam::GeneralSFMFactor2 GeneralSFMFactor2Cal3_S2; @@ -158,6 +162,7 @@ GTSAM_VALUE_EXPORT(gtsam::Cal3DS2); GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo); GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera); GTSAM_VALUE_EXPORT(gtsam::SimpleCamera); +GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2); GTSAM_VALUE_EXPORT(gtsam::StereoCamera); /* Create GUIDs for factors */ @@ -294,7 +299,7 @@ TEST (testSerializationSLAM, factors) { Cal3DS2 cal3ds2(1.0, 2.0, 3.0, 4.0, 5.0,6.0, 7.0, 8.0, 9.0); Cal3_S2Stereo cal3_s2stereo(1.0, 2.0, 3.0, 4.0, 5.0, 1.0); CalibratedCamera calibratedCamera(pose3); - SimpleCamera simpleCamera(pose3, cal3_s2); + PinholeCamera simpleCamera(pose3, cal3_s2); StereoCamera stereoCamera(pose3, boost::make_shared(cal3_s2stereo)); diff --git a/tests/testVisualISAM2.cpp b/tests/testVisualISAM2.cpp index 9c63e1aa8..182408004 100644 --- a/tests/testVisualISAM2.cpp +++ b/tests/testVisualISAM2.cpp @@ -59,7 +59,7 @@ TEST(testVisualISAM2, all) // Add factors for each landmark observation for (size_t j = 0; j < points.size(); ++j) { - SimpleCamera camera(poses[i], *K); + PinholeCamera camera(poses[i], *K); Point2 measurement = camera.project(points[j]); graph.emplace_shared>( measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); From e312abdbf0a77dcbba6fd255cd51896531d7401f Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 1 Mar 2020 15:22:19 -0500 Subject: [PATCH 05/79] Add More Unit Tests for Robust Noise Models --- tests/testNonlinearOptimizer.cpp | 191 +++++++++++++++++++++++++++++-- 1 file changed, 180 insertions(+), 11 deletions(-) diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 0e7793552..7cf5e1e2d 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -339,31 +340,199 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { } /* ************************************************************************* */ -TEST(NonlinearOptimizer, MoreOptimizationWithHuber) { +TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) { NonlinearFactorGraph fg; fg += PriorFactor(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1)); - fg += BetweenFactor(0, 1, Pose2(1,0,M_PI/2), + fg += BetweenFactor(0, 1, Pose2(1,1.1,M_PI/4), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(2.0), noiseModel::Isotropic::Sigma(3,1))); - fg += BetweenFactor(1, 2, Pose2(1,0,M_PI/2), + fg += BetweenFactor(0, 1, Pose2(1,0.9,M_PI/2), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(3.0), noiseModel::Isotropic::Sigma(3,1))); Values init; - init.insert(0, Pose2(10,10,0)); - init.insert(1, Pose2(1,0,M_PI)); - init.insert(2, Pose2(1,1,-M_PI)); + init.insert(0, Pose2(0,0,0)); + init.insert(1, Pose2(0.961187, 0.99965, 1.1781)); Values expected; expected.insert(0, Pose2(0,0,0)); - expected.insert(1, Pose2(1,0,M_PI/2)); - expected.insert(2, Pose2(1,1,M_PI)); + expected.insert(1, Pose2(0.961187, 0.99965, 1.1781)); + + LevenbergMarquardtParams lm_params; + lm_params.setRelativeErrorTol(0); + lm_params.setAbsoluteErrorTol(0); + lm_params.setMaxIterations(100); + lm_params.setlambdaUpperBound(1e10); + // lm_params.setVerbosityLM("TRYLAMBDA"); + + gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; + cg_params.setErrorTol(0); + cg_params.setMaxIterations(100000); + cg_params.setRelativeErrorTol(0); + cg_params.setAbsoluteErrorTol(0); + + DoglegParams dl_params; + dl_params.setRelativeErrorTol(0); + dl_params.setAbsoluteErrorTol(0); + dl_params.setMaxIterations(100); + + // cg_params.setVerbosity("ERROR"); + + auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); + auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); + auto lm_result = LevenbergMarquardtOptimizer(fg, init, lm_params).optimize(); + auto dl_result = DoglegOptimizer(fg, init, dl_params).optimize(); + + EXPECT(assert_equal(expected, cg_result, 3e-2)); + EXPECT(assert_equal(expected, gn_result, 3e-2)); + EXPECT(assert_equal(expected, lm_result, 3e-2)); + EXPECT(assert_equal(expected, dl_result, 3e-2)); +} + +/* ************************************************************************* */ +TEST(NonlinearOptimizer, Point2LinearOptimizationWithHuber) { + + NonlinearFactorGraph fg; + fg += PriorFactor(0, Point2(0,0), noiseModel::Isotropic::Sigma(2,0.01)); + fg += BetweenFactor(0, 1, Point2(1,1.8), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), + noiseModel::Isotropic::Sigma(2,1))); + fg += BetweenFactor(0, 1, Point2(1,0.9), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), + noiseModel::Isotropic::Sigma(2,1))); + fg += BetweenFactor(0, 1, Point2(1,90), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), + noiseModel::Isotropic::Sigma(2,1))); + + Values init; + init.insert(0, Point2(1,1)); + init.insert(1, Point2(1,0)); + + Values expected; + expected.insert(0, Point2(0,0)); + expected.insert(1, Point2(1,1.85)); LevenbergMarquardtParams params; - EXPECT(assert_equal(expected, GaussNewtonOptimizer(fg, init).optimize())); - EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(fg, init, params).optimize())); - EXPECT(assert_equal(expected, DoglegOptimizer(fg, init).optimize())); + gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; + cg_params.setErrorTol(0); + cg_params.setMaxIterations(100000); + cg_params.setRelativeErrorTol(0); + cg_params.setAbsoluteErrorTol(0); + + // cg_params.setVerbosity("ERROR"); + auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); + + auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); + + auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); + + auto dl_result = DoglegOptimizer(fg, init).optimize(); + + EXPECT(assert_equal(expected, gn_result, 1e-4)); + EXPECT(assert_equal(expected, gn_result, 1e-4)); + EXPECT(assert_equal(expected, lm_result, 1e-4)); + EXPECT(assert_equal(expected, dl_result, 1e-4)); +} + +/* ************************************************************************* */ +TEST(NonlinearOptimizer, Pose2OptimizationWithHuber) { + + NonlinearFactorGraph fg; + fg += PriorFactor(0, Pose2(0,0, 0), noiseModel::Isotropic::Sigma(3,0.1)); + fg += BetweenFactor(0, 1, Pose2(0,9, M_PI/2), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), + noiseModel::Isotropic::Sigma(3,1))); + fg += BetweenFactor(0, 1, Pose2(0, 11, M_PI/2), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), + noiseModel::Isotropic::Sigma(3,1))); + fg += BetweenFactor(0, 1, Pose2(0, 10, M_PI/2), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), + noiseModel::Isotropic::Sigma(3,1))); + fg += BetweenFactor(0, 1, Pose2(0,9, 0), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), + noiseModel::Isotropic::Sigma(3,1))); + + Values init; + init.insert(0, Pose2(0, 0, 0)); + init.insert(1, Pose2(0, 10, M_PI/4)); + + Values expected; + expected.insert(0, Pose2(0, 0, 0)); + expected.insert(1, Pose2(0, 10, 1.45212)); + + LevenbergMarquardtParams params; + gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; + cg_params.setErrorTol(0); + cg_params.setMaxIterations(100000); + cg_params.setRelativeErrorTol(0); + cg_params.setAbsoluteErrorTol(0); + + // cg_params.setVerbosity("ERROR"); + auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); + fg.printErrors(cg_result); + auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); + + auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); + + auto dl_result = DoglegOptimizer(fg, init).optimize(); + + EXPECT(assert_equal(expected, gn_result, 1e-1)); + EXPECT(assert_equal(expected, gn_result, 1e-1)); + EXPECT(assert_equal(expected, lm_result, 1e-1)); + EXPECT(assert_equal(expected, dl_result, 1e-1)); +} + +/* ************************************************************************* */ +TEST(NonlinearOptimizer, RobustMeanCalculation) { + + NonlinearFactorGraph fg; + + Values init; + + Values expected; + + auto huber = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(20), + noiseModel::Isotropic::Sigma(1, 1)); + + vector pts{-10,-3,-1,1,3,10,1000}; + for(auto pt : pts) { + fg += PriorFactor(0, pt, huber); + } + + init.insert(0, 100.0); + expected.insert(0, 3.33333333); + + LevenbergMarquardtParams params; + // params.setVerbosityLM("TRYLAMBDA"); + params.setAbsoluteErrorTol(1e-20); + params.setRelativeErrorTol(1e-20); + + gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; + cg_params.setErrorTol(0); + cg_params.setMaxIterations(10000); + cg_params.setRelativeErrorTol(0); + cg_params.setAbsoluteErrorTol(0); + // cg_params.setVerbosity("ERROR"); + auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); +// cg_result.print("CG: "); +// cout << fg.error(cg_result) << endl << endl << endl; + + auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); +// gn_result.print("GN: "); +// cout << fg.error(gn_result) << endl << endl << endl; + + auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); +// lm_result.print("LM: "); +// cout << fg.error(lm_result) << endl << endl << endl; + + auto dl_result = DoglegOptimizer(fg, init).optimize(); +// dl_result.print("DL: "); +// cout << fg.error(dl_result) << endl << endl << endl; + EXPECT(assert_equal(expected, gn_result, tol)); + EXPECT(assert_equal(expected, gn_result, tol)); + EXPECT(assert_equal(expected, lm_result, tol)); + EXPECT(assert_equal(expected, dl_result, tol)); } /* ************************************************************************* */ From 3c0671ba8de1a5fa9b115fc8cf6e5ac3b768ce41 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 1 Mar 2020 19:38:57 -0500 Subject: [PATCH 06/79] Removed commentted out and print-s --- tests/testNonlinearOptimizer.cpp | 34 ++++---------------------------- 1 file changed, 4 insertions(+), 30 deletions(-) diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 7cf5e1e2d..4b7ca6a03 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -364,9 +364,8 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) { lm_params.setAbsoluteErrorTol(0); lm_params.setMaxIterations(100); lm_params.setlambdaUpperBound(1e10); - // lm_params.setVerbosityLM("TRYLAMBDA"); - gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; + NonlinearConjugateGradientOptimizer::Parameters cg_params; cg_params.setErrorTol(0); cg_params.setMaxIterations(100000); cg_params.setRelativeErrorTol(0); @@ -377,8 +376,6 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) { dl_params.setAbsoluteErrorTol(0); dl_params.setMaxIterations(100); - // cg_params.setVerbosity("ERROR"); - auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); auto lm_result = LevenbergMarquardtOptimizer(fg, init, lm_params).optimize(); @@ -414,19 +411,11 @@ TEST(NonlinearOptimizer, Point2LinearOptimizationWithHuber) { expected.insert(1, Point2(1,1.85)); LevenbergMarquardtParams params; - gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; - cg_params.setErrorTol(0); - cg_params.setMaxIterations(100000); - cg_params.setRelativeErrorTol(0); - cg_params.setAbsoluteErrorTol(0); + NonlinearConjugateGradientOptimizer::Parameters cg_params; - // cg_params.setVerbosity("ERROR"); auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); - auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); - auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); - auto dl_result = DoglegOptimizer(fg, init).optimize(); EXPECT(assert_equal(expected, gn_result, 1e-4)); @@ -468,13 +457,9 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuber) { cg_params.setRelativeErrorTol(0); cg_params.setAbsoluteErrorTol(0); - // cg_params.setVerbosity("ERROR"); auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); - fg.printErrors(cg_result); auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); - auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); - auto dl_result = DoglegOptimizer(fg, init).optimize(); EXPECT(assert_equal(expected, gn_result, 1e-1)); @@ -504,7 +489,6 @@ TEST(NonlinearOptimizer, RobustMeanCalculation) { expected.insert(0, 3.33333333); LevenbergMarquardtParams params; - // params.setVerbosityLM("TRYLAMBDA"); params.setAbsoluteErrorTol(1e-20); params.setRelativeErrorTol(1e-20); @@ -513,22 +497,12 @@ TEST(NonlinearOptimizer, RobustMeanCalculation) { cg_params.setMaxIterations(10000); cg_params.setRelativeErrorTol(0); cg_params.setAbsoluteErrorTol(0); - // cg_params.setVerbosity("ERROR"); + auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); -// cg_result.print("CG: "); -// cout << fg.error(cg_result) << endl << endl << endl; - auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); -// gn_result.print("GN: "); -// cout << fg.error(gn_result) << endl << endl << endl; - auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); -// lm_result.print("LM: "); -// cout << fg.error(lm_result) << endl << endl << endl; - auto dl_result = DoglegOptimizer(fg, init).optimize(); -// dl_result.print("DL: "); -// cout << fg.error(dl_result) << endl << endl << endl; + EXPECT(assert_equal(expected, gn_result, tol)); EXPECT(assert_equal(expected, gn_result, tol)); EXPECT(assert_equal(expected, lm_result, tol)); From 4babfe24911eba8348ce5ff857e68637be524862 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 3 Mar 2020 17:39:28 -0500 Subject: [PATCH 07/79] Remove redundant params --- tests/testNonlinearOptimizer.cpp | 39 +------------------------------- 1 file changed, 1 insertion(+), 38 deletions(-) diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 4b7ca6a03..7b5e7a0e0 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -360,28 +360,11 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) { expected.insert(1, Pose2(0.961187, 0.99965, 1.1781)); LevenbergMarquardtParams lm_params; - lm_params.setRelativeErrorTol(0); - lm_params.setAbsoluteErrorTol(0); - lm_params.setMaxIterations(100); - lm_params.setlambdaUpperBound(1e10); - NonlinearConjugateGradientOptimizer::Parameters cg_params; - cg_params.setErrorTol(0); - cg_params.setMaxIterations(100000); - cg_params.setRelativeErrorTol(0); - cg_params.setAbsoluteErrorTol(0); - - DoglegParams dl_params; - dl_params.setRelativeErrorTol(0); - dl_params.setAbsoluteErrorTol(0); - dl_params.setMaxIterations(100); - - auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); auto lm_result = LevenbergMarquardtOptimizer(fg, init, lm_params).optimize(); - auto dl_result = DoglegOptimizer(fg, init, dl_params).optimize(); + auto dl_result = DoglegOptimizer(fg, init).optimize(); - EXPECT(assert_equal(expected, cg_result, 3e-2)); EXPECT(assert_equal(expected, gn_result, 3e-2)); EXPECT(assert_equal(expected, lm_result, 3e-2)); EXPECT(assert_equal(expected, dl_result, 3e-2)); @@ -411,14 +394,11 @@ TEST(NonlinearOptimizer, Point2LinearOptimizationWithHuber) { expected.insert(1, Point2(1,1.85)); LevenbergMarquardtParams params; - NonlinearConjugateGradientOptimizer::Parameters cg_params; - auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); auto dl_result = DoglegOptimizer(fg, init).optimize(); - EXPECT(assert_equal(expected, gn_result, 1e-4)); EXPECT(assert_equal(expected, gn_result, 1e-4)); EXPECT(assert_equal(expected, lm_result, 1e-4)); EXPECT(assert_equal(expected, dl_result, 1e-4)); @@ -451,18 +431,11 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuber) { expected.insert(1, Pose2(0, 10, 1.45212)); LevenbergMarquardtParams params; - gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; - cg_params.setErrorTol(0); - cg_params.setMaxIterations(100000); - cg_params.setRelativeErrorTol(0); - cg_params.setAbsoluteErrorTol(0); - auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); auto dl_result = DoglegOptimizer(fg, init).optimize(); - EXPECT(assert_equal(expected, gn_result, 1e-1)); EXPECT(assert_equal(expected, gn_result, 1e-1)); EXPECT(assert_equal(expected, lm_result, 1e-1)); EXPECT(assert_equal(expected, dl_result, 1e-1)); @@ -489,21 +462,11 @@ TEST(NonlinearOptimizer, RobustMeanCalculation) { expected.insert(0, 3.33333333); LevenbergMarquardtParams params; - params.setAbsoluteErrorTol(1e-20); - params.setRelativeErrorTol(1e-20); - gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; - cg_params.setErrorTol(0); - cg_params.setMaxIterations(10000); - cg_params.setRelativeErrorTol(0); - cg_params.setAbsoluteErrorTol(0); - - auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); auto dl_result = DoglegOptimizer(fg, init).optimize(); - EXPECT(assert_equal(expected, gn_result, tol)); EXPECT(assert_equal(expected, gn_result, tol)); EXPECT(assert_equal(expected, lm_result, tol)); EXPECT(assert_equal(expected, dl_result, tol)); From 0e6efb98d7f3e8b7eb5b38dfc8830d8b19c2f670 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 6 Mar 2020 16:28:31 -0500 Subject: [PATCH 08/79] compute angle in degrees between 2 Rot3 matrices --- gtsam.h | 1 + gtsam/geometry/Rot3.cpp | 5 +++++ gtsam/geometry/Rot3.h | 6 ++++++ gtsam/geometry/tests/testRot3.cpp | 15 +++++++++++++++ 4 files changed, 27 insertions(+) diff --git a/gtsam.h b/gtsam.h index f943c2e9d..6e2eea686 100644 --- a/gtsam.h +++ b/gtsam.h @@ -677,6 +677,7 @@ class Rot3 { // Vector toQuaternion() const; // FIXME: Can't cast to Vector properly Vector quaternion() const; gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const; + double angle(const gtsam::Rot3& other) const; // enabling serialization functionality void serialize() const; diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index b1e8dd14b..4eee045f9 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -228,6 +228,11 @@ Rot3 Rot3::slerp(double t, const Rot3& other) const { return interpolate(*this, other, t); } +/* ************************************************************************* */ +double Rot3::angle(const Rot3& other) const { + return Rot3::Logmap(this->between(other)).norm() * 180.0 / M_PI; +} + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index e4408c9f4..d810285fb 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -479,6 +479,12 @@ namespace gtsam { */ Rot3 slerp(double t, const Rot3& other) const; + /** + * @brief Compute the angle (in degrees) between *this and other + * @param other Rot3 element + */ + double angle(const Rot3& other) const; + /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index c95b85f21..84c5bd094 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -661,6 +661,21 @@ TEST(Rot3, ClosestTo) { EXPECT(assert_equal(expected, actual.matrix(), 1e-6)); } +/* ************************************************************************* */ +TEST(Rot3, angle) { + Rot3 R1(0.996136, -0.0564186, -0.0672982, // + 0.0419472, 0.978941, -0.199787, // + 0.0771527, 0.196192, 0.977525); + + Rot3 R2(0.99755, -0.0377748, -0.0588831, // + 0.0238455, 0.974883, -0.221437, // + 0.0657689, 0.21949, 0.973395); + + double expected = 1.7765686464446904; + + EXPECT(assert_equal(expected, R1.angle(R2), 1e-6)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 087247c6e03bbf2c6032d89c181370cdecf6ab46 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 6 Mar 2020 17:54:25 -0500 Subject: [PATCH 09/79] return axis-angle representation instead of just angle, updated test to be much simpler --- gtsam/geometry/Rot3.cpp | 5 +++-- gtsam/geometry/Rot3.h | 4 ++-- gtsam/geometry/tests/testRot3.cpp | 18 +++++++++--------- 3 files changed, 14 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 4eee045f9..9b68bb1f1 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -229,8 +229,9 @@ Rot3 Rot3::slerp(double t, const Rot3& other) const { } /* ************************************************************************* */ -double Rot3::angle(const Rot3& other) const { - return Rot3::Logmap(this->between(other)).norm() * 180.0 / M_PI; +pair Rot3::axisAngle(const Rot3& other) const { + Vector3 rot = Rot3::Logmap(this->between(other)); + return pair(Unit3(rot), rot.norm()); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index d810285fb..41b5a6ca1 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -480,10 +480,10 @@ namespace gtsam { Rot3 slerp(double t, const Rot3& other) const; /** - * @brief Compute the angle (in degrees) between *this and other + * @brief Compute the Euler axis and angle (in radians) between *this and other * @param other Rot3 element */ - double angle(const Rot3& other) const; + std::pair axisAngle(const Rot3& other) const; /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 84c5bd094..c37dfda35 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -663,17 +663,17 @@ TEST(Rot3, ClosestTo) { /* ************************************************************************* */ TEST(Rot3, angle) { - Rot3 R1(0.996136, -0.0564186, -0.0672982, // - 0.0419472, 0.978941, -0.199787, // - 0.0771527, 0.196192, 0.977525); + Rot3 R1 = Rot3::Ypr(0, 0, 0); - Rot3 R2(0.99755, -0.0377748, -0.0588831, // - 0.0238455, 0.974883, -0.221437, // - 0.0657689, 0.21949, 0.973395); + Rot3 R2 = Rot3::Ypr(M_PI/4, 0, 0); - double expected = 1.7765686464446904; - - EXPECT(assert_equal(expected, R1.angle(R2), 1e-6)); + Unit3 expectedAxis(0, 0, 1); + double expectedAngle = M_PI/4; + + pair actual = R1.axisAngle(R2); + + EXPECT(assert_equal(expectedAxis, actual.first, 1e-6)); + EXPECT(assert_equal(expectedAngle, actual.second, 1e-6)); } /* ************************************************************************* */ From 7019d6f4b8f9d45dae8f5f9cbe7b758bd6a5dac1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 7 Mar 2020 08:35:08 -0500 Subject: [PATCH 10/79] convert axisAngle to static ToAxisAngle, update tests --- gtsam.h | 2 +- gtsam/geometry/Rot3.cpp | 6 ------ gtsam/geometry/Rot3.h | 18 +++++++++++------- gtsam/geometry/tests/testRot3.cpp | 11 +++++------ 4 files changed, 17 insertions(+), 20 deletions(-) diff --git a/gtsam.h b/gtsam.h index 6e2eea686..521495bdf 100644 --- a/gtsam.h +++ b/gtsam.h @@ -642,6 +642,7 @@ class Rot3 { static gtsam::Rot3 Rodrigues(Vector v); static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); static gtsam::Rot3 ClosestTo(const Matrix M); + static std::pair ToAxisAngle(const gtsam::Rot3& R) const; // Testable void print(string s) const; @@ -677,7 +678,6 @@ class Rot3 { // Vector toQuaternion() const; // FIXME: Can't cast to Vector properly Vector quaternion() const; gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const; - double angle(const gtsam::Rot3& other) const; // enabling serialization functionality void serialize() const; diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 9b68bb1f1..b1e8dd14b 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -228,12 +228,6 @@ Rot3 Rot3::slerp(double t, const Rot3& other) const { return interpolate(*this, other, t); } -/* ************************************************************************* */ -pair Rot3::axisAngle(const Rot3& other) const { - Vector3 rot = Rot3::Logmap(this->between(other)); - return pair(Unit3(rot), rot.norm()); -} - /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 41b5a6ca1..974e9b195 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -194,7 +194,7 @@ namespace gtsam { /** * Convert from axis/angle representation - * @param axisw is the rotation axis, unit length + * @param axis is the rotation axis, unit length * @param angle rotation angle * @return incremental rotation */ @@ -216,6 +216,16 @@ namespace gtsam { return AxisAngle(axis.unitVector(),angle); } + /** + * Compute to the Euler axis and angle (in radians) representation + * @param R is the rotation matrix + * @return pair consisting of Unit3 axis and angle in radians + */ + static std::pair ToAxisAngle(const Rot3& R) { + const Vector3 omega = Rot3::Logmap(R); + return std::pair(Unit3(omega), omega.norm()); + } + /** * Rodrigues' formula to compute an incremental rotation * @param w a vector of incremental roll,pitch,yaw @@ -479,12 +489,6 @@ namespace gtsam { */ Rot3 slerp(double t, const Rot3& other) const; - /** - * @brief Compute the Euler axis and angle (in radians) between *this and other - * @param other Rot3 element - */ - std::pair axisAngle(const Rot3& other) const; - /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index c37dfda35..dbd4e84a1 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -14,6 +14,7 @@ * @brief Unit tests for Rot3 class - common between Matrix and Quaternion * @author Alireza Fathi * @author Frank Dellaert + * @author Varun Agrawal */ #include @@ -663,14 +664,12 @@ TEST(Rot3, ClosestTo) { /* ************************************************************************* */ TEST(Rot3, angle) { - Rot3 R1 = Rot3::Ypr(0, 0, 0); - - Rot3 R2 = Rot3::Ypr(M_PI/4, 0, 0); + Rot3 R1 = Rot3::Ypr(M_PI/4, 0, 0); - Unit3 expectedAxis(0, 0, 1); - double expectedAngle = M_PI/4; + Unit3 expectedAxis(-0.350067, -0.472463, 0.808846); + double expectedAngle = 0.709876; - pair actual = R1.axisAngle(R2); + pair actual = Rot3::ToAxisAngle(R.between(R1)); EXPECT(assert_equal(expectedAxis, actual.first, 1e-6)); EXPECT(assert_equal(expectedAngle, actual.second, 1e-6)); From 0fbe63aa1746fcddb02763a8e340df971c746cf7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 8 Mar 2020 17:07:40 -0400 Subject: [PATCH 11/79] static function & method for axis-angle with improved tests and fix for sign ambiguity --- gtsam.h | 5 +- gtsam/geometry/Rot3.cpp | 6 +++ gtsam/geometry/Rot3.h | 20 ++++++-- gtsam/geometry/tests/testRot3.cpp | 77 +++++++++++++++++++++++++++---- 4 files changed, 94 insertions(+), 14 deletions(-) diff --git a/gtsam.h b/gtsam.h index 521495bdf..8382c06a7 100644 --- a/gtsam.h +++ b/gtsam.h @@ -642,7 +642,7 @@ class Rot3 { static gtsam::Rot3 Rodrigues(Vector v); static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); static gtsam::Rot3 ClosestTo(const Matrix M); - static std::pair ToAxisAngle(const gtsam::Rot3& R) const; + static pair ToAxisAngle(const gtsam::Rot3& R); // Testable void print(string s) const; @@ -675,6 +675,7 @@ class Rot3 { double roll() const; double pitch() const; double yaw() const; + pair axisAngle() const; // Vector toQuaternion() const; // FIXME: Can't cast to Vector properly Vector quaternion() const; gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const; @@ -1309,7 +1310,7 @@ class SymbolicBayesTree { // class SymbolicBayesTreeClique { // BayesTreeClique(); // BayesTreeClique(CONDITIONAL* conditional); -// // BayesTreeClique(const std::pair& result) : Base(result) {} +// // BayesTreeClique(const pair& result) : Base(result) {} // // bool equals(const This& other, double tol) const; // void print(string s) const; diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index b1e8dd14b..9fc7ee6b1 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -16,6 +16,7 @@ * @author Christian Potthast * @author Frank Dellaert * @author Richard Roberts + * @author Varun Agrawal */ #include @@ -185,6 +186,11 @@ Vector Rot3::quaternion() const { return v; } +/* ************************************************************************* */ +pair Rot3::axisAngle() { + return Rot3::ToAxisAngle(*this); +} + /* ************************************************************************* */ Matrix3 Rot3::ExpmapDerivative(const Vector3& x) { return SO3::ExpmapDerivative(x); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 974e9b195..3f2a962d5 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -17,6 +17,7 @@ * @author Frank Dellaert * @author Richard Roberts * @author Luca Carlone + * @author Varun Agrawal */ // \callgraph @@ -217,13 +218,19 @@ namespace gtsam { } /** - * Compute to the Euler axis and angle (in radians) representation + * Compute the Euler axis and angle (in radians) representation * @param R is the rotation matrix * @return pair consisting of Unit3 axis and angle in radians */ static std::pair ToAxisAngle(const Rot3& R) { const Vector3 omega = Rot3::Logmap(R); - return std::pair(Unit3(omega), omega.norm()); + int direction = 1; + // Check if any element in axis is negative. + // This implies that the rotation is clockwise and not counterclockwise. + if (omega.minCoeff() < 0.0) { + direction = -1; + } + return std::pair(Unit3(omega), direction * omega.norm()); } /** @@ -439,7 +446,7 @@ namespace gtsam { /** * Use RQ to calculate roll-pitch-yaw angle representation - * @return a vector containing ypr s.t. R = Rot3::Ypr(y,p,r) + * @return a vector containing rpy s.t. R = Rot3::Ypr(y,p,r) */ Vector3 rpy() const; @@ -471,6 +478,13 @@ namespace gtsam { /// @name Advanced Interface /// @{ + /** + * Compute the Euler axis and angle (in radians) representation + * of this rotation. + * @return pair consisting of Unit3 axis and angle in radians + */ + std::pair axisAngle(); + /** Compute the quaternion representation of this rotation. * @return The quaternion */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index dbd4e84a1..59397d199 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -663,16 +663,75 @@ TEST(Rot3, ClosestTo) { } /* ************************************************************************* */ -TEST(Rot3, angle) { - Rot3 R1 = Rot3::Ypr(M_PI/4, 0, 0); +TEST(Rot3, ToAxisAngle) { + /// Test rotations along each axis + Rot3 R1; - Unit3 expectedAxis(-0.350067, -0.472463, 0.808846); - double expectedAngle = 0.709876; - - pair actual = Rot3::ToAxisAngle(R.between(R1)); - - EXPECT(assert_equal(expectedAxis, actual.first, 1e-6)); - EXPECT(assert_equal(expectedAngle, actual.second, 1e-6)); + // Negative because rotation is counterclockwise when looking at unchanging axis + Rot3 yaw = Rot3::Ypr(-M_PI/4, 0, 0); + Rot3 pitch = Rot3::Ypr(0, -M_PI/4, 0); + Rot3 roll = Rot3::Ypr(0, 0, -M_PI/4); + + EXPECT(assert_equal(Unit3(0, 0, 1), Rot3::ToAxisAngle(yaw.between(R1)).first, 1e-6)); + EXPECT_DOUBLES_EQUAL(M_PI/4, Rot3::ToAxisAngle(yaw.between(R1)).second, 1e-9); + + EXPECT(assert_equal(Unit3(0, 1, 0), Rot3::ToAxisAngle(pitch.between(R1)).first, 1e-6)); + EXPECT_DOUBLES_EQUAL(M_PI/4, Rot3::ToAxisAngle(pitch.between(R1)).second, 1e-9); + + EXPECT(assert_equal(Unit3(1, 0, 0), Rot3::ToAxisAngle(roll.between(R1)).first, 1e-6)); + EXPECT_DOUBLES_EQUAL(M_PI/4, Rot3::ToAxisAngle(roll.between(R1)).second, 1e-9); + + Unit3 axis; double angle; + std::tie(axis,angle) = Rot3::ToAxisAngle(R); + EXPECT(assert_equal(Unit3(0.1, 0.4, 0.2), axis)); + EXPECT_DOUBLES_EQUAL(Vector3(0.1, 0.4, 0.2).norm(), angle, 1e-9); + + /// Test for sign ambiguity + double theta = M_PI + M_PI/2; // 270 degrees + Rot3 R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); + + EXPECT_DOUBLES_EQUAL(theta - 2*M_PI, Rot3::ToAxisAngle(R2).second, 1e-9); + + theta = -M_PI/2; // -90 degrees + R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); + + EXPECT_DOUBLES_EQUAL(theta, Rot3::ToAxisAngle(R2).second, 1e-9); +} + +/* ************************************************************************* */ +TEST(Rot3, axisAngle) { + /// Test rotations along each axis + Rot3 R1; + + // Negative because rotation is counterclockwise when looking at unchanging axis + Rot3 yaw = Rot3::Ypr(-M_PI/4, 0, 0); + Rot3 pitch = Rot3::Ypr(0, -M_PI/4, 0); + Rot3 roll = Rot3::Ypr(0, 0, -M_PI/4); + + EXPECT(assert_equal(Unit3(0, 0, 1), yaw.between(R1).axisAngle().first, 1e-6)); + EXPECT_DOUBLES_EQUAL(M_PI/4, yaw.between(R1).axisAngle().second, 1e-9); + + EXPECT(assert_equal(Unit3(0, 1, 0), pitch.between(R1).axisAngle().first, 1e-6)); + EXPECT_DOUBLES_EQUAL(M_PI/4, pitch.between(R1).axisAngle().second, 1e-9); + + EXPECT(assert_equal(Unit3(1, 0, 0), roll.between(R1).axisAngle().first, 1e-6)); + EXPECT_DOUBLES_EQUAL(M_PI/4, roll.between(R1).axisAngle().second, 1e-9); + + Unit3 axis; double angle; + std::tie(axis,angle) = R.axisAngle(); + EXPECT(assert_equal(Unit3(0.1, 0.4, 0.2), axis)); + EXPECT_DOUBLES_EQUAL(Vector3(0.1, 0.4, 0.2).norm(), angle, 1e-9); + + /// Test for sign ambiguity + double theta = M_PI + M_PI/2; // 270 degrees + Rot3 R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); + + EXPECT_DOUBLES_EQUAL(theta - 2*M_PI, R2.axisAngle().second, 1e-9); + + theta = -M_PI/2; // 270 degrees + R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); + + EXPECT_DOUBLES_EQUAL(theta, R2.axisAngle().second, 1e-9); } /* ************************************************************************* */ From ab46c7c3cee81cb69185509b5da3acb3a40f6aff Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 9 Mar 2020 16:01:24 -0400 Subject: [PATCH 12/79] removed static ToAxisAngle function --- gtsam.h | 1 - gtsam/geometry/Rot3.cpp | 9 +++++++- gtsam/geometry/Rot3.h | 16 -------------- gtsam/geometry/tests/testRot3.cpp | 36 ------------------------------- 4 files changed, 8 insertions(+), 54 deletions(-) diff --git a/gtsam.h b/gtsam.h index 8382c06a7..de1a3887b 100644 --- a/gtsam.h +++ b/gtsam.h @@ -642,7 +642,6 @@ class Rot3 { static gtsam::Rot3 Rodrigues(Vector v); static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); static gtsam::Rot3 ClosestTo(const Matrix M); - static pair ToAxisAngle(const gtsam::Rot3& R); // Testable void print(string s) const; diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 9fc7ee6b1..14646659d 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -188,7 +188,14 @@ Vector Rot3::quaternion() const { /* ************************************************************************* */ pair Rot3::axisAngle() { - return Rot3::ToAxisAngle(*this); + const Vector3 omega = Rot3::Logmap(*this); + int direction = 1; + // Check if any element in axis is negative. + // This implies that the rotation is clockwise and not counterclockwise. + if (omega.minCoeff() < 0.0) { + direction = -1; + } + return std::pair(Unit3(omega), direction * omega.norm()); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 3f2a962d5..b1cfc4926 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -217,22 +217,6 @@ namespace gtsam { return AxisAngle(axis.unitVector(),angle); } - /** - * Compute the Euler axis and angle (in radians) representation - * @param R is the rotation matrix - * @return pair consisting of Unit3 axis and angle in radians - */ - static std::pair ToAxisAngle(const Rot3& R) { - const Vector3 omega = Rot3::Logmap(R); - int direction = 1; - // Check if any element in axis is negative. - // This implies that the rotation is clockwise and not counterclockwise. - if (omega.minCoeff() < 0.0) { - direction = -1; - } - return std::pair(Unit3(omega), direction * omega.norm()); - } - /** * Rodrigues' formula to compute an incremental rotation * @param w a vector of incremental roll,pitch,yaw diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 59397d199..1b01b92c2 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -662,42 +662,6 @@ TEST(Rot3, ClosestTo) { EXPECT(assert_equal(expected, actual.matrix(), 1e-6)); } -/* ************************************************************************* */ -TEST(Rot3, ToAxisAngle) { - /// Test rotations along each axis - Rot3 R1; - - // Negative because rotation is counterclockwise when looking at unchanging axis - Rot3 yaw = Rot3::Ypr(-M_PI/4, 0, 0); - Rot3 pitch = Rot3::Ypr(0, -M_PI/4, 0); - Rot3 roll = Rot3::Ypr(0, 0, -M_PI/4); - - EXPECT(assert_equal(Unit3(0, 0, 1), Rot3::ToAxisAngle(yaw.between(R1)).first, 1e-6)); - EXPECT_DOUBLES_EQUAL(M_PI/4, Rot3::ToAxisAngle(yaw.between(R1)).second, 1e-9); - - EXPECT(assert_equal(Unit3(0, 1, 0), Rot3::ToAxisAngle(pitch.between(R1)).first, 1e-6)); - EXPECT_DOUBLES_EQUAL(M_PI/4, Rot3::ToAxisAngle(pitch.between(R1)).second, 1e-9); - - EXPECT(assert_equal(Unit3(1, 0, 0), Rot3::ToAxisAngle(roll.between(R1)).first, 1e-6)); - EXPECT_DOUBLES_EQUAL(M_PI/4, Rot3::ToAxisAngle(roll.between(R1)).second, 1e-9); - - Unit3 axis; double angle; - std::tie(axis,angle) = Rot3::ToAxisAngle(R); - EXPECT(assert_equal(Unit3(0.1, 0.4, 0.2), axis)); - EXPECT_DOUBLES_EQUAL(Vector3(0.1, 0.4, 0.2).norm(), angle, 1e-9); - - /// Test for sign ambiguity - double theta = M_PI + M_PI/2; // 270 degrees - Rot3 R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); - - EXPECT_DOUBLES_EQUAL(theta - 2*M_PI, Rot3::ToAxisAngle(R2).second, 1e-9); - - theta = -M_PI/2; // -90 degrees - R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); - - EXPECT_DOUBLES_EQUAL(theta, Rot3::ToAxisAngle(R2).second, 1e-9); -} - /* ************************************************************************* */ TEST(Rot3, axisAngle) { /// Test rotations along each axis From 1553d4321c479d60cecaea78e5dc537137cc5e67 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 13 Mar 2020 19:33:57 -0400 Subject: [PATCH 13/79] correct way of maintaining angle of axis-angle representation within (-pi,pi] --- gtsam/geometry/Rot3.cpp | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 14646659d..1256271bc 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -188,14 +188,20 @@ Vector Rot3::quaternion() const { /* ************************************************************************* */ pair Rot3::axisAngle() { - const Vector3 omega = Rot3::Logmap(*this); - int direction = 1; - // Check if any element in axis is negative. - // This implies that the rotation is clockwise and not counterclockwise. - if (omega.minCoeff() < 0.0) { - direction = -1; + Vector3 omega = Rot3::Logmap(*this); + // Get the rotation angle. + double theta = omega.norm(); + + // Check if angle belongs to (-pi, pi]. + // If yes, rotate in opposite direction to maintain range. + if (theta > M_PI) { + theta = theta - 2*M_PI; + omega = -omega; + } else if (theta <= -M_PI) { + theta = theta + 2*M_PI; + omega = -omega; } - return std::pair(Unit3(omega), direction * omega.norm()); + return std::pair(Unit3(omega), theta); } /* ************************************************************************* */ From 8f2a00e4bd2222b71e86da5473856875275e91f8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 14 Mar 2020 10:10:00 -0400 Subject: [PATCH 14/79] fix for angle outside the range (-pi,pi] and added more tests to verify --- gtsam/geometry/Rot3.cpp | 12 +++++------- gtsam/geometry/tests/testRot3.cpp | 19 ++++++++++++++----- 2 files changed, 19 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 1256271bc..f2d60f1f7 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -192,14 +192,12 @@ pair Rot3::axisAngle() { // Get the rotation angle. double theta = omega.norm(); - // Check if angle belongs to (-pi, pi]. + // Check if angle `theta` belongs to (-pi, pi]. // If yes, rotate in opposite direction to maintain range. - if (theta > M_PI) { - theta = theta - 2*M_PI; - omega = -omega; - } else if (theta <= -M_PI) { - theta = theta + 2*M_PI; - omega = -omega; + // Since omega = theta * u, if all coefficients are negative, + // then theta is outside the expected range. + if ((omega.array() < 0).all()) { + theta = -theta; } return std::pair(Unit3(omega), theta); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 1b01b92c2..f7d2609d4 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -686,16 +686,25 @@ TEST(Rot3, axisAngle) { EXPECT(assert_equal(Unit3(0.1, 0.4, 0.2), axis)); EXPECT_DOUBLES_EQUAL(Vector3(0.1, 0.4, 0.2).norm(), angle, 1e-9); - /// Test for sign ambiguity - double theta = M_PI + M_PI/2; // 270 degrees - Rot3 R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); + double theta; + /// Test for sign ambiguity + theta = M_PI + M_PI/2; // 270 degrees + Rot3 R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); EXPECT_DOUBLES_EQUAL(theta - 2*M_PI, R2.axisAngle().second, 1e-9); - theta = -M_PI/2; // 270 degrees + theta = -(M_PI + M_PI/2); // 90 (or -270) degrees R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); + EXPECT_DOUBLES_EQUAL(2*M_PI + theta, R2.axisAngle().second, 1e-9); - EXPECT_DOUBLES_EQUAL(theta, R2.axisAngle().second, 1e-9); + /// Non-trivial angles + theta = 195 * M_PI / 180; // 195 degrees + Rot3 R3 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); + EXPECT_DOUBLES_EQUAL(theta - 2*M_PI, R3.axisAngle().second, 1e-9); + + theta = -195 * M_PI / 180; // 165 degrees + R3 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); + EXPECT_DOUBLES_EQUAL(2*M_PI + theta, R3.axisAngle().second, 1e-9); } /* ************************************************************************* */ From 6ec13bdcd5370fbe4022a37ec61d6b9c18710662 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Sun, 12 Jan 2020 14:19:40 -0500 Subject: [PATCH 15/79] add tbb version guard to fix clang build (cherry picked from commit 9b912f6b14d2cf715d17208df35b8253d5e648e7) --- CMakeLists.txt | 5 +++++ gtsam/linear/VectorValues.cpp | 28 ++++++++++++++++++++++++++++ gtsam/linear/VectorValues.h | 6 +++++- 3 files changed, 38 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c84835d82..56250a12c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -200,6 +200,11 @@ find_package(TBB 4.4 COMPONENTS tbb tbbmalloc) # Set up variables if we're using TBB if(TBB_FOUND AND GTSAM_WITH_TBB) set(GTSAM_USE_TBB 1) # This will go into config.h + if (${TBB_VERSION_MAJOR} GREATER_EQUAL 2020) + set(TBB_GREATER_EQUAL_2020 1) + else() + set(TBB_GREATER_EQUAL_2020 0) + endif() # all definitions and link requisites will go via imported targets: # tbb & tbbmalloc list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc) diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index f18ad9c5f..e74d3776d 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -51,7 +51,11 @@ namespace gtsam { Key key; size_t n; boost::tie(key, n) = v; +#ifdef TBB_GREATER_EQUAL_2020 values_.emplace(key, x.segment(j, n)); +#else + values_.insert(std::make_pair(key, x.segment(j, n))); +#endif j += n; } } @@ -60,7 +64,11 @@ namespace gtsam { VectorValues::VectorValues(const Vector& x, const Scatter& scatter) { size_t j = 0; for (const SlotEntry& v : scatter) { +#ifdef TBB_GREATER_EQUAL_2020 values_.emplace(v.key, x.segment(j, v.dimension)); +#else + values_.insert(std::make_pair(v.key, x.segment(j, v.dimension))); +#endif j += v.dimension; } } @@ -70,7 +78,11 @@ namespace gtsam { { VectorValues result; for(const KeyValuePair& v: other) +#ifdef TBB_GREATER_EQUAL_2020 result.values_.emplace(v.first, Vector::Zero(v.second.size())); +#else + result.values_.insert(std::make_pair(v.first, Vector::Zero(v.second.size()))); +#endif return result; } @@ -86,7 +98,11 @@ namespace gtsam { /* ************************************************************************* */ VectorValues::iterator VectorValues::emplace(Key j, const Vector& value) { +#ifdef TBB_GREATER_EQUAL_2020 std::pair result = values_.emplace(j, value); +#else + std::pair result = values_.insert(std::make_pair(j, value)); +#endif if(!result.second) throw std::invalid_argument( "Requested to emplace variable '" + DefaultKeyFormatter(j) @@ -266,7 +282,11 @@ namespace gtsam { VectorValues result; // The result.end() hint here should result in constant-time inserts for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) +#ifdef TBB_GREATER_EQUAL_2020 result.values_.emplace(j1->first, j1->second + j2->second); +#else + result.values_.insert(std::make_pair(j1->first, j1->second + j2->second)); +#endif return result; } @@ -324,7 +344,11 @@ namespace gtsam { VectorValues result; // The result.end() hint here should result in constant-time inserts for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) +#ifdef TBB_GREATER_EQUAL_2020 result.values_.emplace(j1->first, j1->second - j2->second); +#else + result.values_.insert(std::make_pair(j1->first, j1->second - j2->second)); +#endif return result; } @@ -340,7 +364,11 @@ namespace gtsam { { VectorValues result; for(const VectorValues::KeyValuePair& key_v: v) +#ifdef TBB_GREATER_EQUAL_2020 result.values_.emplace(key_v.first, a * key_v.second); +#else + result.values_.insert(std::make_pair(key_v.first, a * key_v.second)); +#endif return result; } diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 46da2d5f9..eed212eda 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -198,7 +198,11 @@ namespace gtsam { * exist, it is inserted and an iterator pointing to the new element, along with 'true', is * returned. */ std::pair tryInsert(Key j, const Vector& value) { - return values_.emplace(j, value); +#ifdef TBB_GREATER_EQUAL_2020 + return values_.emplace(j, value); +#else + return values_.insert(std::make_pair(j, value)); +#endif } /** Erase the vector with the given key, or throw std::out_of_range if it does not exist */ From 170d1526b73a824c8d6b509a66d2b266065b01fc Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 16 Mar 2020 23:33:34 -0400 Subject: [PATCH 16/79] =?UTF-8?q?axisAngle=20maintains=20angle=20between?= =?UTF-8?q?=20[0,=20=CF=80]=20so=20updated=20docs=20and=20tests=20accordin?= =?UTF-8?q?gly?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- gtsam/geometry/Rot3.cpp | 12 +----------- gtsam/geometry/Rot3.h | 3 +++ gtsam/geometry/tests/testRot3.cpp | 6 ++++-- 3 files changed, 8 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index f2d60f1f7..9b305640b 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -189,17 +189,7 @@ Vector Rot3::quaternion() const { /* ************************************************************************* */ pair Rot3::axisAngle() { Vector3 omega = Rot3::Logmap(*this); - // Get the rotation angle. - double theta = omega.norm(); - - // Check if angle `theta` belongs to (-pi, pi]. - // If yes, rotate in opposite direction to maintain range. - // Since omega = theta * u, if all coefficients are negative, - // then theta is outside the expected range. - if ((omega.array() < 0).all()) { - theta = -theta; - } - return std::pair(Unit3(omega), theta); + return std::pair(Unit3(omega), omega.norm()); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index b1cfc4926..80d363d35 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -465,6 +465,9 @@ namespace gtsam { /** * Compute the Euler axis and angle (in radians) representation * of this rotation. + * The angle is in the range [0, π]. If the angle is not in the range, + * the axis is flipped around accordingly so that the returned angle is + * within the specified range. * @return pair consisting of Unit3 axis and angle in radians */ std::pair axisAngle(); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index f7d2609d4..6b0daa368 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -691,7 +691,8 @@ TEST(Rot3, axisAngle) { /// Test for sign ambiguity theta = M_PI + M_PI/2; // 270 degrees Rot3 R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); - EXPECT_DOUBLES_EQUAL(theta - 2*M_PI, R2.axisAngle().second, 1e-9); + EXPECT(assert_equal(Unit3(-0.1, -0.3, -0.4), R2.axisAngle().first, 1e-9)); + EXPECT_DOUBLES_EQUAL(M_PI/2, R2.axisAngle().second, 1e-9); theta = -(M_PI + M_PI/2); // 90 (or -270) degrees R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); @@ -700,7 +701,8 @@ TEST(Rot3, axisAngle) { /// Non-trivial angles theta = 195 * M_PI / 180; // 195 degrees Rot3 R3 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); - EXPECT_DOUBLES_EQUAL(theta - 2*M_PI, R3.axisAngle().second, 1e-9); + EXPECT(assert_equal(Unit3(-0.1, -0.3, -0.4), R3.axisAngle().first, 1e-9)); + EXPECT_DOUBLES_EQUAL(165*M_PI/180, R3.axisAngle().second, 1e-9); theta = -195 * M_PI / 180; // 165 degrees R3 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); From 81b52c674a4e3c1c6303003b8365f039ff06cbd7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 16 Mar 2020 23:33:55 -0400 Subject: [PATCH 17/79] wrap AxisAngle constructor for rotations --- gtsam.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam.h b/gtsam.h index de1a3887b..fbd70622f 100644 --- a/gtsam.h +++ b/gtsam.h @@ -639,6 +639,7 @@ class Rot3 { static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft) static gtsam::Rot3 Ypr(double y, double p, double r); static gtsam::Rot3 Quaternion(double w, double x, double y, double z); + static gtsam::Rot3 AxisAngle(const gtsam::Point3& axis, double angle); static gtsam::Rot3 Rodrigues(Vector v); static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); static gtsam::Rot3 ClosestTo(const Matrix M); From 8f17fbcc8e06f9bf1b79129a28ebf1f2f5961a51 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Mar 2020 13:11:48 -0400 Subject: [PATCH 18/79] improved printing for Point3 and Unit3 --- gtsam/geometry/Point3.cpp | 5 ++++- gtsam/geometry/Unit3.cpp | 5 ++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 8aa339a89..c9452efff 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -29,7 +29,10 @@ bool Point3::equals(const Point3 &q, double tol) const { } void Point3::print(const string& s) const { - cout << s << *this << endl; + if (s.size() != 0) { + cout << s << " "; + } + cout << *this << endl; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 3d46b18b8..a2a7e6851 100755 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -154,7 +154,10 @@ std::ostream& operator<<(std::ostream& os, const Unit3& pair) { /* ************************************************************************* */ void Unit3::print(const std::string& s) const { - cout << s << ":" << p_ << endl; + if(s.size() != 0) { + cout << s << ":"; + } + cout << p_ << endl; } /* ************************************************************************* */ From 0d46932456667015ce8402767987aea40b5514d6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 17 Mar 2020 13:15:35 -0400 Subject: [PATCH 19/79] Proper const --- gtsam/geometry/Rot3.cpp | 4 ++-- gtsam/geometry/Rot3.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 9b305640b..3481ac146 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -187,8 +187,8 @@ Vector Rot3::quaternion() const { } /* ************************************************************************* */ -pair Rot3::axisAngle() { - Vector3 omega = Rot3::Logmap(*this); +pair Rot3::axisAngle() const { + const Vector3 omega = Rot3::Logmap(*this); return std::pair(Unit3(omega), omega.norm()); } diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 80d363d35..e5e65b77d 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -470,7 +470,7 @@ namespace gtsam { * within the specified range. * @return pair consisting of Unit3 axis and angle in radians */ - std::pair axisAngle(); + std::pair axisAngle() const; /** Compute the quaternion representation of this rotation. * @return The quaternion From ca549630debb250112ed44d5bad91387f0daf96d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 17 Mar 2020 13:15:48 -0400 Subject: [PATCH 20/79] More systematic tests --- gtsam/geometry/tests/testRot3.cpp | 80 ++++++++++++++++++------------- 1 file changed, 46 insertions(+), 34 deletions(-) diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 6b0daa368..efb4bf70b 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -664,49 +664,61 @@ TEST(Rot3, ClosestTo) { /* ************************************************************************* */ TEST(Rot3, axisAngle) { - /// Test rotations along each axis - Rot3 R1; + Unit3 actualAxis; + double actualAngle; - // Negative because rotation is counterclockwise when looking at unchanging axis - Rot3 yaw = Rot3::Ypr(-M_PI/4, 0, 0); - Rot3 pitch = Rot3::Ypr(0, -M_PI/4, 0); - Rot3 roll = Rot3::Ypr(0, 0, -M_PI/4); +// not a lambda as otherwise we can't trace error easily +#define CHECK_AXIS_ANGLE(expectedAxis, expectedAngle, rotation) \ + std::tie(actualAxis, actualAngle) = rotation.axisAngle(); \ + EXPECT(assert_equal(expectedAxis, actualAxis, 1e-9)); \ + EXPECT_DOUBLES_EQUAL(expectedAngle, actualAngle, 1e-9); \ + EXPECT(assert_equal(rotation, Rot3::AxisAngle(expectedAxis, expectedAngle))) - EXPECT(assert_equal(Unit3(0, 0, 1), yaw.between(R1).axisAngle().first, 1e-6)); - EXPECT_DOUBLES_EQUAL(M_PI/4, yaw.between(R1).axisAngle().second, 1e-9); + // CHECK R defined at top = Rot3::Rodrigues(0.1, 0.4, 0.2) + Vector3 omega(0.1, 0.4, 0.2); + Unit3 axis(omega), _axis(-omega); + CHECK_AXIS_ANGLE(axis, omega.norm(), R); - EXPECT(assert_equal(Unit3(0, 1, 0), pitch.between(R1).axisAngle().first, 1e-6)); - EXPECT_DOUBLES_EQUAL(M_PI/4, pitch.between(R1).axisAngle().second, 1e-9); + // rotate by 90 + CHECK_AXIS_ANGLE(Unit3(1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, M_PI_2)) + CHECK_AXIS_ANGLE(Unit3(0, 1, 0), M_PI_2, Rot3::Ypr(0, M_PI_2, 0)) + CHECK_AXIS_ANGLE(Unit3(0, 0, 1), M_PI_2, Rot3::Ypr(M_PI_2, 0, 0)) + CHECK_AXIS_ANGLE(axis, M_PI_2, Rot3::AxisAngle(axis, M_PI_2)) - EXPECT(assert_equal(Unit3(1, 0, 0), roll.between(R1).axisAngle().first, 1e-6)); - EXPECT_DOUBLES_EQUAL(M_PI/4, roll.between(R1).axisAngle().second, 1e-9); + // rotate by -90 + CHECK_AXIS_ANGLE(Unit3(-1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, -M_PI_2)) + CHECK_AXIS_ANGLE(Unit3(0, -1, 0), M_PI_2, Rot3::Ypr(0, -M_PI_2, 0)) + CHECK_AXIS_ANGLE(Unit3(0, 0, -1), M_PI_2, Rot3::Ypr(-M_PI_2, 0, 0)) + CHECK_AXIS_ANGLE(_axis, M_PI_2, Rot3::AxisAngle(axis, -M_PI_2)) - Unit3 axis; double angle; - std::tie(axis,angle) = R.axisAngle(); - EXPECT(assert_equal(Unit3(0.1, 0.4, 0.2), axis)); - EXPECT_DOUBLES_EQUAL(Vector3(0.1, 0.4, 0.2).norm(), angle, 1e-9); + // rotate by 270 + const double theta270 = M_PI + M_PI / 2; + CHECK_AXIS_ANGLE(Unit3(-1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, theta270)) + CHECK_AXIS_ANGLE(Unit3(0, -1, 0), M_PI_2, Rot3::Ypr(0, theta270, 0)) + CHECK_AXIS_ANGLE(Unit3(0, 0, -1), M_PI_2, Rot3::Ypr(theta270, 0, 0)) + CHECK_AXIS_ANGLE(_axis, M_PI_2, Rot3::AxisAngle(axis, theta270)) - double theta; + // rotate by -270 + const double theta_270 = -(M_PI + M_PI / 2); // 90 (or -270) degrees + CHECK_AXIS_ANGLE(Unit3(1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, theta_270)) + CHECK_AXIS_ANGLE(Unit3(0, 1, 0), M_PI_2, Rot3::Ypr(0, theta_270, 0)) + CHECK_AXIS_ANGLE(Unit3(0, 0, 1), M_PI_2, Rot3::Ypr(theta_270, 0, 0)) + CHECK_AXIS_ANGLE(axis, M_PI_2, Rot3::AxisAngle(axis, theta_270)) - /// Test for sign ambiguity - theta = M_PI + M_PI/2; // 270 degrees - Rot3 R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); - EXPECT(assert_equal(Unit3(-0.1, -0.3, -0.4), R2.axisAngle().first, 1e-9)); - EXPECT_DOUBLES_EQUAL(M_PI/2, R2.axisAngle().second, 1e-9); + const double theta195 = 195 * M_PI / 180; + const double theta165 = 165 * M_PI / 180; - theta = -(M_PI + M_PI/2); // 90 (or -270) degrees - R2 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); - EXPECT_DOUBLES_EQUAL(2*M_PI + theta, R2.axisAngle().second, 1e-9); + /// Non-trivial angle 165 + CHECK_AXIS_ANGLE(Unit3(1, 0, 0), theta165, Rot3::Ypr(0, 0, theta165)) + CHECK_AXIS_ANGLE(Unit3(0, 1, 0), theta165, Rot3::Ypr(0, theta165, 0)) + CHECK_AXIS_ANGLE(Unit3(0, 0, 1), theta165, Rot3::Ypr(theta165, 0, 0)) + CHECK_AXIS_ANGLE(axis, theta165, Rot3::AxisAngle(axis, theta165)) - /// Non-trivial angles - theta = 195 * M_PI / 180; // 195 degrees - Rot3 R3 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); - EXPECT(assert_equal(Unit3(-0.1, -0.3, -0.4), R3.axisAngle().first, 1e-9)); - EXPECT_DOUBLES_EQUAL(165*M_PI/180, R3.axisAngle().second, 1e-9); - - theta = -195 * M_PI / 180; // 165 degrees - R3 = Rot3::AxisAngle(Unit3(0.1, 0.3, 0.4), theta); - EXPECT_DOUBLES_EQUAL(2*M_PI + theta, R3.axisAngle().second, 1e-9); + /// Non-trivial angle 195 + CHECK_AXIS_ANGLE(Unit3(-1, 0, 0), theta165, Rot3::Ypr(0, 0, theta195)) + CHECK_AXIS_ANGLE(Unit3(0, -1, 0), theta165, Rot3::Ypr(0, theta195, 0)) + CHECK_AXIS_ANGLE(Unit3(0, 0, -1), theta165, Rot3::Ypr(theta195, 0, 0)) + CHECK_AXIS_ANGLE(_axis, theta165, Rot3::AxisAngle(axis, theta195)) } /* ************************************************************************* */ From e987cb53a0e81daf8f5eebc20c86b5933b9d6c9a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Mar 2020 14:00:27 -0400 Subject: [PATCH 21/79] Lots of improvements to Rot3 - Ensure axis in AxisAngle constructor is a unit vector. Added test. - Improved rotation inverse with support for quaternions. - Use Eigen::Transpose for transpose return type. - Roll/Pitch/Yaw is more efficient. - Fix/Remove old TODOs. --- gtsam/geometry/Rot3.cpp | 1 - gtsam/geometry/Rot3.h | 42 ++++++++++++++----------------- gtsam/geometry/Rot3M.cpp | 3 +-- gtsam/geometry/tests/testRot3.cpp | 4 +++ 4 files changed, 24 insertions(+), 26 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index b1e8dd14b..24bdd6f17 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -36,7 +36,6 @@ void Rot3::print(const std::string& s) const { /* ************************************************************************* */ Rot3 Rot3::Random(std::mt19937& rng) { - // TODO allow any engine without including all of boost :-( Unit3 axis = Unit3::Random(rng); uniform_real_distribution randomAngle(-M_PI, M_PI); double angle = randomAngle(rng); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index e4408c9f4..474c88047 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -194,22 +194,24 @@ namespace gtsam { /** * Convert from axis/angle representation - * @param axisw is the rotation axis, unit length - * @param angle rotation angle + * @param axis is the rotation axis, unit length + * @param angle rotation angle * @return incremental rotation */ - static Rot3 AxisAngle(const Point3& axis, double angle) { + static Rot3 AxisAngle(const Vector3& axis, double angle) { + // Convert to unit vector. + Vector3 unitAxis = Unit3(axis).unitVector(); #ifdef GTSAM_USE_QUATERNIONS - return gtsam::Quaternion(Eigen::AngleAxis(angle, axis)); + return gtsam::Quaternion(Eigen::AngleAxis(angle, unitAxis)); #else - return Rot3(SO3::AxisAngle(axis,angle)); + return Rot3(SO3::AxisAngle(unitAxis,angle)); #endif } /** * Convert from axis/angle representation - * @param axis is the rotation axis - * @param angle rotation angle + * @param axis is the rotation axis + * @param angle rotation angle * @return incremental rotation */ static Rot3 AxisAngle(const Unit3& axis, double angle) { @@ -268,9 +270,13 @@ namespace gtsam { /// Syntatic sugar for composing two rotations Rot3 operator*(const Rot3& R2) const; - /// inverse of a rotation, TODO should be different for M/Q + /// inverse of a rotation Rot3 inverse() const { - return Rot3(Matrix3(transpose())); +#ifdef GTSAM_USE_QUATERNIONS + return quaternion_.inverse(); +#else + return Rot3(transpose()); +#endif } /** @@ -405,8 +411,7 @@ namespace gtsam { /** * Return 3*3 transpose (inverse) rotation matrix */ - Matrix3 transpose() const; - // TODO: const Eigen::Transpose transpose() const; + const Eigen::Transpose transpose() const; /// @deprecated, this is base 1, and was just confusing Point3 column(int index) const; @@ -435,27 +440,18 @@ namespace gtsam { /** * Accessor to get to component of angle representations - * NOTE: these are not efficient to get to multiple separate parts, - * you should instead use xyz() or ypr() - * TODO: make this more efficient */ - inline double roll() const { return ypr()(2); } + inline double roll() const { return xyz()(0); } /** * Accessor to get to component of angle representations - * NOTE: these are not efficient to get to multiple separate parts, - * you should instead use xyz() or ypr() - * TODO: make this more efficient */ - inline double pitch() const { return ypr()(1); } + inline double pitch() const { return xyz()(1); } /** * Accessor to get to component of angle representations - * NOTE: these are not efficient to get to multiple separate parts, - * you should instead use xyz() or ypr() - * TODO: make this more efficient */ - inline double yaw() const { return ypr()(0); } + inline double yaw() const { return xyz()(2); } /// @} /// @name Advanced Interface diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index d38d33bf1..85d9923fb 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -110,8 +110,7 @@ Rot3 Rot3::operator*(const Rot3& R2) const { } /* ************************************************************************* */ -// TODO const Eigen::Transpose Rot3::transpose() const { -Matrix3 Rot3::transpose() const { +const Eigen::Transpose Rot3::transpose() const { return rot_.matrix().transpose(); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index c95b85f21..d9d08a48e 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -115,6 +115,10 @@ TEST( Rot3, AxisAngle) CHECK(assert_equal(expected,actual,1e-5)); Rot3 actual2 = Rot3::AxisAngle(axis, angle-2*M_PI); CHECK(assert_equal(expected,actual2,1e-5)); + + axis = Vector3(0, 50, 0); + Rot3 actual3 = Rot3::AxisAngle(axis, angle); + CHECK(assert_equal(expected,actual3,1e-5)); } /* ************************************************************************* */ From 2087075ee787e561e825c84084b0c0e4964a36ca Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 17 Mar 2020 14:34:11 -0400 Subject: [PATCH 22/79] Transitioned toa method to a functor --- gtsam_unstable/geometry/Event.h | 91 ++++++++++++--------- gtsam_unstable/geometry/tests/testEvent.cpp | 42 +++++----- gtsam_unstable/slam/TOAFactor.h | 44 ++++++---- gtsam_unstable/slam/tests/testTOAFactor.cpp | 49 +++++------ wrap/python/pybind11 | 1 + 5 files changed, 125 insertions(+), 102 deletions(-) create mode 160000 wrap/python/pybind11 diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index fc186857f..b2a746595 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -20,42 +20,43 @@ #pragma once #include +#include + #include #include -#include +#include namespace gtsam { -/// A space-time event +/** + * A space-time event models an event that happens at a certain 3D location, at + * a certain time. One use for it is in sound-based or UWB-ranging tracking or + * SLAM, where we have "time of arrival" measurements at a set of sensors. The + * TOA functor below provides a measurement function for those applications. + */ class Event { + double time_; ///< Time event was generated + Point3 location_; ///< Location at time event was generated - double time_; ///< Time event was generated - Point3 location_; ///< Location at time event was generated - -public: + public: enum { dimension = 4 }; /// Default Constructor - Event() : - time_(0), location_(0,0,0) { - } + Event() : time_(0), location_(0, 0, 0) {} /// Constructor from time and location - Event(double t, const Point3& p) : - time_(t), location_(p) { - } + Event(double t, const Point3& p) : time_(t), location_(p) {} /// Constructor with doubles - Event(double t, double x, double y, double z) : - time_(t), location_(x, y, z) { - } + Event(double t, double x, double y, double z) + : time_(t), location_(x, y, z) {} - double time() const { return time_;} - Point3 location() const { return location_;} + double time() const { return time_; } + Point3 location() const { return location_; } - // TODO we really have to think of a better way to do linear arguments - double height(OptionalJacobian<1,4> H = boost::none) const { - static const Matrix14 JacobianZ = (Matrix14() << 0,0,0,1).finished(); + // TODO(frank) we really have to think of a better way to do linear arguments + double height(OptionalJacobian<1, 4> H = boost::none) const { + static const Matrix14 JacobianZ = (Matrix14() << 0, 0, 0, 1).finished(); if (H) *H = JacobianZ; return location_.z(); } @@ -64,7 +65,8 @@ public: GTSAM_UNSTABLE_EXPORT void print(const std::string& s = "") const; /** equals with an tolerance */ - GTSAM_UNSTABLE_EXPORT bool equals(const Event& other, double tol = 1e-9) const; + GTSAM_UNSTABLE_EXPORT bool equals(const Event& other, + double tol = 1e-9) const; /// Updates a with tangent space delta inline Event retract(const Vector4& v) const { @@ -73,28 +75,37 @@ public: /// Returns inverse retraction inline Vector4 localCoordinates(const Event& q) const { - return Vector4::Zero(); // TODO - } - - /// Time of arrival to given microphone - double toa(const Point3& microphone, // - OptionalJacobian<1, 4> H1 = boost::none, // - OptionalJacobian<1, 3> H2 = boost::none) const { - static const double Speed = 330; - Matrix13 D1, D2; - double distance = gtsam::distance3(location_, microphone, D1, D2); - if (H1) - // derivative of toa with respect to event - *H1 << 1.0, D1 / Speed; - if (H2) - // derivative of toa with respect to microphone location - *H2 << D2 / Speed; - return time_ + distance / Speed; + return Vector4::Zero(); // TODO(frank) implement! } }; // Define GTSAM traits -template<> +template <> struct traits : internal::Manifold {}; -} //\ namespace gtsam +/// Time of arrival to given sensor +class TimeOfArrival { + const double speed_; ///< signal speed + public: + typedef double result_type; + + /// Constructor with optional speed of signal, in m/sec + explicit TimeOfArrival(double speed = 330) : speed_(speed) {} + + /// Calculate time of arrival + double operator()(const Event& event, const Point3& sensor, // + OptionalJacobian<1, 4> H1 = boost::none, // + OptionalJacobian<1, 3> H2 = boost::none) const { + Matrix13 D1, D2; + double distance = gtsam::distance3(event.location(), sensor, D1, D2); + if (H1) + // derivative of toa with respect to event + *H1 << 1.0, D1 / speed_; + if (H2) + // derivative of toa with respect to sensor location + *H2 << D2 / speed_; + return event.time() + distance / speed_; + } +}; + +} // namespace gtsam diff --git a/gtsam_unstable/geometry/tests/testEvent.cpp b/gtsam_unstable/geometry/tests/testEvent.cpp index ec8ca1f4b..0349f3293 100644 --- a/gtsam_unstable/geometry/tests/testEvent.cpp +++ b/gtsam_unstable/geometry/tests/testEvent.cpp @@ -17,10 +17,12 @@ * @date December 2014 */ -#include #include #include +#include + #include + #include using namespace std; @@ -30,56 +32,59 @@ using namespace gtsam; static const double ms = 1e-3; static const double cm = 1e-2; typedef Eigen::Matrix Vector1; -static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1,0.5*ms)); +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms)); static const double timeOfEvent = 25; static const Event exampleEvent(timeOfEvent, 1, 0, 0); -static const Point3 microphoneAt0(0,0,0); +static const Point3 microphoneAt0(0, 0, 0); + +static const double kSpeedOfSound = 340; +static const TimeOfArrival kToa(kSpeedOfSound); //***************************************************************************** -TEST( Event, Constructor ) { +TEST(Event, Constructor) { const double t = 0; Event actual(t, 201.5 * cm, 201.5 * cm, (212 - 45) * cm); } //***************************************************************************** -TEST( Event, Toa1 ) { +TEST(Event, Toa1) { Event event(0, 1, 0, 0); - double expected = 1. / 330; - EXPECT_DOUBLES_EQUAL(expected, event.toa(microphoneAt0), 1e-9); + double expected = 1. / kSpeedOfSound; + EXPECT_DOUBLES_EQUAL(expected, kToa(event, microphoneAt0), 1e-9); } //***************************************************************************** -TEST( Event, Toa2 ) { - double expectedTOA = timeOfEvent + 1. / 330; - EXPECT_DOUBLES_EQUAL(expectedTOA, exampleEvent.toa(microphoneAt0), 1e-9); +TEST(Event, Toa2) { + double expectedTOA = timeOfEvent + 1. / kSpeedOfSound; + EXPECT_DOUBLES_EQUAL(expectedTOA, kToa(exampleEvent, microphoneAt0), 1e-9); } //************************************************************************* -TEST (Event, Derivatives) { +TEST(Event, Derivatives) { Matrix14 actualH1; Matrix13 actualH2; - exampleEvent.toa(microphoneAt0, actualH1, actualH2); + kToa(exampleEvent, microphoneAt0, actualH1, actualH2); Matrix expectedH1 = numericalDerivative11( - boost::bind(&Event::toa, _1, microphoneAt0, boost::none, boost::none), + boost::bind(kToa, _1, microphoneAt0, boost::none, boost::none), exampleEvent); EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); Matrix expectedH2 = numericalDerivative11( - boost::bind(&Event::toa, exampleEvent, _1, boost::none, boost::none), + boost::bind(kToa, exampleEvent, _1, boost::none, boost::none), microphoneAt0); EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); } //***************************************************************************** -TEST( Event, Expression ) { +TEST(Event, Expression) { Key key = 12; Expression event_(key); - Expression knownMicrophone_(microphoneAt0); // constant expression - Expression expression(&Event::toa, event_, knownMicrophone_); + Expression knownMicrophone_(microphoneAt0); // constant expression + Expression expression(kToa, event_, knownMicrophone_); Values values; values.insert(key, exampleEvent); - double expectedTOA = timeOfEvent + 1. / 330; + double expectedTOA = timeOfEvent + 1. / kSpeedOfSound; EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9); } @@ -97,4 +102,3 @@ int main() { return TestRegistry::runAllTests(tr); } //***************************************************************************** - diff --git a/gtsam_unstable/slam/TOAFactor.h b/gtsam_unstable/slam/TOAFactor.h index 1df14a8de..af9dce51b 100644 --- a/gtsam_unstable/slam/TOAFactor.h +++ b/gtsam_unstable/slam/TOAFactor.h @@ -17,33 +17,47 @@ * @date December 2014 */ +#pragma once + #include #include namespace gtsam { /// A "Time of Arrival" factor - so little code seems hardly worth it :-) -class TOAFactor: public ExpressionFactor { - +class TOAFactor : public ExpressionFactor { typedef Expression Double_; -public: - + public: /** - * Constructor - * @param some expression yielding an event - * @param microphone_ expression yielding a microphone location - * @param toaMeasurement time of arrival at microphone + * Most genral constructor with two expressions + * @param eventExpression expression yielding an event + * @param sensorExpression expression yielding a sensor location + * @param toaMeasurement time of arrival at sensor * @param model noise model + * @param toa optional time of arrival functor */ TOAFactor(const Expression& eventExpression, - const Expression& microphone_, double toaMeasurement, - const SharedNoiseModel& model) : - ExpressionFactor(model, toaMeasurement, - Double_(&Event::toa, eventExpression, microphone_)) { - } + const Expression& sensorExpression, double toaMeasurement, + const SharedNoiseModel& model, + const TimeOfArrival& toa = TimeOfArrival()) + : ExpressionFactor( + model, toaMeasurement, + Double_(toa, eventExpression, sensorExpression)) {} + /** + * Constructor with fixed sensor + * @param eventExpression expression yielding an event + * @param sensor a known sensor location + * @param toaMeasurement time of arrival at sensor + * @param model noise model + * @param toa optional time of arrival functor + */ + TOAFactor(const Expression& eventExpression, const Point3& sensor, + double toaMeasurement, const SharedNoiseModel& model, + const TimeOfArrival& toa = TimeOfArrival()) + : TOAFactor(eventExpression, Expression(sensor), toaMeasurement, + model, toa) {} }; -} //\ namespace gtsam - +} // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 4b10d5c0b..0e7a19095 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -17,15 +17,16 @@ * @date December 2014 */ -#include -#include -#include -#include #include +#include +#include +#include +#include +#include #include -#include #include +#include using namespace std; using namespace gtsam; @@ -43,21 +44,18 @@ static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms)); static const double timeOfEvent = 25; static const Event exampleEvent(timeOfEvent, 1, 0, 0); -static const Point3 microphoneAt0(0,0,0); +static const Point3 microphoneAt0(0, 0, 0); //***************************************************************************** -TEST( TOAFactor, NewWay ) { +TEST(TOAFactor, NewWay) { Key key = 12; Event_ eventExpression(key); - Point3_ microphoneConstant(microphoneAt0); // constant expression double measurement = 7; - Double_ expression(&Event::toa, eventExpression, microphoneConstant); - ExpressionFactor factor(model, measurement, expression); + TOAFactor factor(eventExpression, microphoneAt0, measurement, model); } //***************************************************************************** -TEST( TOAFactor, WholeEnchilada ) { - +TEST(TOAFactor, WholeEnchilada) { static const bool verbose = false; // Create microphones @@ -68,7 +66,7 @@ TEST( TOAFactor, WholeEnchilada ) { microphones.push_back(Point3(403 * cm, 403 * cm, height)); microphones.push_back(Point3(0, 403 * cm, 2 * height)); EXPECT_LONGS_EQUAL(4, microphones.size()); -// microphones.push_back(Point3(200 * cm, 200 * cm, height)); + // microphones.push_back(Point3(200 * cm, 200 * cm, height)); // Create a ground truth point const double timeOfEvent = 0; @@ -77,8 +75,9 @@ TEST( TOAFactor, WholeEnchilada ) { // Simulate simulatedTOA size_t K = microphones.size(); vector simulatedTOA(K); + TimeOfArrival toa; for (size_t i = 0; i < K; i++) { - simulatedTOA[i] = groundTruthEvent.toa(microphones[i]); + simulatedTOA[i] = toa(groundTruthEvent, microphones[i]); if (verbose) { cout << "mic" << i << " = " << microphones[i] << endl; cout << "z" << i << " = " << simulatedTOA[i] / ms << endl; @@ -86,40 +85,35 @@ TEST( TOAFactor, WholeEnchilada ) { } // Now, estimate using non-linear optimization - ExpressionFactorGraph graph; + NonlinearFactorGraph graph; Key key = 12; Event_ eventExpression(key); for (size_t i = 0; i < K; i++) { - Point3_ microphone_i(microphones[i]); // constant expression - Double_ predictTOA(&Event::toa, eventExpression, microphone_i); - graph.addExpressionFactor(predictTOA, simulatedTOA[i], model); + graph.emplace_shared(eventExpression, microphones[i], + simulatedTOA[i], model); } /// Print the graph - if (verbose) - GTSAM_PRINT(graph); + if (verbose) GTSAM_PRINT(graph); // Create initial estimate Values initialEstimate; - //Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm); + // Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm); Vector4 delta; delta << 0.1, 0.1, -0.1, 0.1; Event estimatedEvent = groundTruthEvent.retract(delta); initialEstimate.insert(key, estimatedEvent); // Print - if (verbose) - initialEstimate.print("Initial Estimate:\n"); + if (verbose) initialEstimate.print("Initial Estimate:\n"); // Optimize using Levenberg-Marquardt optimization. LevenbergMarquardtParams params; params.setAbsoluteErrorTol(1e-10); - if (verbose) - params.setVerbosity("ERROR"); + if (verbose) params.setVerbosity("ERROR"); LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params); Values result = optimizer.optimize(); - if (verbose) - result.print("Final Result:\n"); + if (verbose) result.print("Final Result:\n"); EXPECT(assert_equal(groundTruthEvent, result.at(key), 1e-6)); } @@ -129,4 +123,3 @@ int main() { return TestRegistry::runAllTests(tr); } //***************************************************************************** - diff --git a/wrap/python/pybind11 b/wrap/python/pybind11 new file mode 160000 index 000000000..b3bf248ee --- /dev/null +++ b/wrap/python/pybind11 @@ -0,0 +1 @@ +Subproject commit b3bf248eec9cad8260753c982e1ae6cb72fff470 From cd88c795ae6543ac3f9c9648872bb8b450e09ada Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Mar 2020 15:10:13 -0400 Subject: [PATCH 23/79] more efficient and explicit inverse() --- gtsam/geometry/Rot3.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 474c88047..3e0e33609 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -198,7 +198,7 @@ namespace gtsam { * @param angle rotation angle * @return incremental rotation */ - static Rot3 AxisAngle(const Vector3& axis, double angle) { + static Rot3 AxisAngle(const Point3& axis, double angle) { // Convert to unit vector. Vector3 unitAxis = Unit3(axis).unitVector(); #ifdef GTSAM_USE_QUATERNIONS @@ -273,9 +273,9 @@ namespace gtsam { /// inverse of a rotation Rot3 inverse() const { #ifdef GTSAM_USE_QUATERNIONS - return quaternion_.inverse(); + return Rot3(quaternion_.inverse()); #else - return Rot3(transpose()); + return Rot3(rot_.matrix().transpose()); #endif } From 510d5c500c78544079a5f21c567fd85dfa4d5bb5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Mar 2020 15:10:22 -0400 Subject: [PATCH 24/79] Revert "improved printing for Point3 and Unit3" This reverts commit 8f17fbcc8e06f9bf1b79129a28ebf1f2f5961a51. --- gtsam/geometry/Point3.cpp | 5 +---- gtsam/geometry/Unit3.cpp | 5 +---- 2 files changed, 2 insertions(+), 8 deletions(-) diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index c9452efff..8aa339a89 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -29,10 +29,7 @@ bool Point3::equals(const Point3 &q, double tol) const { } void Point3::print(const string& s) const { - if (s.size() != 0) { - cout << s << " "; - } - cout << *this << endl; + cout << s << *this << endl; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index a2a7e6851..3d46b18b8 100755 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -154,10 +154,7 @@ std::ostream& operator<<(std::ostream& os, const Unit3& pair) { /* ************************************************************************* */ void Unit3::print(const std::string& s) const { - if(s.size() != 0) { - cout << s << ":"; - } - cout << p_ << endl; + cout << s << ":" << p_ << endl; } /* ************************************************************************* */ From 359bc161a67798a79e60ccff593107aef2952aba Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Mar 2020 10:44:21 -0400 Subject: [PATCH 25/79] updated transpose signature for Rot3Q --- gtsam/geometry/Rot3Q.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 8af9a7144..377cc4a5f 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -79,11 +79,9 @@ namespace gtsam { } /* ************************************************************************* */ - // TODO: Could we do this? It works in Rot3M but not here, probably because - // here we create an intermediate value by calling matrix() - // const Eigen::Transpose Rot3::transpose() const { - Matrix3 Rot3::transpose() const { - return matrix().transpose(); + const Eigen::Transpose Rot3::transpose() const { + // `.eval()` to avoid aliasing effect due to transpose (allows compilation). + return matrix().eval().transpose(); } /* ************************************************************************* */ From f3865539c66b495b3684e0ec6bf3d065b8a64374 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 18 Mar 2020 15:44:33 -0400 Subject: [PATCH 26/79] Refactor TOAFactor and test --- gtsam_unstable/slam/TOAFactor.h | 13 +++---- gtsam_unstable/slam/tests/testTOAFactor.cpp | 43 +++++++-------------- 2 files changed, 19 insertions(+), 37 deletions(-) diff --git a/gtsam_unstable/slam/TOAFactor.h b/gtsam_unstable/slam/TOAFactor.h index af9dce51b..c52f1b54c 100644 --- a/gtsam_unstable/slam/TOAFactor.h +++ b/gtsam_unstable/slam/TOAFactor.h @@ -30,20 +30,19 @@ class TOAFactor : public ExpressionFactor { public: /** - * Most genral constructor with two expressions + * Most general constructor with two expressions * @param eventExpression expression yielding an event * @param sensorExpression expression yielding a sensor location * @param toaMeasurement time of arrival at sensor * @param model noise model - * @param toa optional time of arrival functor + * @param speed optional speed of signal, in m/sec */ TOAFactor(const Expression& eventExpression, const Expression& sensorExpression, double toaMeasurement, - const SharedNoiseModel& model, - const TimeOfArrival& toa = TimeOfArrival()) + const SharedNoiseModel& model, double speed = 330) : ExpressionFactor( model, toaMeasurement, - Double_(toa, eventExpression, sensorExpression)) {} + Double_(TimeOfArrival(speed), eventExpression, sensorExpression)) {} /** * Constructor with fixed sensor @@ -55,9 +54,9 @@ class TOAFactor : public ExpressionFactor { */ TOAFactor(const Expression& eventExpression, const Point3& sensor, double toaMeasurement, const SharedNoiseModel& model, - const TimeOfArrival& toa = TimeOfArrival()) + double speed = 330) : TOAFactor(eventExpression, Expression(sensor), toaMeasurement, - model, toa) {} + model, speed) {} }; } // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 0e7a19095..042130a24 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -44,58 +44,46 @@ static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms)); static const double timeOfEvent = 25; static const Event exampleEvent(timeOfEvent, 1, 0, 0); -static const Point3 microphoneAt0(0, 0, 0); +static const Point3 sensorAt0(0, 0, 0); //***************************************************************************** TEST(TOAFactor, NewWay) { Key key = 12; - Event_ eventExpression(key); double measurement = 7; - TOAFactor factor(eventExpression, microphoneAt0, measurement, model); + TOAFactor factor(key, sensorAt0, measurement, model); } //***************************************************************************** TEST(TOAFactor, WholeEnchilada) { - static const bool verbose = false; - - // Create microphones + // Create sensors const double height = 0.5; - vector microphones; - microphones.push_back(Point3(0, 0, height)); - microphones.push_back(Point3(403 * cm, 0, height)); - microphones.push_back(Point3(403 * cm, 403 * cm, height)); - microphones.push_back(Point3(0, 403 * cm, 2 * height)); - EXPECT_LONGS_EQUAL(4, microphones.size()); - // microphones.push_back(Point3(200 * cm, 200 * cm, height)); + vector sensors; + sensors.push_back(Point3(0, 0, height)); + sensors.push_back(Point3(403 * cm, 0, height)); + sensors.push_back(Point3(403 * cm, 403 * cm, height)); + sensors.push_back(Point3(0, 403 * cm, 2 * height)); + EXPECT_LONGS_EQUAL(4, sensors.size()); + // sensors.push_back(Point3(200 * cm, 200 * cm, height)); // Create a ground truth point const double timeOfEvent = 0; Event groundTruthEvent(timeOfEvent, 245 * cm, 201.5 * cm, (212 - 45) * cm); // Simulate simulatedTOA - size_t K = microphones.size(); + size_t K = sensors.size(); vector simulatedTOA(K); TimeOfArrival toa; for (size_t i = 0; i < K; i++) { - simulatedTOA[i] = toa(groundTruthEvent, microphones[i]); - if (verbose) { - cout << "mic" << i << " = " << microphones[i] << endl; - cout << "z" << i << " = " << simulatedTOA[i] / ms << endl; - } + simulatedTOA[i] = toa(groundTruthEvent, sensors[i]); } // Now, estimate using non-linear optimization NonlinearFactorGraph graph; Key key = 12; - Event_ eventExpression(key); for (size_t i = 0; i < K; i++) { - graph.emplace_shared(eventExpression, microphones[i], - simulatedTOA[i], model); + graph.emplace_shared(key, sensors[i], simulatedTOA[i], model); } - /// Print the graph - if (verbose) GTSAM_PRINT(graph); - // Create initial estimate Values initialEstimate; // Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm); @@ -104,16 +92,11 @@ TEST(TOAFactor, WholeEnchilada) { Event estimatedEvent = groundTruthEvent.retract(delta); initialEstimate.insert(key, estimatedEvent); - // Print - if (verbose) initialEstimate.print("Initial Estimate:\n"); - // Optimize using Levenberg-Marquardt optimization. LevenbergMarquardtParams params; params.setAbsoluteErrorTol(1e-10); - if (verbose) params.setVerbosity("ERROR"); LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params); Values result = optimizer.optimize(); - if (verbose) result.print("Final Result:\n"); EXPECT(assert_equal(groundTruthEvent, result.at(key), 1e-6)); } From 2e75ffd01da7f65af78751271a01fccc5ce3d813 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 18 Mar 2020 15:44:45 -0400 Subject: [PATCH 27/79] C++ example --- .../examples/TimeOfArrivalExample.cpp | 160 ++++++++++++++++++ gtsam_unstable/geometry/Event.cpp | 9 +- 2 files changed, 165 insertions(+), 4 deletions(-) create mode 100644 gtsam_unstable/examples/TimeOfArrivalExample.cpp diff --git a/gtsam_unstable/examples/TimeOfArrivalExample.cpp b/gtsam_unstable/examples/TimeOfArrivalExample.cpp new file mode 100644 index 000000000..f49d47fb7 --- /dev/null +++ b/gtsam_unstable/examples/TimeOfArrivalExample.cpp @@ -0,0 +1,160 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 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 TimeOfArrivalExample.cpp + * @brief Track a moving object "Time of Arrival" measurements at 4 + * microphones. + * @author Frank Dellaert + * @author Jay Chakravarty + * @date March 2020 + */ + +#include +#include +#include +#include +#include + +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +// units +static const double ms = 1e-3; +static const double cm = 1e-2; + +// Instantiate functor with speed of sound value +static const TimeOfArrival kTimeOfArrival(330); + +/* ************************************************************************* */ +// Create microphones +vector defineMicrophones() { + const double height = 0.5; + vector microphones; + microphones.push_back(Point3(0, 0, height)); + microphones.push_back(Point3(403 * cm, 0, height)); + microphones.push_back(Point3(403 * cm, 403 * cm, height)); + microphones.push_back(Point3(0, 403 * cm, 2 * height)); + return microphones; +} + +/* ************************************************************************* */ +// Create ground truth trajectory +vector createTrajectory(int n) { + vector trajectory; + double timeOfEvent = 10; + // simulate emitting a sound every second while moving on straight line + for (size_t key = 0; key < n; key++) { + trajectory.push_back( + Event(timeOfEvent, 245 * cm + key * 1.0, 201.5 * cm, (212 - 45) * cm)); + timeOfEvent += 1; + } + return trajectory; +} + +/* ************************************************************************* */ +// Simulate time-of-arrival measurements for a single event +vector simulateTOA(const vector& microphones, + const Event& event) { + size_t K = microphones.size(); + vector simulatedTOA(K); + for (size_t i = 0; i < K; i++) { + simulatedTOA[i] = kTimeOfArrival(event, microphones[i]); + } + return simulatedTOA; +} + +/* ************************************************************************* */ +// Simulate time-of-arrival measurements for an entire trajectory +vector> simulateTOA(const vector& microphones, + const vector& trajectory) { + vector> simulatedTOA; + for (auto event : trajectory) { + simulatedTOA.push_back(simulateTOA(microphones, event)); + } + return simulatedTOA; +} + +/* ************************************************************************* */ +// create factor graph +NonlinearFactorGraph createGraph(const vector& microphones, + const vector>& simulatedTOA) { + NonlinearFactorGraph graph; + + // Create a noise model for the TOA error + auto model = noiseModel::Isotropic::Sigma(1, 0.5 * ms); + + size_t K = microphones.size(); + size_t key = 0; + for (auto toa : simulatedTOA) { + for (size_t i = 0; i < K; i++) { + graph.emplace_shared(key, microphones[i], toa[i], model); + } + key += 1; + } + return graph; +} + +/* ************************************************************************* */ +// create initial estimate for n events +Values createInitialEstimate(int n) { + Values initial; + + Event zero; + for (size_t key = 0; key < n; key++) { + initial.insert(key, zero); + } + return initial; +} + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + // Create microphones + auto microphones = defineMicrophones(); + size_t K = microphones.size(); + for (size_t i = 0; i < K; i++) { + cout << "mic" << i << " = " << microphones[i] << endl; + } + + // Create a ground truth trajectory + const size_t n = 5; + auto groundTruth = createTrajectory(n); + + // Simulate time-of-arrival measurements + auto simulatedTOA = simulateTOA(microphones, groundTruth); + for (size_t key = 0; key < n; key++) { + for (size_t i = 0; i < K; i++) { + cout << "z_" << key << i << " = " << simulatedTOA[key][i] / ms << " ms" + << endl; + } + } + + // Create factor graph + auto graph = createGraph(microphones, simulatedTOA); + + // Create initial estimate + auto initialEstimate = createInitialEstimate(n); + initialEstimate.print("Initial Estimate:\n"); + + // Optimize using Levenberg-Marquardt optimization. + LevenbergMarquardtParams params; + params.setAbsoluteErrorTol(1e-10); + params.setVerbosityLM("SUMMARY"); + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params); + Values result = optimizer.optimize(); + result.print("Final Result:\n"); +} +/* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/Event.cpp b/gtsam_unstable/geometry/Event.cpp index c503514a6..45a24c101 100644 --- a/gtsam_unstable/geometry/Event.cpp +++ b/gtsam_unstable/geometry/Event.cpp @@ -24,15 +24,16 @@ namespace gtsam { /* ************************************************************************* */ void Event::print(const std::string& s) const { - std::cout << s << "time = " << time_ << "location = " << location_.transpose(); + 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); + return std::abs(time_ - other.time_) < tol && + traits::Equals(location_, other.location_, tol); } /* ************************************************************************* */ -} //\ namespace gtsam +} // namespace gtsam From cd318b2295dd0d23c0119d4a55d7a5a7c810e90a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 18 Mar 2020 17:28:40 -0400 Subject: [PATCH 28/79] Python example and necessary wrapper gymnastics --- .../examples/TimeOfArrivalExample.py | 128 ++++++++++++++++++ gtsam_unstable/geometry/Event.h | 7 + gtsam_unstable/gtsam_unstable.h | 24 ++++ gtsam_unstable/slam/TOAFactor.h | 5 + 4 files changed, 164 insertions(+) create mode 100644 cython/gtsam_unstable/examples/TimeOfArrivalExample.py diff --git a/cython/gtsam_unstable/examples/TimeOfArrivalExample.py b/cython/gtsam_unstable/examples/TimeOfArrivalExample.py new file mode 100644 index 000000000..ac3af8a38 --- /dev/null +++ b/cython/gtsam_unstable/examples/TimeOfArrivalExample.py @@ -0,0 +1,128 @@ +""" +GTSAM Copyright 2010-2018, 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 + +Track a moving object "Time of Arrival" measurements at 4 microphones. +Author: Frank Dellaert +""" +# pylint: disable=invalid-name, no-name-in-module + +from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams, + NonlinearFactorGraph, Point3, Values, noiseModel_Isotropic) +from gtsam_unstable import Event, TimeOfArrival, TOAFactor + +# units +MS = 1e-3 +CM = 1e-2 + +# Instantiate functor with speed of sound value +TIME_OF_ARRIVAL = TimeOfArrival(330) + + +def defineMicrophones(): + """Create microphones.""" + height = 0.5 + microphones = [] + microphones.append(Point3(0, 0, height)) + microphones.append(Point3(403 * CM, 0, height)) + microphones.append(Point3(403 * CM, 403 * CM, height)) + microphones.append(Point3(0, 403 * CM, 2 * height)) + return microphones + + +def createTrajectory(n): + """Create ground truth trajectory.""" + trajectory = [] + timeOfEvent = 10 + # simulate emitting a sound every second while moving on straight line + for key in range(n): + trajectory.append( + Event(timeOfEvent, 245 * CM + key * 1.0, 201.5 * CM, (212 - 45) * CM)) + timeOfEvent += 1 + + return trajectory + + +def simulateOneTOA(microphones, event): + """Simulate time-of-arrival measurements for a single event.""" + return [TIME_OF_ARRIVAL.measure(event, microphones[i]) + for i in range(len(microphones))] + + +def simulateTOA(microphones, trajectory): + """Simulate time-of-arrival measurements for an entire trajectory.""" + return [simulateOneTOA(microphones, event) + for event in trajectory] + + +def createGraph(microphones, simulatedTOA): + """Create factor graph.""" + graph = NonlinearFactorGraph() + + # Create a noise model for the TOA error + model = noiseModel_Isotropic.Sigma(1, 0.5 * MS) + + K = len(microphones) + key = 0 + for toa in simulatedTOA: + for i in range(K): + factor = TOAFactor(key, microphones[i], toa[i], model) + graph.push_back(factor) + key += 1 + + return graph + + +def createInitialEstimate(n): + """Create initial estimate for n events.""" + initial = Values() + zero = Event() + for key in range(n): + TOAFactor.InsertEvent(key, zero, initial) + return initial + + +def TimeOfArrivalExample(): + """Run example with 4 microphones and 5 events in a straight line.""" + # Create microphones + microphones = defineMicrophones() + K = len(microphones) + for i in range(K): + print("mic {} = {}".format(i, microphones[i])) + + # Create a ground truth trajectory + n = 5 + groundTruth = createTrajectory(n) + for event in groundTruth: + print(event) + + # Simulate time-of-arrival measurements + simulatedTOA = simulateTOA(microphones, groundTruth) + for key in range(n): + for i in range(K): + print("z_{}{} = {} ms".format(key, i, simulatedTOA[key][i] / MS)) + + # create factor graph + graph = createGraph(microphones, simulatedTOA) + print(graph.at(0)) + + # Create initial estimate + initial_estimate = createInitialEstimate(n) + print(initial_estimate) + + # Optimize using Levenberg-Marquardt optimization. + params = LevenbergMarquardtParams() + params.setAbsoluteErrorTol(1e-10) + params.setVerbosityLM("SUMMARY") + optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params) + result = optimizer.optimize() + print("Final Result:\n", result) + + +if __name__ == '__main__': + TimeOfArrivalExample() + print("Example complete") diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index b2a746595..383c34915 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -86,6 +86,7 @@ struct traits : internal::Manifold {}; /// Time of arrival to given sensor class TimeOfArrival { const double speed_; ///< signal speed + public: typedef double result_type; @@ -93,6 +94,12 @@ class TimeOfArrival { explicit TimeOfArrival(double speed = 330) : speed_(speed) {} /// Calculate time of arrival + double measure(const Event& event, const Point3& sensor) const { + double distance = gtsam::distance3(event.location(), sensor); + return event.time() + distance / speed_; + } + + /// Calculate time of arrival, with derivatives double operator()(const Event& event, const Point3& sensor, // OptionalJacobian<1, 4> H1 = boost::none, // OptionalJacobian<1, 3> H2 = boost::none) const { diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index d77895d86..b46c5354b 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -377,6 +377,30 @@ virtual class RangeFactor : gtsam::NoiseModelFactor { typedef gtsam::RangeFactor RangeFactorRTV; +#include +class Event { + Event(); + Event(double t, const gtsam::Point3& p); + Event(double t, double x, double y, double z); + double time() const; + gtsam::Point3 location() const; + double height() const; + void print(string s) const; +}; + +class TimeOfArrival { + TimeOfArrival(); + TimeOfArrival(double speed); + double measure(const gtsam::Event& event, const gtsam::Point3& sensor) const; +}; + +#include +virtual class TOAFactor : gtsam::NonlinearFactor { + // For now, because of overload issues, we only expose constructor with known sensor coordinates: + TOAFactor(size_t key1, gtsam::Point3 sensor, double measured, + const gtsam::noiseModel::Base* noiseModel); + static void InsertEvent(size_t key, const gtsam::Event& event, gtsam::Values* values); +}; #include template diff --git a/gtsam_unstable/slam/TOAFactor.h b/gtsam_unstable/slam/TOAFactor.h index c52f1b54c..8ad4a14a6 100644 --- a/gtsam_unstable/slam/TOAFactor.h +++ b/gtsam_unstable/slam/TOAFactor.h @@ -57,6 +57,11 @@ class TOAFactor : public ExpressionFactor { double speed = 330) : TOAFactor(eventExpression, Expression(sensor), toaMeasurement, model, speed) {} + + static void InsertEvent(Key key, const Event& event, + boost::shared_ptr values) { + values->insert(key, event); + } }; } // namespace gtsam From 2a07cfed6604d4b9a7f43e71a7a600a0756b6fd0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Mar 2020 18:18:51 -0400 Subject: [PATCH 29/79] improved rotation transpose with quaternions --- gtsam/geometry/Rot3Q.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 377cc4a5f..64e8324b8 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -19,8 +19,8 @@ #ifdef GTSAM_USE_QUATERNIONS -#include #include +#include #include using namespace std; @@ -80,8 +80,8 @@ namespace gtsam { /* ************************************************************************* */ const Eigen::Transpose Rot3::transpose() const { - // `.eval()` to avoid aliasing effect due to transpose (allows compilation). - return matrix().eval().transpose(); + // `eval` for immediate evaluation (allows compilation). + return Rot3(matrix()).matrix().eval().transpose(); } /* ************************************************************************* */ From bde27615c506abf1b0343265af2e711514d866bc Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Mar 2020 18:19:40 -0400 Subject: [PATCH 30/79] test in testAHRSFactor is already fixed --- gtsam/navigation/tests/testAHRSFactor.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index d32553b94..dd216ce34 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -15,6 +15,7 @@ * @author Krunal Chande * @author Luca Carlone * @author Frank Dellaert + * @author Varun Agrawal */ #include @@ -200,7 +201,7 @@ TEST(AHRSFactor, Error) { // 1e-5 needs to be added only when using quaternions for rotations EXPECT(assert_equal(H3e, H3a, 1e-5)); - // FIXME!! DOes not work. Different matrix dimensions. + // 1e-5 needs to be added only when using quaternions for rotations } /* ************************************************************************* */ From 1d5239dd251e91e4a12907d5ce4bc0d1ddd96703 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Mar 2020 18:20:18 -0400 Subject: [PATCH 31/79] test for stability of RQ due to atan2 as well as fix --- gtsam/geometry/Rot3.cpp | 18 +++++++++++++++--- gtsam/geometry/tests/testRot3.cpp | 26 +++++++++++++++++++++++++- 2 files changed, 40 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 24bdd6f17..96c714166 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -197,15 +197,27 @@ Matrix3 Rot3::LogmapDerivative(const Vector3& x) { /* ************************************************************************* */ pair RQ(const Matrix3& A) { - double x = -atan2(-A(2, 1), A(2, 2)); + // atan2 has curious/unstable behavior near 0. + // If either x or y is close to zero, the results can vary depending on + // what the sign of either x or y is. + // E.g. for x = 1e-15, y = -0.08, atan2(x, y) != atan2(-x, y), + // even though arithmetically, they are both atan2(0, y). + // Suppressing small numbers to 0.0 doesn't work since the floating point + // representation still causes the issue due to a delta difference. + // The only fix is to convert to an int so we are guaranteed the value is 0. + // This lambda function checks if a number is close to 0. + // If yes, then return the integer representation, else do nothing. + auto suppress = [](auto x) { return abs(x) > 1e-15 ? x : int(x); }; + + double x = -atan2(-suppress(A(2, 1)), suppress(A(2, 2))); Rot3 Qx = Rot3::Rx(-x); Matrix3 B = A * Qx.matrix(); - double y = -atan2(B(2, 0), B(2, 2)); + double y = -atan2(suppress(B(2, 0)), suppress(B(2, 2))); Rot3 Qy = Rot3::Ry(-y); Matrix3 C = B * Qy.matrix(); - double z = -atan2(-C(1, 0), C(1, 1)); + double z = -atan2(-suppress(C(1, 0)), suppress(C(1, 1))); Rot3 Qz = Rot3::Rz(-z); Matrix3 R = C * Qz.matrix(); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index d9d08a48e..2cdb9c4f0 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -14,6 +14,7 @@ * @brief Unit tests for Rot3 class - common between Matrix and Quaternion * @author Alireza Fathi * @author Frank Dellaert + * @author Varun Agrawal */ #include @@ -380,7 +381,7 @@ TEST( Rot3, inverse ) Rot3 actual = R.inverse(actualH); CHECK(assert_equal(I,R*actual)); CHECK(assert_equal(I,actual*R)); - CHECK(assert_equal((Matrix)actual.matrix(), R.transpose())); + CHECK(assert_equal(actual, Rot3(R.transpose()))); Matrix numericalH = numericalDerivative11(testing::inverse, R); CHECK(assert_equal(numericalH,actualH)); @@ -502,6 +503,29 @@ TEST( Rot3, RQ) CHECK(assert_equal(expected,actual,1e-6)); } +/* ************************************************************************* */ +TEST( Rot3, RQStability) +{ + // Test case to check if xyz() is computed correctly when using quaternions. + Matrix actualR; + Vector actualXyz; + + // A is equivalent to the below + double kDegree = M_PI / 180; + const Rot3 nRy = Rot3::Yaw(315 * kDegree); + const Rot3 yRp = Rot3::Pitch(275 * kDegree); + const Rot3 pRb = Rot3::Roll(180 * kDegree); + const Rot3 nRb = nRy * yRp * pRb; + + // A(2, 1) ~= -0.0 + // Since A(2, 2) < 0, x-angle should be positive + Matrix A = nRb.matrix(); + boost::tie(actualR, actualXyz) = RQ(A); + + Vector3 expectedXyz(3.14159, -1.48353, -0.785398); + CHECK(assert_equal(expectedXyz,actualXyz,1e-6)); +} + /* ************************************************************************* */ TEST( Rot3, expmapStability ) { Vector w = Vector3(78e-9, 5e-8, 97e-7); From 7b6a80eba247843ae74a74b53cfe93f4d15d371b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Mar 2020 19:14:19 -0400 Subject: [PATCH 32/79] Revert "test for stability of RQ due to atan2 as well as fix" This reverts commit 1d5239dd251e91e4a12907d5ce4bc0d1ddd96703. --- gtsam/geometry/Rot3.cpp | 18 +++--------------- gtsam/geometry/tests/testRot3.cpp | 26 +------------------------- 2 files changed, 4 insertions(+), 40 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 96c714166..24bdd6f17 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -197,27 +197,15 @@ Matrix3 Rot3::LogmapDerivative(const Vector3& x) { /* ************************************************************************* */ pair RQ(const Matrix3& A) { - // atan2 has curious/unstable behavior near 0. - // If either x or y is close to zero, the results can vary depending on - // what the sign of either x or y is. - // E.g. for x = 1e-15, y = -0.08, atan2(x, y) != atan2(-x, y), - // even though arithmetically, they are both atan2(0, y). - // Suppressing small numbers to 0.0 doesn't work since the floating point - // representation still causes the issue due to a delta difference. - // The only fix is to convert to an int so we are guaranteed the value is 0. - // This lambda function checks if a number is close to 0. - // If yes, then return the integer representation, else do nothing. - auto suppress = [](auto x) { return abs(x) > 1e-15 ? x : int(x); }; - - double x = -atan2(-suppress(A(2, 1)), suppress(A(2, 2))); + double x = -atan2(-A(2, 1), A(2, 2)); Rot3 Qx = Rot3::Rx(-x); Matrix3 B = A * Qx.matrix(); - double y = -atan2(suppress(B(2, 0)), suppress(B(2, 2))); + double y = -atan2(B(2, 0), B(2, 2)); Rot3 Qy = Rot3::Ry(-y); Matrix3 C = B * Qy.matrix(); - double z = -atan2(-suppress(C(1, 0)), suppress(C(1, 1))); + double z = -atan2(-C(1, 0), C(1, 1)); Rot3 Qz = Rot3::Rz(-z); Matrix3 R = C * Qz.matrix(); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 2cdb9c4f0..d9d08a48e 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -14,7 +14,6 @@ * @brief Unit tests for Rot3 class - common between Matrix and Quaternion * @author Alireza Fathi * @author Frank Dellaert - * @author Varun Agrawal */ #include @@ -381,7 +380,7 @@ TEST( Rot3, inverse ) Rot3 actual = R.inverse(actualH); CHECK(assert_equal(I,R*actual)); CHECK(assert_equal(I,actual*R)); - CHECK(assert_equal(actual, Rot3(R.transpose()))); + CHECK(assert_equal((Matrix)actual.matrix(), R.transpose())); Matrix numericalH = numericalDerivative11(testing::inverse, R); CHECK(assert_equal(numericalH,actualH)); @@ -503,29 +502,6 @@ TEST( Rot3, RQ) CHECK(assert_equal(expected,actual,1e-6)); } -/* ************************************************************************* */ -TEST( Rot3, RQStability) -{ - // Test case to check if xyz() is computed correctly when using quaternions. - Matrix actualR; - Vector actualXyz; - - // A is equivalent to the below - double kDegree = M_PI / 180; - const Rot3 nRy = Rot3::Yaw(315 * kDegree); - const Rot3 yRp = Rot3::Pitch(275 * kDegree); - const Rot3 pRb = Rot3::Roll(180 * kDegree); - const Rot3 nRb = nRy * yRp * pRb; - - // A(2, 1) ~= -0.0 - // Since A(2, 2) < 0, x-angle should be positive - Matrix A = nRb.matrix(); - boost::tie(actualR, actualXyz) = RQ(A); - - Vector3 expectedXyz(3.14159, -1.48353, -0.785398); - CHECK(assert_equal(expectedXyz,actualXyz,1e-6)); -} - /* ************************************************************************* */ TEST( Rot3, expmapStability ) { Vector w = Vector3(78e-9, 5e-8, 97e-7); From dec918c3d54ee24764c0ff1d59152d8f632075a9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Mar 2020 19:25:31 -0400 Subject: [PATCH 33/79] undo return type of Eigen::Transpose, and add back TODO to optimize RPY --- gtsam/geometry/Rot3.h | 11 ++++++++++- gtsam/geometry/Rot3M.cpp | 2 +- gtsam/geometry/Rot3Q.cpp | 5 +++-- 3 files changed, 14 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 3e0e33609..fead20ea9 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -411,7 +411,7 @@ namespace gtsam { /** * Return 3*3 transpose (inverse) rotation matrix */ - const Eigen::Transpose transpose() const; + Matrix3 transpose() const; /// @deprecated, this is base 1, and was just confusing Point3 column(int index) const; @@ -440,16 +440,25 @@ namespace gtsam { /** * Accessor to get to component of angle representations + * NOTE: these are not efficient to get to multiple separate parts, + * you should instead use xyz() or ypr() + * TODO: make this more efficient */ inline double roll() const { return xyz()(0); } /** * Accessor to get to component of angle representations + * NOTE: these are not efficient to get to multiple separate parts, + * you should instead use xyz() or ypr() + * TODO: make this more efficient */ inline double pitch() const { return xyz()(1); } /** * Accessor to get to component of angle representations + * NOTE: these are not efficient to get to multiple separate parts, + * you should instead use xyz() or ypr() + * TODO: make this more efficient */ inline double yaw() const { return xyz()(2); } diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 85d9923fb..46a07e50a 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -110,7 +110,7 @@ Rot3 Rot3::operator*(const Rot3& R2) const { } /* ************************************************************************* */ -const Eigen::Transpose Rot3::transpose() const { +Matrix3 Rot3::transpose() const { return rot_.matrix().transpose(); } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 64e8324b8..0116f137d 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -79,9 +79,10 @@ namespace gtsam { } /* ************************************************************************* */ - const Eigen::Transpose Rot3::transpose() const { + Matrix3 Rot3::transpose() const { // `eval` for immediate evaluation (allows compilation). - return Rot3(matrix()).matrix().eval().transpose(); + // return Rot3(matrix()).matrix().eval().transpose(); + return matrix().eval().transpose(); } /* ************************************************************************* */ From c96300a7c6b06768cd1fdd44dbb9aa501b42d9ff Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Mar 2020 12:04:32 -0400 Subject: [PATCH 34/79] added const to matrix() function for Rot3 to be consistent with SOn and added note about using Eigen transpose expression --- gtsam/geometry/Rot3.h | 4 ++-- gtsam/geometry/Rot3M.cpp | 4 ++-- gtsam/geometry/Rot3Q.cpp | 15 ++++++++++----- gtsam/geometry/tests/testRot3.cpp | 2 +- 4 files changed, 15 insertions(+), 10 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index fead20ea9..9b3bff53a 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -406,12 +406,12 @@ namespace gtsam { /// @{ /** return 3*3 rotation matrix */ - Matrix3 matrix() const; + const Matrix3 matrix() const; /** * Return 3*3 transpose (inverse) rotation matrix */ - Matrix3 transpose() const; + const Matrix3 transpose() const; /// @deprecated, this is base 1, and was just confusing Point3 column(int index) const; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 46a07e50a..b39f3725e 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -110,7 +110,7 @@ Rot3 Rot3::operator*(const Rot3& R2) const { } /* ************************************************************************* */ -Matrix3 Rot3::transpose() const { +const Matrix3 Rot3::transpose() const { return rot_.matrix().transpose(); } @@ -175,7 +175,7 @@ Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) { } /* ************************************************************************* */ -Matrix3 Rot3::matrix() const { +const Matrix3 Rot3::matrix() const { return rot_.matrix(); } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 0116f137d..9c06d54e9 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -79,10 +79,15 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix3 Rot3::transpose() const { - // `eval` for immediate evaluation (allows compilation). - // return Rot3(matrix()).matrix().eval().transpose(); - return matrix().eval().transpose(); + // TODO: Maybe use return type `const Eigen::Transpose`? + // It works in Rot3M but not here, because of some weird behavior + // due to Eigen's expression templates which needs more investigation. + // For example, if we use matrix(), the value returned has a 1e-10, + // and if we use quaternion_.toRotationMatrix(), the matrix is arbitrary. + // Using eval() here doesn't help, it only helps if we use it in + // the downstream code. + const Matrix3 Rot3::transpose() const { + return matrix().transpose(); } /* ************************************************************************* */ @@ -115,7 +120,7 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix3 Rot3::matrix() const {return quaternion_.toRotationMatrix();} + const Matrix3 Rot3::matrix() const {return quaternion_.toRotationMatrix();} /* ************************************************************************* */ Point3 Rot3::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index d9d08a48e..eeda3c3d3 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -380,7 +380,7 @@ TEST( Rot3, inverse ) Rot3 actual = R.inverse(actualH); CHECK(assert_equal(I,R*actual)); CHECK(assert_equal(I,actual*R)); - CHECK(assert_equal((Matrix)actual.matrix(), R.transpose())); + CHECK(assert_equal(actual.matrix(), R.transpose())); Matrix numericalH = numericalDerivative11(testing::inverse, R); CHECK(assert_equal(numericalH,actualH)); From b058adc675782354561ad94507e360020faa790f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 20 Mar 2020 17:50:34 -0400 Subject: [PATCH 35/79] remove const from return types for Rot3 --- gtsam/geometry/Rot3.h | 4 ++-- gtsam/geometry/Rot3M.cpp | 4 ++-- gtsam/geometry/Rot3Q.cpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index b900b8c82..fc3a8b3f2 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -407,12 +407,12 @@ namespace gtsam { /// @{ /** return 3*3 rotation matrix */ - const Matrix3 matrix() const; + Matrix3 matrix() const; /** * Return 3*3 transpose (inverse) rotation matrix */ - const Matrix3 transpose() const; + Matrix3 transpose() const; /// @deprecated, this is base 1, and was just confusing Point3 column(int index) const; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index b39f3725e..46a07e50a 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -110,7 +110,7 @@ Rot3 Rot3::operator*(const Rot3& R2) const { } /* ************************************************************************* */ -const Matrix3 Rot3::transpose() const { +Matrix3 Rot3::transpose() const { return rot_.matrix().transpose(); } @@ -175,7 +175,7 @@ Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) { } /* ************************************************************************* */ -const Matrix3 Rot3::matrix() const { +Matrix3 Rot3::matrix() const { return rot_.matrix(); } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 9c06d54e9..d609c289c 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -86,7 +86,7 @@ namespace gtsam { // and if we use quaternion_.toRotationMatrix(), the matrix is arbitrary. // Using eval() here doesn't help, it only helps if we use it in // the downstream code. - const Matrix3 Rot3::transpose() const { + Matrix3 Rot3::transpose() const { return matrix().transpose(); } @@ -120,7 +120,7 @@ namespace gtsam { } /* ************************************************************************* */ - const Matrix3 Rot3::matrix() const {return quaternion_.toRotationMatrix();} + Matrix3 Rot3::matrix() const {return quaternion_.toRotationMatrix();} /* ************************************************************************* */ Point3 Rot3::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); } From 8fdbf2fa6e25f3fde36947e710a7221fe74f4434 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 21 Mar 2020 15:29:07 -0400 Subject: [PATCH 36/79] added Cal3_S2 header and Frank's recommendations --- examples/CameraResectioning.cpp | 1 + examples/ISAM2Example_SmartFactor.cpp | 1 + examples/ImuFactorExample2.cpp | 3 ++- gtsam/sam/tests/testRangeFactor.cpp | 8 +++++--- 4 files changed, 9 insertions(+), 4 deletions(-) diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index e93e6ffca..b12418098 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include using namespace gtsam; diff --git a/examples/ISAM2Example_SmartFactor.cpp b/examples/ISAM2Example_SmartFactor.cpp index 46b9fcd47..21fe6e661 100644 --- a/examples/ISAM2Example_SmartFactor.cpp +++ b/examples/ISAM2Example_SmartFactor.cpp @@ -5,6 +5,7 @@ */ #include +#include #include #include #include diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp index 4ebc342eb..f83a539af 100644 --- a/examples/ImuFactorExample2.cpp +++ b/examples/ImuFactorExample2.cpp @@ -1,5 +1,6 @@ #include +#include #include #include #include @@ -34,7 +35,7 @@ int main(int argc, char* argv[]) { double radius = 30; const Point3 up(0, 0, 1), target(0, 0, 0); const Point3 position(radius, 0, 0); - const PinholeCamera camera = PinholeCamera::Lookat(position, target, up); + const auto camera = PinholeCamera::Lookat(position, target, up); const auto pose_0 = camera.pose(); // Now, create a constant-twist scenario that makes the camera orbit the diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp index 0d19918f6..54d4a43c0 100644 --- a/gtsam/sam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -356,9 +356,11 @@ TEST(RangeFactor, Point3) { /* ************************************************************************* */ // Do tests with PinholeCamera TEST( RangeFactor, Camera) { - RangeFactor,Point3> factor1(poseKey, pointKey, measurement, model); - RangeFactor,Pose3> factor2(poseKey, pointKey, measurement, model); - RangeFactor,PinholeCamera> factor3(poseKey, pointKey, measurement, model); + using Camera = PinholeCamera; + + RangeFactor factor1(poseKey, pointKey, measurement, model); + RangeFactor factor2(poseKey, pointKey, measurement, model); + RangeFactor factor3(poseKey, pointKey, measurement, model); } /* ************************************************************************* */ From 87c338a18b2d083bd66dc9b755d17fad60f4f937 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 22 Mar 2020 12:32:16 -0400 Subject: [PATCH 37/79] import python classes --- cython/gtsam/examples/ImuFactorExample2.py | 40 ++++++++++--------- cython/gtsam/utils/visual_data_generator.py | 43 +++++++++++---------- 2 files changed, 44 insertions(+), 39 deletions(-) diff --git a/cython/gtsam/examples/ImuFactorExample2.py b/cython/gtsam/examples/ImuFactorExample2.py index 4b86d4837..80f573d12 100644 --- a/cython/gtsam/examples/ImuFactorExample2.py +++ b/cython/gtsam/examples/ImuFactorExample2.py @@ -14,6 +14,11 @@ from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 import gtsam import gtsam.utils.plot as gtsam_plot +from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2, + ConstantTwistScenario, ImuFactor, NonlinearFactorGraph, + PinholeCameraCal3_S2, Point3, Pose3, + PriorFactorConstantBias, PriorFactorPose3, + PriorFactorVector, Rot3, Values) def X(key): @@ -69,8 +74,8 @@ PARAMS.setUse2ndOrderCoriolis(False) PARAMS.setOmegaCoriolis(vector3(0, 0, 0)) BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1) -DELTA = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0), - gtsam.Point3(0.05, -0.10, 0.20)) +DELTA = Pose3(Rot3.Rodrigues(0, 0, 0), + Point3(0.05, -0.10, 0.20)) def IMU_example(): @@ -78,10 +83,10 @@ def IMU_example(): # Start with a camera on x-axis looking at origin radius = 30 - up = gtsam.Point3(0, 0, 1) - target = gtsam.Point3(0, 0, 0) - position = gtsam.Point3(radius, 0, 0) - camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, gtsam.Cal3_S2()) + up = Point3(0, 0, 1) + target = Point3(0, 0, 0) + position = Point3(radius, 0, 0) + camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2()) pose_0 = camera.pose() # Create the set of ground-truth landmarks and poses @@ -90,37 +95,37 @@ def IMU_example(): angular_velocity_vector = vector3(0, -angular_velocity, 0) linear_velocity_vector = vector3(radius * angular_velocity, 0, 0) - scenario = gtsam.ConstantTwistScenario( + scenario = ConstantTwistScenario( angular_velocity_vector, linear_velocity_vector, pose_0) # Create a factor graph - newgraph = gtsam.NonlinearFactorGraph() + newgraph = NonlinearFactorGraph() # Create (incremental) ISAM2 solver - isam = gtsam.ISAM2() + isam = ISAM2() # Create the initial estimate to the solution # Intentionally initialize the variables off from the ground truth - initialEstimate = gtsam.Values() + initialEstimate = Values() # Add a prior on pose x0. This indirectly specifies where the origin is. # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw noise = gtsam.noiseModel_Diagonal.Sigmas( np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3])) - newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise)) + newgraph.push_back(PriorFactorPose3(X(0), pose_0, noise)) # Add imu priors biasKey = gtsam.symbol(ord('b'), 0) biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) - biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(), - biasnoise) + biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(), + biasnoise) newgraph.push_back(biasprior) initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) # Calculate with correct initial velocity n_velocity = vector3(0, angular_velocity * radius, 0) - velprior = gtsam.PriorFactorVector(V(0), n_velocity, velnoise) + velprior = PriorFactorVector(V(0), n_velocity, velnoise) newgraph.push_back(velprior) initialEstimate.insert(V(0), n_velocity) @@ -141,7 +146,7 @@ def IMU_example(): # Add Bias variables periodically if i % 5 == 0: biasKey += 1 - factor = gtsam.BetweenFactorConstantBias( + factor = BetweenFactorConstantBias( biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE) newgraph.add(factor) initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) @@ -154,8 +159,7 @@ def IMU_example(): accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t) # Add Imu Factor - imufac = gtsam.ImuFactor( - X(i - 1), V(i - 1), X(i), V(i), biasKey, accum) + imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum) newgraph.add(imufac) # insert new velocity, which is wrong @@ -168,7 +172,7 @@ def IMU_example(): ISAM2_plot(result) # reset - newgraph = gtsam.NonlinearFactorGraph() + newgraph = NonlinearFactorGraph() initialEstimate.clear() diff --git a/cython/gtsam/utils/visual_data_generator.py b/cython/gtsam/utils/visual_data_generator.py index 7cd885d49..f04588e70 100644 --- a/cython/gtsam/utils/visual_data_generator.py +++ b/cython/gtsam/utils/visual_data_generator.py @@ -1,8 +1,9 @@ from __future__ import print_function import numpy as np -from math import pi, cos, sin + import gtsam +from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point2, Point3, Pose3 class Options: @@ -10,7 +11,7 @@ class Options: Options to generate test scenario """ - def __init__(self, triangle=False, nrCameras=3, K=gtsam.Cal3_S2()): + def __init__(self, triangle=False, nrCameras=3, K=Cal3_S2()): """ Options to generate test scenario @param triangle: generate a triangle scene with 3 points if True, otherwise @@ -27,10 +28,10 @@ class GroundTruth: Object holding generated ground-truth data """ - def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4): + def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4): self.K = K - self.cameras = [gtsam.Pose3()] * nrCameras - self.points = [gtsam.Point3()] * nrPoints + self.cameras = [Pose3()] * nrCameras + self.points = [Point3()] * nrPoints def print_(self, s=""): print(s) @@ -52,11 +53,11 @@ class Data: class NoiseModels: pass - def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4): + def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4): self.K = K - self.Z = [x[:] for x in [[gtsam.Point2()] * nrPoints] * nrCameras] + self.Z = [x[:] for x in [[Point2()] * nrPoints] * nrCameras] self.J = [x[:] for x in [[0] * nrPoints] * nrCameras] - self.odometry = [gtsam.Pose3()] * nrCameras + self.odometry = [Pose3()] * nrCameras # Set Noise parameters self.noiseModels = Data.NoiseModels() @@ -73,7 +74,7 @@ class Data: def generate_data(options): """ Generate ground-truth and measurement data. """ - K = gtsam.Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.) + K = Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.) nrPoints = 3 if options.triangle else 8 truth = GroundTruth(K=K, nrCameras=options.nrCameras, nrPoints=nrPoints) @@ -83,26 +84,26 @@ def generate_data(options): if options.triangle: # Create a triangle target, just 3 points on a plane r = 10 for j in range(len(truth.points)): - theta = j * 2 * pi / nrPoints - truth.points[j] = gtsam.Point3(r * cos(theta), r * sin(theta), 0) + theta = j * 2 * np.pi / nrPoints + truth.points[j] = Point3(r * np.cos(theta), r * np.sin(theta), 0) else: # 3D landmarks as vertices of a cube truth.points = [ - gtsam.Point3(10, 10, 10), gtsam.Point3(-10, 10, 10), - gtsam.Point3(-10, -10, 10), gtsam.Point3(10, -10, 10), - gtsam.Point3(10, 10, -10), gtsam.Point3(-10, 10, -10), - gtsam.Point3(-10, -10, -10), gtsam.Point3(10, -10, -10) + Point3(10, 10, 10), Point3(-10, 10, 10), + Point3(-10, -10, 10), Point3(10, -10, 10), + Point3(10, 10, -10), Point3(-10, 10, -10), + Point3(-10, -10, -10), Point3(10, -10, -10) ] # Create camera cameras on a circle around the triangle height = 10 r = 40 for i in range(options.nrCameras): - theta = i * 2 * pi / options.nrCameras - t = gtsam.Point3(r * cos(theta), r * sin(theta), height) - truth.cameras[i] = gtsam.PinholeCameraCal3_S2.Lookat(t, - gtsam.Point3(), - gtsam.Point3(0, 0, 1), - truth.K) + theta = i * 2 * np.pi / options.nrCameras + t = Point3(r * np.cos(theta), r * np.sin(theta), height) + truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t, + Point3(), + Point3(0, 0, 1), + truth.K) # Create measurements for j in range(nrPoints): # All landmarks seen in every frame From 73271816a6c26aab9d40de7f8011dddf5d243e30 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 22 Mar 2020 12:45:21 -0400 Subject: [PATCH 38/79] make exceptions as const reference --- gtsam/nonlinear/utilities.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index 867db70e0..1228cd4db 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -237,12 +237,12 @@ Values localToWorld(const Values& local, const Pose2& base, // if value is a Pose2, compose it with base pose Pose2 pose = local.at(key); world.insert(key, base.compose(pose)); - } catch (std::exception e1) { + } catch (const std::exception& e1) { try { // if value is a Point2, transform it from base pose Point2 point = local.at(key); world.insert(key, base.transformFrom(point)); - } catch (std::exception e2) { + } catch (const std::exception& e2) { // if not Pose2 or Point2, do nothing } } From d2d5ce1166d6e01101b2679020edf66899305702 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 22 Mar 2020 12:46:25 -0400 Subject: [PATCH 39/79] Eigen alignment --- gtsam/nonlinear/NonlinearEquality.h | 6 ++++++ gtsam_unstable/slam/AHRS.h | 2 ++ 2 files changed, 8 insertions(+) diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 4d928482e..d4eb655c3 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -175,6 +175,8 @@ public: /// @} + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + private: /** Serialization function */ @@ -263,6 +265,8 @@ public: traits::Print(value_, "Value"); } + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + private: /** Serialization function */ @@ -327,6 +331,8 @@ public: return traits::Local(x1,x2); } + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + private: /** Serialization function */ diff --git a/gtsam_unstable/slam/AHRS.h b/gtsam_unstable/slam/AHRS.h index f22de48cf..35b4677d5 100644 --- a/gtsam_unstable/slam/AHRS.h +++ b/gtsam_unstable/slam/AHRS.h @@ -77,6 +77,8 @@ public: void print(const std::string& s = "") const; virtual ~AHRS(); + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } /* namespace gtsam */ From c2d7df3f148894badce29fd7ee21cdaa46e9fca2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 22 Mar 2020 20:03:10 -0400 Subject: [PATCH 40/79] make exception as reference --- gtsam/geometry/tests/testEssentialMatrix.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index c923e398b..86a498cdc 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -159,7 +159,7 @@ TEST (EssentialMatrix, rotate) { Matrix actH1, actH2; try { bodyE.rotate(cRb, actH1, actH2); - } catch (exception e) { + } catch (exception& e) { } // avoid exception Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), // expH2 = numericalDerivative22(rotate_, bodyE, cRb); From 9bef6bfded7c0c46d4706d0d23f1b3fe963c539d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 22 Mar 2020 20:04:04 -0400 Subject: [PATCH 41/79] initialize PreintegrationParams default constructor and make serialization test more explicit --- gtsam/navigation/PreintegrationParams.h | 9 +++++++-- gtsam/navigation/tests/testImuFactorSerialization.cpp | 6 +++--- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h index 0fb54a358..962fef277 100644 --- a/gtsam/navigation/PreintegrationParams.h +++ b/gtsam/navigation/PreintegrationParams.h @@ -29,6 +29,13 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams { bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration Vector3 n_gravity; ///< Gravity vector in nav frame + /// Default constructor for serialization only + PreintegrationParams() + : accelerometerCovariance(I_3x3), + integrationCovariance(I_3x3), + use2ndOrderCoriolis(false), + n_gravity(0, 0, -1) {} + /// The Params constructor insists on getting the navigation frame gravity vector /// For convenience, two commonly used conventions are provided by named constructors below PreintegrationParams(const Vector3& n_gravity) @@ -60,8 +67,6 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams { bool getUse2ndOrderCoriolis() const { return use2ndOrderCoriolis; } protected: - /// Default constructor for serialization only: uninitialized! - PreintegrationParams() {} /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/navigation/tests/testImuFactorSerialization.cpp b/gtsam/navigation/tests/testImuFactorSerialization.cpp index 9f9781d2c..59d0ac199 100644 --- a/gtsam/navigation/tests/testImuFactorSerialization.cpp +++ b/gtsam/navigation/tests/testImuFactorSerialization.cpp @@ -64,9 +64,9 @@ TEST(ImuFactor, serialization) { ImuFactor factor(1, 2, 3, 4, 5, pim); - EXPECT(equalsObj(factor)); - EXPECT(equalsXML(factor)); - EXPECT(equalsBinary(factor)); + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); } /* ************************************************************************* */ From 0479223b3fbbdc7b5957c682cda03d5471cb8cc5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 22 Mar 2020 20:04:33 -0400 Subject: [PATCH 42/79] suppress warning when wrapper indentation is too long --- CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6ac85bff6..42ebe7ea4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -416,6 +416,8 @@ add_subdirectory(CppUnitLite) # Build wrap if (GTSAM_BUILD_WRAP) add_subdirectory(wrap) + # suppress warning of cython line being too long + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-misleading-indentation") endif(GTSAM_BUILD_WRAP) # Build GTSAM library From cd809309f72e3193518ffaf01550126884d5a16e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 23 Mar 2020 08:04:17 -0400 Subject: [PATCH 43/79] suppress warning only on linux for now, need to figure out for other OSes --- CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 42ebe7ea4..be556d27e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -417,7 +417,9 @@ add_subdirectory(CppUnitLite) if (GTSAM_BUILD_WRAP) add_subdirectory(wrap) # suppress warning of cython line being too long - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-misleading-indentation") + if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-misleading-indentation") + endif() endif(GTSAM_BUILD_WRAP) # Build GTSAM library From 1cfcd2075a6beb8e6c289029feaa9caa6dd9fcb0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 25 Mar 2020 08:59:58 -0400 Subject: [PATCH 44/79] Style and dates --- .../examples/TimeOfArrivalExample.py | 30 +++++++++---------- .../examples/TimeOfArrivalExample.cpp | 2 +- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/cython/gtsam_unstable/examples/TimeOfArrivalExample.py b/cython/gtsam_unstable/examples/TimeOfArrivalExample.py index ac3af8a38..6ba06f0f2 100644 --- a/cython/gtsam_unstable/examples/TimeOfArrivalExample.py +++ b/cython/gtsam_unstable/examples/TimeOfArrivalExample.py @@ -1,5 +1,5 @@ """ -GTSAM Copyright 2010-2018, Georgia Tech Research Corporation, +GTSAM Copyright 2010-2020, Georgia Tech Research Corporation, Atlanta, Georgia 30332-0415 All Rights Reserved Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -23,7 +23,7 @@ CM = 1e-2 TIME_OF_ARRIVAL = TimeOfArrival(330) -def defineMicrophones(): +def define_microphones(): """Create microphones.""" height = 0.5 microphones = [] @@ -34,7 +34,7 @@ def defineMicrophones(): return microphones -def createTrajectory(n): +def create_trajectory(n): """Create ground truth trajectory.""" trajectory = [] timeOfEvent = 10 @@ -47,19 +47,19 @@ def createTrajectory(n): return trajectory -def simulateOneTOA(microphones, event): +def simulate_one_toa(microphones, event): """Simulate time-of-arrival measurements for a single event.""" return [TIME_OF_ARRIVAL.measure(event, microphones[i]) for i in range(len(microphones))] -def simulateTOA(microphones, trajectory): +def simulate_toa(microphones, trajectory): """Simulate time-of-arrival measurements for an entire trajectory.""" - return [simulateOneTOA(microphones, event) + return [simulate_one_toa(microphones, event) for event in trajectory] -def createGraph(microphones, simulatedTOA): +def create_graph(microphones, simulatedTOA): """Create factor graph.""" graph = NonlinearFactorGraph() @@ -77,7 +77,7 @@ def createGraph(microphones, simulatedTOA): return graph -def createInitialEstimate(n): +def create_initial_estimate(n): """Create initial estimate for n events.""" initial = Values() zero = Event() @@ -86,32 +86,32 @@ def createInitialEstimate(n): return initial -def TimeOfArrivalExample(): +def toa_example(): """Run example with 4 microphones and 5 events in a straight line.""" # Create microphones - microphones = defineMicrophones() + microphones = define_microphones() K = len(microphones) for i in range(K): print("mic {} = {}".format(i, microphones[i])) # Create a ground truth trajectory n = 5 - groundTruth = createTrajectory(n) + groundTruth = create_trajectory(n) for event in groundTruth: print(event) # Simulate time-of-arrival measurements - simulatedTOA = simulateTOA(microphones, groundTruth) + simulatedTOA = simulate_toa(microphones, groundTruth) for key in range(n): for i in range(K): print("z_{}{} = {} ms".format(key, i, simulatedTOA[key][i] / MS)) # create factor graph - graph = createGraph(microphones, simulatedTOA) + graph = create_graph(microphones, simulatedTOA) print(graph.at(0)) # Create initial estimate - initial_estimate = createInitialEstimate(n) + initial_estimate = create_initial_estimate(n) print(initial_estimate) # Optimize using Levenberg-Marquardt optimization. @@ -124,5 +124,5 @@ def TimeOfArrivalExample(): if __name__ == '__main__': - TimeOfArrivalExample() + toa_example() print("Example complete") diff --git a/gtsam_unstable/examples/TimeOfArrivalExample.cpp b/gtsam_unstable/examples/TimeOfArrivalExample.cpp index f49d47fb7..9991e04b6 100644 --- a/gtsam_unstable/examples/TimeOfArrivalExample.cpp +++ b/gtsam_unstable/examples/TimeOfArrivalExample.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) From 6945845868bc3598ddcbb255e9e3bae2964af378 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Wed, 25 Mar 2020 20:03:03 -0400 Subject: [PATCH 45/79] add default value of GTSAM_WITH_TBB --- .travis.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/.travis.sh b/.travis.sh index bc9029595..d57b784df 100755 --- a/.travis.sh +++ b/.travis.sh @@ -24,6 +24,7 @@ function configure() -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \ -DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \ -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \ + -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-ON} \ -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \ -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \ From ea8f774f8d723f053e3c53170476673e2fbdad75 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Thu, 26 Mar 2020 13:38:17 -0400 Subject: [PATCH 46/79] make GTSAM_WITH_TBB=OFF default in ci --- .travis.sh | 2 +- .travis.yml | 18 +++++++++--------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/.travis.sh b/.travis.sh index d57b784df..78351f095 100755 --- a/.travis.sh +++ b/.travis.sh @@ -24,7 +24,7 @@ function configure() -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \ -DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \ -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \ - -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-ON} \ + -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \ -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \ -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \ diff --git a/.travis.yml b/.travis.yml index e4e1a3440..870a4bdb9 100644 --- a/.travis.yml +++ b/.travis.yml @@ -44,7 +44,7 @@ jobs: - stage: compile os: osx compiler: gcc - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF script: bash .travis.sh -b - stage: compile os: osx @@ -55,7 +55,7 @@ jobs: - stage: compile os: osx compiler: clang - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF script: bash .travis.sh -b - stage: compile os: osx @@ -66,7 +66,7 @@ jobs: - stage: compile os: linux compiler: gcc - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF script: bash .travis.sh -b - stage: compile os: linux @@ -77,7 +77,7 @@ jobs: - stage: compile os: linux compiler: clang - env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF + env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF script: bash .travis.sh -b - stage: compile os: linux @@ -88,13 +88,13 @@ jobs: - stage: special os: linux compiler: clang - env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON GTSAM_WITH_TBB=OFF + env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON script: bash .travis.sh -b -# on Linux, with GTSAM_WITH_TBB not off to make sure GTSAM still compiles/tests +# on Linux, with GTSAM_WITH_TBB on to make sure GTSAM still compiles/tests - stage: special os: linux compiler: gcc - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=ON script: bash .travis.sh -b # -------- STAGE 2: TESTS ----------- # on Mac, GCC @@ -106,7 +106,7 @@ jobs: - stage: test os: osx compiler: clang - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF script: bash .travis.sh -t - stage: test os: linux @@ -116,7 +116,7 @@ jobs: - stage: test os: linux compiler: gcc - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF script: bash .travis.sh -t - stage: test os: linux From 88005a99a13dff3dacb726ebb9ae36b4be63df15 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Thu, 26 Mar 2020 13:46:30 -0400 Subject: [PATCH 47/79] fix tbb CMakeLists indentation --- CMakeLists.txt | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 56250a12c..57c9d5f2f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -199,17 +199,17 @@ find_package(TBB 4.4 COMPONENTS tbb tbbmalloc) # Set up variables if we're using TBB if(TBB_FOUND AND GTSAM_WITH_TBB) - set(GTSAM_USE_TBB 1) # This will go into config.h - if (${TBB_VERSION_MAJOR} GREATER_EQUAL 2020) - set(TBB_GREATER_EQUAL_2020 1) - else() - set(TBB_GREATER_EQUAL_2020 0) - endif() - # all definitions and link requisites will go via imported targets: - # tbb & tbbmalloc - list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc) + set(GTSAM_USE_TBB 1) # This will go into config.h + if (${TBB_VERSION_MAJOR} GREATER_EQUAL 2020) + set(TBB_GREATER_EQUAL_2020 1) + else() + set(TBB_GREATER_EQUAL_2020 0) + endif() + # all definitions and link requisites will go via imported targets: + # tbb & tbbmalloc + list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc) else() - set(GTSAM_USE_TBB 0) # This will go into config.h + set(GTSAM_USE_TBB 0) # This will go into config.h endif() ############################################################################### From 80fc06cad7fbb31089e4e11709fd7c81bc98327b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 26 Mar 2020 16:03:51 -0400 Subject: [PATCH 48/79] function to add tbb with debug --- .travis.sh | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/.travis.sh b/.travis.sh index 78351f095..9ff1f22cd 100755 --- a/.travis.sh +++ b/.travis.sh @@ -1,5 +1,17 @@ #!/bin/bash +# install TBB with _debug.so files +function install_tbb() +{ + wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.5/tbb44_20160526oss_lin.tgz + if [ $(uname -s) == "Linux"]; then + tar -xvf tbb44_20160526oss_lin.tgz + elif [ $(uname -s) == "Linux" ]; then + tar -xvf tbb44_20160526oss_mac.tgz + fi + source tbb44_20160526oss/bin/tbbvars.sh intel64 linux auto_tbbroot +} + # common tasks before either build or test function configure() { @@ -19,6 +31,8 @@ function configure() export CXX=g++-$GCC_VERSION fi + install_tbb + # GTSAM_BUILD_WITH_MARCH_NATIVE=OFF: to avoid crashes in builder VMs cmake $SOURCE_DIR \ -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \ From 026c794a9279b3b764f9101f4e410c25793c69c1 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Thu, 26 Mar 2020 16:32:05 -0400 Subject: [PATCH 49/79] use the same tbb as xenial --- .travis.sh | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/.travis.sh b/.travis.sh index 9ff1f22cd..5db84e1e5 100755 --- a/.travis.sh +++ b/.travis.sh @@ -3,13 +3,14 @@ # install TBB with _debug.so files function install_tbb() { - wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.5/tbb44_20160526oss_lin.tgz - if [ $(uname -s) == "Linux"]; then - tar -xvf tbb44_20160526oss_lin.tgz - elif [ $(uname -s) == "Linux" ]; then - tar -xvf tbb44_20160526oss_mac.tgz + if [ $(uname -s) == "Linux" ]; then + wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_lin.tgz + tar -xvf tbb44_20151115oss_lin.tgz + elif [ $(uname -s) == "Darwin" ]; then + wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_osx.tgz + tar -xvf tbb44_20151115oss_osx.tgz fi - source tbb44_20160526oss/bin/tbbvars.sh intel64 linux auto_tbbroot + source tbb44_20151115oss/bin/tbbvars.sh intel64 linux auto_tbbroot } # common tasks before either build or test From 4a356b9bd93ab4f7076a778b073c77b104a89c40 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 26 Mar 2020 18:00:47 -0400 Subject: [PATCH 50/79] improved install_tbb function --- .travis.sh | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/.travis.sh b/.travis.sh index 5db84e1e5..8fdf27bd3 100755 --- a/.travis.sh +++ b/.travis.sh @@ -5,12 +5,23 @@ function install_tbb() { if [ $(uname -s) == "Linux" ]; then wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_lin.tgz - tar -xvf tbb44_20151115oss_lin.tgz + tar -xf tbb44_20151115oss_lin.tgz elif [ $(uname -s) == "Darwin" ]; then wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_osx.tgz - tar -xvf tbb44_20151115oss_osx.tgz + tar -xf tbb44_20151115oss_osx.tgz fi - source tbb44_20151115oss/bin/tbbvars.sh intel64 linux auto_tbbroot + + # Variables needed for setting the correct library path + TBB_TARGET_ARCH="intel64" + TBBROOT=$(cd tbb44_20151115oss/bin && pwd -P)/.. + library_directory="${TBB_TARGET_ARCH}/gcc4.1" + + # Set library paths + MIC_LD_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LD_LIBRARY_PATH}"; export MIC_LD_LIBRARY_PATH + MIC_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LIBRARY_PATH}"; export MIC_LIBRARY_PATH + LD_LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LD_LIBRARY_PATH}"; export LD_LIBRARY_PATH + LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LIBRARY_PATH}"; export LIBRARY_PATH + CPATH="${TBBROOT}/include:$CPATH"; export CPATH } # common tasks before either build or test From 34069a60df3cb216f74a6faabdcc2ee54e7e2d90 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 26 Mar 2020 21:00:23 -0400 Subject: [PATCH 51/79] specific paths for Mac, removed libtbb from travis packages list --- .travis.sh | 28 ++++++++++++++++++---------- .travis.yml | 1 - 2 files changed, 18 insertions(+), 11 deletions(-) diff --git a/.travis.sh b/.travis.sh index 8fdf27bd3..1f681043a 100755 --- a/.travis.sh +++ b/.travis.sh @@ -6,21 +6,29 @@ function install_tbb() if [ $(uname -s) == "Linux" ]; then wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_lin.tgz tar -xf tbb44_20151115oss_lin.tgz + + TBBROOT=$(cd tbb44_20151115oss/bin && pwd -P)/.. + + TBB_TARGET_ARCH="intel64" + library_directory="${TBB_TARGET_ARCH}/gcc4.1" + + # Set library paths + MIC_LD_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LD_LIBRARY_PATH}"; export MIC_LD_LIBRARY_PATH + MIC_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LIBRARY_PATH}"; export MIC_LIBRARY_PATH + LD_LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LD_LIBRARY_PATH}"; export LD_LIBRARY_PATH + LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LIBRARY_PATH}"; export LIBRARY_PATH + elif [ $(uname -s) == "Darwin" ]; then wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_osx.tgz tar -xf tbb44_20151115oss_osx.tgz + + TBBROOT=$(cd tbb44_20151115oss/bin && pwd -P)/.. + + # Set library paths + LIBRARY_PATH="${TBBROOT}/lib:$LIBRARY_PATH"; export LIBRARY_PATH + DYLD_LIBRARY_PATH="${TBBROOT}/lib:$DYLD_LIBRARY_PATH"; export DYLD_LIBRARY_PATH fi - # Variables needed for setting the correct library path - TBB_TARGET_ARCH="intel64" - TBBROOT=$(cd tbb44_20151115oss/bin && pwd -P)/.. - library_directory="${TBB_TARGET_ARCH}/gcc4.1" - - # Set library paths - MIC_LD_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LD_LIBRARY_PATH}"; export MIC_LD_LIBRARY_PATH - MIC_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LIBRARY_PATH}"; export MIC_LIBRARY_PATH - LD_LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LD_LIBRARY_PATH}"; export LD_LIBRARY_PATH - LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LIBRARY_PATH}"; export LIBRARY_PATH CPATH="${TBBROOT}/include:$CPATH"; export CPATH } diff --git a/.travis.yml b/.travis.yml index 870a4bdb9..2de325818 100644 --- a/.travis.yml +++ b/.travis.yml @@ -16,7 +16,6 @@ addons: - cmake - libpython-dev python-numpy - libboost-all-dev - - libtbb-dev # before_install: # - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update ; fi From 2e34a175f7aca1bd0d8a1f11e83550b80bddfe57 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 26 Mar 2020 22:05:28 -0400 Subject: [PATCH 52/79] properly export variables --- .travis.sh | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.travis.sh b/.travis.sh index 1f681043a..708572ef2 100755 --- a/.travis.sh +++ b/.travis.sh @@ -13,10 +13,10 @@ function install_tbb() library_directory="${TBB_TARGET_ARCH}/gcc4.1" # Set library paths - MIC_LD_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LD_LIBRARY_PATH}"; export MIC_LD_LIBRARY_PATH - MIC_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LIBRARY_PATH}"; export MIC_LIBRARY_PATH - LD_LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LD_LIBRARY_PATH}"; export LD_LIBRARY_PATH - LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LIBRARY_PATH}"; export LIBRARY_PATH + export MIC_LD_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LD_LIBRARY_PATH}" + export MIC_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LIBRARY_PATH}" + export LD_LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LD_LIBRARY_PATH}" + export LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LIBRARY_PATH}" elif [ $(uname -s) == "Darwin" ]; then wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_osx.tgz @@ -25,8 +25,8 @@ function install_tbb() TBBROOT=$(cd tbb44_20151115oss/bin && pwd -P)/.. # Set library paths - LIBRARY_PATH="${TBBROOT}/lib:$LIBRARY_PATH"; export LIBRARY_PATH - DYLD_LIBRARY_PATH="${TBBROOT}/lib:$DYLD_LIBRARY_PATH"; export DYLD_LIBRARY_PATH + export LIBRARY_PATH="${TBBROOT}/lib:$LIBRARY_PATH" + export DYLD_LIBRARY_PATH="${TBBROOT}/lib:$DYLD_LIBRARY_PATH" fi CPATH="${TBBROOT}/include:$CPATH"; export CPATH From 7248b149fd0bbc7f077dc6bd5361bb280b4af6f3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 Mar 2020 08:39:06 -0400 Subject: [PATCH 53/79] save tbb download to /tmp --- .travis.sh | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/.travis.sh b/.travis.sh index 708572ef2..e08ab5a30 100755 --- a/.travis.sh +++ b/.travis.sh @@ -3,11 +3,11 @@ # install TBB with _debug.so files function install_tbb() { - if [ $(uname -s) == "Linux" ]; then - wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_lin.tgz - tar -xf tbb44_20151115oss_lin.tgz + if [ "$(uname -s)" == "Linux" ]; then + wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_lin.tgz -O /tmp/tbb442.tgz + tar -C /tmp -xvf /tmp/tbb442.tgz - TBBROOT=$(cd tbb44_20151115oss/bin && pwd -P)/.. + TBBROOT=/tmp/tbb44_20151115oss TBB_TARGET_ARCH="intel64" library_directory="${TBB_TARGET_ARCH}/gcc4.1" @@ -18,18 +18,18 @@ function install_tbb() export LD_LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LD_LIBRARY_PATH}" export LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LIBRARY_PATH}" - elif [ $(uname -s) == "Darwin" ]; then - wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_osx.tgz - tar -xf tbb44_20151115oss_osx.tgz + elif [ "$(uname -s)" == "Darwin" ]; then + wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_osx.tgz -O /tmp/tbb442.tgz + tar -C /tmp -xvf /tmp/tbb442.tgz - TBBROOT=$(cd tbb44_20151115oss/bin && pwd -P)/.. + TBBROOT=/tmp/tbb44_20151115oss # Set library paths export LIBRARY_PATH="${TBBROOT}/lib:$LIBRARY_PATH" export DYLD_LIBRARY_PATH="${TBBROOT}/lib:$DYLD_LIBRARY_PATH" fi - CPATH="${TBBROOT}/include:$CPATH"; export CPATH + # CPATH="${TBBROOT}/include:$CPATH"; export CPATH } # common tasks before either build or test From 5dc19e074674ab3aa7cc23656491294ead0cbef8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 Mar 2020 09:21:51 -0400 Subject: [PATCH 54/79] install tbb before install to allow propagation of env variables --- .travis.sh | 9 +++++---- .travis.yml | 5 +++-- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/.travis.sh b/.travis.sh index e08ab5a30..5f999e7a9 100755 --- a/.travis.sh +++ b/.travis.sh @@ -5,7 +5,7 @@ function install_tbb() { if [ "$(uname -s)" == "Linux" ]; then wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_lin.tgz -O /tmp/tbb442.tgz - tar -C /tmp -xvf /tmp/tbb442.tgz + tar -C /tmp -xf /tmp/tbb442.tgz TBBROOT=/tmp/tbb44_20151115oss @@ -20,7 +20,7 @@ function install_tbb() elif [ "$(uname -s)" == "Darwin" ]; then wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_osx.tgz -O /tmp/tbb442.tgz - tar -C /tmp -xvf /tmp/tbb442.tgz + tar -C /tmp -xf /tmp/tbb442.tgz TBBROOT=/tmp/tbb44_20151115oss @@ -51,8 +51,6 @@ function configure() export CXX=g++-$GCC_VERSION fi - install_tbb - # GTSAM_BUILD_WITH_MARCH_NATIVE=OFF: to avoid crashes in builder VMs cmake $SOURCE_DIR \ -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \ @@ -111,4 +109,7 @@ case $1 in -t) test ;; + -tbb) + install_tbb + ;; esac diff --git a/.travis.yml b/.travis.yml index 2de325818..7e5b33ec4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -17,8 +17,9 @@ addons: - libpython-dev python-numpy - libboost-all-dev -# before_install: -# - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update ; fi +before_install: + - ./.travis.sh -tbb + # - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update; fi install: - if [ "$TRAVIS_OS_NAME" == "osx" ]; then HOMEBREW_NO_AUTO_UPDATE=1 brew install ccache ; fi From 99e1c42282d2cde3bf5e42e33f38c14171820d6a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 Mar 2020 10:53:22 -0400 Subject: [PATCH 55/79] update travis.yml to install tbb during install phase, source script to add variables to current shell --- .travis.sh | 8 +++----- .travis.yml | 4 ++-- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/.travis.sh b/.travis.sh index 5f999e7a9..2bf4f038c 100755 --- a/.travis.sh +++ b/.travis.sh @@ -3,12 +3,12 @@ # install TBB with _debug.so files function install_tbb() { + TBBROOT=/tmp/tbb44_20151115oss + if [ "$(uname -s)" == "Linux" ]; then wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_lin.tgz -O /tmp/tbb442.tgz tar -C /tmp -xf /tmp/tbb442.tgz - TBBROOT=/tmp/tbb44_20151115oss - TBB_TARGET_ARCH="intel64" library_directory="${TBB_TARGET_ARCH}/gcc4.1" @@ -22,14 +22,12 @@ function install_tbb() wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_osx.tgz -O /tmp/tbb442.tgz tar -C /tmp -xf /tmp/tbb442.tgz - TBBROOT=/tmp/tbb44_20151115oss - # Set library paths export LIBRARY_PATH="${TBBROOT}/lib:$LIBRARY_PATH" export DYLD_LIBRARY_PATH="${TBBROOT}/lib:$DYLD_LIBRARY_PATH" fi - # CPATH="${TBBROOT}/include:$CPATH"; export CPATH + CPATH="${TBBROOT}/include:$CPATH"; export CPATH } # common tasks before either build or test diff --git a/.travis.yml b/.travis.yml index 7e5b33ec4..893627df3 100644 --- a/.travis.yml +++ b/.travis.yml @@ -17,13 +17,13 @@ addons: - libpython-dev python-numpy - libboost-all-dev -before_install: - - ./.travis.sh -tbb +# before_install: # - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update; fi install: - if [ "$TRAVIS_OS_NAME" == "osx" ]; then HOMEBREW_NO_AUTO_UPDATE=1 brew install ccache ; fi - if [ "$TRAVIS_OS_NAME" == "osx" ]; then export PATH="/usr/local/opt/ccache/libexec:$PATH" ; fi + - source .travis.sh -tbb # We first do the compile stage specified below, then the matrix expansion specified after. stages: From ba5086f2717466f413796068c53fa337ecd73531 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 Mar 2020 13:15:59 -0400 Subject: [PATCH 56/79] vastly improved install_tbb function which copies files correctly --- .travis.sh | 42 ++++++++++++++++++++++-------------------- 1 file changed, 22 insertions(+), 20 deletions(-) diff --git a/.travis.sh b/.travis.sh index 2bf4f038c..10d5fd545 100755 --- a/.travis.sh +++ b/.travis.sh @@ -3,31 +3,33 @@ # install TBB with _debug.so files function install_tbb() { - TBBROOT=/tmp/tbb44_20151115oss + TBB_BASEURL=https://github.com/oneapi-src/oneTBB/releases/download + TBB_VERSION=4.4.2 + TBB_DIR=tbb44_20151115oss + TBB_SAVEPATH="/tmp/tbb.tgz" - if [ "$(uname -s)" == "Linux" ]; then - wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_lin.tgz -O /tmp/tbb442.tgz - tar -C /tmp -xf /tmp/tbb442.tgz + if [ "$TRAVIS_OS_NAME" == "linux" ]; then + OS_SHORT="lin" + TBB_LIB_DIR="intel64/gcc4.4" + SUDO="sudo" - TBB_TARGET_ARCH="intel64" - library_directory="${TBB_TARGET_ARCH}/gcc4.1" + elif [ "$TRAVIS_OS_NAME" == "osx" ]; then + OS_SHORT="lin" + TBB_LIB_DIR="" + SUDO="" - # Set library paths - export MIC_LD_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LD_LIBRARY_PATH}" - export MIC_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LIBRARY_PATH}" - export LD_LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LD_LIBRARY_PATH}" - export LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LIBRARY_PATH}" - - elif [ "$(uname -s)" == "Darwin" ]; then - wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_osx.tgz -O /tmp/tbb442.tgz - tar -C /tmp -xf /tmp/tbb442.tgz - - # Set library paths - export LIBRARY_PATH="${TBBROOT}/lib:$LIBRARY_PATH" - export DYLD_LIBRARY_PATH="${TBBROOT}/lib:$DYLD_LIBRARY_PATH" fi - CPATH="${TBBROOT}/include:$CPATH"; export CPATH + wget "${TBB_BASEURL}/${TBB_VERSION}/${TBB_DIR}_${OS_SHORT}.tgz" -O $TBB_SAVEPATH + tar -C /tmp -xf $TBB_SAVEPATH + + TBBROOT=/tmp/$TBB_DIR + # Copy the needed files to the correct places. + # This works correctly for travis builds, instead of setting path variables. + # This is what Homebrew does to install TBB on Macs + $SUDO cp -R $TBBROOT/lib/$TBB_LIB_DIR/* /usr/local/lib/ + $SUDO cp -R $TBBROOT/include/ /usr/local/include/ + } # common tasks before either build or test From 8dc7359924176c0c575d8b1f014057ce99aaac03 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 Mar 2020 23:30:50 -0400 Subject: [PATCH 57/79] set makefile to not verbose to speed up travis and reduce log length --- .travis.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.sh b/.travis.sh index 10d5fd545..d1940db52 100755 --- a/.travis.sh +++ b/.travis.sh @@ -61,7 +61,7 @@ function configure() -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \ -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ - -DCMAKE_VERBOSE_MAKEFILE=ON + -DCMAKE_VERBOSE_MAKEFILE=OFF } From d608611c871035269dcc702bd13377050c8757b9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 28 Mar 2020 06:56:58 -0400 Subject: [PATCH 58/79] install tbb as part of configure --- .travis.sh | 5 ++--- .travis.yml | 1 - 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/.travis.sh b/.travis.sh index d1940db52..535a72f4b 100755 --- a/.travis.sh +++ b/.travis.sh @@ -46,6 +46,8 @@ function configure() rm -fr $BUILD_DIR || true mkdir $BUILD_DIR && cd $BUILD_DIR + install_tbb + if [ ! -z "$GCC_VERSION" ]; then export CC=gcc-$GCC_VERSION export CXX=g++-$GCC_VERSION @@ -109,7 +111,4 @@ case $1 in -t) test ;; - -tbb) - install_tbb - ;; esac diff --git a/.travis.yml b/.travis.yml index 893627df3..c272cff07 100644 --- a/.travis.yml +++ b/.travis.yml @@ -23,7 +23,6 @@ addons: install: - if [ "$TRAVIS_OS_NAME" == "osx" ]; then HOMEBREW_NO_AUTO_UPDATE=1 brew install ccache ; fi - if [ "$TRAVIS_OS_NAME" == "osx" ]; then export PATH="/usr/local/opt/ccache/libexec:$PATH" ; fi - - source .travis.sh -tbb # We first do the compile stage specified below, then the matrix expansion specified after. stages: From 02ff7ae276514bb3633638416fda95e7b3100fae Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 28 Mar 2020 09:56:29 -0400 Subject: [PATCH 59/79] output message for exception in debug mode --- gtsam/nonlinear/utilities.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index 1228cd4db..2044d091f 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -244,6 +244,9 @@ Values localToWorld(const Values& local, const Pose2& base, world.insert(key, base.transformFrom(point)); } catch (const std::exception& e2) { // if not Pose2 or Point2, do nothing + #ifndef NDEBUG + std::cerr << "Values[key] is neither Pose2 nor Point2, so skip" << std::endl; + #endif } } } From 35bfc8468485796e1de882c753a8777273c3682f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 28 Mar 2020 10:05:38 -0400 Subject: [PATCH 60/79] make makefile verbose again --- .travis.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.sh b/.travis.sh index 535a72f4b..9fc09a3f8 100755 --- a/.travis.sh +++ b/.travis.sh @@ -63,7 +63,7 @@ function configure() -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \ -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ - -DCMAKE_VERBOSE_MAKEFILE=OFF + -DCMAKE_VERBOSE_MAKEFILE=ON } From 4197fa3c54751a32bcaa4dabaee9955e476ea28b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 29 Mar 2020 19:11:44 -0400 Subject: [PATCH 61/79] removed graphWithPrior from all examples while keeping functionality the same --- cython/gtsam/examples/Pose2SLAMExample_g2o.py | 5 ++--- cython/gtsam/examples/Pose3SLAMExample_g2o.py | 9 ++++----- examples/Pose2SLAMExample_g2o.cpp | 5 ++--- examples/Pose2SLAMExample_lago.cpp | 12 +++++++----- examples/Pose3Localization.cpp | 12 +++++++----- examples/Pose3SLAMExample_g2o.cpp | 10 ++++++---- examples/Pose3SLAMExample_initializePose3Chordal.cpp | 10 ++++++---- .../Pose3SLAMExample_initializePose3Gradient.cpp | 10 ++++++---- 8 files changed, 40 insertions(+), 33 deletions(-) diff --git a/cython/gtsam/examples/Pose2SLAMExample_g2o.py b/cython/gtsam/examples/Pose2SLAMExample_g2o.py index 3aee7daff..83d543310 100644 --- a/cython/gtsam/examples/Pose2SLAMExample_g2o.py +++ b/cython/gtsam/examples/Pose2SLAMExample_g2o.py @@ -53,16 +53,15 @@ graph, initial = gtsam.readG2o(g2oFile, is3D) assert args.kernel == "none", "Supplied kernel type is not yet implemented" # Add prior on the pose having index (key) = 0 -graphWithPrior = graph priorModel = gtsam.noiseModel_Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8)) -graphWithPrior.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel)) +graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel)) params = gtsam.GaussNewtonParams() params.setVerbosity("Termination") params.setMaxIterations(maxIterations) # parameters.setRelativeErrorTol(1e-5) # Create the optimizer ... -optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params) +optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) # ... and optimize result = optimizer.optimize() diff --git a/cython/gtsam/examples/Pose3SLAMExample_g2o.py b/cython/gtsam/examples/Pose3SLAMExample_g2o.py index 38c5db275..25686c762 100644 --- a/cython/gtsam/examples/Pose3SLAMExample_g2o.py +++ b/cython/gtsam/examples/Pose3SLAMExample_g2o.py @@ -43,18 +43,17 @@ priorModel = gtsam.noiseModel_Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)) print("Adding prior to g2o file ") -graphWithPrior = graph firstKey = initial.keys().at(0) -graphWithPrior.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) +graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) params = gtsam.GaussNewtonParams() params.setVerbosity("Termination") # this will show info about stopping conds -optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params) +optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) result = optimizer.optimize() print("Optimization complete") -print("initial error = ", graphWithPrior.error(initial)) -print("final error = ", graphWithPrior.error(result)) +print("initial error = ", graph.error(initial)) +print("final error = ", graph.error(result)) if args.output is None: print("Final Result:\n{}".format(result)) diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index 35f9884e1..a38dfafa6 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -63,10 +63,9 @@ int main(const int argc, const char *argv[]) { } // Add prior on the pose having index (key) = 0 - NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8)); - graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); + graph->add(PriorFactor(0, Pose2(), priorModel)); std::cout << "Adding prior on pose 0 " << std::endl; GaussNewtonParams params; @@ -77,7 +76,7 @@ int main(const int argc, const char *argv[]) { } std::cout << "Optimizing the factor graph" << std::endl; - GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params); + GaussNewtonOptimizer optimizer(*graph, *initial, params); Values result = optimizer.optimize(); std::cout << "Optimization complete" << std::endl; diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index b83dfa1db..d6164450b 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -42,14 +42,13 @@ int main(const int argc, const char *argv[]) { boost::tie(graph, initial) = readG2o(g2oFile); // Add prior on the pose having index (key) = 0 - NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8)); - graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); - graphWithPrior.print(); + graph->add(PriorFactor(0, Pose2(), priorModel)); + graph->print(); std::cout << "Computing LAGO estimate" << std::endl; - Values estimateLago = lago::initialize(graphWithPrior); + Values estimateLago = lago::initialize(*graph); std::cout << "done!" << std::endl; if (argc < 3) { @@ -57,7 +56,10 @@ int main(const int argc, const char *argv[]) { } else { const string outputFile = argv[2]; std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(*graph, estimateLago, outputFile); + NonlinearFactorGraph::shared_ptr graphNoKernel; + Values::shared_ptr initial2; + boost::tie(graphNoKernel, initial2) = readG2o(g2oFile); + writeG2o(*graphNoKernel, estimateLago, outputFile); std::cout << "done! " << std::endl; } diff --git a/examples/Pose3Localization.cpp b/examples/Pose3Localization.cpp index becb9530c..05a04b353 100644 --- a/examples/Pose3Localization.cpp +++ b/examples/Pose3Localization.cpp @@ -21,8 +21,8 @@ #include #include #include -#include #include +#include using namespace std; using namespace gtsam; @@ -42,21 +42,20 @@ int main(const int argc, const char *argv[]) { boost::tie(graph, initial) = readG2o(g2oFile, is3D); // Add prior on the first key - NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; for(const Values::ConstKeyValuePair& key_value: *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; - graphWithPrior.add(PriorFactor(firstKey, Pose3(), priorModel)); + graph->add(PriorFactor(firstKey, Pose3(), priorModel)); break; } std::cout << "Optimizing the factor graph" << std::endl; GaussNewtonParams params; params.setVerbosity("TERMINATION"); // this will show info about stopping conditions - GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params); + GaussNewtonOptimizer optimizer(*graph, *initial, params); Values result = optimizer.optimize(); std::cout << "Optimization complete" << std::endl; @@ -68,7 +67,10 @@ int main(const int argc, const char *argv[]) { } else { const string outputFile = argv[2]; std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(*graph, result, outputFile); + NonlinearFactorGraph::shared_ptr graphNoKernel; + Values::shared_ptr initial2; + boost::tie(graphNoKernel, initial2) = readG2o(g2oFile); + writeG2o(*graphNoKernel, result, outputFile); std::cout << "done! " << std::endl; } diff --git a/examples/Pose3SLAMExample_g2o.cpp b/examples/Pose3SLAMExample_g2o.cpp index 5066222e5..25297806e 100644 --- a/examples/Pose3SLAMExample_g2o.cpp +++ b/examples/Pose3SLAMExample_g2o.cpp @@ -41,21 +41,20 @@ int main(const int argc, const char *argv[]) { boost::tie(graph, initial) = readG2o(g2oFile, is3D); // Add prior on the first key - NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; for(const Values::ConstKeyValuePair& key_value: *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; - graphWithPrior.add(PriorFactor(firstKey, Pose3(), priorModel)); + graph->add(PriorFactor(firstKey, Pose3(), priorModel)); break; } std::cout << "Optimizing the factor graph" << std::endl; GaussNewtonParams params; params.setVerbosity("TERMINATION"); // this will show info about stopping conditions - GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params); + GaussNewtonOptimizer optimizer(*graph, *initial, params); Values result = optimizer.optimize(); std::cout << "Optimization complete" << std::endl; @@ -67,7 +66,10 @@ int main(const int argc, const char *argv[]) { } else { const string outputFile = argv[2]; std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(*graph, result, outputFile); + NonlinearFactorGraph::shared_ptr graphNoKernel; + Values::shared_ptr initial2; + boost::tie(graphNoKernel, initial2) = readG2o(g2oFile); + writeG2o(*graphNoKernel, result, outputFile); std::cout << "done! " << std::endl; } return 0; diff --git a/examples/Pose3SLAMExample_initializePose3Chordal.cpp b/examples/Pose3SLAMExample_initializePose3Chordal.cpp index e90d014c0..9726f467c 100644 --- a/examples/Pose3SLAMExample_initializePose3Chordal.cpp +++ b/examples/Pose3SLAMExample_initializePose3Chordal.cpp @@ -41,19 +41,18 @@ int main(const int argc, const char *argv[]) { boost::tie(graph, initial) = readG2o(g2oFile, is3D); // Add prior on the first key - NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; for(const Values::ConstKeyValuePair& key_value: *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; - graphWithPrior.add(PriorFactor(firstKey, Pose3(), priorModel)); + graph->add(PriorFactor(firstKey, Pose3(), priorModel)); break; } std::cout << "Initializing Pose3 - chordal relaxation" << std::endl; - Values initialization = InitializePose3::initialize(graphWithPrior); + Values initialization = InitializePose3::initialize(*graph); std::cout << "done!" << std::endl; if (argc < 3) { @@ -61,7 +60,10 @@ int main(const int argc, const char *argv[]) { } else { const string outputFile = argv[2]; std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(*graph, initialization, outputFile); + NonlinearFactorGraph::shared_ptr graphNoKernel; + Values::shared_ptr initial2; + boost::tie(graphNoKernel, initial2) = readG2o(g2oFile); + writeG2o(*graphNoKernel, initialization, outputFile); std::cout << "done! " << std::endl; } return 0; diff --git a/examples/Pose3SLAMExample_initializePose3Gradient.cpp b/examples/Pose3SLAMExample_initializePose3Gradient.cpp index 10960cf31..000150846 100644 --- a/examples/Pose3SLAMExample_initializePose3Gradient.cpp +++ b/examples/Pose3SLAMExample_initializePose3Gradient.cpp @@ -41,20 +41,19 @@ int main(const int argc, const char *argv[]) { boost::tie(graph, initial) = readG2o(g2oFile, is3D); // Add prior on the first key - NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; for(const Values::ConstKeyValuePair& key_value: *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; - graphWithPrior.add(PriorFactor(firstKey, Pose3(), priorModel)); + graph->add(PriorFactor(firstKey, Pose3(), priorModel)); break; } std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl; bool useGradient = true; - Values initialization = InitializePose3::initialize(graphWithPrior, *initial, useGradient); + Values initialization = InitializePose3::initialize(*graph, *initial, useGradient); std::cout << "done!" << std::endl; std::cout << "initial error=" <error(*initial)<< std::endl; @@ -65,7 +64,10 @@ int main(const int argc, const char *argv[]) { } else { const string outputFile = argv[2]; std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(*graph, initialization, outputFile); + NonlinearFactorGraph::shared_ptr graphNoKernel; + Values::shared_ptr initial2; + boost::tie(graphNoKernel, initial2) = readG2o(g2oFile); + writeG2o(*graphNoKernel, initialization, outputFile); std::cout << "done! " << std::endl; } return 0; From 7fcacf90c69b73076e2e67e286e03febbd07f8d6 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 31 Mar 2020 22:22:12 -0400 Subject: [PATCH 62/79] Fix unittest when GTSAM_DEFINE_POINTS_AS_VECTORS is defined --- gtsam/geometry/tests/testPose3.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 8433bbb01..ec0450abb 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -1008,7 +1008,14 @@ TEST(Pose3, print) { // Generate the expected output std::stringstream expected; Point3 translation(1, 2, 3); + +#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS + expected << "1\n" + "2\n" + "3;\n"; +#else expected << '[' << translation.x() << ", " << translation.y() << ", " << translation.z() << "]\';"; +#endif // reset cout to the original stream std::cout.rdbuf(oldbuf); From 8c2bd979401df08ed1a5944a6573ad05b9e5472c Mon Sep 17 00:00:00 2001 From: yetongumich Date: Wed, 1 Apr 2020 15:45:04 -0400 Subject: [PATCH 63/79] fix comments --- gtsam/linear/GaussianBayesNet.h | 2 +- gtsam/linear/GaussianBayesTree.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 9da7a1609..3f6d69e91 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -143,7 +143,7 @@ namespace gtsam { * allocateVectorValues */ VectorValues gradientAtZero() const; - /** Mahalanobis norm error. */ + /** 0.5 * sum of squared Mahalanobis distances. */ double error(const VectorValues& x) const; /** diff --git a/gtsam/linear/GaussianBayesTree.h b/gtsam/linear/GaussianBayesTree.h index fcc73f80e..b6f5acd52 100644 --- a/gtsam/linear/GaussianBayesTree.h +++ b/gtsam/linear/GaussianBayesTree.h @@ -106,7 +106,7 @@ namespace gtsam { * @return A VectorValues storing the gradient. */ VectorValues gradientAtZero() const; - /** Mahalanobis norm error. */ + /** 0.5 * sum of squared Mahalanobis distances. */ double error(const VectorValues& x) const; /** Computes the determinant of a GassianBayesTree, as if the Bayes tree is reorganized into a From fbd5aef61a261879ea1dea3abeb8a246ede3ee13 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Wed, 1 Apr 2020 15:45:58 -0400 Subject: [PATCH 64/79] deprecated Mahalanobis, add better named functions --- gtsam/linear/NoiseModel.cpp | 4 ++-- gtsam/linear/NoiseModel.h | 19 +++++++++++++++---- gtsam/linear/tests/testNoiseModel.cpp | 4 ++-- 3 files changed, 19 insertions(+), 8 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index a6ebea394..33f51e1f0 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -157,7 +157,7 @@ Vector Gaussian::unwhiten(const Vector& v) const { } /* ************************************************************************* */ -double Gaussian::Mahalanobis(const Vector& v) const { +double Gaussian::SquaredMahalanobisDistance(const Vector& v) const { // Note: for Diagonal, which does ediv_, will be correct for constraints Vector w = whiten(v); return w.dot(w); @@ -573,7 +573,7 @@ void Isotropic::print(const string& name) const { } /* ************************************************************************* */ -double Isotropic::Mahalanobis(const Vector& v) const { +double Isotropic::SquaredMahalanobisDistance(const Vector& v) const { return v.dot(v) * invsigma_ * invsigma_; } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 6c42fc7ba..b08207566 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -207,12 +207,23 @@ namespace gtsam { virtual Vector unwhiten(const Vector& v) const; /** - * Mahalanobis distance v'*R'*R*v = + * Squared Mahalanobis distance v'*R'*R*v = */ + virtual double SquaredMahalanobisDistance(const Vector& v) const; + + /** + * Mahalanobis distance + */ + virtual double MahalanobisDistance(const Vector& v) const { + return std::sqrt(SquaredMahalanobisDistance(v)); + } + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 virtual double Mahalanobis(const Vector& v) const; +#endif inline virtual double distance(const Vector& v) const { - return Mahalanobis(v); + return SquaredMahalanobisDistance(v); } /** @@ -564,7 +575,7 @@ namespace gtsam { } virtual void print(const std::string& name) const; - virtual double Mahalanobis(const Vector& v) const; + virtual double SquaredMahalanobisDistance(const Vector& v) const; virtual Vector whiten(const Vector& v) const; virtual Vector unwhiten(const Vector& v) const; virtual Matrix Whiten(const Matrix& H) const; @@ -616,7 +627,7 @@ namespace gtsam { virtual bool isUnit() const { return true; } virtual void print(const std::string& name) const; - virtual double Mahalanobis(const Vector& v) const {return v.dot(v); } + virtual double SquaredMahalanobisDistance(const Vector& v) const {return v.dot(v); } virtual Vector whiten(const Vector& v) const { return v; } virtual Vector unwhiten(const Vector& v) const { return v; } virtual Matrix Whiten(const Matrix& H) const { return H; } diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 10578627f..523a38b2b 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -68,10 +68,10 @@ TEST(NoiseModel, constructors) for(Gaussian::shared_ptr mi: m) EXPECT(assert_equal(unwhitened,mi->unwhiten(whitened))); - // test Mahalanobis distance + // test squared Mahalanobis distance double distance = 5*5+10*10+15*15; for(Gaussian::shared_ptr mi: m) - DOUBLES_EQUAL(distance,mi->Mahalanobis(unwhitened),1e-9); + DOUBLES_EQUAL(distance,mi->SquaredMahalanobisDistance(unwhitened),1e-9); // test R matrix for(Gaussian::shared_ptr mi: m) From 351c6f8bccca726e3515721cde22626750fc4011 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Wed, 1 Apr 2020 16:45:37 -0400 Subject: [PATCH 65/79] add implementation for deprecated Mahalanobis --- gtsam/linear/NoiseModel.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index b08207566..c4128909b 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -219,7 +219,9 @@ namespace gtsam { } #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 - virtual double Mahalanobis(const Vector& v) const; + virtual double Mahalanobis(const Vector& v) const { + return SquaredMahalanobisDistance(v); + } #endif inline virtual double distance(const Vector& v) const { From 83bd42c72fe022a74c9301eac2b2b4a5e40782df Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Apr 2020 16:52:57 -0400 Subject: [PATCH 66/79] maintain backwards compatibility of CMake --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 86234afa8..7a49a8aa6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -200,7 +200,7 @@ find_package(TBB 4.4 COMPONENTS tbb tbbmalloc) # Set up variables if we're using TBB if(TBB_FOUND AND GTSAM_WITH_TBB) set(GTSAM_USE_TBB 1) # This will go into config.h - if (${TBB_VERSION_MAJOR} GREATER_EQUAL 2020) + if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020)) set(TBB_GREATER_EQUAL_2020 1) else() set(TBB_GREATER_EQUAL_2020 0) From 4e3542a74310d098b4e3f4eba104db3d3a47d9e5 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Wed, 1 Apr 2020 18:55:23 -0400 Subject: [PATCH 67/79] renamed residual to loss, follow PR #188 --- gtsam.h | 18 +++--- gtsam/linear/LossFunctions.cpp | 16 ++--- gtsam/linear/LossFunctions.h | 33 +++++----- gtsam/linear/NoiseModel.h | 6 +- gtsam/linear/tests/testNoiseModel.cpp | 64 ++++++++++---------- matlab/gtsam_examples/VisualizeMEstimators.m | 2 +- 6 files changed, 71 insertions(+), 68 deletions(-) diff --git a/gtsam.h b/gtsam.h index 8ee778f4c..1094d9dd9 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1458,7 +1458,7 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Fair: gtsam::noiseModel::mEstimator::Base { @@ -1469,7 +1469,7 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Huber: gtsam::noiseModel::mEstimator::Base { @@ -1480,7 +1480,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { @@ -1491,7 +1491,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Tukey: gtsam::noiseModel::mEstimator::Base { @@ -1502,7 +1502,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Welsch: gtsam::noiseModel::mEstimator::Base { @@ -1513,7 +1513,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { @@ -1524,7 +1524,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class DCS: gtsam::noiseModel::mEstimator::Base { @@ -1535,7 +1535,7 @@ virtual class DCS: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { @@ -1546,7 +1546,7 @@ virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; }///\namespace mEstimator diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp index 6bc737e2c..8bb670a92 100644 --- a/gtsam/linear/LossFunctions.cpp +++ b/gtsam/linear/LossFunctions.cpp @@ -141,7 +141,7 @@ double Fair::weight(double error) const { return 1.0 / (1.0 + std::abs(error) / c_); } -double Fair::residual(double error) const { +double Fair::loss(double error) const { const double absError = std::abs(error); const double normalizedError = absError / c_; const double c_2 = c_ * c_; @@ -175,7 +175,7 @@ double Huber::weight(double error) const { return (absError <= k_) ? (1.0) : (k_ / absError); } -double Huber::residual(double error) const { +double Huber::loss(double error) const { const double absError = std::abs(error); if (absError <= k_) { // |x| <= k return error*error / 2; @@ -212,7 +212,7 @@ double Cauchy::weight(double error) const { return ksquared_ / (ksquared_ + error*error); } -double Cauchy::residual(double error) const { +double Cauchy::loss(double error) const { const double val = std::log1p(error * error / ksquared_); return ksquared_ * val * 0.5; } @@ -249,7 +249,7 @@ double Tukey::weight(double error) const { return 0.0; } -double Tukey::residual(double error) const { +double Tukey::loss(double error) const { double absError = std::abs(error); if (absError <= c_) { const double one_minus_xc2 = 1.0 - error*error/csquared_; @@ -285,7 +285,7 @@ double Welsch::weight(double error) const { return std::exp(-xc2); } -double Welsch::residual(double error) const { +double Welsch::loss(double error) const { const double xc2 = (error*error)/csquared_; return csquared_ * 0.5 * -std::expm1(-xc2); } @@ -318,7 +318,7 @@ double GemanMcClure::weight(double error) const { return c4/(c2error*c2error); } -double GemanMcClure::residual(double error) const { +double GemanMcClure::loss(double error) const { const double c2 = c_*c_; const double error2 = error*error; return 0.5 * (c2 * error2) / (c2 + error2); @@ -356,7 +356,7 @@ double DCS::weight(double error) const { return 1.0; } -double DCS::residual(double error) const { +double DCS::loss(double error) const { // This is the simplified version of Eq 9 from (Agarwal13icra) // after you simplify and cancel terms. const double e2 = error*error; @@ -400,7 +400,7 @@ double L2WithDeadZone::weight(double error) const { else return (k_+error)/error; } -double L2WithDeadZone::residual(double error) const { +double L2WithDeadZone::loss(double error) const { const double abs_error = std::abs(error); return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); } diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h index 1f7cc1377..1b6f444e8 100644 --- a/gtsam/linear/LossFunctions.h +++ b/gtsam/linear/LossFunctions.h @@ -36,12 +36,12 @@ namespace noiseModel { * The mEstimator name space contains all robust error functions. * It mirrors the exposition at * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf - * which talks about minimizing \sum \rho(r_i), where \rho is a residual function of choice. + * which talks about minimizing \sum \rho(r_i), where \rho is a loss function of choice. * * To illustrate, let's consider the least-squares (L2), L1, and Huber estimators as examples: * * Name Symbol Least-Squares L1-norm Huber - * Residual \rho(x) 0.5*x^2 |x| 0.5*x^2 if |x| shared_ptr; @@ -131,7 +134,7 @@ class GTSAM_EXPORT Null : public Base { Null(const ReweightScheme reweight = Block) : Base(reweight) {} ~Null() {} double weight(double /*error*/) const { return 1.0; } - double residual(double error) const { return error; } + double loss(double error) const { return 0.5 * error * error; } void print(const std::string &s) const; bool equals(const Base & /*expected*/, double /*tol*/) const { return true; } static shared_ptr Create(); @@ -155,7 +158,7 @@ class GTSAM_EXPORT Fair : public Base { Fair(double c = 1.3998, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double c, const ReweightScheme reweight = Block); @@ -180,7 +183,7 @@ class GTSAM_EXPORT Huber : public Base { Huber(double k = 1.345, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -210,7 +213,7 @@ class GTSAM_EXPORT Cauchy : public Base { Cauchy(double k = 0.1, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -235,7 +238,7 @@ class GTSAM_EXPORT Tukey : public Base { Tukey(double c = 4.6851, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -260,7 +263,7 @@ class GTSAM_EXPORT Welsch : public Base { Welsch(double c = 2.9846, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -296,7 +299,7 @@ class GTSAM_EXPORT GemanMcClure : public Base { GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); ~GemanMcClure() {} double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -326,7 +329,7 @@ class GTSAM_EXPORT DCS : public Base { DCS(double c = 1.0, const ReweightScheme reweight = Block); ~DCS() {} double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -359,7 +362,7 @@ class GTSAM_EXPORT L2WithDeadZone : public Base { L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index c4128909b..a4d15c0f8 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -705,11 +705,11 @@ namespace gtsam { { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; } inline virtual Vector unwhiten(const Vector& /*v*/) const { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); } - // Fold the use of the m-estimator residual(...) function into distance(...) + // Fold the use of the m-estimator loss(...) function into distance(...) inline virtual double distance(const Vector& v) const - { return robust_->residual(this->unweightedWhiten(v).norm()); } + { return robust_->loss(this->unweightedWhiten(v).norm()); } inline virtual double distance_non_whitened(const Vector& v) const - { return robust_->residual(v.norm()); } + { return robust_->loss(v.norm()); } // TODO: these are really robust iterated re-weighting support functions virtual void WhitenSystem(Vector& b) const; virtual void WhitenSystem(std::vector& A, Vector& b) const; diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 523a38b2b..0290fc5d8 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -467,10 +467,10 @@ TEST(NoiseModel, robustFunctionFair) DOUBLES_EQUAL(0.3333333333333333, fair->weight(error3), 1e-8); DOUBLES_EQUAL(0.8333333333333333, fair->weight(error4), 1e-8); - DOUBLES_EQUAL(0.441961080151135, fair->residual(error1), 1e-8); - DOUBLES_EQUAL(22.534692783297260, fair->residual(error2), 1e-8); - DOUBLES_EQUAL(22.534692783297260, fair->residual(error3), 1e-8); - DOUBLES_EQUAL(0.441961080151135, fair->residual(error4), 1e-8); + DOUBLES_EQUAL(0.441961080151135, fair->loss(error1), 1e-8); + DOUBLES_EQUAL(22.534692783297260, fair->loss(error2), 1e-8); + DOUBLES_EQUAL(22.534692783297260, fair->loss(error3), 1e-8); + DOUBLES_EQUAL(0.441961080151135, fair->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionHuber) @@ -483,10 +483,10 @@ TEST(NoiseModel, robustFunctionHuber) DOUBLES_EQUAL(0.5, huber->weight(error3), 1e-8); DOUBLES_EQUAL(1.0, huber->weight(error4), 1e-8); - DOUBLES_EQUAL(0.5000, huber->residual(error1), 1e-8); - DOUBLES_EQUAL(37.5000, huber->residual(error2), 1e-8); - DOUBLES_EQUAL(37.5000, huber->residual(error3), 1e-8); - DOUBLES_EQUAL(0.5000, huber->residual(error4), 1e-8); + DOUBLES_EQUAL(0.5000, huber->loss(error1), 1e-8); + DOUBLES_EQUAL(37.5000, huber->loss(error2), 1e-8); + DOUBLES_EQUAL(37.5000, huber->loss(error3), 1e-8); + DOUBLES_EQUAL(0.5000, huber->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionCauchy) @@ -499,10 +499,10 @@ TEST(NoiseModel, robustFunctionCauchy) DOUBLES_EQUAL(0.2000, cauchy->weight(error3), 1e-8); DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error4), 1e-8); - DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error1), 1e-8); - DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error2), 1e-8); - DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error3), 1e-8); - DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error4), 1e-8); + DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error1), 1e-8); + DOUBLES_EQUAL(20.117973905426254, cauchy->loss(error2), 1e-8); + DOUBLES_EQUAL(20.117973905426254, cauchy->loss(error3), 1e-8); + DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionGemanMcClure) @@ -514,10 +514,10 @@ TEST(NoiseModel, robustFunctionGemanMcClure) DOUBLES_EQUAL(9.80296e-5, gmc->weight(error3), 1e-8); DOUBLES_EQUAL(0.25 , gmc->weight(error4), 1e-8); - DOUBLES_EQUAL(0.2500, gmc->residual(error1), 1e-8); - DOUBLES_EQUAL(0.495049504950495, gmc->residual(error2), 1e-8); - DOUBLES_EQUAL(0.495049504950495, gmc->residual(error3), 1e-8); - DOUBLES_EQUAL(0.2500, gmc->residual(error4), 1e-8); + DOUBLES_EQUAL(0.2500, gmc->loss(error1), 1e-8); + DOUBLES_EQUAL(0.495049504950495, gmc->loss(error2), 1e-8); + DOUBLES_EQUAL(0.495049504950495, gmc->loss(error3), 1e-8); + DOUBLES_EQUAL(0.2500, gmc->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionWelsch) @@ -530,10 +530,10 @@ TEST(NoiseModel, robustFunctionWelsch) DOUBLES_EQUAL(0.018315638888734, welsch->weight(error3), 1e-8); DOUBLES_EQUAL(0.960789439152323, welsch->weight(error4), 1e-8); - DOUBLES_EQUAL(0.490132010595960, welsch->residual(error1), 1e-8); - DOUBLES_EQUAL(12.271054513890823, welsch->residual(error2), 1e-8); - DOUBLES_EQUAL(12.271054513890823, welsch->residual(error3), 1e-8); - DOUBLES_EQUAL(0.490132010595960, welsch->residual(error4), 1e-8); + DOUBLES_EQUAL(0.490132010595960, welsch->loss(error1), 1e-8); + DOUBLES_EQUAL(12.271054513890823, welsch->loss(error2), 1e-8); + DOUBLES_EQUAL(12.271054513890823, welsch->loss(error3), 1e-8); + DOUBLES_EQUAL(0.490132010595960, welsch->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionTukey) @@ -546,10 +546,10 @@ TEST(NoiseModel, robustFunctionTukey) DOUBLES_EQUAL(0.0, tukey->weight(error3), 1e-8); DOUBLES_EQUAL(0.9216, tukey->weight(error4), 1e-8); - DOUBLES_EQUAL(0.480266666666667, tukey->residual(error1), 1e-8); - DOUBLES_EQUAL(4.166666666666667, tukey->residual(error2), 1e-8); - DOUBLES_EQUAL(4.166666666666667, tukey->residual(error3), 1e-8); - DOUBLES_EQUAL(0.480266666666667, tukey->residual(error4), 1e-8); + DOUBLES_EQUAL(0.480266666666667, tukey->loss(error1), 1e-8); + DOUBLES_EQUAL(4.166666666666667, tukey->loss(error2), 1e-8); + DOUBLES_EQUAL(4.166666666666667, tukey->loss(error3), 1e-8); + DOUBLES_EQUAL(0.480266666666667, tukey->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionDCS) @@ -560,8 +560,8 @@ TEST(NoiseModel, robustFunctionDCS) DOUBLES_EQUAL(1.0 , dcs->weight(error1), 1e-8); DOUBLES_EQUAL(0.00039211, dcs->weight(error2), 1e-8); - DOUBLES_EQUAL(0.5 , dcs->residual(error1), 1e-8); - DOUBLES_EQUAL(0.9900990099, dcs->residual(error2), 1e-8); + DOUBLES_EQUAL(0.5 , dcs->loss(error1), 1e-8); + DOUBLES_EQUAL(0.9900990099, dcs->loss(error2), 1e-8); } TEST(NoiseModel, robustFunctionL2WithDeadZone) @@ -576,12 +576,12 @@ TEST(NoiseModel, robustFunctionL2WithDeadZone) DOUBLES_EQUAL(0.00990099009, lsdz->weight(e4), 1e-8); DOUBLES_EQUAL(0.9, lsdz->weight(e5), 1e-8); - DOUBLES_EQUAL(40.5, lsdz->residual(e0), 1e-8); - DOUBLES_EQUAL(0.00005, lsdz->residual(e1), 1e-8); - DOUBLES_EQUAL(0.0, lsdz->residual(e2), 1e-8); - DOUBLES_EQUAL(0.0, lsdz->residual(e3), 1e-8); - DOUBLES_EQUAL(0.00005, lsdz->residual(e4), 1e-8); - DOUBLES_EQUAL(40.5, lsdz->residual(e5), 1e-8); + DOUBLES_EQUAL(40.5, lsdz->loss(e0), 1e-8); + DOUBLES_EQUAL(0.00005, lsdz->loss(e1), 1e-8); + DOUBLES_EQUAL(0.0, lsdz->loss(e2), 1e-8); + DOUBLES_EQUAL(0.0, lsdz->loss(e3), 1e-8); + DOUBLES_EQUAL(0.00005, lsdz->loss(e4), 1e-8); + DOUBLES_EQUAL(40.5, lsdz->loss(e5), 1e-8); } /* ************************************************************************* */ diff --git a/matlab/gtsam_examples/VisualizeMEstimators.m b/matlab/gtsam_examples/VisualizeMEstimators.m index 8a0485334..ce505df5d 100644 --- a/matlab/gtsam_examples/VisualizeMEstimators.m +++ b/matlab/gtsam_examples/VisualizeMEstimators.m @@ -48,7 +48,7 @@ function plot_m_estimator(x, model, plot_title, fig_id, filename) rho = zeros(size(x)); for i = 1:size(x, 2) w(i) = model.weight(x(i)); - rho(i) = model.residual(x(i)); + rho(i) = model.loss(x(i)); end psi = w .* x; From 4c6feb4769b8dc2e55cfe39904baffc138ea8462 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Thu, 2 Apr 2020 11:04:31 -0400 Subject: [PATCH 68/79] Revert "add implementation for deprecated Mahalanobis" This reverts commit 351c6f8bccca726e3515721cde22626750fc4011. --- gtsam/linear/NoiseModel.h | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index a4d15c0f8..73f663240 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -219,9 +219,7 @@ namespace gtsam { } #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 - virtual double Mahalanobis(const Vector& v) const { - return SquaredMahalanobisDistance(v); - } + virtual double Mahalanobis(const Vector& v) const; #endif inline virtual double distance(const Vector& v) const { From 0f713ec077c0e51cca96efa4500614f67463fc8a Mon Sep 17 00:00:00 2001 From: yetongumich Date: Thu, 2 Apr 2020 11:06:31 -0400 Subject: [PATCH 69/79] renamed squaredMahalanobisDistance --- gtsam/linear/NoiseModel.cpp | 4 ++-- gtsam/linear/NoiseModel.h | 10 +++++----- gtsam/linear/tests/testNoiseModel.cpp | 2 +- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 33f51e1f0..b324ea784 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -157,7 +157,7 @@ Vector Gaussian::unwhiten(const Vector& v) const { } /* ************************************************************************* */ -double Gaussian::SquaredMahalanobisDistance(const Vector& v) const { +double Gaussian::squaredMahalanobisDistance(const Vector& v) const { // Note: for Diagonal, which does ediv_, will be correct for constraints Vector w = whiten(v); return w.dot(w); @@ -573,7 +573,7 @@ void Isotropic::print(const string& name) const { } /* ************************************************************************* */ -double Isotropic::SquaredMahalanobisDistance(const Vector& v) const { +double Isotropic::squaredMahalanobisDistance(const Vector& v) const { return v.dot(v) * invsigma_ * invsigma_; } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 73f663240..bdb25bec0 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -209,13 +209,13 @@ namespace gtsam { /** * Squared Mahalanobis distance v'*R'*R*v = */ - virtual double SquaredMahalanobisDistance(const Vector& v) const; + virtual double squaredMahalanobisDistance(const Vector& v) const; /** * Mahalanobis distance */ virtual double MahalanobisDistance(const Vector& v) const { - return std::sqrt(SquaredMahalanobisDistance(v)); + return std::sqrt(squaredMahalanobisDistance(v)); } #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 @@ -223,7 +223,7 @@ namespace gtsam { #endif inline virtual double distance(const Vector& v) const { - return SquaredMahalanobisDistance(v); + return squaredMahalanobisDistance(v); } /** @@ -575,7 +575,7 @@ namespace gtsam { } virtual void print(const std::string& name) const; - virtual double SquaredMahalanobisDistance(const Vector& v) const; + virtual double squaredMahalanobisDistance(const Vector& v) const; virtual Vector whiten(const Vector& v) const; virtual Vector unwhiten(const Vector& v) const; virtual Matrix Whiten(const Matrix& H) const; @@ -627,7 +627,7 @@ namespace gtsam { virtual bool isUnit() const { return true; } virtual void print(const std::string& name) const; - virtual double SquaredMahalanobisDistance(const Vector& v) const {return v.dot(v); } + virtual double squaredMahalanobisDistance(const Vector& v) const {return v.dot(v); } virtual Vector whiten(const Vector& v) const { return v; } virtual Vector unwhiten(const Vector& v) const { return v; } virtual Matrix Whiten(const Matrix& H) const { return H; } diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 0290fc5d8..2322d8432 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -71,7 +71,7 @@ TEST(NoiseModel, constructors) // test squared Mahalanobis distance double distance = 5*5+10*10+15*15; for(Gaussian::shared_ptr mi: m) - DOUBLES_EQUAL(distance,mi->SquaredMahalanobisDistance(unwhitened),1e-9); + DOUBLES_EQUAL(distance,mi->squaredMahalanobisDistance(unwhitened),1e-9); // test R matrix for(Gaussian::shared_ptr mi: m) From 17e8f21e75590c1c6c7120e02e138d8da3b887a0 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Thu, 2 Apr 2020 11:08:58 -0400 Subject: [PATCH 70/79] renamed mahalanobisDistance --- gtsam/linear/NoiseModel.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index bdb25bec0..225497b42 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -214,7 +214,7 @@ namespace gtsam { /** * Mahalanobis distance */ - virtual double MahalanobisDistance(const Vector& v) const { + virtual double mahalanobisDistance(const Vector& v) const { return std::sqrt(squaredMahalanobisDistance(v)); } From 0acca9f8da665d40e3df4cf6c45c6e6734a10a31 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Thu, 2 Apr 2020 11:24:26 -0400 Subject: [PATCH 71/79] Revert "renamed residual to loss, follow PR #188" This reverts commit 4e3542a74310d098b4e3f4eba104db3d3a47d9e5. --- gtsam.h | 18 +++--- gtsam/linear/LossFunctions.cpp | 16 ++--- gtsam/linear/LossFunctions.h | 33 +++++----- gtsam/linear/NoiseModel.h | 6 +- gtsam/linear/tests/testNoiseModel.cpp | 64 ++++++++++---------- matlab/gtsam_examples/VisualizeMEstimators.m | 2 +- 6 files changed, 68 insertions(+), 71 deletions(-) diff --git a/gtsam.h b/gtsam.h index 1094d9dd9..8ee778f4c 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1458,7 +1458,7 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class Fair: gtsam::noiseModel::mEstimator::Base { @@ -1469,7 +1469,7 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class Huber: gtsam::noiseModel::mEstimator::Base { @@ -1480,7 +1480,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { @@ -1491,7 +1491,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class Tukey: gtsam::noiseModel::mEstimator::Base { @@ -1502,7 +1502,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class Welsch: gtsam::noiseModel::mEstimator::Base { @@ -1513,7 +1513,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { @@ -1524,7 +1524,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class DCS: gtsam::noiseModel::mEstimator::Base { @@ -1535,7 +1535,7 @@ virtual class DCS: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { @@ -1546,7 +1546,7 @@ virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; }///\namespace mEstimator diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp index 8bb670a92..6bc737e2c 100644 --- a/gtsam/linear/LossFunctions.cpp +++ b/gtsam/linear/LossFunctions.cpp @@ -141,7 +141,7 @@ double Fair::weight(double error) const { return 1.0 / (1.0 + std::abs(error) / c_); } -double Fair::loss(double error) const { +double Fair::residual(double error) const { const double absError = std::abs(error); const double normalizedError = absError / c_; const double c_2 = c_ * c_; @@ -175,7 +175,7 @@ double Huber::weight(double error) const { return (absError <= k_) ? (1.0) : (k_ / absError); } -double Huber::loss(double error) const { +double Huber::residual(double error) const { const double absError = std::abs(error); if (absError <= k_) { // |x| <= k return error*error / 2; @@ -212,7 +212,7 @@ double Cauchy::weight(double error) const { return ksquared_ / (ksquared_ + error*error); } -double Cauchy::loss(double error) const { +double Cauchy::residual(double error) const { const double val = std::log1p(error * error / ksquared_); return ksquared_ * val * 0.5; } @@ -249,7 +249,7 @@ double Tukey::weight(double error) const { return 0.0; } -double Tukey::loss(double error) const { +double Tukey::residual(double error) const { double absError = std::abs(error); if (absError <= c_) { const double one_minus_xc2 = 1.0 - error*error/csquared_; @@ -285,7 +285,7 @@ double Welsch::weight(double error) const { return std::exp(-xc2); } -double Welsch::loss(double error) const { +double Welsch::residual(double error) const { const double xc2 = (error*error)/csquared_; return csquared_ * 0.5 * -std::expm1(-xc2); } @@ -318,7 +318,7 @@ double GemanMcClure::weight(double error) const { return c4/(c2error*c2error); } -double GemanMcClure::loss(double error) const { +double GemanMcClure::residual(double error) const { const double c2 = c_*c_; const double error2 = error*error; return 0.5 * (c2 * error2) / (c2 + error2); @@ -356,7 +356,7 @@ double DCS::weight(double error) const { return 1.0; } -double DCS::loss(double error) const { +double DCS::residual(double error) const { // This is the simplified version of Eq 9 from (Agarwal13icra) // after you simplify and cancel terms. const double e2 = error*error; @@ -400,7 +400,7 @@ double L2WithDeadZone::weight(double error) const { else return (k_+error)/error; } -double L2WithDeadZone::loss(double error) const { +double L2WithDeadZone::residual(double error) const { const double abs_error = std::abs(error); return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); } diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h index 1b6f444e8..1f7cc1377 100644 --- a/gtsam/linear/LossFunctions.h +++ b/gtsam/linear/LossFunctions.h @@ -36,12 +36,12 @@ namespace noiseModel { * The mEstimator name space contains all robust error functions. * It mirrors the exposition at * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf - * which talks about minimizing \sum \rho(r_i), where \rho is a loss function of choice. + * which talks about minimizing \sum \rho(r_i), where \rho is a residual function of choice. * * To illustrate, let's consider the least-squares (L2), L1, and Huber estimators as examples: * * Name Symbol Least-Squares L1-norm Huber - * Loss \rho(x) 0.5*x^2 |x| 0.5*x^2 if |x| shared_ptr; @@ -134,7 +131,7 @@ class GTSAM_EXPORT Null : public Base { Null(const ReweightScheme reweight = Block) : Base(reweight) {} ~Null() {} double weight(double /*error*/) const { return 1.0; } - double loss(double error) const { return 0.5 * error * error; } + double residual(double error) const { return error; } void print(const std::string &s) const; bool equals(const Base & /*expected*/, double /*tol*/) const { return true; } static shared_ptr Create(); @@ -158,7 +155,7 @@ class GTSAM_EXPORT Fair : public Base { Fair(double c = 1.3998, const ReweightScheme reweight = Block); double weight(double error) const override; - double loss(double error) const override; + double residual(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double c, const ReweightScheme reweight = Block); @@ -183,7 +180,7 @@ class GTSAM_EXPORT Huber : public Base { Huber(double k = 1.345, const ReweightScheme reweight = Block); double weight(double error) const override; - double loss(double error) const override; + double residual(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -213,7 +210,7 @@ class GTSAM_EXPORT Cauchy : public Base { Cauchy(double k = 0.1, const ReweightScheme reweight = Block); double weight(double error) const override; - double loss(double error) const override; + double residual(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -238,7 +235,7 @@ class GTSAM_EXPORT Tukey : public Base { Tukey(double c = 4.6851, const ReweightScheme reweight = Block); double weight(double error) const override; - double loss(double error) const override; + double residual(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -263,7 +260,7 @@ class GTSAM_EXPORT Welsch : public Base { Welsch(double c = 2.9846, const ReweightScheme reweight = Block); double weight(double error) const override; - double loss(double error) const override; + double residual(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -299,7 +296,7 @@ class GTSAM_EXPORT GemanMcClure : public Base { GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); ~GemanMcClure() {} double weight(double error) const override; - double loss(double error) const override; + double residual(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -329,7 +326,7 @@ class GTSAM_EXPORT DCS : public Base { DCS(double c = 1.0, const ReweightScheme reweight = Block); ~DCS() {} double weight(double error) const override; - double loss(double error) const override; + double residual(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -362,7 +359,7 @@ class GTSAM_EXPORT L2WithDeadZone : public Base { L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block); double weight(double error) const override; - double loss(double error) const override; + double residual(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 225497b42..7c3cb1294 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -703,11 +703,11 @@ namespace gtsam { { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; } inline virtual Vector unwhiten(const Vector& /*v*/) const { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); } - // Fold the use of the m-estimator loss(...) function into distance(...) + // Fold the use of the m-estimator residual(...) function into distance(...) inline virtual double distance(const Vector& v) const - { return robust_->loss(this->unweightedWhiten(v).norm()); } + { return robust_->residual(this->unweightedWhiten(v).norm()); } inline virtual double distance_non_whitened(const Vector& v) const - { return robust_->loss(v.norm()); } + { return robust_->residual(v.norm()); } // TODO: these are really robust iterated re-weighting support functions virtual void WhitenSystem(Vector& b) const; virtual void WhitenSystem(std::vector& A, Vector& b) const; diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 2322d8432..3f6550b9f 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -467,10 +467,10 @@ TEST(NoiseModel, robustFunctionFair) DOUBLES_EQUAL(0.3333333333333333, fair->weight(error3), 1e-8); DOUBLES_EQUAL(0.8333333333333333, fair->weight(error4), 1e-8); - DOUBLES_EQUAL(0.441961080151135, fair->loss(error1), 1e-8); - DOUBLES_EQUAL(22.534692783297260, fair->loss(error2), 1e-8); - DOUBLES_EQUAL(22.534692783297260, fair->loss(error3), 1e-8); - DOUBLES_EQUAL(0.441961080151135, fair->loss(error4), 1e-8); + DOUBLES_EQUAL(0.441961080151135, fair->residual(error1), 1e-8); + DOUBLES_EQUAL(22.534692783297260, fair->residual(error2), 1e-8); + DOUBLES_EQUAL(22.534692783297260, fair->residual(error3), 1e-8); + DOUBLES_EQUAL(0.441961080151135, fair->residual(error4), 1e-8); } TEST(NoiseModel, robustFunctionHuber) @@ -483,10 +483,10 @@ TEST(NoiseModel, robustFunctionHuber) DOUBLES_EQUAL(0.5, huber->weight(error3), 1e-8); DOUBLES_EQUAL(1.0, huber->weight(error4), 1e-8); - DOUBLES_EQUAL(0.5000, huber->loss(error1), 1e-8); - DOUBLES_EQUAL(37.5000, huber->loss(error2), 1e-8); - DOUBLES_EQUAL(37.5000, huber->loss(error3), 1e-8); - DOUBLES_EQUAL(0.5000, huber->loss(error4), 1e-8); + DOUBLES_EQUAL(0.5000, huber->residual(error1), 1e-8); + DOUBLES_EQUAL(37.5000, huber->residual(error2), 1e-8); + DOUBLES_EQUAL(37.5000, huber->residual(error3), 1e-8); + DOUBLES_EQUAL(0.5000, huber->residual(error4), 1e-8); } TEST(NoiseModel, robustFunctionCauchy) @@ -499,10 +499,10 @@ TEST(NoiseModel, robustFunctionCauchy) DOUBLES_EQUAL(0.2000, cauchy->weight(error3), 1e-8); DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error4), 1e-8); - DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error1), 1e-8); - DOUBLES_EQUAL(20.117973905426254, cauchy->loss(error2), 1e-8); - DOUBLES_EQUAL(20.117973905426254, cauchy->loss(error3), 1e-8); - DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error4), 1e-8); + DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error1), 1e-8); + DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error2), 1e-8); + DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error3), 1e-8); + DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error4), 1e-8); } TEST(NoiseModel, robustFunctionGemanMcClure) @@ -514,10 +514,10 @@ TEST(NoiseModel, robustFunctionGemanMcClure) DOUBLES_EQUAL(9.80296e-5, gmc->weight(error3), 1e-8); DOUBLES_EQUAL(0.25 , gmc->weight(error4), 1e-8); - DOUBLES_EQUAL(0.2500, gmc->loss(error1), 1e-8); - DOUBLES_EQUAL(0.495049504950495, gmc->loss(error2), 1e-8); - DOUBLES_EQUAL(0.495049504950495, gmc->loss(error3), 1e-8); - DOUBLES_EQUAL(0.2500, gmc->loss(error4), 1e-8); + DOUBLES_EQUAL(0.2500, gmc->residual(error1), 1e-8); + DOUBLES_EQUAL(0.495049504950495, gmc->residual(error2), 1e-8); + DOUBLES_EQUAL(0.495049504950495, gmc->residual(error3), 1e-8); + DOUBLES_EQUAL(0.2500, gmc->residual(error4), 1e-8); } TEST(NoiseModel, robustFunctionWelsch) @@ -530,10 +530,10 @@ TEST(NoiseModel, robustFunctionWelsch) DOUBLES_EQUAL(0.018315638888734, welsch->weight(error3), 1e-8); DOUBLES_EQUAL(0.960789439152323, welsch->weight(error4), 1e-8); - DOUBLES_EQUAL(0.490132010595960, welsch->loss(error1), 1e-8); - DOUBLES_EQUAL(12.271054513890823, welsch->loss(error2), 1e-8); - DOUBLES_EQUAL(12.271054513890823, welsch->loss(error3), 1e-8); - DOUBLES_EQUAL(0.490132010595960, welsch->loss(error4), 1e-8); + DOUBLES_EQUAL(0.490132010595960, welsch->residual(error1), 1e-8); + DOUBLES_EQUAL(12.271054513890823, welsch->residual(error2), 1e-8); + DOUBLES_EQUAL(12.271054513890823, welsch->residual(error3), 1e-8); + DOUBLES_EQUAL(0.490132010595960, welsch->residual(error4), 1e-8); } TEST(NoiseModel, robustFunctionTukey) @@ -546,10 +546,10 @@ TEST(NoiseModel, robustFunctionTukey) DOUBLES_EQUAL(0.0, tukey->weight(error3), 1e-8); DOUBLES_EQUAL(0.9216, tukey->weight(error4), 1e-8); - DOUBLES_EQUAL(0.480266666666667, tukey->loss(error1), 1e-8); - DOUBLES_EQUAL(4.166666666666667, tukey->loss(error2), 1e-8); - DOUBLES_EQUAL(4.166666666666667, tukey->loss(error3), 1e-8); - DOUBLES_EQUAL(0.480266666666667, tukey->loss(error4), 1e-8); + DOUBLES_EQUAL(0.480266666666667, tukey->residual(error1), 1e-8); + DOUBLES_EQUAL(4.166666666666667, tukey->residual(error2), 1e-8); + DOUBLES_EQUAL(4.166666666666667, tukey->residual(error3), 1e-8); + DOUBLES_EQUAL(0.480266666666667, tukey->residual(error4), 1e-8); } TEST(NoiseModel, robustFunctionDCS) @@ -560,8 +560,8 @@ TEST(NoiseModel, robustFunctionDCS) DOUBLES_EQUAL(1.0 , dcs->weight(error1), 1e-8); DOUBLES_EQUAL(0.00039211, dcs->weight(error2), 1e-8); - DOUBLES_EQUAL(0.5 , dcs->loss(error1), 1e-8); - DOUBLES_EQUAL(0.9900990099, dcs->loss(error2), 1e-8); + DOUBLES_EQUAL(0.5 , dcs->residual(error1), 1e-8); + DOUBLES_EQUAL(0.9900990099, dcs->residual(error2), 1e-8); } TEST(NoiseModel, robustFunctionL2WithDeadZone) @@ -576,12 +576,12 @@ TEST(NoiseModel, robustFunctionL2WithDeadZone) DOUBLES_EQUAL(0.00990099009, lsdz->weight(e4), 1e-8); DOUBLES_EQUAL(0.9, lsdz->weight(e5), 1e-8); - DOUBLES_EQUAL(40.5, lsdz->loss(e0), 1e-8); - DOUBLES_EQUAL(0.00005, lsdz->loss(e1), 1e-8); - DOUBLES_EQUAL(0.0, lsdz->loss(e2), 1e-8); - DOUBLES_EQUAL(0.0, lsdz->loss(e3), 1e-8); - DOUBLES_EQUAL(0.00005, lsdz->loss(e4), 1e-8); - DOUBLES_EQUAL(40.5, lsdz->loss(e5), 1e-8); + DOUBLES_EQUAL(40.5, lsdz->residual(e0), 1e-8); + DOUBLES_EQUAL(0.00005, lsdz->residual(e1), 1e-8); + DOUBLES_EQUAL(0.0, lsdz->residual(e2), 1e-8); + DOUBLES_EQUAL(0.0, lsdz->residual(e3), 1e-8); + DOUBLES_EQUAL(0.00005, lsdz->residual(e4), 1e-8); + DOUBLES_EQUAL(40.5, lsdz->residual(e5), 1e-8); } /* ************************************************************************* */ diff --git a/matlab/gtsam_examples/VisualizeMEstimators.m b/matlab/gtsam_examples/VisualizeMEstimators.m index ce505df5d..8a0485334 100644 --- a/matlab/gtsam_examples/VisualizeMEstimators.m +++ b/matlab/gtsam_examples/VisualizeMEstimators.m @@ -48,7 +48,7 @@ function plot_m_estimator(x, model, plot_title, fig_id, filename) rho = zeros(size(x)); for i = 1:size(x, 2) w(i) = model.weight(x(i)); - rho(i) = model.loss(x(i)); + rho(i) = model.residual(x(i)); end psi = w .* x; From ae85b640a868b58ee9a5abcd4d0d979003dd2d97 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Thu, 2 Apr 2020 11:04:31 -0400 Subject: [PATCH 72/79] re-add implemntation for deprecated Mahalanobis --- gtsam/linear/NoiseModel.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 7c3cb1294..4d86e77da 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -219,7 +219,9 @@ namespace gtsam { } #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 - virtual double Mahalanobis(const Vector& v) const; + virtual double Mahalanobis(const Vector& v) const { + return squaredMahalanobisDistance(v); + } #endif inline virtual double distance(const Vector& v) const { From 8383e9e285d908140988b8dfbfcc4bfa535ba7d4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Apr 2020 12:28:46 -0400 Subject: [PATCH 73/79] Deprecated several methods and madrng mutable, so methods are const --- gtsam/linear/Sampler.cpp | 25 +++++----- gtsam/linear/Sampler.h | 79 ++++++++++++++++-------------- gtsam/linear/tests/testSampler.cpp | 18 ++++--- 3 files changed, 66 insertions(+), 56 deletions(-) diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp index 9f1527bf9..57a34f364 100644 --- a/gtsam/linear/Sampler.cpp +++ b/gtsam/linear/Sampler.cpp @@ -11,6 +11,8 @@ /** * @file Sampler.cpp + * @brief sampling from a NoiseModel to generate + * @author Frank Dellaert * @author Alex Cunningham */ @@ -19,24 +21,18 @@ namespace gtsam { /* ************************************************************************* */ Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model, int32_t seed) - : model_(model), generator_(static_cast(seed)) -{ -} + : model_(model), generator_(static_cast(seed)) {} /* ************************************************************************* */ Sampler::Sampler(const Vector& sigmas, int32_t seed) -: model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(static_cast(seed)) -{ -} + : model_(noiseModel::Diagonal::Sigmas(sigmas, true)), + generator_(static_cast(seed)) {} /* ************************************************************************* */ -Sampler::Sampler(int32_t seed) -: generator_(static_cast(seed)) -{ -} +Sampler::Sampler(int32_t seed) : generator_(static_cast(seed)) {} /* ************************************************************************* */ -Vector Sampler::sampleDiagonal(const Vector& sigmas) { +Vector Sampler::sampleDiagonal(const Vector& sigmas) const { size_t d = sigmas.size(); Vector result(d); for (size_t i = 0; i < d; i++) { @@ -55,18 +51,19 @@ Vector Sampler::sampleDiagonal(const Vector& sigmas) { } /* ************************************************************************* */ -Vector Sampler::sample() { +Vector Sampler::sample() const { assert(model_.get()); const Vector& sigmas = model_->sigmas(); return sampleDiagonal(sigmas); } /* ************************************************************************* */ -Vector Sampler::sampleNewModel(const noiseModel::Diagonal::shared_ptr& model) { +Vector Sampler::sampleNewModel( + const noiseModel::Diagonal::shared_ptr& model) const { assert(model.get()); const Vector& sigmas = model->sigmas(); return sampleDiagonal(sigmas); } /* ************************************************************************* */ -} // \namespace gtsam +} // namespace gtsam diff --git a/gtsam/linear/Sampler.h b/gtsam/linear/Sampler.h index 93145c31a..b53f02d23 100644 --- a/gtsam/linear/Sampler.h +++ b/gtsam/linear/Sampler.h @@ -10,9 +10,9 @@ * -------------------------------------------------------------------------- */ /** - * @brief sampling that can be parameterized using a NoiseModel to generate samples from * @file Sampler.h - * the given distribution + * @brief sampling from a NoiseModel to generate + * @author Frank Dellaert * @author Alex Cunningham */ @@ -27,67 +27,74 @@ namespace gtsam { /** * Sampling structure that keeps internal random number generators for * diagonal distributions specified by NoiseModel - * - * This is primarily to allow for variable seeds, and does roughly the same - * thing as sample() in NoiseModel. */ class GTSAM_EXPORT Sampler { -protected: + protected: /** noiseModel created at generation */ noiseModel::Diagonal::shared_ptr model_; /** generator */ - std::mt19937_64 generator_; + mutable std::mt19937_64 generator_; -public: + public: typedef boost::shared_ptr shared_ptr; + /// @name constructors + /// @{ + /** * Create a sampler for the distribution specified by a diagonal NoiseModel * with a manually specified seed * * NOTE: do not use zero as a seed, it will break the generator */ - Sampler(const noiseModel::Diagonal::shared_ptr& model, int32_t seed = 42u); + explicit Sampler(const noiseModel::Diagonal::shared_ptr& model, + int32_t seed = 42u); /** - * Create a sampler for a distribution specified by a vector of sigmas directly + * Create a sampler for a distribution specified by a vector of sigmas + * directly * * NOTE: do not use zero as a seed, it will break the generator */ - Sampler(const Vector& sigmas, int32_t seed = 42u); + explicit Sampler(const Vector& sigmas, int32_t seed = 42u); - /** - * Create a sampler without a given noisemodel - pass in to sample - * - * NOTE: do not use zero as a seed, it will break the generator - */ - Sampler(int32_t seed = 42u); + /// @} + /// @name access functions + /// @{ + + size_t dim() const { + assert(model_.get()); + return model_->dim(); + } + + Vector sigmas() const { + assert(model_.get()); + return model_->sigmas(); + } - /** access functions */ - size_t dim() const { assert(model_.get()); return model_->dim(); } - Vector sigmas() const { assert(model_.get()); return model_->sigmas(); } const noiseModel::Diagonal::shared_ptr& model() const { return model_; } - /** - * sample from distribution - * NOTE: not const due to need to update the underlying generator - */ - Vector sample(); + /// @} + /// @name basic functionality + /// @{ - /** - * Sample from noisemodel passed in as an argument, - * can be used without having initialized a model for the system. - * - * NOTE: not const due to need to update the underlying generator - */ - Vector sampleNewModel(const noiseModel::Diagonal::shared_ptr& model); + /// sample from distribution + Vector sample() const; -protected: + /// @} +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + explicit Sampler(int32_t seed = 42u); + Vector sampleNewModel(const noiseModel::Diagonal::shared_ptr& model) const; + /// @} +#endif + + protected: /** given sigmas for a diagonal model, returns a sample */ - Vector sampleDiagonal(const Vector& sigmas); - + Vector sampleDiagonal(const Vector& sigmas) const; }; -} // \namespace gtsam +} // namespace gtsam diff --git a/gtsam/linear/tests/testSampler.cpp b/gtsam/linear/tests/testSampler.cpp index bd718500e..5831d9048 100644 --- a/gtsam/linear/tests/testSampler.cpp +++ b/gtsam/linear/tests/testSampler.cpp @@ -10,8 +10,10 @@ * -------------------------------------------------------------------------- */ /** - * @file testSampler + * @file testSampler.cpp + * @brief unit tests for Sampler class * @author Alex Cunningham + * @author Frank Dellaert */ #include @@ -22,14 +24,15 @@ using namespace gtsam; const double tol = 1e-5; +static const Vector3 kSigmas(1.0, 0.1, 0.0); + /* ************************************************************************* */ TEST(testSampler, basic) { - Vector sigmas = Vector3(1.0, 0.1, 0.0); - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(sigmas); + auto model = noiseModel::Diagonal::Sigmas(kSigmas); char seed = 'A'; Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1); - EXPECT(assert_equal(sigmas, sampler1.sigmas())); - EXPECT(assert_equal(sigmas, sampler2.sigmas())); + EXPECT(assert_equal(kSigmas, sampler1.sigmas())); + EXPECT(assert_equal(kSigmas, sampler2.sigmas())); EXPECT_LONGS_EQUAL(3, sampler1.dim()); EXPECT_LONGS_EQUAL(3, sampler2.dim()); Vector actual1 = sampler1.sample(); @@ -38,5 +41,8 @@ TEST(testSampler, basic) { } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ From 054e9f19c0cefee6cdff10bec0016c7e914394ca Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Apr 2020 12:30:34 -0400 Subject: [PATCH 74/79] Comment typos --- gtsam/linear/Sampler.cpp | 2 +- gtsam/linear/Sampler.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp index 57a34f364..ed204fcac 100644 --- a/gtsam/linear/Sampler.cpp +++ b/gtsam/linear/Sampler.cpp @@ -11,7 +11,7 @@ /** * @file Sampler.cpp - * @brief sampling from a NoiseModel to generate + * @brief sampling from a diagonal NoiseModel * @author Frank Dellaert * @author Alex Cunningham */ diff --git a/gtsam/linear/Sampler.h b/gtsam/linear/Sampler.h index b53f02d23..a52cde330 100644 --- a/gtsam/linear/Sampler.h +++ b/gtsam/linear/Sampler.h @@ -11,7 +11,7 @@ /** * @file Sampler.h - * @brief sampling from a NoiseModel to generate + * @brief sampling from a NoiseModel * @author Frank Dellaert * @author Alex Cunningham */ From 218cb5537b31a11d4fe2a066d7318fd468449fd1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Apr 2020 12:34:39 -0400 Subject: [PATCH 75/79] Changed argument to obviate need for cast --- gtsam/linear/Sampler.cpp | 12 ++++++------ gtsam/linear/Sampler.h | 6 +++--- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp index ed204fcac..fc58b2ba2 100644 --- a/gtsam/linear/Sampler.cpp +++ b/gtsam/linear/Sampler.cpp @@ -20,16 +20,16 @@ namespace gtsam { /* ************************************************************************* */ -Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model, int32_t seed) - : model_(model), generator_(static_cast(seed)) {} +Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model, + uint_fast64_t seed) + : model_(model), generator_(seed) {} /* ************************************************************************* */ -Sampler::Sampler(const Vector& sigmas, int32_t seed) - : model_(noiseModel::Diagonal::Sigmas(sigmas, true)), - generator_(static_cast(seed)) {} +Sampler::Sampler(const Vector& sigmas, uint_fast64_t seed) + : model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(seed) {} /* ************************************************************************* */ -Sampler::Sampler(int32_t seed) : generator_(static_cast(seed)) {} +Sampler::Sampler(uint_fast64_t seed) : generator_(seed) {} /* ************************************************************************* */ Vector Sampler::sampleDiagonal(const Vector& sigmas) const { diff --git a/gtsam/linear/Sampler.h b/gtsam/linear/Sampler.h index a52cde330..54c240a2b 100644 --- a/gtsam/linear/Sampler.h +++ b/gtsam/linear/Sampler.h @@ -49,7 +49,7 @@ class GTSAM_EXPORT Sampler { * NOTE: do not use zero as a seed, it will break the generator */ explicit Sampler(const noiseModel::Diagonal::shared_ptr& model, - int32_t seed = 42u); + uint_fast64_t seed = 42u); /** * Create a sampler for a distribution specified by a vector of sigmas @@ -57,7 +57,7 @@ class GTSAM_EXPORT Sampler { * * NOTE: do not use zero as a seed, it will break the generator */ - explicit Sampler(const Vector& sigmas, int32_t seed = 42u); + explicit Sampler(const Vector& sigmas, uint_fast64_t seed = 42u); /// @} /// @name access functions @@ -87,7 +87,7 @@ class GTSAM_EXPORT Sampler { #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ - explicit Sampler(int32_t seed = 42u); + explicit Sampler(uint_fast64_t seed = 42u); Vector sampleNewModel(const noiseModel::Diagonal::shared_ptr& model) const; /// @} #endif From 45cf253cecbdc8128fe8bc5a0e2b278991822285 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Apr 2020 13:08:36 -0400 Subject: [PATCH 76/79] Deprecated parts of cpp --- gtsam/linear/Sampler.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp index fc58b2ba2..16c7e73e0 100644 --- a/gtsam/linear/Sampler.cpp +++ b/gtsam/linear/Sampler.cpp @@ -28,9 +28,6 @@ Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model, Sampler::Sampler(const Vector& sigmas, uint_fast64_t seed) : model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(seed) {} -/* ************************************************************************* */ -Sampler::Sampler(uint_fast64_t seed) : generator_(seed) {} - /* ************************************************************************* */ Vector Sampler::sampleDiagonal(const Vector& sigmas) const { size_t d = sigmas.size(); @@ -58,12 +55,16 @@ Vector Sampler::sample() const { } /* ************************************************************************* */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +Sampler::Sampler(uint_fast64_t seed) : generator_(seed) {} + Vector Sampler::sampleNewModel( const noiseModel::Diagonal::shared_ptr& model) const { assert(model.get()); const Vector& sigmas = model->sigmas(); return sampleDiagonal(sigmas); } +#endif /* ************************************************************************* */ } // namespace gtsam From d1d8543fc718197265570bc7ddcb304fd6a535d7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Apr 2020 13:57:05 -0400 Subject: [PATCH 77/79] Fixed issues with deprecated Sampler methods --- gtsam.h | 6 ++---- gtsam/linear/NoiseModel.cpp | 1 - gtsam/navigation/ScenarioRunner.h | 2 +- gtsam/slam/dataset.cpp | 6 +++--- 4 files changed, 6 insertions(+), 9 deletions(-) diff --git a/gtsam.h b/gtsam.h index 8ee778f4c..413a1bc7c 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1563,17 +1563,15 @@ virtual class Robust : gtsam::noiseModel::Base { #include class Sampler { - //Constructors + // Constructors Sampler(gtsam::noiseModel::Diagonal* model, int seed); Sampler(Vector sigmas, int seed); - Sampler(int seed); - //Standard Interface + // Standard Interface size_t dim() const; Vector sigmas() const; gtsam::noiseModel::Diagonal* model() const; Vector sample(); - Vector sampleNewModel(gtsam::noiseModel::Diagonal* model); }; #include diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index b324ea784..df99191b2 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -25,7 +25,6 @@ #include #include #include -#include #include #include diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 5fb9f78d7..649d0fe12 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -48,7 +48,7 @@ class GTSAM_EXPORT ScenarioRunner { const Bias estimatedBias_; // Create two samplers for acceleration and omega noise - mutable Sampler gyroSampler_, accSampler_; + Sampler gyroSampler_, accSampler_; public: ScenarioRunner(const Scenario& scenario, const SharedParams& p, diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 45d009b1c..052bb3343 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -252,7 +252,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, is.seekg(0, ios::beg); // If asked, create a sampler with random number generator - Sampler sampler; + std::unique_ptr sampler; if (addNoise) { noiseModel::Diagonal::shared_ptr noise; if (model) @@ -261,7 +261,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, throw invalid_argument( "gtsam::load2D: invalid noise model for adding noise" "(current version assumes diagonal noise model)!"); - sampler = Sampler(noise); + sampler.reset(new Sampler(noise)); } // Parse the pose constraints @@ -289,7 +289,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, model = modelInFile; if (addNoise) - l1Xl2 = l1Xl2.retract(sampler.sample()); + l1Xl2 = l1Xl2.retract(sampler->sample()); // Insert vertices if pure odometry file if (!initial->exists(id1)) From e50f79b8954fe07490389e02d48778154ae346dc Mon Sep 17 00:00:00 2001 From: kvmanohar22 Date: Sat, 4 Apr 2020 23:50:53 +0530 Subject: [PATCH 78/79] fix typo --- gtsam/navigation/NavState.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index e9afcd3ac..f66e9d7a9 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -64,7 +64,7 @@ public: R_(pose.rotation()), t_(pose.translation()), v_(v) { } /// Construct from SO(3) and R^6 - NavState(const Matrix3& R, const Vector9 tv) : + NavState(const Matrix3& R, const Vector6& tv) : R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) { } /// Named constructor with derivatives From 76b29b78af8658d2828959f5e69285018801f19b Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Mon, 6 Apr 2020 23:31:05 +0200 Subject: [PATCH 79/79] Prefer C++11 nullptr --- examples/SFMExample_SmartFactor.cpp | 2 +- examples/SFMExample_SmartFactorPCG.cpp | 2 +- gtsam/base/OptionalJacobian.h | 26 +++++++++---------- .../treeTraversal/parallelTraversalTasks.h | 12 ++++----- gtsam/discrete/AlgebraicDecisionTree.h | 2 +- gtsam/discrete/DiscreteFactorGraph.cpp | 2 +- gtsam/geometry/tests/testOrientedPlane3.cpp | 2 +- gtsam/geometry/tests/testUnit3.cpp | 2 +- gtsam/inference/BayesTree-inst.h | 4 +-- gtsam/inference/FactorGraph-inst.h | 4 +-- gtsam/inference/FactorGraph.h | 2 +- gtsam/inference/Ordering.cpp | 4 +-- gtsam/linear/JacobianFactor.cpp | 2 +- gtsam/linear/JacobianFactor.h | 2 +- gtsam/linear/LossFunctions.cpp | 16 ++++++------ gtsam/linear/NoiseModel.cpp | 4 +-- gtsam/linear/tests/testJacobianFactor.cpp | 2 +- gtsam/navigation/AHRSFactor.cpp | 2 +- gtsam/navigation/AttitudeFactor.cpp | 4 +-- gtsam/navigation/CombinedImuFactor.cpp | 2 +- gtsam/navigation/GPSFactor.cpp | 4 +-- gtsam/nonlinear/ISAM2Params.h | 2 +- gtsam/nonlinear/NonlinearFactorGraph.cpp | 6 ++--- gtsam/nonlinear/tests/testCallRecord.cpp | 2 +- gtsam/slam/AntiFactor.h | 2 +- gtsam/slam/BetweenFactor.h | 2 +- gtsam/slam/EssentialMatrixConstraint.cpp | 2 +- gtsam/slam/JacobianFactorSVD.h | 2 +- gtsam/slam/OrientedPlane3Factor.cpp | 2 +- gtsam/slam/PoseRotationPrior.h | 2 +- gtsam/slam/PoseTranslationPrior.h | 2 +- gtsam/slam/PriorFactor.h | 2 +- gtsam/slam/tests/testOrientedPlane3Factor.cpp | 2 +- gtsam_unstable/base/BTree.h | 4 +-- .../linear/tests/testLinearEquality.cpp | 2 +- .../nonlinear/BatchFixedLagSmoother.cpp | 6 ++--- .../nonlinear/ConcurrentBatchFilter.cpp | 4 +-- .../nonlinear/ConcurrentBatchSmoother.cpp | 4 +-- .../nonlinear/IncrementalFixedLagSmoother.cpp | 2 +- gtsam_unstable/partition/FindSeparator-inl.h | 6 ++--- gtsam_unstable/slam/BiasedGPSFactor.h | 2 +- .../slam/EquivInertialNavFactor_GlobalVel.h | 2 +- .../EquivInertialNavFactor_GlobalVel_NoBias.h | 2 +- .../slam/GaussMarkov1stOrderFactor.h | 2 +- .../slam/InertialNavFactor_GlobalVelocity.h | 2 +- gtsam_unstable/slam/PartialPriorFactor.h | 2 +- gtsam_unstable/slam/PoseBetweenFactor.h | 2 +- gtsam_unstable/slam/PosePriorFactor.h | 2 +- .../slam/RelativeElevationFactor.cpp | 2 +- tests/testExtendedKalmanFilter.cpp | 8 +++--- 50 files changed, 92 insertions(+), 92 deletions(-) diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index c5545fc0c..6e28b87c9 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -117,7 +117,7 @@ int main(int argc, char* argv[]) { // The output of point() is in boost::optional, as sometimes // the triangulation operation inside smart factor will encounter degeneracy. boost::optional point = smart->point(result); - if (point) // ignore if boost::optional return NULL + if (point) // ignore if boost::optional return nullptr landmark_result.insert(j, *point); } } diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp index 075e2a653..47d69160f 100644 --- a/examples/SFMExample_SmartFactorPCG.cpp +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -114,7 +114,7 @@ int main(int argc, char* argv[]) { boost::dynamic_pointer_cast(graph[j]); if (smart) { boost::optional point = smart->point(result); - if (point) // ignore if boost::optional return NULL + if (point) // ignore if boost::optional return nullptr landmark_result.insert(j, *point); } } diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 82a5ae7f4..602c5915e 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -55,7 +55,7 @@ private: } // Private and very dangerous constructor straight from memory - OptionalJacobian(double* data) : map_(NULL) { + OptionalJacobian(double* data) : map_(nullptr) { if (data) usurp(data); } @@ -66,25 +66,25 @@ public: /// Default constructor acts like boost::none OptionalJacobian() : - map_(NULL) { + map_(nullptr) { } /// Constructor that will usurp data of a fixed-size matrix OptionalJacobian(Jacobian& fixed) : - map_(NULL) { + map_(nullptr) { usurp(fixed.data()); } /// Constructor that will usurp data of a fixed-size matrix, pointer version OptionalJacobian(Jacobian* fixedPtr) : - map_(NULL) { + map_(nullptr) { if (fixedPtr) usurp(fixedPtr->data()); } /// Constructor that will resize a dynamic matrix (unless already correct) OptionalJacobian(Eigen::MatrixXd& dynamic) : - map_(NULL) { + map_(nullptr) { dynamic.resize(Rows, Cols); // no malloc if correct size usurp(dynamic.data()); } @@ -93,12 +93,12 @@ public: /// Constructor with boost::none just makes empty OptionalJacobian(boost::none_t /*none*/) : - map_(NULL) { + map_(nullptr) { } /// Constructor compatible with old-style derivatives OptionalJacobian(const boost::optional optional) : - map_(NULL) { + map_(nullptr) { if (optional) { optional->resize(Rows, Cols); usurp(optional->data()); @@ -110,11 +110,11 @@ public: /// Constructor that will usurp data of a block expression /// TODO(frank): unfortunately using a Map makes usurping non-contiguous memory impossible // template - // OptionalJacobian(Eigen::Block block) : map_(NULL) { ?? } + // OptionalJacobian(Eigen::Block block) : map_(nullptr) { ?? } /// Return true is allocated, false if default constructor was used operator bool() const { - return map_.data() != NULL; + return map_.data() != nullptr; } /// De-reference, like boost optional @@ -173,7 +173,7 @@ public: /// Default constructor acts like boost::none OptionalJacobian() : - pointer_(NULL) { + pointer_(nullptr) { } /// Construct from pointer to dynamic matrix @@ -186,12 +186,12 @@ public: /// Constructor with boost::none just makes empty OptionalJacobian(boost::none_t /*none*/) : - pointer_(NULL) { + pointer_(nullptr) { } /// Constructor compatible with old-style derivatives OptionalJacobian(const boost::optional optional) : - pointer_(NULL) { + pointer_(nullptr) { if (optional) pointer_ = &(*optional); } @@ -199,7 +199,7 @@ public: /// Return true is allocated, false if default constructor was used operator bool() const { - return pointer_!=NULL; + return pointer_!=nullptr; } /// De-reference, like boost optional diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index 9b2dae3d0..db3181b87 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -53,7 +53,7 @@ namespace gtsam { // Run the post-order visitor (void) visitorPost(treeNode, *myData); - return NULL; + return nullptr; } }; @@ -88,7 +88,7 @@ namespace gtsam { { // Run the post-order visitor since this task was recycled to run the post-order visitor (void) visitorPost(treeNode, *myData); - return NULL; + return nullptr; } else { @@ -129,14 +129,14 @@ namespace gtsam { { // Run the post-order visitor in this task if we have no children (void) visitorPost(treeNode, *myData); - return NULL; + return nullptr; } } else { // Process this node and its children in this task processNodeRecursively(treeNode, *myData); - return NULL; + return nullptr; } } } @@ -184,8 +184,8 @@ namespace gtsam { set_ref_count(1 + (int) roots.size()); // Spawn tasks spawn_and_wait_for_all(tasks); - // Return NULL - return NULL; + // Return nullptr + return nullptr; } }; diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index 041d4c206..9cc55ed6a 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -101,7 +101,7 @@ namespace gtsam { /** Create a new function splitting on a variable */ template AlgebraicDecisionTree(Iterator begin, Iterator end, const L& label) : - Super(NULL) { + Super(nullptr) { this->root_ = compose(begin, end, label); } diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index af11d4b14..e41968d6b 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -71,7 +71,7 @@ namespace gtsam { for (size_t i = 0; i < factors_.size(); i++) { std::stringstream ss; ss << "factor " << i << ": "; - if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter); + if (factors_[i] != nullptr) factors_[i]->print(ss.str(), formatter); } } diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 54cf84ea8..849b76483 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -183,7 +183,7 @@ TEST (OrientedPlane3, jacobian_retract) { /* ************************************************************************* */ int main() { - srand(time(NULL)); + srand(time(nullptr)); TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 59b99e525..e08ad97bb 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -495,7 +495,7 @@ TEST(actualH, Serialization) { /* ************************************************************************* */ int main() { - srand(time(NULL)); + srand(time(nullptr)); TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 639bcbab0..5b53a5719 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -125,7 +125,7 @@ namespace gtsam { void BayesTree::addClique(const sharedClique& clique, const sharedClique& parent_clique) { for(Key j: clique->conditional()->frontals()) nodes_[j] = clique; - if (parent_clique != NULL) { + if (parent_clique != nullptr) { clique->parent_ = parent_clique; parent_clique->children.push_back(clique); } else { @@ -430,7 +430,7 @@ namespace gtsam { template void BayesTree::removePath(sharedClique clique, BayesNetType* bn, Cliques* orphans) { - // base case is NULL, if so we do nothing and return empties above + // base case is nullptr, if so we do nothing and return empties above if (clique) { // remove the clique from orphans in case it has been added earlier orphans->remove(clique); diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h index 34ca8ab7f..df68019e1 100644 --- a/gtsam/inference/FactorGraph-inst.h +++ b/gtsam/inference/FactorGraph-inst.h @@ -55,8 +55,8 @@ bool FactorGraph::equals(const This& fg, double tol) const { // check whether the factors are the same, in same order. for (size_t i = 0; i < factors_.size(); i++) { sharedFactor f1 = factors_[i], f2 = fg.factors_[i]; - if (f1 == NULL && f2 == NULL) continue; - if (f1 == NULL || f2 == NULL) return false; + if (f1 == nullptr && f2 == nullptr) continue; + if (f1 == nullptr || f2 == nullptr) return false; if (!f1->equals(*f2, tol)) return false; } return true; diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 600e3f9ed..2bc9578b2 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -353,7 +353,7 @@ class FactorGraph { */ void resize(size_t size) { factors_.resize(size); } - /** delete factor without re-arranging indexes by inserting a NULL pointer + /** delete factor without re-arranging indexes by inserting a nullptr pointer */ void remove(size_t i) { factors_[i].reset(); } diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 0875755aa..266c54ca6 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -91,7 +91,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, assert((size_t)count == variableIndex.nEntries()); - //double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */ + //double* knobs = nullptr; /* colamd arg 6: parameters (uses defaults if nullptr) */ double knobs[CCOLAMD_KNOBS]; ccolamd_set_defaults(knobs); knobs[CCOLAMD_DENSE_ROW] = -1; @@ -235,7 +235,7 @@ Ordering Ordering::Metis(const MetisIndex& met) { int outputError; - outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0], + outputError = METIS_NodeND(&size, &xadj[0], &adj[0], nullptr, nullptr, &perm[0], &iperm[0]); Ordering result; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 2310d88f0..839ffe69d 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -840,7 +840,7 @@ GaussianConditional::shared_ptr JacobianFactor::splitConditional(size_t nrFronta if (!model_) { throw std::invalid_argument( - "JacobianFactor::splitConditional cannot be given a NULL noise model"); + "JacobianFactor::splitConditional cannot be given a nullptr noise model"); } if (nrFrontals > size()) { diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index fad05a729..9b9d02726 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -398,7 +398,7 @@ namespace gtsam { * @param keys in some order * @param diemnsions of the variables in same order * @param m output dimension - * @param model noise model (default NULL) + * @param model noise model (default nullptr) */ template JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m, diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp index 6bc737e2c..de2a5142d 100644 --- a/gtsam/linear/LossFunctions.cpp +++ b/gtsam/linear/LossFunctions.cpp @@ -153,7 +153,7 @@ void Fair::print(const std::string &s="") const bool Fair::equals(const Base &expected, double tol) const { const Fair* p = dynamic_cast (&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(c_ - p->c_ ) < tol; } @@ -190,7 +190,7 @@ void Huber::print(const std::string &s="") const { bool Huber::equals(const Base &expected, double tol) const { const Huber* p = dynamic_cast(&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(k_ - p->k_) < tol; } @@ -223,7 +223,7 @@ void Cauchy::print(const std::string &s="") const { bool Cauchy::equals(const Base &expected, double tol) const { const Cauchy* p = dynamic_cast(&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(ksquared_ - p->ksquared_) < tol; } @@ -266,7 +266,7 @@ void Tukey::print(const std::string &s="") const { bool Tukey::equals(const Base &expected, double tol) const { const Tukey* p = dynamic_cast(&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(c_ - p->c_) < tol; } @@ -296,7 +296,7 @@ void Welsch::print(const std::string &s="") const { bool Welsch::equals(const Base &expected, double tol) const { const Welsch* p = dynamic_cast(&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(c_ - p->c_) < tol; } @@ -330,7 +330,7 @@ void GemanMcClure::print(const std::string &s="") const { bool GemanMcClure::equals(const Base &expected, double tol) const { const GemanMcClure* p = dynamic_cast(&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(c_ - p->c_) < tol; } @@ -372,7 +372,7 @@ void DCS::print(const std::string &s="") const { bool DCS::equals(const Base &expected, double tol) const { const DCS* p = dynamic_cast(&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(c_ - p->c_) < tol; } @@ -411,7 +411,7 @@ void L2WithDeadZone::print(const std::string &s="") const { bool L2WithDeadZone::equals(const Base &expected, double tol) const { const L2WithDeadZone* p = dynamic_cast(&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(k_ - p->k_) < tol; } diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index df99191b2..c62fe4f21 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -133,7 +133,7 @@ void Gaussian::print(const string& name) const { /* ************************************************************************* */ bool Gaussian::equals(const Base& expected, double tol) const { const Gaussian* p = dynamic_cast (&expected); - if (p == NULL) return false; + if (p == nullptr) return false; if (typeid(*this) != typeid(*p)) return false; //if (!sqrt_information_) return true; // ALEX todo; return equal_with_abs_tol(R(), p->R(), sqrt(tol)); @@ -624,7 +624,7 @@ void Robust::print(const std::string& name) const { bool Robust::equals(const Base& expected, double tol) const { const Robust* p = dynamic_cast (&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol); } diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 110a1223a..21146066d 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -253,7 +253,7 @@ TEST(JacobianFactor, error) /* ************************************************************************* */ TEST(JacobianFactor, matrices_NULL) { - // Make sure everything works with NULL noise model + // Make sure everything works with nullptr noise model JacobianFactor factor(simple::terms, simple::b); Matrix jacobianExpected(3, 9); diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 0d7e05515..4604a55dd 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -114,7 +114,7 @@ void AHRSFactor::print(const string& s, //------------------------------------------------------------------------------ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const { const This *e = dynamic_cast(&other); - return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol); + return e != nullptr && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol); } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/AttitudeFactor.cpp b/gtsam/navigation/AttitudeFactor.cpp index 3c6af9332..7f335152e 100644 --- a/gtsam/navigation/AttitudeFactor.cpp +++ b/gtsam/navigation/AttitudeFactor.cpp @@ -52,7 +52,7 @@ void Rot3AttitudeFactor::print(const string& s, bool Rot3AttitudeFactor::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol) + return e != nullptr && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol) && this->bRef_.equals(e->bRef_, tol); } @@ -69,7 +69,7 @@ void Pose3AttitudeFactor::print(const string& s, bool Pose3AttitudeFactor::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol) + return e != nullptr && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol) && this->bRef_.equals(e->bRef_, tol); } diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 1967e4a56..149067269 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -174,7 +174,7 @@ void CombinedImuFactor::print(const string& s, //------------------------------------------------------------------------------ bool CombinedImuFactor::equals(const NonlinearFactor& other, double tol) const { const This* e = dynamic_cast(&other); - return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol); + return e != nullptr && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol); } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index 05b7259bf..f2448c488 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -32,7 +32,7 @@ void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { //*************************************************************************** bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && traits::Equals(nT_, e->nT_, tol); + return e != nullptr && Base::equals(*e, tol) && traits::Equals(nT_, e->nT_, tol); } //*************************************************************************** @@ -73,7 +73,7 @@ void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const //*************************************************************************** bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && + return e != nullptr && Base::equals(*e, tol) && traits::Equals(nT_, e->nT_, tol); } diff --git a/gtsam/nonlinear/ISAM2Params.h b/gtsam/nonlinear/ISAM2Params.h index 5cf962e43..c6e1001c4 100644 --- a/gtsam/nonlinear/ISAM2Params.h +++ b/gtsam/nonlinear/ISAM2Params.h @@ -219,7 +219,7 @@ struct GTSAM_EXPORT ISAM2Params { /// When you will be removing many factors, e.g. when using ISAM2 as a /// fixed-lag smoother, enable this option to add factors in the first - /// available factor slots, to avoid accumulating NULL factor slots, at the + /// available factor slots, to avoid accumulating nullptr factor slots, at the /// cost of having to search for slots every time a factor is added. bool findUnusedFactorSlots; diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index d4b9fbb68..42b2d07f6 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -54,7 +54,7 @@ void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& key for (size_t i = 0; i < factors_.size(); i++) { stringstream ss; ss << "Factor " << i << ": "; - if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter); + if (factors_[i] != nullptr) factors_[i]->print(ss.str(), keyFormatter); cout << endl; } } @@ -67,8 +67,8 @@ void NonlinearFactorGraph::printErrors(const Values& values, const std::string& for (size_t i = 0; i < factors_.size(); i++) { stringstream ss; ss << "Factor " << i << ": "; - if (factors_[i] == NULL) { - cout << "NULL" << endl; + if (factors_[i] == nullptr) { + cout << "nullptr" << endl; } else { factors_[i]->print(ss.str(), keyFormatter); cout << "error = " << factors_[i]->error(values) << endl; diff --git a/gtsam/nonlinear/tests/testCallRecord.cpp b/gtsam/nonlinear/tests/testCallRecord.cpp index 494a59fff..c5ccc0f52 100644 --- a/gtsam/nonlinear/tests/testCallRecord.cpp +++ b/gtsam/nonlinear/tests/testCallRecord.cpp @@ -98,7 +98,7 @@ struct Record: public internal::CallRecordImplementor { friend struct internal::CallRecordImplementor; }; -internal::JacobianMap & NJM= *static_cast(NULL); +internal::JacobianMap & NJM= *static_cast(nullptr); /* ************************************************************************* */ typedef Eigen::Matrix DynRowMat; diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h index b52e115fd..9e4f7db5a 100644 --- a/gtsam/slam/AntiFactor.h +++ b/gtsam/slam/AntiFactor.h @@ -67,7 +67,7 @@ namespace gtsam { /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && this->factor_->equals(*e->factor_, tol); + return e != nullptr && Base::equals(*e, tol) && this->factor_->equals(*e->factor_, tol); } /** implement functions needed to derive from Factor */ diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index f949514e3..23138b9e6 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -81,7 +81,7 @@ namespace gtsam { /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); + return e != nullptr && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); } /** implement functions needed to derive from Factor */ diff --git a/gtsam/slam/EssentialMatrixConstraint.cpp b/gtsam/slam/EssentialMatrixConstraint.cpp index e27bbc8c5..5397c2e57 100644 --- a/gtsam/slam/EssentialMatrixConstraint.cpp +++ b/gtsam/slam/EssentialMatrixConstraint.cpp @@ -37,7 +37,7 @@ void EssentialMatrixConstraint::print(const std::string& s, bool EssentialMatrixConstraint::equals(const NonlinearFactor& expected, double tol) const { const This *e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) + return e != nullptr && Base::equals(*e, tol) && this->measuredE_.equals(e->measuredE_, tol); } diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index b94714dd6..bc906d24e 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -65,7 +65,7 @@ public: Base() { size_t numKeys = Enull.rows() / ZDim; size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()? - // PLAIN NULL SPACE TRICK + // PLAIN nullptr SPACE TRICK // Matrix Q = Enull * Enull.transpose(); // for(const KeyMatrixZD& it: Fblocks) // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index 5b0c6a6b9..56968301a 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -31,7 +31,7 @@ void OrientedPlane3DirectionPrior::print(const string& s, bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) + return e != nullptr && Base::equals(*e, tol) && this->measured_p_.equals(e->measured_p_, tol); } diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index 78030ff34..f4ce1520a 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -62,7 +62,7 @@ public: /** equals specialized to this factor */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && measured_.equals(e->measured_, tol); + return e != nullptr && Base::equals(*e, tol) && measured_.equals(e->measured_, tol); } /** print contents */ diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index c38d13b58..516582d83 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -76,7 +76,7 @@ public: /** equals specialized to this factor */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && traits::Equals(measured_, e->measured_, tol); + return e != nullptr && Base::equals(*e, tol) && traits::Equals(measured_, e->measured_, tol); } /** print contents */ diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 5b085652f..8d8c67d5c 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -85,7 +85,7 @@ namespace gtsam { /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This* e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && traits::Equals(prior_, e->prior_, tol); + return e != nullptr && Base::equals(*e, tol) && traits::Equals(prior_, e->prior_, tol); } /** implement functions needed to derive from Factor */ diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index ea42a1ecd..f1dbb4239 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -175,7 +175,7 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) { /* ************************************************************************* */ int main() { - srand(time(NULL)); + srand(time(nullptr)); TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/gtsam_unstable/base/BTree.h b/gtsam_unstable/base/BTree.h index fd675d0d5..96424324b 100644 --- a/gtsam_unstable/base/BTree.h +++ b/gtsam_unstable/base/BTree.h @@ -72,7 +72,7 @@ namespace gtsam { }; // Node // We store a shared pointer to the root of the functional tree - // composed of Node classes. If root_==NULL, the tree is empty. + // composed of Node classes. If root_==nullptr, the tree is empty. typedef boost::shared_ptr sharedNode; sharedNode root_; @@ -223,7 +223,7 @@ namespace gtsam { /** Return height of the tree, 0 if empty */ size_t height() const { - return (root_ != NULL) ? root_->height_ : 0; + return (root_ != nullptr) ? root_->height_ : 0; } /** return size of the tree */ diff --git a/gtsam_unstable/linear/tests/testLinearEquality.cpp b/gtsam_unstable/linear/tests/testLinearEquality.cpp index fa94dd255..36a2cacd8 100644 --- a/gtsam_unstable/linear/tests/testLinearEquality.cpp +++ b/gtsam_unstable/linear/tests/testLinearEquality.cpp @@ -136,7 +136,7 @@ TEST(LinearEquality, error) /* ************************************************************************* */ TEST(LinearEquality, matrices_NULL) { - // Make sure everything works with NULL noise model + // Make sure everything works with nullptr noise model LinearEquality factor(simple::terms, simple::b, 0); Matrix AExpected(3, 9); diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index b0a19c6a4..777e4b2d3 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -39,7 +39,7 @@ bool BatchFixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { const BatchFixedLagSmoother* e = dynamic_cast(&rhs); - return e != NULL && FixedLagSmoother::equals(*e, tol) + return e != nullptr && FixedLagSmoother::equals(*e, tol) && factors_.equals(e->factors_, tol) && theta_.equals(e->theta_, tol); } @@ -145,7 +145,7 @@ void BatchFixedLagSmoother::removeFactors( } else { // TODO: Throw an error?? cout << "Attempting to remove a factor from slot " << slot - << ", but it is already NULL." << endl; + << ", but it is already nullptr." << endl; } } } @@ -370,7 +370,7 @@ void BatchFixedLagSmoother::PrintSymbolicFactor( cout << " " << DefaultKeyFormatter(key); } } else { - cout << " NULL"; + cout << " nullptr"; } cout << " )" << endl; } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 97df375d5..758bcfe48 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -38,7 +38,7 @@ void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_p } std::cout << ")" << std::endl; } else { - std::cout << "{ NULL }" << std::endl; + std::cout << "{ nullptr }" << std::endl; } } @@ -79,7 +79,7 @@ void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr& } std::cout << ")" << std::endl; } else { - std::cout << "{ NULL }" << std::endl; + std::cout << "{ nullptr }" << std::endl; } } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 775dccbb0..600baa9f0 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -394,7 +394,7 @@ void ConcurrentBatchSmoother::PrintNonlinearFactor(const NonlinearFactor::shared } std::cout << ")" << std::endl; } else { - std::cout << "{ NULL }" << std::endl; + std::cout << "{ nullptr }" << std::endl; } } @@ -408,7 +408,7 @@ void ConcurrentBatchSmoother::PrintLinearFactor(const GaussianFactor::shared_ptr } std::cout << ")" << std::endl; } else { - std::cout << "{ NULL }" << std::endl; + std::cout << "{ nullptr }" << std::endl; } } diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index a99360ffb..d29dfe8ca 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -58,7 +58,7 @@ bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { const IncrementalFixedLagSmoother* e = dynamic_cast(&rhs); - return e != NULL && FixedLagSmoother::equals(*e, tol) + return e != nullptr && FixedLagSmoother::equals(*e, tol) && isam_.equals(e->isam_, tol); } diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index af5415469..d87a8fd42 100644 --- a/gtsam_unstable/partition/FindSeparator-inl.h +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -83,14 +83,14 @@ namespace gtsam { namespace partition { graph_t *graph; real_t *tpwgts2; ctrl_t *ctrl; - ctrl = SetupCtrl(METIS_OP_OMETIS, options, 1, 3, NULL, NULL); + ctrl = SetupCtrl(METIS_OP_OMETIS, options, 1, 3, nullptr, nullptr); ctrl->iptype = METIS_IPTYPE_GROW; - //if () == NULL) + //if () == nullptr) // return METIS_ERROR_INPUT; InitRandom(ctrl->seed); - graph = SetupGraph(ctrl, *nvtxs, 1, xadj, adjncy, vwgt, NULL, NULL); + graph = SetupGraph(ctrl, *nvtxs, 1, xadj, adjncy, vwgt, nullptr, nullptr); AllocateWorkSpace(ctrl, graph); diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h index f2bcb7cd7..cf56afab2 100644 --- a/gtsam_unstable/slam/BiasedGPSFactor.h +++ b/gtsam_unstable/slam/BiasedGPSFactor.h @@ -66,7 +66,7 @@ namespace gtsam { /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); + return e != nullptr && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); } /** implement functions needed to derive from Factor */ diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 58284c3a6..f499a0f4b 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -155,7 +155,7 @@ public: /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) + return e != nullptr && Base::equals(*e, tol) && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol && (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol && (delta_angles_ - e->delta_angles_).norm() < tol diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index acca233d9..86efd10c1 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -153,7 +153,7 @@ public: /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) + return e != nullptr && Base::equals(*e, tol) && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol && (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol && (delta_angles_ - e->delta_angles_).norm() < tol diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h index 68c972a86..762199753 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -81,7 +81,7 @@ public: /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol); + return e != nullptr && Base::equals(*e, tol); } /** implement functions needed to derive from Factor */ diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index c3a67232a..4763c4263 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -135,7 +135,7 @@ public: /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) + return e != nullptr && Base::equals(*e, tol) && (measurement_acc_ - e->measurement_acc_).norm() < tol && (measurement_gyro_ - e->measurement_gyro_).norm() < tol && (dt_ - e->dt_) < tol diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index c71ee7abd..c6d892e93 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -100,7 +100,7 @@ namespace gtsam { /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && + return e != nullptr && Base::equals(*e, tol) && gtsam::equal_with_abs_tol(this->prior_, e->prior_, tol) && this->mask_ == e->mask_; } diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index 03803b5f4..bb5835f80 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -79,7 +79,7 @@ namespace gtsam { /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL + return e != nullptr && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol) && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index d8649a0d5..ce9861160 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -74,7 +74,7 @@ namespace gtsam { /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This* e = dynamic_cast (&expected); - return e != NULL + return e != nullptr && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol) && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); diff --git a/gtsam_unstable/slam/RelativeElevationFactor.cpp b/gtsam_unstable/slam/RelativeElevationFactor.cpp index 32e8731cd..b35171041 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/RelativeElevationFactor.cpp @@ -38,7 +38,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p /* ************************************************************************* */ bool RelativeElevationFactor::equals(const NonlinearFactor& expected, double tol) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && std::abs(this->measured_ - e->measured_) < tol; + return e != nullptr && Base::equals(*e, tol) && std::abs(this->measured_ - e->measured_) < tol; } /* ************************************************************************* */ diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index b3e8a3449..212a8e107 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -162,7 +162,7 @@ public: /** Check if two factors are equal. Note type is IndexFactor and needs cast. */ virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { const This *e = dynamic_cast (&f); - return (e != NULL) && (key1() == e->key1()) && (key2() == e->key2()); + return (e != nullptr) && (key1() == e->key1()) && (key2() == e->key2()); } /** @@ -196,7 +196,7 @@ public: Vector b = -evaluateError(x1, x2, A1, A2); SharedDiagonal constrained = boost::dynamic_pointer_cast(this->noiseModel_); - if (constrained.get() != NULL) { + if (constrained.get() != nullptr) { return JacobianFactor::shared_ptr(new JacobianFactor(key1(), A1, key2(), A2, b, constrained)); } @@ -292,7 +292,7 @@ public: /** Check if two factors are equal. Note type is IndexFactor and needs cast. */ virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { const This *e = dynamic_cast (&f); - return (e != NULL) && Base::equals(f); + return (e != nullptr) && Base::equals(f); } /** @@ -325,7 +325,7 @@ public: Vector b = -evaluateError(x1, A1); SharedDiagonal constrained = boost::dynamic_pointer_cast(this->noiseModel_); - if (constrained.get() != NULL) { + if (constrained.get() != nullptr) { return JacobianFactor::shared_ptr(new JacobianFactor(key(), A1, b, constrained)); } // "Whiten" the system before converting to a Gaussian Factor