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

Merge branch 'develop' of github.com:borglab/gtsam into feature/1dsfm

release/4.3a0
akrishnan86 2020-06-29 20:05:56 -07:00
parent 9d9c30e5dc 8ebd7f7f54
commit 0e6dc6a016
315 changed files with 41719 additions and 1271 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
.github/workflows/trigger-python.yml vendored Normal file
View File

@ -0,0 +1,14 @@
# This triggers Cython builds on `gtsam-manylinux-build`
name: Trigger Python Builds
on: push
jobs:
triggerCython:
runs-on: ubuntu-latest
steps:
- name: Repository Dispatch
uses: ProfFan/repository-dispatch@master
with:
token: ${{ secrets.PYTHON_CI_REPO_ACCESS_TOKEN }}
repository: borglab/gtsam-manylinux-build
event-type: cython-wrapper
client-payload: '{"ref": "${{ github.ref }}", "sha": "${{ github.sha }}"}'

2
.travis.sh
View File

@ -63,7 +63,7 @@ function configure()
-DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-DCMAKE_VERBOSE_MAKEFILE=ON
-DCMAKE_VERBOSE_MAKEFILE=OFF
}

5
.travis.yml
View File

@ -14,7 +14,8 @@ addons:
- clang-9
- build-essential pkg-config
- cmake
- libpython-dev python-numpy
- python3-dev libpython-dev
- python3-numpy
- libboost-all-dev
# before_install:
@ -94,7 +95,7 @@ jobs:
os: linux
compiler: gcc
env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=ON
script: bash .travis.sh -b
script: bash .travis.sh -t
# -------- STAGE 2: TESTS -----------
# on Mac, GCC
- stage: test

1
CMakeLists.txt
View File

@ -22,6 +22,7 @@ set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM
set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
include(GtsamMakeConfigFile)
include(GNUInstallDirs)
# Record the root dir for gtsam - needed during external builds, e.g., ROS
set(GTSAM_SOURCE_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR})

4
CppUnitLite/CMakeLists.txt
View File

@ -12,6 +12,6 @@ gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC
option(GTSAM_INSTALL_CPPUNITLITE "Enable/Disable installation of CppUnitLite library" ON)
if (GTSAM_INSTALL_CPPUNITLITE)
install(FILES ${cppunitlite_headers} DESTINATION include/CppUnitLite)
install(TARGETS CppUnitLite EXPORT GTSAM-exports ARCHIVE DESTINATION lib)
install(FILES ${cppunitlite_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/CppUnitLite)
install(TARGETS CppUnitLite EXPORT GTSAM-exports ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR})
endif(GTSAM_INSTALL_CPPUNITLITE)

9
cmake/GtsamCythonWrap.cmake
View File

@ -87,6 +87,15 @@ endfunction()
# - output_dir: The output directory
function(build_cythonized_cpp target cpp_file output_lib_we output_dir)
add_library(${target} MODULE ${cpp_file})
if(WIN32)
# Use .pyd extension instead of .dll on Windows
set_target_properties(${target} PROPERTIES SUFFIX ".pyd")
# Add full path to the Python library
target_link_libraries(${target} ${PYTHON_LIBRARIES})
endif()
if(APPLE)
set(link_flags "-undefined dynamic_lookup")
endif()

5
cython/CMakeLists.txt
View File

