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

Merge remote-tracking branch 'origin/develop' into feature/AHRSFactor

release/4.3a0
krunalchande 2014-11-13 12:52:15 -05:00
parent 3774194651 c6c611ae11
commit 9afee71399
196 changed files with 150790 additions and 3979 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
.cproject
View File

@ -1,19 +1,17 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?fileVersion 4.0.0?>
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<storageModule moduleId="org.eclipse.cdt.core.settings">
<cconfiguration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" moduleId="org.eclipse.cdt.core.settings" name="MacOSX GCC">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
@ -62,13 +60,13 @@
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890" moduleId="org.eclipse.cdt.core.settings" name="Timing">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
@ -118,13 +116,13 @@
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216" moduleId="org.eclipse.cdt.core.settings" name="fast">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
@ -2654,6 +2652,30 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGPSFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGPSFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussMarkov1stOrderFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussMarkov1stOrderFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testImplicitSchurFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testImplicitSchurFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>

3
.gitignore vendored
View File

@ -3,4 +3,5 @@
*.pyc
*.DS_Store
/examples/Data/dubrovnik-3-7-pre-rewritten.txt
/examples/Data/pose2example-rewritten.txt
/examples/Data/pose2example-rewritten.txt
/examples/Data/pose3example-rewritten.txt

15
CMakeLists.txt
View File

@ -2,6 +2,12 @@
project(GTSAM CXX C)
cmake_minimum_required(VERSION 2.6)
# new feature to Cmake Version > 2.8.12
# Mac ONLY. Define Relative Path on Mac OS
if(NOT DEFINED CMAKE_MACOSX_RPATH)
set(CMAKE_MACOSX_RPATH 0)
endif()
# Set the version number for the library
set (GTSAM_VERSION_MAJOR 3)
set (GTSAM_VERSION_MINOR 1)
@ -123,6 +129,11 @@ else()
endif()
if(${Boost_VERSION} EQUAL 105600)
message("Ignoring Boost restriction on optional lvalue assignment from rvalues")
add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES)
endif()
###############################################################################
# Find TBB
find_package(TBB)
@ -169,9 +180,9 @@ endif()
###############################################################################
# Find OpenMP (if we're also using MKL)
if(GTSAM_WITH_EIGEN_MKL AND GTSAM_USE_EIGEN_MKL_OPENMP AND GTSAM_USE_EIGEN_MKL)
find_package(OpenMP)
find_package(OpenMP) # do this here to generate correct message if disabled
if(GTSAM_WITH_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP AND GTSAM_USE_EIGEN_MKL)
if(OPENMP_FOUND AND GTSAM_USE_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP)
set(GTSAM_USE_EIGEN_MKL_OPENMP 1) # This will go into config.h
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")

25
cmake/FindMKL.cmake
View File

