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

Merge branch 'develop' into isam2_imu_example

release/4.3a0
Frank Dellaert 2020-07-13 21:06:24 -04:00 committed by GitHub
parent 221dcaa13a 73209e6faa
commit 954741093c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
545 changed files with 48223 additions and 3466 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

53
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})
@ -200,7 +201,7 @@ find_package(TBB 4.4 COMPONENTS tbb tbbmalloc)
# Set up variables if we're using TBB
if(TBB_FOUND AND GTSAM_WITH_TBB)
set(GTSAM_USE_TBB 1) # This will go into config.h
if (${TBB_VERSION_MAJOR} GREATER_EQUAL 2020)
if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020))
set(TBB_GREATER_EQUAL_2020 1)
else()
set(TBB_GREATER_EQUAL_2020 0)
@ -536,54 +537,54 @@ endif()
print_build_options_for_target(gtsam)
message(STATUS " Use System Eigen : ${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})")
message(STATUS " Use System Eigen : ${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})")
if(GTSAM_USE_TBB)
message(STATUS " Use Intel TBB : Yes")
message(STATUS " Use Intel TBB : Yes")
elseif(TBB_FOUND)
message(STATUS " Use Intel TBB : TBB found but GTSAM_WITH_TBB is disabled")
message(STATUS " Use Intel TBB : TBB found but GTSAM_WITH_TBB is disabled")
else()
message(STATUS " Use Intel TBB : TBB not found")
message(STATUS " Use Intel TBB : TBB not found")
endif()
if(GTSAM_USE_EIGEN_MKL)
message(STATUS " Eigen will use MKL : Yes")
message(STATUS " Eigen will use MKL : Yes")
elseif(MKL_FOUND)
message(STATUS " Eigen will use MKL : MKL found but GTSAM_WITH_EIGEN_MKL is disabled")
message(STATUS " Eigen will use MKL : MKL found but GTSAM_WITH_EIGEN_MKL is disabled")
else()
message(STATUS " Eigen will use MKL : MKL not found")
message(STATUS " Eigen will use MKL : MKL not found")
endif()
if(GTSAM_USE_EIGEN_MKL_OPENMP)
message(STATUS " Eigen will use MKL and OpenMP : Yes")
message(STATUS " Eigen will use MKL and OpenMP : Yes")
elseif(OPENMP_FOUND AND NOT GTSAM_WITH_EIGEN_MKL)
message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but GTSAM_WITH_EIGEN_MKL is disabled")
message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but GTSAM_WITH_EIGEN_MKL is disabled")
elseif(OPENMP_FOUND AND NOT MKL_FOUND)
message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but MKL not found")
message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but MKL not found")
elseif(OPENMP_FOUND)
message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but GTSAM_WITH_EIGEN_MKL_OPENMP is disabled")
message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but GTSAM_WITH_EIGEN_MKL_OPENMP is disabled")
else()
message(STATUS " Eigen will use MKL and OpenMP : OpenMP not found")
message(STATUS " Eigen will use MKL and OpenMP : OpenMP not found")
endif()
message(STATUS " Default allocator : ${GTSAM_DEFAULT_ALLOCATOR}")
message(STATUS " Default allocator : ${GTSAM_DEFAULT_ALLOCATOR}")
if(GTSAM_THROW_CHEIRALITY_EXCEPTION)
message(STATUS " Cheirality exceptions enabled : YES")
message(STATUS " Cheirality exceptions enabled : YES")
else()
message(STATUS " Cheirality exceptions enabled : NO")
message(STATUS " Cheirality exceptions enabled : NO")
endif()
if(NOT MSVC AND NOT XCODE_VERSION)
if(CCACHE_FOUND AND GTSAM_BUILD_WITH_CCACHE)
message(STATUS " Build with ccache : Yes")
message(STATUS " Build with ccache : Yes")
elseif(CCACHE_FOUND)
message(STATUS " Build with ccache : ccache found but GTSAM_BUILD_WITH_CCACHE is disabled")
message(STATUS " Build with ccache : ccache found but GTSAM_BUILD_WITH_CCACHE is disabled")
else()
message(STATUS " Build with ccache : No")
message(STATUS " Build with ccache : No")
endif()
endif()
message(STATUS "Packaging flags ")
message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}")
message(STATUS " CPack Generator : ${CPACK_GENERATOR}")
message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}")
message(STATUS " CPack Generator : ${CPACK_GENERATOR}")
message(STATUS "GTSAM flags ")
print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ")
@ -594,13 +595,17 @@ print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 al
print_config_flag(${GTSAM_TYPEDEF_POINTS_TO_VECTORS} "Point3 is typedef to Vector3 ")
print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ")
print_config_flag(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration")
print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ")
print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ")
message(STATUS "MATLAB toolbox flags ")
print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ")
print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install MATLAB toolbox ")
if (${GTSAM_INSTALL_MATLAB_TOOLBOX})
message(STATUS " MATLAB root : ${MATLAB_ROOT}")
message(STATUS " MEX binary : ${MEX_COMMAND}")
endif()
message(STATUS "Cython toolbox flags ")
print_config_flag(${GTSAM_INSTALL_CYTHON_TOOLBOX} "Install Cython toolbox ")
print_config_flag(${GTSAM_INSTALL_CYTHON_TOOLBOX} "Install Cython toolbox ")
if(GTSAM_INSTALL_CYTHON_TOOLBOX)
message(STATUS " Python version : ${GTSAM_PYTHON_VERSION}")
endif()

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/FindMKL.cmake
View File

@ -115,7 +115,7 @@ IF(WIN32 AND MKL_ROOT_DIR)
IF (MKL_INCLUDE_DIR MATCHES "10.3")
SET(MKL_LIBS ${MKL_LIBS} libiomp5md)
ENDIF()
FOREACH (LIB ${MKL_LIBS})
FIND_LIBRARY(${LIB}_PATH ${LIB} PATHS ${MKL_LIB_SEARCHPATH} ENV LIBRARY_PATH)
IF(${LIB}_PATH)
@ -147,7 +147,7 @@ ELSEIF(MKL_ROOT_DIR) # UNIX and macOS
${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
${MKL_ROOT_DIR}/lib/
)
# MKL on Mac OS doesn't ship with GNU thread versions, only Intel versions (see above)
IF(NOT APPLE)
FIND_LIBRARY(MKL_GNUTHREAD_LIBRARY
@ -231,6 +231,7 @@ ELSEIF(MKL_ROOT_DIR) # UNIX and macOS
FIND_LIBRARY(MKL_IOMP5_LIBRARY
iomp5
PATHS
${MKL_ROOT_DIR}/lib/intel64
${MKL_ROOT_DIR}/../lib/intel64
)
ELSE()
@ -254,7 +255,7 @@ ELSEIF(MKL_ROOT_DIR) # UNIX and macOS
ELSE()
SET(MKL_LIBRARIES ${MKL_LP_GNUTHREAD_LIBRARIES})
ENDIF()
MARK_AS_ADVANCED(MKL_CORE_LIBRARY MKL_LP_LIBRARY MKL_ILP_LIBRARY
MKL_SEQUENTIAL_LIBRARY MKL_INTELTHREAD_LIBRARY MKL_GNUTHREAD_LIBRARY)
ENDIF()
@ -266,4 +267,4 @@ find_package_handle_standard_args(MKL DEFAULT_MSG MKL_INCLUDE_DIR MKL_LIBRARIES)
# LINK_DIRECTORIES(${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}) # hack
#endif()
MARK_AS_ADVANCED(MKL_INCLUDE_DIR MKL_LIBRARIES)
MARK_AS_ADVANCED(MKL_INCLUDE_DIR MKL_LIBRARIES)

5
cmake/GtsamBuildTypes.cmake
View File

@ -1,3 +1,8 @@
# Set cmake policy to recognize the AppleClang compiler
# independently from the Clang compiler.
if(POLICY CMP0025)
cmake_policy(SET CMP0025 NEW)
endif()
# function: list_append_cache(var [new_values ...])
# Like "list(APPEND ...)" but working for CACHE variables.

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()

3
cmake/example_cmake_find_gtsam/main.cpp
View File

@ -33,7 +33,6 @@
// Here we will use Between factors for the relative motion described by odometry measurements.
// We will also use a Between Factor to encode the loop closure constraint
// Also, we will initialize the robot at the origin using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
@ -69,7 +68,7 @@ int main(int argc, char** argv) {
// 2a. Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.emplace_shared<PriorFactor<Pose2> >(1, Pose2(0, 0, 0), priorNoise);
graph.addPrior(1, Pose2(0, 0, 0), priorNoise);
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));

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

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

@ -11,17 +11,17 @@ 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
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, NonlinearFactorGraph,
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)
GenericProjectionFactorCal3_S2, Marginals,
NonlinearFactorGraph, PinholeCameraCal3_S2, Point3,
Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3,
SimpleCamera, Values)
from gtsam.utils import plot
def main():
@ -70,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
@ -79,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')
@ -94,13 +94,11 @@ def main():
# Intentionally initialize the variables off from the ground truth
initial_estimate = Values()
for i, pose in enumerate(poses):
r = Rot3.Rodrigues(-0.1, 0.2, 0.25)
t = Point3(0.05, -0.10, 0.20)
transformed_pose = pose.compose(Pose3(r, t))
initial_estimate.insert(symbol('x', i), transformed_pose)
transformed_pose = pose.retract(0.1*np.random.randn(6,1))
initial_estimate.insert(X(i), transformed_pose)
for j, point in enumerate(points):
transformed_point = Point3(point.vector() + np.array([-0.25, 0.20, 0.15]))
initial_estimate.insert(symbol('l', j), transformed_point)
transformed_point = Point3(point.vector() + 0.1*np.random.randn(3))
initial_estimate.insert(L(j), transformed_point)
initial_estimate.print_('Initial Estimates:\n')
# Optimize the graph and print results
@ -113,6 +111,11 @@ def main():
print('initial error = {}'.format(graph.error(initial_estimate)))
print('final error = {}'.format(graph.error(result)))
marginals = Marginals(graph, result)
plot.plot_3d_points(1, result, marginals=marginals)
plot.plot_trajectory(1, result, marginals=marginals, scale=8)
plot.set_axes_equal(1)
plt.show()
if __name__ == '__main__':
main()

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

@ -25,14 +25,15 @@ def createPoints():
def createPoses(K):
# Create the set of ground-truth poses
radius = 30.0
radius = 40.0
height = 10.0
angles = np.linspace(0, 2*np.pi, 8, endpoint=False)
up = gtsam.Point3(0, 0, 1)
target = gtsam.Point3(0, 0, 0)
poses = []
for theta in angles:
position = gtsam.Point3(radius*np.cos(theta),
radius*np.sin(theta), 0.0)
radius*np.sin(theta), height)
camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K)
poses.append(camera.pose())
return poses

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)

56
cython/gtsam/tests/test_FrobeniusFactor.py Normal file
View File

