Merge
						commit
						28f543a4ca
					
				| 
						 | 
				
			
			@ -121,15 +121,19 @@ if(MSVC)
 | 
			
		|||
	# 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)
 | 
			
		||||
		list(APPEND GTSAM_COMPILE_DEFINITIONS BOOST_ALL_NO_LIB BOOST_ALL_DYN_LINK)
 | 
			
		||||
		list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_ALL_NO_LIB BOOST_ALL_DYN_LINK)
 | 
			
		||||
	endif()
 | 
			
		||||
	# Virtual memory range for PCH exceeded on VS2015
 | 
			
		||||
	if(MSVC_VERSION LESS 1910) # older than VS2017
 | 
			
		||||
	  list(APPEND GTSAM_COMPILE_OPTIONS -Zm295)
 | 
			
		||||
	  list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE -Zm295)
 | 
			
		||||
	endif()
 | 
			
		||||
endif()
 | 
			
		||||
 | 
			
		||||
find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex)
 | 
			
		||||
# Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such.
 | 
			
		||||
set(BOOST_FIND_MINIMUM_VERSION 1.43)
 | 
			
		||||
set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex)
 | 
			
		||||
 | 
			
		||||
find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS})
 | 
			
		||||
 | 
			
		||||
# Required components
 | 
			
		||||
if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR
 | 
			
		||||
| 
						 | 
				
			
			@ -149,7 +153,7 @@ set(GTSAM_BOOST_LIBRARIES
 | 
			
		|||
)
 | 
			
		||||
if (GTSAM_DISABLE_NEW_TIMERS)
 | 
			
		||||
    message("WARNING:  GTSAM timing instrumentation manually disabled")
 | 
			
		||||
    list(APPEND GTSAM_COMPILE_DEFINITIONS DGTSAM_DISABLE_NEW_TIMERS)
 | 
			
		||||
    list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS)
 | 
			
		||||
else()
 | 
			
		||||
    if(Boost_TIMER_LIBRARY)
 | 
			
		||||
      list(APPEND GTSAM_BOOST_LIBRARIES Boost::timer Boost::chrono)
 | 
			
		||||
| 
						 | 
				
			
			@ -162,7 +166,7 @@ endif()
 | 
			
		|||
 | 
			
		||||
if(NOT (${Boost_VERSION} LESS 105600))
 | 
			
		||||
	message("Ignoring Boost restriction on optional lvalue assignment from rvalues")
 | 
			
		||||
	list(APPEND GTSAM_COMPILE_DEFINITIONS BOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES BOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES)
 | 
			
		||||
	list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES BOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES)
 | 
			
		||||
endif()
 | 
			
		||||
 | 
			
		||||
###############################################################################
 | 
			
		||||
| 
						 | 
				
			
			@ -212,7 +216,6 @@ find_package(MKL)
 | 
			
		|||
if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL)
 | 
			
		||||
    set(GTSAM_USE_EIGEN_MKL 1) # This will go into config.h
 | 
			
		||||
    set(EIGEN_USE_MKL_ALL 1) # This will go into config.h - it makes Eigen use MKL
 | 
			
		||||
    include_directories(${MKL_INCLUDE_DIR})
 | 
			
		||||
    list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES})
 | 
			
		||||
 | 
			
		||||
    # --no-as-needed is required with gcc according to the MKL link advisor
 | 
			
		||||
| 
						 | 
				
			
			@ -247,10 +250,9 @@ option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', us
 | 
			
		|||
# Switch for using system Eigen or GTSAM-bundled Eigen
 | 
			
		||||
if(GTSAM_USE_SYSTEM_EIGEN)
 | 
			
		||||
	find_package(Eigen3 REQUIRED)
 | 
			
		||||
	include_directories(AFTER "${EIGEN3_INCLUDE_DIR}")
 | 
			
		||||
 | 
			
		||||
	# Use generic Eigen include paths e.g. <Eigen/Core>
 | 
			
		||||
    set(GTSAM_EIGEN_INCLUDE_PREFIX "${EIGEN3_INCLUDE_DIR}")
 | 
			
		||||
	set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "${EIGEN3_INCLUDE_DIR}")
 | 
			
		||||
 | 
			
		||||
	# check if MKL is also enabled - can have one or the other, but not both!
 | 
			
		||||
	# Note: Eigen >= v3.2.5 includes our patches
 | 
			
		||||
| 
						 | 
				
			
			@ -264,28 +266,29 @@ if(GTSAM_USE_SYSTEM_EIGEN)
 | 
			
		|||
		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()
 | 
			
		||||
 | 
			
		||||
	# The actual include directory (for BUILD cmake target interface):
 | 
			
		||||
	set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${EIGEN3_INCLUDE_DIR}")
 | 
			
		||||
else()
 | 
			
		||||
	# Use bundled Eigen include path.
 | 
			
		||||
	# Clear any variables set by FindEigen3
 | 
			
		||||
	if(EIGEN3_INCLUDE_DIR)
 | 
			
		||||
		set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE)
 | 
			
		||||
	endif()
 | 
			
		||||
	# Add the bundled version of eigen to the include path so that it can still be included
 | 
			
		||||
	# with  #include <Eigen/Core>
 | 
			
		||||
	include_directories(BEFORE "gtsam/3rdparty/Eigen/")
 | 
			
		||||
 | 
			
		||||
	# set full path to be used by external projects
 | 
			
		||||
	# this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in
 | 
			
		||||
	set(GTSAM_EIGEN_INCLUDE_PREFIX "${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/Eigen/")
 | 
			
		||||
	set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "include/gtsam/3rdparty/Eigen/")
 | 
			
		||||
 | 
			
		||||
	# The actual include directory (for BUILD cmake target interface):
 | 
			
		||||
	set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${CMAKE_SOURCE_DIR}/gtsam/3rdparty/Eigen/")
 | 
			
		||||
endif()
 | 
			
		||||
 | 
			
		||||
if (MSVC)
 | 
			
		||||
	if (BUILD_SHARED_LIBS)
 | 
			
		||||
		# mute eigen static assert to avoid errors in shared lib
 | 
			
		||||
		list(APPEND GTSAM_COMPILE_DEFINITIONS DEIGEN_NO_STATIC_ASSERT)
 | 
			
		||||
		list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC DEIGEN_NO_STATIC_ASSERT)
 | 
			
		||||
	endif()
 | 
			
		||||
	list(APPEND GTSAM_COMPILE_OPTIONS "/wd4244") # Disable loss of precision which is thrown all over our Eigen
 | 
			
		||||
	list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE "/wd4244") # Disable loss of precision which is thrown all over our Eigen
 | 
			
		||||
endif()
 | 
			
		||||
 | 
			
		||||
###############################################################################
 | 
			
		||||
| 
						 | 
				
			
			@ -326,52 +329,29 @@ elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "tcmalloc")
 | 
			
		|||
	list(APPEND GTSAM_ADDITIONAL_LIBRARIES "tcmalloc")
 | 
			
		||||
endif()
 | 
			
		||||
 | 
			
		||||
# Include boost - use 'BEFORE' so that a specific boost specified to CMake
 | 
			
		||||
# takes precedence over a system-installed one.
 | 
			
		||||
include_directories(BEFORE SYSTEM ${Boost_INCLUDE_DIR})
 | 
			
		||||
 | 
			
		||||
if(GTSAM_SUPPORT_NESTED_DISSECTION)
 | 
			
		||||
  set(METIS_INCLUDE_DIRECTORIES
 | 
			
		||||
    gtsam/3rdparty/metis/include
 | 
			
		||||
    gtsam/3rdparty/metis/libmetis
 | 
			
		||||
    gtsam/3rdparty/metis/GKlib)
 | 
			
		||||
else()
 | 
			
		||||
  set(METIS_INCLUDE_DIRECTORIES)
 | 
			
		||||
endif()
 | 
			
		||||
 | 
			
		||||
# Add includes for source directories 'BEFORE' boost and any system include
 | 
			
		||||
# paths so that the compiler uses GTSAM headers in our source directory instead
 | 
			
		||||
# of any previously installed GTSAM headers.
 | 
			
		||||
include_directories(BEFORE
 | 
			
		||||
  gtsam/3rdparty/SuiteSparse_config
 | 
			
		||||
  gtsam/3rdparty/CCOLAMD/Include
 | 
			
		||||
  ${METIS_INCLUDE_DIRECTORIES}
 | 
			
		||||
  ${PROJECT_SOURCE_DIR}
 | 
			
		||||
  ${PROJECT_BINARY_DIR} # So we can include generated config header files
 | 
			
		||||
  CppUnitLite)
 | 
			
		||||
 | 
			
		||||
if(MSVC)
 | 
			
		||||
	list(APPEND GTSAM_COMPILE_DEFINITIONS _CRT_SECURE_NO_WARNINGS _SCL_SECURE_NO_WARNINGS)
 | 
			
		||||
	list(APPEND GTSAM_COMPILE_OPTIONS /wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings
 | 
			
		||||
	list(APPEND GTSAM_COMPILE_OPTIONS /bigobj) # Allow large object files for template-based code
 | 
			
		||||
	list(APPEND GTSAM_COMPILE_DEFINITIONS_PRIVATE _CRT_SECURE_NO_WARNINGS _SCL_SECURE_NO_WARNINGS)
 | 
			
		||||
	list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE /wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings
 | 
			
		||||
	list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE /bigobj) # Allow large object files for template-based code
 | 
			
		||||
endif()
 | 
			
		||||
 | 
			
		||||
# GCC 4.8+ complains about local typedefs which we use for shared_ptr etc.
 | 
			
		||||
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
 | 
			
		||||
  if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.8)
 | 
			
		||||
    list(APPEND GTSAM_COMPILE_OPTIONS -Wno-unused-local-typedefs)
 | 
			
		||||
    list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs)
 | 
			
		||||
  endif()
 | 
			
		||||
endif()
 | 
			
		||||
 | 
			
		||||
