diff --git a/CMakeLists.txt b/CMakeLists.txt index 16fa5d4c5..220dbcc55 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -305,7 +305,7 @@ endif() # paths so that the compiler uses GTSAM headers in our source directory instead # of any previously installed GTSAM headers. include_directories(BEFORE - gtsam/3rdparty/UFconfig + gtsam/3rdparty/SuiteSparse_config gtsam/3rdparty/CCOLAMD/Include ${METIS_INCLUDE_DIRECTORIES} ${PROJECT_SOURCE_DIR} diff --git a/gtsam.h b/gtsam.h index c5424ef80..c12055916 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1798,9 +1798,6 @@ class Values { void insert(size_t j, Vector t); void insert(size_t j, Matrix t); - // Fixed size version - void insertFixed(size_t j, Vector t, size_t n); - void update(size_t j, const gtsam::Point2& t); void update(size_t j, const gtsam::Point3& t); void update(size_t j, const gtsam::Rot2& t); @@ -1818,12 +1815,6 @@ class Values { template T at(size_t j); - /// Fixed size versions, for n in 1..9 - void insertFixed(size_t j, Vector t, size_t n); - - /// Fixed size versions, for n in 1..9 - Vector atFixed(size_t j, size_t n); - /// version for double void insertDouble(size_t j, double c); double atDouble(size_t j) const; @@ -2489,10 +2480,30 @@ class NavState { gtsam::Pose3 pose() const; }; +#include +virtual class PreintegratedRotationParams { + PreintegratedRotationParams(); + void setGyroscopeCovariance(Matrix cov); + void setOmegaCoriolis(Vector omega); + void setBodyPSensor(const gtsam::Pose3& pose); + + Matrix getGyroscopeCovariance() const; + + // TODO(frank): allow optional + // boost::optional getOmegaCoriolis() const; + // boost::optional getBodyPSensor() const; +}; + #include -class PreintegrationParams { +virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { PreintegrationParams(Vector n_gravity); - // TODO(frank): add setters/getters or make this MATLAB wrapper feature + void setAccelerometerCovariance(Matrix cov); + void setIntegrationCovariance(Matrix cov); + void setUse2ndOrderCoriolis(bool flag); + + Matrix getAccelerometerCovariance() const; + Matrix getIntegrationCovariance() const; + bool getUse2ndOrderCoriolis() const; }; #include diff --git a/gtsam/3rdparty/CCOLAMD/Demo/Makefile b/gtsam/3rdparty/CCOLAMD/Demo/Makefile new file mode 100644 index 000000000..c0ad95683 --- /dev/null +++ b/gtsam/3rdparty/CCOLAMD/Demo/Makefile @@ -0,0 +1,49 @@ +#----------------------------------------------------------------------------- +# compile the CCOLAMD demo +#----------------------------------------------------------------------------- + +default: all + +include ../../SuiteSparse_config/SuiteSparse_config.mk + +I = -I../../include + +C = $(CC) $(CF) $(I) + +LIB2 = $(LDFLAGS) -L../../lib -lccolamd -lsuitesparseconfig $(LDLIBS) + +all: library ccolamd_example ccolamd_l_example + +library: + ( cd ../../SuiteSparse_config ; $(MAKE) ) + ( cd ../Lib ; $(MAKE) ) + +#------------------------------------------------------------------------------ +# Create the demo program, run it, and compare the output +#------------------------------------------------------------------------------ + +dist: + +ccolamd_example: ccolamd_example.c + $(C) -o ccolamd_example ccolamd_example.c $(LIB2) + - ./ccolamd_example > my_ccolamd_example.out + - diff ccolamd_example.out my_ccolamd_example.out + +ccolamd_l_example: ccolamd_l_example.c + $(C) -o ccolamd_l_example ccolamd_l_example.c $(LIB2) + - ./ccolamd_l_example > my_ccolamd_l_example.out + - diff ccolamd_l_example.out my_ccolamd_l_example.out + +#------------------------------------------------------------------------------ +# Remove all but the files in the original distribution +#------------------------------------------------------------------------------ + +clean: + - $(RM) -r $(CLEAN) + +purge: distclean + +distclean: clean + - $(RM) ccolamd_example ccolamd_l_example + - $(RM) my_ccolamd_example.out my_ccolamd_l_example.out + - $(RM) -r $(PURGE) diff --git a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.c b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.c index 253fdc988..44423574e 100644 --- a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.c +++ b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.c @@ -5,8 +5,6 @@ /* ---------------------------------------------------------------------------- * CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* diff --git a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.out b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.out index dd2dc4955..b456b0f11 100644 --- a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.out +++ b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.out @@ -15,7 +15,7 @@ Column 3, with 2 entries: row 1 row 3 -ccolamd version 2.7, Jan 25, 2011: OK. +ccolamd version 2.9, Apr 1, 2016: OK. ccolamd: number of dense or empty rows ignored: 0 ccolamd: number of dense or empty columns ignored: 0 ccolamd: number of garbage collections performed: 0 @@ -38,7 +38,7 @@ Column 3, with 1 entries: row 4 Column 4, with 0 entries: -csymamd version 2.7, Jan 25, 2011: OK. +csymamd version 2.9, Apr 1, 2016: OK. csymamd: number of dense or empty rows ignored: 0 csymamd: number of dense or empty columns ignored: 0 csymamd: number of garbage collections performed: 0 diff --git a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.c b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.c index b213aad0e..b68e1d0d6 100644 --- a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.c +++ b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.c @@ -1,12 +1,10 @@ /* ========================================================================== */ -/* === ccolamd and csymamd example (UF_long integer version) ================ */ +/* === ccolamd and csymamd example (long integer version) =================== */ /* ========================================================================== */ /* ---------------------------------------------------------------------------- * CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* @@ -46,9 +44,6 @@ #define B_NNZ 4 #define B_N 5 -/* define UF_long */ -#include "UFconfig.h" - int main (void) { @@ -56,14 +51,14 @@ int main (void) /* input matrix A definition */ /* ====================================================================== */ - UF_long A [ALEN] = { + SuiteSparse_long A [ALEN] = { 0, 1, 4, /* row indices of nonzeros in column 0 */ 2, 4, /* row indices of nonzeros in column 1 */ 0, 1, 2, 3, /* row indices of nonzeros in column 2 */ 1, 3} ; /* row indices of nonzeros in column 3 */ - UF_long p [ ] = { + SuiteSparse_long p [ ] = { 0, /* column 0 is in A [0..2] */ 3, /* column 1 is in A [3..4] */ @@ -75,7 +70,7 @@ int main (void) /* input matrix B definition */ /* ====================================================================== */ - UF_long B [ ] = { /* Note: only strictly lower triangular part */ + SuiteSparse_long B [ ] = { /* Note: only strictly lower triangular part */ /* is included, since symamd ignores the */ /* diagonal and upper triangular part of B. */ @@ -85,7 +80,7 @@ int main (void) 4 /* row indices of nonzeros in column 3 */ } ; /* row indices of nonzeros in column 4 (none) */ - UF_long q [ ] = { + SuiteSparse_long q [ ] = { 0, /* column 0 is in B [0] */ 1, /* column 1 is in B [1..2] */ @@ -98,10 +93,9 @@ int main (void) /* other variable definitions */ /* ====================================================================== */ - UF_long perm [B_N+1] ; /* note the size is N+1 */ - UF_long stats [CCOLAMD_STATS] ; /* for ccolamd and csymamd output stats */ - - UF_long row, col, pp, length, ok ; + SuiteSparse_long perm [B_N+1] ; /* note the size is N+1 */ + SuiteSparse_long stats [CCOLAMD_STATS] ; /* ccolamd/csymamd output stats */ + SuiteSparse_long row, col, pp, length, ok ; /* ====================================================================== */ /* dump the input matrix A */ diff --git a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.out b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.out index fc87f5474..559c66098 100644 --- a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.out +++ b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.out @@ -15,7 +15,7 @@ Column 3, with 2 entries: row 1 row 3 -ccolamd version 2.7, Jan 25, 2011: OK. +ccolamd version 2.9, Apr 1, 2016: OK. ccolamd: number of dense or empty rows ignored: 0 ccolamd: number of dense or empty columns ignored: 0 ccolamd: number of garbage collections performed: 0 @@ -38,7 +38,7 @@ Column 3, with 1 entries: row 4 Column 4, with 0 entries: -csymamd version 2.7, Jan 25, 2011: OK. +csymamd version 2.9, Apr 1, 2016: OK. csymamd: number of dense or empty rows ignored: 0 csymamd: number of dense or empty columns ignored: 0 csymamd: number of garbage collections performed: 0 diff --git a/gtsam/3rdparty/CCOLAMD/Doc/ChangeLog b/gtsam/3rdparty/CCOLAMD/Doc/ChangeLog index 499d2fc69..85f375c7a 100644 --- a/gtsam/3rdparty/CCOLAMD/Doc/ChangeLog +++ b/gtsam/3rdparty/CCOLAMD/Doc/ChangeLog @@ -1,3 +1,37 @@ +Apr 1, 2016: version 2.9.5 + + * licensing simplified (no other change); refer to CCOLAMD/Doc/License.txt + +Feb 1, 2016: version 2.9.4 + + * update to Makefiles + +Jan 30, 2016: version 2.9.3 + + * modifications to Makefiles + +Jan 1, 2016: version 2.9.2 + + * modified Makefile to create shared libraries + No change to C code except version number. + The empty file ccolamd_global.c removed. + +Oct 10, 2014: version 2.9.1 + + modified MATLAB/ccolamd_make.m. No change to C code except version number. + +July 31, 2013: version 2.9.0 + + * changed malloc and printf pointers to use SuiteSparse_config + +Jun 1, 2012: version 2.8.0 + + * changed from UFconfig to SuiteSparse_config + +Dec 7, 2011: version 2.7.4 + + * fixed the Makefile to better align with CFLAGS and other standards + Jan 25, 2011: version 2.7.3 * minor fix to "make install" diff --git a/gtsam/3rdparty/CCOLAMD/Doc/License.txt b/gtsam/3rdparty/CCOLAMD/Doc/License.txt new file mode 100644 index 000000000..089509a6b --- /dev/null +++ b/gtsam/3rdparty/CCOLAMD/Doc/License.txt @@ -0,0 +1,21 @@ +CCOLAMD: constrained column approximate minimum degree ordering +Copyright (C) 2005-2016, Univ. of Florida. Authors: Timothy A. Davis, +Sivasankaran Rajamanickam, and Stefan Larimore. Closely based on COLAMD by +Davis, Stefan Larimore, in collaboration with Esmond Ng, and John Gilbert. +http://www.suitesparse.com + +-------------------------------------------------------------------------------- + +CCOLAMD is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +CCOLAMD is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this Module; if not, write to the Free Software +Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA diff --git a/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h b/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h index 55693d47d..b4ee829be 100644 --- a/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h +++ b/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h @@ -5,8 +5,6 @@ /* ---------------------------------------------------------------------------- * CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* @@ -32,24 +30,24 @@ extern "C" { /* All versions of CCOLAMD will include the following definitions. * As an example, to test if the version you are using is 1.3 or later: * - * if (CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3)) ... + * if (CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3)) ... * * This also works during compile-time: * - * #if CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3) - * printf ("This is version 1.3 or later\n") ; - * #else - * printf ("This is an early version\n") ; - * #endif + * #if CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3) + * printf ("This is version 1.3 or later\n") ; + * #else + * printf ("This is an early version\n") ; + * #endif */ -#define CCOLAMD_DATE "Jan 25, 2011" +#define CCOLAMD_DATE "Apr 1, 2016" #define CCOLAMD_VERSION_CODE(main,sub) ((main) * 1000 + (sub)) #define CCOLAMD_MAIN_VERSION 2 -#define CCOLAMD_SUB_VERSION 7 -#define CCOLAMD_SUBSUB_VERSION 3 +#define CCOLAMD_SUB_VERSION 9 +#define CCOLAMD_SUBSUB_VERSION 5 #define CCOLAMD_VERSION \ - CCOLAMD_VERSION_CODE(CCOLAMD_MAIN_VERSION,CCOLAMD_SUB_VERSION) + CCOLAMD_VERSION_CODE(CCOLAMD_MAIN_VERSION,CCOLAMD_SUB_VERSION) /* ========================================================================== */ /* === Knob and statistics definitions ====================================== */ @@ -94,106 +92,105 @@ extern "C" { #define CCOLAMD_NEWLY_EMPTY_COL 10 /* error codes returned in stats [3]: */ -#define CCOLAMD_OK (0) -#define CCOLAMD_OK_BUT_JUMBLED (1) -#define CCOLAMD_ERROR_A_not_present (-1) -#define CCOLAMD_ERROR_p_not_present (-2) -#define CCOLAMD_ERROR_nrow_negative (-3) -#define CCOLAMD_ERROR_ncol_negative (-4) -#define CCOLAMD_ERROR_nnz_negative (-5) -#define CCOLAMD_ERROR_p0_nonzero (-6) -#define CCOLAMD_ERROR_A_too_small (-7) -#define CCOLAMD_ERROR_col_length_negative (-8) -#define CCOLAMD_ERROR_row_index_out_of_bounds (-9) -#define CCOLAMD_ERROR_out_of_memory (-10) -#define CCOLAMD_ERROR_invalid_cmember (-11) -#define CCOLAMD_ERROR_internal_error (-999) +#define CCOLAMD_OK (0) +#define CCOLAMD_OK_BUT_JUMBLED (1) +#define CCOLAMD_ERROR_A_not_present (-1) +#define CCOLAMD_ERROR_p_not_present (-2) +#define CCOLAMD_ERROR_nrow_negative (-3) +#define CCOLAMD_ERROR_ncol_negative (-4) +#define CCOLAMD_ERROR_nnz_negative (-5) +#define CCOLAMD_ERROR_p0_nonzero (-6) +#define CCOLAMD_ERROR_A_too_small (-7) +#define CCOLAMD_ERROR_col_length_negative (-8) +#define CCOLAMD_ERROR_row_index_out_of_bounds (-9) +#define CCOLAMD_ERROR_out_of_memory (-10) +#define CCOLAMD_ERROR_invalid_cmember (-11) +#define CCOLAMD_ERROR_internal_error (-999) /* ========================================================================== */ /* === Prototypes of user-callable routines ================================= */ /* ========================================================================== */ -/* define UF_long */ -#include "UFconfig.h" +#include "SuiteSparse_config.h" -size_t ccolamd_recommended /* returns recommended value of Alen, */ - /* or 0 if input arguments are erroneous */ +size_t ccolamd_recommended /* returns recommended value of Alen, */ + /* or 0 if input arguments are erroneous */ ( - int nnz, /* nonzeros in A */ - int n_row, /* number of rows in A */ - int n_col /* number of columns in A */ + int nnz, /* nonzeros in A */ + int n_row, /* number of rows in A */ + int n_col /* number of columns in A */ ) ; -size_t ccolamd_l_recommended /* returns recommended value of Alen, */ - /* or 0 if input arguments are erroneous */ +size_t ccolamd_l_recommended /* returns recommended value of Alen, */ + /* or 0 if input arguments are erroneous */ ( - UF_long nnz, /* nonzeros in A */ - UF_long n_row, /* number of rows in A */ - UF_long n_col /* number of columns in A */ + SuiteSparse_long nnz, /* nonzeros in A */ + SuiteSparse_long n_row, /* number of rows in A */ + SuiteSparse_long n_col /* number of columns in A */ ) ; -void ccolamd_set_defaults /* sets default parameters */ -( /* knobs argument is modified on output */ - double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */ +void ccolamd_set_defaults /* sets default parameters */ +( /* knobs argument is modified on output */ + double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */ ) ; -void ccolamd_l_set_defaults /* sets default parameters */ -( /* knobs argument is modified on output */ - double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */ +void ccolamd_l_set_defaults /* sets default parameters */ +( /* knobs argument is modified on output */ + double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */ ) ; -int ccolamd /* returns (1) if successful, (0) otherwise*/ -( /* A and p arguments are modified on output */ - int n_row, /* number of rows in A */ - int n_col, /* number of columns in A */ - int Alen, /* size of the array A */ - int A [ ], /* row indices of A, of size Alen */ - int p [ ], /* column pointers of A, of size n_col+1 */ +int ccolamd /* returns (1) if successful, (0) otherwise*/ +( /* A and p arguments are modified on output */ + int n_row, /* number of rows in A */ + int n_col, /* number of columns in A */ + int Alen, /* size of the array A */ + int A [ ], /* row indices of A, of size Alen */ + int p [ ], /* column pointers of A, of size n_col+1 */ double knobs [CCOLAMD_KNOBS],/* parameter settings for ccolamd */ - int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */ - int cmember [ ] /* Constraint set of A, of size n_col */ + int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */ + int cmember [ ] /* Constraint set of A, of size n_col */ ) ; -UF_long ccolamd_l /* same as ccolamd, but with UF_long integers */ +SuiteSparse_long ccolamd_l /* as ccolamd w/ SuiteSparse_long integers */ ( - UF_long n_row, - UF_long n_col, - UF_long Alen, - UF_long A [ ], - UF_long p [ ], + SuiteSparse_long n_row, + SuiteSparse_long n_col, + SuiteSparse_long Alen, + SuiteSparse_long A [ ], + SuiteSparse_long p [ ], double knobs [CCOLAMD_KNOBS], - UF_long stats [CCOLAMD_STATS], - UF_long cmember [ ] + SuiteSparse_long stats [CCOLAMD_STATS], + SuiteSparse_long cmember [ ] ) ; -int csymamd /* return (1) if OK, (0) otherwise */ +int csymamd /* return (1) if OK, (0) otherwise */ ( - int n, /* number of rows and columns of A */ - int A [ ], /* row indices of A */ - int p [ ], /* column pointers of A */ - int perm [ ], /* output permutation, size n_col+1 */ + int n, /* number of rows and columns of A */ + int A [ ], /* row indices of A */ + int p [ ], /* column pointers of A */ + int perm [ ], /* output permutation, size n_col+1 */ double knobs [CCOLAMD_KNOBS],/* parameters (uses defaults if NULL) */ - int stats [CCOLAMD_STATS], /* output statistics and error codes */ + int stats [CCOLAMD_STATS], /* output statistics and error codes */ void * (*allocate) (size_t, size_t), /* pointer to calloc (ANSI C) or */ - /* mxCalloc (for MATLAB mexFunction) */ - void (*release) (void *), /* pointer to free (ANSI C) or */ - /* mxFree (for MATLAB mexFunction) */ - int cmember [ ], /* Constraint set of A */ - int stype /* 0: use both parts, >0: upper, <0: lower */ + /* mxCalloc (for MATLAB mexFunction) */ + void (*release) (void *), /* pointer to free (ANSI C) or */ + /* mxFree (for MATLAB mexFunction) */ + int cmember [ ], /* Constraint set of A */ + int stype /* 0: use both parts, >0: upper, <0: lower */ ) ; -UF_long csymamd_l /* same as csymamd, but with UF_long integers */ +SuiteSparse_long csymamd_l /* as csymamd, w/ SuiteSparse_long integers */ ( - UF_long n, - UF_long A [ ], - UF_long p [ ], - UF_long perm [ ], + SuiteSparse_long n, + SuiteSparse_long A [ ], + SuiteSparse_long p [ ], + SuiteSparse_long perm [ ], double knobs [CCOLAMD_KNOBS], - UF_long stats [CCOLAMD_STATS], + SuiteSparse_long stats [CCOLAMD_STATS], void * (*allocate) (size_t, size_t), void (*release) (void *), - UF_long cmember [ ], - UF_long stype + SuiteSparse_long cmember [ ], + SuiteSparse_long stype ) ; void ccolamd_report @@ -203,7 +200,7 @@ void ccolamd_report void ccolamd_l_report ( - UF_long stats [CCOLAMD_STATS] + SuiteSparse_long stats [CCOLAMD_STATS] ) ; void csymamd_report @@ -213,7 +210,7 @@ void csymamd_report void csymamd_l_report ( - UF_long stats [CCOLAMD_STATS] + SuiteSparse_long stats [CCOLAMD_STATS] ) ; @@ -227,42 +224,42 @@ void csymamd_l_report */ int ccolamd2 -( /* A and p arguments are modified on output */ - int n_row, /* number of rows in A */ - int n_col, /* number of columns in A */ - int Alen, /* size of the array A */ - int A [ ], /* row indices of A, of size Alen */ - int p [ ], /* column pointers of A, of size n_col+1 */ +( /* A and p arguments are modified on output */ + int n_row, /* number of rows in A */ + int n_col, /* number of columns in A */ + int Alen, /* size of the array A */ + int A [ ], /* row indices of A, of size Alen */ + int p [ ], /* column pointers of A, of size n_col+1 */ double knobs [CCOLAMD_KNOBS],/* parameter settings for ccolamd */ - int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */ + int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */ /* each Front_ array is of size n_col+1: */ - int Front_npivcol [ ], /* # pivot cols in each front */ - int Front_nrows [ ], /* # of rows in each front (incl. pivot rows) */ - int Front_ncols [ ], /* # of cols in each front (incl. pivot cols) */ - int Front_parent [ ], /* parent of each front */ - int Front_cols [ ], /* link list of pivot columns for each front */ - int *p_nfr, /* total number of frontal matrices */ - int InFront [ ], /* InFront [row] = f if row in front f */ - int cmember [ ] /* Constraint set of A */ + int Front_npivcol [ ], /* # pivot cols in each front */ + int Front_nrows [ ], /* # of rows in each front (incl. pivot rows) */ + int Front_ncols [ ], /* # of cols in each front (incl. pivot cols) */ + int Front_parent [ ], /* parent of each front */ + int Front_cols [ ], /* link list of pivot columns for each front */ + int *p_nfr, /* total number of frontal matrices */ + int InFront [ ], /* InFront [row] = f if row in front f */ + int cmember [ ] /* Constraint set of A */ ) ; -UF_long ccolamd2_l /* same as ccolamd2, but with UF_long integers */ +SuiteSparse_long ccolamd2_l /* as ccolamd2, w/ SuiteSparse_long integers */ ( - UF_long n_row, - UF_long n_col, - UF_long Alen, - UF_long A [ ], - UF_long p [ ], + SuiteSparse_long n_row, + SuiteSparse_long n_col, + SuiteSparse_long Alen, + SuiteSparse_long A [ ], + SuiteSparse_long p [ ], double knobs [CCOLAMD_KNOBS], - UF_long stats [CCOLAMD_STATS], - UF_long Front_npivcol [ ], - UF_long Front_nrows [ ], - UF_long Front_ncols [ ], - UF_long Front_parent [ ], - UF_long Front_cols [ ], - UF_long *p_nfr, - UF_long InFront [ ], - UF_long cmember [ ] + SuiteSparse_long stats [CCOLAMD_STATS], + SuiteSparse_long Front_npivcol [ ], + SuiteSparse_long Front_nrows [ ], + SuiteSparse_long Front_ncols [ ], + SuiteSparse_long Front_parent [ ], + SuiteSparse_long Front_cols [ ], + SuiteSparse_long *p_nfr, + SuiteSparse_long InFront [ ], + SuiteSparse_long cmember [ ] ) ; void ccolamd_apply_order @@ -276,11 +273,11 @@ void ccolamd_apply_order void ccolamd_l_apply_order ( - UF_long Front [ ], - const UF_long Order [ ], - UF_long Temp [ ], - UF_long nn, - UF_long nfr + SuiteSparse_long Front [ ], + const SuiteSparse_long Order [ ], + SuiteSparse_long Temp [ ], + SuiteSparse_long nn, + SuiteSparse_long nfr ) ; @@ -296,12 +293,12 @@ void ccolamd_fsize void ccolamd_l_fsize ( - UF_long nn, - UF_long MaxFsize [ ], - UF_long Fnrows [ ], - UF_long Fncols [ ], - UF_long Parent [ ], - UF_long Npiv [ ] + SuiteSparse_long nn, + SuiteSparse_long MaxFsize [ ], + SuiteSparse_long Fnrows [ ], + SuiteSparse_long Fncols [ ], + SuiteSparse_long Parent [ ], + SuiteSparse_long Npiv [ ] ) ; void ccolamd_postorder @@ -320,16 +317,16 @@ void ccolamd_postorder void ccolamd_l_postorder ( - UF_long nn, - UF_long Parent [ ], - UF_long Npiv [ ], - UF_long Fsize [ ], - UF_long Order [ ], - UF_long Child [ ], - UF_long Sibling [ ], - UF_long Stack [ ], - UF_long Front_cols [ ], - UF_long cmember [ ] + SuiteSparse_long nn, + SuiteSparse_long Parent [ ], + SuiteSparse_long Npiv [ ], + SuiteSparse_long Fsize [ ], + SuiteSparse_long Order [ ], + SuiteSparse_long Child [ ], + SuiteSparse_long Sibling [ ], + SuiteSparse_long Stack [ ], + SuiteSparse_long Front_cols [ ], + SuiteSparse_long cmember [ ] ) ; int ccolamd_post_tree @@ -342,22 +339,16 @@ int ccolamd_post_tree int Stack [ ] ) ; -UF_long ccolamd_l_post_tree +SuiteSparse_long ccolamd_l_post_tree ( - UF_long root, - UF_long k, - UF_long Child [ ], - const UF_long Sibling [ ], - UF_long Order [ ], - UF_long Stack [ ] + SuiteSparse_long root, + SuiteSparse_long k, + SuiteSparse_long Child [ ], + const SuiteSparse_long Sibling [ ], + SuiteSparse_long Order [ ], + SuiteSparse_long Stack [ ] ) ; -#ifndef EXTERN -#define EXTERN extern -#endif - -EXTERN int (*ccolamd_printf) (const char *, ...) ; - #ifdef __cplusplus } #endif diff --git a/gtsam/3rdparty/CCOLAMD/Lib/Makefile b/gtsam/3rdparty/CCOLAMD/Lib/Makefile new file mode 100644 index 000000000..c2352c90e --- /dev/null +++ b/gtsam/3rdparty/CCOLAMD/Lib/Makefile @@ -0,0 +1,71 @@ +#------------------------------------------------------------------------------- +# CCOLAMD Lib/Makefile +#------------------------------------------------------------------------------- + +LIBRARY = libccolamd +VERSION = 2.9.5 +SO_VERSION = 2 + +default: library + +include ../../SuiteSparse_config/SuiteSparse_config.mk + +# CCOLAMD depends on SuiteSparse_config +LDLIBS += -lsuitesparseconfig + +# compile and install in SuiteSparse/lib +library: + $(MAKE) install INSTALL=$(SUITESPARSE) + +I = -I../Include -I../../SuiteSparse_config + +INC = ../Include/ccolamd.h ../../SuiteSparse_config/SuiteSparse_config.h + +SRC = ../Source/ccolamd.c + +OBJ = ccolamd.o ccolamd_l.o + +ccolamd.o: $(SRC) $(INC) + $(CC) $(CF) $(I) -c ../Source/ccolamd.c + +ccolamd_l.o: $(SRC) $(INC) + $(CC) $(CF) $(I) -c ../Source/ccolamd.c -DDLONG -o ccolamd_l.o + +# creates libccolamd.a, a C-callable CCOLAMD library +$(AR_TARGET): $(OBJ) + $(ARCHIVE) $@ $^ + - $(RANLIB) $@ + +ccode: library + +clean: + - $(RM) -r $(CLEAN) + +purge: distclean + +distclean: clean + - $(RM) -r $(PURGE) + +# install CCOLAMD +install: $(AR_TARGET) $(INSTALL_LIB)/$(SO_TARGET) + +$(INSTALL_LIB)/$(SO_TARGET): $(OBJ) + @mkdir -p $(INSTALL_LIB) + @mkdir -p $(INSTALL_INCLUDE) + @mkdir -p $(INSTALL_DOC) + $(CC) $(SO_OPTS) $^ -o $@ $(LDLIBS) + ( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_PLAIN) ) + ( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_MAIN) ) + $(CP) ../Include/ccolamd.h $(INSTALL_INCLUDE) + $(CP) ../README.txt $(INSTALL_DOC)/CCOLAMD_README.txt + chmod 755 $(INSTALL_LIB)/$(SO_TARGET) + chmod 644 $(INSTALL_INCLUDE)/ccolamd.h + chmod 644 $(INSTALL_DOC)/CCOLAMD_README.txt + +uninstall: + $(RM) $(INSTALL_LIB)/$(SO_TARGET) + $(RM) $(INSTALL_LIB)/$(SO_PLAIN) + $(RM) $(INSTALL_LIB)/$(SO_MAIN) + $(RM) $(INSTALL_INCLUDE)/ccolamd.h + $(RM) $(INSTALL_DOC)/CCOLAMD_README.txt + diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_demo.m b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_demo.m index e2fd68457..2c69a502e 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_demo.m +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_demo.m @@ -47,9 +47,8 @@ spparms ('default') ; A = sprandn (n, n, 2/n) + speye (n) ; b = (1:n)' ; -figure (1) clf ; -subplot (2,2,1) +subplot (3,4,1) spy (A) title ('original matrix') @@ -62,7 +61,7 @@ fl = luflops (L, U) ; x = Q * (U \ (L \ (P * b))) ; fprintf (1, '\nFlop count for [L,U,P] = lu (A*Q): %d\n', fl) ; fprintf (1, 'residual: %e\n', norm (A*x-b)); -subplot (2,2,2) ; +subplot (3,4,2) ; spy (L|U) ; title ('LU with ccolamd') ; @@ -76,7 +75,7 @@ fl = luflops (L, U) ; x = Q * (U \ (L \ (P * b))) ; fprintf (1, '\nFlop count for [L,U,P] = lu (A*Q): %d\n', fl) ; fprintf (1, 'residual: %e\n', norm (A*x-b)); -subplot (2,2,3) ; +subplot (3,4,3) ; spy (L|U) ; title ('LU with colamd') ; catch @@ -89,7 +88,7 @@ fl = luflops (L, U) ; x = U \ (L \ (P * b)) ; fprintf (1, '\nFlop count for [L,U,P] = lu (A*Q): %d\n', fl) ; fprintf (1, 'residual: %e\n', norm (A*x-b)); -subplot (2,2,4) ; +subplot (3,4,4) ; spy (L|U) ; title ('LU with no ordering') ; @@ -111,9 +110,7 @@ n = 1000 ; fprintf (1, 'Generating a random %d-by-%d sparse matrix.\n', n, n) ; A = sprandn (n, n, 2/n) + speye (n) ; -figure (2) -clf ; -subplot (2,2,1) +subplot (3,4,5) spy (A) title ('original matrix') @@ -121,7 +118,7 @@ fprintf (1, '\n\nUnordered matrix:\n') ; [lnz,h,parent,post,R] = symbfact (A, 'col') ; fprintf (1, 'nz in Cholesky factors of A''A: %d\n', sum (lnz)) ; fprintf (1, 'flop count for Cholesky of A''A: %d\n', sum (lnz.^2)) ; -subplot (2,2,4) ; +subplot (3,4,6) ; spy (R) ; title ('Cholesky with no ordering') ; @@ -133,7 +130,7 @@ fprintf (1, '\n\nccolamd run time: %f\n', t) ; fprintf (1, 'ccolamd ordering quality: \n') ; fprintf (1, 'nz in Cholesky factors of A(:,p)''A(:,p): %d\n', sum (lnz)) ; fprintf (1, 'flop count for Cholesky of A(:,p)''A(:,p): %d\n', sum (lnz.^2)) ; -subplot (2,2,2) ; +subplot (3,4,7) ; spy (R) ; title ('Cholesky with ccolamd') ; @@ -146,7 +143,7 @@ fprintf (1, '\n\ncolamd run time: %f\n', t) ; fprintf (1, 'colamd ordering quality: \n') ; fprintf (1, 'nz in Cholesky factors of A(:,p)''A(:,p): %d\n', sum (lnz)) ; fprintf (1, 'flop count for Cholesky of A(:,p)''A(:,p): %d\n', sum (lnz.^2)) ; -subplot (2,2,3) ; +subplot (3,4,8) ; spy (R) ; title ('Cholesky with colamd') ; catch @@ -164,9 +161,7 @@ fprintf (1, '\n-----------------------------------------------------------\n') ; fprintf (1, 'Generating a random symmetric %d-by-%d sparse matrix.\n', n, n) ; A = A+A' ; -figure (3) -clf ; -subplot (2,2,1) +subplot (3,4,9) ; spy (A) title ('original matrix') @@ -174,7 +169,7 @@ fprintf (1, '\n\nUnordered matrix:\n') ; [lnz,h,parent,post,R] = symbfact (A, 'sym') ; fprintf (1, 'nz in Cholesky factors of A: %d\n', sum (lnz)) ; fprintf (1, 'flop count for Cholesky of A: %d\n', sum (lnz.^2)) ; -subplot (2,2,4) ; +subplot (3,4,10) ; spy (R) ; title ('Cholesky with no ordering') ; @@ -186,7 +181,7 @@ fprintf (1, '\n\ncsymamd run time: %f\n', t) ; fprintf (1, 'csymamd ordering quality: \n') ; fprintf (1, 'nz in Cholesky factors of A(p,p): %d\n', sum (lnz)) ; fprintf (1, 'flop count for Cholesky of A(p,p): %d\n', sum (lnz.^2)) ; -subplot (2,2,2) ; +subplot (3,4,11) ; spy (R) ; title ('Cholesky with csymamd') ; @@ -199,7 +194,7 @@ fprintf (1, '\n\nsymamd run time: %f\n', t) ; fprintf (1, 'symamd ordering quality: \n') ; fprintf (1, 'nz in Cholesky factors of A(p,p): %d\n', sum (lnz)) ; fprintf (1, 'flop count for Cholesky of A(p,p): %d\n', sum (lnz.^2)) ; -subplot (2,2,3) ; +subplot (3,4,12) ; spy (R) ; title ('Cholesky with symamd') ; catch diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_make.m b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_make.m index 4637f3884..9a84da909 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_make.m +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_make.m @@ -14,14 +14,33 @@ d = '' ; if (~isempty (strfind (computer, '64'))) d = '-largeArrayDims' ; end -src = '../Source/ccolamd.c ../Source/ccolamd_global.c' ; -cmd = sprintf ('mex -DDLONG -O %s -I../../UFconfig -I../Include -output ', d) ; + +% MATLAB 8.3.0 now has a -silent option to keep 'mex' from burbling too much +if (~verLessThan ('matlab', '8.3.0')) + d = ['-silent ' d] ; +end + +src = '../Source/ccolamd.c ../../SuiteSparse_config/SuiteSparse_config.c' ; +cmd = sprintf ( ... + 'mex -DDLONG -O %s -I../../SuiteSparse_config -I../Include -output ', d) ; s = [cmd 'ccolamd ccolamdmex.c ' src] ; + +if (~(ispc || ismac)) + % for POSIX timing routine + s = [s ' -lrt'] ; +end if (details) fprintf ('%s\n', s) ; end eval (s) ; + s = [cmd 'csymamd csymamdmex.c ' src] ; + +if (~(ispc || ismac)) + % for POSIX timing routine + s = [s ' -lrt'] ; +end + if (details) fprintf ('%s\n', s) ; end diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_test.m b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_test.m index e359b9fea..0514ad9c7 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_test.m +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_test.m @@ -22,8 +22,13 @@ csymamd_default_knobs = [10 1 0] ; if (~isempty (strfind (computer, '64'))) d = '-largeArrayDims' ; end - src = '../Source/ccolamd.c ../Source/ccolamd_global.c' ; - cmd = sprintf ('mex -DDLONG -O %s -I../../UFconfig -I../Include ', d) ; + cmd = sprintf ( ... + 'mex -DDLONG -O %s -I../../SuiteSparse_config -I../Include ', d) ; + src = '../Source/ccolamd.c ../../SuiteSparse_config/SuiteSparse_config.c' ; + if (~(ispc || ismac)) + % for POSIX timing routine + src = [src ' -lrt'] ; + end eval ([cmd 'ccolamdtestmex.c ' src]) ; eval ([cmd 'csymamdtestmex.c ' src]) ; fprintf ('Done compiling.\n') ; diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdmex.c b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdmex.c index fbcb87873..09d21efee 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdmex.c +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdmex.c @@ -5,8 +5,6 @@ /* ---------------------------------------------------------------------------- * CCOLAMD, Copyright (C), Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* @@ -26,7 +24,7 @@ #include "matrix.h" #include #include -#include "UFconfig.h" +#define Long SuiteSparse_long /* ========================================================================== */ /* === ccolamd mexFunction ================================================== */ @@ -44,24 +42,24 @@ void mexFunction { /* === Local variables ================================================== */ - UF_long *A ; /* ccolamd's copy of the matrix and workspace */ - UF_long *cmember ; /* ccolamd's copy of the constraint set */ - double *in_cmember ; /* input constraint set */ - UF_long *p ; /* ccolamd's copy of the column pointers */ - UF_long Alen ; /* size of A */ - UF_long cslen ; /* size of CS */ - UF_long n_col ; /* number of columns of A */ - UF_long n_row ; /* number of rows of A */ - UF_long nnz ; /* number of entries in A */ - UF_long full ; /* TRUE if input matrix full, FALSE if sparse */ + Long *A ; /* ccolamd's copy of the matrix and workspace */ + Long *cmember ; /* ccolamd's copy of the constraint set */ + double *in_cmember ; /* input constraint set */ + Long *p ; /* ccolamd's copy of the column pointers */ + Long Alen ; /* size of A */ + Long cslen ; /* size of CS */ + Long n_col ; /* number of columns of A */ + Long n_row ; /* number of rows of A */ + Long nnz ; /* number of entries in A */ + Long full ; /* TRUE if input matrix full, FALSE if sparse */ double knobs [CCOLAMD_KNOBS] ; /* ccolamd user-controllable parameters */ - double *out_perm ; /* output permutation vector */ - double *out_stats ; /* output stats vector */ - double *in_knobs ; /* input knobs vector */ - UF_long i ; /* loop counter */ - mxArray *Ainput ; /* input matrix handle */ - UF_long spumoni ; /* verbosity variable */ - UF_long stats [CCOLAMD_STATS] ; /* stats for ccolamd */ + double *out_perm ; /* output permutation vector */ + double *out_stats ; /* output stats vector */ + double *in_knobs ; /* input knobs vector */ + Long i ; /* loop counter */ + mxArray *Ainput ; /* input matrix handle */ + Long spumoni ; /* verbosity variable */ + Long stats [CCOLAMD_STATS] ;/* stats for ccolamd */ /* === Check inputs ===================================================== */ @@ -80,11 +78,11 @@ void mexFunction cslen = mxGetNumberOfElements (pargin [2]) ; if (cslen != 0) { - cmember = (UF_long *) mxCalloc (cslen, sizeof (UF_long)) ; + cmember = (Long *) mxCalloc (cslen, sizeof (Long)) ; for (i = 0 ; i < cslen ; i++) { /* convert cmember from 1-based to 0-based */ - cmember[i] = ((UF_long) in_cmember [i] - 1) ; + cmember[i] = ((Long) in_cmember [i] - 1) ; } } } @@ -157,10 +155,10 @@ void mexFunction n_col = mxGetN (Ainput) ; /* get column pointer vector */ - p = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ; - (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (UF_long)) ; + p = (Long *) mxCalloc (n_col+1, sizeof (Long)) ; + (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (Long)) ; nnz = p [n_col] ; - Alen = (UF_long) ccolamd_l_recommended (nnz, n_row, n_col) ; + Alen = (Long) ccolamd_l_recommended (nnz, n_row, n_col) ; if (Alen == 0) { mexErrMsgTxt ("ccolamd: problem too large") ; @@ -168,8 +166,8 @@ void mexFunction /* === Copy input matrix into workspace ================================= */ - A = (UF_long *) mxCalloc (Alen, sizeof (UF_long)) ; - (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (UF_long)) ; + A = (Long *) mxCalloc (Alen, sizeof (Long)) ; + (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (Long)) ; if (full) { diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdtestmex.c b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdtestmex.c index b5b04da73..18c850dc6 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdtestmex.c +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdtestmex.c @@ -5,8 +5,6 @@ /* ---------------------------------------------------------------------------- * CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* @@ -43,7 +41,7 @@ #include "matrix.h" #include #include -#include "UFconfig.h" +#define Long SuiteSparse_long /* Here only for testing */ #undef MIN @@ -61,15 +59,15 @@ static void dump_matrix ( - UF_long A [ ], - UF_long p [ ], - UF_long n_row, - UF_long n_col, - UF_long Alen, - UF_long limit + Long A [ ], + Long p [ ], + Long n_row, + Long n_col, + Long Alen, + Long limit ) { - UF_long col, k, row ; + Long col, k, row ; mexPrintf ("dump matrix: nrow %d ncol %d Alen %d\n", n_row, n_col, Alen) ; @@ -102,24 +100,24 @@ void mexFunction { /* === Local variables ================================================== */ - UF_long *A ; /* ccolamd's copy of the matrix and workspace */ - UF_long *p ; /* ccolamd's copy of the column pointers */ - UF_long Alen ; /* size of A */ - UF_long n_col ; /* number of columns of A */ - UF_long n_row ; /* number of rows of A */ - UF_long nnz ; /* number of entries in A */ - UF_long full ; /* TRUE if input matrix full, FALSE if sparse */ + Long *A ; /* ccolamd's copy of the matrix and workspace */ + Long *p ; /* ccolamd's copy of the column pointers */ + Long Alen ; /* size of A */ + Long n_col ; /* number of columns of A */ + Long n_row ; /* number of rows of A */ + Long nnz ; /* number of entries in A */ + Long full ; /* TRUE if input matrix full, FALSE if sparse */ double knobs [CCOLAMD_KNOBS] ; /* ccolamd user-controllable parameters */ - double *out_perm ; /* output permutation vector */ - double *out_stats ; /* output stats vector */ - double *in_knobs ; /* input knobs vector */ - UF_long i ; /* loop counter */ - mxArray *Ainput ; /* input matrix handle */ - UF_long spumoni ; /* verbosity variable */ - UF_long stats2 [CCOLAMD_STATS] ; /* stats for ccolamd */ + double *out_perm ; /* output permutation vector */ + double *out_stats ; /* output stats vector */ + double *in_knobs ; /* input knobs vector */ + Long i ; /* loop counter */ + mxArray *Ainput ; /* input matrix handle */ + Long spumoni ; /* verbosity variable */ + Long stats2 [CCOLAMD_STATS] ; /* stats for ccolamd */ - UF_long *cp, *cp_end, result, col, length, ok ; - UF_long *stats ; + Long *cp, *cp_end, result, col, length, ok ; + Long *stats ; stats = stats2 ; /* === Check inputs ===================================================== */ @@ -199,10 +197,10 @@ void mexFunction n_col = mxGetN (Ainput) ; /* get column pointer vector so we can find nnz */ - p = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ; - (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (UF_long)) ; + p = (Long *) mxCalloc (n_col+1, sizeof (Long)) ; + (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (Long)) ; nnz = p [n_col] ; - Alen = (UF_long) ccolamd_l_recommended (nnz, n_row, n_col) ; + Alen = (Long) ccolamd_l_recommended (nnz, n_row, n_col) ; if (Alen == 0) { mexErrMsgTxt ("ccolamd: problem too large") ; @@ -230,8 +228,8 @@ void mexFunction /* === Copy input matrix into workspace ================================= */ - A = (UF_long *) mxCalloc (Alen, sizeof (UF_long)) ; - (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (UF_long)) ; + A = (Long *) mxCalloc (Alen, sizeof (Long)) ; + (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (Long)) ; if (full) { @@ -261,7 +259,7 @@ void mexFunction */ /* jumble appropriately */ - switch ((UF_long) in_knobs [6]) + switch ((Long) in_knobs [6]) { case 0 : @@ -359,7 +357,7 @@ void mexFunction mexPrintf ("ccolamdtest: A not present\n") ; } result = 0 ; /* A not present */ - A = (UF_long *) NULL ; + A = (Long *) NULL ; break ; case 8 : @@ -368,7 +366,7 @@ void mexFunction mexPrintf ("ccolamdtest: p not present\n") ; } result = 0 ; /* p not present */ - p = (UF_long *) NULL ; + p = (Long *) NULL ; break ; case 9 : @@ -456,7 +454,7 @@ void mexFunction mexPrintf ("ccolamdtest: stats not present\n") ; } result = 0 ; /* stats not present */ - stats = (UF_long *) NULL ; + stats = (Long *) NULL ; break ; case 13 : diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/csymamd.m b/gtsam/3rdparty/CCOLAMD/MATLAB/csymamd.m index 901040df6..4308f9934 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/csymamd.m +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/csymamd.m @@ -34,10 +34,10 @@ function [p, stats] = csymamd (S, knobs, cmember) %#ok % p = csymamd(S) is about the same as p = symamd(S). knobs and its default % values differ. % -% Authors: S. Larimore, T. Davis (Univ of Florida), and S. Rajamanickam, in +% Authors: S. Larimore, T. Davis, and S. Rajamanickam, in % collaboration with J. Gilbert and E. Ng. Supported by the National % Science Foundation (DMS-9504974, DMS-9803599, CCR-0203270), and a grant -% from Sandia National Lab. See http://www.cise.ufl.edu/research/sparse +% from Sandia National Lab. See http://www.suitesparse.com % for ccolamd, csymamd, amd, colamd, symamd, and other related orderings. % % See also AMD, CCOLAMD, COLAMD, SYMAMD, SYMRCM. diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdmex.c b/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdmex.c index e52d92619..41a3b4346 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdmex.c +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdmex.c @@ -5,8 +5,6 @@ /* ---------------------------------------------------------------------------- * CCOLAMD, Copyright (C), Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* @@ -25,7 +23,7 @@ #include "mex.h" #include "matrix.h" #include -#include "UFconfig.h" +#define Long SuiteSparse_long /* ========================================================================== */ /* === csymamd mexFunction ================================================== */ @@ -43,23 +41,23 @@ void mexFunction { /* === Local variables ================================================== */ - UF_long *A ; /* row indices of input matrix A */ - UF_long *perm ; /* column ordering of M and ordering of A */ - UF_long *cmember ; /* csymamd's copy of the constraint set */ - double *in_cmember ; /* input constraint set */ - UF_long *p ; /* column pointers of input matrix A */ - UF_long cslen ; /* size of constraint set */ - UF_long n_col ; /* number of columns of A */ - UF_long n_row ; /* number of rows of A */ - UF_long full ; /* TRUE if input matrix full, FALSE if sparse */ + Long *A ; /* row indices of input matrix A */ + Long *perm ; /* column ordering of M and ordering of A */ + Long *cmember ; /* csymamd's copy of the constraint set */ + double *in_cmember ; /* input constraint set */ + Long *p ; /* column pointers of input matrix A */ + Long cslen ; /* size of constraint set */ + Long n_col ; /* number of columns of A */ + Long n_row ; /* number of rows of A */ + Long full ; /* TRUE if input matrix full, FALSE if sparse */ double knobs [CCOLAMD_KNOBS] ; /* csymamd user-controllable parameters */ - double *out_perm ; /* output permutation vector */ - double *out_stats ; /* output stats vector */ - double *in_knobs ; /* input knobs vector */ - UF_long i ; /* loop counter */ - mxArray *Ainput ; /* input matrix handle */ - UF_long spumoni ; /* verbosity variable */ - UF_long stats [CCOLAMD_STATS] ; /* stats for symamd */ + double *out_perm ; /* output permutation vector */ + double *out_stats ; /* output stats vector */ + double *in_knobs ; /* input knobs vector */ + Long i ; /* loop counter */ + mxArray *Ainput ; /* input matrix handle */ + Long spumoni ; /* verbosity variable */ + Long stats [CCOLAMD_STATS] ;/* stats for symamd */ /* === Check inputs ===================================================== */ @@ -78,11 +76,11 @@ void mexFunction cslen = mxGetNumberOfElements (pargin [2]) ; if (cslen != 0) { - cmember = (UF_long *) mxCalloc (cslen, sizeof (UF_long)) ; + cmember = (Long *) mxCalloc (cslen, sizeof (Long)) ; for (i = 0 ; i < cslen ; i++) { /* convert cmember from 1-based to 0-based */ - cmember[i] = ((UF_long) in_cmember [i] - 1) ; + cmember[i] = ((Long) in_cmember [i] - 1) ; } } } @@ -153,9 +151,9 @@ void mexFunction mexErrMsgTxt ("csymamd: cmember must be of length equal to #cols of A"); } - A = (UF_long *) mxGetIr (Ainput) ; - p = (UF_long *) mxGetJc (Ainput) ; - perm = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ; + A = (Long *) mxGetIr (Ainput) ; + p = (Long *) mxGetJc (Ainput) ; + perm = (Long *) mxCalloc (n_col+1, sizeof (Long)) ; /* === Order the rows and columns of A (does not destroy A) ============= */ diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdtestmex.c b/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdtestmex.c index 428b41185..60b0d6c04 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdtestmex.c +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdtestmex.c @@ -5,8 +5,6 @@ /* ---------------------------------------------------------------------------- * CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* @@ -37,7 +35,7 @@ #include "matrix.h" #include #include -#include "UFconfig.h" +#define Long SuiteSparse_long #ifdef MIN #undef MIN @@ -47,15 +45,15 @@ static void dump_matrix ( - UF_long A [ ], - UF_long p [ ], - UF_long n_row, - UF_long n_col, - UF_long Alen, - UF_long limit + Long A [ ], + Long p [ ], + Long n_row, + Long n_col, + Long Alen, + Long limit ) { - UF_long col, k, row ; + Long col, k, row ; mexPrintf ("dump matrix: nrow %d ncol %d Alen %d\n", n_row, n_col, Alen) ; @@ -100,23 +98,23 @@ void mexFunction { /* === Local variables ================================================== */ - UF_long *perm ; /* column ordering of M and ordering of A */ - UF_long *A ; /* row indices of input matrix A */ - UF_long *p ; /* column pointers of input matrix A */ - UF_long n_col ; /* number of columns of A */ - UF_long n_row ; /* number of rows of A */ - UF_long full ; /* TRUE if input matrix full, FALSE if sparse */ + Long *perm ; /* column ordering of M and ordering of A */ + Long *A ; /* row indices of input matrix A */ + Long *p ; /* column pointers of input matrix A */ + Long n_col ; /* number of columns of A */ + Long n_row ; /* number of rows of A */ + Long full ; /* TRUE if input matrix full, FALSE if sparse */ double knobs [CCOLAMD_KNOBS] ; /* ccolamd user-controllable parameters */ - double *out_perm ; /* output permutation vector */ - double *out_stats ; /* output stats vector */ - double *in_knobs ; /* input knobs vector */ - UF_long i ; /* loop counter */ - mxArray *Ainput ; /* input matrix handle */ - UF_long spumoni ; /* verbosity variable */ - UF_long stats2 [CCOLAMD_STATS] ;/* stats for csymamd */ + double *out_perm ; /* output permutation vector */ + double *out_stats ; /* output stats vector */ + double *in_knobs ; /* input knobs vector */ + Long i ; /* loop counter */ + mxArray *Ainput ; /* input matrix handle */ + Long spumoni ; /* verbosity variable */ + Long stats2 [CCOLAMD_STATS] ;/* stats for csymamd */ - UF_long *cp, *cp_end, result, nnz, col, length, ok ; - UF_long *stats ; + Long *cp, *cp_end, result, nnz, col, length, ok ; + Long *stats ; stats = stats2 ; /* === Check inputs ===================================================== */ @@ -192,8 +190,8 @@ void mexFunction } /* p = mxGetJc (Ainput) ; */ - p = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ; - (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (UF_long)) ; + p = (Long *) mxCalloc (n_col+1, sizeof (Long)) ; + (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (Long)) ; nnz = p [n_col] ; if (spumoni) @@ -202,10 +200,10 @@ void mexFunction } /* A = mxGetIr (Ainput) ; */ - A = (UF_long *) mxCalloc (nnz+1, sizeof (UF_long)) ; - (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (UF_long)) ; + A = (Long *) mxCalloc (nnz+1, sizeof (Long)) ; + (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (Long)) ; - perm = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ; + perm = (Long *) mxCalloc (n_col+1, sizeof (Long)) ; /* === Jumble matrix ==================================================== */ @@ -230,7 +228,7 @@ void mexFunction */ /* jumble appropriately */ - switch ((UF_long) in_knobs [3]) + switch ((Long) in_knobs [3]) { case 0 : @@ -321,7 +319,7 @@ void mexFunction mexPrintf ("csymamdtest: A not present\n") ; } result = 0 ; /* A not present */ - A = (UF_long *) NULL ; + A = (Long *) NULL ; break ; case 8 : @@ -330,7 +328,7 @@ void mexFunction mexPrintf ("csymamdtest: p not present\n") ; } result = 0 ; /* p not present */ - p = (UF_long *) NULL ; + p = (Long *) NULL ; break ; case 9 : @@ -418,7 +416,7 @@ void mexFunction mexPrintf ("csymamdtest: stats not present\n") ; } result = 0 ; /* stats not present */ - stats = (UF_long *) NULL ; + stats = (Long *) NULL ; break ; case 13 : diff --git a/gtsam/3rdparty/CCOLAMD/Makefile b/gtsam/3rdparty/CCOLAMD/Makefile new file mode 100644 index 000000000..f04181d60 --- /dev/null +++ b/gtsam/3rdparty/CCOLAMD/Makefile @@ -0,0 +1,51 @@ +#------------------------------------------------------------------------------ +# CCOLAMD Makefile +#------------------------------------------------------------------------------ + +SUITESPARSE ?= $(realpath $(CURDIR)/..) +export SUITESPARSE + +default: all + +include ../SuiteSparse_config/SuiteSparse_config.mk + +demos: all + +# Compile all C code +all: + ( cd Lib ; $(MAKE) ) + ( cd Demo ; $(MAKE) ) + +# compile just the C-callable libraries (not Demos) +library: + ( cd Lib ; $(MAKE) ) + +# remove object files, but keep the compiled programs and library archives +clean: + ( cd Lib ; $(MAKE) clean ) + ( cd Demo ; $(MAKE) clean ) + ( cd MATLAB ; $(RM) $(CLEAN) ) + +# clean, and then remove compiled programs and library archives +purge: + ( cd Lib ; $(MAKE) purge ) + ( cd Demo ; $(MAKE) purge ) + ( cd MATLAB ; $(RM) $(CLEAN) ; $(RM) *.mex* ) + +distclean: purge + +# get ready for distribution +dist: purge + ( cd Demo ; $(MAKE) dist ) + +ccode: library + +lib: library + +# install CCOLAMD +install: + ( cd Lib ; $(MAKE) install ) + +# uninstall CCOLAMD +uninstall: + ( cd Lib ; $(MAKE) uninstall ) diff --git a/gtsam/3rdparty/CCOLAMD/README.txt b/gtsam/3rdparty/CCOLAMD/README.txt index ccc5a19a5..6d8f1ce83 100644 --- a/gtsam/3rdparty/CCOLAMD/README.txt +++ b/gtsam/3rdparty/CCOLAMD/README.txt @@ -1,8 +1,8 @@ CCOLAMD: constrained column approximate minimum degree ordering -Copyright (C) 2005-2011, Univ. of Florida. Authors: Timothy A. Davis, +Copyright (C) 2005-2016, Univ. of Florida. Authors: Timothy A. Davis, Sivasankaran Rajamanickam, and Stefan Larimore. Closely based on COLAMD by Davis, Stefan Larimore, in collaboration with Esmond Ng, and John Gilbert. -http://www.cise.ufl.edu/research/sparse +http://www.suitesparse.com ------------------------------------------------------------------------------- The CCOLAMD column approximate minimum degree ordering algorithm computes @@ -14,7 +14,8 @@ available as a MATLAB-callable function. It constructs a matrix M such that M'*M has the same pattern as A, and then uses CCOLAMD to compute a column ordering of M. -Requires UFconfig, in the ../UFconfig directory relative to this directory. +Requires SuiteSparse_config, in the ../SuiteSparse_config directory relative to +this directory. To compile and install the ccolamd m-files and mexFunctions, just cd to CCOLAMD/MATLAB and type ccolamd_install in the MATLAB command window. @@ -22,47 +23,27 @@ A short demo will run. Optionally, type ccolamd_test to run an extensive tests. Type "make" in Unix in the CCOLAMD directory to compile the C-callable library and to run a short demo. -If you have MATLAB 7.2 or earlier, you must first edit UFconfig/UFconfig.h to -remove the "-largeArrayDims" option from the MEX command (or just use -ccolamd_install.m inside MATLAB). - Other "make" targets: - make mex compiles MATLAB mexFunctions only - make libccolamd.a compiles a C-callable library containing ccolamd - make clean removes all files not in the distribution, except for - libccolamd.a + make library compiles a C-callable library containing ccolamd + make clean removes all files not in the distribution + but keeps the compiled libraries. make distclean removes all files not in the distribution + make install installs the library in /usr/local/lib and + /usr/local/include + make uninstall uninstalls the library from /usr/local/lib and + /usr/local/include To use ccolamd and csymamd within an application written in C, all you need are ccolamd.c and ccolamd.h, which are the C-callable ccolamd/csymamd codes. See ccolamd.c for more information on how to call ccolamd from a C program. It contains a complete description of the C-interface to CCOLAMD and CSYMAMD. - Copyright (c) 1998-2007 by the University of Florida. - All Rights Reserved. - Licensed under the GNU LESSER GENERAL PUBLIC LICENSE. +See CCOLAMD/Doc/License.txt for the license. ------------------------------------------------------------------------------- -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA - -------------------------------------------------------------------------------- - - Related papers: T. A. Davis and W. W. Hager, Rajamanickam, Multiple-rank updates @@ -86,21 +67,18 @@ Related papers: "An approximate minimum degree column ordering algorithm", S. I. Larimore, MS Thesis, Dept. of Computer and Information Science and Engineering, University of Florida, Gainesville, FL, - 1998. CISE Tech Report TR-98-016. Available at - ftp://ftp.cise.ufl.edu/cis/tech-reports/tr98/tr98-016.ps - via anonymous ftp. + 1998. CISE Tech Report TR-98-016. Approximate Deficiency for Ordering the Columns of a Matrix, J. L. Kern, Senior Thesis, Dept. of Computer and Information Science and Engineering, University of Florida, Gainesville, FL, - 1999. Available at http://www.cise.ufl.edu/~davis/Kern/kern.ps + 1999. Authors: Timothy A. Davis, Sivasankaran Rajamanickam, and Stefan Larimore. Closely based on COLAMD by Stefan I. Larimore and Timothy A. Davis, - University of Florida, in collaboration with John Gilbert, Xerox PARC - (now at UC Santa Barbara), and Esmong Ng, Lawrence Berkeley National - Laboratory (much of this work he did while at Oak Ridge National - Laboratory). + in collaboration with John Gilbert, Xerox PARC (now at UC Santa + Barbara), and Esmong Ng, Lawrence Berkeley National Laboratory (much of + this work he did while at Oak Ridge National Laboratory). CCOLAMD files: @@ -122,7 +100,6 @@ CCOLAMD files: ./Doc: ChangeLog change log - lesser.txt license ./Include: ccolamd.h include file @@ -147,4 +124,3 @@ CCOLAMD files: ./Source: ccolamd.c primary source code - ccolamd_global.c globally defined function pointers (malloc, free, ...) diff --git a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c index 1c35ffa41..9a08e3ea8 100644 --- a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c +++ b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c @@ -5,8 +5,6 @@ /* ---------------------------------------------------------------------------- * CCOLAMD, Copyright (C) Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* @@ -58,39 +56,13 @@ * COLAMD is also available under alternate licenses, contact T. Davis * for details. * - * This library is free software; you can redistribute it and/or - * modify it under the terms of the GNU Lesser General Public - * License as published by the Free Software Foundation; either - * version 2.1 of the License, or (at your option) any later version. - * - * This library is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * Lesser General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public - * License along with this library; if not, write to the Free Software - * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 - * USA - * - * Permission is hereby granted to use or copy this program under the - * terms of the GNU LGPL, provided that the Copyright, this License, - * and the Availability of the original version is retained on all copies. - * User documentation of any code that uses this code or any modified - * version of this code must cite the Copyright, this License, the - * Availability note, and "Used by permission." Permission to modify - * the code and to distribute modified code is granted, provided the - * Copyright, this License, and the Availability note are retained, - * and a notice that the code was modified is included. + * See CCOLAMD/Doc/License.txt for the license. * * Availability: * * The CCOLAMD/CSYMAMD library is available at * - * http://www.cise.ufl.edu/research/sparse/ccolamd/ - * - * This is the http://www.cise.ufl.edu/research/sparse/ccolamd/ccolamd.c - * file. + * http://www.suitesparse.com * * See the ChangeLog file for changes since Version 1.0. */ @@ -99,10 +71,10 @@ /* === Description of user-callable routines ================================ */ /* ========================================================================== */ -/* CCOLAMD includes both int and UF_long versions of all its routines. The - * description below is for the int version. For UF_long, all int arguments - * become UF_long integers. UF_long is normally defined as long, except for - * WIN64 */ +/* CCOLAMD includes both int and SuiteSparse_long versions of all its routines. + * The description below is for the int version. For SuiteSparse_long, all + * int arguments become SuiteSparse_long integers. SuiteSparse_long is + * normally defined as long, except for WIN64 */ /* ---------------------------------------------------------------------------- * ccolamd_recommended: @@ -112,8 +84,8 @@ * * #include "ccolamd.h" * size_t ccolamd_recommended (int nnz, int n_row, int n_col) ; - * size_t ccolamd_l_recommended (UF_long nnz, UF_long n_row, - * UF_long n_col) ; + * size_t ccolamd_l_recommended (SuiteSparse_long nnz, + * SuiteSparse_long n_row, SuiteSparse_long n_col) ; * * Purpose: * @@ -209,9 +181,12 @@ * double knobs [CCOLAMD_KNOBS], int stats [CCOLAMD_STATS], * int *cmember) ; * - * UF_long ccolamd_l (UF_long n_row, UF_long n_col, UF_long Alen, - * UF_long *A, UF_long *p, double knobs [CCOLAMD_KNOBS], - * UF_long stats [CCOLAMD_STATS], UF_long *cmember) ; + * SuiteSparse_long ccolamd_l (SuiteSparse_long n_row, + * SuiteSparse_long n_col, SuiteSparse_long Alen, + * SuiteSparse_long *A, SuiteSparse_long *p, + * double knobs [CCOLAMD_KNOBS], + * SuiteSparse_long stats [CCOLAMD_STATS], + * SuiteSparse_long *cmember) ; * * Purpose: * @@ -385,9 +360,7 @@ * * Example: * - * See - * http://www.cise.ufl.edu/research/sparse/ccolamd/ccolamd_example.c - * for a complete example. + * See ccolamd_example.c for a complete example. * * To order the columns of a 5-by-4 matrix with 11 nonzero entries in * the following nonzero pattern @@ -423,10 +396,12 @@ * void (*allocate) (size_t, size_t), void (*release) (void *), * int *cmember, int stype) ; * - * UF_long csymamd_l (UF_long n, UF_long *A, UF_long *p, UF_long *perm, - * double knobs [CCOLAMD_KNOBS], UF_long stats [CCOLAMD_STATS], - * void (*allocate) (size_t, size_t), void (*release) (void *), - * UF_long *cmember, UF_long stype) ; + * SuiteSparse_long csymamd_l (SuiteSparse_long n, + * SuiteSparse_long *A, SuiteSparse_long *p, + * SuiteSparse_long *perm, double knobs [CCOLAMD_KNOBS], + * SuiteSparse_long stats [CCOLAMD_STATS], void (*allocate) + * (size_t, size_t), void (*release) (void *), + * SuiteSparse_long *cmember, SuiteSparse_long stype) ; * * Purpose: * @@ -562,7 +537,7 @@ * * #include "ccolamd.h" * ccolamd_report (int stats [CCOLAMD_STATS]) ; - * ccolamd_l_report (UF_long stats [CCOLAMD_STATS]) ; + * ccolamd_l_report (SuiteSparse_long stats [CCOLAMD_STATS]) ; * * Purpose: * @@ -583,7 +558,7 @@ * * #include "ccolamd.h" * csymamd_report (int stats [CCOLAMD_STATS]) ; - * csymamd_l_report (UF_long stats [CCOLAMD_STATS]) ; + * csymamd_l_report (SuiteSparse_long stats [CCOLAMD_STATS]) ; * * Purpose: * @@ -617,12 +592,11 @@ #include "ccolamd.h" +#include #include #include #ifdef MATLAB_MEX_FILE -#include -typedef uint16_t char16_t; #include "mex.h" #include "matrix.h" #endif @@ -636,17 +610,14 @@ typedef uint16_t char16_t; #endif /* ========================================================================== */ -/* === int or UF_long ======================================================= */ +/* === int or SuiteSparse_long ============================================== */ /* ========================================================================== */ -/* define UF_long */ -#include "UFconfig.h" - #ifdef DLONG -#define Int UF_long -#define ID UF_long_id -#define Int_MAX UF_long_max +#define Int SuiteSparse_long +#define ID SuiteSparse_long_id +#define Int_MAX SuiteSparse_long_max #define CCOLAMD_recommended ccolamd_l_recommended #define CCOLAMD_set_defaults ccolamd_l_set_defaults @@ -811,9 +782,6 @@ typedef struct CColamd_Row_struct #define INDEX(i) (i) #endif -/* All output goes through the PRINTF macro. */ -#define PRINTF(params) { if (ccolamd_printf != NULL) (void) ccolamd_printf params ; } - /* ========================================================================== */ /* === Debugging prototypes and definitions ================================= */ @@ -827,11 +795,11 @@ typedef struct CColamd_Row_struct PRIVATE Int ccolamd_debug ; /* debug print statements */ -#define DEBUG0(params) { PRINTF (params) ; } -#define DEBUG1(params) { if (ccolamd_debug >= 1) PRINTF (params) ; } -#define DEBUG2(params) { if (ccolamd_debug >= 2) PRINTF (params) ; } -#define DEBUG3(params) { if (ccolamd_debug >= 3) PRINTF (params) ; } -#define DEBUG4(params) { if (ccolamd_debug >= 4) PRINTF (params) ; } +#define DEBUG0(params) { SUITESPARSE_PRINTF (params) ; } +#define DEBUG1(params) { if (ccolamd_debug >= 1) SUITESPARSE_PRINTF (params) ; } +#define DEBUG2(params) { if (ccolamd_debug >= 2) SUITESPARSE_PRINTF (params) ; } +#define DEBUG3(params) { if (ccolamd_debug >= 3) SUITESPARSE_PRINTF (params) ; } +#define DEBUG4(params) { if (ccolamd_debug >= 4) SUITESPARSE_PRINTF (params) ; } #ifdef MATLAB_MEX_FILE #define ASSERT(expression) (mxAssert ((expression), "")) @@ -3752,12 +3720,12 @@ PRIVATE void print_report Int i1, i2, i3 ; - PRINTF (("\n%s version %d.%d, %s: ", method, + SUITESPARSE_PRINTF (("\n%s version %d.%d, %s: ", method, CCOLAMD_MAIN_VERSION, CCOLAMD_SUB_VERSION, CCOLAMD_DATE)) ; if (!stats) { - PRINTF (("No statistics available.\n")) ; + SUITESPARSE_PRINTF (("No statistics available.\n")) ; return ; } @@ -3767,11 +3735,11 @@ PRIVATE void print_report if (stats [CCOLAMD_STATUS] >= 0) { - PRINTF(("OK. ")) ; + SUITESPARSE_PRINTF(("OK. ")) ; } else { - PRINTF(("ERROR. ")) ; + SUITESPARSE_PRINTF(("ERROR. ")) ; } switch (stats [CCOLAMD_STATUS]) @@ -3779,91 +3747,105 @@ PRIVATE void print_report case CCOLAMD_OK_BUT_JUMBLED: - PRINTF(("Matrix has unsorted or duplicate row indices.\n")) ; + SUITESPARSE_PRINTF(( + "Matrix has unsorted or duplicate row indices.\n")) ; - PRINTF(("%s: duplicate or out-of-order row indices: "ID"\n", - method, i3)) ; + SUITESPARSE_PRINTF(( + "%s: duplicate or out-of-order row indices: "ID"\n", + method, i3)) ; - PRINTF(("%s: last seen duplicate or out-of-order row: "ID"\n", - method, INDEX (i2))) ; + SUITESPARSE_PRINTF(( + "%s: last seen duplicate or out-of-order row: "ID"\n", + method, INDEX (i2))) ; - PRINTF(("%s: last seen in column: "ID"", - method, INDEX (i1))) ; + SUITESPARSE_PRINTF(( + "%s: last seen in column: "ID"", + method, INDEX (i1))) ; /* no break - fall through to next case instead */ case CCOLAMD_OK: - PRINTF(("\n")) ; + SUITESPARSE_PRINTF(("\n")) ; - PRINTF(("%s: number of dense or empty rows ignored: "ID"\n", - method, stats [CCOLAMD_DENSE_ROW])) ; + SUITESPARSE_PRINTF(( + "%s: number of dense or empty rows ignored: "ID"\n", + method, stats [CCOLAMD_DENSE_ROW])) ; - PRINTF(("%s: number of dense or empty columns ignored: "ID"\n", - method, stats [CCOLAMD_DENSE_COL])) ; + SUITESPARSE_PRINTF(( + "%s: number of dense or empty columns ignored: "ID"\n", + method, stats [CCOLAMD_DENSE_COL])) ; - PRINTF(("%s: number of garbage collections performed: "ID"\n", - method, stats [CCOLAMD_DEFRAG_COUNT])) ; + SUITESPARSE_PRINTF(( + "%s: number of garbage collections performed: "ID"\n", + method, stats [CCOLAMD_DEFRAG_COUNT])) ; break ; case CCOLAMD_ERROR_A_not_present: - PRINTF(("Array A (row indices of matrix) not present.\n")) ; + SUITESPARSE_PRINTF(( + "Array A (row indices of matrix) not present.\n")) ; break ; case CCOLAMD_ERROR_p_not_present: - PRINTF(("Array p (column pointers for matrix) not present.\n")) ; + SUITESPARSE_PRINTF(( + "Array p (column pointers for matrix) not present.\n")) ; break ; case CCOLAMD_ERROR_nrow_negative: - PRINTF(("Invalid number of rows ("ID").\n", i1)) ; + SUITESPARSE_PRINTF(("Invalid number of rows ("ID").\n", i1)) ; break ; case CCOLAMD_ERROR_ncol_negative: - PRINTF(("Invalid number of columns ("ID").\n", i1)) ; + SUITESPARSE_PRINTF(("Invalid number of columns ("ID").\n", i1)) ; break ; case CCOLAMD_ERROR_nnz_negative: - PRINTF(("Invalid number of nonzero entries ("ID").\n", i1)) ; + SUITESPARSE_PRINTF(( + "Invalid number of nonzero entries ("ID").\n", i1)) ; break ; case CCOLAMD_ERROR_p0_nonzero: - PRINTF(("Invalid column pointer, p [0] = "ID", must be 0.\n", i1)) ; + SUITESPARSE_PRINTF(( + "Invalid column pointer, p [0] = "ID", must be 0.\n", i1)) ; break ; case CCOLAMD_ERROR_A_too_small: - PRINTF(("Array A too small.\n")) ; - PRINTF((" Need Alen >= "ID", but given only Alen = "ID".\n", - i1, i2)) ; + SUITESPARSE_PRINTF(("Array A too small.\n")) ; + SUITESPARSE_PRINTF(( + " Need Alen >= "ID", but given only Alen = "ID".\n", + i1, i2)) ; break ; case CCOLAMD_ERROR_col_length_negative: - PRINTF(("Column "ID" has a negative number of entries ("ID").\n", - INDEX (i1), i2)) ; + SUITESPARSE_PRINTF(( + "Column "ID" has a negative number of entries ("ID").\n", + INDEX (i1), i2)) ; break ; case CCOLAMD_ERROR_row_index_out_of_bounds: - PRINTF(("Row index (row "ID") out of bounds ("ID" to "ID") in" - "column "ID".\n", INDEX (i2), INDEX (0), INDEX (i3-1), - INDEX (i1))) ; + SUITESPARSE_PRINTF(( + "Row index (row "ID") out of bounds ("ID" to "ID") in" + "column "ID".\n", INDEX (i2), INDEX (0), INDEX (i3-1), + INDEX (i1))) ; break ; case CCOLAMD_ERROR_out_of_memory: - PRINTF(("Out of memory.\n")) ; + SUITESPARSE_PRINTF(("Out of memory.\n")) ; break ; case CCOLAMD_ERROR_invalid_cmember: - PRINTF(("cmember invalid\n")) ; + SUITESPARSE_PRINTF(("cmember invalid\n")) ; break ; } } diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 8534a8d7e..b890bc4af 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -1,6 +1,6 @@ # install CCOLAMD headers install(FILES CCOLAMD/Include/ccolamd.h DESTINATION include/gtsam/3rdparty/CCOLAMD) -install(FILES UFconfig/UFconfig.h DESTINATION include/gtsam/3rdparty/UFconfig) +install(FILES SuiteSparse_config/SuiteSparse_config.h DESTINATION include/gtsam/3rdparty/SuiteSparse_config) if(NOT GTSAM_USE_SYSTEM_EIGEN) # Find plain .h files diff --git a/gtsam/3rdparty/SuiteSparse_config/Makefile b/gtsam/3rdparty/SuiteSparse_config/Makefile new file mode 100644 index 000000000..96db5772f --- /dev/null +++ b/gtsam/3rdparty/SuiteSparse_config/Makefile @@ -0,0 +1,70 @@ +#------------------------------------------------------------------------------- +# SuiteSparse_config Makefile +#------------------------------------------------------------------------------- + +SUITESPARSE ?= $(realpath $(CURDIR)/..) +export SUITESPARSE + +# version of SuiteSparse_config is also version of SuiteSparse meta-package +LIBRARY = libsuitesparseconfig +VERSION = 4.5.2 +SO_VERSION = 4 + +default: library + +include SuiteSparse_config.mk + +ccode: all + +all: library + +# compile and install in SuiteSparse/lib +library: $(AR_TARGET) + $(MAKE) install INSTALL=$(SUITESPARSE) + +OBJ = SuiteSparse_config.o + +SuiteSparse_config.o: SuiteSparse_config.c SuiteSparse_config.h + $(CC) $(CF) -c SuiteSparse_config.c + +$(AR_TARGET): $(OBJ) + $(ARCHIVE) $(AR_TARGET) SuiteSparse_config.o + $(RANLIB) $(AR_TARGET) + +distclean: purge + +purge: clean + ( cd xerbla ; $(MAKE) purge ) + - $(RM) -r $(PURGE) + +clean: + ( cd xerbla ; $(MAKE) clean ) + - $(RM) -r $(CLEAN) + +# install SuiteSparse_config +install: $(AR_TARGET) $(INSTALL_LIB)/$(SO_TARGET) + +$(INSTALL_LIB)/$(SO_TARGET): $(OBJ) + @mkdir -p $(INSTALL_LIB) + @mkdir -p $(INSTALL_INCLUDE) + @mkdir -p $(INSTALL_DOC) + $(CC) $(SO_OPTS) $^ -o $@ $(LDLIBS) + ( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_PLAIN) ) + ( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_MAIN) ) + $(CP) SuiteSparse_config.h $(INSTALL_INCLUDE) + $(CP) README.txt $(INSTALL_DOC)/SUITESPARSECONFIG_README.txt + chmod 755 $(INSTALL_LIB)/$(SO_TARGET) + chmod 755 $(INSTALL_LIB)/$(SO_PLAIN) + chmod 644 $(INSTALL_INCLUDE)/SuiteSparse_config.h + chmod 644 $(INSTALL_DOC)/SUITESPARSECONFIG_README.txt + +# uninstall SuiteSparse_config +uninstall: + $(RM) $(INSTALL_LIB)/$(SO_TARGET) + $(RM) $(INSTALL_LIB)/$(SO_PLAIN) + $(RM) $(INSTALL_LIB)/$(SO_MAIN) + $(RM) $(INSTALL_INCLUDE)/SuiteSparse_config.h + $(RM) $(INSTALL_DOC)/SUITESPARSECONFIG_README.txt + ( cd xerbla ; $(MAKE) uninstall ) + + diff --git a/gtsam/3rdparty/SuiteSparse_config/README.txt b/gtsam/3rdparty/SuiteSparse_config/README.txt new file mode 100644 index 000000000..a76a5fab6 --- /dev/null +++ b/gtsam/3rdparty/SuiteSparse_config/README.txt @@ -0,0 +1,51 @@ +SuiteSparse_config, 2016, Timothy A. Davis, http://www.suitesparse.com +(formerly the UFconfig package) + +This directory contains a default SuiteSparse_config.mk file. It tries to +detect your system (Linux, SunOS, or Mac), which compiler to use (icc or cc), +which BLAS and LAPACK library to use (OpenBLAS or MKL), and whether or not to +compile with CUDA. + +For alternatives, see the comments in the SuiteSparse_config.mk file. + +License: No licensing restrictions apply to this file or to the +SuiteSparse_config directory. + +-------------------------------------------------------------------------------- + +SuiteSparse_config contains configuration settings for all many of the software +packages that I develop or co-author. Note that older versions of some of +these packages do not require SuiteSparse_config. + + Package Description + ------- ----------- + AMD approximate minimum degree ordering + CAMD constrained AMD + COLAMD column approximate minimum degree ordering + CCOLAMD constrained approximate minimum degree ordering + UMFPACK sparse LU factorization, with the BLAS + CXSparse int/long/real/complex version of CSparse + CHOLMOD sparse Cholesky factorization, update/downdate + KLU sparse LU factorization, BLAS-free + BTF permutation to block triangular form + LDL concise sparse LDL' + LPDASA LP Dual Active Set Algorithm + RBio read/write files in Rutherford/Boeing format + SPQR sparse QR factorization (full name: SuiteSparseQR) + +SuiteSparse_config is not required by these packages: + + CSparse a Concise Sparse matrix package + MATLAB_Tools toolboxes for use in MATLAB + +In addition, the xerbla/ directory contains Fortan and C versions of the +BLAS/LAPACK xerbla routine, which is called when an invalid input is passed to +the BLAS or LAPACK. The xerbla provided here does not print any message, so +the entire Fortran I/O library does not need to be linked into a C application. +Most versions of the BLAS contain xerbla, but those from K. Goto do not. Use +this if you need too. + +If you edit this directory (SuiteSparse_config.mk in particular) then you +must do "make purge ; make" in the parent directory to recompile all of +SuiteSparse. Otherwise, the changes will not necessarily be applied. + diff --git a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.c b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.c new file mode 100644 index 000000000..b491539fe --- /dev/null +++ b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.c @@ -0,0 +1,531 @@ +/* ========================================================================== */ +/* === SuiteSparse_config =================================================== */ +/* ========================================================================== */ + +/* SuiteSparse configuration : memory manager and printf functions. */ + +/* Copyright (c) 2013, Timothy A. Davis. No licensing restrictions + * apply to this file or to the SuiteSparse_config directory. + * Author: Timothy A. Davis. + */ + +#include +#include + +#ifndef NPRINT +#include +#endif + +#ifdef MATLAB_MEX_FILE +#include "mex.h" +#include "matrix.h" +#endif + +#ifndef NULL +#define NULL ((void *) 0) +#endif + +#include "SuiteSparse_config.h" + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_config : a global extern struct */ +/* -------------------------------------------------------------------------- */ + +/* The SuiteSparse_config struct is available to all SuiteSparse functions and + to all applications that use those functions. It must be modified with + care, particularly in a multithreaded context. Normally, the application + will initialize this object once, via SuiteSparse_start, possibily followed + by application-specific modifications if the applications wants to use + alternative memory manager functions. + + The user can redefine these global pointers at run-time to change the + memory manager and printf function used by SuiteSparse. + + If -DNMALLOC is defined at compile-time, then no memory-manager is + specified. You must define them at run-time, after calling + SuiteSparse_start. + + If -DPRINT is defined a compile time, then printf is disabled, and + SuiteSparse will not use printf. + */ + +struct SuiteSparse_config_struct SuiteSparse_config = +{ + + /* memory management functions */ + #ifndef NMALLOC + #ifdef MATLAB_MEX_FILE + /* MATLAB mexFunction: */ + mxMalloc, mxCalloc, mxRealloc, mxFree, + #else + /* standard ANSI C: */ + malloc, calloc, realloc, free, + #endif + #else + /* no memory manager defined; you must define one at run-time: */ + NULL, NULL, NULL, NULL, + #endif + + /* printf function */ + #ifndef NPRINT + #ifdef MATLAB_MEX_FILE + /* MATLAB mexFunction: */ + mexPrintf, + #else + /* standard ANSI C: */ + printf, + #endif + #else + /* printf is disabled */ + NULL, + #endif + + SuiteSparse_hypot, + SuiteSparse_divcomplex + +} ; + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_start */ +/* -------------------------------------------------------------------------- */ + +/* All applications that use SuiteSparse should call SuiteSparse_start prior + to using any SuiteSparse function. Only a single thread should call this + function, in a multithreaded application. Currently, this function is + optional, since all this function currently does is to set the four memory + function pointers to NULL (which tells SuiteSparse to use the default + functions). In a multi- threaded application, only a single thread should + call this function. + + Future releases of SuiteSparse might enforce a requirement that + SuiteSparse_start be called prior to calling any SuiteSparse function. + */ + +void SuiteSparse_start ( void ) +{ + + /* memory management functions */ + #ifndef NMALLOC + #ifdef MATLAB_MEX_FILE + /* MATLAB mexFunction: */ + SuiteSparse_config.malloc_func = mxMalloc ; + SuiteSparse_config.calloc_func = mxCalloc ; + SuiteSparse_config.realloc_func = mxRealloc ; + SuiteSparse_config.free_func = mxFree ; + #else + /* standard ANSI C: */ + SuiteSparse_config.malloc_func = malloc ; + SuiteSparse_config.calloc_func = calloc ; + SuiteSparse_config.realloc_func = realloc ; + SuiteSparse_config.free_func = free ; + #endif + #else + /* no memory manager defined; you must define one after calling + SuiteSparse_start */ + SuiteSparse_config.malloc_func = NULL ; + SuiteSparse_config.calloc_func = NULL ; + SuiteSparse_config.realloc_func = NULL ; + SuiteSparse_config.free_func = NULL ; + #endif + + /* printf function */ + #ifndef NPRINT + #ifdef MATLAB_MEX_FILE + /* MATLAB mexFunction: */ + SuiteSparse_config.printf_func = mexPrintf ; + #else + /* standard ANSI C: */ + SuiteSparse_config.printf_func = printf ; + #endif + #else + /* printf is disabled */ + SuiteSparse_config.printf_func = NULL ; + #endif + + /* math functions */ + SuiteSparse_config.hypot_func = SuiteSparse_hypot ; + SuiteSparse_config.divcomplex_func = SuiteSparse_divcomplex ; +} + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_finish */ +/* -------------------------------------------------------------------------- */ + +/* This currently does nothing, but in the future, applications should call + SuiteSparse_start before calling any SuiteSparse function, and then + SuiteSparse_finish after calling the last SuiteSparse function, just before + exiting. In a multithreaded application, only a single thread should call + this function. + + Future releases of SuiteSparse might use this function for any + SuiteSparse-wide cleanup operations or finalization of statistics. + */ + +void SuiteSparse_finish ( void ) +{ + /* do nothing */ ; +} + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_malloc: malloc wrapper */ +/* -------------------------------------------------------------------------- */ + +void *SuiteSparse_malloc /* pointer to allocated block of memory */ +( + size_t nitems, /* number of items to malloc */ + size_t size_of_item /* sizeof each item */ +) +{ + void *p ; + size_t size ; + if (nitems < 1) nitems = 1 ; + if (size_of_item < 1) size_of_item = 1 ; + size = nitems * size_of_item ; + + if (size != ((double) nitems) * size_of_item) + { + /* size_t overflow */ + p = NULL ; + } + else + { + p = (void *) (SuiteSparse_config.malloc_func) (size) ; + } + return (p) ; +} + + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_calloc: calloc wrapper */ +/* -------------------------------------------------------------------------- */ + +void *SuiteSparse_calloc /* pointer to allocated block of memory */ +( + size_t nitems, /* number of items to calloc */ + size_t size_of_item /* sizeof each item */ +) +{ + void *p ; + size_t size ; + if (nitems < 1) nitems = 1 ; + if (size_of_item < 1) size_of_item = 1 ; + size = nitems * size_of_item ; + + if (size != ((double) nitems) * size_of_item) + { + /* size_t overflow */ + p = NULL ; + } + else + { + p = (void *) (SuiteSparse_config.calloc_func) (nitems, size_of_item) ; + } + return (p) ; +} + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_realloc: realloc wrapper */ +/* -------------------------------------------------------------------------- */ + +/* If p is non-NULL on input, it points to a previously allocated object of + size nitems_old * size_of_item. The object is reallocated to be of size + nitems_new * size_of_item. If p is NULL on input, then a new object of that + size is allocated. On success, a pointer to the new object is returned, + and ok is returned as 1. If the allocation fails, ok is set to 0 and a + pointer to the old (unmodified) object is returned. + */ + +void *SuiteSparse_realloc /* pointer to reallocated block of memory, or + to original block if the realloc failed. */ +( + size_t nitems_new, /* new number of items in the object */ + size_t nitems_old, /* old number of items in the object */ + size_t size_of_item, /* sizeof each item */ + void *p, /* old object to reallocate */ + int *ok /* 1 if successful, 0 otherwise */ +) +{ + size_t size ; + if (nitems_old < 1) nitems_old = 1 ; + if (nitems_new < 1) nitems_new = 1 ; + if (size_of_item < 1) size_of_item = 1 ; + size = nitems_new * size_of_item ; + + if (size != ((double) nitems_new) * size_of_item) + { + /* size_t overflow */ + (*ok) = 0 ; + } + else if (p == NULL) + { + /* a fresh object is being allocated */ + p = SuiteSparse_malloc (nitems_new, size_of_item) ; + (*ok) = (p != NULL) ; + } + else if (nitems_old == nitems_new) + { + /* the object does not change; do nothing */ + (*ok) = 1 ; + } + else + { + /* change the size of the object from nitems_old to nitems_new */ + void *pnew ; + pnew = (void *) (SuiteSparse_config.realloc_func) (p, size) ; + if (pnew == NULL) + { + if (nitems_new < nitems_old) + { + /* the attempt to reduce the size of the block failed, but + the old block is unchanged. So pretend to succeed. */ + (*ok) = 1 ; + } + else + { + /* out of memory */ + (*ok) = 0 ; + } + } + else + { + /* success */ + p = pnew ; + (*ok) = 1 ; + } + } + return (p) ; +} + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_free: free wrapper */ +/* -------------------------------------------------------------------------- */ + +void *SuiteSparse_free /* always returns NULL */ +( + void *p /* block to free */ +) +{ + if (p) + { + (SuiteSparse_config.free_func) (p) ; + } + return (NULL) ; +} + + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_tic: return current wall clock time */ +/* -------------------------------------------------------------------------- */ + +/* Returns the number of seconds (tic [0]) and nanoseconds (tic [1]) since some + * unspecified but fixed time in the past. If no timer is installed, zero is + * returned. A scalar double precision value for 'tic' could be used, but this + * might cause loss of precision because clock_getttime returns the time from + * some distant time in the past. Thus, an array of size 2 is used. + * + * The timer is enabled by default. To disable the timer, compile with + * -DNTIMER. If enabled on a POSIX C 1993 system, the timer requires linking + * with the -lrt library. + * + * example: + * + * double tic [2], r, s, t ; + * SuiteSparse_tic (tic) ; // start the timer + * // do some work A + * t = SuiteSparse_toc (tic) ; // t is time for work A, in seconds + * // do some work B + * s = SuiteSparse_toc (tic) ; // s is time for work A and B, in seconds + * SuiteSparse_tic (tic) ; // restart the timer + * // do some work C + * r = SuiteSparse_toc (tic) ; // s is time for work C, in seconds + * + * A double array of size 2 is used so that this routine can be more easily + * ported to non-POSIX systems. The caller does not rely on the POSIX + * include file. + */ + +#ifdef SUITESPARSE_TIMER_ENABLED + +#include + +void SuiteSparse_tic +( + double tic [2] /* output, contents undefined on input */ +) +{ + /* POSIX C 1993 timer, requires -librt */ + struct timespec t ; + clock_gettime (CLOCK_MONOTONIC, &t) ; + tic [0] = (double) (t.tv_sec) ; + tic [1] = (double) (t.tv_nsec) ; +} + +#else + +void SuiteSparse_tic +( + double tic [2] /* output, contents undefined on input */ +) +{ + /* no timer installed */ + tic [0] = 0 ; + tic [1] = 0 ; +} + +#endif + + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_toc: return time since last tic */ +/* -------------------------------------------------------------------------- */ + +/* Assuming SuiteSparse_tic is accurate to the nanosecond, this function is + * accurate down to the nanosecond for 2^53 nanoseconds since the last call to + * SuiteSparse_tic, which is sufficient for SuiteSparse (about 104 days). If + * additional accuracy is required, the caller can use two calls to + * SuiteSparse_tic and do the calculations differently. + */ + +double SuiteSparse_toc /* returns time in seconds since last tic */ +( + double tic [2] /* input, not modified from last call to SuiteSparse_tic */ +) +{ + double toc [2] ; + SuiteSparse_tic (toc) ; + return ((toc [0] - tic [0]) + 1e-9 * (toc [1] - tic [1])) ; +} + + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_time: return current wallclock time in seconds */ +/* -------------------------------------------------------------------------- */ + +/* This function might not be accurate down to the nanosecond. */ + +double SuiteSparse_time /* returns current wall clock time in seconds */ +( + void +) +{ + double toc [2] ; + SuiteSparse_tic (toc) ; + return (toc [0] + 1e-9 * toc [1]) ; +} + + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_version: return the current version of SuiteSparse */ +/* -------------------------------------------------------------------------- */ + +int SuiteSparse_version +( + int version [3] +) +{ + if (version != NULL) + { + version [0] = SUITESPARSE_MAIN_VERSION ; + version [1] = SUITESPARSE_SUB_VERSION ; + version [2] = SUITESPARSE_SUBSUB_VERSION ; + } + return (SUITESPARSE_VERSION) ; +} + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_hypot */ +/* -------------------------------------------------------------------------- */ + +/* There is an equivalent routine called hypot in , which conforms + * to ANSI C99. However, SuiteSparse does not assume that ANSI C99 is + * available. You can use the ANSI C99 hypot routine with: + * + * #include + *i SuiteSparse_config.hypot_func = hypot ; + * + * Default value of the SuiteSparse_config.hypot_func pointer is + * SuiteSparse_hypot, defined below. + * + * s = hypot (x,y) computes s = sqrt (x*x + y*y) but does so more accurately. + * The NaN cases for the double relops x >= y and x+y == x are safely ignored. + * + * Source: Algorithm 312, "Absolute value and square root of a complex number," + * P. Friedland, Comm. ACM, vol 10, no 10, October 1967, page 665. + */ + +double SuiteSparse_hypot (double x, double y) +{ + double s, r ; + x = fabs (x) ; + y = fabs (y) ; + if (x >= y) + { + if (x + y == x) + { + s = x ; + } + else + { + r = y / x ; + s = x * sqrt (1.0 + r*r) ; + } + } + else + { + if (y + x == y) + { + s = y ; + } + else + { + r = x / y ; + s = y * sqrt (1.0 + r*r) ; + } + } + return (s) ; +} + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_divcomplex */ +/* -------------------------------------------------------------------------- */ + +/* c = a/b where c, a, and b are complex. The real and imaginary parts are + * passed as separate arguments to this routine. The NaN case is ignored + * for the double relop br >= bi. Returns 1 if the denominator is zero, + * 0 otherwise. + * + * This uses ACM Algo 116, by R. L. Smith, 1962, which tries to avoid + * underflow and overflow. + * + * c can be the same variable as a or b. + * + * Default value of the SuiteSparse_config.divcomplex_func pointer is + * SuiteSparse_divcomplex. + */ + +int SuiteSparse_divcomplex +( + double ar, double ai, /* real and imaginary parts of a */ + double br, double bi, /* real and imaginary parts of b */ + double *cr, double *ci /* real and imaginary parts of c */ +) +{ + double tr, ti, r, den ; + if (fabs (br) >= fabs (bi)) + { + r = bi / br ; + den = br + r * bi ; + tr = (ar + ai * r) / den ; + ti = (ai - ar * r) / den ; + } + else + { + r = br / bi ; + den = r * br + bi ; + tr = (ar * r + ai) / den ; + ti = (ai * r - ar) / den ; + } + *cr = tr ; + *ci = ti ; + return (den == 0.) ; +} diff --git a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.h b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.h new file mode 100644 index 000000000..49296fc5a --- /dev/null +++ b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.h @@ -0,0 +1,248 @@ +/* ========================================================================== */ +/* === SuiteSparse_config =================================================== */ +/* ========================================================================== */ + +/* Configuration file for SuiteSparse: a Suite of Sparse matrix packages + * (AMD, COLAMD, CCOLAMD, CAMD, CHOLMOD, UMFPACK, CXSparse, and others). + * + * SuiteSparse_config.h provides the definition of the long integer. On most + * systems, a C program can be compiled in LP64 mode, in which long's and + * pointers are both 64-bits, and int's are 32-bits. Windows 64, however, uses + * the LLP64 model, in which int's and long's are 32-bits, and long long's and + * pointers are 64-bits. + * + * SuiteSparse packages that include long integer versions are + * intended for the LP64 mode. However, as a workaround for Windows 64 + * (and perhaps other systems), the long integer can be redefined. + * + * If _WIN64 is defined, then the __int64 type is used instead of long. + * + * The long integer can also be defined at compile time. For example, this + * could be added to SuiteSparse_config.mk: + * + * CFLAGS = -O -D'SuiteSparse_long=long long' \ + * -D'SuiteSparse_long_max=9223372036854775801' -D'SuiteSparse_long_idd="lld"' + * + * This file defines SuiteSparse_long as either long (on all but _WIN64) or + * __int64 on Windows 64. The intent is that a SuiteSparse_long is always a + * 64-bit integer in a 64-bit code. ptrdiff_t might be a better choice than + * long; it is always the same size as a pointer. + * + * This file also defines the SUITESPARSE_VERSION and related definitions. + * + * Copyright (c) 2012, Timothy A. Davis. No licensing restrictions apply + * to this file or to the SuiteSparse_config directory. + * Author: Timothy A. Davis. + */ + +#ifndef SUITESPARSE_CONFIG_H +#define SUITESPARSE_CONFIG_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +/* ========================================================================== */ +/* === SuiteSparse_long ===================================================== */ +/* ========================================================================== */ + +#ifndef SuiteSparse_long + +#ifdef _WIN64 + +#define SuiteSparse_long __int64 +#define SuiteSparse_long_max _I64_MAX +#define SuiteSparse_long_idd "I64d" + +#else + +#define SuiteSparse_long long +#define SuiteSparse_long_max LONG_MAX +#define SuiteSparse_long_idd "ld" + +#endif +#define SuiteSparse_long_id "%" SuiteSparse_long_idd +#endif + +/* ========================================================================== */ +/* === SuiteSparse_config parameters and functions ========================== */ +/* ========================================================================== */ + +/* SuiteSparse-wide parameters are placed in this struct. It is meant to be + an extern, globally-accessible struct. It is not meant to be updated + frequently by multiple threads. Rather, if an application needs to modify + SuiteSparse_config, it should do it once at the beginning of the application, + before multiple threads are launched. + + The intent of these function pointers is that they not be used in your + application directly, except to assign them to the desired user-provided + functions. Rather, you should use the + */ + +struct SuiteSparse_config_struct +{ + void *(*malloc_func) (size_t) ; /* pointer to malloc */ + void *(*calloc_func) (size_t, size_t) ; /* pointer to calloc */ + void *(*realloc_func) (void *, size_t) ; /* pointer to realloc */ + void (*free_func) (void *) ; /* pointer to free */ + int (*printf_func) (const char *, ...) ; /* pointer to printf */ + double (*hypot_func) (double, double) ; /* pointer to hypot */ + int (*divcomplex_func) (double, double, double, double, double *, double *); +} ; + +extern struct SuiteSparse_config_struct SuiteSparse_config ; + +void SuiteSparse_start ( void ) ; /* called to start SuiteSparse */ + +void SuiteSparse_finish ( void ) ; /* called to finish SuiteSparse */ + +void *SuiteSparse_malloc /* pointer to allocated block of memory */ +( + size_t nitems, /* number of items to malloc (>=1 is enforced) */ + size_t size_of_item /* sizeof each item */ +) ; + +void *SuiteSparse_calloc /* pointer to allocated block of memory */ +( + size_t nitems, /* number of items to calloc (>=1 is enforced) */ + size_t size_of_item /* sizeof each item */ +) ; + +void *SuiteSparse_realloc /* pointer to reallocated block of memory, or + to original block if the realloc failed. */ +( + size_t nitems_new, /* new number of items in the object */ + size_t nitems_old, /* old number of items in the object */ + size_t size_of_item, /* sizeof each item */ + void *p, /* old object to reallocate */ + int *ok /* 1 if successful, 0 otherwise */ +) ; + +void *SuiteSparse_free /* always returns NULL */ +( + void *p /* block to free */ +) ; + +void SuiteSparse_tic /* start the timer */ +( + double tic [2] /* output, contents undefined on input */ +) ; + +double SuiteSparse_toc /* return time in seconds since last tic */ +( + double tic [2] /* input: from last call to SuiteSparse_tic */ +) ; + +double SuiteSparse_time /* returns current wall clock time in seconds */ +( + void +) ; + +/* returns sqrt (x^2 + y^2), computed reliably */ +double SuiteSparse_hypot (double x, double y) ; + +/* complex division of c = a/b */ +int SuiteSparse_divcomplex +( + double ar, double ai, /* real and imaginary parts of a */ + double br, double bi, /* real and imaginary parts of b */ + double *cr, double *ci /* real and imaginary parts of c */ +) ; + +/* determine which timer to use, if any */ +#ifndef NTIMER +#ifdef _POSIX_C_SOURCE +#if _POSIX_C_SOURCE >= 199309L +#define SUITESPARSE_TIMER_ENABLED +#endif +#endif +#endif + +/* SuiteSparse printf macro */ +#define SUITESPARSE_PRINTF(params) \ +{ \ + if (SuiteSparse_config.printf_func != NULL) \ + { \ + (void) (SuiteSparse_config.printf_func) params ; \ + } \ +} + +/* ========================================================================== */ +/* === SuiteSparse version ================================================== */ +/* ========================================================================== */ + +/* SuiteSparse is not a package itself, but a collection of packages, some of + * which must be used together (UMFPACK requires AMD, CHOLMOD requires AMD, + * COLAMD, CAMD, and CCOLAMD, etc). A version number is provided here for the + * collection itself. The versions of packages within each version of + * SuiteSparse are meant to work together. Combining one package from one + * version of SuiteSparse, with another package from another version of + * SuiteSparse, may or may not work. + * + * SuiteSparse contains the following packages: + * + * SuiteSparse_config version 4.5.2 (version always the same as SuiteSparse) + * AMD version 2.4.5 + * BTF version 1.2.5 + * CAMD version 2.4.5 + * CCOLAMD version 2.9.5 + * CHOLMOD version 3.0.10 + * COLAMD version 2.9.5 + * CSparse version 3.1.8 + * CXSparse version 3.1.8 + * GPUQREngine version 1.0.4 + * KLU version 1.3.7 + * LDL version 2.2.5 + * RBio version 2.2.5 + * SPQR version 2.0.6 + * SuiteSparse_GPURuntime version 1.0.4 + * UMFPACK version 5.7.5 + * MATLAB_Tools various packages & M-files + * xerbla version 1.0.2 + * + * Other package dependencies: + * BLAS required by CHOLMOD and UMFPACK + * LAPACK required by CHOLMOD + * METIS 5.1.0 required by CHOLMOD (optional) and KLU (optional) + * CUBLAS, CUDART NVIDIA libraries required by CHOLMOD and SPQR when + * they are compiled with GPU acceleration. + */ + + +int SuiteSparse_version /* returns SUITESPARSE_VERSION */ +( + /* output, not defined on input. Not used if NULL. Returns + the three version codes in version [0..2]: + version [0] is SUITESPARSE_MAIN_VERSION + version [1] is SUITESPARSE_SUB_VERSION + version [2] is SUITESPARSE_SUBSUB_VERSION + */ + int version [3] +) ; + +/* Versions prior to 4.2.0 do not have the above function. The following + code fragment will work with any version of SuiteSparse: + + #ifdef SUITESPARSE_HAS_VERSION_FUNCTION + v = SuiteSparse_version (NULL) ; + #else + v = SUITESPARSE_VERSION ; + #endif +*/ +#define SUITESPARSE_HAS_VERSION_FUNCTION + +#define SUITESPARSE_DATE "Apr 1, 2016" +#define SUITESPARSE_VER_CODE(main,sub) ((main) * 1000 + (sub)) +#define SUITESPARSE_MAIN_VERSION 4 +#define SUITESPARSE_SUB_VERSION 5 +#define SUITESPARSE_SUBSUB_VERSION 2 +#define SUITESPARSE_VERSION \ + SUITESPARSE_VER_CODE(SUITESPARSE_MAIN_VERSION,SUITESPARSE_SUB_VERSION) + +#ifdef __cplusplus +} +#endif +#endif diff --git a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.mk b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.mk new file mode 100644 index 000000000..40ad6b9af --- /dev/null +++ b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.mk @@ -0,0 +1,600 @@ +#=============================================================================== +# SuiteSparse_config.mk: common configuration file for the SuiteSparse +#=============================================================================== + +# This file contains all configuration settings for all packages in SuiteSparse, +# except for CSparse (which is stand-alone) and the packages in MATLAB_Tools. + +SUITESPARSE_VERSION = 4.5.2 + +#=============================================================================== +# Options you can change without editing this file: +#=============================================================================== + + # To list the options you can modify at the 'make' command line, type + # 'make config', which also lists their default values. You can then + # change them with 'make OPTION=value'. For example, to use an INSTALL + # path of /my/path, and to use your own BLAS and LAPACK libraries, do: + # + # make install INSTALL=/my/path BLAS=-lmyblas LAPACK=-lmylapackgoeshere + # + # which will install the package into /my/path/lib and /my/path/include, + # and use -lmyblas -lmylapackgoes here when building the demo program. + +#=============================================================================== +# Defaults for any system +#=============================================================================== + + #--------------------------------------------------------------------------- + # SuiteSparse root directory + #--------------------------------------------------------------------------- + + # Most Makefiles are in SuiteSparse/Pkg/Lib or SuiteSparse/Pkg/Demo, so + # the top-level of SuiteSparse is in ../.. unless otherwise specified. + # This is true for all but the SuiteSparse_config package. + SUITESPARSE ?= $(realpath $(CURDIR)/../..) + + #--------------------------------------------------------------------------- + # installation location + #--------------------------------------------------------------------------- + + # For "make install" and "make uninstall", the default location is + # SuiteSparse/lib, SuiteSparse/include, and + # SuiteSparse/share/doc/suitesparse-x.y.z + # If you do this: + # make install INSTALL=/usr/local + # then the libraries are installed in /usr/local/lib, include files in + # /usr/local/include, and documentation in + # /usr/local/share/doc/suitesparse-x.y.z. + # You can instead specify the install location of each of these 3 components + # separately, via (for example): + # make install INSTALL_LIB=/yada/mylibs INSTALL_INCLUDE=/yoda/myinc \ + # INSTALL_DOC=/solo/mydox + # which puts the libraries in /yada/mylibs, include files in /yoda/myinc, + # and documentation in /solo/mydox. + INSTALL ?= $(SUITESPARSE) + INSTALL_LIB ?= $(INSTALL)/lib + INSTALL_INCLUDE ?= $(INSTALL)/include + INSTALL_DOC ?= $(INSTALL)/share/doc/suitesparse-$(SUITESPARSE_VERSION) + + #--------------------------------------------------------------------------- + # optimization level + #--------------------------------------------------------------------------- + + OPTIMIZATION ?= -O3 + + #--------------------------------------------------------------------------- + # statement coverage for */Tcov + #--------------------------------------------------------------------------- + + ifeq ($(TCOV),yes) + # Each package has a */Tcov directory for extensive testing, including + # statement coverage. The Tcov tests require Linux and gcc, and use + # the vanilla BLAS. For those tests, the packages use 'make TCOV=yes', + # which overrides the following settings: + MKLROOT = + AUTOCC = no + CC = gcc + CXX = g++ + BLAS = -lrefblas -lgfortran -lstdc++ + LAPACK = -llapack + CFLAGS += --coverage + OPTIMIZATION = -g + LDFLAGS += --coverage + endif + + #--------------------------------------------------------------------------- + # CFLAGS for the C/C++ compiler + #--------------------------------------------------------------------------- + + # The CF macro is used by SuiteSparse Makefiles as a combination of + # CFLAGS, CPPFLAGS, TARGET_ARCH, and system-dependent settings. + CF ?= $(CFLAGS) $(CPPFLAGS) $(TARGET_ARCH) $(OPTIMIZATION) -fexceptions -fPIC + + #--------------------------------------------------------------------------- + # OpenMP is used in CHOLMOD + #--------------------------------------------------------------------------- + + # with gcc, enable OpenMP directives via -fopenmp + # This is not supported on Darwin, so this string is cleared, below. + CFOPENMP ?= -fopenmp + + #--------------------------------------------------------------------------- + # compiler + #--------------------------------------------------------------------------- + + # By default, look for the Intel compilers. If present, they are used + # instead of $(CC), $(CXX), and $(F77). To disable this feature and + # use the $(CC), $(CXX), and $(F77) compilers, use 'make AUTOCC=no' + + AUTOCC ?= yes + + ifneq ($(AUTOCC),no) + ifneq ($(shell which icc 2>/dev/null),) + # use the Intel icc compiler for C codes, and -qopenmp for OpenMP + CC = icc -D_GNU_SOURCE + CXX = $(CC) + CFOPENMP = -qopenmp -I$(MKLROOT)/include + endif + ifneq ($(shell which ifort 2>/dev/null),) + # use the Intel ifort compiler for Fortran codes + F77 = ifort + endif + endif + + #--------------------------------------------------------------------------- + # code formatting (for Tcov only) + #--------------------------------------------------------------------------- + + PRETTY ?= grep -v "^\#" | indent -bl -nce -bli0 -i4 -sob -l120 + + #--------------------------------------------------------------------------- + # required libraries + #--------------------------------------------------------------------------- + + # SuiteSparse requires the BLAS, LAPACK, and -lm (Math) libraries. + # It places its shared *.so libraries in SuiteSparse/lib. + # Linux also requires the -lrt library (see below) + LDLIBS ?= -lm + LDFLAGS += -L$(INSTALL_LIB) + + # See http://www.openblas.net for a recent and freely available optimzed + # BLAS. LAPACK is at http://www.netlib.org/lapack/ . You can use the + # standard Fortran LAPACK along with OpenBLAS to obtain very good + # performance. This script can also detect if the Intel MKL BLAS is + # installed. + + LAPACK ?= -llapack + + ifndef BLAS + ifdef MKLROOT + # use the Intel MKL for BLAS and LAPACK + # using static linking: + # BLAS = -Wl,--start-group \ + # $(MKLROOT)/lib/intel64/libmkl_intel_lp64.a \ + # $(MKLROOT)/lib/intel64/libmkl_core.a \ + # $(MKLROOT)/lib/intel64/libmkl_intel_thread.a \ + # -Wl,--end-group -lpthread -lm + # using dynamic linking: + BLAS = -lmkl_intel_lp64 -lmkl_core -lmkl_intel_thread -lpthread -lm + LAPACK = + else + # use the OpenBLAS at http://www.openblas.net + BLAS = -lopenblas + endif + endif + + # For ACML, use this instead: + # make BLAS='-lacml -lgfortran' + + #--------------------------------------------------------------------------- + # shell commands + #--------------------------------------------------------------------------- + + # ranlib, and ar, for generating libraries. If you don't need ranlib, + # just change it to RANLAB = echo + RANLIB ?= ranlib + ARCHIVE ?= $(AR) $(ARFLAGS) + CP ?= cp -f + MV ?= mv -f + + #--------------------------------------------------------------------------- + # Fortran compiler (not required for 'make' or 'make library') + #--------------------------------------------------------------------------- + + # A Fortran compiler is optional. Only required for the optional Fortran + # interfaces to AMD and UMFPACK. Not needed by 'make' or 'make install' + F77 ?= gfortran + F77FLAGS ?= $(FFLAGS) $(OPTIMIZATION) + + #--------------------------------------------------------------------------- + # NVIDIA CUDA configuration for CHOLMOD and SPQR + #--------------------------------------------------------------------------- + + # CUDA is detected automatically, and used if found. To disable CUDA, + # use CUDA=no + + ifneq ($(CUDA),no) + CUDA_PATH = $(shell which nvcc 2>/dev/null | sed "s/\/bin\/nvcc//") + endif + + ifeq ($(wildcard $(CUDA_PATH)),) + # CUDA is not present + CUDA_PATH = + GPU_BLAS_PATH = + GPU_CONFIG = + CUDART_LIB = + CUBLAS_LIB = + CUDA_INC_PATH = + CUDA_INC = + NVCC = echo + NVCCFLAGS = + else + # with CUDA for CHOLMOD and SPQR + GPU_BLAS_PATH = $(CUDA_PATH) + # GPU_CONFIG must include -DGPU_BLAS to compile SuiteSparse for the + # GPU. You can add additional GPU-related flags to it as well. + # with 4 cores (default): + GPU_CONFIG = -DGPU_BLAS + # For example, to compile CHOLMOD for 10 CPU cores when using the GPU: + # GPU_CONFIG = -DGPU_BLAS -DCHOLMOD_OMP_NUM_THREADS=10 + CUDART_LIB = $(CUDA_PATH)/lib64/libcudart.so + CUBLAS_LIB = $(CUDA_PATH)/lib64/libcublas.so + CUDA_INC_PATH = $(CUDA_PATH)/include/ + CUDA_INC = -I$(CUDA_INC_PATH) + NVCC = $(CUDA_PATH)/bin/nvcc + NVCCFLAGS = -Xcompiler -fPIC -O3 \ + -gencode=arch=compute_20,code=sm_20 \ + -gencode=arch=compute_30,code=sm_30 \ + -gencode=arch=compute_35,code=sm_35 \ + -gencode=arch=compute_50,code=sm_50 \ + -gencode=arch=compute_50,code=compute_50 + endif + + #--------------------------------------------------------------------------- + # UMFPACK configuration: + #--------------------------------------------------------------------------- + + # Configuration for UMFPACK. See UMFPACK/Source/umf_config.h for details. + # + # -DNBLAS do not use the BLAS. UMFPACK will be very slow. + # -D'LONGBLAS=long' or -DLONGBLAS='long long' defines the integers used by + # LAPACK and the BLAS (defaults to 'int') + # -DNSUNPERF do not use the Sun Perf. Library on Solaris + # -DNRECIPROCAL do not multiply by the reciprocal + # -DNO_DIVIDE_BY_ZERO do not divide by zero + # -DNCHOLMOD do not use CHOLMOD as a ordering method. If -DNCHOLMOD is + # included in UMFPACK_CONFIG, then UMFPACK does not rely on + # CHOLMOD, CAMD, CCOLAMD, COLAMD, and METIS. + + UMFPACK_CONFIG ?= + + # For example, uncomment this line to compile UMFPACK without CHOLMOD: + # UMFPACK_CONFIG = -DNCHOLMOD + # or use 'make UMFPACK_CONFIG=-DNCHOLMOD' + + #--------------------------------------------------------------------------- + # CHOLMOD configuration + #--------------------------------------------------------------------------- + + # CHOLMOD Library Modules, which appear in -lcholmod + # Core requires: none + # Check requires: Core + # Cholesky requires: Core, AMD, COLAMD. optional: Partition, Supernodal + # MatrixOps requires: Core + # Modify requires: Core + # Partition requires: Core, CCOLAMD, METIS. optional: Cholesky + # Supernodal requires: Core, BLAS, LAPACK + # + # CHOLMOD test/demo Modules (these do not appear in -lcholmod): + # Tcov requires: Core, Check, Cholesky, MatrixOps, Modify, Supernodal + # optional: Partition + # Valgrind same as Tcov + # Demo requires: Core, Check, Cholesky, MatrixOps, Supernodal + # optional: Partition + # + # Configuration flags: + # -DNCHECK do not include the Check module. + # -DNCHOLESKY do not include the Cholesky module. + # -DNPARTITION do not include the Partition module. + # also do not include METIS. + # -DNCAMD do not use CAMD & CCOLAMD in Parition Module. + # -DNMATRIXOPS do not include the MatrixOps module. + # -DNMODIFY do not include the Modify module. + # -DNSUPERNODAL do not include the Supernodal module. + # + # -DNPRINT do not print anything. + # -D'LONGBLAS=long' or -DLONGBLAS='long long' defines the integers used by + # LAPACK and the BLAS (defaults to 'int') + # -DNSUNPERF for Solaris only. If defined, do not use the Sun + # Performance Library + # -DGPU_BLAS enable the use of the CUDA BLAS + + CHOLMOD_CONFIG ?= $(GPU_CONFIG) + + #--------------------------------------------------------------------------- + # SuiteSparseQR configuration: + #--------------------------------------------------------------------------- + + # The SuiteSparseQR library can be compiled with the following options: + # + # -DNPARTITION do not include the CHOLMOD partition module + # -DNEXPERT do not include the functions in SuiteSparseQR_expert.cpp + # -DHAVE_TBB enable the use of Intel's Threading Building Blocks + # -DGPU_BLAS enable the use of the CUDA BLAS + + SPQR_CONFIG ?= $(GPU_CONFIG) + + # to compile with Intel's TBB, use TBB=-ltbb SPQR_CONFIG=-DHAVE_TBB + TBB ?= + + # TODO: this *mk file should auto-detect the presence of Intel's TBB, + # and set the compiler flags accordingly. + +#=============================================================================== +# System-dependent configurations +#=============================================================================== + + #--------------------------------------------------------------------------- + # determine what system we are on + #--------------------------------------------------------------------------- + + # To disable these auto configurations, use 'make UNAME=custom' + + ifndef UNAME + ifeq ($(OS),Windows_NT) + # Cygwin Make on Windows has an $(OS) variable, but not uname. + # Note that this option is untested. + UNAME = Windows + else + # Linux and Darwin (Mac OSX) have been tested. + UNAME := $(shell uname) + endif + endif + + #--------------------------------------------------------------------------- + # Linux + #--------------------------------------------------------------------------- + + ifeq ($(UNAME),Linux) + # add the realtime library, librt, and SuiteSparse/lib + LDLIBS += -lrt -Wl,-rpath=$(INSTALL_LIB) + endif + + #--------------------------------------------------------------------------- + # Mac + #--------------------------------------------------------------------------- + + ifeq ($(UNAME), Darwin) + # To compile on the Mac, you must install Xcode. Then do this at the + # command line in the Terminal, before doing 'make': + # xcode-select --install + CF += -fno-common + BLAS = -framework Accelerate + LAPACK = -framework Accelerate + # OpenMP is not yet supported by default in clang + CFOPENMP = + endif + + #--------------------------------------------------------------------------- + # Solaris + #--------------------------------------------------------------------------- + + ifeq ($(UNAME), SunOS) + # Using the Sun compiler and the Sun Performance Library + # This hasn't been tested recently. + # I leave it here in case you need it. It likely needs updating. + CF += -fast -KPIC -xc99=%none -xlibmieee -xlibmil -m64 -Xc + F77FLAGS = -O -fast -KPIC -dalign -xlibmil -m64 + BLAS = -xlic_lib=sunperf + LAPACK = + # Using the GCC compiler and the reference BLAS + ## CC = gcc + ## CXX = g++ + ## MAKE = gmake + ## BLAS = -lrefblas -lgfortran + ## LAPACK = -llapack + endif + + #--------------------------------------------------------------------------- + # IBM AIX + #--------------------------------------------------------------------------- + + ifeq ($(UNAME), AIX) + # hasn't been tested for a very long time... + # I leave it here in case you need it. It likely needs updating. + CF += -O4 -qipa -qmaxmem=16384 -q64 -qproto -DBLAS_NO_UNDERSCORE + F77FLAGS = -O4 -qipa -qmaxmem=16384 -q64 + BLAS = -lessl + LAPACK = + endif + +#=============================================================================== +# finalize the CF compiler flags +#=============================================================================== + + CF += $(CFOPENMP) + +#=============================================================================== +# internal configuration +#=============================================================================== + + # The user should not have to change these definitions, and they are + # not displayed by 'make config' + + #--------------------------------------------------------------------------- + # for removing files not in the distribution + #--------------------------------------------------------------------------- + + # remove object files, but keep compiled libraries via 'make clean' + CLEAN = *.o *.obj *.ln *.bb *.bbg *.da *.tcov *.gcov gmon.out *.bak *.d \ + *.gcda *.gcno *.aux *.bbl *.blg *.log *.toc *.dvi *.lof *.lot + + # also remove compiled libraries, via 'make distclean' + PURGE = *.so* *.a *.dll *.dylib *.dSYM + + # location of TCOV test output + TCOV_TMP ?= /tmp + +#=============================================================================== +# Building the shared and static libraries +#=============================================================================== + +# How to build/install shared and static libraries for Mac and Linux/Unix. +# This assumes that LIBRARY and VERSION have already been defined by the +# Makefile that includes this file. + +SO_OPTS = $(LDFLAGS) + +ifeq ($(UNAME),Windows) + # Cygwin Make on Windows (untested) + AR_TARGET = $(LIBRARY).lib + SO_PLAIN = $(LIBRARY).dll + SO_MAIN = $(LIBRARY).$(SO_VERSION).dll + SO_TARGET = $(LIBRARY).$(VERSION).dll + SO_INSTALL_NAME = echo +else + # Mac or Linux/Unix + AR_TARGET = $(LIBRARY).a + ifeq ($(UNAME),Darwin) + # Mac + SO_PLAIN = $(LIBRARY).dylib + SO_MAIN = $(LIBRARY).$(SO_VERSION).dylib + SO_TARGET = $(LIBRARY).$(VERSION).dylib + SO_OPTS += -dynamiclib -compatibility_version $(SO_VERSION) \ + -current_version $(VERSION) \ + -shared -undefined dynamic_lookup + # When a Mac *.dylib file is moved, this command is required + # to change its internal name to match its location in the filesystem: + SO_INSTALL_NAME = install_name_tool -id + else + # Linux and other variants of Unix + SO_PLAIN = $(LIBRARY).so + SO_MAIN = $(LIBRARY).so.$(SO_VERSION) + SO_TARGET = $(LIBRARY).so.$(VERSION) + SO_OPTS += -shared -Wl,-soname -Wl,$(SO_MAIN) -Wl,--no-undefined + # Linux/Unix *.so files can be moved without modification: + SO_INSTALL_NAME = echo + endif +endif + +#=============================================================================== +# Configure CHOLMOD/Partition module with METIS, CAMD, and CCOLAMD +#=============================================================================== + +# By default, SuiteSparse uses METIS 5.1.0 in the SuiteSparse/metis-5.1.0 +# directory. SuiteSparse's interface to METIS is only through the +# SuiteSparse/CHOLMOD/Partition module, which also requires SuiteSparse/CAMD +# and SuiteSparse/CCOLAMD. +# +# If you wish to use your own pre-installed copy of METIS, use the MY_METIS_LIB +# and MY_METIS_INC options passed to 'make'. For example: +# make MY_METIS_LIB=-lmetis +# make MY_METIS_LIB=/home/myself/mylibraries/libmetis.so +# make MY_METIS_LIB='-L/home/myself/mylibraries -lmetis' +# If you need to tell the compiler where to find the metis.h include file, +# then add MY_METIS_INC=/home/myself/metis-5.1.0/include as well, which points +# to the directory containing metis.h. If metis.h is already installed in +# a location known to the compiler (/usr/local/include/metis.h for example) +# then you do not need to add MY_METIS_INC. + +I_WITH_PARTITION = +LIB_WITH_PARTITION = +CONFIG_PARTITION = -DNPARTITION -DNCAMD +# check if CAMD/CCOLAMD and METIS are requested and available +ifeq (,$(findstring -DNCAMD, $(CHOLMOD_CONFIG))) + # CAMD and CCOLAMD are requested. See if they are available in + # SuiteSparse/CAMD and SuiteSparse/CCOLAMD + ifneq (, $(wildcard $(SUITESPARSE)/CAMD)) + ifneq (, $(wildcard $(SUITESPARSE)/CCOLAMD)) + # CAMD and CCOLAMD are requested and available + LIB_WITH_PARTITION = -lccolamd -lcamd + I_WITH_PARTITION = -I$(SUITESPARSE)/CCOLAMD/Include -I$(SUITESPARSE)/CAMD/Include + CONFIG_PARTITION = -DNPARTITION + # check if METIS is requested and available + ifeq (,$(findstring -DNPARTITION, $(CHOLMOD_CONFIG))) + # METIS is requested. See if it is available. + ifneq (,$(MY_METIS_LIB)) + # METIS 5.1.0 is provided elsewhere, and we are not using + # SuiteSparse/metis-5.1.0. To do so, we link with + # $(MY_METIS_LIB) and add the -I$(MY_METIS_INC) option for + # the compiler. The latter can be empty if you have METIS + # installed in a place where the compiler can find the + # metis.h include file by itself without any -I option + # (/usr/local/include/metis.h for example). + LIB_WITH_PARTITION += $(MY_METIS_LIB) + ifneq (,$(MY_METIS_INC)) + I_WITH_PARTITION += -I$(MY_METIS_INC) + endif + CONFIG_PARTITION = + else + # see if METIS is in SuiteSparse/metis-5.1.0 + ifneq (, $(wildcard $(SUITESPARSE)/metis-5.1.0)) + # SuiteSparse/metis5.1.0 is available + ifeq ($(UNAME), Darwin) + LIB_WITH_PARTITION += $(SUITESPARSE)/lib/libmetis.dylib + else + LIB_WITH_PARTITION += -lmetis + endif + I_WITH_PARTITION += -I$(SUITESPARSE)/metis-5.1.0/include + CONFIG_PARTITION = + endif + endif + endif + endif + endif +endif + +#=============================================================================== +# display configuration +#=============================================================================== + +ifeq ($(LIBRARY),) + # placeholders, for 'make config' in the top-level SuiteSparse + LIBRARY=PackageNameWillGoHere + VERSION=x.y.z + SO_VERSION=x +endif + +# 'make config' lists the primary installation options +config: + @echo ' ' + @echo '----------------------------------------------------------------' + @echo 'SuiteSparse package compilation options:' + @echo '----------------------------------------------------------------' + @echo ' ' + @echo 'SuiteSparse Version: ' '$(SUITESPARSE_VERSION)' + @echo 'SuiteSparse top folder: ' '$(SUITESPARSE)' + @echo 'Package: LIBRARY= ' '$(LIBRARY)' + @echo 'Version: VERSION= ' '$(VERSION)' + @echo 'SO version: SO_VERSION= ' '$(SO_VERSION)' + @echo 'System: UNAME= ' '$(UNAME)' + @echo 'Install directory: INSTALL= ' '$(INSTALL)' + @echo 'Install libraries in: INSTALL_LIB= ' '$(INSTALL_LIB)' + @echo 'Install include files in: INSTALL_INCLUDE=' '$(INSTALL_INCLUDE)' + @echo 'Install documentation in: INSTALL_DOC= ' '$(INSTALL_DOC)' + @echo 'Optimization level: OPTIMIZATION= ' '$(OPTIMIZATION)' + @echo 'BLAS library: BLAS= ' '$(BLAS)' + @echo 'LAPACK library: LAPACK= ' '$(LAPACK)' + @echo 'Intel TBB library: TBB= ' '$(TBB)' + @echo 'Other libraries: LDLIBS= ' '$(LDLIBS)' + @echo 'static library: AR_TARGET= ' '$(AR_TARGET)' + @echo 'shared library (full): SO_TARGET= ' '$(SO_TARGET)' + @echo 'shared library (main): SO_MAIN= ' '$(SO_MAIN)' + @echo 'shared library (short): SO_PLAIN= ' '$(SO_PLAIN)' + @echo 'shared library options: SO_OPTS= ' '$(SO_OPTS)' + @echo 'shared library name tool: SO_INSTALL_NAME=' '$(SO_INSTALL_NAME)' + @echo 'ranlib, for static libs: RANLIB= ' '$(RANLIB)' + @echo 'static library command: ARCHIVE= ' '$(ARCHIVE)' + @echo 'copy file: CP= ' '$(CP)' + @echo 'move file: MV= ' '$(MV)' + @echo 'remove file: RM= ' '$(RM)' + @echo 'pretty (for Tcov tests): PRETTY= ' '$(PRETTY)' + @echo 'C compiler: CC= ' '$(CC)' + @echo 'C++ compiler: CXX= ' '$(CXX)' + @echo 'CUDA compiler: NVCC= ' '$(NVCC)' + @echo 'CUDA root directory: CUDA_PATH= ' '$(CUDA_PATH)' + @echo 'OpenMP flags: CFOPENMP= ' '$(CFOPENMP)' + @echo 'C/C++ compiler flags: CF= ' '$(CF)' + @echo 'LD flags: LDFLAGS= ' '$(LDFLAGS)' + @echo 'Fortran compiler: F77= ' '$(F77)' + @echo 'Fortran flags: F77FLAGS= ' '$(F77FLAGS)' + @echo 'Intel MKL root: MKLROOT= ' '$(MKLROOT)' + @echo 'Auto detect Intel icc: AUTOCC= ' '$(AUTOCC)' + @echo 'UMFPACK config: UMFPACK_CONFIG= ' '$(UMFPACK_CONFIG)' + @echo 'CHOLMOD config: CHOLMOD_CONFIG= ' '$(CHOLMOD_CONFIG)' + @echo 'SuiteSparseQR config: SPQR_CONFIG= ' '$(SPQR_CONFIG)' + @echo 'CUDA library: CUDART_LIB= ' '$(CUDART_LIB)' + @echo 'CUBLAS library: CUBLAS_LIB= ' '$(CUBLAS_LIB)' + @echo 'METIS and CHOLMOD/Partition configuration:' + @echo 'Your METIS library: MY_METIS_LIB= ' '$(MY_METIS_LIB)' + @echo 'Your metis.h is in: MY_METIS_INC= ' '$(MY_METIS_INC)' + @echo 'METIS is used via the CHOLMOD/Partition module, configured as follows.' + @echo 'If the next line has -DNPARTITION then METIS will not be used:' + @echo 'CHOLMOD Partition config: ' '$(CONFIG_PARTITION)' + @echo 'CHOLMOD Partition libs: ' '$(LIB_WITH_PARTITION)' + @echo 'CHOLMOD Partition include:' '$(I_WITH_PARTITION)' +ifeq ($(TCOV),yes) + @echo 'TCOV=yes, for extensive testing only (gcc, g++, vanilla BLAS)' +endif + diff --git a/gtsam/3rdparty/SuiteSparse_config/xerbla/Makefile b/gtsam/3rdparty/SuiteSparse_config/xerbla/Makefile new file mode 100644 index 000000000..420c50e88 --- /dev/null +++ b/gtsam/3rdparty/SuiteSparse_config/xerbla/Makefile @@ -0,0 +1,73 @@ +# Makefile for null-output xerbla, both C and Fortran versions. +# By default, the C version (libcerbla.a and *.so) is compiled and installed. +# Set the USE_FORTRAN option to 1 to create the Fortran instead (libxerbla): + USE_FORTRAN = 0 +# USE_FORTRAN = 1 + +VERSION = 1.0.2 +SO_VERSION = 1 + +default: library + +# compile and install in SuiteSparse/lib +library: + $(MAKE) install INSTALL=$(SUITESPARSE) + +all: library + +ifeq ($(USE_FORTRAN),0) + LIBRARY = libcerbla +else + LIBRARY = libxerbla +endif + +include ../SuiteSparse_config.mk + +ifeq ($(USE_FORTRAN),0) + COMPILE = $(CC) $(CF) -c xerbla.c + DEPENDS = xerbla.c xerbla.h +else + COMPILE = $(F77) $(F77FLAGS) -c xerbla.f + DEPENDS = xerbla.f +endif + +ccode: all + +fortran: all + +$(AR_TARGET): $(DEPENDS) + $(COMPILE) + $(ARCHIVE) $(AR_TARGET) xerbla.o + - $(RANLIB) $(AR_TARGET) + - $(RM) xerbla.o + +# install libcerbla / libxerbla +install: $(AR_TARGET) $(INSTALL_LIB)/$(SO_TARGET) + +$(INSTALL_LIB)/$(SO_TARGET): $(DEPENDS) + @mkdir -p $(INSTALL_LIB) + @mkdir -p $(INSTALL_INCLUDE) + @mkdir -p $(INSTALL_DOC) + $(COMPILE) + $(CC) $(SO_OPTS) xerbla.o -o $@ + - $(RM) xerbla.o + ( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_PLAIN) ) + ( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_MAIN) ) + $(CP) xerbla.h $(INSTALL_INCLUDE) + chmod 755 $(INSTALL_LIB)/$(SO_TARGET) + chmod 644 $(INSTALL_INCLUDE)/xerbla.h + +# uninstall libcerbla / libxerbla +uninstall: + $(RM) $(INSTALL_LIB)/$(SO_TARGET) + $(RM) $(INSTALL_LIB)/$(SO_PLAIN) + $(RM) $(INSTALL_INCLUDE)/xerbla.h + +distclean: purge + +purge: clean + - $(RM) -r $(PURGE) + +clean: + - $(RM) -r $(CLEAN) + diff --git a/gtsam/3rdparty/UFconfig/xerbla/xerbla.c b/gtsam/3rdparty/SuiteSparse_config/xerbla/xerbla.c similarity index 100% rename from gtsam/3rdparty/UFconfig/xerbla/xerbla.c rename to gtsam/3rdparty/SuiteSparse_config/xerbla/xerbla.c diff --git a/gtsam/3rdparty/UFconfig/xerbla/xerbla.f b/gtsam/3rdparty/SuiteSparse_config/xerbla/xerbla.f similarity index 100% rename from gtsam/3rdparty/UFconfig/xerbla/xerbla.f rename to gtsam/3rdparty/SuiteSparse_config/xerbla/xerbla.f diff --git a/gtsam/3rdparty/UFconfig/xerbla/xerbla.h b/gtsam/3rdparty/SuiteSparse_config/xerbla/xerbla.h similarity index 100% rename from gtsam/3rdparty/UFconfig/xerbla/xerbla.h rename to gtsam/3rdparty/SuiteSparse_config/xerbla/xerbla.h diff --git a/gtsam/3rdparty/UFconfig/README.txt b/gtsam/3rdparty/UFconfig/README.txt deleted file mode 100644 index d9edc239a..000000000 --- a/gtsam/3rdparty/UFconfig/README.txt +++ /dev/null @@ -1,35 +0,0 @@ -UFconfig contains configuration settings for all many of the software packages -that I develop or co-author. Note that older versions of some of these packages -do not require UFconfig. - - Package Description - ------- ----------- - AMD approximate minimum degree ordering - CAMD constrained AMD - COLAMD column approximate minimum degree ordering - CCOLAMD constrained approximate minimum degree ordering - UMFPACK sparse LU factorization, with the BLAS - CXSparse int/long/real/complex version of CSparse - CHOLMOD sparse Cholesky factorization, update/downdate - KLU sparse LU factorization, BLAS-free - BTF permutation to block triangular form - LDL concise sparse LDL' - LPDASA LP Dual Active Set Algorithm - SuiteSparseQR sparse QR factorization - -UFconfig is not required by: - - CSparse a Concise Sparse matrix package - RBio read/write files in Rutherford/Boeing format - UFcollection tools for managing the UF Sparse Matrix Collection - LINFACTOR simple m-file to show how to use LU and CHOL to solve Ax=b - MESHND 2D and 3D mesh generation and nested dissection ordering - MATLAB_Tools misc collection of m-files - SSMULT sparse matrix times sparse matrix, for use in MATLAB - -In addition, the xerbla/ directory contains Fortan and C versions of the -BLAS/LAPACK xerbla routine, which is called when an invalid input is passed to -the BLAS or LAPACK. The xerbla provided here does not print any message, so -the entire Fortran I/O library does not need to be linked into a C application. -Most versions of the BLAS contain xerbla, but those from K. Goto do not. Use -this if you need too. diff --git a/gtsam/3rdparty/UFconfig/UFconfig.c b/gtsam/3rdparty/UFconfig/UFconfig.c deleted file mode 100644 index 8b8d45ef7..000000000 --- a/gtsam/3rdparty/UFconfig/UFconfig.c +++ /dev/null @@ -1,71 +0,0 @@ -/* ========================================================================== */ -/* === UFconfig ============================================================= */ -/* ========================================================================== */ - -/* Copyright (c) 2009, University of Florida. No licensing restrictions - * apply to this file or to the UFconfig directory. Author: Timothy A. Davis. - */ - -#include "UFconfig.h" - -/* -------------------------------------------------------------------------- */ -/* UFmalloc: malloc wrapper */ -/* -------------------------------------------------------------------------- */ - -void *UFmalloc /* pointer to allocated block of memory */ -( - size_t nitems, /* number of items to malloc (>=1 is enforced) */ - size_t size_of_item, /* sizeof each item */ - int *ok, /* TRUE if successful, FALSE otherwise */ - UFconfig *config /* SuiteSparse-wide configuration */ -) -{ - void *p ; - if (nitems < 1) nitems = 1 ; - if (nitems * size_of_item != ((double) nitems) * size_of_item) - { - /* Int overflow */ - *ok = 0 ; - return (NULL) ; - } - if (!config || config->malloc_memory == NULL) - { - /* use malloc by default */ - p = (void *) malloc (nitems * size_of_item) ; - } - else - { - /* use the pointer to malloc in the config */ - p = (void *) (config->malloc_memory) (nitems * size_of_item) ; - } - *ok = (p != NULL) ; - return (p) ; -} - - -/* -------------------------------------------------------------------------- */ -/* UFfree: free wrapper */ -/* -------------------------------------------------------------------------- */ - -void *UFfree /* always returns NULL */ -( - void *p, /* block to free */ - UFconfig *config /* SuiteSparse-wide configuration */ -) -{ - if (p) - { - if (!config || config->free_memory == NULL) - { - /* use free by default */ - free (p) ; - } - else - { - /* use the pointer to free in the config */ - (config->free_memory) (p) ; - } - } - return (NULL) ; -} - diff --git a/gtsam/3rdparty/UFconfig/UFconfig.h b/gtsam/3rdparty/UFconfig/UFconfig.h deleted file mode 100644 index 795f5668c..000000000 --- a/gtsam/3rdparty/UFconfig/UFconfig.h +++ /dev/null @@ -1,152 +0,0 @@ -/* ========================================================================== */ -/* === UFconfig.h =========================================================== */ -/* ========================================================================== */ - -/* Configuration file for SuiteSparse: a Suite of Sparse matrix packages - * (AMD, COLAMD, CCOLAMD, CAMD, CHOLMOD, UMFPACK, CXSparse, and others). - * - * UFconfig.h provides the definition of the long integer. On most systems, - * a C program can be compiled in LP64 mode, in which long's and pointers are - * both 64-bits, and int's are 32-bits. Windows 64, however, uses the LLP64 - * model, in which int's and long's are 32-bits, and long long's and pointers - * are 64-bits. - * - * SuiteSparse packages that include long integer versions are - * intended for the LP64 mode. However, as a workaround for Windows 64 - * (and perhaps other systems), the long integer can be redefined. - * - * If _WIN64 is defined, then the __int64 type is used instead of long. - * - * The long integer can also be defined at compile time. For example, this - * could be added to UFconfig.mk: - * - * CFLAGS = -O -D'UF_long=long long' -D'UF_long_max=9223372036854775801' \ - * -D'UF_long_idd="lld"' - * - * This file defines UF_long as either long (on all but _WIN64) or - * __int64 on Windows 64. The intent is that a UF_long is always a 64-bit - * integer in a 64-bit code. ptrdiff_t might be a better choice than long; - * it is always the same size as a pointer. - * - * This file also defines the SUITESPARSE_VERSION and related definitions. - * - * Copyright (c) 2007, University of Florida. No licensing restrictions - * apply to this file or to the UFconfig directory. Author: Timothy A. Davis. - */ - -#ifndef _UFCONFIG_H -#define _UFCONFIG_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include - -/* ========================================================================== */ -/* === UF_long ============================================================== */ -/* ========================================================================== */ - -#ifndef UF_long - -#ifdef _WIN64 - -#define UF_long __int64 -#define UF_long_max _I64_MAX -#define UF_long_idd "I64d" - -#else - -#define UF_long long -#define UF_long_max LONG_MAX -#define UF_long_idd "ld" - -#endif -#define UF_long_id "%" UF_long_idd -#endif - -/* ========================================================================== */ -/* === UFconfig parameters and functions ==================================== */ -/* ========================================================================== */ - -/* SuiteSparse-wide parameters will be placed in this struct. So far, they - are only used by RBio. */ - -typedef struct UFconfig_struct -{ - void *(*malloc_memory) (size_t) ; /* pointer to malloc */ - void *(*realloc_memory) (void *, size_t) ; /* pointer to realloc */ - void (*free_memory) (void *) ; /* pointer to free */ - void *(*calloc_memory) (size_t, size_t) ; /* pointer to calloc */ - -} UFconfig ; - -void *UFmalloc /* pointer to allocated block of memory */ -( - size_t nitems, /* number of items to malloc (>=1 is enforced) */ - size_t size_of_item, /* sizeof each item */ - int *ok, /* TRUE if successful, FALSE otherwise */ - UFconfig *config /* SuiteSparse-wide configuration */ -) ; - -void *UFfree /* always returns NULL */ -( - void *p, /* block to free */ - UFconfig *config /* SuiteSparse-wide configuration */ -) ; - - -/* ========================================================================== */ -/* === SuiteSparse version ================================================== */ -/* ========================================================================== */ - -/* SuiteSparse is not a package itself, but a collection of packages, some of - * which must be used together (UMFPACK requires AMD, CHOLMOD requires AMD, - * COLAMD, CAMD, and CCOLAMD, etc). A version number is provided here for the - * collection itself. The versions of packages within each version of - * SuiteSparse are meant to work together. Combining one packge from one - * version of SuiteSparse, with another package from another version of - * SuiteSparse, may or may not work. - * - * SuiteSparse Version 3.6.1 contains the following packages: - * - * AMD version 2.2.2 - * BTF version 1.1.2 - * CAMD version 2.2.2 - * CCOLAMD version 2.7.3 - * CHOLMOD version 1.7.3 - * COLAMD version 2.7.3 - * CSparse version 2.2.5 - * CSparse3 version 3.0.1 - * CXSparse version 2.2.5 - * KLU version 1.1.2 - * LDL version 2.0.3 - * RBio version 2.0.1 - * SPQR version 1.2.2 (also called SuiteSparseQR) - * UFcollection version 1.5.0 - * UFconfig version number is the same as SuiteSparse - * UMFPACK version 5.5.1 - * LINFACTOR version 1.1.0 - * MESHND version 1.1.1 - * SSMULT version 2.0.2 - * MATLAB_Tools no specific version number - * - * Other package dependencies: - * BLAS required by CHOLMOD and UMFPACK - * LAPACK required by CHOLMOD - * METIS 4.0.1 required by CHOLMOD (optional) and KLU (optional) - */ - -#define SUITESPARSE_DATE "May 10, 2011" -#define SUITESPARSE_VER_CODE(main,sub) ((main) * 1000 + (sub)) -#define SUITESPARSE_MAIN_VERSION 3 -#define SUITESPARSE_SUB_VERSION 6 -#define SUITESPARSE_SUBSUB_VERSION 1 -#define SUITESPARSE_VERSION \ - SUITESPARSE_VER_CODE(SUITESPARSE_MAIN_VERSION,SUITESPARSE_SUB_VERSION) - -#ifdef __cplusplus -} -#endif -#endif diff --git a/gtsam/3rdparty/UFconfig/UFconfig.mk b/gtsam/3rdparty/UFconfig/UFconfig.mk deleted file mode 100644 index 60c951b6a..000000000 --- a/gtsam/3rdparty/UFconfig/UFconfig.mk +++ /dev/null @@ -1,386 +0,0 @@ -#=============================================================================== -# UFconfig.mk: common configuration file for the SuiteSparse -#=============================================================================== - -# This file contains all configuration settings for all packages authored or -# co-authored by Tim Davis at the University of Florida: -# -# Package Version Description -# ------- ------- ----------- -# AMD 1.2 or later approximate minimum degree ordering -# COLAMD 2.4 or later column approximate minimum degree ordering -# CCOLAMD 1.0 or later constrained column approximate minimum degree ordering -# CAMD any constrained approximate minimum degree ordering -# UMFPACK 4.5 or later sparse LU factorization, with the BLAS -# CHOLMOD any sparse Cholesky factorization, update/downdate -# KLU 0.8 or later sparse LU factorization, BLAS-free -# BTF 0.8 or later permutation to block triangular form -# LDL 1.2 or later concise sparse LDL' -# LPDASA any linear program solve (dual active set algorithm) -# CXSparse any extended version of CSparse (int/long, real/complex) -# SuiteSparseQR any sparse QR factorization -# -# The UFconfig directory and the above packages should all appear in a single -# directory, in order for the Makefile's within each package to find this file. -# -# To enable an option of the form "# OPTION = ...", edit this file and -# delete the "#" in the first column of the option you wish to use. - -#------------------------------------------------------------------------------ -# Generic configuration -#------------------------------------------------------------------------------ - -# C compiler and compiler flags: These will normally not give you optimal -# performance. You should select the optimization parameters that are best -# for your system. On Linux, use "CFLAGS = -O3 -fexceptions" for example. -CC = cc -CFLAGS = -O3 -fexceptions - -# C++ compiler (also uses CFLAGS) -CPLUSPLUS = g++ - -# ranlib, and ar, for generating libraries -RANLIB = ranlib -AR = ar cr - -# copy, delete, and rename a file -CP = cp -f -RM = rm -f -MV = mv -f - -# Fortran compiler (not normally required) -F77 = f77 -F77FLAGS = -O -F77LIB = - -# C and Fortran libraries -LIB = -lm - -# For compiling MATLAB mexFunctions (MATLAB 7.5 or later) -MEX = mex -O -largeArrayDims -lmwlapack -lmwblas - -# For compiling MATLAB mexFunctions (MATLAB 7.3 and 7.4) -# MEX = mex -O -largeArrayDims -lmwlapack - -# For MATLAB 7.2 or earlier, you must use one of these options: -# MEX = mex -O -lmwlapack -# MEX = mex -O - -# Which version of MAKE you are using (default is "make") -# MAKE = make -# MAKE = gmake - -# For "make install" -INSTALL_LIB = /usr/local/lib -INSTALL_INCLUDE = /usr/local/include - -#------------------------------------------------------------------------------ -# BLAS and LAPACK configuration: -#------------------------------------------------------------------------------ - -# UMFPACK and CHOLMOD both require the BLAS. CHOLMOD also requires LAPACK. -# See Kazushige Goto's BLAS at http://www.cs.utexas.edu/users/flame/goto/ or -# http://www.tacc.utexas.edu/~kgoto/ for the best BLAS to use with CHOLMOD. -# LAPACK is at http://www.netlib.org/lapack/ . You can use the standard -# Fortran LAPACK along with Goto's BLAS to obtain very good performance. -# CHOLMOD gets a peak numeric factorization rate of 3.6 Gflops on a 3.2 GHz -# Pentium 4 (512K cache, 4GB main memory) with the Goto BLAS, and 6 Gflops -# on a 2.5Ghz dual-core AMD Opteron. - -# These settings will probably not work, since there is no fixed convention for -# naming the BLAS and LAPACK library (*.a or *.so) files. - -# This is probably slow ... it might connect to the Standard Reference BLAS: -BLAS = -lblas -lgfortran -LAPACK = -llapack - -# NOTE: this next option for the "Goto BLAS" has nothing to do with a "goto" -# statement. Rather, the Goto BLAS is written by Dr. Kazushige Goto. -# Using the Goto BLAS: -# BLAS = -lgoto -lgfortran -lgfortranbegin - -# Using non-optimized versions: -# BLAS = -lblas_plain -lgfortran -lgfortranbegin -# LAPACK = -llapack_plain - -# BLAS = -lblas_plain -lgfortran -lgfortranbegin -# LAPACK = -llapack - -# The BLAS might not contain xerbla, an error-handling routine for LAPACK and -# the BLAS. Also, the standard xerbla requires the Fortran I/O library, and -# stops the application program if an error occurs. A C version of xerbla -# distributed with this software (UFconfig/xerbla/libcerbla.a) includes a -# Fortran-callable xerbla routine that prints nothing and does not stop the -# application program. This is optional. -# XERBLA = ../../UFconfig/xerbla/libcerbla.a - -# If you wish to use the XERBLA in LAPACK and/or the BLAS instead, -# use this option: -XERBLA = - -# If you wish to use the Fortran UFconfig/xerbla/xerbla.f instead, use this: -# XERBLA = ../../UFconfig/xerbla/libxerbla.a - -#------------------------------------------------------------------------------ -# METIS, optionally used by CHOLMOD -#------------------------------------------------------------------------------ - -# If you do not have METIS, or do not wish to use it in CHOLMOD, you must -# compile CHOLMOD with the -DNPARTITION flag. You must also use the -# "METIS =" option, below. - -# The path is relative to where it is used, in CHOLMOD/Lib, CHOLMOD/MATLAB, etc. -# You may wish to use an absolute path. METIS is optional. Compile -# CHOLMOD with -DNPARTITION if you do not wish to use METIS. -METIS_PATH = ../../metis-4.0 -METIS = ../../metis-4.0/libmetis.a - -# If you use CHOLMOD_CONFIG = -DNPARTITION then you must use the following -# options: -# METIS_PATH = -# METIS = - -#------------------------------------------------------------------------------ -# UMFPACK configuration: -#------------------------------------------------------------------------------ - -# Configuration flags for UMFPACK. See UMFPACK/Source/umf_config.h for details. -# -# -DNBLAS do not use the BLAS. UMFPACK will be very slow. -# -D'LONGBLAS=long' or -DLONGBLAS='long long' defines the integers used by -# LAPACK and the BLAS (defaults to 'int') -# -DNSUNPERF do not use the Sun Perf. Library (default is use it on Solaris) -# -DNPOSIX do not use POSIX routines sysconf and times. -# -DGETRUSAGE use getrusage -# -DNO_TIMER do not use any timing routines -# -DNRECIPROCAL do not multiply by the reciprocal -# -DNO_DIVIDE_BY_ZERO do not divide by zero - -UMFPACK_CONFIG = - -#------------------------------------------------------------------------------ -# CHOLMOD configuration -#------------------------------------------------------------------------------ - -# CHOLMOD Library Modules, which appear in libcholmod.a: -# Core requires: none -# Check requires: Core -# Cholesky requires: Core, AMD, COLAMD. optional: Partition, Supernodal -# MatrixOps requires: Core -# Modify requires: Core -# Partition requires: Core, CCOLAMD, METIS. optional: Cholesky -# Supernodal requires: Core, BLAS, LAPACK -# -# CHOLMOD test/demo Modules (all are GNU GPL, do not appear in libcholmod.a): -# Tcov requires: Core, Check, Cholesky, MatrixOps, Modify, Supernodal -# optional: Partition -# Valgrind same as Tcov -# Demo requires: Core, Check, Cholesky, MatrixOps, Supernodal -# optional: Partition -# -# Configuration flags: -# -DNCHECK do not include the Check module. License GNU LGPL -# -DNCHOLESKY do not include the Cholesky module. License GNU LGPL -# -DNPARTITION do not include the Partition module. License GNU LGPL -# also do not include METIS. -# -DNGPL do not include any GNU GPL Modules in the CHOLMOD library: -# -DNMATRIXOPS do not include the MatrixOps module. License GNU GPL -# -DNMODIFY do not include the Modify module. License GNU GPL -# -DNSUPERNODAL do not include the Supernodal module. License GNU GPL -# -# -DNPRINT do not print anything. -# -D'LONGBLAS=long' or -DLONGBLAS='long long' defines the integers used by -# LAPACK and the BLAS (defaults to 'int') -# -DNSUNPERF for Solaris only. If defined, do not use the Sun -# Performance Library - -CHOLMOD_CONFIG = - -#------------------------------------------------------------------------------ -# SuiteSparseQR configuration: -#------------------------------------------------------------------------------ - -# The SuiteSparseQR library can be compiled with the following options: -# -# -DNPARTITION do not include the CHOLMOD partition module -# -DNEXPERT do not include the functions in SuiteSparseQR_expert.cpp -# -DTIMING enable timing and flop counts -# -DHAVE_TBB enable the use of Intel's Threading Building Blocks (TBB) - -# default, without timing, without TBB: -SPQR_CONFIG = -# with timing and TBB: -# SPQR_CONFIG = -DTIMING -DHAVE_TBB -# with timing -# SPQR_CONFIG = -DTIMING - -# This is needed for IBM AIX: (but not for and C codes, just C++) -# SPQR_CONFIG = -DBLAS_NO_UNDERSCORE - -# with TBB, you must select this: -# TBB = -ltbb -# without TBB: -TBB = - -# with timing, you must include the timing library: -# RTLIB = -lrt -# without timing -RTLIB = - -#------------------------------------------------------------------------------ -# Linux -#------------------------------------------------------------------------------ - -# Using default compilers: -# CC = gcc -# CFLAGS = -O3 -fexceptions - -# alternatives: -# CFLAGS = -g -fexceptions \ - -Wall -W -Wshadow -Wmissing-prototypes -Wstrict-prototypes \ - -Wredundant-decls -Wnested-externs -Wdisabled-optimization -ansi \ - -funit-at-a-time -# CFLAGS = -O3 -fexceptions \ - -Wall -W -Werror -Wshadow -Wmissing-prototypes -Wstrict-prototypes \ - -Wredundant-decls -Wnested-externs -Wdisabled-optimization -ansi -# CFLAGS = -O3 -fexceptions -D_FILE_OFFSET_BITS=64 -D_LARGEFILE64_SOURCE -# CFLAGS = -O3 -# CFLAGS = -O3 -g -fexceptions -# CFLAGS = -g -fexceptions \ - -Wall -W -Wshadow \ - -Wredundant-decls -Wdisabled-optimization -ansi - -# consider: -# -fforce-addr -fmove-all-movables -freduce-all-givs -ftsp-ordering -# -frename-registers -ffast-math -funroll-loops - -# Using the Goto BLAS: -# BLAS = -lgoto -lfrtbegin -lg2c $(XERBLA) -lpthread - -# Using Intel's icc and ifort compilers: -# (does not work for mexFunctions unless you add a mexopts.sh file) -# F77 = ifort -# CC = icc -# CFLAGS = -O3 -xN -vec_report=0 -# CFLAGS = -g -# old (broken): CFLAGS = -ansi -O3 -ip -tpp7 -xW -vec_report0 - -# 64bit: -# F77FLAGS = -O -m64 -# CFLAGS = -O3 -fexceptions -m64 -# BLAS = -lgoto64 -lfrtbegin -lg2c -lpthread $(XERBLA) -# LAPACK = -llapack64 - - -# SUSE Linux 10.1, AMD Opteron, with GOTO Blas -# F77 = gfortran -# BLAS = -lgoto_opteron64 -lgfortran - -# SUSE Linux 10.1, Intel Pentium, with GOTO Blas -# F77 = gfortran -# BLAS = -lgoto -lgfortran - -#------------------------------------------------------------------------------ -# Mac -#------------------------------------------------------------------------------ - -# As recommended by macports, http://suitesparse.darwinports.com/ -# I've tested them myself on Mac OSX 10.6.1 (Snow Leopard), on my MacBook Air. -# F77 = gfortran -# CFLAGS = -O3 -fno-common -no-cpp-precomp -fexceptions -# BLAS = -framework Accelerate -# LAPACK = -framework Accelerate - -# Using netlib.org LAPACK and BLAS compiled by gfortran, with and without -# optimzation: -# BLAS = -lblas_plain -lgfortran -# LAPACK = -llapack_plain -# BLAS = -lblas_optimized -lgfortran -# LAPACK = -llapack_optimized - -#------------------------------------------------------------------------------ -# Solaris -#------------------------------------------------------------------------------ - -# 32-bit -# CFLAGS = -KPIC -dalign -xc99=%none -Xc -xlibmieee -xO5 -xlibmil -m32 - -# 64-bit -# CFLAGS = -fast -KPIC -xc99=%none -xlibmieee -xlibmil -m64 -Xc - -# FFLAGS = -fast -KPIC -dalign -xlibmil -m64 - -# The Sun Performance Library includes both LAPACK and the BLAS: -# BLAS = -xlic_lib=sunperf -# LAPACK = - - -#------------------------------------------------------------------------------ -# Compaq Alpha -#------------------------------------------------------------------------------ - -# 64-bit mode only -# CFLAGS = -O2 -std1 -# BLAS = -ldxml -# LAPACK = - -#------------------------------------------------------------------------------ -# Macintosh -#------------------------------------------------------------------------------ - -# CC = gcc -# CFLAGS = -O3 -fno-common -no-cpp-precomp -fexceptions -# LIB = -lstdc++ -# BLAS = -framework Accelerate -# LAPACK = -framework Accelerate - -#------------------------------------------------------------------------------ -# IBM RS 6000 -#------------------------------------------------------------------------------ - -# BLAS = -lessl -# LAPACK = - -# 32-bit mode: -# CFLAGS = -O4 -qipa -qmaxmem=16384 -qproto -# F77FLAGS = -O4 -qipa -qmaxmem=16384 - -# 64-bit mode: -# CFLAGS = -O4 -qipa -qmaxmem=16384 -q64 -qproto -# F77FLAGS = -O4 -qipa -qmaxmem=16384 -q64 -# AR = ar -X64 - -#------------------------------------------------------------------------------ -# SGI IRIX -#------------------------------------------------------------------------------ - -# BLAS = -lscsl -# LAPACK = - -# 32-bit mode -# CFLAGS = -O - -# 64-bit mode (32 bit int's and 64-bit long's): -# CFLAGS = -64 -# F77FLAGS = -64 - -# SGI doesn't have ranlib -# RANLIB = echo - -#------------------------------------------------------------------------------ -# AMD Opteron (64 bit) -#------------------------------------------------------------------------------ - -# BLAS = -lgoto_opteron64 -lg2c -# LAPACK = -llapack_opteron64 - -# SUSE Linux 10.1, AMD Opteron -# F77 = gfortran -# BLAS = -lgoto_opteron64 -lgfortran -# LAPACK = -llapack_opteron64 - -#------------------------------------------------------------------------------ -# remove object files and profile output -#------------------------------------------------------------------------------ - -CLEAN = *.o *.obj *.ln *.bb *.bbg *.da *.tcov *.gcov gmon.out *.bak *.d *.gcda *.gcno diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index d3229427e..1801e30b4 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -26,7 +26,7 @@ set (3rdparty_srcs ${eigen_headers} # Set by 3rdparty/CMakeLists.txt ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Source/ccolamd.c ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Source/ccolamd_global.c - ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/UFconfig/UFconfig.c) + ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/SuiteSparse_config/SuiteSparse_config.c) gtsam_assign_source_folders("${3rdparty_srcs}") # Create MSVC structure # To exclude a source from the library build (in any subfolder) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 1e5da7d86..a9d521a7f 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -38,26 +38,6 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -Matrix zeros( size_t m, size_t n ) { - return Matrix::Zero(m,n); -} - -/* ************************************************************************* */ -Matrix ones( size_t m, size_t n ) { - return Matrix::Ones(m,n); -} - -/* ************************************************************************* */ -Matrix eye( size_t m, size_t n) { - return Matrix::Identity(m, n); -} - -/* ************************************************************************* */ -Matrix diag(const Vector& v) { - return v.asDiagonal(); -} - /* ************************************************************************* */ bool assert_equal(const Matrix& expected, const Matrix& actual, double tol) { @@ -146,16 +126,6 @@ bool linear_dependent(const Matrix& A, const Matrix& B, double tol) { } } -/* ************************************************************************* */ -void multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e) { - e += alpha * A * x; -} - -/* ************************************************************************* */ -void multiplyAdd(const Matrix& A, const Vector& x, Vector& e) { - e += A * x; -} - /* ************************************************************************* */ Vector operator^(const Matrix& A, const Vector & v) { if (A.rows()!=v.size()) throw std::invalid_argument( @@ -166,21 +136,6 @@ Vector operator^(const Matrix& A, const Vector & v) { return A.transpose() * v; } -/* ************************************************************************* */ -void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector& x) { - x += alpha * A.transpose() * e; -} - -/* ************************************************************************* */ -void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x) { - x += A.transpose() * e; -} - -/* ************************************************************************* */ -void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x) { - x += alpha * A.transpose() * e; -} - /* ************************************************************************* */ //3 argument call void print(const Matrix& A, const string &s, ostream& stream) { @@ -250,7 +205,7 @@ Matrix diag(const std::vector& Hs) { rows+= Hs[i].rows(); cols+= Hs[i].cols(); } - Matrix results = zeros(rows,cols); + Matrix results = Matrix::Zero(rows,cols); size_t r = 0, c = 0; for (size_t i = 0; i& Hs) { return results; } -/* ************************************************************************* */ -void insertColumn(Matrix& A, const Vector& col, size_t j) { - A.col(j) = col; -} - -/* ************************************************************************* */ -void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j) { - A.col(j).segment(i, col.size()) = col; -} - /* ************************************************************************* */ Vector columnNormSquare(const Matrix &A) { Vector v (A.cols()) ; @@ -279,24 +224,13 @@ Vector columnNormSquare(const Matrix &A) { return v ; } -/* ************************************************************************* */ -void solve(Matrix& A, Matrix& B) { - // Eigen version - untested - B = A.fullPivLu().solve(B); -} - -/* ************************************************************************* */ -Matrix inverse(const Matrix& A) { - return A.inverse(); -} - /* ************************************************************************* */ /** Householder QR factorization, Golub & Van Loan p 224, explicit version */ /* ************************************************************************* */ pair qr(const Matrix& A) { const size_t m = A.rows(), n = A.cols(), kprime = min(m,n); - Matrix Q=eye(m,m),R(A); + Matrix Q=Matrix::Identity(m,m),R(A); Vector v(m); // loop over the kprime first columns @@ -319,7 +253,7 @@ pair qr(const Matrix& A) { v(k) = k llt(A); - Matrix inv = eye(A.rows()); + Matrix inv = Matrix::Identity(A.rows(),A.rows()); llt.matrixU().solveInPlace(inv); return inv*inv.transpose(); } @@ -612,7 +546,7 @@ Matrix cholesky_inverse(const Matrix &A) // inv(B' * B) == A Matrix inverse_square_root(const Matrix& A) { Eigen::LLT llt(A); - Matrix inv = eye(A.rows()); + Matrix inv = Matrix::Identity(A.rows(),A.rows()); llt.matrixU().solveInPlace(inv); return inv.transpose(); } @@ -648,7 +582,7 @@ boost::tuple DLT(const Matrix& A, double rank_tol) { /* ************************************************************************* */ Matrix expm(const Matrix& A, size_t K) { - Matrix E = eye(A.rows()), A_k = eye(A.rows()); + Matrix E = Matrix::Identity(A.rows(),A.rows()), A_k = Matrix::Identity(A.rows(),A.rows()); for(size_t k=1;k<=K;k++) { A_k = A_k*A/double(k); E = E + A_k; diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index b0b292c56..235cd30f3 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -16,6 +16,7 @@ * @author Kai Ni * @author Frank Dellaert * @author Alex Cunningham + * @author Alex Hagiopol */ // \callgraph @@ -23,17 +24,17 @@ #pragma once #include #include -#include // Configuration from CMake - +#include +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #include #include #include +#endif #include #include #include #include - /** * Matrix is a typedef in the gtsam namespace * TODO: make a version to work with matlab wrapping @@ -74,40 +75,8 @@ GTSAM_MAKE_MATRIX_DEFS(9); typedef Eigen::Block SubMatrix; typedef Eigen::Block ConstSubMatrix; -// Matlab-like syntax - /** - * Creates an zeros matrix, with matlab-like syntax - * - * Note: if assigning a block (created from an Eigen block() function) of a matrix to zeros, - * don't use this function, instead use ".setZero(m,n)" to avoid an Eigen error. - */ -GTSAM_EXPORT Matrix zeros(size_t m, size_t n); - -/** - * Creates an ones matrix, with matlab-like syntax - */ -GTSAM_EXPORT Matrix ones(size_t m, size_t n); - -/** - * Creates an identity matrix, with matlab-like syntax - * - * Note: if assigning a block (created from an Eigen block() function) of a matrix to identity, - * don't use this function, instead use ".setIdentity(m,n)" to avoid an Eigen error. - */ -GTSAM_EXPORT Matrix eye(size_t m, size_t n); - -/** - * Creates a square identity matrix, with matlab-like syntax - * - * Note: if assigning a block (created from an Eigen block() function) of a matrix to identity, - * don't use this function, instead use ".setIdentity(m)" to avoid an Eigen error. - */ -inline Matrix eye( size_t m ) { return eye(m,m); } -GTSAM_EXPORT Matrix diag(const Vector& v); - -/** - * equals with an tolerance + * equals with a tolerance */ template bool equal_with_abs_tol(const Eigen::DenseBase& A, const Eigen::DenseBase& B, double tol = 1e-9) { @@ -166,37 +135,12 @@ GTSAM_EXPORT bool linear_independent(const Matrix& A, const Matrix& B, double to */ GTSAM_EXPORT bool linear_dependent(const Matrix& A, const Matrix& B, double tol = 1e-9); -/** - * BLAS Level-2 style e <- e + alpha*A*x - */ -GTSAM_EXPORT void multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e); - -/** - * BLAS Level-2 style e <- e + A*x - */ -GTSAM_EXPORT void multiplyAdd(const Matrix& A, const Vector& x, Vector& e); - /** * overload ^ for trans(A)*v * We transpose the vectors for speed. */ GTSAM_EXPORT Vector operator^(const Matrix& A, const Vector & v); -/** - * BLAS Level-2 style x <- x + alpha*A'*e - */ -GTSAM_EXPORT void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector& x); - -/** - * BLAS Level-2 style x <- x + A'*e - */ -GTSAM_EXPORT void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x); - -/** - * BLAS Level-2 style x <- x + alpha*A'*e - */ -GTSAM_EXPORT void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x); - /** products using old-style format to improve compatibility */ template inline MATRIX prod(const MATRIX& A, const MATRIX&B) { @@ -281,19 +225,6 @@ const typename MATRIX::ConstRowXpr row(const MATRIX& A, size_t j) { return A.row(j); } -/** - * inserts a column into a matrix IN PLACE - * NOTE: there is no size checking - * Alternate form allows for vectors smaller than the whole column to be inserted - * @param A matrix to be modified in place - * @param col is the vector to be inserted - * @param j is the index to insert the column - */ -GTSAM_EXPORT void insertColumn(Matrix& A, const Vector& col, size_t j); -GTSAM_EXPORT void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j); - -GTSAM_EXPORT Vector columnNormSquare(const Matrix &A); - /** * Zeros all of the elements below the diagonal of a matrix, in place * @param A is a matrix, to be modified in place @@ -355,17 +286,6 @@ inline typename Reshape::ReshapedTy return Reshape::reshape(m); } -/** - * solve AX=B via in-place Lu factorization and backsubstitution - * After calling, A contains LU, B the solved RHS vectors - */ -GTSAM_EXPORT void solve(Matrix& A, Matrix& B); - -/** - * invert A - */ -GTSAM_EXPORT Matrix inverse(const Matrix& A); - /** * QR factorization, inefficient, best use imperative householder below * m*n matrix -> m*m Q, m*n R @@ -492,12 +412,6 @@ inline Matrix3 skewSymmetric(const Eigen::MatrixBase& w) { /** Use Cholesky to calculate inverse square root of a matrix */ GTSAM_EXPORT Matrix inverse_square_root(const Matrix& A); -/** Calculate the LL^t decomposition of a S.P.D matrix */ -GTSAM_EXPORT Matrix LLt(const Matrix& A); - -/** Calculate the R^tR decomposition of a S.P.D matrix */ -GTSAM_EXPORT Matrix RtR(const Matrix& A); - /** Return the inverse of a S.P.D. matrix. Inversion is done via Cholesky decomposition. */ GTSAM_EXPORT Matrix cholesky_inverse(const Matrix &A); @@ -603,6 +517,28 @@ struct MultiplyWithInverseFunction { const Operator phi_; }; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +inline Matrix zeros( size_t m, size_t n ) { return Matrix::Zero(m,n); } +inline Matrix ones( size_t m, size_t n ) { return Matrix::Ones(m,n); } +inline Matrix eye( size_t m, size_t n) { return Matrix::Identity(m, n); } +inline Matrix eye( size_t m ) { return eye(m,m); } +inline Matrix diag(const Vector& v) { return v.asDiagonal(); } +inline void multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e) { e += alpha * A * x; } +inline void multiplyAdd(const Matrix& A, const Vector& x, Vector& e) { e += A * x; } +inline void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector& x) { x += alpha * A.transpose() * e; } +inline void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x) { x += A.transpose() * e; } +inline void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x) { x += alpha * A.transpose() * e; } +inline void insertColumn(Matrix& A, const Vector& col, size_t j) { A.col(j) = col; } +inline void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j) { A.col(j).segment(i, col.size()) = col; } +inline void solve(Matrix& A, Matrix& B) { B = A.fullPivLu().solve(B); } +inline Matrix inverse(const Matrix& A) { return A.inverse(); } +#endif + +GTSAM_EXPORT Matrix LLt(const Matrix& A); + +GTSAM_EXPORT Matrix RtR(const Matrix& A); + +GTSAM_EXPORT Vector columnNormSquare(const Matrix &A); } // namespace gtsam #include diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index dbfe7ab87..40d819de9 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -33,20 +33,6 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -bool zero(const Vector& v) { - bool result = true; - size_t n = v.size(); - for( size_t j = 0 ; j < n ; j++) - result = result && (v(j) == 0.0); - return result; -} - -/* ************************************************************************* */ -Vector delta(size_t n, size_t i, double value) { - return Vector::Unit(n, i) * value; -} - /* ************************************************************************* */ //3 argument call void print(const Vector& v, const string& s, ostream& stream) { @@ -235,7 +221,7 @@ double weightedPseudoinverse(const Vector& a, const Vector& weights, // Basically, instead of doing a normal QR step with the weighted // pseudoinverse, we enforce the constraint by turning // ax + AS = b into x + (A/a)S = b/a, for the first row where a!=0 - pseudo = delta(m, i, 1.0 / a[i]); + pseudo = Vector::Unit(m,i)*(1.0/a[i]); return inf; } } diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 51d2f07c0..91aec85b8 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -14,11 +14,11 @@ * @brief typedef and functions to augment Eigen's VectorXd * @author Kai Ni * @author Frank Dellaert + * @author Alex Hagiopol */ // \callgraph - #pragma once #ifndef MKL_BLAS #define MKL_BLAS MKL_DOMAIN_BLAS @@ -63,47 +63,6 @@ GTSAM_MAKE_VECTOR_DEFS(12); typedef Eigen::VectorBlock SubVector; typedef Eigen::VectorBlock ConstSubVector; -/** - * Create basis vector of dimension n, - * with a constant in spot i - * @param n is the size of the vector - * @param i index of the one - * @param value is the value to insert into the vector - * @return delta vector - */ -GTSAM_EXPORT Vector delta(size_t n, size_t i, double value); - -/** - * Create basis vector of dimension n, - * with one in spot i - * @param n is the size of the vector - * @param i index of the one - * @return basis vector - */ -inline Vector basis(size_t n, size_t i) { return delta(n, i, 1.0); } - -/** - * Create zero vector - * @param n size - */ -inline Vector zero(size_t n) { return Vector::Zero(n);} - -/** - * Create vector initialized to ones - * @param n size - */ -inline Vector ones(size_t n) { return Vector::Ones(n); } - -/** - * check if all zero - */ -GTSAM_EXPORT bool zero(const Vector& v); - -/** - * dimensionality == size - */ -inline size_t dim(const Vector& v) { return v.size(); } - /** * print without optional string, must specify cout yourself */ @@ -272,21 +231,25 @@ GTSAM_EXPORT Vector concatVectors(const std::list& vs); */ GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...); - #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 -GTSAM_EXPORT inline Vector abs(const Vector& v){return v.cwiseAbs();} -GTSAM_EXPORT inline Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);} -GTSAM_EXPORT inline Vector esqrt(const Vector& v) { return v.cwiseSqrt();} -GTSAM_EXPORT inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);} -GTSAM_EXPORT inline double max(const Vector &a){return a.maxCoeff();} -GTSAM_EXPORT inline double norm_2(const Vector& v) {return v.norm();} -GTSAM_EXPORT inline Vector reciprocal(const Vector &a) {return a.array().inverse();} -GTSAM_EXPORT inline Vector repeat(size_t n, double value) {return Vector::Constant(n, value);} -GTSAM_EXPORT inline const Vector sub(const Vector &v, size_t i1, size_t i2) {return v.segment(i1,i2-i1);} -GTSAM_EXPORT inline void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;} -GTSAM_EXPORT inline double sum(const Vector &a){return a.sum();} +inline Vector abs(const Vector& v){return v.cwiseAbs();} +inline Vector basis(size_t n, size_t i) { return Vector::Unit(n,i); } +inline Vector delta(size_t n, size_t i, double value){ return Vector::Unit(n, i) * value;} +inline size_t dim(const Vector& v) { return v.size(); } +inline Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);} +inline Vector esqrt(const Vector& v) { return v.cwiseSqrt();} +inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);} +inline double max(const Vector &a){return a.maxCoeff();} +inline double norm_2(const Vector& v) {return v.norm();} +inline Vector ones(size_t n) { return Vector::Ones(n); } +inline Vector reciprocal(const Vector &a) {return a.array().inverse();} +inline Vector repeat(size_t n, double value) {return Vector::Constant(n, value);} +inline const Vector sub(const Vector &v, size_t i1, size_t i2) {return v.segment(i1,i2-i1);} +inline void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;} +inline double sum(const Vector &a){return a.sum();} +inline bool zero(const Vector& v){ return v.isZero(); } +inline Vector zero(size_t n) { return Vector::Zero(n); } #endif - } // namespace gtsam #include diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 9a42db3ce..6cd28b951 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -88,7 +88,7 @@ typename internal::FixedSizeMatrix::type numericalGradient(boost::function g; g.setZero(); // Can be fixed size for (int j = 0; j < N; j++) { d(j) = delta; double hxplus = h(traits::Retract(x, d)); @@ -142,7 +142,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative11(boost::funct dx.setZero(); // Fill in Jacobian H - Matrix H = zeros(m, N); + Matrix H = Matrix::Zero(m, N); const double factor = 1.0 / (2.0 * delta); for (int j = 0; j < N; j++) { dx(j) = delta; diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 13d32794e..093f22961 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -156,8 +156,8 @@ TEST(Matrix, collect2 ) TEST(Matrix, collect3 ) { Matrix A, B; - A = eye(2, 3); - B = eye(2, 3); + A = Matrix::Identity(2,3); + B = Matrix::Identity(2,3); vector matrices; matrices.push_back(&A); matrices.push_back(&B); @@ -211,48 +211,6 @@ TEST(Matrix, column ) EXPECT(assert_equal(a3, exp3)); } -/* ************************************************************************* */ -TEST(Matrix, insert_column ) -{ - Matrix big = zeros(5, 6); - Vector col = ones(5); - size_t j = 3; - - insertColumn(big, col, j); - - Matrix expected = (Matrix(5, 6) << - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0).finished(); - - EXPECT(assert_equal(expected, big)); -} - -/* ************************************************************************* */ -TEST(Matrix, insert_subcolumn ) -{ - Matrix big = zeros(5, 6); - Vector col1 = ones(2); - size_t i = 1; - size_t j = 3; - - insertColumn(big, col1, i, j); // check 1 - - Vector col2 = ones(1); - insertColumn(big, col2, 4, 5); // check 2 - - Matrix expected = (Matrix(5, 6) << - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 1.0).finished(); - - EXPECT(assert_equal(expected, big)); -} - /* ************************************************************************* */ TEST(Matrix, row ) { @@ -272,26 +230,10 @@ TEST(Matrix, row ) EXPECT(assert_equal(a3, exp3)); } -/* ************************************************************************* */ -TEST(Matrix, zeros ) -{ - Matrix A(2, 3); - A(0, 0) = 0; - A(0, 1) = 0; - A(0, 2) = 0; - A(1, 0) = 0; - A(1, 1) = 0; - A(1, 2) = 0; - - Matrix zero = zeros(2, 3); - - EQUALITY(A , zero); -} - /* ************************************************************************* */ TEST(Matrix, insert_sub ) { - Matrix big = zeros(5, 6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0, + Matrix big = Matrix::Zero(5,6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0).finished(); insertSub(big, small, 1, 2); @@ -307,9 +249,9 @@ TEST(Matrix, insert_sub ) TEST(Matrix, diagMatrices ) { std::vector Hs; - Hs.push_back(ones(3,3)); - Hs.push_back(ones(4,4)*2); - Hs.push_back(ones(2,2)*3); + Hs.push_back(Matrix::Ones(3,3)); + Hs.push_back(Matrix::Ones(4,4)*2); + Hs.push_back(Matrix::Ones(2,2)*3); Matrix actual = diag(Hs); @@ -723,9 +665,9 @@ TEST(Matrix, inverse ) A(2, 1) = 0; A(2, 2) = 6; - Matrix Ainv = inverse(A); - EXPECT(assert_equal(eye(3), A*Ainv)); - EXPECT(assert_equal(eye(3), Ainv*A)); + Matrix Ainv = A.inverse(); + EXPECT(assert_equal((Matrix) I_3x3, A*Ainv)); + EXPECT(assert_equal((Matrix) I_3x3, Ainv*A)); Matrix expected(3, 3); expected(0, 0) = 1.0909; @@ -746,13 +688,13 @@ TEST(Matrix, inverse ) 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0).finished(), - inverse(lMg))); + lMg.inverse())); Matrix gMl((Matrix(3, 3) << 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0).finished()); EXPECT(assert_equal((Matrix(3, 3) << 0.0, 1.0,-2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0).finished(), - inverse(gMl))); + gMl.inverse())); } /* ************************************************************************* */ @@ -769,7 +711,7 @@ TEST(Matrix, inverse2 ) A(2, 1) = 0; A(2, 2) = 1; - Matrix Ainv = inverse(A); + Matrix Ainv = A.inverse(); Matrix expected(3, 3); expected(0, 0) = 0; @@ -996,7 +938,7 @@ TEST(Matrix, inverse_square_root ) 10.0).finished(); EQUALITY(expected,actual); - EQUALITY(measurement_covariance,inverse(actual*actual)); + EQUALITY(measurement_covariance,(actual*actual).inverse()); // Randomly generated test. This test really requires inverse to // be working well; if it's not, there's the possibility of a @@ -1052,28 +994,6 @@ TEST(Matrix, cholesky_inverse ) EQUALITY(cholesky::M.inverse(), cholesky_inverse(cholesky::M)); } -/* ************************************************************************* */ -TEST(Matrix, multiplyAdd ) -{ - Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished(); - Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.), - expected = e + A * x; - - multiplyAdd(1, A, x, e); - EXPECT(assert_equal(expected, e)); -} - -/* ************************************************************************* */ -TEST(Matrix, transposeMultiplyAdd ) -{ - Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished(); - Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.), - expected = x + trans(A) * e; - - transposeMultiplyAdd(1, A, e, x); - EXPECT(assert_equal(expected, x)); -} - /* ************************************************************************* */ TEST(Matrix, linear_dependent ) { @@ -1102,12 +1022,12 @@ TEST(Matrix, linear_dependent3 ) TEST(Matrix, svd1 ) { Vector v = Vector3(2., 1., 0.); - Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1) + Matrix U1 = Matrix::Identity(4, 3), S1 = v.asDiagonal(), V1 = I_3x3, A = (U1 * S1) * Matrix(trans(V1)); Matrix U, V; Vector s; svd(A, U, s, V); - Matrix S = diag(s); + Matrix S = s.asDiagonal(); EXPECT(assert_equal(U*S*Matrix(trans(V)),A)); EXPECT(assert_equal(S,S1)); } @@ -1158,7 +1078,7 @@ TEST(Matrix, svd3 ) V = -V; } - Matrix S = diag(s); + Matrix S = s.asDiagonal(); Matrix t = U * S; Matrix Vt = trans(V); @@ -1202,7 +1122,7 @@ TEST(Matrix, svd4 ) V.col(1) = -V.col(1); } - Matrix reconstructed = U * diag(s) * trans(V); + Matrix reconstructed = U * s.asDiagonal() * trans(V); EXPECT(assert_equal(A, reconstructed, 1e-4)); EXPECT(assert_equal(expectedU,U, 1e-3)); diff --git a/gtsam/base/tests/testSymmetricBlockMatrix.cpp b/gtsam/base/tests/testSymmetricBlockMatrix.cpp index df3403cf4..da193aec5 100644 --- a/gtsam/base/tests/testSymmetricBlockMatrix.cpp +++ b/gtsam/base/tests/testSymmetricBlockMatrix.cpp @@ -42,7 +42,7 @@ TEST(SymmetricBlockMatrix, ReadBlocks) 23, 29).finished(); Matrix actual1 = testBlockMatrix(1, 1); // Test only writing the upper triangle for efficiency - Matrix actual1t = Matrix::Zero(2, 2); + Matrix actual1t = Z_2x2; actual1t.triangularView() = testBlockMatrix(1, 1).triangularView(); EXPECT(assert_equal(expected1, actual1)); EXPECT(assert_equal(Matrix(expected1.triangularView()), actual1t)); diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 790e0922c..8a54d5469 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -79,22 +79,6 @@ TEST(Vector, copy ) EXPECT(assert_equal(a, b)); } -/* ************************************************************************* */ -TEST(Vector, zero1 ) -{ - Vector v = Vector::Zero(2); - EXPECT(zero(v)); -} - -/* ************************************************************************* */ -TEST(Vector, zero2 ) -{ - Vector a = zero(2); - Vector b = Vector::Zero(2); - EXPECT(a==b); - EXPECT(assert_equal(a, b)); -} - /* ************************************************************************* */ TEST(Vector, scalar_multiply ) { @@ -256,7 +240,7 @@ TEST(Vector, equals ) TEST(Vector, greater_than ) { Vector v1 = Vector3(1.0, 2.0, 3.0), - v2 = zero(3); + v2 = Z_3x1; EXPECT(greaterThanOrEqual(v1, v1)); // test basic greater than EXPECT(greaterThanOrEqual(v1, v2)); // test equals } diff --git a/gtsam/discrete/tests/testDiscreteMarginals.cpp b/gtsam/discrete/tests/testDiscreteMarginals.cpp index 24bec4fa1..4e9f956b6 100644 --- a/gtsam/discrete/tests/testDiscreteMarginals.cpp +++ b/gtsam/discrete/tests/testDiscreteMarginals.cpp @@ -167,7 +167,7 @@ TEST_UNSAFE( DiscreteMarginals, truss2 ) { // Calculate the marginals by brute force vector allPosbValues = cartesianProduct( key[0] & key[1] & key[2] & key[3] & key[4]); - Vector T = zero(5), F = zero(5); + Vector T = Z_5x1, F = Z_5x1; for (size_t i = 0; i < allPosbValues.size(); ++i) { DiscreteFactor::Values x = allPosbValues[i]; double px = graph(x); diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index aa023a09a..4af0257ec 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -42,7 +42,7 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> double pred_d = n_.unitVector().dot(xr.translation()) + d_; if (Hr) { - *Hr = zeros(3, 6); + *Hr = Matrix::Zero(3,6); Hr->block<2, 3>(0, 0) = D_rotated_plane; Hr->block<1, 3>(2, 3) = unit_vec; } diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 1aa8f060a..8a0e8fbf5 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -132,7 +132,7 @@ Matrix3 Pose2::AdjointMap() const { /* ************************************************************************* */ Matrix3 Pose2::adjointMap(const Vector3& v) { // See Chirikjian12book2, vol.2, pg. 36 - Matrix3 ad = zeros(3,3); + Matrix3 ad = Z_3x3; ad(0,1) = -v[2]; ad(1,0) = v[2]; ad(0,2) = v[1]; diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 41d56b6e3..9780cb49a 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -63,7 +63,7 @@ Rot2& Rot2::normalize() { Rot2 Rot2::Expmap(const Vector1& v, OptionalJacobian<1, 1> H) { if (H) *H = I_1x1; - if (zero(v)) + if (v.isZero()) return (Rot2()); else return Rot2::fromAngle(v(0)); diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 198cd5862..65dd5f609 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -119,12 +119,12 @@ namespace gtsam { /// Left-trivialized derivative of the exponential map static Matrix ExpmapDerivative(const Vector& /*v*/) { - return ones(1); + return I_1x1; } /// Left-trivialized derivative inverse of the exponential map static Matrix LogmapDerivative(const Vector& /*v*/) { - return ones(1); + return I_1x1; } // Chart at origin simply uses exponential map and its inverse diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index 3e93dedc1..4ccb6075b 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -122,8 +122,8 @@ TEST(Cal3_S2, between) { Matrix H1, H2; EXPECT(assert_equal(Cal3_S2(0,1,2,3,4), k1.between(k2, H1, H2))); - EXPECT(assert_equal(-eye(5), H1)); - EXPECT(assert_equal(eye(5), H2)); + EXPECT(assert_equal(-I_5x5, H1)); + EXPECT(assert_equal(I_5x5, H2)); } diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 79759678d..75ee9427d 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -62,7 +62,7 @@ TEST (EssentialMatrix, FromPose3) { //******************************************************************************* TEST(EssentialMatrix, localCoordinates0) { EssentialMatrix E; - Vector expected = zero(5); + Vector expected = Z_5x1; Vector actual = E.localCoordinates(E); EXPECT(assert_equal(expected, actual, 1e-8)); } @@ -74,7 +74,7 @@ TEST (EssentialMatrix, localCoordinates) { Pose3 pose(trueRotation, trueTranslation); EssentialMatrix hx = EssentialMatrix::FromPose3(pose); Vector actual = hx.localCoordinates(EssentialMatrix::FromPose3(pose)); - EXPECT(assert_equal(zero(5), actual, 1e-8)); + EXPECT(assert_equal(Z_5x1, actual, 1e-8)); Vector6 d; d << 0.1, 0.2, 0.3, 0, 0, 0; @@ -85,7 +85,7 @@ TEST (EssentialMatrix, localCoordinates) { //************************************************************************* TEST (EssentialMatrix, retract0) { - EssentialMatrix actual = trueE.retract(zero(5)); + EssentialMatrix actual = trueE.retract(Z_5x1); EXPECT(assert_equal(trueE, actual)); } diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index d3a107570..b3d87f18c 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -96,8 +96,8 @@ inline static Vector randomVector(const Vector& minLimits, const Vector& maxLimits) { // Get the number of dimensions and create the return vector - size_t numDims = dim(minLimits); - Vector vector = zero(numDims); + size_t numDims = minLimits.size(); + Vector vector = Vector::Zero(numDims); // Create the random vector for (size_t i = 0; i < numDims; i++) { @@ -145,7 +145,7 @@ TEST (OrientedPlane3, error2) { OrientedPlane3 plane2(-1.1, 0.2, 0.3, 5.4); // Hard-coded regression values, to ensure the result doesn't change. - EXPECT(assert_equal(zero(3), plane1.errorVector(plane1), 1e-8)); + EXPECT(assert_equal((Vector) Z_3x1, plane1.errorVector(plane1), 1e-8)); EXPECT(assert_equal(Vector3(-0.0677674148, -0.0760543588, -0.4), plane1.errorVector(plane2), 1e-5)); // Test the jacobians of transform diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 4fbdbcbe1..99dcb95bf 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -116,7 +116,7 @@ TEST( PinholeCamera, lookat) Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; - EXPECT(assert_equal(I, eye(3))); + EXPECT(assert_equal(I, I_3x3)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index c5137e3b3..0d840de7e 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -87,7 +87,7 @@ TEST( PinholePose, lookat) Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; - EXPECT(assert_equal(I, eye(3))); + EXPECT(assert_equal(I, I_3x3)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 6f8d45b3e..3a636b9bf 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -73,12 +73,12 @@ TEST(Point2, Lie) { Matrix H1, H2; EXPECT(assert_equal(Point2(5,7), traits::Compose(p1, p2, H1, H2))); - EXPECT(assert_equal(eye(2), H1)); - EXPECT(assert_equal(eye(2), H2)); + EXPECT(assert_equal(I_2x2, H1)); + EXPECT(assert_equal(I_2x2, H2)); EXPECT(assert_equal(Point2(3,3), traits::Between(p1, p2, H1, H2))); - EXPECT(assert_equal(-eye(2), H1)); - EXPECT(assert_equal(eye(2), H2)); + EXPECT(assert_equal(-I_2x2, H1)); + EXPECT(assert_equal(I_2x2, H2)); EXPECT(assert_equal(Point2(5,7), traits::Retract(p1, Vector2(4., 5.)))); EXPECT(assert_equal(Vector2(3.,3.), traits::Local(p1,p2))); diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index da7696d0d..9b6e53323 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -47,12 +47,12 @@ TEST(Point3, Lie) { Matrix H1, H2; EXPECT(assert_equal(Point3(5, 7, 9), traits::Compose(p1, p2, H1, H2))); - EXPECT(assert_equal(eye(3), H1)); - EXPECT(assert_equal(eye(3), H2)); + EXPECT(assert_equal(I_3x3, H1)); + EXPECT(assert_equal(I_3x3, H2)); EXPECT(assert_equal(Point3(3, 3, 3), traits::Between(p1, p2, H1, H2))); - EXPECT(assert_equal(-eye(3), H1)); - EXPECT(assert_equal(eye(3), H2)); + EXPECT(assert_equal(-I_3x3, H1)); + EXPECT(assert_equal(I_3x3, H2)); EXPECT(assert_equal(Point3(5, 7, 9), traits::Retract(p1, Vector3(4,5,6)))); EXPECT(assert_equal(Vector3(3, 3, 3), traits::Local(p1,p2))); diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 5b835200a..d1f68fe28 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -102,7 +102,7 @@ TEST(Pose2, expmap3) { 0.99, 0.0, -0.015, 0.0, 0.0, 0.0).finished(); Matrix A2 = A*A/2.0, A3 = A2*A/3.0, A4=A3*A/4.0; - Matrix expected = eye(3) + A + A2 + A3 + A4; + Matrix expected = I_3x3 + A + A2 + A3 + A4; Vector v = Vector3(0.01, -0.015, 0.99); Pose2 pose = Pose2::Expmap(v); @@ -311,7 +311,7 @@ TEST(Pose2, compose_a) -1.0, 0.0, 2.0, 0.0, 0.0, 1.0 ).finished(); - Matrix expectedH2 = eye(3); + Matrix expectedH2 = I_3x3; Matrix numericalH1 = numericalDerivative21(testing::compose, pose1, pose2); Matrix numericalH2 = numericalDerivative22(testing::compose, pose1, pose2); EXPECT(assert_equal(expectedH1,actualDcompose1)); diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 648197ced..97ccbcb34 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -61,7 +61,7 @@ TEST( Pose3, constructors) TEST( Pose3, retract_first_order) { Pose3 id; - Vector v = zero(6); + Vector v = Z_6x1; v(0) = 0.3; EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), id.retract(v),1e-2)); v(3)=0.2;v(4)=0.7;v(5)=-2; @@ -71,7 +71,7 @@ TEST( Pose3, retract_first_order) /* ************************************************************************* */ TEST( Pose3, retract_expmap) { - Vector v = zero(6); v(0) = 0.3; + Vector v = Z_6x1; v(0) = 0.3; Pose3 pose = Pose3::Expmap(v); EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), pose, 1e-2)); EXPECT(assert_equal(v,Pose3::Logmap(pose),1e-2)); @@ -81,7 +81,7 @@ TEST( Pose3, retract_expmap) TEST( Pose3, expmap_a_full) { Pose3 id; - Vector v = zero(6); + Vector v = Z_6x1; v(0) = 0.3; EXPECT(assert_equal(expmap_default(id, v), Pose3(R, Point3(0,0,0)))); v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; @@ -92,7 +92,7 @@ TEST( Pose3, expmap_a_full) TEST( Pose3, expmap_a_full2) { Pose3 id; - Vector v = zero(6); + Vector v = Z_6x1; v(0) = 0.3; EXPECT(assert_equal(expmap_default(id, v), Pose3(R, Point3(0,0,0)))); v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; @@ -153,7 +153,7 @@ Pose3 Agrawal06iros(const Vector& xi) { return Pose3(Rot3(), Point3(v)); else { Matrix W = skewSymmetric(w/t); - Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W); + Matrix A = I_3x3 + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W); return Pose3(Rot3::Expmap (w), Point3(A * v)); } } @@ -267,7 +267,7 @@ TEST( Pose3, inverse) { Matrix actualDinverse; Matrix actual = T.inverse(actualDinverse).matrix(); - Matrix expected = inverse(T.matrix()); + Matrix expected = T.matrix().inverse(); EXPECT(assert_equal(actual,expected,1e-8)); Matrix numericalH = numericalDerivative11(testing::inverse, T); @@ -293,7 +293,7 @@ TEST( Pose3, inverseDerivatives2) TEST( Pose3, compose_inverse) { Matrix actual = (T*T.inverse()).matrix(); - Matrix expected = eye(4,4); + Matrix expected = I_4x4; EXPECT(assert_equal(actual,expected,1e-8)); } @@ -712,7 +712,7 @@ TEST(Pose3, Bearing2) { TEST( Pose3, unicycle ) { // velocity in X should be X in inertial frame, rather than global frame - Vector x_step = delta(6,3,1.0); + Vector x_step = Vector::Unit(6,3)*1.0; EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), l1), expmap_default(x1, x_step), tol)); EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), Point3(2,1,0)), expmap_default(x2, x_step), tol)); EXPECT(assert_equal(Pose3(Rot3::Ypr(M_PI/4.0,0,0), Point3(2,2,0)), expmap_default(x3, sqrt(2.0) * x_step), tol)); @@ -723,9 +723,8 @@ TEST( Pose3, adjointMap) { Matrix res = Pose3::adjointMap(screwPose3::xi); Matrix wh = skewSymmetric(screwPose3::xi(0), screwPose3::xi(1), screwPose3::xi(2)); Matrix vh = skewSymmetric(screwPose3::xi(3), screwPose3::xi(4), screwPose3::xi(5)); - Matrix Z3 = zeros(3,3); Matrix6 expected; - expected << wh, Z3, vh, wh; + expected << wh, Z_3x3, vh, wh; EXPECT(assert_equal(expected,res,1e-5)); } diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 6ead22860..d6d1f3149 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -62,8 +62,8 @@ TEST( Rot2, compose) Matrix H1, H2; (void) Rot2::fromAngle(1.0).compose(Rot2::fromAngle(2.0), H1, H2); - EXPECT(assert_equal(eye(1), H1)); - EXPECT(assert_equal(eye(1), H2)); + EXPECT(assert_equal(I_1x1, H1)); + EXPECT(assert_equal(I_1x1, H2)); } /* ************************************************************************* */ @@ -74,8 +74,8 @@ TEST( Rot2, between) Matrix H1, H2; (void) Rot2::fromAngle(1.0).between(Rot2::fromAngle(2.0), H1, H2); - EXPECT(assert_equal(-eye(1), H1)); - EXPECT(assert_equal(eye(1), H2)); + EXPECT(assert_equal(-I_1x1, H1)); + EXPECT(assert_equal(I_1x1, H2)); } /* ************************************************************************* */ @@ -89,7 +89,7 @@ TEST( Rot2, equals) /* ************************************************************************* */ TEST( Rot2, expmap) { - Vector v = zero(1); + Vector v = Z_1x1; CHECK(assert_equal(R.retract(v), R)); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 876897ab3..5e72d4c5b 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -152,7 +152,7 @@ TEST( Rot3, Rodrigues4) /* ************************************************************************* */ TEST( Rot3, retract) { - Vector v = zero(3); + Vector v = Z_3x1; CHECK(assert_equal(R, R.retract(v))); // // test Canonical coordinates @@ -213,7 +213,7 @@ TEST(Rot3, log) #define CHECK_OMEGA_ZERO(X,Y,Z) \ w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \ R = Rot3::Rodrigues(w); \ - EXPECT(assert_equal(zero(3), Rot3::Logmap(R))); + EXPECT(assert_equal((Vector) Z_3x1, Rot3::Logmap(R))); CHECK_OMEGA_ZERO( 2.0*PI, 0, 0) CHECK_OMEGA_ZERO( 0, 2.0*PI, 0) @@ -555,8 +555,8 @@ TEST(Rot3, quaternion) { /* ************************************************************************* */ Matrix Cayley(const Matrix& A) { Matrix::Index n = A.cols(); - const Matrix I = eye(n); - return (I-A)*inverse(I+A); + const Matrix I = Matrix::Identity(n,n); + return (I-A)*(I+A).inverse(); } TEST( Rot3, Cayley ) { diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 71dcabd4e..70b3069f2 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -76,7 +76,7 @@ TEST( SimpleCamera, lookat) Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; - CHECK(assert_equal(I, eye(3))); + CHECK(assert_equal(I, I_3x3)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 396c8b0f3..0e99268ee 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -237,7 +237,7 @@ TEST(Unit3, distance) { TEST(Unit3, localCoordinates0) { Unit3 p; Vector actual = p.localCoordinates(p); - EXPECT(assert_equal(zero(2), actual, 1e-8)); + EXPECT(assert_equal(Z_2x1, actual, 1e-8)); } TEST(Unit3, localCoordinates) { @@ -245,14 +245,14 @@ TEST(Unit3, localCoordinates) { Unit3 p, q; Vector2 expected = Vector2::Zero(); Vector2 actual = p.localCoordinates(q); - EXPECT(assert_equal(zero(2), actual, 1e-8)); + EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-8)); EXPECT(assert_equal(q, p.retract(expected), 1e-8)); } { Unit3 p, q(1, 6.12385e-21, 0); Vector2 expected = Vector2::Zero(); Vector2 actual = p.localCoordinates(q); - EXPECT(assert_equal(zero(2), actual, 1e-8)); + EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-8)); EXPECT(assert_equal(q, p.retract(expected), 1e-8)); } { diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 7ec9edbb3..a1dc3269a 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -31,7 +31,7 @@ Vector4 triangulateHomogeneousDLT( size_t m = projection_matrices.size(); // Allocate DLT matrix - Matrix A = zeros(m * 2, 4); + Matrix A = Matrix::Zero(m * 2, 4); for (size_t i = 0; i < m; i++) { size_t row = i * 2; diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 3e3b40f8f..931610639 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -53,10 +53,10 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, gttic(Ordering_COLAMDConstrained); gttic(Prepare); - size_t nEntries = variableIndex.nEntries(), nFactors = + const size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size(); // Convert to compressed column major format colamd wants it in (== MATLAB format!) - size_t Alen = ccolamd_recommended((int) nEntries, (int) nFactors, + const size_t Alen = ccolamd_recommended((int) nEntries, (int) nFactors, (int) nVars); /* colamd arg 3: size of the array A */ vector A = vector(Alen); /* colamd arg 4: row indices of A, of size Alen */ vector p = vector(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */ @@ -66,13 +66,10 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, int count = 0; vector keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order size_t index = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) { + for (auto key_factors: variableIndex) { // Arrange factor indices into COLAMD format const VariableIndex::Factors& column = key_factors.second; - size_t lastFactorId = numeric_limits::max(); - BOOST_FOREACH(size_t factorIndex, column) { - if (lastFactorId != numeric_limits::max()) - assert(factorIndex > lastFactorId); + for(size_t factorIndex: column) { A[count++] = (int) factorIndex; // copy sparse column } p[index + 1] = count; // column j (base 1) goes from A[j-1] to A[j]-1 @@ -106,8 +103,8 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, // ccolamd_report(stats); - gttic(Fill_Ordering); // Convert elimination ordering in p to an ordering + gttic(Fill_Ordering); Ordering result; result.resize(nVars); for (size_t j = 0; j < nVars; ++j) @@ -128,13 +125,13 @@ Ordering Ordering::ColamdConstrainedLast(const VariableIndex& variableIndex, // Build a mapping to look up sorted Key indices by Key FastMap keyIndices; size_t j = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) + for (auto key_factors: variableIndex) keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); // If at least some variables are not constrained to be last, constrain the // ones that should be constrained. int group = (constrainLast.size() != n ? 1 : 0); - BOOST_FOREACH(Key key, constrainLast) { + for (Key key: constrainLast) { cmember[keyIndices.at(key)] = group; if (forceOrder) ++group; @@ -155,13 +152,13 @@ Ordering Ordering::ColamdConstrainedFirst(const VariableIndex& variableIndex, // Build a mapping to look up sorted Key indices by Key FastMap keyIndices; size_t j = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) + for (auto key_factors: variableIndex) keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); // If at least some variables are not constrained to be last, constrain the // ones that should be constrained. int group = 0; - BOOST_FOREACH(Key key, constrainFirst) { + for (Key key: constrainFirst) { cmember[keyIndices.at(key)] = group; if (forceOrder) ++group; @@ -169,7 +166,7 @@ Ordering Ordering::ColamdConstrainedFirst(const VariableIndex& variableIndex, if (!forceOrder && !constrainFirst.empty()) ++group; - BOOST_FOREACH(int& c, cmember) + for(int& c: cmember) if (c == none) c = group; @@ -186,12 +183,12 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, // Build a mapping to look up sorted Key indices by Key FastMap keyIndices; size_t j = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) + for (auto key_factors: variableIndex) keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); // Assign groups typedef FastMap::value_type key_group; - BOOST_FOREACH(const key_group& p, groups) { + for(const key_group& p: groups) { // FIXME: check that no groups are skipped cmember[keyIndices.at(p.first)] = p.second; } diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index c209d086b..52528ad7f 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -172,6 +172,7 @@ TEST(Ordering, csr_format_3) { } /* ************************************************************************* */ +#ifdef GTSAM_SUPPORT_NESTED_DISSECTION TEST(Ordering, csr_format_4) { SymbolicFactorGraph sfg; @@ -206,8 +207,9 @@ TEST(Ordering, csr_format_4) { Ordering metOrder2 = Ordering::Metis(sfg); } - +#endif /* ************************************************************************* */ +#ifdef GTSAM_SUPPORT_NESTED_DISSECTION TEST(Ordering, metis) { SymbolicFactorGraph sfg; @@ -228,8 +230,9 @@ TEST(Ordering, metis) { Ordering metis = Ordering::Metis(sfg); } - +#endif /* ************************************************************************* */ +#ifdef GTSAM_SUPPORT_NESTED_DISSECTION TEST(Ordering, MetisLoop) { // create linear graph @@ -261,7 +264,7 @@ TEST(Ordering, MetisLoop) { } #endif } - +#endif /* ************************************************************************* */ TEST(Ordering, Create) { @@ -280,6 +283,7 @@ TEST(Ordering, Create) { EXPECT(assert_equal(expected, actual)); } +#ifdef GTSAM_SUPPORT_NESTED_DISSECTION // METIS { Ordering actual = Ordering::Create(Ordering::METIS, sfg); @@ -289,6 +293,7 @@ TEST(Ordering, Create) { Ordering expected = Ordering(list_of(5)(3)(4)(1)(0)(2)); EXPECT(assert_equal(expected, actual)); } +#endif // CUSTOM CHECK_EXCEPTION(Ordering::Create(Ordering::CUSTOM, sfg), runtime_error); diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index feb477a4e..eefb6302f 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -183,7 +183,7 @@ namespace gtsam { if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front()); for (const_iterator it = beginParents(); it!= endParents(); it++) - gtsam::transposeMultiplyAdd(-1.0, Matrix(getA(it)), frontalVec, gy[*it]); + gy[*it] += -1.0 * Matrix(getA(it)).transpose() * frontalVec; // Scale by sigmas if(model_) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 9277f3903..1c55d293b 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -377,7 +377,7 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, vector y; y.reserve(size()); for (const_iterator it = begin(); it != end(); it++) - y.push_back(zero(getDim(it))); + y.push_back(Vector::Zero(getDim(it))); // Accessing the VectorValues one by one is expensive // So we will loop over columns to access x only once per column @@ -427,7 +427,7 @@ void HessianFactor::gradientAtZero(double* d) const { Vector HessianFactor::gradient(Key key, const VectorValues& x) const { Factor::const_iterator i = find(key); // Sum over G_ij*xj for all xj connecting to xi - Vector b = zero(x.at(key).size()); + Vector b = Vector::Zero(x.at(key).size()); for (Factor::const_iterator j = begin(); j != end(); ++j) { // Obtain Gij from the Hessian factor // Hessian factor only stores an upper triangular matrix, so be careful when i>j diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index fc1a7c841..7fecefe3c 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -543,7 +543,7 @@ void JacobianFactor::updateHessian(const FastVector& infoKeys, /* ************************************************************************* */ Vector JacobianFactor::operator*(const VectorValues& x) const { - Vector Ax = zero(Ab_.rows()); + Vector Ax = Vector::Zero(Ab_.rows()); if (empty()) return Ax; @@ -565,8 +565,7 @@ void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e, pair xi = x.tryInsert(j, Vector()); if (xi.second) xi.first->second = Vector::Zero(getDim(begin() + pos)); - gtsam::transposeMultiplyAdd(Ab_(pos), E, xi.first->second); - + xi.first->second += Ab_(pos).transpose()*E; } } @@ -595,7 +594,7 @@ void JacobianFactor::multiplyHessianAdd(double alpha, const double* x, double* y if (empty()) return; - Vector Ax = zero(Ab_.rows()); + Vector Ax = Vector::Zero(Ab_.rows()); /// Just iterate over all A matrices and multiply in correct config part (looping over keys) /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index 68e6a00f1..c0d294adf 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -115,7 +115,7 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F, // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T // See documentation in HessianFactor, we have A1 = -F, A2 = I_, b = B*u: // TODO: starts to seem more elaborate than straight-up KF equations? - Matrix M = inverse(Q), Ft = trans(F); + Matrix M = Q.inverse(), Ft = trans(F); Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M; Vector b = B * u, g2 = M * b, g1 = -Ft * g2; double f = dot(b, g2); @@ -147,7 +147,7 @@ KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H, KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H, const Vector& z, const Matrix& Q) const { Key k = step(p); - Matrix M = inverse(Q), Ht = trans(H); + Matrix M = Q.inverse(), Ht = trans(H); Matrix G = Ht * M * H; Vector g = Ht * M * z; double f = dot(z, M * z); diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index 63271401c..7db29d861 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -69,7 +69,7 @@ public: // Constructor KalmanFilter(size_t n, Factorization method = KALMANFILTER_DEFAULT_FACTORIZATION) : - n_(n), I_(eye(n_, n_)), function_( + n_(n), I_(Matrix::Identity(n_, n_)), function_( method == QR ? GaussianFactorGraph::Eliminate(EliminateQR) : GaussianFactorGraph::Eliminate(EliminateCholesky)) { } diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 31b059989..5bcf3d635 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -405,7 +405,7 @@ void Constrained::WhitenInPlace(Eigen::Block H) const { /* ************************************************************************* */ Constrained::shared_ptr Constrained::unit() const { - Vector sigmas = ones(dim()); + Vector sigmas = Vector::Ones(dim()); for (size_t i=0; i actualBD = factor.hessianBlockDiagonal(); LONGS_EQUAL(3,actualBD.size()); - EXPECT(assert_equal(1*eye(3),actualBD[5])); - EXPECT(assert_equal(4*eye(3),actualBD[10])); - EXPECT(assert_equal(9*eye(3),actualBD[15])); + EXPECT(assert_equal(1*I_3x3,actualBD[5])); + EXPECT(assert_equal(4*I_3x3,actualBD[10])); + EXPECT(assert_equal(9*I_3x3,actualBD[15])); } /* ************************************************************************* */ @@ -314,9 +314,9 @@ TEST(JacobianFactor, matrices) // hessianBlockDiagonal map actualBD = factor.hessianBlockDiagonal(); LONGS_EQUAL(3,actualBD.size()); - EXPECT(assert_equal(4*eye(3),actualBD[5])); - EXPECT(assert_equal(16*eye(3),actualBD[10])); - EXPECT(assert_equal(36*eye(3),actualBD[15])); + EXPECT(assert_equal(4*I_3x3,actualBD[5])); + EXPECT(assert_equal(16*I_3x3,actualBD[10])); + EXPECT(assert_equal(36*I_3x3,actualBD[15])); } /* ************************************************************************* */ @@ -324,7 +324,7 @@ TEST(JacobianFactor, operators ) { SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); - Matrix I = eye(2); + Matrix I = I_2x2; Vector b = Vector2(0.2,-0.1); JacobianFactor lf(1, -I, 2, I, b, sigma0_1); @@ -405,7 +405,7 @@ TEST(JacobianFactor, eliminate) gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); - Matrix zero3x3 = zeros(3,3); + Matrix zero3x3 = Matrix::Zero(3,3); Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); Vector9 b; b << b1, b0, b2; @@ -561,7 +561,7 @@ TEST ( JacobianFactor, constraint_eliminate1 ) { // construct a linear constraint Vector v(2); v(0)=1.2; v(1)=3.4; - JacobianFactor lc(1, eye(2), v, noiseModel::Constrained::All(2)); + JacobianFactor lc(1, I_2x2, v, noiseModel::Constrained::All(2)); // eliminate it pair @@ -572,7 +572,7 @@ TEST ( JacobianFactor, constraint_eliminate1 ) // verify conditional Gaussian Vector sigmas = Vector2(0.0, 0.0); - GaussianConditional expCG(1, v, eye(2), noiseModel::Diagonal::Sigmas(sigmas)); + GaussianConditional expCG(1, v, I_2x2, noiseModel::Diagonal::Sigmas(sigmas)); EXPECT(assert_equal(expCG, *actual.first)); } diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index 16be98306..c4da59496 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -51,7 +51,7 @@ TEST( KalmanFilter, constructor ) { EXPECT(assert_equal(x_initial, p1->mean())); Matrix Sigma = (Matrix(2, 2) << 0.01, 0.0, 0.0, 0.01).finished(); EXPECT(assert_equal(Sigma, p1->covariance())); - EXPECT(assert_equal(inverse(Sigma), p1->information())); + EXPECT(assert_equal(Sigma.inverse(), p1->information())); // Create one with a sharedGaussian KalmanFilter::State p2 = kf1.init(x_initial, Sigma); @@ -65,33 +65,33 @@ TEST( KalmanFilter, constructor ) { TEST( KalmanFilter, linear1 ) { // Create the controls and measurement properties for our example - Matrix F = eye(2, 2); - Matrix B = eye(2, 2); + Matrix F = I_2x2; + Matrix B = I_2x2; Vector u = Vector2(1.0, 0.0); SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1); - Matrix Q = 0.01*eye(2, 2); - Matrix H = eye(2, 2); + Matrix Q = 0.01*I_2x2; + Matrix H = I_2x2; State z1(1.0, 0.0); State z2(2.0, 0.0); State z3(3.0, 0.0); SharedDiagonal modelR = noiseModel::Isotropic::Sigma(2, 0.1); - Matrix R = 0.01*eye(2, 2); + Matrix R = 0.01*I_2x2; // Create the set of expected output TestValues State expected0(0.0, 0.0); - Matrix P00 = 0.01*eye(2, 2); + Matrix P00 = 0.01*I_2x2; State expected1(1.0, 0.0); Matrix P01 = P00 + Q; - Matrix I11 = inverse(P01) + inverse(R); + Matrix I11 = P01.inverse() + R.inverse(); State expected2(2.0, 0.0); - Matrix P12 = inverse(I11) + Q; - Matrix I22 = inverse(P12) + inverse(R); + Matrix P12 = I11.inverse() + Q; + Matrix I22 = P12.inverse() + R.inverse(); State expected3(3.0, 0.0); - Matrix P23 = inverse(I22) + Q; - Matrix I33 = inverse(P23) + inverse(R); + Matrix P23 = I22.inverse() + Q; + Matrix I33 = P23.inverse() + R.inverse(); // Create a Kalman filter of dimension 2 KalmanFilter kf(2); @@ -140,7 +140,7 @@ TEST( KalmanFilter, predict ) { Vector u = Vector3(1.0, 0.0, 2.0); Matrix R = (Matrix(2, 2) << 1.0, 0.5, 0.0, 3.0).finished(); Matrix M = trans(R)*R; - Matrix Q = inverse(M); + Matrix Q = M.inverse(); // Create a Kalman filter of dimension 2 KalmanFilter kf(2); @@ -167,7 +167,7 @@ TEST( KalmanFilter, predict ) { // Test both QR and Cholesky versions in case of a realistic (AHRS) dynamics update TEST( KalmanFilter, QRvsCholesky ) { - Vector mean = ones(9); + Vector mean = Vector::Ones(9); Matrix covariance = 1e-6 * (Matrix(9, 9) << 15.0, -6.2, 0.0, 0.0, 0.0, 0.0, 0.0, 63.8, -0.6, -6.2, 21.9, -0.0, 0.0, 0.0, 0.0, -63.8, -0.0, -0.1, @@ -197,8 +197,8 @@ TEST( KalmanFilter, QRvsCholesky ) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0).finished(); - Matrix B = zeros(9, 1); - Vector u = zero(1); + Matrix B = Matrix::Zero(9, 1); + Vector u = Z_1x1; Matrix dt_Q_k = 1e-6 * (Matrix(9, 9) << 33.7, 3.1, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.1, 126.4, -0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, @@ -270,7 +270,7 @@ TEST( KalmanFilter, QRvsCholesky ) { EXPECT(assert_equal(expected2, pb2->covariance(), 1e-7)); // do the above update again, this time with a full Matrix Q - Matrix modelQ = diag(sigmas.array().square()); + Matrix modelQ = ((Matrix) sigmas.array().square()).asDiagonal(); KalmanFilter::State pa3 = kfa.updateQ(pa, H, z, modelQ); KalmanFilter::State pb3 = kfb.updateQ(pb, H, z, modelQ); diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 434d94d7f..43ce271f3 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -112,7 +112,7 @@ TEST(NoiseModel, Unit) TEST(NoiseModel, equals) { Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R), - g2 = Gaussian::SqrtInformation(eye(3,3)); + g2 = Gaussian::SqrtInformation(I_3x3); Diagonal::shared_ptr d1 = Diagonal::Sigmas(Vector3(kSigma, kSigma, kSigma)), d2 = Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3)); Isotropic::shared_ptr i1 = Isotropic::Sigma(3, kSigma), @@ -395,7 +395,7 @@ TEST(NoiseModel, SmartSqrtInformation ) { bool smart = true; gtsam::SharedGaussian expected = Unit::Create(3); - gtsam::SharedGaussian actual = Gaussian::SqrtInformation(eye(3), smart); + gtsam::SharedGaussian actual = Gaussian::SqrtInformation(I_3x3, smart); EXPECT(assert_equal(*expected,*actual)); } @@ -404,7 +404,7 @@ TEST(NoiseModel, SmartSqrtInformation2 ) { bool smart = true; gtsam::SharedGaussian expected = Unit::Isotropic::Sigma(3,2); - gtsam::SharedGaussian actual = Gaussian::SqrtInformation(0.5*eye(3), smart); + gtsam::SharedGaussian actual = Gaussian::SqrtInformation(0.5*I_3x3, smart); EXPECT(assert_equal(*expected,*actual)); } @@ -413,7 +413,7 @@ TEST(NoiseModel, SmartInformation ) { bool smart = true; gtsam::SharedGaussian expected = Unit::Isotropic::Variance(3,2); - Matrix M = 0.5*eye(3); + Matrix M = 0.5*I_3x3; EXPECT(checkIfDiagonal(M)); gtsam::SharedGaussian actual = Gaussian::Information(M, smart); EXPECT(assert_equal(*expected,*actual)); @@ -424,7 +424,7 @@ TEST(NoiseModel, SmartCovariance ) { bool smart = true; gtsam::SharedGaussian expected = Unit::Create(3); - gtsam::SharedGaussian actual = Gaussian::Covariance(eye(3), smart); + gtsam::SharedGaussian actual = Gaussian::Covariance(I_3x3, smart); EXPECT(assert_equal(*expected,*actual)); } @@ -433,7 +433,7 @@ TEST(NoiseModel, ScalarOrVector ) { bool smart = true; SharedGaussian expected = Unit::Create(3); - SharedGaussian actual = Gaussian::Covariance(eye(3), smart); + SharedGaussian actual = Gaussian::Covariance(I_3x3, smart); EXPECT(assert_equal(*expected,*actual)); } @@ -442,9 +442,9 @@ TEST(NoiseModel, WhitenInPlace) { Vector sigmas = Vector3(0.1, 0.1, 0.1); SharedDiagonal model = Diagonal::Sigmas(sigmas); - Matrix A = eye(3); + Matrix A = I_3x3; model->WhitenInPlace(A); - Matrix expected = eye(3) * 10; + Matrix expected = I_3x3 * 10; EXPECT(assert_equal(expected, A)); } diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp index 71fdbe6a6..20fe3ac40 100644 --- a/gtsam/linear/tests/testSerializationLinear.cpp +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -51,7 +51,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); /* ************************************************************************* */ // example noise models static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3)); -static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3)); +static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * I_3x3); static noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2); static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector3(0.0, 0.0, 0.1)); static noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3); @@ -144,8 +144,8 @@ TEST (Serialization, linear_factors) { EXPECT(equalsBinary(values)); Key i1 = 4, i2 = 7; - Matrix A1 = eye(3), A2 = -1.0 * eye(3); - Vector b = ones(3); + Matrix A1 = I_3x3, A2 = -1.0 * I_3x3; + Vector b = Vector::Ones(3); SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(1.0, 2.0, 3.0)); JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model); EXPECT(equalsObj(jacobianfactor)); @@ -185,8 +185,8 @@ TEST (Serialization, gaussian_factor_graph) { { Key i1 = 4, i2 = 7; - Matrix A1 = eye(3), A2 = -1.0 * eye(3); - Vector b = ones(3); + Matrix A1 = I_3x3, A2 = -1.0 * I_3x3; + Vector b = Vector::Ones(3); SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(1.0, 2.0, 3.0)); JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model); HessianFactor hessianfactor(jacobianfactor); diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index b61a861d6..21f74ac06 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -189,7 +189,7 @@ public: Vector e = attitudeError(nTb.rotation(), H); if (H) { Matrix H23 = *H; - *H = Matrix::Zero(2, 6); + *H = Matrix::Zero(2,6); H->block<2,3>(0,0) = H23; } return e; diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index bee3e8944..3875391d0 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -154,7 +154,7 @@ public: // measured bM = nRb� * nM + b, where b is unknown bias Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias; if (H2) - *H2 = eye(3); + *H2 = I_3x3; return (hx - measured_); } }; @@ -205,7 +205,7 @@ public: *H2 = scale * H * (*H2); } if (H3) - *H3 = eye(3); + *H3 = I_3x3; return (hx - measured_); } }; diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index b170ea863..88d9c6437 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -26,6 +26,38 @@ namespace gtsam { +/// Parameters for pre-integration: +/// Usage: Create just a single Params and pass a shared pointer to the constructor +struct PreintegratedRotationParams { + Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements + boost::optional omegaCoriolis; ///< Coriolis constant + boost::optional body_P_sensor; ///< The pose of the sensor in the body frame + + PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {} + + virtual void print(const std::string& s) const; + virtual bool equals(const PreintegratedRotationParams& other, double tol=1e-9) const; + + void setGyroscopeCovariance(const Matrix3& cov) { gyroscopeCovariance = cov; } + void setOmegaCoriolis(const Vector3& omega) { omegaCoriolis.reset(omega); } + void setBodyPSensor(const Pose3& pose) { body_P_sensor.reset(pose); } + + const Matrix3& getGyroscopeCovariance() const { return gyroscopeCovariance; } + boost::optional getOmegaCoriolis() const { return omegaCoriolis; } + boost::optional getBodyPSensor() const { return body_P_sensor; } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size())); + ar & BOOST_SERIALIZATION_NVP(omegaCoriolis); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor); + } +}; + /** * PreintegratedRotation is the base class for all PreintegratedMeasurements * classes (in AHRSFactor, ImuFactor, and CombinedImuFactor). @@ -33,29 +65,7 @@ namespace gtsam { */ class PreintegratedRotation { public: - /// Parameters for pre-integration: - /// Usage: Create just a single Params and pass a shared pointer to the constructor - struct Params { - Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements - boost::optional omegaCoriolis; ///< Coriolis constant - boost::optional body_P_sensor; ///< The pose of the sensor in the body frame - - Params() : gyroscopeCovariance(I_3x3) {} - - virtual void print(const std::string& s) const; - virtual bool equals(const Params& other, double tol=1e-9) const; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - namespace bs = ::boost::serialization; - ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size())); - ar & BOOST_SERIALIZATION_NVP(omegaCoriolis); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor); - } - }; + typedef PreintegratedRotationParams Params; protected: /// Parameters diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index fefd4b23c..c243ca860 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -105,7 +105,7 @@ pair PreintegrationBase::correctMeasurementsBySensorPose( // Update derivative: centrifugal causes the correlation between acc and omega!!! if (correctedAcc_H_unbiasedOmega) { double wdp = correctedOmega.dot(b_arm); - *correctedAcc_H_unbiasedOmega = -(diag(Vector3::Constant(wdp)) + *correctedAcc_H_unbiasedOmega = -( (Matrix) Vector3::Constant(wdp).asDiagonal() + correctedOmega * b_arm.transpose()) * bRs.matrix() + 2 * b_arm * unbiasedOmega.transpose(); } diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h index f68f711f1..f2b2c0fef 100644 --- a/gtsam/navigation/PreintegrationParams.h +++ b/gtsam/navigation/PreintegrationParams.h @@ -23,7 +23,7 @@ namespace gtsam { /// Parameters for pre-integration: /// Usage: Create just a single Params and pass a shared pointer to the constructor -struct PreintegrationParams: PreintegratedRotation::Params { +struct PreintegrationParams: PreintegratedRotationParams { Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration @@ -50,6 +50,14 @@ struct PreintegrationParams: PreintegratedRotation::Params { void print(const std::string& s) const; bool equals(const PreintegratedRotation::Params& other, double tol) const; + void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; } + void setIntegrationCovariance(const Matrix3& cov) { integrationCovariance = cov; } + void setUse2ndOrderCoriolis(bool flag) { use2ndOrderCoriolis = flag; } + + const Matrix3& getAccelerometerCovariance() const { return accelerometerCovariance; } + const Matrix3& getIntegrationCovariance() const { return integrationCovariance; } + bool getUse2ndOrderCoriolis() const { return use2ndOrderCoriolis; } + protected: /// Default constructor for serialization only: uninitialized! PreintegrationParams() {} @@ -60,7 +68,7 @@ protected: void serialize(ARCHIVE & ar, const unsigned int /*version*/) { namespace bs = ::boost::serialization; ar & boost::serialization::make_nvp("PreintegratedRotation_Params", - boost::serialization::base_object(*this)); + boost::serialization::base_object(*this)); ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size())); ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size())); ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis); diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index 790080556..f7f0aa9ad 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -42,7 +42,7 @@ TEST( Rot3AttitudeFactor, Constructor ) { // Create a linearization point at the zero-error point Rot3 nRb; - EXPECT(assert_equal(zero(2),factor.evaluateError(nRb),1e-5)); + EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(nRb),1e-5)); // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( @@ -75,7 +75,7 @@ TEST( Pose3AttitudeFactor, Constructor ) { // Create a linearization point at the zero-error point Pose3 T(Rot3(), Point3(-5.0, 8.0, -11.0)); - EXPECT(assert_equal(zero(2),factor.evaluateError(T),1e-5)); + EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index de318b3e4..293bffa00 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -58,7 +58,7 @@ TEST( GPSFactor, Constructor ) { // Create a linearization point at zero error Pose3 T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U)); - EXPECT(assert_equal(zero(3),factor.evaluateError(T),1e-5)); + EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( @@ -87,7 +87,7 @@ TEST( GPSFactor2, Constructor ) { // Create a linearization point at zero error NavState T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U), Vector3::Zero()); - EXPECT(assert_equal(zero(3),factor.evaluateError(T),1e-5)); + EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 38aecfcbc..45db58e33 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -71,19 +71,19 @@ TEST( MagFactor, Factors ) { // MagFactor MagFactor f(1, measured, s, dir, bias, model); - EXPECT( assert_equal(zero(3),f.evaluateError(theta,H1),1e-5)); + EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5)); EXPECT( assert_equal((Matrix)numericalDerivative11 // (boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); // MagFactor1 MagFactor1 f1(1, measured, s, dir, bias, model); - EXPECT( assert_equal(zero(3),f1.evaluateError(nRb,H1),1e-5)); + EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5)); EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); // MagFactor2 MagFactor2 f2(1, 2, measured, nRb, model); - EXPECT( assert_equal(zero(3),f2.evaluateError(scaled,bias,H1,H2),1e-5)); + EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5)); EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// H1, 1e-7)); @@ -93,7 +93,7 @@ TEST( MagFactor, Factors ) { // MagFactor2 MagFactor3 f3(1, 2, 3, measured, nRb, model); - EXPECT(assert_equal(zero(3),f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); + EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); EXPECT(assert_equal((Matrix)numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// H1, 1e-7)); diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index e24c0d610..0d8d717bc 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -142,12 +142,12 @@ public: const size_t nj = traits::GetDimension(feasible_); if (allow_error_) { if (H) - *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare + *H = Matrix::Identity(nj,nj); // FIXME: this is not the right linearization for nonlinear compare return traits::Local(xj,feasible_); } else if (compare_(feasible_, xj)) { if (H) - *H = eye(nj); - return zero(nj); // set error to zero if equal + *H = Matrix::Identity(nj,nj); + return Vector::Zero(nj); // set error to zero if equal } else { if (H) throw std::invalid_argument( @@ -249,7 +249,7 @@ public: Vector evaluateError(const X& x1, boost::optional H = boost::none) const { if (H) - (*H) = eye(traits::GetDimension(x1)); + (*H) = Matrix::Identity(traits::GetDimension(x1),traits::GetDimension(x1)); // manifold equivalent of h(x)-z -> log(z,h(x)) return traits::Local(value_,x1); } @@ -322,8 +322,8 @@ public: Vector evaluateError(const X& x1, const X& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { static const size_t p = traits::dimension; - if (H1) *H1 = -eye(p); - if (H2) *H2 = eye(p); + if (H1) *H1 = -Matrix::Identity(p,p); + if (H2) *H2 = Matrix::Identity(p,p); return traits::Local(x1,x2); } diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index ec77b2bd6..505f58394 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -313,7 +313,7 @@ public: return evaluateError(x1); } } else { - return zero(this->dim()); + return Vector::Zero(this->dim()); } } @@ -388,7 +388,7 @@ public: return evaluateError(x1, x2); } } else { - return zero(this->dim()); + return Vector::Zero(this->dim()); } } @@ -463,7 +463,7 @@ public: else return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2])); } else { - return zero(this->dim()); + return Vector::Zero(this->dim()); } } @@ -543,7 +543,7 @@ public: else return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3])); } else { - return zero(this->dim()); + return Vector::Zero(this->dim()); } } @@ -627,7 +627,7 @@ public: else return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4])); } else { - return zero(this->dim()); + return Vector::Zero(this->dim()); } } @@ -715,7 +715,7 @@ public: else return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), x.at(keys_[5])); } else { - return zero(this->dim()); + return Vector::Zero(this->dim()); } } diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 0008252c4..446a67ec7 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -269,25 +269,114 @@ namespace gtsam { return filter(key_value.key); } - /* ************************************************************************* */ - template - const ValueType& Values::at(Key j) const { - // Find the item - KeyValueMap::const_iterator item = values_.find(j); + /* ************************************************************************* */ - // Throw exception if it does not exist - if(item == values_.end()) - throw ValuesKeyDoesNotExist("retrieve", j); + namespace internal { // Check the type and throw exception if incorrect - const Value& value = *item->second; - try { - return dynamic_cast&>(value).value(); - } catch (std::bad_cast &) { - // NOTE(abe): clang warns about potential side effects if done in typeid - const Value* value = item->second; - throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType)); - } + // Generic version, partially specialized below for various Eigen Matrix types + template + struct handle { + ValueType operator()(Key j, const gtsam::Value * const pointer) { + try { + // value returns a const ValueType&, and the return makes a copy !!!!! + return dynamic_cast&>(*pointer).value(); + } catch (std::bad_cast &) { + throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType)); + } + } + }; + + // Handle dynamic vectors + template<> + struct handle > { + Eigen::Matrix operator()(Key j, + const gtsam::Value * const pointer) { + try { + // value returns a const Vector&, and the return makes a copy !!!!! + return dynamic_cast >&>(*pointer).value(); + } catch (std::bad_cast &) { + // If a fixed vector was stored, we end up here as well. + throw ValuesIncorrectType(j, typeid(*pointer), + typeid(Eigen::Matrix)); + } + } + }; + + // Handle dynamic matrices + template + struct handle > { + Eigen::Matrix operator()(Key j, + const gtsam::Value * const pointer) { + try { + // value returns a const Matrix&, and the return makes a copy !!!!! + return dynamic_cast >&>(*pointer).value(); + } catch (std::bad_cast &) { + // If a fixed matrix was stored, we end up here as well. + throw ValuesIncorrectType(j, typeid(*pointer), + typeid(Eigen::Matrix)); + } + } + }; + + // Request for a fixed vector + // TODO(jing): is this piece of code really needed ??? + template + struct handle > { + Eigen::Matrix operator()(Key j, + const gtsam::Value * const pointer) { + try { + // value returns a const VectorM&, and the return makes a copy !!!!! + return dynamic_cast >&>(*pointer).value(); + } catch (std::bad_cast &) { + // Check if a dynamic vector was stored + Matrix A = handle()(j, pointer); // will throw if not.... + // Yes: check size, and throw if not a match + if (A.rows() != M || A.cols() != 1) + throw NoMatchFoundForFixed(M, 1, A.rows(), A.cols()); + else + // This is not a copy because of RVO + return A; + } + } + }; + + // Request for a fixed matrix + template + struct handle > { + Eigen::Matrix operator()(Key j, + const gtsam::Value * const pointer) { + try { + // value returns a const MatrixMN&, and the return makes a copy !!!!! + return dynamic_cast >&>(*pointer).value(); + } catch (std::bad_cast &) { + // Check if a dynamic matrix was stored + Matrix A = handle()(j, pointer); // will throw if not.... + // Yes: check size, and throw if not a match + if (A.rows() != M || A.cols() != N) + throw NoMatchFoundForFixed(M, N, A.rows(), A.cols()); + else + // This is not a copy because of RVO + return A; + } + } + }; + + + } // internal + + /* ************************************************************************* */ + template + ValueType Values::at(Key j) const { + // Find the item + KeyValueMap::const_iterator item = values_.find(j); + + // Throw exception if it does not exist + if(item == values_.end()) + throw ValuesKeyDoesNotExist("at", j); + + // Check the type and throw exception if incorrect + return internal::handle()(j,item->second); } /* ************************************************************************* */ @@ -312,16 +401,48 @@ namespace gtsam { } /* ************************************************************************* */ + + // wrap all fixed in dynamics when insert and update + namespace internal { + + // general type + template + struct handle_wrap { + inline gtsam::GenericValue operator()(Key j, const ValueType& val) { + return gtsam::GenericValue(val); + } + }; + + // when insert/update a fixed size vector: convert to dynamic size + template + struct handle_wrap > { + inline gtsam::GenericValue > operator()( + Key j, const Eigen::Matrix& val) { + return gtsam::GenericValue >(val); + } + }; + + // when insert/update a fixed size matrix: convert to dynamic size + template + struct handle_wrap > { + inline gtsam::GenericValue > operator()( + Key j, const Eigen::Matrix& val) { + return gtsam::GenericValue >(val); + } + }; + + } + // insert a templated value template - void Values::insert(Key j, const ValueType& val) { - insert(j, static_cast(GenericValue(val))); - } + void Values::insert(Key j, const ValueType& val) { + insert(j, static_cast(internal::handle_wrap()(j, val))); + } // update with templated value template void Values::update(Key j, const ValueType& val) { - update(j, static_cast(GenericValue(val))); + update(j, static_cast(internal::handle_wrap()(j, val))); } } diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 07757062c..ba7a040cd 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -25,8 +25,6 @@ #include #include -#include - #include #ifdef __GNUC__ #pragma GCC diagnostic push @@ -38,6 +36,9 @@ #endif #include +#include +#include + using namespace std; namespace gtsam { @@ -112,24 +113,6 @@ namespace gtsam { return result; } - /* ************************************************************************* */ - Vector Values::atFixed(Key j, size_t n) { - switch (n) { - case 1: return at(j); - case 2: return at(j); - case 3: return at(j); - case 4: return at(j); - case 5: return at(j); - case 6: return at(j); - case 7: return at(j); - case 8: return at(j); - case 9: return at(j); - default: - throw runtime_error( - "Values::at fixed size can only handle n in 1..9"); - } - } - /* ************************************************************************* */ const Value& Values::at(Key j) const { // Find the item @@ -148,24 +131,6 @@ namespace gtsam { throw ValuesKeyAlreadyExists(j); } - /* ************************************************************************* */ - void Values::insertFixed(Key j, const Vector& v, size_t n) { - switch (n) { - case 1: insert(j,v); break; - case 2: insert(j,v); break; - case 3: insert(j,v); break; - case 4: insert(j,v); break; - case 5: insert(j,v); break; - case 6: insert(j,v); break; - case 7: insert(j,v); break; - case 8: insert(j,v); break; - case 9: insert(j,v); break; - default: - throw runtime_error( - "Values::insert fixed size can only handle n in 1..9"); - } - } - /* ************************************************************************* */ void Values::insert(const Values& values) { for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { @@ -269,4 +234,18 @@ namespace gtsam { return message_.c_str(); } + /* ************************************************************************* */ + const char* NoMatchFoundForFixed::what() const throw() { + if(message_.empty()) { + ostringstream oss; + oss + << "Attempting to retrieve fixed-size matrix with dimensions " // + << M1_ << "x" << N1_ + << ", but found dynamic Matrix with mismatched dimensions " // + << M2_ << "x" << N2_; + message_ = oss.str(); + } + return message_.c_str(); + } + } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 70aadfa06..a61d01f23 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -168,16 +168,13 @@ namespace gtsam { /** Retrieve a variable by key \c j. The type of the value associated with * this key is supplied as a template argument to this function. * @param j Retrieve the value associated with this key - * @tparam Value The type of the value stored with this key, this method - * throws DynamicValuesIncorrectType if this requested type is not correct. - * @return A const reference to the stored value + * @tparam ValueType The type of the value stored with this key, this method + * Throws DynamicValuesIncorrectType if this requested type is not correct. + * Dynamic matrices/vectors can be retrieved as fixed-size, but not vice-versa. + * @return The stored value */ template - const ValueType& at(Key j) const; - - /// Special version for small fixed size vectors, for matlab/python - /// throws truntime error if n<1 || n>9 - Vector atFixed(Key j, size_t n); + ValueType at(Key j) const; /// version for double double atDouble(size_t key) const { return at(key);} @@ -259,10 +256,6 @@ namespace gtsam { template void insert(Key j, const ValueType& val); - /// Special version for small fixed size vectors, for matlab/python - /// throws truntime error if n<1 || n>9 - void insertFixed(Key j, const Vector& v, size_t n); - /// version for double void insertDouble(Key j, double c) { insert(j,c); } @@ -500,6 +493,28 @@ namespace gtsam { } }; + /* ************************************************************************* */ + class GTSAM_EXPORT NoMatchFoundForFixed: public std::exception { + + protected: + const size_t M1_, N1_; + const size_t M2_, N2_; + + private: + mutable std::string message_; + + public: + NoMatchFoundForFixed(size_t M1, size_t N1, size_t M2, size_t N2) throw () : + M1_(M1), N1_(N1), M2_(M2), N2_(N2) { + } + + virtual ~NoMatchFoundForFixed() throw () { + } + + virtual const char* what() const throw (); + }; + + /* ************************************************************************* */ /// traits template<> struct traits : public Testable { diff --git a/gtsam/nonlinear/expressions.h b/gtsam/nonlinear/expressions.h index 5b591bcf0..19fd257e8 100644 --- a/gtsam/nonlinear/expressions.h +++ b/gtsam/nonlinear/expressions.h @@ -25,7 +25,7 @@ Expression compose(const Expression& t1, const Expression& t2) { } // Some typedefs -typedef Expression double_; +typedef Expression Double_; typedef Expression Vector1_; typedef Expression Vector2_; typedef Expression Vector3_; diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 43f1cd017..dece9bad8 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -17,7 +17,7 @@ * @brief unit tests for Block Automatic Differentiation */ -#include +#include #include #include #include @@ -32,9 +32,7 @@ using boost::assign::map_list_of; using namespace std; using namespace gtsam; -typedef Expression double_; typedef Expression Point3_; -typedef Expression Vector3_; typedef Expression Pose3_; typedef Expression Rot3_; @@ -101,7 +99,7 @@ TEST(Expression, Unary1) { } TEST(Expression, Unary2) { using namespace unary; - double_ e(f2, p); + Double_ e(f2, p); EXPECT(expected == e.keys()); } @@ -156,7 +154,7 @@ Point3_ p_cam(x, &Pose3::transform_to, p); // Check that creating an expression to double compiles TEST(Expression, BinaryToDouble) { using namespace binary; - double_ p_cam(doubleF, x, p); + Double_ p_cam(doubleF, x, p); } /* ************************************************************************* */ @@ -269,11 +267,11 @@ Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobi OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) if (H1) - *H1 = eye(3); + *H1 = I_3x3; if (H2) - *H2 = eye(3); + *H2 = I_3x3; if (H3) - *H3 = eye(3); + *H3 = I_3x3; return R1 * (R2 * R3); } @@ -372,8 +370,8 @@ TEST(Expression, TripleSum) { /* ************************************************************************* */ TEST(Expression, SumOfUnaries) { const Key key(67); - const double_ norm_(>sam::norm, Point3_(key)); - const double_ sum_ = norm_ + norm_; + const Double_ norm_(>sam::norm, Point3_(key)); + const Double_ sum_ = norm_ + norm_; Values values; values.insert(key, Point3(6, 0, 0)); @@ -391,7 +389,7 @@ TEST(Expression, SumOfUnaries) { TEST(Expression, UnaryOfSum) { const Key key1(42), key2(67); const Point3_ sum_ = Point3_(key1) + Point3_(key2); - const double_ norm_(>sam::norm, sum_); + const Double_ norm_(>sam::norm, sum_); map actual_dims, expected_dims = map_list_of(key1, 3)(key2, 3); norm_.dims(actual_dims); diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index bd862ef94..df1df0d20 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -234,7 +234,7 @@ TEST( testLinearContainerFactor, creation ) { // create a linear factor SharedDiagonal model = noiseModel::Unit::Create(2); JacobianFactor::shared_ptr linear_factor(new JacobianFactor( - l3, eye(2,2), l5, 2.0 * eye(2,2), zero(2), model)); + l3, I_2x2, l5, 2.0 * I_2x2, Z_2x1, model)); // create a set of values - build with full set of values gtsam::Values full_values, exp_values; diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index dfc14e1e6..8c1c85038 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -477,13 +477,59 @@ TEST(Values, Destructors) { } /* ************************************************************************* */ -TEST(Values, FixedSize) { +TEST(Values, VectorDynamicInsertFixedRead) { Values values; Vector v(3); v << 5.0, 6.0, 7.0; - values.insertFixed(key1, v, 3); + values.insert(key1, v); Vector3 expected(5.0, 6.0, 7.0); - CHECK(assert_equal((Vector)expected, values.at(key1))); - CHECK_EXCEPTION(values.insertFixed(key1, v, 12),runtime_error); + Vector3 actual = values.at(key1); + CHECK(assert_equal(expected, actual)); + CHECK_EXCEPTION(values.at(key1), exception); +} + +/* ************************************************************************* */ +TEST(Values, VectorDynamicInsertDynamicRead) { + Values values; + Vector v(3); v << 5.0, 6.0, 7.0; + values.insert(key1, v); + Vector expected(3); expected << 5.0, 6.0, 7.0; + Vector actual = values.at(key1); + LONGS_EQUAL(3, actual.rows()); + LONGS_EQUAL(1, actual.cols()); + CHECK(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(Values, VectorFixedInsertFixedRead) { + Values values; + Vector3 v; v << 5.0, 6.0, 7.0; + values.insert(key1, v); + Vector3 expected; expected << 5.0, 6.0, 7.0; + Vector3 actual = values.at(key1); + CHECK(assert_equal(expected, actual)); + CHECK_EXCEPTION(values.at(key1), exception); +} + +/* ************************************************************************* */ +TEST(Values, VectorFixedInsertDynamicRead) { + Values values; + Vector3 v; v << 5.0, 6.0, 7.0; + values.insert(key1, v); + Vector expected(3); expected << 5.0, 6.0, 7.0; + Vector actual = values.at(key1); + LONGS_EQUAL(3, actual.rows()); + LONGS_EQUAL(1, actual.cols()); + CHECK(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(Values, MatrixDynamicInsertFixedRead) { + Values values; + Matrix v(1,3); v << 5.0, 6.0, 7.0; + values.insert(key1, v); + Vector3 expected(5.0, 6.0, 7.0); + CHECK(assert_equal((Vector)expected, values.at(key1))); + CHECK_EXCEPTION(values.at(key1), exception); } /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index d2b042fed..0f187db75 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -129,7 +129,7 @@ public: if (H1) *H1 = JacobianC::Zero(); if (H2) *H2 = JacobianL::Zero(); // TODO warn if verbose output asked for - return zero(2); + return Z_2x1; } } @@ -266,13 +266,13 @@ public: return reprojError.vector(); } catch( CheiralityException& e) { - if (H1) *H1 = zeros(2, 6); - if (H2) *H2 = zeros(2, 3); - if (H3) *H3 = zeros(2, DimK); + if (H1) *H1 = Matrix::Zero(2, 6); + if (H2) *H2 = Matrix::Zero(2, 3); + if (H3) *H3 = Matrix::Zero(2, DimK); std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; } - return zero(2); + return Z_2x1; } /** return the measured */ diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index b93fe1fc1..180e5cd24 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -31,10 +31,9 @@ using namespace std; namespace gtsam { namespace InitializePose3 { -//static const Matrix I = eye(1); -static const Matrix I9 = eye(9); +static const Matrix I9 = I_9x9; static const Vector zero9 = Vector::Zero(9); -static const Matrix zero33= Matrix::Zero(3,3); +static const Matrix zero33 = Z_3x3; static const Key keyAnchor = symbol('Z', 9999999); @@ -54,11 +53,9 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { else std::cout << "Error in buildLinearOrientationGraph" << std::endl; - // std::cout << "Rij \n" << Rij << std::endl; - const FastVector& keys = factor->keys(); Key key1 = keys[0], key2 = keys[1]; - Matrix M9 = Matrix::Zero(9,9); + Matrix M9 = Z_9x9; M9.block(0,0,3,3) = Rij; M9.block(3,3,3,3) = Rij; M9.block(6,6,3,3) = Rij; @@ -74,7 +71,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { gttic(InitializePose3_computeOrientationsChordal); - Matrix ppm = Matrix::Zero(3,3); // plus plus minus + Matrix ppm = Z_3x3; // plus plus minus ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1; Values validRot3; diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index 16560a43e..f9f258055 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -56,7 +56,7 @@ public: Base() { size_t j = 0, m2 = E.rows(), m = m2 / ZDim; // Calculate projector Q - Matrix Q = eye(m2) - E * P * E.transpose(); + Matrix Q = Matrix::Identity(m2,m2) - E * P * E.transpose(); // Calculate pre-computed Jacobian matrices // TODO: can we do better ? std::vector QF; diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index 3728e53b1..5b0c6a6b9 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -47,7 +47,7 @@ Vector OrientedPlane3DirectionPrior::evaluateError(const OrientedPlane3& plane, Vector e = n_hat_p.error(n_hat_q, H_p); H->resize(2, 3); H->block<2, 2>(0, 0) << H_p; - H->block<2, 1>(0, 2) << Matrix::Zero(2, 1); + H->block<2, 1>(0, 2) << Z_2x1; return e; } else { Unit3 n_hat_p = measured_p_.normal(); diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index 94c19a9d0..44f317a49 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -75,7 +75,7 @@ public: Vector evaluateError(const Pose& pose, boost::optional H = boost::none) const { const Rotation& newR = pose.rotation(); if (H) { - *H = gtsam::zeros(rDim, xDim); + *H = Matrix::Zero(rDim, xDim); std::pair rotInterval = POSE::rotationInterval(); (*H).middleCols(rotInterval.first, rDim).setIdentity(rDim, rDim); } diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index a24421b34..0b1c0cf63 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -65,7 +65,7 @@ public: const int tDim = traits::GetDimension(newTrans); const int xDim = traits::GetDimension(pose); if (H) { - *H = gtsam::zeros(tDim, xDim); + *H = Matrix::Zero(tDim, xDim); std::pair transInterval = POSE::translationInterval(); (*H).middleCols(transInterval.first, tDim) = R.matrix(); } diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 8305fce12..9a3a4a47a 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -86,7 +86,7 @@ namespace gtsam { /** vector of errors */ Vector evaluateError(const T& x, boost::optional H = boost::none) const { - if (H) (*H) = eye(traits::GetDimension(x)); + if (H) (*H) = Matrix::Identity(traits::GetDimension(x),traits::GetDimension(x)); // manifold equivalent of z-x -> Local(x,z) // TODO(ASL) Add Jacobians. return -traits::Local(x, prior_); diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 1056527d1..dee8e925f 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -146,15 +146,15 @@ namespace gtsam { return reprojectionError.vector(); } } catch( CheiralityException& e) { - if (H1) *H1 = zeros(2,6); - if (H2) *H2 = zeros(2,3); + if (H1) *H1 = Matrix::Zero(2,6); + if (H2) *H2 = Matrix::Zero(2,3); if (verboseCheirality_) std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; if (throwCheirality_) throw e; } - return ones(2) * 2.0 * K_->fx(); + return Vector2::Constant(2.0 * K_->fx()); } /** return the measurement */ diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 90ceeafc8..966ade343 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -100,7 +100,7 @@ public: boost::optional Dlocal = boost::none) const { Point newlocal = transform_point(trans, global, Dtrans, Dforeign); if (Dlocal) - *Dlocal = -1* gtsam::eye(Point::dimension); + *Dlocal = -1* Matrix::Identity(Point::dimension,Point::dimension); return traits::Local(local,newlocal); } diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index a5d5d1883..01f89e526 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -291,9 +291,9 @@ public: if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) { // failed: return"empty" Hessian BOOST_FOREACH(Matrix& m, Gs) - m = zeros(Base::Dim, Base::Dim); + m = Matrix::Zero(Base::Dim, Base::Dim); BOOST_FOREACH(Vector& v, gs) - v = zero(Base::Dim); + v = Vector::Zero(Base::Dim); return boost::make_shared >(this->keys_, Gs, gs, 0.0); } @@ -477,7 +477,7 @@ public: if (nonDegenerate) return Base::unwhitenedError(cameras, *result_); else - return zero(cameras.size() * 2); + return Vector::Zero(cameras.size() * 2); } /** diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 578422eaf..59fc372cb 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -138,15 +138,15 @@ public: return (stereoCam.project(point, H1, H2) - measured_).vector(); } } catch(StereoCheiralityException& e) { - if (H1) *H1 = zeros(3,6); - if (H2) *H2 = zeros(3,3); + if (H1) *H1 = Matrix::Zero(3,6); + if (H2) *H2 = Z_3x3; if (verboseCheirality_) std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; if (throwCheirality_) throw e; } - return ones(3) * 2.0 * K_->fx(); + return Vector3::Constant(2.0 * K_->fx()); } /** return the measured */ diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index aa50929a5..e97cd2730 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -124,14 +124,14 @@ public: return error.vector(); } catch (CheiralityException& e) { if (H2) - *H2 = zeros(Measurement::dimension, 3); + *H2 = Matrix::Zero(Measurement::dimension, 3); if (verboseCheirality_) std::cout << e.what() << ": Landmark " << DefaultKeyFormatter(this->key()) << " moved behind camera" << std::endl; if (throwCheirality_) throw e; - return ones(Measurement::dimension) * 2.0 * camera_.calibration().fx(); + return Eigen::Matrix::Constant(2.0 * camera_.calibration().fx()); } } diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 14192fcaa..b2a90cb84 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -472,7 +472,7 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, << p.x() << " " << p.y() << " " << p.z() << " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z() << " " << R.toQuaternion().w(); - Matrix InfoG2o = eye(6); + Matrix InfoG2o = I_6x6; InfoG2o.block(0,0,3,3) = Info.block(3,3,3,3); // cov translation InfoG2o.block(3,3,3,3) = Info.block(0,0,3,3); // cov rotation InfoG2o.block(0,3,3,3) = Info.block(0,3,3,3); // off diagonal @@ -539,7 +539,7 @@ GraphAndValues load3D(const string& filename) { ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; Rot3 R = Rot3::Ypr(yaw,pitch,roll); Point3 t = Point3(x, y, z); - Matrix m = eye(6); + Matrix m = I_6x6; for (int i = 0; i < 6; i++) for (int j = i; j < 6; j++) ls >> m(i, j); @@ -549,7 +549,7 @@ GraphAndValues load3D(const string& filename) { graph->push_back(factor); } if (tag == "EDGE_SE3:QUAT") { - Matrix m = eye(6); + Matrix m = I_6x6; Key id1, id2; double x, y, z, qx, qy, qz, qw; ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; @@ -563,11 +563,13 @@ GraphAndValues load3D(const string& filename) { m(j, i) = mij; } } - Matrix mgtsam = eye(6); - mgtsam.block(0,0,3,3) = m.block(3,3,3,3); // cov rotation - mgtsam.block(3,3,3,3) = m.block(0,0,3,3); // cov translation - mgtsam.block(0,3,3,3) = m.block(0,3,3,3); // off diagonal - mgtsam.block(3,0,3,3) = m.block(3,0,3,3); // off diagonal + Matrix mgtsam = I_6x6; + + mgtsam.block<3,3>(0,0) = m.block<3,3>(3,3); // cov rotation + mgtsam.block<3,3>(3,3) = m.block<3,3>(0,0); // cov translation + mgtsam.block<3,3>(0,3) = m.block<3,3>(0,3); // off diagonal + mgtsam.block<3,3>(3,0) = m.block<3,3>(3,0); // off diagonal + SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, Pose3(R,t), model)); graph->push_back(factor); diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index e46b8ee71..fa6fce37a 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -30,8 +30,8 @@ using namespace std; namespace gtsam { namespace lago { -static const Matrix I = eye(1); -static const Matrix I3 = eye(3); +static const Matrix I = I_1x1; +static const Matrix I3 = I_3x3; static const Key keyAnchor = symbol('Z', 9999999); static const noiseModel::Diagonal::shared_ptr priorOrientationNoise = diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index 31b276642..b2150dd86 100644 --- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -45,7 +45,7 @@ TEST( EssentialMatrixConstraint, test ) { Rot3::RzRyRx(0.179693265735950, 0.002945368776519, 0.102274823253840), Point3(-3.37493895, 6.14660244, -8.93650986)); - Vector expected = zero(5); + Vector expected = Z_5x1; Vector actual = factor.evaluateError(pose1,pose2); CHECK(assert_equal(expected, actual, 1e-8)); diff --git a/gtsam/slam/tests/testPoseRotationPrior.cpp b/gtsam/slam/tests/testPoseRotationPrior.cpp index 3c7d5f2b2..f61beab6c 100644 --- a/gtsam/slam/tests/testPoseRotationPrior.cpp +++ b/gtsam/slam/tests/testPoseRotationPrior.cpp @@ -52,7 +52,7 @@ TEST( testPoseRotationFactor, level3_zero_error ) { Pose3 pose1(rot3A, point3A); Pose3RotationPrior factor(poseKey, rot3A, model3); Matrix actH1; - EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -78,7 +78,7 @@ TEST( testPoseRotationFactor, level2_zero_error ) { Pose2 pose1(rot2A, point2A); Pose2RotationPrior factor(poseKey, rot2A, model1); Matrix actH1; - EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } diff --git a/gtsam/slam/tests/testPoseTranslationPrior.cpp b/gtsam/slam/tests/testPoseTranslationPrior.cpp index 2fd471c9c..bbc0c6518 100644 --- a/gtsam/slam/tests/testPoseTranslationPrior.cpp +++ b/gtsam/slam/tests/testPoseTranslationPrior.cpp @@ -48,7 +48,7 @@ TEST( testPoseTranslationFactor, level3_zero_error ) { Pose3 pose1(rot3A, point3A); Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; - EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -68,7 +68,7 @@ TEST( testPoseTranslationFactor, pitched3_zero_error ) { Pose3 pose1(rot3B, point3A); Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; - EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -88,7 +88,7 @@ TEST( testPoseTranslationFactor, smallrot3_zero_error ) { Pose3 pose1(rot3C, point3A); Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; - EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -108,7 +108,7 @@ TEST( testPoseTranslationFactor, level2_zero_error ) { Pose2 pose1(rot2A, point2A); Pose2TranslationPrior factor(poseKey, point2A, model2); Matrix actH1; - EXPECT(assert_equal(zero(2), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Z_2x1, factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp index 309801ccb..f955aa191 100644 --- a/gtsam/slam/tests/testReferenceFrameFactor.cpp +++ b/gtsam/slam/tests/testReferenceFrameFactor.cpp @@ -93,7 +93,7 @@ TEST( ReferenceFrameFactor, jacobians_zero ) { PointReferenceFrameFactor tc(lA1, tA1, lB1); Vector actCost = tc.evaluateError(global, trans, local), - expCost = zero(2); + expCost = Z_2x1; EXPECT(assert_equal(expCost, actCost, 1e-5)); Matrix actualDT, actualDL, actualDF; diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 3664de9c5..4184d6769 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -49,9 +49,9 @@ const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished(); //************************************************************************************* TEST( regularImplicitSchurFactor, creation ) { // Matrix E = Matrix::Ones(6,3); - Matrix E = zeros(6, 3); - E.block<2,2>(0, 0) = eye(2); - E.block<2,3>(2, 0) = 2 * ones(2, 3); + Matrix E = Matrix::Zero(6, 3); + E.block<2,2>(0, 0) = I_2x2; + E.block<2,3>(2, 0) = 2 * Matrix::Ones(2, 3); Matrix3 P = (E.transpose() * E).inverse(); RegularImplicitSchurFactor expected(keys, FBlocks, E, P, b); Matrix expectedP = expected.getPointCovariance(); @@ -61,10 +61,10 @@ TEST( regularImplicitSchurFactor, creation ) { /* ************************************************************************* */ TEST( regularImplicitSchurFactor, addHessianMultiply ) { - Matrix E = zeros(6, 3); - E.block<2,2>(0, 0) = eye(2); - E.block<2,3>(2, 0) = 2 * ones(2, 3); - E.block<2,2>(4, 1) = eye(2); + Matrix E = Matrix::Zero(6, 3); + E.block<2,2>(0, 0) = I_2x2; + E.block<2,3>(2, 0) = 2 * Matrix::Ones(2, 3); + E.block<2,2>(4, 1) = I_2x2; Matrix3 P = (E.transpose() * E).inverse(); double alpha = 0.5; @@ -83,13 +83,13 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { // Create full F size_t M=4, m = 3, d = 6; Matrix F(2 * m, d * M); - F << F0, zeros(2, d * 3), zeros(2, d), F1, zeros(2, d*2), zeros(2, d * 3), F3; + F << F0, Matrix::Zero(2, d * 3), Matrix::Zero(2, d), F1, Matrix::Zero(2, d*2), Matrix::Zero(2, d * 3), F3; // Calculate expected result F'*alpha*(I - E*P*E')*F*x FastVector keys2; keys2 += 0,1,2,3; Vector x = xvalues.vector(keys2); - Vector expected = zero(24); + Vector expected = Vector::Zero(24); RegularImplicitSchurFactor::multiplyHessianAdd(F, E, P, alpha, x, expected); EXPECT(assert_equal(expected, yExpected.vector(keys2), 1e-8)); @@ -170,9 +170,9 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { } VectorValues expectedVV; - expectedVV.insert(0,-3.5*ones(6)); - expectedVV.insert(1,10*ones(6)/3); - expectedVV.insert(3,-19.5*ones(6)); + expectedVV.insert(0,-3.5*Vector::Ones(6)); + expectedVV.insert(1,10*Vector::Ones(6)/3); + expectedVV.insert(3,-19.5*Vector::Ones(6)); { // Check gradientAtZero VectorValues actual = implicitFactor.gradientAtZero(); EXPECT(assert_equal(expectedVV, jfQ.gradientAtZero(), 1e-8)); @@ -210,9 +210,9 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { TEST(regularImplicitSchurFactor, hessianDiagonal) { /* TESTED AGAINST MATLAB - * F = [ones(2,6) zeros(2,6) zeros(2,6) - zeros(2,6) 2*ones(2,6) zeros(2,6) - zeros(2,6) zeros(2,6) 3*ones(2,6)] + * F = [Vector::Ones(2,6) zeros(2,6) zeros(2,6) + zeros(2,6) 2*Vector::Ones(2,6) zeros(2,6) + zeros(2,6) zeros(2,6) 3*Vector::Ones(2,6)] E = [[1:6] [1:6] [0.5 1:5]]; E = reshape(E',3,6)' P = inv(E' * E) @@ -228,9 +228,9 @@ TEST(regularImplicitSchurFactor, hessianDiagonal) // hessianDiagonal VectorValues expected; - expected.insert(0, 1.195652*ones(6)); - expected.insert(1, 4.782608*ones(6)); - expected.insert(3, 7.043478*ones(6)); + expected.insert(0, 1.195652*Vector::Ones(6)); + expected.insert(1, 4.782608*Vector::Ones(6)); + expected.insert(3, 7.043478*Vector::Ones(6)); EXPECT(assert_equal(expected, factor.hessianDiagonal(),1e-5)); // hessianBlockDiagonal @@ -246,7 +246,7 @@ TEST(regularImplicitSchurFactor, hessianDiagonal) EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3])); // variant two - Matrix I2 = eye(2); + Matrix I2 = I_2x2; Matrix E0 = E.block<2,3>(0, 0); Matrix F0t = F0.transpose(); EXPECT(assert_equal(F0t*F0-F0t*E0*P*E0.transpose()*F0,actualBD[0])); diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index a8f48b050..9bb296444 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -58,7 +58,7 @@ TEST (RotateFactor, checkMath) { TEST (RotateFactor, test) { Model model = noiseModel::Isotropic::Sigma(3, 0.01); RotateFactor f(1, i1Ri2, c1Zc2, model); - EXPECT(assert_equal(zero(3), f.evaluateError(iRc), 1e-8)); + EXPECT(assert_equal(Z_3x1, f.evaluateError(iRc), 1e-8)); Rot3 R = iRc.retract(Vector3(0.1, 0.2, 0.1)); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) @@ -127,7 +127,7 @@ TEST (RotateFactor, minimization) { TEST (RotateDirectionsFactor, test) { Model model = noiseModel::Isotropic::Sigma(2, 0.01); RotateDirectionsFactor f(1, p1, z1, model); - EXPECT(assert_equal(zero(2), f.evaluateError(iRc), 1e-8)); + EXPECT(assert_equal(Z_2x1, f.evaluateError(iRc), 1e-8)); Rot3 R = iRc.retract(Vector3(0.1, 0.2, 0.1)); diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index daecb56bf..7eefb2398 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -123,7 +123,7 @@ TEST( SmartProjectionCameraFactor, noiseless ) { double expectedError = 0.0; DOUBLES_EQUAL(expectedError, factor1->error(values), 1e-7); CHECK( - assert_equal(zero(4), + assert_equal(Z_4x1, factor1->reprojectionErrorAfterTriangulation(values), 1e-7)); } diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index b710c252d..ace75f80f 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -140,7 +140,7 @@ TEST( SmartProjectionPoseFactor, noiseless ) { Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); EXPECT(assert_equal(expectedE, E, 1e-7)); - EXPECT(assert_equal(zero(4), actualErrors, 1e-7)); + EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7)); // Calculate using computeJacobians Vector b; diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 82d8fb3ec..13849895d 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -726,6 +726,7 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { EXPECT(assert_equal(expected, actual)); } +#ifdef GTSAM_SUPPORT_NESTED_DISSECTION // METIS { Ordering ordering = Ordering::Create(Ordering::METIS, sfg); @@ -759,6 +760,7 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering); EXPECT(assert_equal(expected, actual)); } +#endif } /* ************************************************************************* */ diff --git a/gtsam_unstable/dynamics/DynamicsPriors.h b/gtsam_unstable/dynamics/DynamicsPriors.h index 5b986415e..1e768ef2a 100644 --- a/gtsam_unstable/dynamics/DynamicsPriors.h +++ b/gtsam_unstable/dynamics/DynamicsPriors.h @@ -57,7 +57,7 @@ struct VelocityPrior : public gtsam::PartialPriorFactor { this->mask_[0] = 6; this->mask_[1] = 7; this->mask_[2] = 8; - this->H_ = zeros(3, 9); + this->H_ = Matrix::Zero(3, 9); this->fillH(); } }; @@ -75,13 +75,13 @@ struct DGroundConstraint : public gtsam::PartialPriorFactor { */ DGroundConstraint(Key key, double height, const gtsam::SharedNoiseModel& model) : Base(key, model) { - this->prior_ = delta(4, 0, height); // [z, vz, roll, pitch] + this->prior_ = Vector::Unit(4,0)*height; // [z, vz, roll, pitch] this->mask_.resize(4); this->mask_[0] = 5; // z = height this->mask_[1] = 8; // vz this->mask_[2] = 0; // roll this->mask_[3] = 1; // pitch - this->H_ = zeros(3, 9); + this->H_ = Matrix::Zero(3, 9); this->fillH(); } @@ -97,7 +97,7 @@ struct DGroundConstraint : public gtsam::PartialPriorFactor { this->mask_[1] = 8; // vz this->mask_[2] = 0; // roll this->mask_[3] = 1; // pitch - this->H_ = zeros(3, 9); + this->H_ = Matrix::Zero(3, 9); this->fillH(); } }; diff --git a/gtsam_unstable/dynamics/Pendulum.h b/gtsam_unstable/dynamics/Pendulum.h index ecc43ad79..9ec10e39f 100644 --- a/gtsam_unstable/dynamics/Pendulum.h +++ b/gtsam_unstable/dynamics/Pendulum.h @@ -50,9 +50,9 @@ public: boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { const size_t p = 1; - if (H1) *H1 = -eye(p); - if (H2) *H2 = eye(p); - if (H3) *H3 = eye(p)*h_; + if (H1) *H1 = -Matrix::Identity(p,p); + if (H2) *H2 = Matrix::Identity(p,p); + if (H3) *H3 = Matrix::Identity(p,p)*h_; return (Vector(1) << qk+v*h_-qk1).finished(); } @@ -98,9 +98,9 @@ public: boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { const size_t p = 1; - if (H1) *H1 = -eye(p); - if (H2) *H2 = eye(p); - if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q); + if (H1) *H1 = -Matrix::Identity(p,p); + if (H2) *H2 = Matrix::Identity(p,p); + if (H3) *H3 = -Matrix::Identity(p,p)*h_*g_/r_*cos(q); return (Vector(1) << vk - h_ * g_ / r_ * sin(q) - vk1).finished(); } @@ -154,9 +154,9 @@ public: double mr2_h = 1/h_*m_*r_*r_; double mgrh = m_*g_*r_*h_; - if (H1) *H1 = -eye(p); - if (H2) *H2 = eye(p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid)); - if (H3) *H3 = eye(p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid)); + if (H1) *H1 = -Matrix::Identity(p,p); + if (H2) *H2 = Matrix::Identity(p,p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid)); + if (H3) *H3 = Matrix::Identity(p,p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid)); return (Vector(1) << mr2_h * (qk1 - qk) + mgrh * (1 - alpha_) * sin(qmid) - pk).finished(); } @@ -210,9 +210,9 @@ public: double mr2_h = 1/h_*m_*r_*r_; double mgrh = m_*g_*r_*h_; - if (H1) *H1 = -eye(p); - if (H2) *H2 = eye(p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid)); - if (H3) *H3 = eye(p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid)); + if (H1) *H1 = -Matrix::Identity(p,p); + if (H2) *H2 = Matrix::Identity(p,p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid)); + if (H3) *H3 = Matrix::Identity(p,p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid)); return (Vector(1) << mr2_h * (qk1 - qk) - mgrh * alpha_ * sin(qmid) - pk1).finished(); } diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index b6fc61411..c1afe3882 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -11,7 +11,7 @@ namespace gtsam { using namespace std; -static const Vector kGravity = delta(3, 2, 9.81); +static const Vector kGravity = Vector::Unit(3,2)*9.81; /* ************************************************************************* */ double bound(double a, double min, double max) { @@ -105,7 +105,7 @@ PoseRTV PoseRTV::flyingDynamics( Vector Acc_n = yaw_correction_bn.rotate(forward) // applies locally forward force in the global frame - drag * (Vector(3) << v1.x(), v1.y(), 0.0).finished() // drag term dependent on v1 - + delta(3, 2, loss_lift - lift_control); // falling due to lift lost from pitch + + Vector::Unit(3,2)*(loss_lift - lift_control); // falling due to lift lost from pitch // Update Vehicle Position and Velocity Velocity3 v2 = v1 + Velocity3(Acc_n * dt); diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index a3dd6327b..f38c256b1 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -140,7 +140,7 @@ public: } if (H3) { - *H3 = zeros(6,6); + *H3 = Z_6x6; insertSub(*H3, -h_*D_gravityBody_gk, 3, 0); } diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h index 9ecc7619e..f98879e41 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -41,9 +41,9 @@ public: boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { const size_t p = 1; - if (H1) *H1 = eye(p); - if (H2) *H2 = -eye(p); - if (H3) *H3 = eye(p)*dt_; + if (H1) *H1 = Matrix::Identity(p,p); + if (H2) *H2 = -Matrix::Identity(p,p); + if (H3) *H3 = Matrix::Identity(p,p)*dt_; return (Vector(1) << x1+v*dt_-x2).finished(); } diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index 3fc6a0197..494f2731f 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -26,7 +26,7 @@ using namespace gtsam; const double tol=1e-5; static const Key x0 = 0, x1 = 1, x2 = 2, x3 = 3, x4 = 4; -static const Vector g = delta(3, 2, -9.81); +static const Vector g = Vector::Unit(3,2)*(-9.81); /* ************************************************************************* */ TEST(testIMUSystem, instantiations) { @@ -38,7 +38,7 @@ TEST(testIMUSystem, instantiations) { gtsam::SharedNoiseModel model6 = gtsam::noiseModel::Unit::Create(6); gtsam::SharedNoiseModel model9 = gtsam::noiseModel::Unit::Create(9); - Vector accel = ones(3), gyro = ones(3); + Vector accel = Vector::Ones(3), gyro = Vector::Ones(3); IMUFactor imu(accel, gyro, 0.01, x1, x2, model6); FullIMUFactor full_imu(accel, gyro, 0.01, x1, x2, model9); @@ -48,7 +48,7 @@ TEST(testIMUSystem, instantiations) { VelocityConstraint constraint(x1, x2, 0.1, 10000); PriorFactor posePrior(x1, x1_v, model9); DHeightPrior heightPrior(x1, 0.1, model1); - VelocityPrior velPrior(x1, ones(3), model3); + VelocityPrior velPrior(x1, Vector::Ones(3), model3); } /* ************************************************************************* */ @@ -149,8 +149,8 @@ TEST( testIMUSystem, linear_trajectory) { const double dt = 1.0; PoseRTV start; - Vector accel = delta(3, 0, 0.5); // forward force - Vector gyro = delta(3, 0, 0.1); // constant rotation + Vector accel = Vector::Unit(3,0)*0.5; // forward force + Vector gyro = Vector::Unit(3,0)*0.1; // constant rotation SharedDiagonal model = noiseModel::Unit::Create(9); Values true_traj, init_traj; diff --git a/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp b/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp index 5a4593270..d4b877008 100644 --- a/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp +++ b/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp @@ -29,7 +29,7 @@ TEST( testPendulumFactor1, evaluateError) { PendulumFactor1 constraint(Q(2), Q(1), V(1), h); // verify error function - EXPECT(assert_equal(zero(1), constraint.evaluateError(q2, q1, v1), tol)); + EXPECT(assert_equal(Z_1x1, constraint.evaluateError(q2, q1, v1), tol)); } /* ************************************************************************* */ @@ -38,7 +38,7 @@ TEST( testPendulumFactor2, evaluateError) { PendulumFactor2 constraint(V(2), V(1), Q(1), h); // verify error function - EXPECT(assert_equal(zero(1), constraint.evaluateError(v2, v1, q1), tol)); + EXPECT(assert_equal(Z_1x1, constraint.evaluateError(v2, v1, q1), tol)); } /* ************************************************************************* */ @@ -49,7 +49,7 @@ TEST( testPendulumFactorPk, evaluateError) { double pk( 1/h * (q2-q1) + h*g*sin(q1) ); // verify error function - EXPECT(assert_equal(zero(1), constraint.evaluateError(pk, q1, q2), tol)); + EXPECT(assert_equal(Z_1x1, constraint.evaluateError(pk, q1, q2), tol)); } /* ************************************************************************* */ @@ -60,7 +60,7 @@ TEST( testPendulumFactorPk1, evaluateError) { double pk1( 1/h * (q2-q1) ); // verify error function - EXPECT(assert_equal(zero(1), constraint.evaluateError(pk1, q1, q2), tol)); + EXPECT(assert_equal(Z_1x1, constraint.evaluateError(pk1, q1, q2), tol)); } diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index 855131042..025c838c9 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -76,12 +76,12 @@ TEST( testPoseRTV, equals ) { TEST( testPoseRTV, Lie ) { // origin and zero deltas PoseRTV identity; - EXPECT(assert_equal(identity, (PoseRTV)identity.retract(zero(9)))); - EXPECT(assert_equal(zero(9), identity.localCoordinates(identity))); + EXPECT(assert_equal(identity, (PoseRTV)identity.retract(Z_9x1))); + EXPECT(assert_equal((Vector) Z_9x1, identity.localCoordinates(identity))); PoseRTV state1(pt, rot, vel); - EXPECT(assert_equal(state1, (PoseRTV)state1.retract(zero(9)))); - EXPECT(assert_equal(zero(9), state1.localCoordinates(state1))); + EXPECT(assert_equal(state1, (PoseRTV)state1.retract(Z_9x1))); + EXPECT(assert_equal((Vector) Z_9x1, state1.localCoordinates(state1))); Vector delta(9); delta << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3; @@ -111,7 +111,7 @@ TEST( testPoseRTV, dynamics_identities ) { const double dt = 0.1; Vector accel = Vector3(0.2, 0.0, 0.0), gyro = Vector3(0.0, 0.0, 0.2); - Vector imu01 = zero(6), imu12 = zero(6), imu23 = zero(6), imu34 = zero(6); + Vector imu01 = Z_6x1, imu12 = Z_6x1, imu23 = Z_6x1, imu34 = Z_6x1; x1 = x0.generalDynamics(accel, gyro, dt); x2 = x1.generalDynamics(accel, gyro, dt); @@ -227,15 +227,15 @@ TEST( testPoseRTV, transformed_from_2 ) { /* ************************************************************************* */ TEST(testPoseRTV, RRTMbn) { - EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(kZero3))); - EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3()))); + EXPECT(assert_equal(I_3x3, PoseRTV::RRTMbn(kZero3))); + EXPECT(assert_equal(I_3x3, PoseRTV::RRTMbn(Rot3()))); EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::Ypr(0.1, 0.2, 0.3)))); } /* ************************************************************************* */ TEST(testPoseRTV, RRTMnb) { - EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(kZero3))); - EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3()))); + EXPECT(assert_equal(I_3x3, PoseRTV::RRTMnb(kZero3))); + EXPECT(assert_equal(I_3x3, PoseRTV::RRTMnb(Rot3()))); EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::Ypr(0.1, 0.2, 0.3)))); } diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index 6f75c264c..fe21d5e00 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -29,8 +29,8 @@ Vector gamma2 = Vector2(0.0, 0.0); // no shape Vector u2 = Vector2(0.0, 0.0); // no control at time 2 double distT = 1.0; // distance from the body-centered x axis to the big top motor double distR = 5.0; // distance from the body-centered z axis to the small motor -Matrix Mass = diag((Vector(3) << mass, mass, mass).finished()); -Matrix Inertia = diag((Vector(6) << 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass).finished()); +Matrix Mass = ((Vector(3) << mass, mass, mass).finished()).asDiagonal(); +Matrix Inertia = (Vector(6) << 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass).finished().asDiagonal(); Vector computeFu(const Vector& gamma, const Vector& control) { double gamma_r = gamma(0), gamma_p = gamma(1); @@ -53,7 +53,7 @@ TEST( Reconstruction, evaluateError) { // verify error function Matrix H1, H2, H3; EXPECT( - assert_equal(zero(6), constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol)); + assert_equal(Z_6x1, constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol)); Matrix numericalH1 = numericalDerivative31( boost::function( @@ -89,7 +89,7 @@ Vector newtonEuler(const Vector& Vb, const Vector& Fb, const Matrix& Inertia) { TEST( DiscreteEulerPoincareHelicopter, evaluateError) { Vector Fu = computeFu(gamma2, u2); - Vector fGravity_g1 = zero(6); + Vector fGravity_g1 = Z_6x1; fGravity_g1.segment<3>(3) = g1.rotation().unrotate(Vector3(0, 0, -mass*9.81)); // gravity force in g1 frame Vector Fb = Fu+fGravity_g1; @@ -106,7 +106,7 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) { // verify error function Matrix H1, H2, H3; - EXPECT(assert_equal(zero(6), constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0)); + EXPECT(assert_equal(Z_6x1, constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0)); Matrix numericalH1 = numericalDerivative31( boost::function( diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp index f253be371..6d8206177 100644 --- a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp +++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp @@ -25,10 +25,10 @@ TEST( testVelocityConstraint, trapezoidal ) { VelocityConstraint constraint(x1, x2, dynamics::TRAPEZOIDAL, dt); // verify error function - EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, pose1), tol)); - EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, origin), tol)); - EXPECT(assert_equal(delta(3, 0,-1.0), constraint.evaluateError(pose1, pose1), tol)); - EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol)); + EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, pose1), tol)); + EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol)); + EXPECT(assert_equal(Vector::Unit(3,0)*(-1.0), constraint.evaluateError(pose1, pose1), tol)); + EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1a), tol)); } /* ************************************************************************* */ @@ -37,10 +37,10 @@ TEST( testEulerVelocityConstraint, euler_start ) { VelocityConstraint constraint(x1, x2, dynamics::EULER_START, dt); // verify error function - EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1), tol)); - EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, origin), tol)); - EXPECT(assert_equal(zero(3), constraint.evaluateError(pose1, pose2), tol)); - EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol)); + EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1), tol)); + EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol)); + EXPECT(assert_equal(Z_3x1, constraint.evaluateError(pose1, pose2), tol)); + EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1a), tol)); } /* ************************************************************************* */ @@ -49,10 +49,10 @@ TEST( testEulerVelocityConstraint, euler_end ) { VelocityConstraint constraint(x1, x2, dynamics::EULER_END, dt); // verify error function - EXPECT(assert_equal(delta(3, 0,-0.5), constraint.evaluateError(origin, pose1), tol)); - EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, origin), tol)); - EXPECT(assert_equal(zero(3), constraint.evaluateError(pose1, pose2), tol)); - EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol)); + EXPECT(assert_equal(Vector::Unit(3,0)*(-0.5), constraint.evaluateError(origin, pose1), tol)); + EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol)); + EXPECT(assert_equal(Z_3x1, constraint.evaluateError(pose1, pose2), tol)); + EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1a), tol)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp index ac27ae563..b8caf4414 100644 --- a/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp +++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp @@ -23,7 +23,7 @@ TEST( testVelocityConstraint3, evaluateError) { VelocityConstraint3 constraint(X(1), X(2), V(1), dt); // verify error function - EXPECT(assert_equal(zero(1), constraint.evaluateError(x1, x2, v), tol)); + EXPECT(assert_equal(Z_1x1, constraint.evaluateError(x1, x2, v), tol)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 4ce0eaedb..4f1b42610 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -102,7 +102,7 @@ public: gtsam::Matrix J2; gtsam::Point2 uv= camera.project(landmark,H1, J2, boost::none); if (H1) { - *H1 = (*H1) * gtsam::eye(6); + *H1 = (*H1) * I_6x6; } double cos_theta = cos(theta); diff --git a/gtsam_unstable/geometry/Pose3Upright.cpp b/gtsam_unstable/geometry/Pose3Upright.cpp index f83cb19fb..4237170f0 100644 --- a/gtsam_unstable/geometry/Pose3Upright.cpp +++ b/gtsam_unstable/geometry/Pose3Upright.cpp @@ -81,7 +81,7 @@ Pose3 Pose3Upright::pose() const { Pose3Upright Pose3Upright::inverse(boost::optional H1) const { Pose3Upright result(T_.inverse(H1), -z_); if (H1) { - Matrix H1_ = -eye(4,4); + Matrix H1_ = -I_4x4; H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2); H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1); *H1 = H1_; @@ -96,12 +96,12 @@ Pose3Upright Pose3Upright::compose(const Pose3Upright& p2, return Pose3Upright(T_.compose(p2.T_), z_ + p2.z_); Pose3Upright result(T_.compose(p2.T_, H1), z_ + p2.z_); if (H1) { - Matrix H1_ = eye(4,4); + Matrix H1_ = I_4x4; H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2); H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1); *H1 = H1_; } - if (H2) *H2 = eye(4,4); + if (H2) *H2 = I_4x4; return result; } @@ -112,12 +112,12 @@ Pose3Upright Pose3Upright::between(const Pose3Upright& p2, return Pose3Upright(T_.between(p2.T_), p2.z_ - z_); Pose3Upright result(T_.between(p2.T_, H1, H2), p2.z_ - z_); if (H1) { - Matrix H1_ = -eye(4,4); + Matrix H1_ = -I_4x4; H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2); H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1); *H1 = H1_; } - if (H2) *H2 = eye(4,4); + if (H2) *H2 = I_4x4; return result; } diff --git a/gtsam_unstable/geometry/SimPolygon2D.cpp b/gtsam_unstable/geometry/SimPolygon2D.cpp index f60ea107c..90e3eeea6 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.cpp +++ b/gtsam_unstable/geometry/SimPolygon2D.cpp @@ -149,7 +149,7 @@ SimPolygon2D SimPolygon2D::randomTriangle( // extend from B to find C double dBC = randomDistance(mean_side_len, sigma_side_len, min_side_len); - Pose2 xC = xB.retract(delta(3, 0, dBC)); + Pose2 xC = xB.retract(Vector::Unit(3,0)*dBC); // use triangle equality to verify non-degenerate triangle double dAC = xA.t().distance(xC.t()); diff --git a/gtsam_unstable/geometry/tests/testBearingS2.cpp b/gtsam_unstable/geometry/tests/testBearingS2.cpp index 48d6e29af..f29b30621 100644 --- a/gtsam_unstable/geometry/tests/testBearingS2.cpp +++ b/gtsam_unstable/geometry/tests/testBearingS2.cpp @@ -48,11 +48,11 @@ TEST( testBearingS2, manifold ) { BearingS2 origin, b1(0.2, 0.3); EXPECT_LONGS_EQUAL(2, origin.dim()); - EXPECT(assert_equal(zero(2), origin.localCoordinates(origin), tol)); - EXPECT(assert_equal(origin, origin.retract(zero(2)), tol)); + EXPECT(assert_equal(Z_2x1, origin.localCoordinates(origin), tol)); + EXPECT(assert_equal(origin, origin.retract(Z_2x1), tol)); - EXPECT(assert_equal(zero(2), b1.localCoordinates(b1), tol)); - EXPECT(assert_equal(b1, b1.retract(zero(2)), tol)); + EXPECT(assert_equal(Z_2x1, b1.localCoordinates(b1), tol)); + EXPECT(assert_equal(b1, b1.retract(Z_2x1), tol)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/tests/testPose3Upright.cpp b/gtsam_unstable/geometry/tests/testPose3Upright.cpp index cd54a8813..5e4c2468d 100644 --- a/gtsam_unstable/geometry/tests/testPose3Upright.cpp +++ b/gtsam_unstable/geometry/tests/testPose3Upright.cpp @@ -68,9 +68,9 @@ TEST( testPose3Upright, manifold ) { Pose3Upright origin, x1(1.0, 2.0, 3.0, 0.0), x2(4.0, 2.0, 7.0, 0.0); EXPECT_LONGS_EQUAL(4, origin.dim()); - EXPECT(assert_equal(origin, origin.retract(zero(4)), tol)); - EXPECT(assert_equal(x1, x1.retract(zero(4)), tol)); - EXPECT(assert_equal(x2, x2.retract(zero(4)), tol)); + EXPECT(assert_equal(origin, origin.retract(Z_4x1), tol)); + EXPECT(assert_equal(x1, x1.retract(Z_4x1), tol)); + EXPECT(assert_equal(x2, x2.retract(Z_4x1), tol)); Vector delta12 = (Vector(4) << 3.0, 0.0, 4.0, 0.0).finished(), delta21 = -delta12; EXPECT(assert_equal(x2, x1.retract(delta12), tol)); @@ -83,8 +83,8 @@ TEST( testPose3Upright, manifold ) { /* ************************************************************************* */ TEST( testPose3Upright, lie ) { Pose3Upright origin, x1(1.0, 2.0, 3.0, 0.1); - EXPECT(assert_equal(zero(4), Pose3Upright::Logmap(origin), tol)); - EXPECT(assert_equal(origin, Pose3Upright::Expmap(zero(4)), tol)); + EXPECT(assert_equal(Z_4x1, Pose3Upright::Logmap(origin), tol)); + EXPECT(assert_equal(origin, Pose3Upright::Expmap(Z_4x1), tol)); EXPECT(assert_equal(x1, Pose3Upright::Expmap(Pose3Upright::Logmap(x1)), tol)); } diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 1866fafc6..d2fbd7579 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -156,7 +156,7 @@ TEST(Similarity3, Manifold) { //****************************************************************************** TEST( Similarity3, retract_first_order) { Similarity3 id; - Vector v = zero(7); + Vector v = Z_7x1; v(0) = 0.3; EXPECT(assert_equal(Similarity3(R, Point3(0,0,0), 1), id.retract(v), 1e-2)); // v(3) = 0.2; diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index 86a8b6dbf..c045c85a2 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -66,7 +66,7 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key, // Collect the gradients of unconstrained cost factors to the b vector if (Aterms.size() > 0) { - Vector b = zero(delta.at(key).size()); + Vector b = Vector::Zero(delta.at(key).size()); if (costVariableIndex_.find(key) != costVariableIndex_.end()) { for (size_t factorIx : costVariableIndex_[key]) { GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx); diff --git a/gtsam_unstable/linear/tests/testLinearEquality.cpp b/gtsam_unstable/linear/tests/testLinearEquality.cpp index fb8e743f6..13b2fa5b5 100644 --- a/gtsam_unstable/linear/tests/testLinearEquality.cpp +++ b/gtsam_unstable/linear/tests/testLinearEquality.cpp @@ -186,7 +186,7 @@ TEST(LinearEquality, matrices) /* ************************************************************************* */ TEST(LinearEquality, operators ) { - Matrix I = eye(2); + Matrix I = I_2x2; Vector b = (Vector(2) << 0.2,-0.1).finished(); LinearEquality lf(1, -I, 2, I, b, 0); diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index c15ccb48c..780262615 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -28,7 +28,7 @@ using namespace std; using namespace gtsam; using namespace gtsam::symbol_shorthand; -const Matrix One = ones(1, 1); +const Matrix One = I_1x1; /* ************************************************************************* */ // Create test graph according to Forst10book_pg171Ex5 @@ -42,15 +42,14 @@ QP createTestCase() { //TODO: THIS TEST MIGHT BE WRONG : the last parameter might be 5 instead of 10 because the form of the equation // Should be 0.5x'Gx + gx + f : Nocedal 449 qp.cost.push_back( - HessianFactor(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1), 3.0 * ones(1), - 2.0 * ones(1, 1), zero(1), 10)); + HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), -Matrix::Ones(1, 1), 3.0 * I_1x1, + 2.0 * Matrix::Ones(1, 1), Z_1x1, 10.0)); // Inequality constraints - qp.inequalities.push_back( - LinearInequality(X(1), ones(1, 1), X(2), ones(1, 1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 - qp.inequalities.push_back(LinearInequality(X(1), -ones(1, 1), 0, 1)); // -x1 <= 0 - qp.inequalities.push_back(LinearInequality(X(2), -ones(1, 1), 0, 2)); // -x2 <= 0 - qp.inequalities.push_back(LinearInequality(X(1), ones(1, 1), 1.5, 3)); // x1 <= 3/2 + qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 + qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 1)); // -x1 <= 0 + qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 2)); // -x2 <= 0 + qp.inequalities.push_back(LinearInequality(X(1), I_1x1, 1.5, 3)); // x1 <= 3/2 return qp; } @@ -58,8 +57,8 @@ QP createTestCase() { TEST(QPSolver, TestCase) { VectorValues values; double x1 = 5, x2 = 7; - values.insert(X(1), x1 * ones(1, 1)); - values.insert(X(2), x2 * ones(1, 1)); + values.insert(X(1), x1 * Matrix::Ones(1, 1)); + values.insert(X(2), x2 * Matrix::Ones(1, 1)); QP qp = createTestCase(); DOUBLES_EQUAL(29, x1 * x1 - x1 * x2 + x2 * x2 - 3 * x1 + 5, 1e-9); DOUBLES_EQUAL(29, qp.cost[0]->error(values), 1e-9); @@ -97,8 +96,8 @@ QP createEqualityConstrainedTest() { // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0 qp.cost.push_back( - HessianFactor(X(1), X(2), 2.0 * ones(1, 1), zeros(1, 1), zero(1), - 2.0 * ones(1, 1), zero(1), 0.0)); + HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), Z_1x1, Z_1x1, + 2.0 * Matrix::Ones(1, 1), Z_1x1, 0.0)); // Equality constraints // x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector @@ -115,8 +114,8 @@ TEST(QPSolver, dual) { // Initials values VectorValues initialValues; - initialValues.insert(X(1), ones(1)); - initialValues.insert(X(2), ones(1)); + initialValues.insert(X(1), I_1x1); + initialValues.insert(X(2), I_1x1); QPSolver solver(qp); @@ -133,8 +132,8 @@ TEST(QPSolver, indentifyActiveConstraints) { QPSolver solver(qp); VectorValues currentSolution; - currentSolution.insert(X(1), zero(1)); - currentSolution.insert(X(2), zero(1)); + currentSolution.insert(X(1), Z_1x1); + currentSolution.insert(X(2), Z_1x1); InequalityFactorGraph workingSet = solver.identifyActiveConstraints(qp.inequalities, currentSolution); @@ -158,8 +157,8 @@ TEST(QPSolver, iterate) { QPSolver solver(qp); VectorValues currentSolution; - currentSolution.insert(X(1), zero(1)); - currentSolution.insert(X(2), zero(1)); + currentSolution.insert(X(1), Z_1x1); + currentSolution.insert(X(2), Z_1x1); std::vector expectedSolutions(4), expectedDuals(4); expectedSolutions[0].insert(X(1), (Vector(1) << 0.0).finished()); @@ -203,8 +202,8 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) { QP qp = createTestCase(); QPSolver solver(qp); VectorValues initialValues; - initialValues.insert(X(1), zero(1)); - initialValues.insert(X(2), zero(1)); + initialValues.insert(X(1), Z_1x1); + initialValues.insert(X(2), Z_1x1); VectorValues solution; boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); VectorValues expectedSolution; @@ -266,8 +265,8 @@ QP createTestMatlabQPEx() { // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0 qp.cost.push_back( - HessianFactor(X(1), X(2), 1.0 * ones(1, 1), -ones(1, 1), 2.0 * ones(1), - 2.0 * ones(1, 1), 6 * ones(1), 1000.0)); + HessianFactor(X(1), X(2), 1.0 * I_1x1, -Matrix::Ones(1, 1), 2.0 * I_1x1, + 2.0 * Matrix::Ones(1, 1), 6 * I_1x1, 1000.0)); // Inequality constraints qp.inequalities.push_back(LinearInequality(X(1), One, X(2), One, 2, 0)); // x1 + x2 <= 2 @@ -284,8 +283,8 @@ TEST(QPSolver, optimizeMatlabEx) { QP qp = createTestMatlabQPEx(); QPSolver solver(qp); VectorValues initialValues; - initialValues.insert(X(1), zero(1)); - initialValues.insert(X(2), zero(1)); + initialValues.insert(X(1), Z_1x1); + initialValues.insert(X(2), Z_1x1); VectorValues solution; boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); VectorValues expectedSolution; @@ -311,8 +310,8 @@ TEST(QPSolver, optimizeMatlabExNoinitials) { QP createTestNocedal06bookEx16_4() { QP qp; - qp.cost.push_back(JacobianFactor(X(1), ones(1, 1), ones(1))); - qp.cost.push_back(JacobianFactor(X(2), ones(1, 1), 2.5 * ones(1))); + qp.cost.push_back(JacobianFactor(X(1), Matrix::Ones(1, 1), I_1x1)); + qp.cost.push_back(JacobianFactor(X(2), Matrix::Ones(1, 1), 2.5 * I_1x1)); // Inequality constraints qp.inequalities.push_back( @@ -332,7 +331,7 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) { QPSolver solver(qp); VectorValues initialValues; initialValues.insert(X(1), (Vector(1) << 2.0).finished()); - initialValues.insert(X(2), zero(1)); + initialValues.insert(X(2), Z_1x1); VectorValues solution; boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); @@ -345,8 +344,8 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) { /* ************************************************************************* */ TEST(QPSolver, failedSubproblem) { QP qp; - qp.cost.push_back(JacobianFactor(X(1), eye(2), zero(2))); - qp.cost.push_back(HessianFactor(X(1), zeros(2, 2), zero(2), 100.0)); + qp.cost.push_back(JacobianFactor(X(1), I_2x2, Z_2x1)); + qp.cost.push_back(HessianFactor(X(1), Z_2x2, Z_2x1, 100.0)); qp.inequalities.push_back( LinearInequality(X(1), (Matrix(1,2) << -1.0, 0.0).finished(), -1.0, 0)); diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp index f8dca75a4..84dffe7be 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp @@ -194,7 +194,7 @@ bool LinearizedHessianFactor::equals(const NonlinearFactor& expected, double tol double LinearizedHessianFactor::error(const Values& c) const { // Construct an error vector in key-order from the Values - Vector dx = zero(dim()); + Vector dx = Vector::Zero(dim()); size_t index = 0; for(unsigned int i = 0; i < this->size(); ++i){ Key key = this->keys()[i]; @@ -217,7 +217,7 @@ boost::shared_ptr LinearizedHessianFactor::linearize(const Values& c) const { // Construct an error vector in key-order from the Values - Vector dx = zero(dim()); + Vector dx = Vector::Zero(dim()); size_t index = 0; for(unsigned int i = 0; i < this->size(); ++i){ Key key = this->keys()[i]; diff --git a/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp b/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp index bfd3c9168..1cf94b161 100644 --- a/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp @@ -126,33 +126,33 @@ TEST( ParticleFilter, constructor) { TEST( ParticleFilter, linear1 ) { // Create the controls and measurement properties for our example - Matrix F = eye(2, 2); - Matrix B = eye(2, 2); + Matrix F = I_2x2; + Matrix B = I_2x2; Vector u = Vector2(1.0, 0.0); SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1); - Matrix Q = 0.01 * eye(2, 2); - Matrix H = eye(2, 2); + Matrix Q = 0.01 * I_2x2; + Matrix H = I_2x2; State z1(1.0, 0.0); State z2(2.0, 0.0); State z3(3.0, 0.0); SharedDiagonal modelR = noiseModel::Isotropic::Sigma(2, 0.1); - Matrix R = 0.01 * eye(2, 2); + Matrix R = 0.01 * I_2x2; // Create the set of expected output TestValues State expected0(0.0, 0.0); - Matrix P00 = 0.01 * eye(2, 2); + Matrix P00 = 0.01 * I_2x2; State expected1(1.0, 0.0); Matrix P01 = P00 + Q; - Matrix I11 = inverse(P01) + inverse(R); + Matrix I11 = P01.inverse() + R.inverse(); State expected2(2.0, 0.0); - Matrix P12 = inverse(I11) + Q; - Matrix I22 = inverse(P12) + inverse(R); + Matrix P12 = I11.inverse() + Q; + Matrix I22 = P12.inverse() + R.inverse(); State expected3(3.0, 0.0); - Matrix P23 = inverse(I22) + Q; - Matrix I33 = inverse(P23) + inverse(R); + Matrix P23 = I22.inverse() + Q; + Matrix I33 = P23.inverse() + R.inverse(); // Create a Kalman filter of dimension 2 KalmanFilter kf(2); diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index ce8fd29f3..0a3fa0283 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -45,9 +45,9 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, F_g_ = -I_3x3 / tau_g; F_a_ = -I_3x3 / tau_a; - Vector3 var_omega_w = 0 * ones(3); // TODO - Vector3 var_omega_g = (0.0034 * 0.0034) * ones(3); - Vector3 var_omega_a = (0.034 * 0.034) * ones(3); + Vector3 var_omega_w = 0 * Vector::Ones(3); // TODO + Vector3 var_omega_g = (0.0034 * 0.0034) * Vector::Ones(3); + Vector3 var_omega_a = (0.034 * 0.034) * Vector::Ones(3); Vector3 sigmas_v_g_sq = sigmas_v_g.array().square(); var_w_ << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a; @@ -83,7 +83,7 @@ std::pair AHRS::initialize(double g_e) 0.0, 0.0, 0.0).finished(); // we don't know anything on yaw // Calculate the initial covariance matrix for the error state dx, Farrell08book eq. 10.66 - Matrix Pa = 0.025 * 0.025 * eye(3); + Matrix Pa = 0.025 * 0.025 * I_3x3; Matrix P11 = Omega_T * (H_g * (Pa + Pa_) * trans(H_g)) * trans(Omega_T); P11(2, 2) = 0.0001; Matrix P12 = -Omega_T * H_g * Pa; @@ -101,7 +101,7 @@ std::pair AHRS::initialize(double g_e) P_plus_k2.block<3,3>(6, 3) = Z_3x3; P_plus_k2.block<3,3>(6, 6) = Pa; - Vector dx = zero(9); + Vector dx = Z_9x1; KalmanFilter::State state = KF_.init(dx, P_plus_k2); return std::make_pair(mech0_, state); } @@ -171,7 +171,7 @@ std::pair AHRS::aid( // calculate residual gravity measurement z = n_g_ - trans(bRn) * measured_b_g; H = collect(3, &n_g_cross_, &Z_3x3, &bRn); - R = trans(bRn) * diag(sigmas_v_a_.array().square()) * bRn; + R = trans(bRn) * ((Vector3) sigmas_v_a_.array().square()).asDiagonal() * bRn; } else { // my measurement prediction (in body frame): // F(:,k) = bias - b_g @@ -186,7 +186,7 @@ std::pair AHRS::aid( Matrix b_g = bRn * n_g_cross_; H = collect(3, &b_g, &Z_3x3, &I_3x3); // And the measurement noise, TODO: should be created once where sigmas_v_a is given - R = diag(sigmas_v_a_.array().square()); + R = ((Vector3) sigmas_v_a_.array().square()).asDiagonal(); } // update the Kalman filter @@ -196,7 +196,7 @@ std::pair AHRS::aid( Mechanization_bRn2 newMech = mech.correct(updatedState->mean()); // reset KF state - Vector dx = zeros(9, 1); + Vector dx = Z_9x1; updatedState = KF_.init(dx, updatedState->covariance()); return make_pair(newMech, updatedState); @@ -217,7 +217,7 @@ std::pair AHRS::aidGeneral( Matrix H = collect(3, &b_g, &I_3x3, &Z_3x3); // Matrix R = diag(emul(sigmas_v_a_, sigmas_v_a_)); // Matrix R = diag(Vector3(1.0, 0.2, 1.0)); // good for L_twice - Matrix R = diag(Vector3(0.01, 0.0001, 0.01)); + Matrix R = Vector3(0.01, 0.0001, 0.01).asDiagonal(); // update the Kalman filter KalmanFilter::State updatedState = KF_.updateQ(state, H, z, R); @@ -226,7 +226,7 @@ std::pair AHRS::aidGeneral( Mechanization_bRn2 newMech = mech.correct(updatedState->mean()); // reset KF state - Vector dx = zeros(9, 1); + Vector dx = Z_9x1; updatedState = KF_.init(dx, updatedState->covariance()); return make_pair(newMech, updatedState); diff --git a/gtsam_unstable/slam/DummyFactor.cpp b/gtsam_unstable/slam/DummyFactor.cpp index 1ce3a878d..8519f6f8d 100644 --- a/gtsam_unstable/slam/DummyFactor.cpp +++ b/gtsam_unstable/slam/DummyFactor.cpp @@ -50,12 +50,12 @@ DummyFactor::linearize(const Values& c) const { std::vector > terms(this->size()); for(size_t j=0; jsize(); ++j) { terms[j].first = keys()[j]; - terms[j].second = zeros(rowDim_, dims_[j]); + terms[j].second = Matrix::Zero(rowDim_, dims_[j]); } noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(rowDim_); return GaussianFactor::shared_ptr( - new JacobianFactor(terms, zero(rowDim_), model)); + new JacobianFactor(terms, Vector::Zero(rowDim_), model)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 79a37c2ff..58284c3a6 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -434,8 +434,8 @@ public: delta_t += msr_dt; // Update EquivCov_Overall - Matrix Z_3x3 = zeros(3,3); - Matrix I_3x3 = eye(3,3); + Matrix Z_3x3 = Z_3x3; + Matrix I_3x3 = I_3x3; Matrix H_pos_pos = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); @@ -461,7 +461,7 @@ public: noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt ); - Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() ); + Matrix Q_d = (model_discrete_curr->R().transpose() * model_discrete_curr->R()).inverse(); EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d; // Luca: force identity covariance matrix (for testing purposes) @@ -536,9 +536,9 @@ public: static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, const noiseModel::Gaussian::shared_ptr& gaussian_process){ - Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); - Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); - Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); + Matrix cov_acc = ( gaussian_acc->R().transpose() * gaussian_acc->R() ).inverse(); + Matrix cov_gyro = ( gaussian_gyro->R().transpose() * gaussian_gyro->R() ).inverse(); + Matrix cov_process = ( gaussian_process->R().transpose() * gaussian_process->R() ).inverse(); cov_process.block(0,0, 3,3) += cov_gyro; cov_process.block(6,6, 3,3) += cov_acc; @@ -550,9 +550,9 @@ public: const noiseModel::Gaussian::shared_ptr& gaussian_process, Matrix& cov_acc, Matrix& cov_gyro, Matrix& cov_process_without_acc_gyro){ - cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); - cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); - cov_process_without_acc_gyro = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); + cov_acc = ( gaussian_acc->R().transpose() * gaussian_acc->R() ).inverse(); + cov_gyro = ( gaussian_gyro->R().transpose() * gaussian_gyro->R() ).inverse(); + cov_process_without_acc_gyro = ( gaussian_process->R().transpose() * gaussian_process->R() ).inverse(); } static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial, diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index 8216942cd..acca233d9 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -367,8 +367,8 @@ public: delta_t += msr_dt; // Update EquivCov_Overall - Matrix Z_3x3 = zeros(3,3); - Matrix I_3x3 = eye(3,3); + Matrix Z_3x3 = Z_3x3; + Matrix I_3x3 = I_3x3; Matrix H_pos_pos = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h index 7f9564ee3..68c972a86 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -103,8 +103,8 @@ public: Vector hx(v2 - alpha_v1); - if(H1) *H1 = - diag(alpha); - if(H2) *H2 = eye(v2.size()); + if(H1) *H1 = -1 * alpha.asDiagonal(); + if(H2) *H2 = Matrix::Identity(v2.size(),v2.size()); return hx; } diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 30d3a216d..c3a67232a 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -278,9 +278,9 @@ public: static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, const noiseModel::Gaussian::shared_ptr& gaussian_process){ - Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); - Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); - Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); + Matrix cov_acc = ( gaussian_acc->R().transpose() * gaussian_acc->R() ).inverse(); + Matrix cov_gyro = ( gaussian_gyro->R().transpose() * gaussian_gyro->R() ).inverse(); + Matrix cov_process = ( gaussian_process->R().transpose() * gaussian_process->R() ).inverse(); cov_process.block(0,0, 3,3) += cov_gyro; cov_process.block(6,6, 3,3) += cov_acc; diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index ac481f979..7509fe3b2 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -89,12 +89,12 @@ public: gtsam::Point2 reprojectionError(camera.project(point, invDepth, H1, H2, H3) - measured_); return reprojectionError.vector(); } catch( CheiralityException& e) { - if (H1) *H1 = gtsam::zeros(2,6); - if (H2) *H2 = gtsam::zeros(2,5); - if (H3) *H2 = gtsam::zeros(2,1); + if (H1) *H1 = Matrix::Zero(2,6); + if (H2) *H2 = Matrix::Zero(2,5); + if (H3) *H2 = Matrix::Zero(2,1); std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; - return gtsam::ones(2) * 2.0 * K_->fx(); + return gtsam::Vector::Ones(2) * 2.0 * K_->fx(); } return (gtsam::Vector(1) << 0.0).finished(); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index 2fd964a35..e9f894faf 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -93,7 +93,7 @@ public: << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]" << " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]" << std::endl; - return gtsam::ones(2) * 2.0 * K_->fx(); + return gtsam::Vector::Ones(2) * 2.0 * K_->fx(); } return (gtsam::Vector(1) << 0.0).finished(); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index fdba78445..ec2615ed6 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -96,7 +96,7 @@ public: << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]" << " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]" << std::endl; - return gtsam::ones(2) * 2.0 * K_->fx(); + return gtsam::Vector::Ones(2) * 2.0 * K_->fx(); } return (gtsam::Vector(1) << 0.0).finished(); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index feff0b62c..cc5878d85 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -96,7 +96,7 @@ public: << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key2()) << "]" << " moved behind camera [" << DefaultKeyFormatter(this->key1()) << "]" << std::endl; - return gtsam::ones(2) * 2.0 * K_->fx(); + return gtsam::Vector::Ones(2) * 2.0 * K_->fx(); } return (Vector(1) << 0.0).finished(); } @@ -215,7 +215,7 @@ public: << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key3()) << "]" << " moved behind camera " << DefaultKeyFormatter(this->key2()) << std::endl; - return gtsam::ones(2) * 2.0 * K_->fx(); + return gtsam::Vector::Ones(2) * 2.0 * K_->fx(); } return (Vector(1) << 0.0).finished(); } diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index 4268aa0e5..a228b2347 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -30,7 +30,7 @@ public: * @param r3 Z-axis of rotated frame */ Mechanization_bRn2(const Rot3& initial_bRn = Rot3(), - const Vector3& initial_x_g = zero(3), const Vector3& initial_x_a = zero(3)) : + const Vector3& initial_x_g = Z_3x1, const Vector3& initial_x_a = Z_3x1) : bRn_(initial_bRn), x_g_(initial_x_g), x_a_(initial_x_a) { } diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index f1487f562..dc250fd9d 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -184,15 +184,15 @@ namespace gtsam { return reprojectionError.vector(); } } catch( CheiralityException& e) { - if (H1) *H1 = zeros(2,6); - if (H2) *H2 = zeros(2,3); + if (H1) *H1 = Matrix::Zero(2,6); + if (H2) *H2 = Matrix::Zero(2,3); if (verboseCheirality_) std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; if (throwCheirality_) throw e; } - return ones(2) * 2.0 * K_->fx(); + return Vector::Ones(2) * 2.0 * K_->fx(); } /** return the measurements */ diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index e3080466f..fa06d47a3 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -70,7 +70,7 @@ namespace gtsam { /** Single Element Constructor: acts on a single parameter specified by idx */ PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) : - Base(model, key), prior_((Vector(1) << prior).finished()), mask_(1, idx), H_(zeros(1, T::dimension)) { + Base(model, key), prior_((Vector(1) << prior).finished()), mask_(1, idx), H_(Matrix::Zero(1, T::dimension)) { assert(model->dim() == 1); this->fillH(); } @@ -78,7 +78,7 @@ namespace gtsam { /** Indices Constructor: specify the mask with a set of indices */ PartialPriorFactor(Key key, const std::vector& mask, const Vector& prior, const SharedNoiseModel& model) : - Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::dimension)) { + Base(model, key), prior_(prior), mask_(mask), H_(Matrix::Zero(mask.size(), T::dimension)) { assert((size_t)prior_.size() == mask.size()); assert(model->dim() == (size_t) prior.size()); this->fillH(); @@ -113,7 +113,7 @@ namespace gtsam { // FIXME: this was originally the generic retraction - may not produce same results Vector full_logmap = T::Logmap(p); // Vector full_logmap = T::identity().localCoordinates(p); // Alternate implementation - Vector masked_logmap = zero(this->dim()); + Vector masked_logmap = Vector::Zero(this->dim()); for (size_t i=0; ikey2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; if (throwCheirality_) throw e; } - return ones(2) * 2.0 * K_->fx(); + return Vector::Ones(2) * 2.0 * K_->fx(); } /** return the measurement */ diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index 069274065..2fd622ea1 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -132,17 +132,17 @@ namespace gtsam { return reprojectionError.vector(); } } catch( CheiralityException& e) { - if (H1) *H1 = zeros(2,6); - if (H2) *H2 = zeros(2,6); - if (H3) *H3 = zeros(2,3); - if (H4) *H4 = zeros(2,CALIBRATION::Dim()); + if (H1) *H1 = Matrix::Zero(2,6); + if (H2) *H2 = Matrix::Zero(2,6); + if (H3) *H3 = Matrix::Zero(2,3); + if (H4) *H4 = Matrix::Zero(2,CALIBRATION::Dim()); if (verboseCheirality_) std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; if (throwCheirality_) throw e; } - return ones(2) * 2.0 * K.fx(); + return Vector::Ones(2) * 2.0 * K.fx(); } /** return the measurement */ diff --git a/gtsam_unstable/slam/RelativeElevationFactor.cpp b/gtsam_unstable/slam/RelativeElevationFactor.cpp index 941a1db89..db077994f 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/RelativeElevationFactor.cpp @@ -21,7 +21,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p boost::optional H1, boost::optional H2) const { double hx = pose.z() - point.z(); if (H1) { - *H1 = zeros(1, 6); + *H1 = Matrix::Zero(1,6); // Use bottom row of rotation matrix for derivative of translation (*H1)(0, 3) = pose.rotation().r1().z(); (*H1)(0, 4) = pose.rotation().r2().z(); @@ -29,7 +29,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p } if (H2) { - *H2 = zeros(1, 3); + *H2 = Matrix::Zero(1,3); (*H2)(0, 2) = -1.0; } return (Vector(1) << hx - measured_).finished(); diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 84aed6ca8..3db302d0b 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -141,10 +141,10 @@ public: if (H) // set Jacobians to zero for n<3 for (size_t j = 0; j < n; j++) - (*H)[j] = zeros(3, 1); - return zero(1); + (*H)[j] = Matrix::Zero(3, 1); + return Z_1x1; } else { - Vector error = zero(1); + Vector error = Z_1x1; // triangulate to get the optimized point // TODO: Should we have a (better?) variant that does this in relative coordinates ? diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 7cbeaae65..56e22aae2 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -340,9 +340,9 @@ public: if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) { // failed: return"empty" Hessian BOOST_FOREACH(Matrix& m, Gs) - m = zeros(Base::Dim, Base::Dim); + m = Matrix::Zero(Base::Dim, Base::Dim); BOOST_FOREACH(Vector& v, gs) - v = zero(Base::Dim); + v = Vector::Zero(Base::Dim); return boost::make_shared >(this->keys_, Gs, gs, 0.0); } @@ -528,7 +528,7 @@ public: if (nonDegenerate) return Base::unwhitenedError(cameras, *result_); else - return zero(cameras.size() * Base::ZDim); + return Vector::Zero(cameras.size() * Base::ZDim); } /** diff --git a/gtsam_unstable/slam/TOAFactor.h b/gtsam_unstable/slam/TOAFactor.h index b500b36e3..e37e8ff20 100644 --- a/gtsam_unstable/slam/TOAFactor.h +++ b/gtsam_unstable/slam/TOAFactor.h @@ -17,7 +17,7 @@ * @date December 2014 */ -#include +#include #include namespace gtsam { @@ -25,7 +25,7 @@ namespace gtsam { /// A "Time of Arrival" factor - so little code seems hardly worth it :-) class TOAFactor: public ExpressionFactor { - typedef Expression double_; + typedef Expression Double_; public: @@ -40,7 +40,7 @@ public: const Expression& microphone_, double toaMeasurement, const SharedNoiseModel& model) : ExpressionFactor(model, toaMeasurement, - double_(&Event::toa, eventExpression, microphone_)) { + Double_(&Event::toa, eventExpression, microphone_)) { } }; diff --git a/gtsam_unstable/slam/tests/testDummyFactor.cpp b/gtsam_unstable/slam/tests/testDummyFactor.cpp index 86b4e5584..743c7398c 100644 --- a/gtsam_unstable/slam/tests/testDummyFactor.cpp +++ b/gtsam_unstable/slam/tests/testDummyFactor.cpp @@ -47,7 +47,7 @@ TEST( testDummyFactor, basics ) { CHECK(actLinearization); noiseModel::Diagonal::shared_ptr model3 = noiseModel::Unit::Create(3); GaussianFactor::shared_ptr expLinearization(new JacobianFactor( - key1, zeros(3,3), key2, zeros(3,3), zero(3), model3)); + key1, Matrix::Zero(3,3), key2, Matrix::Zero(3,3), Z_3x1, model3)); EXPECT(assert_equal(*expLinearization, *actLinearization, tol)); } diff --git a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp index 476447f8b..35bf5790e 100644 --- a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp +++ b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp @@ -41,8 +41,8 @@ TEST( EquivInertialNavFactor_GlobalVel, Constructor) Vector delta_vel_in_t0 = Vector3(0.0, 0.0, 0.0); Vector delta_angles = Vector3(0.0, 0.0, 0.0); double delta_t = 0.0; - Matrix EquivCov_Overall = zeros(15,15); - Matrix Jacobian_wrt_t0_Overall = eye(15); + Matrix EquivCov_Overall = Matrix::Zero(15,15); + Matrix Jacobian_wrt_t0_Overall = Matrix::Identity(15,15); imuBias::ConstantBias bias1 = imuBias::ConstantBias(); // Earth Terms (gravity, etc) diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index 75535eebc..b84f7e080 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -152,7 +152,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ExpectedErr(Z_9x1); CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } @@ -185,7 +185,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ExpectedErr(Z_9x1); CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } @@ -225,7 +225,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ExpectedErr(Z_9x1); // TODO: Expected values need to be updated for global velocity version CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); @@ -488,7 +488,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ExpectedErr(Z_9x1); CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } @@ -529,7 +529,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ExpectedErr(Z_9x1); CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } @@ -579,7 +579,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ExpectedErr(Z_9x1); // TODO: Expected values need to be updated for global velocity version CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); diff --git a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp index a06c39182..210018ed3 100644 --- a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp @@ -34,7 +34,7 @@ TEST( testRelativeElevationFactor, level_zero_error ) { double measured = 2.0; RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; - EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, point1, actH1, actH2))); + EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose1, point1, actH1, actH2))); Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); Matrix expH2 = numericalDerivative22( @@ -79,7 +79,7 @@ TEST( testRelativeElevationFactor, rotated_zero_error ) { double measured = 2.0; RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; - EXPECT(assert_equal(zero(1), factor.evaluateError(pose2, point1, actH1, actH2))); + EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose2, point1, actH1, actH2))); Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); Matrix expH2 = numericalDerivative22( diff --git a/gtsam_unstable/slam/tests/testSerialization.cpp b/gtsam_unstable/slam/tests/testSerialization.cpp index 1d8b30850..dc05711e3 100644 --- a/gtsam_unstable/slam/tests/testSerialization.cpp +++ b/gtsam_unstable/slam/tests/testSerialization.cpp @@ -48,9 +48,9 @@ Values exampleValues() { NonlinearFactorGraph exampleGraph() { NonlinearFactorGraph graph; - graph.add(PriorFactor(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(ones(3)))); - graph.add(BetweenFactor(234, 567, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(ones(3)))); - graph.add(BearingRangeFactor(234, 567, Rot2::fromAngle(0.3), 2.0, noiseModel::Diagonal::Sigmas(ones(2)))); + graph.add(PriorFactor(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3)))); + graph.add(BetweenFactor(234, 567, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3)))); + graph.add(BearingRangeFactor(234, 567, Rot2::fromAngle(0.3), 2.0, noiseModel::Diagonal::Sigmas(Vector::Ones(2)))); return graph; } diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index ee06dbce2..9ae4aa928 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -70,7 +70,7 @@ TEST( SmartRangeFactor, unwhitenedError ) { // Check Jacobian for n==1 vector H1(1); f.unwhitenedError(values, H1); // with H now ! - CHECK(assert_equal(zeros(3,1), H1.front())); + CHECK(assert_equal(Matrix::Zero(3,1), H1.front())); // Whenever there are two ranges or less, error should be zero Vector actual1 = f.unwhitenedError(values); diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 52786702d..233dbdceb 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -31,7 +31,6 @@ using namespace std; using namespace gtsam; // typedefs -typedef Eigen::Matrix Vector1; typedef Expression Point3_; typedef Expression Event_; @@ -52,7 +51,7 @@ TEST( TOAFactor, NewWay ) { Event_ eventExpression(key); Point3_ microphoneConstant(microphoneAt0); // constant expression double measurement = 7; - double_ expression(&Event::toa, eventExpression, microphoneConstant); + Double_ expression(&Event::toa, eventExpression, microphoneConstant); ExpressionFactor factor(model, measurement, expression); } @@ -92,7 +91,7 @@ TEST( TOAFactor, WholeEnchilada ) { Event_ eventExpression(key); for (size_t i = 0; i < K; i++) { Point3_ microphone_i(microphones[i]); // constant expression - double_ predictTOA(&Event::toa, eventExpression, microphone_i); + Double_ predictTOA(&Event::toa, eventExpression, microphone_i); graph.addExpressionFactor(predictTOA, simulatedTOA[i], model); } diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp index d8d720e83..01d4b152d 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp @@ -97,7 +97,7 @@ TEST( TransformBtwRobotsUnaryFactor, unwhitenedError) Vector err = g.unwhitenedError(values); // Equals - CHECK(assert_equal(err, zero(3), 1e-5)); + CHECK(assert_equal(err, (Vector) Z_3x1, 1e-5)); } /* ************************************************************************* */ @@ -131,7 +131,7 @@ TEST( TransformBtwRobotsUnaryFactor, unwhitenedError2) Vector err = g.unwhitenedError(values); // Equals - CHECK(assert_equal(err, zero(3), 1e-5)); + CHECK(assert_equal(err, Z_3x1, 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp index dbd90f3a3..b200a3e58 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp @@ -108,7 +108,7 @@ TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError) Vector err = g.unwhitenedError(values); // Equals - CHECK(assert_equal(err, zero(3), 1e-5)); + CHECK(assert_equal(err, Z_3x1, 1e-5)); } /* ************************************************************************* */ @@ -147,7 +147,7 @@ TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError2) Vector err = g.unwhitenedError(values); // Equals - CHECK(assert_equal(err, zero(3), 1e-5)); + CHECK(assert_equal(err, Z_3x1, 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/testing_tools/base/cholScalingTest.m b/gtsam_unstable/testing_tools/base/cholScalingTest.m index ed347a523..24f597ed7 100644 --- a/gtsam_unstable/testing_tools/base/cholScalingTest.m +++ b/gtsam_unstable/testing_tools/base/cholScalingTest.m @@ -19,8 +19,8 @@ badscale = 1e-8; Acoeffs = [ 1 11 badscale - (1:10)' (1:10)' -ones(10,1) - (1:10)' (2:11)' ones(10,1) + (1:10)' (1:10)' -Matrix::Ones(10,1) + (1:10)' (2:11)' Matrix::Ones(10,1) ]'; A = full(sparse(Acoeffs(1,:), Acoeffs(2,:), Acoeffs(3,:))); b = zeros(size(A,1), 1); diff --git a/matlab/+gtsam/plot3DPoints.m b/matlab/+gtsam/plot3DPoints.m index 46e161807..20931499b 100644 --- a/matlab/+gtsam/plot3DPoints.m +++ b/matlab/+gtsam/plot3DPoints.m @@ -7,7 +7,7 @@ function plot3DPoints(values, linespec, marginals) import gtsam.* if ~exist('linespec', 'var') || isempty(linespec) - linespec = 'g'; + linespec = 'g*'; end haveMarginals = exist('marginals', 'var'); keys = KeyVector(values.keys); @@ -15,19 +15,25 @@ keys = KeyVector(values.keys); holdstate = ishold; hold on -% Plot points and covariance matrices -for i = 0:keys.size-1 - key = keys.at(i); - try - p = values.atPoint3(key); - if haveMarginals +if haveMarginals + % Plot points and covariance matrices (slow) + for i = 0:keys.size-1 + key = keys.at(i); + try + p = values.atPoint3(key) P = marginals.marginalCovariance(key); gtsam.plotPoint3(p, linespec, P); - else - gtsam.plotPoint3(p, linespec); + catch + % I guess it's not a Point3 end - catch - % I guess it's not a Point3 + end +else + % Extract all in C++ and plot all at once (fast) + P = utilities.extractPoint3(values); + if size(linespec,2)==1 + plot3(P(:,1),P(:,2),P(:,3),[linespec '*']); + else + plot3(P(:,1),P(:,2),P(:,3),linespec); end end diff --git a/matlab/unstable_examples/FlightCameraTransformIMU.m b/matlab/unstable_examples/FlightCameraTransformIMU.m index b0b37ee33..9a8a27344 100644 --- a/matlab/unstable_examples/FlightCameraTransformIMU.m +++ b/matlab/unstable_examples/FlightCameraTransformIMU.m @@ -1,3 +1,7 @@ +% Simulation for concurrent IMU, camera, IMU-camera transform estimation during flight with known landmarks +% author: Chris Beall +% date: July 2014 + clear all; clf; @@ -24,13 +28,19 @@ if(write_video) open(videoObj); end -% IMU parameters +%% IMU parameters IMU_metadata.AccelerometerSigma = 1e-2; IMU_metadata.GyroscopeSigma = 1e-2; IMU_metadata.AccelerometerBiasSigma = 1e-6; IMU_metadata.GyroscopeBiasSigma = 1e-6; IMU_metadata.IntegrationSigma = 1e-1; +n_gravity = [0;0;-9.8]; +IMU_params = PreintegrationParams(n_gravity); +IMU_params.setAccelerometerCovariance(IMU_metadata.AccelerometerSigma.^2 * eye(3)); +IMU_params.setGyroscopeCovariance(IMU_metadata.GyroscopeSigma.^2 * eye(3)); +IMU_params.setIntegrationCovariance(IMU_metadata.IntegrationSigma.^2 * eye(3)); + transformKey = 1000; calibrationKey = 2000; @@ -52,7 +62,7 @@ K = Cal3_S2(20,1280,960); % initialize K incorrectly K_corrupt = Cal3_S2(K.fx()+10,K.fy()+10,0,K.px(),K.py()); -isamParams = gtsam.ISAM2Params; +isamParams = ISAM2Params; isamParams.setFactorization('QR'); isam = ISAM2(isamParams); @@ -63,7 +73,6 @@ currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0); sigma_init_b = noiseModel.Isotropic.Sigmas([ 0.100; 0.100; 0.100; 5.00e-05; 5.00e-05; 5.00e-05 ]); sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * ones(3,1); IMU_metadata.GyroscopeBiasSigma * ones(3,1) ]; -g = [0;0;-9.8]; w_coriolis = [0;0;0]; %% generate trajectory and landmarks @@ -99,7 +108,9 @@ zlabel('z'); title('Estimated vs. actual IMU-cam transform'); axis equal; +%% Main loop for i=1:size(trajectory)-1 + %% Preliminaries xKey = symbol('x',i); pose = trajectory.atPose3(xKey); % GT pose pose_t = pose.translation(); % GT pose-translation @@ -134,6 +145,7 @@ for i=1:size(trajectory)-1 gps_pose = pose.retract([0; 0; 0; normrnd(0,gps_noise,3,1)]); fg.add(PoseTranslationPrior3D(xKey, gps_pose, GPS_trans_cov)); + %% First-time initialization if i==1 % camera transform if use_camera_transform_noise @@ -153,12 +165,10 @@ for i=1:size(trajectory)-1 result = initial; end - % priors on first two poses + %% priors on first two poses if i < 3 -% fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); + % fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b)); - - end %% the 'normal' case @@ -181,12 +191,14 @@ for i=1:size(trajectory)-1 zKey = measurementKeys.at(zz); lKey = symbol('l',symbolIndex(zKey)); - fg.add(TransformCalProjectionFactorCal3_S2(measurements.atPoint2(zKey), ... + fg.add(ProjectionFactorPPPCCal3_S2(measurements.atPoint2(zKey), ... z_cov, xKey, transformKey, lKey, calibrationKey, false, true)); % only add landmark to values if doesn't exist yet if ~result.exists(lKey) - noisy_landmark = landmarks.atPoint3(lKey).compose(Point3(normrnd(0,landmark_noise,3,1))); + p = landmarks.atPoint3(lKey); + n = normrnd(0,landmark_noise,3,1); + noisy_landmark = Point3(p.x()+n(1),p.y()+n(2),p.z()+n(3)); initial.insert(lKey, noisy_landmark); % and add a prior since its position is known @@ -195,14 +207,12 @@ for i=1:size(trajectory)-1 end end % end landmark observations - %% IMU deltaT = 1; logmap = Pose3.Logmap(step); omega = logmap(1:3); velocity = logmap(4:6); - % Simulate IMU measurements, considering Coriolis effect % (in this simple example we neglect gravity and there are no other forces acting on the body) acc_omega = imuSimulator.calculateIMUMeas_coriolis( ... @@ -211,11 +221,9 @@ for i=1:size(trajectory)-1 % [ currentIMUPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ... % currentIMUPoseGlobal, omega, velocity, velocity, deltaT); - currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ... - currentBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ... - IMU_metadata.GyroscopeSigma.^2 * eye(3), IMU_metadata.IntegrationSigma.^2 * eye(3)); + currentSummarizedMeasurement = PreintegratedImuMeasurements(IMU_params,currentBias); - accMeas = acc_omega(1:3)-g; + accMeas = acc_omega(1:3)-n_gravity; omegaMeas = acc_omega(4:6); currentSummarizedMeasurement.integrateMeasurement(accMeas, omegaMeas, deltaT); @@ -223,13 +231,13 @@ for i=1:size(trajectory)-1 fg.add(ImuFactor( ... xKey_prev, currentVelKey-1, ... xKey, currentVelKey, ... - currentBiasKey, currentSummarizedMeasurement, g, w_coriolis)); + currentBiasKey, currentSummarizedMeasurement)); % Bias evolution as given in the IMU metadata fg.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ... noiseModel.Diagonal.Sigmas(sqrt(10) * sigma_between_b))); - % ISAM update + %% ISAM update isam.update(fg, initial); result = isam.calculateEstimate(); @@ -295,10 +303,8 @@ for i=1:size(trajectory)-1 end -% print out final camera transform +%% print out final camera transform and write video result.at(transformKey); - - if(write_video) close(videoObj); end \ No newline at end of file diff --git a/python/handwritten/nonlinear/Values.cpp b/python/handwritten/nonlinear/Values.cpp index b635589c3..0d1349972 100644 --- a/python/handwritten/nonlinear/Values.cpp +++ b/python/handwritten/nonlinear/Values.cpp @@ -56,7 +56,6 @@ void exportValues(){ .def("empty", &Values::empty) .def("equals", &Values::equals) .def("erase", &Values::erase) - .def("insert_fixed", &Values::insertFixed) .def("print", &Values::print, print_overloads(args("s"))) .def("size", &Values::size) .def("swap", &Values::swap) @@ -69,15 +68,15 @@ void exportValues(){ .def("insert", insert_bias) .def("insert", insert_navstate) .def("insert", insert_vector3) - .def("atPoint2", &Values::at, return_value_policy()) - .def("atRot2", &Values::at, return_value_policy()) - .def("atPose2", &Values::at, return_value_policy()) - .def("atPoint3", &Values::at, return_value_policy()) - .def("atRot3", &Values::at, return_value_policy()) - .def("atPose3", &Values::at, return_value_policy()) - .def("atConstantBias", &Values::at, return_value_policy()) - .def("atNavState", &Values::at, return_value_policy()) - .def("atVector3", &Values::at, return_value_policy()) + .def("atPoint2", &Values::at) + .def("atRot2", &Values::at) + .def("atPose2", &Values::at) + .def("atPoint3", &Values::at) + .def("atRot3", &Values::at) + .def("atPose3", &Values::at) + .def("atConstantBias", &Values::at) + .def("atNavState", &Values::at) + .def("atVector3", &Values::at) .def("exists", exists1) .def("keys", &Values::keys) ; diff --git a/tests/simulated2D.h b/tests/simulated2D.h index a1080a6de..0012f5f45 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -90,7 +90,7 @@ namespace simulated2D { /// Prior on a single pose, optionally returns derivative inline Point2 prior(const Point2& x, boost::optional H = boost::none) { - if (H) *H = gtsam::eye(2); + if (H) *H = I_2x2; return x; } @@ -102,8 +102,8 @@ namespace simulated2D { /// odometry between two poses, optionally returns derivative inline Point2 odo(const Point2& x1, const Point2& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) { - if (H1) *H1 = -gtsam::eye(2); - if (H2) *H2 = gtsam::eye(2); + if (H1) *H1 = -I_2x2; + if (H2) *H2 = I_2x2; return x2 - x1; } @@ -115,8 +115,8 @@ namespace simulated2D { /// measurement between landmark and pose, optionally returns derivative inline Point2 mea(const Point2& x, const Point2& l, boost::optional H1 = boost::none, boost::optional H2 = boost::none) { - if (H1) *H1 = -gtsam::eye(2); - if (H2) *H2 = gtsam::eye(2); + if (H1) *H1 = -I_2x2; + if (H2) *H2 = I_2x2; return l - x; } diff --git a/tests/simulated2DConstraints.h b/tests/simulated2DConstraints.h index f593ab23a..ccc734cfd 100644 --- a/tests/simulated2DConstraints.h +++ b/tests/simulated2DConstraints.h @@ -90,7 +90,7 @@ namespace simulated2D { virtual double value(const Point& x, boost::optional H = boost::none) const { if (H) { - Matrix D = zeros(1, traits::GetDimension(x)); + Matrix D = Matrix::Zero(1, traits::GetDimension(x)); D(0, IDX) = 1.0; *H = D; } diff --git a/tests/simulated2DOriented.h b/tests/simulated2DOriented.h index b05fb45c1..9e604cb3c 100644 --- a/tests/simulated2DOriented.h +++ b/tests/simulated2DOriented.h @@ -63,7 +63,7 @@ namespace simulated2DOriented { /// Prior on a single pose, optional derivative version Pose2 prior(const Pose2& x, boost::optional H = boost::none) { - if (H) *H = gtsam::eye(3); + if (H) *H = I_3x3; return x; } diff --git a/tests/simulated3D.h b/tests/simulated3D.h index 84d9ec8cd..3c92e733e 100644 --- a/tests/simulated3D.h +++ b/tests/simulated3D.h @@ -39,7 +39,7 @@ namespace simulated3D { * Prior on a single pose */ Point3 prior(const Point3& x, boost::optional H = boost::none) { - if (H) *H = eye(3); + if (H) *H = I_3x3; return x; } @@ -49,8 +49,8 @@ Point3 prior(const Point3& x, boost::optional H = boost::none) { Point3 odo(const Point3& x1, const Point3& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) { - if (H1) *H1 = -1 * eye(3); - if (H2) *H2 = eye(3); + if (H1) *H1 = -1 * I_3x3; + if (H2) *H2 = I_3x3; return x2 - x1; } @@ -60,8 +60,8 @@ Point3 odo(const Point3& x1, const Point3& x2, Point3 mea(const Point3& x, const Point3& l, boost::optional H1 = boost::none, boost::optional H2 = boost::none) { - if (H1) *H1 = -1 * eye(3); - if (H2) *H2 = eye(3); + if (H1) *H1 = -1 * I_3x3; + if (H2) *H2 = I_3x3; return l - x; } diff --git a/tests/smallExample.h b/tests/smallExample.h index 9866d22aa..215655593 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -260,9 +260,9 @@ inline VectorValues createZeroDelta() { using symbol_shorthand::X; using symbol_shorthand::L; VectorValues c; - c.insert(L(1), zero(2)); - c.insert(X(1), zero(2)); - c.insert(X(2), zero(2)); + c.insert(L(1), Z_2x1); + c.insert(X(1), Z_2x1); + c.insert(X(2), Z_2x1); return c; } @@ -274,16 +274,16 @@ inline GaussianFactorGraph createGaussianFactorGraph() { GaussianFactorGraph fg; // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg += JacobianFactor(X(1), 10*eye(2), -1.0*ones(2)); + fg += JacobianFactor(X(1), 10*I_2x2, -1.0*Vector::Ones(2)); // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg += JacobianFactor(X(1), -10*eye(2), X(2), 10*eye(2), Vector2(2.0, -1.0)); + fg += JacobianFactor(X(1), -10*I_2x2, X(2), 10*I_2x2, Vector2(2.0, -1.0)); // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg += JacobianFactor(X(1), -5*eye(2), L(1), 5*eye(2), Vector2(0.0, 1.0)); + fg += JacobianFactor(X(1), -5*I_2x2, L(1), 5*I_2x2, Vector2(0.0, 1.0)); // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg += JacobianFactor(X(2), -5*eye(2), L(1), 5*eye(2), Vector2(-1.0, 1.5)); + fg += JacobianFactor(X(2), -5*I_2x2, L(1), 5*I_2x2, Vector2(-1.0, 1.5)); return fg; } @@ -409,7 +409,7 @@ inline GaussianFactorGraph createSimpleConstraintGraph() { using namespace impl; // create unary factor // prior on _x_, mean = [1,-1], sigma=0.1 - Matrix Ax = eye(2); + Matrix Ax = I_2x2; Vector b1(2); b1(0) = 1.0; b1(1) = -1.0; @@ -419,8 +419,8 @@ inline GaussianFactorGraph createSimpleConstraintGraph() { // between _x_ and _y_, that is going to be the only factor on _y_ // |1 0||x_1| + |-1 0||y_1| = |0| // |0 1||x_2| | 0 -1||y_2| |0| - Matrix Ax1 = eye(2); - Matrix Ay1 = eye(2) * -1; + Matrix Ax1 = I_2x2; + Matrix Ay1 = I_2x2 * -1; Vector b2 = Vector2(0.0, 0.0); JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, constraintModel)); @@ -450,7 +450,7 @@ inline GaussianFactorGraph createSingleConstraintGraph() { using namespace impl; // create unary factor // prior on _x_, mean = [1,-1], sigma=0.1 - Matrix Ax = eye(2); + Matrix Ax = I_2x2; Vector b1(2); b1(0) = 1.0; b1(1) = -1.0; @@ -466,7 +466,7 @@ inline GaussianFactorGraph createSingleConstraintGraph() { Ax1(0, 1) = 2.0; Ax1(1, 0) = 2.0; Ax1(1, 1) = 1.0; - Matrix Ay1 = eye(2) * 10; + Matrix Ay1 = I_2x2 * 10; Vector b2 = Vector2(1.0, 2.0); JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, constraintModel)); @@ -492,7 +492,7 @@ inline VectorValues createSingleConstraintValues() { inline GaussianFactorGraph createMultiConstraintGraph() { using namespace impl; // unary factor 1 - Matrix A = eye(2); + Matrix A = I_2x2; Vector b = Vector2(-2.0, 2.0); JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1)); diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index d4cefd329..b0b748d95 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -54,10 +54,10 @@ TEST( testBoundingConstraint, unary_basics_inactive1 ) { EXPECT_DOUBLES_EQUAL(2.0, constraint2.threshold(), tol); EXPECT(constraint1.isGreaterThan()); EXPECT(constraint2.isGreaterThan()); - EXPECT(assert_equal(ones(1), constraint1.evaluateError(pt1), tol)); - EXPECT(assert_equal(ones(1), constraint2.evaluateError(pt1), tol)); - EXPECT(assert_equal(zero(1), constraint1.unwhitenedError(config), tol)); - EXPECT(assert_equal(zero(1), constraint2.unwhitenedError(config), tol)); + EXPECT(assert_equal(I_1x1, constraint1.evaluateError(pt1), tol)); + EXPECT(assert_equal(I_1x1, constraint2.evaluateError(pt1), tol)); + EXPECT(assert_equal(Z_1x1, constraint1.unwhitenedError(config), tol)); + EXPECT(assert_equal(Z_1x1, constraint2.unwhitenedError(config), tol)); EXPECT_DOUBLES_EQUAL(0.0, constraint1.error(config), tol); EXPECT_DOUBLES_EQUAL(0.0, constraint2.error(config), tol); } @@ -75,8 +75,8 @@ TEST( testBoundingConstraint, unary_basics_inactive2 ) { EXPECT(!constraint4.isGreaterThan()); EXPECT(assert_equal(Vector::Constant(1, 3.0), constraint3.evaluateError(pt2), tol)); EXPECT(assert_equal(Vector::Constant(1, 5.0), constraint4.evaluateError(pt2), tol)); - EXPECT(assert_equal(zero(1), constraint3.unwhitenedError(config), tol)); - EXPECT(assert_equal(zero(1), constraint4.unwhitenedError(config), tol)); + EXPECT(assert_equal(Z_1x1, constraint3.unwhitenedError(config), tol)); + EXPECT(assert_equal(Z_1x1, constraint4.unwhitenedError(config), tol)); EXPECT_DOUBLES_EQUAL(0.0, constraint3.error(config), tol); EXPECT_DOUBLES_EQUAL(0.0, constraint4.error(config), tol); } @@ -103,10 +103,10 @@ TEST( testBoundingConstraint, unary_basics_active2 ) { config.insert(key, pt1); EXPECT(constraint3.active(config)); EXPECT(constraint4.active(config)); - EXPECT(assert_equal(-1.0 * ones(1), constraint3.evaluateError(pt1), tol)); - EXPECT(assert_equal(-1.0 * ones(1), constraint4.evaluateError(pt1), tol)); - EXPECT(assert_equal(-1.0 * ones(1), constraint3.unwhitenedError(config), tol)); - EXPECT(assert_equal(-1.0 * ones(1), constraint4.unwhitenedError(config), tol)); + EXPECT(assert_equal(-1.0 * I_1x1, constraint3.evaluateError(pt1), tol)); + EXPECT(assert_equal(-1.0 * I_1x1, constraint4.evaluateError(pt1), tol)); + EXPECT(assert_equal(-1.0 * I_1x1, constraint3.unwhitenedError(config), tol)); + EXPECT(assert_equal(-1.0 * I_1x1, constraint4.unwhitenedError(config), tol)); EXPECT_DOUBLES_EQUAL(5.0, constraint3.error(config), tol); EXPECT_DOUBLES_EQUAL(5.0, constraint4.error(config), tol); } @@ -188,32 +188,32 @@ TEST( testBoundingConstraint, MaxDistance_basics) { EXPECT(rangeBound.dim() == 1); EXPECT(assert_equal((Vector(1) << 2.0).finished(), rangeBound.evaluateError(pt1, pt1))); - EXPECT(assert_equal(ones(1), rangeBound.evaluateError(pt1, pt2))); - EXPECT(assert_equal(zero(1), rangeBound.evaluateError(pt1, pt3))); - EXPECT(assert_equal(-1.0*ones(1), rangeBound.evaluateError(pt1, pt4))); + EXPECT(assert_equal(I_1x1, rangeBound.evaluateError(pt1, pt2))); + EXPECT(assert_equal(Z_1x1, rangeBound.evaluateError(pt1, pt3))); + EXPECT(assert_equal(-1.0*I_1x1, rangeBound.evaluateError(pt1, pt4))); Values config1; config1.insert(key1, pt1); config1.insert(key2, pt1); EXPECT(!rangeBound.active(config1)); - EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1))); + EXPECT(assert_equal(Z_1x1, rangeBound.unwhitenedError(config1))); EXPECT(!rangeBound.linearize(config1)); EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol); config1.update(key2, pt2); EXPECT(!rangeBound.active(config1)); - EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1))); + EXPECT(assert_equal(Z_1x1, rangeBound.unwhitenedError(config1))); EXPECT(!rangeBound.linearize(config1)); EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol); config1.update(key2, pt3); EXPECT(rangeBound.active(config1)); - EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1))); + EXPECT(assert_equal(Z_1x1, rangeBound.unwhitenedError(config1))); EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol); config1.update(key2, pt4); EXPECT(rangeBound.active(config1)); - EXPECT(assert_equal(-1.0*ones(1), rangeBound.unwhitenedError(config1))); + EXPECT(assert_equal(-1.0*I_1x1, rangeBound.unwhitenedError(config1))); EXPECT_DOUBLES_EQUAL(0.5*mu, rangeBound.error(config1), tol); } diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 282a6b816..4fda27cdb 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -140,7 +140,7 @@ typedef Eigen::Matrix Matrix93; Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) { Vector9 v; v << p, p, p; - if (H) *H << eye(3), eye(3), eye(3); + if (H) *H << I_3x3, I_3x3, I_3x3; return v; } typedef Eigen::Matrix Matrix9; @@ -334,11 +334,11 @@ TEST(ExpressionFactor, Compose1) { // Check unwhitenedError std::vector H(2); Vector actual = f.unwhitenedError(values, H); - EXPECT( assert_equal(eye(3), H[0],1e-9)); - EXPECT( assert_equal(eye(3), H[1],1e-9)); + EXPECT( assert_equal(I_3x3, H[0],1e-9)); + EXPECT( assert_equal(I_3x3, H[1],1e-9)); // Check linearization - JacobianFactor expected(1, eye(3), 2, eye(3), zero(3)); + JacobianFactor expected(1, I_3x3, 2, I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); @@ -364,10 +364,10 @@ TEST(ExpressionFactor, compose2) { std::vector H(1); Vector actual = f.unwhitenedError(values, H); EXPECT_LONGS_EQUAL(1, H.size()); - EXPECT( assert_equal(2*eye(3), H[0],1e-9)); + EXPECT( assert_equal(2*I_3x3, H[0],1e-9)); // Check linearization - JacobianFactor expected(1, 2 * eye(3), zero(3)); + JacobianFactor expected(1, 2 * I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); @@ -393,10 +393,10 @@ TEST(ExpressionFactor, compose3) { std::vector H(1); Vector actual = f.unwhitenedError(values, H); EXPECT_LONGS_EQUAL(1, H.size()); - EXPECT( assert_equal(eye(3), H[0],1e-9)); + EXPECT( assert_equal(I_3x3, H[0],1e-9)); // Check linearization - JacobianFactor expected(3, eye(3), zero(3)); + JacobianFactor expected(3, I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); @@ -409,11 +409,11 @@ Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) if (H1) - *H1 = eye(3); + *H1 = I_3x3; if (H2) - *H2 = eye(3); + *H2 = I_3x3; if (H3) - *H3 = eye(3); + *H3 = I_3x3; return R1 * (R2 * R3); } @@ -436,12 +436,12 @@ TEST(ExpressionFactor, composeTernary) { std::vector H(3); Vector actual = f.unwhitenedError(values, H); EXPECT_LONGS_EQUAL(3, H.size()); - EXPECT( assert_equal(eye(3), H[0],1e-9)); - EXPECT( assert_equal(eye(3), H[1],1e-9)); - EXPECT( assert_equal(eye(3), H[2],1e-9)); + EXPECT( assert_equal(I_3x3, H[0],1e-9)); + EXPECT( assert_equal(I_3x3, H[1],1e-9)); + EXPECT( assert_equal(I_3x3, H[2],1e-9)); // Check linearization - JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3)); + JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index a7bcf7153..00ab4a16c 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -223,7 +223,7 @@ public: *H1 = -F(p1); if(H2) - *H2 = eye(dim()); + *H2 = Matrix::Identity(dim(),dim()); // Return the error between the prediction and the supplied value of p2 return (p2 - prediction).vector(); diff --git a/tests/testGaussianBayesTreeB.cpp b/tests/testGaussianBayesTreeB.cpp index cf54ce96e..e3c17fa3f 100644 --- a/tests/testGaussianBayesTreeB.cpp +++ b/tests/testGaussianBayesTreeB.cpp @@ -79,7 +79,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts ) double sigma3 = 0.61808; Matrix A56 = (Matrix(2,2) << -0.382022,0.,0.,-0.382022).finished(); GaussianBayesNet expected3; - expected3 += GaussianConditional(X(5), zero(2), eye(2)/sigma3, X(6), A56/sigma3); + expected3 += GaussianConditional(X(5), Z_2x1, I_2x2/sigma3, X(6), A56/sigma3); GaussianBayesTree::sharedClique C3 = bayesTree[X(4)]; GaussianBayesNet actual3 = C3->shortcut(R); EXPECT(assert_equal(expected3,actual3,tol)); @@ -88,7 +88,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts ) double sigma4 = 0.661968; Matrix A46 = (Matrix(2,2) << -0.146067,0.,0.,-0.146067).finished(); GaussianBayesNet expected4; - expected4 += GaussianConditional(X(4), zero(2), eye(2)/sigma4, X(6), A46/sigma4); + expected4 += GaussianConditional(X(4), Z_2x1, I_2x2/sigma4, X(6), A46/sigma4); GaussianBayesTree::sharedClique C4 = bayesTree[X(3)]; GaussianBayesNet actual4 = C4->shortcut(R); EXPECT(assert_equal(expected4,actual4,tol)); @@ -132,9 +132,9 @@ TEST( GaussianBayesTree, balanced_smoother_marginals ) double tol=1e-5; // Check marginal on x1 - JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), zero(2), sigmax1); + JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), Z_2x1, sigmax1); JacobianFactor actual1 = *bayesTree.marginalFactor(X(1)); - Matrix expectedCovarianceX1 = eye(2,2) * (sigmax1 * sigmax1); + Matrix expectedCovarianceX1 = I_2x2 * (sigmax1 * sigmax1); Matrix actualCovarianceX1; GaussianFactor::shared_ptr m = bayesTree.marginalFactor(X(1), EliminateCholesky); actualCovarianceX1 = bayesTree.marginalFactor(X(1), EliminateCholesky)->information().inverse(); @@ -143,22 +143,22 @@ TEST( GaussianBayesTree, balanced_smoother_marginals ) // Check marginal on x2 double sigx2 = 0.68712938; // FIXME: this should be corrected analytically - JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), zero(2), sigx2); + JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), Z_2x1, sigx2); JacobianFactor actual2 = *bayesTree.marginalFactor(X(2)); EXPECT(assert_equal(expected2,actual2,tol)); // Check marginal on x3 - JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), zero(2), sigmax3); + JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), Z_2x1, sigmax3); JacobianFactor actual3 = *bayesTree.marginalFactor(X(3)); EXPECT(assert_equal(expected3,actual3,tol)); // Check marginal on x4 - JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), zero(2), sigmax4); + JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), Z_2x1, sigmax4); JacobianFactor actual4 = *bayesTree.marginalFactor(X(4)); EXPECT(assert_equal(expected4,actual4,tol)); // Check marginal on x7 (should be equal to x1) - JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), zero(2), sigmax7); + JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), Z_2x1, sigmax7); JacobianFactor actual7 = *bayesTree.marginalFactor(X(7)); EXPECT(assert_equal(expected7,actual7,tol)); } @@ -212,8 +212,8 @@ TEST( GaussianBayesTree, balanced_smoother_shortcuts ) // // // Check the clique marginal P(C3) // double sigmax2_alt = 1/1.45533; // THIS NEEDS TO BE CHECKED! -// GaussianBayesNet expected = simpleGaussian(ordering[X(2)],zero(2),sigmax2_alt); -// push_front(expected,ordering[X(1)], zero(2), eye(2)*sqrt(2), ordering[X(2)], -eye(2)*sqrt(2)/2, ones(2)); +// GaussianBayesNet expected = simpleGaussian(ordering[X(2)],Z_2x1,sigmax2_alt); +// push_front(expected,ordering[X(1)], Z_2x1, eye(2)*sqrt(2), ordering[X(2)], -eye(2)*sqrt(2)/2, ones(2)); // GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering[X(1)]]; // GaussianFactorGraph marginal = C3->marginal(R); // GaussianVariableIndex varIndex(marginal); @@ -243,22 +243,22 @@ TEST( GaussianBayesTree, balanced_smoother_joint ) GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering); // Conditional density elements reused by both tests - const Matrix I = eye(2), A = -0.00429185*I; + const Matrix I = I_2x2, A = -0.00429185*I; // Check the joint density P(x1,x7) factored as P(x1|x7)P(x7) GaussianBayesNet expected1 = list_of // Why does the sign get flipped on the prior? - (GaussianConditional(X(1), zero(2), I/sigmax7, X(7), A/sigmax7)) - (GaussianConditional(X(7), zero(2), -1*I/sigmax7)); + (GaussianConditional(X(1), Z_2x1, I/sigmax7, X(7), A/sigmax7)) + (GaussianConditional(X(7), Z_2x1, -1*I/sigmax7)); GaussianBayesNet actual1 = *bayesTree.jointBayesNet(X(1),X(7)); EXPECT(assert_equal(expected1, actual1, tol)); // // Check the joint density P(x7,x1) factored as P(x7|x1)P(x1) // GaussianBayesNet expected2; // GaussianConditional::shared_ptr - // parent2(new GaussianConditional(X(1), zero(2), -1*I/sigmax1, ones(2))); + // parent2(new GaussianConditional(X(1), Z_2x1, -1*I/sigmax1, ones(2))); // expected2.push_front(parent2); - // push_front(expected2,X(7), zero(2), I/sigmax1, X(1), A/sigmax1, sigma); + // push_front(expected2,X(7), Z_2x1, I/sigmax1, X(1), A/sigmax1, sigma); // GaussianBayesNet actual2 = *bayesTree.jointBayesNet(X(7),X(1)); // EXPECT(assert_equal(expected2,actual2,tol)); @@ -266,19 +266,19 @@ TEST( GaussianBayesTree, balanced_smoother_joint ) double sig14 = 0.784465; Matrix A14 = -0.0769231*I; GaussianBayesNet expected3 = list_of - (GaussianConditional(X(1), zero(2), I/sig14, X(4), A14/sig14)) - (GaussianConditional(X(4), zero(2), I/sigmax4)); + (GaussianConditional(X(1), Z_2x1, I/sig14, X(4), A14/sig14)) + (GaussianConditional(X(4), Z_2x1, I/sigmax4)); GaussianBayesNet actual3 = *bayesTree.jointBayesNet(X(1),X(4)); EXPECT(assert_equal(expected3,actual3,tol)); // // Check the joint density P(x4,x1), i.e. with a root variable, factored the other way // GaussianBayesNet expected4; // GaussianConditional::shared_ptr - // parent4(new GaussianConditional(X(1), zero(2), -1.0*I/sigmax1, ones(2))); + // parent4(new GaussianConditional(X(1), Z_2x1, -1.0*I/sigmax1, ones(2))); // expected4.push_front(parent4); // double sig41 = 0.668096; // Matrix A41 = -0.055794*I; - // push_front(expected4,X(4), zero(2), I/sig41, X(1), A41/sig41, sigma); + // push_front(expected4,X(4), Z_2x1, I/sig41, X(1), A41/sig41, sigma); // GaussianBayesNet actual4 = *bayesTree.jointBayesNet(X(4),X(1)); // EXPECT(assert_equal(expected4,actual4,tol)); } diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 88dc91ce7..10de57435 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -78,7 +78,7 @@ TEST( GaussianFactorGraph, eliminateOne_x1 ) conditional = result.first->front(); // create expected Conditional Gaussian - Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; + Matrix I = 15*I_2x2, R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; Vector d = Vector2(-0.133333, -0.0222222); GaussianConditional expected(X(1),15*d,R11,L(1),S12,X(2),S13); @@ -96,8 +96,8 @@ TEST( GaussianFactorGraph, eliminateOne_x2 ) // create expected Conditional Gaussian double sig = 0.0894427; - Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; - Vector d = Vector2(0.2, -0.14)/sig, sigma = ones(2); + Matrix I = I_2x2/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; + Vector d = Vector2(0.2, -0.14)/sig, sigma = Vector::Ones(2); GaussianConditional expected(ordering[X(2)],d,R11,ordering[L(1)],S12,ordering[X(1)],S13,sigma); EXPECT(assert_equal(expected,*actual,tol)); @@ -112,8 +112,8 @@ TEST( GaussianFactorGraph, eliminateOne_l1 ) // create expected Conditional Gaussian double sig = sqrt(2.0)/10.; - Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; - Vector d = Vector2(-0.1, 0.25)/sig, sigma = ones(2); + Matrix I = I_2x2/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; + Vector d = Vector2(-0.1, 0.25)/sig, sigma = Vector::Ones(2); GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma); EXPECT(assert_equal(expected,*actual,tol)); @@ -129,8 +129,8 @@ TEST( GaussianFactorGraph, eliminateOne_x1_fast ) boost::tie(conditional,remaining) = fg.eliminateOne(ordering[X(1)], EliminateQR); // create expected Conditional Gaussian - Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; - Vector d = Vector2(-0.133333, -0.0222222), sigma = ones(2); + Matrix I = 15*I_2x2, R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; + Vector d = Vector2(-0.133333, -0.0222222), sigma = Vector::Ones(2); GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma); // Create expected remaining new factor @@ -159,8 +159,8 @@ TEST( GaussianFactorGraph, eliminateOne_x2_fast ) // create expected Conditional Gaussian double sig = 0.0894427; - Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; - Vector d = Vector2(0.2, -0.14)/sig, sigma = ones(2); + Matrix I = I_2x2/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; + Vector d = Vector2(0.2, -0.14)/sig, sigma = Vector::Ones(2); GaussianConditional expected(ordering[X(2)],d,R11,ordering[X(1)],S13,ordering[L(1)],S12,sigma); EXPECT(assert_equal(expected,*actual,tol)); @@ -175,8 +175,8 @@ TEST( GaussianFactorGraph, eliminateOne_l1_fast ) // create expected Conditional Gaussian double sig = sqrt(2.0)/10.; - Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; - Vector d = Vector2(-0.1, 0.25)/sig, sigma = ones(2); + Matrix I = I_2x2/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; + Vector d = Vector2(-0.1, 0.25)/sig, sigma = Vector::Ones(2); GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma); EXPECT(assert_equal(expected,*actual,tol)); @@ -186,7 +186,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1_fast ) TEST( GaussianFactorGraph, eliminateAll ) { // create expected Chordal bayes Net - Matrix I = eye(2); + Matrix I = I_2x2; Ordering ordering; ordering += X(2),L(1),X(1); @@ -195,11 +195,11 @@ TEST( GaussianFactorGraph, eliminateAll ) GaussianBayesNet expected = simpleGaussian(ordering[X(1)],d1,0.1); double sig1 = 0.149071; - Vector d2 = Vector2(0.0, 0.2)/sig1, sigma2 = ones(2); + Vector d2 = Vector2(0.0, 0.2)/sig1, sigma2 = Vector::Ones(2); push_front(expected,ordering[L(1)],d2, I/sig1,ordering[X(1)], (-1)*I/sig1,sigma2); double sig2 = 0.0894427; - Vector d3 = Vector2(0.2, -0.14)/sig2, sigma3 = ones(2); + Vector d3 = Vector2(0.2, -0.14)/sig2, sigma3 = Vector::Ones(2); push_front(expected,ordering[X(2)],d3, I/sig2,ordering[L(1)], (-0.2)*I/sig2, ordering[X(1)], (-0.8)*I/sig2, sigma3); // Check one ordering @@ -389,7 +389,7 @@ TEST( GaussianFactorGraph, elimination ) ord += X(1), X(2); // Create Gaussian Factor Graph GaussianFactorGraph fg; - Matrix Ap = eye(1), An = eye(1) * -1; + Matrix Ap = I_2x2, An = I_2x2 * -1; Vector b = (Vector(1) << 0.0).finished(); SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,2.0); fg += ord[X(1)], An, ord[X(2)], Ap, b, sigma; @@ -473,13 +473,13 @@ TEST(GaussianFactorGraph, replace) SharedDiagonal noise(noiseModel::Isotropic::Sigma(3, 1.0)); GaussianFactorGraph::sharedFactor f1(new JacobianFactor( - ord[X(1)], eye(3,3), ord[X(2)], eye(3,3), zero(3), noise)); + ord[X(1)], I_3x3, ord[X(2)], I_3x3, Z_3x1, noise)); GaussianFactorGraph::sharedFactor f2(new JacobianFactor( - ord[X(2)], eye(3,3), ord[X(3)], eye(3,3), zero(3), noise)); + ord[X(2)], I_3x3, ord[X(3)], I_3x3, Z_3x1, noise)); GaussianFactorGraph::sharedFactor f3(new JacobianFactor( - ord[X(3)], eye(3,3), ord[X(4)], eye(3,3), zero(3), noise)); + ord[X(3)], I_3x3, ord[X(4)], I_3x3, Z_3x1, noise)); GaussianFactorGraph::sharedFactor f4(new JacobianFactor( - ord[X(5)], eye(3,3), ord[X(6)], eye(3,3), zero(3), noise)); + ord[X(5)], I_3x3, ord[X(6)], I_3x3, Z_3x1, noise)); GaussianFactorGraph actual; actual.push_back(f1); @@ -588,7 +588,7 @@ TEST( GaussianFactorGraph, conditional_sigma_failure) { BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, actBT.nodes() | br::map_values) { GaussianConditional::shared_ptr conditional = clique->conditional(); //size_t dim = conditional->rows(); - //EXPECT(assert_equal(gtsam::ones(dim), conditional->get_model()->sigmas(), tol)); + //EXPECT(assert_equal(gtsam::Vector::Ones(dim), conditional->get_model()->sigmas(), tol)); EXPECT(!conditional->get_model()); } } diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 6c1fd7dd5..b95f16e76 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -112,7 +112,7 @@ TEST( Graph, composePoses ) TEST( GaussianFactorGraph, findMinimumSpanningTree ) { GaussianFactorGraph g; - Matrix I = eye(2); + Matrix I = I_2x2; Vector2 b(0, 0); const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5)); using namespace symbol_shorthand; diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp index 071b9d12d..e0c2b7b66 100644 --- a/tests/testIterative.cpp +++ b/tests/testIterative.cpp @@ -62,7 +62,7 @@ TEST( Iterative, conjugateGradientDescent ) // get matrices Matrix A; Vector b; - Vector x0 = gtsam::zero(6); + Vector x0 = Z_6x1; boost::tie(A, b) = fg.jacobian(); Vector expectedX = (Vector(6) << -0.1, 0.1, -0.1, -0.1, 0.1, -0.2).finished(); @@ -104,7 +104,7 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint ) VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters); VectorValues expected; - expected.insert(X(1), zero(3)); + expected.insert(X(1), Z_3x1); expected.insert(X(2), Vector3(-0.5,0.,0.)); CHECK(assert_equal(expected, actual)); } @@ -131,7 +131,7 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint ) VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters); VectorValues expected; - expected.insert(X(1), zero(3)); + expected.insert(X(1), Z_3x1); expected.insert(X(2), Vector3(-0.5,0.,0.)); CHECK(assert_equal(expected, actual)); } diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index 89b296824..65d26eb98 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -131,7 +131,7 @@ TEST(Manifold, DefaultChart) { EXPECT_DOUBLES_EQUAL(traits::Retract(0, v1), 1, 1e-9); // Dynamic does not work yet ! - Vector z = zero(2), v(2); + Vector z = Z_2x1, v(2); v << 1, 0; //DefaultChart chart4; // EXPECT(assert_equal(traits::Local(z, v), v)); @@ -146,7 +146,7 @@ TEST(Manifold, DefaultChart) { EXPECT(assert_equal(traits::Retract(I, v3), R)); // Check zero vector //DefaultChart chart6; - EXPECT(assert_equal(zero(3), traits::Local(R, R))); + EXPECT(assert_equal((Vector) Z_3x1, traits::Local(R, R))); } //****************************************************************************** diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 7142f72d5..86080b673 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -56,7 +56,7 @@ TEST ( NonlinearEquality, linearization ) { // check linearize SharedDiagonal constraintModel = noiseModel::Constrained::All(3); - JacobianFactor expLF(key, eye(3), zero(3), constraintModel); + JacobianFactor expLF(key, I_3x3, Z_3x1, constraintModel); GaussianFactor::shared_ptr actualLF = nle->linearize(linearize); EXPECT(assert_equal((const GaussianFactor&)expLF, *actualLF)); } @@ -133,7 +133,7 @@ TEST ( NonlinearEquality, error ) { // check error function outputs Vector actual = nle->unwhitenedError(feasible); - EXPECT(assert_equal(actual, zero(3))); + EXPECT(assert_equal(actual, Z_3x1)); actual = nle->unwhitenedError(bad_linearize); EXPECT( @@ -180,7 +180,7 @@ TEST ( NonlinearEquality, allow_error_pose ) { // check linearization GaussianFactor::shared_ptr actLinFactor = nle.linearize(config); - Matrix A1 = eye(3, 3); + Matrix A1 = I_3x3; Vector b = expVec; SharedDiagonal model = noiseModel::Constrained::All(3); GaussianFactor::shared_ptr expLinFactor( @@ -263,8 +263,8 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) { Values config1; config1.insert(key, pt); EXPECT(constraint.active(config1)); - EXPECT(assert_equal(zero(2), constraint.evaluateError(pt), tol)); - EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol)); + EXPECT(assert_equal(Z_2x1, constraint.evaluateError(pt), tol)); + EXPECT(assert_equal(Z_2x1, constraint.unwhitenedError(config1), tol)); EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol); Values config2; @@ -289,7 +289,7 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) { config1.insert(key, pt); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); GaussianFactor::shared_ptr expected1( - new JacobianFactor(key, eye(2, 2), zero(2), hard_model)); + new JacobianFactor(key, I_2x2, Z_2x1, hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); Values config2; @@ -297,7 +297,7 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) { config2.insert(key, ptBad); GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); GaussianFactor::shared_ptr expected2( - new JacobianFactor(key, eye(2, 2), Vector2(-1.0, 0.0), hard_model)); + new JacobianFactor(key, I_2x2, Vector2(-1.0, 0.0), hard_model)); EXPECT(assert_equal(*expected2, *actual2, tol)); } @@ -345,8 +345,8 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) { config1.insert(key1, x1); config1.insert(key2, x2); EXPECT(constraint.active(config1)); - EXPECT(assert_equal(zero(2), constraint.evaluateError(x1, x2), tol)); - EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol)); + EXPECT(assert_equal(Z_2x1, constraint.evaluateError(x1, x2), tol)); + EXPECT(assert_equal(Z_2x1, constraint.unwhitenedError(config1), tol)); EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol); Values config2; @@ -374,7 +374,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { config1.insert(key2, x2); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); GaussianFactor::shared_ptr expected1( - new JacobianFactor(key1, -eye(2, 2), key2, eye(2, 2), zero(2), + new JacobianFactor(key1, -I_2x2, key2, I_2x2, Z_2x1, hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); @@ -385,7 +385,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { config2.insert(key2, x2bad); GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); GaussianFactor::shared_ptr expected2( - new JacobianFactor(key1, -eye(2, 2), key2, eye(2, 2), Vector2(1.0, 1.0), + new JacobianFactor(key1, -I_2x2, key2, I_2x2, Vector2(1.0, 1.0), hard_model)); EXPECT(assert_equal(*expected2, *actual2, tol)); } diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 05cb9c4ad..fb85c3742 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -91,7 +91,7 @@ TEST( NonlinearFactor, NonlinearFactor ) // calculate the error_vector from the factor "f1" // error_vector = [0.1 0.1] Vector actual_e = boost::dynamic_pointer_cast(factor)->unwhitenedError(cfg); - CHECK(assert_equal(0.1*ones(2),actual_e)); + CHECK(assert_equal(0.1*Vector::Ones(2),actual_e)); // error = 0.5 * [1 1] * [1;1] = 1 double expected = 1.0; diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index f6cdd6ee5..7a22abc67 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -257,9 +257,9 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { expected.insert(2, Pose2(1, 1, M_PI)); VectorValues expectedGradient; - expectedGradient.insert(0,zero(3)); - expectedGradient.insert(1,zero(3)); - expectedGradient.insert(2,zero(3)); + expectedGradient.insert(0,Z_3x1); + expectedGradient.insert(1,Z_3x1); + expectedGradient.insert(2,Z_3x1); // Try LM and Dogleg LevenbergMarquardtParams params = LevenbergMarquardtParams::LegacyDefaults(); diff --git a/timing/timeMatrix.cpp b/timing/timeMatrix.cpp index d51b0abd2..a094f9628 100644 --- a/timing/timeMatrix.cpp +++ b/timing/timeMatrix.cpp @@ -46,7 +46,7 @@ double timeCollect(size_t p, size_t m, size_t n, bool passDims, size_t reps) { vector matrices; for (size_t i=0; i H1, boost::optional H2) { Rot2 hx = Rot2betweenOptimized(p1, p2); // h(x) - if (H1) *H1 = -eye(1); - if (H2) *H2 = eye(1); + if (H1) *H1 = -I_1x1; + if (H2) *H2 = I_1x1; // manifold equivalent of h(x)-z -> log(z,h(x)) return Rot2::Logmap(Rot2betweenOptimized(measured, hx)); } @@ -67,8 +67,8 @@ Vector Rot2BetweenFactorEvaluateErrorOptimizedBetweenNoeye(const Rot2& measured, { // TODO: Implement Rot2 hx = Rot2betweenOptimized(p1, p2); // h(x) - if (H1) *H1 = -Matrix::Identity(1,1); - if (H2) *H2 = Matrix::Identity(1,1); + if (H1) *H1 = -I_1x1; + if (H2) *H2 = I_1x1; // manifold equivalent of h(x)-z -> log(z,h(x)) return Rot2::Logmap(Rot2betweenOptimized(measured, hx)); } diff --git a/timing/timeSchurFactors.cpp b/timing/timeSchurFactors.cpp index 29e8e9916..9be55f831 100644 --- a/timing/timeSchurFactors.cpp +++ b/timing/timeSchurFactors.cpp @@ -109,7 +109,7 @@ void timeAll(size_t m, size_t N) { double* xdata = x.data(); // create a y - Vector y = zero(m * D); + Vector y = Vector::Zero(m * D); TIME(RawImplicit, implicitFactor, xdata, y.data()) TIME(RawJacobianQ, jf, xdata, y.data()) TIME(RawJacobianQR, jqr, xdata, y.data())