@ -58,6 +58,7 @@ FIND_PATH(MKL_ROOT_DIR
/opt/intel/mkl/*/
/opt/intel/cmkl/
/opt/intel/cmkl/*/
/opt/intel/*/mkl/
/Library/Frameworks/Intel_MKL.framework/Versions/Current/lib/universal
"C:/Program Files (x86)/Intel/ComposerXE-2011/mkl"
"C:/Program Files (x86)/Intel/Composer XE 2013/mkl"
@ -136,13 +137,16 @@ ELSE() # UNIX and macOS
${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
${MKL_ROOT_DIR}/lib/
)
FIND_LIBRARY(MKL_GNUTHREAD_LIBRARY
mkl_gnu_thread
PATHS
${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
${MKL_ROOT_DIR}/lib/
)
# MKL on Mac OS doesn't ship with GNU thread versions, only Intel versions (see above)
IF(NOT APPLE)
FIND_LIBRARY(MKL_GNUTHREAD_LIBRARY
mkl_gnu_thread
PATHS
${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
${MKL_ROOT_DIR}/lib/
)
ENDIF()
# Intel Libraries
IF("${MKL_ARCH_DIR}" STREQUAL "32")
@ -226,7 +230,12 @@ ELSE() # UNIX and macOS
endforeach()
endforeach()
SET(MKL_LIBRARIES ${MKL_LP_GNUTHREAD_LIBRARIES})
IF(APPLE)
SET(MKL_LIBRARIES ${MKL_LP_INTELTHREAD_LIBRARIES})
ELSE()
SET(MKL_LIBRARIES ${MKL_LP_GNUTHREAD_LIBRARIES})
ENDIF()
MARK_AS_ADVANCED(MKL_CORE_LIBRARY MKL_LP_LIBRARY MKL_ILP_LIBRARY
MKL_SEQUENTIAL_LIBRARY MKL_INTELTHREAD_LIBRARY MKL_GNUTHREAD_LIBRARY)
ENDIF()

8
cmake/GtsamMatlabWrap.cmake
View File

@ -28,13 +28,13 @@ endif()
# finding the LaTeX mex program (totally unrelated to MATLAB Mex) when LaTeX is
# on the system path.
list(REVERSE matlab_bin_directories) # Reverse list so the highest version (sorted alphabetically) is preferred
find_program(mex_command ${mex_program_name}
find_program(MEX_COMMAND ${mex_program_name}
PATHS ${matlab_bin_directories} ENV PATH
NO_DEFAULT_PATH)
mark_as_advanced(FORCE mex_command)
mark_as_advanced(FORCE MEX_COMMAND)
# Now that we have mex, trace back to find the Matlab installation root
get_filename_component(mex_command "${mex_command}" REALPATH)
get_filename_component(mex_path "${mex_command}" PATH)
get_filename_component(MEX_COMMAND "${MEX_COMMAND}" REALPATH)
get_filename_component(mex_path "${MEX_COMMAND}" PATH)
get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE)
set(MATLAB_ROOT "${MATLAB_ROOT}" CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)")

1
examples/Data/VO_calibration00.txt Normal file
View File

@ -0,0 +1 @@
718.856 718.856 0.0 607.1928 185.2157 0.5371657189

1
examples/Data/VO_calibration00s.txt Normal file
View File

@ -0,0 +1 @@
718.856 718.856 0.0 607.1928 185.2157 0.5371657189

135
examples/Data/VO_camera_poses00.txt Normal file
View File

@ -0,0 +1,135 @@
0 1 0 0 0 0 1 0 0 -0 0 1 0 0 0 0 1
1 0.99999 -0.00268679 -0.00354618 6.43221e-05 0.00267957 0.999994 -0.00204036 -0.0073023 0.00355164 0.00203084 0.999992 0.676456 0 0 0 1
2 0.999969 -0.00120771 -0.00772489 -0.0100328 0.00117985 0.999993 -0.003611 -0.0111185 0.00772919 0.00360178 0.999964 1.37125 0 0 0 1
3 0.999931 -0.00128098 -0.0117006 -0.0237327 0.00122052 0.999986 -0.00517227 -0.0136538 0.0117071 0.00515763 0.999918 2.08563 0 0 0 1
4 0.99986 5.79321e-05 -0.0167106 -0.0402272 -0.000155312 0.999983 -0.00582618 -0.0194327 0.01671 0.00582796 0.999843 2.81528 0 0 0 1
5 0.999772 -0.00118366 -0.0213077 -0.0572378 0.0010545 0.999981 -0.00607208 -0.0278191 0.0213145 0.00604822 0.999755 3.56204 0 0 0 1
6 0.999662 0.000544425 -0.0259946 -0.081545 -0.000735472 0.999973 -0.00734051 -0.0358844 0.0259899 0.00735714 0.999635 4.32265 0 0 0 1
7 0.999513 0.0032602 -0.0310324 -0.112137 -0.0035101 0.999962 -0.00800188 -0.0447209 0.0310051 0.00810691 0.999486 5.09668 0 0 0 1
8 0.999361 0.00349173 -0.0355658 -0.143594 -0.00372162 0.999973 -0.00639979 -0.0532611 0.0355425 0.00652807 0.999347 5.88701 0 0 0 1
9 0.999185 0.00268131 -0.040271 -0.176401 -0.0028332 0.999989 -0.00371493 -0.0632884 0.0402606 0.003826 0.999182 6.6897 0 0 0 1
10 0.99903 0.00226305 -0.0439747 -0.211687 -0.00231163 0.999997 -0.00105382 -0.072362 0.0439722 0.00115445 0.999032 7.50361 0 0 0 1
11 0.998896 0.00366482 -0.0468376 -0.254125 -0.00374515 0.999992 -0.00162734 -0.0820263 0.0468312 0.00180096 0.998901 8.32333 0 0 0 1
12 0.998775 0.00304285 -0.0493866 -0.295424 -0.00313866 0.999993 -0.00186268 -0.0885739 0.0493806 0.00201541 0.998778 9.15211 0 0 0 1
13 0.998682 7.09894e-05 -0.0513155 -0.334647 -0.000203775 0.999997 -0.00258241 -0.0938889 0.0513152 0.00258946 0.998679 9.98839 0 0 0 1
14 0.998565 -8.82523e-05 -0.0535542 -0.380835 -9.36659e-06 0.999998 -0.00182255 -0.10173 0.0535542 0.00182044 0.998563 10.832 0 0 0 1
15 0.998481 -0.00146793 -0.0550718 -0.429135 0.0013525 0.999997 -0.00213307 -0.111427 0.0550748 0.00205535 0.99848 11.687 0 0 0 1
16 0.998373 0.000738731 -0.0570218 -0.483426 -0.000993083 0.99999 -0.00443241 -0.122139 0.0570179 0.00448183 0.998363 12.5483 0 0 0 1
17 0.998285 0.00120595 -0.0585258 -0.540056 -0.00162301 0.999974 -0.00707907 -0.132598 0.0585158 0.00716191 0.998261 13.4179 0 0 0 1
18 0.998165 0.00516151 -0.060337 -0.6023 -0.00570195 0.999945 -0.00878826 -0.143753 0.0602883 0.00911617 0.998139 14.2952 0 0 0 1
19 0.998101 0.00610094 -0.0612993 -0.66308 -0.00663017 0.999942 -0.00843386 -0.157854 0.0612443 0.00882427 0.998084 15.1802 0 0 0 1
20 0.998014 0.0052997 -0.0627662 -0.722045 -0.00574767 0.999959 -0.0069587 -0.172847 0.0627268 0.00730564 0.998004 16.074 0 0 0 1
21 0.99792 0.00591748 -0.0641975 -0.78346 -0.00627924 0.999966 -0.00543487 -0.186221 0.0641631 0.00582667 0.997922 16.9738 0 0 0 1
22 0.997857 0.00547694 -0.0651993 -0.845347 -0.00584101 0.999968 -0.00539455 -0.199741 0.0651677 0.00576382 0.997858 17.8786 0 0 0 1
23 0.997737 0.00536917 -0.0670282 -0.908218 -0.00579979 0.999964 -0.0062316 -0.212775 0.0669924 0.00660624 0.997732 18.7877 0 0 0 1
24 0.997663 0.00386695 -0.0682185 -0.971291 -0.00435203 0.999966 -0.00696344 -0.226442 0.0681893 0.00724406 0.997646 19.7046 0 0 0 1
25 0.997629 0.00410637 -0.0687004 -1.03663 -0.00448288 0.999976 -0.00532714 -0.239555 0.0686769 0.00562249 0.997623 20.6257 0 0 0 1
26 0.997617 0.00588773 -0.0687501 -1.10557 -0.0060349 0.99998 -0.00193325 -0.254273 0.0687373 0.00234355 0.997632 21.55 0 0 0 1
27 0.997662 0.00693766 -0.0679906 -1.17297 -0.00682806 0.999975 0.0018442 -0.26563 0.0680017 -0.00137565 0.997684 22.4875 0 0 0 1
28 0.997774 0.00579785 -0.0664343 -1.23728 -0.00550265 0.999974 0.00462554 -0.271962 0.0664594 -0.00424968 0.99778 23.4285 0 0 0 1
29 0.997872 0.00589563 -0.0649408 -1.30214 -0.00556012 0.99997 0.00534586 -0.277922 0.0649704 -0.0049734 0.997875 24.3732 0 0 0 1
30 0.997958 0.00627024 -0.0635595 -1.36462 -0.00612984 0.999978 0.00240374 -0.285335 0.0635732 -0.00200922 0.997975 25.314 0 0 0 1
31 0.998004 0.00714074 -0.0627411 -1.42783 -0.00731158 0.99997 -0.00249375 -0.293171 0.0627215 0.00294751 0.998027 26.2605 0 0 0 1
32 0.99808 0.0063692 -0.0616159 -1.48954 -0.00671918 0.999962 -0.00547459 -0.302321 0.0615787 0.00587809 0.998085 27.2168 0 0 0 1
33 0.99813 0.00376787 -0.0610159 -1.54654 -0.00404632 0.999982 -0.0044408 -0.313516 0.0609981 0.00467938 0.998127 28.1829 0 0 0 1
34 0.998113 0.00193972 -0.0613743 -1.60668 -0.00191171 0.999998 0.000515183 -0.324411 0.0613752 -0.000396881 0.998115 29.1626 0 0 0 1
35 0.99806 -0.0017885 -0.062228 -1.66532 0.00203402 0.99999 0.00388232 -0.335656 0.0622204 -0.00400136 0.998054 30.1428 0 0 0 1
36 0.997945 -0.00917543 -0.0634115 -1.72059 0.00939451 0.999951 0.00315749 -0.343316 0.0633794 -0.00374672 0.997982 31.1244 0 0 0 1
37 0.997825 -0.0112684 -0.0649459 -1.78049 0.011242 0.999937 -0.000771312 -0.350864 0.0649504 3.95099e-05 0.997888 32.1064 0 0 0 1
38 0.997739 -0.0110126 -0.0662983 -1.85007 0.0107254 0.999932 -0.00468596 -0.361068 0.0663454 0.00396429 0.997789 33.0886 0 0 0 1
39 0.997597 -0.00959503 -0.0686163 -1.92119 0.00924037 0.999942 -0.00548426 -0.373466 0.0686649 0.00483704 0.997628 34.0774 0 0 0 1
40 0.99755 -0.0095802 -0.0693031 -1.99331 0.00931271 0.999948 -0.00418184 -0.387047 0.0693396 0.00352619 0.997587 35.0736 0 0 0 1
41 0.997473 -0.00634387 -0.0707596 -2.0707 0.00626661 0.99998 -0.0013139 -0.403858 0.0707665 0.00086716 0.997493 36.0721 0 0 0 1
42 0.99739 -0.00624366 -0.0719343 -2.14553 0.00625582 0.99998 -5.62375e-05 -0.416888 0.0719332 -0.000393917 0.997409 37.0728 0 0 0 1
43 0.997312 -0.00473093 -0.0731254 -2.21909 0.00492848 0.999985 0.00252135 -0.428625 0.0731123 -0.00287497 0.99732 38.0643 0 0 0 1
44 0.997318 -0.00467696 -0.0730348 -2.29215 0.00509473 0.999972 0.00553481 -0.440023 0.0730068 -0.00589206 0.997314 39.0618 0 0 0 1
45 0.997274 0.00138304 -0.0737801 -2.37574 -0.000811217 0.999969 0.00777971 -0.447869 0.0737886 -0.00769865 0.997244 40.0548 0 0 0 1
46 0.997262 0.00149131 -0.0739326 -2.45529 -0.000969511 0.999974 0.00709318 -0.454763 0.0739413 -0.00700208 0.997238 41.0557 0 0 0 1
47 0.997266 0.00175929 -0.0738699 -2.53081 -0.00136899 0.999985 0.00533379 -0.460519 0.0738782 -0.00521809 0.997254 42.0518 0 0 0 1
48 0.997253 0.00408494 -0.0739555 -2.61212 -0.00386552 0.999988 0.00310988 -0.469863 0.0739673 -0.00281546 0.997257 43.0493 0 0 0 1
49 0.997185 0.00365371 -0.0748884 -2.68799 -0.00342799 0.999989 0.00314243 -0.47951 0.0748991 -0.00287687 0.997187 44.0473 0 0 0 1
50 0.997077 0.00181435 -0.0763845 -2.76071 -0.00149292 0.99999 0.00426495 -0.487845 0.0763915 -0.00413845 0.997069 45.0403 0 0 0 1
51 0.997018 0.00246727 -0.0771352 -2.84117 -0.00206285 0.999984 0.00532227 -0.499132 0.0771471 -0.00514727 0.997006 46.0244 0 0 0 1
52 0.996991 0.00504805 -0.0773507 -2.92304 -0.00493379 0.999986 0.00166824 -0.510863 0.0773581 -0.00128158 0.997003 46.994 0 0 0 1
53 0.996911 0.00581773 -0.0783264 -3.00373 -0.00604061 0.999978 -0.00260888 -0.521193 0.0783095 0.00307396 0.996924 47.9551 0 0 0 1
54 0.996846 0.00678413 -0.0790757 -3.08343 -0.00711636 0.999967 -0.00392044 -0.534186 0.0790465 0.00447081 0.996861 48.9236 0 0 0 1
55 0.996843 0.00557268 -0.0792034 -3.16262 -0.00562268 0.999984 -0.000408328 -0.54901 0.0791999 0.000852374 0.996858 49.9005 0 0 0 1
56 0.996831 0.00375007 -0.0794568 -3.23868 -0.00354655 0.99999 0.00270227 -0.563036 0.0794661 -0.0024119 0.996835 50.8752 0 0 0 1
57 0.996805 0.00190455 -0.0798474 -3.31582 -0.00164885 0.999993 0.00326822 -0.574113 0.0798531 -0.00312612 0.996802 51.8394 0 0 0 1
58 0.996782 -0.00124932 -0.0801505 -3.39153 0.00141878 0.999997 0.0020573 -0.586659 0.0801477 -0.00216439 0.996781 52.8005 0 0 0 1
59 0.996745 -0.0038025 -0.0805262 -3.4676 0.0038689 0.999992 0.000668539 -0.59892 0.080523 -0.00097791 0.996752 53.7575 0 0 0 1
60 0.996643 -0.00519016 -0.0817059 -3.54489 0.00535256 0.999984 0.00176869 -0.60864 0.0816955 -0.00220009 0.996655 54.708 0 0 0 1
61 0.996534 -0.0079249 -0.0828082 -3.62139 0.00842977 0.999948 0.00574894 -0.618858 0.0827583 -0.00642707 0.996549 55.6588 0 0 0 1
62 0.996473 -0.00854289 -0.0834829 -3.69959 0.00945654 0.9999 0.0105549 -0.624401 0.0833844 -0.0113071 0.996453 56.6119 0 0 0 1
63 0.996447 -0.00664747 -0.083957 -3.78502 0.00773966 0.99989 0.0126902 -0.629769 0.0838633 -0.0132949 0.996389 57.5607 0 0 0 1
64 0.996335 -0.00522633 -0.0853755 -3.8689 0.00597793 0.999946 0.00855017 -0.636709 0.0853262 -0.0090292 0.996312 58.4941 0 0 0 1
65 0.996221 -0.00343661 -0.0867892 -3.95276 0.00350579 0.999994 0.000644619 -0.644008 0.0867865 -0.000946448 0.996226 59.4131 0 0 0 1
66 0.996144 -0.00149623 -0.0877201 -4.03806 0.00112725 0.99999 -0.00425562 -0.655271 0.0877256 0.00414033 0.996136 60.3236 0 0 0 1
67 0.996055 0.00375138 -0.0886573 -4.12895 -0.00406723 0.999986 -0.00338223 -0.671324 0.0886434 0.00372948 0.996056 61.2274 0 0 0 1
68 0.995922 0.00719305 -0.0899263 -4.21985 -0.0073202 0.999973 -0.00108421 -0.691307 0.089916 0.00173807 0.995948 62.125 0 0 0 1
69 0.99582 0.00967277 -0.0908194 -4.30702 -0.00966905 0.999953 0.000481019 -0.708494 0.0908198 0.000399128 0.995867 63.0134 0 0 0 1
70 0.995713 0.0102896 -0.0919182 -4.39131 -0.0103098 0.999947 0.000255248 -0.721276 0.091916 0.000693502 0.995767 63.8776 0 0 0 1
71 0.99554 0.0119225 -0.0935844 -4.477 -0.0118725 0.999929 0.00109156 -0.734766 0.0935908 2.43836e-05 0.995611 64.7307 0 0 0 1
72 0.995397 0.0126524 -0.0950024 -4.56121 -0.0125521 0.99992 0.00165348 -0.749039 0.0950157 -0.000453392 0.995476 65.5703 0 0 0 1
73 0.995256 0.0126635 -0.0964665 -4.64297 -0.0125254 0.999919 0.00203772 -0.761909 0.0964846 -0.00081977 0.995334 66.3938 0 0 0 1
74 0.995133 0.0127023 -0.0977168 -4.72623 -0.0124698 0.999918 0.00298947 -0.7711 0.0977468 -0.00175641 0.99521 67.2017 0 0 0 1
75 0.994948 0.015548 -0.0991814 -4.81287 -0.0150604 0.999871 0.00566291 -0.780301 0.0992566 -0.0041406 0.995053 67.9995 0 0 0 1
76 0.994794 0.0171065 -0.100462 -4.90076 -0.0162095 0.999821 0.009738 -0.788037 0.100611 -0.00805885 0.994893 68.7922 0 0 0 1
77 0.994568 0.0201446 -0.102122 -4.98771 -0.0190681 0.999752 0.0115061 -0.794955 0.102328 -0.00949629 0.994705 69.5653 0 0 0 1
78 0.99437 0.0229894 -0.10344 -5.07707 -0.0223435 0.999723 0.00739861 -0.804841 0.103581 -0.00504575 0.994608 70.3178 0 0 0 1
79 0.99423 0.0228747 -0.1048 -5.16223 -0.0229899 0.999736 0.000108822 -0.81531 0.104774 0.00230114 0.994493 71.0475 0 0 0 1
80 0.994077 0.0219938 -0.106431 -5.24094 -0.0227304 0.999725 -0.00571252 -0.825441 0.106276 0.00809789 0.994304 71.7584 0 0 0 1
81 0.994023 0.0228054 -0.106762 -5.32437 -0.0236929 0.999694 -0.00705161 -0.831667 0.106569 0.00953897 0.99426 72.4548 0 0 0 1
82 0.99386 0.0255543 -0.107653 -5.40648 -0.0260808 0.999654 -0.00348505 -0.846106 0.107527 0.00627133 0.994182 73.1337 0 0 0 1
83 0.993702 0.0257681 -0.109048 -5.48436 -0.02605 0.99966 -0.00116096 -0.865059 0.108981 0.00399435 0.994036 73.7942 0 0 0 1
84 0.99367 0.0225468 -0.110051 -5.5561 -0.0231761 0.999722 -0.00444219 -0.879535 0.10992 0.00696462 0.993916 74.4324 0 0 0 1
85 0.993802 0.0143509 -0.110234 -5.61528 -0.0155198 0.999832 -0.00975281 -0.89282 0.110075 0.0114032 0.993858 75.0504 0 0 0 1
86 0.993949 0.0102 -0.10937 -5.67796 -0.0118817 0.999821 -0.0147354 -0.904058 0.1092 0.0159457 0.993892 75.6535 0 0 0 1
87 0.994244 0.0126451 -0.106395 -5.74524 -0.014328 0.999784 -0.0150673 -0.916949 0.106181 0.016505 0.99421 76.2455 0 0 0 1
88 0.994592 0.0175824 -0.102356 -5.81375 -0.0189231 0.999747 -0.0121417 -0.930972 0.102117 0.0140129 0.994674 76.8222 0 0 0 1
89 0.995077 0.0159295 -0.0978149 -5.86699 -0.0169123 0.999814 -0.00922648 -0.942909 0.0976498 0.0108353 0.995162 77.3862 0 0 0 1
90 0.995665 0.0122046 -0.0922106 -5.90659 -0.0127358 0.999906 -0.00517412 -0.954487 0.0921387 0.00632606 0.995726 77.9355 0 0 0 1
91 0.996426 0.00781104 -0.084105 -5.94257 -0.00798227 0.999967 -0.0016998 -0.970792 0.0840889 0.00236507 0.996455 78.4692 0 0 0 1
92 0.997233 0.0114593 -0.0734453 -5.98049 -0.0116369 0.99993 -0.00198965 -0.981019 0.0734174 0.00283882 0.997297 78.9883 0 0 0 1
93 0.998165 0.0165636 -0.0582361 -6.0154 -0.0167456 0.999856 -0.0026387 -0.99003 0.058184 0.00360906 0.998299 79.4952 0 0 0 1
95 0.999635 0.0200255 -0.0181151 -6.02981 -0.0200623 0.999797 -0.00185529 -1.00502 0.0180742 0.00221804 0.999834 80.4727 0 0 0 1
97 0.999162 0.015548 0.037857 -5.96801 -0.0155684 0.999879 0.000243918 -1.02389 -0.0378486 -0.000833085 0.999283 81.4025 0 0 0 1
99 0.993959 0.0151454 0.108698 -5.84553 -0.0154328 0.999879 0.0018028 -1.04109 -0.108657 -0.00346942 0.994073 82.2952 0 0 0 1
101 0.980499 0.0151504 0.195937 -5.64466 -0.0157106 0.999876 0.00130478 -1.05761 -0.195893 -0.00435763 0.980616 83.1489 0 0 0 1
103 0.954186 0.0182833 0.298656 -5.36588 -0.0179595 0.999831 -0.00382887 -1.08348 -0.298675 -0.00171027 0.954353 83.9397 0 0 0 1
105 0.910736 0.0194893 0.412529 -4.99648 -0.0175815 0.99981 -0.00842014 -1.10057 -0.412615 0.000415655 0.910905 84.6633 0 0 0 1
107 0.848724 0.0183908 0.528517 -4.54701 -0.0135972 0.999824 -0.0129557 -1.12003 -0.528662 0.00380946 0.848824 85.2983 0 0 0 1
109 0.772259 0.0170098 0.63508 -4.0183 -0.0106749 0.999848 -0.0137989 -1.14244 -0.635218 0.00387688 0.772323 85.8601 0 0 0 1
111 0.684256 0.0156411 0.729074 -3.42903 -0.0102231 0.999877 -0.0118561 -1.16079 -0.72917 0.000659179 0.684332 86.3474 0 0 0 1
113 0.590745 0.011826 0.806772 -2.77931 -0.000173089 0.999894 -0.0145301 -1.17886 -0.806858 0.00844396 0.590684 86.7443 0 0 0 1
115 0.496173 0.0169181 0.868059 -2.10955 -0.00504039 0.999849 -0.0166057 -1.20339 -0.868209 0.00386392 0.496183 87.0847 0 0 0 1
117 0.408192 0.0165355 0.912746 -1.40862 0.00231553 0.999814 -0.0191484 -1.2249 -0.912893 0.00992974 0.408078 87.3396 0 0 0 1
119 0.333443 0.00543386 0.942754 -0.671521 0.0223493 0.999657 -0.0136666 -1.23662 -0.942505 0.025627 0.333208 87.522 0 0 0 1
121 0.269054 0.0173163 0.962969 0.0638526 0.00961829 0.99974 -0.0206648 -1.26307 -0.963077 0.0148221 0.268818 87.7199 0 0 0 1
123 0.214897 0.0233915 0.976357 0.843046 0.00677025 0.999653 -0.0254398 -1.30009 -0.976613 0.0120771 0.214664 87.8763 0 0 0 1
125 0.171479 0.031054 0.984698 1.66216 0.0212619 0.999154 -0.0352125 -1.3179 -0.984958 0.0269747 0.170674 87.9743 0 0 0 1
127 0.134011 0.0386308 0.990227 2.52547 0.0207141 0.998912 -0.041773 -1.34147 -0.990763 0.0261097 0.133065 88.0809 0 0 0 1
129 0.10418 0.0310179 0.994075 3.44652 0.0195614 0.999256 -0.0332297 -1.39013 -0.994366 0.0229074 0.103496 88.1692 0 0 0 1
131 0.0794366 0.027788 0.996453 4.42556 0.0261822 0.999208 -0.0299521 -1.42776 -0.996496 0.0284686 0.0786462 88.2295 0 0 0 1
132 0.0693462 0.028443 0.997187 4.93885 0.0294969 0.999098 -0.0305488 -1.44582 -0.997156 0.0315324 0.0684447 88.2553 0 0 0 1
133 0.0615414 0.0290168 0.997683 5.46907 0.0316982 0.999016 -0.0310108 -1.46406 -0.997601 0.0335332 0.0605611 88.2814 0 0 0 1
134 0.0559347 0.029371 0.998002 6.0151 0.0334765 0.99895 -0.0312751 -1.48373 -0.997873 0.035159 0.0548927 88.307 0 0 0 1
135 0.0504312 0.0304374 0.998264 6.58025 0.0349281 0.99887 -0.0322204 -1.50267 -0.998117 0.0364923 0.0493112 88.3306 0 0 0 1
136 0.0445067 0.0311103 0.998525 7.16082 0.0355578 0.998832 -0.0327048 -1.52353 -0.998376 0.0369609 0.0433485 88.3531 0 0 0 1
137 0.040243 0.0311989 0.998703 7.76375 0.0381603 0.998735 -0.0327376 -1.54487 -0.998461 0.0394283 0.0390016 88.3716 0 0 0 1
138 0.0373982 0.0312027 0.998813 8.38568 0.0397152 0.998676 -0.0326855 -1.56772 -0.998511 0.0408905 0.0361095 88.3901 0 0 0 1
139 0.0343726 0.0307634 0.998936 9.02449 0.0406913 0.998654 -0.0321549 -1.59059 -0.99858 0.0417533 0.0330745 88.4092 0 0 0 1
140 0.0320861 0.0302694 0.999027 9.68038 0.0427798 0.998584 -0.03163 -1.61442 -0.998569 0.043753 0.0307457 88.4263 0 0 0 1
141 0.0316452 0.0299561 0.99905 10.3542 0.0473602 0.998383 -0.0314363 -1.63856 -0.998376 0.04831 0.0301753 88.4381 0 0 0 1
142 0.0327723 0.029714 0.999021 11.0457 0.0507142 0.998221 -0.0313539 -1.66282 -0.998175 0.0516921 0.031207 88.4556 0 0 0 1
143 0.0353027 0.0297602 0.998933 11.7546 0.0522842 0.998133 -0.0315841 -1.68678 -0.998008 0.0533435 0.0336808 88.4781 0 0 0 1
144 0.0392372 0.0297502 0.998787 12.4771 0.0547241 0.997993 -0.0318763 -1.71289 -0.99773 0.0559084 0.0375304 88.5062 0 0 0 1
145 0.0437096 0.0293188 0.998614 13.219 0.0550685 0.997979 -0.0317105 -1.73922 -0.997525 0.0563782 0.0420067 88.5387 0 0 0 1
146 0.0477725 0.0278103 0.998471 13.9764 0.0564652 0.997939 -0.0304971 -1.76499 -0.997261 0.0578358 0.0461037 88.5751 0 0 0 1
147 0.0518486 0.0263145 0.998308 14.7472 0.0562418 0.997989 -0.0292271 -1.79222 -0.99707 0.057662 0.0502644 88.6178 0 0 0 1
148 0.0560658 0.0242863 0.998132 15.5313 0.0531494 0.998214 -0.0272738 -1.82056 -0.997011 0.0545792 0.0546748 88.6693 0 0 0 1
149 0.0600218 0.0233355 0.997924 16.3271 0.0522059 0.998285 -0.0264839 -1.84733 -0.996831 0.0536871 0.0587006 88.7243 0 0 0 1
150 0.0641513 0.0243795 0.997642 17.1258 0.0492204 0.998408 -0.0275632 -1.87761 -0.996726 0.0508726 0.0628492 88.7821 0 0 0 1
151 0.0672583 0.028483 0.997329 17.929 0.0470717 0.998389 -0.0316877 -1.91204 -0.996625 0.0490772 0.0658092 88.842 0 0 0 1
152 0.0688453 0.0337446 0.997056 18.7357 0.0413971 0.99847 -0.0366509 -1.9468 -0.996768 0.0437985 0.067343 88.9041 0 0 0 1
153 0.0686545 0.0370247 0.996953 19.5482 0.0387033 0.99846 -0.0397459 -1.98038 -0.996889 0.0413142 0.0671158 88.9665 0 0 0 1

77
examples/Data/VO_camera_poses00s.txt Normal file
View File

@ -0,0 +1,77 @@
0 1 0 0 0 0 1 0 0 -0 0 1 0 0 0 0 1
1 0.99999 -0.00268679 -0.00354618 6.43221e-05 0.00267957 0.999994 -0.00204036 -0.0073023 0.00355164 0.00203084 0.999992 0.676456 0 0 0 1
2 0.999969 -0.00120771 -0.00772489 -0.0100328 0.00117985 0.999993 -0.003611 -0.0111185 0.00772919 0.00360178 0.999964 1.37125 0 0 0 1
3 0.999931 -0.00128098 -0.0117006 -0.0237327 0.00122052 0.999986 -0.00517227 -0.0136538 0.0117071 0.00515763 0.999918 2.08563 0 0 0 1
4 0.99986 5.79321e-05 -0.0167106 -0.0402272 -0.000155312 0.999983 -0.00582618 -0.0194327 0.01671 0.00582796 0.999843 2.81528 0 0 0 1
5 0.999772 -0.00118366 -0.0213077 -0.0572378 0.0010545 0.999981 -0.00607208 -0.0278191 0.0213145 0.00604822 0.999755 3.56204 0 0 0 1
6 0.999662 0.000544425 -0.0259946 -0.081545 -0.000735472 0.999973 -0.00734051 -0.0358844 0.0259899 0.00735714 0.999635 4.32265 0 0 0 1
7 0.999513 0.0032602 -0.0310324 -0.112137 -0.0035101 0.999962 -0.00800188 -0.0447209 0.0310051 0.00810691 0.999486 5.09668 0 0 0 1
8 0.999361 0.00349173 -0.0355658 -0.143594 -0.00372162 0.999973 -0.00639979 -0.0532611 0.0355425 0.00652807 0.999347 5.88701 0 0 0 1
9 0.999185 0.00268131 -0.040271 -0.176401 -0.0028332 0.999989 -0.00371493 -0.0632884 0.0402606 0.003826 0.999182 6.6897 0 0 0 1
10 0.99903 0.00226305 -0.0439747 -0.211687 -0.00231163 0.999997 -0.00105382 -0.072362 0.0439722 0.00115445 0.999032 7.50361 0 0 0 1
11 0.998896 0.00366482 -0.0468376 -0.254125 -0.00374515 0.999992 -0.00162734 -0.0820263 0.0468312 0.00180096 0.998901 8.32333 0 0 0 1
12 0.998775 0.00304285 -0.0493866 -0.295424 -0.00313866 0.999993 -0.00186268 -0.0885739 0.0493806 0.00201541 0.998778 9.15211 0 0 0 1
13 0.998682 7.09894e-05 -0.0513155 -0.334647 -0.000203775 0.999997 -0.00258241 -0.0938889 0.0513152 0.00258946 0.998679 9.98839 0 0 0 1
14 0.998565 -8.82523e-05 -0.0535542 -0.380835 -9.36659e-06 0.999998 -0.00182255 -0.10173 0.0535542 0.00182044 0.998563 10.832 0 0 0 1
15 0.998481 -0.00146793 -0.0550718 -0.429135 0.0013525 0.999997 -0.00213307 -0.111427 0.0550748 0.00205535 0.99848 11.687 0 0 0 1
16 0.998373 0.000738731 -0.0570218 -0.483426 -0.000993083 0.99999 -0.00443241 -0.122139 0.0570179 0.00448183 0.998363 12.5483 0 0 0 1
17 0.998285 0.00120595 -0.0585258 -0.540056 -0.00162301 0.999974 -0.00707907 -0.132598 0.0585158 0.00716191 0.998261 13.4179 0 0 0 1
18 0.998165 0.00516151 -0.060337 -0.6023 -0.00570195 0.999945 -0.00878826 -0.143753 0.0602883 0.00911617 0.998139 14.2952 0 0 0 1
19 0.998101 0.00610094 -0.0612993 -0.66308 -0.00663017 0.999942 -0.00843386 -0.157854 0.0612443 0.00882427 0.998084 15.1802 0 0 0 1
20 0.998014 0.0052997 -0.0627662 -0.722045 -0.00574767 0.999959 -0.0069587 -0.172847 0.0627268 0.00730564 0.998004 16.074 0 0 0 1
21 0.99792 0.00591748 -0.0641975 -0.78346 -0.00627924 0.999966 -0.00543487 -0.186221 0.0641631 0.00582667 0.997922 16.9738 0 0 0 1
22 0.997857 0.00547694 -0.0651993 -0.845347 -0.00584101 0.999968 -0.00539455 -0.199741 0.0651677 0.00576382 0.997858 17.8786 0 0 0 1
23 0.997737 0.00536917 -0.0670282 -0.908218 -0.00579979 0.999964 -0.0062316 -0.212775 0.0669924 0.00660624 0.997732 18.7877 0 0 0 1
24 0.997663 0.00386695 -0.0682185 -0.971291 -0.00435203 0.999966 -0.00696344 -0.226442 0.0681893 0.00724406 0.997646 19.7046 0 0 0 1
25 0.997629 0.00410637 -0.0687004 -1.03663 -0.00448288 0.999976 -0.00532714 -0.239555 0.0686769 0.00562249 0.997623 20.6257 0 0 0 1
26 0.997617 0.00588773 -0.0687501 -1.10557 -0.0060349 0.99998 -0.00193325 -0.254273 0.0687373 0.00234355 0.997632 21.55 0 0 0 1
27 0.997662 0.00693766 -0.0679906 -1.17297 -0.00682806 0.999975 0.0018442 -0.26563 0.0680017 -0.00137565 0.997684 22.4875 0 0 0 1
28 0.997774 0.00579785 -0.0664343 -1.23728 -0.00550265 0.999974 0.00462554 -0.271962 0.0664594 -0.00424968 0.99778 23.4285 0 0 0 1
29 0.997872 0.00589563 -0.0649408 -1.30214 -0.00556012 0.99997 0.00534586 -0.277922 0.0649704 -0.0049734 0.997875 24.3732 0 0 0 1
30 0.997958 0.00627024 -0.0635595 -1.36462 -0.00612984 0.999978 0.00240374 -0.285335 0.0635732 -0.00200922 0.997975 25.314 0 0 0 1
31 0.998004 0.00714074 -0.0627411 -1.42783 -0.00731158 0.99997 -0.00249375 -0.293171 0.0627215 0.00294751 0.998027 26.2605 0 0 0 1
32 0.99808 0.0063692 -0.0616159 -1.48954 -0.00671918 0.999962 -0.00547459 -0.302321 0.0615787 0.00587809 0.998085 27.2168 0 0 0 1
33 0.99813 0.00376787 -0.0610159 -1.54654 -0.00404632 0.999982 -0.0044408 -0.313516 0.0609981 0.00467938 0.998127 28.1829 0 0 0 1
34 0.998113 0.00193972 -0.0613743 -1.60668 -0.00191171 0.999998 0.000515183 -0.324411 0.0613752 -0.000396881 0.998115 29.1626 0 0 0 1
35 0.99806 -0.0017885 -0.062228 -1.66532 0.00203402 0.99999 0.00388232 -0.335656 0.0622204 -0.00400136 0.998054 30.1428 0 0 0 1
36 0.997945 -0.00917543 -0.0634115 -1.72059 0.00939451 0.999951 0.00315749 -0.343316 0.0633794 -0.00374672 0.997982 31.1244 0 0 0 1
37 0.997825 -0.0112684 -0.0649459 -1.78049 0.011242 0.999937 -0.000771312 -0.350864 0.0649504 3.95099e-05 0.997888 32.1064 0 0 0 1
38 0.997739 -0.0110126 -0.0662983 -1.85007 0.0107254 0.999932 -0.00468596 -0.361068 0.0663454 0.00396429 0.997789 33.0886 0 0 0 1
39 0.997597 -0.00959503 -0.0686163 -1.92119 0.00924037 0.999942 -0.00548426 -0.373466 0.0686649 0.00483704 0.997628 34.0774 0 0 0 1
40 0.99755 -0.0095802 -0.0693031 -1.99331 0.00931271 0.999948 -0.00418184 -0.387047 0.0693396 0.00352619 0.997587 35.0736 0 0 0 1
41 0.997473 -0.00634387 -0.0707596 -2.0707 0.00626661 0.99998 -0.0013139 -0.403858 0.0707665 0.00086716 0.997493 36.0721 0 0 0 1
42 0.99739 -0.00624366 -0.0719343 -2.14553 0.00625582 0.99998 -5.62375e-05 -0.416888 0.0719332 -0.000393917 0.997409 37.0728 0 0 0 1
43 0.997312 -0.00473093 -0.0731254 -2.21909 0.00492848 0.999985 0.00252135 -0.428625 0.0731123 -0.00287497 0.99732 38.0643 0 0 0 1
44 0.997318 -0.00467696 -0.0730348 -2.29215 0.00509473 0.999972 0.00553481 -0.440023 0.0730068 -0.00589206 0.997314 39.0618 0 0 0 1
45 0.997274 0.00138304 -0.0737801 -2.37574 -0.000811217 0.999969 0.00777971 -0.447869 0.0737886 -0.00769865 0.997244 40.0548 0 0 0 1
46 0.997262 0.00149131 -0.0739326 -2.45529 -0.000969511 0.999974 0.00709318 -0.454763 0.0739413 -0.00700208 0.997238 41.0557 0 0 0 1
47 0.997266 0.00175929 -0.0738699 -2.53081 -0.00136899 0.999985 0.00533379 -0.460519 0.0738782 -0.00521809 0.997254 42.0518 0 0 0 1
48 0.997253 0.00408494 -0.0739555 -2.61212 -0.00386552 0.999988 0.00310988 -0.469863 0.0739673 -0.00281546 0.997257 43.0493 0 0 0 1
49 0.997185 0.00365371 -0.0748884 -2.68799 -0.00342799 0.999989 0.00314243 -0.47951 0.0748991 -0.00287687 0.997187 44.0473 0 0 0 1
50 0.997077 0.00181435 -0.0763845 -2.76071 -0.00149292 0.99999 0.00426495 -0.487845 0.0763915 -0.00413845 0.997069 45.0403 0 0 0 1
51 0.997018 0.00246727 -0.0771352 -2.84117 -0.00206285 0.999984 0.00532227 -0.499132 0.0771471 -0.00514727 0.997006 46.0244 0 0 0 1
52 0.996991 0.00504805 -0.0773507 -2.92304 -0.00493379 0.999986 0.00166824 -0.510863 0.0773581 -0.00128158 0.997003 46.994 0 0 0 1
53 0.996911 0.00581773 -0.0783264 -3.00373 -0.00604061 0.999978 -0.00260888 -0.521193 0.0783095 0.00307396 0.996924 47.9551 0 0 0 1
54 0.996846 0.00678413 -0.0790757 -3.08343 -0.00711636 0.999967 -0.00392044 -0.534186 0.0790465 0.00447081 0.996861 48.9236 0 0 0 1
55 0.996843 0.00557268 -0.0792034 -3.16262 -0.00562268 0.999984 -0.000408328 -0.54901 0.0791999 0.000852374 0.996858 49.9005 0 0 0 1
56 0.996831 0.00375007 -0.0794568 -3.23868 -0.00354655 0.99999 0.00270227 -0.563036 0.0794661 -0.0024119 0.996835 50.8752 0 0 0 1
57 0.996805 0.00190455 -0.0798474 -3.31582 -0.00164885 0.999993 0.00326822 -0.574113 0.0798531 -0.00312612 0.996802 51.8394 0 0 0 1
58 0.996782 -0.00124932 -0.0801505 -3.39153 0.00141878 0.999997 0.0020573 -0.586659 0.0801477 -0.00216439 0.996781 52.8005 0 0 0 1
59 0.996745 -0.0038025 -0.0805262 -3.4676 0.0038689 0.999992 0.000668539 -0.59892 0.080523 -0.00097791 0.996752 53.7575 0 0 0 1
60 0.996643 -0.00519016 -0.0817059 -3.54489 0.00535256 0.999984 0.00176869 -0.60864 0.0816955 -0.00220009 0.996655 54.708 0 0 0 1
61 0.996534 -0.0079249 -0.0828082 -3.62139 0.00842977 0.999948 0.00574894 -0.618858 0.0827583 -0.00642707 0.996549 55.6588 0 0 0 1
62 0.996473 -0.00854289 -0.0834829 -3.69959 0.00945654 0.9999 0.0105549 -0.624401 0.0833844 -0.0113071 0.996453 56.6119 0 0 0 1
63 0.996447 -0.00664747 -0.083957 -3.78502 0.00773966 0.99989 0.0126902 -0.629769 0.0838633 -0.0132949 0.996389 57.5607 0 0 0 1
64 0.996335 -0.00522633 -0.0853755 -3.8689 0.00597793 0.999946 0.00855017 -0.636709 0.0853262 -0.0090292 0.996312 58.4941 0 0 0 1
65 0.996221 -0.00343661 -0.0867892 -3.95276 0.00350579 0.999994 0.000644619 -0.644008 0.0867865 -0.000946448 0.996226 59.4131 0 0 0 1
66 0.996144 -0.00149623 -0.0877201 -4.03806 0.00112725 0.99999 -0.00425562 -0.655271 0.0877256 0.00414033 0.996136 60.3236 0 0 0 1
67 0.996055 0.00375138 -0.0886573 -4.12895 -0.00406723 0.999986 -0.00338223 -0.671324 0.0886434 0.00372948 0.996056 61.2274 0 0 0 1
68 0.995922 0.00719305 -0.0899263 -4.21985 -0.0073202 0.999973 -0.00108421 -0.691307 0.089916 0.00173807 0.995948 62.125 0 0 0 1
69 0.99582 0.00967277 -0.0908194 -4.30702 -0.00966905 0.999953 0.000481019 -0.708494 0.0908198 0.000399128 0.995867 63.0134 0 0 0 1
70 0.995713 0.0102896 -0.0919182 -4.39131 -0.0103098 0.999947 0.000255248 -0.721276 0.091916 0.000693502 0.995767 63.8776 0 0 0 1
71 0.99554 0.0119225 -0.0935844 -4.477 -0.0118725 0.999929 0.00109156 -0.734766 0.0935908 2.43836e-05 0.995611 64.7307 0 0 0 1
72 0.995397 0.0126524 -0.0950024 -4.56121 -0.0125521 0.99992 0.00165348 -0.749039 0.0950157 -0.000453392 0.995476 65.5703 0 0 0 1
73 0.995256 0.0126635 -0.0964665 -4.64297 -0.0125254 0.999919 0.00203772 -0.761909 0.0964846 -0.00081977 0.995334 66.3938 0 0 0 1
74 0.995133 0.0127023 -0.0977168 -4.72623 -0.0124698 0.999918 0.00298947 -0.7711 0.0977468 -0.00175641 0.99521 67.2017 0 0 0 1
75 0.994948 0.015548 -0.0991814 -4.81287 -0.0150604 0.999871 0.00566291 -0.780301 0.0992566 -0.0041406 0.995053 67.9995 0 0 0 1
76 0.994794 0.0171065 -0.100462 -4.90076 -0.0162095 0.999821 0.009738 -0.788037 0.100611 -0.00805885 0.994893 68.7922 0 0 0 1

88781
examples/Data/VO_stereo_factors00.txt Normal file
View File

File diff suppressed because it is too large Load Diff

52544
examples/Data/VO_stereo_factors00s.txt Normal file
View File

File diff suppressed because it is too large Load Diff

71
examples/Data/pose3example-grid.txt Normal file
View File

@ -0,0 +1,71 @@
VERTEX_SE3:QUAT 0 1.63791e-12 7.56548e-14 -3.02811e-12 5.35657e-13 2.43616e-13 9.71152e-14 1
VERTEX_SE3:QUAT 1 1.01609 0.00274307 -0.0351514 -0.499545 0.247735 0.723569 -0.406854
VERTEX_SE3:QUAT 2 1.99996 0.0304956 -0.040662 0.403501 -0.294714 -0.4254 0.754563
VERTEX_SE3:QUAT 3 1.94371 1.06535 0.0118614 -0.0471731 -0.541615 0.820893 0.17482
VERTEX_SE3:QUAT 4 0.962753 0.999477 0.0211017 -0.19663 -0.66009 0.470743 0.551379
VERTEX_SE3:QUAT 5 -0.00956768 0.965396 -0.021854 -0.320221 -0.518368 0.47521 0.634766
VERTEX_SE3:QUAT 6 -0.0863793 1.97682 0.000531117 -0.0173439 -0.573793 -0.450627 0.683663
VERTEX_SE3:QUAT 7 0.918905 2.01556 -0.0139773 0.56169 -0.440513 0.199057 0.671438
VERTEX_SE3:QUAT 8 1.92094 2.05524 0.0469884 0.0073084 -0.372357 -0.467582 0.801663
VERTEX_SE3:QUAT 9 1.86182 2.05449 1.09237 0.0131731 -0.05784 0.0335652 0.997674
VERTEX_SE3:QUAT 10 0.880176 2.02406 1.00997 -0.39342 -0.287909 0.757918 0.433462
VERTEX_SE3:QUAT 11 -0.0960463 1.98653 0.995791 0.434103 -0.199044 0.585176 0.655367
VERTEX_SE3:QUAT 12 -0.0911401 0.997117 0.988217 -0.0925477 0.572872 0.537294 0.612019
VERTEX_SE3:QUAT 13 0.948316 1.02239 0.991745 0.142484 0.560062 0.750078 0.321578
VERTEX_SE3:QUAT 14 1.92631 1.08945 1.06749 0.23878 0.380837 0.796564 -0.404269
VERTEX_SE3:QUAT 15 1.95398 0.0777667 0.982353 -0.384392 0.58733 0.685207 -0.194366
VERTEX_SE3:QUAT 16 0.946032 0.0482667 0.952308 -0.218979 0.186315 -0.494185 0.820437
VERTEX_SE3:QUAT 17 -0.0625076 -0.034424 0.942171 0.514725 -0.185043 -0.44771 0.707371
VERTEX_SE3:QUAT 18 -0.083807 -0.0106666 1.9853 0.00792651 1.98919e-05 -0.00128106 0.999968
VERTEX_SE3:QUAT 19 0.918067 -0.000897795 1.92157 -0.342141 0.241241 -0.726975 0.544288
VERTEX_SE3:QUAT 20 1.90041 0.0323631 2.00636 0.412572 -0.0930131 -0.133075 0.896339
VERTEX_SE3:QUAT 21 1.84895 1.05013 2.0738 -0.580757 0.35427 0.729393 -0.0721062
VERTEX_SE3:QUAT 22 0.880221 1.00671 1.99021 0.147752 0.355662 0.917953 0.095058
VERTEX_SE3:QUAT 23 -0.0950872 1.00374 1.95013 -0.29909 -0.0578461 0.857019 0.415594
VERTEX_SE3:QUAT 24 -0.111581 1.97979 1.98762 0.565153 0.214463 -0.523058 0.600848
VERTEX_SE3:QUAT 25 0.837568 2.01589 2.03075 -0.284756 0.369992 0.875484 -0.124692
VERTEX_SE3:QUAT 26 1.82708 2.05081 2.07052 0.254696 0.250865 0.653216 0.667462
EDGE_SE3:QUAT 0 1 1.00497 0.002077 -0.015539 -0.508004 0.250433 0.711222 -0.416386 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 1 2 -0.200593 0.339956 -0.908079 -0.093598 0.151993 0.42829 0.885836 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 2 3 -0.922791 0.330629 -0.292682 0.365657 -0.051986 0.924849 -0.090813 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 3 4 0.893075 0.246476 0.331154 -0.285927 0.341221 -0.267609 0.854517 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 4 5 0.280674 0.244242 0.923726 0.035064 0.21101 0.083834 0.973251 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 5 6 0.955621 0.355669 -0.025152 -0.306713 0.131221 -0.781587 0.527096 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 6 7 -0.076631 0.636081 -0.771439 0.702021 0.326514 0.122181 0.620988 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 7 8 0.582761 -0.721177 -0.376875 -0.733841 -0.170725 -0.256653 0.605359 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 8 9 0.600312 0.298765 0.767014 0.057612 0.332574 0.486324 0.805956 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 9 10 -0.986649 0.03008 -0.008766 -0.362177 -0.253215 0.763748 0.470531 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 10 11 0.275109 0.534769 0.823463 0.450708 -0.472399 -0.432689 0.621677 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 11 12 -0.61882 0.024878 0.773748 0.0927029 0.786162 -0.21122 0.573359 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 12 13 -0.175537 -0.730832 0.634529 -0.018628 0.006375 0.428306 0.903419 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 13 14 -0.700208 -0.245198 0.637353 -0.035865 0.273394 0.645363 0.712374 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 14 15 0.373495 0.373768 -0.846199 0.400323 0.310362 -0.422222 0.751762 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 15 16 0.648588 0.157829 0.72252 0.781502 -0.210141 -0.501005 -0.30674 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 16 17 -0.390339 -0.702656 -0.572321 0.765815 0.055816 0.032478 0.63981 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 17 18 -0.261114 0.908685 0.421318 -0.501833 0.166567 0.448468 0.720622 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 18 19 1.00815 0.012634 -0.029822 -0.347007 0.205082 -0.740641 0.537569 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 19 20 -0.162376 0.581623 0.810804 0.628338 0.075411 0.650639 0.41973 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 20 21 -0.358942 0.627689 -0.704045 -0.469133 0.542456 0.530583 -0.451816 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 21 22 0.362417 0.298352 0.854822 0.004058 -0.696926 0.140345 0.703265 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 22 23 0.934942 0.020321 -0.358044 -0.445461 0.260916 -0.379862 0.767589 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 23 24 0.741887 -0.657659 0.215293 -0.584859 0.196138 0.688031 0.38221 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 24 25 0.300145 0.82011 -0.39974 0.46538 -0.593595 -0.202131 0.624668 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 25 26 -0.85591 0.022701 -0.510794 0.12929 -0.685192 -0.503707 0.509978 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 0 5 0.026721 0.990497 -0.007651 -0.317476 -0.510239 0.467341 0.648427 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 3 8 0.390516 -0.401461 -0.830724 0.503106 -0.367814 0.780584 0.047806 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 4 1 -0.813838 -0.446181 0.319175 0.224903 -0.031827 0.97265 0.048561 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 4 13 0.571273 -0.805401 0.077339 0.892031 0.329761 0.275468 0.140201 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 5 12 0.389794 -0.882655 0.268063 0.712423 0.550662 0.275339 0.33677 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 6 11 0.800298 0.505022 0.361738 0.739335 0.419366 0.443817 0.283801 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 10 13 -0.912531 0.430955 -0.018942 0.830493 -0.093519 0.272041 0.477001 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 12 23 -0.797606 0.437737 0.311476 -0.657137 -0.196625 0.136652 0.714728 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 13 22 -0.116836 0.952032 0.269398 -0.216437 0.086571 0.260965 0.936781 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 14 21 0.749295 0.373389 0.581641 0.253048 0.511007 -0.537262 0.621439 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 16 1 0.160985 0.555966 -0.811911 0.748057 0.122381 -0.369631 0.537407 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 18 23 0.028909 1.02689 -0.00265 -0.294167 -0.071607 0.850901 0.429308 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 19 16 -0.230711 0.750637 -0.607511 0.14647 -0.102538 0.297899 0.937704 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 20 15 -0.031986 -0.741129 -0.728721 -0.278926 0.731172 0.404675 -0.473103 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 22 19 -0.332601 0.704401 -0.687251 -0.372165 -0.054346 0.713024 0.591725 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 22 25 0.347067 -0.634646 0.657147 0.018567 0.476762 0.040939 0.877882 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 25 10 0.388971 -0.723981 -0.559653 -0.373459 -0.014654 -0.696123 0.612965 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 26 21 -0.979482 -0.024822 0.043763 -0.326753 0.819942 0.292615 0.367837 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400

3
examples/Data/pose3example-offdiagonal-rewritten.txt Normal file
View File

@ -0,0 +1,3 @@
VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1
VERTEX_SE3:QUAT 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423
EDGE_SE3:QUAT 0 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 10000 1 1 1 1 1 10000 2 2 2 2 10000 3 3 3 10000 4 4 10000 5 10000

3
examples/Data/pose3example-offdiagonal.txt Normal file
View File

@ -0,0 +1,3 @@
VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
VERTEX_SE3:QUAT 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230
EDGE_SE3:QUAT 0 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230 10000.000000 1.000000 1.000000 1.000000 1.000000 1.000000 10000.000000 2.000000 2.000000 2.000000 2.000000 10000.000000 3.000000 3.000000 3.000000 10000.000000 4.000000 4.000000 10000.000000 5.000000 10000.0000

11
examples/Data/pose3example.txt Normal file
View File

@ -0,0 +1,11 @@
VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
VERTEX_SE3:QUAT 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230
VERTEX_SE3:QUAT 2 1.993500 0.023275 0.003793 -0.351729 -0.597838 0.584174 0.421446
VERTEX_SE3:QUAT 3 2.004291 1.024305 0.018047 0.331798 -0.200659 0.919323 0.067024
VERTEX_SE3:QUAT 4 0.999908 1.055073 0.020212 -0.035697 -0.462490 0.445933 0.765488
EDGE_SE3:QUAT 0 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000
EDGE_SE3:QUAT 1 2 0.523923 0.776654 0.326659 0.311512 0.656877 -0.678505 0.105373 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000
EDGE_SE3:QUAT 2 3 0.910927 0.055169 -0.411761 0.595795 -0.561677 0.079353 0.568551 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000
EDGE_SE3:QUAT 3 4 0.775288 0.228798 -0.596923 -0.592077 0.303380 -0.513226 0.542221 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000
EDGE_SE3:QUAT 1 4 -0.577841 0.628016 -0.543592 -0.125250 -0.534379 0.769122 0.327419 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000
EDGE_SE3:QUAT 3 0 -0.623267 0.086928 0.773222 0.104639 0.627755 0.766795 0.083672 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000

11
examples/Data/simpleGraph10gradIter.txt Normal file
View File

@ -0,0 +1,11 @@
VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.0008187 0.0011723 0.0895466 0.9959816
VERTEX_SE3:QUAT 1 0.000000 -0.000000 0.000000 0.0010673 0.0015636 0.1606931 0.9870026
VERTEX_SE3:QUAT 2 -0.388822 0.632954 0.001223 0.0029920 0.0014066 0.0258235 0.9996610
VERTEX_SE3:QUAT 3 -1.143204 0.050638 0.006026 -0.0012800 -0.0002767 -0.2850291 0.9585180
VERTEX_SE3:QUAT 4 -0.512416 0.486441 0.005171 0.0002681 0.0023574 0.0171476 0.9998502
EDGE_SE3:QUAT 1 2 1.000000 2.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
EDGE_SE3:QUAT 2 3 -0.000000 1.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
EDGE_SE3:QUAT 3 4 1.000000 1.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
EDGE_SE3:QUAT 3 1 0.000001 2.000000 0.000000 0.0000000 0.0000000 1.0000000 0.0000002 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
EDGE_SE3:QUAT 1 4 -1.000000 1.000000 0.000000 0.0000000 0.0000000 -0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
EDGE_SE3:QUAT 0 1 0.000000 0.000000 0.000000 0.0000000 0.0000000 0.0000000 1.0000000 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000

10
examples/LocalizationExample.cpp
View File

@ -120,15 +120,15 @@ int main(int argc, char** argv) {
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise));
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise));
// 2b. Add "GPS-like" measurements
// We will use our custom UnaryFactor for this.
noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); // 10cm std on x,y
graph.push_back(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
graph.push_back(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
graph.push_back(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
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));
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution

6
examples/OdometryExample.cpp
View File

@ -65,15 +65,15 @@ int main(int argc, char** argv) {
// A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
graph.push_back(PriorFactor<Pose2>(1, priorMean, priorNoise));
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
// Add odometry factors
Pose2 odometry(2.0, 0.0, 0.0);
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.push_back(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.push_back(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
graph.print("\nFactor Graph:\n"); // print
// Create the data structure to hold the initialEstimate estimate to the solution

12
examples/PlanarSLAMExample.cpp
View File

@ -81,13 +81,13 @@ int main(int argc, char** argv) {
// Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.push_back(PriorFactor<Pose2>(x1, prior, priorNoise)); // add directly to graph
graph.add(PriorFactor<Pose2>(x1, prior, priorNoise)); // add directly to graph
// Add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
graph.push_back(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
graph.push_back(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise));
// Add Range-Bearing measurements to two different landmarks
// create a noise model for the landmark measurements
@ -101,9 +101,9 @@ int main(int argc, char** argv) {
range32 = 2.0;
// Add Bearing-Range factors
graph.push_back(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise));
graph.push_back(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise));
graph.push_back(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise));
graph.add(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise));
graph.add(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise));
graph.add(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise));
// Print
graph.print("Factor Graph:\n");

12
examples/Pose2SLAMExample.cpp
View File

@ -72,23 +72,23 @@ int main(int argc, char** argv) {
// 2a. Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
graph.push_back(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
// 2b. Add odometry factors
// Create odometry (Between) factors between consecutive poses
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
graph.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
graph.push_back(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
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));
// 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.push_back(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));
graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution

54
examples/Pose2SLAMExample_g2o.cpp
View File

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

89
examples/Pose3SLAMExample_changeKeys.cpp Normal file
View File

@ -0,0 +1,89 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Pose3SLAMExample_initializePose3.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
* Syntax for the script is ./Pose3SLAMExample_changeKeys input.g2o rewritted.g2o
* @date Aug 25, 2014
* @author Luca Carlone
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <fstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = findExampleDataFile("pose3example.txt");
else
g2oFile = argv[1];
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = true;
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
bool add = false;
Key firstKey = 8646911284551352320;
std::cout << "Using reference key: " << firstKey << std::endl;
if(add)
std::cout << "adding key " << std::endl;
else
std::cout << "subtracting key " << std::endl;
if (argc < 3) {
std::cout << "Please provide output file to write " << std::endl;
} else {
const string inputFileRewritten = argv[2];
std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
// Additional: rewrite input with simplified keys 0,1,...
Values simpleInitial;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
Key key;
if(add)
key = key_value.key + firstKey;
else
key = key_value.key - firstKey;
simpleInitial.insert(key, initial->at(key_value.key));
}
NonlinearFactorGraph simpleGraph;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, *graph) {
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between){
Key key1, key2;
if(add){
key1 = pose3Between->key1() + firstKey;
key2 = pose3Between->key2() + firstKey;
}else{
key1 = pose3Between->key1() - firstKey;
key2 = pose3Between->key2() - firstKey;
}
NonlinearFactor::shared_ptr simpleFactor(
new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->get_noiseModel()));
simpleGraph.add(simpleFactor);
}
}
writeG2o(simpleGraph, simpleInitial, inputFileRewritten);
}
return 0;
}

74
examples/Pose3SLAMExample_g2o.cpp Normal file
View File

@ -0,0 +1,74 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Pose3SLAMExample_initializePose3.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
* @date Aug 25, 2014
* @author Luca Carlone
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <fstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = findExampleDataFile("pose3example.txt");
else
g2oFile = argv[1];
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = true;
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
break;
}
std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonParams params;
params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params);
Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl;
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
std::cout << "final error=" <<graph->error(result)<< std::endl;
if (argc < 3) {
result.print("result");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, result, outputFile);
std::cout << "done! " << std::endl;
}
return 0;
}

68
examples/Pose3SLAMExample_initializePose3Chordal.cpp Normal file
View File

@ -0,0 +1,68 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Pose3SLAMExample_initializePose3.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
* @date Aug 25, 2014
* @author Luca Carlone
*/
#include <gtsam/slam/InitializePose3.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <fstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = findExampleDataFile("pose3example.txt");
else
g2oFile = argv[1];
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = true;
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
break;
}
std::cout << "Initializing Pose3 - chordal relaxation" << std::endl;
Values initialization = InitializePose3::initialize(graphWithPrior);
std::cout << "done!" << std::endl;
if (argc < 3) {
initialization.print("initialization");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, initialization, outputFile);
std::cout << "done! " << std::endl;
}
return 0;
}

