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
# install TBB with _debug.so files
function install_tbb()
{
TBB_BASEURL=https://github.com/oneapi-src/oneTBB/releases/download
TBB_VERSION=4.4.2
TBB_DIR=tbb44_20151115oss
TBB_SAVEPATH="/tmp/tbb.tgz"
if [ "$TRAVIS_OS_NAME" == "linux" ]; then
OS_SHORT="lin"
TBB_LIB_DIR="intel64/gcc4.4"
SUDO="sudo"
elif [ "$TRAVIS_OS_NAME" == "osx" ]; then
OS_SHORT="lin"
TBB_LIB_DIR=""
SUDO=""
fi
wget "${TBB_BASEURL}/${TBB_VERSION}/${TBB_DIR}_${OS_SHORT}.tgz" -O $TBB_SAVEPATH
tar -C /tmp -xf $TBB_SAVEPATH
TBBROOT=/tmp/$TBB_DIR
# Copy the needed files to the correct places.
# This works correctly for travis builds, instead of setting path variables.
# This is what Homebrew does to install TBB on Macs
$SUDO cp -R $TBBROOT/lib/$TBB_LIB_DIR/* /usr/local/lib/
$SUDO cp -R $TBBROOT/include/ /usr/local/include/
}
# common tasks before either build or test
function configure()
{
@ -14,6 +46,8 @@ function configure()
rm -fr $BUILD_DIR || true
mkdir $BUILD_DIR && cd $BUILD_DIR
install_tbb
if [ ! -z "$GCC_VERSION" ]; then
export CC=gcc-$GCC_VERSION
export CXX=g++-$GCC_VERSION
@ -24,6 +58,7 @@ function configure()
-DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \
-DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \
-DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
-DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
-DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \
-DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \

8
.travis.yml
View File

@ -18,7 +18,7 @@ addons:
- libboost-all-dev
# before_install:
# - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update ; fi
# - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update; fi
install:
- if [ "$TRAVIS_OS_NAME" == "osx" ]; then HOMEBREW_NO_AUTO_UPDATE=1 brew install ccache ; fi
@ -89,6 +89,12 @@ jobs:
compiler: clang
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON
script: bash .travis.sh -b
# on Linux, with GTSAM_WITH_TBB on to make sure GTSAM still compiles/tests
- stage: special
os: linux
compiler: gcc
env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=ON
script: bash .travis.sh -b
# -------- STAGE 2: TESTS -----------
# on Mac, GCC
- stage: test

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
if(TBB_FOUND AND GTSAM_WITH_TBB)
set(GTSAM_USE_TBB 1) # This will go into config.h
if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020))
set(TBB_GREATER_EQUAL_2020 1)
else()
set(TBB_GREATER_EQUAL_2020 0)
endif()
# all definitions and link requisites will go via imported targets:
# tbb & tbbmalloc
list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc)
@ -416,6 +421,10 @@ add_subdirectory(CppUnitLite)
# Build wrap
if (GTSAM_BUILD_WRAP)
add_subdirectory(wrap)
# suppress warning of cython line being too long
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-misleading-indentation")
endif()
endif(GTSAM_BUILD_WRAP)
# Build GTSAM library

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

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"
# Add prior on the pose having index (key) = 0
graphWithPrior = graph
priorModel = gtsam.noiseModel_Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
graphWithPrior.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
params = gtsam.GaussNewtonParams()
params.setVerbosity("Termination")
params.setMaxIterations(maxIterations)
# parameters.setRelativeErrorTol(1e-5)
# Create the optimizer ...
optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
# ... and optimize
result = optimizer.optimize()

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))
print("Adding prior to g2o file ")
graphWithPrior = graph
firstKey = initial.keys().at(0)
graphWithPrior.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
params = gtsam.GaussNewtonParams()
params.setVerbosity("Termination") # this will show info about stopping conds
optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
result = optimizer.optimize()
print("Optimization complete")
print("initial error = ", graphWithPrior.error(initial))
print("final error = ", graphWithPrior.error(result))
print("initial error = ", graph.error(initial))
print("final error = ", graph.error(result))
if args.output is None:
print("Final Result:\n{}".format(result))

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

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

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

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

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

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

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

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/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <boost/make_shared.hpp>
using namespace gtsam;
@ -47,7 +48,7 @@ public:
/// evaluate the error
virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
boost::none) const {
SimpleCamera camera(pose, *K_);
PinholeCamera<Cal3_S2> camera(pose, *K_);
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)
*/
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/slam/BetweenFactor.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/navigation/ImuBias.h>
#include <gtsam/navigation/ImuFactor.h>
@ -34,7 +35,7 @@ int main(int argc, char* argv[]) {
double radius = 30;
const Point3 up(0, 0, 1), target(0, 0, 0);
const Point3 position(radius, 0, 0);
const SimpleCamera camera = SimpleCamera::Lookat(position, target, up);
const auto camera = PinholeCamera<Cal3_S2>::Lookat(position, target, up);
const auto pose_0 = camera.pose();
// 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
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
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;
GaussNewtonParams params;
@ -77,7 +76,7 @@ int main(const int argc, const char *argv[]) {
}
std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params);
GaussNewtonOptimizer optimizer(*graph, *initial, params);
Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl;

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

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

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

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

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
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) {
Point2 measurement = camera.project(points[j]);
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>
// Header order is close to far
#include <examples/SFMdata.h>
#include "SFMdata.h"
#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/DoglegOptimizer.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
for (size_t i = 0; i < poses.size(); ++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) {
Point2 measurement = camera.project(points[j]);
// 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 triangulation operation inside smart factor will encounter degeneracy.
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);
}
}