@ -7,6 +7,11 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
add_subdirectory(gtsam_eigency)
include_directories(${PROJECT_BINARY_DIR}/cython/gtsam_eigency)
# Fix for error "C1128: number of sections exceeded object file format limit"
if(MSVC)
add_compile_options(/bigobj)
endif()
# wrap gtsam
add_custom_target(gtsam_header DEPENDS "../gtsam.h")
wrap_and_install_library_cython("../gtsam.h" # interface_header

42
cython/gtsam/examples/ImuFactorExample.py
View File

@ -5,32 +5,27 @@ All Rights Reserved
See LICENSE for the license information
A script validating the ImuFactor inference.
A script validating and demonstrating the ImuFactor inference.
Author: Frank Dellaert, Varun Agrawal
"""
from __future__ import print_function
import math
import gtsam
import matplotlib.pyplot as plt
import numpy as np
from gtsam import symbol_shorthand_B as B
from gtsam import symbol_shorthand_V as V
from gtsam import symbol_shorthand_X as X
from gtsam.utils.plot import plot_pose3
from mpl_toolkits.mplot3d import Axes3D
import gtsam
from gtsam.utils.plot import plot_pose3
from PreintegrationExample import POSES_FIG, PreintegrationExample
BIAS_KEY = int(gtsam.symbol(ord('b'), 0))
def X(key):
"""Create symbol for pose key."""
return gtsam.symbol(ord('x'), key)
def V(key):
"""Create symbol for velocity key."""
return gtsam.symbol(ord('v'), key)
BIAS_KEY = B(0)
np.set_printoptions(precision=3, suppress=True)
@ -76,8 +71,14 @@ class ImuFactorExample(PreintegrationExample):
initial.insert(BIAS_KEY, self.actualBias)
for i in range(num_poses):
state_i = self.scenario.navState(float(i))
initial.insert(X(i), state_i.pose())
initial.insert(V(i), state_i.velocity())
poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1)
pose = state_i.pose().compose(poseNoise)
velocity = state_i.velocity() + np.random.randn(3)*0.1
initial.insert(X(i), pose)
initial.insert(V(i), velocity)
# simulate the loop
i = 0 # state index
@ -88,6 +89,12 @@ class ImuFactorExample(PreintegrationExample):
measuredAcc = self.runner.measuredSpecificForce(t)
pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
poseNoise = gtsam.Pose3.Expmap(np.random.randn(3)*0.1)
actual_state_i = gtsam.NavState(
actual_state_i.pose().compose(poseNoise),
actual_state_i.velocity() + np.random.randn(3)*0.1)
# Plot IMU many times
if k % 10 == 0:
self.plotImu(t, measuredOmega, measuredAcc)
@ -133,6 +140,9 @@ class ImuFactorExample(PreintegrationExample):
pose_i = result.atPose3(X(i))
plot_pose3(POSES_FIG, pose_i, 0.1)
i += 1
gtsam.utils.plot.set_axes_equal(POSES_FIG)
print(result.atimuBias_ConstantBias(BIAS_KEY))
plt.ioff()

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

@ -8,27 +8,19 @@ from __future__ import print_function
import math
import matplotlib.pyplot as plt
import numpy as np
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
import gtsam
import gtsam.utils.plot as gtsam_plot
import matplotlib.pyplot as plt
import numpy as np
from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2,
ConstantTwistScenario, ImuFactor, NonlinearFactorGraph,
PinholeCameraCal3_S2, Point3, Pose3,
PriorFactorConstantBias, PriorFactorPose3,
PriorFactorVector, Rot3, Values)
def X(key):
"""Create symbol for pose key."""
return gtsam.symbol(ord('x'), key)
def V(key):
"""Create symbol for velocity key."""
return gtsam.symbol(ord('v'), key)
from gtsam import symbol_shorthand_B as B
from gtsam import symbol_shorthand_V as V
from gtsam import symbol_shorthand_X as X
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
def vector3(x, y, z):
@ -115,7 +107,7 @@ def IMU_example():
newgraph.push_back(PriorFactorPose3(X(0), pose_0, noise))
# Add imu priors
biasKey = gtsam.symbol(ord('b'), 0)
biasKey = B(0)
biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
biasnoise)

15
cython/gtsam/examples/PlanarSLAMExample.py
View File

@ -13,9 +13,10 @@ Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
from __future__ import print_function
import numpy as np
import gtsam
import numpy as np
from gtsam import symbol_shorthand_L as L
from gtsam import symbol_shorthand_X as X
# Create noise models
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
@ -26,11 +27,11 @@ MEASUREMENT_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2]))
graph = gtsam.NonlinearFactorGraph()
# Create the keys corresponding to unknown variables in the factor graph
X1 = gtsam.symbol(ord('x'), 1)
X2 = gtsam.symbol(ord('x'), 2)
X3 = gtsam.symbol(ord('x'), 3)
L1 = gtsam.symbol(ord('l'), 4)
L2 = gtsam.symbol(ord('l'), 5)
X1 = X(1)
X2 = X(2)
X3 = X(3)
L1 = L(4)
L2 = L(5)
# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model
graph.add(gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))

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

@ -82,7 +82,7 @@ else:
print ("Done!")
if args.plot:
resultPoses = gtsam.extractPose2(result)
resultPoses = gtsam.utilities_extractPose2(result)
for i in range(resultPoses.shape[0]):
plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :]))
plt.show()

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

@ -65,7 +65,7 @@ else:
print ("Done!")
if args.plot:
resultPoses = gtsam.allPose3s(result)
resultPoses = gtsam.utilities_allPose3s(result)
for i in range(resultPoses.size()):
plot.plot_pose3(1, resultPoses.atPose3(i))
plt.show()

2
cython/gtsam/examples/README.md
View File

@ -1,7 +1,7 @@
These examples are almost identical to the old handwritten python wrapper
examples. However, there are just some slight name changes, for example
`noiseModel.Diagonal` becomes `noiseModel_Diagonal` etc...
Also, annoyingly, instead of `gtsam.Symbol('b', 0)` we now need to say `gtsam.symbol(ord('b'), 0))`
Also, instead of `gtsam.Symbol('b', 0)` we can simply say `gtsam.symbol_shorthand_B(0)` or `B(0)` if we use python aliasing.
# Porting Progress

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

@ -10,24 +10,20 @@ A structure-from-motion problem on a simulated dataset
"""
from __future__ import print_function
import gtsam
import matplotlib.pyplot as plt
import numpy as np
import gtsam
from gtsam import symbol_shorthand_L as L
from gtsam import symbol_shorthand_X as X
from gtsam.examples import SFMdata
from gtsam.gtsam import (Cal3_S2, DoglegOptimizer,
GenericProjectionFactorCal3_S2, Marginals,
NonlinearFactorGraph, Point3, Pose3,
PriorFactorPoint3, PriorFactorPose3, Rot3,
NonlinearFactorGraph, PinholeCameraCal3_S2, Point3,
Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3,
SimpleCamera, Values)
from gtsam.utils import plot
def symbol(name: str, index: int) -> int:
""" helper for creating a symbol without explicitly casting 'name' from str to int """
return gtsam.symbol(ord(name), index)
def main():
"""
Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
@ -74,7 +70,7 @@ def main():
# Add a prior on pose x1. This indirectly specifies where the origin is.
# 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z
pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise)
factor = PriorFactorPose3(X(0), poses[0], pose_noise)
graph.push_back(factor)
# Simulated measurements from each camera pose, adding them to the factor graph
@ -83,14 +79,14 @@ def main():
for j, point in enumerate(points):
measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(
measurement, measurement_noise, symbol('x', i), symbol('l', j), K)
measurement, measurement_noise, X(i), L(j), K)
graph.push_back(factor)
# Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
# Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
# between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise)
factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor)
graph.print_('Factor Graph:\n')
@ -99,10 +95,10 @@ def main():
initial_estimate = Values()
for i, pose in enumerate(poses):
transformed_pose = pose.retract(0.1*np.random.randn(6,1))
initial_estimate.insert(symbol('x', i), transformed_pose)
initial_estimate.insert(X(i), transformed_pose)
for j, point in enumerate(points):
transformed_point = Point3(point.vector() + 0.1*np.random.randn(3))
initial_estimate.insert(symbol('l', j), transformed_point)
initial_estimate.insert(L(j), transformed_point)
initial_estimate.print_('Initial Estimates:\n')
# Optimize the graph and print results

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

@ -10,8 +10,9 @@ This example will perform a relatively trivial optimization on
a single variable with a single factor.
"""
import numpy as np
import gtsam
import numpy as np
from gtsam import symbol_shorthand_X as X
def main():
@ -33,7 +34,7 @@ def main():
prior = gtsam.Rot2.fromAngle(np.deg2rad(30))
prior.print_('goal angle')
model = gtsam.noiseModel_Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
key = gtsam.symbol(ord('x'), 1)
key = X(1)
factor = gtsam.PriorFactorRot2(key, prior, model)
"""

19
cython/gtsam/examples/VisualISAM2Example.py
View File

@ -13,23 +13,14 @@ Author: Duy-Nguyen Ta (C++), Frank Dellaert (Python)
from __future__ import print_function
import matplotlib.pyplot as plt
import numpy as np
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
import gtsam
import gtsam.utils.plot as gtsam_plot
import matplotlib.pyplot as plt
import numpy as np
from gtsam import symbol_shorthand_L as L
from gtsam import symbol_shorthand_X as X
from gtsam.examples import SFMdata
def X(i):
"""Create key for pose i."""
return int(gtsam.symbol(ord('x'), i))
def L(j):
"""Create key for landmark j."""
return int(gtsam.symbol(ord('l'), j))
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611
def visual_ISAM2_plot(result):

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

@ -19,11 +19,8 @@ from gtsam.gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2,
NonlinearFactorGraph, NonlinearISAM, Point3, Pose3,
PriorFactorPoint3, PriorFactorPose3, Rot3,
PinholeCameraCal3_S2, Values)
def symbol(name: str, index: int) -> int:
""" helper for creating a symbol without explicitly casting 'name' from str to int """
return gtsam.symbol(ord(name), index)
from gtsam import symbol_shorthand_L as L
from gtsam import symbol_shorthand_X as X
def main():
@ -58,7 +55,7 @@ def main():
# Add factors for each landmark observation
for j, point in enumerate(points):
measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, symbol('x', i), symbol('l', j), K)
factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, X(i), L(j), K)
graph.push_back(factor)
# Intentionally initialize the variables off from the ground truth
@ -66,7 +63,7 @@ def main():
initial_xi = pose.compose(noise)
# Add an initial guess for the current pose
initial_estimate.insert(symbol('x', i), initial_xi)
initial_estimate.insert(X(i), initial_xi)
# If this is the first iteration, add a prior on the first pose to set the coordinate frame
# and a prior on the first landmark to set the scale
@ -75,12 +72,12 @@ def main():
if i == 0:
# Add a prior on pose x0, with 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z
pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise)
factor = PriorFactorPose3(X(0), poses[0], pose_noise)
graph.push_back(factor)
# Add a prior on landmark l0
point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise)
factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor)
# Add initial guesses to all observed landmarks
@ -88,7 +85,7 @@ def main():
for j, point in enumerate(points):
# Intentionally initialize the variables off from the ground truth
initial_lj = points[j].vector() + noise
initial_estimate.insert(symbol('l', j), Point3(initial_lj))
initial_estimate.insert(L(j), Point3(initial_lj))
else:
# Update iSAM with the new factors
isam.update(graph, initial_estimate)

9
cython/gtsam/tests/test_GaussianFactorGraph.py
View File

@ -15,17 +15,18 @@ from __future__ import print_function
import unittest
import gtsam
import numpy as np
from gtsam import symbol_shorthand_X as X
from gtsam.utils.test_case import GtsamTestCase
import numpy as np
def create_graph():
"""Create a basic linear factor graph for testing"""
graph = gtsam.GaussianFactorGraph()
x0 = gtsam.symbol(ord('x'), 0)
x1 = gtsam.symbol(ord('x'), 1)
x2 = gtsam.symbol(ord('x'), 2)
x0 = X(0)
x1 = X(1)
x2 = X(2)
BETWEEN_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1))
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1))

22
cython/gtsam/tests/test_PriorFactor.py
View File

@ -35,5 +35,27 @@ class TestPriorFactor(GtsamTestCase):
values.insert(key, priorVector)
self.assertEqual(factor.error(values), 0)
def test_AddPrior(self):
"""
Test adding prior factors directly to factor graph via the .addPrior method.
"""
# define factor graph
graph = gtsam.NonlinearFactorGraph()
# define and add Pose3 prior
key = 5
priorPose3 = gtsam.Pose3()
model = gtsam.noiseModel_Unit.Create(6)
graph.addPriorPose3(key, priorPose3, model)
self.assertEqual(graph.size(), 1)
# define and add Vector prior
key = 3
priorVector = np.array([0., 0., 0.])
model = gtsam.noiseModel_Unit.Create(3)
graph.addPriorVector(key, priorVector, model)
self.assertEqual(graph.size(), 2)
if __name__ == "__main__":
unittest.main()

2
cython/gtsam/tests/test_dsf_map.py
View File

@ -33,7 +33,7 @@ class TestDSFMap(GtsamTestCase):
# testing the merge feature of dsf
dsf.merge(pair1, pair2)
self.assertEquals(key(dsf.find(pair1)), key(dsf.find(pair2)))
self.assertEqual(key(dsf.find(pair1)), key(dsf.find(pair2)))
if __name__ == '__main__':

79
cython/gtsam/tests/test_logging_optimizer.py Normal file
View File

@ -0,0 +1,79 @@
"""
Unit tests for optimization that logs to comet.ml.
Author: Jing Wu and Frank Dellaert
"""
# pylint: disable=invalid-name
import unittest
from datetime import datetime
import gtsam
import numpy as np
from gtsam import Rot3
from gtsam.utils.test_case import GtsamTestCase
from gtsam.utils.logging_optimizer import gtsam_optimize
KEY = 0
MODEL = gtsam.noiseModel_Unit.Create(3)
class TestOptimizeComet(GtsamTestCase):
"""Check correct logging to comet.ml."""
def setUp(self):
"""Set up a small Karcher mean optimization example."""
# Grabbed from KarcherMeanFactor unit tests.
R = Rot3.Expmap(np.array([0.1, 0, 0]))
rotations = {R, R.inverse()} # mean is the identity
self.expected = Rot3()
graph = gtsam.NonlinearFactorGraph()
for R in rotations:
graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL))
initial = gtsam.Values()
initial.insert(KEY, R)
self.params = gtsam.GaussNewtonParams()
self.optimizer = gtsam.GaussNewtonOptimizer(
graph, initial, self.params)
def test_simple_printing(self):
"""Test with a simple hook."""
# Provide a hook that just prints
def hook(_, error: float):
print(error)
# Only thing we require from optimizer is an iterate method
gtsam_optimize(self.optimizer, self.params, hook)
# Check that optimizing yields the identity.
actual = self.optimizer.values()
self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6)
@unittest.skip("Not a test we want run every time, as needs comet.ml account")
def test_comet(self):
"""Test with a comet hook."""
from comet_ml import Experiment
comet = Experiment(project_name="Testing",
auto_output_logging="native")
comet.log_dataset_info(name="Karcher", path="shonan")
comet.add_tag("GaussNewton")
comet.log_parameter("method", "GaussNewton")
time = datetime.now()
comet.set_name("GaussNewton-" + str(time.month) + "/" + str(time.day) + " "
+ str(time.hour)+":"+str(time.minute)+":"+str(time.second))
# I want to do some comet thing here
def hook(optimizer, error: float):
comet.log_metric("Karcher error",
error, optimizer.iterations())
gtsam_optimize(self.optimizer, self.params, hook)
comet.end()
actual = self.optimizer.values()
self.gtsamAssertEquals(actual.atRot3(KEY), self.expected)
if __name__ == "__main__":
unittest.main()

54
cython/gtsam/utils/logging_optimizer.py Normal file
View File

@ -0,0 +1,54 @@
"""
Optimization with logging via a hook.
Author: Jing Wu and Frank Dellaert
"""
# pylint: disable=invalid-name
from typing import TypeVar
from gtsam import NonlinearOptimizer, NonlinearOptimizerParams
import gtsam
T = TypeVar('T')
def optimize(optimizer: T, check_convergence, hook):
""" Given an optimizer and a convergence check, iterate until convergence.
After each iteration, hook(optimizer, error) is called.
After the function, use values and errors to get the result.
Arguments:
optimizer (T): needs an iterate and an error function.
check_convergence: T * float * float -> bool
hook -- hook function to record the error
"""
# the optimizer is created with default values which incur the error below
current_error = optimizer.error()
hook(optimizer, current_error)
# Iterative loop
while True:
# Do next iteration
optimizer.iterate()
new_error = optimizer.error()
hook(optimizer, new_error)
if check_convergence(optimizer, current_error, new_error):
return
current_error = new_error
def gtsam_optimize(optimizer: NonlinearOptimizer,
params: NonlinearOptimizerParams,
hook):
""" Given an optimizer and params, iterate until convergence.
After each iteration, hook(optimizer) is called.
After the function, use values and errors to get the result.
Arguments:
optimizer {NonlinearOptimizer} -- Nonlinear optimizer
params {NonlinearOptimizarParams} -- Nonlinear optimizer parameters
hook -- hook function to record the error
"""
def check_convergence(optimizer, current_error, new_error):
return (optimizer.iterations() >= params.getMaxIterations()) or (
gtsam.checkConvergence(params.getRelativeErrorTol(), params.getAbsoluteErrorTol(), params.getErrorTol(),
current_error, new_error))
optimize(optimizer, check_convergence, hook)

140
cython/gtsam/utils/plot.py
View File

@ -13,8 +13,9 @@ def set_axes_equal(fignum):
Make axes of 3D plot have equal scale so that spheres appear as spheres,
cubes as cubes, etc.. This is one possible solution to Matplotlib's
ax.set_aspect('equal') and ax.axis('equal') not working for 3D.
Input
ax: a matplotlib axis, e.g., as output from plt.gca().
Args:
fignum (int): An integer representing the figure number for Matplotlib.
"""
fig = plt.figure(fignum)
ax = fig.gca(projection='3d')
@ -34,7 +35,21 @@ def set_axes_equal(fignum):
def ellipsoid(xc, yc, zc, rx, ry, rz, n):
"""Numpy equivalent of Matlab's ellipsoid function"""
"""
Numpy equivalent of Matlab's ellipsoid function.
Args:
xc (double): Center of ellipsoid in X-axis.
yc (double): Center of ellipsoid in Y-axis.
zc (double): Center of ellipsoid in Z-axis.
rx (double): Radius of ellipsoid in X-axis.
ry (double): Radius of ellipsoid in Y-axis.
rz (double): Radius of ellipsoid in Z-axis.
n (int): The granularity of the ellipsoid plotted.
Returns:
tuple[numpy.ndarray]: The points in the x, y and z axes to use for the surface plot.
"""
u = np.linspace(0, 2*np.pi, n+1)
v = np.linspace(0, np.pi, n+1)
x = -rx * np.outer(np.cos(u), np.sin(v)).T
@ -51,6 +66,14 @@ def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5):
Based on Maybeck Vol 1, page 366
k=2.296 corresponds to 1 std, 68.26% of all probability
k=11.82 corresponds to 3 std, 99.74% of all probability
Args:
axes (matplotlib.axes.Axes): Matplotlib axes.
origin (gtsam.Point3): The origin in the world frame.
P (numpy.ndarray): The marginal covariance matrix of the 3D point which will be represented as an ellipse.
scale (float): Scaling factor of the radii of the covariance ellipse.
n (int): Defines the granularity of the ellipse. Higher values indicate finer ellipses.
alpha (float): Transparency value for the plotted surface in the range [0, 1].
"""
k = 11.82
U, S, _ = np.linalg.svd(P)
@ -74,7 +97,15 @@ def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5):
def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None):
"""Plot a 2D pose on given axis 'axes' with given 'axis_length'."""
"""
Plot a 2D pose on given axis `axes` with given `axis_length`.
Args:
axes (matplotlib.axes.Axes): Matplotlib axes.
pose (gtsam.Pose2): The pose to be plotted.
axis_length (float): The length of the camera axes.
covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
"""
# get rotation and translation (center)
gRp = pose.rotation().matrix() # rotation from pose to global
t = pose.translation()
@ -105,22 +136,47 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None):
def plot_pose2(fignum, pose, axis_length=0.1, covariance=None):
"""Plot a 2D pose on given figure with given 'axis_length'."""
"""
Plot a 2D pose on given figure with given `axis_length`.
Args:
fignum (int): Integer representing the figure number to use for plotting.
pose (gtsam.Pose2): The pose to be plotted.
axis_length (float): The length of the camera axes.
covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
"""
# get figure object
fig = plt.figure(fignum)
axes = fig.gca()
plot_pose2_on_axes(axes, pose, axis_length, covariance)
plot_pose2_on_axes(axes, pose, axis_length=axis_length,
covariance=covariance)
def plot_point3_on_axes(axes, point, linespec, P=None):
"""Plot a 3D point on given axis 'axes' with given 'linespec'."""
"""
Plot a 3D point on given axis `axes` with given `linespec`.
Args:
axes (matplotlib.axes.Axes): Matplotlib axes.
point (gtsam.Point3): The point to be plotted.
linespec (string): String representing formatting options for Matplotlib.
P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
"""
axes.plot([point.x()], [point.y()], [point.z()], linespec)
if P is not None:
plot_covariance_ellipse_3d(axes, point.vector(), P)
def plot_point3(fignum, point, linespec, P=None):
"""Plot a 3D point on given figure with given 'linespec'."""
"""
Plot a 3D point on given figure with given `linespec`.
Args:
fignum (int): Integer representing the figure number to use for plotting.
point (gtsam.Point3): The point to be plotted.
linespec (string): String representing formatting options for Matplotlib.
P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
"""
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
plot_point3_on_axes(axes, point, linespec, P)
@ -128,10 +184,16 @@ def plot_point3(fignum, point, linespec, P=None):
def plot_3d_points(fignum, values, linespec="g*", marginals=None):
"""
Plots the Point3s in 'values', with optional covariances.
Plots the Point3s in `values`, with optional covariances.
Finds all the Point3 objects in the given Values object and plots them.
If a Marginals object is given, this function will also plot marginal
covariance ellipses for each point.
Args:
fignum (int): Integer representing the figure number to use for plotting.
values (gtsam.Values): Values dictionary consisting of points to be plotted.
linespec (string): String representing formatting options for Matplotlib.
covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
"""
keys = values.keys()
@ -142,19 +204,27 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None):
key = keys.at(i)
point = values.atPoint3(key)
if marginals is not None:
P = marginals.marginalCovariance(key);
covariance = marginals.marginalCovariance(key)
else:
P = None
covariance = None
plot_point3(fignum, point, linespec, P)
plot_point3(fignum, point, linespec, covariance)
except RuntimeError:
continue
# I guess it's not a Point3
def plot_pose3_on_axes(axes, pose, P=None, scale=1, axis_length=0.1):
"""Plot a 3D pose on given axis 'axes' with given 'axis_length'."""
def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1):
"""
Plot a 3D pose on given axis `axes` with given `axis_length`.
Args:
axes (matplotlib.axes.Axes): Matplotlib axes.
point (gtsam.Point3): The point to be plotted.
linespec (string): String representing formatting options for Matplotlib.
P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
"""
# get rotation and translation (center)
gRp = pose.rotation().matrix() # rotation from pose to global
origin = pose.translation().vector()
@ -181,15 +251,35 @@ def plot_pose3_on_axes(axes, pose, P=None, scale=1, axis_length=0.1):
plot_covariance_ellipse_3d(axes, origin, gPp)
def plot_pose3(fignum, pose, P, axis_length=0.1):
"""Plot a 3D pose on given figure with given 'axis_length'."""
def plot_pose3(fignum, pose, axis_length=0.1, P=None):
"""
Plot a 3D pose on given figure with given `axis_length`.
Args:
fignum (int): Integer representing the figure number to use for plotting.
pose (gtsam.Pose3): 3D pose to be plotted.
linespec (string): String representing formatting options for Matplotlib.
P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
"""
# get figure object
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
plot_pose3_on_axes(axes, pose, P=P, axis_length=axis_length)
plot_pose3_on_axes(axes, pose, P=P,
axis_length=axis_length)
def plot_trajectory(fignum, values, scale=1, marginals=None):
pose3Values = gtsam.allPose3s(values)
"""
Plot a complete 3D trajectory using poses in `values`.
Args:
fignum (int): Integer representing the figure number to use for plotting.
values (gtsam.Values): Values dict containing the poses.
scale (float): Value to scale the poses by.
marginals (gtsam.Marginals): Marginalized probability values of the estimation.
Used to plot uncertainty bounds.
"""
pose3Values = gtsam.utilities_allPose3s(values)
keys = gtsam.KeyVector(pose3Values.keys())
lastIndex = None
@ -209,11 +299,12 @@ def plot_trajectory(fignum, values, scale=1, marginals=None):
pass
if marginals:
P = marginals.marginalCovariance(lastKey)
covariance = marginals.marginalCovariance(lastKey)
else:
P = None
covariance = None
plot_pose3(fignum, lastPose, P, scale)
plot_pose3(fignum, lastPose, P=covariance,
axis_length=scale)
lastIndex = i
@ -223,11 +314,12 @@ def plot_trajectory(fignum, values, scale=1, marginals=None):
try:
lastPose = pose3Values.atPose3(lastKey)
if marginals:
P = marginals.marginalCovariance(lastKey)
covariance = marginals.marginalCovariance(lastKey)
else:
P = None
covariance = None
plot_pose3(fignum, lastPose, P, scale)
plot_pose3(fignum, lastPose, P=covariance,
axis_length=scale)
except:
pass

2
cython/requirements.txt
View File

@ -1,3 +1,3 @@
Cython>=0.25.2
backports_abc>=0.5
numpy>=1.12.0
numpy>=1.11.0

2
cython/setup.py.in
View File

@ -35,7 +35,7 @@ setup(
packages=packages,
package_data={package:
[f for f in os.listdir(package.replace('.', os.path.sep)) if os.path.splitext(f)[1] in ('.so', '.dll')]
[f for f in os.listdir(package.replace('.', os.path.sep)) if os.path.splitext(f)[1] in ('.so', '.pyd')]
for package in packages
},
install_requires=[line.strip() for line in '''

12
debian/README.md vendored
View File

@ -1,12 +0,0 @@
# How to build a GTSAM debian package
To use the ``debuild`` command, install the ``devscripts`` package
sudo apt install devscripts
Change into the gtsam directory, then run:
debuild -us -uc -j4
Adjust the ``-j4`` depending on how many CPUs you want to build on in
parallel.

5
debian/changelog vendored
View File

@ -1,5 +0,0 @@
gtsam (4.0.0-1berndpfrommer) bionic; urgency=medium
* initial release
-- Bernd Pfrommer <bernd.pfrommer@gmail.com> Wed, 18 Jul 2018 20:36:44 -0400

1
debian/compat vendored
View File

@ -1 +0,0 @@
9

15
debian/control vendored
View File

@ -1,15 +0,0 @@
Source: gtsam
Section: libs
Priority: optional
Maintainer: Frank Dellaert <frank@cc.gatech.edu>
Uploaders: Jose Luis Blanco Claraco <joseluisblancoc@gmail.com>, Bernd Pfrommer <bernd.pfrommer@gmail.com>
Build-Depends: cmake, libboost-all-dev (>= 1.58), libeigen3-dev, libtbb-dev, debhelper (>=9)
Standards-Version: 3.9.7
Homepage: https://github.com/borglab/gtsam
Vcs-Browser: https://github.com/borglab/gtsam
Package: libgtsam-dev
Architecture: any
Depends: ${shlibs:Depends}, ${misc:Depends}, libboost-serialization-dev, libboost-system-dev, libboost-filesystem-dev, libboost-thread-dev, libboost-program-options-dev, libboost-date-time-dev, libboost-timer-dev, libboost-chrono-dev, libboost-regex-dev
Description: Georgia Tech Smoothing and Mapping Library
gtsam: Georgia Tech Smoothing and Mapping library for SLAM type applications

15
debian/copyright vendored
View File

@ -1,15 +0,0 @@
Format: https://www.debian.org/doc/packaging-manuals/copyright-format/1.0/
Upstream-Name: gtsam
Source: https://bitbucket.org/gtborg/gtsam.git
Files: *
Copyright: 2017, Frank Dellaert
License: BSD
Files: gtsam/3rdparty/CCOLAMD/*
Copyright: 2005-2011, Univ. of Florida. Authors: Timothy A. Davis, Sivasankaran Rajamanickam, and Stefan Larimore. Closely based on COLAMD by Davis, Stefan Larimore, in collaboration with Esmond Ng, and John Gilbert. http://www.cise.ufl.edu/research/sparse
License: GNU LESSER GENERAL PUBLIC LICENSE
Files: gtsam/3rdparty/Eigen/*
Copyright: 2017, Multiple Authors
License: MPL2

0
debian/gtsam-docs.docs vendored
View File

29
debian/rules vendored
View File

@ -1,29 +0,0 @@
#!/usr/bin/make -f
# See debhelper(7) (uncomment to enable)
# output every command that modifies files on the build system.
export DH_VERBOSE = 1
# Makefile target name for running unit tests:
GTSAM_TEST_TARGET = check
# see FEATURE AREAS in dpkg-buildflags(1)
#export DEB_BUILD_MAINT_OPTIONS = hardening=+all
# see ENVIRONMENT in dpkg-buildflags(1)
# package maintainers to append CFLAGS
#export DEB_CFLAGS_MAINT_APPEND = -Wall -pedantic
# package maintainers to append LDFLAGS
#export DEB_LDFLAGS_MAINT_APPEND = -Wl,--as-needed
%:
dh $@ --parallel
# dh_make generated override targets
# This is example for Cmake (See https://bugs.debian.org/641051 )
override_dh_auto_configure:
dh_auto_configure -- -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_INSTALL_PREFIX=/usr -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_BUILD_TESTS=ON -DGTSAM_BUILD_WRAP=OFF -DGTSAM_BUILD_DOCS=OFF -DGTSAM_INSTALL_CPPUNITLITE=OFF -DGTSAM_INSTALL_GEOGRAPHICLIB=OFF -DGTSAM_BUILD_TYPE_POSTFIXES=OFF -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF
override_dh_auto_test-arch:
# Tests for arch-dependent :
echo "[override_dh_auto_test-arch]"
dh_auto_build -O--buildsystem=cmake -- $(GTSAM_TEST_TARGET)

1
debian/source/format vendored
View File

@ -1 +0,0 @@
3.0 (quilt)

17
doc/gtsam-coordinate-frames.lyx
View File

@ -2291,15 +2291,11 @@ uncalibration
used in the residual).
\end_layout
\begin_layout Standard
\begin_inset Note Note
status collapsed
\begin_layout Section
Noise models of prior factors
\end_layout
\begin_layout Plain Layout
\begin_layout Standard
The simplest way to describe noise models is by an example.
Let's take a prior factor on a 3D pose
\begin_inset Formula $x\in\SE 3$
@ -2353,7 +2349,7 @@ e\left(x\right)=\norm{h\left(x\right)}_{\Sigma}^{2}=h\left(x\right)^{\t}\Sigma^{
useful answer out quickly ]
\end_layout
\begin_layout Plain Layout
\begin_layout Standard
The density induced by a noise model on the prior factor is Gaussian in
the tangent space about the linearization point.
Suppose that the pose is linearized at
@ -2431,7 +2427,7 @@ Here we see that the update
.
\end_layout
\begin_layout Plain Layout
\begin_layout Standard
This means that to draw random pose samples, we actually draw random samples
of
\begin_inset Formula $\delta x$
@ -2456,7 +2452,7 @@ This means that to draw random pose samples, we actually draw random samples
Noise models of between factors
\end_layout
\begin_layout Plain Layout
\begin_layout Standard
The noise model of a BetweenFactor is a bit more complicated.
The unwhitened error is
\begin_inset Formula
@ -2516,11 +2512,6 @@ e\left(\delta x_{1}\right) & \approx\norm{\log\left(z^{-1}\left(x_{1}\exp\delta
\end_inset
\end_layout
\end_inset
\end_layout
\end_body

BIN
doc/gtsam-coordinate-frames.pdf
View File

Binary file not shown.

56
gtsam.h
View File

@ -281,7 +281,7 @@ virtual class Value {
};
#include <gtsam/base/GenericValue.h>
template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
template<T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class GenericValue : gtsam::Value {
void serializable() const;
};
@ -1977,6 +1977,35 @@ size_t symbol(char chr, size_t index);
char symbolChr(size_t key);
size_t symbolIndex(size_t key);
namespace symbol_shorthand {
size_t A(size_t j);
size_t B(size_t j);
size_t C(size_t j);
size_t D(size_t j);
size_t E(size_t j);
size_t F(size_t j);
size_t G(size_t j);
size_t H(size_t j);
size_t I(size_t j);
size_t J(size_t j);
size_t K(size_t j);
size_t L(size_t j);
size_t M(size_t j);
size_t N(size_t j);
size_t O(size_t j);
size_t P(size_t j);
size_t Q(size_t j);
size_t R(size_t j);
size_t S(size_t j);
size_t T(size_t j);
size_t U(size_t j);
size_t V(size_t j);
size_t W(size_t j);
size_t X(size_t j);
size_t Y(size_t j);
size_t Z(size_t j);
}///\namespace symbol
// Default keyformatter
void PrintKeyList (const gtsam::KeyList& keys);
void PrintKeyList (const gtsam::KeyList& keys, string s);
@ -2050,6 +2079,9 @@ class NonlinearFactorGraph {
gtsam::KeySet keys() const;
gtsam::KeyVector keyVector() const;
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}>
void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
// NonlinearFactorGraph
void printErrors(const gtsam::Values& values) const;
double error(const gtsam::Values& values) const;
@ -2138,6 +2170,7 @@ class Values {
void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix);
void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
void insert(size_t j, const gtsam::NavState& nav_state);
void insert(size_t j, Vector vector);
void insert(size_t j, Matrix matrix);
@ -2155,10 +2188,11 @@ class Values {
void update(size_t j, const gtsam::Cal3Bundler& cal3bundler);
void update(size_t j, const gtsam::EssentialMatrix& essential_matrix);
void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
void update(size_t j, const gtsam::NavState& nav_state);
void update(size_t j, Vector vector);
void update(size_t j, Matrix matrix);
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, Matrix}>
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}>
T at(size_t j);
/// version for double
@ -2258,8 +2292,10 @@ virtual class NonlinearOptimizerParams {
};
bool checkConvergence(double relativeErrorTreshold,
double absoluteErrorTreshold, double errorThreshold,
double currentError, double newError);
double absoluteErrorTreshold, double errorThreshold,
double currentError, double newError);
bool checkConvergence(const gtsam::NonlinearOptimizerParams& params,
double currentError, double newError);
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams {
@ -2495,7 +2531,7 @@ class NonlinearISAM {
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/nonlinear/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::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, 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 +2554,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::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, 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);
@ -2759,8 +2795,10 @@ void save2D(const gtsam::NonlinearFactorGraph& graph,
// std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
class BetweenFactorPose3s
{
BetweenFactorPose3s();
size_t size() const;
gtsam::BetweenFactorPose3* at(size_t i) const;
void push_back(const gtsam::BetweenFactorPose3* factor);
};
#include <gtsam/slam/InitializePose3.h>
@ -2910,11 +2948,14 @@ class PreintegratedImuMeasurements {
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
double deltaT);
void resetIntegration();
void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
Matrix preintMeasCov() const;
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
gtsam::imuBias::ConstantBias biasHat() const;
Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;
@ -2969,11 +3010,14 @@ class PreintegratedCombinedMeasurements {
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
double deltaT);
void resetIntegration();
void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
Matrix preintMeasCov() const;
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
gtsam::imuBias::ConstantBias biasHat() const;
Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;

14
gtsam/3rdparty/CMakeLists.txt vendored
View File

@ -1,6 +1,6 @@
# install CCOLAMD headers
install(FILES CCOLAMD/Include/ccolamd.h DESTINATION include/gtsam/3rdparty/CCOLAMD)
install(FILES SuiteSparse_config/SuiteSparse_config.h DESTINATION include/gtsam/3rdparty/SuiteSparse_config)
install(FILES CCOLAMD/Include/ccolamd.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/CCOLAMD)
install(FILES SuiteSparse_config/SuiteSparse_config.h DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/SuiteSparse_config)
if(NOT GTSAM_USE_SYSTEM_EIGEN)
# Find plain .h files
@ -12,12 +12,12 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
get_filename_component(filename ${eigen_dir} NAME)
if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES ".svn")))
list(APPEND eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/Eigen/${filename}")
install(FILES Eigen/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/Eigen)
install(FILES Eigen/Eigen/${filename} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/Eigen/Eigen)
endif()
endforeach(eigen_dir)
if(GTSAM_WITH_EIGEN_UNSUPPORTED)
message("-- Installing Eigen Unsupported modules")
message(STATUS "Installing Eigen Unsupported modules")
# do the same for the unsupported eigen folder
file(GLOB_RECURSE unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h")
@ -26,7 +26,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
get_filename_component(filename ${unsupported_eigen_dir} NAME)
if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES "CXX11") OR (${filename} MATCHES ".svn")))
list(APPEND unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/${filename}")
install(FILES Eigen/unsupported/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/unsupported/Eigen)
install(FILES Eigen/unsupported/Eigen/${filename} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/Eigen/unsupported/Eigen)
endif()
endforeach(unsupported_eigen_dir)
endif()
@ -37,12 +37,12 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
# install Eigen - only the headers in our 3rdparty directory
install(DIRECTORY Eigen/Eigen
DESTINATION include/gtsam/3rdparty/Eigen
DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/Eigen
FILES_MATCHING PATTERN "*.h")
if(GTSAM_WITH_EIGEN_UNSUPPORTED)
install(DIRECTORY Eigen/unsupported/Eigen
DESTINATION include/gtsam/3rdparty/Eigen/unsupported/
DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam/3rdparty/Eigen/unsupported/
FILES_MATCHING PATTERN "*.h")
endif()

6
gtsam/3rdparty/Eigen/blas/CMakeLists.txt vendored
View File

@ -39,9 +39,9 @@ endif()
add_dependencies(blas eigen_blas eigen_blas_static)
install(TARGETS eigen_blas eigen_blas_static
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib)
RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR})
if(EIGEN_Fortran_COMPILER_WORKS)

6
gtsam/3rdparty/Eigen/lapack/CMakeLists.txt vendored
View File

@ -103,9 +103,9 @@ endif()
add_dependencies(lapack eigen_lapack eigen_lapack_static)
install(TARGETS eigen_lapack eigen_lapack_static
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib)
RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR})

6
gtsam/3rdparty/metis/GKlib/CMakeLists.txt vendored
View File

@ -16,6 +16,6 @@ include_directories("test")
add_subdirectory("test")
install(TARGETS GKlib
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib)
install(FILES ${GKlib_includes} DESTINATION include)
ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR})
install(FILES ${GKlib_includes} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})

15
gtsam/3rdparty/metis/libmetis/CMakeLists.txt vendored
View File

@ -4,23 +4,26 @@ include_directories(.)
file(GLOB metis_sources *.c)
# Build libmetis.
add_definitions(-fPIC)
add_library(metis ${METIS_LIBRARY_TYPE} ${GKlib_sources} ${metis_sources})
add_library(metis-gtsam ${METIS_LIBRARY_TYPE} ${GKlib_sources} ${metis_sources})
if(UNIX)
target_link_libraries(metis m)
target_link_libraries(metis-gtsam m)
endif()
if(WIN32)
set_target_properties(metis PROPERTIES
set_target_properties(metis-gtsam PROPERTIES
PREFIX ""
RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin")
endif()
if (APPLE)
set_target_properties(metis PROPERTIES
set_target_properties(metis-gtsam PROPERTIES
INSTALL_NAME_DIR
"${CMAKE_INSTALL_PREFIX}/lib")
endif()
install(TARGETS metis EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
list(APPEND GTSAM_EXPORTED_TARGETS metis)
install(TARGETS metis-gtsam EXPORT GTSAM-exports
LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR})
list(APPEND GTSAM_EXPORTED_TARGETS metis-gtsam)
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)

2
gtsam/3rdparty/metis/programs/CMakeLists.txt vendored
View File

@ -15,7 +15,7 @@ endforeach(prog)
if(METIS_INSTALL)
install(TARGETS gpmetis ndmetis mpmetis m2gmetis graphchk cmpfillin
RUNTIME DESTINATION bin)
RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR})
endif()
# Try to find subversion revision.

16
gtsam/CMakeLists.txt
View File

@ -49,7 +49,7 @@ endif()
# Common headers
file(GLOB gtsam_core_headers "*.h")
install(FILES ${gtsam_core_headers} DESTINATION include/gtsam)
install(FILES ${gtsam_core_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam)
# assemble core libaries
foreach(subdir ${gtsam_subdirs})
@ -86,10 +86,10 @@ configure_file(config.h.in config.h)
set(library_name GTSAM) # For substitution in dllexport.h.in
configure_file("${GTSAM_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h")
list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h")
install(FILES "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION include/gtsam)
install(FILES "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam)
if(GTSAM_SUPPORT_NESTED_DISSECTION)
list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis)
list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis-gtsam)
endif()
# Versions
@ -172,12 +172,16 @@ if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with
endif()
endif()
if(WIN32) # library to help with demangling variable names on Windows
target_link_libraries(gtsam PRIVATE Dbghelp)
endif()
install(
TARGETS gtsam
EXPORT GTSAM-exports
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin)
LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR})
list(APPEND GTSAM_EXPORTED_TARGETS gtsam)
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)

5
gtsam/base/GenericValue.h
View File

@ -20,6 +20,7 @@
#pragma once
#include <gtsam/base/Manifold.h>
#include <gtsam/base/types.h>
#include <gtsam/base/Value.h>
#include <boost/make_shared.hpp>
@ -83,7 +84,7 @@ public:
/// Virtual print function, uses traits
virtual void print(const std::string& str) const {
std::cout << "(" << typeid(T).name() << ") ";
std::cout << "(" << demangle(typeid(T).name()) << ") ";
traits<T>::Print(value_, str);
}
@ -180,7 +181,7 @@ public:
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};
/// use this macro instead of BOOST_CLASS_EXPORT for GenericValues

4
gtsam/base/Lie.h
View File

@ -170,7 +170,7 @@ namespace internal {
/// Assumes existence of: identity, dimension, localCoordinates, retract,
/// and additionally Logmap, Expmap, compose, between, and inverse
template<class Class>
struct LieGroupTraits {
struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> {
typedef lie_group_tag structure_category;
/// @name Group
@ -186,8 +186,6 @@ struct LieGroupTraits {
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
static int GetDimension(const Class&) {return dimension;}
static TangentVector Local(const Class& origin, const Class& other,
ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
return origin.localCoordinates(other, Horigin, Hother);

8
gtsam/base/Manifold.h
View File

@ -72,7 +72,7 @@ struct HasManifoldPrereqs {
/// Extra manifold traits for fixed-dimension types
template<class Class, int N>
struct ManifoldImpl {
struct GetDimensionImpl {
// Compile-time dimensionality
static int GetDimension(const Class&) {
return N;
@ -81,7 +81,7 @@ struct ManifoldImpl {
/// Extra manifold traits for variable-dimension types
template<class Class>
struct ManifoldImpl<Class, Eigen::Dynamic> {
struct GetDimensionImpl<Class, Eigen::Dynamic> {
// Run-time dimensionality
static int GetDimension(const Class& m) {
return m.dim();
@ -92,7 +92,7 @@ struct ManifoldImpl<Class, Eigen::Dynamic> {
/// To use this for your class type, define:
/// template<> struct traits<Class> : public internal::ManifoldTraits<Class> { };
template<class Class>
struct ManifoldTraits: ManifoldImpl<Class, Class::dimension> {
struct ManifoldTraits: GetDimensionImpl<Class, Class::dimension> {
// Check that Class has the necessary machinery
BOOST_CONCEPT_ASSERT((HasManifoldPrereqs<Class>));
@ -214,7 +214,7 @@ public:
enum { NeedsToAlign = (sizeof(M1) % 16) == 0 || (sizeof(M2) % 16) == 0
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};
// Define any direct product group to be a model of the multiplicative Group concept

1
gtsam/base/SymmetricBlockMatrix.h
View File

@ -416,4 +416,3 @@ namespace gtsam {
class CholeskyFailed;
}

67
gtsam/base/make_shared.h Normal file
View File

@ -0,0 +1,67 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 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 make_shared.h
* @brief make_shared trampoline function to ensure proper alignment
* @author Fan Jiang
*/
#pragma once
#include <gtsam/base/types.h>
#include <Eigen/Core>
#include <boost/make_shared.hpp>
#include <type_traits>
namespace gtsam {
/// An shorthand alias for accessing the ::type inside std::enable_if that can be used in a template directly
template<bool B, class T = void>
using enable_if_t = typename std::enable_if<B, T>::type;
}
namespace gtsam {
/**
* Add our own `make_shared` as a layer of wrapping on `boost::make_shared`
* This solves the problem with the stock `make_shared` that custom alignment is not respected, causing SEGFAULTs
* at runtime, which is notoriously hard to debug.
*
* Explanation
* ===============
* The template `needs_eigen_aligned_allocator<T>::value` will evaluate to `std::true_type` if the type alias
* `_eigen_aligned_allocator_trait = void` is present in a class, which is automatically added by the
* `GTSAM_MAKE_ALIGNED_OPERATOR_NEW` macro.
*
* This function declaration will only be taken when the above condition is true, so if some object does not need to
* be aligned, `gtsam::make_shared` will fall back to the next definition, which is a simple wrapper for
* `boost::make_shared`.
*
* @tparam T The type of object being constructed
* @tparam Args Type of the arguments of the constructor
* @param args Arguments of the constructor
* @return The object created as a boost::shared_ptr<T>
*/
template<typename T, typename ... Args>
gtsam::enable_if_t<needs_eigen_aligned_allocator<T>::value, boost::shared_ptr<T>> make_shared(Args &&... args) {
return boost::allocate_shared<T>(Eigen::aligned_allocator<T>(), std::forward<Args>(args)...);
}
/// Fall back to the boost version if no need for alignment
template<typename T, typename ... Args>
gtsam::enable_if_t<!needs_eigen_aligned_allocator<T>::value, boost::shared_ptr<T>> make_shared(Args &&... args) {
return boost::make_shared<T>(std::forward<Args>(args)...);
}
}

239
gtsam/base/serialization.h
View File

@ -42,123 +42,218 @@
namespace gtsam {
// Serialization directly to strings in compressed format
template<class T>
std::string serialize(const T& input) {
std::ostringstream out_archive_stream;
/** @name Standard serialization
* Serialization in default compressed format
*/
///@{
/// serializes to a stream
template <class T>
void serializeToStream(const T& input, std::ostream& out_archive_stream) {
boost::archive::text_oarchive out_archive(out_archive_stream);
out_archive << input;
return out_archive_stream.str();
}
template<class T>
void deserialize(const std::string& serialized, T& output) {
std::istringstream in_archive_stream(serialized);
/// deserializes from a stream
template <class T>
void deserializeFromStream(std::istream& in_archive_stream, T& output) {
boost::archive::text_iarchive in_archive(in_archive_stream);
in_archive >> output;
}
template<class T>
/// serializes to a string
template <class T>
std::string serializeToString(const T& input) {
std::ostringstream out_archive_stream;
serializeToStream(input, out_archive_stream);
return out_archive_stream.str();
}
/// deserializes from a string
template <class T>
void deserializeFromString(const std::string& serialized, T& output) {
std::istringstream in_archive_stream(serialized);
deserializeFromStream(in_archive_stream, output);
}
/// serializes to a file
template <class T>
bool serializeToFile(const T& input, const std::string& filename) {
std::ofstream out_archive_stream(filename.c_str());
if (!out_archive_stream.is_open())
return false;
boost::archive::text_oarchive out_archive(out_archive_stream);
out_archive << input;
if (!out_archive_stream.is_open()) return false;
serializeToStream(input, out_archive_stream);
out_archive_stream.close();
return true;
}
template<class T>
/// deserializes from a file
template <class T>
bool deserializeFromFile(const std::string& filename, T& output) {
std::ifstream in_archive_stream(filename.c_str());
if (!in_archive_stream.is_open())
return false;
boost::archive::text_iarchive in_archive(in_archive_stream);
in_archive >> output;
if (!in_archive_stream.is_open()) return false;
deserializeFromStream(in_archive_stream, output);
in_archive_stream.close();
return true;
}
// Serialization to XML format with named structures
template<class T>
std::string serializeXML(const T& input, const std::string& name="data") {
std::ostringstream out_archive_stream;
// braces to flush out_archive as it goes out of scope before taking str()
// fixes crash with boost 1.66-1.68
// see https://github.com/boostorg/serialization/issues/82
{
boost::archive::xml_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp(name.c_str(), input);
}
return out_archive_stream.str();
/// serializes to a string
template <class T>
std::string serialize(const T& input) {
return serializeToString(input);
}
template<class T>
void deserializeXML(const std::string& serialized, T& output, const std::string& name="data") {
std::istringstream in_archive_stream(serialized);
boost::archive::xml_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp(name.c_str(), output);
/// deserializes from a string
template <class T>
void deserialize(const std::string& serialized, T& output) {
deserializeFromString(serialized, output);
}
///@}
template<class T>
bool serializeToXMLFile(const T& input, const std::string& filename, const std::string& name="data") {
std::ofstream out_archive_stream(filename.c_str());
if (!out_archive_stream.is_open())
return false;
/** @name XML Serialization
* Serialization to XML format with named structures
*/
///@{
/// serializes to a stream in XML
template <class T>
void serializeToXMLStream(const T& input, std::ostream& out_archive_stream,
const std::string& name = "data") {
boost::archive::xml_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp(name.c_str(), input);;
out_archive_stream.close();
return true;
out_archive << boost::serialization::make_nvp(name.c_str(), input);
}
template<class T>
bool deserializeFromXMLFile(const std::string& filename, T& output, const std::string& name="data") {
std::ifstream in_archive_stream(filename.c_str());
if (!in_archive_stream.is_open())
return false;
/// deserializes from a stream in XML
template <class T>
void deserializeFromXMLStream(std::istream& in_archive_stream, T& output,
const std::string& name = "data") {
boost::archive::xml_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp(name.c_str(), output);
in_archive_stream.close();
return true;
}
// Serialization to binary format with named structures
template<class T>
std::string serializeBinary(const T& input, const std::string& name="data") {
/// serializes to a string in XML
template <class T>
std::string serializeToXMLString(const T& input,
const std::string& name = "data") {
std::ostringstream out_archive_stream;
boost::archive::binary_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp(name.c_str(), input);
serializeToXMLStream(input, out_archive_stream, name);
return out_archive_stream.str();
}
template<class T>
void deserializeBinary(const std::string& serialized, T& output, const std::string& name="data") {
/// deserializes from a string in XML
template <class T>
void deserializeFromXMLString(const std::string& serialized, T& output,
const std::string& name = "data") {
std::istringstream in_archive_stream(serialized);
boost::archive::binary_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp(name.c_str(), output);
deserializeFromXMLStream(in_archive_stream, output, name);
}
template<class T>
bool serializeToBinaryFile(const T& input, const std::string& filename, const std::string& name="data") {
/// serializes to an XML file
template <class T>
bool serializeToXMLFile(const T& input, const std::string& filename,
const std::string& name = "data") {
std::ofstream out_archive_stream(filename.c_str());
if (!out_archive_stream.is_open())
return false;
boost::archive::binary_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp(name.c_str(), input);
if (!out_archive_stream.is_open()) return false;
serializeToXMLStream(input, out_archive_stream, name);
out_archive_stream.close();
return true;
}
template<class T>
bool deserializeFromBinaryFile(const std::string& filename, T& output, const std::string& name="data") {
/// deserializes from an XML file
template <class T>
bool deserializeFromXMLFile(const std::string& filename, T& output,
const std::string& name = "data") {
std::ifstream in_archive_stream(filename.c_str());
if (!in_archive_stream.is_open())
return false;
boost::archive::binary_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp(name.c_str(), output);
if (!in_archive_stream.is_open()) return false;
deserializeFromXMLStream(in_archive_stream, output, name);
in_archive_stream.close();
return true;
}
} // \namespace gtsam
/// serializes to a string in XML
template <class T>
std::string serializeXML(const T& input,
const std::string& name = "data") {
return serializeToXMLString(input, name);
}
/// deserializes from a string in XML
template <class T>
void deserializeXML(const std::string& serialized, T& output,
const std::string& name = "data") {
deserializeFromXMLString(serialized, output, name);
}
///@}
/** @name Binary Serialization
* Serialization to binary format with named structures
*/
///@{
/// serializes to a stream in binary
template <class T>
void serializeToBinaryStream(const T& input, std::ostream& out_archive_stream,
const std::string& name = "data") {
boost::archive::binary_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp(name.c_str(), input);
}
/// deserializes from a stream in binary
template <class T>
void deserializeFromBinaryStream(std::istream& in_archive_stream, T& output,
const std::string& name = "data") {
boost::archive::binary_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp(name.c_str(), output);
}
/// serializes to a string in binary
template <class T>
std::string serializeToBinaryString(const T& input,
const std::string& name = "data") {
std::ostringstream out_archive_stream;
serializeToBinaryStream(input, out_archive_stream, name);
return out_archive_stream.str();
}
/// deserializes from a string in binary
template <class T>
void deserializeFromBinaryString(const std::string& serialized, T& output,
const std::string& name = "data") {
std::istringstream in_archive_stream(serialized);
deserializeFromBinaryStream(in_archive_stream, output, name);
}
/// serializes to a binary file
template <class T>
bool serializeToBinaryFile(const T& input, const std::string& filename,
const std::string& name = "data") {
std::ofstream out_archive_stream(filename.c_str());
if (!out_archive_stream.is_open()) return false;
serializeToBinaryStream(input, out_archive_stream, name);
out_archive_stream.close();
return true;
}
/// deserializes from a binary file
template <class T>
bool deserializeFromBinaryFile(const std::string& filename, T& output,
const std::string& name = "data") {
std::ifstream in_archive_stream(filename.c_str());
if (!in_archive_stream.is_open()) return false;
deserializeFromBinaryStream(in_archive_stream, output, name);
in_archive_stream.close();
return true;
}
/// serializes to a string in binary
template <class T>
std::string serializeBinary(const T& input,
const std::string& name = "data") {
return serializeToBinaryString(input, name);
}
/// deserializes from a string in binary
template <class T>
void deserializeBinary(const std::string& serialized, T& output,
const std::string& name = "data") {
deserializeFromBinaryString(serialized, output, name);
}
///@}
} // namespace gtsam

58
gtsam/base/serializationTestHelpers.h
View File

@ -26,6 +26,7 @@
#include <gtsam/base/serialization.h>
#include <boost/serialization/serialization.hpp>
#include <boost/filesystem.hpp>
// whether to print the serialized text to stdout
@ -40,22 +41,37 @@ T create() {
return T();
}
// Creates or empties a folder in the build folder and returns the relative path
boost::filesystem::path resetFilesystem(
boost::filesystem::path folder = "actual") {
boost::filesystem::remove_all(folder);
boost::filesystem::create_directory(folder);
return folder;
}
// Templated round-trip serialization
template<class T>
void roundtrip(const T& input, T& output) {
// Serialize
std::string serialized = serialize(input);
if (verbose) std::cout << serialized << std::endl << std::endl;
deserialize(serialized, output);
}
// This version requires equality operator
// Templated round-trip serialization using a file
template<class T>
void roundtripFile(const T& input, T& output) {
boost::filesystem::path path = resetFilesystem()/"graph.dat";
serializeToFile(input, path.string());
deserializeFromFile(path.string(), output);
}
// This version requires equality operator and uses string and file round-trips
template<class T>
bool equality(const T& input = T()) {
T output = create<T>();
T output = create<T>(), outputf = create<T>();
roundtrip<T>(input,output);
return input==output;
roundtripFile<T>(input,outputf);
return (input==output) && (input==outputf);
}
// This version requires Testable
@ -77,20 +93,26 @@ bool equalsDereferenced(const T& input) {
// Templated round-trip serialization using XML
template<class T>
void roundtripXML(const T& input, T& output) {
// Serialize
std::string serialized = serializeXML<T>(input);
if (verbose) std::cout << serialized << std::endl << std::endl;
// De-serialize
deserializeXML(serialized, output);
}
// Templated round-trip serialization using XML File
template<class T>
void roundtripXMLFile(const T& input, T& output) {
boost::filesystem::path path = resetFilesystem()/"graph.xml";
serializeToXMLFile(input, path.string());
deserializeFromXMLFile(path.string(), output);
}
// This version requires equality operator
template<class T>
bool equalityXML(const T& input = T()) {
T output = create<T>();
T output = create<T>(), outputf = create<T>();
roundtripXML<T>(input,output);
return input==output;
roundtripXMLFile<T>(input,outputf);
return (input==output) && (input==outputf);
}
// This version requires Testable
@ -112,20 +134,26 @@ bool equalsDereferencedXML(const T& input = T()) {
// Templated round-trip serialization using XML
template<class T>
void roundtripBinary(const T& input, T& output) {
// Serialize
std::string serialized = serializeBinary<T>(input);
if (verbose) std::cout << serialized << std::endl << std::endl;
// De-serialize
deserializeBinary(serialized, output);
}
// Templated round-trip serialization using Binary file
template<class T>
void roundtripBinaryFile(const T& input, T& output) {
boost::filesystem::path path = resetFilesystem()/"graph.bin";
serializeToBinaryFile(input, path.string());
deserializeFromBinaryFile(path.string(), output);
}
// This version requires equality operator
template<class T>
bool equalityBinary(const T& input = T()) {
T output = create<T>();
T output = create<T>(), outputf = create<T>();
roundtripBinary<T>(input,output);
return input==output;
roundtripBinaryFile<T>(input,outputf);
return (input==output) && (input==outputf);
}
// This version requires Testable

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

@ -32,28 +32,6 @@ namespace gtsam {
namespace internal {
/* ************************************************************************* */
template<typename NODE, typename DATA, typename VISITOR_POST>
class PostOrderTask : public tbb::task
{
public:
const boost::shared_ptr<NODE>& treeNode;
boost::shared_ptr<DATA> myData;
VISITOR_POST& visitorPost;
PostOrderTask(const boost::shared_ptr<NODE>& treeNode,
const boost::shared_ptr<DATA>& myData, VISITOR_POST& visitorPost)
: treeNode(treeNode), myData(myData), visitorPost(visitorPost) {}
tbb::task* execute()
{
// Run the post-order visitor
(void) visitorPost(treeNode, *myData);
return nullptr;
}
};
/* ************************************************************************* */
template<typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
class PreOrderTask : public tbb::task

67
gtsam/base/types.cpp Normal file
View File

@ -0,0 +1,67 @@
/* ----------------------------------------------------------------------------
* 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 types.cpp
* @brief Functions for handling type information
* @author Varun Agrawal
* @date May 18, 2020
* @addtogroup base
*/
#include <gtsam/base/types.h>
#ifdef _WIN32
#define WIN32_LEAN_AND_MEAN
#include <windows.h>
#include <DbgHelp.h>
#endif
#ifdef __GNUG__
#include <cstdlib>
#include <cxxabi.h>
#include <string>
#endif
namespace gtsam {
/// Pretty print Value type name
std::string demangle(const char* name) {
// by default set to the original mangled name
std::string demangled_name = std::string(name);
#ifdef __GNUG__
// g++ version of demangle
char* demangled = nullptr;
int status = -1; // some arbitrary value to eliminate the compiler warning
demangled = abi::__cxa_demangle(name, nullptr, nullptr, &status),
demangled_name = (status == 0) ? std::string(demangled) : std::string(name);
std::free(demangled);
#elif _WIN32
char undecorated_name[1024];
if (UnDecorateSymbolName(
name, undecorated_name, sizeof(undecorated_name),
UNDNAME_COMPLETE))
{
// successful conversion, take value from: undecorated_name
demangled_name = std::string(undecorated_name);
}
// else keep using mangled name
#endif
return demangled_name;
}
} /* namespace gtsam */

50
gtsam/base/types.h
View File

@ -53,6 +53,9 @@
namespace gtsam {
/// Function to demangle type name of variable, e.g. demangle(typeid(x).name())
std::string demangle(const char* name);
/// Integer nonlinear key type
typedef std::uint64_t Key;
@ -227,3 +230,50 @@ namespace std {
#ifdef ERROR
#undef ERROR
#endif
namespace gtsam {
/// Convenience void_t as we assume C++11, it will not conflict the std one in C++17 as this is in `gtsam::`
template<typename ...> using void_t = void;
/**
* A SFINAE trait to mark classes that need special alignment.
*
* This is required to make boost::make_shared and etc respect alignment, which is essential for the Python
* wrappers to work properly.
*
* Explanation
* =============
* When a GTSAM type is not declared with the type alias `_eigen_aligned_allocator_trait = void`, the first template
* will be taken so `needs_eigen_aligned_allocator` will be resolved to `std::false_type`.
*
* Otherwise, it will resolve to the second template, which will be resolved to `std::true_type`.
*
* Please refer to `gtsam/base/make_shared.h` for an example.
*/
template<typename, typename = void_t<>>
struct needs_eigen_aligned_allocator : std::false_type {
};
template<typename T>
struct needs_eigen_aligned_allocator<T, void_t<typename T::_eigen_aligned_allocator_trait>> : std::true_type {
};
}
/**
* This marks a GTSAM object to require alignment. With this macro an object will automatically be allocated in aligned
* memory when one uses `gtsam::make_shared`. It reduces future misalignment problems that is hard to debug.
* See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for detailed explanation.
*/
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW \
EIGEN_MAKE_ALIGNED_OPERATOR_NEW \
using _eigen_aligned_allocator_trait = void;
/**
* This marks a GTSAM object to require alignment. With this macro an object will automatically be allocated in aligned
* memory when one uses `gtsam::make_shared`. It reduces future misalignment problems that is hard to debug.
* See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for detailed explanation.
*/
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \
using _eigen_aligned_allocator_trait = void;

3
gtsam/config.h.in
View File

@ -42,6 +42,9 @@
// Whether we are using TBB (if TBB was found and GTSAM_WITH_TBB is enabled in CMake)
#cmakedefine GTSAM_USE_TBB
// Whether we are using a TBB version higher than 2020
#cmakedefine TBB_GREATER_EQUAL_2020
// Whether we are using system-Eigen or our own patched version
#cmakedefine GTSAM_USE_SYSTEM_EIGEN

2
gtsam/geometry/BearingRange.h
View File

@ -162,7 +162,7 @@ private:
NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0
};
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};
// Declare this to be both Testable and a Manifold

138
gtsam/geometry/Cal3Fisheye.cpp
View File

@ -24,7 +24,7 @@
namespace gtsam {
/* ************************************************************************* */
Cal3Fisheye::Cal3Fisheye(const Vector& v)
Cal3Fisheye::Cal3Fisheye(const Vector9& v)
: fx_(v[0]),
fy_(v[1]),
s_(v[2]),
@ -50,76 +50,73 @@ Matrix3 Cal3Fisheye::K() const {
}
/* ************************************************************************* */
static Matrix29 D2dcalibration(const double xd, const double yd,
const double xi, const double yi,
const double t3, const double t5,
const double t7, const double t9, const double r,
Matrix2& DK) {
// order: fx, fy, s, u0, v0
Matrix25 DR1;
DR1 << xd, 0.0, yd, 1.0, 0.0, 0.0, yd, 0.0, 0.0, 1.0;
// order: k1, k2, k3, k4
Matrix24 DR2;
DR2 << t3 * xi, t5 * xi, t7 * xi, t9 * xi, t3 * yi, t5 * yi, t7 * yi, t9 * yi;
DR2 /= r;
Matrix29 D;
D << DR1, DK * DR2;
return D;
}
/* ************************************************************************* */
static Matrix2 D2dintrinsic(const double xi, const double yi, const double r,
const double td, const double t, const double tt,
const double t4, const double t6, const double t8,
const double k1, const double k2, const double k3,
const double k4, const Matrix2& DK) {
const double dr_dxi = xi / sqrt(xi * xi + yi * yi);
const double dr_dyi = yi / sqrt(xi * xi + yi * yi);
const double dt_dr = 1 / (1 + r * r);
const double dtd_dt =
1 + 3 * k1 * tt + 5 * k2 * t4 + 7 * k3 * t6 + 9 * k4 * t8;
const double dtd_dxi = dtd_dt * dt_dr * dr_dxi;
const double dtd_dyi = dtd_dt * dt_dr * dr_dyi;
const double rinv = 1 / r;
const double rrinv = 1 / (r * r);
const double dxd_dxi =
dtd_dxi * xi * rinv + td * rinv + td * xi * (-rrinv) * dr_dxi;
const double dxd_dyi = dtd_dyi * xi * rinv - td * xi * rrinv * dr_dyi;
const double dyd_dxi = dtd_dxi * yi * rinv - td * yi * rrinv * dr_dxi;
const double dyd_dyi =
dtd_dyi * yi * rinv + td * rinv + td * yi * (-rrinv) * dr_dyi;
Matrix2 DR;
DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;
return DK * DR;
double Cal3Fisheye::Scaling(double r) {
static constexpr double threshold = 1e-8;
if (r > threshold || r < -threshold) {
return atan(r) / r;
} else {
// Taylor expansion close to 0
double r2 = r * r, r4 = r2 * r2;
return 1.0 - r2 / 3 + r4 / 5;
}
}
/* ************************************************************************* */
Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
OptionalJacobian<2, 2> H2) const {
const double xi = p.x(), yi = p.y();
const double r = sqrt(xi * xi + yi * yi);
const double r2 = xi * xi + yi * yi, r = sqrt(r2);
const double t = atan(r);
const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4;
const double td = t * (1 + k1_ * tt + k2_ * t4 + k3_ * t6 + k4_ * t8);
const double td_o_r = r > 1e-8 ? td / r : 1;
const double xd = td_o_r * xi, yd = td_o_r * yi;
const double t2 = t * t, t4 = t2 * t2, t6 = t2 * t4, t8 = t4 * t4;
Vector5 K, T;
K << 1, k1_, k2_, k3_, k4_;
T << 1, t2, t4, t6, t8;
const double scaling = Scaling(r);
const double s = scaling * K.dot(T);
const double xd = s * xi, yd = s * yi;
Point2 uv(fx_ * xd + s_ * yd + u0_, fy_ * yd + v0_);
Matrix2 DK;
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
// Derivative for calibration parameters (2 by 9)
if (H1)
*H1 = D2dcalibration(xd, yd, xi, yi, t * tt, t * t4, t * t6, t * t8, r, DK);
if (H1) {
Matrix25 DR1;
// order: fx, fy, s, u0, v0
DR1 << xd, 0.0, yd, 1.0, 0.0, 0.0, yd, 0.0, 0.0, 1.0;
// order: k1, k2, k3, k4
Matrix24 DR2;
auto T4 = T.tail<4>().transpose();
DR2 << xi * T4, yi * T4;
*H1 << DR1, DK * scaling * DR2;
}
// Derivative for points in intrinsic coords (2 by 2)
if (H2)
*H2 =
D2dintrinsic(xi, yi, r, td, t, tt, t4, t6, t8, k1_, k2_, k3_, k4_, DK);
if (H2) {
const double dtd_dt =
1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8;
const double dt_dr = 1 / (1 + r2);
const double rinv = 1 / r;
const double dr_dxi = xi * rinv;
const double dr_dyi = yi * rinv;
const double dtd_dxi = dtd_dt * dt_dr * dr_dxi;
const double dtd_dyi = dtd_dt * dt_dr * dr_dyi;
const double td = t * K.dot(T);
const double rrinv = 1 / r2;
const double dxd_dxi =
dtd_dxi * dr_dxi + td * rinv - td * xi * rrinv * dr_dxi;
const double dxd_dyi = dtd_dyi * dr_dxi - td * xi * rrinv * dr_dyi;
const double dyd_dxi = dtd_dxi * dr_dyi - td * yi * rrinv * dr_dxi;
const double dyd_dyi =
dtd_dyi * dr_dyi + td * rinv - td * yi * rrinv * dr_dyi;
Matrix2 DR;
DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;
*H2 = DK * DR;
}
return uv;
}
@ -157,39 +154,10 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
return pi;
}
/* ************************************************************************* */
Matrix2 Cal3Fisheye::D2d_intrinsic(const Point2& p) const {
const double xi = p.x(), yi = p.y();
const double r = sqrt(xi * xi + yi * yi);
const double t = atan(r);
const double tt = t * t, t4 = tt * tt, t6 = t4 * tt, t8 = t4 * t4;
const double td = t * (1 + k1_ * tt + k2_ * t4 + k3_ * t6 + k4_ * t8);
Matrix2 DK;
DK << fx_, s_, 0.0, fy_;
return D2dintrinsic(xi, yi, r, td, t, tt, t4, t6, t8, k1_, k2_, k3_, k4_, DK);
}
/* ************************************************************************* */
Matrix29 Cal3Fisheye::D2d_calibration(const Point2& p) const {
const double xi = p.x(), yi = p.y();
const double r = sqrt(xi * xi + yi * yi);
const double t = atan(r);
const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4;
const double td = t * (1 + k1_ * tt + k2_ * t4 + k3_ * t6 + k4_ * t8);
const double xd = td / r * xi, yd = td / r * yi;
Matrix2 DK;
DK << fx_, s_, 0.0, fy_;
return D2dcalibration(xd, yd, xi, yi, t * tt, t * t4, t * t6, t * t8, r, DK);
}
/* ************************************************************************* */
void Cal3Fisheye::print(const std::string& s_) const {
gtsam::print((Matrix)K(), s_ + ".K");
gtsam::print(Vector(k()), s_ + ".k");
;
}
/* ************************************************************************* */

16
gtsam/geometry/Cal3Fisheye.h
View File

@ -20,6 +20,8 @@
#include <gtsam/geometry/Point2.h>
#include <string>
namespace gtsam {
/**
@ -43,7 +45,7 @@ namespace gtsam {
* [u; v; 1] = K*[x_d; y_d; 1]
*/
class GTSAM_EXPORT Cal3Fisheye {
protected:
private:
double fx_, fy_, s_, u0_, v0_; // focal length, skew and principal point
double k1_, k2_, k3_, k4_; // fisheye distortion coefficients
@ -78,7 +80,7 @@ class GTSAM_EXPORT Cal3Fisheye {
/// @name Advanced Constructors
/// @{
Cal3Fisheye(const Vector& v);
explicit Cal3Fisheye(const Vector9& v);
/// @}
/// @name Standard Interface
@ -120,6 +122,9 @@ class GTSAM_EXPORT Cal3Fisheye {
/// Return all parameters as a vector
Vector9 vector() const;
/// Helper function that calculates atan(r)/r
static double Scaling(double r);
/**
* @brief convert intrinsic coordinates [x_i; y_i] to (distorted) image
* coordinates [u; v]
@ -136,13 +141,6 @@ class GTSAM_EXPORT Cal3Fisheye {
/// y_i]
Point2 calibrate(const Point2& p, const double tol = 1e-5) const;
/// Derivative of uncalibrate wrpt intrinsic coordinates [x_i; y_i]
Matrix2 D2d_intrinsic(const Point2& p) const;
/// Derivative of uncalibrate wrpt the calibration parameters
/// [fx, fy, s, u0, v0, k1, k2, k3, k4]
Matrix29 D2d_calibration(const Point2& p) const;
/// @}
/// @name Testable
/// @{

2
gtsam/geometry/CameraSet.h
View File

@ -319,7 +319,7 @@ private:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
template<class CAMERA>

2
gtsam/geometry/EssentialMatrix.h
View File

@ -212,7 +212,7 @@ class EssentialMatrix {
/// @}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
template<>

2
gtsam/geometry/PinholeCamera.h
View File

@ -325,7 +325,7 @@ private:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// manifold traits

4
gtsam/geometry/PinholePose.h
View File

@ -222,7 +222,7 @@ private:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// end of class PinholeBaseK
@ -425,7 +425,7 @@ private:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// end of class PinholePose

2
gtsam/geometry/Pose2.h
View File

@ -317,7 +317,7 @@ public:
public:
// Align for Point2, which is either derived from, or is typedef, of Vector2
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; // Pose2
/** specialization for pose2 wedge function (generic template in Lie.h) */

2
gtsam/geometry/Pose3.h
View File

@ -355,7 +355,7 @@ public:
#ifdef GTSAM_USE_QUATERNIONS
// Align if we are using Quaternions
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif
};
// Pose3 class

2
gtsam/geometry/Rot3.h
View File

@ -544,7 +544,7 @@ namespace gtsam {
#ifdef GTSAM_USE_QUATERNIONS
// only align if quaternion, Matrix3 has no alignment requirements
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif
};

8
gtsam/geometry/SO3.cpp
View File

@ -261,13 +261,13 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
// we do something special
if (std::abs(tr + 1.0) < 1e-10) {
if (std::abs(R33 + 1.0) > 1e-10)
if (tr + 1.0 < 1e-10) {
if (std::abs(R33 + 1.0) > 1e-5)
omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
else if (std::abs(R22 + 1.0) > 1e-10)
else if (std::abs(R22 + 1.0) > 1e-5)
omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
else
// if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
// if(std::abs(R.r1_.x()+1.0) > 1e-5) This is implicit
omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
} else {
double magnitude;

27
gtsam/geometry/SO4.cpp
View File

@ -33,9 +33,10 @@ using namespace std;
namespace gtsam {
// TODO(frank): is this better than SOn::Random?
// /* *************************************************************************
// */ static Vector3 randomOmega(boost::mt19937 &rng) {
// static boost::uniform_real<double> randomAngle(-M_PI, M_PI);
// static std::uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
// return Unit3::Random(rng).unitVector() * randomAngle(rng);
// }
@ -58,9 +59,9 @@ Matrix4 SO4::Hat(const Vector6& xi) {
Y(0, 1) = -xi(5);
Y(0, 2) = +xi(4);
Y(1, 2) = -xi(3);
Y(0, 3) = -xi(2);
Y(1, 3) = +xi(1);
Y(2, 3) = -xi(0);
Y(0, 3) = +xi(2);
Y(1, 3) = -xi(1);
Y(2, 3) = +xi(0);
return Y - Y.transpose();
}
@ -72,9 +73,9 @@ Vector6 SO4::Vee(const Matrix4& X) {
xi(5) = -X(0, 1);
xi(4) = +X(0, 2);
xi(3) = -X(1, 2);
xi(2) = -X(0, 3);
xi(1) = +X(1, 3);
xi(0) = -X(2, 3);
xi(2) = +X(0, 3);
xi(1) = -X(1, 3);
xi(0) = +X(2, 3);
return xi;
}
@ -208,9 +209,9 @@ GTSAM_EXPORT Matrix3 topLeft(const SO4& Q, OptionalJacobian<9, 6> H) {
if (H) {
const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2),
q = R.topRightCorner<3, 1>();
*H << Z_3x1, Z_3x1, q, Z_3x1, -m3, m2, //
Z_3x1, -q, Z_3x1, m3, Z_3x1, -m1, //
q, Z_3x1, Z_3x1, -m2, m1, Z_3x1;
*H << Z_3x1, Z_3x1, -q, Z_3x1, -m3, m2, //
Z_3x1, q, Z_3x1, m3, Z_3x1, -m1, //
-q, Z_3x1, Z_3x1, -m2, m1, Z_3x1;
}
return M;
}
@ -221,9 +222,9 @@ GTSAM_EXPORT Matrix43 stiefel(const SO4& Q, OptionalJacobian<12, 6> H) {
const Matrix43 M = R.leftCols<3>();
if (H) {
const auto &m1 = R.col(0), m2 = R.col(1), m3 = R.col(2), q = R.col(3);
*H << Z_4x1, Z_4x1, q, Z_4x1, -m3, m2, //
Z_4x1, -q, Z_4x1, m3, Z_4x1, -m1, //
q, Z_4x1, Z_4x1, -m2, m1, Z_4x1;
*H << Z_4x1, Z_4x1, -q, Z_4x1, -m3, m2, //
Z_4x1, q, Z_4x1, m3, Z_4x1, -m1, //
-q, Z_4x1, Z_4x1, -m2, m1, Z_4x1;
}
return M;
}

9
gtsam/geometry/SOn.cpp
View File

@ -38,11 +38,12 @@ Matrix SOn::Hat(const Vector& xi) {
const size_t dmin = (n - 1) * (n - 2) / 2;
X.topLeftCorner(n - 1, n - 1) = Hat(xi.tail(dmin));
// determine sign of last element (signs alternate)
double sign = pow(-1.0, xi.size());
// Now fill last row and column
double sign = 1.0;
for (size_t i = 0; i < n - 1; i++) {
const size_t j = n - 2 - i;
X(n - 1, j) = sign * xi(i);
X(n - 1, j) = -sign * xi(i);
X(j, n - 1) = -X(n - 1, j);
sign = -sign;
}
@ -67,10 +68,10 @@ Vector SOn::Vee(const Matrix& X) {
Vector xi(d);
// Fill first n-1 spots from last row of X
double sign = 1.0;
double sign = pow(-1.0, xi.size());
for (size_t i = 0; i < n - 1; i++) {
const size_t j = n - 2 - i;
xi(i) = sign * X(n - 1, j);
xi(i) = -sign * X(n - 1, j);
sign = -sign;
}

27
gtsam/geometry/SOn.h
View File

@ -20,14 +20,15 @@
#include <gtsam/base/Lie.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/make_shared.h>
#include <gtsam/dllexport.h>
#include <Eigen/Core>
#include <random>
#include <iostream> // TODO(frank): how to avoid?
#include <string>
#include <type_traits>
#include <vector>
#include <random>
namespace gtsam {
@ -53,7 +54,7 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
using VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1>;
using MatrixDD = Eigen::Matrix<double, dimension, dimension>;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(true)
protected:
MatrixNN matrix_; ///< Rotation matrix
@ -93,6 +94,16 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
return SO(R);
}
/// Named constructor from lower dimensional matrix
template <typename Derived, int N_ = N, typename = IsDynamic<N_>>
static SO Lift(size_t n, const Eigen::MatrixBase<Derived> &R) {
Matrix Q = Matrix::Identity(n, n);
size_t p = R.rows();
assert(p < n && R.cols() == p);
Q.topLeftCorner(p, p) = R;
return SO(Q);
}
/// Construct dynamic SO(n) from Fixed SO<M>
template <int M, int N_ = N, typename = IsDynamic<N_>>
explicit SO(const SO<M>& R) : matrix_(R.matrix()) {}
@ -207,11 +218,11 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
* etc... For example, the vector-space isomorphic to so(5) is laid out as:
* a b c d | u v w | x y | z
* where the latter elements correspond to "telescoping" sub-algebras:
* 0 -z y -w d
* z 0 -x v -c
* -y x 0 -u b
* w -v u 0 -a
* -d c -b a 0
* 0 -z y w -d
* z 0 -x -v c
* -y x 0 u -b
* -w v -u 0 a
* d -c b -a 0
* This scheme behaves exactly as expected for SO(2) and SO(3).
*/
static MatrixNN Hat(const TangentVector& xi);

2
gtsam/geometry/Unit3.h
View File

@ -214,7 +214,7 @@ private:
/// @}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// Define GTSAM traits

129
gtsam/geometry/tests/testCal3DFisheye.cpp
View File

@ -10,17 +10,18 @@
* -------------------------------------------------------------------------- */
/**
* @file testCal3Fisheye.cpp
* @file testCal3DFisheye.cpp
* @brief Unit tests for fisheye calibration class
* @author ghaggin
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Point3.h>
#include <CppUnitLite/TestHarness.h>
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Cal3Fisheye)
@ -30,12 +31,27 @@ static const double fx = 250, fy = 260, s = 0.1, u0 = 320, v0 = 240;
static Cal3Fisheye K(fx, fy, s, u0, v0, -0.013721808247486035,
0.020727425669427896, -0.012786476702685545,
0.0025242267320687625);
static Point2 p(2, 3);
static Point2 kTestPoint2(2, 3);
/* ************************************************************************* */
TEST(Cal3Fisheye, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); }
/* ************************************************************************* */
TEST(Cal3Fisheye, retract) {
Cal3Fisheye expected(K.fx() + 1, K.fy() + 2, K.skew() + 3, K.px() + 4,
K.py() + 5, K.k1() + 6, K.k2() + 7, K.k3() + 8,
K.k4() + 9);
Vector d(9);
d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
Cal3Fisheye actual = K.retract(d);
CHECK(assert_equal(expected, actual, 1e-7));
CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
}
/* ************************************************************************* */
TEST(Cal3Fisheye, uncalibrate1) {
// Calculate the solution
const double xi = p.x(), yi = p.y();
const double xi = kTestPoint2.x(), yi = kTestPoint2.y();
const double r = sqrt(xi * xi + yi * yi);
const double t = atan(r);
const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4;
@ -46,32 +62,42 @@ TEST(Cal3Fisheye, uncalibrate1) {
Point2 uv_sol(v[0] / v[2], v[1] / v[2]);
Point2 uv = K.uncalibrate(p);
Point2 uv = K.uncalibrate(kTestPoint2);
CHECK(assert_equal(uv, uv_sol));
}
/* ************************************************************************* */
/**
* Check that a point at (0,0) projects to the
* image center.
*/
TEST(Cal3Fisheye, uncalibrate2) {
Point2 pz(0, 0);
auto uv = K.uncalibrate(pz);
CHECK(assert_equal(uv, Point2(u0, v0)));
// For numerical derivatives
Point2 f(const Cal3Fisheye& k, const Point2& pt) { return k.uncalibrate(pt); }
/* ************************************************************************* */
TEST(Cal3Fisheye, Derivatives) {
Matrix H1, H2;
K.uncalibrate(kTestPoint2, H1, H2);
CHECK(assert_equal(numericalDerivative21(f, K, kTestPoint2, 1e-7), H1, 1e-5));
CHECK(assert_equal(numericalDerivative22(f, K, kTestPoint2, 1e-7), H2, 1e-5));
}
/* ************************************************************************* */
/**
* This test uses cv2::fisheye::projectPoints to test that uncalibrate
* properly projects a point into the image plane. One notable difference
* between opencv and the Cal3Fisheye::uncalibrate function is the skew
* parameter. The equivalence is alpha = s/fx.
*
*
* Python script to project points with fisheye model in OpenCv
* (script run with OpenCv version 4.2.0 and Numpy version 1.18.2)
*/
// Check that a point at (0,0) projects to the image center.
TEST(Cal3Fisheye, uncalibrate2) {
Point2 pz(0, 0);
Matrix H1, H2;
auto uv = K.uncalibrate(pz, H1, H2);
CHECK(assert_equal(uv, Point2(u0, v0)));
CHECK(assert_equal(numericalDerivative21(f, K, pz, 1e-7), H1, 1e-5));
// TODO(frank): the second jacobian is all NaN for the image center!
// CHECK(assert_equal(numericalDerivative22(f, K, pz, 1e-7), H2, 1e-5));
}
/* ************************************************************************* */
// This test uses cv2::fisheye::projectPoints to test that uncalibrate
// properly projects a point into the image plane. One notable difference
// between opencv and the Cal3Fisheye::uncalibrate function is the skew
// parameter. The equivalence is alpha = s/fx.
//
// Python script to project points with fisheye model in OpenCv
// (script run with OpenCv version 4.2.0 and Numpy version 1.18.2)
// clang-format off
/*
===========================================================
@ -94,6 +120,7 @@ tvec = np.float64([[0.,0.,0.]]);
imagePoints, jacobian = cv2.fisheye.projectPoints(objpts, rvec, tvec, cameraMatrix, distCoeffs, alpha=alpha)
np.set_printoptions(precision=14)
print(imagePoints)
===========================================================
* Script output: [[[457.82638130304935 408.18905848512986]]]
*/
@ -134,21 +161,18 @@ TEST(Cal3Fisheye, calibrate1) {
}
/* ************************************************************************* */
/**
* Check that calibrate returns (0,0) for the image center
*/
// Check that calibrate returns (0,0) for the image center
TEST(Cal3Fisheye, calibrate2) {
Point2 uv(u0, v0);
auto xi_hat = K.calibrate(uv);
CHECK(assert_equal(xi_hat, Point2(0, 0)))
}
/**
* Run calibrate on OpenCv test from uncalibrate3
* (script shown above)
* 3d point: (23, 27, 31)
* 2d point in image plane: (457.82638130304935, 408.18905848512986)
*/
/* ************************************************************************* */
// Run calibrate on OpenCv test from uncalibrate3
// (script shown above)
// 3d point: (23, 27, 31)
// 2d point in image plane: (457.82638130304935, 408.18905848512986)
TEST(Cal3Fisheye, calibrate3) {
Point3 p3(23, 27, 31);
Point2 xi(p3.x() / p3.z(), p3.y() / p3.z());
@ -157,47 +181,6 @@ TEST(Cal3Fisheye, calibrate3) {
CHECK(assert_equal(xi_hat, xi));
}
/* ************************************************************************* */
// For numerical derivatives
Point2 uncalibrate_(const Cal3Fisheye& k, const Point2& pt) {
return k.uncalibrate(pt);
}
/* ************************************************************************* */
TEST(Cal3Fisheye, Duncalibrate1) {
Matrix computed;
K.uncalibrate(p, computed, boost::none);
Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical, computed, 1e-5));
Matrix separate = K.D2d_calibration(p);
CHECK(assert_equal(numerical, separate, 1e-5));
}
/* ************************************************************************* */
TEST(Cal3Fisheye, Duncalibrate2) {
Matrix computed;
K.uncalibrate(p, boost::none, computed);
Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical, computed, 1e-5));
Matrix separate = K.D2d_intrinsic(p);
CHECK(assert_equal(numerical, separate, 1e-5));
}
/* ************************************************************************* */
TEST(Cal3Fisheye, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); }
/* ************************************************************************* */
TEST(Cal3Fisheye, retract) {
Cal3Fisheye expected(K.fx() + 1, K.fy() + 2, K.skew() + 3, K.px() + 4,
K.py() + 5, K.k1() + 6, K.k2() + 7, K.k3() + 8,
K.k4() + 9);
Vector d(9);
d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
Cal3Fisheye actual = K.retract(d);
CHECK(assert_equal(expected, actual, 1e-7));
CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
}
/* ************************************************************************* */
int main() {
TestResult tr;

6
gtsam/geometry/tests/testLine3.cpp
View File

@ -2,10 +2,10 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Line3.h>
#include <gtsam/slam/expressions.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/expressionTesting.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/expressions.h>
using namespace gtsam;
@ -146,7 +146,7 @@ TEST(Line3, projection) {
SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.1);
Unit3_ projected_(wL_, &Line3::project);
ExpressionFactor<Unit3> f(model, expected, projected_);
EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7);
EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-5);
}
int main() {

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

@ -181,63 +181,81 @@ TEST( Rot3, retract)
}
/* ************************************************************************* */
TEST(Rot3, log)
{
TEST(Rot3, log) {
static const double PI = boost::math::constants::pi<double>();
Vector w;
Rot3 R;
#define CHECK_OMEGA(X,Y,Z) \
w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \
R = Rot3::Rodrigues(w); \
EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12));
#define CHECK_OMEGA(X, Y, Z) \
w = (Vector(3) << X, Y, Z).finished(); \
R = Rot3::Rodrigues(w); \
EXPECT(assert_equal(w, Rot3::Logmap(R), 1e-12));
// Check zero
CHECK_OMEGA( 0, 0, 0)
CHECK_OMEGA(0, 0, 0)
// create a random direction:
double norm=sqrt(1.0+16.0+4.0);
double x=1.0/norm, y=4.0/norm, z=2.0/norm;
double norm = sqrt(1.0 + 16.0 + 4.0);
double x = 1.0 / norm, y = 4.0 / norm, z = 2.0 / norm;
// Check very small rotation for Taylor expansion
// Note that tolerance above is 1e-12, so Taylor is pretty good !
double d = 0.0001;
CHECK_OMEGA( d, 0, 0)
CHECK_OMEGA( 0, d, 0)
CHECK_OMEGA( 0, 0, d)
CHECK_OMEGA(x*d, y*d, z*d)
CHECK_OMEGA(d, 0, 0)
CHECK_OMEGA(0, d, 0)
CHECK_OMEGA(0, 0, d)
CHECK_OMEGA(x * d, y * d, z * d)
// check normal rotation
d = 0.1;
CHECK_OMEGA( d, 0, 0)
CHECK_OMEGA( 0, d, 0)
CHECK_OMEGA( 0, 0, d)
CHECK_OMEGA(x*d, y*d, z*d)
CHECK_OMEGA(d, 0, 0)
CHECK_OMEGA(0, d, 0)
CHECK_OMEGA(0, 0, d)
CHECK_OMEGA(x * d, y * d, z * d)
// Check 180 degree rotations
CHECK_OMEGA( PI, 0, 0)
CHECK_OMEGA( 0, PI, 0)
CHECK_OMEGA( 0, 0, PI)
CHECK_OMEGA(PI, 0, 0)
CHECK_OMEGA(0, PI, 0)
CHECK_OMEGA(0, 0, PI)
// Windows and Linux have flipped sign in quaternion mode
#if !defined(__APPLE__) && defined (GTSAM_USE_QUATERNIONS)
w = (Vector(3) << x*PI, y*PI, z*PI).finished();
#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS)
w = (Vector(3) << x * PI, y * PI, z * PI).finished();
R = Rot3::Rodrigues(w);
EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R),1e-12));
EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R), 1e-12));
#else
CHECK_OMEGA(x*PI,y*PI,z*PI)
CHECK_OMEGA(x * PI, y * PI, z * PI)
#endif
// Check 360 degree rotations
#define CHECK_OMEGA_ZERO(X,Y,Z) \
w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \
R = Rot3::Rodrigues(w); \
EXPECT(assert_equal((Vector) Z_3x1, Rot3::Logmap(R)));
#define CHECK_OMEGA_ZERO(X, Y, Z) \
w = (Vector(3) << X, Y, Z).finished(); \
R = Rot3::Rodrigues(w); \
EXPECT(assert_equal((Vector)Z_3x1, Rot3::Logmap(R)));
CHECK_OMEGA_ZERO( 2.0*PI, 0, 0)
CHECK_OMEGA_ZERO( 0, 2.0*PI, 0)
CHECK_OMEGA_ZERO( 0, 0, 2.0*PI)
CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI)
CHECK_OMEGA_ZERO(2.0 * PI, 0, 0)
CHECK_OMEGA_ZERO(0, 2.0 * PI, 0)
CHECK_OMEGA_ZERO(0, 0, 2.0 * PI)
CHECK_OMEGA_ZERO(x * 2. * PI, y * 2. * PI, z * 2. * PI)
// Check problematic case from Lund dataset vercingetorix.g2o
// This is an almost rotation with determinant not *quite* 1.
Rot3 Rlund(-0.98582676, -0.03958746, -0.16303092, //
-0.03997006, -0.88835923, 0.45740671, //
-0.16293753, 0.45743998, 0.87418537);
// Rot3's Logmap returns different, but equivalent compacted
// axis-angle vectors depending on whether Rot3 is implemented
// by Quaternions or SO3.
#if defined(GTSAM_USE_QUATERNIONS)
// Quaternion bounds angle to [-pi, pi] resulting in ~179.9 degrees
EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211),
(Vector)Rot3::Logmap(Rlund), 1e-8));
#else
// SO3 does not bound angle resulting in ~180.1 degrees
EXPECT(assert_equal(Vector3(-0.264544406, 0.742217405, 3.04117314),
(Vector)Rot3::Logmap(Rlund), 1e-8));
#endif
}
/* ************************************************************************* */
@ -536,16 +554,15 @@ TEST( Rot3, logmapStability ) {
TEST(Rot3, quaternion) {
// NOTE: This is also verifying the ability to convert Vector to Quaternion
Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782);
Rot3 R1 = Rot3((Matrix)(Matrix(3, 3) <<
0.271018623057411, 0.278786459830371, 0.921318086098018,
0.578529366719085, 0.717799701969298, -0.387385285854279,
-0.769319620053772, 0.637998195662053, 0.033250932803219).finished());
Rot3 R1(0.271018623057411, 0.278786459830371, 0.921318086098018,
0.578529366719085, 0.717799701969298, -0.387385285854279,
-0.769319620053772, 0.637998195662053, 0.033250932803219);
Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053);
Rot3 R2 = Rot3((Matrix)(Matrix(3, 3) <<
-0.207341903877828, 0.250149415542075, 0.945745528564780,
0.881304914479026, -0.371869043667957, 0.291573424846290,
0.424630407073532, 0.893945571198514, -0.143353873763946).finished());
Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335,
0.599136268678053);
Rot3 R2(-0.207341903877828, 0.250149415542075, 0.945745528564780,
0.881304914479026, -0.371869043667957, 0.291573424846290,
0.424630407073532, 0.893945571198514, -0.143353873763946);
// Check creating Rot3 from quaternion
EXPECT(assert_equal(R1, Rot3(q1)));

48
gtsam/geometry/tests/testSOn.cpp
View File

@ -46,7 +46,7 @@ TEST(SOn, SO0) {
EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension);
EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::Dim());
EXPECT_LONGS_EQUAL(0, R.dim());
EXPECT_LONGS_EQUAL(-1, traits<SOn>::GetDimension(R));
EXPECT_LONGS_EQUAL(0, traits<SOn>::GetDimension(R));
}
//******************************************************************************
@ -56,7 +56,7 @@ TEST(SOn, SO5) {
EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension);
EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::Dim());
EXPECT_LONGS_EQUAL(10, R.dim());
EXPECT_LONGS_EQUAL(-1, traits<SOn>::GetDimension(R));
EXPECT_LONGS_EQUAL(10, traits<SOn>::GetDimension(R));
}
//******************************************************************************
@ -95,32 +95,42 @@ TEST(SOn, Random) {
//******************************************************************************
TEST(SOn, HatVee) {
Vector6 v;
v << 1, 2, 3, 4, 5, 6;
Vector10 v;
v << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10;
Matrix expected2(2, 2);
expected2 << 0, -1, 1, 0;
const auto actual2 = SOn::Hat(v.head<1>());
CHECK(assert_equal(expected2, actual2));
CHECK(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2)));
EXPECT(assert_equal(expected2, actual2));
EXPECT(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2)));
Matrix expected3(3, 3);
expected3 << 0, -3, 2, //
3, 0, -1, //
-2, 1, 0;
expected3 << 0, -3, 2, //
3, 0, -1, //
-2, 1, 0;
const auto actual3 = SOn::Hat(v.head<3>());
CHECK(assert_equal(expected3, actual3));
CHECK(assert_equal(skewSymmetric(1, 2, 3), actual3));
CHECK(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3)));
EXPECT(assert_equal(expected3, actual3));
EXPECT(assert_equal(skewSymmetric(1, 2, 3), actual3));
EXPECT(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3)));
Matrix expected4(4, 4);
expected4 << 0, -6, 5, -3, //
6, 0, -4, 2, //
-5, 4, 0, -1, //
3, -2, 1, 0;
const auto actual4 = SOn::Hat(v);
CHECK(assert_equal(expected4, actual4));
CHECK(assert_equal((Vector)v, SOn::Vee(actual4)));
expected4 << 0, -6, 5, 3, //
6, 0, -4, -2, //
-5, 4, 0, 1, //
-3, 2, -1, 0;
const auto actual4 = SOn::Hat(v.head<6>());
EXPECT(assert_equal(expected4, actual4));
EXPECT(assert_equal((Vector)v.head<6>(), SOn::Vee(actual4)));
Matrix expected5(5, 5);
expected5 << 0,-10, 9, 7, -4, //
10, 0, -8, -6, 3, //
-9, 8, 0, 5, -2, //
-7, 6, -5, 0, 1, //
4, -3, 2, -1, 0;
const auto actual5 = SOn::Hat(v);
EXPECT(assert_equal(expected5, actual5));
EXPECT(assert_equal((Vector)v, SOn::Vee(actual5)));
}
//******************************************************************************

2
gtsam/geometry/triangulation.h
View File

@ -215,7 +215,7 @@ struct CameraProjectionMatrix {
private:
const Matrix3 K_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
/**

33
gtsam/linear/GaussianFactor.cpp Normal file
View File

@ -0,0 +1,33 @@
/* ----------------------------------------------------------------------------
* 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 GaussianFactor.cpp
* @brief A factor with a quadratic error function - a Gaussian
* @brief GaussianFactor
* @author Fan Jiang
*/
// \callgraph
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/VectorValues.h>
namespace gtsam {
/* ************************************************************************* */
VectorValues GaussianFactor::hessianDiagonal() const {
VectorValues d;
hessianDiagonalAdd(d);
return d;
}
}

5
gtsam/linear/GaussianFactor.h
View File

@ -100,7 +100,10 @@ namespace gtsam {
virtual Matrix information() const = 0;
/// Return the diagonal of the Hessian for this factor
virtual VectorValues hessianDiagonal() const = 0;
VectorValues hessianDiagonal() const;
/// Add the current diagonal to a VectorValues instance
virtual void hessianDiagonalAdd(VectorValues& d) const = 0;
/// Raw memory access version of hessianDiagonal
virtual void hessianDiagonal(double* d) const = 0;

3
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -255,8 +255,7 @@ namespace gtsam {
VectorValues d;
for (const sharedFactor& factor : *this) {
if(factor){
VectorValues di = factor->hessianDiagonal();
d.addInPlace_(di);
factor->hessianDiagonalAdd(d);
}
}
return d;

10
gtsam/linear/HessianFactor.cpp
View File

@ -302,12 +302,14 @@ Matrix HessianFactor::information() const {
}
/* ************************************************************************* */
VectorValues HessianFactor::hessianDiagonal() const {
VectorValues d;
void HessianFactor::hessianDiagonalAdd(VectorValues &d) const {
for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
d.emplace(keys_[j], info_.diagonal(j));
auto result = d.emplace(keys_[j], info_.diagonal(j));
if(!result.second) {
// if emplace fails, it returns an iterator to the existing element, which we add to:
result.first->second += info_.diagonal(j);
}
}
return d;
}
/* ************************************************************************* */

7
gtsam/linear/HessianFactor.h
View File

@ -293,8 +293,11 @@ namespace gtsam {
*/
Matrix information() const override;
/// Return the diagonal of the Hessian for this factor
VectorValues hessianDiagonal() const override;
/// Add the current diagonal to a VectorValues instance
void hessianDiagonalAdd(VectorValues& d) const override;
/// Using the base method
using Base::hessianDiagonal;
/// Raw memory access version of hessianDiagonal
void hessianDiagonal(double* d) const override;

28
gtsam/linear/JacobianFactor.cpp
View File

@ -542,21 +542,31 @@ Matrix JacobianFactor::information() const {
}
/* ************************************************************************* */
VectorValues JacobianFactor::hessianDiagonal() const {
VectorValues d;
void JacobianFactor::hessianDiagonalAdd(VectorValues& d) const {
for (size_t pos = 0; pos < size(); ++pos) {
Key j = keys_[pos];
size_t nj = Ab_(pos).cols();
Vector dj(nj);
auto result = d.emplace(j, nj);
Vector& dj = result.first->second;
for (size_t k = 0; k < nj; ++k) {
Vector column_k = Ab_(pos).col(k);
if (model_)
model_->whitenInPlace(column_k);
dj(k) = dot(column_k, column_k);
Eigen::Ref<const Vector> column_k = Ab_(pos).col(k);
if (model_) {
Vector column_k_copy = column_k;
model_->whitenInPlace(column_k_copy);
if(!result.second)
dj(k) += dot(column_k_copy, column_k_copy);
else
dj(k) = dot(column_k_copy, column_k_copy);
} else {
if (!result.second)
dj(k) += dot(column_k, column_k);
else
dj(k) = dot(column_k, column_k);
}
}
d.emplace(j, dj);
}
return d;
}
/* ************************************************************************* */

7
gtsam/linear/JacobianFactor.h
View File

@ -215,8 +215,11 @@ namespace gtsam {
*/
Matrix information() const override;
/// Return the diagonal of the Hessian for this factor
VectorValues hessianDiagonal() const override;
/// Using the base method
using Base::hessianDiagonal;
/// Add the current diagonal to a VectorValues instance
void hessianDiagonalAdd(VectorValues& d) const override;
/// Raw memory access version of hessianDiagonal
void hessianDiagonal(double* d) const override;

24
gtsam/linear/Preconditioner.cpp
View File

@ -183,21 +183,21 @@ void BlockJacobiPreconditioner::clean() {
}
/***************************************************************************************/
boost::shared_ptr<Preconditioner> createPreconditioner(const boost::shared_ptr<PreconditionerParameters> parameters) {
if ( DummyPreconditionerParameters::shared_ptr dummy = boost::dynamic_pointer_cast<DummyPreconditionerParameters>(parameters) ) {
boost::shared_ptr<Preconditioner> createPreconditioner(
const boost::shared_ptr<PreconditionerParameters> params) {
using boost::dynamic_pointer_cast;
if (dynamic_pointer_cast<DummyPreconditionerParameters>(params)) {
return boost::make_shared<DummyPreconditioner>();
}
else if ( BlockJacobiPreconditionerParameters::shared_ptr blockJacobi = boost::dynamic_pointer_cast<BlockJacobiPreconditionerParameters>(parameters) ) {
} else if (dynamic_pointer_cast<BlockJacobiPreconditionerParameters>(
params)) {
return boost::make_shared<BlockJacobiPreconditioner>();
}
else if ( SubgraphPreconditionerParameters::shared_ptr subgraph = boost::dynamic_pointer_cast<SubgraphPreconditionerParameters>(parameters) ) {
} else if (auto subgraph =
dynamic_pointer_cast<SubgraphPreconditionerParameters>(
params)) {
return boost::make_shared<SubgraphPreconditioner>(*subgraph);
}
throw invalid_argument("createPreconditioner: unexpected preconditioner parameter type");
throw invalid_argument(
"createPreconditioner: unexpected preconditioner parameter type");
}
}
} // namespace gtsam

6
gtsam/linear/RegularJacobianFactor.h
View File

@ -102,10 +102,8 @@ public:
DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax;
}
/// Expose base class hessianDiagonal
virtual VectorValues hessianDiagonal() const {
return JacobianFactor::hessianDiagonal();
}
/// Using the base method
using GaussianFactor::hessianDiagonal;
/// Raw memory access version of hessianDiagonal
void hessianDiagonal(double* d) const {

14
gtsam/linear/VectorValues.cpp
View File

@ -96,20 +96,6 @@ namespace gtsam {
return result.first;
}
/* ************************************************************************* */
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)
+ "' already in this VectorValues.");
return result.first;
}
/* ************************************************************************* */
void VectorValues::update(const VectorValues& values)
{

11
gtsam/linear/VectorValues.h
View File

@ -179,7 +179,14 @@ namespace gtsam {
* j is already used.
* @param value The vector to be inserted.
* @param j The index with which the value will be associated. */
iterator emplace(Key j, const Vector& value);
template<class... Args>
inline std::pair<VectorValues::iterator, bool> emplace(Key j, Args&&... args) {
#if ! defined(GTSAM_USE_TBB) || defined (TBB_GREATER_EQUAL_2020)
return values_.emplace(std::piecewise_construct, std::forward_as_tuple(j), std::forward_as_tuple(args...));
#else
return values_.insert(std::make_pair(j, Vector(std::forward<Args>(args)...)));
#endif
}
/** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c
* j is already used.
@ -197,7 +204,7 @@ namespace gtsam {
* and an iterator to the existing value is returned, along with 'false'. If the value did not
* 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) {
inline std::pair<iterator, bool> tryInsert(Key j, const Vector& value) {
#ifdef TBB_GREATER_EQUAL_2020
return values_.emplace(j, value);
#else

9
gtsam/linear/linearAlgorithms-inst.h
View File

@ -98,8 +98,13 @@ namespace gtsam
// Insert solution into a VectorValues
DenseIndex vectorPosition = 0;
for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) {
VectorValues::const_iterator r =
collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal)));
auto result = collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal)));
if(!result.second)
throw std::runtime_error(
"Internal error while optimizing clique. Trying to insert key '" + DefaultKeyFormatter(*frontal)
+ "' that exists.");
VectorValues::const_iterator r = result.first;
myData.cliqueResults.emplace(r->first, r);
vectorPosition += c.getDim(frontal);
}

4
gtsam/navigation/AttitudeFactor.h
View File

@ -139,7 +139,7 @@ private:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
/// traits
@ -219,7 +219,7 @@ private:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
/// traits

6
gtsam/navigation/CombinedImuFactor.h
View File

@ -100,7 +100,7 @@ private:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
@ -210,7 +210,7 @@ public:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
/**
@ -332,7 +332,7 @@ private:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// class CombinedImuFactor

2
gtsam/navigation/ImuBias.h
View File

@ -171,7 +171,7 @@ private:
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
/// @}
}; // ConstantBias class

4
gtsam/navigation/PreintegratedRotation.h
View File

@ -69,7 +69,7 @@ struct GTSAM_EXPORT PreintegratedRotationParams {
#ifdef GTSAM_USE_QUATERNIONS
// Align if we are using Quaternions
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif
};
@ -182,7 +182,7 @@ class GTSAM_EXPORT PreintegratedRotation {
#ifdef GTSAM_USE_QUATERNIONS
// Align if we are using Quaternions
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif
};

11
gtsam/navigation/PreintegrationBase.cpp
View File

@ -115,20 +115,21 @@ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
NavState PreintegrationBase::predict(const NavState& state_i,
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
OptionalJacobian<9, 6> H2) const {
// TODO(frank): make sure this stuff is still correct
Matrix96 D_biasCorrected_bias;
Vector9 biasCorrected = biasCorrectedDelta(bias_i,
H2 ? &D_biasCorrected_bias : 0);
H2 ? &D_biasCorrected_bias : nullptr);
// Correct for initial velocity and gravity
Matrix9 D_delta_state, D_delta_biasCorrected;
Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity,
p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0,
H2 ? &D_delta_biasCorrected : 0);
p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : nullptr,
H2 ? &D_delta_biasCorrected : nullptr);
// Use retract to get back to NavState manifold
Matrix9 D_predict_state, D_predict_delta;
NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta);
NavState state_j = state_i.retract(xi,
H1 ? &D_predict_state : nullptr,
H2 || H2 ? &D_predict_delta : nullptr);
if (H1)
*H1 = D_predict_state + D_predict_delta * D_delta_state;
if (H2)

2
gtsam/navigation/PreintegrationBase.h
View File

@ -214,7 +214,7 @@ class GTSAM_EXPORT PreintegrationBase {
#endif
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
} /// namespace gtsam

2
gtsam/navigation/PreintegrationParams.h
View File

@ -84,7 +84,7 @@ protected:
#ifdef GTSAM_USE_QUATERNIONS
// Align if we are using Quaternions
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif
};

2
gtsam/navigation/TangentPreintegration.h
View File

@ -141,7 +141,7 @@ private:
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
} /// namespace gtsam

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

@ -342,7 +342,7 @@ TEST( AHRSFactor, fistOrderExponential ) {
//******************************************************************************
TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
// Linearization point
Vector3 bias; ///< Current estimate of rotation rate bias
Vector3 bias = Vector3::Zero(); ///< Current estimate of rotation rate bias
Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1));

0
gtsam/navigation/tests/testScenarios.cpp → gtsam/navigation/tests/testScenarioRunner.cpp
View File

2
gtsam/nonlinear/ExpressionFactor.h
View File

@ -209,7 +209,7 @@ private:
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};
// ExpressionFactor

6
gtsam/nonlinear/NonlinearEquality.h
View File

@ -175,7 +175,7 @@ public:
/// @}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
private:
@ -265,7 +265,7 @@ public:
traits<X>::Print(value_, "Value");
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
private:
@ -331,7 +331,7 @@ public:
return traits<X>::Local(x1,x2);
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
private:

12
gtsam/nonlinear/NonlinearOptimizer.cpp
View File

@ -147,13 +147,15 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg,
} else if (params.isIterative()) {
// Conjugate Gradient -> needs params.iterativeParams
if (!params.iterativeParams)
throw std::runtime_error("NonlinearOptimizer::solve: cg parameter has to be assigned ...");
throw std::runtime_error(
"NonlinearOptimizer::solve: cg parameter has to be assigned ...");
if (boost::shared_ptr<PCGSolverParameters> pcg =
boost::dynamic_pointer_cast<PCGSolverParameters>(params.iterativeParams)) {
if (auto pcg = boost::dynamic_pointer_cast<PCGSolverParameters>(
params.iterativeParams)) {
delta = PCGSolver(*pcg).optimize(gfg);
} else if (boost::shared_ptr<SubgraphSolverParameters> spcg =
boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams)) {
} else if (auto spcg =
boost::dynamic_pointer_cast<SubgraphSolverParameters>(
params.iterativeParams)) {
if (!params.ordering)
throw std::runtime_error("SubgraphSolver needs an ordering");
delta = SubgraphSolver(gfg, *spcg, *params.ordering).optimize();

2
gtsam/nonlinear/PriorFactor.h
View File

@ -114,7 +114,7 @@ namespace gtsam {
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};
} /// namespace gtsam

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