72
examples/Pose3SLAMExample_initializePose3Gradient.cpp Normal file
View File

@ -0,0 +1,72 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Pose3SLAMExample_initializePose3.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
* @date Aug 25, 2014
* @author Luca Carlone
*/
#include <gtsam/slam/InitializePose3.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <fstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = findExampleDataFile("pose3example.txt");
else
g2oFile = argv[1];
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = true;
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
break;
}
std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl;
bool useGradient = true;
Values initialization = InitializePose3::initialize(graphWithPrior, *initial, useGradient);
std::cout << "done!" << std::endl;
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
std::cout << "initialization error=" <<graph->error(initialization)<< std::endl;
if (argc < 3) {
initialization.print("initialization");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, initialization, outputFile);
std::cout << "done! " << std::endl;
}
return 0;
}

16
examples/SolverComparer.cpp
View File

@ -13,6 +13,22 @@
* @brief Incremental and batch solving, timing, and accuracy comparisons
* @author Richard Roberts
* @date August, 2013
*
* Here is an example. Below, to run in batch mode, we first generate an initialization in incremental mode.
*
* Solve in incremental and write to file w_inc:
* ./SolverComparer --incremental -d w10000 -o w_inc
*
* You can then perturb that initialization to get batch something to optimize.
* Read in w_inc, perturb it with noise of stddev 0.6, and write to w_pert:
* ./SolverComparer --perturb 0.6 -i w_inc -o w_pert
*
* Then optimize with batch, read in w_pert, solve in batch, and write to w_batch:
* ./SolverComparer --batch -d w10000 -i w_pert -o w_batch
*
* And finally compare solutions in w_inc and w_batch to check that batch converged to the global minimum
* ./SolverComparer --compare w_inc w_batch
*
*/
#include <gtsam/base/timing.h>

43
examples/VisualISAMExample.cpp
View File