2
examples/SFMExample_SmartFactorPCG.cpp
View File

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

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
* @author Duy-Nguyen Ta
*/
@ -30,7 +30,8 @@
#include <gtsam/geometry/Point3.h>
// 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() {

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);
for (size_t i = 0; i < poses.size(); ++i) {
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]);
// 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

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

28
gtsam.h
View File

@ -281,7 +281,7 @@ virtual class Value {
};
#include <gtsam/base/GenericValue.h>
template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class GenericValue : gtsam::Value {
void serializable() const;
};
@ -639,6 +639,7 @@ class Rot3 {
static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft)
static gtsam::Rot3 Ypr(double y, double p, double r);
static gtsam::Rot3 Quaternion(double w, double x, double y, double z);
static gtsam::Rot3 AxisAngle(const gtsam::Point3& axis, double angle);
static gtsam::Rot3 Rodrigues(Vector v);
static gtsam::Rot3 Rodrigues(double wx, double wy, double wz);
static gtsam::Rot3 ClosestTo(const Matrix M);
@ -674,6 +675,7 @@ class Rot3 {
double roll() const;
double pitch() const;
double yaw() const;
pair<gtsam::Unit3, double> axisAngle() const;
// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly
Vector quaternion() const;
gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const;
@ -1310,7 +1312,7 @@ class SymbolicBayesTree {
// class SymbolicBayesTreeClique {
// BayesTreeClique();
// BayesTreeClique(CONDITIONAL* conditional);
// // BayesTreeClique(const std::pair<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;
// void print(string s) const;
@ -1561,17 +1563,15 @@ virtual class Robust : gtsam::noiseModel::Base {
#include <gtsam/linear/Sampler.h>
class Sampler {
//Constructors
// Constructors
Sampler(gtsam::noiseModel::Diagonal* model, int seed);
Sampler(Vector sigmas, int seed);
Sampler(int seed);
//Standard Interface
// Standard Interface
size_t dim() const;
Vector sigmas() const;
gtsam::noiseModel::Diagonal* model() const;
Vector sample();
Vector sampleNewModel(gtsam::noiseModel::Diagonal* model);
};
#include <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::Cal3Bundler& cal3bundler);
void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix);
void insert(size_t j, const gtsam::SimpleCamera& simpel_camera);
void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
void insert(size_t j, Vector vector);
void insert(size_t j, Matrix matrix);
@ -2457,7 +2457,7 @@ class ISAM2 {
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
gtsam::Cal3Bundler, gtsam::EssentialMatrix,
gtsam::SimpleCamera, Vector, Matrix}>
gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, Vector, Matrix}>
VALUE calculateEstimate(size_t key) const;
gtsam::Values calculateBestEstimate() const;
Matrix marginalCovariance(size_t key) const;
@ -2495,7 +2495,7 @@ class NonlinearISAM {
#include <gtsam/geometry/StereoPoint2.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 {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const;
@ -2518,7 +2518,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
#include <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 {
// Constructor - forces exact evaluation
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::Pose3, gtsam::Pose3> RangeFactorPose3;
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::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
typedef gtsam::RangeFactor<gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3_S2> RangeFactorSimpleCamera;
#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);
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
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
@ -3151,7 +3151,7 @@ namespace utilities {
void perturbPoint2(gtsam::Values& values, double sigma, int seed);
void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed);
void perturbPoint3(gtsam::Values& values, double sigma, int seed);
void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth);
void insertBackprojections(gtsam::Values& values, const gtsam::PinholeCameraCal3_S2& c, Vector J, Matrix Z, double depth);
void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K);
void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor);
Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values);

26
gtsam/base/OptionalJacobian.h
View File

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

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

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

2
gtsam/discrete/AlgebraicDecisionTree.h
View File