@ -0,0 +1,56 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
FrobeniusFactor unit tests.
Author: Frank Dellaert
"""
# pylint: disable=no-name-in-module, import-error, invalid-name
import unittest
import numpy as np
from gtsam import (Rot3, SO3, SO4, FrobeniusBetweenFactorSO4, FrobeniusFactorSO4,
FrobeniusWormholeFactor, SOn)
id = SO4()
v1 = np.array([0, 0, 0, 0.1, 0, 0])
Q1 = SO4.Expmap(v1)
v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
Q2 = SO4.Expmap(v2)
class TestFrobeniusFactorSO4(unittest.TestCase):
"""Test FrobeniusFactor factors."""
def test_frobenius_factor(self):
"""Test creation of a factor that calculates the Frobenius norm."""
factor = FrobeniusFactorSO4(1, 2)
actual = factor.evaluateError(Q1, Q2)
expected = (Q2.matrix()-Q1.matrix()).transpose().reshape((16,))
np.testing.assert_array_equal(actual, expected)
def test_frobenius_between_factor(self):
"""Test creation of a Frobenius BetweenFactor."""
factor = FrobeniusBetweenFactorSO4(1, 2, Q1.between(Q2))
actual = factor.evaluateError(Q1, Q2)
expected = np.zeros((16,))
np.testing.assert_array_almost_equal(actual, expected)
def test_frobenius_wormhole_factor(self):
"""Test creation of a factor that calculates Shonan error."""
R1 = SO3.Expmap(v1[3:])
R2 = SO3.Expmap(v2[3:])
factor = FrobeniusWormholeFactor(1, 2, Rot3(R1.between(R2).matrix()), p=4)
I4 = SOn(4)
Q1 = I4.retract(v1)
Q2 = I4.retract(v2)
actual = factor.evaluateError(Q1, Q2)
expected = np.zeros((12,))
np.testing.assert_array_almost_equal(actual, expected, decimal=4)
if __name__ == "__main__":
unittest.main()

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

13
cython/gtsam/tests/test_NonlinearOptimizer.py
View File

@ -17,7 +17,8 @@ import unittest
import gtsam
from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer,
GaussNewtonParams, LevenbergMarquardtOptimizer,
LevenbergMarquardtParams, NonlinearFactorGraph, Ordering,
LevenbergMarquardtParams, PCGSolverParameters,
DummyPreconditionerParameters, NonlinearFactorGraph, Ordering,
Point2, PriorFactorPoint2, Values)
from gtsam.utils.test_case import GtsamTestCase
@ -61,6 +62,16 @@ class TestScenario(GtsamTestCase):
fg, initial_values, lmParams).optimize()
self.assertAlmostEqual(0, fg.error(actual2))
# Levenberg-Marquardt
lmParams = LevenbergMarquardtParams.CeresDefaults()
lmParams.setLinearSolverType("ITERATIVE")
cgParams = PCGSolverParameters()
cgParams.setPreconditionerParams(DummyPreconditionerParameters())
lmParams.setIterativeParams(cgParams)
actual2 = LevenbergMarquardtOptimizer(
fg, initial_values, lmParams).optimize()
self.assertAlmostEqual(0, fg.error(actual2))
# Dogleg
dlParams = DoglegParams()
dlParams.setOrdering(ordering)

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__':

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

@ -0,0 +1,108 @@
"""
Unit tests for optimization that logs to comet.ml.
Author: Jing Wu and Frank Dellaert
"""
# pylint: disable=invalid-name
import sys
if sys.version_info.major >= 3:
from io import StringIO
else:
from cStringIO import StringIO
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)
self.lmparams = gtsam.LevenbergMarquardtParams()
self.lmoptimizer = gtsam.LevenbergMarquardtOptimizer(
graph, initial, self.lmparams
)
# setup output capture
self.capturedOutput = StringIO()
sys.stdout = self.capturedOutput
def tearDown(self):
"""Reset print capture."""
sys.stdout = sys.__stdout__
def test_simple_printing(self):
"""Test with a simple hook."""
# Provide a hook that just prints
def hook(_, error):
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)
def test_lm_simple_printing(self):
"""Make sure we are properly terminating LM"""
def hook(_, error):
print(error)
gtsam_optimize(self.lmoptimizer, self.lmparams, hook)
actual = self.lmoptimizer.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):
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()

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

@ -0,0 +1,52 @@
"""
Optimization with logging via a hook.
Author: Jing Wu and Frank Dellaert
"""
# pylint: disable=invalid-name
from gtsam import NonlinearOptimizer, NonlinearOptimizerParams
import gtsam
def optimize(optimizer, 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,
params,
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)) or (
isinstance(optimizer, gtsam.LevenbergMarquardtOptimizer) and optimizer.lambda_() > params.getlambdaUpperBound())
optimize(optimizer, check_convergence, hook)
return optimizer.values()

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

@ -3,10 +3,109 @@
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import patches
from mpl_toolkits.mplot3d import Axes3D
import gtsam
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.
Args:
fignum (int): An integer representing the figure number for Matplotlib.
"""
fig = plt.figure(fignum)
ax = fig.gca(projection='3d')
limits = np.array([
ax.get_xlim3d(),
ax.get_ylim3d(),
ax.get_zlim3d(),
])
origin = np.mean(limits, axis=1)
radius = 0.5 * np.max(np.abs(limits[:, 1] - limits[:, 0]))
ax.set_xlim3d([origin[0] - radius, origin[0] + radius])
ax.set_ylim3d([origin[1] - radius, origin[1] + radius])
ax.set_zlim3d([origin[2] - radius, origin[2] + radius])
def ellipsoid(xc, yc, zc, rx, ry, rz, n):
"""
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
y = -ry * np.outer(np.sin(u), np.sin(v)).T
z = -rz * np.outer(np.ones_like(u), np.cos(v)).T
return x, y, z
def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5):
"""
Plots a Gaussian as an uncertainty ellipse
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)
radii = k * np.sqrt(S)
radii = radii * scale
rx, ry, rz = radii
# generate data for "unrotated" ellipsoid
xc, yc, zc = ellipsoid(0, 0, 0, rx, ry, rz, n)
# rotate data with orientation matrix U and center c
data = np.kron(U[:, 0:1], xc) + np.kron(U[:, 1:2], yc) + \
np.kron(U[:, 2:3], zc)
n = data.shape[1]
x = data[0:n, :] + origin[0]
y = data[n:2*n, :] + origin[1]
z = data[2*n:, :] + origin[2]
axes.plot_surface(x, y, z, alpha=alpha, cmap='hot')
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()
@ -35,32 +134,66 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None):
np.rad2deg(angle), fill=False)
axes.add_patch(e1)
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):
"""Plot a 3D point on given axis 'axes' with given 'linespec'."""
def plot_point3_on_axes(axes, point, linespec, P=None):
"""
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):
"""Plot a 3D point on given figure with given 'linespec'."""
def plot_point3(fignum, point, linespec, P=None):
"""
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)
plot_point3_on_axes(axes, point, linespec, P)
def plot_3d_points(fignum, values, linespec, marginals=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()
@ -68,23 +201,33 @@ def plot_3d_points(fignum, values, linespec, marginals=None):
# Plot points and covariance matrices
for i in range(keys.size()):
try:
p = values.atPoint3(keys.at(i))
# if haveMarginals
# P = marginals.marginalCovariance(key);
# gtsam.plot_point3(p, linespec, P);
# else
plot_point3(fignum, p, linespec)
key = keys.at(i)
point = values.atPoint3(key)
if marginals is not None:
covariance = marginals.marginalCovariance(key)
else:
covariance = None
plot_point3(fignum, point, linespec, covariance)
except RuntimeError:
continue
# I guess it's not a Point3
def plot_pose3_on_axes(axes, pose, 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
t = pose.translation()
origin = np.array([t.x(), t.y(), t.z()])
origin = pose.translation().vector()
# draw the camera axes
x_axis = origin + gRp[:, 0] * axis_length
@ -100,17 +243,83 @@ def plot_pose3_on_axes(axes, pose, axis_length=0.1):
axes.plot(line[:, 0], line[:, 1], line[:, 2], 'b-')
# plot the covariance
# TODO (dellaert): make this work
# if (nargin>2) && (~isempty(P))
# pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame
# gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame
# gtsam.covarianceEllipse3D(origin,gPp);
# end
if P is not None:
# covariance matrix in pose coordinate frame
pPp = P[3:6, 3:6]
# convert the covariance matrix to global coordinate frame
gPp = gRp @ pPp @ gRp.T
plot_covariance_ellipse_3d(axes, origin, gPp)
def plot_pose3(fignum, pose, 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, axis_length)
plot_pose3_on_axes(axes, pose, P=P,
axis_length=axis_length)
def plot_trajectory(fignum, values, scale=1, marginals=None):
"""
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
for i in range(keys.size()):
key = keys.at(i)
try:
pose = pose3Values.atPose3(key)
except:
print("Warning: no Pose3 at key: {0}".format(key))
if lastIndex is not None:
lastKey = keys.at(lastIndex)
try:
lastPose = pose3Values.atPose3(lastKey)
except:
print("Warning: no Pose3 at key: {0}".format(lastKey))
pass
if marginals:
covariance = marginals.marginalCovariance(lastKey)
else:
covariance = None
plot_pose3(fignum, lastPose, P=covariance,
axis_length=scale)
lastIndex = i
# Draw final pose
if lastIndex is not None:
lastKey = keys.at(lastIndex)
try:
lastPose = pose3Values.atPose3(lastKey)
if marginals:
covariance = marginals.marginalCovariance(lastKey)
else:
covariance = None
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

25
debian/rules vendored
View File

@ -1,25 +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
# 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=OFF -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

1
debian/source/format vendored
View File

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

2
doc/Code/OdometryExample.cpp
View File

@ -5,7 +5,7 @@ NonlinearFactorGraph graph;
Pose2 priorMean(0.0, 0.0, 0.0);
noiseModel::Diagonal::shared_ptr priorNoise =
noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
graph.addPrior(1, priorMean, priorNoise);
// Add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0);

2
doc/Code/Pose2SLAMExample.cpp
View File

@ -1,7 +1,7 @@
NonlinearFactorGraph graph;
noiseModel::Diagonal::shared_ptr priorNoise =
noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
graph.addPrior(1, Pose2(0, 0, 0), priorNoise);
// Add odometry factors
noiseModel::Diagonal::shared_ptr model =

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.

189
doc/math.lyx
View File

@ -1,7 +1,9 @@
#LyX 2.1 created this file. For more info see http://www.lyx.org/
\lyxformat 474
#LyX 2.3 created this file. For more info see http://www.lyx.org/
\lyxformat 544
\begin_document
\begin_header
\save_transient_properties true
\origin unavailable
\textclass article
\use_default_options false
\begin_modules
@ -14,16 +16,18 @@ theorems-ams-bytype
\language_package default
\inputencoding auto
\fontencoding global
\font_roman times
\font_sans default
\font_typewriter default
\font_math auto
\font_roman "times" "default"
\font_sans "default" "default"
\font_typewriter "default" "default"
\font_math "auto" "auto"
\font_default_family rmdefault
\use_non_tex_fonts false
\font_sc false
\font_osf false
\font_sf_scale 100
\font_tt_scale 100
\font_sf_scale 100 100
\font_tt_scale 100 100
\use_microtype false
\use_dash_ligatures true
\graphics default
\default_output_format default
\output_sync 0
@ -53,6 +57,7 @@ theorems-ams-bytype
\suppress_date false
\justification true
\use_refstyle 0
\use_minted 0
\index Index
\shortcut idx
\color #008000
@ -65,7 +70,10 @@ theorems-ams-bytype
\tocdepth 3
\paragraph_separation indent
\paragraph_indentation default
\quotes_language english
\is_math_indent 0
\math_numbering_side default
\quotes_style english
\dynamic_quotes 0
\papercolumns 1
\papersides 1
\paperpagestyle default
@ -98,6 +106,11 @@ width "100col%"
special "none"
height "1in"
height_special "totalheight"
thickness "0.4pt"
separation "3pt"
shadowsize "4pt"
framecolor "black"
backgroundcolor "none"
status collapsed
\begin_layout Plain Layout
@ -654,6 +667,7 @@ reference "eq:LocalBehavior"
\begin_inset CommandInset citation
LatexCommand cite
key "Spivak65book"
literal "true"
\end_inset
@ -934,6 +948,7 @@ See
\begin_inset CommandInset citation
LatexCommand cite
key "Spivak65book"
literal "true"
\end_inset
@ -1025,6 +1040,7 @@ See
\begin_inset CommandInset citation
LatexCommand cite
key "Spivak65book"
literal "true"
\end_inset
@ -2209,6 +2225,7 @@ instantaneous velocity
LatexCommand cite
after "page 51 for rotations, page 419 for SE(3)"
key "Murray94book"
literal "true"
\end_inset
@ -2965,7 +2982,7 @@ B^{T} & I_{3}\end{array}\right]
\begin_layout Subsection
\begin_inset CommandInset label
LatexCommand label
name "sub:Pushforward-of-Between"
name "subsec:Pushforward-of-Between"
\end_inset
@ -3419,6 +3436,7 @@ A retraction
\begin_inset CommandInset citation
LatexCommand cite
key "Absil07book"
literal "true"
\end_inset
@ -3873,7 +3891,7 @@ BetweenFactor
, derived in Section
\begin_inset CommandInset ref
LatexCommand ref
reference "sub:Pushforward-of-Between"
reference "subsec:Pushforward-of-Between"
\end_inset
@ -4430,7 +4448,7 @@ In the case of
\begin_inset Formula $\SOthree$
\end_inset
the vector space is
the vector space is
\begin_inset Formula $\Rthree$
\end_inset
@ -4502,7 +4520,7 @@ reference "Th:InverseAction"
\begin_layout Subsection
\begin_inset CommandInset label
LatexCommand label
name "sub:3DAngularVelocities"
name "subsec:3DAngularVelocities"
\end_inset
@ -4695,6 +4713,7 @@ Absil
LatexCommand cite
after "page 58"
key "Absil07book"
literal "true"
\end_inset
@ -5395,6 +5414,7 @@ While not a Lie group, we can define an exponential map, which is given
\begin_inset CommandInset citation
LatexCommand cite
key "Ma01ijcv"
literal "true"
\end_inset
@ -5402,6 +5422,7 @@ key "Ma01ijcv"
\begin_inset CommandInset href
LatexCommand href
name "http://stat.fsu.edu/~anuj/CVPR_Tutorial/Part2.pdf"
literal "false"
\end_inset
@ -5605,6 +5626,7 @@ The exponential map uses
\begin_inset CommandInset citation
LatexCommand cite
key "Absil07book"
literal "true"
\end_inset
@ -6293,7 +6315,7 @@ d^{c}=R_{w}^{c}\left(d^{w}+(t^{w}v^{w})v^{w}-t^{w}\right)
\end_layout
\begin_layout Section
Line3 (Ocaml)
Line3
\end_layout
\begin_layout Standard
@ -6345,6 +6367,14 @@ R'=R(I+\Omega)
\end_inset
\end_layout
\begin_layout Subsection
Projecting Line3
\end_layout
\begin_layout Standard
Projecting a line to 2D can be done easily, as both
\begin_inset Formula $v$
\end_inset
@ -6430,13 +6460,21 @@ or the
\end_layout
\begin_layout Subsection
Action of
\begin_inset Formula $\SEthree$
\end_inset
on the line
\end_layout
\begin_layout Standard
Transforming a 3D line
\begin_inset Formula $(R,(a,b))$
\end_inset
from a world coordinate frame to a camera frame
\begin_inset Formula $(R_{w}^{c},t^{w})$
\begin_inset Formula $T_{c}^{w}=(R_{c}^{w},t^{w})$
\end_inset
is done by
@ -6466,17 +6504,115 @@ b'=b-R_{2}^{T}t^{w}
\end_inset
Again, we need to redo the derivatives, as R is incremented from the right.
The first argument is incremented from the left, but the result is incremented
on the right:
where
\begin_inset Formula $R_{1}$
\end_inset
and
\begin_inset Formula $R_{2}$
\end_inset
are the columns of
\begin_inset Formula $R$
\end_inset
, as before.
\end_layout
\begin_layout Standard
To find the derivatives, the transformation of a line
\begin_inset Formula $l^{w}=(R,a,b)$
\end_inset
from world coordinates to a camera coordinate frame
\begin_inset Formula $T_{c}^{w}$
\end_inset
, specified in world coordinates, can be written as a function
\begin_inset Formula $f:\SEthree\times L\rightarrow L$
\end_inset
, as given above, i.e.,
\begin_inset Formula
\begin{eqnarray*}
R'(I+\Omega')=(AB)(I+\Omega') & = & (I+\Skew{S\omega})AB\\
I+\Omega' & = & (AB)^{T}(I+\Skew{S\omega})(AB)\\
\Omega' & = & R'^{T}\Skew{S\omega}R'\\
\Omega' & = & \Skew{R'^{T}S\omega}\\
\omega' & = & R'^{T}S\omega
\end{eqnarray*}
\[
f(T_{c}^{w},l^{w})=\left(\left(R_{c}^{w}\right)^{T}R,a-R_{1}^{T}t^{w},b-R_{2}^{T}t^{w}\right).
\]
\end_inset
Let us find the Jacobian
\begin_inset Formula $J_{1}$
\end_inset
of
\begin_inset Formula $f$
\end_inset
with respect to the first argument
\begin_inset Formula $T_{c}^{w}$
\end_inset
, which should obey
\begin_inset Formula
\begin{align*}
f(T_{c}^{w}e^{\xihat},l^{w}) & \approx f(T_{c}^{w},l^{w})+J_{1}\xi
\end{align*}
\end_inset
Note that
\begin_inset Formula
\[
T_{c}^{w}e^{\xihat}\approx\left[\begin{array}{cc}
R_{c}^{w}\left(I_{3}+\Skew{\omega}\right) & t^{w}+R_{c}^{w}v\\
0 & 1
\end{array}\right]
\]
\end_inset
Let's write this out separately for each of
\begin_inset Formula $R,a,b$
\end_inset
:
\begin_inset Formula
\begin{align*}
\left(R_{c}^{w}\left(I_{3}+\Skew{\omega}\right)\right)^{T}R & \approx\left(R_{c}^{w}\right)^{T}R(I+\left[J_{R\omega}\omega\right]_{\times})\\
a-R_{1}^{T}\left(t^{w}+R_{c}^{w}v\right) & \approx a-R_{1}^{T}t^{w}+J_{av}v\\
b-R_{2}^{T}\left(t^{w}+R_{c}^{w}v\right) & \approx b-R_{2}^{T}t^{w}+J_{bv}v
\end{align*}
\end_inset
Simplifying, we get:
\begin_inset Formula
\begin{align*}
-\Skew{\omega}R' & \approx R'\left[J_{R\omega}\omega\right]_{\times}\\
-R_{1}^{T}R_{c}^{w} & \approx J_{av}\\
-R_{2}^{T}R_{c}^{w} & \approx J_{bv}
\end{align*}
\end_inset
which gives the expressions for
\begin_inset Formula $J_{av}$
\end_inset
and
\begin_inset Formula $J_{bv}$
\end_inset
.
The top line can be further simplified:
\begin_inset Formula
\begin{align*}
-\Skew{\omega}R' & \approx R'\left[J_{R\omega}\omega\right]_{\times}\\
-R'^{T}\Skew{\omega}R' & \approx\left[J_{R\omega}\omega\right]_{\times}\\
-\Skew{R'^{T}\omega} & \approx\left[J_{R\omega}\omega\right]_{\times}\\
-R'^{T} & \approx J_{R\omega}
\end{align*}
\end_inset
@ -6687,6 +6823,7 @@ Spivak
\begin_inset CommandInset citation
LatexCommand cite
key "Spivak65book"
literal "true"
\end_inset
@ -6795,6 +6932,7 @@ The following is adapted from Appendix A in
\begin_inset CommandInset citation
LatexCommand cite
key "Murray94book"
literal "true"
\end_inset
@ -6924,6 +7062,7 @@ might be
LatexCommand cite
after "page 45"
key "Hall00book"
literal "true"
\end_inset

BIN
doc/math.pdf
View File

Binary file not shown.

BIN
doc/robust.pdf Normal file
View File

Binary file not shown.

3
examples/CMakeLists.txt
View File

@ -1,7 +1,4 @@
set (excluded_examples
DiscreteBayesNet_FG.cpp
UGM_chain.cpp
UGM_small.cpp
elaboratePoint2KalmanFilter.cpp
)

6
examples/Data/Klaus3.g2o Normal file
View File

@ -0,0 +1,6 @@
VERTEX_SE3:QUAT 0 -3.865747774038187 0.06639337702667497 -0.16064874691945374 0.024595211709139555 0.49179523413089893 -0.06279232989379242 0.8680954132776109
VERTEX_SE3:QUAT 1 -3.614793159814815 0.04774490041587656 -0.2837650367985949 0.00991721787943912 0.4854918961891193 -0.042343290945895576 0.8731588132957809
VERTEX_SE3:QUAT 2 -3.255096913553434 0.013296754286114112 -0.5339792269680574 -0.027851108010665374 0.585478168397957 -0.05088341463532465 0.8086102325762403
EDGE_SE3:QUAT 0 1 0.2509546142233723 -0.01864847661079841 -0.12311628987914114 -0.022048798853273946 -0.01796327847857683 0.010210006313668573 0.9995433591728293 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0
EDGE_SE3:QUAT 0 2 0.6106508604847534 -0.05309662274056086 -0.3733304800486037 -0.054972994022992064 0.10432547598981769 -0.02221474884651081 0.9927742290779572 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0
EDGE_SE3:QUAT 1 2 0.3596962462613811 -0.03444814612976245 -0.25021419016946256 -0.03174661848656213 0.11646825423134777 -0.02951742735854383 0.9922479626852876 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0

11
examples/Data/toyExample.g2o Executable file
View File

@ -0,0 +1,11 @@
VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1
VERTEX_SE3:QUAT 1 0 0 0 0 0 0 1
VERTEX_SE3:QUAT 2 0 0 0 0.00499994 0.00499994 0.00499994 0.999963
VERTEX_SE3:QUAT 3 0 0 0 -0.00499994 -0.00499994 -0.00499994 0.999963
VERTEX_SE3:QUAT 4 0 0 0 0.00499994 0.00499994 0.00499994 0.999963
EDGE_SE3:QUAT 1 2 1 2 0 0 0 0.707107 0.707107 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100
EDGE_SE3:QUAT 2 3 -3.26795e-07 1 0 0 0 0.707107 0.707107 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100
EDGE_SE3:QUAT 3 4 1 1 0 0 0 0.707107 0.707107 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100
EDGE_SE3:QUAT 3 1 6.9282e-07 2 0 0 0 1 1.73205e-07 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100
EDGE_SE3:QUAT 1 4 -1 1 0 0 0 -0.707107 0.707107 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100
EDGE_SE3:QUAT 0 1 0 0 0 0 0 0 1 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100

83
examples/DiscreteBayesNetExample.cpp Normal file
View File

@ -0,0 +1,83 @@
/* ----------------------------------------------------------------------------
* 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 DiscreteBayesNetExample.cpp
* @brief Discrete Bayes Net example with famous Asia Bayes Network
* @author Frank Dellaert
* @date JULY 10, 2020
*/
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteMarginals.h>
#include <gtsam/inference/BayesNet-inst.h>
#include <iomanip>
using namespace std;
using namespace gtsam;
int main(int argc, char **argv) {
DiscreteBayesNet asia;
DiscreteKey Asia(0, 2), Smoking(4, 2), Tuberculosis(3, 2), LungCancer(6, 2),
Bronchitis(7, 2), Either(5, 2), XRay(2, 2), Dyspnea(1, 2);
asia.add(Asia % "99/1");
asia.add(Smoking % "50/50");
asia.add(Tuberculosis | Asia = "99/1 95/5");
asia.add(LungCancer | Smoking = "99/1 90/10");
asia.add(Bronchitis | Smoking = "70/30 40/60");
asia.add((Either | Tuberculosis, LungCancer) = "F T T T");
asia.add(XRay | Either = "95/5 2/98");
asia.add((Dyspnea | Either, Bronchitis) = "9/1 2/8 3/7 1/9");
// print
vector<string> pretty = {"Asia", "Dyspnea", "XRay", "Tuberculosis",
"Smoking", "Either", "LungCancer", "Bronchitis"};
auto formatter = [pretty](Key key) { return pretty[key]; };
asia.print("Asia", formatter);
// Convert to factor graph
DiscreteFactorGraph fg(asia);
// Create solver and eliminate
Ordering ordering;
ordering += Key(0), Key(1), Key(2), Key(3), Key(4), Key(5), Key(6), Key(7);
DiscreteBayesNet::shared_ptr chordal = fg.eliminateSequential(ordering);
// solve
DiscreteFactor::sharedValues mpe = chordal->optimize();
GTSAM_PRINT(*mpe);
// We can also build a Bayes tree (directed junction tree).
// The elimination order above will do fine:
auto bayesTree = fg.eliminateMultifrontal(ordering);
bayesTree->print("bayesTree", formatter);
// add evidence, we were in Asia and we have dyspnea
fg.add(Asia, "0 1");
fg.add(Dyspnea, "0 1");
// solve again, now with evidence
DiscreteBayesNet::shared_ptr chordal2 = fg.eliminateSequential(ordering);
DiscreteFactor::sharedValues mpe2 = chordal2->optimize();
GTSAM_PRINT(*mpe2);
// We can also sample from it
cout << "\n10 samples:" << endl;
for (size_t i = 0; i < 10; i++) {
DiscreteFactor::sharedValues sample = chordal2->sample();
GTSAM_PRINT(*sample);
}
return 0;
}

117
examples/DiscreteBayesNet_FG.cpp
View File

@ -15,105 +15,106 @@
* @author Abhijit
* @date Jun 4, 2012
*
* We use the famous Rain/Cloudy/Sprinkler Example of [Russell & Norvig, 2009, p529]
* You may be familiar with other graphical model packages like BNT (available
* at http://bnt.googlecode.com/svn/trunk/docs/usage.html) where this is used as an
* example. The following demo is same as that in the above link, except that
* everything is using GTSAM.
* We use the famous Rain/Cloudy/Sprinkler Example of [Russell & Norvig, 2009,
* p529] You may be familiar with other graphical model packages like BNT
* (available at http://bnt.googlecode.com/svn/trunk/docs/usage.html) where this
* is used as an example. The following demo is same as that in the above link,
* except that everything is using GTSAM.
*/
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteSequentialSolver.h>
#include <gtsam/discrete/DiscreteMarginals.h>
#include <iomanip>
using namespace std;
using namespace gtsam;
int main(int argc, char **argv) {
// Define keys and a print function
Key C(1), S(2), R(3), W(4);
auto print = [=](DiscreteFactor::sharedValues values) {
cout << boolalpha << "Cloudy = " << static_cast<bool>((*values)[C])
<< " Sprinkler = " << static_cast<bool>((*values)[S])
<< " Rain = " << boolalpha << static_cast<bool>((*values)[R])
<< " WetGrass = " << static_cast<bool>((*values)[W]) << endl;
};
// We assume binary state variables
// we have 0 == "False" and 1 == "True"
const size_t nrStates = 2;
// define variables
DiscreteKey Cloudy(1, nrStates), Sprinkler(2, nrStates), Rain(3, nrStates),
WetGrass(4, nrStates);
DiscreteKey Cloudy(C, nrStates), Sprinkler(S, nrStates), Rain(R, nrStates),
WetGrass(W, nrStates);
// create Factor Graph of the bayes net
DiscreteFactorGraph graph;
// add factors
graph.add(Cloudy, "0.5 0.5"); //P(Cloudy)
graph.add(Cloudy & Sprinkler, "0.5 0.5 0.9 0.1"); //P(Sprinkler | Cloudy)
graph.add(Cloudy & Rain, "0.8 0.2 0.2 0.8"); //P(Rain | Cloudy)
graph.add(Cloudy, "0.5 0.5"); // P(Cloudy)
graph.add(Cloudy & Sprinkler, "0.5 0.5 0.9 0.1"); // P(Sprinkler | Cloudy)
graph.add(Cloudy & Rain, "0.8 0.2 0.2 0.8"); // P(Rain | Cloudy)
graph.add(Sprinkler & Rain & WetGrass,
"1 0 0.1 0.9 0.1 0.9 0.001 0.99"); //P(WetGrass | Sprinkler, Rain)
"1 0 0.1 0.9 0.1 0.9 0.001 0.99"); // P(WetGrass | Sprinkler, Rain)
// Alternatively we can also create a DiscreteBayesNet, add DiscreteConditional
// factors and create a FactorGraph from it. (See testDiscreteBayesNet.cpp)
// Alternatively we can also create a DiscreteBayesNet, add
// DiscreteConditional factors and create a FactorGraph from it. (See
// testDiscreteBayesNet.cpp)
// Since this is a relatively small distribution, we can as well print
// the whole distribution..
cout << "Distribution of Example: " << endl;
cout << setw(11) << "Cloudy(C)" << setw(14) << "Sprinkler(S)" << setw(10)
<< "Rain(R)" << setw(14) << "WetGrass(W)" << setw(15) << "P(C,S,R,W)"
<< endl;
<< "Rain(R)" << setw(14) << "WetGrass(W)" << setw(15) << "P(C,S,R,W)"
<< endl;
for (size_t a = 0; a < nrStates; a++)
for (size_t m = 0; m < nrStates; m++)
for (size_t h = 0; h < nrStates; h++)
for (size_t c = 0; c < nrStates; c++) {
DiscreteFactor::Values values;
values[Cloudy.first] = c;
values[Sprinkler.first] = h;
values[Rain.first] = m;
values[WetGrass.first] = a;
values[C] = c;
values[S] = h;
values[R] = m;
values[W] = a;
double prodPot = graph(values);
cout << boolalpha << setw(8) << (bool) c << setw(14)
<< (bool) h << setw(12) << (bool) m << setw(13)
<< (bool) a << setw(16) << prodPot << endl;
cout << setw(8) << static_cast<bool>(c) << setw(14)
<< static_cast<bool>(h) << setw(12) << static_cast<bool>(m)
<< setw(13) << static_cast<bool>(a) << setw(16) << prodPot
<< endl;
}
// "Most Probable Explanation", i.e., configuration with largest value
DiscreteSequentialSolver solver(graph);
DiscreteFactor::sharedValues optimalDecoding = solver.optimize();
cout <<"\nMost Probable Explanation (MPE):" << endl;
cout << boolalpha << "Cloudy = " << (bool)(*optimalDecoding)[Cloudy.first]
<< " Sprinkler = " << (bool)(*optimalDecoding)[Sprinkler.first]
<< " Rain = " << boolalpha << (bool)(*optimalDecoding)[Rain.first]
<< " WetGrass = " << (bool)(*optimalDecoding)[WetGrass.first]<< endl;
DiscreteFactor::sharedValues mpe = graph.eliminateSequential()->optimize();
cout << "\nMost Probable Explanation (MPE):" << endl;
print(mpe);
// "Inference" We show an inference query like: probability that the Sprinkler
// was on; given that the grass is wet i.e. P( S | C=0) = ?
// "Inference" We show an inference query like: probability that the Sprinkler was on;
// given that the grass is wet i.e. P( S | W=1) =?
cout << "\nInference Query: Probability of Sprinkler being on given Grass is Wet" << endl;
// add evidence that it is not Cloudy
graph.add(Cloudy, "1 0");
// Method 1: we can compute the joint marginal P(S,W) and from that we can compute
// P(S | W=1) = P(S,W=1)/P(W=1) We do this in following three steps..
// solve again, now with evidence
DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential();
DiscreteFactor::sharedValues mpe_with_evidence = chordal->optimize();
//Step1: Compute P(S,W)
DiscreteFactorGraph jointFG;
jointFG = *solver.jointFactorGraph(DiscreteKeys(Sprinkler & WetGrass).indices());
DecisionTreeFactor probSW = jointFG.product();
cout << "\nMPE given C=0:" << endl;
print(mpe_with_evidence);
//Step2: Compute P(W)
DiscreteFactor::shared_ptr probW = solver.marginalFactor(WetGrass.first);
//Step3: Computer P(S | W=1) = P(S,W=1)/P(W=1)
DiscreteFactor::Values values;
values[WetGrass.first] = 1;
//print P(S=0|W=1)
values[Sprinkler.first] = 0;
cout << "P(S=0|W=1) = " << probSW(values)/(*probW)(values) << endl;
//print P(S=1|W=1)
values[Sprinkler.first] = 1;
cout << "P(S=1|W=1) = " << probSW(values)/(*probW)(values) << endl;
// TODO: Method 2 : One way is to modify the factor graph to
// incorporate the evidence node and compute the marginal
// TODO: graph.addEvidence(Cloudy,0);
// we can also calculate arbitrary marginals:
DiscreteMarginals marginals(graph);
cout << "\nP(S=1|C=0):" << marginals.marginalProbabilities(Sprinkler)[1]
<< endl;
cout << "\nP(R=0|C=0):" << marginals.marginalProbabilities(Rain)[0] << endl;
cout << "\nP(W=1|C=0):" << marginals.marginalProbabilities(WetGrass)[1]
<< endl;
// We can also sample from it
cout << "\n10 samples:" << endl;
for (size_t i = 0; i < 10; i++) {
DiscreteFactor::sharedValues sample = chordal->sample();
print(sample);
}
return 0;
}

130
examples/FisheyeExample.cpp Normal file
View File

@ -0,0 +1,130 @@
/* ----------------------------------------------------------------------------
* 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 FisheyeExample.cpp
* @brief A visualSLAM example for the structure-from-motion problem on a
* simulated dataset. This version uses a fisheye camera model and a GaussNewton
* solver to solve the graph in one batch
* @author ghaggin
* @Date Apr 9,2020
*/
/**
* A structure-from-motion example with landmarks
* - The landmarks form a 10 meter cube
* - The robot rotates around the landmarks, always facing towards the cube
*/
// For loading the data
#include "SFMdata.h"
// Camera observations of landmarks will be stored as Point2 (x, y).
#include <gtsam/geometry/Point2.h>
// Each variable in the system (poses and landmarks) must be identified with a
// unique key. We can either use simple integer keys (1, 2, 3, ...) or symbols
// (X1, X2, L1). Here we will use Symbols
#include <gtsam/inference/Symbol.h>
// Use GaussNewtonOptimizer to solve graph
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common
// factors have been provided with the library for solving robotics/SLAM/Bundle
// Adjustment problems. Here we will use Projection factors to model the
// camera's landmark observations. Also, we will initialize the robot at some
// location using a Prior factor.
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <fstream>
#include <vector>
using namespace std;
using namespace gtsam;
using symbol_shorthand::L; // for landmarks
using symbol_shorthand::X; // for poses
/* ************************************************************************* */
int main(int argc, char *argv[]) {
// Define the camera calibration parameters
auto K = boost::make_shared<Cal3Fisheye>(
278.66, 278.48, 0.0, 319.75, 241.96, -0.013721808247486035,
0.020727425669427896, -0.012786476702685545, 0.0025242267320687625);
// Define the camera observation noise model, 1 pixel stddev
auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
// Create the set of ground-truth landmarks
const vector<Point3> points = createPoints();
// Create the set of ground-truth poses
const vector<Pose3> poses = createPoses();
// Create a Factor Graph and Values to hold the new data
NonlinearFactorGraph graph;
Values initialEstimate;
// Add a prior on pose x0, 0.1 rad on roll,pitch,yaw, and 30cm std on x,y,z
auto posePrior = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
graph.emplace_shared<PriorFactor<Pose3>>(X(0), poses[0], posePrior);
// Add a prior on landmark l0
auto pointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
graph.emplace_shared<PriorFactor<Point3>>(L(0), points[0], pointPrior);
// Add initial guesses to all observed landmarks
// Intentionally initialize the variables off from the ground truth
static const Point3 kDeltaPoint(-0.25, 0.20, 0.15);
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert<Point3>(L(j), points[j] + kDeltaPoint);
// Loop over the poses, adding the observations to the graph
for (size_t i = 0; i < poses.size(); ++i) {
// Add factors for each landmark observation
for (size_t j = 0; j < points.size(); ++j) {
PinholeCamera<Cal3Fisheye> camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3Fisheye>>(
measurement, measurementNoise, X(i), L(j), K);
}
// Add an initial guess for the current pose
// Intentionally initialize the variables off from the ground truth
static const Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25),
Point3(0.05, -0.10, 0.20));
initialEstimate.insert(X(i), poses[i] * kDeltaPose);
}
GaussNewtonParams params;
params.setVerbosity("TERMINATION");
params.maxIterations = 10000;
std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonOptimizer optimizer(graph, initialEstimate, params);
Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl;
std::cout << "initial error=" << graph.error(initialEstimate) << std::endl;
std::cout << "final error=" << graph.error(result) << std::endl;
std::ofstream os("examples/vio_batch.dot");
graph.saveGraph(os, result);
return 0;
}
/* ************************************************************************* */

94
examples/HMMExample.cpp Normal file
View File

@ -0,0 +1,94 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file DiscreteBayesNetExample.cpp
* @brief Hidden Markov Model example, discrete.
* @author Frank Dellaert
* @date July 12, 2020
*/
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteMarginals.h>
#include <gtsam/inference/BayesNet-inst.h>
#include <iomanip>
#include <sstream>
using namespace std;
using namespace gtsam;
int main(int argc, char **argv) {
const int nrNodes = 4;
const size_t nrStates = 3;
// Define variables as well as ordering
Ordering ordering;
vector<DiscreteKey> keys;
for (int k = 0; k < nrNodes; k++) {
DiscreteKey key_i(k, nrStates);
keys.push_back(key_i);
ordering.emplace_back(k);
}
// Create HMM as a DiscreteBayesNet
DiscreteBayesNet hmm;
// Define backbone
const string transition = "8/1/1 1/8/1 1/1/8";
for (int k = 1; k < nrNodes; k++) {
hmm.add(keys[k] | keys[k - 1] = transition);
}
// Add some measurements, not needed for all time steps!
hmm.add(keys[0] % "7/2/1");
hmm.add(keys[1] % "1/9/0");
hmm.add(keys.back() % "5/4/1");
// print
hmm.print("HMM");
// Convert to factor graph
DiscreteFactorGraph factorGraph(hmm);
// Create solver and eliminate
// This will create a DAG ordered with arrow of time reversed
DiscreteBayesNet::shared_ptr chordal =
factorGraph.eliminateSequential(ordering);
chordal->print("Eliminated");
// solve
DiscreteFactor::sharedValues mpe = chordal->optimize();
GTSAM_PRINT(*mpe);
// We can also sample from it
cout << "\n10 samples:" << endl;
for (size_t k = 0; k < 10; k++) {
DiscreteFactor::sharedValues sample = chordal->sample();
GTSAM_PRINT(*sample);
}
// Or compute the marginals. This re-eliminates the FG into a Bayes tree
cout << "\nComputing Node Marginals .." << endl;
DiscreteMarginals marginals(factorGraph);
for (int k = 0; k < nrNodes; k++) {
Vector margProbs = marginals.marginalProbabilities(keys[k]);
stringstream ss;
ss << "marginal " << k;
print(margProbs, ss.str());
}
// TODO(frank): put in the glue to have DiscreteMarginals produce *arbitrary*
// joints efficiently, by the Bayes tree shortcut magic. All the code is there
// but it's not yet connected.
return 0;
}

359
examples/IMUKittiExampleGPS.cpp Normal file
View File

@ -0,0 +1,359 @@
/* ----------------------------------------------------------------------------
* 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 IMUKittiExampleGPS
* @brief Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BENCHMARK SUITE
* @author Ported by Thomas Jespersen (thomasj@tkjelectronics.dk), TKJ Electronics
*/
// GTSAM related includes.
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/ISAM2Params.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <cstring>
#include <fstream>
#include <iostream>
using namespace std;
using namespace gtsam;
using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
struct KittiCalibration {
double body_ptx;
double body_pty;
double body_ptz;
double body_prx;
double body_pry;
double body_prz;
double accelerometer_sigma;
double gyroscope_sigma;
double integration_sigma;
double accelerometer_bias_sigma;
double gyroscope_bias_sigma;
double average_delta_t;
};
struct ImuMeasurement {
double time;
double dt;
Vector3 accelerometer;
Vector3 gyroscope; // omega
};
struct GpsMeasurement {
double time;
Vector3 position; // x,y,z
};
const string output_filename = "IMUKittiExampleGPSResults.csv";
void loadKittiData(KittiCalibration& kitti_calibration,
vector<ImuMeasurement>& imu_measurements,
vector<GpsMeasurement>& gps_measurements) {
string line;
// Read IMU metadata and compute relative sensor pose transforms
// BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma GyroscopeSigma IntegrationSigma
// AccelerometerBiasSigma GyroscopeBiasSigma AverageDeltaT
string imu_metadata_file = findExampleDataFile("KittiEquivBiasedImu_metadata.txt");
ifstream imu_metadata(imu_metadata_file.c_str());
printf("-- Reading sensor metadata\n");
getline(imu_metadata, line, '\n'); // ignore the first line
// Load Kitti calibration
getline(imu_metadata, line, '\n');
sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
&kitti_calibration.body_ptx,
&kitti_calibration.body_pty,
&kitti_calibration.body_ptz,
&kitti_calibration.body_prx,
&kitti_calibration.body_pry,
&kitti_calibration.body_prz,
&kitti_calibration.accelerometer_sigma,
&kitti_calibration.gyroscope_sigma,
&kitti_calibration.integration_sigma,
&kitti_calibration.accelerometer_bias_sigma,
&kitti_calibration.gyroscope_bias_sigma,
&kitti_calibration.average_delta_t);
printf("IMU metadata: %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n",
kitti_calibration.body_ptx,
kitti_calibration.body_pty,
kitti_calibration.body_ptz,
kitti_calibration.body_prx,
kitti_calibration.body_pry,
kitti_calibration.body_prz,
kitti_calibration.accelerometer_sigma,
kitti_calibration.gyroscope_sigma,
kitti_calibration.integration_sigma,
kitti_calibration.accelerometer_bias_sigma,
kitti_calibration.gyroscope_bias_sigma,
kitti_calibration.average_delta_t);
// Read IMU data
// Time dt accelX accelY accelZ omegaX omegaY omegaZ
string imu_data_file = findExampleDataFile("KittiEquivBiasedImu.txt");
printf("-- Reading IMU measurements from file\n");
{
ifstream imu_data(imu_data_file.c_str());
getline(imu_data, line, '\n'); // ignore the first line
double time = 0, dt = 0, acc_x = 0, acc_y = 0, acc_z = 0, gyro_x = 0, gyro_y = 0, gyro_z = 0;
while (!imu_data.eof()) {
getline(imu_data, line, '\n');
sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf",
&time, &dt,
&acc_x, &acc_y, &acc_z,
&gyro_x, &gyro_y, &gyro_z);
ImuMeasurement measurement;
measurement.time = time;
measurement.dt = dt;
measurement.accelerometer = Vector3(acc_x, acc_y, acc_z);
measurement.gyroscope = Vector3(gyro_x, gyro_y, gyro_z);
imu_measurements.push_back(measurement);
}
}
// Read GPS data
// Time,X,Y,Z
string gps_data_file = findExampleDataFile("KittiGps_converted.txt");
printf("-- Reading GPS measurements from file\n");
{
ifstream gps_data(gps_data_file.c_str());
getline(gps_data, line, '\n'); // ignore the first line
double time = 0, gps_x = 0, gps_y = 0, gps_z = 0;
while (!gps_data.eof()) {
getline(gps_data, line, '\n');
sscanf(line.c_str(), "%lf,%lf,%lf,%lf", &time, &gps_x, &gps_y, &gps_z);
GpsMeasurement measurement;
measurement.time = time;
measurement.position = Vector3(gps_x, gps_y, gps_z);
gps_measurements.push_back(measurement);
}
}
}
int main(int argc, char* argv[]) {
KittiCalibration kitti_calibration;
vector<ImuMeasurement> imu_measurements;
vector<GpsMeasurement> gps_measurements;
loadKittiData(kitti_calibration, imu_measurements, gps_measurements);
Vector6 BodyP = (Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty, kitti_calibration.body_ptz,
kitti_calibration.body_prx, kitti_calibration.body_pry, kitti_calibration.body_prz)
.finished();
auto body_T_imu = Pose3::Expmap(BodyP);
if (!body_T_imu.equals(Pose3(), 1e-5)) {
printf("Currently only support IMUinBody is identity, i.e. IMU and body frame are the same");
exit(-1);
}
// Configure different variables
// double t_offset = gps_measurements[0].time;
size_t first_gps_pose = 1;
size_t gps_skip = 10; // Skip this many GPS measurements each time
double g = 9.8;
auto w_coriolis = Vector3::Zero(); // zero vector
// Configure noise models
auto noise_model_gps = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0),
Vector3::Constant(1.0/0.07))
.finished());
// Set initial conditions for the estimated trajectory
// initial pose is the reference frame (navigation frame)
auto current_pose_global = Pose3(Rot3(), gps_measurements[first_gps_pose].position);
// the vehicle is stationary at the beginning at position 0,0,0
Vector3 current_velocity_global = Vector3::Zero();
auto current_bias = imuBias::ConstantBias(); // init with zero bias
auto sigma_init_x = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0),
Vector3::Constant(1.0))
.finished());
auto sigma_init_v = noiseModel::Diagonal::Sigmas(Vector3::Constant(1000.0));
auto sigma_init_b = noiseModel::Diagonal::Sigmas((Vector6() << Vector3::Constant(0.100),
Vector3::Constant(5.00e-05))
.finished());
// Set IMU preintegration parameters
Matrix33 measured_acc_cov = I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2);
Matrix33 measured_omega_cov = I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2);
// error committed in integrating position from velocities
Matrix33 integration_error_cov = I_3x3 * pow(kitti_calibration.integration_sigma, 2);
auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(g);
imu_params->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous
imu_params->integrationCovariance = integration_error_cov; // integration uncertainty continuous
imu_params->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous
imu_params->omegaCoriolis = w_coriolis;
std::shared_ptr<PreintegratedImuMeasurements> current_summarized_measurement = nullptr;
// Set ISAM2 parameters and create ISAM2 solver object
ISAM2Params isam_params;
isam_params.factorization = ISAM2Params::CHOLESKY;
isam_params.relinearizeSkip = 10;
ISAM2 isam(isam_params);
// Create the factor graph and values object that will store new factors and values to add to the incremental graph
NonlinearFactorGraph new_factors;
Values new_values; // values storing the initial estimates of new nodes in the factor graph
/// Main loop:
/// (1) we read the measurements
/// (2) we create the corresponding factors in the graph
/// (3) we solve the graph to obtain and optimal estimate of robot trajectory
printf("-- Starting main loop: inference is performed at each time step, but we plot trajectory every 10 steps\n");
size_t j = 0;
for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
// At each non=IMU measurement we initialize a new node in the graph
auto current_pose_key = X(i);
auto current_vel_key = V(i);
auto current_bias_key = B(i);
double t = gps_measurements[i].time;
if (i == first_gps_pose) {
// Create initial estimate and prior on initial pose, velocity, and biases
new_values.insert(current_pose_key, current_pose_global);
new_values.insert(current_vel_key, current_velocity_global);
new_values.insert(current_bias_key, current_bias);
new_factors.emplace_shared<PriorFactor<Pose3>>(current_pose_key, current_pose_global, sigma_init_x);
new_factors.emplace_shared<PriorFactor<Vector3>>(current_vel_key, current_velocity_global, sigma_init_v);
new_factors.emplace_shared<PriorFactor<imuBias::ConstantBias>>(current_bias_key, current_bias, sigma_init_b);
} else {
double t_previous = gps_measurements[i-1].time;
// Summarize IMU data between the previous GPS measurement and now
current_summarized_measurement = std::make_shared<PreintegratedImuMeasurements>(imu_params, current_bias);
static size_t included_imu_measurement_count = 0;
while (j < imu_measurements.size() && imu_measurements[j].time <= t) {
if (imu_measurements[j].time >= t_previous) {
current_summarized_measurement->integrateMeasurement(imu_measurements[j].accelerometer,
imu_measurements[j].gyroscope,
imu_measurements[j].dt);
included_imu_measurement_count++;
}
j++;
}
// Create IMU factor
auto previous_pose_key = X(i-1);
auto previous_vel_key = V(i-1);
auto previous_bias_key = B(i-1);
new_factors.emplace_shared<ImuFactor>(previous_pose_key, previous_vel_key,
current_pose_key, current_vel_key,
previous_bias_key, *current_summarized_measurement);
// Bias evolution as given in the IMU metadata
auto sigma_between_b = noiseModel::Diagonal::Sigmas((Vector6() <<
Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.accelerometer_bias_sigma),
Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.gyroscope_bias_sigma))
.finished());
new_factors.emplace_shared<BetweenFactor<imuBias::ConstantBias>>(previous_bias_key,
current_bias_key,
imuBias::ConstantBias(),
sigma_between_b);
// Create GPS factor
auto gps_pose = Pose3(current_pose_global.rotation(), gps_measurements[i].position);
if ((i % gps_skip) == 0) {
new_factors.emplace_shared<PriorFactor<Pose3>>(current_pose_key, gps_pose, noise_model_gps);
new_values.insert(current_pose_key, gps_pose);
printf("################ POSE INCLUDED AT TIME %lf ################\n", t);
gps_pose.translation().print();
printf("\n\n");
} else {
new_values.insert(current_pose_key, current_pose_global);
}
// Add initial values for velocity and bias based on the previous estimates
new_values.insert(current_vel_key, current_velocity_global);
new_values.insert(current_bias_key, current_bias);
// Update solver
// =======================================================================
// We accumulate 2*GPSskip GPS measurements before updating the solver at
// first so that the heading becomes observable.
if (i > (first_gps_pose + 2*gps_skip)) {
printf("################ NEW FACTORS AT TIME %lf ################\n", t);
new_factors.print();
isam.update(new_factors, new_values);
// Reset the newFactors and newValues list
new_factors.resize(0);
new_values.clear();
// Extract the result/current estimates
Values result = isam.calculateEstimate();
current_pose_global = result.at<Pose3>(current_pose_key);
current_velocity_global = result.at<Vector3>(current_vel_key);
current_bias = result.at<imuBias::ConstantBias>(current_bias_key);
printf("\n################ POSE AT TIME %lf ################\n", t);
current_pose_global.print();
printf("\n\n");
}
}
}
// Save results to file
printf("\nWriting results to file...\n");
FILE* fp_out = fopen(output_filename.c_str(), "w+");
fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n");
Values result = isam.calculateEstimate();
for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
auto pose_key = X(i);
auto vel_key = V(i);
auto bias_key = B(i);
auto pose = result.at<Pose3>(pose_key);
auto velocity = result.at<Vector3>(vel_key);
auto bias = result.at<imuBias::ConstantBias>(bias_key);
auto pose_quat = pose.rotation().toQuaternion();
auto gps = gps_measurements[i].position;
cout << "State at #" << i << endl;
cout << "Pose:" << endl << pose << endl;
cout << "Velocity:" << endl << velocity << endl;
cout << "Bias:" << endl << bias << endl;
fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
gps_measurements[i].time,
pose.x(), pose.y(), pose.z(),
pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(),
gps(0), gps(1), gps(2));
}
fclose(fp_out);
}

4
examples/ISAM2Example_SmartFactor.cpp
View File

@ -57,7 +57,7 @@ int main(int argc, char* argv[]) {
vector<Pose3> poses = {pose1, pose2, pose3, pose4, pose5};
// Add first pose
graph.emplace_shared<PriorFactor<Pose3>>(X(0), poses[0], noise);
graph.addPrior(X(0), poses[0], noise);
initialEstimate.insert(X(0), poses[0].compose(delta));
// Create smart factor with measurement from first pose only
@ -71,7 +71,7 @@ int main(int argc, char* argv[]) {
cout << "i = " << i << endl;
// Add prior on new pose
graph.emplace_shared<PriorFactor<Pose3>>(X(i), poses[i], noise);
graph.addPrior(X(i), poses[i], noise);
initialEstimate.insert(X(i), poses[i].compose(delta));
// "Simulate" measurement from this pose

10
examples/ISAM2_SmartFactorStereo_IMU.cpp
View File

@ -129,18 +129,16 @@ int main(int argc, char* argv[]) {
// Pose prior - at identity
auto priorPoseNoise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished());
graph.emplace_shared<PriorFactor<Pose3>>(X(1), Pose3::identity(),
priorPoseNoise);
graph.addPrior(X(1), Pose3::identity(), priorPoseNoise);
initialEstimate.insert(X(0), Pose3::identity());
// Bias prior
graph.add(PriorFactor<imuBias::ConstantBias>(B(1), imu.priorImuBias,
imu.biasNoiseModel));
graph.addPrior(B(1), imu.priorImuBias,
imu.biasNoiseModel);
initialEstimate.insert(B(0), imu.priorImuBias);
// Velocity prior - assume stationary
graph.add(
PriorFactor<Vector3>(V(1), Vector3(0, 0, 0), imu.velocityNoiseModel));
graph.addPrior(V(1), Vector3(0, 0, 0), imu.velocityNoiseModel);
initialEstimate.insert(V(0), Vector3(0, 0, 0));
int lastFrame = 1;

10
examples/ImuFactorExample2.cpp
View File

@ -7,7 +7,6 @@
#include <gtsam/navigation/Scenario.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <vector>
@ -61,21 +60,18 @@ int main(int argc, char* argv[]) {
// 0.1 rad std on roll, pitch, yaw, 30cm std on x,y,z.
auto noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
newgraph.push_back(PriorFactor<Pose3>(X(0), pose_0, noise));
newgraph.addPrior(X(0), pose_0, noise);
// Add imu priors
Key biasKey = Symbol('b', 0);
auto biasnoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(0.1));
PriorFactor<imuBias::ConstantBias> biasprior(biasKey, imuBias::ConstantBias(),
biasnoise);
newgraph.push_back(biasprior);
newgraph.addPrior(biasKey, imuBias::ConstantBias(), biasnoise);
initialEstimate.insert(biasKey, imuBias::ConstantBias());
auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
Vector n_velocity(3);
n_velocity << 0, angular_velocity * radius, 0;
PriorFactor<Vector> velprior(V(0), n_velocity, velnoise);
newgraph.push_back(velprior);
newgraph.addPrior(V(0), n_velocity, velnoise);
initialEstimate.insert(V(0), n_velocity);

189
examples/ImuFactorsExample.cpp
View File

@ -11,12 +11,14 @@
/**
* @file imuFactorsExample
* @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor navigation code.
* @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor
* navigation code.
* @author Garrett (ghemann@gmail.com), Luca Carlone
*/
/**
* Example of use of the imuFactors (imuFactor and combinedImuFactor) in conjunction with GPS
* Example of use of the imuFactors (imuFactor and combinedImuFactor) in
* conjunction with GPS
* - imuFactor is used by default. You can test combinedImuFactor by
* appending a `-c` flag at the end (see below for example command).
* - we read IMU and GPS data from a CSV file, with the following format:
@ -26,8 +28,8 @@
* linAccN, linAccE, linAccD, angVelN, angVelE, angVelD
* A row starting with "1" is a gps correction formatted with
* N, E, D, qX, qY, qZ, qW
* Note that for GPS correction, we're only using the position not the rotation. The
* rotation is provided in the file for ground truth comparison.
* Note that for GPS correction, we're only using the position not the
* rotation. The rotation is provided in the file for ground truth comparison.
*
* Usage: ./ImuFactorsExample [data_csv_path] [-c]
* optional arguments:
@ -41,13 +43,13 @@
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/inference/Symbol.h>
#include <cstring>
#include <fstream>
#include <iostream>
@ -58,19 +60,14 @@
using namespace gtsam;
using namespace std;
using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
const string output_filename = "imuFactorExampleResults.csv";
const string use_combined_imu_flag = "-c";
static const char output_filename[] = "imuFactorExampleResults.csv";
static const char use_combined_imu_flag[3] = "-c";
// This will either be PreintegratedImuMeasurements (for ImuFactor) or
// PreintegratedCombinedMeasurements (for CombinedImuFactor).
PreintegrationType *imu_preintegrated_;
int main(int argc, char* argv[])
{
int main(int argc, char* argv[]) {
string data_filename;
bool use_combined_imu = false;
@ -87,8 +84,8 @@ int main(int argc, char* argv[])
if (argc < 2) {
printf("using default CSV file\n");
data_filename = findExampleDataFile("imuAndGPSdata.csv");
} else if (argc < 3){
if (strcmp(argv[1], use_combined_imu_flag.c_str()) == 0) { // strcmp returns 0 for a match
} else if (argc < 3) {
if (strcmp(argv[1], use_combined_imu_flag) == 0) {
printf("using CombinedImuFactor\n");
use_combined_imu = true;
printf("using default CSV file\n");
@ -98,15 +95,17 @@ int main(int argc, char* argv[])
}
} else {
data_filename = argv[1];
if (strcmp(argv[2], use_combined_imu_flag.c_str()) == 0) { // strcmp returns 0 for a match
if (strcmp(argv[2], use_combined_imu_flag) == 0) {
printf("using CombinedImuFactor\n");
use_combined_imu = true;
}
}
// Set up output file for plotting errors
FILE* fp_out = fopen(output_filename.c_str(), "w+");
fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx,gt_qy,gt_qz,gt_qw\n");
FILE* fp_out = fopen(output_filename, "w+");
fprintf(fp_out,
"#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx,"
"gt_qy,gt_qz,gt_qw\n");
// Begin parsing the CSV file. Input the first line for initialization.
// From there, we'll iterate through the file and we'll preintegrate the IMU
@ -115,9 +114,9 @@ int main(int argc, char* argv[])
string value;
// Format is (N,E,D,qX,qY,qZ,qW,velN,velE,velD)
Eigen::Matrix<double,10,1> initial_state = Eigen::Matrix<double,10,1>::Zero();
getline(file, value, ','); // i
for (int i=0; i<9; i++) {
Vector10 initial_state;
getline(file, value, ','); // i
for (int i = 0; i < 9; i++) {
getline(file, value, ',');
initial_state(i) = atof(value.c_str());
}
@ -125,13 +124,14 @@ int main(int argc, char* argv[])
initial_state(9) = atof(value.c_str());
cout << "initial state:\n" << initial_state.transpose() << "\n\n";
// Assemble initial quaternion through gtsam constructor ::quaternion(w,x,y,z);
// Assemble initial quaternion through GTSAM constructor
// ::quaternion(w,x,y,z);
Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3),
initial_state(4), initial_state(5));
Point3 prior_point(initial_state.head<3>());
Pose3 prior_pose(prior_rotation, prior_point);
Vector3 prior_velocity(initial_state.tail<3>());
imuBias::ConstantBias prior_imu_bias; // assume zero initial bias
imuBias::ConstantBias prior_imu_bias; // assume zero initial bias
Values initial_values;
int correction_count = 0;
@ -139,57 +139,64 @@ int main(int argc, char* argv[])
initial_values.insert(V(correction_count), prior_velocity);
initial_values.insert(B(correction_count), prior_imu_bias);
// Assemble prior noise model and add it the graph.
noiseModel::Diagonal::shared_ptr pose_noise_model = noiseModel::Diagonal::Sigmas((Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5).finished()); // rad,rad,rad,m, m, m
noiseModel::Diagonal::shared_ptr velocity_noise_model = noiseModel::Isotropic::Sigma(3,0.1); // m/s
noiseModel::Diagonal::shared_ptr bias_noise_model = noiseModel::Isotropic::Sigma(6,1e-3);
// Assemble prior noise model and add it the graph.`
auto pose_noise_model = noiseModel::Diagonal::Sigmas(
(Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5)
.finished()); // rad,rad,rad,m, m, m
auto velocity_noise_model = noiseModel::Isotropic::Sigma(3, 0.1); // m/s
auto bias_noise_model = noiseModel::Isotropic::Sigma(6, 1e-3);
// Add all prior factors (pose, velocity, bias) to the graph.
NonlinearFactorGraph *graph = new NonlinearFactorGraph();
graph->add(PriorFactor<Pose3>(X(correction_count), prior_pose, pose_noise_model));
graph->add(PriorFactor<Vector3>(V(correction_count), prior_velocity,velocity_noise_model));
graph->add(PriorFactor<imuBias::ConstantBias>(B(correction_count), prior_imu_bias,bias_noise_model));
NonlinearFactorGraph* graph = new NonlinearFactorGraph();
graph->addPrior(X(correction_count), prior_pose, pose_noise_model);
graph->addPrior(V(correction_count), prior_velocity, velocity_noise_model);
graph->addPrior(B(correction_count), prior_imu_bias, bias_noise_model);
// We use the sensor specs to build the noise model for the IMU factor.
double accel_noise_sigma = 0.0003924;
double gyro_noise_sigma = 0.000205689024915;
double accel_bias_rw_sigma = 0.004905;
double gyro_bias_rw_sigma = 0.000001454441043;
Matrix33 measured_acc_cov = Matrix33::Identity(3,3) * pow(accel_noise_sigma,2);
Matrix33 measured_omega_cov = Matrix33::Identity(3,3) * pow(gyro_noise_sigma,2);
Matrix33 integration_error_cov = Matrix33::Identity(3,3)*1e-8; // error committed in integrating position from velocities
Matrix33 bias_acc_cov = Matrix33::Identity(3,3) * pow(accel_bias_rw_sigma,2);
Matrix33 bias_omega_cov = Matrix33::Identity(3,3) * pow(gyro_bias_rw_sigma,2);
Matrix66 bias_acc_omega_int = Matrix::Identity(6,6)*1e-5; // error in the bias used for preintegration
Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2);
Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2);
Matrix33 integration_error_cov =
I_3x3 * 1e-8; // error committed in integrating position from velocities
Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
Matrix66 bias_acc_omega_int =
I_6x6 * 1e-5; // error in the bias used for preintegration
boost::shared_ptr<PreintegratedCombinedMeasurements::Params> p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
// PreintegrationBase params:
p->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous
p->integrationCovariance = integration_error_cov; // integration uncertainty continuous
p->accelerometerCovariance =
measured_acc_cov; // acc white noise in continuous
p->integrationCovariance =
integration_error_cov; // integration uncertainty continuous
// should be using 2nd order integration
// PreintegratedRotation params:
p->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous
p->gyroscopeCovariance =
measured_omega_cov; // gyro white noise in continuous
// PreintegrationCombinedMeasurements params:
p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
p->biasAccOmegaInt = bias_acc_omega_int;
std::shared_ptr<PreintegrationType> imu_preintegrated_ = nullptr;
std::shared_ptr<PreintegrationType> preintegrated = nullptr;
if (use_combined_imu) {
imu_preintegrated_ =
preintegrated =
std::make_shared<PreintegratedCombinedMeasurements>(p, prior_imu_bias);
} else {
imu_preintegrated_ =
preintegrated =
std::make_shared<PreintegratedImuMeasurements>(p, prior_imu_bias);
}
assert(imu_preintegrated_);
assert(preintegrated);
// Store previous state for the imu integration and the latest predicted outcome.
// Store previous state for imu integration and latest predicted outcome.
NavState prev_state(prior_pose, prior_velocity);
NavState prop_state = prev_state;
imuBias::ConstantBias prev_bias = prior_imu_bias;
// Keep track of the total error over the entire run for a simple performance metric.
// Keep track of total error over the entire run as simple performance metric.
double current_position_error = 0.0, current_orientation_error = 0.0;
double output_time = 0.0;
@ -198,14 +205,13 @@ int main(int argc, char* argv[])
// All priors have been set up, now iterate through the data file.
while (file.good()) {
// Parse out first value
getline(file, value, ',');
int type = atoi(value.c_str());
if (type == 0) { // IMU measurement
Eigen::Matrix<double,6,1> imu = Eigen::Matrix<double,6,1>::Zero();
for (int i=0; i<5; ++i) {
if (type == 0) { // IMU measurement
Vector6 imu;
for (int i = 0; i < 5; ++i) {
getline(file, value, ',');
imu(i) = atof(value.c_str());
}
@ -213,11 +219,11 @@ int main(int argc, char* argv[])
imu(5) = atof(value.c_str());
// Adding the IMU preintegration.
imu_preintegrated_->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);
preintegrated->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);
} else if (type == 1) { // GPS measurement
Eigen::Matrix<double,7,1> gps = Eigen::Matrix<double,7,1>::Zero();
for (int i=0; i<6; ++i) {
} else if (type == 1) { // GPS measurement
Vector7 gps;
for (int i = 0; i < 6; ++i) {
getline(file, value, ',');
gps(i) = atof(value.c_str());
}
@ -228,39 +234,37 @@ int main(int argc, char* argv[])
// Adding IMU factor and GPS factor and optimizing.
if (use_combined_imu) {
const PreintegratedCombinedMeasurements& preint_imu_combined =
auto preint_imu_combined =
dynamic_cast<const PreintegratedCombinedMeasurements&>(
*imu_preintegrated_);
CombinedImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
X(correction_count ), V(correction_count ),
B(correction_count-1), B(correction_count ),
preint_imu_combined);
*preintegrated);
CombinedImuFactor imu_factor(
X(correction_count - 1), V(correction_count - 1),
X(correction_count), V(correction_count), B(correction_count - 1),
B(correction_count), preint_imu_combined);
graph->add(imu_factor);
} else {
const PreintegratedImuMeasurements& preint_imu =
dynamic_cast<const PreintegratedImuMeasurements&>(
*imu_preintegrated_);
ImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
X(correction_count ), V(correction_count ),
B(correction_count-1),
preint_imu);
auto preint_imu =
dynamic_cast<const PreintegratedImuMeasurements&>(*preintegrated);
ImuFactor imu_factor(X(correction_count - 1), V(correction_count - 1),
X(correction_count), V(correction_count),
B(correction_count - 1), preint_imu);
graph->add(imu_factor);
imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
graph->add(BetweenFactor<imuBias::ConstantBias>(B(correction_count-1),
B(correction_count ),
zero_bias, bias_noise_model));
graph->add(BetweenFactor<imuBias::ConstantBias>(
B(correction_count - 1), B(correction_count), zero_bias,
bias_noise_model));
}
noiseModel::Diagonal::shared_ptr correction_noise = noiseModel::Isotropic::Sigma(3,1.0);
auto correction_noise = noiseModel::Isotropic::Sigma(3, 1.0);
GPSFactor gps_factor(X(correction_count),
Point3(gps(0), // N,
gps(1), // E,
gps(2)), // D,
Point3(gps(0), // N,
gps(1), // E,
gps(2)), // D,
correction_noise);
graph->add(gps_factor);
// Now optimize and compare results.
prop_state = imu_preintegrated_->predict(prev_state, prev_bias);
prop_state = preintegrated->predict(prev_state, prev_bias);
initial_values.insert(X(correction_count), prop_state.pose());
initial_values.insert(V(correction_count), prop_state.v());
initial_values.insert(B(correction_count), prev_bias);
@ -284,7 +288,7 @@ int main(int argc, char* argv[])
prev_bias = result.at<imuBias::ConstantBias>(B(correction_count));
// Reset the preintegration object.
imu_preintegrated_->resetIntegrationAndSetBias(prev_bias);
preintegrated->resetIntegrationAndSetBias(prev_bias);
// Print out the position and orientation error for comparison.
Vector3 gtsam_position = prev_state.pose().translation();
@ -295,19 +299,19 @@ int main(int argc, char* argv[])
Quaternion gps_quat(gps(6), gps(3), gps(4), gps(5));
Quaternion quat_error = gtsam_quat * gps_quat.inverse();
quat_error.normalize();
Vector3 euler_angle_error(quat_error.x()*2,
quat_error.y()*2,
quat_error.z()*2);
Vector3 euler_angle_error(quat_error.x() * 2, quat_error.y() * 2,
quat_error.z() * 2);
current_orientation_error = euler_angle_error.norm();
// display statistics
cout << "Position error:" << current_position_error << "\t " << "Angular error:" << current_orientation_error << "\n";
cout << "Position error:" << current_position_error << "\t "
<< "Angular error:" << current_orientation_error << "\n";
fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
output_time, gtsam_position(0), gtsam_position(1), gtsam_position(2),
gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(), gtsam_quat.w(),
gps(0), gps(1), gps(2),
gps_quat.x(), gps_quat.y(), gps_quat.z(), gps_quat.w());
output_time, gtsam_position(0), gtsam_position(1),
gtsam_position(2), gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(),
gtsam_quat.w(), gps(0), gps(1), gps(2), gps_quat.x(),
gps_quat.y(), gps_quat.z(), gps_quat.w());
output_time += 1.0;
@ -317,6 +321,7 @@ int main(int argc, char* argv[])
}
}
fclose(fp_out);
cout << "Complete, results written to " << output_filename << "\n\n";;
cout << "Complete, results written to " << output_filename << "\n\n";
return 0;
}

1
examples/InverseKinematicsExampleExpressions.cpp
View File

@ -22,7 +22,6 @@
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/expressions.h>
#include <cmath>

22
examples/LocalizationExample.cpp
View File

@ -66,12 +66,11 @@ using namespace gtsam;
#include <gtsam/nonlinear/NonlinearFactor.h>
class UnaryFactor: public NoiseModelFactor1<Pose2> {
// The factor will hold a measurement consisting of an (X,Y) location
// We could this with a Point2 but here we just use two doubles
double mx_, my_;
public:
public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<UnaryFactor> shared_ptr;
@ -85,15 +84,15 @@ public:
// The first is the 'evaluateError' function. This function implements the desired measurement
// function, returning a vector of errors when evaluated at the provided variable value. It
// must also calculate the Jacobians for this measurement function, if requested.
Vector evaluateError(const Pose2& q, boost::optional<Matrix&> H = boost::none) const
{
Vector evaluateError(const Pose2& q,
boost::optional<Matrix&> H = boost::none) const {
// The measurement function for a GPS-like measurement is simple:
// error_x = pose.x - measurement.x
// error_y = pose.y - measurement.y
// Consequently, the Jacobians are:
// [ derror_x/dx derror_x/dy derror_x/dtheta ] = [1 0 0]
// [ derror_y/dx derror_y/dy derror_y/dtheta ] = [0 1 0]
if (H) (*H) = (Matrix(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0).finished();
if (H) (*H) = (Matrix(2, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0).finished();
return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
}
@ -107,29 +106,28 @@ public:
// Additionally, we encourage you the use of unit testing your custom factors,
// (as all GTSAM factors are), in which you would need an equals and print, to satisfy the
// GTSAM_CONCEPT_TESTABLE_INST(T) defined in Testable.h, but these are not needed below.
}; // UnaryFactor
}; // UnaryFactor
int main(int argc, char** argv) {
// 1. Create a factor graph container and add factors to it
NonlinearFactorGraph graph;
// 2a. Add odometry factors
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise);
// 2b. Add "GPS-like" measurements
// We will use our custom UnaryFactor for this.
noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
auto unaryNoise =
noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
graph.emplace_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise);
graph.emplace_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise);
graph.emplace_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise);
graph.print("\nFactor Graph:\n"); // print
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
@ -137,7 +135,7 @@ int main(int argc, char** argv) {
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1));
initialEstimate.print("\nInitial Estimate:\n"); // print
initialEstimate.print("\nInitial Estimate:\n"); // print
// 4. Optimize using Levenberg-Marquardt optimization. The optimizer
// accepts an optional set of configuration parameters, controlling

24
examples/METISOrderingExample.cpp
View File

@ -11,7 +11,8 @@
/**
* @file METISOrdering.cpp
* @brief Simple robot motion example, with prior and two odometry measurements, using a METIS ordering
* @brief Simple robot motion example, with prior and two odometry measurements,
* using a METIS ordering
* @author Frank Dellaert
* @author Andrew Melim
*/
@ -22,12 +23,11 @@
*/
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/BetweenFactor.h>
using namespace std;
using namespace gtsam;
@ -35,26 +35,26 @@ using namespace gtsam;
int main(int argc, char** argv) {
NonlinearFactorGraph graph;
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.emplace_shared<PriorFactor<Pose2> >(1, priorMean, priorNoise);
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.addPrior(1, priorMean, priorNoise);
Pose2 odometry(2.0, 0.0, 0.0);
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, odometry, odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, odometry, odometryNoise);
graph.print("\nFactor Graph:\n"); // print
graph.print("\nFactor Graph:\n"); // print
Values initial;
initial.insert(1, Pose2(0.5, 0.0, 0.2));
initial.insert(2, Pose2(2.3, 0.1, -0.2));
initial.insert(3, Pose2(4.1, 0.1, 0.1));
initial.print("\nInitial Estimate:\n"); // print
initial.print("\nInitial Estimate:\n"); // print
// optimize using Levenberg-Marquardt optimization
LevenbergMarquardtParams params;
// In order to specify the ordering type, we need to se the NonlinearOptimizerParameter "orderingType"
// By default this parameter is set to OrderingType::COLAMD
// In order to specify the ordering type, we need to se the "orderingType". By
// default this parameter is set to OrderingType::COLAMD
params.orderingType = Ordering::METIS;
LevenbergMarquardtOptimizer optimizer(graph, initial, params);
Values result = optimizer.optimize();

14
examples/OdometryExample.cpp
View File

@ -29,7 +29,6 @@
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
// Here we will use Between factors for the relative motion described by odometry measurements.
// Also, we will initialize the robot at the origin using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
@ -57,24 +56,23 @@ using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
// Create an empty nonlinear factor graph
NonlinearFactorGraph graph;
// Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.emplace_shared<PriorFactor<Pose2> >(1, priorMean, priorNoise);
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.addPrior(1, priorMean, priorNoise);
// Add odometry factors
Pose2 odometry(2.0, 0.0, 0.0);
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, odometry, odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, odometry, odometryNoise);
graph.print("\nFactor Graph:\n"); // print
graph.print("\nFactor Graph:\n"); // print
// Create the data structure to hold the initialEstimate estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
@ -82,7 +80,7 @@ int main(int argc, char** argv) {
initial.insert(1, Pose2(0.5, 0.0, 0.2));
initial.insert(2, Pose2(2.3, 0.1, -0.2));
initial.insert(3, Pose2(4.1, 0.1, 0.1));
initial.print("\nInitial Estimate:\n"); // print
initial.print("\nInitial Estimate:\n"); // print
// optimize using Levenberg-Marquardt optimization
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();

35
examples/PlanarSLAMExample.cpp
View File

@ -40,7 +40,6 @@
// Here we will use a RangeBearing factor for the range-bearing measurements to identified
// landmarks, and Between factors for the relative motion described by odometry measurements.
// Also, we will initialize the robot at the origin using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
@ -70,35 +69,36 @@ using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
// Create a factor graph
NonlinearFactorGraph graph;
// Create the keys we need for this simple example
static Symbol x1('x',1), x2('x',2), x3('x',3);
static Symbol l1('l',1), l2('l',2);
static Symbol x1('x', 1), x2('x', 2), x3('x', 3);
static Symbol l1('l', 1), l2('l', 2);
// Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.emplace_shared<PriorFactor<Pose2> >(x1, prior, priorNoise); // add directly to graph
// Add a prior on pose x1 at the origin. A prior factor consists of a mean and
// a noise model (covariance matrix)
Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
auto priorNoise = noiseModel::Diagonal::Sigmas(
Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.addPrior(x1, prior, priorNoise); // add directly to graph
// Add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
Pose2 odometry(2.0, 0.0, 0.0);
// create a measurement for both factors (the same in this case)
auto odometryNoise = noiseModel::Diagonal::Sigmas(
Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
graph.emplace_shared<BetweenFactor<Pose2> >(x1, x2, odometry, odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(x2, x3, odometry, odometryNoise);
// Add Range-Bearing measurements to two different landmarks
// create a noise model for the landmark measurements
noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
auto measurementNoise = noiseModel::Diagonal::Sigmas(
Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
// create the measurement values - indices are (pose id, landmark id)
Rot2 bearing11 = Rot2::fromDegrees(45),
bearing21 = Rot2::fromDegrees(90),
Rot2 bearing11 = Rot2::fromDegrees(45), bearing21 = Rot2::fromDegrees(90),
bearing32 = Rot2::fromDegrees(90);
double range11 = std::sqrt(4.0+4.0),
range21 = 2.0,
range32 = 2.0;
double range11 = std::sqrt(4.0 + 4.0), range21 = 2.0, range32 = 2.0;
// Add Bearing-Range factors
graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x1, l1, bearing11, range11, measurementNoise);
@ -111,7 +111,7 @@ int main(int argc, char** argv) {
// Create (deliberately inaccurate) initial estimate
Values initialEstimate;
initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insert(x2, Pose2(2.3, 0.1, -0.2));
initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
initialEstimate.insert(l1, Point2(1.8, 2.1));
initialEstimate.insert(l2, Point2(4.1, 1.8));
@ -139,4 +139,3 @@ int main(int argc, char** argv) {
return 0;
}

22
examples/Pose2SLAMExample.cpp
View File

@ -36,7 +36,6 @@
// Here we will use Between factors for the relative motion described by odometry measurements.
// We will also use a Between Factor to encode the loop closure constraint
// Also, we will initialize the robot at the origin using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
@ -65,21 +64,20 @@ using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
// 1. Create a factor graph container and add factors to it
NonlinearFactorGraph graph;
// 2a. Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.emplace_shared<PriorFactor<Pose2> >(1, Pose2(0, 0, 0), priorNoise);
auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.addPrior(1, Pose2(0, 0, 0), priorNoise);
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// 2b. Add odometry factors
// Create odometry (Between) factors between consecutive poses
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0 ), model);
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0), model);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2, 0, M_PI_2), model);
graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2, 0, M_PI_2), model);
graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2, 0, M_PI_2), model);
@ -89,17 +87,17 @@ int main(int argc, char** argv) {
// these constraints may be identified in many ways, such as appearance-based techniques
// with camera images. We will use another Between Factor to enforce this constraint:
graph.emplace_shared<BetweenFactor<Pose2> >(5, 2, Pose2(2, 0, M_PI_2), model);
graph.print("\nFactor Graph:\n"); // print
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
Values initialEstimate;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 ));
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2 ));
initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2));
initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI ));
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2));
initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI));
initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2));
initialEstimate.print("\nInitial Estimate:\n"); // print
initialEstimate.print("\nInitial Estimate:\n"); // print
// 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
// The optimizer accepts an optional set of configuration parameters,

35
examples/Pose2SLAMExampleExpressions.cpp
View File

@ -21,7 +21,6 @@
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
// For an explanation of headers below, please see Pose2SLAMExample.cpp
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
@ -31,7 +30,6 @@ using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
// 1. Create a factor graph container and add factors to it
ExpressionFactorGraph graph;
@ -39,31 +37,32 @@ int main(int argc, char** argv) {
Pose2_ x1(1), x2(2), x3(3), x4(4), x5(5);
// 2a. Add a prior on the first pose, setting it to the origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.addExpressionFactor(x1, Pose2(0, 0, 0), priorNoise);
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// For simplicity, we use the same noise model for odometry and loop closures
auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// 2b. Add odometry factors
graph.addExpressionFactor(between(x1,x2), Pose2(2, 0, 0 ), model);
graph.addExpressionFactor(between(x2,x3), Pose2(2, 0, M_PI_2), model);
graph.addExpressionFactor(between(x3,x4), Pose2(2, 0, M_PI_2), model);
graph.addExpressionFactor(between(x4,x5), Pose2(2, 0, M_PI_2), model);
graph.addExpressionFactor(between(x1, x2), Pose2(2, 0, 0), model);
graph.addExpressionFactor(between(x2, x3), Pose2(2, 0, M_PI_2), model);
graph.addExpressionFactor(between(x3, x4), Pose2(2, 0, M_PI_2), model);
graph.addExpressionFactor(between(x4, x5), Pose2(2, 0, M_PI_2), model);
// 2c. Add the loop closure constraint
graph.addExpressionFactor(between(x5,x2), Pose2(2, 0, M_PI_2), model);
graph.print("\nFactor Graph:\n"); // print
graph.addExpressionFactor(between(x5, x2), Pose2(2, 0, M_PI_2), model);
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
// 3. Create the data structure to hold the initialEstimate estimate to the
// solution For illustrative purposes, these have been deliberately set to
// incorrect values
Values initialEstimate;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 ));
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2 ));
initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2));
initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI ));
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2));
initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI));
initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2));
initialEstimate.print("\nInitial Estimate:\n"); // print
initialEstimate.print("\nInitial Estimate:\n"); // print
// 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
GaussNewtonParams parameters;

44
examples/Pose2SLAMExample_g2o.cpp
View File

@ -19,7 +19,6 @@
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <fstream>
@ -29,50 +28,51 @@ using namespace gtsam;
// HOWTO: ./Pose2SLAMExample_g2o inputFile outputFile (maxIterations) (tukey/huber)
int main(const int argc, const char *argv[]) {
string kernelType = "none";
int maxIterations = 100; // default
string g2oFile = findExampleDataFile("noisyToyGraph.txt"); // default
int maxIterations = 100; // default
string g2oFile = findExampleDataFile("noisyToyGraph.txt"); // default
// Parse user's inputs
if (argc > 1){
g2oFile = argv[1]; // input dataset filename
// outputFile = g2oFile = argv[2]; // done later
if (argc > 1) {
g2oFile = argv[1]; // input dataset filename
}
if (argc > 3){
maxIterations = atoi(argv[3]); // user can specify either tukey or huber
if (argc > 3) {
maxIterations = atoi(argv[3]); // user can specify either tukey or huber
}
if (argc > 4){
kernelType = argv[4]; // user can specify either tukey or huber
if (argc > 4) {
kernelType = argv[4]; // user can specify either tukey or huber
}
// reading file and creating factor graph
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = false;
if(kernelType.compare("none") == 0){
boost::tie(graph, initial) = readG2o(g2oFile,is3D);
if (kernelType.compare("none") == 0) {
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
}
if(kernelType.compare("huber") == 0){
if (kernelType.compare("huber") == 0) {
std::cout << "Using robust kernel: huber " << std::endl;
boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeHUBER);
boost::tie(graph, initial) =
readG2o(g2oFile, is3D, KernelFunctionTypeHUBER);
}
if(kernelType.compare("tukey") == 0){
if (kernelType.compare("tukey") == 0) {
std::cout << "Using robust kernel: tukey " << std::endl;
boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeTUKEY);
boost::tie(graph, initial) =
readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);
}
// Add prior on the pose having index (key) = 0
noiseModel::Diagonal::shared_ptr priorModel = //
auto priorModel = //
noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
graph->add(PriorFactor<Pose2>(0, Pose2(), priorModel));
graph->addPrior(0, Pose2(), priorModel);
std::cout << "Adding prior on pose 0 " << std::endl;
GaussNewtonParams params;
params.setVerbosity("TERMINATION");
if (argc > 3) {
params.maxIterations = maxIterations;
std::cout << "User required to perform maximum " << params.maxIterations << " iterations "<< std::endl;
std::cout << "User required to perform maximum " << params.maxIterations
<< " iterations " << std::endl;
}
std::cout << "Optimizing the factor graph" << std::endl;
@ -80,8 +80,8 @@ int main(const int argc, const char *argv[]) {
Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl;
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
std::cout << "final error=" <<graph->error(result)<< std::endl;
std::cout << "initial error=" << graph->error(*initial) << std::endl;
std::cout << "final error=" << graph->error(result) << std::endl;
if (argc < 3) {
result.print("result");

3
examples/Pose2SLAMExample_graph.cpp
View File

@ -17,7 +17,6 @@
*/
// For an explanation of headers below, please see Pose2SLAMExample.cpp
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -43,7 +42,7 @@ int main (int argc, char** argv) {
// Add a Gaussian prior on first poses
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01));
graph->push_back(PriorFactor<Pose2>(0, priorMean, priorNoise));
graph -> addPrior(0, priorMean, priorNoise);
// Single Step Optimization using Levenberg-Marquardt
Values result = LevenbergMarquardtOptimizer(*graph, *initial).optimize();

23
examples/Pose2SLAMExample_graphviz.cpp
View File

@ -17,7 +17,6 @@
*/
// For an explanation of headers below, please see Pose2SLAMExample.cpp
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -26,20 +25,20 @@
using namespace std;
using namespace gtsam;
int main (int argc, char** argv) {
int main(int argc, char** argv) {
// 1. Create a factor graph container and add factors to it
NonlinearFactorGraph graph;
// 2a. Add a prior on the first pose, setting it to the origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.emplace_shared<PriorFactor<Pose2> >(1, Pose2(0, 0, 0), priorNoise);
auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.addPrior(1, Pose2(0, 0, 0), priorNoise);
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// For simplicity, we will use the same noise model for odometry and loop
// closures
auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// 2b. Add odometry factors
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0 ), model);
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0), model);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2, 0, M_PI_2), model);
graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2, 0, M_PI_2), model);
graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2, 0, M_PI_2), model);
@ -50,10 +49,10 @@ int main (int argc, char** argv) {
// 3. Create the data structure to hold the initial estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
Values initial;
initial.insert(1, Pose2(0.5, 0.0, 0.2 ));
initial.insert(2, Pose2(2.3, 0.1, -0.2 ));
initial.insert(3, Pose2(4.1, 0.1, M_PI_2));
initial.insert(4, Pose2(4.0, 2.0, M_PI ));
initial.insert(1, Pose2(0.5, 0.0, 0.2));
initial.insert(2, Pose2(2.3, 0.1, -0.2));
initial.insert(3, Pose2(4.1, 0.1, M_PI_2));
initial.insert(4, Pose2(4.0, 2.0, M_PI));
initial.insert(5, Pose2(2.1, 2.1, -M_PI_2));
// Single Step Optimization using Levenberg-Marquardt

7
examples/Pose2SLAMExample_lago.cpp
View File

@ -21,7 +21,6 @@
#include <gtsam/slam/lago.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <fstream>
@ -29,7 +28,6 @@ using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
@ -42,9 +40,8 @@ int main(const int argc, const char *argv[]) {
boost::tie(graph, initial) = readG2o(g2oFile);
// Add prior on the pose having index (key) = 0
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
graph->add(PriorFactor<Pose2>(0, Pose2(), priorModel));
auto priorModel = noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
graph->addPrior(0, Pose2(), priorModel);
graph->print();
std::cout << "Computing LAGO estimate" << std::endl;

3
examples/Pose2SLAMStressTest.cpp
View File

@ -19,7 +19,6 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/StereoFactor.h>
#include <random>
@ -48,7 +47,7 @@ void testGtsam(int numberNodes) {
Pose3 first = Pose3(first_M);
NonlinearFactorGraph graph;
graph.add(PriorFactor<Pose3>(0, first, priorModel));
graph.addPrior(0, first, priorModel);
// vo noise model
auto VOCovarianceModel = noiseModel::Isotropic::Variance(6, 1e-3);

31
examples/Pose2SLAMwSPCG.cpp
View File

@ -17,7 +17,6 @@
*/
// For an explanation of headers below, please see Pose2SLAMExample.cpp
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -29,45 +28,45 @@ using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
// 1. Create a factor graph container and add factors to it
NonlinearFactorGraph graph;
// 2a. Add a prior on the first pose, setting it to the origin
Pose2 prior(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.emplace_shared<PriorFactor<Pose2> >(1, prior, priorNoise);
Pose2 prior(0.0, 0.0, 0.0); // prior at origin
auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.addPrior(1, prior, priorNoise);
// 2b. Add odometry factors
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
// 2c. Add the loop closure constraint
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.emplace_shared<BetweenFactor<Pose2> >(5, 1, Pose2(0.0, 0.0, 0.0), model);
graph.print("\nFactor Graph:\n"); // print
auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.emplace_shared<BetweenFactor<Pose2> >(5, 1, Pose2(0.0, 0.0, 0.0),
model);
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution
// 3. Create the data structure to hold the initialEstimate estimate to the
// solution
Values initialEstimate;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(2, Pose2(2.3, 0.1, 1.1));
initialEstimate.insert(3, Pose2(2.1, 1.9, 2.8));
initialEstimate.insert(4, Pose2(-.3, 2.5, 4.2));
initialEstimate.insert(5, Pose2(0.1,-0.7, 5.8));
initialEstimate.print("\nInitial Estimate:\n"); // print
initialEstimate.insert(5, Pose2(0.1, -0.7, 5.8));
initialEstimate.print("\nInitial Estimate:\n"); // print
// 4. Single Step Optimization using Levenberg-Marquardt
LevenbergMarquardtParams parameters;
parameters.verbosity = NonlinearOptimizerParams::ERROR;
parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA;
// LM is still the outer optimization loop, but by specifying "Iterative" below
// We indicate that an iterative linear solver should be used.
// In addition, the *type* of the iterativeParams decides on the type of
// LM is still the outer optimization loop, but by specifying "Iterative"
// below We indicate that an iterative linear solver should be used. In
// addition, the *type* of the iterativeParams decides on the type of
// iterative solver, in this case the SPCG (subgraph PCG)
parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();

24
examples/Pose3Localization.cpp
View File

@ -10,16 +10,15 @@
* -------------------------------------------------------------------------- */
/**
* @file Pose3SLAMExample_initializePose3.cpp
* @file Pose3Localization.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
* Syntax for the script is ./Pose3Localization input.g2o output.g2o
* @date Aug 25, 2014
* @author Luca Carlone
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <fstream>
@ -27,8 +26,7 @@
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
int main(const int argc, const char* argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
@ -42,25 +40,25 @@ int main(const int argc, const char *argv[]) {
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
auto priorModel = noiseModel::Diagonal::Variances(
(Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
for(const Values::ConstKeyValuePair& key_value: *initial) {
for (const Values::ConstKeyValuePair& key_value : *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
graph->addPrior(firstKey, Pose3(), priorModel);
break;
}
std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonParams params;
params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
params.setVerbosity("TERMINATION"); // show info about stopping conditions
GaussNewtonOptimizer optimizer(*graph, *initial, params);
Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl;
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
std::cout << "final error=" <<graph->error(result)<< std::endl;
std::cout << "initial error=" << graph->error(*initial) << std::endl;
std::cout << "final error=" << graph->error(result) << std::endl;
if (argc < 3) {
result.print("result");
@ -76,7 +74,7 @@ int main(const int argc, const char *argv[]) {
// Calculate and print marginal covariances for all variables
Marginals marginals(*graph, result);
for(const auto& key_value: result) {
for (const auto& key_value : result) {
auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
if (!p) continue;
std::cout << marginals.marginalCovariance(key_value.key) << endl;

1
examples/Pose3SLAMExample_changeKeys.cpp
View File

@ -19,7 +19,6 @@
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <fstream>
using namespace std;

22
examples/Pose3SLAMExample_g2o.cpp
View File

@ -10,24 +10,22 @@
* -------------------------------------------------------------------------- */
/**
* @file Pose3SLAMExample_initializePose3.cpp
* @file Pose3SLAMExample_g2o.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
* Syntax for the script is ./Pose3SLAMExample_g2o input.g2o output.g2o
* @date Aug 25, 2014
* @author Luca Carlone
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <fstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
int main(const int argc, const char* argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
@ -41,25 +39,25 @@ int main(const int argc, const char *argv[]) {
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
auto priorModel = noiseModel::Diagonal::Variances(
(Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
for(const Values::ConstKeyValuePair& key_value: *initial) {
for (const Values::ConstKeyValuePair& key_value : *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
graph->addPrior(firstKey, Pose3(), priorModel);
break;
}
std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonParams params;
params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
params.setVerbosity("TERMINATION"); // show info about stopping conditions
GaussNewtonOptimizer optimizer(*graph, *initial, params);
Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl;
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
std::cout << "final error=" <<graph->error(result)<< std::endl;
std::cout << "initial error=" << graph->error(*initial) << std::endl;
std::cout << "final error=" << graph->error(result) << std::endl;
if (argc < 3) {
result.print("result");

18
examples/Pose3SLAMExample_initializePose3Chordal.cpp
View File

@ -10,9 +10,9 @@
* -------------------------------------------------------------------------- */
/**
* @file Pose3SLAMExample_initializePose3.cpp
* @file Pose3SLAMExample_initializePose3Chordal.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
* Syntax for the script is ./Pose3SLAMExample_initializePose3Chordal input.g2o output.g2o
* @date Aug 25, 2014
* @author Luca Carlone
*/
@ -20,14 +20,12 @@
#include <gtsam/slam/InitializePose3.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <fstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
int main(const int argc, const char* argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
@ -41,13 +39,13 @@ int main(const int argc, const char *argv[]) {
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
auto priorModel = noiseModel::Diagonal::Variances(
(Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
for(const Values::ConstKeyValuePair& key_value: *initial) {
for (const Values::ConstKeyValuePair& key_value : *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
graph->addPrior(firstKey, Pose3(), priorModel);
break;
}
@ -59,7 +57,7 @@ int main(const int argc, const char *argv[]) {
initialization.print("initialization");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
std::cout << "Writing results to file: " << outputFile << std::endl;
NonlinearFactorGraph::shared_ptr graphNoKernel;
Values::shared_ptr initial2;
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);

26
examples/Pose3SLAMExample_initializePose3Gradient.cpp
View File

@ -10,9 +10,9 @@
* -------------------------------------------------------------------------- */
/**
* @file Pose3SLAMExample_initializePose3.cpp
* @file Pose3SLAMExample_initializePose3Gradient.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
* Syntax for the script is ./Pose3SLAMExample_initializePose3Gradient input.g2o output.g2o
* @date Aug 25, 2014
* @author Luca Carlone
*/
@ -20,14 +20,12 @@
#include <gtsam/slam/InitializePose3.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <fstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
int main(const int argc, const char* argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
@ -41,29 +39,31 @@ int main(const int argc, const char *argv[]) {
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
auto priorModel = noiseModel::Diagonal::Variances(
(Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
for(const Values::ConstKeyValuePair& key_value: *initial) {
for (const Values::ConstKeyValuePair& key_value : *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
graph->addPrior(firstKey, Pose3(), priorModel);
break;
}
std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl;
bool useGradient = true;
Values initialization = InitializePose3::initialize(*graph, *initial, useGradient);
Values initialization =
InitializePose3::initialize(*graph, *initial, useGradient);
std::cout << "done!" << std::endl;
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
std::cout << "initialization error=" <<graph->error(initialization)<< std::endl;
std::cout << "initial error=" << graph->error(*initial) << std::endl;
std::cout << "initialization error=" << graph->error(initialization)
<< std::endl;
if (argc < 3) {
initialization.print("initialization");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
std::cout << "Writing results to file: " << outputFile << std::endl;
NonlinearFactorGraph::shared_ptr graphNoKernel;
Values::shared_ptr initial2;
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);

3
examples/RangeISAMExample_plaza2.cpp
View File

@ -37,7 +37,6 @@
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics SLAM problems.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sam/RangeFactor.h>
#include <gtsam/slam/dataset.h>
@ -124,7 +123,7 @@ int main (int argc, char** argv) {
Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
M_PI - 2.02108900000000);
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose2>(0, pose0, priorNoise));
newFactors.addPrior(0, pose0, priorNoise);
Values initial;
initial.insert(0, pose0);

44
examples/SFMExample.cpp
View File

@ -30,7 +30,6 @@
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
// Here we will use Projection factors to model the camera's landmark observations.
// Also, we will initialize the robot at some location using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
@ -57,12 +56,12 @@ using namespace gtsam;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Define the camera calibration parameters
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
// Define the camera observation noise model
noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
auto measurementNoise =
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks
vector<Point3> points = createPoints();
@ -74,32 +73,45 @@ int main(int argc, char* argv[]) {
NonlinearFactorGraph graph;
// Add a prior on pose x1. This indirectly specifies where the origin is.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise); // add directly to graph
auto poseNoise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
.finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.addPrior(Symbol('x', 0), poses[0], poseNoise); // add directly to graph
// Simulated measurements from each camera pose, adding them to the factor graph
// Simulated measurements from each camera pose, adding them to the factor
// graph
for (size_t i = 0; i < poses.size(); ++i) {
PinholeCamera<Cal3_S2> camera(poses[i], *K);
for (size_t j = 0; j < points.size(); ++j) {
Point2 measurement = camera.project(points[j]);
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(
measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
}
}
// 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.
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise); // add directly to graph
// 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.
auto pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.addPrior(Symbol('l', 0), points[0],
pointNoise); // add directly to graph
graph.print("Factor Graph:\n");
// Create the data structure to hold the initial estimate to the solution
// Intentionally initialize the variables off from the ground truth
Values initialEstimate;
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
for (size_t i = 0; i < poses.size(); ++i) {
auto corrupted_pose = poses[i].compose(
Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)));
initialEstimate.insert(
Symbol('x', i), corrupted_pose);
}
for (size_t j = 0; j < points.size(); ++j) {
Point3 corrupted_point = points[j] + Point3(-0.25, 0.20, 0.15);
initialEstimate.insert<Point3>(Symbol('l', j), corrupted_point);
}
initialEstimate.print("Initial Estimates:\n");
/* Optimize the graph and print results */

34
examples/SFMExampleExpressions_bal.cpp
View File

@ -28,7 +28,7 @@
// Header order is close to far
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
#include <vector>
using namespace std;
@ -40,20 +40,16 @@ using symbol_shorthand::P;
// An SfmCamera is defined in datase.h as a camera with unknown Cal3Bundler calibration
// and has a total of 9 free parameters
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Find default file, but if an argument is given, try loading a file
string filename = findExampleDataFile("dubrovnik-3-7-pre");
if (argc > 1)
filename = string(argv[1]);
if (argc > 1) filename = string(argv[1]);
// Load the SfM data from file
SfmData mydata;
readBAL(filename, mydata);
cout
<< boost::format("read %1% tracks on %2% cameras\n")
% mydata.number_tracks() % mydata.number_cameras();
cout << boost::format("read %1% tracks on %2% cameras\n") %
mydata.number_tracks() % mydata.number_cameras();
// Create a factor graph
ExpressionFactorGraph graph;
@ -65,23 +61,23 @@ int main(int argc, char* argv[]) {
Pose3_ pose0_(&SfmCamera::getPose, camera0_);
// Finally, we say it should be equal to first guess
graph.addExpressionFactor(pose0_, mydata.cameras[0].pose(),
noiseModel::Isotropic::Sigma(6, 0.1));
noiseModel::Isotropic::Sigma(6, 0.1));
// similarly, we create a prior on the first point
Point3_ point0_(P(0));
graph.addExpressionFactor(point0_, mydata.tracks[0].p,
noiseModel::Isotropic::Sigma(3, 0.1));
noiseModel::Isotropic::Sigma(3, 0.1));
// We share *one* noiseModel between all projection factors
noiseModel::Isotropic::shared_ptr noise = noiseModel::Isotropic::Sigma(2,
1.0); // one pixel in u and v
auto noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Simulated measurements from each camera pose, adding them to the factor graph
// Simulated measurements from each camera pose, adding them to the factor
// graph
size_t j = 0;
for(const SfmTrack& track: mydata.tracks) {
for (const SfmTrack& track : mydata.tracks) {
// Leaf expression for j^th point
Point3_ point_('p', j);
for(const SfmMeasurement& m: track.measurements) {
for (const SfmMeasurement& m : track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
// Leaf expression for i^th camera
@ -98,10 +94,8 @@ int main(int argc, char* argv[]) {
Values initial;
size_t i = 0;
j = 0;
for(const SfmCamera& camera: mydata.cameras)
initial.insert(C(i++), camera);
for(const SfmTrack& track: mydata.tracks)
initial.insert(P(j++), track.p);
for (const SfmCamera& camera : mydata.cameras) initial.insert(C(i++), camera);
for (const SfmTrack& track : mydata.tracks) initial.insert(P(j++), track.p);
/* Optimize the graph and print results */
Values result;
@ -117,5 +111,3 @@ int main(int argc, char* argv[]) {
return 0;
}
/* ************************************************************************* */

10
examples/SFMExample_SmartFactor.cpp
View File

@ -44,7 +44,7 @@ int main(int argc, char* argv[]) {
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
// Define the camera observation noise model
noiseModel::Isotropic::shared_ptr measurementNoise =
auto measurementNoise =
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks and poses
@ -80,15 +80,15 @@ int main(int argc, char* argv[]) {
// Add a prior on pose x0. This indirectly specifies where the origin is.
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
auto noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
graph.emplace_shared<PriorFactor<Pose3> >(0, poses[0], noise);
graph.addPrior(0, poses[0], noise);
// Because the structure-from-motion problem has a scale ambiguity, the problem is
// still under-constrained. Here we add a prior on the second pose x1, so this will
// fix the scale by indicating the distance between x0 and x1.
// Because these two are fixed, the rest of the poses will be also be fixed.
graph.emplace_shared<PriorFactor<Pose3> >(1, poses[1], noise); // add directly to graph
graph.addPrior(1, poses[1], noise); // add directly to graph
graph.print("Factor Graph:\n");
@ -117,7 +117,7 @@ int main(int argc, char* argv[]) {
// The output of point() is in boost::optional<Point3>, as sometimes
// the triangulation operation inside smart factor will encounter degeneracy.
boost::optional<Point3> point = smart->point(result);
if (point) // ignore if boost::optional return NULL
if (point) // ignore if boost::optional return nullptr
landmark_result.insert(j, *point);
}
}

29
examples/SFMExample_SmartFactorPCG.cpp
View File

@ -35,13 +35,12 @@ typedef PinholePose<Cal3_S2> Camera;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Define the camera calibration parameters
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
// Define the camera observation noise model
noiseModel::Isotropic::shared_ptr measurementNoise =
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
auto measurementNoise =
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks and poses
vector<Point3> points = createPoints();
@ -52,17 +51,16 @@ int main(int argc, char* argv[]) {
// Simulated measurements from each camera pose, adding them to the factor graph
for (size_t j = 0; j < points.size(); ++j) {
// every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
// every landmark represent a single landmark, we use shared pointer to init
// the factor, and then insert measurements.
SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
for (size_t i = 0; i < poses.size(); ++i) {
// generate the 2D measurement
Camera camera(poses[i], K);
Point2 measurement = camera.project(points[j]);
// call add() function to add measurement into a single factor, here we need to add:
// call add() function to add measurement into a single factor
smartfactor->add(measurement, i);
}
@ -72,12 +70,12 @@ int main(int argc, char* argv[]) {
// Add a prior on pose x0. This indirectly specifies where the origin is.
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
auto noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
graph.emplace_shared<PriorFactor<Pose3> >(0, poses[0], noise);
graph.addPrior(0, poses[0], noise);
// Fix the scale ambiguity by adding a prior
graph.emplace_shared<PriorFactor<Pose3> >(1, poses[0], noise);
graph.addPrior(1, poses[0], noise);
// Create the initial estimate to the solution
Values initialEstimate;
@ -85,9 +83,9 @@ int main(int argc, char* argv[]) {
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(i, poses[i].compose(delta));
// We will use LM in the outer optimization loop, but by specifying "Iterative" below
// We indicate that an iterative linear solver should be used.
// In addition, the *type* of the iterativeParams decides on the type of
// We will use LM in the outer optimization loop, but by specifying
// "Iterative" below We indicate that an iterative linear solver should be
// used. In addition, the *type* of the iterativeParams decides on the type of
// iterative solver, in this case the SPCG (subgraph PCG)
LevenbergMarquardtParams parameters;
parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
@ -110,11 +108,10 @@ int main(int argc, char* argv[]) {
result.print("Final results:\n");
Values landmark_result;
for (size_t j = 0; j < points.size(); ++j) {
SmartFactor::shared_ptr smart = //
boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
auto smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
if (smart) {
boost::optional<Point3> point = smart->point(result);
if (point) // ignore if boost::optional return NULL
if (point) // ignore if boost::optional return nullptr
landmark_result.insert(j, *point);
}
}

7
examples/SFMExample_bal.cpp
View File

@ -19,7 +19,6 @@
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
#include <vector>
@ -50,7 +49,7 @@ int main (int argc, char* argv[]) {
NonlinearFactorGraph graph;
// We share *one* noiseModel between all projection factors
noiseModel::Isotropic::shared_ptr noise =
auto noise =
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Add measurements to the factor graph
@ -66,8 +65,8 @@ int main (int argc, char* argv[]) {
// Add a prior on pose x1. This indirectly specifies where the origin is.
// and a prior on the position of the first landmark to fix the scale
graph.emplace_shared<PriorFactor<SfmCamera> >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
graph.emplace_shared<PriorFactor<Point3> > (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
graph.addPrior(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
graph.addPrior(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
// Create initial estimate
Values initial;

52
examples/SFMExample_bal_COLAMD_METIS.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file SFMExample.cpp
* @file SFMExample_bal_COLAMD_METIS.cpp
* @brief This file is to compare the ordering performance for COLAMD vs METIS.
* Example problem is to solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file.
* @author Frank Dellaert, Zhaoyang Lv
@ -21,9 +21,8 @@
#include <gtsam/inference/Ordering.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
#include <gtsam/base/timing.h>
@ -35,50 +34,52 @@ using symbol_shorthand::C;
using symbol_shorthand::P;
// We will be using a projection factor that ties a SFM_Camera to a 3D point.
// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
// and has a total of 9 free parameters
typedef GeneralSFMFactor<SfmCamera,Point3> MyFactor;
// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler
// calibration and has a total of 9 free parameters
typedef GeneralSFMFactor<SfmCamera, Point3> MyFactor;
/* ************************************************************************* */
int main (int argc, char* argv[]) {
int main(int argc, char* argv[]) {
// Find default file, but if an argument is given, try loading a file
string filename = findExampleDataFile("dubrovnik-3-7-pre");
if (argc>1) filename = string(argv[1]);
if (argc > 1) filename = string(argv[1]);
// Load the SfM data from file
SfmData mydata;
readBAL(filename, mydata);
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
cout << boost::format("read %1% tracks on %2% cameras\n") %
mydata.number_tracks() % mydata.number_cameras();
// Create a factor graph
NonlinearFactorGraph graph;
// We share *one* noiseModel between all projection factors
noiseModel::Isotropic::shared_ptr noise =
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
auto noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Add measurements to the factor graph
size_t j = 0;
for(const SfmTrack& track: mydata.tracks) {
for(const SfmMeasurement& m: track.measurements) {
for (const SfmTrack& track : mydata.tracks) {
for (const SfmMeasurement& m : track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P
graph.emplace_shared<MyFactor>(
uv, noise, C(i), P(j)); // note use of shorthand symbols C and P
}
j += 1;
}
// Add a prior on pose x1. This indirectly specifies where the origin is.
// and a prior on the position of the first landmark to fix the scale
graph.emplace_shared<PriorFactor<SfmCamera> >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
graph.emplace_shared<PriorFactor<Point3> >(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
graph.addPrior(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
graph.addPrior(P(0), mydata.tracks[0].p,
noiseModel::Isotropic::Sigma(3, 0.1));
// Create initial estimate
Values initial;
size_t i = 0; j = 0;
for(const SfmCamera& camera: mydata.cameras) initial.insert(C(i++), camera);
for(const SfmTrack& track: mydata.tracks) initial.insert(P(j++), track.p);
size_t i = 0;
j = 0;
for (const SfmCamera& camera : mydata.cameras) initial.insert(C(i++), camera);
for (const SfmTrack& track : mydata.tracks) initial.insert(P(j++), track.p);
/** --------------- COMPARISON -----------------------**/
/** ----------------------------------------------------**/
@ -99,7 +100,7 @@ int main (int argc, char* argv[]) {
}
// expect they have different ordering results
if(params_using_COLAMD.ordering == params_using_METIS.ordering) {
if (params_using_COLAMD.ordering == params_using_METIS.ordering) {
cout << "COLAMD and METIS produce the same ordering. "
<< "Problem here!!!" << endl;
}
@ -121,8 +122,7 @@ int main (int argc, char* argv[]) {
cout << e.what();
}
{ // printing the result
{ // printing the result
cout << "COLAMD final error: " << graph.error(result_COLAMD) << endl;
cout << "METIS final error: " << graph.error(result_METIS) << endl;
@ -130,15 +130,13 @@ int main (int argc, char* argv[]) {
cout << endl << endl;
cout << "Time comparison by solving " << filename << " results:" << endl;
cout << boost::format("%1% point tracks and %2% cameras\n") \
% mydata.number_tracks() % mydata.number_cameras() \
cout << boost::format("%1% point tracks and %2% cameras\n") %
mydata.number_tracks() % mydata.number_cameras()
<< endl;
tictoc_print_();
}
return 0;
}
/* ************************************************************************* */

45
examples/SelfCalibrationExample.cpp
View File

@ -33,8 +33,7 @@
#include <gtsam/nonlinear/Values.h>
// SFM-specific factors
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/GeneralSFMFactor.h> // does calibration !
#include <gtsam/slam/GeneralSFMFactor.h> // does calibration !
// Standard headers
#include <vector>
@ -42,9 +41,7 @@
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Create the set of ground-truth
vector<Point3> points = createPoints();
vector<Pose3> poses = createPoses();
@ -53,38 +50,51 @@ int main(int argc, char* argv[]) {
NonlinearFactorGraph graph;
// Add a prior on pose x1.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise);
auto poseNoise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
.finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.addPrior(Symbol('x', 0), poses[0], poseNoise);
// Simulated measurements from each camera pose, adding them to the factor graph
// Simulated measurements from each camera pose, adding them to the factor
// graph
Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
auto measurementNoise =
noiseModel::Isotropic::Sigma(2, 1.0);
for (size_t i = 0; i < poses.size(); ++i) {
for (size_t j = 0; j < points.size(); ++j) {
PinholeCamera<Cal3_S2> camera(poses[i], K);
Point2 measurement = camera.project(points[j]);
// The only real difference with the Visual SLAM example is that here we use a
// different factor type, that also calculates the Jacobian with respect to calibration
graph.emplace_shared<GeneralSFMFactor2<Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0));
// The only real difference with the Visual SLAM example is that here we
// use a different factor type, that also calculates the Jacobian with
// respect to calibration
graph.emplace_shared<GeneralSFMFactor2<Cal3_S2> >(
measurement, measurementNoise, Symbol('x', i), Symbol('l', j),
Symbol('K', 0));
}
}
// Add a prior on the position of the first landmark.
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise); // add directly to graph
auto pointNoise =
noiseModel::Isotropic::Sigma(3, 0.1);
graph.addPrior(Symbol('l', 0), points[0],
pointNoise); // add directly to graph
// Add a prior on the calibration.
noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100).finished());
graph.emplace_shared<PriorFactor<Cal3_S2> >(Symbol('K', 0), K, calNoise);
auto calNoise = noiseModel::Diagonal::Sigmas(
(Vector(5) << 500, 500, 0.1, 100, 100).finished());
graph.addPrior(Symbol('K', 0), K, calNoise);
// Create the initial estimate to the solution
// now including an estimate on the camera calibration parameters
Values initialEstimate;
initialEstimate.insert(Symbol('K', 0), Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0));
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
initialEstimate.insert(
Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25),
Point3(0.05, -0.10, 0.20))));
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
initialEstimate.insert<Point3>(Symbol('l', j),
points[j] + Point3(-0.25, 0.20, 0.15));
/* Optimize the graph and print results */
Values result = DoglegOptimizer(graph, initialEstimate).optimize();
@ -92,5 +102,4 @@ int main(int argc, char* argv[]) {
return 0;
}
/* ************************************************************************* */

12
examples/SimpleRotation.cpp
View File

@ -32,8 +32,8 @@
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
// We will apply a simple prior on the rotation
#include <gtsam/slam/PriorFactor.h>
// We will apply a simple prior on the rotation. We do so via the `addPrior` convenience
// method in NonlinearFactorGraph.
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
// are nonlinear factors, we will need a Nonlinear Factor Graph.
@ -59,7 +59,6 @@ using namespace gtsam;
const double degree = M_PI / 180;
int main() {
/**
* Step 1: Create a factor to express a unary constraint
* The "prior" in this case is the measurement from a sensor,
@ -76,9 +75,8 @@ int main() {
*/
Rot2 prior = Rot2::fromAngle(30 * degree);
prior.print("goal angle");
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree);
Symbol key('x',1);
PriorFactor<Rot2> factor(key, prior, model);
auto model = noiseModel::Isotropic::Sigma(1, 1 * degree);
Symbol key('x', 1);
/**
* Step 2: Create a graph container and add the factor to it
@ -90,7 +88,7 @@ int main() {
* many more factors would be added.
*/
NonlinearFactorGraph graph;
graph.push_back(factor);
graph.addPrior(key, prior, model);
graph.print("full graph");
/**

25
examples/SolverComparer.cpp
View File

@ -34,7 +34,6 @@
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
@ -58,9 +57,8 @@
#include <iostream>
#ifdef GTSAM_USE_TBB
#include <tbb/tbb.h>
#undef max // TBB seems to include windows.h and we don't want these macros
#undef min
#include <tbb/task_arena.h> // tbb::task_arena
#include <tbb/task_group.h> // tbb::task_group
#endif
using namespace std;
@ -206,10 +204,11 @@ int main(int argc, char *argv[]) {
}
#ifdef GTSAM_USE_TBB
std::unique_ptr<tbb::task_scheduler_init> init;
tbb::task_arena arena;
tbb::task_group tg;
if(nThreads > 0) {
cout << "Using " << nThreads << " threads" << endl;
init.reset(new tbb::task_scheduler_init(nThreads));
arena.initialize(nThreads);
} else
cout << "Using threads for all processors" << endl;
#else
@ -219,6 +218,10 @@ int main(int argc, char *argv[]) {
}
#endif
#ifdef GTSAM_USE_TBB
arena.execute([&]{
tg.run_and_wait([&]{
#endif
// Run mode
if(incremental)
runIncremental();
@ -230,6 +233,10 @@ int main(int argc, char *argv[]) {
runPerturb();
else if(stats)
runStats();
#ifdef GTSAM_USE_TBB
});
});
#endif
return 0;
}
@ -248,7 +255,7 @@ void runIncremental()
cout << "Looking for first measurement from step " << firstStep << endl;
size_t nextMeasurement = 0;
bool havePreviousPose = false;
Key firstPose;
Key firstPose = 0;
while(nextMeasurement < datasetMeasurements.size())
{
if(BetweenFactor<Pose>::shared_ptr factor =
@ -281,7 +288,7 @@ void runIncremental()
NonlinearFactorGraph newFactors;
Values newVariables;
newFactors.push_back(boost::make_shared<PriorFactor<Pose> >(firstPose, Pose(), noiseModel::Unit::Create(3)));
newFactors.addPrior(firstPose, Pose(), noiseModel::Unit::Create(3));
newVariables.insert(firstPose, Pose());
isam2.update(newFactors, newVariables);
@ -464,7 +471,7 @@ void runBatch()
cout << "Creating batch optimizer..." << endl;
NonlinearFactorGraph measurements = datasetMeasurements;
measurements.push_back(boost::make_shared<PriorFactor<Pose> >(0, Pose(), noiseModel::Unit::Create(3)));
measurements.addPrior(0, Pose(), noiseModel::Unit::Create(3));
gttic_(Create_optimizer);
GaussNewtonParams params;

23
examples/StereoVOExample.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file SteroVOExample.cpp
* @file StereoVOExample.cpp
* @brief A stereo visual odometry example
* @date May 25, 2014
* @author Stephen Camp
@ -34,17 +34,17 @@
using namespace std;
using namespace gtsam;
int main(int argc, char** argv){
//create graph object, add first pose at origin with key '1'
int main(int argc, char** argv) {
// create graph object, add first pose at origin with key '1'
NonlinearFactorGraph graph;
Pose3 first_pose;
graph.emplace_shared<NonlinearEquality<Pose3> >(1, Pose3());
//create factor noise model with 3 sigmas of value 1
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
//create stereo camera calibration object with .2m between cameras
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
// create factor noise model with 3 sigmas of value 1
const auto model = noiseModel::Isotropic::Sigma(3, 1);
// create stereo camera calibration object with .2m between cameras
const Cal3_S2Stereo::shared_ptr K(
new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
//create and add stereo factors between first pose (key value 1) and the three landmarks
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(520, 480, 440), model, 1, 3, K);
@ -56,17 +56,18 @@ int main(int argc, char** argv){
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(70, 20, 490), model, 2, 4, K);
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(320, 270, 115), model, 2, 5, K);
//create Values object to contain initial estimates of camera poses and landmark locations
// create Values object to contain initial estimates of camera poses and
// landmark locations
Values initial_estimate;
//create and add iniital estimates
// create and add iniital estimates
initial_estimate.insert(1, first_pose);
initial_estimate.insert(2, Pose3(Rot3(), Point3(0.1, -0.1, 1.1)));
initial_estimate.insert(3, Point3(1, 1, 5));
initial_estimate.insert(4, Point3(-1, 1, 5));
initial_estimate.insert(5, Point3(0, -0.5, 5));
//create Levenberg-Marquardt optimizer for resulting factor graph, optimize
// create Levenberg-Marquardt optimizer for resulting factor graph, optimize
LevenbergMarquardtOptimizer optimizer(graph, initial_estimate);
Values result = optimizer.optimize();

46
examples/StereoVOExample_large.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file SteroVOExample.cpp
* @file StereoVOExample_large.cpp
* @brief A stereo visual odometry example
* @date May 25, 2014
* @author Stephen Camp
@ -41,31 +41,31 @@
using namespace std;
using namespace gtsam;
int main(int argc, char** argv){
int main(int argc, char** argv) {
Values initial_estimate;
NonlinearFactorGraph graph;
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
const auto model = noiseModel::Isotropic::Sigma(3, 1);
string calibration_loc = findExampleDataFile("VO_calibration.txt");
string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
//read camera calibration info from file
// read camera calibration info from file
// focal lengths fx, fy, skew s, principal point u0, v0, baseline b
double fx, fy, s, u0, v0, b;
ifstream calibration_file(calibration_loc.c_str());
cout << "Reading calibration info" << endl;
calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
//create stereo camera calibration object
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b));
// create stereo camera calibration object
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, s, u0, v0, b));
ifstream pose_file(pose_loc.c_str());
cout << "Reading camera poses" << endl;
int pose_id;
MatrixRowMajor m(4,4);
//read camera pose parameters and use to make initial estimates of camera poses
MatrixRowMajor m(4, 4);
// read camera pose parameters and use to make initial estimates of camera
// poses
while (pose_file >> pose_id) {
for (int i = 0; i < 16; i++) {
pose_file >> m.data()[i];
@ -76,33 +76,37 @@ int main(int argc, char** argv){
// camera and landmark keys
size_t x, l;
// pixel coordinates uL, uR, v (same for left/right images due to rectification)
// landmark coordinates X, Y, Z in camera frame, resulting from triangulation
// pixel coordinates uL, uR, v (same for left/right images due to
// rectification) landmark coordinates X, Y, Z in camera frame, resulting from
// triangulation
double uL, uR, v, X, Y, Z;
ifstream factor_file(factor_loc.c_str());
cout << "Reading stereo factors" << endl;
//read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
// read stereo measurement details from file and use to create and add
// GenericStereoFactor objects to the graph representation
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
graph.emplace_shared<
GenericStereoFactor<Pose3, Point3> >(StereoPoint2(uL, uR, v), model,
Symbol('x', x), Symbol('l', l), K);
//if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
graph.emplace_shared<GenericStereoFactor<Pose3, Point3> >(
StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K);
// if the landmark variable included in this factor has not yet been added
// to the initial variable value estimate, add it
if (!initial_estimate.exists(Symbol('l', l))) {
Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
//transformFrom() transforms the input Point3 from the camera pose space, camPose, to the global space
// transformFrom() transforms the input Point3 from the camera pose space,
// camPose, to the global space
Point3 worldPoint = camPose.transformFrom(Point3(X, Y, Z));
initial_estimate.insert(Symbol('l', l), worldPoint);
}
}
Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));
//constrain the first pose such that it cannot change from its original value during optimization
Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x', 1));
// constrain the first pose such that it cannot change from its original value
// during optimization
// NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
// QR is much slower than Cholesky, but numerically more stable
graph.emplace_shared<NonlinearEquality<Pose3> >(Symbol('x',1),first_pose);
graph.emplace_shared<NonlinearEquality<Pose3> >(Symbol('x', 1), first_pose);
cout << "Optimizing" << endl;
//create Levenberg-Marquardt optimizer to optimize the factor graph
// create Levenberg-Marquardt optimizer to optimize the factor graph
LevenbergMarquardtParams params;
params.orderingType = Ordering::METIS;
LevenbergMarquardtOptimizer optimizer(graph, initial_estimate, params);

44
examples/TimeTBB.cpp
View File

@ -28,9 +28,12 @@ using boost::assign::list_of;
#ifdef GTSAM_USE_TBB
#include <tbb/tbb.h>
#undef max // TBB seems to include windows.h and we don't want these macros
#undef min
#include <tbb/blocked_range.h> // tbb::blocked_range
#include <tbb/tick_count.h> // tbb::tick_count
#include <tbb/parallel_for.h> // tbb::parallel_for
#include <tbb/cache_aligned_allocator.h> // tbb::cache_aligned_allocator
#include <tbb/task_arena.h> // tbb::task_arena
#include <tbb/task_group.h> // tbb::task_group
static const DenseIndex numberOfProblems = 1000000;
static const DenseIndex problemSize = 4;
@ -67,10 +70,14 @@ struct WorkerWithoutAllocation
};
/* ************************************************************************* */
map<int, double> testWithoutMemoryAllocation()
map<int, double> testWithoutMemoryAllocation(int num_threads)
{
// A function to do some matrix operations without allocating any memory
// Create task_arena and task_group
tbb::task_arena arena(num_threads);
tbb::task_group tg;
// Now call it
vector<double> results(numberOfProblems);
@ -79,7 +86,14 @@ map<int, double> testWithoutMemoryAllocation()
for(size_t grainSize: grainSizes)
{
tbb::tick_count t0 = tbb::tick_count::now();
tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithoutAllocation(results));
// Run parallel code (as a task group) inside of task arena
arena.execute([&]{
tg.run_and_wait([&]{
tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithoutAllocation(results));
});
});
tbb::tick_count t1 = tbb::tick_count::now();
cout << "Without memory allocation, grain size = " << grainSize << ", time = " << (t1 - t0).seconds() << endl;
timingResults[(int)grainSize] = (t1 - t0).seconds();
@ -120,10 +134,14 @@ struct WorkerWithAllocation
};
/* ************************************************************************* */
map<int, double> testWithMemoryAllocation()
map<int, double> testWithMemoryAllocation(int num_threads)
{
// A function to do some matrix operations with allocating memory
// Create task_arena and task_group
tbb::task_arena arena(num_threads);
tbb::task_group tg;
// Now call it
vector<double> results(numberOfProblems);
@ -132,7 +150,14 @@ map<int, double> testWithMemoryAllocation()
for(size_t grainSize: grainSizes)
{
tbb::tick_count t0 = tbb::tick_count::now();
tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithAllocation(results));
// Run parallel code (as a task group) inside of task arena
arena.execute([&]{
tg.run_and_wait([&]{
tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithAllocation(results));
});
});
tbb::tick_count t1 = tbb::tick_count::now();
cout << "With memory allocation, grain size = " << grainSize << ", time = " << (t1 - t0).seconds() << endl;
timingResults[(int)grainSize] = (t1 - t0).seconds();
@ -153,9 +178,8 @@ int main(int argc, char* argv[])
for(size_t n: numThreads)
{
cout << "With " << n << " threads:" << endl;
tbb::task_scheduler_init init((int)n);
results[(int)n].grainSizesWithoutAllocation = testWithoutMemoryAllocation();
results[(int)n].grainSizesWithAllocation = testWithMemoryAllocation();
results[(int)n].grainSizesWithoutAllocation = testWithoutMemoryAllocation((int)n);
results[(int)n].grainSizesWithAllocation = testWithMemoryAllocation((int)n);
cout << endl;
}

74
examples/UGM_chain.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file small.cpp
* @file UGM_chain.cpp
* @brief UGM (undirected graphical model) examples: chain
* @author Frank Dellaert
* @author Abhijit Kundu
@ -19,10 +19,9 @@
* for more explanation. This code demos the same example using GTSAM.
*/
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteSequentialSolver.h>
#include <gtsam/discrete/DiscreteMarginals.h>
#include <gtsam/base/timing.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteMarginals.h>
#include <iomanip>
@ -30,9 +29,8 @@ using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
// Set Number of Nodes in the Graph
const int nrNodes = 60;
// Set Number of Nodes in the Graph
const int nrNodes = 60;
// Each node takes 1 of 7 possible states denoted by 0-6 in following order:
// ["VideoGames" "Industry" "GradSchool" "VideoGames(with PhD)"
@ -40,70 +38,55 @@ int main(int argc, char** argv) {
const size_t nrStates = 7;
// define variables
vector<DiscreteKey> nodes;
for (int i = 0; i < nrNodes; i++){
DiscreteKey dk(i, nrStates);
nodes.push_back(dk);
}
vector<DiscreteKey> nodes;
for (int i = 0; i < nrNodes; i++) {
DiscreteKey dk(i, nrStates);
nodes.push_back(dk);
}
// create graph
DiscreteFactorGraph graph;
// add node potentials
graph.add(nodes[0], ".3 .6 .1 0 0 0 0");
for (int i = 1; i < nrNodes; i++)
graph.add(nodes[i], "1 1 1 1 1 1 1");
for (int i = 1; i < nrNodes; i++) graph.add(nodes[i], "1 1 1 1 1 1 1");
const std::string edgePotential = ".08 .9 .01 0 0 0 .01 "
".03 .95 .01 0 0 0 .01 "
".06 .06 .75 .05 .05 .02 .01 "
"0 0 0 .3 .6 .09 .01 "
"0 0 0 .02 .95 .02 .01 "
"0 0 0 .01 .01 .97 .01 "
"0 0 0 0 0 0 1";
const std::string edgePotential =
".08 .9 .01 0 0 0 .01 "
".03 .95 .01 0 0 0 .01 "
".06 .06 .75 .05 .05 .02 .01 "
"0 0 0 .3 .6 .09 .01 "
"0 0 0 .02 .95 .02 .01 "
"0 0 0 .01 .01 .97 .01 "
"0 0 0 0 0 0 1";
// add edge potentials
for (int i = 0; i < nrNodes - 1; i++)
graph.add(nodes[i] & nodes[i + 1], edgePotential);
cout << "Created Factor Graph with " << nrNodes << " variable nodes and "
<< graph.size() << " factors (Unary+Edge).";
<< graph.size() << " factors (Unary+Edge).";
// "Decoding", i.e., configuration with largest value
// We use sequential variable elimination
DiscreteSequentialSolver solver(graph);
DiscreteFactor::sharedValues optimalDecoding = solver.optimize();
DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential();
DiscreteFactor::sharedValues optimalDecoding = chordal->optimize();
optimalDecoding->print("\nMost Probable Explanation (optimalDecoding)\n");
// "Inference" Computing marginals for each node
cout << "\nComputing Node Marginals ..(Sequential Elimination)" << endl;
gttic_(Sequential);
for (vector<DiscreteKey>::iterator itr = nodes.begin(); itr != nodes.end();
++itr) {
//Compute the marginal
Vector margProbs = solver.marginalProbabilities(*itr);
//Print the marginals
cout << "Node#" << setw(4) << itr->first << " : ";
print(margProbs);
}
gttoc_(Sequential);
// Here we'll make use of DiscreteMarginals class, which makes use of
// bayes-tree based shortcut evaluation of marginals
DiscreteMarginals marginals(graph);
cout << "\nComputing Node Marginals ..(BayesTree based)" << endl;
gttic_(Multifrontal);
for (vector<DiscreteKey>::iterator itr = nodes.begin(); itr != nodes.end();
++itr) {
//Compute the marginal
Vector margProbs = marginals.marginalProbabilities(*itr);
for (vector<DiscreteKey>::iterator it = nodes.begin(); it != nodes.end();
++it) {
// Compute the marginal
Vector margProbs = marginals.marginalProbabilities(*it);
//Print the marginals
cout << "Node#" << setw(4) << itr->first << " : ";
// Print the marginals
cout << "Node#" << setw(4) << it->first << " : ";
print(margProbs);
}
gttoc_(Multifrontal);
@ -111,4 +94,3 @@ int main(int argc, char** argv) {
tictoc_print_();
return 0;
}

19
examples/UGM_small.cpp
View File

@ -10,15 +10,16 @@
* -------------------------------------------------------------------------- */
/**
* @file small.cpp
* @file UGM_small.cpp
* @brief UGM (undirected graphical model) examples: small
* @author Frank Dellaert
*
* See http://www.di.ens.fr/~mschmidt/Software/UGM/small.html
*/
#include <gtsam/base/Vector.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteSequentialSolver.h>
#include <gtsam/discrete/DiscreteMarginals.h>
using namespace std;
using namespace gtsam;
@ -61,24 +62,24 @@ int main(int argc, char** argv) {
// "Decoding", i.e., configuration with largest value (MPE)
// We use sequential variable elimination
DiscreteSequentialSolver solver(graph);
DiscreteFactor::sharedValues optimalDecoding = solver.optimize();
DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential();
DiscreteFactor::sharedValues optimalDecoding = chordal->optimize();
optimalDecoding->print("\noptimalDecoding");
// "Inference" Computing marginals
cout << "\nComputing Node Marginals .." << endl;
Vector margProbs;
DiscreteMarginals marginals(graph);
margProbs = solver.marginalProbabilities(Cathy);
Vector margProbs = marginals.marginalProbabilities(Cathy);
print(margProbs, "Cathy's Node Marginal:");
margProbs = solver.marginalProbabilities(Heather);
margProbs = marginals.marginalProbabilities(Heather);
print(margProbs, "Heather's Node Marginal");
margProbs = solver.marginalProbabilities(Mark);
margProbs = marginals.marginalProbabilities(Mark);
print(margProbs, "Mark's Node Marginal");
margProbs = solver.marginalProbabilities(Allison);
margProbs = marginals.marginalProbabilities(Allison);
print(margProbs, "Allison's Node Marginal");
return 0;

7
examples/VisualISAM2Example.cpp
View File

@ -47,7 +47,6 @@
// Adjustment problems. Here we will use Projection factors to model the
// camera's landmark observations. Also, we will initialize the robot at some
// location using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <vector>
@ -110,13 +109,11 @@ int main(int argc, char* argv[]) {
static auto kPosePrior = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
.finished());
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0],
kPosePrior);
graph.addPrior(Symbol('x', 0), poses[0], kPosePrior);
// Add a prior on landmark l0
static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0],
kPointPrior);
graph.addPrior(Symbol('l', 0), points[0], kPointPrior);
// Add initial guesses to all observed landmarks
// Intentionally initialize the variables off from the ground truth

14
examples/VisualISAMExample.cpp
View File

@ -38,7 +38,6 @@
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
// Here we will use Projection factors to model the camera's landmark observations.
// Also, we will initialize the robot at some location using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
// We want to use iSAM to solve the structure-from-motion problem incrementally, so
@ -57,13 +56,11 @@ using namespace gtsam;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Define the camera calibration parameters
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
// Define the camera observation noise model
noiseModel::Isotropic::shared_ptr noise = //
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
auto noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks
vector<Point3> points = createPoints();
@ -82,7 +79,6 @@ int main(int argc, char* argv[]) {
// Loop over the different poses, adding the observations to iSAM incrementally
for (size_t i = 0; i < poses.size(); ++i) {
// Add factors for each landmark observation
for (size_t j = 0; j < points.size(); ++j) {
// Create ground truth measurement
@ -106,14 +102,14 @@ int main(int argc, char* argv[]) {
// adding it to iSAM.
if (i == 0) {
// Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
auto poseNoise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise);
graph.addPrior(Symbol('x', 0), poses[0], poseNoise);
// Add a prior on landmark l0
noiseModel::Isotropic::shared_ptr pointNoise =
auto pointNoise =
noiseModel::Isotropic::Sigma(3, 0.1);
graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise);
graph.addPrior(Symbol('l', 0), points[0], pointNoise);
// Add initial guesses to all observed landmarks
Point3 noise(-0.25, 0.20, 0.15);

2
examples/easyPoint2KalmanFilter.cpp
View File

@ -23,7 +23,7 @@
#include <gtsam/nonlinear/ExtendedKalmanFilter.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Point2.h>

2
examples/elaboratePoint2KalmanFilter.cpp
View File

@ -20,7 +20,7 @@
* @author Stephen Williams
*/
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
//#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/inference/Symbol.h>

127
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;
};
@ -598,6 +598,7 @@ class SOn {
// Standard Constructors
SOn(size_t n);
static gtsam::SOn FromMatrix(Matrix R);
static gtsam::SOn Lift(size_t n, Matrix R);
// Testable
void print(string s) const;
@ -1458,7 +1459,7 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base {
void serializable() const;
double weight(double error) const;
double residual(double error) const;
double loss(double error) const;
};
virtual class Fair: gtsam::noiseModel::mEstimator::Base {
@ -1469,7 +1470,7 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base {
void serializable() const;
double weight(double error) const;
double residual(double error) const;
double loss(double error) const;
};
virtual class Huber: gtsam::noiseModel::mEstimator::Base {
@ -1480,7 +1481,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base {
void serializable() const;
double weight(double error) const;
double residual(double error) const;
double loss(double error) const;
};
virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
@ -1491,7 +1492,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
void serializable() const;
double weight(double error) const;
double residual(double error) const;
double loss(double error) const;
};
virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
@ -1502,7 +1503,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
void serializable() const;
double weight(double error) const;
double residual(double error) const;
double loss(double error) const;
};
virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
@ -1513,7 +1514,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
void serializable() const;
double weight(double error) const;
double residual(double error) const;
double loss(double error) const;
};
virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
@ -1524,7 +1525,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
void serializable() const;
double weight(double error) const;
double residual(double error) const;
double loss(double error) const;
};
virtual class DCS: gtsam::noiseModel::mEstimator::Base {
@ -1535,7 +1536,7 @@ virtual class DCS: gtsam::noiseModel::mEstimator::Base {
void serializable() const;
double weight(double error) const;
double residual(double error) const;
double loss(double error) const;
};
virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base {
@ -1546,7 +1547,7 @@ virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base {
void serializable() const;
double weight(double error) const;
double residual(double error) const;
double loss(double error) const;
};
}///\namespace mEstimator
@ -1563,17 +1564,15 @@ virtual class Robust : gtsam::noiseModel::Base {
#include <gtsam/linear/Sampler.h>
class Sampler {
//Constructors
// Constructors
Sampler(gtsam::noiseModel::Diagonal* model, int seed);
Sampler(Vector sigmas, int seed);
Sampler(int seed);
//Standard Interface
// Standard Interface
size_t dim() const;
Vector sigmas() const;
gtsam::noiseModel::Diagonal* model() const;
Vector sample();
Vector sampleNewModel(gtsam::noiseModel::Diagonal* model);
};
#include <gtsam/linear/VectorValues.h>
@ -1939,6 +1938,22 @@ virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParamete
void print() const;
};
#include <gtsam/linear/Preconditioner.h>
virtual class PreconditionerParameters {
PreconditionerParameters();
};
virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters {
DummyPreconditionerParameters();
};
#include <gtsam/linear/PCGSolver.h>
virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters {
PCGSolverParameters();
void print(string s);
void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner);
};
#include <gtsam/linear/SubgraphSolver.h>
virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
SubgraphSolverParameters();
@ -1979,6 +1994,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);
@ -2052,6 +2096,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;
@ -2140,6 +2187,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);
@ -2157,10 +2205,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
@ -2260,8 +2309,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 {
@ -2496,8 +2547,8 @@ class NonlinearISAM {
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/slam/PriorFactor.h>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
#include <gtsam/nonlinear/PriorFactor.h>
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;
@ -2520,7 +2571,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);
@ -2761,8 +2812,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>
@ -2799,6 +2852,34 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
KarcherMeanFactor(const gtsam::KeyVector& keys);
};
#include <gtsam/slam/FrobeniusFactor.h>
gtsam::noiseModel::Isotropic* ConvertPose3NoiseModel(
gtsam::noiseModel::Base* model, size_t d);
template<T = {gtsam::SO3, gtsam::SO4}>
virtual class FrobeniusFactor : gtsam::NoiseModelFactor {
FrobeniusFactor(size_t key1, size_t key2);
FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model);
Vector evaluateError(const T& R1, const T& R2);
};
template<T = {gtsam::SO3, gtsam::SO4}>
virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor {
FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12);
FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12, gtsam::noiseModel::Base* model);
Vector evaluateError(const T& R1, const T& R2);
};
virtual class FrobeniusWormholeFactor : gtsam::NoiseModelFactor {
FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12,
size_t p);
FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12,
size_t p, gtsam::noiseModel::Base* model);
Vector evaluateError(const gtsam::SOn& Q1, const gtsam::SOn& Q2);
};
//*************************************************************************
// Navigation
//*************************************************************************
@ -2912,11 +2993,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;
@ -2971,11 +3055,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})

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