@ -14,6 +14,7 @@
* @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset
* This version uses iSAM to solve the problem incrementally
* @author Duy-Nguyen Ta
* @author Frank Dellaert
*/
/**
@ -61,7 +62,8 @@ int main(int argc, char* argv[]) {
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
// Define the camera observation noise model
noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
noiseModel::Isotropic::shared_ptr noise = //
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks
vector<Point3> points = createPoints();
@ -69,7 +71,8 @@ int main(int argc, char* argv[]) {
// Create the set of ground-truth poses
vector<Pose3> poses = createPoses();
// Create a NonlinearISAM object which will relinearize and reorder the variables every "relinearizeInterval" updates
// Create a NonlinearISAM object which will relinearize and reorder the variables
// every "relinearizeInterval" updates
int relinearizeInterval = 3;
NonlinearISAM isam(relinearizeInterval);
@ -82,32 +85,44 @@ int main(int argc, char* argv[]) {
// Add factors for each landmark observation
for (size_t j = 0; j < points.size(); ++j) {
// Create ground truth measurement
SimpleCamera camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
// Add measurement
graph.add(
GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, noise,
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::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
Pose3 noise(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
Pose3 initial_xi = poses[i].compose(noise);
// Add an initial guess for the current pose
initialEstimate.insert(Symbol('x', i), initial_xi);
// If this is the first iteration, add a prior on the first pose to set the coordinate frame
// and a prior on the first landmark to set the scale
// 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))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
if (i == 0) {
// Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)));
graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
// Add a prior on landmark l0
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
noiseModel::Isotropic::shared_ptr pointNoise =
noiseModel::Isotropic::Sigma(3, 0.1);
graph.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise));
// Add initial guesses to all observed landmarks
// Intentionally initialize the variables off from the ground truth
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
Point3 noise(-0.25, 0.20, 0.15);
for (size_t j = 0; j < points.size(); ++j) {
// Intentionally initialize the variables off from the ground truth
Point3 initial_lj = points[j].compose(noise);
initialEstimate.insert(Symbol('l', j), initial_lj);
}
} else {
// Update iSAM with the new factors

8
gtsam/3rdparty/Eigen/CTestConfig.cmake vendored
View File

@ -4,14 +4,10 @@
## # The following are required to uses Dart and the Cdash dashboard
## ENABLE_TESTING()
## INCLUDE(CTest)
set(CTEST_PROJECT_NAME "Eigen")
set(CTEST_PROJECT_NAME "Eigen3.2")
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=Eigen")
set(CTEST_DROP_LOCATION "/CDash/submit.php?project=Eigen3.2")
set(CTEST_DROP_SITE_CDASH TRUE)
set(CTEST_PROJECT_SUBPROJECTS
Official
Unsupported
)

4
gtsam/3rdparty/Eigen/Eigen/Core vendored
View File

@ -95,7 +95,7 @@
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:
#ifdef __INTEL_COMPILER
#if defined(__INTEL_COMPILER) && __INTEL_COMPILER >= 1110
#include <immintrin.h>
#else
#include <emmintrin.h>
@ -165,7 +165,7 @@
#endif
// required for __cpuid, needs to be included after cmath
#if defined(_MSC_VER) && (defined(_M_IX86)||defined(_M_X64))
#if defined(_MSC_VER) && (defined(_M_IX86)||defined(_M_X64)) && (!defined(_WIN32_WCE))
#include <intrin.h>
#endif

50
gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h vendored
View File

@ -274,30 +274,13 @@ template<> struct ldlt_inplace<Lower>
return true;
}
RealScalar cutoff(0), biggest_in_corner;
for (Index k = 0; k < size; ++k)
{
// Find largest diagonal element
Index index_of_biggest_in_corner;
biggest_in_corner = mat.diagonal().tail(size-k).cwiseAbs().maxCoeff(&index_of_biggest_in_corner);
mat.diagonal().tail(size-k).cwiseAbs().maxCoeff(&index_of_biggest_in_corner);
index_of_biggest_in_corner += k;
if(k == 0)
{
// The biggest overall is the point of reference to which further diagonals
// are compared; if any diagonal is negligible compared
// to the largest overall, the algorithm bails.
cutoff = abs(NumTraits<Scalar>::epsilon() * biggest_in_corner);
}
// Finish early if the matrix is not full rank.
if(biggest_in_corner < cutoff)
{
for(Index i = k; i < size; i++) transpositions.coeffRef(i) = i;
break;
}
transpositions.coeffRef(k) = index_of_biggest_in_corner;
if(k != index_of_biggest_in_corner)
{
@ -328,15 +311,20 @@ template<> struct ldlt_inplace<Lower>
if(k>0)
{
temp.head(k) = mat.diagonal().head(k).asDiagonal() * A10.adjoint();
temp.head(k) = mat.diagonal().real().head(k).asDiagonal() * A10.adjoint();
mat.coeffRef(k,k) -= (A10 * temp.head(k)).value();
if(rs>0)
A21.noalias() -= A20 * temp.head(k);
}
if((rs>0) && (abs(mat.coeffRef(k,k)) > cutoff))
A21 /= mat.coeffRef(k,k);
// In some previous versions of Eigen (e.g., 3.2.1), the scaling was omitted if the pivot
// was smaller than the cutoff value. However, soince LDLT is not rank-revealing
// we should only make sure we do not introduce INF or NaN values.
// LAPACK also uses 0 as the cutoff value.
RealScalar realAkk = numext::real(mat.coeffRef(k,k));
if((rs>0) && (abs(realAkk) > RealScalar(0)))
A21 /= realAkk;
if (sign == PositiveSemiDef) {
if (realAkk < 0) sign = Indefinite;
} else if (sign == NegativeSemiDef) {
@ -516,14 +504,20 @@ struct solve_retval<LDLT<_MatrixType,_UpLo>, Rhs>
typedef typename LDLTType::MatrixType MatrixType;
typedef typename LDLTType::Scalar Scalar;
typedef typename LDLTType::RealScalar RealScalar;
const Diagonal<const MatrixType> vectorD = dec().vectorD();
RealScalar tolerance = (max)(vectorD.array().abs().maxCoeff() * NumTraits<Scalar>::epsilon(),
RealScalar(1) / NumTraits<RealScalar>::highest()); // motivated by LAPACK's xGELSS
const typename Diagonal<const MatrixType>::RealReturnType vectorD(dec().vectorD());
// In some previous versions, tolerance was set to the max of 1/highest and the maximal diagonal entry * epsilon
// as motivated by LAPACK's xGELSS:
// RealScalar tolerance = (max)(vectorD.array().abs().maxCoeff() *NumTraits<RealScalar>::epsilon(),RealScalar(1) / NumTraits<RealScalar>::highest());
// However, LDLT is not rank revealing, and so adjusting the tolerance wrt to the highest
// diagonal element is not well justified and to numerical issues in some cases.
// Moreover, Lapack's xSYTRS routines use 0 for the tolerance.
RealScalar tolerance = RealScalar(1) / NumTraits<RealScalar>::highest();
for (Index i = 0; i < vectorD.size(); ++i) {
if(abs(vectorD(i)) > tolerance)
dst.row(i) /= vectorD(i);
dst.row(i) /= vectorD(i);
else
dst.row(i).setZero();
dst.row(i).setZero();
}
// dst = L^-T (D^-1 L^-1 P b)
@ -576,7 +570,7 @@ MatrixType LDLT<MatrixType,_UpLo>::reconstructedMatrix() const
// L^* P
res = matrixU() * res;
// D(L^*P)
res = vectorD().asDiagonal() * res;
res = vectorD().real().asDiagonal() * res;
// L(DL^*P)
res = matrixL() * res;
// P^T (LDL^*P)

2
gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h vendored
View File

@ -81,7 +81,7 @@ struct traits<Block<XprType, BlockRows, BlockCols, InnerPanel> > : traits<XprTyp
&& (InnerStrideAtCompileTime == 1)
? PacketAccessBit : 0,
MaskAlignedBit = (InnerPanel && (OuterStrideAtCompileTime!=Dynamic) && (((OuterStrideAtCompileTime * int(sizeof(Scalar))) % 16) == 0)) ? AlignedBit : 0,
FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1) ? LinearAccessBit : 0,
FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1 || (InnerPanel && (traits<XprType>::Flags&LinearAccessBit))) ? LinearAccessBit : 0,
FlagsLvalueBit = is_lvalue<XprType>::value ? LvalueBit : 0,
FlagsRowMajorBit = IsRowMajor ? RowMajorBit : 0,
Flags0 = traits<XprType>::Flags & ( (HereditaryBits & ~RowMajorBit) |

11
gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h vendored
View File

@ -47,6 +47,17 @@ struct CommaInitializer :
m_xpr.block(0, 0, other.rows(), other.cols()) = other;
}
/* Copy/Move constructor which transfers ownership. This is crucial in
* absence of return value optimization to avoid assertions during destruction. */
// FIXME in C++11 mode this could be replaced by a proper RValue constructor
inline CommaInitializer(const CommaInitializer& o)
: m_xpr(o.m_xpr), m_row(o.m_row), m_col(o.m_col), m_currentBlockRows(o.m_currentBlockRows) {
// Mark original object as finished. In absence of R-value references we need to const_cast:
const_cast<CommaInitializer&>(o).m_row = m_xpr.rows();
const_cast<CommaInitializer&>(o).m_col = m_xpr.cols();
const_cast<CommaInitializer&>(o).m_currentBlockRows = 0;
}
/* inserts a scalar value in the target matrix */
CommaInitializer& operator,(const Scalar& s)
{

154
gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h.orig vendored Normal file
View File

@ -0,0 +1,154 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// 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_COMMAINITIALIZER_H
#define EIGEN_COMMAINITIALIZER_H
namespace Eigen {
/** \class CommaInitializer
* \ingroup Core_Module
*
* \brief Helper class used by the comma initializer operator
*
* This class is internally used to implement the comma initializer feature. It is
* the return type of MatrixBase::operator<<, and most of the time this is the only
* way it is used.
*
* \sa \ref MatrixBaseCommaInitRef "MatrixBase::operator<<", CommaInitializer::finished()
*/
template<typename XprType>
struct CommaInitializer
{
typedef typename XprType::Scalar Scalar;
typedef typename XprType::Index Index;
inline CommaInitializer(XprType& xpr, const Scalar& s)
: m_xpr(xpr), m_row(0), m_col(1), m_currentBlockRows(1)
{
m_xpr.coeffRef(0,0) = s;
}
template<typename OtherDerived>
inline CommaInitializer(XprType& xpr, const DenseBase<OtherDerived>& other)
: m_xpr(xpr), m_row(0), m_col(other.cols()), m_currentBlockRows(other.rows())
{
m_xpr.block(0, 0, other.rows(), other.cols()) = other;
}
/* Copy/Move constructor which transfers ownership. This is crucial in
* absence of return value optimization to avoid assertions during destruction. */
// FIXME in C++11 mode this could be replaced by a proper RValue constructor
inline CommaInitializer(const CommaInitializer& o)
: m_xpr(o.m_xpr), m_row(o.m_row), m_col(o.m_col), m_currentBlockRows(o.m_currentBlockRows) {
// Mark original object as finished. In absence of R-value references we need to const_cast:
const_cast<CommaInitializer&>(o).m_row = m_xpr.rows();
const_cast<CommaInitializer&>(o).m_col = m_xpr.cols();
const_cast<CommaInitializer&>(o).m_currentBlockRows = 0;
}
/* inserts a scalar value in the target matrix */
CommaInitializer& operator,(const Scalar& s)
{
if (m_col==m_xpr.cols())
{
m_row+=m_currentBlockRows;
m_col = 0;
m_currentBlockRows = 1;
eigen_assert(m_row<m_xpr.rows()
&& "Too many rows passed to comma initializer (operator<<)");
}
eigen_assert(m_col<m_xpr.cols()
&& "Too many coefficients passed to comma initializer (operator<<)");
eigen_assert(m_currentBlockRows==1);
m_xpr.coeffRef(m_row, m_col++) = s;
return *this;
}
/* inserts a matrix expression in the target matrix */
template<typename OtherDerived>
CommaInitializer& operator,(const DenseBase<OtherDerived>& other)
{
if(other.cols()==0 || other.rows()==0)
return *this;
if (m_col==m_xpr.cols())
{
m_row+=m_currentBlockRows;
m_col = 0;
m_currentBlockRows = other.rows();
eigen_assert(m_row+m_currentBlockRows<=m_xpr.rows()
&& "Too many rows passed to comma initializer (operator<<)");
}
eigen_assert(m_col<m_xpr.cols()
&& "Too many coefficients passed to comma initializer (operator<<)");
eigen_assert(m_currentBlockRows==other.rows());
if (OtherDerived::SizeAtCompileTime != Dynamic)
m_xpr.template block<OtherDerived::RowsAtCompileTime != Dynamic ? OtherDerived::RowsAtCompileTime : 1,
OtherDerived::ColsAtCompileTime != Dynamic ? OtherDerived::ColsAtCompileTime : 1>
(m_row, m_col) = other;
else
m_xpr.block(m_row, m_col, other.rows(), other.cols()) = other;
m_col += other.cols();
return *this;
}
inline ~CommaInitializer()
{
eigen_assert((m_row+m_currentBlockRows) == m_xpr.rows()
&& m_col == m_xpr.cols()
&& "Too few coefficients passed to comma initializer (operator<<)");
}
/** \returns the built matrix once all its coefficients have been set.
* Calling finished is 100% optional. Its purpose is to write expressions
* like this:
* \code
* quaternion.fromRotationMatrix((Matrix3f() << axis0, axis1, axis2).finished());
* \endcode
*/
inline XprType& finished() { return m_xpr; }
XprType& m_xpr; // target expression
Index m_row; // current row id
Index m_col; // current col id
Index m_currentBlockRows; // current block height
};
/** \anchor MatrixBaseCommaInitRef
* Convenient operator to set the coefficients of a matrix.
*
* The coefficients must be provided in a row major order and exactly match
* the size of the matrix. Otherwise an assertion is raised.
*
* Example: \include MatrixBase_set.cpp
* Output: \verbinclude MatrixBase_set.out
*
* \note According the c++ standard, the argument expressions of this comma initializer are evaluated in arbitrary order.
*
* \sa CommaInitializer::finished(), class CommaInitializer
*/
template<typename Derived>
inline CommaInitializer<Derived> DenseBase<Derived>::operator<< (const Scalar& s)
{
return CommaInitializer<Derived>(*static_cast<Derived*>(this), s);
}
/** \sa operator<<(const Scalar&) */
template<typename Derived>
template<typename OtherDerived>
inline CommaInitializer<Derived>
DenseBase<Derived>::operator<<(const DenseBase<OtherDerived>& other)
{
return CommaInitializer<Derived>(*static_cast<Derived *>(this), other);
}
} // end namespace Eigen
#endif // EIGEN_COMMAINITIALIZER_H

16
gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h vendored
View File

@ -24,6 +24,14 @@ namespace internal {
struct constructor_without_unaligned_array_assert {};
template<typename T, int Size> void check_static_allocation_size()
{
// if EIGEN_STACK_ALLOCATION_LIMIT is defined to 0, then no limit
#if EIGEN_STACK_ALLOCATION_LIMIT
EIGEN_STATIC_ASSERT(Size * sizeof(T) <= EIGEN_STACK_ALLOCATION_LIMIT, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG);
#endif
}
/** \internal
* Static array. If the MatrixOrArrayOptions require auto-alignment, the array will be automatically aligned:
* to 16 bytes boundary if the total size is a multiple of 16 bytes.
@ -38,12 +46,12 @@ struct plain_array
plain_array()
{
EIGEN_STATIC_ASSERT(Size * sizeof(T) <= 128 * 128 * 8, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG);
check_static_allocation_size<T,Size>();
}
plain_array(constructor_without_unaligned_array_assert)
{
EIGEN_STATIC_ASSERT(Size * sizeof(T) <= 128 * 128 * 8, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG);
check_static_allocation_size<T,Size>();
}
};
@ -76,12 +84,12 @@ struct plain_array<T, Size, MatrixOrArrayOptions, 16>
plain_array()
{
EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(0xf);
EIGEN_STATIC_ASSERT(Size * sizeof(T) <= 128 * 128 * 8, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG);
check_static_allocation_size<T,Size>();
}
plain_array(constructor_without_unaligned_array_assert)
{
EIGEN_STATIC_ASSERT(Size * sizeof(T) <= 128 * 128 * 8, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG);
check_static_allocation_size<T,Size>();
}
};

4
gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h vendored
View File

@ -589,7 +589,7 @@ struct linspaced_op_impl<Scalar,true>
template<typename Index>
EIGEN_STRONG_INLINE const Packet packetOp(Index i) const
{ return internal::padd(m_lowPacket, pmul(m_stepPacket, padd(pset1<Packet>(i),m_interPacket))); }
{ return internal::padd(m_lowPacket, pmul(m_stepPacket, padd(pset1<Packet>(Scalar(i)),m_interPacket))); }
const Scalar m_low;
const Scalar m_step;
@ -609,7 +609,7 @@ template <typename Scalar, bool RandomAccess> struct functor_traits< linspaced_o
template <typename Scalar, bool RandomAccess> struct linspaced_op
{
typedef typename packet_traits<Scalar>::type Packet;
linspaced_op(const Scalar& low, const Scalar& high, DenseIndex num_steps) : impl((num_steps==1 ? high : low), (num_steps==1 ? Scalar() : (high-low)/(num_steps-1))) {}
linspaced_op(const Scalar& low, const Scalar& high, DenseIndex num_steps) : impl((num_steps==1 ? high : low), (num_steps==1 ? Scalar() : (high-low)/Scalar(num_steps-1))) {}
template<typename Index>
EIGEN_STRONG_INLINE const Scalar operator() (Index i) const { return impl(i); }

2
gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h vendored
View File

@ -237,6 +237,8 @@ template<typename Derived> class MapBase<Derived, WriteAccessors>
using Base::Base::operator=;
};
#undef EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS
} // end namespace Eigen
#endif // EIGEN_MAPBASE_H

10
gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h vendored
View File

@ -101,7 +101,7 @@ struct traits<Ref<_PlainObjectType, _Options, _StrideType> >
template<typename Derived> struct match {
enum {
HasDirectAccess = internal::has_direct_access<Derived>::ret,
StorageOrderMatch = PlainObjectType::IsVectorAtCompileTime || ((PlainObjectType::Flags&RowMajorBit)==(Derived::Flags&RowMajorBit)),
StorageOrderMatch = PlainObjectType::IsVectorAtCompileTime || Derived::IsVectorAtCompileTime || ((PlainObjectType::Flags&RowMajorBit)==(Derived::Flags&RowMajorBit)),
InnerStrideMatch = int(StrideType::InnerStrideAtCompileTime)==int(Dynamic)
|| int(StrideType::InnerStrideAtCompileTime)==int(Derived::InnerStrideAtCompileTime)
|| (int(StrideType::InnerStrideAtCompileTime)==0 && int(Derived::InnerStrideAtCompileTime)==1),
@ -172,8 +172,12 @@ protected:
}
else
::new (static_cast<Base*>(this)) Base(expr.data(), expr.rows(), expr.cols());
::new (&m_stride) StrideBase(StrideType::OuterStrideAtCompileTime==0?0:expr.outerStride(),
StrideType::InnerStrideAtCompileTime==0?0:expr.innerStride());
if(Expression::IsVectorAtCompileTime && (!PlainObjectType::IsVectorAtCompileTime) && ((Expression::Flags&RowMajorBit)!=(PlainObjectType::Flags&RowMajorBit)))
::new (&m_stride) StrideBase(expr.innerStride(), StrideType::InnerStrideAtCompileTime==0?0:1);
else
::new (&m_stride) StrideBase(StrideType::OuterStrideAtCompileTime==0?0:expr.outerStride(),
StrideType::InnerStrideAtCompileTime==0?0:expr.innerStride());
}
StrideBase m_stride;

8
gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h vendored
View File

@ -278,21 +278,21 @@ template<typename _MatrixType, unsigned int _Mode> class TriangularView
/** Efficient triangular matrix times vector/matrix product */
template<typename OtherDerived>
TriangularProduct<Mode,true,MatrixType,false,OtherDerived, OtherDerived::IsVectorAtCompileTime>
TriangularProduct<Mode, true, MatrixType, false, OtherDerived, OtherDerived::ColsAtCompileTime==1>
operator*(const MatrixBase<OtherDerived>& rhs) const
{
return TriangularProduct
<Mode,true,MatrixType,false,OtherDerived,OtherDerived::IsVectorAtCompileTime>
<Mode, true, MatrixType, false, OtherDerived, OtherDerived::ColsAtCompileTime==1>
(m_matrix, rhs.derived());
}
/** Efficient vector/matrix times triangular matrix product */
template<typename OtherDerived> friend
TriangularProduct<Mode,false,OtherDerived,OtherDerived::IsVectorAtCompileTime,MatrixType,false>
TriangularProduct<Mode, false, OtherDerived, OtherDerived::RowsAtCompileTime==1, MatrixType, false>
operator*(const MatrixBase<OtherDerived>& lhs, const TriangularView& rhs)
{
return TriangularProduct
<Mode,false,OtherDerived,OtherDerived::IsVectorAtCompileTime,MatrixType,false>
<Mode, false, OtherDerived, OtherDerived::RowsAtCompileTime==1, MatrixType, false>
(lhs.derived(),rhs.m_matrix);
}

19
gtsam/3rdparty/Eigen/Eigen/src/Core/util/MKL_support.h vendored
View File

@ -54,8 +54,25 @@
#endif
#if defined EIGEN_USE_MKL
# include <mkl.h>
/*Check IMKL version for compatibility: < 10.3 is not usable with Eigen*/
# ifndef INTEL_MKL_VERSION
# undef EIGEN_USE_MKL /* INTEL_MKL_VERSION is not even defined on older versions */
# elif INTEL_MKL_VERSION < 100305 /* the intel-mkl-103-release-notes say this was when the lapacke.h interface was added*/
# undef EIGEN_USE_MKL
# endif
# ifndef EIGEN_USE_MKL
/*If the MKL version is too old, undef everything*/
# undef EIGEN_USE_MKL_ALL
# undef EIGEN_USE_BLAS
# undef EIGEN_USE_LAPACKE
# undef EIGEN_USE_MKL_VML
# undef EIGEN_USE_LAPACKE_STRICT
# undef EIGEN_USE_LAPACKE
# endif
#endif
#include <mkl.h>
#if defined EIGEN_USE_MKL
#include <mkl_lapacke.h>
#define EIGEN_MKL_VML_THRESHOLD 128

5
gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h vendored
View File

@ -13,7 +13,7 @@
#define EIGEN_WORLD_VERSION 3
#define EIGEN_MAJOR_VERSION 2
#define EIGEN_MINOR_VERSION 1
#define EIGEN_MINOR_VERSION 2
#define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \
(EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \
@ -289,7 +289,8 @@ namespace Eigen {
#endif
#ifndef EIGEN_STACK_ALLOCATION_LIMIT
#define EIGEN_STACK_ALLOCATION_LIMIT 20000
// 131072 == 128 KB
#define EIGEN_STACK_ALLOCATION_LIMIT 131072
#endif
#ifndef EIGEN_DEFAULT_IO_FORMAT

17
gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h vendored
View File

@ -272,12 +272,12 @@ inline void* aligned_realloc(void *ptr, size_t new_size, size_t old_size)
// The defined(_mm_free) is just here to verify that this MSVC version
// implements _mm_malloc/_mm_free based on the corresponding _aligned_
// functions. This may not always be the case and we just try to be safe.
#if defined(_MSC_VER) && defined(_mm_free)
#if defined(_MSC_VER) && (!defined(_WIN32_WCE)) && defined(_mm_free)
result = _aligned_realloc(ptr,new_size,16);
#else
result = generic_aligned_realloc(ptr,new_size,old_size);
#endif
#elif defined(_MSC_VER)
#elif defined(_MSC_VER) && (!defined(_WIN32_WCE))
result = _aligned_realloc(ptr,new_size,16);
#else
result = handmade_aligned_realloc(ptr,new_size,old_size);
@ -630,6 +630,8 @@ template<typename T> class aligned_stack_memory_handler
} \
void operator delete(void * ptr) throw() { Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr); } \
void operator delete[](void * ptr) throw() { Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr); } \
void operator delete(void * ptr, std::size_t /* sz */) throw() { Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr); } \
void operator delete[](void * ptr, std::size_t /* sz */) throw() { Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr); } \
/* in-place new and delete. since (at least afaik) there is no actual */ \
/* memory allocated we can safely let the default implementation handle */ \
/* this particular case. */ \
@ -777,9 +779,9 @@ namespace internal {
#ifdef EIGEN_CPUID
inline bool cpuid_is_vendor(int abcd[4], const char* vendor)
inline bool cpuid_is_vendor(int abcd[4], const int vendor[3])
{
return abcd[1]==(reinterpret_cast<const int*>(vendor))[0] && abcd[3]==(reinterpret_cast<const int*>(vendor))[1] && abcd[2]==(reinterpret_cast<const int*>(vendor))[2];
return abcd[1]==vendor[0] && abcd[3]==vendor[1] && abcd[2]==vendor[2];
}
inline void queryCacheSizes_intel_direct(int& l1, int& l2, int& l3)
@ -921,13 +923,16 @@ inline void queryCacheSizes(int& l1, int& l2, int& l3)
{
#ifdef EIGEN_CPUID
int abcd[4];
const int GenuineIntel[] = {0x756e6547, 0x49656e69, 0x6c65746e};
const int AuthenticAMD[] = {0x68747541, 0x69746e65, 0x444d4163};
const int AMDisbetter_[] = {0x69444d41, 0x74656273, 0x21726574}; // "AMDisbetter!"
// identify the CPU vendor
EIGEN_CPUID(abcd,0x0,0);
int max_std_funcs = abcd[1];
if(cpuid_is_vendor(abcd,"GenuineIntel"))
if(cpuid_is_vendor(abcd,GenuineIntel))
queryCacheSizes_intel(l1,l2,l3,max_std_funcs);
else if(cpuid_is_vendor(abcd,"AuthenticAMD") || cpuid_is_vendor(abcd,"AMDisbetter!"))
else if(cpuid_is_vendor(abcd,AuthenticAMD) || cpuid_is_vendor(abcd,AMDisbetter_))
queryCacheSizes_amd(l1,l2,l3);
else
// by default let's use Intel's API

14
gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h vendored
View File

@ -203,6 +203,8 @@ public:
* \li \c Quaternionf for \c float
* \li \c Quaterniond for \c double
*
* \warning Operations interpreting the quaternion as rotation have undefined behavior if the quaternion is not normalized.
*
* \sa class AngleAxis, class Transform
*/
@ -344,7 +346,7 @@ class Map<const Quaternion<_Scalar>, _Options >
/** Constructs a Mapped Quaternion object from the pointer \a coeffs
*
* The pointer \a coeffs must reference the four coeffecients of Quaternion in the following order:
* The pointer \a coeffs must reference the four coefficients of Quaternion in the following order:
* \code *coeffs == {x, y, z, w} \endcode
*
* If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */
@ -464,7 +466,7 @@ QuaternionBase<Derived>::_transformVector(Vector3 v) const
// Note that this algorithm comes from the optimization by hand
// of the conversion to a Matrix followed by a Matrix/Vector product.
// It appears to be much faster than the common algorithm found
// in the litterature (30 versus 39 flops). It also requires two
// in the literature (30 versus 39 flops). It also requires two
// Vector3 as temporaries.
Vector3 uv = this->vec().cross(v);
uv += uv;
@ -584,7 +586,7 @@ inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Deri
// which yields a singular value problem
if (c < Scalar(-1)+NumTraits<Scalar>::dummy_precision())
{
c = max<Scalar>(c,-1);
c = (max)(c,Scalar(-1));
Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV);
Vector3 axis = svd.matrixV().col(2);
@ -667,10 +669,10 @@ QuaternionBase<Derived>::angularDistance(const QuaternionBase<OtherDerived>& oth
{
using std::acos;
using std::abs;
double d = abs(this->dot(other));
if (d>=1.0)
Scalar d = abs(this->dot(other));
if (d>=Scalar(1))
return Scalar(0);
return static_cast<Scalar>(2 * acos(d));
return Scalar(2) * acos(d);
}

4
gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h vendored
View File

@ -194,9 +194,9 @@ public:
/** type of the matrix used to represent the linear part of the transformation */
typedef Matrix<Scalar,Dim,Dim,Options> LinearMatrixType;
/** type of read/write reference to the linear part of the transformation */
typedef Block<MatrixType,Dim,Dim,int(Mode)==(AffineCompact)> LinearPart;
typedef Block<MatrixType,Dim,Dim,int(Mode)==(AffineCompact) && (Options&RowMajor)==0> LinearPart;
/** type of read reference to the linear part of the transformation */
typedef const Block<ConstMatrixType,Dim,Dim,int(Mode)==(AffineCompact)> ConstLinearPart;
typedef const Block<ConstMatrixType,Dim,Dim,int(Mode)==(AffineCompact) && (Options&RowMajor)==0> ConstLinearPart;
/** type of read/write reference to the affine part of the transformation */
typedef typename internal::conditional<int(Mode)==int(AffineCompact),
MatrixType&,

10
gtsam/3rdparty/Eigen/Eigen/src/Geometry/Umeyama.h vendored
View File

@ -113,7 +113,7 @@ umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, boo
const Index n = src.cols(); // number of measurements
// required for demeaning ...
const RealScalar one_over_n = 1 / static_cast<RealScalar>(n);
const RealScalar one_over_n = RealScalar(1) / static_cast<RealScalar>(n);
// computation of mean
const VectorType src_mean = src.rowwise().sum() * one_over_n;
@ -136,16 +136,16 @@ umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, boo
// Eq. (39)
VectorType S = VectorType::Ones(m);
if (sigma.determinant()<0) S(m-1) = -1;
if (sigma.determinant()<Scalar(0)) S(m-1) = Scalar(-1);
// Eq. (40) and (43)
const VectorType& d = svd.singularValues();
Index rank = 0; for (Index i=0; i<m; ++i) if (!internal::isMuchSmallerThan(d.coeff(i),d.coeff(0))) ++rank;
if (rank == m-1) {
if ( svd.matrixU().determinant() * svd.matrixV().determinant() > 0 ) {
if ( svd.matrixU().determinant() * svd.matrixV().determinant() > Scalar(0) ) {
Rt.block(0,0,m,m).noalias() = svd.matrixU()*svd.matrixV().transpose();
} else {
const Scalar s = S(m-1); S(m-1) = -1;
const Scalar s = S(m-1); S(m-1) = Scalar(-1);
Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose();
S(m-1) = s;
}
@ -156,7 +156,7 @@ umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, boo
if (with_scaling)
{
// Eq. (42)
const Scalar c = 1/src_var * svd.singularValues().dot(S);
const Scalar c = Scalar(1)/src_var * svd.singularValues().dot(S);
// Eq. (41)
Rt.col(m).head(m) = dst_mean;

2
gtsam/3rdparty/Eigen/Eigen/src/Householder/BlockHouseholder.h vendored
View File

@ -48,7 +48,7 @@ void apply_block_householder_on_the_left(MatrixType& mat, const VectorsType& vec
typedef typename MatrixType::Index Index;
enum { TFactorSize = MatrixType::ColsAtCompileTime };
Index nbVecs = vectors.cols();
Matrix<typename MatrixType::Scalar, TFactorSize, TFactorSize> T(nbVecs,nbVecs);
Matrix<typename MatrixType::Scalar, TFactorSize, TFactorSize, ColMajor> T(nbVecs,nbVecs);
make_block_householder_triangular_factor(T, vectors, hCoeffs);
const TriangularView<const VectorsType, UnitLower>& V(vectors);

3
gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h vendored
View File

@ -61,6 +61,7 @@ bool bicgstab(const MatrixType& mat, const Rhs& rhs, Dest& x,
VectorType s(n), t(n);
RealScalar tol2 = tol*tol;
RealScalar eps2 = NumTraits<Scalar>::epsilon()*NumTraits<Scalar>::epsilon();
int i = 0;
int restarts = 0;
@ -69,7 +70,7 @@ bool bicgstab(const MatrixType& mat, const Rhs& rhs, Dest& x,
Scalar rho_old = rho;
rho = r0.dot(r);
if (internal::isMuchSmallerThan(rho,r0_sqnorm))
if (abs(rho) < eps2*r0_sqnorm)
{
// The new residual vector became too orthogonal to the arbitrarily choosen direction r0
// Let's restart with a new r0:

13
gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h vendored
View File

@ -20,10 +20,11 @@ namespace Eigen {
*
* \param MatrixType the type of the matrix of which we are computing the LU decomposition
*
* This class represents a LU decomposition of any matrix, with complete pivoting: the matrix A
* is decomposed as A = PLUQ where L is unit-lower-triangular, U is upper-triangular, and P and Q
* are permutation matrices. This is a rank-revealing LU decomposition. The eigenvalues (diagonal
* coefficients) of U are sorted in such a way that any zeros are at the end.
* This class represents a LU decomposition of any matrix, with complete pivoting: the matrix A is
* decomposed as \f$ A = P^{-1} L U Q^{-1} \f$ where L is unit-lower-triangular, U is
* upper-triangular, and P and Q are permutation matrices. This is a rank-revealing LU
* decomposition. The eigenvalues (diagonal coefficients) of U are sorted in such a way that any
* zeros are at the end.
*
* This decomposition provides the generic approach to solving systems of linear equations, computing
* the rank, invertibility, inverse, kernel, and determinant.
@ -511,8 +512,8 @@ typename internal::traits<MatrixType>::Scalar FullPivLU<MatrixType>::determinant
}
/** \returns the matrix represented by the decomposition,
* i.e., it returns the product: P^{-1} L U Q^{-1}.
* This function is provided for debug purpose. */
* i.e., it returns the product: \f$ P^{-1} L U Q^{-1} \f$.
* This function is provided for debug purposes. */
template<typename MatrixType>
MatrixType FullPivLU<MatrixType>::reconstructedMatrix() const
{

12
gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Ordering.h vendored
View File

@ -109,7 +109,7 @@ class NaturalOrdering
* \class COLAMDOrdering
*
* Functor computing the \em column \em approximate \em minimum \em degree ordering
* The matrix should be in column-major format
* The matrix should be in column-major and \b compressed format (see SparseMatrix::makeCompressed()).
*/
template<typename Index>
class COLAMDOrdering
@ -118,10 +118,14 @@ class COLAMDOrdering
typedef PermutationMatrix<Dynamic, Dynamic, Index> PermutationType;
typedef Matrix<Index, Dynamic, 1> IndexVector;
/** Compute the permutation vector form a sparse matrix */
/** Compute the permutation vector \a perm form the sparse matrix \a mat
* \warning The input sparse matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
*/
template <typename MatrixType>
void operator() (const MatrixType& mat, PermutationType& perm)
{
eigen_assert(mat.isCompressed() && "COLAMDOrdering requires a sparse matrix in compressed mode. Call .makeCompressed() before passing it to COLAMDOrdering");
Index m = mat.rows();
Index n = mat.cols();
Index nnz = mat.nonZeros();
@ -132,12 +136,12 @@ class COLAMDOrdering
Index stats [COLAMD_STATS];
internal::colamd_set_defaults(knobs);
Index info;
IndexVector p(n+1), A(Alen);
for(Index i=0; i <= n; i++) p(i) = mat.outerIndexPtr()[i];
for(Index i=0; i < nnz; i++) A(i) = mat.innerIndexPtr()[i];
// Call Colamd routine to compute the ordering
info = internal::colamd(m, n, Alen, A.data(), p.data(), knobs, stats);
Index info = internal::colamd(m, n, Alen, A.data(), p.data(), knobs, stats);
EIGEN_UNUSED_VARIABLE(info);
eigen_assert( info && "COLAMD failed " );
perm.resize(n);

3
gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h vendored
View File

@ -76,7 +76,8 @@ template<typename _MatrixType> class ColPivHouseholderQR
m_colsTranspositions(),
m_temp(),
m_colSqNorms(),
m_isInitialized(false) {}
m_isInitialized(false),
m_usePrescribedThreshold(false) {}
/** \brief Default Constructor with memory preallocation
*

105
gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h vendored
View File

@ -375,17 +375,19 @@ struct svd_precondition_2x2_block_to_be_real<MatrixType, QRPreconditioner, true>
Scalar z;
JacobiRotation<Scalar> rot;
RealScalar n = sqrt(numext::abs2(work_matrix.coeff(p,p)) + numext::abs2(work_matrix.coeff(q,p)));
if(n==0)
{
z = abs(work_matrix.coeff(p,q)) / work_matrix.coeff(p,q);
work_matrix.row(p) *= z;
if(svd.computeU()) svd.m_matrixU.col(p) *= conj(z);
if(work_matrix.coeff(q,q)!=Scalar(0))
{
z = abs(work_matrix.coeff(q,q)) / work_matrix.coeff(q,q);
else
z = Scalar(0);
work_matrix.row(q) *= z;
if(svd.computeU()) svd.m_matrixU.col(q) *= conj(z);
work_matrix.row(q) *= z;
if(svd.computeU()) svd.m_matrixU.col(q) *= conj(z);
}
// otherwise the second row is already zero, so we have nothing to do.
}
else
{
@ -415,6 +417,7 @@ void real_2x2_jacobi_svd(const MatrixType& matrix, Index p, Index q,
JacobiRotation<RealScalar> *j_right)
{
using std::sqrt;
using std::abs;
Matrix<RealScalar,2,2> m;
m << numext::real(matrix.coeff(p,p)), numext::real(matrix.coeff(p,q)),
numext::real(matrix.coeff(q,p)), numext::real(matrix.coeff(q,q));
@ -428,9 +431,11 @@ void real_2x2_jacobi_svd(const MatrixType& matrix, Index p, Index q,
}
else
{
RealScalar u = d / t;
rot1.c() = RealScalar(1) / sqrt(RealScalar(1) + numext::abs2(u));
rot1.s() = rot1.c() * u;
RealScalar t2d2 = numext::hypot(t,d);
rot1.c() = abs(t)/t2d2;
rot1.s() = d/t2d2;
if(t<RealScalar(0))
rot1.s() = -rot1.s();
}
m.applyOnTheLeft(0,1,rot1);
j_right->makeJacobi(m,0,1);
@ -531,8 +536,9 @@ template<typename _MatrixType, int QRPreconditioner> class JacobiSVD
JacobiSVD()
: m_isInitialized(false),
m_isAllocated(false),
m_usePrescribedThreshold(false),
m_computationOptions(0),
m_rows(-1), m_cols(-1)
m_rows(-1), m_cols(-1), m_diagSize(0)
{}
@ -545,6 +551,7 @@ template<typename _MatrixType, int QRPreconditioner> class JacobiSVD
JacobiSVD(Index rows, Index cols, unsigned int computationOptions = 0)
: m_isInitialized(false),
m_isAllocated(false),
m_usePrescribedThreshold(false),
m_computationOptions(0),
m_rows(-1), m_cols(-1)
{
@ -564,6 +571,7 @@ template<typename _MatrixType, int QRPreconditioner> class JacobiSVD
JacobiSVD(const MatrixType& matrix, unsigned int computationOptions = 0)
: m_isInitialized(false),
m_isAllocated(false),
m_usePrescribedThreshold(false),
m_computationOptions(0),
m_rows(-1), m_cols(-1)
{
@ -665,6 +673,69 @@ template<typename _MatrixType, int QRPreconditioner> class JacobiSVD
eigen_assert(m_isInitialized && "JacobiSVD is not initialized.");
return m_nonzeroSingularValues;
}
/** \returns the rank of the matrix of which \c *this is the SVD.
*
* \note This method has to determine which singular values should be considered nonzero.
* For that, it uses the threshold value that you can control by calling
* setThreshold(const RealScalar&).
*/
inline Index rank() const
{
using std::abs;
eigen_assert(m_isInitialized && "JacobiSVD is not initialized.");
if(m_singularValues.size()==0) return 0;
RealScalar premultiplied_threshold = m_singularValues.coeff(0) * threshold();
Index i = m_nonzeroSingularValues-1;
while(i>=0 && m_singularValues.coeff(i) < premultiplied_threshold) --i;
return i+1;
}
/** Allows to prescribe a threshold to be used by certain methods, such as rank() and solve(),
* which need to determine when singular values are to be considered nonzero.
* This is not used for the SVD decomposition itself.
*
* When it needs to get the threshold value, Eigen calls threshold().
* The default is \c NumTraits<Scalar>::epsilon()
*
* \param threshold The new value to use as the threshold.
*
* A singular value will be considered nonzero if its value is strictly greater than
* \f$ \vert singular value \vert \leqslant threshold \times \vert max singular value \vert \f$.
*
* If you want to come back to the default behavior, call setThreshold(Default_t)
*/
JacobiSVD& setThreshold(const RealScalar& threshold)
{
m_usePrescribedThreshold = true;
m_prescribedThreshold = threshold;
return *this;
}
/** Allows to come back to the default behavior, letting Eigen use its default formula for
* determining the threshold.
*
* You should pass the special object Eigen::Default as parameter here.
* \code svd.setThreshold(Eigen::Default); \endcode
*
* See the documentation of setThreshold(const RealScalar&).
*/
JacobiSVD& setThreshold(Default_t)
{
m_usePrescribedThreshold = false;
return *this;
}
/** Returns the threshold that will be used by certain methods such as rank().
*
* See the documentation of setThreshold(const RealScalar&).
*/
RealScalar threshold() const
{
eigen_assert(m_isInitialized || m_usePrescribedThreshold);
return m_usePrescribedThreshold ? m_prescribedThreshold
: (std::max<Index>)(1,m_diagSize)*NumTraits<Scalar>::epsilon();
}
inline Index rows() const { return m_rows; }
inline Index cols() const { return m_cols; }
@ -677,11 +748,12 @@ template<typename _MatrixType, int QRPreconditioner> class JacobiSVD
MatrixVType m_matrixV;
SingularValuesType m_singularValues;
WorkMatrixType m_workMatrix;
bool m_isInitialized, m_isAllocated;
bool m_isInitialized, m_isAllocated, m_usePrescribedThreshold;
bool m_computeFullU, m_computeThinU;
bool m_computeFullV, m_computeThinV;
unsigned int m_computationOptions;
Index m_nonzeroSingularValues, m_rows, m_cols, m_diagSize;
RealScalar m_prescribedThreshold;
template<typename __MatrixType, int _QRPreconditioner, bool _IsComplex>
friend struct internal::svd_precondition_2x2_block_to_be_real;
@ -764,6 +836,11 @@ JacobiSVD<MatrixType, QRPreconditioner>::compute(const MatrixType& matrix, unsig
if(m_computeFullV) m_matrixV.setIdentity(m_cols,m_cols);
if(m_computeThinV) m_matrixV.setIdentity(m_cols, m_diagSize);
}
// Scaling factor to reduce over/under-flows
RealScalar scale = m_workMatrix.cwiseAbs().maxCoeff();
if(scale==RealScalar(0)) scale = RealScalar(1);
m_workMatrix /= scale;
/*** step 2. The main Jacobi SVD iteration. ***/
@ -833,6 +910,8 @@ JacobiSVD<MatrixType, QRPreconditioner>::compute(const MatrixType& matrix, unsig
if(computeV()) m_matrixV.col(pos).swap(m_matrixV.col(i));
}
}
m_singularValues *= scale;
m_isInitialized = true;
return *this;
@ -854,11 +933,11 @@ struct solve_retval<JacobiSVD<_MatrixType, QRPreconditioner>, Rhs>
// So A^{-1} = V S^{-1} U^*
Matrix<Scalar, Dynamic, Rhs::ColsAtCompileTime, 0, _MatrixType::MaxRowsAtCompileTime, Rhs::MaxColsAtCompileTime> tmp;
Index nonzeroSingVals = dec().nonzeroSingularValues();
Index rank = dec().rank();
tmp.noalias() = dec().matrixU().leftCols(nonzeroSingVals).adjoint() * rhs();
tmp = dec().singularValues().head(nonzeroSingVals).asDiagonal().inverse() * tmp;
dst = dec().matrixV().leftCols(nonzeroSingVals) * tmp;
tmp.noalias() = dec().matrixU().leftCols(rank).adjoint() * rhs();
tmp = dec().singularValues().head(rank).asDiagonal().inverse() * tmp;
dst = dec().matrixV().leftCols(rank) * tmp;
}
};
} // end namespace internal

42
gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h vendored
View File

@ -37,6 +37,7 @@ class SimplicialCholeskyBase : internal::noncopyable
{
public:
typedef typename internal::traits<Derived>::MatrixType MatrixType;
typedef typename internal::traits<Derived>::OrderingType OrderingType;
enum { UpLo = internal::traits<Derived>::UpLo };
typedef typename MatrixType::Scalar Scalar;
typedef typename MatrixType::RealScalar RealScalar;
@ -240,15 +241,16 @@ class SimplicialCholeskyBase : internal::noncopyable
RealScalar m_shiftScale;
};
template<typename _MatrixType, int _UpLo = Lower> class SimplicialLLT;
template<typename _MatrixType, int _UpLo = Lower> class SimplicialLDLT;
template<typename _MatrixType, int _UpLo = Lower> class SimplicialCholesky;
template<typename _MatrixType, int _UpLo = Lower, typename _Ordering = AMDOrdering<typename _MatrixType::Index> > class SimplicialLLT;
template<typename _MatrixType, int _UpLo = Lower, typename _Ordering = AMDOrdering<typename _MatrixType::Index> > class SimplicialLDLT;
template<typename _MatrixType, int _UpLo = Lower, typename _Ordering = AMDOrdering<typename _MatrixType::Index> > class SimplicialCholesky;
namespace internal {
template<typename _MatrixType, int _UpLo> struct traits<SimplicialLLT<_MatrixType,_UpLo> >
template<typename _MatrixType, int _UpLo, typename _Ordering> struct traits<SimplicialLLT<_MatrixType,_UpLo,_Ordering> >
{
typedef _MatrixType MatrixType;
typedef _Ordering OrderingType;
enum { UpLo = _UpLo };
typedef typename MatrixType::Scalar Scalar;
typedef typename MatrixType::Index Index;
@ -259,9 +261,10 @@ template<typename _MatrixType, int _UpLo> struct traits<SimplicialLLT<_MatrixTyp
static inline MatrixU getU(const MatrixType& m) { return m.adjoint(); }
};
template<typename _MatrixType,int _UpLo> struct traits<SimplicialLDLT<_MatrixType,_UpLo> >
template<typename _MatrixType,int _UpLo, typename _Ordering> struct traits<SimplicialLDLT<_MatrixType,_UpLo,_Ordering> >
{
typedef _MatrixType MatrixType;
typedef _Ordering OrderingType;
enum { UpLo = _UpLo };
typedef typename MatrixType::Scalar Scalar;
typedef typename MatrixType::Index Index;
@ -272,9 +275,10 @@ template<typename _MatrixType,int _UpLo> struct traits<SimplicialLDLT<_MatrixTyp
static inline MatrixU getU(const MatrixType& m) { return m.adjoint(); }
};
template<typename _MatrixType, int _UpLo> struct traits<SimplicialCholesky<_MatrixType,_UpLo> >
template<typename _MatrixType, int _UpLo, typename _Ordering> struct traits<SimplicialCholesky<_MatrixType,_UpLo,_Ordering> >
{
typedef _MatrixType MatrixType;
typedef _Ordering OrderingType;
enum { UpLo = _UpLo };
};
@ -294,11 +298,12 @@ template<typename _MatrixType, int _UpLo> struct traits<SimplicialCholesky<_Matr
* \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
* \tparam _UpLo the triangular part that will be used for the computations. It can be Lower
* or Upper. Default is Lower.
* \tparam _Ordering The ordering method to use, either AMDOrdering<> or NaturalOrdering<>. Default is AMDOrdering<>
*
* \sa class SimplicialLDLT
* \sa class SimplicialLDLT, class AMDOrdering, class NaturalOrdering
*/
template<typename _MatrixType, int _UpLo>
class SimplicialLLT : public SimplicialCholeskyBase<SimplicialLLT<_MatrixType,_UpLo> >
template<typename _MatrixType, int _UpLo, typename _Ordering>
class SimplicialLLT : public SimplicialCholeskyBase<SimplicialLLT<_MatrixType,_UpLo,_Ordering> >
{
public:
typedef _MatrixType MatrixType;
@ -382,11 +387,12 @@ public:
* \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
* \tparam _UpLo the triangular part that will be used for the computations. It can be Lower
* or Upper. Default is Lower.
* \tparam _Ordering The ordering method to use, either AMDOrdering<> or NaturalOrdering<>. Default is AMDOrdering<>
*
* \sa class SimplicialLLT
* \sa class SimplicialLLT, class AMDOrdering, class NaturalOrdering
*/
template<typename _MatrixType, int _UpLo>
class SimplicialLDLT : public SimplicialCholeskyBase<SimplicialLDLT<_MatrixType,_UpLo> >
template<typename _MatrixType, int _UpLo, typename _Ordering>
class SimplicialLDLT : public SimplicialCholeskyBase<SimplicialLDLT<_MatrixType,_UpLo,_Ordering> >
{
public:
typedef _MatrixType MatrixType;
@ -467,8 +473,8 @@ public:
*
* \sa class SimplicialLDLT, class SimplicialLLT
*/
template<typename _MatrixType, int _UpLo>
class SimplicialCholesky : public SimplicialCholeskyBase<SimplicialCholesky<_MatrixType,_UpLo> >
template<typename _MatrixType, int _UpLo, typename _Ordering>
class SimplicialCholesky : public SimplicialCholeskyBase<SimplicialCholesky<_MatrixType,_UpLo,_Ordering> >
{
public:
typedef _MatrixType MatrixType;
@ -612,15 +618,13 @@ void SimplicialCholeskyBase<Derived>::ordering(const MatrixType& a, CholMatrixTy
{
eigen_assert(a.rows()==a.cols());
const Index size = a.rows();
// TODO allows to configure the permutation
// Note that amd compute the inverse permutation
{
CholMatrixType C;
C = a.template selfadjointView<UpLo>();
// remove diagonal entries:
// seems not to be needed
// C.prune(keep_diag());
internal::minimum_degree_ordering(C, m_Pinv);
OrderingType ordering;
ordering(C,m_Pinv);
}
if(m_Pinv.size()>0)

8
gtsam/3rdparty/Eigen/Eigen/src/SparseCore/CompressedStorage.h vendored
View File

@ -51,8 +51,8 @@ class CompressedStorage
CompressedStorage& operator=(const CompressedStorage& other)
{
resize(other.size());
memcpy(m_values, other.m_values, m_size * sizeof(Scalar));
memcpy(m_indices, other.m_indices, m_size * sizeof(Index));
internal::smart_copy(other.m_values, other.m_values + m_size, m_values);
internal::smart_copy(other.m_indices, other.m_indices + m_size, m_indices);
return *this;
}
@ -83,10 +83,10 @@ class CompressedStorage
reallocate(m_size);
}
void resize(size_t size, float reserveSizeFactor = 0)
void resize(size_t size, double reserveSizeFactor = 0)
{
if (m_allocatedSize<size)
reallocate(size + size_t(reserveSizeFactor*size));
reallocate(size + size_t(reserveSizeFactor*double(size)));
m_size = size;
}

3
gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseBinaryOp.h vendored
View File

@ -73,7 +73,8 @@ class CwiseBinaryOpImpl<BinaryOp,Lhs,Rhs,Sparse>::InnerIterator
typedef internal::sparse_cwise_binary_op_inner_iterator_selector<
BinaryOp,Lhs,Rhs, InnerIterator> Base;
EIGEN_STRONG_INLINE InnerIterator(const CwiseBinaryOpImpl& binOp, Index outer)
// NOTE: we have to prefix Index by "typename Lhs::" to avoid an ICE with VC11
EIGEN_STRONG_INLINE InnerIterator(const CwiseBinaryOpImpl& binOp, typename Lhs::Index outer)
: Base(binOp.derived(),outer)
{}
};

33
gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h vendored
View File

@ -19,7 +19,10 @@ template<typename Lhs, typename Rhs, int InnerSize> struct SparseDenseProductRet
template<typename Lhs, typename Rhs> struct SparseDenseProductReturnType<Lhs,Rhs,1>
{
typedef SparseDenseOuterProduct<Lhs,Rhs,false> Type;
typedef typename internal::conditional<
Lhs::IsRowMajor,
SparseDenseOuterProduct<Rhs,Lhs,true>,
SparseDenseOuterProduct<Lhs,Rhs,false> >::type Type;
};
template<typename Lhs, typename Rhs, int InnerSize> struct DenseSparseProductReturnType
@ -29,7 +32,10 @@ template<typename Lhs, typename Rhs, int InnerSize> struct DenseSparseProductRet
template<typename Lhs, typename Rhs> struct DenseSparseProductReturnType<Lhs,Rhs,1>
{
typedef SparseDenseOuterProduct<Rhs,Lhs,true> Type;
typedef typename internal::conditional<
Rhs::IsRowMajor,
SparseDenseOuterProduct<Rhs,Lhs,true>,
SparseDenseOuterProduct<Lhs,Rhs,false> >::type Type;
};
namespace internal {
@ -114,17 +120,30 @@ class SparseDenseOuterProduct<Lhs,Rhs,Transpose>::InnerIterator : public _LhsNes
typedef typename SparseDenseOuterProduct::Index Index;
public:
EIGEN_STRONG_INLINE InnerIterator(const SparseDenseOuterProduct& prod, Index outer)
: Base(prod.lhs(), 0), m_outer(outer), m_factor(prod.rhs().coeff(outer))
{
}
: Base(prod.lhs(), 0), m_outer(outer), m_factor(get(prod.rhs(), outer, typename internal::traits<Rhs>::StorageKind() ))
{ }
inline Index outer() const { return m_outer; }
inline Index row() const { return Transpose ? Base::row() : m_outer; }
inline Index col() const { return Transpose ? m_outer : Base::row(); }
inline Index row() const { return Transpose ? m_outer : Base::index(); }
inline Index col() const { return Transpose ? Base::index() : m_outer; }
inline Scalar value() const { return Base::value() * m_factor; }
protected:
static Scalar get(const _RhsNested &rhs, Index outer, Dense = Dense())
{
return rhs.coeff(outer);
}
static Scalar get(const _RhsNested &rhs, Index outer, Sparse = Sparse())
{
typename Traits::_RhsNested::InnerIterator it(rhs, outer);
if (it && it.index()==0)
return it.value();
return Scalar(0);
}
Index m_outer;
Scalar m_factor;
};

12
gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrix.h vendored
View File

@ -940,7 +940,7 @@ void set_from_triplets(const InputIterator& begin, const InputIterator& end, Spa
enum { IsRowMajor = SparseMatrixType::IsRowMajor };
typedef typename SparseMatrixType::Scalar Scalar;
typedef typename SparseMatrixType::Index Index;
SparseMatrix<Scalar,IsRowMajor?ColMajor:RowMajor> trMat(mat.rows(),mat.cols());
SparseMatrix<Scalar,IsRowMajor?ColMajor:RowMajor,Index> trMat(mat.rows(),mat.cols());
if(begin!=end)
{
@ -1178,7 +1178,7 @@ EIGEN_DONT_INLINE typename SparseMatrix<_Scalar,_Options,_Index>::Scalar& Sparse
size_t p = m_outerIndex[outer+1];
++m_outerIndex[outer+1];
float reallocRatio = 1;
double reallocRatio = 1;
if (m_data.allocatedSize()<=m_data.size())
{
// if there is no preallocated memory, let's reserve a minimum of 32 elements
@ -1190,13 +1190,13 @@ EIGEN_DONT_INLINE typename SparseMatrix<_Scalar,_Options,_Index>::Scalar& Sparse
{
// we need to reallocate the data, to reduce multiple reallocations
// we use a smart resize algorithm based on the current filling ratio
// in addition, we use float to avoid integers overflows
float nnzEstimate = float(m_outerIndex[outer])*float(m_outerSize)/float(outer+1);
reallocRatio = (nnzEstimate-float(m_data.size()))/float(m_data.size());
// in addition, we use double to avoid integers overflows
double nnzEstimate = double(m_outerIndex[outer])*double(m_outerSize)/double(outer+1);
reallocRatio = (nnzEstimate-double(m_data.size()))/double(m_data.size());
// furthermore we bound the realloc ratio to:
// 1) reduce multiple minor realloc when the matrix is almost filled
// 2) avoid to allocate too much memory when the matrix is almost empty
reallocRatio = (std::min)((std::max)(reallocRatio,1.5f),8.f);
reallocRatio = (std::min)((std::max)(reallocRatio,1.5),8.);
}
}
m_data.resize(m_data.size()+1,reallocRatio);

10
gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseTranspose.h vendored
View File

@ -26,7 +26,7 @@ template<typename MatrixType> class TransposeImpl<MatrixType,Sparse>
inline Index nonZeros() const { return derived().nestedExpression().nonZeros(); }
};
// NOTE: VC10 trigger an ICE if don't put typename TransposeImpl<MatrixType,Sparse>:: in front of Index,
// NOTE: VC10 and VC11 trigger an ICE if don't put typename TransposeImpl<MatrixType,Sparse>:: in front of Index,
// a typedef typename TransposeImpl<MatrixType,Sparse>::Index Index;
// does not fix the issue.
// An alternative is to define the nested class in the parent class itself.
@ -40,8 +40,8 @@ template<typename MatrixType> class TransposeImpl<MatrixType,Sparse>::InnerItera
EIGEN_STRONG_INLINE InnerIterator(const TransposeImpl& trans, typename TransposeImpl<MatrixType,Sparse>::Index outer)
: Base(trans.derived().nestedExpression(), outer)
{}
Index row() const { return Base::col(); }
Index col() const { return Base::row(); }
typename TransposeImpl<MatrixType,Sparse>::Index row() const { return Base::col(); }
typename TransposeImpl<MatrixType,Sparse>::Index col() const { return Base::row(); }
};
template<typename MatrixType> class TransposeImpl<MatrixType,Sparse>::ReverseInnerIterator
@ -54,8 +54,8 @@ template<typename MatrixType> class TransposeImpl<MatrixType,Sparse>::ReverseInn
EIGEN_STRONG_INLINE ReverseInnerIterator(const TransposeImpl& xpr, typename TransposeImpl<MatrixType,Sparse>::Index outer)
: Base(xpr.derived().nestedExpression(), outer)
{}
Index row() const { return Base::col(); }
Index col() const { return Base::row(); }
typename TransposeImpl<MatrixType,Sparse>::Index row() const { return Base::col(); }
typename TransposeImpl<MatrixType,Sparse>::Index col() const { return Base::row(); }
};
} // end namespace Eigen

6
gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h vendored
View File

@ -84,8 +84,10 @@ template<typename Lhs, typename Rhs> class DenseTimeSparseProduct;
template<typename Lhs, typename Rhs, bool Transpose> class SparseDenseOuterProduct;
template<typename Lhs, typename Rhs> struct SparseSparseProductReturnType;
template<typename Lhs, typename Rhs, int InnerSize = internal::traits<Lhs>::ColsAtCompileTime> struct DenseSparseProductReturnType;
template<typename Lhs, typename Rhs, int InnerSize = internal::traits<Lhs>::ColsAtCompileTime> struct SparseDenseProductReturnType;
template<typename Lhs, typename Rhs,
int InnerSize = EIGEN_SIZE_MIN_PREFER_FIXED(internal::traits<Lhs>::ColsAtCompileTime,internal::traits<Rhs>::RowsAtCompileTime)> struct DenseSparseProductReturnType;
template<typename Lhs, typename Rhs,
int InnerSize = EIGEN_SIZE_MIN_PREFER_FIXED(internal::traits<Lhs>::ColsAtCompileTime,internal::traits<Rhs>::RowsAtCompileTime)> struct SparseDenseProductReturnType;
template<typename MatrixType,int UpLo> class SparseSymmetricPermutationProduct;
namespace internal {

136
gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h vendored
View File

@ -2,7 +2,7 @@
// for linear algebra.
//
// Copyright (C) 2012-2013 Desire Nuentsa <desire.nuentsa_wakam@inria.fr>
// Copyright (C) 2012-2013 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2012-2014 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
@ -58,6 +58,7 @@ namespace internal {
* \tparam _OrderingType The fill-reducing ordering method. See the \link OrderingMethods_Module
* OrderingMethods \endlink module for the list of built-in and external ordering methods.
*
* \warning The input sparse matrix A must be in compressed mode (see SparseMatrix::makeCompressed()).
*
*/
template<typename _MatrixType, typename _OrderingType>
@ -77,10 +78,23 @@ class SparseQR
SparseQR () : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false)
{ }
/** Construct a QR factorization of the matrix \a mat.
*
* \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
*
* \sa compute()
*/
SparseQR(const MatrixType& mat) : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false)
{
compute(mat);
}
/** Computes the QR factorization of the sparse matrix \a mat.
*
* \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
*
* \sa analyzePattern(), factorize()
*/
void compute(const MatrixType& mat)
{
analyzePattern(mat);
@ -166,7 +180,7 @@ class SparseQR
y.bottomRows(y.rows()-rank).setZero();
// Apply the column permutation
if (m_perm_c.size()) dest.topRows(cols()) = colsPermutation() * y.topRows(cols());
if (m_perm_c.size()) dest = colsPermutation() * y.topRows(cols());
else dest = y.topRows(cols());
m_info = Success;
@ -206,7 +220,7 @@ class SparseQR
/** \brief Reports whether previous computation was successful.
*
* \returns \c Success if computation was succesful,
* \returns \c Success if computation was successful,
* \c NumericalIssue if the QR factorization reports a numerical problem
* \c InvalidInput if the input matrix is invalid
*
@ -255,20 +269,24 @@ class SparseQR
};
/** \brief Preprocessing step of a QR factorization
*
* \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
*
* In this step, the fill-reducing permutation is computed and applied to the columns of A
* and the column elimination tree is computed as well. Only the sparcity pattern of \a mat is exploited.
* and the column elimination tree is computed as well. Only the sparsity pattern of \a mat is exploited.
*
* \note In this step it is assumed that there is no empty row in the matrix \a mat.
*/
template <typename MatrixType, typename OrderingType>
void SparseQR<MatrixType,OrderingType>::analyzePattern(const MatrixType& mat)
{
eigen_assert(mat.isCompressed() && "SparseQR requires a sparse matrix in compressed mode. Call .makeCompressed() before passing it to SparseQR");
// Compute the column fill reducing ordering
OrderingType ord;
ord(mat, m_perm_c);
Index n = mat.cols();
Index m = mat.rows();
Index diagSize = (std::min)(m,n);
if (!m_perm_c.size())
{
@ -280,20 +298,20 @@ void SparseQR<MatrixType,OrderingType>::analyzePattern(const MatrixType& mat)
m_outputPerm_c = m_perm_c.inverse();
internal::coletree(mat, m_etree, m_firstRowElt, m_outputPerm_c.indices().data());
m_R.resize(n, n);
m_Q.resize(m, n);
m_R.resize(m, n);
m_Q.resize(m, diagSize);
// Allocate space for nonzero elements : rough estimation
m_R.reserve(2*mat.nonZeros()); //FIXME Get a more accurate estimation through symbolic factorization with the etree
m_Q.reserve(2*mat.nonZeros());
m_hcoeffs.resize(n);
m_hcoeffs.resize(diagSize);
m_analysisIsok = true;
}
/** \brief Performs the numerical QR factorization of the input matrix
*
* The function SparseQR::analyzePattern(const MatrixType&) must have been called beforehand with
* a matrix having the same sparcity pattern than \a mat.
* a matrix having the same sparsity pattern than \a mat.
*
* \param mat The sparse column-major matrix
*/
@ -306,11 +324,12 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
eigen_assert(m_analysisIsok && "analyzePattern() should be called before this step");
Index m = mat.rows();
Index n = mat.cols();
IndexVector mark(m); mark.setConstant(-1); // Record the visited nodes
IndexVector Ridx(n), Qidx(m); // Store temporarily the row indexes for the current column of R and Q
Index nzcolR, nzcolQ; // Number of nonzero for the current column of R and Q
ScalarVector tval(m); // The dense vector used to compute the current column
bool found_diag;
Index diagSize = (std::min)(m,n);
IndexVector mark((std::max)(m,n)); mark.setConstant(-1); // Record the visited nodes
IndexVector Ridx(n), Qidx(m); // Store temporarily the row indexes for the current column of R and Q
Index nzcolR, nzcolQ; // Number of nonzero for the current column of R and Q
ScalarVector tval(m); // The dense vector used to compute the current column
RealScalar pivotThreshold = m_threshold;
m_pmat = mat;
m_pmat.uncompress(); // To have the innerNonZeroPtr allocated
@ -322,7 +341,7 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
m_pmat.innerNonZeroPtr()[p] = mat.outerIndexPtr()[i+1] - mat.outerIndexPtr()[i];
}
/* Compute the default threshold, see :
/* Compute the default threshold as in MatLab, see:
* Tim Davis, "Algorithm 915, SuiteSparseQR: Multifrontal Multithreaded Rank-Revealing
* Sparse QR Factorization, ACM Trans. on Math. Soft. 38(1), 2011, Page 8:3
*/
@ -330,24 +349,24 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
{
RealScalar max2Norm = 0.0;
for (int j = 0; j < n; j++) max2Norm = (max)(max2Norm, m_pmat.col(j).norm());
m_threshold = 20 * (m + n) * max2Norm * NumTraits<RealScalar>::epsilon();
pivotThreshold = 20 * (m + n) * max2Norm * NumTraits<RealScalar>::epsilon();
}
// Initialize the numerical permutation
m_pivotperm.setIdentity(n);
Index nonzeroCol = 0; // Record the number of valid pivots
m_Q.startVec(0);
// Left looking rank-revealing QR factorization: compute a column of R and Q at a time
for (Index col = 0; col < (std::min)(n,m); ++col)
for (Index col = 0; col < n; ++col)
{
mark.setConstant(-1);
m_R.startVec(col);
m_Q.startVec(col);
mark(nonzeroCol) = col;
Qidx(0) = nonzeroCol;
nzcolR = 0; nzcolQ = 1;
found_diag = col>=m;
bool found_diag = nonzeroCol>=m;
tval.setZero();
// Symbolic factorization: find the nonzero locations of the column k of the factors R and Q, i.e.,
@ -356,7 +375,7 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
// thus the trick with found_diag that permits to do one more iteration on the diagonal element if this one has not been found.
for (typename MatrixType::InnerIterator itp(m_pmat, col); itp || !found_diag; ++itp)
{
Index curIdx = nonzeroCol ;
Index curIdx = nonzeroCol;
if(itp) curIdx = itp.row();
if(curIdx == nonzeroCol) found_diag = true;
@ -398,7 +417,7 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
// Browse all the indexes of R(:,col) in reverse order
for (Index i = nzcolR-1; i >= 0; i--)
{
Index curIdx = m_pivotperm.indices()(Ridx(i));
Index curIdx = Ridx(i);
// Apply the curIdx-th householder vector to the current column (temporarily stored into tval)
Scalar tdot(0);
@ -427,33 +446,37 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
}
}
} // End update current column
// Compute the Householder reflection that eliminate the current column
// FIXME this step should call the Householder module.
Scalar tau;
RealScalar beta;
Scalar c0 = nzcolQ ? tval(Qidx(0)) : Scalar(0);
RealScalar beta = 0;
// First, the squared norm of Q((col+1):m, col)
RealScalar sqrNorm = 0.;
for (Index itq = 1; itq < nzcolQ; ++itq) sqrNorm += numext::abs2(tval(Qidx(itq)));
if(sqrNorm == RealScalar(0) && numext::imag(c0) == RealScalar(0))
if(nonzeroCol < diagSize)
{
tau = RealScalar(0);
beta = numext::real(c0);
tval(Qidx(0)) = 1;
}
else
{
beta = std::sqrt(numext::abs2(c0) + sqrNorm);
if(numext::real(c0) >= RealScalar(0))
beta = -beta;
tval(Qidx(0)) = 1;
for (Index itq = 1; itq < nzcolQ; ++itq)
tval(Qidx(itq)) /= (c0 - beta);
tau = numext::conj((beta-c0) / beta);
// Compute the Householder reflection that eliminate the current column
// FIXME this step should call the Householder module.
Scalar c0 = nzcolQ ? tval(Qidx(0)) : Scalar(0);
// First, the squared norm of Q((col+1):m, col)
RealScalar sqrNorm = 0.;
for (Index itq = 1; itq < nzcolQ; ++itq) sqrNorm += numext::abs2(tval(Qidx(itq)));
if(sqrNorm == RealScalar(0) && numext::imag(c0) == RealScalar(0))
{
tau = RealScalar(0);
beta = numext::real(c0);
tval(Qidx(0)) = 1;
}
else
{
using std::sqrt;
beta = sqrt(numext::abs2(c0) + sqrNorm);
if(numext::real(c0) >= RealScalar(0))
beta = -beta;
tval(Qidx(0)) = 1;
for (Index itq = 1; itq < nzcolQ; ++itq)
tval(Qidx(itq)) /= (c0 - beta);
tau = numext::conj((beta-c0) / beta);
}
}
// Insert values in R
@ -467,24 +490,25 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
}
}
if(abs(beta) >= m_threshold)
if(nonzeroCol < diagSize && abs(beta) >= pivotThreshold)
{
m_R.insertBackByOuterInner(col, nonzeroCol) = beta;
nonzeroCol++;
// The householder coefficient
m_hcoeffs(col) = tau;
m_hcoeffs(nonzeroCol) = tau;
// Record the householder reflections
for (Index itq = 0; itq < nzcolQ; ++itq)
{
Index iQ = Qidx(itq);
m_Q.insertBackByOuterInnerUnordered(col,iQ) = tval(iQ);
m_Q.insertBackByOuterInnerUnordered(nonzeroCol,iQ) = tval(iQ);
tval(iQ) = Scalar(0.);
}
}
nonzeroCol++;
if(nonzeroCol<diagSize)
m_Q.startVec(nonzeroCol);
}
else
{
// Zero pivot found: move implicitly this column to the end
m_hcoeffs(col) = Scalar(0);
for (Index j = nonzeroCol; j < n-1; j++)
std::swap(m_pivotperm.indices()(j), m_pivotperm.indices()[j+1]);
@ -493,6 +517,8 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
}
}
m_hcoeffs.tail(diagSize-nonzeroCol).setZero();
// Finalize the column pointers of the sparse matrices R and Q
m_Q.finalize();
m_Q.makeCompressed();
@ -561,14 +587,16 @@ struct SparseQR_QProduct : ReturnByValue<SparseQR_QProduct<SparseQRType, Derived
template<typename DesType>
void evalTo(DesType& res) const
{
Index m = m_qr.rows();
Index n = m_qr.cols();
Index diagSize = (std::min)(m,n);
res = m_other;
if (m_transpose)
{
eigen_assert(m_qr.m_Q.rows() == m_other.rows() && "Non conforming object sizes");
//Compute res = Q' * other column by column
for(Index j = 0; j < res.cols(); j++){
for (Index k = 0; k < n; k++)
for (Index k = 0; k < diagSize; k++)
{
Scalar tau = Scalar(0);
tau = m_qr.m_Q.col(k).dot(res.col(j));
@ -581,10 +609,10 @@ struct SparseQR_QProduct : ReturnByValue<SparseQR_QProduct<SparseQRType, Derived
else
{
eigen_assert(m_qr.m_Q.rows() == m_other.rows() && "Non conforming object sizes");
// Compute res = Q' * other column by column
// Compute res = Q * other column by column
for(Index j = 0; j < res.cols(); j++)
{
for (Index k = n-1; k >=0; k--)
for (Index k = diagSize-1; k >=0; k--)
{
Scalar tau = Scalar(0);
tau = m_qr.m_Q.col(k).dot(res.col(j));
@ -618,7 +646,7 @@ struct SparseQRMatrixQReturnType : public EigenBase<SparseQRMatrixQReturnType<Sp
return SparseQRMatrixQTransposeReturnType<SparseQRType>(m_qr);
}
inline Index rows() const { return m_qr.rows(); }
inline Index cols() const { return m_qr.cols(); }
inline Index cols() const { return (std::min)(m_qr.rows(),m_qr.cols()); }
// To use for operations with the transpose of Q
SparseQRMatrixQTransposeReturnType<SparseQRType> transpose() const
{

2
gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdDeque.h vendored
View File

@ -11,7 +11,7 @@
#ifndef EIGEN_STDDEQUE_H
#define EIGEN_STDDEQUE_H
#include "Eigen/src/StlSupport/details.h"
#include "details.h"
// Define the explicit instantiation (e.g. necessary for the Intel compiler)
#if defined(__INTEL_COMPILER) || defined(__GNUC__)

2
gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdList.h vendored
View File

@ -10,7 +10,7 @@
#ifndef EIGEN_STDLIST_H
#define EIGEN_STDLIST_H
#include "Eigen/src/StlSupport/details.h"
#include "details.h"
// Define the explicit instantiation (e.g. necessary for the Intel compiler)
#if defined(__INTEL_COMPILER) || defined(__GNUC__)

2
gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdVector.h vendored
View File

@ -11,7 +11,7 @@
#ifndef EIGEN_STDVECTOR_H
#define EIGEN_STDVECTOR_H
#include "Eigen/src/StlSupport/details.h"
#include "details.h"
/**
* This section contains a convenience MACRO which allows an easy specialization of

3
gtsam/3rdparty/Eigen/blas/README.txt vendored
View File

@ -1,9 +1,6 @@
This directory contains a BLAS library built on top of Eigen.
This is currently a work in progress which is far to be ready for use,
but feel free to contribute to it if you wish.
This module is not built by default. In order to compile it, you need to
type 'make blas' from within your build dir.

4
gtsam/3rdparty/Eigen/cmake/EigenConfigureTesting.cmake vendored
View File

@ -41,7 +41,7 @@ endif()
# copy ctest properties, which currently
# o raise the warning levels
configure_file(${CMAKE_BINARY_DIR}/DartConfiguration.tcl ${CMAKE_BINARY_DIR}/DartConfiguration.tcl)
configure_file(${CMAKE_CURRENT_BINARY_DIR}/DartConfiguration.tcl ${CMAKE_BINARY_DIR}/DartConfiguration.tcl)
# restore default CMAKE_MAKE_PROGRAM
set(CMAKE_MAKE_PROGRAM ${CMAKE_MAKE_PROGRAM_SAVE})
@ -50,7 +50,7 @@ set(CMAKE_MAKE_PROGRAM ${CMAKE_MAKE_PROGRAM_SAVE})
set(CMAKE_MAKE_PROGRAM_SAVE)
set(EIGEN_MAKECOMMAND_PLACEHOLDER)
configure_file(${CMAKE_SOURCE_DIR}/CTestCustom.cmake.in ${CMAKE_BINARY_DIR}/CTestCustom.cmake)
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/CTestCustom.cmake.in ${CMAKE_BINARY_DIR}/CTestCustom.cmake)
# some documentation of this function would be nice
ei_init_testing()

8
gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt vendored
View File

@ -41,8 +41,8 @@ MatrixXd::Ones(rows,cols) // ones(rows,cols)
C.setOnes(rows,cols) // C = ones(rows,cols)
MatrixXd::Random(rows,cols) // rand(rows,cols)*2-1 // MatrixXd::Random returns uniform random numbers in (-1, 1).
C.setRandom(rows,cols) // C = rand(rows,cols)*2-1
VectorXd::LinSpace(size,low,high) // linspace(low,high,size)'
v.setLinSpace(size,low,high) // v = linspace(low,high,size)'
VectorXd::LinSpaced(size,low,high) // linspace(low,high,size)'
v.setLinSpaced(size,low,high) // v = linspace(low,high,size)'
// Matrix slicing and blocks. All expressions listed here are read/write.
@ -91,6 +91,8 @@ R.adjoint() // R'
R.transpose() // R.' or conj(R')
R.diagonal() // diag(R)
x.asDiagonal() // diag(x)
R.transpose().colwise().reverse(); // rot90(R)
R.conjugate() // conj(R)
// All the same as Matlab, but matlab doesn't have *= style operators.
// Matrix-vector. Matrix-matrix. Matrix-scalar.
@ -167,6 +169,8 @@ x.cross(y) // cross(x, y) Requires #include <Eigen/Geometry>
A.cast<double>(); // double(A)
A.cast<float>(); // single(A)
A.cast<int>(); // int32(A)
A.real(); // real(A)
A.imag(); // imag(A)
// if the original type equals destination type, no work is done
// Note that for most operations Eigen requires all operands to have the same type:

27
gtsam/3rdparty/Eigen/doc/LinearLeastSquares.dox vendored
View File

@ -1,27 +0,0 @@
namespace Eigen {
/** \eigenManualPage LinearLeastSquares Solving linear least squares problems
lede
\eigenAutoToc
\section LinearLeastSquaresCopied Copied
The best way to do least squares solving is with a SVD decomposition. Eigen provides one as the JacobiSVD class, and its solve()
is doing least-squares solving.
Here is an example:
<table class="example">
<tr><th>Example:</th><th>Output:</th></tr>
<tr>
<td>\include TutorialLinAlgSVDSolve.cpp </td>
<td>\verbinclude TutorialLinAlgSVDSolve.out </td>
</tr>
</table>
For more information, including faster but less reliable methods, read our page concentrating on \ref LinearLeastSquares "linear least squares problems".
*/
}

7
gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox vendored
View File

@ -62,6 +62,8 @@ run time. However, these assertions do cost time and can thus be turned off.
expect that any objects passed to it are aligned. This will turn off vectorization. Not defined by default.
- \b EIGEN_DONT_ALIGN_STATICALLY - disables alignment of arrays on the stack. Not defined by default, unless
\c EIGEN_DONT_ALIGN is defined.
- \b EIGEN_DONT_PARALLELIZE - if defined, this disables multi-threading. This is only relevant if you enabled OpenMP.
See \ref TopicMultiThreading for details.
- \b EIGEN_DONT_VECTORIZE - disables explicit vectorization when defined. Not defined by default, unless
alignment is disabled by %Eigen's platform test or the user defining \c EIGEN_DONT_ALIGN.
- \b EIGEN_FAST_MATH - enables some optimizations which might affect the accuracy of the result. This currently
@ -69,7 +71,10 @@ run time. However, these assertions do cost time and can thus be turned off.
Define it to 0 to disable.
- \b EIGEN_UNROLLING_LIMIT - defines the size of a loop to enable meta unrolling. Set it to zero to disable
unrolling. The size of a loop here is expressed in %Eigen's own notion of "number of FLOPS", it does not
correspond to the number of iterations or the number of instructions. The default is value 100.
correspond to the number of iterations or the number of instructions. The default is value 100.
- \b EIGEN_STACK_ALLOCATION_LIMIT - defines the maximum bytes for a buffer to be allocated on the stack. For internal
temporary buffers, dynamic memory allocation is employed as a fall back. For fixed-size matrices or arrays, exceeding
this threshold raises a compile time assertion. Use 0 to set no limit. Default is 128 KB.
\section TopicPreprocessorDirectivesPlugins Plugins

7
gtsam/3rdparty/Eigen/doc/TutorialSparse.dox vendored
View File

@ -253,12 +253,15 @@ SparseMatrix<double> A, B;
B = SparseMatrix<double>(A.transpose()) + A;
\endcode
Binary coefficient wise operators can also mix sparse and dense expressions:
Some binary coefficient-wise operators can also mix sparse and dense expressions:
\code
sm2 = sm1.cwiseProduct(dm1);
dm2 = sm1 + dm1;
dm1 += sm1;
\endcode
However, it is not yet possible to add a sparse and a dense matrix as in <tt>dm2 = sm1 + dm1</tt>.
Please write this as the equivalent <tt>dm2 = dm1; dm2 += sm1</tt> (we plan to lift this restriction
in the next release of %Eigen).
%Sparse expressions also support transposition:
\code

25
gtsam/3rdparty/Eigen/test/block.cpp vendored
View File

@ -10,6 +10,26 @@
#define EIGEN_NO_STATIC_ASSERT // otherwise we fail at compile time on unused paths
#include "main.h"
template<typename MatrixType, typename Index, typename Scalar>
typename Eigen::internal::enable_if<!NumTraits<typename MatrixType::Scalar>::IsComplex,typename MatrixType::Scalar>::type
block_real_only(const MatrixType &m1, Index r1, Index r2, Index c1, Index c2, const Scalar& s1) {
// check cwise-Functions:
VERIFY_IS_APPROX(m1.row(r1).cwiseMax(s1), m1.cwiseMax(s1).row(r1));
VERIFY_IS_APPROX(m1.col(c1).cwiseMin(s1), m1.cwiseMin(s1).col(c1));
VERIFY_IS_APPROX(m1.block(r1,c1,r2-r1+1,c2-c1+1).cwiseMin(s1), m1.cwiseMin(s1).block(r1,c1,r2-r1+1,c2-c1+1));
VERIFY_IS_APPROX(m1.block(r1,c1,r2-r1+1,c2-c1+1).cwiseMax(s1), m1.cwiseMax(s1).block(r1,c1,r2-r1+1,c2-c1+1));
return Scalar(0);
}
template<typename MatrixType, typename Index, typename Scalar>
typename Eigen::internal::enable_if<NumTraits<typename MatrixType::Scalar>::IsComplex,typename MatrixType::Scalar>::type
block_real_only(const MatrixType &, Index, Index, Index, Index, const Scalar&) {
return Scalar(0);
}
template<typename MatrixType> void block(const MatrixType& m)
{
typedef typename MatrixType::Index Index;
@ -37,6 +57,8 @@ template<typename MatrixType> void block(const MatrixType& m)
Index c1 = internal::random<Index>(0,cols-1);
Index c2 = internal::random<Index>(c1,cols-1);
block_real_only(m1, r1, r2, c1, c1, s1);
//check row() and col()
VERIFY_IS_EQUAL(m1.col(c1).transpose(), m1.transpose().row(c1));
//check operator(), both constant and non-constant, on row() and col()
@ -51,7 +73,8 @@ template<typename MatrixType> void block(const MatrixType& m)
VERIFY_IS_APPROX(m1.col(c1), m1_copy.col(c1) + s1 * m1_copy.col(c2));
m1.col(c1).col(0) += s1 * m1_copy.col(c2);
VERIFY_IS_APPROX(m1.col(c1), m1_copy.col(c1) + Scalar(2) * s1 * m1_copy.col(c2));
//check block()
Matrix<Scalar,Dynamic,Dynamic> b1(1,1); b1(0,0) = m1(r1,c1);

52
gtsam/3rdparty/Eigen/test/cholesky.cpp vendored
View File

@ -68,6 +68,7 @@ template<typename MatrixType> void cholesky(const MatrixType& m)
Index cols = m.cols();
typedef typename MatrixType::Scalar Scalar;
typedef typename NumTraits<Scalar>::Real RealScalar;
typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime> SquareMatrixType;
typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType;
@ -179,6 +180,57 @@ template<typename MatrixType> void cholesky(const MatrixType& m)
// restore
if(sign == -1)
symm = -symm;
// check matrices coming from linear constraints with Lagrange multipliers
if(rows>=3)
{
SquareMatrixType A = symm;
int c = internal::random<int>(0,rows-2);
A.bottomRightCorner(c,c).setZero();
// Make sure a solution exists:
vecX.setRandom();
vecB = A * vecX;
vecX.setZero();
ldltlo.compute(A);
VERIFY_IS_APPROX(A, ldltlo.reconstructedMatrix());
vecX = ldltlo.solve(vecB);
VERIFY_IS_APPROX(A * vecX, vecB);
}
// check non-full rank matrices
if(rows>=3)
{
int r = internal::random<int>(1,rows-1);
Matrix<Scalar,Dynamic,Dynamic> a = Matrix<Scalar,Dynamic,Dynamic>::Random(rows,r);
SquareMatrixType A = a * a.adjoint();
// Make sure a solution exists:
vecX.setRandom();
vecB = A * vecX;
vecX.setZero();
ldltlo.compute(A);
VERIFY_IS_APPROX(A, ldltlo.reconstructedMatrix());
vecX = ldltlo.solve(vecB);
VERIFY_IS_APPROX(A * vecX, vecB);
}
// check matrices with a wide spectrum
if(rows>=3)
{
RealScalar s = (std::min)(16,std::numeric_limits<RealScalar>::max_exponent10/8);
Matrix<Scalar,Dynamic,Dynamic> a = Matrix<Scalar,Dynamic,Dynamic>::Random(rows,rows);
Matrix<RealScalar,Dynamic,1> d = Matrix<RealScalar,Dynamic,1>::Random(rows);
for(int k=0; k<rows; ++k)
d(k) = d(k)*std::pow(RealScalar(10),internal::random<RealScalar>(-s,s));
SquareMatrixType A = a * d.asDiagonal() * a.adjoint();
// Make sure a solution exists:
vecX.setRandom();
vecB = A * vecX;
vecX.setZero();
ldltlo.compute(A);
VERIFY_IS_APPROX(A, ldltlo.reconstructedMatrix());
vecX = ldltlo.solve(vecB);
VERIFY_IS_APPROX(A * vecX, vecB);
}
}
// update/downdate

34
gtsam/3rdparty/Eigen/test/dynalloc.cpp vendored
View File

@ -53,7 +53,7 @@ void check_aligned_new()
void check_aligned_stack_alloc()
{
for(int i = 1; i < 1000; i++)
for(int i = 1; i < 400; i++)
{
ei_declare_aligned_stack_constructed_variable(float,p,i,0);
VERIFY(size_t(p)%ALIGNMENT==0);
@ -87,6 +87,32 @@ template<typename T> void check_dynaligned()
delete obj;
}
template<typename T> void check_custom_new_delete()
{
{
T* t = new T;
delete t;
}
{
std::size_t N = internal::random<std::size_t>(1,10);
T* t = new T[N];
delete[] t;
}
#ifdef EIGEN_ALIGN
{
T* t = static_cast<T *>((T::operator new)(sizeof(T)));
(T::operator delete)(t, sizeof(T));
}
{
T* t = static_cast<T *>((T::operator new)(sizeof(T)));
(T::operator delete)(t);
}
#endif
}
void test_dynalloc()
{
// low level dynamic memory allocation
@ -102,6 +128,12 @@ void test_dynalloc()
CALL_SUBTEST(check_dynaligned<Matrix4f>() );
CALL_SUBTEST(check_dynaligned<Vector4d>() );
CALL_SUBTEST(check_dynaligned<Vector4i>() );
CALL_SUBTEST(check_dynaligned<Vector8f>() );
CALL_SUBTEST( check_custom_new_delete<Vector4f>() );
CALL_SUBTEST( check_custom_new_delete<Vector2f>() );
CALL_SUBTEST( check_custom_new_delete<Matrix4f>() );
CALL_SUBTEST( check_custom_new_delete<MatrixXi>() );
}
// check static allocation, who knows ?

141
gtsam/3rdparty/Eigen/test/jacobisvd.cpp vendored
View File

@ -67,6 +67,7 @@ template<typename MatrixType, int QRPreconditioner>
void jacobisvd_solve(const MatrixType& m, unsigned int computationOptions)
{
typedef typename MatrixType::Scalar Scalar;
typedef typename MatrixType::RealScalar RealScalar;
typedef typename MatrixType::Index Index;
Index rows = m.rows();
Index cols = m.cols();
@ -81,9 +82,90 @@ void jacobisvd_solve(const MatrixType& m, unsigned int computationOptions)
RhsType rhs = RhsType::Random(rows, internal::random<Index>(1, cols));
JacobiSVD<MatrixType, QRPreconditioner> svd(m, computationOptions);
if(internal::is_same<RealScalar,double>::value) svd.setThreshold(1e-8);
else if(internal::is_same<RealScalar,float>::value) svd.setThreshold(1e-4);
SolutionType x = svd.solve(rhs);
RealScalar residual = (m*x-rhs).norm();
// Check that there is no significantly better solution in the neighborhood of x
if(!test_isMuchSmallerThan(residual,rhs.norm()))
{
// If the residual is very small, then we have an exact solution, so we are already good.
for(int k=0;k<x.rows();++k)
{
SolutionType y(x);
y.row(k).array() += 2*NumTraits<RealScalar>::epsilon();
RealScalar residual_y = (m*y-rhs).norm();
VERIFY( test_isApprox(residual_y,residual) || residual < residual_y );
y.row(k) = x.row(k).array() - 2*NumTraits<RealScalar>::epsilon();
residual_y = (m*y-rhs).norm();
VERIFY( test_isApprox(residual_y,residual) || residual < residual_y );
}
}
// evaluate normal equation which works also for least-squares solutions
VERIFY_IS_APPROX(m.adjoint()*m*x,m.adjoint()*rhs);
if(internal::is_same<RealScalar,double>::value)
{
// This test is not stable with single precision.
// This is probably because squaring m signicantly affects the precision.
VERIFY_IS_APPROX(m.adjoint()*m*x,m.adjoint()*rhs);
}
// check minimal norm solutions
{
// generate a full-rank m x n problem with m<n
enum {
RankAtCompileTime2 = ColsAtCompileTime==Dynamic ? Dynamic : (ColsAtCompileTime)/2+1,
RowsAtCompileTime3 = ColsAtCompileTime==Dynamic ? Dynamic : ColsAtCompileTime+1
};
typedef Matrix<Scalar, RankAtCompileTime2, ColsAtCompileTime> MatrixType2;
typedef Matrix<Scalar, RankAtCompileTime2, 1> RhsType2;
typedef Matrix<Scalar, ColsAtCompileTime, RankAtCompileTime2> MatrixType2T;
Index rank = RankAtCompileTime2==Dynamic ? internal::random<Index>(1,cols) : Index(RankAtCompileTime2);
MatrixType2 m2(rank,cols);
int guard = 0;
do {
m2.setRandom();
} while(m2.jacobiSvd().setThreshold(test_precision<Scalar>()).rank()!=rank && (++guard)<10);
VERIFY(guard<10);
RhsType2 rhs2 = RhsType2::Random(rank);
// use QR to find a reference minimal norm solution
HouseholderQR<MatrixType2T> qr(m2.adjoint());
Matrix<Scalar,Dynamic,1> tmp = qr.matrixQR().topLeftCorner(rank,rank).template triangularView<Upper>().adjoint().solve(rhs2);
tmp.conservativeResize(cols);
tmp.tail(cols-rank).setZero();
SolutionType x21 = qr.householderQ() * tmp;
// now check with SVD
JacobiSVD<MatrixType2, ColPivHouseholderQRPreconditioner> svd2(m2, computationOptions);
SolutionType x22 = svd2.solve(rhs2);
VERIFY_IS_APPROX(m2*x21, rhs2);
VERIFY_IS_APPROX(m2*x22, rhs2);
VERIFY_IS_APPROX(x21, x22);
// Now check with a rank deficient matrix
typedef Matrix<Scalar, RowsAtCompileTime3, ColsAtCompileTime> MatrixType3;
typedef Matrix<Scalar, RowsAtCompileTime3, 1> RhsType3;
Index rows3 = RowsAtCompileTime3==Dynamic ? internal::random<Index>(rank+1,2*cols) : Index(RowsAtCompileTime3);
Matrix<Scalar,RowsAtCompileTime3,Dynamic> C = Matrix<Scalar,RowsAtCompileTime3,Dynamic>::Random(rows3,rank);
MatrixType3 m3 = C * m2;
RhsType3 rhs3 = C * rhs2;
JacobiSVD<MatrixType3, ColPivHouseholderQRPreconditioner> svd3(m3, computationOptions);
SolutionType x3 = svd3.solve(rhs3);
if(svd3.rank()!=rank) {
std::cout << m3 << "\n\n";
std::cout << svd3.singularValues().transpose() << "\n";
std::cout << svd3.rank() << " == " << rank << "\n";
std::cout << x21.norm() << " == " << x3.norm() << "\n";
}
// VERIFY_IS_APPROX(m3*x3, rhs3);
VERIFY_IS_APPROX(m3*x21, rhs3);
VERIFY_IS_APPROX(m2*x3, rhs2);
VERIFY_IS_APPROX(x21, x3);
}
}
template<typename MatrixType, int QRPreconditioner>
@ -92,10 +174,9 @@ void jacobisvd_test_all_computation_options(const MatrixType& m)
if (QRPreconditioner == NoQRPreconditioner && m.rows() != m.cols())
return;
JacobiSVD<MatrixType, QRPreconditioner> fullSvd(m, ComputeFullU|ComputeFullV);
jacobisvd_check_full(m, fullSvd);
jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeFullU | ComputeFullV);
CALL_SUBTEST(( jacobisvd_check_full(m, fullSvd) ));
CALL_SUBTEST(( jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeFullU | ComputeFullV) ));
#if defined __INTEL_COMPILER
// remark #111: statement is unreachable
#pragma warning disable 111
@ -103,20 +184,20 @@ void jacobisvd_test_all_computation_options(const MatrixType& m)
if(QRPreconditioner == FullPivHouseholderQRPreconditioner)
return;
jacobisvd_compare_to_full(m, ComputeFullU, fullSvd);
jacobisvd_compare_to_full(m, ComputeFullV, fullSvd);
jacobisvd_compare_to_full(m, 0, fullSvd);
CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeFullU, fullSvd) ));
CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeFullV, fullSvd) ));
CALL_SUBTEST(( jacobisvd_compare_to_full(m, 0, fullSvd) ));
if (MatrixType::ColsAtCompileTime == Dynamic) {
// thin U/V are only available with dynamic number of columns
jacobisvd_compare_to_full(m, ComputeFullU|ComputeThinV, fullSvd);
jacobisvd_compare_to_full(m, ComputeThinV, fullSvd);
jacobisvd_compare_to_full(m, ComputeThinU|ComputeFullV, fullSvd);
jacobisvd_compare_to_full(m, ComputeThinU , fullSvd);
jacobisvd_compare_to_full(m, ComputeThinU|ComputeThinV, fullSvd);
jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeFullU | ComputeThinV);
jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeThinU | ComputeFullV);
jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeThinU | ComputeThinV);
CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeFullU|ComputeThinV, fullSvd) ));
CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeThinV, fullSvd) ));
CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeThinU|ComputeFullV, fullSvd) ));
CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeThinU , fullSvd) ));
CALL_SUBTEST(( jacobisvd_compare_to_full(m, ComputeThinU|ComputeThinV, fullSvd) ));
CALL_SUBTEST(( jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeFullU | ComputeThinV) ));
CALL_SUBTEST(( jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeThinU | ComputeFullV) ));
CALL_SUBTEST(( jacobisvd_solve<MatrixType, QRPreconditioner>(m, ComputeThinU | ComputeThinV) ));
// test reconstruction
typedef typename MatrixType::Index Index;
@ -129,12 +210,29 @@ void jacobisvd_test_all_computation_options(const MatrixType& m)
template<typename MatrixType>
void jacobisvd(const MatrixType& a = MatrixType(), bool pickrandom = true)
{
MatrixType m = pickrandom ? MatrixType::Random(a.rows(), a.cols()) : a;
MatrixType m = a;
if(pickrandom)
{
typedef typename MatrixType::Scalar Scalar;
typedef typename MatrixType::RealScalar RealScalar;
typedef typename MatrixType::Index Index;
Index diagSize = (std::min)(a.rows(), a.cols());
RealScalar s = std::numeric_limits<RealScalar>::max_exponent10/4;
s = internal::random<RealScalar>(1,s);
Matrix<RealScalar,Dynamic,1> d = Matrix<RealScalar,Dynamic,1>::Random(diagSize);
for(Index k=0; k<diagSize; ++k)
d(k) = d(k)*std::pow(RealScalar(10),internal::random<RealScalar>(-s,s));
m = Matrix<Scalar,Dynamic,Dynamic>::Random(a.rows(),diagSize) * d.asDiagonal() * Matrix<Scalar,Dynamic,Dynamic>::Random(diagSize,a.cols());
// cancel some coeffs
Index n = internal::random<Index>(0,m.size()-1);
for(Index i=0; i<n; ++i)
m(internal::random<Index>(0,m.rows()-1), internal::random<Index>(0,m.cols()-1)) = Scalar(0);
}
jacobisvd_test_all_computation_options<MatrixType, FullPivHouseholderQRPreconditioner>(m);
jacobisvd_test_all_computation_options<MatrixType, ColPivHouseholderQRPreconditioner>(m);
jacobisvd_test_all_computation_options<MatrixType, HouseholderQRPreconditioner>(m);
jacobisvd_test_all_computation_options<MatrixType, NoQRPreconditioner>(m);
CALL_SUBTEST(( jacobisvd_test_all_computation_options<MatrixType, FullPivHouseholderQRPreconditioner>(m) ));
CALL_SUBTEST(( jacobisvd_test_all_computation_options<MatrixType, ColPivHouseholderQRPreconditioner>(m) ));
CALL_SUBTEST(( jacobisvd_test_all_computation_options<MatrixType, HouseholderQRPreconditioner>(m) ));
CALL_SUBTEST(( jacobisvd_test_all_computation_options<MatrixType, NoQRPreconditioner>(m) ));
}
template<typename MatrixType> void jacobisvd_verify_assert(const MatrixType& m)
@ -328,6 +426,7 @@ void test_jacobisvd()
TEST_SET_BUT_UNUSED_VARIABLE(r)
TEST_SET_BUT_UNUSED_VARIABLE(c)
CALL_SUBTEST_10(( jacobisvd<MatrixXd>(MatrixXd(r,c)) ));
CALL_SUBTEST_7(( jacobisvd<MatrixXf>(MatrixXf(r,c)) ));
CALL_SUBTEST_8(( jacobisvd<MatrixXcd>(MatrixXcd(r,c)) ));
(void) r;