# As of XCode 7, clang also complains about this
 | 
			
		||||
if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
 | 
			
		||||
  if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 7.0)
 | 
			
		||||
    list(APPEND GTSAM_COMPILE_OPTIONS -Wno-unused-local-typedefs)
 | 
			
		||||
    list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs)
 | 
			
		||||
  endif()
 | 
			
		||||
endif()
 | 
			
		||||
 | 
			
		||||
if(GTSAM_ENABLE_CONSISTENCY_CHECKS)
 | 
			
		||||
  list(APPEND GTSAM_COMPILE_DEFINITIONS GTSAM_EXTRA_CONSISTENCY_CHECKS)
 | 
			
		||||
  # This should be made PUBLIC if GTSAM_EXTRA_CONSISTENCY_CHECKS is someday used in a public .h
 | 
			
		||||
  list(APPEND GTSAM_COMPILE_DEFINITIONS_PRIVATE GTSAM_EXTRA_CONSISTENCY_CHECKS)
 | 
			
		||||
endif()
 | 
			
		||||
 | 
			
		||||
###############################################################################
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -6,6 +6,7 @@ file(GLOB cppunitlite_src "*.cpp")
 | 
			
		|||
add_library(CppUnitLite STATIC ${cppunitlite_src} ${cppunitlite_headers})
 | 
			
		||||
list(APPEND GTSAM_EXPORTED_TARGETS CppUnitLite)
 | 
			
		||||
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
 | 
			
		||||
target_link_libraries(CppUnitLite PUBLIC Boost::boost) # boost/lexical_cast.h
 | 
			
		||||
 | 
			
		||||
gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC project structure
 | 
			
		||||
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
							
								
								
									
										6
									
								
								INSTALL
								
								
								
								
							
							
						
						
									
										6
									
								
								INSTALL
								
								
								
								
							| 
						 | 
				
			
			@ -14,7 +14,7 @@ Important Installation Notes
 | 
			
		|||
1)
 | 
			
		||||
GTSAM requires the following libraries to be installed on your system:
 | 
			
		||||
 - BOOST version 1.43 or greater (install through Linux repositories or MacPorts)
 | 
			
		||||
 - Cmake version 2.6 or higher
 | 
			
		||||
 - Cmake version 3.0 or higher
 | 
			
		||||
 - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher
 | 
			
		||||
 | 
			
		||||
Optional dependent libraries:
 | 
			
		||||
| 
						 | 
				
			
			@ -33,9 +33,9 @@ Tested compilers:
 | 
			
		|||
 | 
			
		||||
Tested systems:
 | 
			
		||||
 | 
			
		||||
- Ubuntu 11.04 - 13.10
 | 
			
		||||
- Ubuntu 11.04 - 18.04
 | 
			
		||||
- MacOS 10.6 - 10.9
 | 
			
		||||
- Windows 7, 8, 8.1
 | 
			
		||||
- Windows 7, 8, 8.1, 10
 | 
			
		||||
 | 
			
		||||
Known issues:
 | 
			
		||||
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -30,7 +30,7 @@ $ make install
 | 
			
		|||
Prerequisites:
 | 
			
		||||
 | 
			
		||||
- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
 | 
			
		||||
- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`)
 | 
			
		||||
- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 3.0 (Ubuntu: `sudo apt-get install cmake`)
 | 
			
		||||
- A modern compiler, i.e., at least gcc 4.7.3 on Linux.
 | 
			
		||||
 | 
			
		||||
Optional prerequisites - used automatically if findable by CMake:
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -12,6 +12,10 @@ else()
 | 
			
		|||
  set(@PACKAGE_NAME@_INCLUDE_DIR "${OUR_CMAKE_DIR}/@CONF_REL_INCLUDE_DIR@" CACHE PATH "@PACKAGE_NAME@ include directory")
 | 
			
		||||
endif()
 | 
			
		||||
 | 
			
		||||
# Find dependencies, required by cmake exported targets:
 | 
			
		||||
include(CMakeFindDependencyMacro)
 | 
			
		||||
find_dependency(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)
 | 
			
		||||
 | 
			
		||||
# Load exports
 | 
			
		||||
include(${OUR_CMAKE_DIR}/@PACKAGE_NAME@-exports.cmake)
 | 
			
		||||
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -22,7 +22,10 @@ include_directories(BEFORE "${PROJECT_SOURCE_DIR}")
 | 
			
		|||
###################################################################################
 | 
			
		||||
# Find GTSAM components
 | 
			
		||||
find_package(GTSAM REQUIRED) # Uses installed package
 | 
			
		||||
include_directories(${GTSAM_INCLUDE_DIR})
 | 
			
		||||
# Note: Since Jan-2019, GTSAMConfig.cmake defines exported CMake targets
 | 
			
		||||
# that automatically do include the include_directories() without the need
 | 
			
		||||
# to call include_directories(), just target_link_libraries(NAME gtsam)
 | 
			
		||||
#include_directories(${GTSAM_INCLUDE_DIR})
 | 
			
		||||
 | 
			
		||||
###################################################################################
 | 
			
		||||
# Build static library from common sources
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -1,112 +0,0 @@
 | 
			
		|||
"""
 | 
			
		||||
This file is not a real python unittest. It contains small experiments
 | 
			
		||||
to test the wrapper with gtsam_test, a short version of gtsam.h.
 | 
			
		||||
Its name convention is different from other tests so it won't be discovered.
 | 
			
		||||
"""
 | 
			
		||||
import gtsam
 | 
			
		||||
import numpy as np
 | 
			
		||||
 | 
			
		||||
r = gtsam.Rot3()
 | 
			
		||||
print(r)
 | 
			
		||||
print(r.pitch())
 | 
			
		||||
r2 = gtsam.Rot3()
 | 
			
		||||
r3 = r.compose(r2)
 | 
			
		||||
print("r3 pitch:", r3.pitch())
 | 
			
		||||
 | 
			
		||||
v = np.array([1, 1, 1]) 
 | 
			
		||||
print("v = ", v)
 | 
			
		||||
r4 = r3.retract(v)
 | 
			
		||||
print("r4 pitch:", r4.pitch())
 | 
			
		||||
r4.print_(b'r4: ')
 | 
			
		||||
r3.print_(b"r3: ")
 | 
			
		||||
 | 
			
		||||
v = r3.localCoordinates(r4)
 | 
			
		||||
print("localCoordinates:", v)
 | 
			
		||||
 | 
			
		||||
Rmat = np.array([
 | 
			
		||||
    [0.990074, -0.0942928, 0.104218], 
 | 
			
		||||
    [0.104218,  0.990074, -0.0942928], 
 | 
			
		||||
    [-0.0942928, 0.104218, 0.990074]
 | 
			
		||||
    ])
 | 
			
		||||
r5 = gtsam.Rot3(Rmat)
 | 
			
		||||
r5.print_(b"r5: ")
 | 
			
		||||
 | 
			
		||||
l = gtsam.Rot3.Logmap(r5)
 | 
			
		||||
print("l = ", l)
 | 
			
		||||
 | 
			
		||||
 | 
			
		||||
noise = gtsam.noiseModel_Gaussian.Covariance(Rmat)
 | 
			
		||||
noise.print_(b"noise:")
 | 
			
		||||
 | 
			
		||||
D = np.array([1.,2.,3.])
 | 
			
		||||
diag = gtsam.noiseModel_Diagonal.Variances(D)
 | 
			
		||||
print("diag:", diag)
 | 
			
		||||
diag.print_(b"diag:")
 | 
			
		||||
print("diag R:", diag.R())
 | 
			
		||||
 | 
			
		||||
p = gtsam.Point3()
 | 
			
		||||
p.print_("p:")
 | 
			
		||||
factor = gtsam.BetweenFactorPoint3(1,2,p, noise)
 | 
			
		||||
factor.print_(b"factor:")
 | 
			
		||||
 | 
			
		||||
vv = gtsam.VectorValues()
 | 
			
		||||
vv.print_(b"vv:")
 | 
			
		||||
vv.insert(1, np.array([1.,2.,3.]))
 | 
			
		||||
vv.insert(2, np.array([3.,4.]))
 | 
			
		||||
vv.insert(3, np.array([5.,6.,7.,8.]))
 | 
			
		||||
vv.print_(b"vv:")
 | 
			
		||||
 | 
			
		||||
vv2 = gtsam.VectorValues(vv)
 | 
			
		||||
vv2.insert(4, np.array([4.,2.,1]))
 | 
			
		||||
vv2.print_(b"vv2:")
 | 
			
		||||
vv.print_(b"vv:")
 | 
			
		||||
 | 
			
		||||
vv.insert(4, np.array([1.,2.,4.]))
 | 
			
		||||
vv.print_(b"vv:")
 | 
			
		||||
vv3 = vv.add(vv2)
 | 
			
		||||
 | 
			
		||||
vv3.print_(b"vv3:")
 | 
			
		||||
 | 
			
		||||
values = gtsam.Values()
 | 
			
		||||
values.insert(1, gtsam.Point3())
 | 
			
		||||
values.insert(2, gtsam.Rot3())
 | 
			
		||||
values.print_(b"values:")
 | 
			
		||||
 | 
			
		||||
factor = gtsam.PriorFactorVector(1, np.array([1.,2.,3.]), diag)
 | 
			
		||||
print "Prior factor vector: ", factor
 | 
			
		||||
 | 
			
		||||
 | 
			
		||||
 | 
			
		||||
keys = gtsam.KeyVector()
 | 
			
		||||
 | 
			
		||||
keys.push_back(1)
 | 
			
		||||
keys.push_back(2)
 | 
			
		||||
print 'size: ', keys.size()
 | 
			
		||||
print keys.at(0)
 | 
			
		||||
print keys.at(1)
 | 
			
		||||
 | 
			
		||||
noise = gtsam.noiseModel_Isotropic.Precision(2, 3.0)
 | 
			
		||||
noise.print_('noise:')
 | 
			
		||||
print 'noise print:', noise
 | 
			
		||||
f = gtsam.JacobianFactor(7, np.ones([2,2]), model=noise,  b=np.ones(2))
 | 
			
		||||
print 'JacobianFactor(7):\n', f
 | 
			
		||||
print "A = ", f.getA()
 | 
			
		||||
print "b = ", f.getb()
 | 
			
		||||
 | 
			
		||||
f = gtsam.JacobianFactor(np.ones(2))
 | 
			
		||||
f.print_('jacoboian b_in:')
 | 
			
		||||
 | 
			
		||||
 | 
			
		||||
print "JacobianFactor initalized with b_in:", f
 | 
			
		||||
 | 
			
		||||
diag = gtsam.noiseModel_Diagonal.Sigmas(np.array([1.,2.,3.]))
 | 
			
		||||
fv = gtsam.PriorFactorVector(1, np.array([4.,5.,6.]), diag)
 | 
			
		||||
print "priorfactorvector: ", fv
 | 
			
		||||
 | 
			
		||||
print "base noise: ", fv.get_noiseModel()
 | 
			
		||||
print "casted to gaussian2: ", gtsam.dynamic_cast_noiseModel_Diagonal_noiseModel_Base(fv.get_noiseModel())
 | 
			
		||||
 | 
			
		||||
X = gtsam.symbol(65, 19)
 | 
			
		||||
print X 
 | 
			
		||||
print gtsam.symbolChr(X)
 | 
			
		||||
print gtsam.symbolIndex(X)
 | 
			
		||||
| 
						 | 
				
			
			@ -1,772 +0,0 @@
 | 
			
		|||
namespace gtsam {
 | 
			
		||||
 | 
			
		||||
#include <gtsam/inference/Key.h>
 | 
			
		||||
typedef size_t Key;
 | 
			
		||||
 | 
			
		||||
#include <gtsam/base/FastVector.h>
 | 
			
		||||
template<T> class FastVector {
 | 
			
		||||
  FastVector();
 | 
			
		||||
  FastVector(const This& f);
 | 
			
		||||
  void push_back(const T& e);
 | 
			
		||||
  //T& operator[](int);
 | 
			
		||||
  T at(int i);
 | 
			
		||||
  size_t size() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
typedef gtsam::FastVector<gtsam::Key> KeyVector;
 | 
			
		||||
 | 
			
		||||
//*************************************************************************
 | 
			
		||||
// geometry
 | 
			
		||||
//*************************************************************************
 | 
			
		||||
 | 
			
		||||
#include <gtsam/geometry/Point2.h>
 | 
			
		||||
class Point2 {
 | 
			
		||||
  // Standard Constructors
 | 
			
		||||
  Point2();
 | 
			
		||||
  Point2(double x, double y);
 | 
			
		||||
  Point2(Vector v);
 | 
			
		||||
  //Point2(const gtsam::Point2& l);
 | 
			
		||||
 | 
			
		||||
  // Testable
 | 
			
		||||
  void print(string s) const;
 | 
			
		||||
  bool equals(const gtsam::Point2& pose, double tol) const;
 | 
			
		||||
 | 
			
		||||
  // Group
 | 
			
		||||
  static gtsam::Point2 identity();
 | 
			
		||||
 | 
			
		||||
  // Standard Interface
 | 
			
		||||
  double x() const;
 | 
			
		||||
  double y() const;
 | 
			
		||||
  Vector vector() const;
 | 
			
		||||
  double distance(const gtsam::Point2& p2) const;
 | 
			
		||||
  double norm() const;
 | 
			
		||||
 | 
			
		||||
  // enabling serialization functionality
 | 
			
		||||
  void serialize() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
#include <gtsam/geometry/Point3.h>
 | 
			
		||||
class Point3 {
 | 
			
		||||
  // Standard Constructors
 | 
			
		||||
  Point3();
 | 
			
		||||
  Point3(double x, double y, double z);
 | 
			
		||||
  Point3(Vector v);
 | 
			
		||||
  //Point3(const gtsam::Point3& l);
 | 
			
		||||
 | 
			
		||||
  // Testable
 | 
			
		||||
  void print(string s) const;
 | 
			
		||||
  bool equals(const gtsam::Point3& p, double tol) const;
 | 
			
		||||
 | 
			
		||||
  // Group
 | 
			
		||||
  static gtsam::Point3 identity();
 | 
			
		||||
 | 
			
		||||
  // Standard Interface
 | 
			
		||||
  Vector vector() const;
 | 
			
		||||
  double x() const;
 | 
			
		||||
  double y() const;
 | 
			
		||||
  double z() const;
 | 
			
		||||
 | 
			
		||||
  // enabling serialization functionality
 | 
			
		||||
  void serialize() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
#include <gtsam/geometry/Rot2.h>
 | 
			
		||||
class Rot2 {
 | 
			
		||||
  // Standard Constructors and Named Constructors
 | 
			
		||||
  Rot2();
 | 
			
		||||
  Rot2(double theta);
 | 
			
		||||
  //Rot2(const gtsam::Rot2& l);
 | 
			
		||||
 | 
			
		||||
  static gtsam::Rot2 fromAngle(double theta);
 | 
			
		||||
  static gtsam::Rot2 fromDegrees(double theta);
 | 
			
		||||
  static gtsam::Rot2 fromCosSin(double c, double s);
 | 
			
		||||
 | 
			
		||||
  // Testable
 | 
			
		||||
  void print(string s) const;
 | 
			
		||||
  bool equals(const gtsam::Rot2& rot, double tol) const;
 | 
			
		||||
 | 
			
		||||
  // Group
 | 
			
		||||
  static gtsam::Rot2 identity();
 | 
			
		||||
  gtsam::Rot2 inverse();
 | 
			
		||||
  gtsam::Rot2 compose(const gtsam::Rot2& p2) const;
 | 
			
		||||
  gtsam::Rot2 between(const gtsam::Rot2& p2) const;
 | 
			
		||||
 | 
			
		||||
  // Manifold
 | 
			
		||||
  gtsam::Rot2 retract(Vector v) const;
 | 
			
		||||
  Vector localCoordinates(const gtsam::Rot2& p) const;
 | 
			
		||||
 | 
			
		||||
  // Lie Group
 | 
			
		||||
  static gtsam::Rot2 Expmap(Vector v);
 | 
			
		||||
  static Vector Logmap(const gtsam::Rot2& p);
 | 
			
		||||
 | 
			
		||||
  // Group Action on Point2
 | 
			
		||||
  gtsam::Point2 rotate(const gtsam::Point2& point) const;
 | 
			
		||||
  gtsam::Point2 unrotate(const gtsam::Point2& point) const;
 | 
			
		||||
 | 
			
		||||
  // Standard Interface
 | 
			
		||||
  static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative
 | 
			
		||||
  static gtsam::Rot2 atan2(double y, double x);
 | 
			
		||||
  double theta() const;
 | 
			
		||||
  double degrees() const;
 | 
			
		||||
  double c() const;
 | 
			
		||||
  double s() const;
 | 
			
		||||
  Matrix matrix() const;
 | 
			
		||||
 | 
			
		||||
  // enabling serialization functionality
 | 
			
		||||
  void serialize() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
#include <gtsam/geometry/Rot3.h>
 | 
			
		||||
class Rot3 {
 | 
			
		||||
  // Standard Constructors and Named Constructors
 | 
			
		||||
  Rot3();
 | 
			
		||||
  Rot3(Matrix R);
 | 
			
		||||
  //Rot3(const gtsam::Rot3& l);
 | 
			
		||||
 | 
			
		||||
  static gtsam::Rot3 Rx(double t);
 | 
			
		||||
  static gtsam::Rot3 Ry(double t);
 | 
			
		||||
  static gtsam::Rot3 Rz(double t);
 | 
			
		||||
  static gtsam::Rot3 RzRyRx(double x, double y, double z);
 | 
			
		||||
  static gtsam::Rot3 RzRyRx(Vector xyz);
 | 
			
		||||
  static gtsam::Rot3 Yaw(double t); // positive yaw is to right (as in aircraft heading)
 | 
			
		||||
  static gtsam::Rot3 Pitch(double t); // positive pitch is up (increasing aircraft altitude)
 | 
			
		||||
  static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft)
 | 
			
		||||
  static gtsam::Rot3 Ypr(double y, double p, double r);
 | 
			
		||||
  static gtsam::Rot3 Quaternion(double w, double x, double y, double z);
 | 
			
		||||
  static gtsam::Rot3 Rodrigues(Vector v);
 | 
			
		||||
 | 
			
		||||
  // Testable
 | 
			
		||||
  void print(string s) const;
 | 
			
		||||
  bool equals(const gtsam::Rot3& rot, double tol) const;
 | 
			
		||||
 | 
			
		||||
  // Group
 | 
			
		||||
  static gtsam::Rot3 identity();
 | 
			
		||||
    gtsam::Rot3 inverse() const;
 | 
			
		||||
  gtsam::Rot3 compose(const gtsam::Rot3& p2) const;
 | 
			
		||||
  gtsam::Rot3 between(const gtsam::Rot3& p2) const;
 | 
			
		||||
 | 
			
		||||
  // Manifold
 | 
			
		||||
  //gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options
 | 
			
		||||
  gtsam::Rot3 retract(Vector v) const;
 | 
			
		||||
  Vector localCoordinates(const gtsam::Rot3& p) const;
 | 
			
		||||
 | 
			
		||||
  // Group Action on Point3
 | 
			
		||||
  gtsam::Point3 rotate(const gtsam::Point3& p) const;
 | 
			
		||||
  gtsam::Point3 unrotate(const gtsam::Point3& p) const;
 | 
			
		||||
 | 
			
		||||
  // Standard Interface
 | 
			
		||||
  static gtsam::Rot3 Expmap(Vector v);
 | 
			
		||||
  static Vector Logmap(const gtsam::Rot3& p);
 | 
			
		||||
  Matrix matrix() const;
 | 
			
		||||
  Matrix transpose() const;
 | 
			
		||||
  gtsam::Point3 column(size_t index) const;
 | 
			
		||||
  Vector xyz() const;
 | 
			
		||||
  Vector ypr() const;
 | 
			
		||||
  Vector rpy() const;
 | 
			
		||||
  double roll() const;
 | 
			
		||||
  double pitch() const;
 | 
			
		||||
  double yaw() const;
 | 
			
		||||
//  Vector toQuaternion() const;  // FIXME: Can't cast to Vector properly
 | 
			
		||||
  Vector quaternion() const;
 | 
			
		||||
 | 
			
		||||
  // enabling serialization functionality
 | 
			
		||||
  void serialize() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
#include <gtsam/geometry/Pose2.h>
 | 
			
		||||
class Pose2 {
 | 
			
		||||
  // Standard Constructor
 | 
			
		||||
  Pose2();
 | 
			
		||||
  //Pose2(const gtsam::Pose2& pose);
 | 
			
		||||
  Pose2(double x, double y, double theta);
 | 
			
		||||
  Pose2(double theta, const gtsam::Point2& t);
 | 
			
		||||
  Pose2(const gtsam::Rot2& r, const gtsam::Point2& t);
 | 
			
		||||
  Pose2(Vector v);
 | 
			
		||||
 | 
			
		||||
  // Testable
 | 
			
		||||
  void print(string s) const;
 | 
			
		||||
  bool equals(const gtsam::Pose2& pose, double tol) const;
 | 
			
		||||
 | 
			
		||||
  // Group
 | 
			
		||||
  static gtsam::Pose2 identity();
 | 
			
		||||
  gtsam::Pose2 inverse() const;
 | 
			
		||||
  gtsam::Pose2 compose(const gtsam::Pose2& p2) const;
 | 
			
		||||
  gtsam::Pose2 between(const gtsam::Pose2& p2) const;
 | 
			
		||||
 | 
			
		||||
  // Manifold
 | 
			
		||||
  gtsam::Pose2 retract(Vector v) const;
 | 
			
		||||
  Vector localCoordinates(const gtsam::Pose2& p) const;
 | 
			
		||||
 | 
			
		||||
  // Lie Group
 | 
			
		||||
  static gtsam::Pose2 Expmap(Vector v);
 | 
			
		||||
  static Vector Logmap(const gtsam::Pose2& p);
 | 
			
		||||
  Matrix AdjointMap() const;
 | 
			
		||||
  Vector Adjoint(const Vector& xi) const;
 | 
			
		||||
  static Matrix wedge(double vx, double vy, double w);
 | 
			
		||||
 | 
			
		||||
  // Group Actions on Point2
 | 
			
		||||
  gtsam::Point2 transform_from(const gtsam::Point2& p) const;
 | 
			
		||||
  gtsam::Point2 transform_to(const gtsam::Point2& p) const;
 | 
			
		||||
 | 
			
		||||
  // Standard Interface
 | 
			
		||||
  double x() const;
 | 
			
		||||
  double y() const;
 | 
			
		||||
  double theta() const;
 | 
			
		||||
  gtsam::Rot2 bearing(const gtsam::Point2& point) const;
 | 
			
		||||
  double range(const gtsam::Point2& point) const;
 | 
			
		||||
  gtsam::Point2 translation() const;
 | 
			
		||||
  gtsam::Rot2 rotation() const;
 | 
			
		||||
  Matrix matrix() const;
 | 
			
		||||
 | 
			
		||||
  // enabling serialization functionality
 | 
			
		||||
  void serialize() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
 | 
			
		||||
#include <gtsam/geometry/Pose3.h>
 | 
			
		||||
class Pose3 {
 | 
			
		||||
  // Standard Constructors
 | 
			
		||||
  Pose3();
 | 
			
		||||
  //Pose3(const gtsam::Pose3& pose);
 | 
			
		||||
  Pose3(const gtsam::Rot3& r, const gtsam::Point3& t);
 | 
			
		||||
  Pose3(const gtsam::Pose2& pose2); // FIXME: shadows Pose3(Pose3 pose)
 | 
			
		||||
  Pose3(Matrix t);
 | 
			
		||||
 | 
			
		||||
  // Testable
 | 
			
		||||
  void print(string s) const;
 | 
			
		||||
  bool equals(const gtsam::Pose3& pose, double tol) const;
 | 
			
		||||
 | 
			
		||||
  // Group
 | 
			
		||||
  static gtsam::Pose3 identity();
 | 
			
		||||
  gtsam::Pose3 inverse() const;
 | 
			
		||||
  gtsam::Pose3 compose(const gtsam::Pose3& p2) const;
 | 
			
		||||
  gtsam::Pose3 between(const gtsam::Pose3& p2) const;
 | 
			
		||||
 | 
			
		||||
  // Manifold
 | 
			
		||||
  gtsam::Pose3 retract(Vector v) const;
 | 
			
		||||
  Vector localCoordinates(const gtsam::Pose3& T2) const;
 | 
			
		||||
 | 
			
		||||
  // Lie Group
 | 
			
		||||
  static gtsam::Pose3 Expmap(Vector v);
 | 
			
		||||
  static Vector Logmap(const gtsam::Pose3& p);
 | 
			
		||||
  Matrix AdjointMap() const;
 | 
			
		||||
  Vector Adjoint(Vector xi) const;
 | 
			
		||||
  static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz);
 | 
			
		||||
 | 
			
		||||
  // Group Action on Point3
 | 
			
		||||
  gtsam::Point3 transform_from(const gtsam::Point3& p) const;
 | 
			
		||||
  gtsam::Point3 transform_to(const gtsam::Point3& p) const;
 | 
			
		||||
 | 
			
		||||
  // Standard Interface
 | 
			
		||||
  gtsam::Rot3 rotation() const;
 | 
			
		||||
  gtsam::Point3 translation() const;
 | 
			
		||||
  double x() const;
 | 
			
		||||
  double y() const;
 | 
			
		||||
  double z() const;
 | 
			
		||||
  Matrix matrix() const;
 | 
			
		||||
  gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to()
 | 
			
		||||
  double range(const gtsam::Point3& point);
 | 
			
		||||
  double range(const gtsam::Pose3& pose);
 | 
			
		||||
 | 
			
		||||
  // enabling serialization functionality
 | 
			
		||||
  void serialize() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
//*************************************************************************
 | 
			
		||||
// noise
 | 
			
		||||
//*************************************************************************
 | 
			
		||||
 | 
			
		||||
namespace noiseModel {
 | 
			
		||||
#include <gtsam/linear/NoiseModel.h>
 | 
			
		||||
virtual class Base {
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
virtual class Gaussian : gtsam::noiseModel::Base {
 | 
			
		||||
  static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R);
 | 
			
		||||
  static gtsam::noiseModel::Gaussian* Covariance(Matrix R);
 | 
			
		||||
  Matrix R() const;
 | 
			
		||||
  bool equals(gtsam::noiseModel::Base& expected, double tol);
 | 
			
		||||
  void print(string s) const;
 | 
			
		||||
 | 
			
		||||
  // enabling serialization functionality
 | 
			
		||||
  void serializable() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
virtual class Diagonal : gtsam::noiseModel::Gaussian {
 | 
			
		||||
  static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas);
 | 
			
		||||
  static gtsam::noiseModel::Diagonal* Variances(Vector variances);
 | 
			
		||||
  static gtsam::noiseModel::Diagonal* Precisions(Vector precisions);
 | 
			
		||||
  Matrix R() const;
 | 
			
		||||
  void print(string s) const;
 | 
			
		||||
 | 
			
		||||
  // enabling serialization functionality
 | 
			
		||||
  void serializable() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
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* All(size_t dim);
 | 
			
		||||
    static gtsam::noiseModel::Constrained* All(size_t dim, double mu);
 | 
			
		||||
 | 
			
		||||
    gtsam::noiseModel::Constrained* unit() const;
 | 
			
		||||
 | 
			
		||||
    // enabling serialization functionality
 | 
			
		||||
    void serializable() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
virtual class Isotropic : gtsam::noiseModel::Diagonal {
 | 
			
		||||
  static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma);
 | 
			
		||||
  static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace);
 | 
			
		||||
  static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision);
 | 
			
		||||
  void print(string s) const;
 | 
			
		||||
 | 
			
		||||
  // enabling serialization functionality
 | 
			
		||||
  void serializable() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
virtual class Unit : gtsam::noiseModel::Isotropic {
 | 
			
		||||
  static gtsam::noiseModel::Unit* Create(size_t dim);
 | 
			
		||||
  void print(string s) const;
 | 
			
		||||
 | 
			
		||||
  // enabling serialization functionality
 | 
			
		||||
  void serializable() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
namespace mEstimator {
 | 
			
		||||
virtual class Base {
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
virtual class Null: gtsam::noiseModel::mEstimator::Base {
 | 
			
		||||
  Null();
 | 
			
		||||
  //Null(const gtsam::noiseModel::mEstimator::Null& other);
 | 
			
		||||
  void print(string s) const;
 | 
			
		||||
  static gtsam::noiseModel::mEstimator::Null* Create();
 | 
			
		||||
 | 
			
		||||
  // enabling serialization functionality
 | 
			
		||||
  void serializable() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
virtual class Fair: gtsam::noiseModel::mEstimator::Base {
 | 
			
		||||
  Fair(double c);
 | 
			
		||||
  //Fair(const gtsam::noiseModel::mEstimator::Fair& other);
 | 
			
		||||
  void print(string s) const;
 | 
			
		||||
  static gtsam::noiseModel::mEstimator::Fair* Create(double c);
 | 
			
		||||
 | 
			
		||||
  // enabling serialization functionality
 | 
			
		||||
  void serializable() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
virtual class Huber: gtsam::noiseModel::mEstimator::Base {
 | 
			
		||||
  Huber(double k);
 | 
			
		||||
  //Huber(const gtsam::noiseModel::mEstimator::Huber& other);
 | 
			
		||||
 | 
			
		||||
  void print(string s) const;
 | 
			
		||||
  static gtsam::noiseModel::mEstimator::Huber* Create(double k);
 | 
			
		||||
 | 
			
		||||
  // enabling serialization functionality
 | 
			
		||||
  void serializable() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
 | 
			
		||||
  Tukey(double k);
 | 
			
		||||
  //Tukey(const gtsam::noiseModel::mEstimator::Tukey& other);
 | 
			
		||||
 | 
			
		||||
  void print(string s) const;
 | 
			
		||||
  static gtsam::noiseModel::mEstimator::Tukey* Create(double k);
 | 
			
		||||
 | 
			
		||||
  // enabling serialization functionality
 | 
			
		||||
  void serializable() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
}///\namespace mEstimator
 | 
			
		||||
 | 
			
		||||
virtual class Robust : gtsam::noiseModel::Base {
 | 
			
		||||
  Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise);
 | 
			
		||||
  //Robust(const gtsam::noiseModel::Robust& other);
 | 
			
		||||
 | 
			
		||||
  static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise);
 | 
			
		||||
  void print(string s) const;
 | 
			
		||||
 | 
			
		||||
  // enabling serialization functionality
 | 
			
		||||
  void serializable() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
}///\namespace noiseModel
 | 
			
		||||
 | 
			
		||||
#include <gtsam/linear/Sampler.h>
 | 
			
		||||
class Sampler {
 | 
			
		||||
    //Constructors
 | 
			
		||||
  Sampler(gtsam::noiseModel::Diagonal* model, int seed);
 | 
			
		||||
  Sampler(Vector sigmas, int seed);
 | 
			
		||||
  Sampler(int seed);
 | 
			
		||||
  //Sampler(const gtsam::Sampler& other);
 | 
			
		||||
 | 
			
		||||
 | 
			
		||||
    //Standard Interface
 | 
			
		||||
  size_t dim() const;
 | 
			
		||||
  Vector sigmas() const;
 | 
			
		||||
  gtsam::noiseModel::Diagonal* model() const;
 | 
			
		||||
  Vector sample();
 | 
			
		||||
  Vector sampleNewModel(gtsam::noiseModel::Diagonal* model);
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
#include <gtsam/linear/VectorValues.h>
 | 
			
		||||
class VectorValues {
 | 
			
		||||
    //Constructors
 | 
			
		||||
  VectorValues();
 | 
			
		||||
  VectorValues(const gtsam::VectorValues& other);
 | 
			
		||||
 | 
			
		||||
  //Named Constructors
 | 
			
		||||
  static gtsam::VectorValues Zero(const gtsam::VectorValues& model);
 | 
			
		||||
 | 
			
		||||
  //Standard Interface
 | 
			
		||||
  size_t size() const;
 | 
			
		||||
  size_t dim(size_t j) const;
 | 
			
		||||
  bool exists(size_t j) const;
 | 
			
		||||
  void print(string s) const;
 | 
			
		||||
  bool equals(const gtsam::VectorValues& expected, double tol) const;
 | 
			
		||||
  void insert(size_t j, Vector value);
 | 
			
		||||
  Vector vector() const;
 | 
			
		||||
  Vector at(size_t j) const;
 | 
			
		||||
  void update(const gtsam::VectorValues& values);
 | 
			
		||||
 | 
			
		||||
  //Advanced Interface
 | 
			
		||||
  void setZero();
 | 
			
		||||
 | 
			
		||||
  gtsam::VectorValues add(const gtsam::VectorValues& c) const;
 | 
			
		||||
  void addInPlace(const gtsam::VectorValues& c);
 | 
			
		||||
  gtsam::VectorValues subtract(const gtsam::VectorValues& c) const;
 | 
			
		||||
  gtsam::VectorValues scale(double a) const;
 | 
			
		||||
  void scaleInPlace(double a);
 | 
			
		||||
 | 
			
		||||
  bool hasSameStructure(const gtsam::VectorValues& other)  const;
 | 
			
		||||
  double dot(const gtsam::VectorValues& V) const;
 | 
			
		||||
  double norm() const;
 | 
			
		||||
  double squaredNorm() const;
 | 
			
		||||
 | 
			
		||||
  // enabling serialization functionality
 | 
			
		||||
  void serialize() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
#include <gtsam/linear/GaussianFactor.h>
 | 
			
		||||
virtual class GaussianFactor {
 | 
			
		||||
  gtsam::KeyVector keys() const;
 | 
			
		||||
  void print(string s) const;
 | 
			
		||||
  bool equals(const gtsam::GaussianFactor& lf, double tol) const;
 | 
			
		||||
  double error(const gtsam::VectorValues& c) const;
 | 
			
		||||
  gtsam::GaussianFactor* clone() const;
 | 
			
		||||
  gtsam::GaussianFactor* negate() const;
 | 
			
		||||
  Matrix augmentedInformation() const;
 | 
			
		||||
  Matrix information() const;
 | 
			
		||||
  Matrix augmentedJacobian() const;
 | 
			
		||||
  pair<Matrix, Vector> jacobian() const;
 | 
			
		||||
  size_t size() const;
 | 
			
		||||
  bool empty() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
#include <gtsam/linear/JacobianFactor.h>
 | 
			
		||||
virtual class JacobianFactor : gtsam::GaussianFactor {
 | 
			
		||||
  //Constructors
 | 
			
		||||
  JacobianFactor();
 | 
			
		||||
  JacobianFactor(const gtsam::GaussianFactor& factor);
 | 
			
		||||
  JacobianFactor(Vector b_in);
 | 
			
		||||
  JacobianFactor(size_t i1, Matrix A1, Vector b,
 | 
			
		||||
      const gtsam::noiseModel::Diagonal* model);
 | 
			
		||||
  JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b,
 | 
			
		||||
      const gtsam::noiseModel::Diagonal* model);
 | 
			
		||||
  JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3,
 | 
			
		||||
      Vector b, const gtsam::noiseModel::Diagonal* model);
 | 
			
		||||
  //JacobianFactor(const gtsam::GaussianFactorGraph& graph);
 | 
			
		||||
  //JacobianFactor(const gtsam::JacobianFactor& other);
 | 
			
		||||
 | 
			
		||||
  //Testable
 | 
			
		||||
  void print(string s) const;
 | 
			
		||||
  void printKeys(string s) const;
 | 
			
		||||
  bool equals(const gtsam::GaussianFactor& lf, double tol) const;
 | 
			
		||||
  size_t size() const;
 | 
			
		||||
  Vector unweighted_error(const gtsam::VectorValues& c) const;
 | 
			
		||||
  Vector error_vector(const gtsam::VectorValues& c) const;
 | 
			
		||||
  double error(const gtsam::VectorValues& c) const;
 | 
			
		||||
 | 
			
		||||
  //Standard Interface
 | 
			
		||||
  Matrix getA() const;
 | 
			
		||||
  Vector getb() const;
 | 
			
		||||
  size_t rows() const;
 | 
			
		||||
  size_t cols() const;
 | 
			
		||||
  bool isConstrained() const;
 | 
			
		||||
  pair<Matrix, Vector> jacobianUnweighted() const;
 | 
			
		||||
  Matrix augmentedJacobianUnweighted() const;
 | 
			
		||||
 | 
			
		||||
  void transposeMultiplyAdd(double alpha, const 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);
 | 
			
		||||
 | 
			
		||||
  gtsam::noiseModel::Diagonal* get_model() const;
 | 
			
		||||
 | 
			
		||||
  // enabling serialization functionality
 | 
			
		||||
  void serialize() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
#include <gtsam/linear/HessianFactor.h>
 | 
			
		||||
virtual class HessianFactor : gtsam::GaussianFactor {
 | 
			
		||||
  //Constructors
 | 
			
		||||
  HessianFactor();
 | 
			
		||||
  HessianFactor(const gtsam::GaussianFactor& factor);
 | 
			
		||||
  HessianFactor(size_t j, Matrix G, Vector g, double f);
 | 
			
		||||
  HessianFactor(size_t j, Vector mu, Matrix Sigma);
 | 
			
		||||
  HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22,
 | 
			
		||||
      Vector g2, double f);
 | 
			
		||||
  HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13,
 | 
			
		||||
      Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3,
 | 
			
		||||
      double f);
 | 
			
		||||
  //HessianFactor(const gtsam::GaussianFactorGraph& factors);
 | 
			
		||||
  //HessianFactor(const gtsam::HessianFactor& other);
 | 
			
		||||
 | 
			
		||||
  //Testable
 | 
			
		||||
  size_t size() const;
 | 
			
		||||
  void print(string s) const;
 | 
			
		||||
  void printKeys(string s) const;
 | 
			
		||||
  bool equals(const gtsam::GaussianFactor& lf, double tol) const;
 | 
			
		||||
  double error(const gtsam::VectorValues& c) const;
 | 
			
		||||
 | 
			
		||||
  //Standard Interface
 | 
			
		||||
  size_t rows() const;
 | 
			
		||||
  Matrix information() const;
 | 
			
		||||
  double constantTerm() const;
 | 
			
		||||
  Vector linearTerm() const;
 | 
			
		||||
 | 
			
		||||
  // enabling serialization functionality
 | 
			
		||||
  void serialize() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
#include <gtsam/nonlinear/Values.h>
 | 
			
		||||
class Values {
 | 
			
		||||
  Values();
 | 
			
		||||
  //Values(const gtsam::Values& other);
 | 
			
		||||
 | 
			
		||||
  size_t size() const;
 | 
			
		||||
  bool empty() const;
 | 
			
		||||
  void clear();
 | 
			
		||||
  size_t dim() const;
 | 
			
		||||
 | 
			
		||||
  void print(string s) const;
 | 
			
		||||
  bool equals(const gtsam::Values& other, double tol) const;
 | 
			
		||||
 | 
			
		||||
  void insert(const gtsam::Values& values);
 | 
			
		||||
  void update(const gtsam::Values& values);
 | 
			
		||||
  void erase(size_t j);
 | 
			
		||||
  void swap(gtsam::Values& values);
 | 
			
		||||
 | 
			
		||||
  bool exists(size_t j) const;
 | 
			
		||||
  gtsam::KeyVector keys() const;
 | 
			
		||||
 | 
			
		||||
  gtsam::VectorValues zeroVectors() const;
 | 
			
		||||
 | 
			
		||||
  gtsam::Values retract(const gtsam::VectorValues& delta) const;
 | 
			
		||||
  gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const;
 | 
			
		||||
 | 
			
		||||
  // enabling serialization functionality
 | 
			
		||||
  void serialize() const;
 | 
			
		||||
 | 
			
		||||
  // New in 4.0, we have to specialize every insert/update/at to generate wrappers
 | 
			
		||||
  // Instead of the old:
 | 
			
		||||
  // void insert(size_t j, const gtsam::Value& value);
 | 
			
		||||
  // void update(size_t j, const gtsam::Value& val);
 | 
			
		||||
  // gtsam::Value at(size_t j) const;
 | 
			
		||||
 | 
			
		||||
  // template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
 | 
			
		||||
  //                gtsam::Rot3, gtsam::Pose3}>
 | 
			
		||||
  // void insert(size_t j, const T& t);
 | 
			
		||||
 | 
			
		||||
  // template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
 | 
			
		||||
  //                gtsam::Rot3, gtsam::Pose3}>
 | 
			
		||||
  // void update(size_t j, const T& t);
 | 
			
		||||
  void insert(size_t j, const gtsam::Point2& t);
 | 
			
		||||
  void insert(size_t j, const gtsam::Point3& t);
 | 
			
		||||
  void insert(size_t j, const gtsam::Rot2& t);
 | 
			
		||||
  void insert(size_t j, const gtsam::Pose2& t);
 | 
			
		||||
  void insert(size_t j, const gtsam::Rot3& t);
 | 
			
		||||
  void insert(size_t j, const gtsam::Pose3& t);
 | 
			
		||||
  void insert(size_t j, Vector t);
 | 
			
		||||
  void insert(size_t j, Matrix t);
 | 
			
		||||
 | 
			
		||||
  void update(size_t j, const gtsam::Point2& t);
 | 
			
		||||
  void update(size_t j, const gtsam::Point3& t);
 | 
			
		||||
  void update(size_t j, const gtsam::Rot2& t);
 | 
			
		||||
  void update(size_t j, const gtsam::Pose2& t);
 | 
			
		||||
  void update(size_t j, const gtsam::Rot3& t);
 | 
			
		||||
  void update(size_t j, const gtsam::Pose3& t);
 | 
			
		||||
  void update(size_t j, Vector t);
 | 
			
		||||
  void update(size_t j, Matrix t);
 | 
			
		||||
 | 
			
		||||
  template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
 | 
			
		||||
                 gtsam::Rot3, gtsam::Pose3, Vector, Matrix}>
 | 
			
		||||
  T at(size_t j);
 | 
			
		||||
 | 
			
		||||
  /// version for double
 | 
			
		||||
  void insertDouble(size_t j, double c);
 | 
			
		||||
  double atDouble(size_t j) const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
#include <gtsam/nonlinear/NonlinearFactor.h>
 | 
			
		||||
virtual class NonlinearFactor {
 | 
			
		||||
  // Factor base class
 | 
			
		||||
  size_t size() const;
 | 
			
		||||
  gtsam::KeyVector keys() const;
 | 
			
		||||
  void print(string s) const;
 | 
			
		||||
  void printKeys(string s) const;
 | 
			
		||||
  // NonlinearFactor
 | 
			
		||||
  bool equals(const gtsam::NonlinearFactor& other, double tol) const;
 | 
			
		||||
 | 
			
		||||
  double error(const gtsam::Values& c) const;
 | 
			
		||||
  size_t dim() const;
 | 
			
		||||
  bool active(const gtsam::Values& c) const;
 | 
			
		||||
  gtsam::GaussianFactor* linearize(const gtsam::Values& c) const;
 | 
			
		||||
  gtsam::NonlinearFactor* clone() const;
 | 
			
		||||
  // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //FIXME: Conversion from KeyVector to std::vector does not happen
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 | 
			
		||||
class NonlinearFactorGraph {
 | 
			
		||||
  NonlinearFactorGraph();
 | 
			
		||||
  //NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph);
 | 
			
		||||
 | 
			
		||||
  // FactorGraph
 | 
			
		||||
  void print(string s) const;
 | 
			
		||||
  bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const;
 | 
			
		||||
  size_t size() const;
 | 
			
		||||
  bool empty() const;
 | 
			
		||||
  void remove(size_t i);
 | 
			
		||||
  size_t nrFactors() const;
 | 
			
		||||
  gtsam::NonlinearFactor* at(size_t idx) const;
 | 
			
		||||
  void push_back(const gtsam::NonlinearFactorGraph& factors);
 | 
			
		||||
  void push_back(gtsam::NonlinearFactor* factor);
 | 
			
		||||
  void add(gtsam::NonlinearFactor* factor);
 | 
			
		||||
  bool exists(size_t idx) const;
 | 
			
		||||
  // gtsam::KeySet keys() const;
 | 
			
		||||
 | 
			
		||||
  // NonlinearFactorGraph
 | 
			
		||||
  double error(const gtsam::Values& values) const;
 | 
			
		||||
  double probPrime(const gtsam::Values& values) const;
 | 
			
		||||
  //gtsam::Ordering orderingCOLAMD() const;
 | 
			
		||||
  // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map<gtsam::Key,int>& constraints) const;
 | 
			
		||||
  //gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const;
 | 
			
		||||
  gtsam::NonlinearFactorGraph clone() const;
 | 
			
		||||
 | 
			
		||||
  // enabling serialization functionality
 | 
			
		||||
  void serialize() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
#include <gtsam/nonlinear/NonlinearFactor.h>
 | 
			
		||||
virtual class NoiseModelFactor: gtsam::NonlinearFactor {
 | 
			
		||||
  void equals(const gtsam::NoiseModelFactor& other, double tol) const;
 | 
			
		||||
  gtsam::noiseModel::Base* get_noiseModel() const; // deprecated by below
 | 
			
		||||
  gtsam::noiseModel::Base* noiseModel() const;
 | 
			
		||||
  Vector unwhitenedError(const gtsam::Values& x) const;
 | 
			
		||||
  Vector whitenedError(const gtsam::Values& x) const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
#include <gtsam/slam/PriorFactor.h>
 | 
			
		||||
template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3}>
 | 
			
		||||
virtual class PriorFactor : gtsam::NoiseModelFactor {
 | 
			
		||||
  PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
 | 
			
		||||
  //PriorFactor(const This& other);
 | 
			
		||||
  T prior() const;
 | 
			
		||||
 | 
			
		||||
  // enabling serialization functionality
 | 
			
		||||
  void serialize() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
 | 
			
		||||
#include <gtsam/slam/BetweenFactor.h>
 | 
			
		||||
template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3}>
 | 
			
		||||
virtual class BetweenFactor : gtsam::NoiseModelFactor {
 | 
			
		||||
  BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
 | 
			
		||||
  //BetweenFactor(const This& other);
 | 
			
		||||
  T measured() const;
 | 
			
		||||
 | 
			
		||||
  // enabling serialization functionality
 | 
			
		||||
  void serialize() const;
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
#include <gtsam/inference/Symbol.h>
 | 
			
		||||
size_t symbol(char chr, size_t index);
 | 
			
		||||
char symbolChr(size_t key);
 | 
			
		||||
size_t symbolIndex(size_t key);
 | 
			
		||||
 | 
			
		||||
#include <gtsam/inference/Key.h>
 | 
			
		||||
// Default keyformatter
 | 
			
		||||
void PrintKeyVector(const gtsam::KeyVector& keys);
 | 
			
		||||
void PrintKeyVector(const gtsam::KeyVector& keys, string s);
 | 
			
		||||
 | 
			
		||||
#include <gtsam/nonlinear/NonlinearOptimizer.h>
 | 
			
		||||
bool checkConvergence(double relativeErrorTreshold,
 | 
			
		||||
    double absoluteErrorTreshold, double errorThreshold,
 | 
			
		||||
    double currentError, double newError);
 | 
			
		||||
 | 
			
		||||
#include <gtsam/slam/dataset.h>
 | 
			
		||||
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
 | 
			
		||||
    gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart);
 | 
			
		||||
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
 | 
			
		||||
    gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise);
 | 
			
		||||
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
 | 
			
		||||
    gtsam::noiseModel::Diagonal* model, int maxID);
 | 
			
		||||
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
 | 
			
		||||
    gtsam::noiseModel::Diagonal* model);
 | 
			
		||||
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
 | 
			
		||||
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D_robust(string filename,
 | 
			
		||||
    gtsam::noiseModel::Base* model);
 | 
			
		||||
void save2D(const gtsam::NonlinearFactorGraph& graph,
 | 
			
		||||
    const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
 | 
			
		||||
    string filename);
 | 
			
		||||
 | 
			
		||||
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
 | 
			
		||||
void writeG2o(const gtsam::NonlinearFactorGraph& graph,
 | 
			
		||||
    const gtsam::Values& estimate, string filename);
 | 
			
		||||
 | 
			
		||||
//*************************************************************************
 | 
			
		||||
// Utilities
 | 
			
		||||
//*************************************************************************
 | 
			
		||||
 | 
			
		||||
namespace utilities {
 | 
			
		||||
 | 
			
		||||
  #include <gtsam/nonlinear/utilities.h>
 | 
			
		||||
  // gtsam::KeyList createKeyList(Vector I);
 | 
			
		||||
  // gtsam::KeyList createKeyList(string s, Vector I);
 | 
			
		||||
  gtsam::KeyVector createKeyVector(Vector I);
 | 
			
		||||
  gtsam::KeyVector createKeyVector(string s, Vector I);
 | 
			
		||||
  // gtsam::KeySet createKeySet(Vector I);
 | 
			
		||||
  // gtsam::KeySet createKeySet(string s, Vector I);
 | 
			
		||||
  Matrix extractPoint2(const gtsam::Values& values);
 | 
			
		||||
  Matrix extractPoint3(const gtsam::Values& values);
 | 
			
		||||
  Matrix extractPose2(const gtsam::Values& values);
 | 
			
		||||
  gtsam::Values allPose3s(gtsam::Values& values);
 | 
			
		||||
  Matrix extractPose3(const gtsam::Values& values);
 | 
			
		||||
  void perturbPoint2(gtsam::Values& values, double sigma, int seed);
 | 
			
		||||
  void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed);
 | 
			
		||||
  void perturbPoint3(gtsam::Values& values, double sigma, int seed);
 | 
			
		||||
  // void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth);
 | 
			
		||||
  // void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K);
 | 
			
		||||
  // void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor);
 | 
			
		||||
  Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values);
 | 
			
		||||
  gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base);
 | 
			
		||||
  gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys);
 | 
			
		||||
 | 
			
		||||
} //\namespace utilities
 | 
			
		||||
 | 
			
		||||
#include <gtsam/nonlinear/utilities.h>
 | 
			
		||||
class RedirectCout {
 | 
			
		||||
  RedirectCout();
 | 
			
		||||
  string str();
 | 
			
		||||
};
 | 
			
		||||
 | 
			
		||||
} //\namespace gtsam
 | 
			
		||||
| 
						 | 
				
			
			@ -0,0 +1,21 @@
 | 
			
		|||
"""Pose2 unit tests."""
 | 
			
		||||
import unittest
 | 
			
		||||
 | 
			
		||||
import numpy as np
 | 
			
		||||
 | 
			
		||||
from gtsam import Pose2
 | 
			
		||||
 | 
			
		||||
 | 
			
		||||
class TestPose2(unittest.TestCase):
 | 
			
		||||
    """Test selected Pose2 methods."""
 | 
			
		||||
 | 
			
		||||
    def test_adjoint(self):
 | 
			
		||||
        """Test adjoint method."""
 | 
			
		||||
        xi = np.array([1, 2, 3])
 | 
			
		||||
        expected = np.dot(Pose2.adjointMap_(xi), xi)
 | 
			
		||||
        actual = Pose2.adjoint_(xi, xi)
 | 
			
		||||
        np.testing.assert_array_equal(actual, expected)
 | 
			
		||||
 | 
			
		||||
 | 
			
		||||
if __name__ == "__main__":
 | 
			
		||||
    unittest.main()
 | 
			
		||||
| 
						 | 
				
			
			@ -49,8 +49,8 @@ class TestPose3(unittest.TestCase):
 | 
			
		|||
    def test_adjoint(self):
 | 
			
		||||
        """Test adjoint method."""
 | 
			
		||||
        xi = np.array([1, 2, 3, 4, 5, 6])
 | 
			
		||||
        expected = np.dot(Pose3.adjointMap(xi), xi)
 | 
			
		||||
        actual = Pose3.adjoint(xi, xi)
 | 
			
		||||
        expected = np.dot(Pose3.adjointMap_(xi), xi)
 | 
			
		||||
        actual = Pose3.adjoint_(xi, xi)
 | 
			
		||||
        np.testing.assert_array_equal(actual, expected)
 | 
			
		||||
 | 
			
		||||
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -22,6 +22,17 @@ cythonize(cythonize_eigency_conversions "../gtsam_eigency/conversions.pyx" "conv
 | 
			
		|||
  "${OUTPUT_DIR}" "${EIGENCY_INCLUDE_DIR}" "" "" "")
 | 
			
		||||
cythonize(cythonize_eigency_core "../gtsam_eigency/core.pyx" "core"
 | 
			
		||||
  ${OUTPUT_DIR} "${EIGENCY_INCLUDE_DIR}" "" "" "")
 | 
			
		||||
 | 
			
		||||
# Include Eigen headers:
 | 
			
		||||
target_include_directories(cythonize_eigency_conversions PUBLIC
 | 
			
		||||
  $<BUILD_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_BUILD}>
 | 
			
		||||
  $<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}>
 | 
			
		||||
)
 | 
			
		||||
target_include_directories(cythonize_eigency_core PUBLIC
 | 
			
		||||
  $<BUILD_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_BUILD}>
 | 
			
		||||
  $<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}>
 | 
			
		||||
)
 | 
			
		||||
 | 
			
		||||
add_dependencies(cythonize_eigency_core cythonize_eigency_conversions)
 | 
			
		||||
add_custom_target(cythonize_eigency)
 | 
			
		||||
add_dependencies(cythonize_eigency cythonize_eigency_conversions cythonize_eigency_core)
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -1,7 +1,7 @@
 | 
			
		|||
import os
 | 
			
		||||
import numpy as np
 | 
			
		||||
 | 
			
		||||
__eigen_dir__ = "${GTSAM_EIGEN_INCLUDE_PREFIX}"
 | 
			
		||||
__eigen_dir__ = "${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}"
 | 
			
		||||
 | 
			
		||||
def get_includes(include_eigen=True):
 | 
			
		||||
    root = os.path.dirname(__file__)
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
							
								
								
									
										10
									
								
								gtsam.h
								
								
								
								
							
							
						
						
									
										10
									
								
								gtsam.h
								
								
								
								
							| 
						 | 
				
			
			@ -577,9 +577,9 @@ class Pose2 {
 | 
			
		|||
  static Matrix LogmapDerivative(const gtsam::Pose2& v);
 | 
			
		||||
  Matrix AdjointMap() const;
 | 
			
		||||
  Vector Adjoint(Vector xi) const;
 | 
			
		||||
  static Matrix adjointMap(Vector v);
 | 
			
		||||
  Vector adjoint(Vector xi, Vector y);
 | 
			
		||||
  Vector adjointTranspose(Vector xi, Vector y);
 | 
			
		||||
  static Matrix adjointMap_(Vector v);
 | 
			
		||||
  static Vector adjoint_(Vector xi, Vector y);
 | 
			
		||||
  static Vector adjointTranspose(Vector xi, Vector y);
 | 
			
		||||
  static Matrix wedge(double vx, double vy, double w);
 | 
			
		||||
 | 
			
		||||
  // Group Actions on Point2
 | 
			
		||||
| 
						 | 
				
			
			@ -628,8 +628,8 @@ class Pose3 {
 | 
			
		|||
  static Vector Logmap(const gtsam::Pose3& pose);
 | 
			
		||||
  Matrix AdjointMap() const;
 | 
			
		||||
  Vector Adjoint(Vector xi) const;
 | 
			
		||||
  static Matrix adjointMap(Vector xi);
 | 
			
		||||
  static Vector adjoint(Vector xi, Vector y);
 | 
			
		||||
  static Matrix adjointMap_(Vector xi);
 | 
			
		||||
  static Vector adjoint_(Vector xi, Vector y);
 | 
			
		||||
  static Vector adjointTranspose(Vector xi, Vector y);
 | 
			
		||||
  static Matrix ExpmapDerivative(Vector xi);
 | 
			
		||||
  static Matrix LogmapDerivative(const gtsam::Pose3& xi);
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -103,14 +103,62 @@ message(STATUS "Building GTSAM - shared: ${BUILD_SHARED_LIBS}")
 | 
			
		|||
add_library(gtsam ${gtsam_srcs})
 | 
			
		||||
target_link_libraries(gtsam PUBLIC ${GTSAM_BOOST_LIBRARIES})
 | 
			
		||||
target_link_libraries(gtsam PUBLIC ${GTSAM_ADDITIONAL_LIBRARIES})
 | 
			
		||||
target_compile_definitions(gtsam PUBLIC ${GTSAM_COMPILE_DEFINITIONS})
 | 
			
		||||
target_compile_options(gtsam PUBLIC ${GTSAM_COMPILE_OPTIONS})
 | 
			
		||||
target_compile_definitions(gtsam PRIVATE ${GTSAM_COMPILE_DEFINITIONS_PRIVATE})
 | 
			
		||||
target_compile_definitions(gtsam PUBLIC ${GTSAM_COMPILE_DEFINITIONS_PUBLIC})
 | 
			
		||||
if (NOT "${GTSAM_COMPILE_OPTIONS_PUBLIC}" STREQUAL "")
 | 
			
		||||
  target_compile_options(gtsam PUBLIC ${GTSAM_COMPILE_OPTIONS_PUBLIC})
 | 
			
		||||
endif()
 | 
			
		||||
target_compile_options(gtsam PRIVATE ${GTSAM_COMPILE_OPTIONS_PRIVATE})
 | 
			
		||||
set_target_properties(gtsam PROPERTIES
 | 
			
		||||
    OUTPUT_NAME         gtsam
 | 
			
		||||
    CLEAN_DIRECT_OUTPUT 1
 | 
			
		||||
    VERSION             ${gtsam_version}
 | 
			
		||||
    SOVERSION           ${gtsam_soversion})
 | 
			
		||||
 | 
			
		||||
# Append Eigen include path, set in top-level CMakeLists.txt to either
 | 
			
		||||
# system-eigen, or GTSAM eigen path
 | 
			
		||||
target_include_directories(gtsam PUBLIC
 | 
			
		||||
  $<BUILD_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_BUILD}>
 | 
			
		||||
  $<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}>
 | 
			
		||||
)
 | 
			
		||||
# MKL include dir:
 | 
			
		||||
if (GTSAM_USE_EIGEN_MKL)
 | 
			
		||||
  target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR})
 | 
			
		||||
endif()
 | 
			
		||||
 | 
			
		||||
if(GTSAM_USE_TBB)
 | 
			
		||||
  target_include_directories(gtsam PUBLIC ${TBB_INCLUDE_DIRS})
 | 
			
		||||
endif()
 | 
			
		||||
 | 
			
		||||
# Add includes for source directories 'BEFORE' boost and any system include
 | 
			
		||||
# paths so that the compiler uses GTSAM headers in our source directory instead
 | 
			
		||||
# of any previously installed GTSAM headers.
 | 
			
		||||
target_include_directories(gtsam BEFORE PUBLIC
 | 
			
		||||
  # SuiteSparse_config
 | 
			
		||||
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/SuiteSparse_config>
 | 
			
		||||
  $<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/SuiteSparse_config>
 | 
			
		||||
  # CCOLAMD
 | 
			
		||||
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Include>
 | 
			
		||||
  $<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/CCOLAMD>
 | 
			
		||||
  # main gtsam includes:
 | 
			
		||||
  $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
 | 
			
		||||
  $<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/include/>
 | 
			
		||||
  # config.h
 | 
			
		||||
  $<BUILD_INTERFACE:${CMAKE_BINARY_DIR}>
 | 
			
		||||
  # unit tests:
 | 
			
		||||
  $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/CppUnitLite>
 | 
			
		||||
)
 | 
			
		||||
if(GTSAM_SUPPORT_NESTED_DISSECTION)
 | 
			
		||||
  target_include_directories(gtsam BEFORE PUBLIC
 | 
			
		||||
    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/gtsam/3rdparty/metis/include>
 | 
			
		||||
    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/gtsam/3rdparty/metis/libmetis>
 | 
			
		||||
    $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/gtsam/3rdparty/metis/GKlib>
 | 
			
		||||
    $<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/metis/>
 | 
			
		||||
  )
 | 
			
		||||
endif()
 | 
			
		||||
 | 
			
		||||
 | 
			
		||||
 | 
			
		||||
if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library
 | 
			
		||||
	if (NOT BUILD_SHARED_LIBS)
 | 
			
		||||
		set_target_properties(gtsam PROPERTIES
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -146,17 +146,21 @@ public:
 | 
			
		|||
  /**
 | 
			
		||||
   * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
 | 
			
		||||
   */
 | 
			
		||||
  Vector3 adjoint(const Vector3& xi, const Vector3& y) {
 | 
			
		||||
  static Vector3 adjoint(const Vector3& xi, const Vector3& y) {
 | 
			
		||||
    return adjointMap(xi) * y;
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
  /**
 | 
			
		||||
   * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
 | 
			
		||||
   */
 | 
			
		||||
  Vector3 adjointTranspose(const Vector3& xi, const Vector3& y) {
 | 
			
		||||
  static Vector3 adjointTranspose(const Vector3& xi, const Vector3& y) {
 | 
			
		||||
    return adjointMap(xi).transpose() * y;
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
  // temporary fix for wrappers until case issue is resolved
 | 
			
		||||
  static Matrix3 adjointMap_(const Vector3 &xi) { return adjointMap(xi);}
 | 
			
		||||
  static Vector3 adjoint_(const Vector3 &xi, const Vector3 &y) { return adjoint(xi, y);}
 | 
			
		||||
 | 
			
		||||
  /**
 | 
			
		||||
   * wedge for SE(2):
 | 
			
		||||
   * @param xi 3-dim twist (v,omega) where
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -160,6 +160,10 @@ public:
 | 
			
		|||
  static Vector6 adjoint(const Vector6 &xi, const Vector6 &y,
 | 
			
		||||
      OptionalJacobian<6, 6> = boost::none);
 | 
			
		||||
 | 
			
		||||
  // temporary fix for wrappers until case issue is resolved
 | 
			
		||||
  static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);}
 | 
			
		||||
  static Vector6 adjoint_(const Vector6 &xi, const Vector6 &y) { return adjoint(xi, y);}
 | 
			
		||||
 | 
			
		||||
  /**
 | 
			
		||||
   * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
 | 
			
		||||
   */
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -51,7 +51,6 @@ using namespace gtsam::symbol_shorthand;
 | 
			
		|||
 | 
			
		||||
namespace gtsam {
 | 
			
		||||
 | 
			
		||||
#ifndef MATLAB_MEX_FILE
 | 
			
		||||
/* ************************************************************************* */
 | 
			
		||||
string findExampleDataFile(const string& name) {
 | 
			
		||||
  // Search source tree and installed location
 | 
			
		||||
| 
						 | 
				
			
			@ -100,9 +99,6 @@ string createRewrittenFileName(const string& name) {
 | 
			
		|||
  return newpath.string();
 | 
			
		||||
}
 | 
			
		||||
 | 
			
		||||
/* ************************************************************************* */
 | 
			
		||||
#endif
 | 
			
		||||
 | 
			
		||||
/* ************************************************************************* */
 | 
			
		||||
GraphAndValues load2D(pair<string, SharedNoiseModel> dataset, int maxID,
 | 
			
		||||
    bool addNoise, bool smart, NoiseFormat noiseFormat,
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -37,7 +37,6 @@
 | 
			
		|||
 | 
			
		||||
namespace gtsam {
 | 
			
		||||
 | 
			
		||||
#ifndef MATLAB_MEX_FILE
 | 
			
		||||
/**
 | 
			
		||||
 * Find the full path to an example dataset distributed with gtsam.  The name
 | 
			
		||||
 * may be specified with or without a file extension - if no extension is
 | 
			
		||||
| 
						 | 
				
			
			@ -56,7 +55,6 @@ GTSAM_EXPORT std::string findExampleDataFile(const std::string& name);
 | 
			
		|||
 * for checking read-write oprations
 | 
			
		||||
 */
 | 
			
		||||
GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
 | 
			
		||||
#endif
 | 
			
		||||
 | 
			
		||||
/// Indicates how noise parameters are stored in file
 | 
			
		||||
enum NoiseFormat {
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -9,18 +9,6 @@ set (GTSAM_VERSION_STRING "@GTSAM_VERSION_STRING@")
 | 
			
		|||
set (GTSAM_USE_TBB @GTSAM_USE_TBB@)
 | 
			
		||||
set (GTSAM_DEFAULT_ALLOCATOR @GTSAM_DEFAULT_ALLOCATOR@)
 | 
			
		||||
 | 
			
		||||
if("@GTSAM_USE_TBB@")
 | 
			
		||||
  list(APPEND GTSAM_INCLUDE_DIR "@TBB_INCLUDE_DIRS@")
 | 
			
		||||
endif()
 | 
			
		||||
 | 
			
		||||
# Append Eigen include path, set in top-level CMakeLists.txt to either
 | 
			
		||||
# system-eigen, or GTSAM eigen path
 | 
			
		||||
list(APPEND GTSAM_INCLUDE_DIR "@GTSAM_EIGEN_INCLUDE_PREFIX@")
 | 
			
		||||
 | 
			
		||||
if("@GTSAM_USE_EIGEN_MKL@")
 | 
			
		||||
  list(APPEND GTSAM_INCLUDE_DIR "@MKL_INCLUDE_DIR@")
 | 
			
		||||
endif()
 | 
			
		||||
 | 
			
		||||
if("@GTSAM_INSTALL_CYTHON_TOOLBOX@")
 | 
			
		||||
  list(APPEND GTSAM_EIGENCY_INSTALL_PATH "@GTSAM_EIGENCY_INSTALL_PATH@")
 | 
			
		||||
endif()
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -1,22 +1,30 @@
 | 
			
		|||
# Build/install Wrap
 | 
			
		||||
 | 
			
		||||
set(WRAP_BOOST_LIBRARIES ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_THREAD_LIBRARY})
 | 
			
		||||
set(WRAP_BOOST_LIBRARIES
 | 
			
		||||
  Boost::system
 | 
			
		||||
  Boost::filesystem
 | 
			
		||||
  Boost::thread
 | 
			
		||||
)
 | 
			
		||||
 | 
			
		||||
# Allow for disabling serialization to handle errors related to Clang's linker
 | 
			
		||||
option(GTSAM_WRAP_SERIALIZATION "If enabled, allows for wrapped objects to be saved via boost.serialization" ON)
 | 
			
		||||
if (NOT GTSAM_WRAP_SERIALIZATION)
 | 
			
		||||
    add_definitions(-DWRAP_DISABLE_SERIALIZE)
 | 
			
		||||
endif()  
 | 
			
		||||
 | 
			
		||||
# Build the executable itself
 | 
			
		||||
file(GLOB wrap_srcs "*.cpp")
 | 
			
		||||
file(GLOB wrap_headers "*.h")
 | 
			
		||||
list(REMOVE_ITEM wrap_srcs ${CMAKE_CURRENT_SOURCE_DIR}/wrap.cpp)
 | 
			
		||||
add_library(wrap_lib STATIC ${wrap_srcs} ${wrap_headers})
 | 
			
		||||
target_include_directories(wrap_lib PUBLIC
 | 
			
		||||
  $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}>
 | 
			
		||||
)
 | 
			
		||||
if (NOT GTSAM_WRAP_SERIALIZATION)
 | 
			
		||||
  target_compile_definitions(wrap_lib PUBLIC -DWRAP_DISABLE_SERIALIZE)
 | 
			
		||||
endif()
 | 
			
		||||
 | 
			
		||||
target_link_libraries(wrap_lib ${WRAP_BOOST_LIBRARIES})
 | 
			
		||||
gtsam_assign_source_folders(${wrap_srcs} ${wrap_headers})
 | 
			
		||||
add_executable(wrap wrap.cpp)
 | 
			
		||||
target_link_libraries(wrap wrap_lib ${WRAP_BOOST_LIBRARIES})
 | 
			
		||||
target_link_libraries(wrap PRIVATE wrap_lib)
 | 
			
		||||
 | 
			
		||||
# Set folder in Visual Studio
 | 
			
		||||
file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
 | 
			
		||||
| 
						 | 
				
			
			@ -32,4 +40,3 @@ install(FILES matlab.h DESTINATION include/wrap)
 | 
			
		|||
 | 
			
		||||
# Build tests
 | 
			
		||||
add_subdirectory(tests)
 | 
			
		||||
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
		Loading…
	
		Reference in New Issue