12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'develop' into fix/todos

release/4.3a0
Varun Agrawal 2020-04-08 19:34:25 -04:00
parent 44ec8ba8c0 5c9c4bed93
commit d74e2666d3
140 changed files with 1355 additions and 576 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
.travis.sh
View File

@ -1,5 +1,37 @@
#!/bin/bash #!/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 # common tasks before either build or test
function configure() function configure()
{ {
@ -14,6 +46,8 @@ function configure()
rm -fr $BUILD_DIR || true rm -fr $BUILD_DIR || true
mkdir $BUILD_DIR && cd $BUILD_DIR mkdir $BUILD_DIR && cd $BUILD_DIR
install_tbb
if [ ! -z "$GCC_VERSION" ]; then if [ ! -z "$GCC_VERSION" ]; then
export CC=gcc-$GCC_VERSION export CC=gcc-$GCC_VERSION
export CXX=g++-$GCC_VERSION export CXX=g++-$GCC_VERSION
@ -24,6 +58,7 @@ function configure()
-DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \ -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \
-DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \ -DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \
-DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \ -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
-DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
-DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \ -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \
-DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \ -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \

6
.travis.yml
View File

@ -89,6 +89,12 @@ jobs:
compiler: clang compiler: clang
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON
script: bash .travis.sh -b 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 ----------- # -------- STAGE 2: TESTS -----------
# on Mac, GCC # on Mac, GCC
- stage: test - stage: test

9
CMakeLists.txt
View File

@ -200,6 +200,11 @@ find_package(TBB 4.4 COMPONENTS tbb tbbmalloc)
# Set up variables if we're using TBB # Set up variables if we're using TBB
if(TBB_FOUND AND GTSAM_WITH_TBB) if(TBB_FOUND AND GTSAM_WITH_TBB)
set(GTSAM_USE_TBB 1) # This will go into config.h 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: # all definitions and link requisites will go via imported targets:
# tbb & tbbmalloc # tbb & tbbmalloc
list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc) list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc)
@ -416,6 +421,10 @@ add_subdirectory(CppUnitLite)
# Build wrap # Build wrap
if (GTSAM_BUILD_WRAP) if (GTSAM_BUILD_WRAP)
add_subdirectory(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) endif(GTSAM_BUILD_WRAP)
# Build GTSAM library # Build GTSAM library

38
cython/gtsam/examples/ImuFactorExample2.py
View File

@ -14,6 +14,11 @@ from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
import gtsam import gtsam
import gtsam.utils.plot as gtsam_plot 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): def X(key):
@ -69,8 +74,8 @@ PARAMS.setUse2ndOrderCoriolis(False)
PARAMS.setOmegaCoriolis(vector3(0, 0, 0)) PARAMS.setOmegaCoriolis(vector3(0, 0, 0))
BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1) BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1)
DELTA = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0), DELTA = Pose3(Rot3.Rodrigues(0, 0, 0),
gtsam.Point3(0.05, -0.10, 0.20)) Point3(0.05, -0.10, 0.20))
def IMU_example(): def IMU_example():
@ -78,10 +83,10 @@ def IMU_example():
# Start with a camera on x-axis looking at origin # Start with a camera on x-axis looking at origin
radius = 30 radius = 30
up = gtsam.Point3(0, 0, 1) up = Point3(0, 0, 1)
target = gtsam.Point3(0, 0, 0) target = Point3(0, 0, 0)
position = gtsam.Point3(radius, 0, 0) position = Point3(radius, 0, 0)
camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2()) camera = PinholeCameraCal3_S2.Lookat(position, target, up, Cal3_S2())
pose_0 = camera.pose() pose_0 = camera.pose()
# Create the set of ground-truth landmarks and poses # Create the set of ground-truth landmarks and poses
@ -90,29 +95,29 @@ def IMU_example():
angular_velocity_vector = vector3(0, -angular_velocity, 0) angular_velocity_vector = vector3(0, -angular_velocity, 0)
linear_velocity_vector = vector3(radius * angular_velocity, 0, 0) linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
scenario = gtsam.ConstantTwistScenario( scenario = ConstantTwistScenario(
angular_velocity_vector, linear_velocity_vector, pose_0) angular_velocity_vector, linear_velocity_vector, pose_0)
# Create a factor graph # Create a factor graph
newgraph = gtsam.NonlinearFactorGraph() newgraph = NonlinearFactorGraph()
# Create (incremental) ISAM2 solver # Create (incremental) ISAM2 solver
isam = gtsam.ISAM2() isam = ISAM2()
# Create the initial estimate to the solution # Create the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth # 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. # 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 # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noise = gtsam.noiseModel_Diagonal.Sigmas( noise = gtsam.noiseModel_Diagonal.Sigmas(
np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3])) 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 # Add imu priors
biasKey = gtsam.symbol(ord('b'), 0) biasKey = gtsam.symbol(ord('b'), 0)
biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(), biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
biasnoise) biasnoise)
newgraph.push_back(biasprior) newgraph.push_back(biasprior)
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
@ -120,7 +125,7 @@ def IMU_example():
# Calculate with correct initial velocity # Calculate with correct initial velocity
n_velocity = vector3(0, angular_velocity * radius, 0) 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) newgraph.push_back(velprior)
initialEstimate.insert(V(0), n_velocity) initialEstimate.insert(V(0), n_velocity)
@ -141,7 +146,7 @@ def IMU_example():
# Add Bias variables periodically # Add Bias variables periodically
if i % 5 == 0: if i % 5 == 0:
biasKey += 1 biasKey += 1
factor = gtsam.BetweenFactorConstantBias( factor = BetweenFactorConstantBias(
biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE) biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE)
newgraph.add(factor) newgraph.add(factor)
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
@ -154,8 +159,7 @@ def IMU_example():
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t) accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)
# Add Imu Factor # Add Imu Factor
imufac = gtsam.ImuFactor( imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
newgraph.add(imufac) newgraph.add(imufac)
# insert new velocity, which is wrong # insert new velocity, which is wrong
@ -168,7 +172,7 @@ def IMU_example():
ISAM2_plot(result) ISAM2_plot(result)
# reset # reset
newgraph = gtsam.NonlinearFactorGraph() newgraph = NonlinearFactorGraph()
initialEstimate.clear() initialEstimate.clear()

5
cython/gtsam/examples/Pose2SLAMExample_g2o.py
View File

@ -53,16 +53,15 @@ graph, initial = gtsam.readG2o(g2oFile, is3D)
assert args.kernel == "none", "Supplied kernel type is not yet implemented" assert args.kernel == "none", "Supplied kernel type is not yet implemented"
# Add prior on the pose having index (key) = 0 # Add prior on the pose having index (key) = 0
graphWithPrior = graph
priorModel = gtsam.noiseModel_Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8)) 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 = gtsam.GaussNewtonParams()
params.setVerbosity("Termination") params.setVerbosity("Termination")
params.setMaxIterations(maxIterations) params.setMaxIterations(maxIterations)
# parameters.setRelativeErrorTol(1e-5) # parameters.setRelativeErrorTol(1e-5)
# Create the optimizer ... # Create the optimizer ...
optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params) optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
# ... and optimize # ... and optimize
result = optimizer.optimize() result = optimizer.optimize()

9
cython/gtsam/examples/Pose3SLAMExample_g2o.py
View File

@ -43,18 +43,17 @@ priorModel = gtsam.noiseModel_Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6,
1e-4, 1e-4, 1e-4)) 1e-4, 1e-4, 1e-4))
print("Adding prior to g2o file ") print("Adding prior to g2o file ")
graphWithPrior = graph
firstKey = initial.keys().at(0) 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 = gtsam.GaussNewtonParams()
params.setVerbosity("Termination") # this will show info about stopping conds 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() result = optimizer.optimize()
print("Optimization complete") print("Optimization complete")
print("initial error = ", graphWithPrior.error(initial)) print("initial error = ", graph.error(initial))
print("final error = ", graphWithPrior.error(result)) print("final error = ", graph.error(result))
if args.output is None: if args.output is None:
print("Final Result:\n{}".format(result)) print("Final Result:\n{}".format(result))

4
cython/gtsam/examples/SFMExample.py
View File

@ -16,7 +16,7 @@ from gtsam.examples import SFMdata
from gtsam.gtsam import (Cal3_S2, DoglegOptimizer, from gtsam.gtsam import (Cal3_S2, DoglegOptimizer,
GenericProjectionFactorCal3_S2, NonlinearFactorGraph, GenericProjectionFactorCal3_S2, NonlinearFactorGraph,
Point3, Pose3, PriorFactorPoint3, PriorFactorPose3, Point3, Pose3, PriorFactorPoint3, PriorFactorPose3,
Rot3, SimpleCamera, Values) Rot3, PinholeCameraCal3_S2, Values)
def symbol(name: str, index: int) -> int: 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 # Simulated measurements from each camera pose, adding them to the factor graph
for i, pose in enumerate(poses): for i, pose in enumerate(poses):
camera = SimpleCamera(pose, K) camera = PinholeCameraCal3_S2(pose, K)
for j, point in enumerate(points): for j, point in enumerate(points):
measurement = camera.project(point) measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2( factor = GenericProjectionFactorCal3_S2(

4
cython/gtsam/examples/VisualISAMExample.py
View File

@ -18,7 +18,7 @@ from gtsam.examples import SFMdata
from gtsam.gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2, from gtsam.gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2,
NonlinearFactorGraph, NonlinearISAM, Point3, Pose3, NonlinearFactorGraph, NonlinearISAM, Point3, Pose3,
PriorFactorPoint3, PriorFactorPose3, Rot3, PriorFactorPoint3, PriorFactorPose3, Rot3,
SimpleCamera, Values) PinholeCameraCal3_S2, Values)
def symbol(name: str, index: int) -> int: def symbol(name: str, index: int) -> int:
@ -54,7 +54,7 @@ def main():
# Loop over the different poses, adding the observations to iSAM incrementally # Loop over the different poses, adding the observations to iSAM incrementally
for i, pose in enumerate(poses): for i, pose in enumerate(poses):
camera = SimpleCamera(pose, K) camera = PinholeCameraCal3_S2(pose, K)
# Add factors for each landmark observation # Add factors for each landmark observation
for j, point in enumerate(points): for j, point in enumerate(points):
measurement = camera.project(point) measurement = camera.project(point)

8
cython/gtsam/tests/test_SimpleCamera.py
View File

@ -5,7 +5,7 @@ All Rights Reserved
See LICENSE for the license information See LICENSE for the license information
SimpleCamera unit tests. PinholeCameraCal3_S2 unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python) Author: Frank Dellaert & Duy Nguyen Ta (Python)
""" """
import math import math
@ -14,7 +14,7 @@ import unittest
import numpy as np import numpy as np
import gtsam 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 from gtsam.utils.test_case import GtsamTestCase
K = Cal3_S2(625, 625, 0, 0, 0) K = Cal3_S2(625, 625, 0, 0, 0)
@ -23,14 +23,14 @@ class TestSimpleCamera(GtsamTestCase):
def test_constructor(self): def test_constructor(self):
pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5)) 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.calibration(), K, 1e-9)
self.gtsamAssertEquals(camera.pose(), pose1, 1e-9) self.gtsamAssertEquals(camera.pose(), pose1, 1e-9)
def test_level2(self): def test_level2(self):
# Create a level camera, looking in Y-direction # Create a level camera, looking in Y-direction
pose2 = Pose2(0.4,0.3,math.pi/2.0) 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 # expected
x = Point3(1,0,0) x = Point3(1,0,0)

41
cython/gtsam/utils/visual_data_generator.py
View File

@ -1,8 +1,9 @@
from __future__ import print_function from __future__ import print_function
import numpy as np import numpy as np
from math import pi, cos, sin
import gtsam import gtsam
from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point2, Point3, Pose3
class Options: class Options:
@ -10,7 +11,7 @@ class Options:
Options to generate test scenario 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 Options to generate test scenario
@param triangle: generate a triangle scene with 3 points if True, otherwise @param triangle: generate a triangle scene with 3 points if True, otherwise
@ -27,10 +28,10 @@ class GroundTruth:
Object holding generated ground-truth data 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.K = K
self.cameras = [gtsam.Pose3()] * nrCameras self.cameras = [Pose3()] * nrCameras
self.points = [gtsam.Point3()] * nrPoints self.points = [Point3()] * nrPoints
def print_(self, s=""): def print_(self, s=""):
print(s) print(s)
@ -52,11 +53,11 @@ class Data:
class NoiseModels: class NoiseModels:
pass 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.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.J = [x[:] for x in [[0] * nrPoints] * nrCameras]
self.odometry = [gtsam.Pose3()] * nrCameras self.odometry = [Pose3()] * nrCameras
# Set Noise parameters # Set Noise parameters
self.noiseModels = Data.NoiseModels() self.noiseModels = Data.NoiseModels()
@ -73,7 +74,7 @@ class Data:
def generate_data(options): def generate_data(options):
""" Generate ground-truth and measurement data. """ """ 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 nrPoints = 3 if options.triangle else 8
truth = GroundTruth(K=K, nrCameras=options.nrCameras, nrPoints=nrPoints) truth = GroundTruth(K=K, nrCameras=options.nrCameras, nrPoints=nrPoints)
@ -83,25 +84,25 @@ def generate_data(options):
if options.triangle: # Create a triangle target, just 3 points on a plane if options.triangle: # Create a triangle target, just 3 points on a plane
r = 10 r = 10
for j in range(len(truth.points)): for j in range(len(truth.points)):
theta = j * 2 * pi / nrPoints theta = j * 2 * np.pi / nrPoints
truth.points[j] = gtsam.Point3(r * cos(theta), r * sin(theta), 0) truth.points[j] = Point3(r * np.cos(theta), r * np.sin(theta), 0)
else: # 3D landmarks as vertices of a cube else: # 3D landmarks as vertices of a cube
truth.points = [ truth.points = [
gtsam.Point3(10, 10, 10), gtsam.Point3(-10, 10, 10), Point3(10, 10, 10), Point3(-10, 10, 10),
gtsam.Point3(-10, -10, 10), gtsam.Point3(10, -10, 10), Point3(-10, -10, 10), Point3(10, -10, 10),
gtsam.Point3(10, 10, -10), gtsam.Point3(-10, 10, -10), Point3(10, 10, -10), Point3(-10, 10, -10),
gtsam.Point3(-10, -10, -10), gtsam.Point3(10, -10, -10) Point3(-10, -10, -10), Point3(10, -10, -10)
] ]
# Create camera cameras on a circle around the triangle # Create camera cameras on a circle around the triangle
height = 10 height = 10
r = 40 r = 40
for i in range(options.nrCameras): for i in range(options.nrCameras):
theta = i * 2 * pi / options.nrCameras theta = i * 2 * np.pi / options.nrCameras
t = gtsam.Point3(r * cos(theta), r * sin(theta), height) t = Point3(r * np.cos(theta), r * np.sin(theta), height)
truth.cameras[i] = gtsam.SimpleCamera.Lookat(t, truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t,
gtsam.Point3(), Point3(),
gtsam.Point3(0, 0, 1), Point3(0, 0, 1),
truth.K) truth.K)
# Create measurements # Create measurements
for j in range(nrPoints): for j in range(nrPoints):

128
cython/gtsam_unstable/examples/TimeOfArrivalExample.py Normal file
View File

@ -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")

5
examples/CameraResectioning.cpp
View File

@ -18,7 +18,8 @@
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
using namespace gtsam; using namespace gtsam;
@ -47,7 +48,7 @@ public:
/// evaluate the error /// evaluate the error
virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H = virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
boost::none) const { boost::none) const {
SimpleCamera camera(pose, *K_); PinholeCamera<Cal3_S2> camera(pose, *K_);
return camera.project(P_, H, boost::none, boost::none) - p_; return camera.project(P_, H, boost::none, boost::none) - p_;
} }
}; };

9
examples/Data/pose3Localizationexample.txt Normal file
View File

@ -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

3
examples/ISAM2Example_SmartFactor.cpp
View File

@ -4,7 +4,8 @@
* @author Alexander (pumaking on BitBucket) * @author Alexander (pumaking on BitBucket)
*/ */
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/nonlinear/ISAM2.h> #include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/SmartProjectionPoseFactor.h> #include <gtsam/slam/SmartProjectionPoseFactor.h>

5
examples/ImuFactorExample2.cpp
View File

@ -1,5 +1,6 @@
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/ImuBias.h> #include <gtsam/navigation/ImuBias.h>
#include <gtsam/navigation/ImuFactor.h> #include <gtsam/navigation/ImuFactor.h>
@ -34,7 +35,7 @@ int main(int argc, char* argv[]) {
double radius = 30; double radius = 30;
const Point3 up(0, 0, 1), target(0, 0, 0); const Point3 up(0, 0, 1), target(0, 0, 0);
const Point3 position(radius, 0, 0); const Point3 position(radius, 0, 0);
const SimpleCamera camera = SimpleCamera::Lookat(position, target, up); const auto camera = PinholeCamera<Cal3_S2>::Lookat(position, target, up);
const auto pose_0 = camera.pose(); const auto pose_0 = camera.pose();
// Now, create a constant-twist scenario that makes the camera orbit the // Now, create a constant-twist scenario that makes the camera orbit the

5
examples/Pose2SLAMExample_g2o.cpp
View File

@ -63,10 +63,9 @@ int main(const int argc, const char *argv[]) {
} }
// Add prior on the pose having index (key) = 0 // Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8)); noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel)); graph->add(PriorFactor<Pose2>(0, Pose2(), priorModel));
std::cout << "Adding prior on pose 0 " << std::endl; std::cout << "Adding prior on pose 0 " << std::endl;
GaussNewtonParams params; GaussNewtonParams params;
@ -77,7 +76,7 @@ int main(const int argc, const char *argv[]) {
} }
std::cout << "Optimizing the factor graph" << std::endl; std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params); GaussNewtonOptimizer optimizer(*graph, *initial, params);
Values result = optimizer.optimize(); Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl; std::cout << "Optimization complete" << std::endl;

12
examples/Pose2SLAMExample_lago.cpp
View File

@ -42,14 +42,13 @@ int main(const int argc, const char *argv[]) {
boost::tie(graph, initial) = readG2o(g2oFile); boost::tie(graph, initial) = readG2o(g2oFile);
// Add prior on the pose having index (key) = 0 // Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8)); noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel)); graph->add(PriorFactor<Pose2>(0, Pose2(), priorModel));
graphWithPrior.print(); graph->print();
std::cout << "Computing LAGO estimate" << std::endl; std::cout << "Computing LAGO estimate" << std::endl;
Values estimateLago = lago::initialize(graphWithPrior); Values estimateLago = lago::initialize(*graph);
std::cout << "done!" << std::endl; std::cout << "done!" << std::endl;
if (argc < 3) { if (argc < 3) {
@ -57,7 +56,10 @@ int main(const int argc, const char *argv[]) {
} else { } else {
const string outputFile = argv[2]; const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl; 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; std::cout << "done! " << std::endl;
} }

85
examples/Pose3Localization.cpp Normal file
View File

@ -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 <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <fstream>
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<Pose3>(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=" <<graph->error(*initial)<< std::endl;
std::cout << "final error=" <<graph->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<const GenericValue<Pose3>*>(&key_value.value);
if (!p) continue;
std::cout << marginals.marginalCovariance(key_value.key) << endl;
}
return 0;
}

10
examples/Pose3SLAMExample_g2o.cpp
View File

@ -41,21 +41,20 @@ int main(const int argc, const char *argv[]) {
boost::tie(graph, initial) = readG2o(g2oFile, is3D); boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key // Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0; Key firstKey = 0;
for(const Values::ConstKeyValuePair& key_value: *initial) { for(const Values::ConstKeyValuePair& key_value: *initial) {
std::cout << "Adding prior to g2o file " << std::endl; std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key; firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel)); graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
break; break;
} }
std::cout << "Optimizing the factor graph" << std::endl; std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonParams params; GaussNewtonParams params;
params.setVerbosity("TERMINATION"); // this will show info about stopping conditions params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params); GaussNewtonOptimizer optimizer(*graph, *initial, params);
Values result = optimizer.optimize(); Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl; std::cout << "Optimization complete" << std::endl;
@ -67,7 +66,10 @@ int main(const int argc, const char *argv[]) {
} else { } else {
const string outputFile = argv[2]; const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl; 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; std::cout << "done! " << std::endl;
} }
return 0; return 0;

10
examples/Pose3SLAMExample_initializePose3Chordal.cpp
View File

@ -41,19 +41,18 @@ int main(const int argc, const char *argv[]) {
boost::tie(graph, initial) = readG2o(g2oFile, is3D); boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key // Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0; Key firstKey = 0;
for(const Values::ConstKeyValuePair& key_value: *initial) { for(const Values::ConstKeyValuePair& key_value: *initial) {
std::cout << "Adding prior to g2o file " << std::endl; std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key; firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel)); graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
break; break;
} }
std::cout << "Initializing Pose3 - chordal relaxation" << std::endl; std::cout << "Initializing Pose3 - chordal relaxation" << std::endl;
Values initialization = InitializePose3::initialize(graphWithPrior); Values initialization = InitializePose3::initialize(*graph);
std::cout << "done!" << std::endl; std::cout << "done!" << std::endl;
if (argc < 3) { if (argc < 3) {
@ -61,7 +60,10 @@ int main(const int argc, const char *argv[]) {
} else { } else {
const string outputFile = argv[2]; const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl; 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; std::cout << "done! " << std::endl;
} }
return 0; return 0;

10
examples/Pose3SLAMExample_initializePose3Gradient.cpp
View File

@ -41,20 +41,19 @@ int main(const int argc, const char *argv[]) {
boost::tie(graph, initial) = readG2o(g2oFile, is3D); boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key // Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0; Key firstKey = 0;
for(const Values::ConstKeyValuePair& key_value: *initial) { for(const Values::ConstKeyValuePair& key_value: *initial) {
std::cout << "Adding prior to g2o file " << std::endl; std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key; firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel)); graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
break; break;
} }
std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl; std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl;
bool useGradient = true; bool useGradient = true;
Values initialization = InitializePose3::initialize(graphWithPrior, *initial, useGradient); Values initialization = InitializePose3::initialize(*graph, *initial, useGradient);
std::cout << "done!" << std::endl; std::cout << "done!" << std::endl;
std::cout << "initial error=" <<graph->error(*initial)<< std::endl; std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
@ -65,7 +64,10 @@ int main(const int argc, const char *argv[]) {
} else { } else {
const string outputFile = argv[2]; const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl; 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; std::cout << "done! " << std::endl;
} }
return 0; return 0;

2
examples/SFMExample.cpp
View File

@ -79,7 +79,7 @@ int main(int argc, char* argv[]) {
// Simulated measurements from each camera pose, adding them to the factor graph // Simulated measurements from each camera pose, adding them to the factor graph
for (size_t i = 0; i < poses.size(); ++i) { for (size_t i = 0; i < poses.size(); ++i) {
SimpleCamera camera(poses[i], *K); PinholeCamera<Cal3_S2> camera(poses[i], *K);
for (size_t j = 0; j < points.size(); ++j) { for (size_t j = 0; j < points.size(); ++j) {
Point2 measurement = camera.project(points[j]); Point2 measurement = camera.project(points[j]);
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);

4
examples/SFMExampleExpressions.cpp
View File

@ -27,7 +27,7 @@
#include <gtsam/nonlinear/ExpressionFactorGraph.h> #include <gtsam/nonlinear/ExpressionFactorGraph.h>
// Header order is close to far // Header order is close to far
#include <examples/SFMdata.h> #include "SFMdata.h"
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/DoglegOptimizer.h> #include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
@ -67,7 +67,7 @@ int main(int argc, char* argv[]) {
// Simulated measurements from each camera pose, adding them to the factor graph // Simulated measurements from each camera pose, adding them to the factor graph
for (size_t i = 0; i < poses.size(); ++i) { for (size_t i = 0; i < poses.size(); ++i) {
Pose3_ x('x', i); Pose3_ x('x', i);
SimpleCamera camera(poses[i], K); PinholeCamera<Cal3_S2> camera(poses[i], K);
for (size_t j = 0; j < points.size(); ++j) { for (size_t j = 0; j < points.size(); ++j) {
Point2 measurement = camera.project(points[j]); Point2 measurement = camera.project(points[j]);
// Below an expression for the prediction of the measurement: // Below an expression for the prediction of the measurement:

2
examples/SFMExample_SmartFactor.cpp
View File

@ -117,7 +117,7 @@ int main(int argc, char* argv[]) {
// The output of point() is in boost::optional<Point3>, as sometimes // The output of point() is in boost::optional<Point3>, as sometimes
// the triangulation operation inside smart factor will encounter degeneracy. // the triangulation operation inside smart factor will encounter degeneracy.
boost::optional<Point3> point = smart->point(result); boost::optional<Point3> 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); landmark_result.insert(j, *point);
} }
} }

2
examples/SFMExample_SmartFactorPCG.cpp
View File

@ -114,7 +114,7 @@ int main(int argc, char* argv[]) {
boost::dynamic_pointer_cast<SmartFactor>(graph[j]); boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
if (smart) { if (smart) {
boost::optional<Point3> point = smart->point(result); boost::optional<Point3> 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); landmark_result.insert(j, *point);
} }
} }

5
examples/SFMdata.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file SFMMdata.h * @file SFMdata.h
* @brief Simple example for the structure-from-motion problems * @brief Simple example for the structure-from-motion problems
* @author Duy-Nguyen Ta * @author Duy-Nguyen Ta
*/ */
@ -30,7 +30,8 @@
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
// We will also need a camera object to hold calibration information and perform projections. // We will also need a camera object to hold calibration information and perform projections.
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
/* ************************************************************************* */ /* ************************************************************************* */
std::vector<gtsam::Point3> createPoints() { std::vector<gtsam::Point3> createPoints() {

2
examples/SelfCalibrationExample.cpp
View File

@ -61,7 +61,7 @@ int main(int argc, char* argv[]) {
noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
for (size_t i = 0; i < poses.size(); ++i) { for (size_t i = 0; i < poses.size(); ++i) {
for (size_t j = 0; j < points.size(); ++j) { for (size_t j = 0; j < points.size(); ++j) {
SimpleCamera camera(poses[i], K); PinholeCamera<Cal3_S2> camera(poses[i], K);
Point2 measurement = camera.project(points[j]); Point2 measurement = camera.project(points[j]);
// The only real difference with the Visual SLAM example is that here we use a // 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 // different factor type, that also calculates the Jacobian with respect to calibration

2
examples/VisualISAM2Example.cpp
View File

@ -89,7 +89,7 @@ int main(int argc, char* argv[]) {
for (size_t i = 0; i < poses.size(); ++i) { for (size_t i = 0; i < poses.size(); ++i) {
// Add factors for each landmark observation // Add factors for each landmark observation
for (size_t j = 0; j < points.size(); ++j) { for (size_t j = 0; j < points.size(); ++j) {
SimpleCamera camera(poses[i], *K); PinholeCamera<Cal3_S2> camera(poses[i], *K);
Point2 measurement = camera.project(points[j]); Point2 measurement = camera.project(points[j]);
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >( graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(
measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);

2
examples/VisualISAMExample.cpp
View File

@ -86,7 +86,7 @@ int main(int argc, char* argv[]) {
// Add factors for each landmark observation // Add factors for each landmark observation
for (size_t j = 0; j < points.size(); ++j) { for (size_t j = 0; j < points.size(); ++j) {
// Create ground truth measurement // Create ground truth measurement
SimpleCamera camera(poses[i], *K); PinholeCamera<Cal3_S2> camera(poses[i], *K);
Point2 measurement = camera.project(points[j]); Point2 measurement = camera.project(points[j]);
// Add measurement // Add measurement
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, noise, graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, noise,

22
gtsam.h
View File

@ -639,6 +639,7 @@ class Rot3 {
static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft) 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 Ypr(double y, double p, double r);
static gtsam::Rot3 Quaternion(double w, double x, double y, double z); 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(Vector v);
static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); static gtsam::Rot3 Rodrigues(double wx, double wy, double wz);
static gtsam::Rot3 ClosestTo(const Matrix M); static gtsam::Rot3 ClosestTo(const Matrix M);
@ -674,6 +675,7 @@ class Rot3 {
double roll() const; double roll() const;
double pitch() const; double pitch() const;
double yaw() const; double yaw() const;
pair<gtsam::Unit3, double> axisAngle() const;
// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly // Vector toQuaternion() const; // FIXME: Can't cast to Vector properly
Vector quaternion() const; Vector quaternion() const;
gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const; gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const;
@ -1310,7 +1312,7 @@ class SymbolicBayesTree {
// class SymbolicBayesTreeClique { // class SymbolicBayesTreeClique {
// BayesTreeClique(); // BayesTreeClique();
// BayesTreeClique(CONDITIONAL* conditional); // BayesTreeClique(CONDITIONAL* conditional);
// // BayesTreeClique(const std::pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {} // // BayesTreeClique(const pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
// //
// bool equals(const This& other, double tol) const; // bool equals(const This& other, double tol) const;
// void print(string s) const; // void print(string s) const;
@ -1564,14 +1566,12 @@ class Sampler {
// Constructors // Constructors
Sampler(gtsam::noiseModel::Diagonal* model, int seed); Sampler(gtsam::noiseModel::Diagonal* model, int seed);
Sampler(Vector sigmas, int seed); Sampler(Vector sigmas, int seed);
Sampler(int seed);
// Standard Interface // Standard Interface
size_t dim() const; size_t dim() const;
Vector sigmas() const; Vector sigmas() const;
gtsam::noiseModel::Diagonal* model() const; gtsam::noiseModel::Diagonal* model() const;
Vector sample(); Vector sample();
Vector sampleNewModel(gtsam::noiseModel::Diagonal* model);
}; };
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
@ -2136,7 +2136,7 @@ class Values {
void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); 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::Cal3Bundler& cal3bundler);
void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); 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, const gtsam::imuBias::ConstantBias& constant_bias);
void insert(size_t j, Vector vector); void insert(size_t j, Vector vector);
void insert(size_t j, Matrix matrix); void insert(size_t j, Matrix matrix);
@ -2457,7 +2457,7 @@ class ISAM2 {
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3, template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::Cal3Bundler, gtsam::EssentialMatrix,
gtsam::SimpleCamera, Vector, Matrix}> gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, Vector, Matrix}>
VALUE calculateEstimate(size_t key) const; VALUE calculateEstimate(size_t key) const;
gtsam::Values calculateBestEstimate() const; gtsam::Values calculateBestEstimate() const;
Matrix marginalCovariance(size_t key) const; Matrix marginalCovariance(size_t key) const;
@ -2495,7 +2495,7 @@ class NonlinearISAM {
#include <gtsam/geometry/StereoPoint2.h> #include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}> template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
virtual class PriorFactor : gtsam::NoiseModelFactor { virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const; T prior() const;
@ -2518,7 +2518,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
#include <gtsam/nonlinear/NonlinearEquality.h> #include <gtsam/nonlinear/NonlinearEquality.h>
template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}> template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
virtual class NonlinearEquality : gtsam::NoiseModelFactor { virtual class NonlinearEquality : gtsam::NoiseModelFactor {
// Constructor - forces exact evaluation // Constructor - forces exact evaluation
NonlinearEquality(size_t j, const T& feasible); NonlinearEquality(size_t j, const T& feasible);
@ -2544,9 +2544,9 @@ typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D;
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2; typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3; typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint; typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint; typedef gtsam::RangeFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> RangeFactorSimpleCameraPoint;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera; typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera; typedef gtsam::RangeFactor<gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3_S2> RangeFactorSimpleCamera;
#include <gtsam/sam/RangeFactor.h> #include <gtsam/sam/RangeFactor.h>
@ -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); GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey);
gtsam::Point2 measured() const; gtsam::Point2 measured() const;
}; };
typedef gtsam::GeneralSFMFactor<gtsam::SimpleCamera, gtsam::Point3> GeneralSFMFactorCal3_S2; typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
// due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2 // due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2; //typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
@ -3151,7 +3151,7 @@ namespace utilities {
void perturbPoint2(gtsam::Values& values, double sigma, int seed); void perturbPoint2(gtsam::Values& values, double sigma, int seed);
void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed); void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed);
void perturbPoint3(gtsam::Values& values, double sigma, 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);
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); 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); Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values);

26
gtsam/base/OptionalJacobian.h
View File

@ -55,7 +55,7 @@ private:
} }
// Private and very dangerous constructor straight from memory // Private and very dangerous constructor straight from memory
OptionalJacobian(double* data) : map_(NULL) { OptionalJacobian(double* data) : map_(nullptr) {
if (data) usurp(data); if (data) usurp(data);
} }
@ -66,25 +66,25 @@ public:
/// Default constructor acts like boost::none /// Default constructor acts like boost::none
OptionalJacobian() : OptionalJacobian() :
map_(NULL) { map_(nullptr) {
} }
/// Constructor that will usurp data of a fixed-size matrix /// Constructor that will usurp data of a fixed-size matrix
OptionalJacobian(Jacobian& fixed) : OptionalJacobian(Jacobian& fixed) :
map_(NULL) { map_(nullptr) {
usurp(fixed.data()); usurp(fixed.data());
} }
/// Constructor that will usurp data of a fixed-size matrix, pointer version /// Constructor that will usurp data of a fixed-size matrix, pointer version
OptionalJacobian(Jacobian* fixedPtr) : OptionalJacobian(Jacobian* fixedPtr) :
map_(NULL) { map_(nullptr) {
if (fixedPtr) if (fixedPtr)
usurp(fixedPtr->data()); usurp(fixedPtr->data());
} }
/// Constructor that will resize a dynamic matrix (unless already correct) /// Constructor that will resize a dynamic matrix (unless already correct)
OptionalJacobian(Eigen::MatrixXd& dynamic) : OptionalJacobian(Eigen::MatrixXd& dynamic) :
map_(NULL) { map_(nullptr) {
dynamic.resize(Rows, Cols); // no malloc if correct size dynamic.resize(Rows, Cols); // no malloc if correct size
usurp(dynamic.data()); usurp(dynamic.data());
} }
@ -93,12 +93,12 @@ public:
/// Constructor with boost::none just makes empty /// Constructor with boost::none just makes empty
OptionalJacobian(boost::none_t /*none*/) : OptionalJacobian(boost::none_t /*none*/) :
map_(NULL) { map_(nullptr) {
} }
/// Constructor compatible with old-style derivatives /// Constructor compatible with old-style derivatives
OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) : OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
map_(NULL) { map_(nullptr) {
if (optional) { if (optional) {
optional->resize(Rows, Cols); optional->resize(Rows, Cols);
usurp(optional->data()); usurp(optional->data());
@ -110,11 +110,11 @@ public:
/// Constructor that will usurp data of a block expression /// Constructor that will usurp data of a block expression
/// TODO(frank): unfortunately using a Map makes usurping non-contiguous memory impossible /// TODO(frank): unfortunately using a Map makes usurping non-contiguous memory impossible
// template <typename Derived, bool InnerPanel> // template <typename Derived, bool InnerPanel>
// OptionalJacobian(Eigen::Block<Derived,Rows,Cols,InnerPanel> block) : map_(NULL) { ?? } // OptionalJacobian(Eigen::Block<Derived,Rows,Cols,InnerPanel> block) : map_(nullptr) { ?? }
/// Return true is allocated, false if default constructor was used /// Return true is allocated, false if default constructor was used
operator bool() const { operator bool() const {
return map_.data() != NULL; return map_.data() != nullptr;
} }
/// De-reference, like boost optional /// De-reference, like boost optional
@ -173,7 +173,7 @@ public:
/// Default constructor acts like boost::none /// Default constructor acts like boost::none
OptionalJacobian() : OptionalJacobian() :
pointer_(NULL) { pointer_(nullptr) {
} }
/// Construct from pointer to dynamic matrix /// Construct from pointer to dynamic matrix
@ -186,12 +186,12 @@ public:
/// Constructor with boost::none just makes empty /// Constructor with boost::none just makes empty
OptionalJacobian(boost::none_t /*none*/) : OptionalJacobian(boost::none_t /*none*/) :
pointer_(NULL) { pointer_(nullptr) {
} }
/// Constructor compatible with old-style derivatives /// Constructor compatible with old-style derivatives
OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) : OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
pointer_(NULL) { pointer_(nullptr) {
if (optional) pointer_ = &(*optional); if (optional) pointer_ = &(*optional);
} }
@ -199,7 +199,7 @@ public:
/// Return true is allocated, false if default constructor was used /// Return true is allocated, false if default constructor was used
operator bool() const { operator bool() const {
return pointer_!=NULL; return pointer_!=nullptr;
} }
/// De-reference, like boost optional /// De-reference, like boost optional

12
gtsam/base/treeTraversal/parallelTraversalTasks.h
View File

@ -53,7 +53,7 @@ namespace gtsam {
// Run the post-order visitor // Run the post-order visitor
(void) visitorPost(treeNode, *myData); (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 // Run the post-order visitor since this task was recycled to run the post-order visitor
(void) visitorPost(treeNode, *myData); (void) visitorPost(treeNode, *myData);
return NULL; return nullptr;
} }
else else
{ {
@ -129,14 +129,14 @@ namespace gtsam {
{ {
// Run the post-order visitor in this task if we have no children // Run the post-order visitor in this task if we have no children
(void) visitorPost(treeNode, *myData); (void) visitorPost(treeNode, *myData);
return NULL; return nullptr;
} }
} }
else else
{ {
// Process this node and its children in this task // Process this node and its children in this task
processNodeRecursively(treeNode, *myData); processNodeRecursively(treeNode, *myData);
return NULL; return nullptr;
} }
} }
} }
@ -184,8 +184,8 @@ namespace gtsam {
set_ref_count(1 + (int) roots.size()); set_ref_count(1 + (int) roots.size());
// Spawn tasks // Spawn tasks
spawn_and_wait_for_all(tasks); spawn_and_wait_for_all(tasks);
// Return NULL // Return nullptr
return NULL; return nullptr;
} }
}; };

2
gtsam/discrete/AlgebraicDecisionTree.h
View File

@ -101,7 +101,7 @@ namespace gtsam {
/** Create a new function splitting on a variable */ /** Create a new function splitting on a variable */
template<typename Iterator> template<typename Iterator>
AlgebraicDecisionTree(Iterator begin, Iterator end, const L& label) : AlgebraicDecisionTree(Iterator begin, Iterator end, const L& label) :
Super(NULL) { Super(nullptr) {
this->root_ = compose(begin, end, label); this->root_ = compose(begin, end, label);
} }

2
gtsam/discrete/DiscreteFactorGraph.cpp
View File

@ -71,7 +71,7 @@ namespace gtsam {
for (size_t i = 0; i < factors_.size(); i++) { for (size_t i = 0; i < factors_.size(); i++) {
std::stringstream ss; std::stringstream ss;
ss << "factor " << i << ": "; ss << "factor " << i << ": ";
if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter); if (factors_[i] != nullptr) factors_[i]->print(ss.str(), formatter);
} }
} }

4
gtsam/geometry/BearingRange.h
View File

@ -187,8 +187,8 @@ struct HasBearing {
}; };
// Similar helper class for to implement Range traits for classes with a range method // 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: // For classes with overloaded range methods, such as PinholeCamera, this can even be templated:
// template <typename T> struct Range<SimpleCamera, T> : HasRange<SimpleCamera, T, double> {}; // template <typename T> struct Range<PinholeCamera, T> : HasRange<PinholeCamera, T, double> {};
template <class A1, typename A2, class RT> template <class A1, typename A2, class RT>
struct HasRange { struct HasRange {
typedef RT result_type; typedef RT result_type;

8
gtsam/geometry/Rot3.cpp
View File

@ -16,6 +16,7 @@
* @author Christian Potthast * @author Christian Potthast
* @author Frank Dellaert * @author Frank Dellaert
* @author Richard Roberts * @author Richard Roberts
* @author Varun Agrawal
*/ */
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
@ -36,7 +37,6 @@ void Rot3::print(const std::string& s) const {
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::Random(std::mt19937& rng) { Rot3 Rot3::Random(std::mt19937& rng) {
// TODO allow any engine without including all of boost :-(
Unit3 axis = Unit3::Random(rng); Unit3 axis = Unit3::Random(rng);
uniform_real_distribution<double> randomAngle(-M_PI, M_PI); uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
double angle = randomAngle(rng); double angle = randomAngle(rng);
@ -185,6 +185,12 @@ Vector Rot3::quaternion() const {
return v; return v;
} }
/* ************************************************************************* */
pair<Unit3, double> Rot3::axisAngle() const {
const Vector3 omega = Rot3::Logmap(*this);
return std::pair<Unit3, double>(Unit3(omega), omega.norm());
}
/* ************************************************************************* */ /* ************************************************************************* */
Matrix3 Rot3::ExpmapDerivative(const Vector3& x) { Matrix3 Rot3::ExpmapDerivative(const Vector3& x) {
return SO3::ExpmapDerivative(x); return SO3::ExpmapDerivative(x);

36
gtsam/geometry/Rot3.h
View File

@ -17,6 +17,7 @@
* @author Frank Dellaert * @author Frank Dellaert
* @author Richard Roberts * @author Richard Roberts
* @author Luca Carlone * @author Luca Carlone
* @author Varun Agrawal
*/ */
// \callgraph // \callgraph
@ -194,15 +195,17 @@ namespace gtsam {
/** /**
* Convert from axis/angle representation * Convert from axis/angle representation
* @param axisw is the rotation axis, unit length * @param axis is the rotation axis, unit length
* @param angle rotation angle * @param angle rotation angle
* @return incremental rotation * @return incremental rotation
*/ */
static Rot3 AxisAngle(const Point3& axis, double angle) { static Rot3 AxisAngle(const Point3& axis, double angle) {
// Convert to unit vector.
Vector3 unitAxis = Unit3(axis).unitVector();
#ifdef GTSAM_USE_QUATERNIONS #ifdef GTSAM_USE_QUATERNIONS
return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, axis)); return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, unitAxis));
#else #else
return Rot3(SO3::AxisAngle(axis,angle)); return Rot3(SO3::AxisAngle(unitAxis,angle));
#endif #endif
} }
@ -268,9 +271,13 @@ namespace gtsam {
/// Syntatic sugar for composing two rotations /// Syntatic sugar for composing two rotations
Rot3 operator*(const Rot3& R2) const; Rot3 operator*(const Rot3& R2) const;
/// inverse of a rotation, TODO should be different for M/Q /// inverse of a rotation
Rot3 inverse() const { 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 * Return 3*3 transpose (inverse) rotation matrix
*/ */
Matrix3 transpose() const; Matrix3 transpose() const;
// TODO: const Eigen::Transpose<const Matrix3> transpose() const;
/// @deprecated, this is base 1, and was just confusing /// @deprecated, this is base 1, and was just confusing
Point3 column(int index) const; Point3 column(int index) const;
@ -429,7 +435,7 @@ namespace gtsam {
/** /**
* Use RQ to calculate roll-pitch-yaw angle representation * 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; Vector3 rpy() const;
@ -439,7 +445,7 @@ namespace gtsam {
* you should instead use xyz() or ypr() * you should instead use xyz() or ypr()
* TODO: make this more efficient * 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 * Accessor to get to component of angle representations
@ -447,7 +453,7 @@ namespace gtsam {
* you should instead use xyz() or ypr() * you should instead use xyz() or ypr()
* TODO: make this more efficient * 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 * Accessor to get to component of angle representations
@ -455,12 +461,22 @@ namespace gtsam {
* you should instead use xyz() or ypr() * you should instead use xyz() or ypr()
* TODO: make this more efficient * TODO: make this more efficient
*/ */
inline double yaw() const { return ypr()(0); } inline double yaw() const { return xyz()(2); }
/// @} /// @}
/// @name Advanced Interface /// @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<Unit3, double> axisAngle() const;
/** Compute the quaternion representation of this rotation. /** Compute the quaternion representation of this rotation.
* @return The quaternion * @return The quaternion
*/ */

1
gtsam/geometry/Rot3M.cpp
View File

@ -110,7 +110,6 @@ Rot3 Rot3::operator*(const Rot3& R2) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// TODO const Eigen::Transpose<const Matrix3> Rot3::transpose() const {
Matrix3 Rot3::transpose() const { Matrix3 Rot3::transpose() const {
return rot_.matrix().transpose(); return rot_.matrix().transpose();
} }

12
gtsam/geometry/Rot3Q.cpp
View File

@ -19,8 +19,8 @@
#ifdef GTSAM_USE_QUATERNIONS #ifdef GTSAM_USE_QUATERNIONS
#include <boost/math/constants/constants.hpp>
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp>
#include <cmath> #include <cmath>
using namespace std; using namespace std;
@ -79,9 +79,13 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// TODO: Could we do this? It works in Rot3M but not here, probably because // TODO: Maybe use return type `const Eigen::Transpose<const Matrix3>`?
// here we create an intermediate value by calling matrix() // It works in Rot3M but not here, because of some weird behavior
// const Eigen::Transpose<const Matrix3> Rot3::transpose() const { // 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 { Matrix3 Rot3::transpose() const {
return matrix().transpose(); return matrix().transpose();
} }

2
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -159,7 +159,7 @@ TEST (EssentialMatrix, rotate) {
Matrix actH1, actH2; Matrix actH1, actH2;
try { try {
bodyE.rotate(cRb, actH1, actH2); bodyE.rotate(cRb, actH1, actH2);
} catch (exception e) { } catch (exception& e) {
} // avoid exception } // avoid exception
Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), // Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), //
expH2 = numericalDerivative22(rotate_, bodyE, cRb); expH2 = numericalDerivative22(rotate_, bodyE, cRb);

2
gtsam/geometry/tests/testOrientedPlane3.cpp
View File

@ -183,7 +183,7 @@ TEST (OrientedPlane3, jacobian_retract) {
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
srand(time(NULL)); srand(time(nullptr));
TestResult tr; TestResult tr;
return TestRegistry::runAllTests(tr); return TestRegistry::runAllTests(tr);
} }

7
gtsam/geometry/tests/testPose3.cpp
View File

@ -1008,7 +1008,14 @@ TEST(Pose3, print) {
// Generate the expected output // Generate the expected output
std::stringstream expected; std::stringstream expected;
Point3 translation(1, 2, 3); 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() << "]\';"; expected << '[' << translation.x() << ", " << translation.y() << ", " << translation.z() << "]\';";
#endif
// reset cout to the original stream // reset cout to the original stream
std::cout.rdbuf(oldbuf); std::cout.rdbuf(oldbuf);

66
gtsam/geometry/tests/testRot3.cpp
View File

@ -14,6 +14,7 @@
* @brief Unit tests for Rot3 class - common between Matrix and Quaternion * @brief Unit tests for Rot3 class - common between Matrix and Quaternion
* @author Alireza Fathi * @author Alireza Fathi
* @author Frank Dellaert * @author Frank Dellaert
* @author Varun Agrawal
*/ */
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
@ -115,6 +116,10 @@ TEST( Rot3, AxisAngle)
CHECK(assert_equal(expected,actual,1e-5)); CHECK(assert_equal(expected,actual,1e-5));
Rot3 actual2 = Rot3::AxisAngle(axis, angle-2*M_PI); Rot3 actual2 = Rot3::AxisAngle(axis, angle-2*M_PI);
CHECK(assert_equal(expected,actual2,1e-5)); 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); Rot3 actual = R.inverse(actualH);
CHECK(assert_equal(I,R*actual)); CHECK(assert_equal(I,R*actual));
CHECK(assert_equal(I,actual*R)); 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<Rot3>, R); Matrix numericalH = numericalDerivative11(testing::inverse<Rot3>, R);
CHECK(assert_equal(numericalH,actualH)); CHECK(assert_equal(numericalH,actualH));
@ -661,6 +666,65 @@ TEST(Rot3, ClosestTo) {
EXPECT(assert_equal(expected, actual.matrix(), 1e-6)); 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() { int main() {
TestResult tr; TestResult tr;

26
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -17,7 +17,7 @@
*/ */
#include <gtsam/geometry/triangulation.h> #include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/StereoCamera.h> #include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/CameraSet.h> #include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/Cal3Bundler.h> #include <gtsam/geometry/Cal3Bundler.h>
@ -151,7 +151,7 @@ TEST( triangulation, fourPoses) {
// 3. Add a slightly rotated third camera above, again with measurement noise // 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)); Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
SimpleCamera camera3(pose3, *sharedCal); PinholeCamera<Cal3_S2> camera3(pose3, *sharedCal);
Point2 z3 = camera3.project(landmark); Point2 z3 = camera3.project(landmark);
poses += pose3; poses += pose3;
@ -168,7 +168,7 @@ TEST( triangulation, fourPoses) {
// 4. Test failure: Add a 4th camera facing the wrong way // 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)); Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
SimpleCamera camera4(pose4, *sharedCal); PinholeCamera<Cal3_S2> camera4(pose4, *sharedCal);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); CHECK_EXCEPTION(camera4.project(landmark), CheiralityException);
@ -185,17 +185,17 @@ TEST( triangulation, fourPoses) {
TEST( triangulation, fourPoses_distinct_Ks) { TEST( triangulation, fourPoses_distinct_Ks) {
Cal3_S2 K1(1500, 1200, 0, 640, 480); Cal3_S2 K1(1500, 1200, 0, 640, 480);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
SimpleCamera camera1(pose1, K1); PinholeCamera<Cal3_S2> camera1(pose1, K1);
// create second camera 1 meter to the right of first camera // create second camera 1 meter to the right of first camera
Cal3_S2 K2(1600, 1300, 0, 650, 440); Cal3_S2 K2(1600, 1300, 0, 650, 440);
SimpleCamera camera2(pose2, K2); PinholeCamera<Cal3_S2> camera2(pose2, K2);
// 1. Project two landmarks into two cameras and triangulate // 1. Project two landmarks into two cameras and triangulate
Point2 z1 = camera1.project(landmark); Point2 z1 = camera1.project(landmark);
Point2 z2 = camera2.project(landmark); Point2 z2 = camera2.project(landmark);
CameraSet<SimpleCamera> cameras; CameraSet<PinholeCamera<Cal3_S2> > cameras;
Point2Vector measurements; Point2Vector measurements;
cameras += camera1, camera2; cameras += camera1, camera2;
@ -216,7 +216,7 @@ TEST( triangulation, fourPoses_distinct_Ks) {
// 3. Add a slightly rotated third camera above, again with measurement noise // 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)); 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); Cal3_S2 K3(700, 500, 0, 640, 480);
SimpleCamera camera3(pose3, K3); PinholeCamera<Cal3_S2> camera3(pose3, K3);
Point2 z3 = camera3.project(landmark); Point2 z3 = camera3.project(landmark);
cameras += camera3; cameras += camera3;
@ -234,7 +234,7 @@ TEST( triangulation, fourPoses_distinct_Ks) {
// 4. Test failure: Add a 4th camera facing the wrong way // 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)); Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
Cal3_S2 K4(700, 500, 0, 640, 480); Cal3_S2 K4(700, 500, 0, 640, 480);
SimpleCamera camera4(pose4, K4); PinholeCamera<Cal3_S2> camera4(pose4, K4);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); CHECK_EXCEPTION(camera4.project(landmark), CheiralityException);
@ -250,17 +250,17 @@ TEST( triangulation, fourPoses_distinct_Ks) {
TEST( triangulation, outliersAndFarLandmarks) { TEST( triangulation, outliersAndFarLandmarks) {
Cal3_S2 K1(1500, 1200, 0, 640, 480); Cal3_S2 K1(1500, 1200, 0, 640, 480);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
SimpleCamera camera1(pose1, K1); PinholeCamera<Cal3_S2> camera1(pose1, K1);
// create second camera 1 meter to the right of first camera // create second camera 1 meter to the right of first camera
Cal3_S2 K2(1600, 1300, 0, 650, 440); Cal3_S2 K2(1600, 1300, 0, 650, 440);
SimpleCamera camera2(pose2, K2); PinholeCamera<Cal3_S2> camera2(pose2, K2);
// 1. Project two landmarks into two cameras and triangulate // 1. Project two landmarks into two cameras and triangulate
Point2 z1 = camera1.project(landmark); Point2 z1 = camera1.project(landmark);
Point2 z2 = camera2.project(landmark); Point2 z2 = camera2.project(landmark);
CameraSet<SimpleCamera> cameras; CameraSet<PinholeCamera<Cal3_S2> > cameras;
Point2Vector measurements; Point2Vector measurements;
cameras += camera1, camera2; cameras += camera1, camera2;
@ -280,7 +280,7 @@ TEST( triangulation, outliersAndFarLandmarks) {
// 3. Add a slightly rotated third camera above with a wrong measurement (OUTLIER) // 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)); 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); Cal3_S2 K3(700, 500, 0, 640, 480);
SimpleCamera camera3(pose3, K3); PinholeCamera<Cal3_S2> camera3(pose3, K3);
Point2 z3 = camera3.project(landmark); Point2 z3 = camera3.project(landmark);
cameras += camera3; cameras += camera3;
@ -302,7 +302,7 @@ TEST( triangulation, outliersAndFarLandmarks) {
//****************************************************************************** //******************************************************************************
TEST( triangulation, twoIdenticalPoses) { TEST( triangulation, twoIdenticalPoses) {
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
SimpleCamera camera1(pose1, *sharedCal); PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);
// 1. Project two landmarks into two cameras and triangulate // 1. Project two landmarks into two cameras and triangulate
Point2 z1 = camera1.project(landmark); Point2 z1 = camera1.project(landmark);

2
gtsam/geometry/tests/testUnit3.cpp
View File

@ -495,7 +495,7 @@ TEST(actualH, Serialization) {
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
srand(time(NULL)); srand(time(nullptr));
TestResult tr; TestResult tr;
return TestRegistry::runAllTests(tr); return TestRegistry::runAllTests(tr);
} }

4
gtsam/inference/BayesTree-inst.h
View File

@ -125,7 +125,7 @@ namespace gtsam {
void BayesTree<CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) { void BayesTree<CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) {
for(Key j: clique->conditional()->frontals()) for(Key j: clique->conditional()->frontals())
nodes_[j] = clique; nodes_[j] = clique;
if (parent_clique != NULL) { if (parent_clique != nullptr) {
clique->parent_ = parent_clique; clique->parent_ = parent_clique;
parent_clique->children.push_back(clique); parent_clique->children.push_back(clique);
} else { } else {
@ -430,7 +430,7 @@ namespace gtsam {
template <class CLIQUE> template <class CLIQUE>
void BayesTree<CLIQUE>::removePath(sharedClique clique, BayesNetType* bn, void BayesTree<CLIQUE>::removePath(sharedClique clique, BayesNetType* bn,
Cliques* orphans) { 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) { if (clique) {
// remove the clique from orphans in case it has been added earlier // remove the clique from orphans in case it has been added earlier
orphans->remove(clique); orphans->remove(clique);

4
gtsam/inference/FactorGraph-inst.h
View File

@ -55,8 +55,8 @@ bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
// check whether the factors are the same, in same order. // check whether the factors are the same, in same order.
for (size_t i = 0; i < factors_.size(); i++) { for (size_t i = 0; i < factors_.size(); i++) {
sharedFactor f1 = factors_[i], f2 = fg.factors_[i]; sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
if (f1 == NULL && f2 == NULL) continue; if (f1 == nullptr && f2 == nullptr) continue;
if (f1 == NULL || f2 == NULL) return false; if (f1 == nullptr || f2 == nullptr) return false;
if (!f1->equals(*f2, tol)) return false; if (!f1->equals(*f2, tol)) return false;
} }
return true; return true;

2
gtsam/inference/FactorGraph.h
View File

@ -353,7 +353,7 @@ class FactorGraph {
*/ */
void resize(size_t size) { factors_.resize(size); } 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(); } void remove(size_t i) { factors_[i].reset(); }

4
gtsam/inference/Ordering.cpp
View File

@ -91,7 +91,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
assert((size_t)count == variableIndex.nEntries()); 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]; double knobs[CCOLAMD_KNOBS];
ccolamd_set_defaults(knobs); ccolamd_set_defaults(knobs);
knobs[CCOLAMD_DENSE_ROW] = -1; knobs[CCOLAMD_DENSE_ROW] = -1;
@ -235,7 +235,7 @@ Ordering Ordering::Metis(const MetisIndex& met) {
int outputError; 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]); &iperm[0]);
Ordering result; Ordering result;

2
gtsam/linear/GaussianBayesNet.h
View File

@ -143,7 +143,7 @@ namespace gtsam {
* allocateVectorValues */ * allocateVectorValues */
VectorValues gradientAtZero() const; VectorValues gradientAtZero() const;
/** Mahalanobis norm error. */ /** 0.5 * sum of squared Mahalanobis distances. */
double error(const VectorValues& x) const; double error(const VectorValues& x) const;
/** /**

2
gtsam/linear/GaussianBayesTree.h
View File

@ -106,7 +106,7 @@ namespace gtsam {
* @return A VectorValues storing the gradient. */ * @return A VectorValues storing the gradient. */
VectorValues gradientAtZero() const; VectorValues gradientAtZero() const;
/** Mahalanobis norm error. */ /** 0.5 * sum of squared Mahalanobis distances. */
double error(const VectorValues& x) const; double error(const VectorValues& x) const;
/** Computes the determinant of a GassianBayesTree, as if the Bayes tree is reorganized into a /** Computes the determinant of a GassianBayesTree, as if the Bayes tree is reorganized into a

2
gtsam/linear/JacobianFactor.cpp
View File

@ -840,7 +840,7 @@ GaussianConditional::shared_ptr JacobianFactor::splitConditional(size_t nrFronta
if (!model_) { if (!model_) {
throw std::invalid_argument( 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()) { if (nrFrontals > size()) {

2
gtsam/linear/JacobianFactor.h
View File

@ -398,7 +398,7 @@ namespace gtsam {
* @param keys in some order * @param keys in some order
* @param diemnsions of the variables in same order * @param diemnsions of the variables in same order
* @param m output dimension * @param m output dimension
* @param model noise model (default NULL) * @param model noise model (default nullptr)
*/ */
template<class KEYS, class DIMENSIONS> template<class KEYS, class DIMENSIONS>
JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m, JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m,

16
gtsam/linear/LossFunctions.cpp
View File

@ -153,7 +153,7 @@ void Fair::print(const std::string &s="") const
bool Fair::equals(const Base &expected, double tol) const { bool Fair::equals(const Base &expected, double tol) const {
const Fair* p = dynamic_cast<const Fair*> (&expected); const Fair* p = dynamic_cast<const Fair*> (&expected);
if (p == NULL) return false; if (p == nullptr) return false;
return std::abs(c_ - p->c_ ) < tol; 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 { bool Huber::equals(const Base &expected, double tol) const {
const Huber* p = dynamic_cast<const Huber*>(&expected); const Huber* p = dynamic_cast<const Huber*>(&expected);
if (p == NULL) return false; if (p == nullptr) return false;
return std::abs(k_ - p->k_) < tol; 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 { bool Cauchy::equals(const Base &expected, double tol) const {
const Cauchy* p = dynamic_cast<const Cauchy*>(&expected); const Cauchy* p = dynamic_cast<const Cauchy*>(&expected);
if (p == NULL) return false; if (p == nullptr) return false;
return std::abs(ksquared_ - p->ksquared_) < tol; 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 { bool Tukey::equals(const Base &expected, double tol) const {
const Tukey* p = dynamic_cast<const Tukey*>(&expected); const Tukey* p = dynamic_cast<const Tukey*>(&expected);
if (p == NULL) return false; if (p == nullptr) return false;
return std::abs(c_ - p->c_) < tol; 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 { bool Welsch::equals(const Base &expected, double tol) const {
const Welsch* p = dynamic_cast<const Welsch*>(&expected); const Welsch* p = dynamic_cast<const Welsch*>(&expected);
if (p == NULL) return false; if (p == nullptr) return false;
return std::abs(c_ - p->c_) < tol; 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 { bool GemanMcClure::equals(const Base &expected, double tol) const {
const GemanMcClure* p = dynamic_cast<const GemanMcClure*>(&expected); const GemanMcClure* p = dynamic_cast<const GemanMcClure*>(&expected);
if (p == NULL) return false; if (p == nullptr) return false;
return std::abs(c_ - p->c_) < tol; 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 { bool DCS::equals(const Base &expected, double tol) const {
const DCS* p = dynamic_cast<const DCS*>(&expected); const DCS* p = dynamic_cast<const DCS*>(&expected);
if (p == NULL) return false; if (p == nullptr) return false;
return std::abs(c_ - p->c_) < tol; 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 { bool L2WithDeadZone::equals(const Base &expected, double tol) const {
const L2WithDeadZone* p = dynamic_cast<const L2WithDeadZone*>(&expected); const L2WithDeadZone* p = dynamic_cast<const L2WithDeadZone*>(&expected);
if (p == NULL) return false; if (p == nullptr) return false;
return std::abs(k_ - p->k_) < tol; return std::abs(k_ - p->k_) < tol;
} }

9
gtsam/linear/NoiseModel.cpp
View File

@ -25,7 +25,6 @@
#include <cmath> #include <cmath>
#include <iostream> #include <iostream>
#include <limits> #include <limits>
#include <random>
#include <stdexcept> #include <stdexcept>
#include <typeinfo> #include <typeinfo>
@ -134,7 +133,7 @@ void Gaussian::print(const string& name) const {
/* ************************************************************************* */ /* ************************************************************************* */
bool Gaussian::equals(const Base& expected, double tol) const { bool Gaussian::equals(const Base& expected, double tol) const {
const Gaussian* p = dynamic_cast<const Gaussian*> (&expected); const Gaussian* p = dynamic_cast<const Gaussian*> (&expected);
if (p == NULL) return false; if (p == nullptr) return false;
if (typeid(*this) != typeid(*p)) return false; if (typeid(*this) != typeid(*p)) return false;
//TODO(Alex); //TODO(Alex);
//if (!sqrt_information_) return true; //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 // Note: for Diagonal, which does ediv_, will be correct for constraints
Vector w = whiten(v); Vector w = whiten(v);
return w.dot(w); 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_; 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 { bool Robust::equals(const Base& expected, double tol) const {
const Robust* p = dynamic_cast<const Robust*> (&expected); const Robust* p = dynamic_cast<const Robust*> (&expected);
if (p == NULL) return false; if (p == nullptr) return false;
return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol); return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol);
} }

23
gtsam/linear/NoiseModel.h
View File

@ -207,12 +207,25 @@ namespace gtsam {
virtual Vector unwhiten(const Vector& v) const; virtual Vector unwhiten(const Vector& v) const;
/** /**
* Mahalanobis distance v'*R'*R*v = <R*v,R*v> * Squared Mahalanobis distance v'*R'*R*v = <R*v,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 { 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 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 whiten(const Vector& v) const;
virtual Vector unwhiten(const Vector& v) const; virtual Vector unwhiten(const Vector& v) const;
virtual Matrix Whiten(const Matrix& H) const; virtual Matrix Whiten(const Matrix& H) const;
@ -616,7 +629,7 @@ namespace gtsam {
virtual bool isUnit() const { return true; } virtual bool isUnit() const { return true; }
virtual void print(const std::string& name) const; 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 whiten(const Vector& v) const { return v; }
virtual Vector unwhiten(const Vector& v) const { return v; } virtual Vector unwhiten(const Vector& v) const { return v; }
virtual Matrix Whiten(const Matrix& H) const { return H; } virtual Matrix Whiten(const Matrix& H) const { return H; }

34
gtsam/linear/Sampler.cpp
View File

@ -11,6 +11,8 @@
/** /**
* @file Sampler.cpp * @file Sampler.cpp
* @brief sampling from a diagonal NoiseModel
* @author Frank Dellaert
* @author Alex Cunningham * @author Alex Cunningham
*/ */
@ -18,25 +20,16 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model, int32_t seed) Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model,
: model_(model), generator_(static_cast<unsigned>(seed)) uint_fast64_t seed)
{ : model_(model), generator_(seed) {}
}
/* ************************************************************************* */ /* ************************************************************************* */
Sampler::Sampler(const Vector& sigmas, int32_t seed) Sampler::Sampler(const Vector& sigmas, uint_fast64_t seed)
: model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(static_cast<unsigned>(seed)) : model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(seed) {}
{
}
/* ************************************************************************* */ /* ************************************************************************* */
Sampler::Sampler(int32_t seed) Vector Sampler::sampleDiagonal(const Vector& sigmas) const {
: generator_(static_cast<unsigned>(seed))
{
}
/* ************************************************************************* */
Vector Sampler::sampleDiagonal(const Vector& sigmas) {
size_t d = sigmas.size(); size_t d = sigmas.size();
Vector result(d); Vector result(d);
for (size_t i = 0; i < d; i++) { 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()); assert(model_.get());
const Vector& sigmas = model_->sigmas(); const Vector& sigmas = model_->sigmas();
return sampleDiagonal(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()); assert(model.get());
const Vector& sigmas = model->sigmas(); const Vector& sigmas = model->sigmas();
return sampleDiagonal(sigmas); return sampleDiagonal(sigmas);
} }
#endif
/* ************************************************************************* */ /* ************************************************************************* */
} // \namespace gtsam } // namespace gtsam

75
gtsam/linear/Sampler.h
View File

@ -10,9 +10,9 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @brief sampling that can be parameterized using a NoiseModel to generate samples from
* @file Sampler.h * @file Sampler.h
* the given distribution * @brief sampling from a NoiseModel
* @author Frank Dellaert
* @author Alex Cunningham * @author Alex Cunningham
*/ */
@ -27,9 +27,6 @@ namespace gtsam {
/** /**
* Sampling structure that keeps internal random number generators for * Sampling structure that keeps internal random number generators for
* diagonal distributions specified by NoiseModel * 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 { class GTSAM_EXPORT Sampler {
protected: protected:
@ -37,57 +34,67 @@ protected:
noiseModel::Diagonal::shared_ptr model_; noiseModel::Diagonal::shared_ptr model_;
/** generator */ /** generator */
std::mt19937_64 generator_; mutable std::mt19937_64 generator_;
public: public:
typedef boost::shared_ptr<Sampler> shared_ptr; typedef boost::shared_ptr<Sampler> shared_ptr;
/// @name constructors
/// @{
/** /**
* Create a sampler for the distribution specified by a diagonal NoiseModel * Create a sampler for the distribution specified by a diagonal NoiseModel
* with a manually specified seed * with a manually specified seed
* *
* NOTE: do not use zero as a seed, it will break the generator * 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 * 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 /// @name access functions
* /// @{
* NOTE: do not use zero as a seed, it will break the generator
*/ size_t dim() const {
Sampler(int32_t seed = 42u); 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_; } const noiseModel::Diagonal::shared_ptr& model() const { return model_; }
/** /// @}
* sample from distribution /// @name basic functionality
* NOTE: not const due to need to update the underlying generator /// @{
*/
Vector sample();
/** /// sample from distribution
* Sample from noisemodel passed in as an argument, Vector sample() const;
* can be used without having initialized a model for the system.
* /// @}
* NOTE: not const due to need to update the underlying generator
*/ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
Vector sampleNewModel(const noiseModel::Diagonal::shared_ptr& model); /// @name Deprecated
/// @{
explicit Sampler(uint_fast64_t seed = 42u);
Vector sampleNewModel(const noiseModel::Diagonal::shared_ptr& model) const;
/// @}
#endif
protected: protected:
/** given sigmas for a diagonal model, returns a sample */ /** given sigmas for a diagonal model, returns a sample */
Vector sampleDiagonal(const Vector& sigmas); Vector sampleDiagonal(const Vector& sigmas) const;
}; };
} // \namespace gtsam } // namespace gtsam

28
gtsam/linear/VectorValues.cpp
View File

@ -51,7 +51,11 @@ namespace gtsam {
Key key; Key key;
size_t n; size_t n;
boost::tie(key, n) = v; boost::tie(key, n) = v;
#ifdef TBB_GREATER_EQUAL_2020
values_.emplace(key, x.segment(j, n)); values_.emplace(key, x.segment(j, n));
#else
values_.insert(std::make_pair(key, x.segment(j, n)));
#endif
j += n; j += n;
} }
} }
@ -60,7 +64,11 @@ namespace gtsam {
VectorValues::VectorValues(const Vector& x, const Scatter& scatter) { VectorValues::VectorValues(const Vector& x, const Scatter& scatter) {
size_t j = 0; size_t j = 0;
for (const SlotEntry& v : scatter) { for (const SlotEntry& v : scatter) {
#ifdef TBB_GREATER_EQUAL_2020
values_.emplace(v.key, x.segment(j, v.dimension)); 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; j += v.dimension;
} }
} }
@ -70,7 +78,11 @@ namespace gtsam {
{ {
VectorValues result; VectorValues result;
for(const KeyValuePair& v: other) for(const KeyValuePair& v: other)
#ifdef TBB_GREATER_EQUAL_2020
result.values_.emplace(v.first, Vector::Zero(v.second.size())); 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; return result;
} }
@ -86,7 +98,11 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues::iterator VectorValues::emplace(Key j, const Vector& value) { VectorValues::iterator VectorValues::emplace(Key j, const Vector& value) {
#ifdef TBB_GREATER_EQUAL_2020
std::pair<iterator, bool> result = values_.emplace(j, value); std::pair<iterator, bool> result = values_.emplace(j, value);
#else
std::pair<iterator, bool> result = values_.insert(std::make_pair(j, value));
#endif
if(!result.second) if(!result.second)
throw std::invalid_argument( throw std::invalid_argument(
"Requested to emplace variable '" + DefaultKeyFormatter(j) "Requested to emplace variable '" + DefaultKeyFormatter(j)
@ -266,7 +282,11 @@ namespace gtsam {
VectorValues result; VectorValues result;
// The result.end() hint here should result in constant-time inserts // The result.end() hint here should result in constant-time inserts
for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) 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); 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; return result;
} }
@ -324,7 +344,11 @@ namespace gtsam {
VectorValues result; VectorValues result;
// The result.end() hint here should result in constant-time inserts // The result.end() hint here should result in constant-time inserts
for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) 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); 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; return result;
} }
@ -340,7 +364,11 @@ namespace gtsam {
{ {
VectorValues result; VectorValues result;
for(const VectorValues::KeyValuePair& key_v: v) for(const VectorValues::KeyValuePair& key_v: v)
#ifdef TBB_GREATER_EQUAL_2020
result.values_.emplace(key_v.first, a * key_v.second); 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; return result;
} }

4
gtsam/linear/VectorValues.h
View File

@ -198,7 +198,11 @@ namespace gtsam {
* exist, it is inserted and an iterator pointing to the new element, along with 'true', is * exist, it is inserted and an iterator pointing to the new element, along with 'true', is
* returned. */ * returned. */
std::pair<iterator, bool> tryInsert(Key j, const Vector& value) { std::pair<iterator, bool> tryInsert(Key j, const Vector& value) {
#ifdef TBB_GREATER_EQUAL_2020
return values_.emplace(j, value); 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 */ /** Erase the vector with the given key, or throw std::out_of_range if it does not exist */

2
gtsam/linear/tests/testJacobianFactor.cpp
View File

@ -253,7 +253,7 @@ TEST(JacobianFactor, error)
/* ************************************************************************* */ /* ************************************************************************* */
TEST(JacobianFactor, matrices_NULL) 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); JacobianFactor factor(simple::terms, simple::b);
Matrix jacobianExpected(3, 9); Matrix jacobianExpected(3, 9);

4
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -68,10 +68,10 @@ TEST(NoiseModel, constructors)
for(Gaussian::shared_ptr mi: m) for(Gaussian::shared_ptr mi: m)
EXPECT(assert_equal(unwhitened,mi->unwhiten(whitened))); EXPECT(assert_equal(unwhitened,mi->unwhiten(whitened)));
// test Mahalanobis distance // test squared Mahalanobis distance
double distance = 5*5+10*10+15*15; double distance = 5*5+10*10+15*15;
for(Gaussian::shared_ptr mi: m) 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 // test R matrix
for(Gaussian::shared_ptr mi: m) for(Gaussian::shared_ptr mi: m)

18
gtsam/linear/tests/testSampler.cpp
View File

@ -10,8 +10,10 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file testSampler * @file testSampler.cpp
* @brief unit tests for Sampler class
* @author Alex Cunningham * @author Alex Cunningham
* @author Frank Dellaert
*/ */
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
@ -22,14 +24,15 @@ using namespace gtsam;
const double tol = 1e-5; const double tol = 1e-5;
static const Vector3 kSigmas(1.0, 0.1, 0.0);
/* ************************************************************************* */ /* ************************************************************************* */
TEST(testSampler, basic) { TEST(testSampler, basic) {
Vector sigmas = Vector3(1.0, 0.1, 0.0); auto model = noiseModel::Diagonal::Sigmas(kSigmas);
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(sigmas);
char seed = 'A'; char seed = 'A';
Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1); Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1);
EXPECT(assert_equal(sigmas, sampler1.sigmas())); EXPECT(assert_equal(kSigmas, sampler1.sigmas()));
EXPECT(assert_equal(sigmas, sampler2.sigmas())); EXPECT(assert_equal(kSigmas, sampler2.sigmas()));
EXPECT_LONGS_EQUAL(3, sampler1.dim()); EXPECT_LONGS_EQUAL(3, sampler1.dim());
EXPECT_LONGS_EQUAL(3, sampler2.dim()); EXPECT_LONGS_EQUAL(3, sampler2.dim());
Vector actual1 = sampler1.sample(); 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);
}
/* ************************************************************************* */ /* ************************************************************************* */

2
gtsam/navigation/AHRSFactor.cpp
View File

@ -114,7 +114,7 @@ void AHRSFactor::print(const string& s,
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const { bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const {
const This *e = dynamic_cast<const This*>(&other); const This *e = dynamic_cast<const This*>(&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);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------

4
gtsam/navigation/AttitudeFactor.cpp
View File

@ -52,7 +52,7 @@ void Rot3AttitudeFactor::print(const string& s,
bool Rot3AttitudeFactor::equals(const NonlinearFactor& expected, bool Rot3AttitudeFactor::equals(const NonlinearFactor& expected,
double tol) const { double tol) const {
const This* e = dynamic_cast<const This*>(&expected); const This* e = dynamic_cast<const This*>(&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); && this->bRef_.equals(e->bRef_, tol);
} }
@ -69,7 +69,7 @@ void Pose3AttitudeFactor::print(const string& s,
bool Pose3AttitudeFactor::equals(const NonlinearFactor& expected, bool Pose3AttitudeFactor::equals(const NonlinearFactor& expected,
double tol) const { double tol) const {
const This* e = dynamic_cast<const This*>(&expected); const This* e = dynamic_cast<const This*>(&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); && this->bRef_.equals(e->bRef_, tol);
} }

2
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -174,7 +174,7 @@ void CombinedImuFactor::print(const string& s,
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool CombinedImuFactor::equals(const NonlinearFactor& other, double tol) const { bool CombinedImuFactor::equals(const NonlinearFactor& other, double tol) const {
const This* e = dynamic_cast<const This*>(&other); const This* e = dynamic_cast<const This*>(&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);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------

4
gtsam/navigation/GPSFactor.cpp
View File

@ -32,7 +32,7 @@ void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
//*************************************************************************** //***************************************************************************
bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const { bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected); const This* e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) && traits<Point3>::Equals(nT_, e->nT_, tol); return e != nullptr && Base::equals(*e, tol) && traits<Point3>::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 { bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected); const This* e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) && return e != nullptr && Base::equals(*e, tol) &&
traits<Point3>::Equals(nT_, e->nT_, tol); traits<Point3>::Equals(nT_, e->nT_, tol);
} }

2
gtsam/navigation/NavState.h
View File

@ -64,7 +64,7 @@ public:
R_(pose.rotation()), t_(pose.translation()), v_(v) { R_(pose.rotation()), t_(pose.translation()), v_(v) {
} }
/// Construct from SO(3) and R^6 /// 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>()) { R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
} }
/// Named constructor with derivatives /// Named constructor with derivatives

9
gtsam/navigation/PreintegrationParams.h
View File

@ -29,6 +29,13 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
Vector3 n_gravity; ///< Gravity vector in nav frame 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 /// The Params constructor insists on getting the navigation frame gravity vector
/// For convenience, two commonly used conventions are provided by named constructors below /// For convenience, two commonly used conventions are provided by named constructors below
PreintegrationParams(const Vector3& n_gravity) PreintegrationParams(const Vector3& n_gravity)
@ -60,8 +67,6 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
bool getUse2ndOrderCoriolis() const { return use2ndOrderCoriolis; } bool getUse2ndOrderCoriolis() const { return use2ndOrderCoriolis; }
protected: protected:
/// Default constructor for serialization only: uninitialized!
PreintegrationParams() {}
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;

2
gtsam/navigation/ScenarioRunner.h
View File

@ -48,7 +48,7 @@ class GTSAM_EXPORT ScenarioRunner {
const Bias estimatedBias_; const Bias estimatedBias_;
// Create two samplers for acceleration and omega noise // Create two samplers for acceleration and omega noise
mutable Sampler gyroSampler_, accSampler_; Sampler gyroSampler_, accSampler_;
public: public:
ScenarioRunner(const Scenario& scenario, const SharedParams& p, ScenarioRunner(const Scenario& scenario, const SharedParams& p,

3
gtsam/navigation/tests/testAHRSFactor.cpp
View File

@ -15,6 +15,7 @@
* @author Krunal Chande * @author Krunal Chande
* @author Luca Carlone * @author Luca Carlone
* @author Frank Dellaert * @author Frank Dellaert
* @author Varun Agrawal
*/ */
#include <gtsam/navigation/AHRSFactor.h> #include <gtsam/navigation/AHRSFactor.h>
@ -200,7 +201,7 @@ TEST(AHRSFactor, Error) {
// 1e-5 needs to be added only when using quaternions for rotations // 1e-5 needs to be added only when using quaternions for rotations
EXPECT(assert_equal(H3e, H3a, 1e-5)); 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
} }
/* ************************************************************************* */ /* ************************************************************************* */

6
gtsam/navigation/tests/testImuFactorSerialization.cpp
View File

@ -64,9 +64,9 @@ TEST(ImuFactor, serialization) {
ImuFactor factor(1, 2, 3, 4, 5, pim); ImuFactor factor(1, 2, 3, 4, 5, pim);
EXPECT(equalsObj(factor)); EXPECT(equalsObj<ImuFactor>(factor));
EXPECT(equalsXML(factor)); EXPECT(equalsXML<ImuFactor>(factor));
EXPECT(equalsBinary(factor)); EXPECT(equalsBinary<ImuFactor>(factor));
} }
/* ************************************************************************* */ /* ************************************************************************* */

2
gtsam/nonlinear/Expression.h
View File

@ -66,7 +66,7 @@ public:
// Expressions wrap trees of functions that can evaluate their own derivatives. // Expressions wrap trees of functions that can evaluate their own derivatives.
// The meta-functions below are useful to specify the type of those functions. // 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: // Example, a function taking a camera and a 3D point and yielding a 2D point:
// Expression<Point2>::BinaryFunction<SimpleCamera,Point3>::type // Expression<Point2>::BinaryFunction<PinholeCamera<Cal3_S2>,Point3>::type
template<class A1> template<class A1>
struct UnaryFunction { struct UnaryFunction {
typedef boost::function< typedef boost::function<

2
gtsam/nonlinear/ISAM2Params.h
View File

@ -219,7 +219,7 @@ struct GTSAM_EXPORT ISAM2Params {
/// When you will be removing many factors, e.g. when using ISAM2 as a /// 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 /// 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. /// cost of having to search for slots every time a factor is added.
bool findUnusedFactorSlots; bool findUnusedFactorSlots;

6
gtsam/nonlinear/NonlinearEquality.h
View File

@ -175,6 +175,8 @@ public:
/// @} /// @}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private: private:
/** Serialization function */ /** Serialization function */
@ -263,6 +265,8 @@ public:
traits<X>::Print(value_, "Value"); traits<X>::Print(value_, "Value");
} }
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private: private:
/** Serialization function */ /** Serialization function */
@ -327,6 +331,8 @@ public:
return traits<X>::Local(x1,x2); return traits<X>::Local(x1,x2);
} }
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private: private:
/** Serialization function */ /** Serialization function */

6
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -54,7 +54,7 @@ void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& key
for (size_t i = 0; i < factors_.size(); i++) { for (size_t i = 0; i < factors_.size(); i++) {
stringstream ss; stringstream ss;
ss << "Factor " << i << ": "; 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; cout << endl;
} }
} }
@ -67,8 +67,8 @@ void NonlinearFactorGraph::printErrors(const Values& values, const std::string&
for (size_t i = 0; i < factors_.size(); i++) { for (size_t i = 0; i < factors_.size(); i++) {
stringstream ss; stringstream ss;
ss << "Factor " << i << ": "; ss << "Factor " << i << ": ";
if (factors_[i] == NULL) { if (factors_[i] == nullptr) {
cout << "NULL" << endl; cout << "nullptr" << endl;
} else { } else {
factors_[i]->print(ss.str(), keyFormatter); factors_[i]->print(ss.str(), keyFormatter);
cout << "error = " << factors_[i]->error(values) << endl; cout << "error = " << factors_[i]->error(values) << endl;

2
gtsam/nonlinear/tests/testCallRecord.cpp
View File

@ -98,7 +98,7 @@ struct Record: public internal::CallRecordImplementor<Record, Cols> {
friend struct internal::CallRecordImplementor; friend struct internal::CallRecordImplementor;
}; };
internal::JacobianMap & NJM= *static_cast<internal::JacobianMap *>(NULL); internal::JacobianMap & NJM= *static_cast<internal::JacobianMap *>(nullptr);
/* ************************************************************************* */ /* ************************************************************************* */
typedef Eigen::Matrix<double, Eigen::Dynamic, Cols> DynRowMat; typedef Eigen::Matrix<double, Eigen::Dynamic, Cols> DynRowMat;

11
gtsam/nonlinear/utilities.h
View File

@ -28,7 +28,7 @@
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <exception> #include <exception>
@ -169,7 +169,7 @@ void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) {
} }
/// Insert a number of initial point values by backprojecting /// Insert a number of initial point values by backprojecting
void insertBackprojections(Values& values, const SimpleCamera& camera, void insertBackprojections(Values& values, const PinholeCamera<Cal3_S2>& camera,
const Vector& J, const Matrix& Z, double depth) { const Vector& J, const Matrix& Z, double depth) {
if (Z.rows() != 2) if (Z.rows() != 2)
throw std::invalid_argument("insertBackProjections: Z must be 2*K"); 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 // if value is a Pose2, compose it with base pose
Pose2 pose = local.at<Pose2>(key); Pose2 pose = local.at<Pose2>(key);
world.insert(key, base.compose(pose)); world.insert(key, base.compose(pose));
} catch (std::exception e1) { } catch (const std::exception& e1) {
try { try {
// if value is a Point2, transform it from base pose // if value is a Point2, transform it from base pose
Point2 point = local.at<Point2>(key); Point2 point = local.at<Point2>(key);
world.insert(key, base.transformFrom(point)); world.insert(key, base.transformFrom(point));
} catch (std::exception e2) { } catch (const std::exception& e2) {
// if not Pose2 or Point2, do nothing // if not Pose2 or Point2, do nothing
#ifndef NDEBUG
std::cerr << "Values[key] is neither Pose2 nor Point2, so skip" << std::endl;
#endif
} }
} }
} }

13
gtsam/sam/tests/testRangeFactor.cpp
View File

@ -19,7 +19,8 @@
#include <gtsam/sam/RangeFactor.h> #include <gtsam/sam/RangeFactor.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/serializationTestHelpers.h> #include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
@ -353,11 +354,13 @@ TEST(RangeFactor, Point3) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Do tests with SimpleCamera // Do tests with PinholeCamera<Cal3_S2>
TEST( RangeFactor, Camera) { TEST( RangeFactor, Camera) {
RangeFactor<SimpleCamera,Point3> factor1(poseKey, pointKey, measurement, model); using Camera = PinholeCamera<Cal3_S2>;
RangeFactor<SimpleCamera,Pose3> factor2(poseKey, pointKey, measurement, model);
RangeFactor<SimpleCamera,SimpleCamera> factor3(poseKey, pointKey, measurement, model); RangeFactor<Camera, Point3> factor1(poseKey, pointKey, measurement, model);
RangeFactor<Camera, Pose3> factor2(poseKey, pointKey, measurement, model);
RangeFactor<Camera, Camera> factor3(poseKey, pointKey, measurement, model);
} }
/* ************************************************************************* */ /* ************************************************************************* */

2
gtsam/slam/AntiFactor.h
View File

@ -67,7 +67,7 @@ namespace gtsam {
/** equals */ /** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected); const This *e = dynamic_cast<const This*> (&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 */ /** implement functions needed to derive from Factor */

2
gtsam/slam/BetweenFactor.h
View File

@ -81,7 +81,7 @@ namespace gtsam {
/** equals */ /** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected); const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && traits<T>::Equals(this->measured_, e->measured_, tol); return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(this->measured_, e->measured_, tol);
} }
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */

2
gtsam/slam/EssentialMatrixConstraint.cpp
View File

@ -37,7 +37,7 @@ void EssentialMatrixConstraint::print(const std::string& s,
bool EssentialMatrixConstraint::equals(const NonlinearFactor& expected, bool EssentialMatrixConstraint::equals(const NonlinearFactor& expected,
double tol) const { double tol) const {
const This *e = dynamic_cast<const This*>(&expected); const This *e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) return e != nullptr && Base::equals(*e, tol)
&& this->measuredE_.equals(e->measuredE_, tol); && this->measuredE_.equals(e->measuredE_, tol);
} }

2
gtsam/slam/EssentialMatrixFactor.h
View File

@ -9,7 +9,7 @@
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/EssentialMatrix.h> #include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <iostream> #include <iostream>
namespace gtsam { namespace gtsam {

2
gtsam/slam/JacobianFactorSVD.h
View File

@ -65,7 +65,7 @@ public:
Base() { Base() {
size_t numKeys = Enull.rows() / ZDim; size_t numKeys = Enull.rows() / ZDim;
size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()? 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(); // Matrix Q = Enull * Enull.transpose();
// for(const KeyMatrixZD& it: Fblocks) // for(const KeyMatrixZD& it: Fblocks)
// QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));

2
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -31,7 +31,7 @@ void OrientedPlane3DirectionPrior::print(const string& s,
bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected, bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected,
double tol) const { double tol) const {
const This* e = dynamic_cast<const This*>(&expected); const This* e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) return e != nullptr && Base::equals(*e, tol)
&& this->measured_p_.equals(e->measured_p_, tol); && this->measured_p_.equals(e->measured_p_, tol);
} }

2
gtsam/slam/PoseRotationPrior.h
View File

@ -62,7 +62,7 @@ public:
/** equals specialized to this factor */ /** equals specialized to this factor */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected); const This *e = dynamic_cast<const This*> (&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 */ /** print contents */

2
gtsam/slam/PoseTranslationPrior.h
View File

@ -76,7 +76,7 @@ public:
/** equals specialized to this factor */ /** equals specialized to this factor */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected); const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && traits<Translation>::Equals(measured_, e->measured_, tol); return e != nullptr && Base::equals(*e, tol) && traits<Translation>::Equals(measured_, e->measured_, tol);
} }
/** print contents */ /** print contents */

2
gtsam/slam/PriorFactor.h
View File

@ -85,7 +85,7 @@ namespace gtsam {
/** equals */ /** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This* e = dynamic_cast<const This*> (&expected); const This* e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && traits<T>::Equals(prior_, e->prior_, tol); return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(prior_, e->prior_, tol);
} }
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */

3
gtsam/slam/ProjectionFactor.h
View File

@ -21,7 +21,8 @@
#pragma once #pragma once
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <boost/optional.hpp> #include <boost/optional.hpp>
namespace gtsam { namespace gtsam {

6
gtsam/slam/dataset.cpp
View File

@ -252,7 +252,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
is.seekg(0, ios::beg); is.seekg(0, ios::beg);
// If asked, create a sampler with random number generator // If asked, create a sampler with random number generator
Sampler sampler; std::unique_ptr<Sampler> sampler;
if (addNoise) { if (addNoise) {
noiseModel::Diagonal::shared_ptr noise; noiseModel::Diagonal::shared_ptr noise;
if (model) if (model)
@ -261,7 +261,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
throw invalid_argument( throw invalid_argument(
"gtsam::load2D: invalid noise model for adding noise" "gtsam::load2D: invalid noise model for adding noise"
"(current version assumes diagonal noise model)!"); "(current version assumes diagonal noise model)!");
sampler = Sampler(noise); sampler.reset(new Sampler(noise));
} }
// Parse the pose constraints // Parse the pose constraints
@ -289,7 +289,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
model = modelInFile; model = modelInFile;
if (addNoise) if (addNoise)
l1Xl2 = l1Xl2.retract(sampler.sample()); l1Xl2 = l1Xl2.retract(sampler->sample());
// Insert vertices if pure odometry file // Insert vertices if pure odometry file
if (!initial->exists(id1)) if (!initial->exists(id1))

1
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -13,6 +13,7 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/CalibratedCamera.h> #include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>

2
gtsam/slam/tests/testOrientedPlane3Factor.cpp
View File

@ -175,7 +175,7 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) {
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
srand(time(NULL)); srand(time(nullptr));
TestResult tr; TestResult tr;
return TestRegistry::runAllTests(tr); return TestRegistry::runAllTests(tr);
} }

6
gtsam/slam/tests/testTriangulationFactor.cpp
View File

@ -17,7 +17,7 @@
*/ */
#include <gtsam/geometry/triangulation.h> #include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/StereoCamera.h> #include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/Cal3Bundler.h> #include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/nonlinear/Expression.h> #include <gtsam/nonlinear/Expression.h>
@ -39,7 +39,7 @@ static const boost::shared_ptr<Cal3_S2> sharedCal = //
// Looking along X-axis, 1 meter above ground plane (x-y) // 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 Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2);
static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
SimpleCamera camera1(pose1, *sharedCal); PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);
// landmark ~5 meters infront of camera // landmark ~5 meters infront of camera
static const Point3 landmark(5, 0.5, 1.2); static const Point3 landmark(5, 0.5, 1.2);
@ -52,7 +52,7 @@ TEST( triangulation, TriangulationFactor ) {
Key pointKey(1); Key pointKey(1);
SharedNoiseModel model; SharedNoiseModel model;
typedef TriangulationFactor<SimpleCamera> Factor; typedef TriangulationFactor<PinholeCamera<Cal3_S2> > Factor;
Factor factor(camera1, z1, model, pointKey); Factor factor(camera1, z1, model, pointKey);
// Use the factor to calculate the Jacobians // Use the factor to calculate the Jacobians

4
gtsam_unstable/base/BTree.h
View File

@ -72,7 +72,7 @@ namespace gtsam {
}; // Node }; // Node
// We store a shared pointer to the root of the functional tree // 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<const Node> sharedNode; typedef boost::shared_ptr<const Node> sharedNode;
sharedNode root_; sharedNode root_;
@ -223,7 +223,7 @@ namespace gtsam {
/** Return height of the tree, 0 if empty */ /** Return height of the tree, 0 if empty */
size_t height() const { size_t height() const {
return (root_ != NULL) ? root_->height_ : 0; return (root_ != nullptr) ? root_->height_ : 0;
} }
/** return size of the tree */ /** return size of the tree */

160
gtsam_unstable/examples/TimeOfArrivalExample.cpp Normal file
View File

@ -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 <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam_unstable/geometry/Event.h>
#include <gtsam_unstable/slam/TOAFactor.h>
#include <boost/bind.hpp>
#include <boost/format.hpp>
#include <vector>
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<Point3> defineMicrophones() {
const double height = 0.5;
vector<Point3> 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<Event> createTrajectory(int n) {
vector<Event> 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<double> simulateTOA(const vector<Point3>& microphones,
const Event& event) {
size_t K = microphones.size();
vector<double> 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<vector<double>> simulateTOA(const vector<Point3>& microphones,
const vector<Event>& trajectory) {
vector<vector<double>> simulatedTOA;
for (auto event : trajectory) {
simulatedTOA.push_back(simulateTOA(microphones, event));
}
return simulatedTOA;
}
/* ************************************************************************* */
// create factor graph
NonlinearFactorGraph createGraph(const vector<Point3>& microphones,
const vector<vector<double>>& 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<TOAFactor>(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");
}
/* ************************************************************************* */

9
gtsam_unstable/geometry/Event.cpp
View File

@ -24,15 +24,16 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void Event::print(const std::string& s) const { 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 { bool Event::equals(const Event& other, double tol) const {
return std::abs(time_ - other.time_) < tol return std::abs(time_ - other.time_) < tol &&
&& traits<Point3>::Equals(location_, other.location_, tol); traits<Point3>::Equals(location_, other.location_, tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
} //\ namespace gtsam } // namespace gtsam

82
gtsam_unstable/geometry/Event.h
View File

@ -20,15 +20,21 @@
#pragma once #pragma once
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam_unstable/dllexport.h>
#include <cmath> #include <cmath>
#include <iosfwd> #include <iosfwd>
#include <gtsam_unstable/dllexport.h> #include <string>
namespace gtsam { 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 { class Event {
double time_; ///< Time event was generated double time_; ///< Time event was generated
Point3 location_; ///< Location at time event was generated Point3 location_; ///< Location at time event was generated
@ -36,24 +42,19 @@ public:
enum { dimension = 4 }; enum { dimension = 4 };
/// Default Constructor /// Default Constructor
Event() : Event() : time_(0), location_(0, 0, 0) {}
time_(0), location_(0,0,0) {
}
/// Constructor from time and location /// Constructor from time and location
Event(double t, const Point3& p) : Event(double t, const Point3& p) : time_(t), location_(p) {}
time_(t), location_(p) {
}
/// Constructor with doubles /// Constructor with doubles
Event(double t, double x, double y, double z) : Event(double t, double x, double y, double z)
time_(t), location_(x, y, z) { : time_(t), location_(x, y, z) {}
}
double time() const { return time_; } double time() const { return time_; }
Point3 location() const { return location_; } Point3 location() const { return location_; }
// TODO we really have to think of a better way to do linear arguments // TODO(frank) we really have to think of a better way to do linear arguments
double height(OptionalJacobian<1, 4> H = boost::none) const { double height(OptionalJacobian<1, 4> H = boost::none) const {
static const Matrix14 JacobianZ = (Matrix14() << 0, 0, 0, 1).finished(); static const Matrix14 JacobianZ = (Matrix14() << 0, 0, 0, 1).finished();
if (H) *H = JacobianZ; if (H) *H = JacobianZ;
@ -64,7 +65,8 @@ public:
GTSAM_UNSTABLE_EXPORT void print(const std::string& s = "") const; GTSAM_UNSTABLE_EXPORT void print(const std::string& s = "") const;
/** equals with an tolerance */ /** 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 /// Updates a with tangent space delta
inline Event retract(const Vector4& v) const { inline Event retract(const Vector4& v) const {
@ -73,23 +75,7 @@ public:
/// Returns inverse retraction /// Returns inverse retraction
inline Vector4 localCoordinates(const Event& q) const { inline Vector4 localCoordinates(const Event& q) const {
return Vector4::Zero(); // TODO return Vector4::Zero(); // TODO(frank) implement!
}
/// 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;
} }
}; };
@ -97,4 +83,36 @@ public:
template <> template <>
struct traits<Event> : internal::Manifold<Event> {}; struct traits<Event> : internal::Manifold<Event> {};
} //\ 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

26
gtsam_unstable/geometry/tests/testEvent.cpp
View File

@ -17,10 +17,12 @@
* @date December 2014 * @date December 2014
*/ */
#include <gtsam_unstable/geometry/Event.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/nonlinear/Expression.h> #include <gtsam/nonlinear/Expression.h>
#include <gtsam_unstable/geometry/Event.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp> #include <boost/bind.hpp>
using namespace std; using namespace std;
@ -36,6 +38,9 @@ static const double timeOfEvent = 25;
static const Event exampleEvent(timeOfEvent, 1, 0, 0); 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; const double t = 0;
@ -45,27 +50,27 @@ TEST( Event, Constructor ) {
//***************************************************************************** //*****************************************************************************
TEST(Event, Toa1) { TEST(Event, Toa1) {
Event event(0, 1, 0, 0); Event event(0, 1, 0, 0);
double expected = 1. / 330; double expected = 1. / kSpeedOfSound;
EXPECT_DOUBLES_EQUAL(expected, event.toa(microphoneAt0), 1e-9); EXPECT_DOUBLES_EQUAL(expected, kToa(event, microphoneAt0), 1e-9);
} }
//***************************************************************************** //*****************************************************************************
TEST(Event, Toa2) { TEST(Event, Toa2) {
double expectedTOA = timeOfEvent + 1. / 330; double expectedTOA = timeOfEvent + 1. / kSpeedOfSound;
EXPECT_DOUBLES_EQUAL(expectedTOA, exampleEvent.toa(microphoneAt0), 1e-9); EXPECT_DOUBLES_EQUAL(expectedTOA, kToa(exampleEvent, microphoneAt0), 1e-9);
} }
//************************************************************************* //*************************************************************************
TEST(Event, Derivatives) { TEST(Event, Derivatives) {
Matrix14 actualH1; Matrix14 actualH1;
Matrix13 actualH2; Matrix13 actualH2;
exampleEvent.toa(microphoneAt0, actualH1, actualH2); kToa(exampleEvent, microphoneAt0, actualH1, actualH2);
Matrix expectedH1 = numericalDerivative11<double, Event>( Matrix expectedH1 = numericalDerivative11<double, Event>(
boost::bind(&Event::toa, _1, microphoneAt0, boost::none, boost::none), boost::bind(kToa, _1, microphoneAt0, boost::none, boost::none),
exampleEvent); exampleEvent);
EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
Matrix expectedH2 = numericalDerivative11<double, Point3>( Matrix expectedH2 = numericalDerivative11<double, Point3>(
boost::bind(&Event::toa, exampleEvent, _1, boost::none, boost::none), boost::bind(kToa, exampleEvent, _1, boost::none, boost::none),
microphoneAt0); microphoneAt0);
EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
} }
@ -75,11 +80,11 @@ TEST( Event, Expression ) {
Key key = 12; Key key = 12;
Expression<Event> event_(key); Expression<Event> event_(key);
Expression<Point3> knownMicrophone_(microphoneAt0); // constant expression Expression<Point3> knownMicrophone_(microphoneAt0); // constant expression
Expression<double> expression(&Event::toa, event_, knownMicrophone_); Expression<double> expression(kToa, event_, knownMicrophone_);
Values values; Values values;
values.insert(key, exampleEvent); values.insert(key, exampleEvent);
double expectedTOA = timeOfEvent + 1. / 330; double expectedTOA = timeOfEvent + 1. / kSpeedOfSound;
EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9); EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9);
} }
@ -97,4 +102,3 @@ int main() {
return TestRegistry::runAllTests(tr); return TestRegistry::runAllTests(tr);
} }
//***************************************************************************** //*****************************************************************************

5
gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp
View File

@ -9,7 +9,8 @@
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam_unstable/geometry/InvDepthCamera3.h> #include <gtsam_unstable/geometry/InvDepthCamera3.h>
@ -18,7 +19,7 @@ using namespace gtsam;
static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); 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)); 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<Cal3_S2> level_camera(level_pose, *K);
/* ************************************************************************* */ /* ************************************************************************* */
TEST( InvDepthFactor, Project1) { TEST( InvDepthFactor, Project1) {

24
gtsam_unstable/gtsam_unstable.h
View File

@ -377,6 +377,30 @@ virtual class RangeFactor : gtsam::NoiseModelFactor {
typedef gtsam::RangeFactor<gtsam::PoseRTV, gtsam::PoseRTV> RangeFactorRTV; typedef gtsam::RangeFactor<gtsam::PoseRTV, gtsam::PoseRTV> RangeFactorRTV;
#include <gtsam_unstable/geometry/Event.h>
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 <gtsam_unstable/slam/TOAFactor.h>
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 <gtsam/nonlinear/NonlinearEquality.h> #include <gtsam/nonlinear/NonlinearEquality.h>
template<T = {gtsam::PoseRTV}> template<T = {gtsam::PoseRTV}>

Some files were not shown because too many files have changed in this diff Show More