104
gtsam/3rdparty/Eigen/test/ref.cpp vendored
View File

@ -154,59 +154,79 @@ template<typename PlainObjectType> void check_const_correctness(const PlainObjec
VERIFY( !(Ref<ConstPlainObjectType, Aligned>::Flags & LvalueBit) );
}
EIGEN_DONT_INLINE void call_ref_1(Ref<VectorXf> ) { }
EIGEN_DONT_INLINE void call_ref_2(const Ref<const VectorXf>& ) { }
EIGEN_DONT_INLINE void call_ref_3(Ref<VectorXf,0,InnerStride<> > ) { }
EIGEN_DONT_INLINE void call_ref_4(const Ref<const VectorXf,0,InnerStride<> >& ) { }
EIGEN_DONT_INLINE void call_ref_5(Ref<MatrixXf,0,OuterStride<> > ) { }
EIGEN_DONT_INLINE void call_ref_6(const Ref<const MatrixXf,0,OuterStride<> >& ) { }
template<typename B>
EIGEN_DONT_INLINE void call_ref_1(Ref<VectorXf> a, const B &b) { VERIFY_IS_EQUAL(a,b); }
template<typename B>
EIGEN_DONT_INLINE void call_ref_2(const Ref<const VectorXf>& a, const B &b) { VERIFY_IS_EQUAL(a,b); }
template<typename B>
EIGEN_DONT_INLINE void call_ref_3(Ref<VectorXf,0,InnerStride<> > a, const B &b) { VERIFY_IS_EQUAL(a,b); }
template<typename B>
EIGEN_DONT_INLINE void call_ref_4(const Ref<const VectorXf,0,InnerStride<> >& a, const B &b) { VERIFY_IS_EQUAL(a,b); }
template<typename B>
EIGEN_DONT_INLINE void call_ref_5(Ref<MatrixXf,0,OuterStride<> > a, const B &b) { VERIFY_IS_EQUAL(a,b); }
template<typename B>
EIGEN_DONT_INLINE void call_ref_6(const Ref<const MatrixXf,0,OuterStride<> >& a, const B &b) { VERIFY_IS_EQUAL(a,b); }
template<typename B>
EIGEN_DONT_INLINE void call_ref_7(Ref<Matrix<float,Dynamic,3> > a, const B &b) { VERIFY_IS_EQUAL(a,b); }
void call_ref()
{
VectorXcf ca(10);
VectorXf a(10);
VectorXcf ca = VectorXcf::Random(10);
VectorXf a = VectorXf::Random(10);
RowVectorXf b = RowVectorXf::Random(10);
MatrixXf A = MatrixXf::Random(10,10);
RowVector3f c = RowVector3f::Random();
const VectorXf& ac(a);
VectorBlock<VectorXf> ab(a,0,3);
MatrixXf A(10,10);
const VectorBlock<VectorXf> abc(a,0,3);
VERIFY_EVALUATION_COUNT( call_ref_1(a), 0);
//call_ref_1(ac); // does not compile because ac is const
VERIFY_EVALUATION_COUNT( call_ref_1(ab), 0);
VERIFY_EVALUATION_COUNT( call_ref_1(a.head(4)), 0);
VERIFY_EVALUATION_COUNT( call_ref_1(abc), 0);
VERIFY_EVALUATION_COUNT( call_ref_1(A.col(3)), 0);
// call_ref_1(A.row(3)); // does not compile because innerstride!=1
VERIFY_EVALUATION_COUNT( call_ref_3(A.row(3)), 0);
VERIFY_EVALUATION_COUNT( call_ref_4(A.row(3)), 0);
//call_ref_1(a+a); // does not compile for obvious reason
VERIFY_EVALUATION_COUNT( call_ref_1(a,a), 0);
VERIFY_EVALUATION_COUNT( call_ref_1(b,b.transpose()), 0);
// call_ref_1(ac); // does not compile because ac is const
VERIFY_EVALUATION_COUNT( call_ref_1(ab,ab), 0);
VERIFY_EVALUATION_COUNT( call_ref_1(a.head(4),a.head(4)), 0);
VERIFY_EVALUATION_COUNT( call_ref_1(abc,abc), 0);
VERIFY_EVALUATION_COUNT( call_ref_1(A.col(3),A.col(3)), 0);
// call_ref_1(A.row(3)); // does not compile because innerstride!=1
VERIFY_EVALUATION_COUNT( call_ref_3(A.row(3),A.row(3).transpose()), 0);
VERIFY_EVALUATION_COUNT( call_ref_4(A.row(3),A.row(3).transpose()), 0);
// call_ref_1(a+a); // does not compile for obvious reason
VERIFY_EVALUATION_COUNT( call_ref_2(A*A.col(1)), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_2(ac.head(5)), 0);
VERIFY_EVALUATION_COUNT( call_ref_2(ac), 0);
VERIFY_EVALUATION_COUNT( call_ref_2(a), 0);
VERIFY_EVALUATION_COUNT( call_ref_2(ab), 0);
VERIFY_EVALUATION_COUNT( call_ref_2(a.head(4)), 0);
VERIFY_EVALUATION_COUNT( call_ref_2(a+a), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_2(ca.imag()), 1); // evaluated into a temp
MatrixXf tmp = A*A.col(1);
VERIFY_EVALUATION_COUNT( call_ref_2(A*A.col(1), tmp), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_2(ac.head(5),ac.head(5)), 0);
VERIFY_EVALUATION_COUNT( call_ref_2(ac,ac), 0);
VERIFY_EVALUATION_COUNT( call_ref_2(a,a), 0);
VERIFY_EVALUATION_COUNT( call_ref_2(ab,ab), 0);
VERIFY_EVALUATION_COUNT( call_ref_2(a.head(4),a.head(4)), 0);
tmp = a+a;
VERIFY_EVALUATION_COUNT( call_ref_2(a+a,tmp), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_2(ca.imag(),ca.imag()), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_4(ac.head(5)), 0);
VERIFY_EVALUATION_COUNT( call_ref_4(a+a), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_4(ca.imag()), 0);
VERIFY_EVALUATION_COUNT( call_ref_4(ac.head(5),ac.head(5)), 0);
tmp = a+a;
VERIFY_EVALUATION_COUNT( call_ref_4(a+a,tmp), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_4(ca.imag(),ca.imag()), 0);
VERIFY_EVALUATION_COUNT( call_ref_5(a), 0);
VERIFY_EVALUATION_COUNT( call_ref_5(a.head(3)), 0);
VERIFY_EVALUATION_COUNT( call_ref_5(A), 0);
// call_ref_5(A.transpose()); // does not compile
VERIFY_EVALUATION_COUNT( call_ref_5(A.block(1,1,2,2)), 0);
VERIFY_EVALUATION_COUNT( call_ref_5(a,a), 0);
VERIFY_EVALUATION_COUNT( call_ref_5(a.head(3),a.head(3)), 0);
VERIFY_EVALUATION_COUNT( call_ref_5(A,A), 0);
// call_ref_5(A.transpose()); // does not compile
VERIFY_EVALUATION_COUNT( call_ref_5(A.block(1,1,2,2),A.block(1,1,2,2)), 0);
VERIFY_EVALUATION_COUNT( call_ref_5(b,b), 0); // storage order do not match, but this is a degenerate case that should work
VERIFY_EVALUATION_COUNT( call_ref_5(a.row(3),a.row(3)), 0);
VERIFY_EVALUATION_COUNT( call_ref_6(a), 0);
VERIFY_EVALUATION_COUNT( call_ref_6(a.head(3)), 0);
VERIFY_EVALUATION_COUNT( call_ref_6(A.row(3)), 1); // evaluated into a temp thouth it could be avoided by viewing it as a 1xn matrix
VERIFY_EVALUATION_COUNT( call_ref_6(A+A), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_6(A), 0);
VERIFY_EVALUATION_COUNT( call_ref_6(A.transpose()), 1); // evaluated into a temp because the storage orders do not match
VERIFY_EVALUATION_COUNT( call_ref_6(A.block(1,1,2,2)), 0);
VERIFY_EVALUATION_COUNT( call_ref_6(a,a), 0);
VERIFY_EVALUATION_COUNT( call_ref_6(a.head(3),a.head(3)), 0);
VERIFY_EVALUATION_COUNT( call_ref_6(A.row(3),A.row(3)), 1); // evaluated into a temp thouth it could be avoided by viewing it as a 1xn matrix
tmp = A+A;
VERIFY_EVALUATION_COUNT( call_ref_6(A+A,tmp), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_6(A,A), 0);
VERIFY_EVALUATION_COUNT( call_ref_6(A.transpose(),A.transpose()), 1); // evaluated into a temp because the storage orders do not match
VERIFY_EVALUATION_COUNT( call_ref_6(A.block(1,1,2,2),A.block(1,1,2,2)), 0);
VERIFY_EVALUATION_COUNT( call_ref_7(c,c), 0);
}
void test_ref()

