Merge branch 'develop' into feature/improvementsIncrementalFilter
lcarlone
commit
570f41409c
|
|
@ -15,3 +15,7 @@ cython/gtsam.cpython-35m-darwin.so
|
|||
cython/gtsam.pyx
|
||||
cython/gtsam.so
|
||||
cython/gtsam_wrapper.pxd
|
||||
.vscode
|
||||
.env
|
||||
/.vs/
|
||||
/CMakeSettings.json
|
||||
|
|
|
|||
|
|
@ -111,17 +111,23 @@ set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator")
|
|||
# BOOST_ROOT: path to install prefix for boost
|
||||
# Boost_NO_SYSTEM_PATHS: set to true to keep the find script from ignoring BOOST_ROOT
|
||||
|
||||
# If using Boost shared libs, disable auto linking
|
||||
if(MSVC)
|
||||
# Some libraries, at least Boost Program Options, rely on this to export DLL symbols
|
||||
# Disable autolinking
|
||||
# By default, boost only builds static libraries on windows
|
||||
set(Boost_USE_STATIC_LIBS ON) # only find static libs
|
||||
# If we ever reset above on windows and, ...
|
||||
# If we use Boost shared libs, disable auto linking.
|
||||
# Some libraries, at least Boost Program Options, rely on this to export DLL symbols.
|
||||
if(NOT Boost_USE_STATIC_LIBS)
|
||||
add_definitions(-DBOOST_ALL_NO_LIB)
|
||||
add_definitions(-DBOOST_ALL_DYN_LINK)
|
||||
endif()
|
||||
# Virtual memory range for PCH exceeded on VS2015
|
||||
if(MSVC_VERSION LESS 1910) # older than VS2017
|
||||
add_definitions(-Zm295)
|
||||
endif()
|
||||
endif()
|
||||
|
||||
find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time timer chrono)
|
||||
find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex)
|
||||
|
||||
# Required components
|
||||
if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR
|
||||
|
|
@ -133,7 +139,7 @@ option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF)
|
|||
# Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library)
|
||||
set(GTSAM_BOOST_LIBRARIES
|
||||
${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY}
|
||||
${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY})
|
||||
${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY} ${Boost_REGEX_LIBRARY})
|
||||
if (GTSAM_DISABLE_NEW_TIMERS)
|
||||
message("WARNING: GTSAM timing instrumentation manually disabled")
|
||||
add_definitions(-DGTSAM_DISABLE_NEW_TIMERS)
|
||||
|
|
@ -240,6 +246,13 @@ if(GTSAM_USE_SYSTEM_EIGEN)
|
|||
if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_LESS 3.2.5))
|
||||
message(FATAL_ERROR "MKL requires at least Eigen 3.2.5, and your system appears to have an older version. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, or disable GTSAM_WITH_EIGEN_MKL")
|
||||
endif()
|
||||
|
||||
# Check for Eigen version which doesn't work with MKL
|
||||
# See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527 for details.
|
||||
if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_EQUAL 3.3.4))
|
||||
message(FATAL_ERROR "MKL does not work with Eigen 3.3.4 because of a bug in Eigen. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, disable GTSAM_WITH_EIGEN_MKL, or upgrade/patch your installation of Eigen.")
|
||||
endif()
|
||||
|
||||
else()
|
||||
# Use bundled Eigen include path.
|
||||
# Clear any variables set by FindEigen3
|
||||
|
|
@ -256,6 +269,14 @@ else()
|
|||
|
||||
endif()
|
||||
|
||||
if (MSVC)
|
||||
if (NOT GTSAM_BUILD_STATIC_LIBRARY)
|
||||
# mute eigen static assert to avoid errors in shared lib
|
||||
add_definitions(-DEIGEN_NO_STATIC_ASSERT)
|
||||
endif()
|
||||
add_definitions(/wd4244) # Disable loss of precision which is thrown all over our Eigen
|
||||
endif()
|
||||
|
||||
###############################################################################
|
||||
# Global compile options
|
||||
|
||||
|
|
@ -321,6 +342,7 @@ include_directories(BEFORE
|
|||
if(MSVC)
|
||||
add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_SCL_SECURE_NO_WARNINGS)
|
||||
add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings
|
||||
add_definitions(/bigobj) # Allow large object files for template-based code
|
||||
endif()
|
||||
|
||||
# GCC 4.8+ complains about local typedefs which we use for shared_ptr etc.
|
||||
|
|
|
|||
|
|
@ -62,10 +62,11 @@ If you are using the factor in academic work, please cite the publications above
|
|||
In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in docs/ImuFactor.pdf, is enabled by default. To switch to the RSS 2015 version, set the flag **GTSAM_TANGENT_PREINTEGRATION** to OFF.
|
||||
|
||||
|
||||
|
||||
Additional Information
|
||||
----------------------
|
||||
|
||||
There is a [`GTSAM users Google group`](https://groups.google.com/forum/#!forum/gtsam-users) for general discussion.
|
||||
|
||||
Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. A primer on GTSAM Expressions,
|
||||
which support (superfast) automatic differentiation,
|
||||
can be found on the [GTSAM wiki on BitBucket](https://bitbucket.org/gtborg/gtsam/wiki/Home).
|
||||
|
|
|
|||
|
|
@ -1,9 +1,8 @@
|
|||
# This is a sample build configuration for C++ – Make.
|
||||
# Check our guides at https://confluence.atlassian.com/x/5Q4SMw for more examples.
|
||||
# Only use spaces to indent your .yml configuration.
|
||||
# Built from sample configuration for C++ – Make.
|
||||
# Check https://confluence.atlassian.com/x/5Q4SMw for more examples.
|
||||
# -----
|
||||
# You can specify a custom docker image from Docker Hub as your build environment.
|
||||
image: ilssim/cmake-boost-qt5
|
||||
# Our custom docker image from Docker Hub as the build environment.
|
||||
image: dellaert/ubuntu-boost-tbb-eigen3:bionic
|
||||
|
||||
pipelines:
|
||||
default:
|
||||
|
|
@ -11,6 +10,6 @@ pipelines:
|
|||
script: # Modify the commands below to build your repository.
|
||||
- mkdir build
|
||||
- cd build
|
||||
- cmake ..
|
||||
- make
|
||||
- make check
|
||||
- cmake -DGTSAM_USE_SYSTEM_EIGEN=OFF -DGTSAM_USE_EIGEN_MKL=OFF ..
|
||||
- make -j2
|
||||
- make -j2 check
|
||||
|
|
@ -83,10 +83,20 @@ FIND_PATH(MKL_FFTW_INCLUDE_DIR
|
|||
NO_DEFAULT_PATH
|
||||
)
|
||||
|
||||
IF(WIN32)
|
||||
IF(WIN32 AND MKL_ROOT_DIR)
|
||||
SET(MKL_LIB_SEARCHPATH $ENV{ICC_LIB_DIR} $ENV{MKL_LIB_DIR} "${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}" "${MKL_ROOT_DIR}/../compiler" "${MKL_ROOT_DIR}/../compiler/lib/${MKL_ARCH_DIR}")
|
||||
|
||||
IF (MKL_INCLUDE_DIR MATCHES "10.")
|
||||
IF(MKL_INCLUDE_DIR MATCHES "2017" OR MKL_INCLUDE_DIR MATCHES "2018")
|
||||
IF(CMAKE_CL_64)
|
||||
SET(MKL_LIBS mkl_core mkl_intel_lp64 mkl_lapack95_lp64 mkl_blas95_lp64)
|
||||
ELSE()
|
||||
SET(MKL_LIBS mkl_core mkl_intel_s mkl_lapack95 mkl_blas95)
|
||||
ENDIF()
|
||||
IF(TBB_FOUND AND GTSAM_WITH_TBB)
|
||||
SET(MKL_LIBS ${MKL_LIBS} mkl_tbb_thread)
|
||||
ELSE()
|
||||
SET(MKL_LIBS ${MKL_LIBS} mkl_intel_thread libiomp5md)
|
||||
ENDIF()
|
||||
ELSEIF(MKL_INCLUDE_DIR MATCHES "10.")
|
||||
IF(CMAKE_CL_64)
|
||||
SET(MKL_LIBS mkl_solver_lp64 mkl_core mkl_intel_lp64 mkl_intel_thread libguide mkl_lapack95_lp64 mkl_blas95_lp64)
|
||||
ELSE()
|
||||
|
|
@ -115,7 +125,7 @@ IF(WIN32)
|
|||
ENDIF()
|
||||
ENDFOREACH()
|
||||
SET(MKL_FOUND ON)
|
||||
ELSE() # UNIX and macOS
|
||||
ELSEIF(MKL_ROOT_DIR) # UNIX and macOS
|
||||
FIND_LIBRARY(MKL_CORE_LIBRARY
|
||||
mkl_core
|
||||
PATHS
|
||||
|
|
|
|||
|
|
@ -82,6 +82,10 @@ if (WIN32)
|
|||
set(_TBB_COMPILER "vc11")
|
||||
set(TBB_COMPILER "vc11")
|
||||
endif(MSVC11)
|
||||
if(MSVC14)
|
||||
set(_TBB_COMPILER "vc14")
|
||||
set(TBB_COMPILER "vc14")
|
||||
endif(MSVC14)
|
||||
# Todo: add other Windows compilers such as ICL.
|
||||
if(TBB_ARCHITECTURE)
|
||||
set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE})
|
||||
|
|
|
|||
|
|
@ -0,0 +1,27 @@
|
|||
###############################################################################
|
||||
# Macro:
|
||||
#
|
||||
# gtsamAddPch(precompiledHeader precompiledSource sources)
|
||||
#
|
||||
# Adds a precompiled header to compile all sources with. Currently only on MSVC.
|
||||
# Inspired by https://stackoverflow.com/questions/148570/
|
||||
#
|
||||
# Arguments:
|
||||
# precompiledHeader: the header file that includes headers to be precompiled.
|
||||
# precompiledSource: the source file that simply includes that header above.
|
||||
# sources: the list of source files to apply this to.
|
||||
#
|
||||
macro(gtsamAddPch precompiledHeader precompiledSource sources)
|
||||
get_filename_component(pchBasename ${precompiledHeader} NAME_WE)
|
||||
SET(precompiledBinary "${CMAKE_CURRENT_BINARY_DIR}/${pchBasename}.pch")
|
||||
IF(MSVC)
|
||||
message(STATUS "Adding precompiled header for MSVC")
|
||||
set_source_files_properties(${precompiledSource}
|
||||
PROPERTIES COMPILE_FLAGS "/Yc\"${precompiledHeader}\" /Fp\"${precompiledBinary}\""
|
||||
OBJECT_OUTPUTS "${precompiledBinary}")
|
||||
set_source_files_properties(${sources}
|
||||
PROPERTIES COMPILE_FLAGS "/Yu\"${precompiledHeader}\" /FI\"${precompiledHeader}\" /Fp\"${precompiledBinary}\""
|
||||
OBJECT_DEPENDS "${precompiledBinary}")
|
||||
ENDIF(MSVC)
|
||||
endmacro(gtsamAddPch)
|
||||
|
||||
|
|
@ -52,7 +52,7 @@ function(pyx_to_cpp target pyx_file generated_cpp include_dirs)
|
|||
add_custom_command(
|
||||
OUTPUT ${generated_cpp}
|
||||
COMMAND
|
||||
${CYTHON_EXECUTABLE} -X boundscheck=False -a -v --fast-fail --cplus ${includes_for_cython} ${pyx_file} -o ${generated_cpp}
|
||||
${CYTHON_EXECUTABLE} -X boundscheck=False -v --fast-fail --cplus ${includes_for_cython} ${pyx_file} -o ${generated_cpp}
|
||||
VERBATIM)
|
||||
add_custom_target(${target} ALL DEPENDS ${generated_cpp})
|
||||
endfunction()
|
||||
|
|
@ -95,7 +95,7 @@ function(cythonize target pyx_file output_lib_we output_dir include_dirs libs in
|
|||
|
||||
# Late dependency injection, to make sure this gets called whenever the interface header is updated
|
||||
# See: https://stackoverflow.com/questions/40032593/cmake-does-not-rebuild-dependent-after-prerequisite-changes
|
||||
add_custom_command(OUTPUT ${generated_cpp} DEPENDS ${interface_header} APPEND)
|
||||
add_custom_command(OUTPUT ${generated_cpp} DEPENDS ${interface_header} ${pyx_file} APPEND)
|
||||
if (NOT "${dependencies}" STREQUAL "")
|
||||
add_dependencies(${target}_pyx2cpp "${dependencies}")
|
||||
endif()
|
||||
|
|
|
|||
|
|
@ -26,21 +26,27 @@
|
|||
// class __declspec(dllexport) MyClass { ... };
|
||||
// When included while compiling other code against GTSAM:
|
||||
// class __declspec(dllimport) MyClass { ... };
|
||||
|
||||
#pragma once
|
||||
|
||||
// Whether GTSAM is compiled as static or DLL in windows.
|
||||
// This will be used to decide whether include __declspec(dllimport) or not in headers
|
||||
#cmakedefine GTSAM_BUILD_STATIC_LIBRARY
|
||||
|
||||
#ifdef _WIN32
|
||||
# ifdef @library_name@_EXPORTS
|
||||
# define @library_name@_EXPORT __declspec(dllexport)
|
||||
# define @library_name@_EXTERN_EXPORT __declspec(dllexport) extern
|
||||
# ifdef @library_name@_BUILD_STATIC_LIBRARY
|
||||
# define @library_name@_EXPORT
|
||||
# define @library_name@_EXTERN_EXPORT extern
|
||||
# else
|
||||
# ifndef @library_name@_IMPORT_STATIC
|
||||
# ifdef @library_name@_EXPORTS
|
||||
# define @library_name@_EXPORT __declspec(dllexport)
|
||||
# define @library_name@_EXTERN_EXPORT __declspec(dllexport) extern
|
||||
# else
|
||||
# define @library_name@_EXPORT __declspec(dllimport)
|
||||
# define @library_name@_EXTERN_EXPORT __declspec(dllimport)
|
||||
# else /* @library_name@_IMPORT_STATIC */
|
||||
# define @library_name@_EXPORT
|
||||
# define @library_name@_EXTERN_EXPORT extern
|
||||
# endif /* @library_name@_IMPORT_STATIC */
|
||||
# endif /* @library_name@_EXPORTS */
|
||||
#else /* _WIN32 */
|
||||
# endif
|
||||
# endif
|
||||
#else
|
||||
# define @library_name@_EXPORT
|
||||
# define @library_name@_EXTERN_EXPORT extern
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,176 @@
|
|||
"""
|
||||
iSAM2 example with ImuFactor.
|
||||
Author: Robert Truax (C++), Frank Dellaert (Python)
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
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
|
||||
|
||||
|
||||
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)
|
||||
|
||||
|
||||
def vector3(x, y, z):
|
||||
"""Create 3d double numpy array."""
|
||||
return np.array([x, y, z], dtype=np.float)
|
||||
|
||||
|
||||
def ISAM2_plot(values, fignum=0):
|
||||
"""Plot poses."""
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca(projection='3d')
|
||||
plt.cla()
|
||||
|
||||
i = 0
|
||||
min_bounds = 0, 0, 0
|
||||
max_bounds = 0, 0, 0
|
||||
while values.exists(X(i)):
|
||||
pose_i = values.atPose3(X(i))
|
||||
gtsam_plot.plot_pose3(fignum, pose_i, 10)
|
||||
position = pose_i.translation().vector()
|
||||
min_bounds = [min(v1, v2) for (v1, v2) in zip(position, min_bounds)]
|
||||
max_bounds = [max(v1, v2) for (v1, v2) in zip(position, max_bounds)]
|
||||
# max_bounds = min(pose_i.x(), max_bounds[0]), 0, 0
|
||||
i += 1
|
||||
|
||||
# draw
|
||||
axes.set_xlim3d(min_bounds[0]-1, max_bounds[0]+1)
|
||||
axes.set_ylim3d(min_bounds[1]-1, max_bounds[1]+1)
|
||||
axes.set_zlim3d(min_bounds[2]-1, max_bounds[2]+1)
|
||||
plt.pause(1)
|
||||
|
||||
|
||||
# IMU preintegration parameters
|
||||
# Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
|
||||
g = 9.81
|
||||
n_gravity = vector3(0, 0, -g)
|
||||
PARAMS = gtsam.PreintegrationParams.MakeSharedU(g)
|
||||
I = np.eye(3)
|
||||
PARAMS.setAccelerometerCovariance(I * 0.1)
|
||||
PARAMS.setGyroscopeCovariance(I * 0.1)
|
||||
PARAMS.setIntegrationCovariance(I * 0.1)
|
||||
PARAMS.setUse2ndOrderCoriolis(False)
|
||||
PARAMS.setOmegaCoriolis(vector3(0, 0, 0))
|
||||
|
||||
BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1)
|
||||
DELTA = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0),
|
||||
gtsam.Point3(0.05, -0.10, 0.20))
|
||||
|
||||
|
||||
def IMU_example():
|
||||
"""Run iSAM 2 example with IMU factor."""
|
||||
|
||||
# Start with a camera on x-axis looking at origin
|
||||
radius = 30
|
||||
up = gtsam.Point3(0, 0, 1)
|
||||
target = gtsam.Point3(0, 0, 0)
|
||||
position = gtsam.Point3(radius, 0, 0)
|
||||
camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2())
|
||||
pose_0 = camera.pose()
|
||||
|
||||
# Create the set of ground-truth landmarks and poses
|
||||
angular_velocity = math.radians(180) # rad/sec
|
||||
delta_t = 1.0/18 # makes for 10 degrees per step
|
||||
|
||||
angular_velocity_vector = vector3(0, -angular_velocity, 0)
|
||||
linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
|
||||
scenario = gtsam.ConstantTwistScenario(
|
||||
angular_velocity_vector, linear_velocity_vector, pose_0)
|
||||
|
||||
# Create a factor graph
|
||||
newgraph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# Create (incremental) ISAM2 solver
|
||||
isam = gtsam.ISAM2()
|
||||
|
||||
# Create the initial estimate to the solution
|
||||
# Intentionally initialize the variables off from the ground truth
|
||||
initialEstimate = gtsam.Values()
|
||||
|
||||
# Add a prior on pose x0. This indirectly specifies where the origin is.
|
||||
# 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
noise = gtsam.noiseModel_Diagonal.Sigmas(
|
||||
np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
|
||||
newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise))
|
||||
|
||||
# Add imu priors
|
||||
biasKey = gtsam.symbol(ord('b'), 0)
|
||||
biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
|
||||
biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
|
||||
biasnoise)
|
||||
newgraph.push_back(biasprior)
|
||||
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
|
||||
velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
|
||||
|
||||
# Calculate with correct initial velocity
|
||||
n_velocity = vector3(0, angular_velocity * radius, 0)
|
||||
velprior = gtsam.PriorFactorVector(V(0), n_velocity, velnoise)
|
||||
newgraph.push_back(velprior)
|
||||
initialEstimate.insert(V(0), n_velocity)
|
||||
|
||||
accum = gtsam.PreintegratedImuMeasurements(PARAMS)
|
||||
|
||||
# Simulate poses and imu measurements, adding them to the factor graph
|
||||
for i in range(80):
|
||||
t = i * delta_t # simulation time
|
||||
if i == 0: # First time add two poses
|
||||
pose_1 = scenario.pose(delta_t)
|
||||
initialEstimate.insert(X(0), pose_0.compose(DELTA))
|
||||
initialEstimate.insert(X(1), pose_1.compose(DELTA))
|
||||
elif i >= 2: # Add more poses as necessary
|
||||
pose_i = scenario.pose(t)
|
||||
initialEstimate.insert(X(i), pose_i.compose(DELTA))
|
||||
|
||||
if i > 0:
|
||||
# Add Bias variables periodically
|
||||
if i % 5 == 0:
|
||||
biasKey += 1
|
||||
factor = gtsam.BetweenFactorConstantBias(
|
||||
biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE)
|
||||
newgraph.add(factor)
|
||||
initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
|
||||
|
||||
# Predict acceleration and gyro measurements in (actual) body frame
|
||||
nRb = scenario.rotation(t).matrix()
|
||||
bRn = np.transpose(nRb)
|
||||
measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity)
|
||||
measuredOmega = scenario.omega_b(t)
|
||||
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)
|
||||
|
||||
# Add Imu Factor
|
||||
imufac = gtsam.ImuFactor(
|
||||
X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
|
||||
newgraph.add(imufac)
|
||||
|
||||
# insert new velocity, which is wrong
|
||||
initialEstimate.insert(V(i), n_velocity)
|
||||
accum.resetIntegration()
|
||||
|
||||
# Incremental solution
|
||||
isam.update(newgraph, initialEstimate)
|
||||
result = isam.calculateEstimate()
|
||||
ISAM2_plot(result)
|
||||
|
||||
# reset
|
||||
newgraph = gtsam.NonlinearFactorGraph()
|
||||
initialEstimate.clear()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
IMU_example()
|
||||
|
|
@ -0,0 +1,52 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2018, 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
|
||||
|
||||
Simple robot motion example, with prior and two odometry measurements
|
||||
Author: Frank Dellaert
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
|
||||
# Create noise models
|
||||
ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
||||
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
|
||||
|
||||
# Create an empty nonlinear factor graph
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# 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)
|
||||
priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin
|
||||
graph.add(gtsam.PriorFactorPose2(1, priorMean, PRIOR_NOISE))
|
||||
|
||||
# Add odometry factors
|
||||
odometry = gtsam.Pose2(2.0, 0.0, 0.0)
|
||||
# For simplicity, we will use the same noise model for each odometry factor
|
||||
# Create odometry (Between) factors between consecutive poses
|
||||
graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE))
|
||||
graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE))
|
||||
print("\nFactor Graph:\n{}".format(graph))
|
||||
|
||||
# Create the data structure to hold the initialEstimate estimate to the solution
|
||||
# For illustrative purposes, these have been deliberately set to incorrect values
|
||||
initial = gtsam.Values()
|
||||
initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
|
||||
initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
|
||||
initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1))
|
||||
print("\nInitial Estimate:\n{}".format(initial))
|
||||
|
||||
# optimize using Levenberg-Marquardt optimization
|
||||
params = gtsam.LevenbergMarquardtParams()
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
|
||||
result = optimizer.optimize()
|
||||
print("\nFinal Result:\n{}".format(result))
|
||||
|
|
@ -0,0 +1,325 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2018, 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
|
||||
|
||||
Kinematics of three-link manipulator with GTSAM poses and product of exponential maps.
|
||||
Author: Frank Dellaert
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import math
|
||||
import unittest
|
||||
from functools import reduce
|
||||
|
||||
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
|
||||
from gtsam import Pose2
|
||||
|
||||
|
||||
def vector3(x, y, z):
|
||||
"""Create 3D double numpy array."""
|
||||
return np.array([x, y, z], dtype=np.float)
|
||||
|
||||
|
||||
def compose(*poses):
|
||||
"""Compose all Pose2 transforms given as arguments from left to right."""
|
||||
return reduce((lambda x, y: x.compose(y)), poses)
|
||||
|
||||
|
||||
def vee(M):
|
||||
"""Pose2 vee operator."""
|
||||
return vector3(M[0, 2], M[1, 2], M[1, 0])
|
||||
|
||||
|
||||
def delta(g0, g1):
|
||||
"""Difference between x,y,,theta components of SE(2) poses."""
|
||||
return vector3(g1.x() - g0.x(), g1.y() - g0.y(), g1.theta() - g0.theta())
|
||||
|
||||
|
||||
def trajectory(g0, g1, N=20):
|
||||
""" Create an interpolated trajectory in SE(2), treating x,y, and theta separately.
|
||||
g0 and g1 are the initial and final pose, respectively.
|
||||
N is the number of *intervals*
|
||||
Returns N+1 poses
|
||||
"""
|
||||
e = delta(g0, g1)
|
||||
return [Pose2(g0.x()+e[0]*t, g0.y()+e[1]*t, g0.theta()+e[2]*t) for t in np.linspace(0, 1, N)]
|
||||
|
||||
|
||||
class ThreeLinkArm(object):
|
||||
"""Three-link arm class."""
|
||||
|
||||
def __init__(self):
|
||||
self.L1 = 3.5
|
||||
self.L2 = 3.5
|
||||
self.L3 = 2.5
|
||||
self.xi1 = vector3(0, 0, 1)
|
||||
self.xi2 = vector3(self.L1, 0, 1)
|
||||
self.xi3 = vector3(self.L1+self.L2, 0, 1)
|
||||
self.sXt0 = Pose2(0, self.L1+self.L2 + self.L3, math.radians(90))
|
||||
|
||||
def fk(self, q):
|
||||
""" Forward kinematics.
|
||||
Takes numpy array of joint angles, in radians.
|
||||
"""
|
||||
sXl1 = Pose2(0, 0, math.radians(90))
|
||||
l1Zl1 = Pose2(0, 0, q[0])
|
||||
l1Xl2 = Pose2(self.L1, 0, 0)
|
||||
l2Zl2 = Pose2(0, 0, q[1])
|
||||
l2Xl3 = Pose2(self.L2, 0, 0)
|
||||
l3Zl3 = Pose2(0, 0, q[2])
|
||||
l3Xt = Pose2(self.L3, 0, 0)
|
||||
return compose(sXl1, l1Zl1, l1Xl2, l2Zl2, l2Xl3, l3Zl3, l3Xt)
|
||||
|
||||
def jacobian(self, q):
|
||||
""" Calculate manipulator Jacobian.
|
||||
Takes numpy array of joint angles, in radians.
|
||||
"""
|
||||
a = q[0]+q[1]
|
||||
b = a+q[2]
|
||||
return np.array([[-self.L1*math.cos(q[0]) - self.L2*math.cos(a)-self.L3*math.cos(b),
|
||||
-self.L1*math.cos(a)-self.L3*math.cos(b),
|
||||
- self.L3*math.cos(b)],
|
||||
[-self.L1*math.sin(q[0]) - self.L2*math.sin(a)-self.L3*math.sin(b),
|
||||
-self.L1*math.sin(a)-self.L3*math.sin(b),
|
||||
- self.L3*math.sin(b)],
|
||||
[1, 1, 1]], np.float)
|
||||
|
||||
def poe(self, q):
|
||||
""" Forward kinematics.
|
||||
Takes numpy array of joint angles, in radians.
|
||||
"""
|
||||
l1Zl1 = Pose2.Expmap(self.xi1 * q[0])
|
||||
l2Zl2 = Pose2.Expmap(self.xi2 * q[1])
|
||||
l3Zl3 = Pose2.Expmap(self.xi3 * q[2])
|
||||
return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
|
||||
|
||||
def con(self, q):
|
||||
""" Forward kinematics, conjugation form.
|
||||
Takes numpy array of joint angles, in radians.
|
||||
"""
|
||||
def expmap(x, y, theta):
|
||||
"""Implement exponential map via conjugation with axis (x,y)."""
|
||||
return compose(Pose2(x, y, 0), Pose2(0, 0, theta), Pose2(-x, -y, 0))
|
||||
|
||||
l1Zl1 = expmap(0.0, 0.0, q[0])
|
||||
l2Zl2 = expmap(0.0, self.L1, q[1])
|
||||
l3Zl3 = expmap(0.0, 7.0, q[2])
|
||||
return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
|
||||
|
||||
def ik(self, sTt_desired, e=1e-9):
|
||||
""" Inverse kinematics.
|
||||
Takes desired Pose2 of tool T with respect to base S.
|
||||
Optional: mu, gradient descent rate; e: error norm threshold
|
||||
"""
|
||||
q = np.radians(vector3(30, -30, 45)) # well within workspace
|
||||
error = vector3(100, 100, 100)
|
||||
|
||||
while np.linalg.norm(error) > e:
|
||||
error = delta(sTt_desired, self.fk(q))
|
||||
J = self.jacobian(q)
|
||||
q -= np.dot(np.linalg.pinv(J), error)
|
||||
|
||||
# return result in interval [-pi,pi)
|
||||
return np.remainder(q+math.pi, 2*math.pi)-math.pi
|
||||
|
||||
def manipulator_jacobian(self, q):
|
||||
""" Calculate manipulator Jacobian.
|
||||
Takes numpy array of joint angles, in radians.
|
||||
Returns the manipulator Jacobian of differential twists. When multiplied with
|
||||
a vector of joint velocities, will yield a single differential twist which is
|
||||
the spatial velocity d(sTt)/dt * inv(sTt) of the end-effector pose.
|
||||
Just like always, differential twists can be hatted and multiplied with spatial
|
||||
coordinates of a point to give the spatial velocity of the point.
|
||||
"""
|
||||
l1Zl1 = Pose2.Expmap(self.xi1 * q[0])
|
||||
l2Zl2 = Pose2.Expmap(self.xi2 * q[1])
|
||||
# l3Zl3 = Pose2.Expmap(self.xi3 * q[2])
|
||||
|
||||
p1 = self.xi1
|
||||
# p1 = Pose2().Adjoint(self.xi1)
|
||||
|
||||
sTl1 = l1Zl1
|
||||
p2 = sTl1.Adjoint(self.xi2)
|
||||
|
||||
sTl2 = compose(l1Zl1, l2Zl2)
|
||||
p3 = sTl2.Adjoint(self.xi3)
|
||||
|
||||
differential_twists = [p1, p2, p3]
|
||||
return np.stack(differential_twists, axis=1)
|
||||
|
||||
def plot(self, fignum, q):
|
||||
""" Plot arm.
|
||||
Takes figure number, and numpy array of joint angles, in radians.
|
||||
"""
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca()
|
||||
|
||||
sXl1 = Pose2(0, 0, math.radians(90))
|
||||
t = sXl1.translation()
|
||||
p1 = np.array([t.x(), t.y()])
|
||||
gtsam_plot.plot_pose2_on_axes(axes, sXl1)
|
||||
|
||||
def plot_line(p, g, color):
|
||||
t = g.translation()
|
||||
q = np.array([t.x(), t.y()])
|
||||
line = np.append(p[np.newaxis], q[np.newaxis], axis=0)
|
||||
axes.plot(line[:, 0], line[:, 1], color)
|
||||
return q
|
||||
|
||||
l1Zl1 = Pose2(0, 0, q[0])
|
||||
l1Xl2 = Pose2(self.L1, 0, 0)
|
||||
sTl2 = compose(sXl1, l1Zl1, l1Xl2)
|
||||
p2 = plot_line(p1, sTl2, 'r-')
|
||||
gtsam_plot.plot_pose2_on_axes(axes, sTl2)
|
||||
|
||||
l2Zl2 = Pose2(0, 0, q[1])
|
||||
l2Xl3 = Pose2(self.L2, 0, 0)
|
||||
sTl3 = compose(sTl2, l2Zl2, l2Xl3)
|
||||
p3 = plot_line(p2, sTl3, 'g-')
|
||||
gtsam_plot.plot_pose2_on_axes(axes, sTl3)
|
||||
|
||||
l3Zl3 = Pose2(0, 0, q[2])
|
||||
l3Xt = Pose2(self.L3, 0, 0)
|
||||
sTt = compose(sTl3, l3Zl3, l3Xt)
|
||||
plot_line(p3, sTt, 'b-')
|
||||
gtsam_plot.plot_pose2_on_axes(axes, sTt)
|
||||
|
||||
|
||||
# Create common example configurations.
|
||||
Q0 = vector3(0, 0, 0)
|
||||
Q1 = np.radians(vector3(-30, -45, -90))
|
||||
Q2 = np.radians(vector3(-90, 90, 0))
|
||||
|
||||
|
||||
class TestPose2SLAMExample(unittest.TestCase):
|
||||
"""Unit tests for functions used below."""
|
||||
|
||||
def setUp(self):
|
||||
self.arm = ThreeLinkArm()
|
||||
|
||||
def assertPose2Equals(self, actual, expected, tol=1e-2):
|
||||
"""Helper function that prints out actual and expected if not equal."""
|
||||
equal = actual.equals(expected, tol)
|
||||
if not equal:
|
||||
raise self.failureException(
|
||||
"Poses are not equal:\n{}!={}".format(actual, expected))
|
||||
|
||||
def test_fk_arm(self):
|
||||
"""Make sure forward kinematics is correct for some known test configurations."""
|
||||
# at rest
|
||||
expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
|
||||
sTt = self.arm.fk(Q0)
|
||||
self.assertIsInstance(sTt, Pose2)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
# -30, -45, -90
|
||||
expected = Pose2(5.78, 1.52, math.radians(-75))
|
||||
sTt = self.arm.fk(Q1)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
def test_jacobian(self):
|
||||
"""Test Jacobian calculation."""
|
||||
# at rest
|
||||
expected = np.array([[-9.5, -6, -2.5], [0, 0, 0], [1, 1, 1]], np.float)
|
||||
J = self.arm.jacobian(Q0)
|
||||
np.testing.assert_array_almost_equal(J, expected)
|
||||
|
||||
# at -90, 90, 0
|
||||
expected = np.array([[-6, -6, -2.5], [3.5, 0, 0], [1, 1, 1]], np.float)
|
||||
J = self.arm.jacobian(Q2)
|
||||
np.testing.assert_array_almost_equal(J, expected)
|
||||
|
||||
def test_con_arm(self):
|
||||
"""Make sure POE is correct for some known test configurations."""
|
||||
# at rest
|
||||
expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
|
||||
sTt = self.arm.con(Q0)
|
||||
self.assertIsInstance(sTt, Pose2)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
# -30, -45, -90
|
||||
expected = Pose2(5.78, 1.52, math.radians(-75))
|
||||
sTt = self.arm.con(Q1)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
def test_poe_arm(self):
|
||||
"""Make sure POE is correct for some known test configurations."""
|
||||
# at rest
|
||||
expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
|
||||
sTt = self.arm.poe(Q0)
|
||||
self.assertIsInstance(sTt, Pose2)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
# -30, -45, -90
|
||||
expected = Pose2(5.78, 1.52, math.radians(-75))
|
||||
sTt = self.arm.poe(Q1)
|
||||
self.assertPose2Equals(sTt, expected)
|
||||
|
||||
def test_ik(self):
|
||||
"""Check iterative inverse kinematics function."""
|
||||
# at rest
|
||||
actual = self.arm.ik(Pose2(0, 2*3.5 + 2.5, math.radians(90)))
|
||||
np.testing.assert_array_almost_equal(actual, Q0, decimal=2)
|
||||
|
||||
# -30, -45, -90
|
||||
sTt_desired = Pose2(5.78, 1.52, math.radians(-75))
|
||||
actual = self.arm.ik(sTt_desired)
|
||||
self.assertPose2Equals(self.arm.fk(actual), sTt_desired)
|
||||
np.testing.assert_array_almost_equal(actual, Q1, decimal=2)
|
||||
|
||||
def test_manipulator_jacobian(self):
|
||||
"""Test Jacobian calculation."""
|
||||
# at rest
|
||||
expected = np.array([[0, 3.5, 7], [0, 0, 0], [1, 1, 1]], np.float)
|
||||
J = self.arm.manipulator_jacobian(Q0)
|
||||
np.testing.assert_array_almost_equal(J, expected)
|
||||
|
||||
# at -90, 90, 0
|
||||
expected = np.array(
|
||||
[[0, 0, 3.5], [0, -3.5, -3.5], [1, 1, 1]], np.float)
|
||||
J = self.arm.manipulator_jacobian(Q2)
|
||||
np.testing.assert_array_almost_equal(J, expected)
|
||||
|
||||
|
||||
def run_example():
|
||||
""" Use trajectory interpolation and then trajectory tracking a la Murray
|
||||
to move a 3-link arm on a straight line.
|
||||
"""
|
||||
arm = ThreeLinkArm()
|
||||
q = np.radians(vector3(30, -30, 45))
|
||||
sTt_initial = arm.fk(q)
|
||||
sTt_goal = Pose2(2.4, 4.3, math.radians(0))
|
||||
poses = trajectory(sTt_initial, sTt_goal, 50)
|
||||
|
||||
fignum = 0
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca()
|
||||
axes.set_xlim(-5, 5)
|
||||
axes.set_ylim(0, 10)
|
||||
gtsam_plot.plot_pose2(fignum, arm.fk(q))
|
||||
|
||||
for pose in poses:
|
||||
sTt = arm.fk(q)
|
||||
error = delta(sTt, pose)
|
||||
J = arm.jacobian(q)
|
||||
q += np.dot(np.linalg.inv(J), error)
|
||||
arm.plot(fignum, q)
|
||||
plt.pause(0.01)
|
||||
|
||||
plt.pause(10)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
run_example()
|
||||
unittest.main()
|
||||
|
|
@ -0,0 +1,80 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2018, 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
|
||||
|
||||
Simple robotics example using odometry measurements and bearing-range (laser) measurements
|
||||
Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
|
||||
# Create noise models
|
||||
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
|
||||
ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
|
||||
MEASUREMENT_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.2]))
|
||||
|
||||
# Create an empty nonlinear factor graph
|
||||
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)
|
||||
|
||||
# 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))
|
||||
|
||||
# Add odometry factors between X1,X2 and X2,X3, respectively
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))
|
||||
|
||||
# Add Range-Bearing measurements to two different landmarks L1 and L2
|
||||
graph.add(gtsam.BearingRangeFactor2D(
|
||||
X1, L1, gtsam.Rot2.fromDegrees(45), np.sqrt(4.0+4.0), MEASUREMENT_NOISE))
|
||||
graph.add(gtsam.BearingRangeFactor2D(
|
||||
X2, L1, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))
|
||||
graph.add(gtsam.BearingRangeFactor2D(
|
||||
X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))
|
||||
|
||||
# Print graph
|
||||
print("Factor Graph:\n{}".format(graph))
|
||||
|
||||
# Create (deliberately inaccurate) initial estimate
|
||||
initial_estimate = gtsam.Values()
|
||||
initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15))
|
||||
initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20))
|
||||
initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10))
|
||||
initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10))
|
||||
initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80))
|
||||
|
||||
# Print
|
||||
print("Initial Estimate:\n{}".format(initial_estimate))
|
||||
|
||||
# Optimize using Levenberg-Marquardt optimization. The optimizer
|
||||
# accepts an optional set of configuration parameters, controlling
|
||||
# things like convergence criteria, the type of linear system solver
|
||||
# to use, and the amount of information displayed during optimization.
|
||||
# Here we will use the default set of parameters. See the
|
||||
# documentation for the full set of parameters.
|
||||
params = gtsam.LevenbergMarquardtParams()
|
||||
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params)
|
||||
result = optimizer.optimize()
|
||||
print("\nFinal Result:\n{}".format(result))
|
||||
|
||||
# Calculate and print marginal covariances for all variables
|
||||
marginals = gtsam.Marginals(graph, result)
|
||||
for (key, str) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"), (L2, "L2")]:
|
||||
print("{} covariance:\n{}\n".format(str, marginals.marginalCovariance(key)))
|
||||
|
|
@ -0,0 +1,87 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2018, 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
|
||||
|
||||
Simple robotics example using odometry measurements and bearing-range (laser) measurements
|
||||
Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
|
||||
|
||||
def vector3(x, y, z):
|
||||
"""Create 3d double numpy array."""
|
||||
return np.array([x, y, z], dtype=np.float)
|
||||
|
||||
|
||||
# Create noise models
|
||||
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(vector3(0.3, 0.3, 0.1))
|
||||
ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(vector3(0.2, 0.2, 0.1))
|
||||
|
||||
# 1. Create a factor graph container and add factors to it
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
|
||||
# 2a. Add a prior on the first pose, setting it to the origin
|
||||
# A prior factor consists of a mean and a noise ODOMETRY_NOISE (covariance matrix)
|
||||
graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE))
|
||||
|
||||
# 2b. Add odometry factors
|
||||
# Create odometry (Between) factors between consecutive poses
|
||||
graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
2, 3, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
3, 4, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
|
||||
graph.add(gtsam.BetweenFactorPose2(
|
||||
4, 5, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
|
||||
|
||||
# 2c. Add the loop closure constraint
|
||||
# This factor encodes the fact that we have returned to the same pose. In real
|
||||
# systems, 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.add(gtsam.BetweenFactorPose2(
|
||||
5, 2, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
|
||||
print("\nFactor Graph:\n{}".format(graph)) # print
|
||||
|
||||
# 3. Create the data structure to hold the initial_estimate estimate to the
|
||||
# solution. For illustrative purposes, these have been deliberately set to incorrect values
|
||||
initial_estimate = gtsam.Values()
|
||||
initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
|
||||
initial_estimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
|
||||
initial_estimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2))
|
||||
initial_estimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi))
|
||||
initial_estimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2))
|
||||
print("\nInitial Estimate:\n{}".format(initial_estimate)) # print
|
||||
|
||||
# 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
|
||||
# The optimizer accepts an optional set of configuration parameters,
|
||||
# controlling things like convergence criteria, the type of linear
|
||||
# system solver to use, and the amount of information displayed during
|
||||
# optimization. We will set a few parameters as a demonstration.
|
||||
parameters = gtsam.GaussNewtonParams()
|
||||
|
||||
# Stop iterating once the change in error between steps is less than this value
|
||||
parameters.setRelativeErrorTol(1e-5)
|
||||
# Do not perform more than N iteration steps
|
||||
parameters.setMaxIterations(100)
|
||||
# Create the optimizer ...
|
||||
optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, parameters)
|
||||
# ... and optimize
|
||||
result = optimizer.optimize()
|
||||
print("Final Result:\n{}".format(result))
|
||||
|
||||
# 5. Calculate and print marginal covariances for all variables
|
||||
marginals = gtsam.Marginals(graph, result)
|
||||
for i in range(1, 6):
|
||||
print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i)))
|
||||
|
|
@ -0,0 +1,4 @@
|
|||
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))
|
||||
|
|
@ -0,0 +1,38 @@
|
|||
"""
|
||||
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
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
|
||||
|
||||
def createPoints():
|
||||
# Create the set of ground-truth landmarks
|
||||
points = [gtsam.Point3(10.0, 10.0, 10.0),
|
||||
gtsam.Point3(-10.0, 10.0, 10.0),
|
||||
gtsam.Point3(-10.0, -10.0, 10.0),
|
||||
gtsam.Point3(10.0, -10.0, 10.0),
|
||||
gtsam.Point3(10.0, 10.0, -10.0),
|
||||
gtsam.Point3(-10.0, 10.0, -10.0),
|
||||
gtsam.Point3(-10.0, -10.0, -10.0),
|
||||
gtsam.Point3(10.0, -10.0, -10.0)]
|
||||
return points
|
||||
|
||||
|
||||
def createPoses(K):
|
||||
# Create the set of ground-truth poses
|
||||
radius = 30.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)
|
||||
camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K)
|
||||
poses.append(camera.pose())
|
||||
return poses
|
||||
|
|
@ -0,0 +1,163 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2018, 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
|
||||
|
||||
An example of running visual SLAM using iSAM2.
|
||||
Author: Duy-Nguyen Ta (C++), Frank Dellaert (Python)
|
||||
"""
|
||||
# pylint: disable=invalid-name, E1101
|
||||
|
||||
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
|
||||
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))
|
||||
|
||||
|
||||
def visual_ISAM2_plot(result):
|
||||
"""
|
||||
VisualISAMPlot plots current state of ISAM2 object
|
||||
Author: Ellon Paiva
|
||||
Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert
|
||||
"""
|
||||
|
||||
# Declare an id for the figure
|
||||
fignum = 0
|
||||
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca(projection='3d')
|
||||
plt.cla()
|
||||
|
||||
# Plot points
|
||||
# Can't use data because current frame might not see all points
|
||||
# marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate())
|
||||
# gtsam.plot_3d_points(result, [], marginals)
|
||||
gtsam_plot.plot_3d_points(fignum, result, 'rx')
|
||||
|
||||
# Plot cameras
|
||||
i = 0
|
||||
while result.exists(X(i)):
|
||||
pose_i = result.atPose3(X(i))
|
||||
gtsam_plot.plot_pose3(fignum, pose_i, 10)
|
||||
i += 1
|
||||
|
||||
# draw
|
||||
axes.set_xlim3d(-40, 40)
|
||||
axes.set_ylim3d(-40, 40)
|
||||
axes.set_zlim3d(-40, 40)
|
||||
plt.pause(1)
|
||||
|
||||
|
||||
def visual_ISAM2_example():
|
||||
plt.ion()
|
||||
|
||||
# Define the camera calibration parameters
|
||||
K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
|
||||
|
||||
# Define the camera observation noise model
|
||||
measurement_noise = gtsam.noiseModel_Isotropic.Sigma(
|
||||
2, 1.0) # one pixel in u and v
|
||||
|
||||
# Create the set of ground-truth landmarks
|
||||
points = SFMdata.createPoints()
|
||||
|
||||
# Create the set of ground-truth poses
|
||||
poses = SFMdata.createPoses(K)
|
||||
|
||||
# Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps
|
||||
# to maintain proper linearization and efficient variable ordering, iSAM2
|
||||
# performs partial relinearization/reordering at each step. A parameter
|
||||
# structure is available that allows the user to set various properties, such
|
||||
# as the relinearization threshold and type of linear solver. For this
|
||||
# example, we we set the relinearization threshold small so the iSAM2 result
|
||||
# will approach the batch result.
|
||||
parameters = gtsam.ISAM2Params()
|
||||
parameters.setRelinearizeThreshold(0.01)
|
||||
parameters.setRelinearizeSkip(1)
|
||||
isam = gtsam.ISAM2(parameters)
|
||||
|
||||
# Create a Factor Graph and Values to hold the new data
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
initial_estimate = gtsam.Values()
|
||||
|
||||
# Loop over the different poses, adding the observations to iSAM incrementally
|
||||
for i, pose in enumerate(poses):
|
||||
|
||||
# Add factors for each landmark observation
|
||||
for j, point in enumerate(points):
|
||||
camera = gtsam.PinholeCameraCal3_S2(pose, K)
|
||||
measurement = camera.project(point)
|
||||
graph.push_back(gtsam.GenericProjectionFactorCal3_S2(
|
||||
measurement, measurement_noise, X(i), L(j), K))
|
||||
|
||||
# Add an initial guess for the current pose
|
||||
# Intentionally initialize the variables off from the ground truth
|
||||
initial_estimate.insert(X(i), pose.compose(gtsam.Pose3(
|
||||
gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20))))
|
||||
|
||||
# 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.
|
||||
# Also, as iSAM solves incrementally, we must wait until each is observed
|
||||
# at least twice before adding it to iSAM.
|
||||
if i == 0:
|
||||
# Add a prior on pose x0
|
||||
pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array(
|
||||
[0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], pose_noise))
|
||||
|
||||
# Add a prior on landmark l0
|
||||
point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
|
||||
graph.push_back(gtsam.PriorFactorPoint3(
|
||||
L(0), points[0], point_noise)) # add directly to graph
|
||||
|
||||
# Add initial guesses to all observed landmarks
|
||||
# Intentionally initialize the variables off from the ground truth
|
||||
for j, point in enumerate(points):
|
||||
initial_estimate.insert(L(j), gtsam.Point3(
|
||||
point.x()-0.25, point.y()+0.20, point.z()+0.15))
|
||||
else:
|
||||
# Update iSAM with the new factors
|
||||
isam.update(graph, initial_estimate)
|
||||
# Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver.
|
||||
# If accuracy is desired at the expense of time, update(*) can be called additional
|
||||
# times to perform multiple optimizer iterations every step.
|
||||
isam.update()
|
||||
current_estimate = isam.calculateEstimate()
|
||||
print("****************************************************")
|
||||
print("Frame", i, ":")
|
||||
for j in range(i + 1):
|
||||
print(X(j), ":", current_estimate.atPose3(X(j)))
|
||||
|
||||
for j in range(len(points)):
|
||||
print(L(j), ":", current_estimate.atPoint3(L(j)))
|
||||
|
||||
visual_ISAM2_plot(current_estimate)
|
||||
|
||||
# Clear the factor graph and values for the next iteration
|
||||
graph.resize(0)
|
||||
initial_estimate.clear()
|
||||
|
||||
plt.ioff()
|
||||
plt.show()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
visual_ISAM2_example()
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
import math
|
||||
import unittest
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
|
||||
|
||||
class TestScenario(unittest.TestCase):
|
||||
def setUp(self):
|
||||
pass
|
||||
|
||||
def test_loop(self):
|
||||
# Forward velocity 2m/s
|
||||
# Pitch up with angular velocity 6 degree/sec (negative in FLU)
|
||||
v = 2
|
||||
w = math.radians(6)
|
||||
W = np.array([0, -w, 0])
|
||||
V = np.array([v, 0, 0])
|
||||
scenario = gtsam.ConstantTwistScenario(W, V)
|
||||
|
||||
T = 30
|
||||
np.testing.assert_almost_equal(W, scenario.omega_b(T))
|
||||
np.testing.assert_almost_equal(V, scenario.velocity_b(T))
|
||||
np.testing.assert_almost_equal(
|
||||
np.cross(W, V), scenario.acceleration_b(T))
|
||||
|
||||
# R = v/w, so test if loop crests at 2*R
|
||||
R = v / w
|
||||
T30 = scenario.pose(T)
|
||||
np.testing.assert_almost_equal(
|
||||
np.array([math.pi, 0, math.pi]), T30.rotation().xyz())
|
||||
self.assert_(gtsam.Point3(0, 0, 2 * R).equals(T30.translation(), 1e-9))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
|
|
@ -0,0 +1,102 @@
|
|||
"""Various plotting utlities."""
|
||||
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
|
||||
def plot_pose2_on_axes(axes, pose, axis_length=0.1):
|
||||
"""Plot a 2D pose on given axis 'axes' with given 'axis_length'."""
|
||||
# get rotation and translation (center)
|
||||
gRp = pose.rotation().matrix() # rotation from pose to global
|
||||
t = pose.translation()
|
||||
origin = np.array([t.x(), t.y()])
|
||||
|
||||
# draw the camera axes
|
||||
x_axis = origin + gRp[:, 0] * axis_length
|
||||
line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0)
|
||||
axes.plot(line[:, 0], line[:, 1], 'r-')
|
||||
|
||||
y_axis = origin + gRp[:, 1] * axis_length
|
||||
line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0)
|
||||
axes.plot(line[:, 0], line[:, 1], 'g-')
|
||||
|
||||
|
||||
def plot_pose2(fignum, pose, axis_length=0.1):
|
||||
"""Plot a 2D pose on given figure with given 'axis_length'."""
|
||||
# get figure object
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca()
|
||||
plot_pose2_on_axes(axes, pose, axis_length)
|
||||
|
||||
|
||||
def plot_point3_on_axes(axes, point, linespec):
|
||||
"""Plot a 3D point on given axis 'axes' with given 'linespec'."""
|
||||
axes.plot([point.x()], [point.y()], [point.z()], linespec)
|
||||
|
||||
|
||||
def plot_point3(fignum, point, linespec):
|
||||
"""Plot a 3D point on given figure with given 'linespec'."""
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca(projection='3d')
|
||||
plot_point3_on_axes(axes, point, linespec)
|
||||
|
||||
|
||||
def plot_3d_points(fignum, values, linespec, marginals=None):
|
||||
"""
|
||||
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.
|
||||
"""
|
||||
|
||||
keys = values.keys()
|
||||
|
||||
# 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)
|
||||
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'."""
|
||||
# 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()])
|
||||
|
||||
# draw the camera axes
|
||||
x_axis = origin + gRp[:, 0] * axis_length
|
||||
line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0)
|
||||
axes.plot(line[:, 0], line[:, 1], line[:, 2], 'r-')
|
||||
|
||||
y_axis = origin + gRp[:, 1] * axis_length
|
||||
line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0)
|
||||
axes.plot(line[:, 0], line[:, 1], line[:, 2], 'g-')
|
||||
|
||||
z_axis = origin + gRp[:, 2] * axis_length
|
||||
line = np.append(origin[np.newaxis], z_axis[np.newaxis], axis=0)
|
||||
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
|
||||
|
||||
|
||||
def plot_pose3(fignum, pose, axis_length=0.1):
|
||||
"""Plot a 3D pose on given figure with given 'axis_length'."""
|
||||
# get figure object
|
||||
fig = plt.figure(fignum)
|
||||
axes = fig.gca(projection='3d')
|
||||
plot_pose3_on_axes(axes, pose, axis_length)
|
||||
|
|
@ -306,10 +306,17 @@ public:
|
|||
: cols),
|
||||
stride
|
||||
) {}
|
||||
|
||||
MapBase &operator=(const MatrixType &other) {
|
||||
Base::operator=(other);
|
||||
return *this;
|
||||
}
|
||||
|
||||
virtual ~MapBase() { }
|
||||
};
|
||||
|
||||
|
||||
template <template<class,int,int,int,int,int> class DenseBase,
|
||||
template <template<class,int,int,int,int,int> class EigencyDenseBase,
|
||||
typename Scalar,
|
||||
int _Rows, int _Cols,
|
||||
int _Options = Eigen::AutoAlign |
|
||||
|
|
@ -341,16 +348,18 @@ template <template<class,int,int,int,int,int> class DenseBase,
|
|||
int _StrideOuter=0, int _StrideInner=0,
|
||||
int _MaxRows = _Rows,
|
||||
int _MaxCols = _Cols>
|
||||
class FlattenedMap: public MapBase<DenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> > {
|
||||
class FlattenedMap: public MapBase<EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> > {
|
||||
public:
|
||||
typedef MapBase<DenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> > Base;
|
||||
typedef MapBase<EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>, _MapOptions, Eigen::Stride<_StrideOuter, _StrideInner> > Base;
|
||||
|
||||
FlattenedMap()
|
||||
: Base(NULL, 0, 0) {}
|
||||
: Base(NULL, 0, 0),
|
||||
object_(NULL) {}
|
||||
|
||||
FlattenedMap(Scalar *data, long rows, long cols, long outer_stride=0, long inner_stride=0)
|
||||
: Base(data, rows, cols,
|
||||
Eigen::Stride<_StrideOuter, _StrideInner>(outer_stride, inner_stride)) {
|
||||
Eigen::Stride<_StrideOuter, _StrideInner>(outer_stride, inner_stride)),
|
||||
object_(NULL) {
|
||||
}
|
||||
|
||||
FlattenedMap(PyArrayObject *object)
|
||||
|
|
@ -359,18 +368,26 @@ public:
|
|||
(((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[0] : 1,
|
||||
(((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0],
|
||||
Eigen::Stride<_StrideOuter, _StrideInner>(_StrideOuter != Eigen::Dynamic ? _StrideOuter : (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[0] : 1,
|
||||
_StrideInner != Eigen::Dynamic ? _StrideInner : (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0])) {
|
||||
_StrideInner != Eigen::Dynamic ? _StrideInner : (((PyArrayObject*)object)->nd == 2) ? ((PyArrayObject*)object)->dimensions[1] : ((PyArrayObject*)object)->dimensions[0])),
|
||||
object_(object) {
|
||||
|
||||
if (((PyObject*)object != Py_None) && !PyArray_ISONESEGMENT(object))
|
||||
throw std::invalid_argument("Numpy array must be a in one contiguous segment to be able to be transferred to a Eigen Map.");
|
||||
|
||||
Py_XINCREF(object_);
|
||||
}
|
||||
FlattenedMap &operator=(const FlattenedMap &other) {
|
||||
// Replace the memory that we point to (not a memory allocation)
|
||||
new (this) FlattenedMap(const_cast<Scalar*>(other.data()),
|
||||
other.rows(),
|
||||
other.cols(),
|
||||
other.outerStride(),
|
||||
other.innerStride());
|
||||
if (other.object_) {
|
||||
new (this) FlattenedMap(other.object_);
|
||||
} else {
|
||||
// Replace the memory that we point to (not a memory allocation)
|
||||
new (this) FlattenedMap(const_cast<Scalar*>(other.data()),
|
||||
other.rows(),
|
||||
other.cols(),
|
||||
other.outerStride(),
|
||||
other.innerStride());
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
|
@ -382,9 +399,16 @@ public:
|
|||
return static_cast<Base&>(*this);
|
||||
}
|
||||
|
||||
operator DenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>() const {
|
||||
return DenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>(static_cast<Base>(*this));
|
||||
operator EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>() const {
|
||||
return EigencyDenseBase<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>(static_cast<Base>(*this));
|
||||
}
|
||||
|
||||
virtual ~FlattenedMap() {
|
||||
Py_XDECREF(object_);
|
||||
}
|
||||
|
||||
private:
|
||||
PyArrayObject * const object_;
|
||||
};
|
||||
|
||||
|
||||
|
|
@ -394,39 +418,60 @@ public:
|
|||
typedef MapBase<MatrixType> Base;
|
||||
typedef typename MatrixType::Scalar Scalar;
|
||||
|
||||
enum {
|
||||
RowsAtCompileTime = Base::Base::RowsAtCompileTime,
|
||||
ColsAtCompileTime = Base::Base::ColsAtCompileTime
|
||||
};
|
||||
|
||||
Map()
|
||||
: Base(NULL, 0, 0) {
|
||||
: Base(NULL,
|
||||
(RowsAtCompileTime == Eigen::Dynamic) ? 0 : RowsAtCompileTime,
|
||||
(ColsAtCompileTime == Eigen::Dynamic) ? 0 : ColsAtCompileTime),
|
||||
object_(NULL) {
|
||||
}
|
||||
|
||||
Map(Scalar *data, long rows, long cols)
|
||||
: Base(data, rows, cols) {}
|
||||
: Base(data, rows, cols),
|
||||
object_(NULL) {}
|
||||
|
||||
Map(PyArrayObject *object)
|
||||
: Base((PyObject*)object == Py_None? NULL: (Scalar *)object->data,
|
||||
// ROW: If array is in row-major order, transpose (see README)
|
||||
(PyObject*)object == Py_None? 0 :
|
||||
(PyArray_IS_C_CONTIGUOUS(object)
|
||||
(!PyArray_IS_F_CONTIGUOUS(object)
|
||||
? ((object->nd == 1)
|
||||
? 1 // ROW: If 1D row-major numpy array, set to 1 (row vector)
|
||||
: object->dimensions[1])
|
||||
: object->dimensions[0]),
|
||||
// COLUMN: If array is in row-major order: transpose (see README)
|
||||
(PyObject*)object == Py_None? 0 :
|
||||
(PyArray_IS_C_CONTIGUOUS(object)
|
||||
(!PyArray_IS_F_CONTIGUOUS(object)
|
||||
? object->dimensions[0]
|
||||
: ((object->nd == 1)
|
||||
? 1 // COLUMN: If 1D col-major numpy array, set to length (column vector)
|
||||
: object->dimensions[1]))) {
|
||||
: object->dimensions[1]))),
|
||||
object_(object) {
|
||||
|
||||
if (((PyObject*)object != Py_None) && !PyArray_ISONESEGMENT(object))
|
||||
throw std::invalid_argument("Numpy array must be a in one contiguous segment to be able to be transferred to a Eigen Map.");
|
||||
Py_XINCREF(object_);
|
||||
}
|
||||
|
||||
Map &operator=(const Map &other) {
|
||||
// Replace the memory that we point to (not a memory allocation)
|
||||
new (this) Map(const_cast<Scalar*>(other.data()),
|
||||
other.rows(),
|
||||
other.cols());
|
||||
if (other.object_) {
|
||||
new (this) Map(other.object_);
|
||||
} else {
|
||||
// Replace the memory that we point to (not a memory allocation)
|
||||
new (this) Map(const_cast<Scalar*>(other.data()),
|
||||
other.rows(),
|
||||
other.cols());
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
Map &operator=(const MatrixType &other) {
|
||||
MapBase<MatrixType>::operator=(other);
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
|
@ -440,7 +485,14 @@ public:
|
|||
|
||||
operator MatrixType() const {
|
||||
return MatrixType(static_cast<Base>(*this));
|
||||
}
|
||||
}
|
||||
|
||||
virtual ~Map() {
|
||||
Py_XDECREF(object_);
|
||||
}
|
||||
|
||||
private:
|
||||
PyArrayObject * const object_;
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,7 @@
|
|||
// add unary measurement factors, like GPS, on all three poses
|
||||
noiseModel::Diagonal::shared_ptr unaryNoise =
|
||||
noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
|
||||
graph.add(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
|
||||
graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
|
||||
graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
|
||||
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
class UnaryFactor: public NoiseModelFactor1<Pose2> {
|
||||
double mx_, my_; ///< X and Y measurements
|
||||
|
||||
public:
|
||||
UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
|
||||
NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
|
||||
|
||||
Vector evaluateError(const Pose2& q,
|
||||
boost::optional<Matrix&> H = boost::none) const
|
||||
{
|
||||
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();
|
||||
}
|
||||
};
|
||||
|
|
@ -0,0 +1,18 @@
|
|||
Final Result:
|
||||
Values with 3 values:
|
||||
Value 1: (-1.5e-14, 1.3e-15, -1.4e-16)
|
||||
Value 2: (2, 3.1e-16, -8.5e-17)
|
||||
Value 3: (4, -6e-16, -8.2e-17)
|
||||
|
||||
x1 covariance:
|
||||
0.0083 4.3e-19 -1.1e-18
|
||||
4.3e-19 0.0094 -0.0031
|
||||
-1.1e-18 -0.0031 0.0082
|
||||
x2 covariance:
|
||||
0.0071 2.5e-19 -3.4e-19
|
||||
2.5e-19 0.0078 -0.0011
|
||||
-3.4e-19 -0.0011 0.0082
|
||||
x3 covariance:
|
||||
0.0083 4.4e-19 1.2e-18
|
||||
4.4e-19 0.0094 0.0031
|
||||
1.2e-18 0.0031 0.018
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
// Create an empty nonlinear factor graph
|
||||
NonlinearFactorGraph graph;
|
||||
|
||||
// Add a Gaussian prior on pose x_1
|
||||
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));
|
||||
|
||||
// Add two odometry factors
|
||||
Pose2 odometry(2.0, 0.0, 0.0);
|
||||
noiseModel::Diagonal::shared_ptr odometryNoise =
|
||||
noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
|
||||
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
|
||||
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
// Query the marginals
|
||||
cout.precision(2);
|
||||
Marginals marginals(graph, result);
|
||||
cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl;
|
||||
cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl;
|
||||
cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl;
|
||||
|
|
@ -0,0 +1,9 @@
|
|||
// create (deliberatly inaccurate) initial estimate
|
||||
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));
|
||||
|
||||
// optimize using Levenberg-Marquardt optimization
|
||||
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
|
||||
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
Factor Graph:
|
||||
size: 3
|
||||
factor 0: PriorFactor on 1
|
||||
prior mean: (0, 0, 0)
|
||||
noise model: diagonal sigmas [0.3; 0.3; 0.1];
|
||||
factor 1: BetweenFactor(1,2)
|
||||
measured: (2, 0, 0)
|
||||
noise model: diagonal sigmas [0.2; 0.2; 0.1];
|
||||
factor 2: BetweenFactor(2,3)
|
||||
measured: (2, 0, 0)
|
||||
noise model: diagonal sigmas [0.2; 0.2; 0.1];
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
Initial Estimate:
|
||||
Values with 3 values:
|
||||
Value 1: (0.5, 0, 0.2)
|
||||
Value 2: (2.3, 0.1, -0.2)
|
||||
Value 3: (4.1, 0.1, 0.1)
|
||||
|
||||
Final Result:
|
||||
Values with 3 values:
|
||||
Value 1: (-1.8e-16, 8.7e-18, -9.1e-19)
|
||||
Value 2: (2, 7.4e-18, -2.5e-18)
|
||||
Value 3: (4, -1.8e-18, -3.1e-18)
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
x1 covariance:
|
||||
0.09 1.1e-47 5.7e-33
|
||||
1.1e-47 0.09 1.9e-17
|
||||
5.7e-33 1.9e-17 0.01
|
||||
x2 covariance:
|
||||
0.13 4.7e-18 2.4e-18
|
||||
4.7e-18 0.17 0.02
|
||||
2.4e-18 0.02 0.02
|
||||
x3 covariance:
|
||||
0.17 2.7e-17 8.4e-18
|
||||
2.7e-17 0.37 0.06
|
||||
8.4e-18 0.06 0.03
|
||||
|
|
@ -0,0 +1,25 @@
|
|||
% Create graph container and add factors to it
|
||||
graph = NonlinearFactorGraph;
|
||||
|
||||
% Create keys for variables
|
||||
i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
|
||||
j1 = symbol('l',1); j2 = symbol('l',2);
|
||||
|
||||
% Add prior
|
||||
priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
|
||||
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
|
||||
% add directly to graph
|
||||
graph.add(PriorFactorPose2(i1, priorMean, priorNoise));
|
||||
|
||||
% Add odometry
|
||||
odometry = Pose2(2.0, 0.0, 0.0);
|
||||
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
|
||||
graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
|
||||
graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
|
||||
|
||||
% Add bearing/range measurement factors
|
||||
degrees = pi/180;
|
||||
brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
|
||||
graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(8), brNoise));
|
||||
graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));
|
||||
graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));
|
||||
|
|
@ -0,0 +1,7 @@
|
|||
>> result
|
||||
Values with 5 values:
|
||||
l1: (2, 2)
|
||||
l2: (4, 2)
|
||||
x1: (-1.8e-16, 5.1e-17, -1.5e-17)
|
||||
x2: (2, -5.8e-16, -4.6e-16)
|
||||
x3: (4, -3.1e-15, -4.6e-16)
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
%% Initialize graph, initial estimate, and odometry noise
|
||||
datafile = findExampleDataFile('w100.graph');
|
||||
model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]);
|
||||
[graph,initial] = load2D(datafile, model);
|
||||
|
||||
%% Add a Gaussian prior on pose x_0
|
||||
priorMean = Pose2(0, 0, 0);
|
||||
priorNoise = noiseModel.Diagonal.Sigmas([0.01; 0.01; 0.01]);
|
||||
graph.add(PriorFactorPose2(0, priorMean, priorNoise));
|
||||
|
||||
%% Optimize using Levenberg-Marquardt optimization and get marginals
|
||||
optimizer = LevenbergMarquardtOptimizer(graph, initial);
|
||||
result = optimizer.optimizeSafely;
|
||||
marginals = Marginals(graph, result);
|
||||
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
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));
|
||||
|
||||
// Add odometry factors
|
||||
noiseModel::Diagonal::shared_ptr model =
|
||||
noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
|
||||
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
|
||||
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
|
||||
graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
|
||||
graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
|
||||
|
||||
// Add the loop closure constraint
|
||||
graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));
|
||||
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
graph = NonlinearFactorGraph;
|
||||
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
|
||||
graph.add(PriorFactorPose2(1, Pose2(0, 0, 0), priorNoise));
|
||||
|
||||
%% Add odometry factors
|
||||
model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
|
||||
graph.add(BetweenFactorPose2(1, 2, Pose2(2, 0, 0 ), model));
|
||||
graph.add(BetweenFactorPose2(2, 3, Pose2(2, 0, pi/2), model));
|
||||
graph.add(BetweenFactorPose2(3, 4, Pose2(2, 0, pi/2), model));
|
||||
graph.add(BetweenFactorPose2(4, 5, Pose2(2, 0, pi/2), model));
|
||||
|
||||
%% Add pose constraint
|
||||
graph.add(BetweenFactorPose2(5, 2, Pose2(2, 0, pi/2), model));
|
||||
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
%% Initialize graph, initial estimate, and odometry noise
|
||||
datafile = findExampleDataFile('sphere2500.txt');
|
||||
model = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.05]);
|
||||
[graph,initial] = load3D(datafile, model, true, 2500);
|
||||
plot3DTrajectory(initial, 'g-', false); % Plot Initial Estimate
|
||||
|
||||
%% Read again, now with all constraints, and optimize
|
||||
graph = load3D(datafile, model, false, 2500);
|
||||
graph.add(NonlinearEqualityPose3(0, initial.atPose3(0)));
|
||||
optimizer = LevenbergMarquardtOptimizer(graph, initial);
|
||||
result = optimizer.optimizeSafely();
|
||||
plot3DTrajectory(result, 'r-', false); axis equal;
|
||||
|
|
@ -0,0 +1,9 @@
|
|||
%% Add factors for all measurements
|
||||
noise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma);
|
||||
for i = 1:length(Z),
|
||||
for k = 1:length(Z{i})
|
||||
j = J{i}{k};
|
||||
G.add(GenericProjectionFactorCal3_S2(
|
||||
Z{i}{k}, noise, symbol('x', i), symbol('p', j), K));
|
||||
end
|
||||
end
|
||||
|
|
@ -0,0 +1,24 @@
|
|||
int relinearizeInterval = 3;
|
||||
NonlinearISAM isam(relinearizeInterval);
|
||||
|
||||
// ... first frame initialization omitted ...
|
||||
|
||||
// Loop over the different poses, adding the observations to iSAM
|
||||
for (size_t i = 1; i < poses.size(); ++i) {
|
||||
|
||||
// Add factors for each landmark observation
|
||||
NonlinearFactorGraph graph;
|
||||
for (size_t j = 0; j < points.size(); ++j) {
|
||||
graph.add(
|
||||
GenericProjectionFactor<Pose3, Point3, Cal3_S2>
|
||||
(z[i][j], noise,Symbol('x', i), Symbol('l', j), K)
|
||||
);
|
||||
}
|
||||
|
||||
// Add an initial guess for the current pose
|
||||
Values initialEstimate;
|
||||
initialEstimate.insert(Symbol('x', i), initial_x[i]);
|
||||
|
||||
// Update iSAM with the new factors
|
||||
isam.update(graph, initialEstimate);
|
||||
}
|
||||
|
|
@ -0,0 +1,7 @@
|
|||
>> graph.error(initialEstimate)
|
||||
ans =
|
||||
20.1086
|
||||
|
||||
>> graph.error(result)
|
||||
ans =
|
||||
8.2631e-18
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
>> priorNoise
|
||||
diagonal sigmas [0.3; 0.3; 0.1];
|
||||
|
||||
>> graph
|
||||
size: 6
|
||||
factor 0: PriorFactor on 1
|
||||
prior mean: (0, 0, 0)
|
||||
noise model: diagonal sigmas [0.3; 0.3; 0.1];
|
||||
factor 1: BetweenFactor(1,2)
|
||||
measured: (2, 0, 0)
|
||||
noise model: diagonal sigmas [0.2; 0.2; 0.1];
|
||||
factor 2: BetweenFactor(2,3)
|
||||
measured: (2, 0, 1.6)
|
||||
noise model: diagonal sigmas [0.2; 0.2; 0.1];
|
||||
factor 3: BetweenFactor(3,4)
|
||||
measured: (2, 0, 1.6)
|
||||
noise model: diagonal sigmas [0.2; 0.2; 0.1];
|
||||
factor 4: BetweenFactor(4,5)
|
||||
measured: (2, 0, 1.6)
|
||||
noise model: diagonal sigmas [0.2; 0.2; 0.1];
|
||||
factor 5: BetweenFactor(5,2)
|
||||
measured: (2, 0, 1.6)
|
||||
noise model: diagonal sigmas [0.2; 0.2; 0.1];
|
||||
|
|
@ -0,0 +1,7 @@
|
|||
>> whos
|
||||
Name Size Bytes Class
|
||||
graph 1x1 112 gtsam.NonlinearFactorGraph
|
||||
priorNoise 1x1 112 gtsam.noiseModel.Diagonal
|
||||
model 1x1 112 gtsam.noiseModel.Diagonal
|
||||
initialEstimate 1x1 112 gtsam.Values
|
||||
optimizer 1x1 112 gtsam.LevenbergMarquardtOptimizer
|
||||
|
|
@ -0,0 +1,124 @@
|
|||
\global\long\def\Vector#1{{\bf #1}}
|
||||
\global\long\def\Matrix#1{{\bf #1}}
|
||||
|
||||
|
||||
\global\long\def\eq#1{equation (\ref{eq:=0000231})}
|
||||
|
||||
|
||||
\global\long\def\eye#1{\Vector{I_{#1}}}
|
||||
|
||||
|
||||
\global\long\def\leftsparrow#1{\stackrel{#1}{\leftarrow}}
|
||||
\global\long\def\rightsparrow#1{\stackrel{#1}{\rightarrow}}
|
||||
\global\long\def\chain{\mathcal{M}}
|
||||
|
||||
|
||||
\global\long\def\define{\stackrel{\Delta}{=}}
|
||||
|
||||
|
||||
\global\long\def\argmin#1{\mathop{\textrm{argmin \,}}_{#1}}
|
||||
|
||||
|
||||
\global\long\def\Norm#1{\Vert#1\Vert}
|
||||
\global\long\def\SqrNorm#1{\Vert#1\Vert^{2}}
|
||||
\global\long\def\Ltwo#1{\mathcal{L}^{2}\left(#1\right)}
|
||||
|
||||
|
||||
\global\long\def\Normal#1#2#3{\mathcal{N}(#1;#2,#3)}
|
||||
|
||||
|
||||
\global\long\def\LogNormal#1#2#3{ (#1-#2)^{T} #3^{-1} (#1-#2) }
|
||||
|
||||
|
||||
\global\long\def\SqrMah#1#2#3{\Vert{#1}-{#2}\Vert_{#3}^{2}}
|
||||
|
||||
|
||||
\global\long\def\SqrZMah#1#2{\Vert{#1}\Vert_{#2}^{2}}
|
||||
|
||||
|
||||
\global\long\def\Info#1#2#3{\mathcal{N}^{-1}(#1;#2,#3)}
|
||||
|
||||
|
||||
\providecommand{\half}{\frac{1}{2}}
|
||||
|
||||
\global\long\def\Mah#1#2#3{\Vert{#1}-{#2}\Vert_{#3}}
|
||||
\global\long\def\MahDeriv#1#2#3#4{\biggl(\deriv{#2}{#4}\biggr)^{T} #3^{-1} (#1-#2)}
|
||||
|
||||
|
||||
\global\long\def\argmin#1{\mathop{\textrm{argmin \,}}_{#1}}
|
||||
\global\long\def\argmax#1{\mathop{\textrm{argmax \,}}_{#1}}
|
||||
|
||||
|
||||
|
||||
|
||||
\global\long\def\deriv#1#2{\frac{\partial#1}{\partial#2}}
|
||||
|
||||
|
||||
\global\long\def\at#1#2{#1\biggr\rvert_{#2}}
|
||||
|
||||
|
||||
\global\long\def\Jac#1#2#3{ \at{\deriv{#1}{#2}} {#3} }
|
||||
|
||||
|
||||
|
||||
|
||||
\global\long\def\Rone{\mathbb{R}}
|
||||
\global\long\def\Pone{\mathbb{P}}
|
||||
|
||||
|
||||
\global\long\def\Rtwo{\mathbb{R}^{2}}
|
||||
\global\long\def\Ptwo{\mathbb{P}^{2}}
|
||||
|
||||
|
||||
\global\long\def\Stwo{\mathbb{S}^{2}}
|
||||
\global\long\def\Complex{\mathbb{C}}
|
||||
|
||||
|
||||
\global\long\def\Z{\mathbb{Z}}
|
||||
\global\long\def\Rplus{\mathbb{R}^{+}}
|
||||
|
||||
|
||||
\global\long\def\SOtwo{SO(2)}
|
||||
\global\long\def\sotwo{\mathfrak{so(2)}}
|
||||
\global\long\def\skew#1{[#1]_{+}}
|
||||
|
||||
|
||||
\global\long\def\SEtwo{SE(2)}
|
||||
\global\long\def\setwo{\mathfrak{se(2)}}
|
||||
\global\long\def\Skew#1{[#1]_{\times}}
|
||||
|
||||
|
||||
\global\long\def\Rthree{\mathbb{R}^{3}}
|
||||
\global\long\def\Pthree{\mathbb{P}^{3}}
|
||||
|
||||
|
||||
\global\long\def\SOthree{SO(3)}
|
||||
\global\long\def\sothree{\mathfrak{so(3)}}
|
||||
|
||||
|
||||
\global\long\def\Rsix{\mathbb{R}^{6}}
|
||||
\global\long\def\SEthree{SE(3)}
|
||||
\global\long\def\sethree{\mathfrak{se(3)}}
|
||||
|
||||
|
||||
\global\long\def\Rn{\mathbb{R}^{n}}
|
||||
|
||||
|
||||
\global\long\def\Afftwo{Aff(2)}
|
||||
\global\long\def\afftwo{\mathfrak{aff(2)}}
|
||||
|
||||
|
||||
\global\long\def\SLthree{SL(3)}
|
||||
\global\long\def\slthree{\mathfrak{sl(3)}}
|
||||
|
||||
|
||||
|
||||
|
||||
\global\long\def\stirling#1#2{\genfrac{\{}{\}}{0pt}{}{#1}{#2}}
|
||||
|
||||
|
||||
\global\long\def\matlabscript#1#2{\begin{itemize}\item[]\lstinputlisting[caption=#2,label=#1]{#1.m}\end{itemize}}
|
||||
|
||||
|
||||
\global\long\def\atan{\mathop{atan2}}
|
||||
|
||||
|
|
@ -0,0 +1,133 @@
|
|||
@String { ICCV = {Intl. Conf. on Computer Vision (ICCV)} }
|
||||
@String { IROS = {IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS)} }
|
||||
@String { CVPR = {IEEE Conf. on Computer Vision and Pattern Recognition (CVPR)} }
|
||||
@String { IJRR = {Intl. J. of Robotics Research} }
|
||||
@String { RAS = {Robotics and Autonomous Systems} }
|
||||
@String { TRO = {{IEEE} Trans. Robotics} }
|
||||
@String { IT = {{IEEE} Trans. Inform. Theory} }
|
||||
@String { ISRR = {Proc. of the Intl. Symp. of Robotics Research (ISRR)} }
|
||||
|
||||
@inproceedings{Davison03iccv,
|
||||
title = {Real-Time Simultaneous Localisation and Mapping with a Single Camera},
|
||||
author = {A.J. Davison},
|
||||
booktitle = ICCV,
|
||||
year = {2003},
|
||||
month = {Oct},
|
||||
pages = {1403-1410}
|
||||
}
|
||||
|
||||
@inproceedings{Dellaert10iros,
|
||||
title = {Subgraph-preconditioned Conjugate Gradient for Large Scale SLAM},
|
||||
author = {F. Dellaert and J. Carlson and V. Ila and K. Ni and C.E. Thorpe},
|
||||
booktitle = IROS,
|
||||
year = {2010},
|
||||
}
|
||||
|
||||
@inproceedings{Dellaert99b,
|
||||
title = {Using the Condensation Algorithm for Robust, Vision-based Mobile Robot Localization},
|
||||
author = {F. Dellaert and D. Fox and W. Burgard and S. Thrun},
|
||||
booktitle = CVPR,
|
||||
year = {1999}
|
||||
}
|
||||
|
||||
@article{Dellaert06ijrr,
|
||||
title = {Square {Root} {SAM}: Simultaneous Localization and Mapping via Square Root Information Smoothing},
|
||||
author = {F. Dellaert and M. Kaess},
|
||||
journal = IJRR,
|
||||
year = {2006},
|
||||
month = {Dec},
|
||||
number = {12},
|
||||
pages = {1181--1203},
|
||||
volume = {25},
|
||||
}
|
||||
|
||||
@article{DurrantWhyte06ram,
|
||||
title = {Simultaneous Localisation and Mapping ({SLAM}): Part {I} The Essential Algorithms},
|
||||
author = {H.F. Durrant-Whyte and T. Bailey},
|
||||
journal = {Robotics \& Automation Magazine},
|
||||
year = {2006},
|
||||
month = {Jun},
|
||||
}
|
||||
|
||||
@inproceedings{Jian11iccv,
|
||||
title = {Generalized Subgraph Preconditioners for Large-Scale Bundle Adjustment},
|
||||
author = {Y.-D. Jian and D. Balcan and F. Dellaert},
|
||||
booktitle = ICCV,
|
||||
year = {2011},
|
||||
}
|
||||
|
||||
@article{Kaess09ras,
|
||||
title = {Covariance Recovery from a Square Root Information Matrix for Data Association},
|
||||
author = {M. Kaess and F. Dellaert},
|
||||
journal = RAS,
|
||||
year = {2009},
|
||||
}
|
||||
|
||||
@article{Kaess12ijrr,
|
||||
title = {{iSAM2}: Incremental Smoothing and Mapping Using the {B}ayes Tree},
|
||||
author = {M. Kaess and H. Johannsson and R. Roberts and V. Ila and J. Leonard and F. Dellaert},
|
||||
journal = IJRR,
|
||||
year = {2012},
|
||||
month = {Feb},
|
||||
pages = {217--236},
|
||||
volume = {31},
|
||||
issue = {2},
|
||||
}
|
||||
|
||||
@article{Kaess08tro,
|
||||
title = {{iSAM}: Incremental Smoothing and Mapping},
|
||||
author = {M. Kaess and A. Ranganathan and F. Dellaert},
|
||||
journal = TRO,
|
||||
year = {2008},
|
||||
month = {Dec},
|
||||
number = {6},
|
||||
pages = {1365-1378},
|
||||
volume = {24},
|
||||
}
|
||||
|
||||
@book{Koller09book,
|
||||
title = {Probabilistic Graphical Models: Principles and Techniques},
|
||||
author = {D. Koller and N. Friedman},
|
||||
publisher = {The MIT Press},
|
||||
year = {2009}
|
||||
}
|
||||
|
||||
@Article{Kschischang01it,
|
||||
title = {Factor Graphs and the Sum-Product Algorithm},
|
||||
Author = {F.R. Kschischang and B.J. Frey and H-A. Loeliger},
|
||||
Journal = IT,
|
||||
Year = {2001},
|
||||
|
||||
Month = {February},
|
||||
Number = {2},
|
||||
Volume = {47}
|
||||
}
|
||||
|
||||
@article{Loeliger04spm,
|
||||
Title = {An Introduction to Factor Graphs},
|
||||
Author = {H.-A. Loeliger},
|
||||
Journal = {IEEE Signal Processing Magazine},
|
||||
Year = {2004},
|
||||
|
||||
Month = {January},
|
||||
Pages = {28--41}
|
||||
}
|
||||
|
||||
@inproceedings{Nister04cvpr2,
|
||||
title = {Visual Odometry},
|
||||
author = {D. Nist\'er and O. Naroditsky and J. Bergen},
|
||||
booktitle = CVPR,
|
||||
year = {2004},
|
||||
month = {Jun},
|
||||
pages = {652-659},
|
||||
volume = {1}
|
||||
}
|
||||
|
||||
@InProceedings{Smith87b,
|
||||
title = {A stochastic map for uncertain spatial relationships},
|
||||
Author = {R. Smith and M. Self and P. Cheeseman},
|
||||
Booktitle = ISRR,
|
||||
Year = {1988},
|
||||
Pages = {467-474}
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -0,0 +1,101 @@
|
|||
%PDF-1.4
|
||||
%Çì<C387>¢
|
||||
5 0 obj
|
||||
<</Length 6 0 R/Filter /FlateDecode>>
|
||||
stream
|
||||
xœÍ˜ÏŠ$7 ÆïõuLQlKþw
„œgg °<>
a&°ä<C2B0>×<EFBFBD>ì²,¹«Yº‡Þ!»,ßÈ®Ÿ¿’,Õ~Ý øݵ¿ãçç·íçOyÿòÏöu 1aÙ“‹JÙߦâK"ð~Ýb‰R¸¢Èª×íÏí÷ýï<C3BD> ¶?û¿›ßÿâ¿mµ$@ç÷€!Cʼ¿(1G¶ú>]QdÕëiŸ×íysW÷?¯}>=óíÊ3Ÿob}>íë^ßâúö½K‚ šBNP‹<50>ÑNÁ9(aøõ…߸並<C2A9> Ÿßö_^8 J^þØŽ ñ{%H…Ê ‚‡êËþò¶ýð“ƒøãË_Û¯/ÛÓæ)UˆÕRMÅ`
ÍpMEÁn…òÄ VÒ唨d„š-ÑT ÑÐ ÑTÞAT PŕȸB* `˜TQ&Ñ”I•û™B (9/L^‰Rd
ÑT ÑÐ ÑTÞA” `Y]òÆ%t)Z—TQ&Ñ”I•û™Ðä´º”ˆ²‡l]RÅ
ÍMå Dä äÕ¥`]*ùââQÅ0
Í0Må L%B¢• '…ìøš0Dª(‘hJ¤ÊýDx“D+‘q‰RŽà¬Kª¦¡¦©¼ƒ‰¯Úˆ+M¢èÚ<C3A8>g]RE‰D3-j*÷E^ââ‘qi6 ž_ ¿jU–V7£bH
|
||||
-Q·2¾–‚çšæ<O1 -%œ‘
|
||||
ÿžÑ
ÒP¤5‘LÔHüÊ+µË¸xp5 HþŠO°g‡B‰b¡4J lÔ½>ahçן‘V h‘†² ͨ‰d¢îõ ù"o
ø4
9ì½T¡D±P%P6êæÅAF~iĹÞÖu˜Dü+@k“( ÑŒšD&êæ7硤Py-lN§i`2
|
||||
Ìn™†²0ͨÉd¢îv‰áÐÓÅ40ÇM¾Š X—D±D%D6ê^—"çwNé4
L¦Dž›<C5BE>eÊÂ4£&“‰º×¥˜</¦<>ÿëGÆŸïüT}‚ÏPú×THàÂYн®}¾µÏ+™ÖÉB±3—V¬tN[ÃOßùÓò`#gÀ<Õ
|
||||
ÜêD±Ÿ äxħ`>?Èy‚@c°Äæ‹ÞJèjïËöDzîÄãF\œàQe® ”Í^”ù·).{©c²—*ºŸ¯r¥˜(O¥r³†[ÏFÓ(Y§Q<íö<"Ríô%ƒFˆ¦Q²N£xÚ‰€d¢ØS ¦ëE”h%ë4ªÕžÛ?=ðe>Òµï]_röÈ&¡=>÷ýöù§Ç×M£sqž×qTÌ\Y¢D9ßvçÏÍÐ^|EðÔ•±“£åø
|
||||
ðg%ynBÔFšù’øW€¡+ÃD£„öU³¬â~ÍO%<25>º€¥Ï·ä=a殤6<C2A4>1NAÅ(X9«b0«°òÍQÑlŒ•Úÿ|è³±bO<åÃz™êŒ2ÎiV
/¸AŒaNüÂZzq¿O%/}ÿ˜kq¤Ô¯´G^ TP<çðŽ¶i‰píðÓ¢+Až¯å²¥N¢µƒœ ÇP+µ#Í|FÏø1kÊ£wÇ@7«bºený•Ì"ç8é¼)ÉPË(R‡à|WrO¹#›Â©šÓ|1V‘›NWÉk—<6B>±æ~‹ëÓµpÖ›œA+GÎi•Ã
|
||||
]t˜¥e,~jm©ç’I—ïåCkë‘=þ‘½ôƒŠ‹2uß´º(3Vµ Æ\3Dm»\ÇQ¹t»l”(Rq”s·A+nî4Kƒ2vc´žæKš½išhî9ÚUÉ ¹'UH±¬ÅM1 Å=›•(Ú¬Æ"MrÙv–<76><Y+Eè´3É ¬rœÒ¬Nh™Š[ZOê¨$Ï¥ëG:=mÿF3#òendstream
|
||||
endobj
|
||||
6 0 obj
|
||||
1341
|
||||
endobj
|
||||
4 0 obj
|
||||
<</Type/Page/MediaBox [0 0 612 792]
|
||||
/Rotate 0/Parent 3 0 R
|
||||
/Resources<</ProcSet[/PDF /Text]
|
||||
/ExtGState 9 0 R
|
||||
/Font 10 0 R
|
||||
>>
|
||||
/Contents 5 0 R
|
||||
>>
|
||||
endobj
|
||||
3 0 obj
|
||||
<< /Type /Pages /Kids [
|
||||
4 0 R
|
||||
] /Count 1
|
||||
>>
|
||||
endobj
|
||||
1 0 obj
|
||||
<</Type /Catalog /Pages 3 0 R
|
||||
/Metadata 12 0 R
|
||||
>>
|
||||
endobj
|
||||
7 0 obj
|
||||
<</Type/ExtGState
|
||||
/OPM 1>>endobj
|
||||
9 0 obj
|
||||
<</R7
|
||||
7 0 R>>
|
||||
endobj
|
||||
10 0 obj
|
||||
<</R8
|
||||
8 0 R>>
|
||||
endobj
|
||||
8 0 obj
|
||||
<</BaseFont/Helvetica/Type/Font
|
||||
/Encoding 11 0 R/Subtype/Type1>>
|
||||
endobj
|
||||
11 0 obj
|
||||
<</Type/Encoding/Differences[
|
||||
45/minus]>>
|
||||
endobj
|
||||
12 0 obj
|
||||
<</Length 1324>>stream
|
||||
<?xpacket begin='' id='W5M0MpCehiHzreSzNTczkc9d'?>
|
||||
<?adobe-xap-filters esc="CRLF"?>
|
||||
<x:xmpmeta xmlns:x='adobe:ns:meta/' x:xmptk='XMP toolkit 2.9.1-13, framework 1.6'>
|
||||
<rdf:RDF xmlns:rdf='http://www.w3.org/1999/02/22-rdf-syntax-ns#' xmlns:iX='http://ns.adobe.com/iX/1.0/'>
|
||||
<rdf:Description rdf:about='6ea0c85a-ec69-11ec-0000-88f4c9fe0d6d' xmlns:pdf='http://ns.adobe.com/pdf/1.3/' pdf:Producer='Artifex Ghostscript 8.54'/>
|
||||
<rdf:Description rdf:about='6ea0c85a-ec69-11ec-0000-88f4c9fe0d6d' xmlns:xap='http://ns.adobe.com/xap/1.0/' xap:ModifyDate='2012-06-12' xap:CreateDate='2012-06-12'><xap:CreatorTool>Artifex Ghostscript 8.54 PDF Writer</xap:CreatorTool></rdf:Description>
|
||||
<rdf:Description rdf:about='6ea0c85a-ec69-11ec-0000-88f4c9fe0d6d' xmlns:xapMM='http://ns.adobe.com/xap/1.0/mm/' xapMM:DocumentID='6ea0c85a-ec69-11ec-0000-88f4c9fe0d6d'/>
|
||||
<rdf:Description rdf:about='6ea0c85a-ec69-11ec-0000-88f4c9fe0d6d' xmlns:dc='http://purl.org/dc/elements/1.1/' dc:format='application/pdf'><dc:title><rdf:Alt><rdf:li xml:lang='x-default'>/private/tmp/tpfd5bb1cf_55d8_447e_8f97_d6b24a262922.ps</rdf:li></rdf:Alt></dc:title></rdf:Description>
|
||||
</rdf:RDF>
|
||||
</x:xmpmeta>
|
||||
|
||||
|
||||
<?xpacket end='w'?>
|
||||
endstream
|
||||
endobj
|
||||
2 0 obj
|
||||
<</Producer(Artifex Ghostscript 8.54)
|
||||
/CreationDate(D:20120612011010)
|
||||
/ModDate(D:20120612011010)
|
||||
/Creator(MATLAB, The MathWorks, Inc. Version 7.13.0.564 \(R2011b\). Operating System: Darwin 11.4.0 Darwin Kernel Version 11.4.0: Mon Apr 9 19:32:15 PDT 2012; root:xnu-1699.26.8~1/RELEASE_X86_64 x86_64.)
|
||||
/Title(/private/tmp/tpfd5bb1cf_55d8_447e_8f97_d6b24a262922.ps)>>endobj
|
||||
xref
|
||||
0 13
|
||||
0000000000 65535 f
|
||||
0000001664 00000 n
|
||||
0000003341 00000 n
|
||||
0000001605 00000 n
|
||||
0000001446 00000 n
|
||||
0000000015 00000 n
|
||||
0000001426 00000 n
|
||||
0000001729 00000 n
|
||||
0000001829 00000 n
|
||||
0000001770 00000 n
|
||||
0000001799 00000 n
|
||||
0000001909 00000 n
|
||||
0000001967 00000 n
|
||||
trailer
|
||||
<< /Size 13 /Root 1 0 R /Info 2 0 R
|
||||
/ID [<0E15732D337F57A9CB2C0954A12E8EF8><0E15732D337F57A9CB2C0954A12E8EF8>]
|
||||
>>
|
||||
startxref
|
||||
3722
|
||||
%%EOF
|
||||
Binary file not shown.
Binary file not shown.
File diff suppressed because one or more lines are too long
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -5189,7 +5189,7 @@ R_{2}^{T}\left[t_{2}-t_{1}\right]_{\times}R_{1} & -R_{2}^{T}R_{1}
|
|||
and in its second argument,
|
||||
\begin_inset Formula
|
||||
\begin{eqnarray*}
|
||||
\frac{\partial\left(T_{1}^{^{-1}}T_{2}\right)}{\partial\xi_{1}} & = & I_{6}
|
||||
\frac{\partial\left(T_{1}^{^{-1}}T_{2}\right)}{\partial\xi_{2}} & = & I_{6}
|
||||
\end{eqnarray*}
|
||||
|
||||
\end_inset
|
||||
|
|
|
|||
|
|
@ -0,0 +1,18 @@
|
|||
# Get the base Ubuntu image from Docker Hub
|
||||
FROM ubuntu:bionic
|
||||
|
||||
# Update apps on the base image
|
||||
RUN apt-get -y update && apt-get install -y
|
||||
|
||||
# Install C++
|
||||
RUN apt-get -y install build-essential
|
||||
|
||||
# Install boost and cmake
|
||||
RUN apt-get -y install libboost-all-dev cmake
|
||||
|
||||
# Install TBB
|
||||
RUN apt-get -y install libtbb-dev
|
||||
|
||||
# Install latest Eigen
|
||||
RUN apt-get install -y libeigen3-dev
|
||||
|
||||
|
|
@ -0,0 +1,812 @@
|
|||
i 1 0.485693 0.334961 10.0536 0.00585324 0.00159634 0.00532113
|
||||
i 1 0.449805 0.346924 10.0536 0.00744958 0.00159634 0.00478901
|
||||
i 1 0.430664 0.351709 10.0249 0.00904591 0.00159634 0.0042569
|
||||
i 1 0.442627 0.346924 9.99858 0.0106423 0.00159634 0.00478901
|
||||
i 1 0.449805 0.344531 9.99619 0.0117065 0.00159634 0.00478901
|
||||
i 1 0.44502 0.330176 10.0034 0.0127707 0.00159634 0.00478901
|
||||
i 1 0.411523 0.325391 9.99141 0.0138349 0.00159634 0.0042569
|
||||
i 1 0.411523 0.322998 9.98184 0.014367 0.00106423 0.00372479
|
||||
i 1 0.413916 0.325391 9.97944 0.0148992 0.000532113 0.00266056
|
||||
i 1 0.413916 0.332568 9.9938 0.0159634 0 0.00212845
|
||||
i 1 0.385205 0.327783 9.97944 0.0175597 0.000532113 0.00159634
|
||||
i 1 0.358887 0.332568 9.93398 0.0186239 0.00106423 0.00106423
|
||||
i 1 0.368457 0.327783 9.8981 0.0196882 0.00212845 0.00159634
|
||||
i 1 0.37085 0.342139 9.88613 0.0212845 0.00266056 0.00212845
|
||||
i 1 0.361279 0.346924 9.88374 0.0223487 0.00319268 0.00266056
|
||||
i 1 0.337354 0.356494 9.86221 0.023413 0.00266056 0.00266056
|
||||
i 1 0.330176 0.37085 9.82632 0.0244772 0.00212845 0.00266056
|
||||
i 1 0.349316 0.38042 9.80957 0.0250093 0.00159634 0.00266056
|
||||
i 1 0.361279 0.392383 9.81675 0.0255414 0.00159634 0.00266056
|
||||
i 1 0.344531 0.399561 9.80239 0.0266056 0.00159634 0.00266056
|
||||
i 1 0.339746 0.394775 9.78086 0.0276699 0.00212845 0.00212845
|
||||
i 1 0.330176 0.387598 9.74736 0.028202 0.00266056 0.00212845
|
||||
i 1 0.332568 0.382812 9.74736 0.028202 0.00319268 0.00212845
|
||||
i 1 0.313428 0.373242 9.75933 0.028202 0.00319268 0.00212845
|
||||
i 1 0.282324 0.361279 9.74497 0.0276699 0.00319268 0.00266056
|
||||
i 1 0.284717 0.363672 9.74497 0.028202 0.00319268 0.00319268
|
||||
i 1 0.303857 0.358887 9.74497 0.0287341 0.00266056 0.00372479
|
||||
i 1 0.327783 0.363672 9.77129 0.0303304 0.00319268 0.00478901
|
||||
i 2 0.313428 0.368457 9.77129 0.0313946 0.00372479 0.00532113
|
||||
i 2 0.313428 0.37085 9.74019 0.0324589 0.00478901 0.00585324
|
||||
i 2 0.322998 0.368457 9.71387 0.0335231 0.00532113 0.00691746
|
||||
i 2 0.344531 0.382812 9.70669 0.032991 0.00585324 0.00744958
|
||||
i 2 0.342139 0.38999 9.72344 0.032991 0.00585324 0.0085138
|
||||
i 2 0.322998 0.399561 9.71147 0.0324589 0.00532113 0.0085138
|
||||
i 2 0.322998 0.409131 9.69233 0.0324589 0.00478901 0.0085138
|
||||
i 2 0.349316 0.409131 9.69233 0.0324589 0.0042569 0.0085138
|
||||
i 2 0.358887 0.416309 9.71865 0.0324589 0.00372479 0.0085138
|
||||
i 2 0.361279 0.418701 9.73779 0.032991 0.00372479 0.0085138
|
||||
i 2 0.361279 0.430664 9.73779 0.0335231 0.0042569 0.00904591
|
||||
i 2 0.382812 0.437842 9.72822 0.0340552 0.00478901 0.00904591
|
||||
i 2 0.411523 0.452197 9.73301 0.0340552 0.00532113 0.00957803
|
||||
i 2 0.416309 0.449805 9.74258 0.0340552 0.00585324 0.00957803
|
||||
i 2 0.399561 0.45459 9.72583 0.0340552 0.00638535 0.00957803
|
||||
i 2 0.406738 0.452197 9.7043 0.0335231 0.00691746 0.00904591
|
||||
i 2 0.433057 0.45459 9.69233 0.0335231 0.00744958 0.0085138
|
||||
i 2 0.456982 0.452197 9.7019 0.032991 0.00744958 0.0085138
|
||||
i 2 0.461768 0.449805 9.69473 0.032991 0.00798169 0.00798169
|
||||
i 2 0.452197 0.437842 9.65884 0.032991 0.00798169 0.00744958
|
||||
i 2 0.466553 0.430664 9.62534 0.0335231 0.0085138 0.00691746
|
||||
i 2 0.483301 0.428271 9.62534 0.032991 0.0085138 0.00585324
|
||||
i 2 0.480908 0.433057 9.64448 0.032991 0.0085138 0.00532113
|
||||
i 2 0.46416 0.437842 9.64688 0.032991 0.0085138 0.00478901
|
||||
i 2 0.45459 0.449805 9.6397 0.032991 0.0085138 0.00478901
|
||||
i 2 0.478516 0.461768 9.6397 0.032991 0.0085138 0.00478901
|
||||
i 2 0.483301 0.471338 9.65405 0.032991 0.00904591 0.0042569
|
||||
s 2 1 885 772.028 190.341
|
||||
s 2 5 467 386.865 243.455
|
||||
s 2 8 814 710.317 241.333
|
||||
s 2 9 820 715.802 246.233
|
||||
s 2 10 815 711.209 245.21
|
||||
s 2 11 883 770.583 198.045
|
||||
s 2 12 877 764.741 195.175
|
||||
s 2 14 877 764.741 195.175
|
||||
s 2 17 941 820.999 242.057
|
||||
s 2 19 948 827.372 235.193
|
||||
s 2 20 948 827.445 241.199
|
||||
s 2 23 557 473.955 298.317
|
||||
s 2 28 760 663.371 287.116
|
||||
s 2 33 813 709.517 280.212
|
||||
s 2 34 818 713.39 277.34
|
||||
s 2 36 815 711.652 286.066
|
||||
s 2 37 893 779.04 309.272
|
||||
s 2 39 899 784.538 308.196
|
||||
s 2 40 908 793.502 319.35
|
||||
s 2 41 902 787.564 319.381
|
||||
s 2 42 938 819.201 257.197
|
||||
s 2 43 989 863.847 282.826
|
||||
s 2 44 972 848.495 314.307
|
||||
s 2 46 975 852.26 260.734
|
||||
s 2 51 232 144.289 321.996
|
||||
s 2 53 598 513.866 338.268
|
||||
s 2 54 693 603.198 367.966
|
||||
s 2 58 815 710.993 377.494
|
||||
s 2 59 799 697.398 356.37
|
||||
s 2 63 893 779.706 351.987
|
||||
s 2 67 900 786.329 357.122
|
||||
s 2 68 898 784.262 353.125
|
||||
s 2 70 916 800.114 348.207
|
||||
s 2 71 986 861.517 326.212
|
||||
s 2 72 986 861.517 326.212
|
||||
s 2 73 976 854.046 367.861
|
||||
s 2 79 565 493.761 419.421
|
||||
s 2 80 560 488.614 422.453
|
||||
s 2 81 575 501.453 445.367
|
||||
s 2 83 602 525.726 413.384
|
||||
s 2 86 701 611.86 434.26
|
||||
s 2 87 701 611.86 434.26
|
||||
s 2 88 653 570.815 405.361
|
||||
s 2 91 747 652.271 432.316
|
||||
s 2 92 754 658.581 441.421
|
||||
s 2 93 708 618.271 430.162
|
||||
s 2 94 744 649.626 434.257
|
||||
s 2 95 823 718.362 385.033
|
||||
s 2 96 797 696.56 426.269
|
||||
s 2 97 801 700.128 396.737
|
||||
s 2 98 773 675.088 390.936
|
||||
s 2 101 885 773.221 414.264
|
||||
s 2 102 880 768.822 387.314
|
||||
s 2 103 888 776.039 410.211
|
||||
s 2 105 950 831.12 397.844
|
||||
s 2 106 907 792.351 408.117
|
||||
s 2 116 405 331.508 456.012
|
||||
s 2 118 402 328.617 500.451
|
||||
s 2 119 400 326.365 502.275
|
||||
s 2 121 476 406.683 478.349
|
||||
s 2 127 585 511.319 485.324
|
||||
s 2 128 617 538.861 453.39
|
||||
s 2 130 582 508.155 452.366
|
||||
s 2 131 643 561.073 503.265
|
||||
s 2 132 703 613.588 462.253
|
||||
s 2 134 766 669.159 502.063
|
||||
s 2 136 772 674.493 495.248
|
||||
s 2 137 781 682.123 471.232
|
||||
s 2 138 784 685.118 468.173
|
||||
s 2 139 781 682.191 474.374
|
||||
s 2 141 885 773.615 494.269
|
||||
s 2 145 888 776.431 492.348
|
||||
s 2 147 904 790.471 502.254
|
||||
s 2 148 902 788.507 504.272
|
||||
s 2 149 938 820.465 502.391
|
||||
s 2 152 963 842.721 503.376
|
||||
s 2 156 242 148.008 545.764
|
||||
s 2 158 248 154.208 545.888
|
||||
s 2 162 314 224.505 546.204
|
||||
s 2 163 315 225.905 530.765
|
||||
s 2 164 315 225.261 552.166
|
||||
s 2 166 368 291.55 564.057
|
||||
s 2 171 571 498.553 561.333
|
||||
s 2 172 572 499.545 563.335
|
||||
s 2 173 638 556.502 563.852
|
||||
s 2 175 636 554.844 552.416
|
||||
s 2 176 641 559.993 556.383
|
||||
s 2 178 692 604.702 533.137
|
||||
s 2 180 722 630.968 528.786
|
||||
s 2 181 716 624.803 518.298
|
||||
s 2 182 764 666.755 536.361
|
||||
s 2 185 769 671.642 537.317
|
||||
s 2 187 810 708.021 538.302
|
||||
s 2 189 841 735.447 558.082
|
||||
s 2 190 881 770.988 555.105
|
||||
s 2 192 860 754.238 555.186
|
||||
s 2 193 938 820.657 574.383
|
||||
s 2 194 948 829.586 514.034
|
||||
s 2 195 915 800.701 574.39
|
||||
s 2 196 904 790.859 538.891
|
||||
s 2 197 976 854.831 525.328
|
||||
s 2 204 321 230.814 584.287
|
||||
s 2 205 322 231.43 590.824
|
||||
s 2 210 507 413.694 594.058
|
||||
s 2 216 586 512.591 601.786
|
||||
s 2 217 585 511.221 598.227
|
||||
s 2 218 657 574.472 587.268
|
||||
s 2 219 675 589.664 581.33
|
||||
s 2 220 656 573.502 582.378
|
||||
s 2 221 651 568.578 581.394
|
||||
s 2 223 715 624.955 635.236
|
||||
s 2 224 828 724.063 600.264
|
||||
s 2 225 813 710.869 596.413
|
||||
s 2 227 882 771.755 624.292
|
||||
s 2 231 977 855.606 577.181
|
||||
s 2 233 252 132.188 646.925
|
||||
s 2 234 253 133.288 649.949
|
||||
s 2 236 264 144.633 647.915
|
||||
s 2 237 335 226.123 646.194
|
||||
s 2 240 448 356.089 665.294
|
||||
s 2 241 465 362.168 676.569
|
||||
s 2 242 481 383.624 673.992
|
||||
s 2 244 478 380.89 673.024
|
||||
s 2 245 562 458.491 663.422
|
||||
s 2 246 565 461.494 669.205
|
||||
s 2 247 548 446.265 666.974
|
||||
s 2 249 607 530.299 640.373
|
||||
s 2 250 613 512.347 667.362
|
||||
s 2 252 687 591.589 664.392
|
||||
s 2 253 698 601.698 663.19
|
||||
s 2 254 700 603.327 667.232
|
||||
s 2 255 656 563.305 657.902
|
||||
s 2 256 694 598.036 667.268
|
||||
s 2 257 714 618.145 662.355
|
||||
s 2 258 708 618.512 645.366
|
||||
s 2 259 726 634.945 643.204
|
||||
s 2 260 713 616.887 667.415
|
||||
s 2 261 730 638.995 646.386
|
||||
s 2 264 776 678.805 643.356
|
||||
s 2 265 769 672.092 642.262
|
||||
s 2 267 895 781.553 683.16
|
||||
s 2 268 895 781.553 683.16
|
||||
s 2 269 891 777.506 679.182
|
||||
s 2 271 888 774.288 683.233
|
||||
s 2 272 898 784.251 690.38
|
||||
s 2 273 956 836.692 655.37
|
||||
s 2 274 947 828.873 673.098
|
||||
s 2 275 359 196.329 730.773
|
||||
s 2 276 359 196.329 730.773
|
||||
s 2 277 444 291.606 761.777
|
||||
s 2 278 463 357.487 706.022
|
||||
s 2 282 617 476.167 725.153
|
||||
s 2 285 664 515.317 729.1
|
||||
s 2 286 661 513.039 724.11
|
||||
s 2 288 659 511.731 743.188
|
||||
s 2 289 744 604.533 735.338
|
||||
s 2 290 740 601.769 711.221
|
||||
s 2 291 742 601.937 737.397
|
||||
s 2 292 740 600.916 732.141
|
||||
s 2 295 775 641.709 727.255
|
||||
s 2 301 313 146.314 779.858
|
||||
s 2 302 291 111.606 782.816
|
||||
s 2 304 376 193.757 790.556
|
||||
s 2 305 373 191.601 783.684
|
||||
s 2 306 385 200.302 789.599
|
||||
s 2 307 406 218.92 809.926
|
||||
s 2 310 455 295.709 773.864
|
||||
s 2 311 472 310.994 772.922
|
||||
s 2 312 544 356.24 799.68
|
||||
s 2 316 758 564.263 809.259
|
||||
s 2 321 801 633.92 774.402
|
||||
s 2 323 446 219.679 857.011
|
||||
s 2 328 668 430.1 872.24
|
||||
s 2 329 750 498.621 887.234
|
||||
s 2 330 765 511.029 890.334
|
||||
s 2 331 745 494.602 886.002
|
||||
s 2 333 862 641.477 846.36
|
||||
s 2 335 872 644.41 859.881
|
||||
s 2 339 404 147.16 898.548
|
||||
s 2 345 589 323.535 907.722
|
||||
i 3 0.471338 0.478516 9.65645 0.032991 0.0101101 0.0042569
|
||||
i 3 0.471338 0.490479 9.62773 0.0324589 0.0111744 0.00319268
|
||||
i 3 0.480908 0.492871 9.58706 0.0319268 0.0117065 0.00266056
|
||||
i 3 0.502441 0.504834 9.56553 0.0313946 0.0117065 0.00212845
|
||||
i 3 0.502441 0.502441 9.55596 0.0313946 0.0122386 0.00266056
|
||||
i 3 0.46416 0.507227 9.53203 0.0313946 0.0122386 0.00319268
|
||||
i 3 0.456982 0.509619 9.50332 0.0319268 0.0122386 0.00372479
|
||||
i 3 0.45459 0.516797 9.49136 0.0313946 0.0127707 0.00372479
|
||||
i 3 0.466553 0.526367 9.5105 0.0308625 0.0133028 0.00372479
|
||||
i 3 0.452197 0.52876 9.52485 0.0303304 0.014367 0.00319268
|
||||
i 3 0.425879 0.535937 9.50571 0.0292662 0.0154313 0.00266056
|
||||
i 3 0.428271 0.533545 9.50571 0.0292662 0.0164955 0.00212845
|
||||
i 3 0.425879 0.545508 9.51528 0.0287341 0.0170276 0.00159634
|
||||
i 3 0.411523 0.5479 9.54399 0.028202 0.0170276 0.00159634
|
||||
i 3 0.382812 0.559863 9.56074 0.0276699 0.0170276 0.00106423
|
||||
i 3 0.375635 0.571826 9.54878 0.0276699 0.0159634 0.000532113
|
||||
i 3 0.387598 0.581396 9.54878 0.0266056 0.0148992 0.000532113
|
||||
i 3 0.401953 0.586182 9.55835 0.0250093 0.014367 0.000532113
|
||||
i 3 0.382812 0.590967 9.56074 0.0239451 0.0138349 0.000532113
|
||||
i 3 0.363672 0.593359 9.52725 0.0228808 0.0138349 0.000532113
|
||||
i 3 0.366064 0.598145 9.50571 0.0223487 0.0138349 0.000532113
|
||||
i 3 0.37085 0.617285 9.5105 0.0223487 0.0138349 0
|
||||
i 3 0.356494 0.624463 9.53921 0.0228808 0.014367 0
|
||||
i 3 0.327783 0.634033 9.55596 0.0223487 0.0148992 0
|
||||
i 3 0.308643 0.629248 9.56074 0.0218166 0.0159634 0
|
||||
i 3 0.320605 0.638818 9.57031 0.0212845 0.0164955 0
|
||||
i 3 0.322998 0.641211 9.59185 0.0202203 0.0170276 0
|
||||
s 3 1 885 771.886 190.394
|
||||
s 3 8 814 710.222 241.36
|
||||
s 3 10 815 711.117 244.276
|
||||
s 3 11 883 770.656 198.02
|
||||
s 3 13 874 761.801 193.21
|
||||
s 3 15 882 769.349 194.094
|
||||
s 3 16 952 831.218 238.215
|
||||
s 3 17 940 820.031 242.02
|
||||
s 3 19 948 827.354 235.194
|
||||
s 3 20 948 827.391 241.213
|
||||
s 3 28 760 663.312 287.142
|
||||
s 3 33 813 709.44 279.254
|
||||
s 3 34 819 714.246 276.35
|
||||
s 3 36 815 711.636 286.046
|
||||
s 3 37 893 779.028 309.293
|
||||
s 3 39 898 783.513 307.182
|
||||
s 3 40 908 793.523 318.373
|
||||
s 3 42 938 819.064 256.113
|
||||
s 3 43 989 863.966 281.804
|
||||
s 3 45 967 843.818 310.173
|
||||
s 3 46 975 852.424 259.949
|
||||
s 3 51 231 143.198 321.116
|
||||
s 3 52 229 140.636 323.223
|
||||
s 3 53 598 513.854 337.163
|
||||
s 3 58 815 711.008 377.495
|
||||
s 3 59 798 696.404 357.311
|
||||
s 3 61 798 696.404 357.311
|
||||
s 3 63 893 779.621 352
|
||||
s 3 67 900 786.305 357.088
|
||||
s 3 68 898 784.227 353.135
|
||||
s 3 70 917 801.17 347.212
|
||||
s 3 71 986 861.382 325.185
|
||||
s 3 72 985 860.395 327.196
|
||||
s 3 79 564 492.659 419.338
|
||||
s 3 80 560 488.554 421.427
|
||||
s 3 81 575 501.51 445.356
|
||||
s 3 83 603 526.725 413.46
|
||||
s 3 84 586 511.864 447.453
|
||||
s 3 85 650 567.813 402.398
|
||||
s 3 86 700 611.102 433.159
|
||||
s 3 87 703 613.898 434.235
|
||||
s 3 88 652 569.851 405.336
|
||||
s 3 90 706 616.823 433.228
|
||||
s 3 91 747 652.473 432.3
|
||||
s 3 92 754 658.648 441.454
|
||||
s 3 93 708 618.268 429.213
|
||||
s 3 94 743 648.861 434.243
|
||||
s 3 95 823 718.34 385.05
|
||||
s 3 96 796 695.581 425.304
|
||||
s 3 97 800 699.235 396.68
|
||||
s 3 98 773 675.006 390.852
|
||||
s 3 101 884 772.387 414.266
|
||||
s 3 102 879 768.099 387.4
|
||||
s 3 103 888 775.703 409.171
|
||||
s 3 106 907 793.163 407.121
|
||||
s 3 109 224 130.501 479.026
|
||||
s 3 110 232 140.654 474.298
|
||||
s 3 112 353 276.953 460.195
|
||||
s 3 113 326 247.913 479.275
|
||||
s 3 114 323 244.999 481.419
|
||||
s 3 116 405 331.557 456.078
|
||||
s 3 118 402 328.474 500.448
|
||||
s 3 121 476 406.808 478.331
|
||||
s 3 124 560 488.874 453.428
|
||||
s 3 125 559 487.866 451.429
|
||||
s 3 127 585 511.21 485.366
|
||||
s 3 130 582 508.117 452.34
|
||||
s 3 131 642 560.191 503.303
|
||||
s 3 132 703 613.512 462.274
|
||||
s 3 134 766 669.178 501.123
|
||||
s 3 137 780 681.115 471.213
|
||||
s 3 138 785 686.111 468.15
|
||||
s 3 140 766 669.178 501.123
|
||||
s 3 147 904 790.478 501.294
|
||||
s 3 149 938 820.46 502.412
|
||||
s 3 152 963 842.7 502.39
|
||||
s 3 156 242 148.04 545.744
|
||||
s 3 158 248 154.13 544.882
|
||||
s 3 162 314 224.573 545.257
|
||||
s 3 163 315 225.942 530.779
|
||||
s 3 164 315 225.25 552.16
|
||||
s 3 165 320 230.24 575.172
|
||||
s 3 166 368 292.361 563.739
|
||||
s 3 167 411 337.518 559.362
|
||||
s 3 169 456 385.345 575.417
|
||||
s 3 170 576 503.558 560.339
|
||||
s 3 171 572 499.542 562.338
|
||||
s 3 172 572 499.542 562.338
|
||||
s 3 173 638 556.345 563.543
|
||||
s 3 175 636 554.93 552.498
|
||||
s 3 178 692 604.626 533.07
|
||||
s 3 179 763 665.69 529.344
|
||||
s 3 181 716 624.895 518.341
|
||||
s 3 182 764 666.738 535.365
|
||||
s 3 185 769 671.706 536.364
|
||||
s 3 186 769 671.706 536.364
|
||||
s 3 189 841 735.519 558.219
|
||||
s 3 190 881 771.578 554.18
|
||||
s 3 191 839 733.85 555.19
|
||||
s 3 193 938 820.706 573.366
|
||||
s 3 194 948 829.543 513.025
|
||||
s 3 196 904 790.951 538.836
|
||||
s 3 197 976 854.393 525.427
|
||||
s 3 198 236 140.322 603.995
|
||||
s 3 199 235 139.053 600.975
|
||||
s 3 203 290 208.701 584.181
|
||||
s 3 204 321 230.851 584.311
|
||||
s 3 205 322 231.422 590.784
|
||||
s 3 207 425 352.676 598.285
|
||||
s 3 210 507 413.915 594.08
|
||||
s 3 216 586 512.672 602.104
|
||||
s 3 217 585 511.513 598.4
|
||||
s 3 218 657 574.43 587.301
|
||||
s 3 219 675 589.734 581.331
|
||||
s 3 220 656 573.443 582.387
|
||||
s 3 221 650 567.724 580.446
|
||||
s 3 223 715 624.944 635.149
|
||||
s 3 224 828 723.983 600.259
|
||||
s 3 225 813 710.87 596.451
|
||||
s 3 231 977 855.78 576.339
|
||||
s 3 233 252 132.151 646.953
|
||||
s 3 235 270 151.285 641.899
|
||||
s 3 236 263 143.885 646.853
|
||||
s 3 237 335 226.072 646.176
|
||||
s 3 240 448 356.109 665.276
|
||||
s 3 241 465 362.114 676.566
|
||||
s 3 242 481 383.735 674.003
|
||||
s 3 243 472 384.478 660.065
|
||||
s 3 244 478 381.18 672.03
|
||||
s 3 246 564 460.41 668.205
|
||||
s 3 247 548 446.541 666.983
|
||||
s 3 249 607 530.254 640.36
|
||||
s 3 251 615 515.283 663.243
|
||||
s 3 252 687 591.632 664.369
|
||||
s 3 253 698 601.651 663.156
|
||||
s 3 254 700 603.327 667.192
|
||||
s 3 256 693 597.206 667.286
|
||||
s 3 257 714 618.171 662.386
|
||||
s 3 258 708 618.448 645.308
|
||||
s 3 259 726 634.919 642.193
|
||||
s 3 260 713 616.921 667.437
|
||||
s 3 261 730 638.986 646.352
|
||||
s 3 264 776 678.909 643.341
|
||||
s 3 268 893 779.545 683.224
|
||||
s 3 271 888 774.345 683.25
|
||||
s 3 272 898 784.392 690.3
|
||||
s 3 273 956 836.616 654.36
|
||||
s 3 274 947 828.918 673.067
|
||||
s 3 275 359 196.307 730.769
|
||||
s 3 276 356 193.389 729.874
|
||||
s 3 277 444 291.736 761.796
|
||||
s 3 278 463 357.497 706.028
|
||||
s 3 279 610 473.389 731.03
|
||||
s 3 281 602 465.988 723.373
|
||||
s 3 285 665 516.633 728.092
|
||||
s 3 286 661 513.323 723.09
|
||||
s 3 288 659 511.572 743.182
|
||||
s 3 290 738 599.252 709.299
|
||||
s 3 292 740 600.731 731.179
|
||||
s 3 301 313 146.474 779.852
|
||||
s 3 304 375 193.685 790.593
|
||||
s 3 305 373 191.613 783.699
|
||||
s 3 306 385 200.249 789.607
|
||||
s 3 307 406 218.946 809.916
|
||||
s 3 310 455 295.762 773.851
|
||||
s 3 311 472 311.008 772.914
|
||||
s 3 312 544 356.234 798.671
|
||||
s 3 316 757 563.508 808.199
|
||||
s 3 323 445 218.732 856.915
|
||||
s 3 328 668 427.886 871.306
|
||||
s 3 329 750 498.489 887.267
|
||||
s 3 330 765 510.975 890.36
|
||||
s 3 331 744 494.093 885.015
|
||||
s 3 333 863 642.396 846.332
|
||||
s 3 334 857 636.646 843.358
|
||||
s 3 343 514 229.103 934.981
|
||||
i 4 0.294287 0.660352 9.6062 0.0191561 0.0175597 -0.000532113
|
||||
i 4 0.265576 0.674707 9.57988 0.0180918 0.0175597 0
|
||||
i 4 0.256006 0.698633 9.56074 0.0170276 0.0170276 0
|
||||
i 4 0.265576 0.715381 9.56553 0.0164955 0.0164955 0.000532113
|
||||
i 4 0.265576 0.727344 9.58467 0.0164955 0.0154313 0.000532113
|
||||
i 4 0.239258 0.732129 9.59424 0.0164955 0.0148992 0.00106423
|
||||
i 4 0.22251 0.732129 9.57749 0.0164955 0.0138349 0.00159634
|
||||
i 4 0.23208 0.729736 9.57988 0.0164955 0.0133028 0.00159634
|
||||
i 4 0.227295 0.727344 9.58945 0.0159634 0.0133028 0.00212845
|
||||
i 4 0.203369 0.736914 9.60142 0.0148992 0.0127707 0.00159634
|
||||
i 4 0.179443 0.734522 9.59902 0.0138349 0.0122386 0.00159634
|
||||
i 4 0.172266 0.741699 9.59424 0.0133028 0.0117065 0.00159634
|
||||
i 4 0.193799 0.741699 9.63013 0.0127707 0.0111744 0.00212845
|
||||
i 4 0.186621 0.741699 9.66841 0.0122386 0.0106423 0.00266056
|
||||
i 4 0.172266 0.741699 9.68994 0.0122386 0.0101101 0.00319268
|
||||
i 4 0.160303 0.748877 9.67798 0.0122386 0.0101101 0.00372479
|
||||
i 4 0.181836 0.756055 9.67798 0.0117065 0.00957803 0.0042569
|
||||
i 4 0.200977 0.775195 9.69951 0.0117065 0.00904591 0.00478901
|
||||
i 4 0.186621 0.77998 9.7043 0.0111744 0.00957803 0.00478901
|
||||
i 4 0.162695 0.777588 9.68755 0.0106423 0.0101101 0.00478901
|
||||
i 4 0.155518 0.768018 9.66123 0.0106423 0.0111744 0.00532113
|
||||
i 4 0.16748 0.748877 9.67319 0.0111744 0.0122386 0.00638535
|
||||
i 4 0.162695 0.739307 9.7043 0.0111744 0.0127707 0.00798169
|
||||
i 4 0.153125 0.732129 9.70908 0.0117065 0.0122386 0.00904591
|
||||
i 4 0.150732 0.727344 9.69233 0.0122386 0.0117065 0.00957803
|
||||
i 4 0.179443 0.739307 9.66841 0.0133028 0.0111744 0.0101101
|
||||
s 4 1 885 771.994 190.366
|
||||
s 4 8 814 710.223 241.363
|
||||
s 4 10 815 711.068 244.306
|
||||
s 4 11 882 769.488 198.098
|
||||
s 4 12 876 763.708 195.193
|
||||
s 4 13 874 761.731 192.256
|
||||
s 4 15 882 769.341 194.117
|
||||
s 4 16 952 831.207 237.222
|
||||
s 4 17 940 820.094 241.983
|
||||
s 4 18 945 824.618 239.15
|
||||
s 4 19 948 827.346 234.198
|
||||
s 4 22 432 359.221 264.591
|
||||
s 4 28 760 663.293 287.118
|
||||
s 4 33 813 709.512 279.215
|
||||
s 4 34 818 713.314 276.343
|
||||
s 4 36 815 711.529 285.057
|
||||
s 4 37 893 778.938 308.242
|
||||
s 4 39 898 783.55 307.164
|
||||
s 4 40 907 792.695 318.382
|
||||
s 4 41 902 787.654 318.396
|
||||
s 4 42 938 819.106 256.202
|
||||
s 4 43 989 863.908 281.834
|
||||
s 4 44 971 847.662 314.346
|
||||
s 4 45 968 844.579 310.273
|
||||
s 4 46 975 852.254 260.043
|
||||
s 4 52 229 140.68 323.217
|
||||
s 4 53 598 514.121 337.175
|
||||
s 4 58 815 710.937 376.471
|
||||
s 4 59 798 696.344 357.345
|
||||
s 4 61 796 694.481 358.445
|
||||
s 4 63 893 779.58 351.02
|
||||
s 4 67 899 785.244 357.125
|
||||
s 4 68 898 784.179 353.158
|
||||
s 4 70 916 800.164 347.205
|
||||
s 4 71 987 862.374 323.193
|
||||
s 4 72 984 859.517 326.195
|
||||
s 4 78 349 274.104 444.811
|
||||
s 4 79 564 492.65 419.414
|
||||
s 4 81 575 501.478 444.409
|
||||
s 4 83 601 524.76 412.446
|
||||
s 4 84 586 511.86 446.439
|
||||
s 4 85 650 567.837 402.416
|
||||
s 4 86 700 611.156 433.123
|
||||
s 4 87 702 612.913 434.209
|
||||
s 4 90 706 616.807 433.199
|
||||
s 4 91 746 651.408 432.284
|
||||
s 4 93 707 617.559 430.146
|
||||
s 4 94 743 648.795 434.231
|
||||
s 4 95 823 718.332 385.052
|
||||
s 4 96 796 695.579 425.228
|
||||
s 4 97 800 699.055 396.852
|
||||
s 4 98 773 674.916 390.759
|
||||
s 4 102 879 768.454 386.971
|
||||
s 4 103 888 776.252 409.162
|
||||
s 4 106 907 793.277 407.058
|
||||
s 4 109 224 130.613 478.998
|
||||
s 4 110 232 140.621 473.277
|
||||
s 4 112 353 277.014 460.171
|
||||
s 4 114 323 244.961 480.36
|
||||
s 4 115 349 272.766 463.341
|
||||
s 4 116 405 331.398 455.088
|
||||
s 4 118 401 327.557 499.323
|
||||
s 4 119 401 327.557 499.323
|
||||
s 4 120 395 321.112 493.435
|
||||
s 4 121 476 406.743 478.377
|
||||
s 4 127 586 512.557 483.379
|
||||
s 4 130 582 508.157 452.348
|
||||
s 4 131 642 560.134 502.312
|
||||
s 4 132 703 613.486 461.235
|
||||
s 4 134 764 667.34 501.126
|
||||
s 4 136 773 675.456 494.29
|
||||
s 4 137 780 681.045 471.208
|
||||
s 4 138 784 685.045 467.118
|
||||
s 4 139 781 682.087 473.259
|
||||
s 4 140 766 669.129 501.112
|
||||
s 4 141 884 772.588 493.247
|
||||
s 4 143 860 752.712 506.715
|
||||
s 4 149 938 820.417 502.393
|
||||
s 4 152 963 842.708 502.408
|
||||
s 4 156 242 148.092 545.745
|
||||
s 4 158 248 154.161 544.898
|
||||
s 4 162 314 224.577 545.216
|
||||
s 4 163 315 225.946 530.744
|
||||
s 4 164 315 225.235 552.134
|
||||
s 4 165 320 230.327 575.153
|
||||
s 4 167 411 337.774 559.343
|
||||
s 4 169 456 385.542 575.339
|
||||
s 4 170 575 502.563 557.259
|
||||
s 4 171 572 499.587 562.29
|
||||
s 4 172 572 499.587 562.29
|
||||
s 4 173 638 556.429 563.579
|
||||
s 4 175 636 554.961 551.494
|
||||
s 4 177 641 559.492 561.262
|
||||
s 4 178 692 604.626 533.116
|
||||
s 4 179 763 665.738 529.302
|
||||
s 4 181 716 624.827 517.256
|
||||
s 4 182 764 666.839 535.368
|
||||
s 4 185 769 671.733 536.347
|
||||
s 4 186 769 671.733 536.347
|
||||
s 4 187 810 708.024 538.294
|
||||
s 4 189 841 735.558 558.172
|
||||
s 4 190 881 772.053 554.336
|
||||
s 4 191 840 734.563 554.241
|
||||
s 4 192 860 753.225 554.14
|
||||
s 4 193 938 820.664 573.398
|
||||
s 4 194 948 829.589 513.021
|
||||
s 4 196 904 790.866 537.836
|
||||
s 4 197 976 854.583 524.319
|
||||
s 4 198 236 140.38 603.999
|
||||
s 4 201 288 206.836 586.271
|
||||
s 4 204 321 230.91 583.331
|
||||
s 4 207 425 352.585 598.27
|
||||
s 4 210 507 413.476 594.048
|
||||
s 4 216 586 512.578 601.8
|
||||
s 4 217 585 511.154 598.205
|
||||
s 4 218 657 574.497 587.3
|
||||
s 4 220 656 573.483 582.382
|
||||
s 4 221 651 568.555 580.399
|
||||
s 4 223 715 624.942 635.151
|
||||
s 4 224 827 722.978 600.292
|
||||
s 4 231 977 855.254 576.338
|
||||
s 4 234 252 132.144 646.968
|
||||
s 4 240 448 356.099 665.285
|
||||
s 4 242 481 383.757 674.017
|
||||
s 4 244 478 381.174 672.047
|
||||
s 4 246 564 460.379 668.196
|
||||
s 4 247 548 446.621 666.998
|
||||
s 4 249 607 530.219 640.38
|
||||
s 4 250 613 512.601 667.348
|
||||
s 4 251 616 516.659 662.172
|
||||
s 4 252 687 591.584 663.361
|
||||
s 4 253 698 601.767 663.178
|
||||
s 4 254 700 603.389 667.207
|
||||
s 4 256 693 597.195 667.276
|
||||
s 4 258 708 618.612 644.382
|
||||
s 4 259 726 634.907 642.21
|
||||
s 4 260 713 616.921 667.416
|
||||
s 4 261 730 638.979 646.36
|
||||
s 4 267 895 781.565 682.132
|
||||
s 4 268 893 779.549 683.206
|
||||
s 4 272 898 784.406 690.256
|
||||
s 4 273 956 836.687 654.365
|
||||
s 4 274 947 828.881 671.937
|
||||
s 4 275 359 196.281 730.773
|
||||
s 4 276 356 193.356 729.874
|
||||
s 4 277 444 291.69 761.788
|
||||
s 4 278 463 357.45 704.999
|
||||
s 4 281 602 466.206 723.378
|
||||
s 4 283 621 486.721 706.163
|
||||
s 4 286 661 513.021 723.096
|
||||
s 4 288 657 510.171 742.231
|
||||
s 4 290 738 599.306 709.267
|
||||
s 4 292 740 600.83 731.158
|
||||
s 4 301 313 146.359 779.839
|
||||
s 4 303 308 140.181 777.88
|
||||
s 4 310 454 294.447 773.873
|
||||
i 5 0.200977 0.746484 9.66602 0.0133028 0.0106423 0.00957803
|
||||
i 5 0.196191 0.756055 9.66602 0.0133028 0.0101101 0.00957803
|
||||
i 5 0.189014 0.765625 9.65166 0.0122386 0.00957803 0.00957803
|
||||
i 5 0.193799 0.772803 9.64209 0.0106423 0.00957803 0.00957803
|
||||
i 5 0.217725 0.777588 9.66123 0.00904591 0.00904591 0.00957803
|
||||
i 5 0.24165 0.777588 9.69233 0.00744958 0.00904591 0.0106423
|
||||
i 5 0.234473 0.77041 9.7019 0.00691746 0.00957803 0.0111744
|
||||
i 5 0.248828 0.765625 9.68276 0.00638535 0.00957803 0.0122386
|
||||
i 5 0.275146 0.75127 9.67319 0.00691746 0.00957803 0.0122386
|
||||
i 5 0.291895 0.748877 9.67319 0.00744958 0.00904591 0.0117065
|
||||
i 5 0.270361 0.744092 9.68276 0.00798169 0.0085138 0.0106423
|
||||
i 5 0.246436 0.746484 9.66602 0.00904591 0.00798169 0.0101101
|
||||
i 5 0.234473 0.748877 9.64448 0.00957803 0.00798169 0.00904591
|
||||
i 5 0.248828 0.758447 9.64209 0.0101101 0.00798169 0.00904591
|
||||
i 5 0.253613 0.765625 9.6397 0.0106423 0.00798169 0.00904591
|
||||
i 5 0.246436 0.753662 9.61816 0.0106423 0.00904591 0.00904591
|
||||
i 5 0.248828 0.744092 9.58706 0.0106423 0.00904591 0.00957803
|
||||
i 5 0.282324 0.720166 9.57271 0.0106423 0.0085138 0.0101101
|
||||
i 5 0.30625 0.710596 9.58945 0.0101101 0.00744958 0.0106423
|
||||
i 5 0.30625 0.708203 9.60142 0.00904591 0.00638535 0.0111744
|
||||
i 5 0.294287 0.710596 9.59663 0.00798169 0.00585324 0.0117065
|
||||
i 5 0.287109 0.720166 9.57749 0.00744958 0.00585324 0.0111744
|
||||
i 5 0.30625 0.724951 9.59185 0.00691746 0.00585324 0.0111744
|
||||
i 5 0.299072 0.722559 9.61577 0.00691746 0.00585324 0.0106423
|
||||
i 5 0.267969 0.712988 9.61338 0.00638535 0.00532113 0.0101101
|
||||
i 5 0.260791 0.703418 9.57988 0.00585324 0.00478901 0.00957803
|
||||
i 5 0.272754 0.693848 9.55356 0.00585324 0.0042569 0.00957803
|
||||
s 5 1 885 771.967 190.374
|
||||
s 5 8 814 710.258 241.348
|
||||
s 5 11 882 769.5 198.11
|
||||
s 5 12 876 763.725 195.196
|
||||
s 5 13 874 761.761 192.265
|
||||
s 5 14 876 763.725 195.196
|
||||
s 5 15 882 769.333 194.13
|
||||
s 5 16 952 831.211 238.222
|
||||
s 5 17 941 820.874 241.088
|
||||
s 5 19 948 827.331 234.199
|
||||
s 5 28 760 663.304 287.152
|
||||
s 5 33 813 709.417 279.246
|
||||
s 5 34 818 713.31 276.339
|
||||
s 5 36 815 711.553 285.069
|
||||
s 5 37 893 778.921 308.279
|
||||
s 5 39 898 783.503 307.195
|
||||
s 5 40 908 793.456 318.342
|
||||
s 5 41 902 787.598 318.335
|
||||
s 5 42 938 819.203 256.15
|
||||
s 5 43 989 863.912 281.803
|
||||
s 5 44 972 848.245 313.371
|
||||
s 5 45 968 844.392 308.24
|
||||
s 5 46 975 852.111 259.768
|
||||
s 5 52 229 140.643 323.25
|
||||
s 5 58 815 711.009 376.479
|
||||
s 5 59 797 695.427 357.337
|
||||
s 5 61 797 695.427 357.337
|
||||
s 5 63 893 779.641 350.98
|
||||
s 5 67 899 785.25 357.122
|
||||
s 5 70 916 800.144 347.212
|
||||
s 5 71 986 861.363 325.258
|
||||
s 5 72 986 861.363 325.258
|
||||
s 5 78 349 272.718 444.977
|
||||
s 5 79 564 492.645 419.352
|
||||
s 5 81 575 501.478 444.458
|
||||
s 5 83 601 524.669 412.369
|
||||
s 5 84 586 511.845 446.414
|
||||
s 5 85 650 567.806 402.399
|
||||
s 5 86 701 611.648 433.295
|
||||
s 5 87 701 611.648 433.295
|
||||
s 5 90 706 616.746 433.269
|
||||
s 5 91 747 652.381 432.302
|
||||
s 5 93 708 618.322 429.229
|
||||
s 5 94 744 649.616 434.254
|
||||
s 5 95 823 718.328 385.098
|
||||
s 5 96 796 695.506 425.257
|
||||
s 5 97 800 699.015 396.835
|
||||
s 5 98 773 674.898 390.815
|
||||
s 5 103 888 776.053 409.196
|
||||
s 5 106 907 793.174 407.16
|
||||
s 5 109 224 130.598 479.009
|
||||
s 5 112 353 277.059 460.178
|
||||
s 5 113 326 247.919 479.3
|
||||
s 5 114 323 245.04 481.416
|
||||
s 5 118 402 328.417 500.439
|
||||
s 5 119 400 326.248 502.231
|
||||
s 5 121 476 406.735 478.368
|
||||
s 5 127 585 511.229 485.269
|
||||
s 5 130 582 508.086 452.315
|
||||
s 5 131 643 561.152 502.328
|
||||
s 5 132 703 613.454 461.25
|
||||
s 5 134 766 669.174 501.069
|
||||
s 5 136 773 675.449 494.288
|
||||
s 5 137 781 682.128 471.206
|
||||
s 5 138 784 685.119 467.122
|
||||
s 5 139 781 682.162 473.269
|
||||
s 5 140 766 669.174 501.069
|
||||
s 5 145 888 776.3 491.33
|
||||
s 5 149 938 820.54 502.447
|
||||
s 5 152 963 842.697 502.391
|
||||
s 5 156 242 148.049 545.748
|
||||
s 5 158 248 154.189 544.861
|
||||
s 5 162 314 224.58 545.228
|
||||
s 5 163 315 225.943 530.706
|
||||
s 5 164 315 225.254 552.161
|
||||
s 5 165 320 230.248 573.933
|
||||
s 5 167 411 337.603 559.374
|
||||
s 5 169 456 385.477 575.333
|
||||
s 5 170 575 502.564 557.297
|
||||
s 5 171 572 499.585 562.324
|
||||
s 5 172 572 499.585 562.324
|
||||
s 5 173 638 556.357 563.566
|
||||
s 5 177 641 559.473 561.27
|
||||
s 5 178 693 605.572 532.124
|
||||
s 5 179 763 665.768 529.292
|
||||
s 5 181 716 624.878 517.287
|
||||
s 5 182 764 666.826 535.34
|
||||
s 5 185 769 671.754 536.33
|
||||
s 5 186 769 671.754 536.33
|
||||
s 5 187 810 707.981 538.318
|
||||
s 5 189 841 735.367 557.132
|
||||
s 5 190 881 771.284 554.167
|
||||
s 5 191 840 734.433 554.139
|
||||
s 5 192 860 752.832 554.234
|
||||
s 5 193 938 820.661 573.411
|
||||
s 5 194 948 829.548 513.027
|
||||
s 5 196 904 790.811 537.922
|
||||
s 5 197 976 854.459 524.408
|
||||
s 5 198 236 140.438 604.965
|
||||
s 5 199 235 139.069 600.972
|
||||
s 5 201 288 206.856 586.255
|
||||
s 5 203 290 208.734 584.196
|
||||
s 5 204 321 230.86 584.302
|
||||
s 5 205 322 231.412 590.747
|
||||
s 5 207 425 352.658 598.284
|
||||
s 5 208 430 357.846 600.368
|
||||
s 5 209 430 357.846 600.368
|
||||
s 5 210 507 413.862 594.141
|
||||
s 5 216 586 512.524 601.846
|
||||
s 5 217 585 511.371 598.24
|
||||
s 5 220 656 573.388 582.4
|
||||
s 5 221 650 567.67 580.444
|
||||
s 5 223 715 625.005 635.16
|
||||
s 5 224 828 724.003 600.283
|
||||
s 5 231 977 856.036 576.324
|
||||
s 5 233 252 132.27 646.898
|
||||
s 5 235 270 151.147 641.895
|
||||
s 5 237 335 226.3 646.265
|
||||
s 5 240 448 356.101 665.311
|
||||
s 5 241 465 362.419 675.526
|
||||
s 5 242 482 384.778 674.024
|
||||
s 5 244 478 381.143 672.035
|
||||
s 5 246 564 460.402 668.186
|
||||
s 5 247 548 446.693 666.997
|
||||
s 5 250 613 512.473 667.358
|
||||
s 5 251 615 515.389 663.256
|
||||
s 5 252 687 591.542 663.355
|
||||
s 5 253 698 601.734 663.164
|
||||
s 5 254 700 603.352 667.193
|
||||
s 5 257 714 618.128 662.357
|
||||
s 5 258 708 618.561 644.403
|
||||
s 5 259 726 634.882 642.196
|
||||
s 5 260 713 616.859 667.397
|
||||
s 5 261 730 638.896 646.366
|
||||
s 5 264 776 678.964 643.331
|
||||
s 5 268 893 779.594 683.216
|
||||
s 5 271 888 774.41 682.262
|
||||
s 5 273 956 836.775 654.374
|
||||
s 5 274 947 828.877 671.995
|
||||
s 5 275 359 196.336 730.802
|
||||
s 5 276 356 193.409 729.896
|
||||
s 5 277 444 291.551 761.797
|
||||
s 5 278 464 358.548 705.089
|
||||
s 5 279 610 473.571 731.066
|
||||
s 5 281 602 466.135 723.367
|
||||
s 5 282 617 475.86 725.172
|
||||
s 5 285 665 516.486 728.097
|
||||
s 5 286 661 513.146 723.099
|
||||
s 5 289 743 603.491 734.339
|
||||
s 5 290 738 599.323 709.259
|
||||
s 5 292 740 600.943 731.142
|
||||
s 5 301 313 146.284 779.851
|
||||
s 5 304 375 193.624 790.587
|
||||
s 5 306 385 200.303 789.594
|
||||
s 5 307 406 218.914 809.947
|
||||
s 5 310 455 295.69 773.843
|
||||
s 5 311 472 310.886 772.897
|
||||
s 5 312 544 356.329 798.683
|
||||
s 5 329 750 498.867 886.161
|
||||
s 5 330 765 511.01 889.347
|
||||
s 5 331 745 495.029 884.943
|
||||
s 5 333 862 641.525 845.353
|
||||
s 5 334 858 637.569 843.345
|
||||
s 5 345 589 322.892 907.696
|
||||
s 5 349 765 431.497 988.976
|
||||
|
|
@ -0,0 +1,120 @@
|
|||
/**
|
||||
* @file ISAM2Example_SmartFactor.cpp
|
||||
* @brief test of iSAM with smart factors, led to bitbucket issue #367
|
||||
* @author Alexander (pumaking on BitBucket)
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/SimpleCamera.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/slam/SmartProjectionPoseFactor.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using symbol_shorthand::P;
|
||||
using symbol_shorthand::X;
|
||||
|
||||
// Make the typename short so it looks much cleaner
|
||||
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
|
||||
|
||||
int main(int argc, char* argv[]) {
|
||||
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
|
||||
|
||||
auto measurementNoise =
|
||||
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
|
||||
|
||||
Vector6 sigmas;
|
||||
sigmas << Vector3::Constant(0.3), Vector3::Constant(0.1);
|
||||
auto noise = noiseModel::Diagonal::Sigmas(sigmas);
|
||||
|
||||
ISAM2Params parameters;
|
||||
parameters.relinearizeThreshold = 0.01;
|
||||
parameters.relinearizeSkip = 1;
|
||||
parameters.cacheLinearizedFactors = false;
|
||||
parameters.enableDetailedResults = true;
|
||||
parameters.print();
|
||||
ISAM2 isam(parameters);
|
||||
|
||||
// Create a factor graph
|
||||
NonlinearFactorGraph graph;
|
||||
Values initialEstimate;
|
||||
|
||||
Point3 point(0.0, 0.0, 1.0);
|
||||
|
||||
// Intentionally initialize the variables off from the ground truth
|
||||
Pose3 delta(Rot3::Rodrigues(0.0, 0.0, 0.0), Point3(0.05, -0.10, 0.20));
|
||||
|
||||
Pose3 pose1(Rot3(), Point3(0.0, 0.0, 0.0));
|
||||
Pose3 pose2(Rot3(), Point3(0.0, 0.2, 0.0));
|
||||
Pose3 pose3(Rot3(), Point3(0.0, 0.4, 0.0));
|
||||
Pose3 pose4(Rot3(), Point3(0.0, 0.5, 0.0));
|
||||
Pose3 pose5(Rot3(), Point3(0.0, 0.6, 0.0));
|
||||
|
||||
vector<Pose3> poses = {pose1, pose2, pose3, pose4, pose5};
|
||||
|
||||
// Add first pose
|
||||
graph.emplace_shared<PriorFactor<Pose3>>(X(0), poses[0], noise);
|
||||
initialEstimate.insert(X(0), poses[0].compose(delta));
|
||||
|
||||
// Create smart factor with measurement from first pose only
|
||||
SmartFactor::shared_ptr smartFactor(new SmartFactor(measurementNoise, K));
|
||||
smartFactor->add(PinholePose<Cal3_S2>(poses[0], K).project(point), X(0));
|
||||
graph.push_back(smartFactor);
|
||||
|
||||
// loop over remaining poses
|
||||
for (size_t i = 1; i < 5; i++) {
|
||||
cout << "****************************************************" << endl;
|
||||
cout << "i = " << i << endl;
|
||||
|
||||
// Add prior on new pose
|
||||
graph.emplace_shared<PriorFactor<Pose3>>(X(i), poses[i], noise);
|
||||
initialEstimate.insert(X(i), poses[i].compose(delta));
|
||||
|
||||
// "Simulate" measurement from this pose
|
||||
PinholePose<Cal3_S2> camera(poses[i], K);
|
||||
Point2 measurement = camera.project(point);
|
||||
cout << "Measurement " << i << "" << measurement << endl;
|
||||
|
||||
// Add measurement to smart factor
|
||||
smartFactor->add(measurement, X(i));
|
||||
|
||||
// Update iSAM2
|
||||
ISAM2Result result = isam.update(graph, initialEstimate);
|
||||
result.print();
|
||||
|
||||
cout << "Detailed results:" << endl;
|
||||
for (auto keyedStatus : result.detail->variableStatus) {
|
||||
const auto& status = keyedStatus.second;
|
||||
PrintKey(keyedStatus.first);
|
||||
cout << " {" << endl;
|
||||
cout << "reeliminated: " << status.isReeliminated << endl;
|
||||
cout << "relinearized above thresh: " << status.isAboveRelinThreshold
|
||||
<< endl;
|
||||
cout << "relinearized involved: " << status.isRelinearizeInvolved << endl;
|
||||
cout << "relinearized: " << status.isRelinearized << endl;
|
||||
cout << "observed: " << status.isObserved << endl;
|
||||
cout << "new: " << status.isNew << endl;
|
||||
cout << "in the root clique: " << status.inRootClique << endl;
|
||||
cout << "}" << endl;
|
||||
}
|
||||
|
||||
Values currentEstimate = isam.calculateEstimate();
|
||||
currentEstimate.print("Current estimate: ");
|
||||
|
||||
boost::optional<Point3> pointEstimate = smartFactor->point(currentEstimate);
|
||||
if (pointEstimate) {
|
||||
cout << *pointEstimate << endl;
|
||||
} else {
|
||||
cout << "Point degenerate." << endl;
|
||||
}
|
||||
|
||||
// Reset graph and initial estimate for next iteration
|
||||
graph.resize(0);
|
||||
initialEstimate.clear();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,234 @@
|
|||
/**
|
||||
* @file ISAM2_SmartFactorStereo_IMU.cpp
|
||||
* @brief test of iSAM2 with smart stereo factors and IMU preintegration,
|
||||
* originally used to debug valgrind invalid reads with Eigen
|
||||
* @author Nghia Ho
|
||||
*
|
||||
* Setup is a stationary stereo camera with an IMU attached.
|
||||
* The data file is at examples/Data/ISAM2_SmartFactorStereo_IMU.txt
|
||||
* It contains 5 frames of stereo matches and IMU data.
|
||||
*/
|
||||
#include <gtsam/navigation/CombinedImuFactor.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
|
||||
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <vector>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using symbol_shorthand::X;
|
||||
using symbol_shorthand::V;
|
||||
using symbol_shorthand::B;
|
||||
|
||||
struct IMUHelper {
|
||||
IMUHelper() {
|
||||
{
|
||||
auto gaussian = noiseModel::Diagonal::Sigmas(
|
||||
(Vector(6) << Vector3::Constant(5.0e-2), Vector3::Constant(5.0e-3))
|
||||
.finished());
|
||||
auto huber = noiseModel::Robust::Create(
|
||||
noiseModel::mEstimator::Huber::Create(1.345), gaussian);
|
||||
|
||||
biasNoiseModel = huber;
|
||||
}
|
||||
|
||||
{
|
||||
auto gaussian = noiseModel::Isotropic::Sigma(3, 0.01);
|
||||
auto huber = noiseModel::Robust::Create(
|
||||
noiseModel::mEstimator::Huber::Create(1.345), gaussian);
|
||||
|
||||
velocityNoiseModel = huber;
|
||||
}
|
||||
|
||||
// expect IMU to be rotated in image space co-ords
|
||||
auto p = boost::make_shared<PreintegratedCombinedMeasurements::Params>(
|
||||
Vector3(0.0, 9.8, 0.0));
|
||||
|
||||
p->accelerometerCovariance =
|
||||
I_3x3 * pow(0.0565, 2.0); // acc white noise in continuous
|
||||
p->integrationCovariance =
|
||||
I_3x3 * 1e-9; // integration uncertainty continuous
|
||||
p->gyroscopeCovariance =
|
||||
I_3x3 * pow(4.0e-5, 2.0); // gyro white noise in continuous
|
||||
p->biasAccCovariance = I_3x3 * pow(0.00002, 2.0); // acc bias in continuous
|
||||
p->biasOmegaCovariance =
|
||||
I_3x3 * pow(0.001, 2.0); // gyro bias in continuous
|
||||
p->biasAccOmegaInt = Matrix::Identity(6, 6) * 1e-5;
|
||||
|
||||
// body to IMU rotation
|
||||
Rot3 iRb(0.036129, -0.998727, 0.035207,
|
||||
0.045417, -0.033553, -0.998404,
|
||||
0.998315, 0.037670, 0.044147);
|
||||
|
||||
// body to IMU translation (meters)
|
||||
Point3 iTb(0.03, -0.025, -0.06);
|
||||
|
||||
// body in this example is the left camera
|
||||
p->body_P_sensor = Pose3(iRb, iTb);
|
||||
|
||||
Rot3 prior_rotation = Rot3(I_3x3);
|
||||
Pose3 prior_pose(prior_rotation, Point3(0, 0, 0));
|
||||
|
||||
Vector3 acc_bias(0.0, -0.0942015, 0.0); // in camera frame
|
||||
Vector3 gyro_bias(-0.00527483, -0.00757152, -0.00469968);
|
||||
|
||||
priorImuBias = imuBias::ConstantBias(acc_bias, gyro_bias);
|
||||
|
||||
prevState = NavState(prior_pose, Vector3(0, 0, 0));
|
||||
propState = prevState;
|
||||
prevBias = priorImuBias;
|
||||
|
||||
preintegrated = new PreintegratedCombinedMeasurements(p, priorImuBias);
|
||||
}
|
||||
|
||||
imuBias::ConstantBias priorImuBias; // assume zero initial bias
|
||||
noiseModel::Robust::shared_ptr velocityNoiseModel;
|
||||
noiseModel::Robust::shared_ptr biasNoiseModel;
|
||||
NavState prevState;
|
||||
NavState propState;
|
||||
imuBias::ConstantBias prevBias;
|
||||
PreintegratedCombinedMeasurements* preintegrated;
|
||||
};
|
||||
|
||||
int main(int argc, char* argv[]) {
|
||||
if (argc != 2) {
|
||||
cout << "./ISAM2_SmartFactorStereo_IMU [data.txt]\n";
|
||||
return 0;
|
||||
}
|
||||
|
||||
ifstream in(argv[1]);
|
||||
|
||||
if (!in) {
|
||||
cerr << "error opening: " << argv[1] << "\n";
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Camera parameters
|
||||
double fx = 822.37;
|
||||
double fy = 822.37;
|
||||
double cx = 538.73;
|
||||
double cy = 579.10;
|
||||
double baseline = 0.372; // meters
|
||||
|
||||
Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, 0.0, cx, cy, baseline));
|
||||
|
||||
ISAM2Params parameters;
|
||||
parameters.relinearizeThreshold = 0.1;
|
||||
ISAM2 isam(parameters);
|
||||
|
||||
// Create a factor graph
|
||||
std::map<size_t, SmartStereoProjectionPoseFactor::shared_ptr> smartFactors;
|
||||
NonlinearFactorGraph graph;
|
||||
Values initialEstimate;
|
||||
IMUHelper imu;
|
||||
|
||||
// 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);
|
||||
initialEstimate.insert(X(0), Pose3::identity());
|
||||
|
||||
// Bias prior
|
||||
graph.add(PriorFactor<imuBias::ConstantBias>(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));
|
||||
initialEstimate.insert(V(0), Vector3(0, 0, 0));
|
||||
|
||||
int lastFrame = 1;
|
||||
int frame;
|
||||
|
||||
while (true) {
|
||||
char line[1024];
|
||||
|
||||
in.getline(line, sizeof(line));
|
||||
stringstream ss(line);
|
||||
char type;
|
||||
|
||||
ss >> type;
|
||||
ss >> frame;
|
||||
|
||||
if (frame != lastFrame || in.eof()) {
|
||||
cout << "Running iSAM for frame: " << lastFrame << "\n";
|
||||
|
||||
initialEstimate.insert(X(lastFrame), Pose3::identity());
|
||||
initialEstimate.insert(V(lastFrame), Vector3(0, 0, 0));
|
||||
initialEstimate.insert(B(lastFrame), imu.prevBias);
|
||||
|
||||
CombinedImuFactor imuFactor(X(lastFrame - 1), V(lastFrame - 1),
|
||||
X(lastFrame), V(lastFrame), B(lastFrame - 1),
|
||||
B(lastFrame), *imu.preintegrated);
|
||||
|
||||
graph.add(imuFactor);
|
||||
|
||||
isam.update(graph, initialEstimate);
|
||||
|
||||
Values currentEstimate = isam.calculateEstimate();
|
||||
|
||||
imu.propState = imu.preintegrated->predict(imu.prevState, imu.prevBias);
|
||||
imu.prevState = NavState(currentEstimate.at<Pose3>(X(lastFrame)),
|
||||
currentEstimate.at<Vector3>(V(lastFrame)));
|
||||
imu.prevBias = currentEstimate.at<imuBias::ConstantBias>(B(lastFrame));
|
||||
imu.preintegrated->resetIntegrationAndSetBias(imu.prevBias);
|
||||
|
||||
graph.resize(0);
|
||||
initialEstimate.clear();
|
||||
|
||||
if (in.eof()) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (type == 'i') { // Process IMU measurement
|
||||
double ax, ay, az;
|
||||
double gx, gy, gz;
|
||||
double dt = 1 / 800.0; // IMU at ~800Hz
|
||||
|
||||
ss >> ax;
|
||||
ss >> ay;
|
||||
ss >> az;
|
||||
|
||||
ss >> gx;
|
||||
ss >> gy;
|
||||
ss >> gz;
|
||||
|
||||
Vector3 acc(ax, ay, az);
|
||||
Vector3 gyr(gx, gy, gz);
|
||||
|
||||
imu.preintegrated->integrateMeasurement(acc, gyr, dt);
|
||||
} else if (type == 's') { // Process stereo measurement
|
||||
int landmark;
|
||||
double xl, xr, y;
|
||||
|
||||
ss >> landmark;
|
||||
ss >> xl;
|
||||
ss >> xr;
|
||||
ss >> y;
|
||||
|
||||
if (smartFactors.count(landmark) == 0) {
|
||||
auto gaussian = noiseModel::Isotropic::Sigma(3, 1.0);
|
||||
|
||||
SmartProjectionParams params(HESSIAN, ZERO_ON_DEGENERACY);
|
||||
|
||||
smartFactors[landmark] = SmartStereoProjectionPoseFactor::shared_ptr(
|
||||
new SmartStereoProjectionPoseFactor(gaussian, params));
|
||||
graph.push_back(smartFactors[landmark]);
|
||||
}
|
||||
|
||||
smartFactors[landmark]->add(StereoPoint2(xl, xr, y), X(frame), K);
|
||||
} else {
|
||||
throw runtime_error("unexpected data type: " + type);
|
||||
}
|
||||
|
||||
lastFrame = frame;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,134 @@
|
|||
|
||||
#include <gtsam/geometry/SimpleCamera.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/navigation/ImuFactor.h>
|
||||
#include <gtsam/navigation/Scenario.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
|
||||
#include <vector>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// Shorthand for velocity and pose variables
|
||||
using symbol_shorthand::V;
|
||||
using symbol_shorthand::X;
|
||||
|
||||
const double kGravity = 9.81;
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main(int argc, char* argv[]) {
|
||||
auto params = PreintegrationParams::MakeSharedU(kGravity);
|
||||
params->setAccelerometerCovariance(I_3x3 * 0.1);
|
||||
params->setGyroscopeCovariance(I_3x3 * 0.1);
|
||||
params->setIntegrationCovariance(I_3x3 * 0.1);
|
||||
params->setUse2ndOrderCoriolis(false);
|
||||
params->setOmegaCoriolis(Vector3(0, 0, 0));
|
||||
|
||||
Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
|
||||
|
||||
// Start with a camera on x-axis looking at origin
|
||||
double radius = 30;
|
||||
const Point3 up(0, 0, 1), target(0, 0, 0);
|
||||
const Point3 position(radius, 0, 0);
|
||||
const SimpleCamera camera = SimpleCamera::Lookat(position, target, up);
|
||||
const auto pose_0 = camera.pose();
|
||||
|
||||
// Now, create a constant-twist scenario that makes the camera orbit the
|
||||
// origin
|
||||
double angular_velocity = M_PI, // rad/sec
|
||||
delta_t = 1.0 / 18; // makes for 10 degrees per step
|
||||
Vector3 angular_velocity_vector(0, -angular_velocity, 0);
|
||||
Vector3 linear_velocity_vector(radius * angular_velocity, 0, 0);
|
||||
auto scenario = ConstantTwistScenario(angular_velocity_vector,
|
||||
linear_velocity_vector, pose_0);
|
||||
|
||||
// Create a factor graph
|
||||
NonlinearFactorGraph newgraph;
|
||||
|
||||
// Create (incremental) ISAM2 solver
|
||||
ISAM2 isam;
|
||||
|
||||
// Create the initial estimate to the solution
|
||||
// Intentionally initialize the variables off from the ground truth
|
||||
Values initialEstimate, totalEstimate, result;
|
||||
|
||||
// Add a prior on pose x0. This indirectly specifies where the origin is.
|
||||
// 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));
|
||||
|
||||
// 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);
|
||||
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);
|
||||
|
||||
initialEstimate.insert(V(0), n_velocity);
|
||||
|
||||
// IMU preintegrator
|
||||
PreintegratedImuMeasurements accum(params);
|
||||
|
||||
// Simulate poses and imu measurements, adding them to the factor graph
|
||||
for (size_t i = 0; i < 36; ++i) {
|
||||
double t = i * delta_t;
|
||||
if (i == 0) { // First time add two poses
|
||||
auto pose_1 = scenario.pose(delta_t);
|
||||
initialEstimate.insert(X(0), pose_0.compose(delta));
|
||||
initialEstimate.insert(X(1), pose_1.compose(delta));
|
||||
} else if (i >= 2) { // Add more poses as necessary
|
||||
auto pose_i = scenario.pose(t);
|
||||
initialEstimate.insert(X(i), pose_i.compose(delta));
|
||||
}
|
||||
|
||||
if (i > 0) {
|
||||
// Add Bias variables periodically
|
||||
if (i % 5 == 0) {
|
||||
biasKey++;
|
||||
Symbol b1 = biasKey - 1;
|
||||
Symbol b2 = biasKey;
|
||||
Vector6 covvec;
|
||||
covvec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
|
||||
auto cov = noiseModel::Diagonal::Variances(covvec);
|
||||
auto f = boost::make_shared<BetweenFactor<imuBias::ConstantBias> >(
|
||||
b1, b2, imuBias::ConstantBias(), cov);
|
||||
newgraph.add(f);
|
||||
initialEstimate.insert(biasKey, imuBias::ConstantBias());
|
||||
}
|
||||
// Predict acceleration and gyro measurements in (actual) body frame
|
||||
Vector3 measuredAcc = scenario.acceleration_b(t) -
|
||||
scenario.rotation(t).transpose() * params->n_gravity;
|
||||
Vector3 measuredOmega = scenario.omega_b(t);
|
||||
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t);
|
||||
|
||||
// Add Imu Factor
|
||||
ImuFactor imufac(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum);
|
||||
newgraph.add(imufac);
|
||||
|
||||
// insert new velocity, which is wrong
|
||||
initialEstimate.insert(V(i), n_velocity);
|
||||
accum.resetIntegration();
|
||||
}
|
||||
|
||||
// Incremental solution
|
||||
isam.update(newgraph, initialEstimate);
|
||||
result = isam.calculateEstimate();
|
||||
newgraph = NonlinearFactorGraph();
|
||||
initialEstimate.clear();
|
||||
}
|
||||
GTSAM_PRINT(result);
|
||||
return 0;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -206,7 +206,7 @@ int main(int argc, char *argv[]) {
|
|||
}
|
||||
|
||||
#ifdef GTSAM_USE_TBB
|
||||
std::auto_ptr<tbb::task_scheduler_init> init;
|
||||
std::unique_ptr<tbb::task_scheduler_init> init;
|
||||
if(nThreads > 0) {
|
||||
cout << "Using " << nThreads << " threads" << endl;
|
||||
init.reset(new tbb::task_scheduler_init(nThreads));
|
||||
|
|
@ -251,10 +251,10 @@ void runIncremental()
|
|||
Key firstPose;
|
||||
while(nextMeasurement < datasetMeasurements.size())
|
||||
{
|
||||
if(BetweenFactor<Pose>::shared_ptr measurement =
|
||||
if(BetweenFactor<Pose>::shared_ptr factor =
|
||||
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(datasetMeasurements[nextMeasurement]))
|
||||
{
|
||||
Key key1 = measurement->key1(), key2 = measurement->key2();
|
||||
Key key1 = factor->key1(), key2 = factor->key2();
|
||||
if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) {
|
||||
// We found an odometry starting at firstStep
|
||||
firstPose = std::min(key1, key2);
|
||||
|
|
@ -302,52 +302,53 @@ void runIncremental()
|
|||
|
||||
NonlinearFactor::shared_ptr measurementf = datasetMeasurements[nextMeasurement];
|
||||
|
||||
if(BetweenFactor<Pose>::shared_ptr measurement =
|
||||
if(BetweenFactor<Pose>::shared_ptr factor =
|
||||
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(measurementf))
|
||||
{
|
||||
// Stop collecting measurements that are for future steps
|
||||
if(measurement->key1() > step || measurement->key2() > step)
|
||||
if(factor->key1() > step || factor->key2() > step)
|
||||
break;
|
||||
|
||||
// Require that one of the nodes is the current one
|
||||
if(measurement->key1() != step && measurement->key2() != step)
|
||||
if(factor->key1() != step && factor->key2() != step)
|
||||
throw runtime_error("Problem in data file, out-of-sequence measurements");
|
||||
|
||||
// Add a new factor
|
||||
newFactors.push_back(measurement);
|
||||
newFactors.push_back(factor);
|
||||
const auto& measured = factor->measured();
|
||||
|
||||
// Initialize the new variable
|
||||
if(measurement->key1() > measurement->key2()) {
|
||||
if(!newVariables.exists(measurement->key1())) { // Only need to check newVariables since loop closures come after odometry
|
||||
if(factor->key1() > factor->key2()) {
|
||||
if(!newVariables.exists(factor->key1())) { // Only need to check newVariables since loop closures come after odometry
|
||||
if(step == 1)
|
||||
newVariables.insert(measurement->key1(), measurement->measured().inverse());
|
||||
newVariables.insert(factor->key1(), measured.inverse());
|
||||
else {
|
||||
Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key2());
|
||||
newVariables.insert(measurement->key1(), prevPose * measurement->measured().inverse());
|
||||
Pose prevPose = isam2.calculateEstimate<Pose>(factor->key2());
|
||||
newVariables.insert(factor->key1(), prevPose * measured.inverse());
|
||||
}
|
||||
}
|
||||
} else {
|
||||
if(!newVariables.exists(measurement->key2())) { // Only need to check newVariables since loop closures come after odometry
|
||||
if(!newVariables.exists(factor->key2())) { // Only need to check newVariables since loop closures come after odometry
|
||||
if(step == 1)
|
||||
newVariables.insert(measurement->key2(), measurement->measured());
|
||||
newVariables.insert(factor->key2(), measured);
|
||||
else {
|
||||
Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key1());
|
||||
newVariables.insert(measurement->key2(), prevPose * measurement->measured());
|
||||
Pose prevPose = isam2.calculateEstimate<Pose>(factor->key1());
|
||||
newVariables.insert(factor->key2(), prevPose * measured);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(BearingRangeFactor<Pose, Point2>::shared_ptr measurement =
|
||||
else if(BearingRangeFactor<Pose, Point2>::shared_ptr factor =
|
||||
boost::dynamic_pointer_cast<BearingRangeFactor<Pose, Point2> >(measurementf))
|
||||
{
|
||||
Key poseKey = measurement->keys()[0], lmKey = measurement->keys()[1];
|
||||
Key poseKey = factor->keys()[0], lmKey = factor->keys()[1];
|
||||
|
||||
// Stop collecting measurements that are for future steps
|
||||
if(poseKey > step)
|
||||
throw runtime_error("Problem in data file, out-of-sequence measurements");
|
||||
|
||||
// Add new factor
|
||||
newFactors.push_back(measurement);
|
||||
newFactors.push_back(factor);
|
||||
|
||||
// Initialize new landmark
|
||||
if(!isam2.getLinearizationPoint().exists(lmKey))
|
||||
|
|
@ -357,8 +358,9 @@ void runIncremental()
|
|||
pose = isam2.calculateEstimate<Pose>(poseKey);
|
||||
else
|
||||
pose = newVariables.at<Pose>(poseKey);
|
||||
Rot2 measuredBearing = measurement->measured().first;
|
||||
double measuredRange = measurement->measured().second;
|
||||
const auto& measured = factor->measured();
|
||||
Rot2 measuredBearing = measured.bearing();
|
||||
double measuredRange = measured.range();
|
||||
newVariables.insert(lmKey,
|
||||
pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0))));
|
||||
}
|
||||
|
|
@ -427,7 +429,7 @@ void runIncremental()
|
|||
// for (Key key12: boost::adaptors::reverse(values.keys())) {
|
||||
// if(i != j) {
|
||||
// gttic_(jointMarginalInformation);
|
||||
// std::vector<Key> keys(2);
|
||||
// KeyVector keys(2);
|
||||
// keys[0] = key1;
|
||||
// keys[1] = key2;
|
||||
// JointMarginal info = marginals.jointMarginalInformation(keys);
|
||||
|
|
@ -522,7 +524,7 @@ void runCompare()
|
|||
|
||||
// Check solution for equality
|
||||
cout << "Comparing solutions..." << endl;
|
||||
vector<Key> missingKeys;
|
||||
KeyVector missingKeys;
|
||||
br::set_symmetric_difference(soln1.keys(), soln2.keys(), std::back_inserter(missingKeys));
|
||||
if(!missingKeys.empty()) {
|
||||
cout << " Keys unique to one solution file: ";
|
||||
|
|
@ -533,7 +535,7 @@ void runCompare()
|
|||
}
|
||||
cout << endl;
|
||||
}
|
||||
vector<Key> commonKeys;
|
||||
KeyVector commonKeys;
|
||||
br::set_intersection(soln1.keys(), soln2.keys(), std::back_inserter(commonKeys));
|
||||
double maxDiff = 0.0;
|
||||
for(Key j: commonKeys)
|
||||
|
|
|
|||
|
|
@ -11,8 +11,8 @@
|
|||
|
||||
/**
|
||||
* @file VisualISAM2Example.cpp
|
||||
* @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset
|
||||
* This version uses iSAM2 to solve the problem incrementally
|
||||
* @brief A visualSLAM example for the structure-from-motion problem on a
|
||||
* simulated dataset This version uses iSAM2 to solve the problem incrementally
|
||||
* @author Duy-Nguyen Ta
|
||||
*/
|
||||
|
||||
|
|
@ -25,27 +25,28 @@
|
|||
// For loading the data
|
||||
#include "SFMdata.h"
|
||||
|
||||
// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
|
||||
// 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
|
||||
// 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>
|
||||
|
||||
// We want to use iSAM2 to solve the structure-from-motion problem incrementally, so
|
||||
// include iSAM2 here
|
||||
// We want to use iSAM2 to solve the structure-from-motion problem
|
||||
// incrementally, so include iSAM2 here
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
|
||||
// iSAM2 requires as input a set set of new factors to be added stored in a factor graph,
|
||||
// and initial guesses for any new variables used in the added factors
|
||||
// iSAM2 requires as input a set of new factors to be added stored in a factor
|
||||
// graph, and initial guesses for any new variables used in the added factors
|
||||
#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.
|
||||
// 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/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/ProjectionFactor.h>
|
||||
|
||||
|
|
@ -56,12 +57,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 measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
|
||||
// Define the camera observation noise model, 1 pixel stddev
|
||||
auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
|
||||
|
||||
// Create the set of ground-truth landmarks
|
||||
vector<Point3> points = createPoints();
|
||||
|
|
@ -69,10 +69,12 @@ int main(int argc, char* argv[]) {
|
|||
// Create the set of ground-truth poses
|
||||
vector<Pose3> poses = createPoses();
|
||||
|
||||
// Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps to maintain proper linearization
|
||||
// and efficient variable ordering, iSAM2 performs partial relinearization/reordering at each step. A parameter
|
||||
// structure is available that allows the user to set various properties, such as the relinearization threshold
|
||||
// and type of linear solver. For this example, we we set the relinearization threshold small so the iSAM2 result
|
||||
// Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps
|
||||
// to maintain proper linearization and efficient variable ordering, iSAM2
|
||||
// performs partial relinearization/reordering at each step. A parameter
|
||||
// structure is available that allows the user to set various properties, such
|
||||
// as the relinearization threshold and type of linear solver. For this
|
||||
// example, we we set the relinearization threshold small so the iSAM2 result
|
||||
// will approach the batch result.
|
||||
ISAM2Params parameters;
|
||||
parameters.relinearizeThreshold = 0.01;
|
||||
|
|
@ -83,44 +85,52 @@ int main(int argc, char* argv[]) {
|
|||
NonlinearFactorGraph graph;
|
||||
Values initialEstimate;
|
||||
|
||||
// Loop over the different poses, adding the observations to iSAM incrementally
|
||||
// Loop over the 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) {
|
||||
SimpleCamera camera(poses[i], *K);
|
||||
Point2 measurement = camera.project(points[j]);
|
||||
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
|
||||
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(
|
||||
measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
|
||||
}
|
||||
|
||||
// Add an initial guess for the current pose
|
||||
// Intentionally initialize the variables off from the ground truth
|
||||
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
|
||||
static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25),
|
||||
Point3(0.05, -0.10, 0.20));
|
||||
initialEstimate.insert(Symbol('x', i), poses[i] * kDeltaPose);
|
||||
|
||||
// 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
|
||||
// Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
|
||||
// adding it to iSAM.
|
||||
if( i == 0) {
|
||||
// Add a prior on pose x0
|
||||
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3),Vector3::Constant(0.1)).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);
|
||||
// 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 Also,
|
||||
// as iSAM solves incrementally, we must wait until each is observed at
|
||||
// least twice before adding it to iSAM.
|
||||
if (i == 0) {
|
||||
// Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw
|
||||
static auto kPosePrior = noiseModel::Diagonal::Sigmas(
|
||||
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))
|
||||
.finished());
|
||||
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0],
|
||||
kPosePrior);
|
||||
|
||||
// Add a prior on landmark l0
|
||||
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
|
||||
static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
|
||||
graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0],
|
||||
kPointPrior);
|
||||
|
||||
// Add initial guesses to all observed landmarks
|
||||
// Intentionally initialize the variables off from the ground truth
|
||||
static Point3 kDeltaPoint(-0.25, 0.20, 0.15);
|
||||
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] + kDeltaPoint);
|
||||
|
||||
} else {
|
||||
// Update iSAM with the new factors
|
||||
isam.update(graph, initialEstimate);
|
||||
// Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver.
|
||||
// If accuracy is desired at the expense of time, update(*) can be called additional times
|
||||
// to perform multiple optimizer iterations every step.
|
||||
// Each call to iSAM2 update(*) performs one iteration of the iterative
|
||||
// nonlinear solver. If accuracy is desired at the expense of time,
|
||||
// update(*) can be called additional times to perform multiple optimizer
|
||||
// iterations every step.
|
||||
isam.update();
|
||||
Values currentEstimate = isam.calculateEstimate();
|
||||
cout << "****************************************************" << endl;
|
||||
|
|
|
|||
95
gtsam.h
95
gtsam.h
|
|
@ -568,7 +568,7 @@ class Pose2 {
|
|||
static gtsam::Pose2 Expmap(Vector v);
|
||||
static Vector Logmap(const gtsam::Pose2& p);
|
||||
Matrix AdjointMap() const;
|
||||
Vector Adjoint(const Vector& xi) const;
|
||||
Vector Adjoint(Vector xi) const;
|
||||
static Matrix wedge(double vx, double vy, double w);
|
||||
|
||||
// Group Actions on Point2
|
||||
|
|
@ -745,6 +745,9 @@ virtual class Cal3DS2_Base {
|
|||
double py() const;
|
||||
double k1() const;
|
||||
double k2() const;
|
||||
Matrix K() const;
|
||||
Vector k() const;
|
||||
Vector vector() const;
|
||||
|
||||
// Action on Point2
|
||||
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
|
||||
|
|
@ -862,7 +865,7 @@ class CalibratedCamera {
|
|||
// Standard Constructors and Named Constructors
|
||||
CalibratedCamera();
|
||||
CalibratedCamera(const gtsam::Pose3& pose);
|
||||
CalibratedCamera(const Vector& v);
|
||||
CalibratedCamera(Vector v);
|
||||
static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height);
|
||||
|
||||
// Testable
|
||||
|
|
@ -872,7 +875,7 @@ class CalibratedCamera {
|
|||
// Manifold
|
||||
static size_t Dim();
|
||||
size_t dim() const;
|
||||
gtsam::CalibratedCamera retract(const Vector& d) const;
|
||||
gtsam::CalibratedCamera retract(Vector d) const;
|
||||
Vector localCoordinates(const gtsam::CalibratedCamera& T2) const;
|
||||
|
||||
// Action on Point3
|
||||
|
|
@ -908,7 +911,7 @@ class PinholeCamera {
|
|||
CALIBRATION calibration() const;
|
||||
|
||||
// Manifold
|
||||
This retract(const Vector& d) const;
|
||||
This retract(Vector d) const;
|
||||
Vector localCoordinates(const This& T2) const;
|
||||
size_t dim() const;
|
||||
static size_t Dim();
|
||||
|
|
@ -935,6 +938,8 @@ virtual class SimpleCamera {
|
|||
static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height);
|
||||
static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
|
||||
const gtsam::Point3& upVector, const gtsam::Cal3_S2& K);
|
||||
static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
|
||||
const gtsam::Point3& upVector);
|
||||
|
||||
// Testable
|
||||
void print(string s) const;
|
||||
|
|
@ -945,7 +950,7 @@ virtual class SimpleCamera {
|
|||
gtsam::Cal3_S2 calibration() const;
|
||||
|
||||
// Manifold
|
||||
gtsam::SimpleCamera retract(const Vector& d) const;
|
||||
gtsam::SimpleCamera retract(Vector d) const;
|
||||
Vector localCoordinates(const gtsam::SimpleCamera& T2) const;
|
||||
size_t dim() const;
|
||||
static size_t Dim();
|
||||
|
|
@ -966,9 +971,10 @@ virtual class SimpleCamera {
|
|||
// Some typedefs for common camera types
|
||||
// PinholeCameraCal3_S2 is the same as SimpleCamera above
|
||||
typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
|
||||
typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
|
||||
typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
|
||||
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
|
||||
// due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified
|
||||
//typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
|
||||
//typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
|
||||
//typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
|
||||
|
||||
#include <gtsam/geometry/StereoCamera.h>
|
||||
class StereoCamera {
|
||||
|
|
@ -986,7 +992,7 @@ class StereoCamera {
|
|||
gtsam::Cal3_S2Stereo calibration() const;
|
||||
|
||||
// Manifold
|
||||
gtsam::StereoCamera retract(const Vector& d) const;
|
||||
gtsam::StereoCamera retract(Vector d) const;
|
||||
Vector localCoordinates(const gtsam::StereoCamera& T2) const;
|
||||
size_t dim() const;
|
||||
static size_t Dim();
|
||||
|
|
@ -1221,12 +1227,12 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian {
|
|||
};
|
||||
|
||||
virtual class Constrained : gtsam::noiseModel::Diagonal {
|
||||
static gtsam::noiseModel::Constrained* MixedSigmas(const Vector& mu, const Vector& sigmas);
|
||||
static gtsam::noiseModel::Constrained* MixedSigmas(double m, const Vector& sigmas);
|
||||
static gtsam::noiseModel::Constrained* MixedVariances(const Vector& mu, const Vector& variances);
|
||||
static gtsam::noiseModel::Constrained* MixedVariances(const Vector& variances);
|
||||
static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& mu, const Vector& precisions);
|
||||
static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& precisions);
|
||||
static gtsam::noiseModel::Constrained* MixedSigmas(Vector mu, Vector sigmas);
|
||||
static gtsam::noiseModel::Constrained* MixedSigmas(double m, Vector sigmas);
|
||||
static gtsam::noiseModel::Constrained* MixedVariances(Vector mu, Vector variances);
|
||||
static gtsam::noiseModel::Constrained* MixedVariances(Vector variances);
|
||||
static gtsam::noiseModel::Constrained* MixedPrecisions(Vector mu, Vector precisions);
|
||||
static gtsam::noiseModel::Constrained* MixedPrecisions(Vector precisions);
|
||||
|
||||
static gtsam::noiseModel::Constrained* All(size_t dim);
|
||||
static gtsam::noiseModel::Constrained* All(size_t dim, double mu);
|
||||
|
|
@ -1409,12 +1415,12 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
|
|||
pair<Matrix, Vector> jacobianUnweighted() const;
|
||||
Matrix augmentedJacobianUnweighted() const;
|
||||
|
||||
void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const;
|
||||
void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues& x) const;
|
||||
gtsam::JacobianFactor whiten() const;
|
||||
|
||||
pair<gtsam::GaussianConditional*, gtsam::JacobianFactor*> eliminate(const gtsam::Ordering& keys) const;
|
||||
|
||||
void setModel(bool anyConstrained, const Vector& sigmas);
|
||||
void setModel(bool anyConstrained, Vector sigmas);
|
||||
|
||||
gtsam::noiseModel::Diagonal* get_model() const;
|
||||
|
||||
|
|
@ -2334,7 +2340,8 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
|
|||
gtsam::Point2 measured() const;
|
||||
};
|
||||
typedef gtsam::GeneralSFMFactor<gtsam::SimpleCamera, gtsam::Point3> GeneralSFMFactorCal3_S2;
|
||||
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
|
||||
// due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2
|
||||
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
|
||||
|
||||
template<CALIBRATION = {gtsam::Cal3_S2}>
|
||||
virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
|
||||
|
|
@ -2497,6 +2504,11 @@ class NavState {
|
|||
#include <gtsam/navigation/PreintegratedRotation.h>
|
||||
virtual class PreintegratedRotationParams {
|
||||
PreintegratedRotationParams();
|
||||
|
||||
// Testable
|
||||
void print(string s) const;
|
||||
bool equals(const gtsam::PreintegratedRotationParams& expected, double tol);
|
||||
|
||||
void setGyroscopeCovariance(Matrix cov);
|
||||
void setOmegaCoriolis(Vector omega);
|
||||
void setBodyPSensor(const gtsam::Pose3& pose);
|
||||
|
|
@ -2504,13 +2516,23 @@ virtual class PreintegratedRotationParams {
|
|||
Matrix getGyroscopeCovariance() const;
|
||||
|
||||
// TODO(frank): allow optional
|
||||
// boost::optional<Vector3> getOmegaCoriolis() const;
|
||||
// boost::optional<Vector> getOmegaCoriolis() const;
|
||||
// boost::optional<Pose3> getBodyPSensor() const;
|
||||
};
|
||||
|
||||
#include <gtsam/navigation/PreintegrationParams.h>
|
||||
virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
|
||||
PreintegrationParams(Vector n_gravity);
|
||||
|
||||
static gtsam::PreintegrationParams* MakeSharedD(double g);
|
||||
static gtsam::PreintegrationParams* MakeSharedU(double g);
|
||||
static gtsam::PreintegrationParams* MakeSharedD(); // default g = 9.81
|
||||
static gtsam::PreintegrationParams* MakeSharedU(); // default g = 9.81
|
||||
|
||||
// Testable
|
||||
void print(string s) const;
|
||||
bool equals(const gtsam::PreintegrationParams& expected, double tol);
|
||||
|
||||
void setAccelerometerCovariance(Matrix cov);
|
||||
void setIntegrationCovariance(Matrix cov);
|
||||
void setUse2ndOrderCoriolis(bool flag);
|
||||
|
|
@ -2518,7 +2540,6 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
|
|||
Matrix getAccelerometerCovariance() const;
|
||||
Matrix getIntegrationCovariance() const;
|
||||
bool getUse2ndOrderCoriolis() const;
|
||||
void print(string s) const;
|
||||
};
|
||||
|
||||
#include <gtsam/navigation/ImuFactor.h>
|
||||
|
|
@ -2644,10 +2665,12 @@ virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{
|
|||
gtsam::Unit3 bRef() const;
|
||||
};
|
||||
|
||||
virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{
|
||||
Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model,
|
||||
const gtsam::Unit3& bRef);
|
||||
Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model);
|
||||
virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor {
|
||||
Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ,
|
||||
const gtsam::noiseModel::Diagonal* model,
|
||||
const gtsam::Unit3& bRef);
|
||||
Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ,
|
||||
const gtsam::noiseModel::Diagonal* model);
|
||||
Pose3AttitudeFactor();
|
||||
void print(string s) const;
|
||||
bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
|
||||
|
|
@ -2655,6 +2678,30 @@ virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{
|
|||
gtsam::Unit3 bRef() const;
|
||||
};
|
||||
|
||||
#include <gtsam/navigation/Scenario.h>
|
||||
virtual class Scenario {
|
||||
gtsam::Pose3 pose(double t) const;
|
||||
Vector omega_b(double t) const;
|
||||
Vector velocity_n(double t) const;
|
||||
Vector acceleration_n(double t) const;
|
||||
gtsam::Rot3 rotation(double t) const;
|
||||
gtsam::NavState navState(double t) const;
|
||||
Vector velocity_b(double t) const;
|
||||
Vector acceleration_b(double t) const;
|
||||
};
|
||||
|
||||
virtual class ConstantTwistScenario : gtsam::Scenario {
|
||||
ConstantTwistScenario(Vector w, Vector v);
|
||||
ConstantTwistScenario(Vector w, Vector v,
|
||||
const gtsam::Pose3& nTb0);
|
||||
};
|
||||
|
||||
virtual class AcceleratingScenario : gtsam::Scenario {
|
||||
AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0,
|
||||
Vector v0, Vector a_n,
|
||||
Vector omega_b);
|
||||
};
|
||||
|
||||
//*************************************************************************
|
||||
// Utilities
|
||||
//*************************************************************************
|
||||
|
|
|
|||
|
|
@ -1560,8 +1560,9 @@ PUBLIC Int CCOLAMD_2 /* returns TRUE if successful, FALSE otherwise */
|
|||
Int *dead_cols ;
|
||||
Int set1 ;
|
||||
Int set2 ;
|
||||
#ifndef NDEBUG
|
||||
Int cs ;
|
||||
|
||||
#endif
|
||||
int ok ;
|
||||
|
||||
#ifndef NDEBUG
|
||||
|
|
@ -1909,8 +1910,10 @@ PUBLIC Int CCOLAMD_2 /* returns TRUE if successful, FALSE otherwise */
|
|||
p [k] = col ;
|
||||
ASSERT (A [col] == EMPTY) ;
|
||||
|
||||
cs = CMEMBER (col) ;
|
||||
#ifndef NDEBUG
|
||||
cs = CMEMBER (col) ;
|
||||
ASSERT (k >= cset_start [cs] && k < cset_start [cs+1]) ;
|
||||
#endif
|
||||
|
||||
A [col] = k ;
|
||||
k++ ;
|
||||
|
|
@ -1926,11 +1929,11 @@ PUBLIC Int CCOLAMD_2 /* returns TRUE if successful, FALSE otherwise */
|
|||
if (A [col] == EMPTY)
|
||||
{
|
||||
k = Col [col].shared2.order ;
|
||||
cs = CMEMBER (col) ;
|
||||
#ifndef NDEBUG
|
||||
cs = CMEMBER (col) ;
|
||||
dead_cols [cs]-- ;
|
||||
#endif
|
||||
ASSERT (k >= cset_start [cs] && k < cset_start [cs+1]) ;
|
||||
#endif
|
||||
DEBUG1 (("ccolamd output ordering: k "ID" col "ID
|
||||
" (dense or null col)\n", k, col)) ;
|
||||
p [k] = col ;
|
||||
|
|
|
|||
|
|
@ -16,18 +16,6 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
|
|||
endif()
|
||||
endforeach(eigen_dir)
|
||||
|
||||
# do the same for the unsupported eigen folder
|
||||
file(GLOB_RECURSE unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h")
|
||||
|
||||
file(GLOB unsupported_eigen_dir_headers_all "Eigen/unsupported/Eigen/*")
|
||||
foreach(unsupported_eigen_dir ${unsupported_eigen_dir_headers_all})
|
||||
get_filename_component(filename ${unsupported_eigen_dir} NAME)
|
||||
if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") 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)
|
||||
endif()
|
||||
endforeach(unsupported_eigen_dir)
|
||||
|
||||
# Add to project source
|
||||
set(eigen_headers ${eigen_headers} PARENT_SCOPE)
|
||||
# set(unsupported_eigen_headers ${unsupported_eigen_headers} PARENT_SCOPE)
|
||||
|
|
@ -36,10 +24,6 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
|
|||
install(DIRECTORY Eigen/Eigen
|
||||
DESTINATION include/gtsam/3rdparty/Eigen
|
||||
FILES_MATCHING PATTERN "*.h")
|
||||
|
||||
install(DIRECTORY Eigen/unsupported/Eigen
|
||||
DESTINATION include/gtsam/3rdparty/Eigen/unsupported/
|
||||
FILES_MATCHING PATTERN "*.h")
|
||||
endif()
|
||||
|
||||
option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF)
|
||||
|
|
|
|||
|
|
@ -1,4 +0,0 @@
|
|||
repo: 8a21fd850624c931e448cbcfb38168cb2717c790
|
||||
node: b9cd8366d4e8f49471c7afafc4c2a1b00e54a54d
|
||||
branch: 3.2
|
||||
tag: 3.2.10
|
||||
|
|
@ -1,6 +1,9 @@
|
|||
[patterns]
|
||||
*.sh = LF
|
||||
*.MINPACK = CRLF
|
||||
scripts/*.in = LF
|
||||
debug/msvc/*.dat = CRLF
|
||||
debug/msvc/*.natvis = CRLF
|
||||
unsupported/test/mpreal/*.* = CRLF
|
||||
** = native
|
||||
|
||||
|
|
|
|||
|
|
@ -30,3 +30,5 @@ log
|
|||
patch
|
||||
a
|
||||
a.*
|
||||
lapack/testing
|
||||
lapack/reference
|
||||
|
|
|
|||
|
|
@ -21,15 +21,13 @@ a810d5dbab47acfe65b3350236efdd98f67d4d8a 3.1.0-alpha1
|
|||
8383e883ebcc6f14695ff0b5e20bb631abab43fb 3.1.0-rc1
|
||||
bf4cb8c934fa3a79f45f1e629610f0225e93e493 3.1.0-rc2
|
||||
da195914abcc1d739027cbee7c52077aab30b336 3.2-beta1
|
||||
4b687cad1d23066f66863f4f87298447298443df 3.2-rc1
|
||||
1eeda7b1258bcd306018c0738e2b6a8543661141 3.2-rc2
|
||||
ffa86ffb557094721ca71dcea6aed2651b9fd610 3.2.0
|
||||
6b38706d90a9fe182e66ab88477b3dbde34b9f66 3.2.1
|
||||
1306d75b4a21891e59ff9bd96678882cf831e39f 3.2.2
|
||||
36fd1ba04c120cfdd90f3e4cede47f43b21d19ad 3.2.3
|
||||
10219c95fe653d4962aa9db4946f6fbea96dd740 3.2.4
|
||||
bdd17ee3b1b3a166cd5ec36dcad4fc1f3faf774a 3.2.5
|
||||
c58038c56923e0fd86de3ded18e03df442e66dfb 3.2.6
|
||||
b30b87236a1b1552af32ac34075ee5696a9b5a33 3.2.7
|
||||
07105f7124f9aef00a68c85e0fc606e65d3d6c15 3.2.8
|
||||
dc6cfdf9bcec5efc7b6593bddbbb3d675de53524 3.2.9
|
||||
a8e0d153fc5e239ef8b06e3665f1f9e8cb8d49c8 before-evaluators
|
||||
09a8e21866106b49c5dec1d6d543e5794e82efa0 3.3-alpha1
|
||||
ce5a455b34c0a0ac3545a1497cb4a16c38ed90e8 3.3-beta1
|
||||
69d418c0699907bcd0bf9e0b3ba0a112ed091d85 3.3-beta2
|
||||
bef509908b9da05d0d07ffc0da105e2c8c6d3996 3.3-rc1
|
||||
04ab5fa4b241754afcf631117572276444c67239 3.3-rc2
|
||||
26667be4f70baf4f0d39e96f330714c87b399090 3.3.0
|
||||
f562a193118d4f40514e2f4a0ace6e974926ef06 3.3.1
|
||||
da9b4e14c2550e0d11078a3c39e6d56eba9905df 3.3.2
|
||||
67e894c6cd8f5f1f604b27d37ed47fdf012674ff 3.3.3
|
||||
|
|
|
|||
|
|
@ -1,4 +1,5 @@
|
|||
project(Eigen)
|
||||
project(Eigen3)
|
||||
|
||||
cmake_minimum_required(VERSION 2.8.5)
|
||||
|
||||
# guard against in-source builds
|
||||
|
|
@ -7,6 +8,11 @@ if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR})
|
|||
message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ")
|
||||
endif()
|
||||
|
||||
# Alias Eigen_*_DIR to Eigen3_*_DIR:
|
||||
|
||||
set(Eigen_SOURCE_DIR ${Eigen3_SOURCE_DIR})
|
||||
set(Eigen_BINARY_DIR ${Eigen3_BINARY_DIR})
|
||||
|
||||
# guard against bad build-type strings
|
||||
|
||||
if (NOT CMAKE_BUILD_TYPE)
|
||||
|
|
@ -35,10 +41,13 @@ string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen_minor_
|
|||
set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}")
|
||||
set(EIGEN_VERSION_NUMBER ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION})
|
||||
|
||||
# if the mercurial program is absent, this will leave the EIGEN_HG_CHANGESET string empty,
|
||||
# but won't stop CMake.
|
||||
execute_process(COMMAND hg tip -R ${CMAKE_SOURCE_DIR} OUTPUT_VARIABLE EIGEN_HGTIP_OUTPUT)
|
||||
execute_process(COMMAND hg branch -R ${CMAKE_SOURCE_DIR} OUTPUT_VARIABLE EIGEN_BRANCH_OUTPUT)
|
||||
# if we are not in a mercurial clone
|
||||
if(IS_DIRECTORY ${CMAKE_SOURCE_DIR}/.hg)
|
||||
# if the mercurial program is absent or this will leave the EIGEN_HG_CHANGESET string empty,
|
||||
# but won't stop CMake.
|
||||
execute_process(COMMAND hg tip -R ${CMAKE_SOURCE_DIR} OUTPUT_VARIABLE EIGEN_HGTIP_OUTPUT)
|
||||
execute_process(COMMAND hg branch -R ${CMAKE_SOURCE_DIR} OUTPUT_VARIABLE EIGEN_BRANCH_OUTPUT)
|
||||
endif()
|
||||
|
||||
# if this is the default (aka development) branch, extract the mercurial changeset number from the hg tip output...
|
||||
if(EIGEN_BRANCH_OUTPUT MATCHES "default")
|
||||
|
|
@ -58,6 +67,33 @@ include(GNUInstallDirs)
|
|||
|
||||
set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
|
||||
|
||||
|
||||
option(EIGEN_TEST_CXX11 "Enable testing with C++11 and C++11 features (e.g. Tensor module)." OFF)
|
||||
|
||||
|
||||
macro(ei_add_cxx_compiler_flag FLAG)
|
||||
string(REGEX REPLACE "-" "" SFLAG1 ${FLAG})
|
||||
string(REGEX REPLACE "\\+" "p" SFLAG ${SFLAG1})
|
||||
check_cxx_compiler_flag(${FLAG} COMPILER_SUPPORT_${SFLAG})
|
||||
if(COMPILER_SUPPORT_${SFLAG})
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${FLAG}")
|
||||
endif()
|
||||
endmacro(ei_add_cxx_compiler_flag)
|
||||
|
||||
check_cxx_compiler_flag("-std=c++11" EIGEN_COMPILER_SUPPORT_CPP11)
|
||||
|
||||
if(EIGEN_TEST_CXX11)
|
||||
set(CMAKE_CXX_STANDARD 11)
|
||||
set(CMAKE_CXX_EXTENSIONS OFF)
|
||||
if(EIGEN_COMPILER_SUPPORT_CPP11)
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
|
||||
endif()
|
||||
else()
|
||||
#set(CMAKE_CXX_STANDARD 03)
|
||||
#set(CMAKE_CXX_EXTENSIONS OFF)
|
||||
ei_add_cxx_compiler_flag("-std=c++03")
|
||||
endif()
|
||||
|
||||
#############################################################################
|
||||
# find how to link to the standard libraries #
|
||||
#############################################################################
|
||||
|
|
@ -92,9 +128,11 @@ else()
|
|||
endif()
|
||||
|
||||
option(EIGEN_BUILD_BTL "Build benchmark suite" OFF)
|
||||
if(NOT WIN32)
|
||||
|
||||
# Disable pkgconfig only for native Windows builds
|
||||
if(NOT WIN32 OR NOT CMAKE_HOST_SYSTEM_NAME MATCHES Windows)
|
||||
option(EIGEN_BUILD_PKGCONFIG "Build pkg-config .pc file for Eigen" ON)
|
||||
endif(NOT WIN32)
|
||||
endif()
|
||||
|
||||
set(CMAKE_INCLUDE_CURRENT_DIR ON)
|
||||
|
||||
|
|
@ -107,28 +145,15 @@ endif()
|
|||
|
||||
set(EIGEN_TEST_MAX_SIZE "320" CACHE STRING "Maximal matrix/vector size, default is 320")
|
||||
|
||||
macro(ei_add_cxx_compiler_flag FLAG)
|
||||
string(REGEX REPLACE "-" "" SFLAG ${FLAG})
|
||||
check_cxx_compiler_flag(${FLAG} COMPILER_SUPPORT_${SFLAG})
|
||||
if(COMPILER_SUPPORT_${SFLAG})
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${FLAG}")
|
||||
endif()
|
||||
endmacro(ei_add_cxx_compiler_flag)
|
||||
|
||||
if(NOT MSVC)
|
||||
# We assume that other compilers are partly compatible with GNUCC
|
||||
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fexceptions")
|
||||
set(CMAKE_CXX_FLAGS_DEBUG "-g3")
|
||||
set(CMAKE_CXX_FLAGS_RELEASE "-g0 -O2")
|
||||
|
||||
# clang outputs some warnings for unknwon flags that are not caught by check_cxx_compiler_flag
|
||||
|
||||
# clang outputs some warnings for unknown flags that are not caught by check_cxx_compiler_flag
|
||||
# adding -Werror turns such warnings into errors
|
||||
check_cxx_compiler_flag("-Werror" COMPILER_SUPPORT_WERROR)
|
||||
if(COMPILER_SUPPORT_WERROR)
|
||||
set(CMAKE_REQUIRED_FLAGS "-Werror")
|
||||
endif()
|
||||
|
||||
ei_add_cxx_compiler_flag("-pedantic")
|
||||
ei_add_cxx_compiler_flag("-Wall")
|
||||
ei_add_cxx_compiler_flag("-Wextra")
|
||||
|
|
@ -142,6 +167,18 @@ if(NOT MSVC)
|
|||
ei_add_cxx_compiler_flag("-Wpointer-arith")
|
||||
ei_add_cxx_compiler_flag("-Wwrite-strings")
|
||||
ei_add_cxx_compiler_flag("-Wformat-security")
|
||||
ei_add_cxx_compiler_flag("-Wshorten-64-to-32")
|
||||
ei_add_cxx_compiler_flag("-Wlogical-op")
|
||||
ei_add_cxx_compiler_flag("-Wenum-conversion")
|
||||
ei_add_cxx_compiler_flag("-Wc++11-extensions")
|
||||
ei_add_cxx_compiler_flag("-Wdouble-promotion")
|
||||
# ei_add_cxx_compiler_flag("-Wconversion")
|
||||
|
||||
# -Wshadow is insanely too strict with gcc, hopefully it will become usable with gcc 6
|
||||
# if(NOT CMAKE_COMPILER_IS_GNUCXX OR (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER "5.0.0"))
|
||||
if(NOT CMAKE_COMPILER_IS_GNUCXX)
|
||||
ei_add_cxx_compiler_flag("-Wshadow")
|
||||
endif()
|
||||
|
||||
ei_add_cxx_compiler_flag("-Wno-psabi")
|
||||
ei_add_cxx_compiler_flag("-Wno-variadic-macros")
|
||||
|
|
@ -151,7 +188,8 @@ if(NOT MSVC)
|
|||
ei_add_cxx_compiler_flag("-fno-common")
|
||||
ei_add_cxx_compiler_flag("-fstrict-aliasing")
|
||||
ei_add_cxx_compiler_flag("-wd981") # disable ICC's "operands are evaluated in unspecified order" remark
|
||||
ei_add_cxx_compiler_flag("-wd2304") # disbale ICC's "warning #2304: non-explicit constructor with single argument may cause implicit type conversion" produced by -Wnon-virtual-dtor
|
||||
ei_add_cxx_compiler_flag("-wd2304") # disable ICC's "warning #2304: non-explicit constructor with single argument may cause implicit type conversion" produced by -Wnon-virtual-dtor
|
||||
|
||||
|
||||
# The -ansi flag must be added last, otherwise it is also used as a linker flag by check_cxx_compiler_flag making it fails
|
||||
# Moreover we should not set both -strict-ansi and -ansi
|
||||
|
|
@ -163,6 +201,11 @@ if(NOT MSVC)
|
|||
else()
|
||||
ei_add_cxx_compiler_flag("-ansi")
|
||||
endif()
|
||||
|
||||
if(ANDROID_NDK)
|
||||
ei_add_cxx_compiler_flag("-pie")
|
||||
ei_add_cxx_compiler_flag("-fPIE")
|
||||
endif()
|
||||
|
||||
set(CMAKE_REQUIRED_FLAGS "")
|
||||
|
||||
|
|
@ -196,18 +239,65 @@ if(NOT MSVC)
|
|||
message(STATUS "Enabling SSE4.2 in tests/examples")
|
||||
endif()
|
||||
|
||||
option(EIGEN_TEST_AVX "Enable/Disable AVX in tests/examples" OFF)
|
||||
if(EIGEN_TEST_AVX)
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mavx")
|
||||
message(STATUS "Enabling AVX in tests/examples")
|
||||
endif()
|
||||
|
||||
option(EIGEN_TEST_FMA "Enable/Disable FMA in tests/examples" OFF)
|
||||
if(EIGEN_TEST_FMA AND NOT EIGEN_TEST_NEON)
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfma")
|
||||
message(STATUS "Enabling FMA in tests/examples")
|
||||
endif()
|
||||
|
||||
option(EIGEN_TEST_AVX512 "Enable/Disable AVX512 in tests/examples" OFF)
|
||||
if(EIGEN_TEST_AVX512)
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mavx512f -fabi-version=6 -DEIGEN_ENABLE_AVX512")
|
||||
message(STATUS "Enabling AVX512 in tests/examples")
|
||||
endif()
|
||||
|
||||
option(EIGEN_TEST_F16C "Enable/Disable F16C in tests/examples" OFF)
|
||||
if(EIGEN_TEST_F16C)
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mf16c")
|
||||
message(STATUS "Enabling F16C in tests/examples")
|
||||
endif()
|
||||
|
||||
option(EIGEN_TEST_ALTIVEC "Enable/Disable AltiVec in tests/examples" OFF)
|
||||
if(EIGEN_TEST_ALTIVEC)
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -maltivec -mabi=altivec")
|
||||
message(STATUS "Enabling AltiVec in tests/examples")
|
||||
endif()
|
||||
|
||||
option(EIGEN_TEST_VSX "Enable/Disable VSX in tests/examples" OFF)
|
||||
if(EIGEN_TEST_VSX)
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -m64 -mvsx")
|
||||
message(STATUS "Enabling VSX in tests/examples")
|
||||
endif()
|
||||
|
||||
option(EIGEN_TEST_NEON "Enable/Disable Neon in tests/examples" OFF)
|
||||
if(EIGEN_TEST_NEON)
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -mcpu=cortex-a8")
|
||||
if(EIGEN_TEST_FMA)
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon-vfpv4")
|
||||
else()
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon")
|
||||
endif()
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfloat-abi=hard")
|
||||
message(STATUS "Enabling NEON in tests/examples")
|
||||
endif()
|
||||
|
||||
option(EIGEN_TEST_NEON64 "Enable/Disable Neon in tests/examples" OFF)
|
||||
if(EIGEN_TEST_NEON64)
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
|
||||
message(STATUS "Enabling NEON in tests/examples")
|
||||
endif()
|
||||
|
||||
option(EIGEN_TEST_ZVECTOR "Enable/Disable S390X(zEC13) ZVECTOR in tests/examples" OFF)
|
||||
if(EIGEN_TEST_ZVECTOR)
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=z13 -mzvector")
|
||||
message(STATUS "Enabling S390X(zEC13) ZVECTOR in tests/examples")
|
||||
endif()
|
||||
|
||||
check_cxx_compiler_flag("-fopenmp" COMPILER_SUPPORT_OPENMP)
|
||||
if(COMPILER_SUPPORT_OPENMP)
|
||||
option(EIGEN_TEST_OPENMP "Enable/Disable OpenMP in tests/examples" OFF)
|
||||
|
|
@ -284,11 +374,21 @@ if(EIGEN_TEST_NO_EXPLICIT_ALIGNMENT)
|
|||
message(STATUS "Disabling alignment in tests/examples")
|
||||
endif()
|
||||
|
||||
option(EIGEN_TEST_C++0x "Enables all C++0x features." OFF)
|
||||
option(EIGEN_TEST_NO_EXCEPTIONS "Disables C++ exceptions" OFF)
|
||||
if(EIGEN_TEST_NO_EXCEPTIONS)
|
||||
ei_add_cxx_compiler_flag("-fno-exceptions")
|
||||
message(STATUS "Disabling exceptions in tests/examples")
|
||||
endif()
|
||||
|
||||
set(EIGEN_CUDA_COMPUTE_ARCH 30 CACHE STRING "The CUDA compute architecture level to target when compiling CUDA code")
|
||||
|
||||
include_directories(${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR})
|
||||
|
||||
# Backward compatibility support for EIGEN_INCLUDE_INSTALL_DIR
|
||||
if(EIGEN_INCLUDE_INSTALL_DIR)
|
||||
message(WARNING "EIGEN_INCLUDE_INSTALL_DIR is deprecated. Use INCLUDE_INSTALL_DIR instead.")
|
||||
endif()
|
||||
|
||||
if(EIGEN_INCLUDE_INSTALL_DIR AND NOT INCLUDE_INSTALL_DIR)
|
||||
set(INCLUDE_INSTALL_DIR ${EIGEN_INCLUDE_INSTALL_DIR}
|
||||
CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed")
|
||||
|
|
@ -298,9 +398,8 @@ else()
|
|||
CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed"
|
||||
)
|
||||
endif()
|
||||
|
||||
set(CMAKEPACKAGE_INSTALL_DIR
|
||||
"${CMAKE_INSTALL_LIBDIR}/cmake/eigen3"
|
||||
"${CMAKE_INSTALL_DATADIR}/eigen3/cmake"
|
||||
CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen3Config.cmake is installed"
|
||||
)
|
||||
set(PKGCONFIG_INSTALL_DIR
|
||||
|
|
@ -308,6 +407,7 @@ set(PKGCONFIG_INSTALL_DIR
|
|||
CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where eigen3.pc is installed"
|
||||
)
|
||||
|
||||
|
||||
# similar to set_target_properties but append the property instead of overwriting it
|
||||
macro(ei_add_target_property target prop value)
|
||||
|
||||
|
|
@ -329,22 +429,21 @@ if(EIGEN_BUILD_PKGCONFIG)
|
|||
install(FILES ${CMAKE_CURRENT_BINARY_DIR}/eigen3.pc
|
||||
DESTINATION ${PKGCONFIG_INSTALL_DIR}
|
||||
)
|
||||
endif(EIGEN_BUILD_PKGCONFIG)
|
||||
endif()
|
||||
|
||||
add_subdirectory(Eigen)
|
||||
|
||||
add_subdirectory(doc EXCLUDE_FROM_ALL)
|
||||
|
||||
include(EigenConfigureTesting)
|
||||
option(BUILD_TESTING "Enable creation of Eigen tests." ON)
|
||||
if(BUILD_TESTING)
|
||||
include(EigenConfigureTesting)
|
||||
|
||||
# fixme, not sure this line is still needed:
|
||||
enable_testing() # must be called from the root CMakeLists, see man page
|
||||
|
||||
|
||||
if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
|
||||
add_subdirectory(test) # can't do EXCLUDE_FROM_ALL here, breaks CTest
|
||||
else()
|
||||
add_subdirectory(test EXCLUDE_FROM_ALL)
|
||||
if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
|
||||
add_subdirectory(test) # can't do EXCLUDE_FROM_ALL here, breaks CTest
|
||||
else()
|
||||
add_subdirectory(test EXCLUDE_FROM_ALL)
|
||||
endif()
|
||||
endif()
|
||||
|
||||
if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
|
||||
|
|
@ -355,6 +454,13 @@ else()
|
|||
add_subdirectory(lapack EXCLUDE_FROM_ALL)
|
||||
endif()
|
||||
|
||||
# add SYCL
|
||||
option(EIGEN_TEST_SYCL "Add Sycl support." OFF)
|
||||
if(EIGEN_TEST_SYCL)
|
||||
set (CMAKE_MODULE_PATH "${CMAKE_ROOT}/Modules" "cmake/Modules/" "${CMAKE_MODULE_PATH}")
|
||||
include(FindComputeCpp)
|
||||
endif()
|
||||
|
||||
add_subdirectory(unsupported)
|
||||
|
||||
add_subdirectory(demos EXCLUDE_FROM_ALL)
|
||||
|
|
@ -373,7 +479,9 @@ endif(NOT WIN32)
|
|||
|
||||
configure_file(scripts/cdashtesting.cmake.in cdashtesting.cmake @ONLY)
|
||||
|
||||
ei_testing_print_summary()
|
||||
if(BUILD_TESTING)
|
||||
ei_testing_print_summary()
|
||||
endif()
|
||||
|
||||
message(STATUS "")
|
||||
message(STATUS "Configured Eigen ${EIGEN_VERSION_NUMBER}")
|
||||
|
|
@ -403,6 +511,7 @@ if(cmake_generator_tolower MATCHES "makefile")
|
|||
message(STATUS "make check | Build and run the unit-tests. Read this page:")
|
||||
message(STATUS " | http://eigen.tuxfamily.org/index.php?title=Tests")
|
||||
message(STATUS "make blas | Build BLAS library (not the same thing as Eigen)")
|
||||
message(STATUS "make uninstall| Removes files installed by make install")
|
||||
message(STATUS "--------------+--------------------------------------------------------------")
|
||||
else()
|
||||
message(STATUS "To build/run the unit tests, read this page:")
|
||||
|
|
@ -410,3 +519,98 @@ else()
|
|||
endif()
|
||||
|
||||
message(STATUS "")
|
||||
|
||||
|
||||
set ( EIGEN_VERSION_STRING ${EIGEN_VERSION_NUMBER} )
|
||||
set ( EIGEN_VERSION_MAJOR ${EIGEN_WORLD_VERSION} )
|
||||
set ( EIGEN_VERSION_MINOR ${EIGEN_MAJOR_VERSION} )
|
||||
set ( EIGEN_VERSION_PATCH ${EIGEN_MINOR_VERSION} )
|
||||
set ( EIGEN_DEFINITIONS "")
|
||||
set ( EIGEN_INCLUDE_DIR "${CMAKE_INSTALL_PREFIX}/${INCLUDE_INSTALL_DIR}" )
|
||||
set ( EIGEN_ROOT_DIR ${CMAKE_INSTALL_PREFIX} )
|
||||
|
||||
# Interface libraries require at least CMake 3.0
|
||||
if (NOT CMAKE_VERSION VERSION_LESS 3.0)
|
||||
include (CMakePackageConfigHelpers)
|
||||
|
||||
# Imported target support
|
||||
add_library (eigen INTERFACE)
|
||||
|
||||
target_compile_definitions (eigen INTERFACE ${EIGEN_DEFINITIONS})
|
||||
target_include_directories (eigen INTERFACE
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
|
||||
$<INSTALL_INTERFACE:${INCLUDE_INSTALL_DIR}>
|
||||
)
|
||||
|
||||
# Export as title case Eigen
|
||||
set_target_properties (eigen PROPERTIES EXPORT_NAME Eigen)
|
||||
|
||||
install (TARGETS eigen EXPORT Eigen3Targets)
|
||||
|
||||
configure_package_config_file (
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/cmake/Eigen3Config.cmake.in
|
||||
${CMAKE_CURRENT_BINARY_DIR}/Eigen3Config.cmake
|
||||
PATH_VARS EIGEN_INCLUDE_DIR EIGEN_ROOT_DIR
|
||||
INSTALL_DESTINATION ${CMAKEPACKAGE_INSTALL_DIR}
|
||||
NO_CHECK_REQUIRED_COMPONENTS_MACRO # Eigen does not provide components
|
||||
)
|
||||
# Remove CMAKE_SIZEOF_VOID_P from Eigen3ConfigVersion.cmake since Eigen does
|
||||
# not depend on architecture specific settings or libraries. More
|
||||
# specifically, an Eigen3Config.cmake generated from a 64 bit target can be
|
||||
# used for 32 bit targets as well (and vice versa).
|
||||
set (_Eigen3_CMAKE_SIZEOF_VOID_P ${CMAKE_SIZEOF_VOID_P})
|
||||
unset (CMAKE_SIZEOF_VOID_P)
|
||||
write_basic_package_version_file (Eigen3ConfigVersion.cmake
|
||||
VERSION ${EIGEN_VERSION_NUMBER}
|
||||
COMPATIBILITY SameMajorVersion)
|
||||
set (CMAKE_SIZEOF_VOID_P ${_Eigen3_CMAKE_SIZEOF_VOID_P})
|
||||
|
||||
# The Eigen target will be located in the Eigen3 namespace. Other CMake
|
||||
# targets can refer to it using Eigen3::Eigen.
|
||||
export (TARGETS eigen NAMESPACE Eigen3:: FILE Eigen3Targets.cmake)
|
||||
# Export Eigen3 package to CMake registry such that it can be easily found by
|
||||
# CMake even if it has not been installed to a standard directory.
|
||||
export (PACKAGE Eigen3)
|
||||
|
||||
install (EXPORT Eigen3Targets NAMESPACE Eigen3:: DESTINATION ${CMAKEPACKAGE_INSTALL_DIR})
|
||||
|
||||
else (NOT CMAKE_VERSION VERSION_LESS 3.0)
|
||||
# Fallback to legacy Eigen3Config.cmake without the imported target
|
||||
|
||||
# If CMakePackageConfigHelpers module is available (CMake >= 2.8.8)
|
||||
# create a relocatable Config file, otherwise leave the hardcoded paths
|
||||
include(CMakePackageConfigHelpers OPTIONAL RESULT_VARIABLE CPCH_PATH)
|
||||
|
||||
if(CPCH_PATH)
|
||||
configure_package_config_file (
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/cmake/Eigen3ConfigLegacy.cmake.in
|
||||
${CMAKE_CURRENT_BINARY_DIR}/Eigen3Config.cmake
|
||||
PATH_VARS EIGEN_INCLUDE_DIR EIGEN_ROOT_DIR
|
||||
INSTALL_DESTINATION ${CMAKEPACKAGE_INSTALL_DIR}
|
||||
NO_CHECK_REQUIRED_COMPONENTS_MACRO # Eigen does not provide components
|
||||
)
|
||||
else()
|
||||
# The PACKAGE_* variables are defined by the configure_package_config_file
|
||||
# but without it we define them manually to the hardcoded paths
|
||||
set(PACKAGE_INIT "")
|
||||
set(PACKAGE_EIGEN_INCLUDE_DIR ${EIGEN_INCLUDE_DIR})
|
||||
set(PACKAGE_EIGEN_ROOT_DIR ${EIGEN_ROOT_DIR})
|
||||
configure_file ( ${CMAKE_CURRENT_SOURCE_DIR}/cmake/Eigen3ConfigLegacy.cmake.in
|
||||
${CMAKE_CURRENT_BINARY_DIR}/Eigen3Config.cmake
|
||||
@ONLY ESCAPE_QUOTES )
|
||||
endif()
|
||||
|
||||
write_basic_package_version_file( Eigen3ConfigVersion.cmake
|
||||
VERSION ${EIGEN_VERSION_NUMBER}
|
||||
COMPATIBILITY SameMajorVersion )
|
||||
|
||||
endif (NOT CMAKE_VERSION VERSION_LESS 3.0)
|
||||
|
||||
install ( FILES ${CMAKE_CURRENT_SOURCE_DIR}/cmake/UseEigen3.cmake
|
||||
${CMAKE_CURRENT_BINARY_DIR}/Eigen3Config.cmake
|
||||
${CMAKE_CURRENT_BINARY_DIR}/Eigen3ConfigVersion.cmake
|
||||
DESTINATION ${CMAKEPACKAGE_INSTALL_DIR} )
|
||||
|
||||
# Add uninstall target
|
||||
add_custom_target ( uninstall
|
||||
COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_SOURCE_DIR}/cmake/EigenUninstall.cmake)
|
||||
|
|
|
|||
|
|
@ -4,10 +4,10 @@
|
|||
## # The following are required to uses Dart and the Cdash dashboard
|
||||
## ENABLE_TESTING()
|
||||
## INCLUDE(CTest)
|
||||
set(CTEST_PROJECT_NAME "Eigen3.2")
|
||||
set(CTEST_PROJECT_NAME "Eigen 3.3")
|
||||
set(CTEST_NIGHTLY_START_TIME "00:00:00 UTC")
|
||||
|
||||
set(CTEST_DROP_METHOD "http")
|
||||
set(CTEST_DROP_SITE "manao.inria.fr")
|
||||
set(CTEST_DROP_LOCATION "/CDash/submit.php?project=Eigen3.2")
|
||||
set(CTEST_DROP_LOCATION "/CDash/submit.php?project=Eigen+3.3")
|
||||
set(CTEST_DROP_SITE_CDASH TRUE)
|
||||
|
|
|
|||
|
|
@ -1,3 +1,4 @@
|
|||
|
||||
set(CTEST_CUSTOM_MAXIMUM_NUMBER_OF_WARNINGS "2000")
|
||||
set(CTEST_CUSTOM_MAXIMUM_NUMBER_OF_ERRORS "2000")
|
||||
list(APPEND CTEST_CUSTOM_ERROR_EXCEPTION @EIGEN_CTEST_ERROR_EXCEPTION@)
|
||||
|
|
|
|||
|
|
@ -1,11 +0,0 @@
|
|||
#ifndef EIGEN_ARRAY_MODULE_H
|
||||
#define EIGEN_ARRAY_MODULE_H
|
||||
|
||||
// include Core first to handle Eigen2 support macros
|
||||
#include "Core"
|
||||
|
||||
#ifndef EIGEN2_SUPPORT
|
||||
#error The Eigen/Array header does no longer exist in Eigen3. All that functionality has moved to Eigen/Core.
|
||||
#endif
|
||||
|
||||
#endif // EIGEN_ARRAY_MODULE_H
|
||||
|
|
@ -16,4 +16,4 @@ install(FILES
|
|||
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen COMPONENT Devel
|
||||
)
|
||||
|
||||
add_subdirectory(src)
|
||||
install(DIRECTORY src DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen COMPONENT Devel FILES_MATCHING PATTERN "*.h")
|
||||
|
|
|
|||
|
|
@ -1,7 +1,15 @@
|
|||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_CHOLESKY_MODULE_H
|
||||
#define EIGEN_CHOLESKY_MODULE_H
|
||||
|
||||
#include "Core"
|
||||
#include "Jacobi"
|
||||
|
||||
#include "src/Core/util/DisableStupidWarnings.h"
|
||||
|
||||
|
|
@ -10,20 +18,26 @@
|
|||
*
|
||||
*
|
||||
* This module provides two variants of the Cholesky decomposition for selfadjoint (hermitian) matrices.
|
||||
* Those decompositions are accessible via the following MatrixBase methods:
|
||||
* - MatrixBase::llt(),
|
||||
* Those decompositions are also accessible via the following methods:
|
||||
* - MatrixBase::llt()
|
||||
* - MatrixBase::ldlt()
|
||||
* - SelfAdjointView::llt()
|
||||
* - SelfAdjointView::ldlt()
|
||||
*
|
||||
* \code
|
||||
* #include <Eigen/Cholesky>
|
||||
* \endcode
|
||||
*/
|
||||
|
||||
#include "src/misc/Solve.h"
|
||||
#include "src/Cholesky/LLT.h"
|
||||
#include "src/Cholesky/LDLT.h"
|
||||
#ifdef EIGEN_USE_LAPACKE
|
||||
#include "src/Cholesky/LLT_MKL.h"
|
||||
#ifdef EIGEN_USE_MKL
|
||||
#include "mkl_lapacke.h"
|
||||
#else
|
||||
#include "src/misc/lapacke.h"
|
||||
#endif
|
||||
#include "src/Cholesky/LLT_LAPACKE.h"
|
||||
#endif
|
||||
|
||||
#include "src/Core/util/ReenableStupidWarnings.h"
|
||||
|
|
|
|||
|
|
@ -1,3 +1,10 @@
|
|||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_CHOLMODSUPPORT_MODULE_H
|
||||
#define EIGEN_CHOLMODSUPPORT_MODULE_H
|
||||
|
||||
|
|
@ -33,12 +40,8 @@ extern "C" {
|
|||
*
|
||||
*/
|
||||
|
||||
#include "src/misc/Solve.h"
|
||||
#include "src/misc/SparseSolve.h"
|
||||
|
||||
#include "src/CholmodSupport/CholmodSupport.h"
|
||||
|
||||
|
||||
#include "src/Core/util/ReenableStupidWarnings.h"
|
||||
|
||||
#endif // EIGEN_CHOLMODSUPPORT_MODULE_H
|
||||
|
|
|
|||
|
|
@ -14,6 +14,74 @@
|
|||
// first thing Eigen does: stop the compiler from committing suicide
|
||||
#include "src/Core/util/DisableStupidWarnings.h"
|
||||
|
||||
#if defined(__CUDACC__) && !defined(EIGEN_NO_CUDA)
|
||||
#define EIGEN_CUDACC __CUDACC__
|
||||
#endif
|
||||
|
||||
#if defined(__CUDA_ARCH__) && !defined(EIGEN_NO_CUDA)
|
||||
#define EIGEN_CUDA_ARCH __CUDA_ARCH__
|
||||
#endif
|
||||
|
||||
#if defined(__CUDACC_VER_MAJOR__) && (__CUDACC_VER_MAJOR__ >= 9)
|
||||
#define EIGEN_CUDACC_VER ((__CUDACC_VER_MAJOR__ * 10000) + (__CUDACC_VER_MINOR__ * 100))
|
||||
#elif defined(__CUDACC_VER__)
|
||||
#define EIGEN_CUDACC_VER __CUDACC_VER__
|
||||
#else
|
||||
#define EIGEN_CUDACC_VER 0
|
||||
#endif
|
||||
|
||||
// Handle NVCC/CUDA/SYCL
|
||||
#if defined(__CUDACC__) || defined(__SYCL_DEVICE_ONLY__)
|
||||
// Do not try asserts on CUDA and SYCL!
|
||||
#ifndef EIGEN_NO_DEBUG
|
||||
#define EIGEN_NO_DEBUG
|
||||
#endif
|
||||
|
||||
#ifdef EIGEN_INTERNAL_DEBUGGING
|
||||
#undef EIGEN_INTERNAL_DEBUGGING
|
||||
#endif
|
||||
|
||||
#ifdef EIGEN_EXCEPTIONS
|
||||
#undef EIGEN_EXCEPTIONS
|
||||
#endif
|
||||
|
||||
// All functions callable from CUDA code must be qualified with __device__
|
||||
#ifdef __CUDACC__
|
||||
// Do not try to vectorize on CUDA and SYCL!
|
||||
#ifndef EIGEN_DONT_VECTORIZE
|
||||
#define EIGEN_DONT_VECTORIZE
|
||||
#endif
|
||||
|
||||
#define EIGEN_DEVICE_FUNC __host__ __device__
|
||||
// We need cuda_runtime.h to ensure that that EIGEN_USING_STD_MATH macro
|
||||
// works properly on the device side
|
||||
#include <cuda_runtime.h>
|
||||
#else
|
||||
#define EIGEN_DEVICE_FUNC
|
||||
#endif
|
||||
|
||||
#else
|
||||
#define EIGEN_DEVICE_FUNC
|
||||
|
||||
#endif
|
||||
|
||||
// When compiling CUDA device code with NVCC, pull in math functions from the
|
||||
// global namespace. In host mode, and when device doee with clang, use the
|
||||
// std versions.
|
||||
#if defined(__CUDA_ARCH__) && defined(__NVCC__)
|
||||
#define EIGEN_USING_STD_MATH(FUNC) using ::FUNC;
|
||||
#else
|
||||
#define EIGEN_USING_STD_MATH(FUNC) using std::FUNC;
|
||||
#endif
|
||||
|
||||
#if (defined(_CPPUNWIND) || defined(__EXCEPTIONS)) && !defined(__CUDA_ARCH__) && !defined(EIGEN_EXCEPTIONS) && !defined(EIGEN_USE_SYCL)
|
||||
#define EIGEN_EXCEPTIONS
|
||||
#endif
|
||||
|
||||
#ifdef EIGEN_EXCEPTIONS
|
||||
#include <new>
|
||||
#endif
|
||||
|
||||
// then include this file where all our macros are defined. It's really important to do it first because
|
||||
// it's where we do all the alignment settings (platform detection and honoring the user's will if he
|
||||
// defined e.g. EIGEN_DONT_ALIGN) so it needs to be done before we do anything with vectorization.
|
||||
|
|
@ -21,7 +89,7 @@
|
|||
|
||||
// Disable the ipa-cp-clone optimization flag with MinGW 6.x or newer (enabled by default with -O3)
|
||||
// See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=556 for details.
|
||||
#if defined(__MINGW32__) && EIGEN_GNUC_AT_LEAST(4,6)
|
||||
#if EIGEN_COMP_MINGW && EIGEN_GNUC_AT_LEAST(4,6)
|
||||
#pragma GCC optimize ("-fno-ipa-cp-clone")
|
||||
#endif
|
||||
|
||||
|
|
@ -31,26 +99,26 @@
|
|||
// and inclusion of their respective header files
|
||||
#include "src/Core/util/MKL_support.h"
|
||||
|
||||
// if alignment is disabled, then disable vectorization. Note: EIGEN_ALIGN is the proper check, it takes into
|
||||
// account both the user's will (EIGEN_DONT_ALIGN) and our own platform checks
|
||||
#if !EIGEN_ALIGN
|
||||
// if alignment is disabled, then disable vectorization. Note: EIGEN_MAX_ALIGN_BYTES is the proper check, it takes into
|
||||
// account both the user's will (EIGEN_MAX_ALIGN_BYTES,EIGEN_DONT_ALIGN) and our own platform checks
|
||||
#if EIGEN_MAX_ALIGN_BYTES==0
|
||||
#ifndef EIGEN_DONT_VECTORIZE
|
||||
#define EIGEN_DONT_VECTORIZE
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#ifdef _MSC_VER
|
||||
#if EIGEN_COMP_MSVC
|
||||
#include <malloc.h> // for _aligned_malloc -- need it regardless of whether vectorization is enabled
|
||||
#if (_MSC_VER >= 1500) // 2008 or later
|
||||
#if (EIGEN_COMP_MSVC >= 1500) // 2008 or later
|
||||
// Remember that usage of defined() in a #define is undefined by the standard.
|
||||
// a user reported that in 64-bit mode, MSVC doesn't care to define _M_IX86_FP.
|
||||
#if (defined(_M_IX86_FP) && (_M_IX86_FP >= 2)) || defined(_M_X64)
|
||||
#if (defined(_M_IX86_FP) && (_M_IX86_FP >= 2)) || EIGEN_ARCH_x86_64
|
||||
#define EIGEN_SSE2_ON_MSVC_2008_OR_LATER
|
||||
#endif
|
||||
#endif
|
||||
#else
|
||||
// Remember that usage of defined() in a #define is undefined by the standard
|
||||
#if (defined __SSE2__) && ( (!defined __GNUC__) || (defined __INTEL_COMPILER) || EIGEN_GNUC_AT_LEAST(4,2) )
|
||||
#if (defined __SSE2__) && ( (!EIGEN_COMP_GNUC) || EIGEN_COMP_ICC || EIGEN_GNUC_AT_LEAST(4,2) )
|
||||
#define EIGEN_SSE2_ON_NON_MSVC_BUT_NOT_OLD_GCC
|
||||
#endif
|
||||
#endif
|
||||
|
|
@ -82,6 +150,31 @@
|
|||
#ifdef __SSE4_2__
|
||||
#define EIGEN_VECTORIZE_SSE4_2
|
||||
#endif
|
||||
#ifdef __AVX__
|
||||
#define EIGEN_VECTORIZE_AVX
|
||||
#define EIGEN_VECTORIZE_SSE3
|
||||
#define EIGEN_VECTORIZE_SSSE3
|
||||
#define EIGEN_VECTORIZE_SSE4_1
|
||||
#define EIGEN_VECTORIZE_SSE4_2
|
||||
#endif
|
||||
#ifdef __AVX2__
|
||||
#define EIGEN_VECTORIZE_AVX2
|
||||
#endif
|
||||
#ifdef __FMA__
|
||||
#define EIGEN_VECTORIZE_FMA
|
||||
#endif
|
||||
#if defined(__AVX512F__) && defined(EIGEN_ENABLE_AVX512)
|
||||
#define EIGEN_VECTORIZE_AVX512
|
||||
#define EIGEN_VECTORIZE_AVX2
|
||||
#define EIGEN_VECTORIZE_AVX
|
||||
#define EIGEN_VECTORIZE_FMA
|
||||
#ifdef __AVX512DQ__
|
||||
#define EIGEN_VECTORIZE_AVX512DQ
|
||||
#endif
|
||||
#ifdef __AVX512ER__
|
||||
#define EIGEN_VECTORIZE_AVX512ER
|
||||
#endif
|
||||
#endif
|
||||
|
||||
// include files
|
||||
|
||||
|
|
@ -95,9 +188,10 @@
|
|||
extern "C" {
|
||||
// In theory we should only include immintrin.h and not the other *mmintrin.h header files directly.
|
||||
// Doing so triggers some issues with ICC. However old gcc versions seems to not have this file, thus:
|
||||
#if defined(__INTEL_COMPILER) && __INTEL_COMPILER >= 1110
|
||||
#if EIGEN_COMP_ICC >= 1110
|
||||
#include <immintrin.h>
|
||||
#else
|
||||
#include <mmintrin.h>
|
||||
#include <emmintrin.h>
|
||||
#include <xmmintrin.h>
|
||||
#ifdef EIGEN_VECTORIZE_SSE3
|
||||
|
|
@ -112,8 +206,20 @@
|
|||
#ifdef EIGEN_VECTORIZE_SSE4_2
|
||||
#include <nmmintrin.h>
|
||||
#endif
|
||||
#if defined(EIGEN_VECTORIZE_AVX) || defined(EIGEN_VECTORIZE_AVX512)
|
||||
#include <immintrin.h>
|
||||
#endif
|
||||
#endif
|
||||
} // end extern "C"
|
||||
#elif defined __VSX__
|
||||
#define EIGEN_VECTORIZE
|
||||
#define EIGEN_VECTORIZE_VSX
|
||||
#include <altivec.h>
|
||||
// We need to #undef all these ugly tokens defined in <altivec.h>
|
||||
// => use __vector instead of vector
|
||||
#undef bool
|
||||
#undef vector
|
||||
#undef pixel
|
||||
#elif defined __ALTIVEC__
|
||||
#define EIGEN_VECTORIZE
|
||||
#define EIGEN_VECTORIZE_ALTIVEC
|
||||
|
|
@ -123,13 +229,35 @@
|
|||
#undef bool
|
||||
#undef vector
|
||||
#undef pixel
|
||||
#elif defined __ARM_NEON
|
||||
#elif (defined __ARM_NEON) || (defined __ARM_NEON__)
|
||||
#define EIGEN_VECTORIZE
|
||||
#define EIGEN_VECTORIZE_NEON
|
||||
#include <arm_neon.h>
|
||||
#elif (defined __s390x__ && defined __VEC__)
|
||||
#define EIGEN_VECTORIZE
|
||||
#define EIGEN_VECTORIZE_ZVECTOR
|
||||
#include <vecintrin.h>
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#if defined(__F16C__) && !defined(EIGEN_COMP_CLANG)
|
||||
// We can use the optimized fp16 to float and float to fp16 conversion routines
|
||||
#define EIGEN_HAS_FP16_C
|
||||
#endif
|
||||
|
||||
#if defined __CUDACC__
|
||||
#define EIGEN_VECTORIZE_CUDA
|
||||
#include <vector_types.h>
|
||||
#if EIGEN_CUDACC_VER >= 70500
|
||||
#define EIGEN_HAS_CUDA_FP16
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#if defined EIGEN_HAS_CUDA_FP16
|
||||
#include <host_defines.h>
|
||||
#include <cuda_fp16.h>
|
||||
#endif
|
||||
|
||||
#if (defined _OPENMP) && (!defined EIGEN_DONT_PARALLELIZE)
|
||||
#define EIGEN_HAS_OPENMP
|
||||
#endif
|
||||
|
|
@ -139,7 +267,7 @@
|
|||
#endif
|
||||
|
||||
// MSVC for windows mobile does not have the errno.h file
|
||||
#if !(defined(_MSC_VER) && defined(_WIN32_WCE)) && !defined(__ARMCC_VERSION)
|
||||
#if !(EIGEN_COMP_MSVC && EIGEN_OS_WINCE) && !EIGEN_COMP_ARM
|
||||
#define EIGEN_HAS_ERRNO
|
||||
#endif
|
||||
|
||||
|
|
@ -159,29 +287,30 @@
|
|||
// for min/max:
|
||||
#include <algorithm>
|
||||
|
||||
// for std::is_nothrow_move_assignable
|
||||
#ifdef EIGEN_INCLUDE_TYPE_TRAITS
|
||||
#include <type_traits>
|
||||
#endif
|
||||
|
||||
// for outputting debug info
|
||||
#ifdef EIGEN_DEBUG_ASSIGN
|
||||
#include <iostream>
|
||||
#endif
|
||||
|
||||
// required for __cpuid, needs to be included after cmath
|
||||
#if defined(_MSC_VER) && (defined(_M_IX86)||defined(_M_X64)) && (!defined(_WIN32_WCE))
|
||||
#if EIGEN_COMP_MSVC && EIGEN_ARCH_i386_OR_x86_64 && !EIGEN_OS_WINCE
|
||||
#include <intrin.h>
|
||||
#endif
|
||||
|
||||
#if defined(_CPPUNWIND) || defined(__EXCEPTIONS)
|
||||
#define EIGEN_EXCEPTIONS
|
||||
#endif
|
||||
|
||||
#ifdef EIGEN_EXCEPTIONS
|
||||
#include <new>
|
||||
#endif
|
||||
|
||||
/** \brief Namespace containing all symbols from the %Eigen library. */
|
||||
namespace Eigen {
|
||||
|
||||
inline static const char *SimdInstructionSetsInUse(void) {
|
||||
#if defined(EIGEN_VECTORIZE_SSE4_2)
|
||||
#if defined(EIGEN_VECTORIZE_AVX512)
|
||||
return "AVX512, FMA, AVX2, AVX, SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2";
|
||||
#elif defined(EIGEN_VECTORIZE_AVX)
|
||||
return "AVX SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2";
|
||||
#elif defined(EIGEN_VECTORIZE_SSE4_2)
|
||||
return "SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2";
|
||||
#elif defined(EIGEN_VECTORIZE_SSE4_1)
|
||||
return "SSE, SSE2, SSE3, SSSE3, SSE4.1";
|
||||
|
|
@ -193,8 +322,12 @@ inline static const char *SimdInstructionSetsInUse(void) {
|
|||
return "SSE, SSE2";
|
||||
#elif defined(EIGEN_VECTORIZE_ALTIVEC)
|
||||
return "AltiVec";
|
||||
#elif defined(EIGEN_VECTORIZE_VSX)
|
||||
return "VSX";
|
||||
#elif defined(EIGEN_VECTORIZE_NEON)
|
||||
return "ARM NEON";
|
||||
#elif defined(EIGEN_VECTORIZE_ZVECTOR)
|
||||
return "S390X ZVECTOR";
|
||||
#else
|
||||
return "None";
|
||||
#endif
|
||||
|
|
@ -202,42 +335,21 @@ inline static const char *SimdInstructionSetsInUse(void) {
|
|||
|
||||
} // end namespace Eigen
|
||||
|
||||
#define STAGE10_FULL_EIGEN2_API 10
|
||||
#define STAGE20_RESOLVE_API_CONFLICTS 20
|
||||
#define STAGE30_FULL_EIGEN3_API 30
|
||||
#define STAGE40_FULL_EIGEN3_STRICTNESS 40
|
||||
#define STAGE99_NO_EIGEN2_SUPPORT 99
|
||||
|
||||
#if defined EIGEN2_SUPPORT_STAGE40_FULL_EIGEN3_STRICTNESS
|
||||
#define EIGEN2_SUPPORT
|
||||
#define EIGEN2_SUPPORT_STAGE STAGE40_FULL_EIGEN3_STRICTNESS
|
||||
#elif defined EIGEN2_SUPPORT_STAGE30_FULL_EIGEN3_API
|
||||
#define EIGEN2_SUPPORT
|
||||
#define EIGEN2_SUPPORT_STAGE STAGE30_FULL_EIGEN3_API
|
||||
#elif defined EIGEN2_SUPPORT_STAGE20_RESOLVE_API_CONFLICTS
|
||||
#define EIGEN2_SUPPORT
|
||||
#define EIGEN2_SUPPORT_STAGE STAGE20_RESOLVE_API_CONFLICTS
|
||||
#elif defined EIGEN2_SUPPORT_STAGE10_FULL_EIGEN2_API
|
||||
#define EIGEN2_SUPPORT
|
||||
#define EIGEN2_SUPPORT_STAGE STAGE10_FULL_EIGEN2_API
|
||||
#elif defined EIGEN2_SUPPORT
|
||||
// default to stage 3, that's what it's always meant
|
||||
#define EIGEN2_SUPPORT_STAGE30_FULL_EIGEN3_API
|
||||
#define EIGEN2_SUPPORT_STAGE STAGE30_FULL_EIGEN3_API
|
||||
#else
|
||||
#define EIGEN2_SUPPORT_STAGE STAGE99_NO_EIGEN2_SUPPORT
|
||||
#if defined EIGEN2_SUPPORT_STAGE40_FULL_EIGEN3_STRICTNESS || defined EIGEN2_SUPPORT_STAGE30_FULL_EIGEN3_API || defined EIGEN2_SUPPORT_STAGE20_RESOLVE_API_CONFLICTS || defined EIGEN2_SUPPORT_STAGE10_FULL_EIGEN2_API || defined EIGEN2_SUPPORT
|
||||
// This will generate an error message:
|
||||
#error Eigen2-support is only available up to version 3.2. Please go to "http://eigen.tuxfamily.org/index.php?title=Eigen2" for further information
|
||||
#endif
|
||||
|
||||
#ifdef EIGEN2_SUPPORT
|
||||
#undef minor
|
||||
#endif
|
||||
namespace Eigen {
|
||||
|
||||
// we use size_t frequently and we'll never remember to prepend it with std:: everytime just to
|
||||
// ensure QNX/QCC support
|
||||
using std::size_t;
|
||||
// gcc 4.6.0 wants std:: for ptrdiff_t
|
||||
// gcc 4.6.0 wants std:: for ptrdiff_t
|
||||
using std::ptrdiff_t;
|
||||
|
||||
}
|
||||
|
||||
/** \defgroup Core_Module Core module
|
||||
* This is the main module of Eigen providing dense matrix and vector support
|
||||
* (both fixed and dynamic size) with all the features corresponding to a BLAS library
|
||||
|
|
@ -249,8 +361,8 @@ using std::ptrdiff_t;
|
|||
*/
|
||||
|
||||
#include "src/Core/util/Constants.h"
|
||||
#include "src/Core/util/ForwardDeclarations.h"
|
||||
#include "src/Core/util/Meta.h"
|
||||
#include "src/Core/util/ForwardDeclarations.h"
|
||||
#include "src/Core/util/StaticAssert.h"
|
||||
#include "src/Core/util/XprHelper.h"
|
||||
#include "src/Core/util/Memory.h"
|
||||
|
|
@ -258,41 +370,94 @@ using std::ptrdiff_t;
|
|||
#include "src/Core/NumTraits.h"
|
||||
#include "src/Core/MathFunctions.h"
|
||||
#include "src/Core/GenericPacketMath.h"
|
||||
#include "src/Core/MathFunctionsImpl.h"
|
||||
#include "src/Core/arch/Default/ConjHelper.h"
|
||||
|
||||
#if defined EIGEN_VECTORIZE_SSE
|
||||
#if defined EIGEN_VECTORIZE_AVX512
|
||||
#include "src/Core/arch/SSE/PacketMath.h"
|
||||
#include "src/Core/arch/AVX/PacketMath.h"
|
||||
#include "src/Core/arch/AVX512/PacketMath.h"
|
||||
#include "src/Core/arch/AVX512/MathFunctions.h"
|
||||
#elif defined EIGEN_VECTORIZE_AVX
|
||||
// Use AVX for floats and doubles, SSE for integers
|
||||
#include "src/Core/arch/SSE/PacketMath.h"
|
||||
#include "src/Core/arch/SSE/Complex.h"
|
||||
#include "src/Core/arch/SSE/MathFunctions.h"
|
||||
#include "src/Core/arch/AVX/PacketMath.h"
|
||||
#include "src/Core/arch/AVX/MathFunctions.h"
|
||||
#include "src/Core/arch/AVX/Complex.h"
|
||||
#include "src/Core/arch/AVX/TypeCasting.h"
|
||||
#include "src/Core/arch/SSE/TypeCasting.h"
|
||||
#elif defined EIGEN_VECTORIZE_SSE
|
||||
#include "src/Core/arch/SSE/PacketMath.h"
|
||||
#include "src/Core/arch/SSE/MathFunctions.h"
|
||||
#include "src/Core/arch/SSE/Complex.h"
|
||||
#elif defined EIGEN_VECTORIZE_ALTIVEC
|
||||
#include "src/Core/arch/SSE/TypeCasting.h"
|
||||
#elif defined(EIGEN_VECTORIZE_ALTIVEC) || defined(EIGEN_VECTORIZE_VSX)
|
||||
#include "src/Core/arch/AltiVec/PacketMath.h"
|
||||
#include "src/Core/arch/AltiVec/MathFunctions.h"
|
||||
#include "src/Core/arch/AltiVec/Complex.h"
|
||||
#elif defined EIGEN_VECTORIZE_NEON
|
||||
#include "src/Core/arch/NEON/PacketMath.h"
|
||||
#include "src/Core/arch/NEON/MathFunctions.h"
|
||||
#include "src/Core/arch/NEON/Complex.h"
|
||||
#elif defined EIGEN_VECTORIZE_ZVECTOR
|
||||
#include "src/Core/arch/ZVector/PacketMath.h"
|
||||
#include "src/Core/arch/ZVector/MathFunctions.h"
|
||||
#include "src/Core/arch/ZVector/Complex.h"
|
||||
#endif
|
||||
|
||||
// Half float support
|
||||
#include "src/Core/arch/CUDA/Half.h"
|
||||
#include "src/Core/arch/CUDA/PacketMathHalf.h"
|
||||
#include "src/Core/arch/CUDA/TypeCasting.h"
|
||||
|
||||
#if defined EIGEN_VECTORIZE_CUDA
|
||||
#include "src/Core/arch/CUDA/PacketMath.h"
|
||||
#include "src/Core/arch/CUDA/MathFunctions.h"
|
||||
#endif
|
||||
|
||||
#include "src/Core/arch/Default/Settings.h"
|
||||
|
||||
#include "src/Core/Functors.h"
|
||||
#include "src/Core/functors/TernaryFunctors.h"
|
||||
#include "src/Core/functors/BinaryFunctors.h"
|
||||
#include "src/Core/functors/UnaryFunctors.h"
|
||||
#include "src/Core/functors/NullaryFunctors.h"
|
||||
#include "src/Core/functors/StlFunctors.h"
|
||||
#include "src/Core/functors/AssignmentFunctors.h"
|
||||
|
||||
// Specialized functors to enable the processing of complex numbers
|
||||
// on CUDA devices
|
||||
#include "src/Core/arch/CUDA/Complex.h"
|
||||
|
||||
#include "src/Core/IO.h"
|
||||
#include "src/Core/DenseCoeffsBase.h"
|
||||
#include "src/Core/DenseBase.h"
|
||||
#include "src/Core/MatrixBase.h"
|
||||
#include "src/Core/EigenBase.h"
|
||||
|
||||
#include "src/Core/Product.h"
|
||||
#include "src/Core/CoreEvaluators.h"
|
||||
#include "src/Core/AssignEvaluator.h"
|
||||
|
||||
#ifndef EIGEN_PARSED_BY_DOXYGEN // work around Doxygen bug triggered by Assign.h r814874
|
||||
// at least confirmed with Doxygen 1.5.5 and 1.5.6
|
||||
#include "src/Core/Assign.h"
|
||||
#endif
|
||||
|
||||
#include "src/Core/ArrayBase.h"
|
||||
#include "src/Core/util/BlasUtil.h"
|
||||
#include "src/Core/DenseStorage.h"
|
||||
#include "src/Core/NestByValue.h"
|
||||
#include "src/Core/ForceAlignedAccess.h"
|
||||
|
||||
// #include "src/Core/ForceAlignedAccess.h"
|
||||
|
||||
#include "src/Core/ReturnByValue.h"
|
||||
#include "src/Core/NoAlias.h"
|
||||
#include "src/Core/PlainObjectBase.h"
|
||||
#include "src/Core/Matrix.h"
|
||||
#include "src/Core/Array.h"
|
||||
#include "src/Core/CwiseTernaryOp.h"
|
||||
#include "src/Core/CwiseBinaryOp.h"
|
||||
#include "src/Core/CwiseUnaryOp.h"
|
||||
#include "src/Core/CwiseNullaryOp.h"
|
||||
|
|
@ -300,32 +465,32 @@ using std::ptrdiff_t;
|
|||
#include "src/Core/SelfCwiseBinaryOp.h"
|
||||
#include "src/Core/Dot.h"
|
||||
#include "src/Core/StableNorm.h"
|
||||
#include "src/Core/MapBase.h"
|
||||
#include "src/Core/Stride.h"
|
||||
#include "src/Core/MapBase.h"
|
||||
#include "src/Core/Map.h"
|
||||
#include "src/Core/Ref.h"
|
||||
#include "src/Core/Block.h"
|
||||
#include "src/Core/VectorBlock.h"
|
||||
#include "src/Core/Ref.h"
|
||||
#include "src/Core/Transpose.h"
|
||||
#include "src/Core/DiagonalMatrix.h"
|
||||
#include "src/Core/Diagonal.h"
|
||||
#include "src/Core/DiagonalProduct.h"
|
||||
#include "src/Core/PermutationMatrix.h"
|
||||
#include "src/Core/Transpositions.h"
|
||||
#include "src/Core/Redux.h"
|
||||
#include "src/Core/Visitor.h"
|
||||
#include "src/Core/Fuzzy.h"
|
||||
#include "src/Core/IO.h"
|
||||
#include "src/Core/Swap.h"
|
||||
#include "src/Core/CommaInitializer.h"
|
||||
#include "src/Core/Flagged.h"
|
||||
#include "src/Core/ProductBase.h"
|
||||
#include "src/Core/GeneralProduct.h"
|
||||
#include "src/Core/Solve.h"
|
||||
#include "src/Core/Inverse.h"
|
||||
#include "src/Core/SolverBase.h"
|
||||
#include "src/Core/PermutationMatrix.h"
|
||||
#include "src/Core/Transpositions.h"
|
||||
#include "src/Core/TriangularMatrix.h"
|
||||
#include "src/Core/SelfAdjointView.h"
|
||||
#include "src/Core/products/GeneralBlockPanelKernel.h"
|
||||
#include "src/Core/products/Parallelizer.h"
|
||||
#include "src/Core/products/CoeffBasedProduct.h"
|
||||
#include "src/Core/ProductEvaluators.h"
|
||||
#include "src/Core/products/GeneralMatrixVector.h"
|
||||
#include "src/Core/products/GeneralMatrixMatrix.h"
|
||||
#include "src/Core/SolveTriangular.h"
|
||||
|
|
@ -340,6 +505,7 @@ using std::ptrdiff_t;
|
|||
#include "src/Core/products/TriangularSolverVector.h"
|
||||
#include "src/Core/BandMatrix.h"
|
||||
#include "src/Core/CoreIterators.h"
|
||||
#include "src/Core/ConditionEstimator.h"
|
||||
|
||||
#include "src/Core/BooleanRedux.h"
|
||||
#include "src/Core/Select.h"
|
||||
|
|
@ -347,18 +513,17 @@ using std::ptrdiff_t;
|
|||
#include "src/Core/Random.h"
|
||||
#include "src/Core/Replicate.h"
|
||||
#include "src/Core/Reverse.h"
|
||||
#include "src/Core/ArrayBase.h"
|
||||
#include "src/Core/ArrayWrapper.h"
|
||||
|
||||
#ifdef EIGEN_USE_BLAS
|
||||
#include "src/Core/products/GeneralMatrixMatrix_MKL.h"
|
||||
#include "src/Core/products/GeneralMatrixVector_MKL.h"
|
||||
#include "src/Core/products/GeneralMatrixMatrixTriangular_MKL.h"
|
||||
#include "src/Core/products/SelfadjointMatrixMatrix_MKL.h"
|
||||
#include "src/Core/products/SelfadjointMatrixVector_MKL.h"
|
||||
#include "src/Core/products/TriangularMatrixMatrix_MKL.h"
|
||||
#include "src/Core/products/TriangularMatrixVector_MKL.h"
|
||||
#include "src/Core/products/TriangularSolverMatrix_MKL.h"
|
||||
#include "src/Core/products/GeneralMatrixMatrix_BLAS.h"
|
||||
#include "src/Core/products/GeneralMatrixVector_BLAS.h"
|
||||
#include "src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h"
|
||||
#include "src/Core/products/SelfadjointMatrixMatrix_BLAS.h"
|
||||
#include "src/Core/products/SelfadjointMatrixVector_BLAS.h"
|
||||
#include "src/Core/products/TriangularMatrixMatrix_BLAS.h"
|
||||
#include "src/Core/products/TriangularMatrixVector_BLAS.h"
|
||||
#include "src/Core/products/TriangularSolverMatrix_BLAS.h"
|
||||
#endif // EIGEN_USE_BLAS
|
||||
|
||||
#ifdef EIGEN_USE_MKL_VML
|
||||
|
|
@ -369,8 +534,4 @@ using std::ptrdiff_t;
|
|||
|
||||
#include "src/Core/util/ReenableStupidWarnings.h"
|
||||
|
||||
#ifdef EIGEN2_SUPPORT
|
||||
#include "Eigen2Support"
|
||||
#endif
|
||||
|
||||
#endif // EIGEN_CORE_H
|
||||
|
|
|
|||
|
|
@ -1,2 +1,2 @@
|
|||
#include "Dense"
|
||||
//#include "Sparse"
|
||||
#include "Sparse"
|
||||
|
|
|
|||
|
|
@ -1,95 +0,0 @@
|
|||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN2SUPPORT_H
|
||||
#define EIGEN2SUPPORT_H
|
||||
|
||||
#if (!defined(EIGEN2_SUPPORT)) || (!defined(EIGEN_CORE_H))
|
||||
#error Eigen2 support must be enabled by defining EIGEN2_SUPPORT before including any Eigen header
|
||||
#endif
|
||||
|
||||
#ifndef EIGEN_NO_EIGEN2_DEPRECATED_WARNING
|
||||
|
||||
#if defined(__GNUC__) || defined(__INTEL_COMPILER) || defined(__clang__)
|
||||
#warning "Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3. (Define EIGEN_NO_EIGEN2_DEPRECATED_WARNING to disable this warning)"
|
||||
#else
|
||||
#pragma message ("Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3. (Define EIGEN_NO_EIGEN2_DEPRECATED_WARNING to disable this warning)")
|
||||
#endif
|
||||
|
||||
#endif // EIGEN_NO_EIGEN2_DEPRECATED_WARNING
|
||||
|
||||
#include "src/Core/util/DisableStupidWarnings.h"
|
||||
|
||||
/** \ingroup Support_modules
|
||||
* \defgroup Eigen2Support_Module Eigen2 support module
|
||||
*
|
||||
* \warning Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3.
|
||||
*
|
||||
* This module provides a couple of deprecated functions improving the compatibility with Eigen2.
|
||||
*
|
||||
* To use it, define EIGEN2_SUPPORT before including any Eigen header
|
||||
* \code
|
||||
* #define EIGEN2_SUPPORT
|
||||
* \endcode
|
||||
*
|
||||
*/
|
||||
|
||||
#include "src/Eigen2Support/Macros.h"
|
||||
#include "src/Eigen2Support/Memory.h"
|
||||
#include "src/Eigen2Support/Meta.h"
|
||||
#include "src/Eigen2Support/Lazy.h"
|
||||
#include "src/Eigen2Support/Cwise.h"
|
||||
#include "src/Eigen2Support/CwiseOperators.h"
|
||||
#include "src/Eigen2Support/TriangularSolver.h"
|
||||
#include "src/Eigen2Support/Block.h"
|
||||
#include "src/Eigen2Support/VectorBlock.h"
|
||||
#include "src/Eigen2Support/Minor.h"
|
||||
#include "src/Eigen2Support/MathFunctions.h"
|
||||
|
||||
|
||||
#include "src/Core/util/ReenableStupidWarnings.h"
|
||||
|
||||
// Eigen2 used to include iostream
|
||||
#include<iostream>
|
||||
|
||||
#define EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, SizeSuffix) \
|
||||
using Eigen::Matrix##SizeSuffix##TypeSuffix; \
|
||||
using Eigen::Vector##SizeSuffix##TypeSuffix; \
|
||||
using Eigen::RowVector##SizeSuffix##TypeSuffix;
|
||||
|
||||
#define EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(TypeSuffix) \
|
||||
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 2) \
|
||||
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 3) \
|
||||
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 4) \
|
||||
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, X) \
|
||||
|
||||
#define EIGEN_USING_MATRIX_TYPEDEFS \
|
||||
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(i) \
|
||||
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(f) \
|
||||
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(d) \
|
||||
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(cf) \
|
||||
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(cd)
|
||||
|
||||
#define USING_PART_OF_NAMESPACE_EIGEN \
|
||||
EIGEN_USING_MATRIX_TYPEDEFS \
|
||||
using Eigen::Matrix; \
|
||||
using Eigen::MatrixBase; \
|
||||
using Eigen::ei_random; \
|
||||
using Eigen::ei_real; \
|
||||
using Eigen::ei_imag; \
|
||||
using Eigen::ei_conj; \
|
||||
using Eigen::ei_abs; \
|
||||
using Eigen::ei_abs2; \
|
||||
using Eigen::ei_sqrt; \
|
||||
using Eigen::ei_exp; \
|
||||
using Eigen::ei_log; \
|
||||
using Eigen::ei_sin; \
|
||||
using Eigen::ei_cos;
|
||||
|
||||
#endif // EIGEN2SUPPORT_H
|
||||
|
|
@ -1,3 +1,10 @@
|
|||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_EIGENVALUES_MODULE_H
|
||||
#define EIGEN_EIGENVALUES_MODULE_H
|
||||
|
||||
|
|
@ -25,6 +32,7 @@
|
|||
* \endcode
|
||||
*/
|
||||
|
||||
#include "src/misc/RealSvd2x2.h"
|
||||
#include "src/Eigenvalues/Tridiagonalization.h"
|
||||
#include "src/Eigenvalues/RealSchur.h"
|
||||
#include "src/Eigenvalues/EigenSolver.h"
|
||||
|
|
@ -37,9 +45,14 @@
|
|||
#include "src/Eigenvalues/GeneralizedEigenSolver.h"
|
||||
#include "src/Eigenvalues/MatrixBaseEigenvalues.h"
|
||||
#ifdef EIGEN_USE_LAPACKE
|
||||
#include "src/Eigenvalues/RealSchur_MKL.h"
|
||||
#include "src/Eigenvalues/ComplexSchur_MKL.h"
|
||||
#include "src/Eigenvalues/SelfAdjointEigenSolver_MKL.h"
|
||||
#ifdef EIGEN_USE_MKL
|
||||
#include "mkl_lapacke.h"
|
||||
#else
|
||||
#include "src/misc/lapacke.h"
|
||||
#endif
|
||||
#include "src/Eigenvalues/RealSchur_LAPACKE.h"
|
||||
#include "src/Eigenvalues/ComplexSchur_LAPACKE.h"
|
||||
#include "src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h"
|
||||
#endif
|
||||
|
||||
#include "src/Core/util/ReenableStupidWarnings.h"
|
||||
|
|
|
|||
|
|
@ -1,3 +1,10 @@
|
|||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_GEOMETRY_MODULE_H
|
||||
#define EIGEN_GEOMETRY_MODULE_H
|
||||
|
||||
|
|
@ -9,21 +16,17 @@
|
|||
#include "LU"
|
||||
#include <limits>
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846
|
||||
#endif
|
||||
|
||||
/** \defgroup Geometry_Module Geometry module
|
||||
*
|
||||
*
|
||||
*
|
||||
* This module provides support for:
|
||||
* - fixed-size homogeneous transformations
|
||||
* - translation, scaling, 2D and 3D rotations
|
||||
* - quaternions
|
||||
* - \ref MatrixBase::cross() "cross product"
|
||||
* - \ref MatrixBase::unitOrthogonal() "orthognal vector generation"
|
||||
* - some linear components: parametrized-lines and hyperplanes
|
||||
* - \link Quaternion quaternions \endlink
|
||||
* - cross products (\ref MatrixBase::cross, \ref MatrixBase::cross3)
|
||||
* - orthognal vector generation (\ref MatrixBase::unitOrthogonal)
|
||||
* - some linear components: \link ParametrizedLine parametrized-lines \endlink and \link Hyperplane hyperplanes \endlink
|
||||
* - \link AlignedBox axis aligned bounding boxes \endlink
|
||||
* - \link umeyama least-square transformation fitting \endlink
|
||||
*
|
||||
* \code
|
||||
* #include <Eigen/Geometry>
|
||||
|
|
@ -33,27 +36,23 @@
|
|||
#include "src/Geometry/OrthoMethods.h"
|
||||
#include "src/Geometry/EulerAngles.h"
|
||||
|
||||
#if EIGEN2_SUPPORT_STAGE > STAGE20_RESOLVE_API_CONFLICTS
|
||||
#include "src/Geometry/Homogeneous.h"
|
||||
#include "src/Geometry/RotationBase.h"
|
||||
#include "src/Geometry/Rotation2D.h"
|
||||
#include "src/Geometry/Quaternion.h"
|
||||
#include "src/Geometry/AngleAxis.h"
|
||||
#include "src/Geometry/Transform.h"
|
||||
#include "src/Geometry/Translation.h"
|
||||
#include "src/Geometry/Scaling.h"
|
||||
#include "src/Geometry/Hyperplane.h"
|
||||
#include "src/Geometry/ParametrizedLine.h"
|
||||
#include "src/Geometry/AlignedBox.h"
|
||||
#include "src/Geometry/Umeyama.h"
|
||||
#include "src/Geometry/Homogeneous.h"
|
||||
#include "src/Geometry/RotationBase.h"
|
||||
#include "src/Geometry/Rotation2D.h"
|
||||
#include "src/Geometry/Quaternion.h"
|
||||
#include "src/Geometry/AngleAxis.h"
|
||||
#include "src/Geometry/Transform.h"
|
||||
#include "src/Geometry/Translation.h"
|
||||
#include "src/Geometry/Scaling.h"
|
||||
#include "src/Geometry/Hyperplane.h"
|
||||
#include "src/Geometry/ParametrizedLine.h"
|
||||
#include "src/Geometry/AlignedBox.h"
|
||||
#include "src/Geometry/Umeyama.h"
|
||||
|
||||
#if defined EIGEN_VECTORIZE_SSE
|
||||
#include "src/Geometry/arch/Geometry_SSE.h"
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#ifdef EIGEN2_SUPPORT
|
||||
#include "src/Eigen2Support/Geometry/All.h"
|
||||
// Use the SSE optimized version whenever possible. At the moment the
|
||||
// SSE version doesn't compile when AVX is enabled
|
||||
#if defined EIGEN_VECTORIZE_SSE && !defined EIGEN_VECTORIZE_AVX
|
||||
#include "src/Geometry/arch/Geometry_SSE.h"
|
||||
#endif
|
||||
|
||||
#include "src/Core/util/ReenableStupidWarnings.h"
|
||||
|
|
|
|||
|
|
@ -1,3 +1,10 @@
|
|||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_HOUSEHOLDER_MODULE_H
|
||||
#define EIGEN_HOUSEHOLDER_MODULE_H
|
||||
|
||||
|
|
|
|||
|
|
@ -1,3 +1,10 @@
|
|||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_ITERATIVELINEARSOLVERS_MODULE_H
|
||||
#define EIGEN_ITERATIVELINEARSOLVERS_MODULE_H
|
||||
|
||||
|
|
@ -12,28 +19,29 @@
|
|||
* This module currently provides iterative methods to solve problems of the form \c A \c x = \c b, where \c A is a squared matrix, usually very large and sparse.
|
||||
* Those solvers are accessible via the following classes:
|
||||
* - ConjugateGradient for selfadjoint (hermitian) matrices,
|
||||
* - LeastSquaresConjugateGradient for rectangular least-square problems,
|
||||
* - BiCGSTAB for general square matrices.
|
||||
*
|
||||
* These iterative solvers are associated with some preconditioners:
|
||||
* - IdentityPreconditioner - not really useful
|
||||
* - DiagonalPreconditioner - also called JAcobi preconditioner, work very well on diagonal dominant matrices.
|
||||
* - IncompleteILUT - incomplete LU factorization with dual thresholding
|
||||
* - DiagonalPreconditioner - also called Jacobi preconditioner, work very well on diagonal dominant matrices.
|
||||
* - IncompleteLUT - incomplete LU factorization with dual thresholding
|
||||
*
|
||||
* Such problems can also be solved using the direct sparse decomposition modules: SparseCholesky, CholmodSupport, UmfPackSupport, SuperLUSupport.
|
||||
*
|
||||
* \code
|
||||
* #include <Eigen/IterativeLinearSolvers>
|
||||
* \endcode
|
||||
\code
|
||||
#include <Eigen/IterativeLinearSolvers>
|
||||
\endcode
|
||||
*/
|
||||
|
||||
#include "src/misc/Solve.h"
|
||||
#include "src/misc/SparseSolve.h"
|
||||
|
||||
#include "src/IterativeLinearSolvers/SolveWithGuess.h"
|
||||
#include "src/IterativeLinearSolvers/IterativeSolverBase.h"
|
||||
#include "src/IterativeLinearSolvers/BasicPreconditioners.h"
|
||||
#include "src/IterativeLinearSolvers/ConjugateGradient.h"
|
||||
#include "src/IterativeLinearSolvers/LeastSquareConjugateGradient.h"
|
||||
#include "src/IterativeLinearSolvers/BiCGSTAB.h"
|
||||
#include "src/IterativeLinearSolvers/IncompleteLUT.h"
|
||||
#include "src/IterativeLinearSolvers/IncompleteCholesky.h"
|
||||
|
||||
#include "src/Core/util/ReenableStupidWarnings.h"
|
||||
|
||||
|
|
|
|||
|
|
@ -1,3 +1,10 @@
|
|||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_JACOBI_MODULE_H
|
||||
#define EIGEN_JACOBI_MODULE_H
|
||||
|
||||
|
|
|
|||
|
|
@ -1,3 +1,10 @@
|
|||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_LU_MODULE_H
|
||||
#define EIGEN_LU_MODULE_H
|
||||
|
||||
|
|
@ -16,25 +23,27 @@
|
|||
* \endcode
|
||||
*/
|
||||
|
||||
#include "src/misc/Solve.h"
|
||||
#include "src/misc/Kernel.h"
|
||||
#include "src/misc/Image.h"
|
||||
#include "src/LU/FullPivLU.h"
|
||||
#include "src/LU/PartialPivLU.h"
|
||||
#ifdef EIGEN_USE_LAPACKE
|
||||
#include "src/LU/PartialPivLU_MKL.h"
|
||||
#ifdef EIGEN_USE_MKL
|
||||
#include "mkl_lapacke.h"
|
||||
#else
|
||||
#include "src/misc/lapacke.h"
|
||||
#endif
|
||||
#include "src/LU/PartialPivLU_LAPACKE.h"
|
||||
#endif
|
||||
#include "src/LU/Determinant.h"
|
||||
#include "src/LU/Inverse.h"
|
||||
#include "src/LU/InverseImpl.h"
|
||||
|
||||
#if defined EIGEN_VECTORIZE_SSE
|
||||
// Use the SSE optimized version whenever possible. At the moment the
|
||||
// SSE version doesn't compile when AVX is enabled
|
||||
#if defined EIGEN_VECTORIZE_SSE && !defined EIGEN_VECTORIZE_AVX
|
||||
#include "src/LU/arch/Inverse_SSE.h"
|
||||
#endif
|
||||
|
||||
#ifdef EIGEN2_SUPPORT
|
||||
#include "src/Eigen2Support/LU.h"
|
||||
#endif
|
||||
|
||||
#include "src/Core/util/ReenableStupidWarnings.h"
|
||||
|
||||
#endif // EIGEN_LU_MODULE_H
|
||||
|
|
|
|||
|
|
@ -1,32 +0,0 @@
|
|||
#ifndef EIGEN_REGRESSION_MODULE_H
|
||||
#define EIGEN_REGRESSION_MODULE_H
|
||||
|
||||
#ifndef EIGEN2_SUPPORT
|
||||
#error LeastSquares is only available in Eigen2 support mode (define EIGEN2_SUPPORT)
|
||||
#endif
|
||||
|
||||
// exclude from normal eigen3-only documentation
|
||||
#ifdef EIGEN2_SUPPORT
|
||||
|
||||
#include "Core"
|
||||
|
||||
#include "src/Core/util/DisableStupidWarnings.h"
|
||||
|
||||
#include "Eigenvalues"
|
||||
#include "Geometry"
|
||||
|
||||
/** \defgroup LeastSquares_Module LeastSquares module
|
||||
* This module provides linear regression and related features.
|
||||
*
|
||||
* \code
|
||||
* #include <Eigen/LeastSquares>
|
||||
* \endcode
|
||||
*/
|
||||
|
||||
#include "src/Eigen2Support/LeastSquares.h"
|
||||
|
||||
#include "src/Core/util/ReenableStupidWarnings.h"
|
||||
|
||||
#endif // EIGEN2_SUPPORT
|
||||
|
||||
#endif // EIGEN_REGRESSION_MODULE_H
|
||||
|
|
@ -1,3 +1,10 @@
|
|||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_METISSUPPORT_MODULE_H
|
||||
#define EIGEN_METISSUPPORT_MODULE_H
|
||||
|
||||
|
|
|
|||
|
|
@ -1,3 +1,10 @@
|
|||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_ORDERINGMETHODS_MODULE_H
|
||||
#define EIGEN_ORDERINGMETHODS_MODULE_H
|
||||
|
||||
|
|
|
|||
|
|
@ -1,3 +1,10 @@
|
|||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_PASTIXSUPPORT_MODULE_H
|
||||
#define EIGEN_PASTIXSUPPORT_MODULE_H
|
||||
|
||||
|
|
@ -5,7 +12,6 @@
|
|||
|
||||
#include "src/Core/util/DisableStupidWarnings.h"
|
||||
|
||||
#include <complex.h>
|
||||
extern "C" {
|
||||
#include <pastix_nompi.h>
|
||||
#include <pastix.h>
|
||||
|
|
@ -35,12 +41,8 @@ extern "C" {
|
|||
*
|
||||
*/
|
||||
|
||||
#include "src/misc/Solve.h"
|
||||
#include "src/misc/SparseSolve.h"
|
||||
|
||||
#include "src/PaStiXSupport/PaStiXSupport.h"
|
||||
|
||||
|
||||
#include "src/Core/util/ReenableStupidWarnings.h"
|
||||
|
||||
#endif // EIGEN_PASTIXSUPPORT_MODULE_H
|
||||
|
|
|
|||
|
|
@ -1,3 +1,10 @@
|
|||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_PARDISOSUPPORT_MODULE_H
|
||||
#define EIGEN_PARDISOSUPPORT_MODULE_H
|
||||
|
||||
|
|
@ -7,8 +14,6 @@
|
|||
|
||||
#include <mkl_pardiso.h>
|
||||
|
||||
#include <unsupported/Eigen/SparseExtra>
|
||||
|
||||
/** \ingroup Support_modules
|
||||
* \defgroup PardisoSupport_Module PardisoSupport module
|
||||
*
|
||||
|
|
|
|||
|
|
@ -1,3 +1,10 @@
|
|||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_QR_MODULE_H
|
||||
#define EIGEN_QR_MODULE_H
|
||||
|
||||
|
|
@ -15,31 +22,30 @@
|
|||
*
|
||||
* This module provides various QR decompositions
|
||||
* This module also provides some MatrixBase methods, including:
|
||||
* - MatrixBase::qr(),
|
||||
* - MatrixBase::householderQr()
|
||||
* - MatrixBase::colPivHouseholderQr()
|
||||
* - MatrixBase::fullPivHouseholderQr()
|
||||
*
|
||||
* \code
|
||||
* #include <Eigen/QR>
|
||||
* \endcode
|
||||
*/
|
||||
|
||||
#include "src/misc/Solve.h"
|
||||
#include "src/QR/HouseholderQR.h"
|
||||
#include "src/QR/FullPivHouseholderQR.h"
|
||||
#include "src/QR/ColPivHouseholderQR.h"
|
||||
#include "src/QR/CompleteOrthogonalDecomposition.h"
|
||||
#ifdef EIGEN_USE_LAPACKE
|
||||
#include "src/QR/HouseholderQR_MKL.h"
|
||||
#include "src/QR/ColPivHouseholderQR_MKL.h"
|
||||
#ifdef EIGEN_USE_MKL
|
||||
#include "mkl_lapacke.h"
|
||||
#else
|
||||
#include "src/misc/lapacke.h"
|
||||
#endif
|
||||
|
||||
#ifdef EIGEN2_SUPPORT
|
||||
#include "src/Eigen2Support/QR.h"
|
||||
#include "src/QR/HouseholderQR_LAPACKE.h"
|
||||
#include "src/QR/ColPivHouseholderQR_LAPACKE.h"
|
||||
#endif
|
||||
|
||||
#include "src/Core/util/ReenableStupidWarnings.h"
|
||||
|
||||
#ifdef EIGEN2_SUPPORT
|
||||
#include "Eigenvalues"
|
||||
#endif
|
||||
|
||||
#endif // EIGEN_QR_MODULE_H
|
||||
/* vim: set filetype=cpp et sw=2 ts=2 ai: */
|
||||
|
|
|
|||
|
|
@ -1,3 +1,9 @@
|
|||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_QTMALLOC_MODULE_H
|
||||
#define EIGEN_QTMALLOC_MODULE_H
|
||||
|
|
@ -8,7 +14,7 @@
|
|||
|
||||
#include "src/Core/util/DisableStupidWarnings.h"
|
||||
|
||||
void *qMalloc(size_t size)
|
||||
void *qMalloc(std::size_t size)
|
||||
{
|
||||
return Eigen::internal::aligned_malloc(size);
|
||||
}
|
||||
|
|
@ -18,10 +24,10 @@ void qFree(void *ptr)
|
|||
Eigen::internal::aligned_free(ptr);
|
||||
}
|
||||
|
||||
void *qRealloc(void *ptr, size_t size)
|
||||
void *qRealloc(void *ptr, std::size_t size)
|
||||
{
|
||||
void* newPtr = Eigen::internal::aligned_malloc(size);
|
||||
memcpy(newPtr, ptr, size);
|
||||
std::memcpy(newPtr, ptr, size);
|
||||
Eigen::internal::aligned_free(ptr);
|
||||
return newPtr;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,3 +1,10 @@
|
|||
// This file is part of Eigen, a lightweight C++ template library
|
||||
// for linear algebra.
|
||||
//
|
||||
// This Source Code Form is subject to the terms of the Mozilla
|
||||
// Public License v. 2.0. If a copy of the MPL was not distributed
|
||||
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
|
||||
#ifndef EIGEN_SPQRSUPPORT_MODULE_H
|
||||
#define EIGEN_SPQRSUPPORT_MODULE_H
|
||||
|
||||
|
|
@ -21,8 +28,6 @@
|
|||
*
|
||||
*/
|
||||
|
||||
#include "src/misc/Solve.h"
|
||||
#include "src/misc/SparseSolve.h"
|
||||
#include "src/CholmodSupport/CholmodSupport.h"
|
||||
#include "src/SPQRSupport/SuiteSparseQRSupport.h"
|
||||
|
||||
|
|
|
|||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue