diff --git a/.travis.sh b/.travis.sh index bc9029595..9fc09a3f8 100755 --- a/.travis.sh +++ b/.travis.sh @@ -1,5 +1,37 @@ #!/bin/bash +# install TBB with _debug.so files +function install_tbb() +{ + 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 [ "$TRAVIS_OS_NAME" == "linux" ]; then + OS_SHORT="lin" + TBB_LIB_DIR="intel64/gcc4.4" + SUDO="sudo" + + elif [ "$TRAVIS_OS_NAME" == "osx" ]; then + OS_SHORT="lin" + TBB_LIB_DIR="" + SUDO="" + + fi + + 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 function configure() { @@ -14,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 @@ -24,6 +58,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:-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 fe9667330..d3b73f2cf 100644 --- a/.travis.yml +++ b/.travis.yml @@ -18,7 +18,7 @@ addons: - libboost-all-dev # before_install: -# - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update ; fi + # - 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 @@ -89,6 +89,12 @@ jobs: compiler: clang 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 on to make sure GTSAM still compiles/tests + - stage: special + os: linux + compiler: gcc + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=ON + script: bash .travis.sh -b # -------- STAGE 2: TESTS ----------- # on Mac, GCC - stage: test diff --git a/CMakeLists.txt b/CMakeLists.txt index 6ac85bff6..7a49a8aa6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -199,12 +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 - # 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 2020) OR (${TBB_VERSION_MAJOR} 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() ############################################################################### @@ -416,6 +421,10 @@ add_subdirectory(CppUnitLite) # Build wrap if (GTSAM_BUILD_WRAP) add_subdirectory(wrap) + # suppress warning of cython line being too long + 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 diff --git a/cython/gtsam/examples/ImuFactorExample2.py b/cython/gtsam/examples/ImuFactorExample2.py index 1f6732f49..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.SimpleCamera.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/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/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..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.SimpleCamera.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 diff --git a/cython/gtsam_unstable/examples/TimeOfArrivalExample.py b/cython/gtsam_unstable/examples/TimeOfArrivalExample.py new file mode 100644 index 000000000..6ba06f0f2 --- /dev/null +++ b/cython/gtsam_unstable/examples/TimeOfArrivalExample.py @@ -0,0 +1,128 @@ +""" +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) + +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 define_microphones(): + """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 create_trajectory(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 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 simulate_toa(microphones, trajectory): + """Simulate time-of-arrival measurements for an entire trajectory.""" + return [simulate_one_toa(microphones, event) + for event in trajectory] + + +def create_graph(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 create_initial_estimate(n): + """Create initial estimate for n events.""" + initial = Values() + zero = Event() + for key in range(n): + TOAFactor.InsertEvent(key, zero, initial) + return initial + + +def toa_example(): + """Run example with 4 microphones and 5 events in a straight line.""" + # Create microphones + 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 = create_trajectory(n) + for event in groundTruth: + print(event) + + # Simulate time-of-arrival measurements + 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 = create_graph(microphones, simulatedTOA) + print(graph.at(0)) + + # Create initial estimate + initial_estimate = create_initial_estimate(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__': + toa_example() + print("Example complete") diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index e3408b67b..b12418098 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -18,7 +18,8 @@ #include #include -#include +#include +#include #include using namespace gtsam; @@ -47,7 +48,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/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 diff --git a/examples/ISAM2Example_SmartFactor.cpp b/examples/ISAM2Example_SmartFactor.cpp index d8fee1516..21fe6e661 100644 --- a/examples/ISAM2Example_SmartFactor.cpp +++ b/examples/ISAM2Example_SmartFactor.cpp @@ -4,7 +4,8 @@ * @author Alexander (pumaking on BitBucket) */ -#include +#include +#include #include #include #include diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorExample2.cpp index 25a6adf51..f83a539af 100644 --- a/examples/ImuFactorExample2.cpp +++ b/examples/ImuFactorExample2.cpp @@ -1,5 +1,6 @@ -#include +#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 SimpleCamera camera = SimpleCamera::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/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 new file mode 100644 index 000000000..05a04b353 --- /dev/null +++ b/examples/Pose3Localization.cpp @@ -0,0 +1,85 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 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 + 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; + 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(*graph, *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; + NonlinearFactorGraph::shared_ptr graphNoKernel; + Values::shared_ptr initial2; + boost::tie(graphNoKernel, initial2) = readG2o(g2oFile); + writeG2o(*graphNoKernel, 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; +} 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; 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/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/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 062bcb531..413a1bc7c 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; }; @@ -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); @@ -674,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; @@ -1310,7 +1312,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; @@ -1561,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 @@ -2136,7 +2136,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); @@ -2457,7 +2457,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; @@ -2495,7 +2495,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; @@ -2518,7 +2518,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); @@ -2544,9 +2544,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 @@ -2637,7 +2637,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; @@ -3151,7 +3151,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/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/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/Rot3.cpp b/gtsam/geometry/Rot3.cpp index b1e8dd14b..01f62b8cb 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 @@ -36,7 +37,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); @@ -185,6 +185,12 @@ Vector Rot3::quaternion() const { return v; } +/* ************************************************************************* */ +pair Rot3::axisAngle() const { + const Vector3 omega = Rot3::Logmap(*this); + return std::pair(Unit3(omega), omega.norm()); +} + /* ************************************************************************* */ Matrix3 Rot3::ExpmapDerivative(const Vector3& x) { return SO3::ExpmapDerivative(x); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index e4408c9f4..fc3a8b3f2 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 @@ -194,22 +195,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) { + // 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 +271,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 Rot3(quaternion_.inverse()); +#else + return Rot3(rot_.matrix().transpose()); +#endif } /** @@ -406,7 +413,6 @@ namespace gtsam { * Return 3*3 transpose (inverse) rotation matrix */ Matrix3 transpose() const; - // TODO: const Eigen::Transpose transpose() const; /// @deprecated, this is base 1, and was just confusing Point3 column(int index) const; @@ -429,7 +435,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; @@ -439,7 +445,7 @@ namespace gtsam { * 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 @@ -447,7 +453,7 @@ namespace gtsam { * 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 @@ -455,12 +461,22 @@ namespace gtsam { * 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 /// @{ + /** + * 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() const; + /** Compute the quaternion representation of this rotation. * @return The quaternion */ diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index d38d33bf1..46a07e50a 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -110,7 +110,6 @@ Rot3 Rot3::operator*(const Rot3& R2) const { } /* ************************************************************************* */ -// TODO 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 8af9a7144..d609c289c 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; @@ -79,9 +79,13 @@ 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 { + // 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. Matrix3 Rot3::transpose() const { return matrix().transpose(); } 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); 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/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); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index c95b85f21..182db59b0 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 @@ -115,6 +116,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)); } /* ************************************************************************* */ @@ -376,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.matrix(), R.transpose())); Matrix numericalH = numericalDerivative11(testing::inverse, R); CHECK(assert_equal(numericalH,actualH)); @@ -661,6 +666,65 @@ TEST(Rot3, ClosestTo) { EXPECT(assert_equal(expected, actual.matrix(), 1e-6)); } +/* ************************************************************************* */ +TEST(Rot3, axisAngle) { + Unit3 actualAxis; + double actualAngle; + +// 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))) + + // 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); + + // 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)) + + // 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)) + + // 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)) + + // 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)) + + const double theta195 = 195 * M_PI / 180; + const double theta165 = 165 * M_PI / 180; + + /// 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 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)) +} + /* ************************************************************************* */ int main() { TestResult tr; 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/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/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 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 915550d22..14b89dda9 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -25,7 +25,6 @@ #include #include #include -#include #include #include @@ -134,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; //TODO(Alex); //if (!sqrt_information_) return true; @@ -168,7 +167,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); @@ -584,7 +583,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_; } @@ -636,7 +635,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/NoiseModel.h b/gtsam/linear/NoiseModel.h index dda780a36..ce2c84425 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -207,12 +207,25 @@ namespace gtsam { virtual Vector unwhiten(const Vector& v) const; /** - * Mahalanobis distance v'*R'*R*v = + * Squared Mahalanobis distance v'*R'*R*v = */ - virtual double Mahalanobis(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)); + } + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + virtual double Mahalanobis(const Vector& v) const { + return squaredMahalanobisDistance(v); + } +#endif inline virtual double distance(const Vector& v) const { - return Mahalanobis(v); + return squaredMahalanobisDistance(v); } /** @@ -564,7 +577,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 +629,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/Sampler.cpp b/gtsam/linear/Sampler.cpp index 9f1527bf9..16c7e73e0 100644 --- a/gtsam/linear/Sampler.cpp +++ b/gtsam/linear/Sampler.cpp @@ -11,6 +11,8 @@ /** * @file Sampler.cpp + * @brief sampling from a diagonal NoiseModel + * @author Frank Dellaert * @author Alex Cunningham */ @@ -18,25 +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)) -{ -} - -/* ************************************************************************* */ -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 +48,23 @@ 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) { +#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 +} // namespace gtsam diff --git a/gtsam/linear/Sampler.h b/gtsam/linear/Sampler.h index 93145c31a..54c240a2b 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 + * @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, + uint_fast64_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, uint_fast64_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(uint_fast64_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/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 */ 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/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 10578627f..3f6550b9f 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) 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); +} /* ************************************************************************* */ 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/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 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/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/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 } /* ************************************************************************* */ 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)); } /* ************************************************************************* */ 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/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/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/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/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index 867db70e0..a78f7a2d0 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"); @@ -237,13 +237,16 @@ 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 + #ifndef NDEBUG + std::cerr << "Values[key] is neither Pose2 nor Point2, so skip" << std::endl; + #endif } } } diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp index 5d57886f5..54d4a43c0 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,13 @@ 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); + using Camera = PinholeCamera; + + RangeFactor factor1(poseKey, pointKey, measurement, model); + RangeFactor factor2(poseKey, pointKey, measurement, model); + RangeFactor factor3(poseKey, pointKey, measurement, model); } /* ************************************************************************* */ 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/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/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/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/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)) diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 6d6daa33d..95db611d7 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/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/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/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/examples/TimeOfArrivalExample.cpp b/gtsam_unstable/examples/TimeOfArrivalExample.cpp new file mode 100644 index 000000000..9991e04b6 --- /dev/null +++ b/gtsam_unstable/examples/TimeOfArrivalExample.cpp @@ -0,0 +1,160 @@ +/* ---------------------------------------------------------------------------- + + * 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) + + * 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 diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index fc186857f..383c34915 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,44 @@ 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 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 { + 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/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/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/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/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 */ 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/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/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/gtsam_unstable/slam/TOAFactor.h b/gtsam_unstable/slam/TOAFactor.h index 1df14a8de..8ad4a14a6 100644 --- a/gtsam_unstable/slam/TOAFactor.h +++ b/gtsam_unstable/slam/TOAFactor.h @@ -17,33 +17,51 @@ * @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 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 speed optional speed of signal, in m/sec */ 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, double speed = 330) + : ExpressionFactor( + model, toaMeasurement, + Double_(TimeOfArrival(speed), 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, + 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 - +} // 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/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 4b10d5c0b..042130a24 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,83 +44,59 @@ 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 ) { +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(key, sensorAt0, measurement, model); } //***************************************************************************** -TEST( TOAFactor, WholeEnchilada ) { - - static const bool verbose = false; - - // Create microphones +TEST(TOAFactor, WholeEnchilada) { + // 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] = groundTruthEvent.toa(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 - 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(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); + // 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"); - // 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)); } @@ -129,4 +106,3 @@ int main() { return TestRegistry::runAllTests(tr); } //***************************************************************************** - 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/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 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/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 0e7793552..7b5e7a0e0 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -339,31 +340,136 @@ 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; + + auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); + auto lm_result = LevenbergMarquardtOptimizer(fg, init, lm_params).optimize(); + auto dl_result = DoglegOptimizer(fg, init).optimize(); + + 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())); + + 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, 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; + + 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, 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; + + 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, lm_result, tol)); + EXPECT(assert_equal(expected, dl_result, tol)); } /* ************************************************************************* */ 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); 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