41
gtsam/3rdparty/Eigen/test/simplicial_cholesky.cpp vendored
View File

@ -11,26 +11,31 @@
template<typename T> void test_simplicial_cholesky_T()
{
SimplicialCholesky<SparseMatrix<T>, Lower> chol_colmajor_lower;
SimplicialCholesky<SparseMatrix<T>, Upper> chol_colmajor_upper;
SimplicialLLT<SparseMatrix<T>, Lower> llt_colmajor_lower;
SimplicialLDLT<SparseMatrix<T>, Upper> llt_colmajor_upper;
SimplicialLDLT<SparseMatrix<T>, Lower> ldlt_colmajor_lower;
SimplicialLDLT<SparseMatrix<T>, Upper> ldlt_colmajor_upper;
SimplicialCholesky<SparseMatrix<T>, Lower> chol_colmajor_lower_amd;
SimplicialCholesky<SparseMatrix<T>, Upper> chol_colmajor_upper_amd;
SimplicialLLT<SparseMatrix<T>, Lower> llt_colmajor_lower_amd;
SimplicialLLT<SparseMatrix<T>, Upper> llt_colmajor_upper_amd;
SimplicialLDLT<SparseMatrix<T>, Lower> ldlt_colmajor_lower_amd;
SimplicialLDLT<SparseMatrix<T>, Upper> ldlt_colmajor_upper_amd;
SimplicialLDLT<SparseMatrix<T>, Lower, NaturalOrdering<int> > ldlt_colmajor_lower_nat;
SimplicialLDLT<SparseMatrix<T>, Upper, NaturalOrdering<int> > ldlt_colmajor_upper_nat;
check_sparse_spd_solving(chol_colmajor_lower);
check_sparse_spd_solving(chol_colmajor_upper);
check_sparse_spd_solving(llt_colmajor_lower);
check_sparse_spd_solving(llt_colmajor_upper);
check_sparse_spd_solving(ldlt_colmajor_lower);
check_sparse_spd_solving(ldlt_colmajor_upper);
check_sparse_spd_solving(chol_colmajor_lower_amd);
check_sparse_spd_solving(chol_colmajor_upper_amd);
check_sparse_spd_solving(llt_colmajor_lower_amd);
check_sparse_spd_solving(llt_colmajor_upper_amd);
check_sparse_spd_solving(ldlt_colmajor_lower_amd);
check_sparse_spd_solving(ldlt_colmajor_upper_amd);
check_sparse_spd_determinant(chol_colmajor_lower);
check_sparse_spd_determinant(chol_colmajor_upper);
check_sparse_spd_determinant(llt_colmajor_lower);
check_sparse_spd_determinant(llt_colmajor_upper);
check_sparse_spd_determinant(ldlt_colmajor_lower);
check_sparse_spd_determinant(ldlt_colmajor_upper);
check_sparse_spd_determinant(chol_colmajor_lower_amd);
check_sparse_spd_determinant(chol_colmajor_upper_amd);
check_sparse_spd_determinant(llt_colmajor_lower_amd);
check_sparse_spd_determinant(llt_colmajor_upper_amd);
check_sparse_spd_determinant(ldlt_colmajor_lower_amd);
check_sparse_spd_determinant(ldlt_colmajor_upper_amd);
check_sparse_spd_solving(ldlt_colmajor_lower_nat);
check_sparse_spd_solving(ldlt_colmajor_upper_nat);
}
void test_simplicial_cholesky()

