diff --git a/gtsam/DESCR b/gtsam/DESCR
deleted file mode 100644
index 02e603edf1c9aa514e995ed8564ea23acbbde3f7..0000000000000000000000000000000000000000
--- a/gtsam/DESCR
+++ /dev/null
@@ -1 +0,0 @@
-Georgia Tech Smoothing And Mapping Library
diff --git a/gtsam/Makefile b/gtsam/Makefile
deleted file mode 100644
index 83ffcbadf233dde8930b3513870e0417074a2bfb..0000000000000000000000000000000000000000
--- a/gtsam/Makefile
+++ /dev/null
@@ -1,51 +0,0 @@
-# robotpkg Makefile for: wip/gtsam
-# Created: Ellon Paiva Mendes <emendes@laas.fr>, 5 Nov. 2014
-#
-
-DISTNAME= gtsam-3.1.0
-EXTRACT_SUFX= .tgz
-VERSION= 3.1.0
-PKGREVISION= 6
-CATEGORIES= wip
-MASTER_SITES= https://research.cc.gatech.edu/borg/sites/edu.borg/files/downloads/
-
-MAINTAINER= gtsam@lists.gatech.edu
-COMMENT= "Library that implement smoothing and mapping (SAM) in robotics and vision using factor graphs and Bayes networks"
-LICENSE= modified-bsd
-
-MAKE_JOBS_SAFE= yes
-CMAKE_ARG_PATH= ..
-CONFIGURE_DIRS= _build
-
-USE_QMAKE= no
-
-CMAKE_ARGS+= \
- '-DCMAKE_BUILD_TYPE=Release'\
- '-DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF'
-
-PKG_SUPPORTED_OPTIONS+= toolbox
-PKG_OPTION_DESCR.toolbox= Build and install the matlab toolbox (Needs MATLAB_ROOT path set in robotpkg.conf)
-define PKG_OPTION_SET.toolbox
- CMAKE_ARGS+= '-DGTSAM_INSTALL_MATLAB_TOOLBOX=ON'
- CMAKE_ARGS+= '-Dmex_command=${MATLAB_ROOT}/bin/mex'
- DEPEND_ABI.gcc= gcc<=4.7
- include ../../mk/sysdep/gcc.mk
-endef
-define PKG_OPTION_UNSET.toolbox
- CMAKE_ARGS+= '-DGTSAM_INSTALL_MATLAB_TOOLBOX=OFF'
-endef
-
-DEPEND_ABI.boost-libs= boost-libs>=1.43
-DEPEND_ABI.boost-headers= boost-headers>=1.43
-include ../../devel/boost-headers/depend.mk
-include ../../devel/boost-libs/depend.mk
-
-DEPEND_ABI.cmake= cmake>=2.6
-include ../../mk/sysdep/cmake.mk
-
-include ../../mk/language/c.mk
-include ../../mk/language/c++.mk
-include ../../mk/robotpkg.mk
-
-pre-configure:
- ${RUN}${MKDIR} ${WRKSRC}/_build
\ No newline at end of file
diff --git a/gtsam/PLIST b/gtsam/PLIST
deleted file mode 100644
index 85adb89886fa567a03b66d5b49b9f2aaf1aaa593..0000000000000000000000000000000000000000
--- a/gtsam/PLIST
+++ /dev/null
@@ -1,1032 +0,0 @@
-@comment Wed May 6 15:57:27 CEST 2015
-bin/wrap
-include/CppUnitLite/Failure.h
-include/CppUnitLite/Test.h
-include/CppUnitLite/TestHarness.h
-include/CppUnitLite/TestRegistry.h
-include/CppUnitLite/TestResult.h
-include/GeographicLib/Accumulator.hpp
-include/GeographicLib/AlbersEqualArea.hpp
-include/GeographicLib/AzimuthalEquidistant.hpp
-include/GeographicLib/CassiniSoldner.hpp
-include/GeographicLib/CircularEngine.hpp
-include/GeographicLib/Config.h
-include/GeographicLib/Constants.hpp
-include/GeographicLib/DMS.hpp
-include/GeographicLib/Ellipsoid.hpp
-include/GeographicLib/EllipticFunction.hpp
-include/GeographicLib/GeoCoords.hpp
-include/GeographicLib/Geocentric.hpp
-include/GeographicLib/Geodesic.hpp
-include/GeographicLib/GeodesicExact.hpp
-include/GeographicLib/GeodesicLine.hpp
-include/GeographicLib/GeodesicLineExact.hpp
-include/GeographicLib/Geohash.hpp
-include/GeographicLib/Geoid.hpp
-include/GeographicLib/Gnomonic.hpp
-include/GeographicLib/GravityCircle.hpp
-include/GeographicLib/GravityModel.hpp
-include/GeographicLib/LambertConformalConic.hpp
-include/GeographicLib/LocalCartesian.hpp
-include/GeographicLib/MGRS.hpp
-include/GeographicLib/MagneticCircle.hpp
-include/GeographicLib/MagneticModel.hpp
-include/GeographicLib/Math.hpp
-include/GeographicLib/NormalGravity.hpp
-include/GeographicLib/OSGB.hpp
-include/GeographicLib/PolarStereographic.hpp
-include/GeographicLib/PolygonArea.hpp
-include/GeographicLib/SphericalEngine.hpp
-include/GeographicLib/SphericalHarmonic.hpp
-include/GeographicLib/SphericalHarmonic1.hpp
-include/GeographicLib/SphericalHarmonic2.hpp
-include/GeographicLib/TransverseMercator.hpp
-include/GeographicLib/TransverseMercatorExact.hpp
-include/GeographicLib/UTMUPS.hpp
-include/GeographicLib/Utility.hpp
-include/gtsam/3rdparty/CCOLAMD/ccolamd.h
-include/gtsam/3rdparty/Eigen/Eigen/Array
-include/gtsam/3rdparty/Eigen/Eigen/Cholesky
-include/gtsam/3rdparty/Eigen/Eigen/CholmodSupport
-include/gtsam/3rdparty/Eigen/Eigen/Core
-include/gtsam/3rdparty/Eigen/Eigen/Dense
-include/gtsam/3rdparty/Eigen/Eigen/Eigen
-include/gtsam/3rdparty/Eigen/Eigen/Eigen2Support
-include/gtsam/3rdparty/Eigen/Eigen/Eigenvalues
-include/gtsam/3rdparty/Eigen/Eigen/Geometry
-include/gtsam/3rdparty/Eigen/Eigen/Householder
-include/gtsam/3rdparty/Eigen/Eigen/IterativeLinearSolvers
-include/gtsam/3rdparty/Eigen/Eigen/Jacobi
-include/gtsam/3rdparty/Eigen/Eigen/LU
-include/gtsam/3rdparty/Eigen/Eigen/LeastSquares
-include/gtsam/3rdparty/Eigen/Eigen/MetisSupport
-include/gtsam/3rdparty/Eigen/Eigen/OrderingMethods
-include/gtsam/3rdparty/Eigen/Eigen/PaStiXSupport
-include/gtsam/3rdparty/Eigen/Eigen/PardisoSupport
-include/gtsam/3rdparty/Eigen/Eigen/QR
-include/gtsam/3rdparty/Eigen/Eigen/QtAlignedMalloc
-include/gtsam/3rdparty/Eigen/Eigen/SPQRSupport
-include/gtsam/3rdparty/Eigen/Eigen/SVD
-include/gtsam/3rdparty/Eigen/Eigen/Sparse
-include/gtsam/3rdparty/Eigen/Eigen/SparseCholesky
-include/gtsam/3rdparty/Eigen/Eigen/SparseCore
-include/gtsam/3rdparty/Eigen/Eigen/SparseLU
-include/gtsam/3rdparty/Eigen/Eigen/SparseQR
-include/gtsam/3rdparty/Eigen/Eigen/StdDeque
-include/gtsam/3rdparty/Eigen/Eigen/StdList
-include/gtsam/3rdparty/Eigen/Eigen/StdVector
-include/gtsam/3rdparty/Eigen/Eigen/SuperLUSupport
-include/gtsam/3rdparty/Eigen/Eigen/UmfPackSupport
-include/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT_MKL.h
-include/gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayBase.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/Assign_MKL.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/BandMatrix.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/Block.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/BooleanRedux.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/CoreIterators.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseBinaryOp.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseNullaryOp.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryOp.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryView.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseCoeffsBase.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseStorage.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalMatrix.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/DiagonalProduct.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/Flagged.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/ForceAlignedAccess.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/Functors.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/Fuzzy.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/GenericPacketMath.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/GlobalFunctions.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/Map.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/NestByValue.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/NoAlias.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/NumTraits.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/Random.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/Redux.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/Replicate.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/ReturnByValue.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/Reverse.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/Select.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfAdjointView.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/SelfCwiseBinaryOp.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/StableNorm.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/Stride.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/Swap.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpose.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/Transpositions.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/VectorBlock.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/VectorwiseOp.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/Visitor.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AltiVec/Complex.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/AltiVec/PacketMath.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/Default/Settings.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/Complex.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_MKL.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix_MKL.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector_MKL.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/products/Parallelizer.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix_MKL.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointMatrixVector_MKL.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointProduct.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/products/SelfadjointRank2Update.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixMatrix_MKL.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixVector.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularMatrixVector_MKL.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix_MKL.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverVector.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/util/BlasUtil.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Constants.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/util/DisableStupidWarnings.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ForwardDeclarations.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/util/MKL_support.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Meta.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/util/NonMPL2.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/util/ReenableStupidWarnings.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Core/util/XprHelper.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Block.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Cwise.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/CwiseOperators.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Geometry/AlignedBox.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Geometry/All.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Geometry/AngleAxis.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Geometry/Hyperplane.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Geometry/ParametrizedLine.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Geometry/Quaternion.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Geometry/Rotation2D.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Geometry/RotationBase.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Geometry/Scaling.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Geometry/Transform.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Geometry/Translation.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/LU.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Lazy.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/LeastSquares.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Macros.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/MathFunctions.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Memory.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Meta.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/Minor.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/QR.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/SVD.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/TriangularSolver.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/VectorBlock.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexEigenSolver.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexSchur.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexSchur_MKL.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/EigenSolver.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/HessenbergDecomposition.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur_MKL.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver_MKL.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/Tridiagonalization.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AlignedBox.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AngleAxis.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Geometry/EulerAngles.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Homogeneous.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Hyperplane.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Geometry/OrthoMethods.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Geometry/ParametrizedLine.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Rotation2D.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Geometry/RotationBase.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Umeyama.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Geometry/arch/Geometry_SSE.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Householder/BlockHouseholder.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Householder/Householder.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Householder/HouseholderSequence.h
-include/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h
-include/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h
-include/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h
-include/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h
-include/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h
-include/gtsam/3rdparty/Eigen/Eigen/src/Jacobi/Jacobi.h
-include/gtsam/3rdparty/Eigen/Eigen/src/LU/Determinant.h
-include/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h
-include/gtsam/3rdparty/Eigen/Eigen/src/LU/Inverse.h
-include/gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU.h
-include/gtsam/3rdparty/Eigen/Eigen/src/LU/PartialPivLU_MKL.h
-include/gtsam/3rdparty/Eigen/Eigen/src/LU/arch/Inverse_SSE.h
-include/gtsam/3rdparty/Eigen/Eigen/src/MetisSupport/MetisSupport.h
-include/gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Amd.h
-include/gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Eigen_Colamd.h
-include/gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Ordering.h
-include/gtsam/3rdparty/Eigen/Eigen/src/PaStiXSupport/PaStiXSupport.h
-include/gtsam/3rdparty/Eigen/Eigen/src/PardisoSupport/PardisoSupport.h
-include/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h
-include/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR_MKL.h
-include/gtsam/3rdparty/Eigen/Eigen/src/QR/FullPivHouseholderQR.h
-include/gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR.h
-include/gtsam/3rdparty/Eigen/Eigen/src/QR/HouseholderQR_MKL.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD_MKL.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SVD/UpperBidiagonalization.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/CompressedStorage.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseColEtree.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseBinaryOp.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseUnaryOp.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDiagonalProduct.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDot.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseFuzzy.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrix.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseMatrixBase.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseProduct.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseRedux.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSelfAdjointView.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseTranspose.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseTriangularView.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseUtil.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseVector.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseView.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/TriangularSolver.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLUImpl.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Memory.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Structs.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_Utils.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_column_bmod.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_column_dfs.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_gemm_kernel.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_kernel_bmod.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_panel_bmod.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_panel_dfs.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_pivotL.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_pruneL.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_relax_snode.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h
-include/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdDeque.h
-include/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdList.h
-include/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdVector.h
-include/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/details.h
-include/gtsam/3rdparty/Eigen/Eigen/src/SuperLUSupport/SuperLUSupport.h
-include/gtsam/3rdparty/Eigen/Eigen/src/UmfPackSupport/UmfPackSupport.h
-include/gtsam/3rdparty/Eigen/Eigen/src/misc/Image.h
-include/gtsam/3rdparty/Eigen/Eigen/src/misc/Kernel.h
-include/gtsam/3rdparty/Eigen/Eigen/src/misc/Solve.h
-include/gtsam/3rdparty/Eigen/Eigen/src/misc/SparseSolve.h
-include/gtsam/3rdparty/Eigen/Eigen/src/misc/blas.h
-include/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseBinaryOps.h
-include/gtsam/3rdparty/Eigen/Eigen/src/plugins/ArrayCwiseUnaryOps.h
-include/gtsam/3rdparty/Eigen/Eigen/src/plugins/BlockMethods.h
-include/gtsam/3rdparty/Eigen/Eigen/src/plugins/CommonCwiseBinaryOps.h
-include/gtsam/3rdparty/Eigen/Eigen/src/plugins/CommonCwiseUnaryOps.h
-include/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseBinaryOps.h
-include/gtsam/3rdparty/Eigen/Eigen/src/plugins/MatrixCwiseUnaryOps.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/BVH/BVAlgorithms.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/BVH/KdBVH.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Eigenvalues/ArpackSelfAdjointEigenSolver.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/FFT/ei_fftw_impl.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/FFT/ei_kissfft_impl.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/ConstrainedConjGrad.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/DGMRES.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/IncompleteCholesky.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/IncompleteLU.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/IterationController.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/MINRES.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/IterativeSolvers/Scaling.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/KroneckerProduct/KroneckerTensorProduct.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/LMcovar.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/LMonestep.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/LMpar.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/LMqrsolv.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/LevenbergMarquardt/LevenbergMarquardt.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunctionAtomic.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MatrixFunctions/StemFunction.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/MoreVectorization/MathFunctions.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/NonLinearOptimization/chkder.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/NonLinearOptimization/covar.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/NonLinearOptimization/dogleg.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/NonLinearOptimization/fdjac1.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/NonLinearOptimization/lmpar.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/NonLinearOptimization/r1mpyq.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/NonLinearOptimization/r1updt.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/NonLinearOptimization/rwupdt.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/NumericalDiff/NumericalDiff.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/Companion.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/PolynomialSolver.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Polynomials/PolynomialUtils.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SVD/BDCSVD.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SVD/JacobiSVD.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SVD/SVDBase.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Skyline/SkylineInplaceLU.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Skyline/SkylineMatrix.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Skyline/SkylineMatrixBase.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Skyline/SkylineProduct.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Skyline/SkylineStorage.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Skyline/SkylineUtil.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/BlockOfDynamicSparseMatrix.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/DynamicSparseMatrix.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/MarketIO.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/MatrixMarketIterator.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/RandomSetter.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Splines/Spline.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Splines/SplineFitting.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/src/Splines/SplineFwd.h
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/AdolcForward
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/AlignedVector3
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/ArpackSupport
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/AutoDiff
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/BVH
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/FFT
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/IterativeSolvers
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/KroneckerProduct
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/LevenbergMarquardt
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/MPRealSupport
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/MatrixFunctions
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/MoreVectorization
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/NonLinearOptimization
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/NumericalDiff
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/OpenGLSupport
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/Polynomials
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/SVD
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/Skyline
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/SparseExtra
-include/gtsam/3rdparty/Eigen/unsupported/Eigen/Splines
-include/gtsam/3rdparty/UFconfig/UFconfig.h
-include/gtsam/3rdparty/gtsam_eigen_includes.h
-include/gtsam/base/ConcurrentMap.h
-include/gtsam/base/DSFVector.h
-include/gtsam/base/DerivedValue.h
-include/gtsam/base/FastDefaultAllocator.h
-include/gtsam/base/FastList.h
-include/gtsam/base/FastMap.h
-include/gtsam/base/FastSet.h
-include/gtsam/base/FastVector.h
-include/gtsam/base/Group.h
-include/gtsam/base/Lie-inl.h
-include/gtsam/base/Lie.h
-include/gtsam/base/LieMatrix.h
-include/gtsam/base/LieScalar.h
-include/gtsam/base/LieVector.h
-include/gtsam/base/Manifold.h
-include/gtsam/base/Matrix.h
-include/gtsam/base/SymmetricBlockMatrix.h
-include/gtsam/base/SymmetricBlockMatrixBlockExpr.h
-include/gtsam/base/Testable.h
-include/gtsam/base/TestableAssertions.h
-include/gtsam/base/Value.h
-include/gtsam/base/Vector.h
-include/gtsam/base/VerticalBlockMatrix.h
-include/gtsam/base/cholesky.h
-include/gtsam/base/debug.h
-include/gtsam/base/lieProxies.h
-include/gtsam/base/numericalDerivative.h
-include/gtsam/base/serialization.h
-include/gtsam/base/serializationTestHelpers.h
-include/gtsam/base/timing.h
-include/gtsam/base/treeTraversal-inst.h
-include/gtsam/base/treeTraversal/parallelTraversalTasks.h
-include/gtsam/base/treeTraversal/statistics.h
-include/gtsam/base/types.h
-include/gtsam/config.h
-include/gtsam/discrete/AlgebraicDecisionTree.h
-include/gtsam/discrete/Assignment.h
-include/gtsam/discrete/DecisionTree-inl.h
-include/gtsam/discrete/DecisionTree.h
-include/gtsam/discrete/DecisionTreeFactor.h
-include/gtsam/discrete/DiscreteBayesNet.h
-include/gtsam/discrete/DiscreteBayesTree.h
-include/gtsam/discrete/DiscreteConditional.h
-include/gtsam/discrete/DiscreteEliminationTree.h
-include/gtsam/discrete/DiscreteFactor.h
-include/gtsam/discrete/DiscreteFactorGraph.h
-include/gtsam/discrete/DiscreteJunctionTree.h
-include/gtsam/discrete/DiscreteKey.h
-include/gtsam/discrete/DiscreteMarginals.h
-include/gtsam/discrete/Potentials.h
-include/gtsam/discrete/Signature.h
-include/gtsam/dllexport.h
-include/gtsam/geometry/Cal3Bundler.h
-include/gtsam/geometry/Cal3DS2.h
-include/gtsam/geometry/Cal3Unified.h
-include/gtsam/geometry/Cal3_S2.h
-include/gtsam/geometry/Cal3_S2Stereo.h
-include/gtsam/geometry/CalibratedCamera.h
-include/gtsam/geometry/EssentialMatrix.h
-include/gtsam/geometry/ParallaxAnglePoint2.h
-include/gtsam/geometry/ParallaxAnglePoint3.h
-include/gtsam/geometry/PinholeCamera.h
-include/gtsam/geometry/Point2.h
-include/gtsam/geometry/Point3.h
-include/gtsam/geometry/Pose2.h
-include/gtsam/geometry/Pose3.h
-include/gtsam/geometry/Rot2.h
-include/gtsam/geometry/Rot3.h
-include/gtsam/geometry/SimpleCamera.h
-include/gtsam/geometry/StereoCamera.h
-include/gtsam/geometry/StereoPoint2.h
-include/gtsam/geometry/TriangulationFactor.h
-include/gtsam/geometry/Unit3.h
-include/gtsam/geometry/concepts.h
-include/gtsam/geometry/triangulation.h
-include/gtsam/global_includes.h
-include/gtsam/inference/BayesNet-inst.h
-include/gtsam/inference/BayesNet.h
-include/gtsam/inference/BayesTree-inst.h
-include/gtsam/inference/BayesTree.h
-include/gtsam/inference/BayesTreeCliqueBase-inst.h
-include/gtsam/inference/BayesTreeCliqueBase.h
-include/gtsam/inference/ClusterTree-inst.h
-include/gtsam/inference/ClusterTree.h
-include/gtsam/inference/Conditional-inst.h
-include/gtsam/inference/Conditional.h
-include/gtsam/inference/EliminateableFactorGraph-inst.h
-include/gtsam/inference/EliminateableFactorGraph.h
-include/gtsam/inference/EliminationTree-inst.h
-include/gtsam/inference/EliminationTree.h
-include/gtsam/inference/Factor.h
-include/gtsam/inference/FactorGraph-inst.h
-include/gtsam/inference/FactorGraph.h
-include/gtsam/inference/ISAM-inst.h
-include/gtsam/inference/ISAM.h
-include/gtsam/inference/JunctionTree-inst.h
-include/gtsam/inference/JunctionTree.h
-include/gtsam/inference/Key.h
-include/gtsam/inference/LabeledSymbol.h
-include/gtsam/inference/Ordering.h
-include/gtsam/inference/Symbol.h
-include/gtsam/inference/VariableIndex-inl.h
-include/gtsam/inference/VariableIndex.h
-include/gtsam/inference/VariableSlots.h
-include/gtsam/inference/graph-inl.h
-include/gtsam/inference/graph.h
-include/gtsam/inference/inference-inst.h
-include/gtsam/inference/inferenceExceptions.h
-include/gtsam/linear/ConjugateGradientSolver.h
-include/gtsam/linear/Errors.h
-include/gtsam/linear/GaussianBayesNet.h
-include/gtsam/linear/GaussianBayesTree-inl.h
-include/gtsam/linear/GaussianBayesTree.h
-include/gtsam/linear/GaussianConditional-inl.h
-include/gtsam/linear/GaussianConditional.h
-include/gtsam/linear/GaussianDensity.h
-include/gtsam/linear/GaussianEliminationTree.h
-include/gtsam/linear/GaussianFactor.h
-include/gtsam/linear/GaussianFactorGraph.h
-include/gtsam/linear/GaussianISAM.h
-include/gtsam/linear/GaussianJunctionTree.h
-include/gtsam/linear/HessianFactor-inl.h
-include/gtsam/linear/HessianFactor.h
-include/gtsam/linear/IterativeSolver.h
-include/gtsam/linear/JacobianFactor-inl.h
-include/gtsam/linear/JacobianFactor.h
-include/gtsam/linear/KalmanFilter.h
-include/gtsam/linear/NoiseModel.h
-include/gtsam/linear/Sampler.h
-include/gtsam/linear/SubgraphPreconditioner.h
-include/gtsam/linear/SubgraphSolver.h
-include/gtsam/linear/VectorValues.h
-include/gtsam/linear/iterative-inl.h
-include/gtsam/linear/iterative.h
-include/gtsam/linear/linearAlgorithms-inst.h
-include/gtsam/linear/linearExceptions.h
-include/gtsam/navigation/AttitudeFactor.h
-include/gtsam/navigation/CombinedImuFactor.h
-include/gtsam/navigation/GPSFactor.h
-include/gtsam/navigation/ImuBias.h
-include/gtsam/navigation/ImuFactor.h
-include/gtsam/navigation/MagFactor.h
-include/gtsam/nonlinear/DoglegOptimizer.h
-include/gtsam/nonlinear/DoglegOptimizerImpl.h
-include/gtsam/nonlinear/ExtendedKalmanFilter-inl.h
-include/gtsam/nonlinear/ExtendedKalmanFilter.h
-include/gtsam/nonlinear/GaussNewtonOptimizer.h
-include/gtsam/nonlinear/ISAM2-impl.h
-include/gtsam/nonlinear/ISAM2-inl.h
-include/gtsam/nonlinear/ISAM2.h
-include/gtsam/nonlinear/LevenbergMarquardtOptimizer.h
-include/gtsam/nonlinear/LinearContainerFactor.h
-include/gtsam/nonlinear/Marginals.h
-include/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h
-include/gtsam/nonlinear/NonlinearEquality.h
-include/gtsam/nonlinear/NonlinearFactor.h
-include/gtsam/nonlinear/NonlinearFactorGraph.h
-include/gtsam/nonlinear/NonlinearISAM.h
-include/gtsam/nonlinear/NonlinearOptimizer.h
-include/gtsam/nonlinear/NonlinearOptimizerParams.h
-include/gtsam/nonlinear/Symbol.h
-include/gtsam/nonlinear/Values-inl.h
-include/gtsam/nonlinear/Values.h
-include/gtsam/nonlinear/WhiteNoiseFactor.h
-include/gtsam/nonlinear/nonlinearExceptions.h
-include/gtsam/slam/AntiFactor.h
-include/gtsam/slam/BearingFactor.h
-include/gtsam/slam/BearingRangeFactor.h
-include/gtsam/slam/BetweenFactor.h
-include/gtsam/slam/BoundingConstraint.h
-include/gtsam/slam/EssentialMatrixConstraint.h
-include/gtsam/slam/EssentialMatrixFactor.h
-include/gtsam/slam/GeneralSFMFactor.h
-include/gtsam/slam/ImplicitSchurFactor.h
-include/gtsam/slam/JacobianFactorQ.h
-include/gtsam/slam/JacobianFactorQR.h
-include/gtsam/slam/JacobianFactorSVD.h
-include/gtsam/slam/JacobianSchurFactor.h
-include/gtsam/slam/ParallaxAngleProjectionFactor.h
-include/gtsam/slam/PoseRotationPrior.h
-include/gtsam/slam/PoseTranslationPrior.h
-include/gtsam/slam/PriorFactor.h
-include/gtsam/slam/ProjectionFactor.h
-include/gtsam/slam/RangeFactor.h
-include/gtsam/slam/ReferenceFrameFactor.h
-include/gtsam/slam/RegularHessianFactor.h
-include/gtsam/slam/RotateFactor.h
-include/gtsam/slam/SmartFactorBase.h
-include/gtsam/slam/SmartProjectionFactor.h
-include/gtsam/slam/SmartProjectionPoseFactor.h
-include/gtsam/slam/StereoFactor.h
-include/gtsam/slam/dataset.h
-include/gtsam/slam/lago.h
-include/gtsam/symbolic/SymbolicBayesNet.h
-include/gtsam/symbolic/SymbolicBayesTree.h
-include/gtsam/symbolic/SymbolicConditional.h
-include/gtsam/symbolic/SymbolicEliminationTree.h
-include/gtsam/symbolic/SymbolicFactor-inst.h
-include/gtsam/symbolic/SymbolicFactor.h
-include/gtsam/symbolic/SymbolicFactorGraph.h
-include/gtsam/symbolic/SymbolicISAM.h
-include/gtsam/symbolic/SymbolicJunctionTree.h
-include/wrap/matlab.h
-lib/cmake/GTSAM/GTSAM-exports-release.cmake
-lib/cmake/GTSAM/GTSAM-exports.cmake
-lib/cmake/GTSAM/GTSAMConfig.cmake
-lib/cmake/GTSAM/gtsam_extra.cmake
-lib/cmake/GTSAMCMakeTools/Config.cmake.in
-lib/cmake/GTSAMCMakeTools/GTSAMCMakeToolsConfig.cmake
-lib/cmake/GTSAMCMakeTools/GtsamBuildTypes.cmake
-lib/cmake/GTSAMCMakeTools/GtsamMakeConfigFile.cmake
-lib/cmake/GTSAMCMakeTools/GtsamMatlabWrap.cmake
-lib/cmake/GTSAMCMakeTools/GtsamPythonWrap.cmake
-lib/cmake/GTSAMCMakeTools/GtsamTesting.cmake
-lib/cmake/GTSAMCMakeTools/GtsamTestingObsolete.cmake
-lib/cmake/GTSAMCMakeTools/README.html
-lib/cmake/GTSAMCMakeTools/dllexport.h.in
-lib/libCppUnitLite.a
-lib/libGeographic.so
-lib/libGeographic.so.10
-lib/libGeographic.so.10.1.2
-lib/libgtsam.so
-lib/libgtsam.so.3
-lib/libgtsam.so.${PKGVERSION}
-lib/python/site-packages/geographiclib/__init__.py
-lib/python/site-packages/geographiclib/accumulator.py
-lib/python/site-packages/geographiclib/constants.py
-lib/python/site-packages/geographiclib/geodesic.py
-lib/python/site-packages/geographiclib/geodesiccapability.py
-lib/python/site-packages/geographiclib/geodesicline.py
-lib/python/site-packages/geographiclib/geomath.py
-lib/python/site-packages/geographiclib/polygonarea.py
-libexec/GeographicLib/matlab/cassini_fwd.m
-libexec/GeographicLib/matlab/cassini_inv.m
-libexec/GeographicLib/matlab/defaultellipsoid.m
-libexec/GeographicLib/matlab/ecc2flat.m
-libexec/GeographicLib/matlab/eqdazim_fwd.m
-libexec/GeographicLib/matlab/eqdazim_inv.m
-libexec/GeographicLib/matlab/flat2ecc.m
-libexec/GeographicLib/matlab/geocentricforward.cpp
-libexec/GeographicLib/matlab/geocentricforward.m
-libexec/GeographicLib/matlab/geocentricreverse.cpp
-libexec/GeographicLib/matlab/geocentricreverse.m
-libexec/GeographicLib/matlab/geodarea.m
-libexec/GeographicLib/matlab/geoddistance.m
-libexec/GeographicLib/matlab/geoddoc.m
-libexec/GeographicLib/matlab/geodesicdirect.cpp
-libexec/GeographicLib/matlab/geodesicdirect.m
-libexec/GeographicLib/matlab/geodesicinverse.cpp
-libexec/GeographicLib/matlab/geodesicinverse.m
-libexec/GeographicLib/matlab/geodesicline.cpp
-libexec/GeographicLib/matlab/geodesicline.m
-libexec/GeographicLib/matlab/geodproj.m
-libexec/GeographicLib/matlab/geodreckon.m
-libexec/GeographicLib/matlab/geographiclibinterface.m
-libexec/GeographicLib/matlab/geoidheight.cpp
-libexec/GeographicLib/matlab/geoidheight.m
-libexec/GeographicLib/matlab/gnomonic_fwd.m
-libexec/GeographicLib/matlab/gnomonic_inv.m
-libexec/GeographicLib/matlab/localcartesianforward.cpp
-libexec/GeographicLib/matlab/localcartesianforward.m
-libexec/GeographicLib/matlab/localcartesianreverse.cpp
-libexec/GeographicLib/matlab/localcartesianreverse.m
-libexec/GeographicLib/matlab/mgrsforward.cpp
-libexec/GeographicLib/matlab/mgrsforward.m
-libexec/GeographicLib/matlab/mgrsreverse.cpp
-libexec/GeographicLib/matlab/mgrsreverse.m
-libexec/GeographicLib/matlab/polygonarea.cpp
-libexec/GeographicLib/matlab/polygonarea.m
-libexec/GeographicLib/matlab/private/A1m1f.m
-libexec/GeographicLib/matlab/private/A2m1f.m
-libexec/GeographicLib/matlab/private/A3coeff.m
-libexec/GeographicLib/matlab/private/A3f.m
-libexec/GeographicLib/matlab/private/AngDiff.m
-libexec/GeographicLib/matlab/private/AngNormalize.m
-libexec/GeographicLib/matlab/private/AngNormalize2.m
-libexec/GeographicLib/matlab/private/AngRound.m
-libexec/GeographicLib/matlab/private/C1f.m
-libexec/GeographicLib/matlab/private/C1pf.m
-libexec/GeographicLib/matlab/private/C2f.m
-libexec/GeographicLib/matlab/private/C3coeff.m
-libexec/GeographicLib/matlab/private/C3f.m
-libexec/GeographicLib/matlab/private/C4coeff.m
-libexec/GeographicLib/matlab/private/C4f.m
-libexec/GeographicLib/matlab/private/SinCosNorm.m
-libexec/GeographicLib/matlab/private/SinCosSeries.m
-libexec/GeographicLib/matlab/private/atanhee.m
-libexec/GeographicLib/matlab/private/cbrt.m
-libexec/GeographicLib/matlab/private/cvmgt.m
-libexec/GeographicLib/matlab/private/sumx.m
-libexec/GeographicLib/matlab/private/swap.m
-libexec/GeographicLib/matlab/tranmerc_fwd.m
-libexec/GeographicLib/matlab/tranmerc_inv.m
-libexec/GeographicLib/matlab/utm_fwd.m
-libexec/GeographicLib/matlab/utm_inv.m
-libexec/GeographicLib/matlab/utmupsforward.cpp
-libexec/GeographicLib/matlab/utmupsforward.m
-libexec/GeographicLib/matlab/utmupsreverse.cpp
-libexec/GeographicLib/matlab/utmupsreverse.m
-share/cmake/GeographicLib/geographiclib-config-version.cmake
-share/cmake/GeographicLib/geographiclib-config.cmake
-share/cmake/GeographicLib/geographiclib-depends-release.cmake
-share/cmake/GeographicLib/geographiclib-depends.cmake
-share/doc/GeographicLib/html/LICENSE.txt
-share/doc/GeographicLib/html/index.html
-share/doc/GeographicLib/html/utilities.html
-share/doc/GeographicLib/scripts/GeographicLib/DMS.js
-share/doc/GeographicLib/scripts/GeographicLib/Geodesic.js
-share/doc/GeographicLib/scripts/GeographicLib/GeodesicLine.js
-share/doc/GeographicLib/scripts/GeographicLib/Interface.js
-share/doc/GeographicLib/scripts/GeographicLib/Math.js
-share/doc/GeographicLib/scripts/GeographicLib/PolygonArea.js
-share/doc/GeographicLib/scripts/geod-calc.html
-share/doc/GeographicLib/scripts/geod-google-instructions.html
-share/doc/GeographicLib/scripts/geod-google.html
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+imuBias/ConstantBias.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+noiseModel/+mEstimator/Base.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+noiseModel/+mEstimator/Fair.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+noiseModel/+mEstimator/Huber.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+noiseModel/+mEstimator/Null.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+noiseModel/+mEstimator/Tukey.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+noiseModel/Base.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+noiseModel/Constrained.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+noiseModel/Diagonal.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+noiseModel/Gaussian.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+noiseModel/Isotropic.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+noiseModel/Robust.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+noiseModel/Unit.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+utilities/allPose3s.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+utilities/createKeyList.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+utilities/createKeySet.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+utilities/createKeyVector.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+utilities/extractPoint2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+utilities/extractPoint3.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+utilities/extractPose2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+utilities/extractPose3.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+utilities/insertBackprojections.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+utilities/insertProjectionFactors.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+utilities/localToWorld.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+utilities/perturbPoint2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+utilities/perturbPoint3.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+utilities/perturbPose2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/+utilities/reprojectionErrors.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/BearingFactor2D.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/BearingRangeFactor2D.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/BetweenFactorConstantBias.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/BetweenFactorLieMatrix.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/BetweenFactorLieScalar.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/BetweenFactorLieVector.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/BetweenFactorPoint2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/BetweenFactorPoint3.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/BetweenFactorPose2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/BetweenFactorPose3.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/BetweenFactorRot2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/BetweenFactorRot3.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/CHECK.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/Cal3DS2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/Cal3_S2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/Cal3_S2Stereo.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/CalibratedCamera.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/CombinedImuFactor.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/CombinedImuFactorPreintegratedMeasurements.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/ConjugateGradientParameters.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/Contents.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/DoglegOptimizer.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/DoglegParams.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/EQUALITY.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/EXPECT.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/Errors.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/EssentialMatrix.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/EssentialMatrixFactor.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/GaussNewtonOptimizer.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/GaussNewtonParams.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/GaussianBayesNet.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/GaussianBayesTree.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/GaussianConditional.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/GaussianDensity.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/GaussianFactor.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/GaussianFactorGraph.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/GaussianISAM.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/GeneralSFMFactor2Cal3_S2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/GeneralSFMFactorCal3DS2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/GeneralSFMFactorCal3_S2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/GenericProjectionFactorCal3DS2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/GenericProjectionFactorCal3_S2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/GenericStereoFactor3D.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/HessianFactor.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/ISAM2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/ISAM2Clique.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/ISAM2DoglegParams.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/ISAM2GaussNewtonParams.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/ISAM2Params.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/ISAM2Result.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/ISAM2ThresholdMap.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/ISAM2ThresholdMapValue.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/ImuFactor.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/ImuFactorPreintegratedMeasurements.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/IterativeOptimizationParameters.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/JacobianFactor.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/JointMarginal.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/KalmanFilter.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/KeyGroupMap.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/KeyList.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/KeySet.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/KeyVector.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/LabeledSymbol.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/LevenbergMarquardtOptimizer.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/LevenbergMarquardtParams.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/LieMatrix.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/LieScalar.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/LieVector.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/LinearContainerFactor.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/Marginals.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/NoiseModelFactor.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/NonlinearEqualityCal3_S2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/NonlinearEqualityCalibratedCamera.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/NonlinearEqualityConstantBias.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/NonlinearEqualityLieMatrix.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/NonlinearEqualityLieScalar.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/NonlinearEqualityLieVector.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/NonlinearEqualityPoint2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/NonlinearEqualityPoint3.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/NonlinearEqualityPose2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/NonlinearEqualityPose3.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/NonlinearEqualityRot2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/NonlinearEqualityRot3.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/NonlinearEqualitySimpleCamera.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/NonlinearEqualityStereoPoint2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/NonlinearFactor.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/NonlinearFactorGraph.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/NonlinearISAM.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/NonlinearOptimizer.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/NonlinearOptimizerParams.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/Ordering.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PAPoint3OnlyAnchorsProjectionFactorCal3DS2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PAPoint3OnlyAnchorsProjectionFactorCal3_S2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PAPoint3ProjectionFactorCal3DS2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PAPoint3ProjectionFactorCal3_S2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PAPoint2SingleAnchorProjectionFactorCal3DS2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PAPoint2SingleAnchorProjectionFactorCal3_S2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PAPoint3SingleAnchorProjectionFactorCal3DS2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PAPoint3SingleAnchorProjectionFactorCal3_S2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/ParallaxAnglePoint2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/ParallaxAnglePoint3.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PinholeCameraCal3DS2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/Point2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/Point3.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/Pose2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/Pose3.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PoseRotationPrior2D.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PoseRotationPrior3D.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PoseTranslationPrior2D.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PoseTranslationPrior3D.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PriorFactorCal3_S2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PriorFactorCalibratedCamera.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PriorFactorConstantBias.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PriorFactorLieMatrix.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PriorFactorLieScalar.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PriorFactorLieVector.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PriorFactorPoint2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PriorFactorPoint3.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PriorFactorPose2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PriorFactorPose3.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PriorFactorRot2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PriorFactorRot3.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PriorFactorSimpleCamera.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/PriorFactorStereoPoint2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/RangeFactorCalibratedCamera.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/RangeFactorCalibratedCameraPoint.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/RangeFactorPose2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/RangeFactorPose3.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/RangeFactorPosePoint2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/RangeFactorPosePoint3.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/RangeFactorSimpleCamera.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/RangeFactorSimpleCameraPoint.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/Rot2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/Rot3.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/Sampler.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/SimpleCamera.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/StereoCamera.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/StereoPoint2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/SubgraphSolver.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/SubgraphSolverParameters.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/SymbolicBayesNet.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/SymbolicBayesTree.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/SymbolicConditional.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/SymbolicFactor.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/SymbolicFactorGraph.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/Unit3.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/Value.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/Values.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/VariableIndex.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/VectorValues.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/VisualISAMGenerateData.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/VisualISAMInitialize.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/VisualISAMPlot.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/VisualISAMStep.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/checkConvergence.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/circlePose2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/circlePose3.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/covarianceEllipse.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/covarianceEllipse3D.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/findExampleDataFile.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/linear_independent.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/load2D.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/load2D_robust.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/load3D.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/mrsymbol.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/mrsymbolChr.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/mrsymbolIndex.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/mrsymbolLabel.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/plot2DPoints.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/plot2DTrajectory.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/plot3DPoints.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/plot3DTrajectory.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/plotBayesNet.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/plotBayesTree.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/plotCamera.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/plotPoint2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/plotPoint3.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/plotPose2.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/plotPose3.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/printKeyList.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/printKeySet.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/printKeyVector.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/readG2o.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/save2D.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/symbol.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/symbolChr.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/symbolIndex.m
-${PLIST.toolbox}gtsam_toolbox/+gtsam/writeG2o.m
-${PLIST.toolbox}gtsam_toolbox/README-gtsam-toolbox.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/5pointExample1.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/5pointExample2.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/KittiEquivBiasedImu.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/KittiEquivBiasedImu_metadata.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/KittiGps_converted.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/Plaza1_.mat
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/Plaza1_DR.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/Plaza1_TD.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/Plaza2_.mat
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/Plaza2_DR.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/Plaza2_TD.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/VO_calibration.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/VO_camera_poses_large.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/VO_stereo_factors_large.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/dubrovnik-1-1-pre.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/dubrovnik-3-7-18-pre.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/dubrovnik-3-7-pre-rewritten.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/dubrovnik-3-7-pre.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/example.graph
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/noisyToyGraph.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/optimizedNoisyToyGraph.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/orientationsNoisyToyGraph.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/pose2example-rewritten.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/pose2example.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/sphere2500.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/sphere2500_groundtruth.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/sphere_smallnoise.graph
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/victoria_park.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/w100.graph
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/w10000.graph
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Data/w20000.txt
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/IMUKittiExampleGPS.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/LocalizationExample.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/MonocularVOExample.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/OdometryExample.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/PlanarSLAMExample.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/PlanarSLAMExample_graph.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/PlanarSLAMExample_sampling.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Pose2SLAMExample.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Pose2SLAMExample_advanced.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Pose2SLAMExample_circle.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Pose2SLAMExample_graph.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Pose2SLAMwSPCG.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Pose3SLAMExample.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/Pose3SLAMExample_graph.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/RangeISAMExample_plaza.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/RangeSLAMExample_plaza.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/SBAExample.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/SFMExample.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/StereoVOExample.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/StereoVOExample_large.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/VisualISAMDemo.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/VisualISAMExample.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/VisualISAM_gui.fig
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/VisualISAM_gui.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/deg2utm.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/gtsamExamples.fig
-${PLIST.toolbox}gtsam_toolbox/gtsam_examples/gtsamExamples.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_tests/testJacobianFactor.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_tests/testKalmanFilter.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_tests/testLocalizationExample.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_tests/testOdometryExample.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_tests/testPlanarSLAMExample.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_tests/testPose2SLAMExample.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_tests/testPose3SLAMExample.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_tests/testSFMExample.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_tests/testSerialization.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_tests/testStereoVOExample.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_tests/testThinBayesTree.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_tests/testThinTree.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_tests/testThinTreeBayesNet.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_tests/testUtilities.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_tests/testVisualISAMExample.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_tests/test_gtsam.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_tests/thinBayesTree.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_tests/thinTree.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_tests/thinTreeBayesNet.m
-${PLIST.toolbox}gtsam_toolbox/gtsam_wrapper.mexa64
-${PLIST.toolbox}gtsam_toolbox/unstable_examples/ConcurrentFilteringAndSmoothingExample.m
-${PLIST.toolbox}gtsam_toolbox/unstable_examples/IMUKittiExampleAdvanced.m
-${PLIST.toolbox}gtsam_toolbox/unstable_examples/IMUKittiExampleVO.m
-${PLIST.toolbox}gtsam_toolbox/unstable_examples/SmartRangeFactorExample.m
-${PLIST.toolbox}gtsam_toolbox/unstable_examples/testTSAMFactors.m
diff --git a/gtsam/depend.mk b/gtsam/depend.mk
deleted file mode 100644
index e600a52759183f9b6b4e5322630d58adb207ef3a..0000000000000000000000000000000000000000
--- a/gtsam/depend.mk
+++ /dev/null
@@ -1,26 +0,0 @@
-# robotpkg depend.mk for: wip/gtsam
-# Created: Ellon Paiva Mendes on Wed, 12 Nov 2014
-#
-
-DEPEND_DEPTH:= ${DEPEND_DEPTH}+
-gtsam_DEPEND_MK:= ${gtsam_DEPEND_MK}+
-
-ifeq (+,$(DEPEND_DEPTH))
-DEPEND_PKG+= gtsam
-endif
-
-ifeq (+,$(gtsam_DEPEND_MK)) # -----------------------------------------------
-
-PREFER.gtsam?= robotpkg
-
-DEPEND_USE+= gtsam
-DEPEND_ABI.gtsam?= gtsam>=3.1.0
-DEPEND_DIR.gtsam?= ../../wip/gtsam
-
-SYSTEM_SEARCH.gtsam=\
- include/jafar/rtslam/rtslamException.hpp \
- lib/libgtsam.so
-
-endif # gtsam_DEPEND_MK ----------------------------------------------------
-
-DEPEND_DEPTH:= ${DEPEND_DEPTH:+=}
diff --git a/gtsam/distinfo b/gtsam/distinfo
deleted file mode 100644
index ed47fd866887d26d9ef09e1859808d185640afc0..0000000000000000000000000000000000000000
--- a/gtsam/distinfo
+++ /dev/null
@@ -1,11 +0,0 @@
-SHA1 (gtsam-3.1.0.tgz) = faba7b25d6dc4f03c0d2b5486cae6294cde49301
-RMD160 (gtsam-3.1.0.tgz) = ae8c978e16966d66c332bb0183bd71ef840244ac
-Size (gtsam-3.1.0.tgz) = 18228078 bytes
-SHA1 (patch-aa) = 1390af9ce452d0585d13ebcab7f13ecf7fc8b4fc
-SHA1 (patch-ab) = b0324c3061b99d060732f0911860c2fda146d50c
-SHA1 (patch-ac) = 5cd8ee856125dfbcd7705b981a5bead62ac6a4ff
-SHA1 (patch-ad) = 61eacf5a77082c37bd557f0846a284e0a26afad4
-SHA1 (patch-ae) = 2c19ba0c0571c2584f39a8d97d1b636fdf2453f6
-SHA1 (patch-af) = f5d8abc7a653ba16a4349fb91a39a36553447760
-SHA1 (patch-ag) = 4897db95620e525193410034fea5accbe55ca43c
-SHA1 (patch-ah) = 7d2853cf39d4834ff5a3eca5cc8dcf17c5f148af
diff --git a/gtsam/patches/patch-aa b/gtsam/patches/patch-aa
deleted file mode 100644
index 6fe483a2f83ee446d224225bab31d7a98236b2bb..0000000000000000000000000000000000000000
--- a/gtsam/patches/patch-aa
+++ /dev/null
@@ -1,11 +0,0 @@
---- gtsam/slam/SmartFactorBase.h 2014-06-01 21:22:39.000000000 +0200
-+++ gtsam/slam/SmartFactorBase.h 2014-10-03 15:30:49.756247706 +0200
-@@ -572,7 +572,7 @@
-
- FastMap<Key,size_t> KeySlotMap;
- for (size_t slot=0; slot < allKeys.size(); slot++)
-- KeySlotMap.insert(std::make_pair<Key,size_t>(allKeys[slot],slot));
-+ KeySlotMap.insert(std::make_pair(allKeys[slot],slot));
-
- // a single point is observed in numKeys cameras
- size_t numKeys = this->keys_.size(); // cameras observing current point
diff --git a/gtsam/patches/patch-ab b/gtsam/patches/patch-ab
deleted file mode 100644
index 8464cdf15bdfa3614fbcad8938714b0d69d95ab6..0000000000000000000000000000000000000000
--- a/gtsam/patches/patch-ab
+++ /dev/null
@@ -1,17 +0,0 @@
---- gtsam/slam/SmartProjectionPoseFactor.h 2015-02-27 20:50:39.981678671 +0100
-+++ gtsam/slam/SmartProjectionPoseFactor.h 2015-02-27 20:41:07.244188256 +0100
-@@ -157,9 +157,14 @@
- size_t i=0;
- BOOST_FOREACH(const Key& k, this->keys_) {
- Pose3 pose = values.at<Pose3>(k);
-+ if(this->body_P_sensor_) {
-+ typename Base::Camera camera(pose.compose(*this->body_P_sensor_), *K_all_[i++]);
-+ cameras.push_back(camera);
-+ } else {
- typename Base::Camera camera(pose, *K_all_[i++]);
- cameras.push_back(camera);
- }
-+ }
- return cameras;
- }
-
diff --git a/gtsam/patches/patch-ac b/gtsam/patches/patch-ac
deleted file mode 100644
index a14ecfd38847a99654ad15e64c0dd8e847f5239f..0000000000000000000000000000000000000000
--- a/gtsam/patches/patch-ac
+++ /dev/null
@@ -1,2140 +0,0 @@
---- gtsam/geometry/ParallaxAnglePoint2.cpp 1970-01-01 01:00:00.000000000 +0100
-+++ gtsam/geometry/ParallaxAnglePoint2.cpp 2015-05-06 13:52:06.399469742 +0200
-@@ -0,0 +1,66 @@
-+/* ----------------------------------------------------------------------------
-+
-+ * 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 ParallaxAnglePoint.cpp
-+ * @brief Parallax Angle Point without depth
-+ * @author Ellon P. Mendes
-+ */
-+
-+#include "ParallaxAnglePoint2.h"
-+
-+using namespace std;
-+
-+namespace gtsam {
-+
-+/* ************************************************************************* */
-+void ParallaxAnglePoint2::print(const string& s) const {
-+ cout << s << *this << endl;
-+}
-+
-+/* ************************************************************************* */
-+bool ParallaxAnglePoint2::equals(const ParallaxAnglePoint2 & q, double tol) const {
-+ return (fabs(yaw_ - q.yaw()) < tol &&
-+ fabs(pitch_ - q.pitch()) < tol);
-+}
-+
-+/* ************************************************************************* */
-+ParallaxAnglePoint2 ParallaxAnglePoint2::operator+(const ParallaxAnglePoint2& q) const {
-+ return ParallaxAnglePoint2(yaw_ + q.yaw_, pitch_ + q.pitch_);
-+}
-+
-+/* ************************************************************************* */
-+ParallaxAnglePoint2 ParallaxAnglePoint2::operator-(const ParallaxAnglePoint2& q) const {
-+ return ParallaxAnglePoint2(yaw_ - q.yaw_, pitch_ - q.pitch_);
-+}
-+
-+/* ************************************************************************* */
-+Vector3 ParallaxAnglePoint2::directionVector(boost::optional<gtsam::Matrix&> H) const
-+{
-+ double sy = sin(yaw_), cy = cos(yaw_), sp = sin(pitch_), cp = cos(pitch_);
-+ if(H)
-+ {
-+ H->resize(3,2);
-+ *H << -sy*cp, -cy*sp,
-+ cy*cp, -sy*sp,
-+ 0 , cp ;
-+ }
-+ return (Vector(3) << cy*cp, sy*cp, sp);
-+}
-+
-+/* ************************************************************************* */
-+ostream &operator<<(ostream &os, const ParallaxAnglePoint2& p) {
-+ os << '(' << p.yaw() << ", " << p.pitch() << ')';
-+ return os;
-+}
-+
-+/* ************************************************************************* */
-+} // namespace gtsam
---- gtsam/geometry/ParallaxAnglePoint2.h 1970-01-01 01:00:00.000000000 +0100
-+++ gtsam/geometry/ParallaxAnglePoint2.h 2015-05-05 21:36:19.405210807 +0200
-@@ -0,0 +1,144 @@
-+/* ----------------------------------------------------------------------------
-+
-+ * 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 ParallaxAnglePoint2.h
-+ * @brief Parallax Angle Point without depth
-+ * @author Ellon P. Mendes
-+ */
-+
-+// \callgraph
-+
-+#pragma once
-+
-+#include <gtsam/base/Matrix.h>
-+#include <gtsam/base/DerivedValue.h>
-+#include <gtsam/base/Lie.h>
-+
-+#include <boost/serialization/nvp.hpp>
-+
-+#include <cmath>
-+
-+namespace gtsam {
-+
-+ /**
-+ * A Non-Depth Parallax Point
-+ * @addtogroup slam
-+ * \nosubgrouping
-+ */
-+ class GTSAM_EXPORT ParallaxAnglePoint2 : public DerivedValue<ParallaxAnglePoint2> {
-+ public:
-+ /// dimension of the variable - used to autodetect sizes
-+ static const size_t dimension = 2;
-+
-+ private:
-+ double yaw_; ///< Angle around z-axis, in rad.
-+ double pitch_; ///< Angle around y-axis, in rad.
-+
-+ public:
-+
-+ /// @name Standard Constructors
-+ /// @{
-+
-+ /// Default constructor creates a ParallaxAnglePoint2 oriented on x-axis.
-+ ParallaxAnglePoint2(): yaw_(0), pitch_(0) {}
-+
-+ /// Construct from x, y, and z coordinates
-+ ParallaxAnglePoint2(double yaw, double pitch): yaw_(yaw), pitch_(pitch) {}
-+
-+ /// @}
-+ /// @name Advanced Constructors
-+ /// @{
-+
-+ /// Construct from 2-element vector
-+ ParallaxAnglePoint2(const Vector& v) {
-+ if(v.size() != 2)
-+ throw std::invalid_argument("ParallaxAnglePoint2 constructor from Vector requires that the Vector have dimension 2");
-+ yaw_ = v(0);
-+ pitch_ = v(1);
-+ }
-+
-+ /// @}
-+ /// @name Testable
-+ /// @{
-+
-+ /** print with optional string */
-+ void print(const std::string& s = "") const;
-+
-+ /** equals with an tolerance */
-+ bool equals(const ParallaxAnglePoint2& p, double tol = 1e-9) const;
-+
-+ /// @}
-+ /// @name Manifold
-+ /// @{
-+
-+ /// dimension of the variable - used to autodetect sizes
-+ inline static size_t Dim() { return dimension; }
-+
-+ /// return dimensionality of tangent space, DOF = 3
-+ inline size_t dim() const { return dimension; }
-+
-+ /// Updates a with tangent space delta
-+ inline ParallaxAnglePoint2 retract(const Vector& v) const { return ParallaxAnglePoint2(*this + v); }
-+
-+ /// Returns inverse retraction
-+ inline Vector2 localCoordinates(const ParallaxAnglePoint2& q) const { return (q - *this).vector(); }
-+
-+ /// @}
-+ /// @name Standard Interface
-+ /// @{
-+
-+ /// assignment
-+ // ParallaxAnglePoint2 operator = (const ParallaxAnglePoint2& q) const;
-+
-+ ///add two points
-+ ParallaxAnglePoint2 operator + (const ParallaxAnglePoint2& q) const;
-+
-+ ///subtract two points
-+ ParallaxAnglePoint2 operator - (const ParallaxAnglePoint2& q) const;
-+
-+ /** return vectorized form (column-wise)*/
-+ Vector2 vector() const { return Vector2(yaw_,pitch_); }
-+
-+ /// get x
-+ inline double yaw() const {return yaw_;}
-+
-+ /// get y
-+ inline double pitch() const {return pitch_;}
-+
-+ /// @}
-+
-+ Vector3 directionVector(boost::optional<gtsam::Matrix&> H = boost::none) const;
-+
-+ /// Output stream operator
-+ GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const ParallaxAnglePoint2& p);
-+
-+ 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::make_nvp("ParallaxAnglePoint2",
-+ boost::serialization::base_object<Value>(*this));
-+ ar & BOOST_SERIALIZATION_NVP(yaw_);
-+ ar & BOOST_SERIALIZATION_NVP(pitch_);
-+ }
-+
-+ /// @}
-+
-+ };
-+
-+}
---- gtsam/geometry/ParallaxAnglePoint3.cpp 1970-01-01 01:00:00.000000000 +0100
-+++ gtsam/geometry/ParallaxAnglePoint3.cpp 2015-05-06 14:40:46.138648761 +0200
-@@ -0,0 +1,223 @@
-+/* ----------------------------------------------------------------------------
-+
-+ * 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 ParallaxAnglePoint3.cpp
-+ * @brief 3D Parallax Angle Point
-+ * @author Ellon P. Mendes
-+ */
-+
-+#include "ParallaxAnglePoint3.h"
-+
-+using namespace std;
-+
-+// Prototypes of local functions
-+double angleBetweenUnitVectors(
-+ const gtsam::Vector3 & v1, const gtsam::Vector3 & v2,
-+ boost::optional<gtsam::Matrix&> Dv1 = boost::none,
-+ boost::optional<gtsam::Matrix&> Dv2 = boost::none);
-+
-+double norm(gtsam::Vector3 v, boost::optional<gtsam::Matrix&> Dv = boost::none);
-+
-+namespace gtsam {
-+/* ************************************************************************* */
-+void ParallaxAnglePoint3::print(const string& s) const {
-+ cout << s << *this << endl;
-+}
-+
-+/* ************************************************************************* */
-+bool ParallaxAnglePoint3::equals(const ParallaxAnglePoint3 & q, double tol) const {
-+ return (fabs(yaw_ - q.yaw() ) < tol &&
-+ fabs(pitch_ - q.pitch() ) < tol &&
-+ fabs(parallax_ - q.parallax()) < tol);
-+}
-+
-+/* ************************************************************************* */
-+ParallaxAnglePoint3 ParallaxAnglePoint3::operator+(const ParallaxAnglePoint3& q) const {
-+ return ParallaxAnglePoint3(yaw_ + q.yaw_,
-+ pitch_ + q.pitch_,
-+ parallax_ + q.parallax_);
-+}
-+
-+/* ************************************************************************* */
-+ParallaxAnglePoint3 ParallaxAnglePoint3::operator-(const ParallaxAnglePoint3& q) const {
-+ return ParallaxAnglePoint3(yaw_ - q.yaw_,
-+ pitch_ - q.pitch_,
-+ parallax_ - q.parallax_);
-+}
-+
-+/* ************************************************************************* */
-+ostream &operator<<(ostream &os, const ParallaxAnglePoint3& p) {
-+ os << '(' << p.yaw() << ", " << p.pitch() << ", " << p.parallax() << ')';
-+ return os;
-+}
-+
-+/* ************************************************************************* */
-+Vector3 ParallaxAnglePoint3::directionVectorFromMainAnchor(
-+ boost::optional<gtsam::Matrix&> H) const
-+{
-+ double sy = sin(yaw_), cy = cos(yaw_), sp = sin(pitch_), cp = cos(pitch_);
-+ if(H)
-+ {
-+ H->resize(3,3);
-+ *H << -sy*cp, -cy*sp, 0,
-+ cy*cp, -sy*sp, 0,
-+ 0 , cp , 0;
-+ }
-+ return (Vector(3) << cy*cp, sy*cp, sp);
-+}
-+
-+/* ************************************************************************* */
-+Vector3 ParallaxAnglePoint3::directionVectorFromAssoAnchor(
-+ const Point3 & mainAnchor, const Point3 & assoAnchor,
-+ boost::optional<gtsam::Matrix&> Dpoint,
-+ boost::optional<gtsam::Matrix&> Dmain ,
-+ boost::optional<gtsam::Matrix&> Dasso ) const
-+{
-+ if (!Dasso)
-+ {
-+ return directionVectorFromOtheAnchor( mainAnchor, assoAnchor, assoAnchor, Dpoint, Dmain );
-+ }
-+
-+ Matrix Dothe;
-+ Vector3 directVec = directionVectorFromOtheAnchor(
-+ mainAnchor, assoAnchor, assoAnchor,
-+ Dpoint, Dmain, Dasso, Dothe);
-+
-+ *Dasso += Dothe;
-+
-+ return directVec;
-+
-+}
-+
-+/* ************************************************************************* */
-+Vector3 ParallaxAnglePoint3::directionVectorFromOtheAnchor(
-+ const Point3 & mainAnchor, const Point3 & assoAnchor, const Point3 & otheAnchor,
-+ boost::optional<gtsam::Matrix&> Dpoint,
-+ boost::optional<gtsam::Matrix&> Dmain ,
-+ boost::optional<gtsam::Matrix&> Dasso ,
-+ boost::optional<gtsam::Matrix&> Dothe ) const
-+{
-+
-+ if (!Dpoint && !Dmain && !Dasso && !Dothe)
-+ {
-+ // Get vector from main anchor to associated anchor
-+ Point3 main_T_asso(assoAnchor - mainAnchor);
-+
-+ // Get a normalized version of it
-+ Vector3 main_T_asso_normd(main_T_asso.normalize().vector());
-+
-+ // Get the lenght of the vector
-+ double norm_mTa = main_T_asso.norm();
-+
-+ // Get direction vector from main anchor to point
-+ // NOTE: It's already normalized
-+ Vector3 dirvec_mTp(directionVectorFromMainAnchor());
-+
-+ // Get the angle between these vectors
-+ double phi = angleBetweenUnitVectors(dirvec_mTp, main_T_asso_normd);
-+
-+ // Get vector from main anchor to other anchor
-+ Vector3 main_T_other((otheAnchor - mainAnchor).vector());
-+
-+ return sin(parallax_ + phi) * norm_mTa * dirvec_mTp - sin(parallax_) * main_T_other;
-+ }
-+
-+ // Same computation with jacobians
-+
-+ // Get vector from main anchor to associated anchor
-+ Matrix MTA_assoAnchor, MTA_mainAnchor;
-+ Point3 main_T_asso(assoAnchor.sub(mainAnchor, MTA_assoAnchor, MTA_mainAnchor));
-+
-+ // Get a normalized version of it
-+ Matrix MTANORMD_mta;
-+ Vector3 main_T_asso_normd(main_T_asso.normalize(MTANORMD_mta).vector());
-+
-+ // Get the lenght of the vector
-+ Matrix NORMMTA_mta;
-+ double norm_mTa = norm(main_T_asso.vector(), NORMMTA_mta);
-+
-+ // Get direction vector from main anchor to point
-+ // NOTE: It's already normalized
-+ Matrix DIRVECMTP_point;
-+ Vector3 dirvec_mTp(directionVectorFromMainAnchor(DIRVECMTP_point));
-+
-+ // Get the angle between these vectors
-+ Matrix PHI_dirvecmtp, PHI_mtanormd;
-+ double phi = angleBetweenUnitVectors(dirvec_mTp, main_T_asso_normd, PHI_dirvecmtp, PHI_mtanormd);
-+
-+ // Get vector from main anchor to other anchor
-+ Matrix MTO_otheanchor, MTO_mainanchor;
-+ Vector3 main_T_othe(otheAnchor.sub(mainAnchor, MTO_otheanchor, MTO_mainanchor).vector());
-+
-+ // Chain of partial derivates
-+ // main anchor
-+ if(Dmain)
-+ {
-+ *Dmain = dirvec_mTp * ( cos(parallax_ + phi) * norm_mTa * PHI_mtanormd * MTANORMD_mta * MTA_mainAnchor
-+ + sin(parallax_ + phi) * NORMMTA_mta * MTA_mainAnchor )
-+ - sin(parallax_) * MTO_mainanchor;
-+ }
-+ // associated anchor
-+ if(Dasso)
-+ {
-+ *Dasso = dirvec_mTp * ( cos(parallax_ + phi) * norm_mTa * PHI_mtanormd * MTANORMD_mta * MTA_assoAnchor
-+ + sin(parallax_ + phi) * NORMMTA_mta * MTA_assoAnchor );
-+ }
-+ // other anchor
-+ if(Dothe)
-+ {
-+ *Dothe = - sin(parallax_) * MTO_otheanchor;
-+ }
-+ // parallax point
-+ if(Dpoint)
-+ {
-+ Dpoint->resize(3,3);
-+ Dpoint->block(0,0,3,2) = (dirvec_mTp * (cos(parallax_ + phi)*PHI_dirvecmtp*DIRVECMTP_point.block(0,0,3,2)) + sin(parallax_ + phi)*DIRVECMTP_point.block(0,0,3,2)) * norm_mTa;
-+ Dpoint->block(0,2,3,1) = norm_mTa * dirvec_mTp * cos(parallax_ + phi) - main_T_othe * cos(parallax_);
-+ }
-+
-+ return sin(parallax_ + phi) * norm_mTa * dirvec_mTp - sin(parallax_) * main_T_othe;
-+
-+}
-+
-+}
-+
-+// Local Functions
-+double angleBetweenUnitVectors(
-+ const gtsam::Vector3 & v1, const gtsam::Vector3 & v2,
-+ boost::optional<gtsam::Matrix&> Dv1,
-+ boost::optional<gtsam::Matrix&> Dv2)
-+{
-+ double dot_prod = v1.dot(v2);
-+ if(Dv1)
-+ {
-+ Dv1->resize(1,3);
-+ *Dv1 = -(1/sqrt(1-(dot_prod*dot_prod))) * v2.transpose();
-+ }
-+ if(Dv2)
-+ {
-+ Dv2->resize(1,3);
-+ *Dv2 = -(1/sqrt(1-(dot_prod*dot_prod))) * v1.transpose();
-+ }
-+ return acos(dot_prod);
-+}
-+
-+double norm(gtsam::Vector3 v, boost::optional<gtsam::Matrix&> Dv)
-+{
-+ double n = v.norm();
-+ if(Dv)
-+ {
-+ Dv->resize(1,3);
-+ *Dv << v(0)/n, v(1)/n, v(2)/n;
-+ }
-+ return n;
-+}
---- gtsam/geometry/ParallaxAnglePoint3.h 1970-01-01 01:00:00.000000000 +0100
-+++ gtsam/geometry/ParallaxAnglePoint3.h 2015-05-06 15:02:42.114738787 +0200
-@@ -0,0 +1,165 @@
-+/* ----------------------------------------------------------------------------
-+
-+ * 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 ParallaxAnglePoint3.h
-+ * @brief 3D Parallax Angle Point
-+ * @author Ellon P. Mendes
-+ */
-+
-+// \callgraph
-+
-+#pragma once
-+
-+#include <gtsam/base/Matrix.h>
-+#include <gtsam/base/DerivedValue.h>
-+#include <gtsam/base/Lie.h>
-+
-+#include <gtsam/geometry/Point3.h>
-+
-+#include <boost/serialization/nvp.hpp>
-+
-+#include <cmath>
-+
-+namespace gtsam {
-+
-+ /**
-+ * A 3D Parallax Point
-+ * @addtogroup slam
-+ * \nosubgrouping
-+ */
-+ class GTSAM_EXPORT ParallaxAnglePoint3 : public DerivedValue<ParallaxAnglePoint3> {
-+ public:
-+ /// dimension of the variable - used to autodetect sizes
-+ static const size_t dimension = 3;
-+
-+ private:
-+ double yaw_; ///< Angle around z-axis, in rad.
-+ double pitch_; ///< Angle around y-axis, in rad.
-+ double parallax_; ///< Angle between main anchor to feature vector and associated anchor to feature, in rad.
-+
-+ public:
-+
-+ /// @name Standard Constructors
-+ /// @{
-+
-+ /// Default constructor creates a ParallaxAnglePoint3 oriented on x-axis, and with coincident anchors.
-+ ParallaxAnglePoint3(): yaw_(0), pitch_(0), parallax_(0) {}
-+
-+ /// Construct from yaw, pitch, and parallax coordinates
-+ ParallaxAnglePoint3(double yaw, double pitch, double parallax): yaw_(yaw), pitch_(pitch), parallax_(parallax) {}
-+
-+ /// @}
-+ /// @name Advanced Constructors
-+ /// @{
-+
-+ /// Construct from 2-element vector
-+ ParallaxAnglePoint3(const Vector& v) {
-+ if(v.size() != 3)
-+ throw std::invalid_argument("ParallaxAnglePoint3 constructor from Vector requires that the Vector have dimension 3");
-+ yaw_ = v(0);
-+ pitch_ = v(1);
-+ parallax_ = v(2);
-+ }
-+
-+ /// @}
-+ /// @name Testable
-+ /// @{
-+
-+ /** print with optional string */
-+ void print(const std::string& s = "") const;
-+
-+ /** equals with an tolerance */
-+ bool equals(const ParallaxAnglePoint3& p, double tol = 1e-9) const;
-+
-+ /// @}
-+ /// @name Manifold
-+ /// @{
-+
-+ /// dimension of the variable - used to autodetect sizes
-+ inline static size_t Dim() { return dimension; }
-+
-+ /// return dimensionality of tangent space, DOF = 3
-+ inline size_t dim() const { return dimension; }
-+
-+ /// Updates a with tangent space delta
-+ inline ParallaxAnglePoint3 retract(const Vector& v) const { return ParallaxAnglePoint3(*this + v); }
-+
-+ /// Returns inverse retraction
-+ inline Vector3 localCoordinates(const ParallaxAnglePoint3& q) const { return (q - *this).vector(); }
-+
-+ /// @}
-+ /// @name Standard Interface
-+ /// @{
-+
-+ /// assignment
-+ // ParallaxAnglePoint3 operator = (const ParallaxAnglePoint3& q) const;
-+
-+ ///add two points
-+ ParallaxAnglePoint3 operator + (const ParallaxAnglePoint3& q) const;
-+
-+ ///subtract two points
-+ ParallaxAnglePoint3 operator - (const ParallaxAnglePoint3& q) const;
-+
-+ /** return vectorized form (column-wise)*/
-+ Vector3 vector() const { return Vector3(yaw_,pitch_,parallax_); }
-+
-+ /// get yaw
-+ inline double yaw() const {return yaw_;}
-+
-+ /// get pitch
-+ inline double pitch() const {return pitch_;}
-+
-+ /// get parallax
-+ inline double parallax() const {return parallax_;}
-+
-+ /// @}
-+
-+ Vector3 directionVectorFromMainAnchor(boost::optional<gtsam::Matrix&> H = boost::none) const;
-+
-+ Vector3 directionVectorFromAssoAnchor(
-+ const Point3 & mainAnchor, const Point3 & assoAnchor,
-+ boost::optional<gtsam::Matrix&> Dpoint = boost::none,
-+ boost::optional<gtsam::Matrix&> Dmain = boost::none,
-+ boost::optional<gtsam::Matrix&> Dasso = boost::none) const;
-+
-+ Vector3 directionVectorFromOtheAnchor(
-+ const Point3 & mainAnchor, const Point3 & assoAnchor, const Point3 & otheAnchor,
-+ boost::optional<gtsam::Matrix&> Dpoint = boost::none,
-+ boost::optional<gtsam::Matrix&> Dmain = boost::none,
-+ boost::optional<gtsam::Matrix&> Dasso = boost::none,
-+ boost::optional<gtsam::Matrix&> Dothe = boost::none) const;
-+
-+ /// Output stream operator
-+ GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const ParallaxAnglePoint3& p);
-+
-+ 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::make_nvp("ParallaxAnglePoint3",
-+ boost::serialization::base_object<Value>(*this));
-+ ar & BOOST_SERIALIZATION_NVP(yaw_);
-+ ar & BOOST_SERIALIZATION_NVP(pitch_);
-+ ar & BOOST_SERIALIZATION_NVP(parallax_);
-+ }
-+
-+ /// @}
-+
-+ };
-+
-+}
---- gtsam/geometry/tests/testParallaxAnglePoint2.cpp 1970-01-01 01:00:00.000000000 +0100
-+++ gtsam/geometry/tests/testParallaxAnglePoint2.cpp 2015-05-06 14:27:39.561152730 +0200
-@@ -0,0 +1,220 @@
-+/* ----------------------------------------------------------------------------
-+
-+ * 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 testParallaxAnglePoint2.cpp
-+ * @brief Unit tests for ParallaxAnglePoint2 class
-+ * @author Ellon P. Mendes
-+ **/
-+
-+#include <gtsam/geometry/ParallaxAnglePoint2.h>
-+#include <gtsam/base/Testable.h>
-+#include <gtsam/base/numericalDerivative.h>
-+#include <gtsam/base/lieProxies.h>
-+#include <CppUnitLite/TestHarness.h>
-+
-+using namespace std;
-+using namespace gtsam;
-+
-+GTSAM_CONCEPT_TESTABLE_INST(ParallaxAnglePoint2)
-+GTSAM_CONCEPT_MANIFOLD_INST(ParallaxAnglePoint2)
-+
-+/* ************************************************************************* */
-+TEST(ParallaxAnglePoint2, constructor) {
-+ ParallaxAnglePoint2 p1(1, 2), p2 = p1;
-+ EXPECT(assert_equal(p1, p2));
-+}
-+
-+/* ************************************************************************* */
-+// TEST(ParallaxAnglePoint2, Lie) {
-+// Point2 p1(1, 2), p2(4, 5);
-+// Matrix H1, H2;
-+
-+// EXPECT(assert_equal(Point2(5,7), p1.compose(p2, H1, H2)));
-+// EXPECT(assert_equal(eye(2), H1));
-+// EXPECT(assert_equal(eye(2), H2));
-+
-+// EXPECT(assert_equal(Point2(3,3), p1.between(p2, H1, H2)));
-+// EXPECT(assert_equal(-eye(2), H1));
-+// EXPECT(assert_equal(eye(2), H2));
-+
-+// EXPECT(assert_equal(Point2(5,7), p1.retract((Vector(2) << 4., 5.))));
-+// EXPECT(assert_equal((Vector(2) << 3.,3.), p1.localCoordinates(p2)));
-+// }
-+
-+/* ************************************************************************* */
-+TEST( ParallaxAnglePoint2, expmap) {
-+ Vector d(2);
-+ d(0) = 1;
-+ d(1) = -1;
-+ ParallaxAnglePoint2 a(4, 5), b = a.retract(d), c(5, 4);
-+ EXPECT(assert_equal(b,c));
-+}
-+
-+/* ************************************************************************* */
-+TEST( ParallaxAnglePoint2, arithmetic) {
-+ // EXPECT(assert_equal( ParallaxAnglePoint2(-5,-6), -ParallaxAnglePoint2(5,6) ));
-+ EXPECT(assert_equal( ParallaxAnglePoint2(5,6), ParallaxAnglePoint2(4,5)+ParallaxAnglePoint2(1,1)));
-+ EXPECT(assert_equal( ParallaxAnglePoint2(3,4), ParallaxAnglePoint2(4,5)-ParallaxAnglePoint2(1,1) ));
-+ // EXPECT(assert_equal( ParallaxAnglePoint2(8,6), ParallaxAnglePoint2(4,3)*2));
-+ // EXPECT(assert_equal( ParallaxAnglePoint2(4,6), 2*ParallaxAnglePoint2(2,3)));
-+ // EXPECT(assert_equal( ParallaxAnglePoint2(2,3), ParallaxAnglePoint2(4,6)/2));
-+}
-+
-+// namespace {
-+// /* ************************************************************************* */
-+// // some shared test values
-+// ParallaxAnglePoint2 x1, x2(1, 1), x3(1, 1);
-+// ParallaxAnglePoint2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
-+
-+// /* ************************************************************************* */
-+// // LieVector norm_proxy(const ParallaxAnglePoint2& point) {
-+// // return LieVector(point.norm());
-+// // }
-+// }
-+// TEST( ParallaxAnglePoint2, norm ) {
-+// Point2 p0(cos(5.0), sin(5.0));
-+// DOUBLES_EQUAL(1, p0.norm(), 1e-6);
-+// Point2 p1(4, 5), p2(1, 1);
-+// DOUBLES_EQUAL( 5, p1.distance(p2), 1e-6);
-+// DOUBLES_EQUAL( 5, (p2-p1).norm(), 1e-6);
-+
-+// Matrix expectedH, actualH;
-+// double actual;
-+
-+// // exception, for (0,0) derivative is [Inf,Inf] but we return [1,1]
-+// actual = x1.norm(actualH);
-+// EXPECT_DOUBLES_EQUAL(0, actual, 1e-9);
-+// expectedH = (Matrix(1, 2) << 1.0, 1.0);
-+// EXPECT(assert_equal(expectedH,actualH));
-+
-+// actual = x2.norm(actualH);
-+// EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual, 1e-9);
-+// expectedH = numericalDerivative11(norm_proxy, x2);
-+// EXPECT(assert_equal(expectedH,actualH));
-+// }
-+
-+/* ************************************************************************* */
-+// namespace {
-+// LieVector distance_proxy(const Point2& location, const Point2& point) {
-+// return LieVector(location.distance(point));
-+// }
-+// }
-+// TEST( Point2, distance ) {
-+// Matrix expectedH1, actualH1, expectedH2, actualH2;
-+
-+// // establish distance is indeed zero
-+// EXPECT_DOUBLES_EQUAL(1, x1.distance(l1), 1e-9);
-+
-+// // establish distance is indeed 45 degrees
-+// EXPECT_DOUBLES_EQUAL(sqrt(2.0), x1.distance(l2), 1e-9);
-+
-+// // Another pair
-+// double actual23 = x2.distance(l3, actualH1, actualH2);
-+// EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual23, 1e-9);
-+
-+// // Check numerical derivatives
-+// expectedH1 = numericalDerivative21(distance_proxy, x2, l3);
-+// expectedH2 = numericalDerivative22(distance_proxy, x2, l3);
-+// EXPECT(assert_equal(expectedH1,actualH1));
-+// EXPECT(assert_equal(expectedH2,actualH2));
-+
-+// // Another test
-+// double actual34 = x3.distance(l4, actualH1, actualH2);
-+// EXPECT_DOUBLES_EQUAL(2, actual34, 1e-9);
-+
-+// // Check numerical derivatives
-+// expectedH1 = numericalDerivative21(distance_proxy, x3, l4);
-+// expectedH2 = numericalDerivative22(distance_proxy, x3, l4);
-+// EXPECT(assert_equal(expectedH1,actualH1));
-+// EXPECT(assert_equal(expectedH2,actualH2));
-+// }
-+
-+/* ************************************************************************* */
-+// TEST( Point2, circleCircleIntersection) {
-+
-+// double offset = 0.994987;
-+// // Test intersections of circle moving from inside to outside
-+
-+// list<Point2> inside = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(0,0),1);
-+// EXPECT_LONGS_EQUAL(0,inside.size());
-+
-+// list<Point2> touching1 = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(4,0),1);
-+// EXPECT_LONGS_EQUAL(1,touching1.size());
-+// EXPECT(assert_equal(Point2(5,0), touching1.front()));
-+
-+// list<Point2> common = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(5,0),1);
-+// EXPECT_LONGS_EQUAL(2,common.size());
-+// EXPECT(assert_equal(Point2(4.9, offset), common.front(), 1e-6));
-+// EXPECT(assert_equal(Point2(4.9, -offset), common.back(), 1e-6));
-+
-+// list<Point2> touching2 = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(6,0),1);
-+// EXPECT_LONGS_EQUAL(1,touching2.size());
-+// EXPECT(assert_equal(Point2(5,0), touching2.front()));
-+
-+// // test rotated case
-+// list<Point2> rotated = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(0,5),1);
-+// EXPECT_LONGS_EQUAL(2,rotated.size());
-+// EXPECT(assert_equal(Point2(-offset, 4.9), rotated.front(), 1e-6));
-+// EXPECT(assert_equal(Point2( offset, 4.9), rotated.back(), 1e-6));
-+
-+// // test r1<r2
-+// list<Point2> smaller = Point2::CircleCircleIntersection(Point2(0,0),1,Point2(5,0),5);
-+// EXPECT_LONGS_EQUAL(2,smaller.size());
-+// EXPECT(assert_equal(Point2(0.1, offset), smaller.front(), 1e-6));
-+// EXPECT(assert_equal(Point2(0.1, -offset), smaller.back(), 1e-6));
-+
-+// // test offset case, r1>r2
-+// list<Point2> offset1 = Point2::CircleCircleIntersection(Point2(1,1),5,Point2(6,1),1);
-+// EXPECT_LONGS_EQUAL(2,offset1.size());
-+// EXPECT(assert_equal(Point2(5.9, 1+offset), offset1.front(), 1e-6));
-+// EXPECT(assert_equal(Point2(5.9, 1-offset), offset1.back(), 1e-6));
-+
-+// // test offset case, r1<r2
-+// list<Point2> offset2 = Point2::CircleCircleIntersection(Point2(6,1),1,Point2(1,1),5);
-+// EXPECT_LONGS_EQUAL(2,offset2.size());
-+// EXPECT(assert_equal(Point2(5.9, 1-offset), offset2.front(), 1e-6));
-+// EXPECT(assert_equal(Point2(5.9, 1+offset), offset2.back(), 1e-6));
-+
-+// }
-+
-+/* ************************************************************************* */
-+TEST( ParallaxAnglePoint2, stream) {
-+ ParallaxAnglePoint2 p(1, 2);
-+ std::ostringstream os;
-+ os << p;
-+ EXPECT(os.str() == "(1, 2)");
-+}
-+
-+/* ************************************************************************* */
-+TEST( ParallaxAnglePoint2, directionVector) {
-+ // Vector From Main
-+ ParallaxAnglePoint2 p(M_PI/6, M_PI/3);
-+ Vector ExpectedDirectionVector = (Vector(3) << 0.433012701892219,0.25,0.866025403784439);
-+
-+ CHECK(assert_equal(ExpectedDirectionVector, p.directionVector(), 1e-6));
-+
-+ // Vector From Main with Jacobian
-+ Matrix H(3,2), ExpectedH(3,2);
-+ ExpectedH << -0.25 , -0.75 ,
-+ 0.433012701892219, -0.433012701892219,
-+ 0 , 0.5;
-+ CHECK(assert_equal(ExpectedDirectionVector, p.directionVector(H), 1e-6));
-+ EXPECT(assert_equal(ExpectedH, H, 1e-6));
-+}
-+
-+/* ************************************************************************* */
-+int main () {
-+ TestResult tr;
-+ return TestRegistry::runAllTests(tr);
-+}
-+/* ************************************************************************* */
-+
---- gtsam/geometry/tests/testParallaxAnglePoint3.cpp 1970-01-01 01:00:00.000000000 +0100
-+++ gtsam/geometry/tests/testParallaxAnglePoint3.cpp 2015-05-06 15:12:50.554157465 +0200
-@@ -0,0 +1,274 @@
-+/* ----------------------------------------------------------------------------
-+
-+ * 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 testParallaxAnglePoint3.cpp
-+ * @brief Unit tests for ParallaxAnglePoint3 class
-+ * @author Ellon P. Mendes
-+ **/
-+
-+#include <gtsam/geometry/ParallaxAnglePoint3.h>
-+#include <gtsam/base/Testable.h>
-+#include <gtsam/base/numericalDerivative.h>
-+#include <gtsam/base/lieProxies.h>
-+#include <CppUnitLite/TestHarness.h>
-+
-+using namespace std;
-+using namespace gtsam;
-+
-+GTSAM_CONCEPT_TESTABLE_INST(ParallaxAnglePoint3)
-+GTSAM_CONCEPT_MANIFOLD_INST(ParallaxAnglePoint3)
-+
-+/* ************************************************************************* */
-+TEST(ParallaxAnglePoint3, constructor) {
-+ ParallaxAnglePoint3 p1(1, 2, 3), p2 = p1;
-+ EXPECT(assert_equal(p1, p2));
-+}
-+
-+/* ************************************************************************* */
-+// TEST(ParallaxAnglePoint2, Lie) {
-+// Point2 p1(1, 2), p2(4, 5);
-+// Matrix H1, H2;
-+
-+// EXPECT(assert_equal(Point2(5,7), p1.compose(p2, H1, H2)));
-+// EXPECT(assert_equal(eye(2), H1));
-+// EXPECT(assert_equal(eye(2), H2));
-+
-+// EXPECT(assert_equal(Point2(3,3), p1.between(p2, H1, H2)));
-+// EXPECT(assert_equal(-eye(2), H1));
-+// EXPECT(assert_equal(eye(2), H2));
-+
-+// EXPECT(assert_equal(Point2(5,7), p1.retract((Vector(2) << 4., 5.))));
-+// EXPECT(assert_equal((Vector(2) << 3.,3.), p1.localCoordinates(p2)));
-+// }
-+
-+/* ************************************************************************* */
-+TEST( ParallaxAnglePoint3, expmap) {
-+ Vector d(3);
-+ d(0) = 1.;
-+ d(1) = -1.;
-+ d(2) = 2.;
-+ ParallaxAnglePoint3 a(4., 5., 6.), b = a.retract(d), c(5., 4., 8.);
-+ EXPECT(assert_equal(c,b));
-+ EXPECT(assert_equal(d,a.localCoordinates(c)));
-+}
-+
-+/* ************************************************************************* */
-+TEST( ParallaxAnglePoint3, arithmetic) {
-+ // EXPECT(assert_equal( ParallaxAnglePoint2(-5,-6), -ParallaxAnglePoint2(5,6) ));
-+ EXPECT(assert_equal( ParallaxAnglePoint3(5,6,7), ParallaxAnglePoint3(4,5,6)+ParallaxAnglePoint3(1,1,1)));
-+ EXPECT(assert_equal( ParallaxAnglePoint3(3,4,5), ParallaxAnglePoint3(4,5,6)-ParallaxAnglePoint3(1,1,1)));
-+ // EXPECT(assert_equal( ParallaxAnglePoint2(8,6), ParallaxAnglePoint2(4,3)*2));
-+ // EXPECT(assert_equal( ParallaxAnglePoint2(4,6), 2*ParallaxAnglePoint2(2,3)));
-+ // EXPECT(assert_equal( ParallaxAnglePoint2(2,3), ParallaxAnglePoint2(4,6)/2));
-+}
-+
-+// namespace {
-+// /* ************************************************************************* */
-+// // some shared test values
-+// ParallaxAnglePoint2 x1, x2(1, 1), x3(1, 1);
-+// ParallaxAnglePoint2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
-+
-+// /* ************************************************************************* */
-+// // LieVector norm_proxy(const ParallaxAnglePoint2& point) {
-+// // return LieVector(point.norm());
-+// // }
-+// }
-+// TEST( ParallaxAnglePoint2, norm ) {
-+// Point2 p0(cos(5.0), sin(5.0));
-+// DOUBLES_EQUAL(1, p0.norm(), 1e-6);
-+// Point2 p1(4, 5), p2(1, 1);
-+// DOUBLES_EQUAL( 5, p1.distance(p2), 1e-6);
-+// DOUBLES_EQUAL( 5, (p2-p1).norm(), 1e-6);
-+
-+// Matrix expectedH, actualH;
-+// double actual;
-+
-+// // exception, for (0,0) derivative is [Inf,Inf] but we return [1,1]
-+// actual = x1.norm(actualH);
-+// EXPECT_DOUBLES_EQUAL(0, actual, 1e-9);
-+// expectedH = (Matrix(1, 2) << 1.0, 1.0);
-+// EXPECT(assert_equal(expectedH,actualH));
-+
-+// actual = x2.norm(actualH);
-+// EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual, 1e-9);
-+// expectedH = numericalDerivative11(norm_proxy, x2);
-+// EXPECT(assert_equal(expectedH,actualH));
-+// }
-+
-+/* ************************************************************************* */
-+// namespace {
-+// LieVector distance_proxy(const Point2& location, const Point2& point) {
-+// return LieVector(location.distance(point));
-+// }
-+// }
-+// TEST( Point2, distance ) {
-+// Matrix expectedH1, actualH1, expectedH2, actualH2;
-+
-+// // establish distance is indeed zero
-+// EXPECT_DOUBLES_EQUAL(1, x1.distance(l1), 1e-9);
-+
-+// // establish distance is indeed 45 degrees
-+// EXPECT_DOUBLES_EQUAL(sqrt(2.0), x1.distance(l2), 1e-9);
-+
-+// // Another pair
-+// double actual23 = x2.distance(l3, actualH1, actualH2);
-+// EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual23, 1e-9);
-+
-+// // Check numerical derivatives
-+// expectedH1 = numericalDerivative21(distance_proxy, x2, l3);
-+// expectedH2 = numericalDerivative22(distance_proxy, x2, l3);
-+// EXPECT(assert_equal(expectedH1,actualH1));
-+// EXPECT(assert_equal(expectedH2,actualH2));
-+
-+// // Another test
-+// double actual34 = x3.distance(l4, actualH1, actualH2);
-+// EXPECT_DOUBLES_EQUAL(2, actual34, 1e-9);
-+
-+// // Check numerical derivatives
-+// expectedH1 = numericalDerivative21(distance_proxy, x3, l4);
-+// expectedH2 = numericalDerivative22(distance_proxy, x3, l4);
-+// EXPECT(assert_equal(expectedH1,actualH1));
-+// EXPECT(assert_equal(expectedH2,actualH2));
-+// }
-+
-+/* ************************************************************************* */
-+// TEST( Point2, circleCircleIntersection) {
-+
-+// double offset = 0.994987;
-+// // Test intersections of circle moving from inside to outside
-+
-+// list<Point2> inside = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(0,0),1);
-+// EXPECT_LONGS_EQUAL(0,inside.size());
-+
-+// list<Point2> touching1 = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(4,0),1);
-+// EXPECT_LONGS_EQUAL(1,touching1.size());
-+// EXPECT(assert_equal(Point2(5,0), touching1.front()));
-+
-+// list<Point2> common = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(5,0),1);
-+// EXPECT_LONGS_EQUAL(2,common.size());
-+// EXPECT(assert_equal(Point2(4.9, offset), common.front(), 1e-6));
-+// EXPECT(assert_equal(Point2(4.9, -offset), common.back(), 1e-6));
-+
-+// list<Point2> touching2 = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(6,0),1);
-+// EXPECT_LONGS_EQUAL(1,touching2.size());
-+// EXPECT(assert_equal(Point2(5,0), touching2.front()));
-+
-+// // test rotated case
-+// list<Point2> rotated = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(0,5),1);
-+// EXPECT_LONGS_EQUAL(2,rotated.size());
-+// EXPECT(assert_equal(Point2(-offset, 4.9), rotated.front(), 1e-6));
-+// EXPECT(assert_equal(Point2( offset, 4.9), rotated.back(), 1e-6));
-+
-+// // test r1<r2
-+// list<Point2> smaller = Point2::CircleCircleIntersection(Point2(0,0),1,Point2(5,0),5);
-+// EXPECT_LONGS_EQUAL(2,smaller.size());
-+// EXPECT(assert_equal(Point2(0.1, offset), smaller.front(), 1e-6));
-+// EXPECT(assert_equal(Point2(0.1, -offset), smaller.back(), 1e-6));
-+
-+// // test offset case, r1>r2
-+// list<Point2> offset1 = Point2::CircleCircleIntersection(Point2(1,1),5,Point2(6,1),1);
-+// EXPECT_LONGS_EQUAL(2,offset1.size());
-+// EXPECT(assert_equal(Point2(5.9, 1+offset), offset1.front(), 1e-6));
-+// EXPECT(assert_equal(Point2(5.9, 1-offset), offset1.back(), 1e-6));
-+
-+// // test offset case, r1<r2
-+// list<Point2> offset2 = Point2::CircleCircleIntersection(Point2(6,1),1,Point2(1,1),5);
-+// EXPECT_LONGS_EQUAL(2,offset2.size());
-+// EXPECT(assert_equal(Point2(5.9, 1-offset), offset2.front(), 1e-6));
-+// EXPECT(assert_equal(Point2(5.9, 1+offset), offset2.back(), 1e-6));
-+
-+// }
-+
-+/* ************************************************************************* */
-+TEST( ParallaxAnglePoint3, stream) {
-+ ParallaxAnglePoint3 p(1, 2, 3);
-+ std::ostringstream os;
-+ os << p;
-+ EXPECT(os.str() == "(1, 2, 3)");
-+}
-+
-+/* ************************************************************************* */
-+TEST( ParallaxAnglePoint3, directionVector) {
-+ Point3 mainAnchor(0, -3, 0);
-+ Point3 assoAnchor(0, -5, 5);
-+ Point3 otheAnchor(7, 0, 0);
-+ ParallaxAnglePoint3 p(M_PI/2, 0, M_PI/4);
-+
-+ Vector ExpectedVecFromMain = (Vector3() << 0, 1, 0);
-+ Vector ExpectedVecFromOthe = (Vector3() << -4.949747468305833, 0, 0);
-+ Vector ExpectedVecFromAsso = (Vector3() << 0, 3.535533905932738, -3.535533905932738);
-+
-+ // Jacobian matrix evaluated with the values above (check ParallaxPointJacobianTest.m)
-+ Matrix3 EXPECTED_DIRECVECFROMOTHER_tm = (Matrix3() << 0.707106781186548, 0, 0,
-+ 0, -0.000000000000000, -0.707106781186548,
-+ 0, 0, 0.707106781186548);
-+
-+ Matrix3 EXPECTED_DIRECVECFROMOTHER_ta = (Matrix3() << 0, 0, 0,
-+ 0, 0.707106781186548, 0.707106781186548,
-+ 0, 0, 0);
-+
-+ Matrix3 EXPECTED_DIRECVECFROMOTHER_to = (Matrix3() << -0.707106781186548, 0, 0,
-+ 0, -0.707106781186548, 0,
-+ 0, 0, -0.707106781186548);
-+
-+ Matrix3 EXPECTED_DIRECVECFROMOTHER_point = (Matrix3() << -2.121320343559642, 0, -4.949747468305833,
-+ 0, 4.949747468305833, -7.071067811865476,
-+ 0, 2.121320343559642, 0);
-+
-+ Matrix3 EXPECTED_DIRECVECFROMASSO_tm = (Matrix3() << 0.707106781186548, 0, 0,
-+ 0, -0.000000000000000, -0.707106781186548,
-+ 0, 0, 0.707106781186548);
-+
-+ Matrix3 EXPECTED_DIRECVECFROMASSO_ta = (Matrix3() << -0.707106781186548, 0, 0,
-+ 0, 0.000000000000000, 0.707106781186548,
-+ 0, 0, -0.707106781186548);
-+
-+ Matrix3 EXPECTED_DIRECVECFROMASSO_point = (Matrix3() << -2.121320343559642, 0, 0,
-+ 0, 4.949747468305833, -3.535533905932738,
-+ 0, 2.121320343559642, -3.535533905932738);
-+
-+ Matrix3 EXPECTED_DIRECVECFROMMAIN_point = (Matrix3() << -1, 0, 0,
-+ 0, 0, 0,
-+ 0, 1, 0);
-+
-+ Matrix H_point, H_main, H_asso, H_other;
-+ CHECK(assert_equal(ExpectedVecFromMain, p.directionVectorFromMainAnchor(), 1e-6));
-+ CHECK(assert_equal(ExpectedVecFromMain, p.directionVectorFromMainAnchor(H_point), 1e-6));
-+ EXPECT(assert_equal(EXPECTED_DIRECVECFROMMAIN_point, H_point, 1e-6));
-+
-+ H_point = zeros(3,3);
-+ CHECK(assert_equal(ExpectedVecFromAsso, p.directionVectorFromAssoAnchor(mainAnchor, assoAnchor), 1e-6));
-+ CHECK(assert_equal(ExpectedVecFromAsso, p.directionVectorFromAssoAnchor(mainAnchor, assoAnchor, H_point, H_main, H_asso), 1e-6));
-+ EXPECT(assert_equal(EXPECTED_DIRECVECFROMASSO_point, H_point, 1e-6));
-+ EXPECT(assert_equal(EXPECTED_DIRECVECFROMASSO_tm, H_main, 1e-6));
-+ EXPECT(assert_equal(EXPECTED_DIRECVECFROMASSO_ta, H_asso, 1e-6));
-+
-+
-+ H_point = zeros(3,3);
-+ H_main = zeros(3,3);
-+ H_asso = zeros(3,3);
-+ CHECK(assert_equal(ExpectedVecFromOthe, p.directionVectorFromOtheAnchor(mainAnchor, assoAnchor, otheAnchor), 1e-6));
-+ CHECK(assert_equal(ExpectedVecFromOthe, p.directionVectorFromOtheAnchor(mainAnchor, assoAnchor, otheAnchor, H_point, H_main, H_asso, H_other), 1e-6));
-+ EXPECT(assert_equal(EXPECTED_DIRECVECFROMOTHER_point, H_point, 1e-6));
-+ EXPECT(assert_equal(EXPECTED_DIRECVECFROMOTHER_tm, H_main, 1e-6));
-+ EXPECT(assert_equal(EXPECTED_DIRECVECFROMOTHER_ta, H_asso, 1e-6));
-+ EXPECT(assert_equal(EXPECTED_DIRECVECFROMOTHER_to, H_other, 1e-6));
-+
-+}
-+
-+/* ************************************************************************* */
-+int main () {
-+ TestResult tr;
-+ return TestRegistry::runAllTests(tr);
-+}
-+/* ************************************************************************* */
-+
---- gtsam/slam/ParallaxAngleProjectionFactor.h 1970-01-01 01:00:00.000000000 +0100
-+++ gtsam/slam/ParallaxAngleProjectionFactor.h 2015-05-05 22:30:18.017247984 +0200
-@@ -0,0 +1,876 @@
-+/* ----------------------------------------------------------------------------
-+
-+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
-+ * Atlanta, Georgia 30332-0415
-+ * All Rights Reserved
-+ * GTSAM Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-+ * Parallax Angle Projection Factor by Ellon P. Mendes
-+
-+ * See LICENSE for the license information
-+
-+ * -------------------------------------------------------------------------- */
-+
-+/**
-+ * @file ParallaxAngleProjectionFactor.h
-+ * @brief Parallax Angle projection factors
-+ * @author Ellon P. Mendes
-+ */
-+
-+#pragma once
-+
-+#include <gtsam/nonlinear/NonlinearFactor.h>
-+#include <gtsam/geometry/SimpleCamera.h>
-+#include <boost/optional.hpp>
-+
-+#include <gtsam/geometry/Point3.h>
-+#include <gtsam/geometry/Pose3.h>
-+#include <gtsam/geometry/ParallaxAnglePoint2.h>
-+#include <gtsam/geometry/ParallaxAnglePoint3.h>
-+
-+namespace gtsam {
-+
-+ /** Non-linear factor for a 2D measurement to a Parallax Angle Point with only one anchor.
-+ *
-+ * Non-linear factor for a constraint derived from a 2D measurement bewteen a robot pose and a
-+ * landmark parametrized as Parallax Angle Point with no depth (a.k.a ParallaxAnglePoint2). The
-+ * measurement was taken from the anchor, and the calibration is known here.
-+ *
-+ * @addtogroup SLAM
-+ */
-+ template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
-+ class ParallaxAngleSingleAnchorProjectionFactor: public NoiseModelFactor2<POSE, LANDMARK>
-+ {
-+ protected:
-+
-+ // Keep a copy of measurement and calibration for I/O
-+ Point2 measured_; ///< 2D measurement
-+ boost::shared_ptr<CALIBRATION> K_; ///< shared pointer to calibration object
-+ boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
-+
-+ // verbosity handling for Cheirality Exceptions
-+ bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
-+ bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
-+
-+ public:
-+
-+ /// shorthand for base class type
-+ typedef NoiseModelFactor2<POSE, LANDMARK> Base;
-+
-+ /// shorthand for this class
-+ typedef ParallaxAngleSingleAnchorProjectionFactor<POSE, LANDMARK, CALIBRATION> This;
-+
-+ /// shorthand for a smart pointer to a factor
-+ typedef boost::shared_ptr<This> shared_ptr;
-+
-+ /// Default constructor
-+ ParallaxAngleSingleAnchorProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {}
-+
-+ /**
-+ * Constructor
-+ * TODO: Mark argument order standard (keys, measurement, parameters)
-+ * @param measured is the 2 dimensional location of point in image (the measurement)
-+ * @param model is the standard deviation
-+ * @param mainAnchorKey is the index of main anchor
-+ * @param pointKey is the index of the landmark
-+ * @param K shared pointer to the constant calibration
-+ * @param body_P_sensor is the transform from body to sensor frame (default identity)
-+ */
-+ ParallaxAngleSingleAnchorProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
-+ Key mainAnchorKey, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
-+ boost::optional<POSE> body_P_sensor = boost::none) :
-+ Base(model, mainAnchorKey, pointKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
-+ throwCheirality_(false), verboseCheirality_(false) {}
-+
-+ /**
-+ * Constructor with exception-handling flags
-+ * TODO: Mark argument order standard (keys, measurement, parameters)
-+ * @param measured is the 2 dimensional location of point in image (the measurement)
-+ * @param model is the standard deviation
-+ * @param mainAnchorKey is the index of main anchor
-+ * @param pointKey is the index of the landmark
-+ * @param K shared pointer to the constant calibration
-+ * @param throwCheirality determines whether Cheirality exceptions are rethrown
-+ * @param verboseCheirality determines whether exceptions are printed for Cheirality
-+ * @param body_P_sensor is the transform from body to sensor frame (default identity)
-+ */
-+ ParallaxAngleSingleAnchorProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
-+ Key mainAnchorKey, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
-+ bool throwCheirality, bool verboseCheirality,
-+ boost::optional<POSE> body_P_sensor = boost::none) :
-+ Base(model, mainAnchorKey, pointKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
-+ throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
-+
-+ /** Virtual destructor */
-+ virtual ~ParallaxAngleSingleAnchorProjectionFactor() {}
-+
-+ /// @return a deep copy of this factor
-+ virtual gtsam::NonlinearFactor::shared_ptr clone() const {
-+ return boost::static_pointer_cast<gtsam::NonlinearFactor>(
-+ gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
-+
-+ /**
-+ * print
-+ * @param s optional string naming the factor
-+ * @param keyFormatter optional formatter useful for printing Symbols
-+ */
-+ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
-+ std::cout << s << "ParallaxAngleSingleAnchorProjectionFactor, z = ";
-+ measured_.print();
-+ if(this->body_P_sensor_)
-+ this->body_P_sensor_->print(" sensor pose in body frame: ");
-+ Base::print("", keyFormatter);
-+ }
-+
-+ /// equals
-+ virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
-+ const This *e = dynamic_cast<const This*>(&p);
-+ return e
-+ && Base::equals(p, tol)
-+ && this->measured_.equals(e->measured_, tol)
-+ && this->K_->equals(*e->K_, tol)
-+ && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
-+ }
-+
-+ /// Evaluate error h(x)-z and optionally derivatives
-+ Vector evaluateError(const Pose3& mainPose, const ParallaxAnglePoint2& point,
-+ boost::optional<Matrix&> Dmain = boost::none,
-+ boost::optional<Matrix&> Dpoint = boost::none) const {
-+ try {
-+ // Test if we need jacobians
-+ if (!Dmain && !Dpoint)
-+ {
-+ if(body_P_sensor_)
-+ {
-+ // Get the main and associated anchors, and the camera pose
-+ Pose3 mainAnchorPose ( mainPose.compose(*body_P_sensor_) );
-+
-+ // Get the direction to the point from observation point
-+ Point3 obs_T_point( point.directionVector() );
-+
-+ // Put a camera at the origin
-+ PinholeCamera<CALIBRATION> camera(Pose3(mainAnchorPose.rotation(), Point3()), *K_);
-+
-+ // Project direction vector to camera and calculate the error
-+ Point2 reprojectionError(camera.project(obs_T_point) - measured_);
-+ return reprojectionError.vector();
-+ }
-+ else
-+ {
-+ // Get the direction to the point from observation point
-+ Point3 obs_T_point( point.directionVector() );
-+
-+ // Put a camera at the origin
-+ PinholeCamera<CALIBRATION> camera(Pose3(mainPose.rotation(), Point3()), *K_);
-+
-+ // Project direction vector to camera and calculate the error
-+ Point2 reprojectionError(camera.project(obs_T_point) - measured_);
-+ return reprojectionError.vector();
-+ }
-+ }
-+
-+ // Same computation but with jacobians
-+ if(body_P_sensor_)
-+ {
-+ // Get the main and associated anchors, and the camera pose
-+ gtsam::Matrix MAINANCHORPOSE_mainpose;
-+ Pose3 mainAnchorPose ( mainPose.compose(*body_P_sensor_, MAINANCHORPOSE_mainpose) );
-+
-+ // Get the direction to the point from observation point
-+ Matrix OBS_T_POINT_point;
-+ Point3 obs_T_point(point.directionVector( OBS_T_POINT_point ));
-+
-+ // Put a camera at the origin
-+ PinholeCamera<CALIBRATION> camera(Pose3(mainAnchorPose.rotation(), Point3()), *K_);
-+
-+ // Project direction vector to camera and calculate the error
-+ Matrix PROJ_mainanchorori, PROJ_obs_t_point;
-+ Point2 reprojectionError(camera.project(obs_T_point, PROJ_mainanchorori, PROJ_obs_t_point) - measured_);
-+
-+ // Chain of jacobians
-+ if(Dmain)
-+ {
-+ Matrix PROJ_mainanchorpose(2,6);
-+ PROJ_mainanchorpose << PROJ_mainanchorori.block(0,0,2,3), zeros(2,3);
-+ Dmain->resize(2,6);
-+ *Dmain << (PROJ_mainanchorpose * MAINANCHORPOSE_mainpose);
-+ }
-+ if(Dpoint)
-+ {
-+ Dpoint->resize(2,2);
-+ *Dpoint << (PROJ_obs_t_point * OBS_T_POINT_point);
-+ }
-+
-+ return reprojectionError.vector();
-+
-+ }
-+ else
-+ {
-+ // Get the direction to the point from observation point
-+ Matrix OBS_T_POINT_point;
-+ Point3 obs_T_point(point.directionVector( OBS_T_POINT_point ));
-+
-+ // Put a camera at the origin
-+ PinholeCamera<CALIBRATION> camera(Pose3(mainPose.rotation(), Point3()), *K_);
-+
-+ // Project direction vector to camera and calculate the error
-+ Matrix PROJ_mainori, PROJ_obs_t_point;
-+ Point2 reprojectionError(camera.project(obs_T_point, PROJ_mainori, PROJ_obs_t_point) - measured_);
-+
-+ // Chain of jacobians
-+ if(Dmain)
-+ {
-+ Dmain->resize(2,6);
-+ *Dmain << PROJ_mainori.block(0,0,2,3), zeros(2,3);
-+ }
-+ if(Dpoint)
-+ {
-+ Dpoint->resize(2,2);
-+ *Dpoint << (PROJ_obs_t_point * OBS_T_POINT_point);
-+ }
-+
-+ return reprojectionError.vector();
-+ }
-+ } catch( CheiralityException& e)
-+ {
-+ if (Dmain ) *Dmain = zeros(2,6);
-+ if (Dpoint) *Dpoint = zeros(2,2);
-+ if (verboseCheirality_)
-+ std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
-+ " with single anchor (" << DefaultKeyFormatter(this->key1()) << ")" <<
-+ " moved behind camera " << std::endl;
-+ if (throwCheirality_)
-+ throw e;
-+ }
-+ return ones(2) * 2.0 * K_->fx();
-+ }
-+
-+ /** return the measurement */
-+ const Point2& measured() const {
-+ return measured_;
-+ }
-+
-+ /** return the calibration object */
-+ inline const boost::shared_ptr<CALIBRATION> calibration() const {
-+ return K_;
-+ }
-+
-+ /** return verbosity */
-+ inline bool verboseCheirality() const { return verboseCheirality_; }
-+
-+ /** return flag for throwing cheirality exceptions */
-+ inline bool throwCheirality() const { return throwCheirality_; }
-+
-+ private:
-+
-+ /// Serialization function
-+ friend class boost::serialization::access;
-+ template<class ARCHIVE>
-+ void serialize(ARCHIVE & ar, const unsigned int version) {
-+ ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-+ ar & BOOST_SERIALIZATION_NVP(measured_);
-+ ar & BOOST_SERIALIZATION_NVP(K_);
-+ ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
-+ ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
-+ ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
-+ }
-+ };
-+
-+
-+
-+
-+
-+
-+
-+
-+
-+
-+
-+
-+
-+
-+
-+
-+ /** Non-linear factor for a 2D measurement to a Parallax Angle Point with only the anchors.
-+ *
-+ * Non-linear factor for a constraint derived from a 2D measurement bewteen a robot pose and a
-+ * landmark parametrized as 3D Parallax Angle Point (a.k.a ParallaxAnglePoint3). The measurement
-+ * was taken from the associated anchor, and the calibration is known here.
-+ *
-+ * @addtogroup SLAM
-+ */
-+ template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
-+ class ParallaxAngleOnlyAnchorsProjectionFactor: public NoiseModelFactor3<POSE, POSE, LANDMARK> {
-+ protected:
-+
-+ // Keep a copy of measurement and calibration for I/O
-+ Point2 measured_; ///< 2D measurement
-+ boost::shared_ptr<CALIBRATION> K_; ///< shared pointer to calibration object
-+ boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
-+
-+ // verbosity handling for Cheirality Exceptions
-+ bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
-+ bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
-+
-+ public:
-+
-+ /// shorthand for base class type
-+ typedef NoiseModelFactor3<POSE, POSE, LANDMARK> Base;
-+
-+ /// shorthand for this class
-+ typedef ParallaxAngleOnlyAnchorsProjectionFactor<POSE, LANDMARK, CALIBRATION> This;
-+
-+ /// shorthand for a smart pointer to a factor
-+ typedef boost::shared_ptr<This> shared_ptr;
-+
-+ /// Default constructor
-+ ParallaxAngleOnlyAnchorsProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {}
-+
-+ /**
-+ * Constructor
-+ * TODO: Mark argument order standard (keys, measurement, parameters)
-+ * @param measured is the 2 dimensional location of point in image (the measurement)
-+ * @param model is the standard deviation
-+ * @param mainAnchorKey is the index of the main anchor
-+ * @param associatedAnchorKey is the index of the associated anchor
-+ * @param pointKey is the index of the landmark
-+ * @param K shared pointer to the constant calibration
-+ * @param body_P_sensor is the transform from body to sensor frame (default identity)
-+ */
-+ ParallaxAngleOnlyAnchorsProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
-+ Key mainAnchorKey, Key associatedAnchorKey, Key pointKey,
-+ const boost::shared_ptr<CALIBRATION>& K,
-+ boost::optional<POSE> body_P_sensor = boost::none) :
-+ Base(model, mainAnchorKey, associatedAnchorKey, pointKey),
-+ measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
-+ throwCheirality_(false), verboseCheirality_(false) {}
-+
-+ /**
-+ * Constructor with exception-handling flags
-+ * TODO: Mark argument order standard (keys, measurement, parameters)
-+ * @param measured is the 2 dimensional location of point in image (the measurement)
-+ * @param model is the standard deviation
-+ * @param mainAnchorKey is the index of the main anchor
-+ * @param associatedAnchorKey is the index of the associated anchor
-+ * @param pointKey is the index of the landmark
-+ * @param K shared pointer to the constant calibration
-+ * @param throwCheirality determines whether Cheirality exceptions are rethrown
-+ * @param verboseCheirality determines whether exceptions are printed for Cheirality
-+ * @param body_P_sensor is the transform from body to sensor frame (default identity)
-+ */
-+ ParallaxAngleOnlyAnchorsProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
-+ Key mainAnchorKey, Key associatedAnchorKey, Key pointKey,
-+ const boost::shared_ptr<CALIBRATION>& K,
-+ bool throwCheirality, bool verboseCheirality,
-+ boost::optional<POSE> body_P_sensor = boost::none) :
-+ Base(model, mainAnchorKey, associatedAnchorKey, pointKey),
-+ measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
-+ throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
-+
-+ /** Virtual destructor */
-+ virtual ~ParallaxAngleOnlyAnchorsProjectionFactor() {}
-+
-+ /// @return a deep copy of this factor
-+ virtual gtsam::NonlinearFactor::shared_ptr clone() const {
-+ return boost::static_pointer_cast<gtsam::NonlinearFactor>(
-+ gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
-+
-+ /**
-+ * print
-+ * @param s optional string naming the factor
-+ * @param keyFormatter optional formatter useful for printing Symbols
-+ */
-+ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
-+ std::cout << s << "ParallaxAngleOnlyAnchorsProjectionFactor, z = ";
-+ measured_.print();
-+ if(this->body_P_sensor_)
-+ this->body_P_sensor_->print(" sensor pose in body frame: ");
-+ Base::print("", keyFormatter);
-+ }
-+
-+ /// equals
-+ virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
-+ const This *e = dynamic_cast<const This*>(&p);
-+ return e
-+ && Base::equals(p, tol)
-+ && this->measured_.equals(e->measured_, tol)
-+ && this->K_->equals(*e->K_, tol)
-+ && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
-+ }
-+
-+ /// Evaluate error h(x)-z and optionally derivatives
-+ Vector evaluateError(const Pose3& mainPose, const Pose3& assoPose, const ParallaxAnglePoint3& point,
-+ boost::optional<Matrix&> Dmain = boost::none,
-+ boost::optional<Matrix&> Dasso = boost::none,
-+ boost::optional<Matrix&> Dpoint = boost::none) const {
-+ try {
-+ // Test if we need jacobians
-+ if (!Dmain && !Dasso && !Dpoint)
-+ {
-+ if(body_P_sensor_)
-+ {
-+ // Get the main and associated anchors, and the camera pose
-+ Point3 mainAnchor ( mainPose.compose(*body_P_sensor_).translation() );
-+ Pose3 assoAnchorPose ( assoPose.compose(*body_P_sensor_) );
-+
-+ // Get the direction to the point from observation point
-+ Point3 obs_T_point(point.directionVectorFromAssoAnchor( mainAnchor, assoAnchorPose.translation() ));
-+
-+ // Put a camera at the origin
-+ PinholeCamera<CALIBRATION> camera(Pose3(assoAnchorPose.rotation(), Point3()), *K_);
-+
-+ // Project direction vector to camera and calculate the error
-+ Point2 reprojectionError(camera.project(obs_T_point) - measured_);
-+ return reprojectionError.vector();
-+ }
-+ else
-+ {
-+ // Get the direction to the point from observation point
-+ Point3 obs_T_point(point.directionVectorFromAssoAnchor( mainPose.translation(), assoPose.translation() ));
-+
-+ // Put a camera at the origin
-+ PinholeCamera<CALIBRATION> camera(Pose3(assoPose.rotation(), Point3()), *K_);
-+
-+ // Project direction vector to camera and calculate the error
-+ Point2 reprojectionError(camera.project(obs_T_point) - measured_);
-+ return reprojectionError.vector();
-+ }
-+ }
-+
-+ // Same computation but with jacobians
-+ if(body_P_sensor_)
-+ {
-+ // Get the main and associated anchors, and the camera pose
-+ gtsam::Matrix MAINANCHORPOSE_mainpose, ASSOANCHORPOSE_assopose;
-+ Point3 mainAnchor ( mainPose.compose(*body_P_sensor_, MAINANCHORPOSE_mainpose).translation() );
-+ Pose3 assoAnchorPose ( assoPose.compose(*body_P_sensor_, ASSOANCHORPOSE_assopose) );
-+
-+ // Get the direction to the point from observation point
-+ Matrix OBS_T_POINT_point, OBS_T_POINT_mainanchorpos, OBS_T_POINT_assoanchorpos;
-+ Point3 obs_T_point(point.directionVectorFromAssoAnchor( mainAnchor, assoAnchorPose.translation(),
-+ OBS_T_POINT_point,
-+ OBS_T_POINT_mainanchorpos,
-+ OBS_T_POINT_assoanchorpos));
-+
-+ // Put a camera at the origin
-+ PinholeCamera<CALIBRATION> camera(Pose3(assoAnchorPose.rotation(), Point3()), *K_);
-+
-+ // Project direction vector to camera and calculate the error
-+ Matrix PROJ_assoanchorori, PROJ_obs_t_point;
-+ Point2 reprojectionError(camera.project(obs_T_point, PROJ_assoanchorori, PROJ_obs_t_point) - measured_);
-+
-+ // Chain of jacobians
-+ if(Dmain)
-+ {
-+ Matrix PROJ_mainanchorpose = zeros(2,6);
-+ PROJ_mainanchorpose.block(0,3,2,3) << (PROJ_obs_t_point * OBS_T_POINT_mainanchorpos);
-+ Dmain->resize(2,6);
-+ *Dmain << (PROJ_mainanchorpose * MAINANCHORPOSE_mainpose);
-+ }
-+ if(Dasso)
-+ {
-+ Matrix PROJ_assoanchorpose(2,6);
-+ PROJ_assoanchorpose.block(0,0,2,3) << PROJ_assoanchorori.block(0,0,2,3);
-+ PROJ_assoanchorpose.block(0,3,2,3) << (PROJ_obs_t_point * OBS_T_POINT_assoanchorpos);
-+ Dasso->resize(2,6);
-+ *Dasso << (PROJ_assoanchorpose * ASSOANCHORPOSE_assopose);
-+ }
-+ if(Dpoint)
-+ {
-+ Dpoint->resize(2,3);
-+ *Dpoint << (PROJ_obs_t_point * OBS_T_POINT_point);
-+ }
-+
-+ return reprojectionError.vector();
-+
-+ }
-+ else
-+ {
-+ // Get the direction to the point from observation point
-+ Matrix OBS_T_POINT_point, OBS_T_POINT_mainpos, OBS_T_POINT_assopos;
-+ Point3 obs_T_point(point.directionVectorFromAssoAnchor( mainPose.translation(), assoPose.translation(),
-+ OBS_T_POINT_point,
-+ OBS_T_POINT_mainpos,
-+ OBS_T_POINT_assopos));
-+
-+ // Put a camera at the origin
-+ PinholeCamera<CALIBRATION> camera(Pose3(assoPose.rotation(), Point3()), *K_);
-+
-+ // Project direction vector to camera and calculate the error
-+ Matrix PROJ_assoori, PROJ_obs_t_point;
-+ Point2 reprojectionError(camera.project(obs_T_point, PROJ_assoori, PROJ_obs_t_point) - measured_);
-+
-+ // Chain of jacobians
-+ if(Dmain)
-+ {
-+ Dmain->resize(2,6);
-+ *Dmain << zeros(2,3), (PROJ_obs_t_point * OBS_T_POINT_mainpos);
-+ }
-+ if(Dasso)
-+ {
-+ Dasso->resize(2,6);
-+ *Dasso << PROJ_assoori.block(0,0,2,3), (PROJ_obs_t_point * OBS_T_POINT_assopos);
-+ }
-+ if(Dpoint)
-+ {
-+ Dpoint->resize(2,3);
-+ *Dpoint << (PROJ_obs_t_point * OBS_T_POINT_point);
-+ }
-+
-+ return reprojectionError.vector();
-+
-+ }
-+ } catch( CheiralityException& e) {
-+ if (Dmain ) *Dmain = zeros(2,6);
-+ if (Dasso ) *Dasso = zeros(2,6);
-+ if (Dpoint) *Dpoint = zeros(2,3);
-+ if (verboseCheirality_)
-+ std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key3()) <<
-+ " with anchors (" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key2()) <<
-+ " moved behind camera " << DefaultKeyFormatter(this->key2()) << std::endl;
-+ if (throwCheirality_)
-+ throw e;
-+ }
-+ return ones(2) * 2.0 * K_->fx();
-+ }
-+
-+ /** return the measurement */
-+ const Point2& measured() const {
-+ return measured_;
-+ }
-+
-+ /** return the calibration object */
-+ inline const boost::shared_ptr<CALIBRATION> calibration() const {
-+ return K_;
-+ }
-+
-+ /** return verbosity */
-+ inline bool verboseCheirality() const { return verboseCheirality_; }
-+
-+ /** return flag for throwing cheirality exceptions */
-+ inline bool throwCheirality() const { return throwCheirality_; }
-+
-+ private:
-+
-+ /// Serialization function
-+ friend class boost::serialization::access;
-+ template<class ARCHIVE>
-+ void serialize(ARCHIVE & ar, const unsigned int version) {
-+ ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-+ ar & BOOST_SERIALIZATION_NVP(measured_);
-+ ar & BOOST_SERIALIZATION_NVP(K_);
-+ ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
-+ ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
-+ ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
-+ }
-+ };
-+
-+
-+
-+
-+
-+
-+
-+
-+
-+
-+
-+
-+
-+
-+
-+ /** Non-linear factor for a 2D measurement to a Parallax Angle Point.
-+ *
-+ * Non-linear factor for a constraint derived from a 2D measurement bewteen a robot pose and a
-+ * landmark parametrized as Parallax Angle Point (a.k.a ParallaxAnglePoint3). The measurement
-+ * comes from from a pose different from the anchors (known as 'other'), and the calibration
-+ * is known here.
-+ *
-+ * @addtogroup SLAM
-+ */
-+ template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
-+ class ParallaxAngleProjectionFactor: public NoiseModelFactor4<POSE, POSE, POSE, LANDMARK> {
-+ protected:
-+
-+ // Keep a copy of measurement and calibration for I/O
-+ Point2 measured_; ///< 2D measurement
-+ boost::shared_ptr<CALIBRATION> K_; ///< shared pointer to calibration object
-+ boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
-+
-+ // verbosity handling for Cheirality Exceptions
-+ bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
-+ bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
-+
-+ public:
-+
-+ /// shorthand for base class type
-+ typedef NoiseModelFactor4<POSE, POSE, POSE, LANDMARK> Base;
-+
-+ /// shorthand for this class
-+ typedef ParallaxAngleProjectionFactor<POSE, LANDMARK, CALIBRATION> This;
-+
-+ /// shorthand for a smart pointer to a factor
-+ typedef boost::shared_ptr<This> shared_ptr;
-+
-+ /// Default constructor
-+ ParallaxAngleProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {}
-+
-+ /**
-+ * Constructor
-+ * TODO: Mark argument order standard (keys, measurement, parameters)
-+ * @param measured is the 2 dimensional location of point in image (the measurement)
-+ * @param model is the standard deviation
-+ * @param mainAnchorKey is the index of the main anchor
-+ * @param associatedAnchorKey is the index of the associated anchor
-+ * @param otherAnchorKey is the index of the other anchor
-+ * @param pointKey is the index of the landmark
-+ * @param K shared pointer to the constant calibration
-+ * @param body_P_sensor is the transform from body to sensor frame (default identity)
-+ */
-+ ParallaxAngleProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
-+ Key mainAnchorKey, Key associatedAnchorKey, Key otherAnchorKey, Key pointKey,
-+ const boost::shared_ptr<CALIBRATION>& K,
-+ boost::optional<POSE> body_P_sensor = boost::none) :
-+ Base(model, mainAnchorKey, associatedAnchorKey, otherAnchorKey, pointKey),
-+ measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
-+ throwCheirality_(false), verboseCheirality_(false) {}
-+
-+ /**
-+ * Constructor with exception-handling flags
-+ * TODO: Mark argument order standard (keys, measurement, parameters)
-+ * @param measured is the 2 dimensional location of point in image (the measurement)
-+ * @param model is the standard deviation
-+ * @param mainAnchorKey is the index of the main anchor
-+ * @param associatedAnchorKey is the index of the associated anchor
-+ * @param otherAnchorKey is the index of the other anchor
-+ * @param pointKey is the index of the landmark
-+ * @param K shared pointer to the constant calibration
-+ * @param throwCheirality determines whether Cheirality exceptions are rethrown
-+ * @param verboseCheirality determines whether exceptions are printed for Cheirality
-+ * @param body_P_sensor is the transform from body to sensor frame (default identity)
-+ */
-+ ParallaxAngleProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
-+ Key mainAnchorKey, Key associatedAnchorKey, Key otherAnchorKey, Key pointKey,
-+ const boost::shared_ptr<CALIBRATION>& K,
-+ bool throwCheirality, bool verboseCheirality,
-+ boost::optional<POSE> body_P_sensor = boost::none) :
-+ Base(model, mainAnchorKey, associatedAnchorKey, otherAnchorKey, pointKey),
-+ measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
-+ throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
-+
-+ /** Virtual destructor */
-+ virtual ~ParallaxAngleProjectionFactor() {}
-+
-+ /// @return a deep copy of this factor
-+ virtual gtsam::NonlinearFactor::shared_ptr clone() const {
-+ return boost::static_pointer_cast<gtsam::NonlinearFactor>(
-+ gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
-+
-+ /**
-+ * print
-+ * @param s optional string naming the factor
-+ * @param keyFormatter optional formatter useful for printing Symbols
-+ */
-+ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
-+ std::cout << s << "ParallaxAngleProjectionFactor, z = ";
-+ measured_.print();
-+ if(this->body_P_sensor_)
-+ this->body_P_sensor_->print(" sensor pose in body frame: ");
-+ Base::print("", keyFormatter);
-+ }
-+
-+ /// equals
-+ virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
-+ const This *e = dynamic_cast<const This*>(&p);
-+ return e
-+ && Base::equals(p, tol)
-+ && this->measured_.equals(e->measured_, tol)
-+ && this->K_->equals(*e->K_, tol)
-+ && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
-+ }
-+
-+ /// Evaluate error h(x)-z and optionally derivatives
-+ Vector evaluateError(const Pose3& mainPose, const Pose3& assoPose, const Pose3& othePose, const ParallaxAnglePoint3& point,
-+ boost::optional<Matrix&> Dmain = boost::none,
-+ boost::optional<Matrix&> Dasso = boost::none,
-+ boost::optional<Matrix&> Dothe = boost::none,
-+ boost::optional<Matrix&> Dpoint = boost::none) const {
-+ try {
-+ // Test if we need jacobians
-+ if (!Dmain && !Dasso && !Dothe && !Dpoint)
-+ {
-+ if(body_P_sensor_)
-+ {
-+ // Get the main and associated anchors, and the camera pose
-+ Point3 mainAnchor( mainPose.compose(*body_P_sensor_).translation() );
-+ Point3 assoAnchor( assoPose.compose(*body_P_sensor_).translation() );
-+ Pose3 camPose ( othePose.compose(*body_P_sensor_) );
-+
-+ // Get the direction to the point from observation point
-+ Point3 obs_T_point(point.directionVectorFromOtheAnchor( mainAnchor, assoAnchor, camPose.translation()));
-+
-+ // Put a camera at the origin
-+ PinholeCamera<CALIBRATION> camera(Pose3(camPose.rotation(), Point3()), *K_);
-+
-+ // Project direction vector to camera and calculate the error
-+ Point2 reprojectionError(camera.project(obs_T_point) - measured_);
-+ return reprojectionError.vector();
-+ }
-+ else
-+ {
-+ // Get the direction to the point from observation point
-+ Point3 obs_T_point(point.directionVectorFromOtheAnchor( mainPose.translation(), assoPose.translation(), othePose.translation()));
-+
-+ // Put a camera at the origin
-+ PinholeCamera<CALIBRATION> camera(Pose3(othePose.rotation(), Point3()), *K_);
-+
-+ // Project direction vector to camera and calculate the error
-+ Point2 reprojectionError(camera.project(obs_T_point) - measured_);
-+ return reprojectionError.vector();
-+ }
-+ }
-+
-+ // Same computation but with jacobians
-+ if(body_P_sensor_)
-+ {
-+ // Get the main and associated anchors, and the camera pose
-+ gtsam::Matrix MAINANCHORPOSE_mainpose, ASSOANCHORPOSE_assopose, CAMPOSE_othepose;
-+ Point3 mainAnchor( mainPose.compose(*body_P_sensor_, MAINANCHORPOSE_mainpose).translation() );
-+ Point3 assoAnchor( assoPose.compose(*body_P_sensor_, ASSOANCHORPOSE_assopose).translation() );
-+ Pose3 camPose ( othePose.compose(*body_P_sensor_, CAMPOSE_othepose ) );
-+
-+ // Get the direction to the point from observation point
-+ Matrix OBS_T_POINT_point, OBS_T_POINT_mainanchorpos, OBS_T_POINT_assoanchorpos, OBS_T_POINT_campos;
-+ Point3 obs_T_point(point.directionVectorFromOtheAnchor( mainAnchor, assoAnchor, camPose.translation(),
-+ OBS_T_POINT_point,
-+ OBS_T_POINT_mainanchorpos,
-+ OBS_T_POINT_assoanchorpos,
-+ OBS_T_POINT_campos));
-+
-+ // Put a camera at the origin
-+ PinholeCamera<CALIBRATION> camera(Pose3(camPose.rotation(), Point3()), *K_);
-+
-+ // Project direction vector to camera and calculate the error
-+ Matrix PROJ_camori, PROJ_obs_t_point;
-+ Point2 reprojectionError(camera.project(obs_T_point, PROJ_camori, PROJ_obs_t_point) - measured_);
-+
-+ // Chain of jacobians
-+ if(Dmain)
-+ {
-+ Matrix PROJ_mainanchorpose = zeros(2,6);
-+ PROJ_mainanchorpose.block(0,3,2,3) << (PROJ_obs_t_point * OBS_T_POINT_mainanchorpos);
-+ Dmain->resize(2,6);
-+ *Dmain << (PROJ_mainanchorpose * MAINANCHORPOSE_mainpose);
-+ }
-+ if(Dasso)
-+ {
-+ Matrix PROJ_assoanchorpose = zeros(2,6);
-+ PROJ_assoanchorpose.block(0,3,2,3) << (PROJ_obs_t_point * OBS_T_POINT_assoanchorpos);
-+ Dasso->resize(2,6);
-+ *Dasso << (PROJ_assoanchorpose * ASSOANCHORPOSE_assopose);
-+ }
-+ if(Dothe)
-+ {
-+ Matrix PROJ_campose(2,6);
-+ PROJ_campose << PROJ_camori.block(0,0,2,3), (PROJ_obs_t_point * OBS_T_POINT_campos);
-+ Dothe->resize(2,6);
-+ *Dothe << (PROJ_campose * CAMPOSE_othepose);
-+ }
-+ if(Dpoint)
-+ {
-+ Dpoint->resize(2,3);
-+ *Dpoint << (PROJ_obs_t_point * OBS_T_POINT_point);
-+ }
-+
-+ return reprojectionError.vector();
-+
-+ }
-+ else
-+ {
-+ // Get the direction to the point from observation point
-+ Matrix OBS_T_POINT_point, OBS_T_POINT_mainanchorpos, OBS_T_POINT_assoanchorpos, OBS_T_POINT_campos;
-+ Point3 obs_T_point(point.directionVectorFromOtheAnchor(
-+ mainPose.translation(), assoPose.translation(), othePose.translation(),
-+ OBS_T_POINT_point,
-+ OBS_T_POINT_mainanchorpos,
-+ OBS_T_POINT_assoanchorpos,
-+ OBS_T_POINT_campos));
-+
-+ // Put a camera at the origin
-+ PinholeCamera<CALIBRATION> camera(Pose3(othePose.rotation(), Point3()), *K_);
-+
-+ // Project direction vector to camera and calculate the error
-+ Matrix PROJ_camori, PROJ_obs_t_point;
-+ Point2 reprojectionError(camera.project(obs_T_point, PROJ_camori, PROJ_obs_t_point) - measured_);
-+
-+ // Chain of jacobians
-+ if(Dmain)
-+ {
-+ Dmain->resize(2,6);
-+ *Dmain << zeros(2,3), (PROJ_obs_t_point * OBS_T_POINT_mainanchorpos);
-+ }
-+ if(Dasso)
-+ {
-+ Dasso->resize(2,6);
-+ *Dasso << zeros(2,3), (PROJ_obs_t_point * OBS_T_POINT_assoanchorpos);
-+ }
-+ if(Dothe)
-+ {
-+ Dothe->resize(2,6);
-+ *Dothe << PROJ_camori.block(0,0,2,3), (PROJ_obs_t_point * OBS_T_POINT_campos);
-+ }
-+ if(Dpoint)
-+ {
-+ Dpoint->resize(2,3);
-+ *Dpoint << (PROJ_obs_t_point * OBS_T_POINT_point);
-+ }
-+
-+ return reprojectionError.vector();
-+
-+ }
-+ }catch( CheiralityException& e) {
-+ if (Dmain ) *Dmain = zeros(2,6);
-+ if (Dasso ) *Dasso = zeros(2,6);
-+ if (Dothe ) *Dothe = zeros(2,6);
-+ if (Dpoint) *Dpoint = zeros(2,3);
-+ if (verboseCheirality_)
-+ std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key4()) <<
-+ " with anchors (" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key2()) <<
-+ " moved behind camera " << DefaultKeyFormatter(this->key3()) << std::endl;
-+ if (throwCheirality_)
-+ throw e;
-+ }
-+ return ones(2) * 2.0 * K_->fx();
-+ }
-+
-+ /** return the measurement */
-+ const Point2& measured() const {
-+ return measured_;
-+ }
-+
-+ /** return the calibration object */
-+ inline const boost::shared_ptr<CALIBRATION> calibration() const {
-+ return K_;
-+ }
-+
-+ /** return verbosity */
-+ inline bool verboseCheirality() const { return verboseCheirality_; }
-+
-+ /** return flag for throwing cheirality exceptions */
-+ inline bool throwCheirality() const { return throwCheirality_; }
-+
-+ private:
-+
-+ /// Serialization function
-+ friend class boost::serialization::access;
-+ template<class ARCHIVE>
-+ void serialize(ARCHIVE & ar, const unsigned int version) {
-+ ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
-+ ar & BOOST_SERIALIZATION_NVP(measured_);
-+ ar & BOOST_SERIALIZATION_NVP(K_);
-+ ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
-+ ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
-+ ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
-+ }
-+ };
-+
-+} // \ namespace gtsam
---- gtsam.h 2014-06-01 21:22:39.000000000 +0200
-+++ gtsam.h 2015-05-05 22:25:48.977930910 +0200
-@@ -837,6 +837,66 @@
- void serialize() const;
- };
-
-+#include <gtsam/geometry/ParallaxAnglePoint2.h>
-+virtual class ParallaxAnglePoint2 : gtsam::Value {
-+ // Standard Constructors
-+ ParallaxAnglePoint2();
-+ ParallaxAnglePoint2(double yaw, double pitch);
-+ ParallaxAnglePoint2(Vector v);
-+
-+ // Testable
-+ void print(string s) const;
-+ bool equals(const gtsam::ParallaxAnglePoint2& pose, double tol) const;
-+
-+ // Manifold
-+ static size_t Dim();
-+ size_t dim() const;
-+ gtsam::ParallaxAnglePoint2 retract(Vector v) const;
-+ Vector localCoordinates(const gtsam::ParallaxAnglePoint2& p) const;
-+
-+ // Standard Interface
-+ double yaw() const;
-+ double pitch() const;
-+ Vector vector() const;
-+
-+ Vector directionVector() const;
-+
-+ // enabling serialization functionality
-+ void serialize() const;
-+};
-+
-+#include <gtsam/geometry/ParallaxAnglePoint3.h>
-+virtual class ParallaxAnglePoint3 : gtsam::Value {
-+ // Standard Constructors
-+ ParallaxAnglePoint3();
-+ ParallaxAnglePoint3(double yaw, double pitch, double parallax);
-+ ParallaxAnglePoint3(Vector v);
-+
-+ // Testable
-+ void print(string s) const;
-+ bool equals(const gtsam::ParallaxAnglePoint3& pose, double tol) const;
-+
-+ // Manifold
-+ static size_t Dim();
-+ size_t dim() const;
-+ gtsam::ParallaxAnglePoint3 retract(Vector v) const;
-+ Vector localCoordinates(const gtsam::ParallaxAnglePoint3& p) const;
-+
-+ // Standard Interface
-+ double yaw() const;
-+ double pitch() const;
-+ double parallax() const;
-+ Vector vector() const;
-+
-+ Vector directionVectorFromMainAnchor() const;
-+ Vector directionVectorFromAssoAnchor(const gtsam::Point3& mainAnchor, const gtsam::Point3& assoAnchor) const;
-+ Vector directionVectorFromOtheAnchor(const gtsam::Point3& mainAnchor, const gtsam::Point3& assoAnchor, const gtsam::Point3& otheAnchor) const;
-+
-+ // enabling serialization functionality
-+ void serialize() const;
-+};
-+
-+
- //*************************************************************************
- // Symbolic
- //*************************************************************************
-@@ -2180,6 +2240,81 @@
- typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> GenericProjectionFactorCal3_S2;
- typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> GenericProjectionFactorCal3DS2;
-
-+#include <gtsam/slam/ParallaxAngleProjectionFactor.h>
-+template<POSE, LANDMARK, CALIBRATION>
-+virtual class ParallaxAngleSingleAnchorProjectionFactor : gtsam::NoiseModelFactor {
-+ ParallaxAngleSingleAnchorProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
-+ size_t mainAnchorKey, size_t pointKey, const CALIBRATION* k);
-+ ParallaxAngleSingleAnchorProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
-+ size_t mainAnchorKey, size_t pointKey, const CALIBRATION* k, const POSE& body_P_sensor);
-+
-+ ParallaxAngleSingleAnchorProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
-+ size_t mainAnchorKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality);
-+ ParallaxAngleSingleAnchorProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
-+ size_t mainAnchorKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality,
-+ const POSE& body_P_sensor);
-+
-+ gtsam::Point2 measured() const;
-+ CALIBRATION* calibration() const;
-+ bool verboseCheirality() const;
-+ bool throwCheirality() const;
-+
-+ // enabling serialization functionality
-+ void serialize() const;
-+};
-+typedef gtsam::ParallaxAngleSingleAnchorProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint2, gtsam::Cal3_S2> PASingleAnchorProjectionFactorCal3_S2;
-+typedef gtsam::ParallaxAngleSingleAnchorProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint2, gtsam::Cal3DS2> PASingleAnchorProjectionFactorCal3DS2;
-+
-+
-+template<POSE, LANDMARK, CALIBRATION>
-+virtual class ParallaxAngleOnlyAnchorsProjectionFactor : gtsam::NoiseModelFactor {
-+ ParallaxAngleOnlyAnchorsProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
-+ size_t mainAnchorKey, size_t associatedAnchorKey, size_t pointKey, const CALIBRATION* k);
-+ ParallaxAngleOnlyAnchorsProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
-+ size_t mainAnchorKey, size_t associatedAnchorKey, size_t pointKey, const CALIBRATION* k, const POSE& body_P_sensor);
-+
-+ ParallaxAngleOnlyAnchorsProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
-+ size_t mainAnchorKey, size_t associatedAnchorKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality);
-+ ParallaxAngleOnlyAnchorsProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
-+ size_t mainAnchorKey, size_t associatedAnchorKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality,
-+ const POSE& body_P_sensor);
-+
-+ gtsam::Point2 measured() const;
-+ CALIBRATION* calibration() const;
-+ bool verboseCheirality() const;
-+ bool throwCheirality() const;
-+
-+ // enabling serialization functionality
-+ void serialize() const;
-+};
-+typedef gtsam::ParallaxAngleOnlyAnchorsProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3_S2> PAOnlyAnchorsProjectionFactorCal3_S2;
-+typedef gtsam::ParallaxAngleOnlyAnchorsProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3DS2> PAOnlyAnchorsProjectionFactorCal3DS2;
-+
-+
-+template<POSE, LANDMARK, CALIBRATION>
-+virtual class ParallaxAngleProjectionFactor : gtsam::NoiseModelFactor {
-+ ParallaxAngleProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
-+ size_t mainAnchorKey, size_t associatedAnchorKey, size_t otherAnchorKey, size_t pointKey, const CALIBRATION* k);
-+ ParallaxAngleProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
-+ size_t mainAnchorKey, size_t associatedAnchorKey, size_t otherAnchorKey, size_t pointKey, const CALIBRATION* k, const POSE& body_P_sensor);
-+
-+ ParallaxAngleProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
-+ size_t mainAnchorKey, size_t associatedAnchorKey, size_t otherAnchorKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality);
-+ ParallaxAngleProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
-+ size_t mainAnchorKey, size_t associatedAnchorKey, size_t otherAnchorKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality,
-+ const POSE& body_P_sensor);
-+
-+ gtsam::Point2 measured() const;
-+ CALIBRATION* calibration() const;
-+ bool verboseCheirality() const;
-+ bool throwCheirality() const;
-+
-+ // enabling serialization functionality
-+ void serialize() const;
-+};
-+typedef gtsam::ParallaxAngleProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3_S2> PAProjectionFactorCal3_S2;
-+typedef gtsam::ParallaxAngleProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3DS2> PAProjectionFactorCal3DS2;
-+
-
- #include <gtsam/slam/GeneralSFMFactor.h>
- template<CAMERA, LANDMARK>
diff --git a/gtsam/patches/patch-ad b/gtsam/patches/patch-ad
deleted file mode 100644
index 52867bb19815caf6fd7f27cecca86d244dfe523a..0000000000000000000000000000000000000000
--- a/gtsam/patches/patch-ad
+++ /dev/null
@@ -1,161 +0,0 @@
-Add error function from SingleAnchorProjectionFactor to ParallaxAnglePoint3
---- gtsam.h 2015-05-07 14:21:16.768171500 +0200
-+++ gtsam.h 2015-05-07 14:20:30.876077548 +0200
-@@ -2262,8 +2262,10 @@
- // enabling serialization functionality
- void serialize() const;
- };
--typedef gtsam::ParallaxAngleSingleAnchorProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint2, gtsam::Cal3_S2> PASingleAnchorProjectionFactorCal3_S2;
--typedef gtsam::ParallaxAngleSingleAnchorProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint2, gtsam::Cal3DS2> PASingleAnchorProjectionFactorCal3DS2;
-+typedef gtsam::ParallaxAngleSingleAnchorProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint2, gtsam::Cal3_S2> PAPoint2SingleAnchorProjectionFactorCal3_S2;
-+typedef gtsam::ParallaxAngleSingleAnchorProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint2, gtsam::Cal3DS2> PAPoint2SingleAnchorProjectionFactorCal3DS2;
-+typedef gtsam::ParallaxAngleSingleAnchorProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3_S2> PAPoint3SingleAnchorProjectionFactorCal3_S2;
-+typedef gtsam::ParallaxAngleSingleAnchorProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3DS2> PAPoint3SingleAnchorProjectionFactorCal3DS2;
-
-
- template<POSE, LANDMARK, CALIBRATION>
-@@ -2289,8 +2289,8 @@
- // enabling serialization functionality
- void serialize() const;
- };
--typedef gtsam::ParallaxAngleOnlyAnchorsProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3_S2> PAOnlyAnchorsProjectionFactorCal3_S2;
--typedef gtsam::ParallaxAngleOnlyAnchorsProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3DS2> PAOnlyAnchorsProjectionFactorCal3DS2;
-+typedef gtsam::ParallaxAngleOnlyAnchorsProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3_S2> PAPoint3OnlyAnchorsProjectionFactorCal3_S2;
-+typedef gtsam::ParallaxAngleOnlyAnchorsProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3DS2> PAPoint3OnlyAnchorsProjectionFactorCal3DS2;
-
-
- template<POSE, LANDMARK, CALIBRATION>
-@@ -2314,8 +2314,8 @@
- // enabling serialization functionality
- void serialize() const;
- };
--typedef gtsam::ParallaxAngleProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3_S2> PAProjectionFactorCal3_S2;
--typedef gtsam::ParallaxAngleProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3DS2> PAProjectionFactorCal3DS2;
-+typedef gtsam::ParallaxAngleProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3_S2> PAPoint3ProjectionFactorCal3_S2;
-+typedef gtsam::ParallaxAngleProjectionFactor<gtsam::Pose3, gtsam::ParallaxAnglePoint3, gtsam::Cal3DS2> PAPoint3ProjectionFactorCal3DS2;
-
-
- #include <gtsam/slam/GeneralSFMFactor.h>
---- gtsam/slam/ParallaxAngleProjectionFactor.h 2015-05-07 14:21:16.768171500 +0200
-+++ gtsam/slam/ParallaxAngleProjectionFactor.h 2015-05-07 14:11:25.325210470 +0200
-@@ -244,6 +244,120 @@
- return ones(2) * 2.0 * K_->fx();
- }
-
-+ /// Evaluate error h(x)-z and optionally derivatives
-+ Vector evaluateError(const Pose3& mainPose, const ParallaxAnglePoint3& point,
-+ boost::optional<Matrix&> Dmain = boost::none,
-+ boost::optional<Matrix&> Dpoint = boost::none) const {
-+ try {
-+ // Test if we need jacobians
-+ if (!Dmain && !Dpoint)
-+ {
-+ if(body_P_sensor_)
-+ {
-+ // Get the main and associated anchors, and the camera pose
-+ Pose3 mainAnchorPose ( mainPose.compose(*body_P_sensor_) );
-+
-+ // Get the direction to the point from observation point
-+ Point3 obs_T_point( point.directionVectorFromMainAnchor() );
-+
-+ // Put a camera at the origin
-+ PinholeCamera<CALIBRATION> camera(Pose3(mainAnchorPose.rotation(), Point3()), *K_);
-+
-+ // Project direction vector to camera and calculate the error
-+ Point2 reprojectionError(camera.project(obs_T_point) - measured_);
-+ return reprojectionError.vector();
-+ }
-+ else
-+ {
-+ // Get the direction to the point from observation point
-+ Point3 obs_T_point( point.directionVectorFromMainAnchor() );
-+
-+ // Put a camera at the origin
-+ PinholeCamera<CALIBRATION> camera(Pose3(mainPose.rotation(), Point3()), *K_);
-+
-+ // Project direction vector to camera and calculate the error
-+ Point2 reprojectionError(camera.project(obs_T_point) - measured_);
-+ return reprojectionError.vector();
-+ }
-+ }
-+
-+ // Same computation but with jacobians
-+ if(body_P_sensor_)
-+ {
-+ // Get the main and associated anchors, and the camera pose
-+ gtsam::Matrix MAINANCHORPOSE_mainpose;
-+ Pose3 mainAnchorPose ( mainPose.compose(*body_P_sensor_, MAINANCHORPOSE_mainpose) );
-+
-+ // Get the direction to the point from observation point
-+ Matrix OBS_T_POINT_point;
-+ Point3 obs_T_point(point.directionVectorFromMainAnchor( OBS_T_POINT_point ));
-+
-+ // Put a camera at the origin
-+ PinholeCamera<CALIBRATION> camera(Pose3(mainAnchorPose.rotation(), Point3()), *K_);
-+
-+ // Project direction vector to camera and calculate the error
-+ Matrix PROJ_mainanchorori, PROJ_obs_t_point;
-+ Point2 reprojectionError(camera.project(obs_T_point, PROJ_mainanchorori, PROJ_obs_t_point) - measured_);
-+
-+ // Chain of jacobians
-+ if(Dmain)
-+ {
-+ Matrix PROJ_mainanchorpose(2,6);
-+ PROJ_mainanchorpose << PROJ_mainanchorori.block(0,0,2,3), zeros(2,3);
-+ Dmain->resize(2,6);
-+ *Dmain << (PROJ_mainanchorpose * MAINANCHORPOSE_mainpose);
-+ }
-+ if(Dpoint)
-+ {
-+ Dpoint->resize(2,3);
-+ *Dpoint << (PROJ_obs_t_point * OBS_T_POINT_point);
-+ }
-+
-+ return reprojectionError.vector();
-+
-+ }
-+ else
-+ {
-+ // Get the direction to the point from observation point
-+ Matrix OBS_T_POINT_point;
-+ Point3 obs_T_point(point.directionVectorFromMainAnchor( OBS_T_POINT_point ));
-+
-+ // Put a camera at the origin
-+ PinholeCamera<CALIBRATION> camera(Pose3(mainPose.rotation(), Point3()), *K_);
-+
-+ // Project direction vector to camera and calculate the error
-+ Matrix PROJ_mainori, PROJ_obs_t_point;
-+ Point2 reprojectionError(camera.project(obs_T_point, PROJ_mainori, PROJ_obs_t_point) - measured_);
-+
-+ // Chain of jacobians
-+ if(Dmain)
-+ {
-+ Dmain->resize(2,6);
-+ *Dmain << PROJ_mainori.block(0,0,2,3), zeros(2,3);
-+ }
-+ if(Dpoint)
-+ {
-+ Dpoint->resize(2,3);
-+ *Dpoint << (PROJ_obs_t_point * OBS_T_POINT_point);
-+ }
-+
-+ return reprojectionError.vector();
-+ }
-+ } catch( CheiralityException& e)
-+ {
-+ if (Dmain ) *Dmain = zeros(2,6);
-+ if (Dpoint) *Dpoint = zeros(2,3);
-+ if (verboseCheirality_)
-+ std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
-+ " with single anchor (" << DefaultKeyFormatter(this->key1()) << ")" <<
-+ " moved behind camera " << std::endl;
-+ if (throwCheirality_)
-+ throw e;
-+ }
-+ return ones(2) * 2.0 * K_->fx();
-+ }
-+
-+
- /** return the measurement */
- const Point2& measured() const {
- return measured_;
diff --git a/gtsam/patches/patch-ae b/gtsam/patches/patch-ae
deleted file mode 100644
index 30d4dbd4c86872a30764e67403872ecd051444ce..0000000000000000000000000000000000000000
--- a/gtsam/patches/patch-ae
+++ /dev/null
@@ -1,195 +0,0 @@
-Add static constructors to Parallax Angle Points
-
---- gtsam/geometry/ParallaxAnglePoint2.h 2015-05-11 14:28:07.113933768 +0200
-+++ gtsam/geometry/ParallaxAnglePoint2.h 2015-05-08 19:08:41.697786177 +0200
-@@ -22,6 +22,10 @@
- #include <gtsam/base/Matrix.h>
- #include <gtsam/base/DerivedValue.h>
- #include <gtsam/base/Lie.h>
-+#include <gtsam/geometry/Point2.h>
-+#include <gtsam/geometry/Point3.h>
-+#include <gtsam/geometry/Pose3.h>
-+#include <gtsam/geometry/PinholeCamera.h>
-
- #include <boost/serialization/nvp.hpp>
-
-@@ -66,6 +70,39 @@
- pitch_ = v(1);
- }
-
-+ /// Construct from camera and measurement
-+ template<class PinholeCameraType>
-+ static ParallaxAnglePoint2 FromCameraAndMeasurement(const PinholeCameraType &camera, const Point2 &measurement)
-+ {
-+ Point3 vecFromMain = camera.backproject(measurement, 1.0);
-+ double yaw = atan2(vecFromMain.y(),vecFromMain.x());
-+ double pitch = atan2(vecFromMain.z(),Point2(vecFromMain.x(),vecFromMain.y()).norm());
-+
-+ return ParallaxAnglePoint2(yaw,pitch);
-+ }
-+
-+ /// Construct from pose, body to sensor pose and camera calibration
-+ template<class CalibrationType>
-+ static ParallaxAnglePoint2 FromPoseMeasurementAndCalibration(const Pose3 &pose, const Point2 &measurement, const CalibrationType& K, boost::optional<Pose3> body_P_sensor = boost::none)
-+ {
-+ boost::shared_ptr<PinholeCamera<CalibrationType> > camera_ptr;
-+
-+ if(body_P_sensor)
-+ {
-+ camera_ptr = boost::make_shared<PinholeCamera<CalibrationType> >(pose.compose(*body_P_sensor), K);
-+ }
-+ else
-+ {
-+ camera_ptr = boost::make_shared<PinholeCamera<CalibrationType> >(pose, K);
-+ }
-+
-+ Point3 vecFromMain = camera_ptr->backproject(measurement, 1.0);
-+ double yaw = atan2(vecFromMain.y(),vecFromMain.x());
-+ double pitch = atan2(vecFromMain.z(),Point2(vecFromMain.x(),vecFromMain.y()).norm());
-+
-+ return ParallaxAnglePoint2(yaw,pitch);
-+ }
-+
- /// @}
- /// @name Testable
- /// @{
---- gtsam/geometry/ParallaxAnglePoint3.cpp 2015-05-11 14:28:07.113933768 +0200
-+++ gtsam/geometry/ParallaxAnglePoint3.cpp 2015-05-08 18:40:09.733203490 +0200
-@@ -29,6 +29,22 @@
-
- namespace gtsam {
- /* ************************************************************************* */
-+ParallaxAnglePoint3 ParallaxAnglePoint3::FromParallaxAnglePointAndAnchors(
-+ const ParallaxAnglePoint3 &oldPoint,
-+ const Point3 &oldMainAnchor, const Point3 &oldAssoAnchor,
-+ const Point3 &newMainAnchor, const Point3 &newAssoAnchor)
-+{
-+ Vector3 vecFromNewMain(oldPoint.directionVectorFromOtheAnchor(oldMainAnchor, oldAssoAnchor, newMainAnchor));
-+ Vector3 vecFromNewAsso(oldPoint.directionVectorFromOtheAnchor(oldMainAnchor, oldAssoAnchor, newAssoAnchor));
-+
-+ double yaw = atan2(vecFromNewMain.y(),vecFromNewMain.x());
-+ double pitch = atan2(vecFromNewMain.z(),Point2(vecFromNewMain.x(),vecFromNewMain.y()).norm());
-+ double parallax = acos(vecFromNewMain.dot(vecFromNewAsso)/(vecFromNewMain.norm()*vecFromNewAsso.norm()));
-+
-+ return ParallaxAnglePoint3(yaw,pitch,parallax);
-+}
-+
-+/* ************************************************************************* */
- void ParallaxAnglePoint3::print(const string& s) const {
- cout << s << *this << endl;
- }
---- gtsam/geometry/ParallaxAnglePoint3.h 2015-05-11 14:28:07.113933768 +0200
-+++ gtsam/geometry/ParallaxAnglePoint3.h 2015-05-08 18:38:00.960126712 +0200
-@@ -24,6 +24,9 @@
- #include <gtsam/base/Lie.h>
-
- #include <gtsam/geometry/Point3.h>
-+#include <gtsam/geometry/Point3.h>
-+#include <gtsam/geometry/Pose3.h>
-+#include <gtsam/geometry/PinholeCamera.h>
-
- #include <boost/serialization/nvp.hpp>
-
-@@ -70,6 +73,57 @@
- parallax_ = v(2);
- }
-
-+ /// Construct from main and associated cameras and respective measurements
-+ template<class PinholeCameraType>
-+ static ParallaxAnglePoint3 FromCamerasAndMeasurements(
-+ const PinholeCameraType &mainCamera, const Point2 &measurementFromMain,
-+ const PinholeCameraType &assoCamera, const Point2 &measurementFromAsso)
-+ {
-+ Point3 pointFromMain = mainCamera.backproject(measurementFromMain, 1.0);
-+ Point3 pointFromAsso = assoCamera.backproject(measurementFromAsso, 1.0);
-+
-+ double yaw = atan2(pointFromMain.y(),pointFromMain.x());
-+ double pitch = atan2(pointFromMain.z(),Point2(pointFromMain.x(),pointFromMain.y()).norm());
-+ double parallax = acos(pointFromMain.dot(pointFromAsso)/(pointFromMain.norm()*pointFromAsso.norm()));
-+
-+ return ParallaxAnglePoint3(yaw,pitch,parallax);
-+ }
-+
-+ /// Construct from main and associated poses, together with respective measurements, camera calibration and body to sensor pose
-+ template<class CalibrationType>
-+ static ParallaxAnglePoint3 FromPosesMeasurementsAndCalibration(
-+ const Pose3 &mainPose, const Point2 &measurementFromMain,
-+ const Pose3 &assoPose, const Point2 &measurementFromAsso,
-+ const CalibrationType& K, boost::optional<Pose3> body_P_sensor = boost::none)
-+ {
-+ boost::shared_ptr<PinholeCamera<CalibrationType> > mainCamera_ptr;
-+ boost::shared_ptr<PinholeCamera<CalibrationType> > assoCamera_ptr;
-+
-+ if(body_P_sensor)
-+ {
-+ mainCamera_ptr = boost::make_shared<PinholeCamera<CalibrationType> >(mainPose.compose(*body_P_sensor), K);
-+ assoCamera_ptr = boost::make_shared<PinholeCamera<CalibrationType> >(assoPose.compose(*body_P_sensor), K);
-+ }
-+ else
-+ {
-+ mainCamera_ptr = boost::make_shared<PinholeCamera<CalibrationType> >(mainPose, K);
-+ assoCamera_ptr = boost::make_shared<PinholeCamera<CalibrationType> >(assoPose, K);
-+ }
-+
-+ Point3 vecFromMain = mainCamera_ptr->backproject(measurementFromMain, 1.0);
-+ Point3 vecFromAsso = assoCamera_ptr->backproject(measurementFromAsso, 1.0);
-+
-+ double yaw = atan2(vecFromMain.y(),vecFromMain.x());
-+ double pitch = atan2(vecFromMain.z(),Point2(vecFromMain.x(),vecFromMain.y()).norm());
-+ double parallax = acos(vecFromMain.dot(vecFromAsso)/(vecFromMain.norm()*vecFromAsso.norm()));
-+
-+ return ParallaxAnglePoint3(yaw,pitch,parallax);
-+ }
-+
-+ static ParallaxAnglePoint3 FromParallaxAnglePointAndAnchors(const ParallaxAnglePoint3 &oldPoint,
-+ const Point3 &oldMainAnchor, const Point3 &oldAssoAnchor,
-+ const Point3 &newMainAnchor, const Point3 &newAssoAnchor);
-+
- /// @}
- /// @name Testable
- /// @{
---- gtsam.h 2015-05-11 14:28:07.137933964 +0200
-+++ gtsam.h 2015-05-11 14:23:28.263660051 +0200
-@@ -844,6 +844,14 @@
- ParallaxAnglePoint2(double yaw, double pitch);
- ParallaxAnglePoint2(Vector v);
-
-+ // Static Constructors
-+ static gtsam::ParallaxAnglePoint2 FromCameraAndMeasurement(const gtsam::SimpleCamera &camera, const gtsam::Point2 &measurement);
-+ static gtsam::ParallaxAnglePoint2 FromCameraAndMeasurement(const gtsam::PinholeCameraCal3DS2 &camera, const gtsam::Point2 &measurement);
-+ static gtsam::ParallaxAnglePoint2 FromPoseMeasurementAndCalibration(const gtsam::Pose3 &pose, const gtsam::Point2 &measurement, const gtsam::Cal3_S2& K, const gtsam::Pose3& body_P_sensor);
-+ static gtsam::ParallaxAnglePoint2 FromPoseMeasurementAndCalibration(const gtsam::Pose3 &pose, const gtsam::Point2 &measurement, const gtsam::Cal3_S2& K);
-+ static gtsam::ParallaxAnglePoint2 FromPoseMeasurementAndCalibration(const gtsam::Pose3 &pose, const gtsam::Point2 &measurement, const gtsam::Cal3DS2& K);
-+ static gtsam::ParallaxAnglePoint2 FromPoseMeasurementAndCalibration(const gtsam::Pose3 &pose, const gtsam::Point2 &measurement, const gtsam::Cal3DS2& K, const gtsam::Pose3& body_P_sensor);
-+
- // Testable
- void print(string s) const;
- bool equals(const gtsam::ParallaxAnglePoint2& pose, double tol) const;
-@@ -872,6 +880,27 @@
- ParallaxAnglePoint3(double yaw, double pitch, double parallax);
- ParallaxAnglePoint3(Vector v);
-
-+ // Static Constructors
-+ static gtsam::ParallaxAnglePoint3 FromCamerasAndMeasurements( const gtsam::SimpleCamera &mainCamera, const gtsam::Point2 &measurementFromMain,
-+ const gtsam::SimpleCamera &assoCamera, const gtsam::Point2 &measurementFromAsso);
-+ static gtsam::ParallaxAnglePoint3 FromCamerasAndMeasurements( const gtsam::PinholeCameraCal3DS2 &mainCamera, const gtsam::Point2 &measurementFromMain,
-+ const gtsam::PinholeCameraCal3DS2 &assoCamera, const gtsam::Point2 &measurementFromAsso);
-+ static gtsam::ParallaxAnglePoint3 FromPosesMeasurementsAndCalibration( const gtsam::Pose3 &mainPose, const gtsam::Point2 &measurementFromMain,
-+ const gtsam::Pose3 &assoPose, const gtsam::Point2 &measurementFromAsso,
-+ const gtsam::Cal3_S2& K);
-+ static gtsam::ParallaxAnglePoint3 FromPosesMeasurementsAndCalibration( const gtsam::Pose3 &mainPose, const gtsam::Point2 &measurementFromMain,
-+ const gtsam::Pose3 &assoPose, const gtsam::Point2 &measurementFromAsso,
-+ const gtsam::Cal3_S2& K, const gtsam::Pose3 &body_P_sensor);
-+ static gtsam::ParallaxAnglePoint3 FromPosesMeasurementsAndCalibration( const gtsam::Pose3 &mainPose, const gtsam::Point2 &measurementFromMain,
-+ const gtsam::Pose3 &assoPose, const gtsam::Point2 &measurementFromAsso,
-+ const gtsam::Cal3DS2& K);
-+ static gtsam::ParallaxAnglePoint3 FromPosesMeasurementsAndCalibration( const gtsam::Pose3 &mainPose, const gtsam::Point2 &measurementFromMain,
-+ const gtsam::Pose3 &assoPose, const gtsam::Point2 &measurementFromAsso,
-+ const gtsam::Cal3DS2& K, const gtsam::Pose3 &body_P_sensor);
-+ static gtsam::ParallaxAnglePoint3 FromParallaxAnglePointAndAnchors( const gtsam::ParallaxAnglePoint3 &oldPoint,
-+ const gtsam::Point3 &oldMainAnchor, const gtsam::Point3 &oldAssoAnchor,
-+ const gtsam::Point3 &newMainAnchor, const gtsam::Point3 &newAssoAnchor);
-+
- // Testable
- void print(string s) const;
- bool equals(const gtsam::ParallaxAnglePoint3& pose, double tol) const;
diff --git a/gtsam/patches/patch-af b/gtsam/patches/patch-af
deleted file mode 100644
index 31493d11a04a73ee302afc34f99636025fac0615..0000000000000000000000000000000000000000
--- a/gtsam/patches/patch-af
+++ /dev/null
@@ -1,181 +0,0 @@
-Patch with last modifications to work with Parallax Angle Points in the toolbox
- - Add toPoint3 methods and wrap it
- - Fix a bug in the Parallax Angle Point static constructors
- - Wrap evaluateError methods of Parallax Angle Point Projection Factors (was used for debug)
- - Add method to access newFactorsIndices from ISAM2Result, and wrap it in the matlab toolbox
---- gtsam/geometry/ParallaxAnglePoint2.h 2015-05-19 15:06:53.169068033 +0200
-+++ gtsam/geometry/ParallaxAnglePoint2.h 2015-05-14 18:49:59.374873705 +0200
-@@ -74,7 +74,7 @@
- template<class PinholeCameraType>
- static ParallaxAnglePoint2 FromCameraAndMeasurement(const PinholeCameraType &camera, const Point2 &measurement)
- {
-- Point3 vecFromMain = camera.backproject(measurement, 1.0);
-+ Point3 vecFromMain = camera.backproject(measurement, 1.0) - camera.pose().translation();
- double yaw = atan2(vecFromMain.y(),vecFromMain.x());
- double pitch = atan2(vecFromMain.z(),Point2(vecFromMain.x(),vecFromMain.y()).norm());
-
-@@ -96,7 +96,7 @@
- camera_ptr = boost::make_shared<PinholeCamera<CalibrationType> >(pose, K);
- }
-
-- Point3 vecFromMain = camera_ptr->backproject(measurement, 1.0);
-+ Point3 vecFromMain = camera_ptr->backproject(measurement, 1.0) - camera_ptr->pose().translation();
- double yaw = atan2(vecFromMain.y(),vecFromMain.x());
- double pitch = atan2(vecFromMain.z(),Point2(vecFromMain.x(),vecFromMain.y()).norm());
-
---- gtsam/geometry/ParallaxAnglePoint3.cpp 2015-05-19 15:06:53.169068033 +0200
-+++ gtsam/geometry/ParallaxAnglePoint3.cpp 2015-05-19 14:35:56.340826167 +0200
-@@ -205,6 +205,39 @@
-
- }
-
-+Point3 ParallaxAnglePoint3::toPoint3(const Point3 & mainAnchor, const Point3 & assoAnchor) const
-+{
-+ Vector3 vecFromMain = directionVectorFromMainAnchor();
-+
-+ Point3 mainToAsso = (assoAnchor - mainAnchor);
-+
-+ double mainAngle = angleBetweenUnitVectors(vecFromMain, mainToAsso.normalize().vector());
-+
-+ double sinParalax = sin(parallax_);
-+ // Guard to avoid division by zero
-+ if(fabs(sinParalax) <= 1e-6)
-+ {
-+ if (sinParalax < 0) sinParalax = -1e-6;
-+ else sinParalax = 1e-6;
-+ }
-+
-+ double depthFromMain = (sin(mainAngle + parallax_)/sinParalax)*mainToAsso.norm();
-+
-+ return mainAnchor + Point3(vecFromMain*depthFromMain);
-+
-+}
-+
-+Point3 ParallaxAnglePoint3::toPoint3(const Pose3 & mainPose, const Pose3 & assoPose, boost::optional<const Pose3 &> body_P_sensor) const
-+{
-+ if(body_P_sensor)
-+ {
-+ return toPoint3(mainPose.compose(*body_P_sensor).translation(), assoPose.compose(*body_P_sensor).translation());
-+ }
-+ //else
-+
-+ return toPoint3(mainPose.translation(), assoPose.translation());
-+}
-+
- }
-
- // Local Functions
---- gtsam/geometry/ParallaxAnglePoint3.h 2015-05-19 15:06:53.169068033 +0200
-+++ gtsam/geometry/ParallaxAnglePoint3.h 2015-05-18 15:41:52.795327081 +0200
-@@ -79,8 +79,8 @@
- const PinholeCameraType &mainCamera, const Point2 &measurementFromMain,
- const PinholeCameraType &assoCamera, const Point2 &measurementFromAsso)
- {
-- Point3 pointFromMain = mainCamera.backproject(measurementFromMain, 1.0);
-- Point3 pointFromAsso = assoCamera.backproject(measurementFromAsso, 1.0);
-+ Point3 pointFromMain = mainCamera.backproject(measurementFromMain, 1.0) - mainCamera.pose().translation();
-+ Point3 pointFromAsso = assoCamera.backproject(measurementFromAsso, 1.0) - assoCamera.pose().translation();
-
- double yaw = atan2(pointFromMain.y(),pointFromMain.x());
- double pitch = atan2(pointFromMain.z(),Point2(pointFromMain.x(),pointFromMain.y()).norm());
-@@ -110,8 +110,8 @@
- assoCamera_ptr = boost::make_shared<PinholeCamera<CalibrationType> >(assoPose, K);
- }
-
-- Point3 vecFromMain = mainCamera_ptr->backproject(measurementFromMain, 1.0);
-- Point3 vecFromAsso = assoCamera_ptr->backproject(measurementFromAsso, 1.0);
-+ Point3 vecFromMain = mainCamera_ptr->backproject(measurementFromMain, 1.0) - mainCamera_ptr->pose().translation();
-+ Point3 vecFromAsso = assoCamera_ptr->backproject(measurementFromAsso, 1.0) - assoCamera_ptr->pose().translation();
-
- double yaw = atan2(vecFromMain.y(),vecFromMain.x());
- double pitch = atan2(vecFromMain.z(),Point2(vecFromMain.x(),vecFromMain.y()).norm());
-@@ -192,6 +192,10 @@
- boost::optional<gtsam::Matrix&> Dasso = boost::none,
- boost::optional<gtsam::Matrix&> Dothe = boost::none) const;
-
-+ Point3 toPoint3(const Point3 & mainAnchor, const Point3 & assoAnchor) const;
-+
-+ Point3 toPoint3(const Pose3 & mainPose, const Pose3 & assoPose, boost::optional<const Pose3 &> body_P_sensor = boost::none) const;
-+
- /// Output stream operator
- GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const ParallaxAnglePoint3& p);
-
---- gtsam/nonlinear/ISAM2.h 2014-05-08 02:57:09.000000000 +0200
-+++ gtsam/nonlinear/ISAM2.h 2015-05-13 15:52:41.701493931 +0200
-@@ -310,6 +310,10 @@
- */
- FastVector<size_t> newFactorsIndices;
-
-+ /** Function to access newFactorIndices from Matlab toolbox
-+ */
-+ inline FastVector<size_t> getNewFactorsIndices() { return newFactorsIndices; }
-+
- /** A struct holding detailed results, which must be enabled with
- * ISAM2Params::enableDetailedResults.
- */
---- gtsam/slam/ParallaxAngleProjectionFactor.h 2015-05-19 15:06:53.161068144 +0200
-+++ gtsam/slam/ParallaxAngleProjectionFactor.h 2015-05-14 12:27:36.733741098 +0200
-@@ -639,7 +639,7 @@
- if (Dpoint) *Dpoint = zeros(2,3);
- if (verboseCheirality_)
- std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key3()) <<
-- " with anchors (" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key2()) <<
-+ " with anchors (" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key2()) << ")" <<
- " moved behind camera " << DefaultKeyFormatter(this->key2()) << std::endl;
- if (throwCheirality_)
- throw e;
---- gtsam.h 2015-05-19 15:06:53.169068033 +0200
-+++ gtsam.h 2015-05-18 15:48:30.119315060 +0200
-@@ -921,6 +921,10 @@
- Vector directionVectorFromAssoAnchor(const gtsam::Point3& mainAnchor, const gtsam::Point3& assoAnchor) const;
- Vector directionVectorFromOtheAnchor(const gtsam::Point3& mainAnchor, const gtsam::Point3& assoAnchor, const gtsam::Point3& otheAnchor) const;
-
-+ gtsam::Point3 toPoint3(const gtsam::Point3 & mainAnchor, const gtsam::Point3 & assoAnchor) const;
-+ gtsam::Point3 toPoint3(const gtsam::Pose3 & mainPose, const gtsam::Pose3 & assoPose) const;
-+ gtsam::Point3 toPoint3(const gtsam::Pose3 & mainPose, const gtsam::Pose3 & assoPose, const gtsam::Pose3 & body_P_sensor) const;
-+
- // enabling serialization functionality
- void serialize() const;
- };
-@@ -2106,6 +2110,7 @@
- size_t getVariablesRelinearized() const;
- size_t getVariablesReeliminated() const;
- size_t getCliques() const;
-+ gtsam::KeyVector getNewFactorsIndices();
- };
-
- class ISAM2 {
-@@ -2283,6 +2288,10 @@
- size_t mainAnchorKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality,
- const POSE& body_P_sensor);
-
-+ // Error functions
-+ Vector evaluateError(const gtsam::Pose3& mainPose, const gtsam::ParallaxAnglePoint2& point) const;
-+ Vector evaluateError(const gtsam::Pose3& mainPose, const gtsam::ParallaxAnglePoint3& point) const;
-+
- gtsam::Point2 measured() const;
- CALIBRATION* calibration() const;
- bool verboseCheirality() const;
-@@ -2310,6 +2319,11 @@
- size_t mainAnchorKey, size_t associatedAnchorKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality,
- const POSE& body_P_sensor);
-
-+ // Error functions
-+ Vector evaluateError(const gtsam::Pose3& mainPose, const gtsam::Pose3& assoPose, const gtsam::ParallaxAnglePoint3& point) const;
-+
-+
-+
- gtsam::Point2 measured() const;
- CALIBRATION* calibration() const;
- bool verboseCheirality() const;
-@@ -2335,6 +2349,9 @@
- size_t mainAnchorKey, size_t associatedAnchorKey, size_t otherAnchorKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality,
- const POSE& body_P_sensor);
-
-+ // Error functions
-+ Vector evaluateError(const gtsam::Pose3& mainPose, const gtsam::Pose3& assoPose, const gtsam::Pose3& othePose, const gtsam::ParallaxAnglePoint3& point) const;
-+
- gtsam::Point2 measured() const;
- CALIBRATION* calibration() const;
- bool verboseCheirality() const;
-
diff --git a/gtsam/patches/patch-ag b/gtsam/patches/patch-ag
deleted file mode 100644
index 257d124922592b34ff0221cd596f93100d96566e..0000000000000000000000000000000000000000
--- a/gtsam/patches/patch-ag
+++ /dev/null
@@ -1,22 +0,0 @@
-Add method to reinitialize (update) ISAM2 linearization point
---- gtsam/nonlinear/ISAM2.h 2015-06-03 19:36:05.860771545 +0200
-+++ gtsam/nonlinear/ISAM2.h 2015-06-01 20:47:07.677809363 +0200
-@@ -551,6 +551,8 @@
- /** Access the current linearization point */
- const Values& getLinearizationPoint() const { return theta_; }
-
-+ void updateLinearizationPoint(const Values& newTheta) { theta_.update(newTheta); }
-+
- /** Compute an estimate from the incomplete linear delta computed during the last update.
- * This delta is incomplete because it was not updated below wildfire_threshold. If only
- * a single variable is needed, it is faster to call calculateEstimate(const KEY&).
---- gtsam.h 2015-06-03 19:36:05.864771590 +0200
-+++ gtsam.h 2015-06-01 21:24:30.287061081 +0200
-@@ -2132,6 +2132,7 @@
- //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys, bool force_relinearize);
-
- gtsam::Values getLinearizationPoint() const;
-+ void updateLinearizationPoint(const gtsam::Values& newTheta);
- gtsam::Values calculateEstimate() const;
- gtsam::Value calculateEstimate(size_t key) const;
- gtsam::Values calculateBestEstimate() const;
diff --git a/gtsam/patches/patch-ah b/gtsam/patches/patch-ah
deleted file mode 100644
index ceb56fd81fa127a1eb9b465128bd30d2bffeea66..0000000000000000000000000000000000000000
--- a/gtsam/patches/patch-ah
+++ /dev/null
@@ -1,45 +0,0 @@
-Force parallax to be positive when retracting on a ParallaxAnglePoint3
-
---- gtsam/geometry/ParallaxAnglePoint3.cpp 2015-06-16 13:15:09.731247987 +0200
-+++ gtsam/geometry/ParallaxAnglePoint3.cpp 2015-06-08 18:04:35.066192803 +0200
-@@ -71,6 +71,18 @@
- }
-
- /* ************************************************************************* */
-+ParallaxAnglePoint3 ParallaxAnglePoint3::retract(const Vector& v) const
-+{
-+ ParallaxAnglePoint3 point = (*this + v);
-+
-+ // Force parallax to be positive
-+ if(point.parallax_ < 1e-6)
-+ point.parallax_ = 1e-6;
-+
-+ return point;
-+}
-+
-+/* ************************************************************************* */
- ostream &operator<<(ostream &os, const ParallaxAnglePoint3& p) {
- os << '(' << p.yaw() << ", " << p.pitch() << ", " << p.parallax() << ')';
- return os;
---- gtsam/geometry/ParallaxAnglePoint3.h 2015-06-16 13:15:09.731247987 +0200
-+++ gtsam/geometry/ParallaxAnglePoint3.h 2015-06-08 17:59:50.022579786 +0200
-@@ -145,7 +145,7 @@
- inline size_t dim() const { return dimension; }
-
- /// Updates a with tangent space delta
-- inline ParallaxAnglePoint3 retract(const Vector& v) const { return ParallaxAnglePoint3(*this + v); }
-+ ParallaxAnglePoint3 retract(const Vector& v) const;
-
- /// Returns inverse retraction
- inline Vector3 localCoordinates(const ParallaxAnglePoint3& q) const { return (q - *this).vector(); }
---- gtsam/slam/ParallaxAngleProjectionFactor.h 2015-06-16 13:15:09.731247987 +0200
-+++ gtsam/slam/ParallaxAngleProjectionFactor.h 2015-06-16 13:14:29.170816055 +0200
-@@ -948,7 +948,7 @@
- if (Dpoint) *Dpoint = zeros(2,3);
- if (verboseCheirality_)
- std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key4()) <<
-- " with anchors (" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key2()) <<
-+ " with anchors (" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key2()) << ")" <<
- " moved behind camera " << DefaultKeyFormatter(this->key3()) << std::endl;
- if (throwCheirality_)
- throw e;