@ -101,7 +101,7 @@ namespace gtsam {
/** Create a new function splitting on a variable */
template<typename Iterator>
AlgebraicDecisionTree(Iterator begin, Iterator end, const L& label) :
Super(NULL) {
Super(nullptr) {
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++) {
std::stringstream ss;
ss << "factor " << i << ": ";
if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter);
if (factors_[i] != nullptr) factors_[i]->print(ss.str(), formatter);
}
}

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
// For classes with overloaded range methods, such as SimpleCamera, this can even be templated:
// template <typename T> struct Range<SimpleCamera, T> : HasRange<SimpleCamera, T, double> {};
// For classes with overloaded range methods, such as PinholeCamera, this can even be templated:
// template <typename T> struct Range<PinholeCamera, T> : HasRange<PinholeCamera, T, double> {};
template <class A1, typename A2, class RT>
struct HasRange {
typedef RT result_type;

8
gtsam/geometry/Rot3.cpp
View File

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

36
gtsam/geometry/Rot3.h
View File

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

12
gtsam/geometry/Rot3Q.cpp
View File

@ -19,8 +19,8 @@
#ifdef GTSAM_USE_QUATERNIONS
#include <boost/math/constants/constants.hpp>
#include <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp>
#include <cmath>
using namespace std;
@ -79,9 +79,13 @@ namespace gtsam {
}
/* ************************************************************************* */
// TODO: Could we do this? It works in Rot3M but not here, probably because
// here we create an intermediate value by calling matrix()
// const Eigen::Transpose<const Matrix3> Rot3::transpose() const {
// TODO: Maybe use return type `const Eigen::Transpose<const Matrix3>`?
// It works in Rot3M but not here, because of some weird behavior
// due to Eigen's expression templates which needs more investigation.
// For example, if we use matrix(), the value returned has a 1e-10,
// and if we use quaternion_.toRotationMatrix(), the matrix is arbitrary.
// Using eval() here doesn't help, it only helps if we use it in
// the downstream code.
Matrix3 Rot3::transpose() const {
return matrix().transpose();
}

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

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

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

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

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

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

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

@ -14,6 +14,7 @@
* @brief Unit tests for Rot3 class - common between Matrix and Quaternion
* @author Alireza Fathi
* @author Frank Dellaert
* @author Varun Agrawal
*/
#include <gtsam/geometry/Point3.h>
@ -115,6 +116,10 @@ TEST( Rot3, AxisAngle)
CHECK(assert_equal(expected,actual,1e-5));
Rot3 actual2 = Rot3::AxisAngle(axis, angle-2*M_PI);
CHECK(assert_equal(expected,actual2,1e-5));
axis = Vector3(0, 50, 0);
Rot3 actual3 = Rot3::AxisAngle(axis, angle);
CHECK(assert_equal(expected,actual3,1e-5));
}
/* ************************************************************************* */
@ -376,7 +381,7 @@ TEST( Rot3, inverse )
Rot3 actual = R.inverse(actualH);
CHECK(assert_equal(I,R*actual));
CHECK(assert_equal(I,actual*R));
CHECK(assert_equal((Matrix)actual.matrix(), R.transpose()));
CHECK(assert_equal(actual.matrix(), R.transpose()));
Matrix numericalH = numericalDerivative11(testing::inverse<Rot3>, R);
CHECK(assert_equal(numericalH,actualH));
@ -661,6 +666,65 @@ TEST(Rot3, ClosestTo) {
EXPECT(assert_equal(expected, actual.matrix(), 1e-6));
}
/* ************************************************************************* */
TEST(Rot3, axisAngle) {
Unit3 actualAxis;
double actualAngle;
// not a lambda as otherwise we can't trace error easily
#define CHECK_AXIS_ANGLE(expectedAxis, expectedAngle, rotation) \
std::tie(actualAxis, actualAngle) = rotation.axisAngle(); \
EXPECT(assert_equal(expectedAxis, actualAxis, 1e-9)); \
EXPECT_DOUBLES_EQUAL(expectedAngle, actualAngle, 1e-9); \
EXPECT(assert_equal(rotation, Rot3::AxisAngle(expectedAxis, expectedAngle)))
// CHECK R defined at top = Rot3::Rodrigues(0.1, 0.4, 0.2)
Vector3 omega(0.1, 0.4, 0.2);
Unit3 axis(omega), _axis(-omega);
CHECK_AXIS_ANGLE(axis, omega.norm(), R);
// rotate by 90
CHECK_AXIS_ANGLE(Unit3(1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, M_PI_2))
CHECK_AXIS_ANGLE(Unit3(0, 1, 0), M_PI_2, Rot3::Ypr(0, M_PI_2, 0))
CHECK_AXIS_ANGLE(Unit3(0, 0, 1), M_PI_2, Rot3::Ypr(M_PI_2, 0, 0))
CHECK_AXIS_ANGLE(axis, M_PI_2, Rot3::AxisAngle(axis, M_PI_2))
// rotate by -90
CHECK_AXIS_ANGLE(Unit3(-1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, -M_PI_2))
CHECK_AXIS_ANGLE(Unit3(0, -1, 0), M_PI_2, Rot3::Ypr(0, -M_PI_2, 0))
CHECK_AXIS_ANGLE(Unit3(0, 0, -1), M_PI_2, Rot3::Ypr(-M_PI_2, 0, 0))
CHECK_AXIS_ANGLE(_axis, M_PI_2, Rot3::AxisAngle(axis, -M_PI_2))
// rotate by 270
const double theta270 = M_PI + M_PI / 2;
CHECK_AXIS_ANGLE(Unit3(-1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, theta270))
CHECK_AXIS_ANGLE(Unit3(0, -1, 0), M_PI_2, Rot3::Ypr(0, theta270, 0))
CHECK_AXIS_ANGLE(Unit3(0, 0, -1), M_PI_2, Rot3::Ypr(theta270, 0, 0))
CHECK_AXIS_ANGLE(_axis, M_PI_2, Rot3::AxisAngle(axis, theta270))
// rotate by -270
const double theta_270 = -(M_PI + M_PI / 2); // 90 (or -270) degrees
CHECK_AXIS_ANGLE(Unit3(1, 0, 0), M_PI_2, Rot3::Ypr(0, 0, theta_270))
CHECK_AXIS_ANGLE(Unit3(0, 1, 0), M_PI_2, Rot3::Ypr(0, theta_270, 0))
CHECK_AXIS_ANGLE(Unit3(0, 0, 1), M_PI_2, Rot3::Ypr(theta_270, 0, 0))
CHECK_AXIS_ANGLE(axis, M_PI_2, Rot3::AxisAngle(axis, theta_270))
const double theta195 = 195 * M_PI / 180;
const double theta165 = 165 * M_PI / 180;
/// Non-trivial angle 165
CHECK_AXIS_ANGLE(Unit3(1, 0, 0), theta165, Rot3::Ypr(0, 0, theta165))
CHECK_AXIS_ANGLE(Unit3(0, 1, 0), theta165, Rot3::Ypr(0, theta165, 0))
CHECK_AXIS_ANGLE(Unit3(0, 0, 1), theta165, Rot3::Ypr(theta165, 0, 0))
CHECK_AXIS_ANGLE(axis, theta165, Rot3::AxisAngle(axis, theta165))
/// Non-trivial angle 195
CHECK_AXIS_ANGLE(Unit3(-1, 0, 0), theta165, Rot3::Ypr(0, 0, theta195))
CHECK_AXIS_ANGLE(Unit3(0, -1, 0), theta165, Rot3::Ypr(0, theta195, 0))
CHECK_AXIS_ANGLE(Unit3(0, 0, -1), theta165, Rot3::Ypr(theta195, 0, 0))
CHECK_AXIS_ANGLE(_axis, theta165, Rot3::AxisAngle(axis, theta195))
}
/* ************************************************************************* */
int main() {
TestResult tr;

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

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

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

@ -495,7 +495,7 @@ TEST(actualH, Serialization) {
/* ************************************************************************* */
int main() {
srand(time(NULL));
srand(time(nullptr));
TestResult 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) {
for(Key j: clique->conditional()->frontals())
nodes_[j] = clique;
if (parent_clique != NULL) {
if (parent_clique != nullptr) {
clique->parent_ = parent_clique;
parent_clique->children.push_back(clique);
} else {
@ -430,7 +430,7 @@ namespace gtsam {
template <class CLIQUE>
void BayesTree<CLIQUE>::removePath(sharedClique clique, BayesNetType* bn,
Cliques* orphans) {
// base case is NULL, if so we do nothing and return empties above
// base case is nullptr, if so we do nothing and return empties above
if (clique) {
// remove the clique from orphans in case it has been added earlier
orphans->remove(clique);

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.
for (size_t i = 0; i < factors_.size(); i++) {
sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
if (f1 == NULL && f2 == NULL) continue;
if (f1 == NULL || f2 == NULL) return false;
if (f1 == nullptr && f2 == nullptr) continue;
if (f1 == nullptr || f2 == nullptr) return false;
if (!f1->equals(*f2, tol)) return false;
}
return true;

2
gtsam/inference/FactorGraph.h
View File

@ -353,7 +353,7 @@ class FactorGraph {
*/
void resize(size_t size) { factors_.resize(size); }
/** delete factor without re-arranging indexes by inserting a NULL pointer
/** delete factor without re-arranging indexes by inserting a nullptr pointer
*/
void remove(size_t i) { factors_[i].reset(); }

4
gtsam/inference/Ordering.cpp
View File

@ -91,7 +91,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
assert((size_t)count == variableIndex.nEntries());
//double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */
//double* knobs = nullptr; /* colamd arg 6: parameters (uses defaults if nullptr) */
double knobs[CCOLAMD_KNOBS];
ccolamd_set_defaults(knobs);
knobs[CCOLAMD_DENSE_ROW] = -1;
@ -235,7 +235,7 @@ Ordering Ordering::Metis(const MetisIndex& met) {
int outputError;
outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0],
outputError = METIS_NodeND(&size, &xadj[0], &adj[0], nullptr, nullptr, &perm[0],
&iperm[0]);
Ordering result;

2
gtsam/linear/GaussianBayesNet.h
View File

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

2
gtsam/linear/GaussianBayesTree.h
View File

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

2
gtsam/linear/JacobianFactor.cpp
View File

@ -840,7 +840,7 @@ GaussianConditional::shared_ptr JacobianFactor::splitConditional(size_t nrFronta
if (!model_) {
throw std::invalid_argument(
"JacobianFactor::splitConditional cannot be given a NULL noise model");
"JacobianFactor::splitConditional cannot be given a nullptr noise model");
}
if (nrFrontals > size()) {

2
gtsam/linear/JacobianFactor.h
View File

@ -398,7 +398,7 @@ namespace gtsam {
* @param keys in some order
* @param diemnsions of the variables in same order
* @param m output dimension
* @param model noise model (default NULL)
* @param model noise model (default nullptr)
*/
template<class KEYS, class DIMENSIONS>
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 {
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;
}
@ -190,7 +190,7 @@ void Huber::print(const std::string &s="") const {
bool Huber::equals(const Base &expected, double tol) const {
const Huber* p = dynamic_cast<const Huber*>(&expected);
if (p == NULL) return false;
if (p == nullptr) return false;
return std::abs(k_ - p->k_) < tol;
}
@ -223,7 +223,7 @@ void Cauchy::print(const std::string &s="") const {
bool Cauchy::equals(const Base &expected, double tol) const {
const Cauchy* p = dynamic_cast<const Cauchy*>(&expected);
if (p == NULL) return false;
if (p == nullptr) return false;
return std::abs(ksquared_ - p->ksquared_) < tol;
}
@ -266,7 +266,7 @@ void Tukey::print(const std::string &s="") const {
bool Tukey::equals(const Base &expected, double tol) const {
const Tukey* p = dynamic_cast<const Tukey*>(&expected);
if (p == NULL) return false;
if (p == nullptr) return false;
return std::abs(c_ - p->c_) < tol;
}
@ -296,7 +296,7 @@ void Welsch::print(const std::string &s="") const {
bool Welsch::equals(const Base &expected, double tol) const {
const Welsch* p = dynamic_cast<const Welsch*>(&expected);
if (p == NULL) return false;
if (p == nullptr) return false;
return std::abs(c_ - p->c_) < tol;
}
@ -330,7 +330,7 @@ void GemanMcClure::print(const std::string &s="") const {
bool GemanMcClure::equals(const Base &expected, double tol) const {
const GemanMcClure* p = dynamic_cast<const GemanMcClure*>(&expected);
if (p == NULL) return false;
if (p == nullptr) return false;
return std::abs(c_ - p->c_) < tol;
}
@ -372,7 +372,7 @@ void DCS::print(const std::string &s="") const {
bool DCS::equals(const Base &expected, double tol) const {
const DCS* p = dynamic_cast<const DCS*>(&expected);
if (p == NULL) return false;
if (p == nullptr) return false;
return std::abs(c_ - p->c_) < tol;
}
@ -411,7 +411,7 @@ void L2WithDeadZone::print(const std::string &s="") const {
bool L2WithDeadZone::equals(const Base &expected, double tol) const {
const L2WithDeadZone* p = dynamic_cast<const L2WithDeadZone*>(&expected);
if (p == NULL) return false;
if (p == nullptr) return false;
return std::abs(k_ - p->k_) < tol;
}

9
gtsam/linear/NoiseModel.cpp
View File

@ -25,7 +25,6 @@
#include <cmath>
#include <iostream>
#include <limits>
#include <random>
#include <stdexcept>
#include <typeinfo>
@ -134,7 +133,7 @@ void Gaussian::print(const string& name) const {
/* ************************************************************************* */
bool Gaussian::equals(const Base& expected, double tol) const {
const Gaussian* p = dynamic_cast<const Gaussian*> (&expected);
if (p == NULL) return false;
if (p == nullptr) return false;
if (typeid(*this) != typeid(*p)) return false;
//TODO(Alex);
//if (!sqrt_information_) return true;
@ -168,7 +167,7 @@ Vector Gaussian::unwhiten(const Vector& v) const {
}
/* ************************************************************************* */
double Gaussian::Mahalanobis(const Vector& v) const {
double Gaussian::squaredMahalanobisDistance(const Vector& v) const {
// Note: for Diagonal, which does ediv_, will be correct for constraints
Vector w = whiten(v);
return w.dot(w);
@ -584,7 +583,7 @@ void Isotropic::print(const string& name) const {
}
/* ************************************************************************* */
double Isotropic::Mahalanobis(const Vector& v) const {
double Isotropic::squaredMahalanobisDistance(const Vector& v) const {
return v.dot(v) * invsigma_ * invsigma_;
}
@ -636,7 +635,7 @@ void Robust::print(const std::string& name) const {
bool Robust::equals(const Base& expected, double tol) const {
const Robust* p = dynamic_cast<const Robust*> (&expected);
if (p == NULL) return false;
if (p == nullptr) return false;
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;
/**
* 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 {
return Mahalanobis(v);
return squaredMahalanobisDistance(v);
}
/**
@ -564,7 +577,7 @@ namespace gtsam {
}
virtual void print(const std::string& name) const;
virtual double Mahalanobis(const Vector& v) const;
virtual double squaredMahalanobisDistance(const Vector& v) const;
virtual Vector whiten(const Vector& v) const;
virtual Vector unwhiten(const Vector& v) const;
virtual Matrix Whiten(const Matrix& H) const;
@ -616,7 +629,7 @@ namespace gtsam {
virtual bool isUnit() const { return true; }
virtual void print(const std::string& name) const;
virtual double Mahalanobis(const Vector& v) const {return v.dot(v); }
virtual double squaredMahalanobisDistance(const Vector& v) const {return v.dot(v); }
virtual Vector whiten(const Vector& v) const { return v; }
virtual Vector unwhiten(const Vector& v) const { return v; }
virtual Matrix Whiten(const Matrix& H) const { return H; }

34
gtsam/linear/Sampler.cpp
View File

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

79
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
* the given distribution
* @brief sampling from a NoiseModel
* @author Frank Dellaert
* @author Alex Cunningham
*/
@ -27,67 +27,74 @@ namespace gtsam {
/**
* Sampling structure that keeps internal random number generators for
* diagonal distributions specified by NoiseModel
*
* This is primarily to allow for variable seeds, and does roughly the same
* thing as sample() in NoiseModel.
*/
class GTSAM_EXPORT Sampler {
protected:
protected:
/** noiseModel created at generation */
noiseModel::Diagonal::shared_ptr model_;
/** generator */
std::mt19937_64 generator_;
mutable std::mt19937_64 generator_;
public:
public:
typedef boost::shared_ptr<Sampler> shared_ptr;
/// @name constructors
/// @{
/**
* Create a sampler for the distribution specified by a diagonal NoiseModel
* with a manually specified seed
*
* NOTE: do not use zero as a seed, it will break the generator
*/
Sampler(const noiseModel::Diagonal::shared_ptr& model, int32_t seed = 42u);
explicit Sampler(const noiseModel::Diagonal::shared_ptr& model,
uint_fast64_t seed = 42u);
/**
* Create a sampler for a distribution specified by a vector of sigmas directly
* Create a sampler for a distribution specified by a vector of sigmas
* directly
*
* NOTE: do not use zero as a seed, it will break the generator
*/
Sampler(const Vector& sigmas, int32_t seed = 42u);
explicit Sampler(const Vector& sigmas, uint_fast64_t seed = 42u);
/**
* Create a sampler without a given noisemodel - pass in to sample
*
* NOTE: do not use zero as a seed, it will break the generator
*/
Sampler(int32_t seed = 42u);
/// @}
/// @name access functions
/// @{
size_t dim() const {
assert(model_.get());
return model_->dim();
}
Vector sigmas() const {
assert(model_.get());
return model_->sigmas();
}
/** access functions */
size_t dim() const { assert(model_.get()); return model_->dim(); }
Vector sigmas() const { assert(model_.get()); return model_->sigmas(); }
const noiseModel::Diagonal::shared_ptr& model() const { return model_; }
/**
* sample from distribution
* NOTE: not const due to need to update the underlying generator
*/
Vector sample();
/// @}
/// @name basic functionality
/// @{
/**
* Sample from noisemodel passed in as an argument,
* can be used without having initialized a model for the system.
*
* NOTE: not const due to need to update the underlying generator
*/
Vector sampleNewModel(const noiseModel::Diagonal::shared_ptr& model);
/// sample from distribution
Vector sample() const;
protected:
/// @}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
explicit Sampler(uint_fast64_t seed = 42u);
Vector sampleNewModel(const noiseModel::Diagonal::shared_ptr& model) const;
/// @}
#endif
protected:
/** given sigmas for a diagonal model, returns a sample */
Vector sampleDiagonal(const Vector& sigmas);
Vector sampleDiagonal(const Vector& sigmas) const;
};
} // \namespace gtsam
} // namespace gtsam

28
gtsam/linear/VectorValues.cpp
View File

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

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
* returned. */
std::pair<iterator, bool> tryInsert(Key j, const Vector& value) {
#ifdef TBB_GREATER_EQUAL_2020
return values_.emplace(j, value);
#else
return values_.insert(std::make_pair(j, value));
#endif
}
/** Erase the vector with the given key, or throw std::out_of_range if it does not exist */

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

@ -253,7 +253,7 @@ TEST(JacobianFactor, error)
/* ************************************************************************* */
TEST(JacobianFactor, matrices_NULL)
{
// Make sure everything works with NULL noise model
// Make sure everything works with nullptr noise model
JacobianFactor factor(simple::terms, simple::b);
Matrix jacobianExpected(3, 9);

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

@ -68,10 +68,10 @@ TEST(NoiseModel, constructors)
for(Gaussian::shared_ptr mi: m)
EXPECT(assert_equal(unwhitened,mi->unwhiten(whitened)));
// test Mahalanobis distance
// test squared Mahalanobis distance
double distance = 5*5+10*10+15*15;
for(Gaussian::shared_ptr mi: m)
DOUBLES_EQUAL(distance,mi->Mahalanobis(unwhitened),1e-9);
DOUBLES_EQUAL(distance,mi->squaredMahalanobisDistance(unwhitened),1e-9);
// test R matrix
for(Gaussian::shared_ptr mi: m)

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 Frank Dellaert
*/
#include <CppUnitLite/TestHarness.h>
@ -22,14 +24,15 @@ using namespace gtsam;
const double tol = 1e-5;
static const Vector3 kSigmas(1.0, 0.1, 0.0);
/* ************************************************************************* */
TEST(testSampler, basic) {
Vector sigmas = Vector3(1.0, 0.1, 0.0);
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(sigmas);
auto model = noiseModel::Diagonal::Sigmas(kSigmas);
char seed = 'A';
Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1);
EXPECT(assert_equal(sigmas, sampler1.sigmas()));
EXPECT(assert_equal(sigmas, sampler2.sigmas()));
EXPECT(assert_equal(kSigmas, sampler1.sigmas()));
EXPECT(assert_equal(kSigmas, sampler2.sigmas()));
EXPECT_LONGS_EQUAL(3, sampler1.dim());
EXPECT_LONGS_EQUAL(3, sampler2.dim());
Vector actual1 = sampler1.sample();
@ -38,5 +41,8 @@ TEST(testSampler, basic) {
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

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 {
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,
double tol) const {
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);
}
@ -69,7 +69,7 @@ void Pose3AttitudeFactor::print(const string& s,
bool Pose3AttitudeFactor::equals(const NonlinearFactor& expected,
double tol) const {
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);
}

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 {
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 {
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 {
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);
}

2
gtsam/navigation/NavState.h
View File

@ -64,7 +64,7 @@ public:
R_(pose.rotation()), t_(pose.translation()), v_(v) {
}
/// Construct from SO(3) and R^6
NavState(const Matrix3& R, const Vector9 tv) :
NavState(const Matrix3& R, const Vector6& tv) :
R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
}
/// Named constructor with derivatives

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

2
gtsam/navigation/ScenarioRunner.h
View File

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

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

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

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

@ -64,9 +64,9 @@ TEST(ImuFactor, serialization) {
ImuFactor factor(1, 2, 3, 4, 5, pim);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
EXPECT(equalsObj<ImuFactor>(factor));
EXPECT(equalsXML<ImuFactor>(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.
// The meta-functions below are useful to specify the type of those functions.
// Example, a function taking a camera and a 3D point and yielding a 2D point:
// Expression<Point2>::BinaryFunction<SimpleCamera,Point3>::type
// Expression<Point2>::BinaryFunction<PinholeCamera<Cal3_S2>,Point3>::type
template<class A1>
struct UnaryFunction {
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
/// fixed-lag smoother, enable this option to add factors in the first
/// available factor slots, to avoid accumulating NULL factor slots, at the
/// available factor slots, to avoid accumulating nullptr factor slots, at the
/// cost of having to search for slots every time a factor is added.
bool findUnusedFactorSlots;

6
gtsam/nonlinear/NonlinearEquality.h
View File

@ -175,6 +175,8 @@ public:
/// @}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
/** Serialization function */
@ -263,6 +265,8 @@ public:
traits<X>::Print(value_, "Value");
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
/** Serialization function */
@ -327,6 +331,8 @@ public:
return traits<X>::Local(x1,x2);
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
/** 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++) {
stringstream ss;
ss << "Factor " << i << ": ";
if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter);
if (factors_[i] != nullptr) factors_[i]->print(ss.str(), keyFormatter);
cout << endl;
}
}
@ -67,8 +67,8 @@ void NonlinearFactorGraph::printErrors(const Values& values, const std::string&
for (size_t i = 0; i < factors_.size(); i++) {
stringstream ss;
ss << "Factor " << i << ": ";
if (factors_[i] == NULL) {
cout << "NULL" << endl;
if (factors_[i] == nullptr) {
cout << "nullptr" << endl;
} else {
factors_[i]->print(ss.str(), keyFormatter);
cout << "error = " << factors_[i]->error(values) << endl;

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

@ -98,7 +98,7 @@ struct Record: public internal::CallRecordImplementor<Record, Cols> {
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;

11
gtsam/nonlinear/utilities.h
View File

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

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

@ -19,7 +19,8 @@
#include <gtsam/sam/RangeFactor.h>
#include <gtsam/geometry/Pose3.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/serializationTestHelpers.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) {
RangeFactor<SimpleCamera,Point3> factor1(poseKey, pointKey, measurement, model);
RangeFactor<SimpleCamera,Pose3> factor2(poseKey, pointKey, measurement, model);
RangeFactor<SimpleCamera,SimpleCamera> factor3(poseKey, pointKey, measurement, model);
using Camera = PinholeCamera<Cal3_S2>;
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 */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
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 */

2
gtsam/slam/BetweenFactor.h
View File

@ -81,7 +81,7 @@ namespace gtsam {
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
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 */

2
gtsam/slam/EssentialMatrixConstraint.cpp
View File

@ -37,7 +37,7 @@ void EssentialMatrixConstraint::print(const std::string& s,
bool EssentialMatrixConstraint::equals(const NonlinearFactor& expected,
double tol) const {
const This *e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol)
return e != nullptr && Base::equals(*e, 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/geometry/EssentialMatrix.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <iostream>
namespace gtsam {

2
gtsam/slam/JacobianFactorSVD.h
View File

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

2
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -31,7 +31,7 @@ void OrientedPlane3DirectionPrior::print(const string& s,
bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected,
double tol) const {
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);
}

2
gtsam/slam/PoseRotationPrior.h
View File

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

2
gtsam/slam/PoseTranslationPrior.h
View File

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

2
gtsam/slam/PriorFactor.h
View File

@ -85,7 +85,7 @@ namespace gtsam {
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
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 */

3
gtsam/slam/ProjectionFactor.h
View File

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

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

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

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

@ -175,7 +175,7 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) {
/* ************************************************************************* */
int main() {
srand(time(NULL));
srand(time(nullptr));
TestResult 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/SimpleCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/Cal3Bundler.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)
static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2);
static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
SimpleCamera camera1(pose1, *sharedCal);
PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);
// landmark ~5 meters infront of camera
static const Point3 landmark(5, 0.5, 1.2);
@ -52,7 +52,7 @@ TEST( triangulation, TriangulationFactor ) {
Key pointKey(1);
SharedNoiseModel model;
typedef TriangulationFactor<SimpleCamera> Factor;
typedef TriangulationFactor<PinholeCamera<Cal3_S2> > Factor;
Factor factor(camera1, z1, model, pointKey);
// Use the factor to calculate the Jacobians

4
gtsam_unstable/base/BTree.h
View File

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

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 {
std::cout << s << "time = " << time_ << "location = " << location_.transpose();
std::cout << s << "{'time':" << time_
<< ", 'location': " << location_.transpose() << "}";
}
/* ************************************************************************* */
bool Event::equals(const Event& other, double tol) const {
return std::abs(time_ - other.time_) < tol
&& traits<Point3>::Equals(location_, other.location_, tol);
return std::abs(time_ - other.time_) < tol &&
traits<Point3>::Equals(location_, other.location_, tol);
}
/* ************************************************************************* */
} //\ namespace gtsam
} // namespace gtsam

94
gtsam_unstable/geometry/Event.h
View File

@ -20,42 +20,43 @@
#pragma once
#include <gtsam/geometry/Point3.h>
#include <gtsam_unstable/dllexport.h>
#include <cmath>
#include <iosfwd>
#include <gtsam_unstable/dllexport.h>
#include <string>
namespace gtsam {
/// A space-time event
/**
* A space-time event models an event that happens at a certain 3D location, at
* a certain time. One use for it is in sound-based or UWB-ranging tracking or
* SLAM, where we have "time of arrival" measurements at a set of sensors. The
* TOA functor below provides a measurement function for those applications.
*/
class Event {
double time_; ///< Time event was generated
Point3 location_; ///< Location at time event was generated
public:
public:
enum { dimension = 4 };
/// Default Constructor
Event() :
time_(0), location_(0,0,0) {
}
Event() : time_(0), location_(0, 0, 0) {}
/// Constructor from time and location
Event(double t, const Point3& p) :
time_(t), location_(p) {
}
Event(double t, const Point3& p) : time_(t), location_(p) {}
/// Constructor with doubles
Event(double t, double x, double y, double z) :
time_(t), location_(x, y, z) {
}
Event(double t, double x, double y, double z)
: time_(t), location_(x, y, z) {}
double time() const { return time_;}
Point3 location() const { return location_;}
double time() const { return time_; }
Point3 location() const { return location_; }
// TODO we really have to think of a better way to do linear arguments
double height(OptionalJacobian<1,4> H = boost::none) const {
static const Matrix14 JacobianZ = (Matrix14() << 0,0,0,1).finished();
// TODO(frank) we really have to think of a better way to do linear arguments
double height(OptionalJacobian<1, 4> H = boost::none) const {
static const Matrix14 JacobianZ = (Matrix14() << 0, 0, 0, 1).finished();
if (H) *H = JacobianZ;
return location_.z();
}
@ -64,7 +65,8 @@ public:
GTSAM_UNSTABLE_EXPORT void print(const std::string& s = "") const;
/** equals with an tolerance */
GTSAM_UNSTABLE_EXPORT bool equals(const Event& other, double tol = 1e-9) const;
GTSAM_UNSTABLE_EXPORT bool equals(const Event& other,
double tol = 1e-9) const;
/// Updates a with tangent space delta
inline Event retract(const Vector4& v) const {
@ -73,28 +75,44 @@ public:
/// Returns inverse retraction
inline Vector4 localCoordinates(const Event& q) const {
return Vector4::Zero(); // TODO
}
/// Time of arrival to given microphone
double toa(const Point3& microphone, //
OptionalJacobian<1, 4> H1 = boost::none, //
OptionalJacobian<1, 3> H2 = boost::none) const {
static const double Speed = 330;
Matrix13 D1, D2;
double distance = gtsam::distance3(location_, microphone, D1, D2);
if (H1)
// derivative of toa with respect to event
*H1 << 1.0, D1 / Speed;
if (H2)
// derivative of toa with respect to microphone location
*H2 << D2 / Speed;
return time_ + distance / Speed;
return Vector4::Zero(); // TODO(frank) implement!
}
};
// Define GTSAM traits
template<>
template <>
struct traits<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

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

@ -17,10 +17,12 @@
* @date December 2014
*/
#include <gtsam_unstable/geometry/Event.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/nonlinear/Expression.h>
#include <gtsam_unstable/geometry/Event.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
using namespace std;
@ -30,56 +32,59 @@ using namespace gtsam;
static const double ms = 1e-3;
static const double cm = 1e-2;
typedef Eigen::Matrix<double, 1, 1> Vector1;
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1,0.5*ms));
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms));
static const double timeOfEvent = 25;
static const Event exampleEvent(timeOfEvent, 1, 0, 0);
static const Point3 microphoneAt0(0,0,0);
static const Point3 microphoneAt0(0, 0, 0);
static const double kSpeedOfSound = 340;
static const TimeOfArrival kToa(kSpeedOfSound);
//*****************************************************************************
TEST( Event, Constructor ) {
TEST(Event, Constructor) {
const double t = 0;
Event actual(t, 201.5 * cm, 201.5 * cm, (212 - 45) * cm);
}
//*****************************************************************************
TEST( Event, Toa1 ) {
TEST(Event, Toa1) {
Event event(0, 1, 0, 0);
double expected = 1. / 330;
EXPECT_DOUBLES_EQUAL(expected, event.toa(microphoneAt0), 1e-9);
double expected = 1. / kSpeedOfSound;
EXPECT_DOUBLES_EQUAL(expected, kToa(event, microphoneAt0), 1e-9);
}
//*****************************************************************************
TEST( Event, Toa2 ) {
double expectedTOA = timeOfEvent + 1. / 330;
EXPECT_DOUBLES_EQUAL(expectedTOA, exampleEvent.toa(microphoneAt0), 1e-9);
TEST(Event, Toa2) {
double expectedTOA = timeOfEvent + 1. / kSpeedOfSound;
EXPECT_DOUBLES_EQUAL(expectedTOA, kToa(exampleEvent, microphoneAt0), 1e-9);
}
//*************************************************************************
TEST (Event, Derivatives) {
TEST(Event, Derivatives) {
Matrix14 actualH1;
Matrix13 actualH2;
exampleEvent.toa(microphoneAt0, actualH1, actualH2);
kToa(exampleEvent, microphoneAt0, actualH1, actualH2);
Matrix expectedH1 = numericalDerivative11<double, Event>(
boost::bind(&Event::toa, _1, microphoneAt0, boost::none, boost::none),
boost::bind(kToa, _1, microphoneAt0, boost::none, boost::none),
exampleEvent);
EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
Matrix expectedH2 = numericalDerivative11<double, Point3>(
boost::bind(&Event::toa, exampleEvent, _1, boost::none, boost::none),
boost::bind(kToa, exampleEvent, _1, boost::none, boost::none),
microphoneAt0);
EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
}
//*****************************************************************************
TEST( Event, Expression ) {
TEST(Event, Expression) {
Key key = 12;
Expression<Event> event_(key);
Expression<Point3> knownMicrophone_(microphoneAt0); // constant expression
Expression<double> expression(&Event::toa, event_, knownMicrophone_);
Expression<double> expression(kToa, event_, knownMicrophone_);
Values values;
values.insert(key, exampleEvent);
double expectedTOA = timeOfEvent + 1. / 330;
double expectedTOA = timeOfEvent + 1. / kSpeedOfSound;
EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9);
}
@ -97,4 +102,3 @@ int main() {
return TestRegistry::runAllTests(tr);
}
//*****************************************************************************

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

@ -9,7 +9,8 @@
#include <gtsam/base/numericalDerivative.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>
@ -18,7 +19,7 @@ using namespace gtsam;
static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
SimpleCamera level_camera(level_pose, *K);
PinholeCamera<Cal3_S2> level_camera(level_pose, *K);
/* ************************************************************************* */
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;
#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>
template<T = {gtsam::PoseRTV}>

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