35
gtsam/3rdparty/Eigen/test/sparseqr.cpp vendored
View File

@ -2,24 +2,24 @@
// for linear algebra.
//
// Copyright (C) 2012 Desire Nuentsa Wakam <desire.nuentsa_wakam@inria.fr>
// Copyright (C) 2014 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
#include "sparse.h"
#include <Eigen/SparseQR>
template<typename MatrixType,typename DenseMat>
int generate_sparse_rectangular_problem(MatrixType& A, DenseMat& dA, int maxRows = 300, int maxCols = 300)
int generate_sparse_rectangular_problem(MatrixType& A, DenseMat& dA, int maxRows = 300, int maxCols = 150)
{
eigen_assert(maxRows >= maxCols);
typedef typename MatrixType::Scalar Scalar;
int rows = internal::random<int>(1,maxRows);
int cols = internal::random<int>(1,rows);
int cols = internal::random<int>(1,maxCols);
double density = (std::max)(8./(rows*cols), 0.01);
A.resize(rows,rows);
dA.resize(rows,rows);
A.resize(rows,cols);
dA.resize(rows,cols);
initSparse<Scalar>(density, dA, A,ForceNonZeroDiag);
A.makeCompressed();
int nop = internal::random<int>(0, internal::random<double>(0,1) > 0.5 ? cols/2 : 0);
@ -31,6 +31,13 @@ int generate_sparse_rectangular_problem(MatrixType& A, DenseMat& dA, int maxRows
A.col(j0) = s * A.col(j1);
dA.col(j0) = s * dA.col(j1);
}
// if(rows<cols) {
// A.conservativeResize(cols,cols);
// dA.conservativeResize(cols,cols);
// dA.bottomRows(cols-rows).setZero();
// }
return rows;
}
@ -42,11 +49,10 @@ template<typename Scalar> void test_sparseqr_scalar()
MatrixType A;
DenseMat dA;
DenseVector refX,x,b;
SparseQR<MatrixType, AMDOrdering<int> > solver;
SparseQR<MatrixType, COLAMDOrdering<int> > solver;
generate_sparse_rectangular_problem(A,dA);
int n = A.cols();
b = DenseVector::Random(n);
b = dA * DenseVector::Random(A.cols());
solver.compute(A);
if (solver.info() != Success)
{
@ -60,17 +66,19 @@ template<typename Scalar> void test_sparseqr_scalar()
std::cerr << "sparse QR factorization failed\n";
exit(0);
return;
}
}
VERIFY_IS_APPROX(A * x, b);
//Compare with a dense QR solver
ColPivHouseholderQR<DenseMat> dqr(dA);
refX = dqr.solve(b);
VERIFY_IS_EQUAL(dqr.rank(), solver.rank());
if(solver.rank()<A.cols())
VERIFY((dA * refX - b).norm() * 2 > (A * x - b).norm() );
else
if(solver.rank()==A.cols()) // full rank
VERIFY_IS_APPROX(x, refX);
// else
// VERIFY((dA * refX - b).norm() * 2 > (A * x - b).norm() );
// Compute explicitly the matrix Q
MatrixType Q, QtQ, idM;
@ -88,3 +96,4 @@ void test_sparseqr()
CALL_SUBTEST_2(test_sparseqr_scalar<std::complex<double> >());
}
}

12
gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h vendored
View File

@ -2,7 +2,7 @@
// for linear algebra.
//
// Copyright (C) 2011 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2012 Kolja Brix <brix@igpm.rwth-aaachen.de>
// Copyright (C) 2012, 2014 Kolja Brix <brix@igpm.rwth-aaachen.de>
//
// 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
@ -72,16 +72,20 @@ bool gmres(const MatrixType & mat, const Rhs & rhs, Dest & x, const Precondition
VectorType p0 = rhs - mat*x;
VectorType r0 = precond.solve(p0);
// RealScalar r0_sqnorm = r0.squaredNorm();
// is initial guess already good enough?
if(abs(r0.norm()) < tol) {
return true;
}
VectorType w = VectorType::Zero(restart + 1);
FMatrixType H = FMatrixType::Zero(m, restart + 1);
FMatrixType H = FMatrixType::Zero(m, restart + 1); // Hessenberg matrix
VectorType tau = VectorType::Zero(restart + 1);
std::vector < JacobiRotation < Scalar > > G(restart);
// generate first Householder vector
VectorType e;
VectorType e(m-1);
RealScalar beta;
r0.makeHouseholder(e, tau.coeffRef(0), beta);
w(0)=(Scalar) beta;

45
gtsam/3rdparty/Eigen/unsupported/test/autodiff.cpp vendored
View File

@ -127,46 +127,47 @@ template<typename Func> void forward_jacobian(const Func& f)
VERIFY_IS_APPROX(j, jref);
}
// TODO also check actual derivatives!
void test_autodiff_scalar()
{
std::cerr << foo<float>(1,2) << "\n";
Vector2f p = Vector2f::Random();
typedef AutoDiffScalar<Vector2f> AD;
AD ax(1,Vector2f::UnitX());
AD ay(2,Vector2f::UnitY());
AD ax(p.x(),Vector2f::UnitX());
AD ay(p.y(),Vector2f::UnitY());
AD res = foo<AD>(ax,ay);
std::cerr << res.value() << " <> "
<< res.derivatives().transpose() << "\n\n";
VERIFY_IS_APPROX(res.value(), foo(p.x(),p.y()));
}
// TODO also check actual derivatives!
void test_autodiff_vector()
{
std::cerr << foo<Vector2f>(Vector2f(1,2)) << "\n";
Vector2f p = Vector2f::Random();
typedef AutoDiffScalar<Vector2f> AD;
typedef Matrix<AD,2,1> VectorAD;
VectorAD p(AD(1),AD(-1));
p.x().derivatives() = Vector2f::UnitX();
p.y().derivatives() = Vector2f::UnitY();
VectorAD ap = p.cast<AD>();
ap.x().derivatives() = Vector2f::UnitX();
ap.y().derivatives() = Vector2f::UnitY();
AD res = foo<VectorAD>(p);
std::cerr << res.value() << " <> "
<< res.derivatives().transpose() << "\n\n";
AD res = foo<VectorAD>(ap);
VERIFY_IS_APPROX(res.value(), foo(p));
}
void test_autodiff_jacobian()
{
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST(( forward_jacobian(TestFunc1<double,2,2>()) ));
CALL_SUBTEST(( forward_jacobian(TestFunc1<double,2,3>()) ));
CALL_SUBTEST(( forward_jacobian(TestFunc1<double,3,2>()) ));
CALL_SUBTEST(( forward_jacobian(TestFunc1<double,3,3>()) ));
CALL_SUBTEST(( forward_jacobian(TestFunc1<double>(3,3)) ));
}
CALL_SUBTEST(( forward_jacobian(TestFunc1<double,2,2>()) ));
CALL_SUBTEST(( forward_jacobian(TestFunc1<double,2,3>()) ));
CALL_SUBTEST(( forward_jacobian(TestFunc1<double,3,2>()) ));
CALL_SUBTEST(( forward_jacobian(TestFunc1<double,3,3>()) ));
CALL_SUBTEST(( forward_jacobian(TestFunc1<double>(3,3)) ));
}
void test_autodiff()
{
test_autodiff_scalar();
test_autodiff_vector();
// test_autodiff_jacobian();
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( test_autodiff_scalar() );
CALL_SUBTEST_2( test_autodiff_vector() );
CALL_SUBTEST_3( test_autodiff_jacobian() );
}
}

4
gtsam/3rdparty/gtsam_eigen_includes.h.in vendored
View File

@ -17,6 +17,10 @@
#pragma once
#ifndef MKL_BLAS
#define MKL_BLAS MKL_DOMAIN_BLAS
#endif
#cmakedefine EIGEN_USE_MKL_ALL // This is also defined in config.h
#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/Dense>
#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/QR>

4
gtsam/3rdparty/metis-5.1.0/CMakeLists.txt vendored
View File

@ -9,7 +9,9 @@ if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
endif()
endif()
add_definitions(-Wno-unknown-pragmas)
if(NOT ("${CMAKE_C_COMPILER_ID}" MATCHES "MSVC" OR "${CMAKE_CXX_COMPILER_ID}" MATCHES "MSVC"))
#add_definitions(-Wno-unknown-pragmas)
endif()
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
if (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 4.6 OR CMAKE_CXX_COMPILER_VERSION VERSION_EQUAL 4.6)

3
gtsam/3rdparty/metis-5.1.0/GKlib/gk_arch.h vendored
View File

@ -59,9 +59,10 @@ typedef ptrdiff_t ssize_t;
#endif
#ifdef __MSC__
#if(_MSC_VER < 1700)
/* MSC does not have rint() function */
#define rint(x) ((int)((x)+0.5))
#endif
/* MSC does not have INFINITY defined */
#ifndef INFINITY
#define INFINITY FLT_MAX

3
gtsam/base/DerivedValue.h
View File

@ -16,8 +16,9 @@
*/
#pragma once
#include <boost/make_shared.hpp>
#include <gtsam/base/Value.h>
#include <boost/make_shared.hpp>
//////////////////
// The following includes windows.h in some MSVC versions, so we undef min, max, and ERROR

8
gtsam/base/LieMatrix.h
View File

@ -19,9 +19,9 @@
#include <cstdarg>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/DerivedValue.h>
#include <boost/serialization/nvp.hpp>
namespace gtsam {
@ -40,9 +40,12 @@ struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
/** initialize from a normal matrix */
LieMatrix(const Matrix& v) : Matrix(v) {}
// Currently TMP constructor causes ICE on MSVS 2013
#if (_MSC_VER < 1800)
/** initialize from a fixed size normal vector */
template<int M, int N>
LieMatrix(const Eigen::Matrix<double, M, N>& v) : Matrix(v) {}
#endif
/** constructor with size and initial data, row order ! */
LieMatrix(size_t m, size_t n, const double* const data) :
@ -82,6 +85,7 @@ struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
inline LieMatrix retract(const Vector& v) const {
if(v.size() != this->size())
throw std::invalid_argument("LieMatrix::retract called with Vector of incorrect size");
return LieMatrix(*this +
Eigen::Map<const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> >(
&v(0), this->rows(), this->cols()));
@ -153,7 +157,7 @@ struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
result.data(), p.rows(), p.cols()) = p;
return result;
}
/// @}
private:

5
gtsam/base/LieVector.h
View File

