Merge branch 'develop' into fix/todos
Varun Agrawal
commit
d74e2666d3
35
.travis.sh
35
.travis.sh
|
|
@ -1,5 +1,37 @@
|
|||
#!/bin/bash
|
||||
|
||||
# install TBB with _debug.so files
|
||||
function install_tbb()
|
||||
{
|
||||
TBB_BASEURL=https://github.com/oneapi-src/oneTBB/releases/download
|
||||
TBB_VERSION=4.4.2
|
||||
TBB_DIR=tbb44_20151115oss
|
||||
TBB_SAVEPATH="/tmp/tbb.tgz"
|
||||
|
||||
if [ "$TRAVIS_OS_NAME" == "linux" ]; then
|
||||
OS_SHORT="lin"
|
||||
TBB_LIB_DIR="intel64/gcc4.4"
|
||||
SUDO="sudo"
|
||||
|
||||
elif [ "$TRAVIS_OS_NAME" == "osx" ]; then
|
||||
OS_SHORT="lin"
|
||||
TBB_LIB_DIR=""
|
||||
SUDO=""
|
||||
|
||||
fi
|
||||
|
||||
wget "${TBB_BASEURL}/${TBB_VERSION}/${TBB_DIR}_${OS_SHORT}.tgz" -O $TBB_SAVEPATH
|
||||
tar -C /tmp -xf $TBB_SAVEPATH
|
||||
|
||||
TBBROOT=/tmp/$TBB_DIR
|
||||
# Copy the needed files to the correct places.
|
||||
# This works correctly for travis builds, instead of setting path variables.
|
||||
# This is what Homebrew does to install TBB on Macs
|
||||
$SUDO cp -R $TBBROOT/lib/$TBB_LIB_DIR/* /usr/local/lib/
|
||||
$SUDO cp -R $TBBROOT/include/ /usr/local/include/
|
||||
|
||||
}
|
||||
|
||||
# common tasks before either build or test
|
||||
function configure()
|
||||
{
|
||||
|
|
@ -14,6 +46,8 @@ function configure()
|
|||
rm -fr $BUILD_DIR || true
|
||||
mkdir $BUILD_DIR && cd $BUILD_DIR
|
||||
|
||||
install_tbb
|
||||
|
||||
if [ ! -z "$GCC_VERSION" ]; then
|
||||
export CC=gcc-$GCC_VERSION
|
||||
export CXX=g++-$GCC_VERSION
|
||||
|
|
@ -24,6 +58,7 @@ function configure()
|
|||
-DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \
|
||||
-DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \
|
||||
-DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
|
||||
-DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
|
||||
-DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \
|
||||
-DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \
|
||||
-DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
||||
|
|
|
|||
|
|
@ -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))
|
||||
|
|
|
|||
|
|
@ -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(
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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):
|
||||
|
|
|
|||
|
|
@ -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")
|
||||
|
|
@ -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_;
|
||||
}
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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>
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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() {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
22
gtsam.h
22
gtsam.h
|
|
@ -639,6 +639,7 @@ class Rot3 {
|
|||
static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft)
|
||||
static gtsam::Rot3 Ypr(double y, double p, double r);
|
||||
static gtsam::Rot3 Quaternion(double w, double x, double y, double z);
|
||||
static gtsam::Rot3 AxisAngle(const gtsam::Point3& axis, double angle);
|
||||
static gtsam::Rot3 Rodrigues(Vector v);
|
||||
static gtsam::Rot3 Rodrigues(double wx, double wy, double wz);
|
||||
static gtsam::Rot3 ClosestTo(const Matrix M);
|
||||
|
|
@ -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;
|
||||
|
|
@ -1564,14 +1566,12 @@ class Sampler {
|
|||
// Constructors
|
||||
Sampler(gtsam::noiseModel::Diagonal* model, int seed);
|
||||
Sampler(Vector sigmas, int seed);
|
||||
Sampler(int seed);
|
||||
|
||||
// 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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -183,7 +183,7 @@ TEST (OrientedPlane3, jacobian_retract) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
srand(time(NULL));
|
||||
srand(time(nullptr));
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -495,7 +495,7 @@ TEST(actualH, Serialization) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
srand(time(NULL));
|
||||
srand(time(nullptr));
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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(); }
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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()) {
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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; }
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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,9 +27,6 @@ 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:
|
||||
|
|
@ -37,57 +34,67 @@ protected:
|
|||
noiseModel::Diagonal::shared_ptr model_;
|
||||
|
||||
/** generator */
|
||||
std::mt19937_64 generator_;
|
||||
mutable std::mt19937_64 generator_;
|
||||
|
||||
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;
|
||||
|
||||
/// @}
|
||||
|
||||
#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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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<
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -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))
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
||||
|
|
|
|||
|
|
@ -175,7 +175,7 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
srand(time(NULL));
|
||||
srand(time(nullptr));
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -20,15 +20,21 @@
|
|||
#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
|
||||
|
||||
|
|
@ -36,24 +42,19 @@ 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_; }
|
||||
|
||||
// TODO we really have to think of a better way to do linear arguments
|
||||
// TODO(frank) we really have to think of a better way to do linear arguments
|
||||
double height(OptionalJacobian<1, 4> H = boost::none) const {
|
||||
static const Matrix14 JacobianZ = (Matrix14() << 0, 0, 0, 1).finished();
|
||||
if (H) *H = JacobianZ;
|
||||
|
|
@ -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,23 +75,7 @@ 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!
|
||||
}
|
||||
};
|
||||
|
||||
|
|
@ -97,4 +83,36 @@ public:
|
|||
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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
@ -36,6 +38,9 @@ static const double timeOfEvent = 25;
|
|||
static const Event exampleEvent(timeOfEvent, 1, 0, 0);
|
||||
static const Point3 microphoneAt0(0, 0, 0);
|
||||
|
||||
static const double kSpeedOfSound = 340;
|
||||
static const TimeOfArrival kToa(kSpeedOfSound);
|
||||
|
||||
//*****************************************************************************
|
||||
TEST(Event, Constructor) {
|
||||
const double t = 0;
|
||||
|
|
@ -45,27 +50,27 @@ TEST( Event, Constructor ) {
|
|||
//*****************************************************************************
|
||||
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);
|
||||
double expectedTOA = timeOfEvent + 1. / kSpeedOfSound;
|
||||
EXPECT_DOUBLES_EQUAL(expectedTOA, kToa(exampleEvent, microphoneAt0), 1e-9);
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
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));
|
||||
}
|
||||
|
|
@ -75,11 +80,11 @@ 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);
|
||||
}
|
||||
//*****************************************************************************
|
||||
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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
Loading…
Reference in New Issue