@ -33,10 +33,13 @@ struct LieVector : public Vector, public DerivedValue<LieVector> {
/** initialize from a normal vector */
LieVector(const Vector& v) : Vector(v) {}
// Currently TMP constructor causes ICE on MSVS 2013
#if (_MSC_VER < 1800)
/** initialize from a fixed size normal vector */
template<int N>
LieVector(const Eigen::Matrix<double, N, 1>& v) : Vector(v) {}
#endif
/** wrap a double */
LieVector(double d) : Vector((Vector(1) << d)) {}

3
gtsam/base/Matrix.cpp
View File

@ -543,8 +543,7 @@ Matrix collect(size_t nrMatrices, ...)
void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) {
const DenseIndex m = A.rows();
if (inf_mask) {
// only scale the first v.size() rows of A to support augmented Matrix
for (DenseIndex i=0; i<v.size(); ++i) {
for (DenseIndex i=0; i<m; ++i) {
const double& vi = v(i);
if (std::isfinite(vi))
A.row(i) *= vi;

3
gtsam/base/Matrix.h
View File

@ -398,7 +398,6 @@ GTSAM_EXPORT Matrix collect(size_t nrMatrices, ...);
* Arguments (Matrix, Vector) scales the columns,
* (Vector, Matrix) scales the rows
* @param inf_mask when true, will not scale with a NaN or inf value.
* The inplace version also allows v.size()<A.rows() and only scales the first v.size() rows of A.
*/
GTSAM_EXPORT void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask = false); // row
GTSAM_EXPORT Matrix vector_scale(const Vector& v, const Matrix& A, bool inf_mask = false); // row
@ -467,7 +466,7 @@ GTSAM_EXPORT Matrix Cayley(const Matrix& A);
/// Implementation of Cayley transform using fixed size matrices to let
/// Eigen do more optimization
template<int N>
Eigen::Matrix<double, N, N> Cayley(const Eigen::Matrix<double, N, N>& A) {
Eigen::Matrix<double, N, N> CayleyFixed(const Eigen::Matrix<double, N, N>& A) {
typedef Eigen::Matrix<double, N, N> FMat;
return (FMat::Identity() - A)*(FMat::Identity() + A).inverse();
}

38
gtsam/base/Value.h
View File

@ -36,18 +36,19 @@ namespace gtsam {
* Values can operate generically on Value objects, retracting or computing
* local coordinates for many Value objects of different types.
*
* When you implement retract_(), localCoordinates_(), and equals_(), we
* suggest first implementing versions of these functions that work directly
* with derived objects, then using the provided helper functions to
* implement the generic Value versions. This makes your implementation
* easier, and also improves performance in situations where the derived type
* is in fact known, such as in most implementations of \c evaluateError() in
* classes derived from NonlinearFactor.
* Inheriting from the DerivedValue class templated provides a generic implementation of
* the pure virtual functions retract_(), localCoordinates_(), and equals_(), eliminating
* the need to implement these functions in your class. Note that you must inherit from
* DerivedValue templated on the class you are defining. For example you cannot define
* the following
* \code
* class Rot3 : public DerivedValue<Point3>{ \\classdef }
* \endcode
*
* Using the above practice, here is an example of implementing a typical
* class derived from Value:
* \code
class Rot3 : public Value {
class GTSAM_EXPORT Rot3 : public DerivedValue<Rot3> {
public:
// Constructor, there is never a need to call the Value base class constructor.
Rot3() { ... }
@ -74,27 +75,6 @@ namespace gtsam {
// Math to implement 3D rotation localCoordinates, e.g. logarithm map
return Vector(result);
}
// Equals implementing the generic Value interface (virtual, implements Value::equals_())
virtual bool equals_(const Value& other, double tol = 1e-9) const {
// Call our provided helper function to call your Rot3-specific
// equals with appropriate casting.
return CallDerivedEquals(this, other, tol);
}
// retract implementing the generic Value interface (virtual, implements Value::retract_())
virtual std::auto_ptr<Value> retract_(const Vector& delta) const {
// Call our provided helper function to call your Rot3-specific
// retract and do the appropriate casting and allocation.
return CallDerivedRetract(this, delta);
}
// localCoordinates implementing the generic Value interface (virtual, implements Value::localCoordinates_())
virtual Vector localCoordinates_(const Value& value) const {
// Call our provided helper function to call your Rot3-specific
// localCoordinates and do the appropriate casting.
return CallDerivedLocalCoordinates(this, value);
}
};
\endcode
*/

54
gtsam/base/Vector.cpp
View File

@ -30,55 +30,11 @@
#include <gtsam/base/Vector.h>
//#ifdef WIN32
//#include <Windows.h>
//#endif
using namespace std;
namespace gtsam {
/* ************************************************************************* */
void odprintf_(const char *format, ostream& stream, ...) {
char buf[4096], *p = buf;
va_list args;
va_start(args, stream);
#ifdef WIN32
_vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
#else
vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
#endif
va_end(args);
//#ifdef WIN32
// OutputDebugString(buf);
//#else
stream << buf;
//#endif
}
/* ************************************************************************* */
void odprintf(const char *format, ...) {
char buf[4096], *p = buf;
va_list args;
va_start(args, format);
#ifdef WIN32
_vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
#else
vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
#endif
va_end(args);
//#ifdef WIN32
// OutputDebugString(buf);
//#else
cout << buf;
//#endif
}
/* ************************************************************************* */
bool zero(const Vector& v) {
bool result = true;
@ -101,10 +57,12 @@ Vector delta(size_t n, size_t i, double value) {
/* ************************************************************************* */
void print(const Vector& v, const string& s, ostream& stream) {
size_t n = v.size();
odprintf_("%s [", stream, s.c_str());
for(size_t i=0; i<n; i++)
odprintf_("%g%s", stream, v[i], (i<n-1 ? "; " : ""));
odprintf_("];\n", stream);
stream << s << "[";
for(size_t i=0; i<n; i++) {
stream << setprecision(9) << v(i) << (i<n-1 ? "; " : "");
}
stream << "];" << endl;
}
/* ************************************************************************* */

5
gtsam/base/Vector.h
View File

@ -41,11 +41,6 @@ typedef Eigen::Matrix<double, 6, 1> Vector6;
typedef Eigen::VectorBlock<Vector> SubVector;
typedef Eigen::VectorBlock<const Vector> ConstSubVector;
/**
* An auxiliary function to printf for Win32 compatibility, added by Kai
*/
GTSAM_EXPORT void odprintf(const char *format, ...);
/**
* Create vector initialized to a constant value
* @param n is the size of the vector

24
gtsam/base/tests/testMatrix.cpp
View File

@ -1127,6 +1127,12 @@ TEST( matrix, svd2 )
svd(sampleA, U, s, V);
// take care of sign ambiguity
if (U(0, 1) > 0) {
U = -U;
V = -V;
}
EXPECT(assert_equal(expectedU,U));
EXPECT(assert_equal(expected_s,s,1e-9));
EXPECT(assert_equal(expectedV,V));
@ -1143,6 +1149,13 @@ TEST( matrix, svd3 )
Matrix expectedV = (Matrix(3, 2) << 0.,1.,0.,0.,-1.,0.);
svd(sampleAt, U, s, V);
// take care of sign ambiguity
if (U(0, 0) > 0) {
U = -U;
V = -V;
}
Matrix S = diag(s);
Matrix t = U * S;
Matrix Vt = trans(V);
@ -1176,6 +1189,17 @@ TEST( matrix, svd4 )
0.6723, 0.7403);
svd(A, U, s, V);
// take care of sign ambiguity
if (U(0, 0) < 0) {
U.col(0) = -U.col(0);
V.col(0) = -V.col(0);
}
if (U(0, 1) < 0) {
U.col(1) = -U.col(1);
V.col(1) = -V.col(1);
}
Matrix reconstructed = U * diag(s) * trans(V);
EXPECT(assert_equal(A, reconstructed, 1e-4));

4
gtsam/base/types.h
View File

@ -299,6 +299,8 @@ namespace gtsam {
// Define some common g++ functions and macros we use that MSVC does not have
#if (_MSC_VER < 1800)
#include <boost/math/special_functions/fpclassify.hpp>
namespace std {
template<typename T> inline int isfinite(T a) {
@ -309,6 +311,8 @@ namespace std {
return (int)boost::math::isinf(a); }
}
#endif
#include <boost/math/constants/constants.hpp>
#ifndef M_PI
#define M_PI (boost::math::constants::pi<double>())

2
gtsam/geometry/Cal3Bundler.h
View File

@ -53,6 +53,8 @@ public:
*/
Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0);
virtual ~Cal3Bundler() {}
/// @}
/// @name Testable
/// @{

146
gtsam/geometry/Cal3DS2.cpp
View File

@ -23,24 +23,9 @@
namespace gtsam {
/* ************************************************************************* */
Cal3DS2::Cal3DS2(const Vector &v):
fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){}
/* ************************************************************************* */
Matrix Cal3DS2::K() const {
return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
}
/* ************************************************************************* */
Vector Cal3DS2::vector() const {
return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_);
}
/* ************************************************************************* */
void Cal3DS2::print(const std::string& s_) const {
gtsam::print(K(), s_ + ".K");
gtsam::print(Vector(k()), s_ + ".k");
Base::print(s_);
}
/* ************************************************************************* */
@ -52,135 +37,6 @@ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
return true;
}
/* ************************************************************************* */
static Eigen::Matrix<double, 2, 9> D2dcalibration(double x, double y, double xx,
double yy, double xy, double rr, double r4, double pnx, double pny,
const Eigen::Matrix<double, 2, 2>& DK) {
Eigen::Matrix<double, 2, 5> DR1;
DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 4> DR2;
DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
y * rr, y * r4, rr + 2 * yy, 2 * xy;
Eigen::Matrix<double, 2, 9> D;
D << DR1, DK * DR2;
return D;
}
/* ************************************************************************* */
static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
double g, double k1, double k2, double p1, double p2,
const Eigen::Matrix<double, 2, 2>& DK) {
const double drdx = 2. * x;
const double drdy = 2. * y;
const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
const double dgdy = k1 * drdy + k2 * 2. * rr * drdy;
// Dx = 2*p1*xy + p2*(rr+2*xx);
// Dy = 2*p2*xy + p1*(rr+2*yy);
const double dDxdx = 2. * p1 * y + p2 * (drdx + 4. * x);
const double dDxdy = 2. * p1 * x + p2 * drdy;
const double dDydx = 2. * p2 * y + p1 * drdx;
const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
Eigen::Matrix<double, 2, 2> DR;
DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
y * dgdx + dDydx, g + y * dgdy + dDydy;
return DK * DR;
}
/* ************************************************************************* */
Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
// rr = x^2 + y^2;
// g = (1 + k(1)*rr + k(2)*rr^2);
// dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)];
// pi(:,i) = g * pn(:,i) + dp;
const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y;
const double rr = xx + yy;
const double r4 = rr * rr;
const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor
// tangential component
const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx);
const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy);
// Radial and tangential distortion applied
const double pnx = g * x + dx;
const double pny = g * y + dy;
Eigen::Matrix<double, 2, 2> DK;
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
// Derivative for calibration
if (H1)
*H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
// Derivative for points
if (H2)
*H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
// Regular uncalibrate after distortion
return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
}
/* ************************************************************************* */
Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const {
// Use the following fixed point iteration to invert the radial distortion.
// pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t})
const Point2 invKPi ((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
(1 / fy_) * (pi.y() - v0_));
// initialize by ignoring the distortion at all, might be problematic for pixels around boundary
Point2 pn = invKPi;
// iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 10;
int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) {
if (uncalibrate(pn).distance(pi) <= tol) break;
const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
const double rr = xx + yy;
const double g = (1 + k1_ * rr + k2_ * rr * rr);
const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
pn = (invKPi - Point2(dx, dy)) / g;
}
if ( iteration >= maxIterations )
throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization");
return pn;
}
/* ************************************************************************* */
Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x * x, yy = y * y;
const double rr = xx + yy;
const double r4 = rr * rr;
const double g = (1 + k1_ * rr + k2_ * r4);
Eigen::Matrix<double, 2, 2> DK;
DK << fx_, s_, 0.0, fy_;
return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
}
/* ************************************************************************* */
Matrix Cal3DS2::D2d_calibration(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y;
const double rr = xx + yy;
const double r4 = rr * rr;
const double g = (1 + k1_ * rr + k2_ * r4);
const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
const double pnx = g * x + dx;
const double pny = g * y + dy;
Eigen::Matrix<double, 2, 2> DK;
DK << fx_, s_, 0.0, fy_;
return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
}
/* ************************************************************************* */
Cal3DS2 Cal3DS2::retract(const Vector& d) const {
return Cal3DS2(vector() + d);

86
gtsam/geometry/Cal3DS2.h
View File

@ -11,7 +11,7 @@
/**
* @file Cal3DS2.h
* @brief Calibration of a camera with radial distortion
* @brief Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base
* @date Feb 28, 2010
* @author ydjian
*/
@ -19,7 +19,7 @@
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Cal3DS2_Base.h>
namespace gtsam {
@ -37,34 +37,29 @@ namespace gtsam {
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
* pi = K*pn
*/
class GTSAM_EXPORT Cal3DS2 : public DerivedValue<Cal3DS2> {
class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base, public DerivedValue<Cal3DS2> {
protected:
double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
double k1_, k2_ ; // radial 2nd-order and 4th-order
double p1_, p2_ ; // tangential distortion
typedef Cal3DS2_Base Base;
public:
Matrix K() const ;
Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
Vector vector() const ;
/// @name Standard Constructors
/// @{
/// Default Constructor with only unit focal length
Cal3DS2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {}
Cal3DS2() : Base() {}
Cal3DS2(double fx, double fy, double s, double u0, double v0,
double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
Base(fx, fy, s, u0, v0, k1, k2, p1, p2) {}
virtual ~Cal3DS2() {}
/// @}
/// @name Advanced Constructors
/// @{
Cal3DS2(const Vector &v) ;
Cal3DS2(const Vector &v) : Base(v) {}
/// @}
/// @name Testable
@ -76,57 +71,6 @@ public:
/// assert equality up to a tolerance
bool equals(const Cal3DS2& K, double tol = 10e-9) const;
/// @}
/// @name Standard Interface
/// @{
/// focal length x
inline double fx() const { return fx_;}
/// focal length x
inline double fy() const { return fy_;}
/// skew
inline double skew() const { return s_;}
/// image center in x
inline double px() const { return u0_;}
/// image center in y
inline double py() const { return v0_;}
/// First distortion coefficient
inline double k1() const { return k1_;}
/// Second distortion coefficient
inline double k2() const { return k2_;}
/// First tangential distortion coefficient
inline double p1() const { return p1_;}
/// Second tangential distortion coefficient
inline double p2() const { return p2_;}
/**
* convert intrinsic coordinates xy to (distorted) image coordinates uv
* @param p point in intrinsic coordinates
* @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in (distorted) image coordinates
*/
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> Dcal = boost::none,
boost::optional<Matrix&> Dp = boost::none) const ;
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
/// Derivative of uncalibrate wrpt intrinsic coordinates
Matrix D2d_intrinsic(const Point2& p) const ;
/// Derivative of uncalibrate wrpt the calibration parameters
Matrix D2d_calibration(const Point2& p) const ;
/// @}
/// @name Manifold
/// @{
@ -156,18 +100,10 @@ private:
{
ar & boost::serialization::make_nvp("Cal3DS2",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(fx_);
ar & BOOST_SERIALIZATION_NVP(fy_);
ar & BOOST_SERIALIZATION_NVP(s_);
ar & BOOST_SERIALIZATION_NVP(u0_);
ar & BOOST_SERIALIZATION_NVP(v0_);
ar & BOOST_SERIALIZATION_NVP(k1_);
ar & BOOST_SERIALIZATION_NVP(k2_);
ar & BOOST_SERIALIZATION_NVP(p1_);
ar & BOOST_SERIALIZATION_NVP(p2_);
ar & boost::serialization::make_nvp("Cal3DS2",
boost::serialization::base_object<Cal3DS2_Base>(*this));
}
/// @}
};

187
gtsam/geometry/Cal3DS2_Base.cpp Normal file
View File

@ -0,0 +1,187 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Cal3DS2_Base.cpp
* @date Feb 28, 2010
* @author ydjian
*/
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Cal3DS2_Base.h>
namespace gtsam {
/* ************************************************************************* */
Cal3DS2_Base::Cal3DS2_Base(const Vector &v):
fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){}
/* ************************************************************************* */
Matrix Cal3DS2_Base::K() const {
return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
}
/* ************************************************************************* */
Vector Cal3DS2_Base::vector() const {
return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_);
}
/* ************************************************************************* */
void Cal3DS2_Base::print(const std::string& s_) const {
gtsam::print(K(), s_ + ".K");
gtsam::print(Vector(k()), s_ + ".k");
}
/* ************************************************************************* */
bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const {
if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol ||
fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol ||
fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol)
return false;
return true;
}
/* ************************************************************************* */
static Eigen::Matrix<double, 2, 9> D2dcalibration(double x, double y, double xx,
double yy, double xy, double rr, double r4, double pnx, double pny,
const Eigen::Matrix<double, 2, 2>& DK) {
Eigen::Matrix<double, 2, 5> DR1;
DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 4> DR2;
DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
y * rr, y * r4, rr + 2 * yy, 2 * xy;
Eigen::Matrix<double, 2, 9> D;
D << DR1, DK * DR2;
return D;
}
/* ************************************************************************* */
static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
double g, double k1, double k2, double p1, double p2,
const Eigen::Matrix<double, 2, 2>& DK) {
const double drdx = 2. * x;
const double drdy = 2. * y;
const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
const double dgdy = k1 * drdy + k2 * 2. * rr * drdy;
// Dx = 2*p1*xy + p2*(rr+2*xx);
// Dy = 2*p2*xy + p1*(rr+2*yy);
const double dDxdx = 2. * p1 * y + p2 * (drdx + 4. * x);
const double dDxdy = 2. * p1 * x + p2 * drdy;
const double dDydx = 2. * p2 * y + p1 * drdx;
const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
Eigen::Matrix<double, 2, 2> DR;
DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
y * dgdx + dDydx, g + y * dgdy + dDydy;
return DK * DR;
}
/* ************************************************************************* */
Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
// rr = x^2 + y^2;
// g = (1 + k(1)*rr + k(2)*rr^2);
// dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)];
// pi(:,i) = g * pn(:,i) + dp;
const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y;
const double rr = xx + yy;
const double r4 = rr * rr;
const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor
// tangential component
const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx);
const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy);
// Radial and tangential distortion applied
const double pnx = g * x + dx;
const double pny = g * y + dy;
Eigen::Matrix<double, 2, 2> DK;
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
// Derivative for calibration
if (H1)
*H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
// Derivative for points
if (H2)
*H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
// Regular uncalibrate after distortion
return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
}
/* ************************************************************************* */
Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
// Use the following fixed point iteration to invert the radial distortion.
// pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t})
const Point2 invKPi ((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
(1 / fy_) * (pi.y() - v0_));
// initialize by ignoring the distortion at all, might be problematic for pixels around boundary
Point2 pn = invKPi;
// iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 10;
int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) {
if (uncalibrate(pn).distance(pi) <= tol) break;
const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
const double rr = xx + yy;
const double g = (1 + k1_ * rr + k2_ * rr * rr);
const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
pn = (invKPi - Point2(dx, dy)) / g;
}
if ( iteration >= maxIterations )
throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization");
return pn;
}
/* ************************************************************************* */
Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x * x, yy = y * y;
const double rr = xx + yy;
const double r4 = rr * rr;
const double g = (1 + k1_ * rr + k2_ * r4);
Eigen::Matrix<double, 2, 2> DK;
DK << fx_, s_, 0.0, fy_;
return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
}
/* ************************************************************************* */
Matrix Cal3DS2_Base::D2d_calibration(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y;
const double rr = xx + yy;
const double r4 = rr * rr;
const double g = (1 + k1_ * rr + k2_ * r4);
const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
const double pnx = g * x + dx;
const double pny = g * y + dy;
Eigen::Matrix<double, 2, 2> DK;
DK << fx_, s_, 0.0, fy_;
return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
}
}
/* ************************************************************************* */

158
gtsam/geometry/Cal3DS2_Base.h Normal file
View File

@ -0,0 +1,158 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Cal3DS2.h
* @brief Calibration of a camera with radial distortion
* @date Feb 28, 2010
* @author ydjian
*/
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
/**
* @brief Calibration of a camera with radial distortion
* @addtogroup geometry
* \nosubgrouping
*
* Uses same distortionmodel as OpenCV, with
* http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
* but using only k1,k2,p1, and p2 coefficients.
* K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
* rr = Pn.x^2 + Pn.y^2
* \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
* pi = K*pn
*/
class GTSAM_EXPORT Cal3DS2_Base {
protected:
double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
double k1_, k2_ ; // radial 2nd-order and 4th-order
double p1_, p2_ ; // tangential distortion
public:
Matrix K() const ;
Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
Vector vector() const ;
/// @name Standard Constructors
/// @{
/// Default Constructor with only unit focal length
Cal3DS2_Base() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {}
Cal3DS2_Base(double fx, double fy, double s, double u0, double v0,
double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
/// @}
/// @name Advanced Constructors
/// @{
Cal3DS2_Base(const Vector &v) ;
/// @}
/// @name Testable
/// @{
/// print with optional string
void print(const std::string& s = "") const ;
/// assert equality up to a tolerance
bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;
/// @}
/// @name Standard Interface
/// @{
/// focal length x
inline double fx() const { return fx_;}
/// focal length x
inline double fy() const { return fy_;}
/// skew
inline double skew() const { return s_;}
/// image center in x
inline double px() const { return u0_;}
/// image center in y
inline double py() const { return v0_;}
/// First distortion coefficient
inline double k1() const { return k1_;}
/// Second distortion coefficient
inline double k2() const { return k2_;}
/// First tangential distortion coefficient
inline double p1() const { return p1_;}
/// Second tangential distortion coefficient
inline double p2() const { return p2_;}
/**
* convert intrinsic coordinates xy to (distorted) image coordinates uv
* @param p point in intrinsic coordinates
* @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in (distorted) image coordinates
*/
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> Dcal = boost::none,
boost::optional<Matrix&> Dp = boost::none) const ;
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
/// Derivative of uncalibrate wrpt intrinsic coordinates
Matrix D2d_intrinsic(const Point2& p) const ;
/// Derivative of uncalibrate wrpt the calibration parameters
Matrix D2d_calibration(const Point2& p) const ;
private:
/// @}
/// @name Advanced Interface
/// @{
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(fx_);
ar & BOOST_SERIALIZATION_NVP(fy_);
ar & BOOST_SERIALIZATION_NVP(s_);
ar & BOOST_SERIALIZATION_NVP(u0_);
ar & BOOST_SERIALIZATION_NVP(v0_);
ar & BOOST_SERIALIZATION_NVP(k1_);
ar & BOOST_SERIALIZATION_NVP(k2_);
ar & BOOST_SERIALIZATION_NVP(p1_);
ar & BOOST_SERIALIZATION_NVP(p2_);
}
/// @}
};
}

14
gtsam/geometry/Cal3Unified.h
View File

@ -22,8 +22,8 @@
#pragma once
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Cal3DS2_Base.h>
#include <gtsam/base/DerivedValue.h>
namespace gtsam {
@ -40,10 +40,10 @@ namespace gtsam {
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
* pi = K*pn
*/
class GTSAM_EXPORT Cal3Unified : public Cal3DS2 {
class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base, public DerivedValue<Cal3Unified> {
typedef Cal3Unified This;
typedef Cal3DS2 Base;
typedef Cal3DS2_Base Base;
private:
@ -90,7 +90,7 @@ public:
/**
* convert intrinsic coordinates xy to image coordinates uv
* @param p point in intrinsic coordinates
* @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters
* @param Dcal optional 2*10 Jacobian wrpt Cal3Unified parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
@ -135,7 +135,9 @@ private:
void serialize(Archive & ar, const unsigned int version)
{
ar & boost::serialization::make_nvp("Cal3Unified",
boost::serialization::base_object<Cal3DS2>(*this));
boost::serialization::base_object<Value>(*this));
ar & boost::serialization::make_nvp("Cal3Unified",
boost::serialization::base_object<Cal3DS2_Base>(*this));
ar & BOOST_SERIALIZATION_NVP(xi_);
}

10
gtsam/geometry/Cal3_S2.h
View File

@ -165,6 +165,16 @@ public:
*/
Vector3 calibrate(const Vector3& p) const;
/// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q)
inline Cal3_S2 between(const Cal3_S2& q,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = -eye(5);
if(H2) *H2 = eye(5);
return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_);
}
/// @}
/// @name Manifold
/// @{

4
gtsam/geometry/Rot3M.cpp
View File

@ -240,7 +240,7 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
return retractCayley(omega);
} else if(mode == Rot3::SLOW_CAYLEY) {
Matrix Omega = skewSymmetric(omega);
return (*this)*Cayley<3>(-Omega/2);
return (*this)*CayleyFixed<3>(-Omega/2);
} else {
assert(false);
exit(1);
@ -269,7 +269,7 @@ Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const
// Create a fixed-size matrix
Eigen::Matrix3d A(between(T).matrix());
// using templated version of Cayley
Eigen::Matrix3d Omega = Cayley<3>(A);
Eigen::Matrix3d Omega = CayleyFixed<3>(A);
return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0));
} else {
assert(false);

32
gtsam/geometry/Rot3Q.cpp
View File

@ -21,6 +21,7 @@
#include <boost/math/constants/constants.hpp>
#include <gtsam/geometry/Rot3.h>
#include <cmath>
using namespace std;
@ -120,14 +121,31 @@ namespace gtsam {
}
/* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation
Vector3 Rot3::Logmap(const Rot3& R) {
Eigen::AngleAxisd angleAxis(R.quaternion_);
if(angleAxis.angle() > M_PI) // Important: use the smallest possible
angleAxis.angle() -= 2.0*M_PI; // angle, e.g. no more than PI, to keep
if(angleAxis.angle() < -M_PI) // error continuous.
angleAxis.angle() += 2.0*M_PI;
return angleAxis.axis() * angleAxis.angle();
using std::acos;
using std::sqrt;
static const double twoPi = 2.0 * M_PI,
// define these compile time constants to avoid std::abs:
NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10;
const Quaternion& q = R.quaternion_;
const double qw = q.w();
if (qw > NearlyOne) {
// Taylor expansion of (angle / s) at 1
return (2 - 2 * (qw - 1) / 3) * q.vec();
} else if (qw < NearlyNegativeOne) {
// Angle is zero, return zero vector
return Vector3::Zero();
} else {
// Normal, away from zero case
double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
// Important: convert to [-pi,pi] to keep error continuous
if (angle > M_PI)
angle -= twoPi;
else if (angle < -M_PI)
angle += twoPi;
return (angle / s) * q.vec();
}
}
/* ************************************************************************* */

22
gtsam/geometry/Unit3.cpp
View File

@ -21,7 +21,16 @@
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Point2.h>
#include <boost/random/mersenne_twister.hpp>
#ifdef __clang__
# pragma clang diagnostic push
# pragma clang diagnostic ignored "-Wunused-variable"
#endif
#include <boost/random/uniform_on_sphere.hpp>
#ifdef __clang__
# pragma clang diagnostic pop
#endif
#include <boost/random/variate_generator.hpp>
#include <iostream>
@ -58,11 +67,11 @@ Unit3 Unit3::Random(boost::mt19937 & rng) {
}
/* ************************************************************************* */
const Matrix& Unit3::basis() const {
const Unit3::Matrix32& Unit3::basis() const {
// Return cached version if exists
if (B_.rows() == 3)
return B_;
if (B_)
return *B_;
// Get the axis of rotation with the minimum projected length of the point
Point3 axis;
@ -83,9 +92,9 @@ const Matrix& Unit3::basis() const {
b2 = b2 / b2.norm();
// Create the basis matrix
B_ = Matrix(3, 2);
B_ << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
return B_;
B_.reset(Unit3::Matrix32());
(*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
return *B_;
}
/* ************************************************************************* */
@ -101,6 +110,7 @@ Matrix Unit3::skew() const {
/* ************************************************************************* */
Vector Unit3::error(const Unit3& q, boost::optional<Matrix&> H) const {
// 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
Matrix Bt = basis().transpose();
Vector xi = Bt * q.p_.vector();
if (H)

7
gtsam/geometry/Unit3.h
View File

@ -23,6 +23,7 @@
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/DerivedValue.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/optional.hpp>
namespace gtsam {
@ -31,8 +32,10 @@ class GTSAM_EXPORT Unit3: public DerivedValue<Unit3> {
private:
typedef Eigen::Matrix<double,3,2> Matrix32;
Point3 p_; ///< The location of the point on the unit sphere
mutable Matrix B_; ///< Cached basis
mutable boost::optional<Matrix32> B_; ///< Cached basis
public:
@ -84,7 +87,7 @@ public:
* It is a 3*2 matrix [b1 b2] composed of two orthogonal directions
* tangent to the sphere at the current direction.
*/
const Matrix& basis() const;
const Matrix32& basis() const;
/// Return skew-symmetric associated with 3D point on unit sphere
Matrix skew() const;

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