diff --git a/.cproject b/.cproject index cf5f88a63..061eab2d5 100644 --- a/.cproject +++ b/.cproject @@ -676,6 +676,30 @@ true true + + make + -j2 + tests/testPose2SLAM.run + true + true + true + + + make + -j2 + tests/testPose3SLAM.run + true + true + true + + + make + -j2 + tests/testPose2SLAMwSPCG.run + true + true + true + make -j2 @@ -2447,6 +2471,30 @@ true true + + make + -j2 + tests/testPose2SLAM.run + true + true + true + + + make + -j2 + tests/testPose3SLAM.run + true + true + true + + + make + -j2 + tests/testPose2SLAMwSPCG.run + true + true + true + make -j2 diff --git a/gtsam/base/DenseQR.cpp b/gtsam/base/DenseQR.cpp deleted file mode 100644 index 6cedee23c..000000000 --- a/gtsam/base/DenseQR.cpp +++ /dev/null @@ -1,228 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * DenseQR.cpp - * - * Created on: Oct 19, 2010 - * Author: nikai - * Description: Dense QR, inspired by Tim Davis's dense solver - */ - -#include -#include -#include -#include -#include - -#include "DenseQR.h" - -//#define DEBUG_MEMORY - -// all the lapack functions we need here -extern "C" { -void dlarft_ (char *direct, char *storev, int *n, int *k, double *V, int *ldv, double *Tau, double *T, int *ldt) ; -void dlarfb_ (char *side, char *trans, char *direct, char *storev, int *m, int *n, int *k, double *V, int *ldv, double *T, int *ldt, double *C, int *ldc, double *Work, int *ldwork) ; -void dlarfg_ (int *n, double *alpha, double *X, int *incx, double *tau) ; -void dlarf_ (char *side, int *m, int *n, double *V, int *incv, double *tau, double *C, int *ldc, double *Work) ; -} - -using namespace std; - -namespace gtsam { - - static int one = 1; - static char left = 'L' ; - static char direct = 'F'; - static char storev = 'C'; - static char trans = 'T'; - - /* ************************************************************************* */ - // check NaN in the input matrix - void CheckNaN(int m, int n, double *A, const char* msg) { - for(int i=0; i n) - throw std::invalid_argument("DenseQR: numPivotColumns < 0l || numPivotColumns > n"); - - double tau, Tau[n]; // the scalar in Householder - int row1stHH = 0, numGoodHHs = 0, numPendingHHs = 0; - int colPendingHHStart = 0, colPendingHHEnd = 0; - double *vectorHH = A; - int numZeros = 0; - int sizeBlock = m < 32 ? m : 32; - int stairStartLast = 0, stairStart = 0, minSizeBlock = max(4l, sizeBlock/4l); - int m1, n1; - - // go through all the columns - for (int col = 0; col < n; col++) { - // early termination because of non-full rank - if (numGoodHHs >= m) { - if (col < numPivotColumns) throw std::runtime_error("DenseQR: some columns were found to be linear independent!"); - return; - } - - // if there is a enough large staircase change, apply Householder - stairStartLast = stairStart; - stairStart = max(numGoodHHs + 1, stairs[col]); - stairs[col] = stairStart; - numZeros += numPendingHHs * (stairStart - stairStartLast); - if (numPendingHHs >= minSizeBlock && colPendingHHEnd < n && - numZeros > max(16, ((numPendingHHs * (numPendingHHs + 1)) / 2 + numPendingHHs * (stairStart - row1stHH - numPendingHHs)) / 2)) { -#ifdef DEBUG_MEMORY - if (row1stHH >= m) throw std::runtime_error("DenseQR: row1stHH >= m"); - if (colPendingHHEnd >= n) throw std::runtime_error("DenseQR: colPendingHHEnd >= n"); -#endif - dlarftb_wrap(stairStartLast - row1stHH, n - colPendingHHEnd, numPendingHHs, m, m, - vectorHH, Tau + colPendingHHStart, &A[row1stHH+colPendingHHEnd*m], workspace, &numPendingHHs, &numZeros); - } - - // compute Householder for the current column - int n_ = stairStart - numGoodHHs; - double *X = A + numGoodHHs + col*m; - dlarfg_(&n_, X, X + 1, &one, &tau); - Tau[col] = tau; - if (!numPendingHHs) { - row1stHH = numGoodHHs; - vectorHH = A + row1stHH + col*m; - colPendingHHStart = col; -#ifdef DEBUG_MEMORY - if (row1stHH+col*m >= m*n) throw std::runtime_error("DenseQR: row1stHH+col*m >= m*n"); -#endif - colPendingHHEnd = NotEnoughLeft(m - row1stHH, n - col, sizeBlock) ? n : min(n, col + sizeBlock); // if not enough work left, go to unblocked mode - } - numPendingHHs++; - - // apply Householder reflector - m1 = stairStart - numGoodHHs; - n1 = colPendingHHEnd - col - 1; - if (m1 > 0 && n1 > 0) { - double *A1 = &A[numGoodHHs+col*m], *C1 = A1 + m, v0 = *A1; - *A1 = 1 ; - dlarf_ (&left, &m1, &n1, A1, &one, &tau, C1, &m, workspace) ; - *A1 = v0; - } - numGoodHHs++; - - if ((numGoodHHs == m || col + 1 == colPendingHHEnd) && colPendingHHEnd < n) { -#ifdef DEBUG_MEMORY - if (row1stHH >= m) throw std::runtime_error("DenseQR: row1stHH >= m"); - if (colPendingHHEnd >= n) throw std::runtime_error("DenseQR: colPendingHHEnd >= n"); -#endif - dlarftb_wrap(stairStart - row1stHH, n - colPendingHHEnd, numPendingHHs, m, m, - vectorHH, Tau + colPendingHHStart, &A[row1stHH+colPendingHHEnd*m], workspace, &numPendingHHs, &numZeros); - } - } - - // CheckNaN(m, n, A, "DenseQR: the output matrix has NaN"); - // CheckInf(m, n, A, "DenseQR: the input matrix has Inf"); - - } -} // namespace gtsam diff --git a/gtsam/base/DenseQR.h b/gtsam/base/DenseQR.h deleted file mode 100644 index c8b696471..000000000 --- a/gtsam/base/DenseQR.h +++ /dev/null @@ -1,25 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * DenseQR.h - * - * Created on: Oct 19, 2010 - * Author: nikai - * Description: Dense QR, inspired by Tim Davis's dense solver - */ - -#pragma once - -namespace gtsam { - void DenseQR( int m, int n, int numPivotColumns, // inputs - double *F, int *Stair, double *W); // outputs -} diff --git a/gtsam/base/DenseQRUtil.cpp b/gtsam/base/DenseQRUtil.cpp deleted file mode 100644 index e10167a67..000000000 --- a/gtsam/base/DenseQRUtil.cpp +++ /dev/null @@ -1,164 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * DenseQRUtil.cpp - * - * Created on: Jul 1, 2010 - * Author: nikai - * Description: the utility functions for DenseQR - */ - -#include -#include - -#include // for memcpy and memset - -using namespace std; -//namespace ublas = boost::numeric::ublas; - -//#ifdef GT_USE_LAPACK -//namespace gtsam { -// -// /* ************************************************************************* */ -// int* MakeStairs(Matrix& A) { -// -// const int m = A.rows(); -// const int n = A.cols(); -// int* Stair = new int[n]; -// -// // record the starting pointer of each row -// double* a[m]; -// list remained; -// a[0] = A.data(); -// for(int i=0; i sorted; -// list::iterator itRemained; -// for(j = 0; j < n; ) { -// // remove the non-zero rows in the current column -// for(itRemained = remained.begin(); itRemained!=remained.end(); ) { -// if (*(a[*itRemained]) != 0) { -// sorted.push_back(*itRemained); -// itRemained = remained.erase(itRemained); -// } else { -// a[*itRemained]++; -// itRemained ++; -// } -// } -// -// // record the stair number -// Stair[j++] = m - remained.size(); -// -// if(remained.empty()) break; -// } -// -// // all the remained columns have maximum stair -// for (; j::const_iterator itSorted; -// double* ptr1 = A.data(); -// double* ptr2 = A_new.data(); -// int row = 0, sizeOfRow = n * sizeof(double); -// for(itSorted=sorted.begin(); itSorted!=sorted.end(); itSorted++, row++) -// memcpy(ptr2+offset[row], ptr1+offset[*itSorted], sizeOfRow); -// -// A = A_new; -// -// return Stair; -// } -// -// void printColumnMajor(const double* a, const int m, const int n) { -// for(int i=0; icol"); -// // convert from row major to column major -// MatrixColMajor Acolwise(A); -// double *a = Acolwise.data(); -// toc("householder_denseqr: row->col"); -// -// tic("householder_denseqr: denseqr_front"); -// int npiv = min(m,n); -// int b = min(min(m,n),32); -// double W[b*(n+b)]; -// DenseQR(m, n, npiv, a, Stair, W); -// toc("householder_denseqr: denseqr_front"); -// -// tic("householder_denseqr: col->row"); -// int k0 = 0; -// int j0; -// int k; -// memset(A.data(), 0, m*n*sizeof(double)); -// for(int j=0; jrow"); -// -// -// if(allocedStair) delete[] Stair; -// -// toc("householder_denseqr"); -// } -// -// void householder_denseqr_colmajor(MatrixColMajor& A, int *Stair) { -// int m = A.rows(); -// int n = A.cols(); -// -// assert(Stair != NULL); -// -// tic(1, "householder_denseqr"); -// int npiv = min(m,n); -// int b = min(min(m,n),32); -// double W[b*(n+b)]; -// DenseQR(m, n, npiv, A.data(), Stair, W); -// toc(1, "householder_denseqr"); -// } -// -//} // namespace gtsam -//#endif diff --git a/gtsam/base/DenseQRUtil.h b/gtsam/base/DenseQRUtil.h deleted file mode 100644 index 86d946b12..000000000 --- a/gtsam/base/DenseQRUtil.h +++ /dev/null @@ -1,37 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * DenseQRUtil.h - * - * Created on: Jul 1, 2010 - * Author: nikai - * Description: the utility functions for DenseQR - */ - -#pragma once - -#include - -//#ifdef GT_USE_LAPACK -//#include -// -//namespace gtsam { -// -// /** make stairs and speed up householder_denseqr. Stair is defined as the row index of where zero entries start in each column */ -// int* MakeStairs(Matrix &A); -// -// /** Householder tranformation, zeros below diagonal */ -// void householder_denseqr(Matrix &A, int* Stair = NULL); -// -// void householder_denseqr_colmajor(MatrixColMajor& A, int *Stair); -//} -//#endif diff --git a/gtsam/base/Makefile.am b/gtsam/base/Makefile.am index a17ea532f..62c158476 100644 --- a/gtsam/base/Makefile.am +++ b/gtsam/base/Makefile.am @@ -20,9 +20,6 @@ check_PROGRAMS += tests/testFixedVector tests/testVector tests/testMatrix tests/ check_PROGRAMS += tests/testCholesky check_PROGRAMS += tests/testNumericalDerivative -#sources += DenseQR.cpp DenseQRUtil.cpp -#check_PROGRAMS += tests/testDenseQRUtil - # Testing headers += Testable.h TestableAssertions.h numericalDerivative.h sources += timing.cpp debug.cpp diff --git a/gtsam/base/tests/testDenseQRUtil.cpp b/gtsam/base/tests/testDenseQRUtil.cpp deleted file mode 100644 index 62ae6a7d7..000000000 --- a/gtsam/base/tests/testDenseQRUtil.cpp +++ /dev/null @@ -1,207 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testSPQRUtil.cpp - * @brief Unit test for SPQR utility functions - * @author Kai Ni - **/ - -#include -#include -#include - -using namespace std; -using namespace gtsam; - -#ifdef GT_USE_LAPACK -/* ************************************************************************* */ -TEST(SPQRUtil, MakeStair) -{ - double data[] = { -5, 0, 5, 0, 0, 0, -1, - 00,-5, 0, 5, 0, 0, 1.5, - 10, 0, 0, 0,-10,0, 2, - 00, 10,0, 0, 0, -10, -1 }; - Matrix A = Matrix_(4, 7, data); - - int* Stair = MakeStairs(A); - - double data2[] = { -5, 0, 5, 0, 0, 0, -1, - 10, 0, 0, 0,-10,0, 2, - 00,-5, 0, 5, 0, 0, 1.5, - 00, 10,0, 0, 0, -10, -1 }; - Matrix A_expected = Matrix_(4, 7, data2); - CHECK(assert_equal(A_expected, A, 1e-10)); - - int Stair_expected[] = {2, 4, 4, 4, 4, 4, 4}; - for (int i=0; i<7; i++) - DOUBLES_EQUAL(Stair_expected[i], Stair[i], 1e-7); - delete []Stair; -} - -/* ************************************************************************* */ -TEST(SPQRUtil, MakeStair2) -{ - double data[] = { 0.1, 0, 0, 0, - 0, 0.3, 0, 0, - 0, 0, 0.3, 0, - 1.6,-0.2, -2.5, 0.2, - 0, 1.6, 0.7, 0.1, - 0, 0, -7.8, 0.7 }; - Matrix A = Matrix_(6, 4, data); - - int* Stair = MakeStairs(A); - - double data2[] = { 0.1, 0, 0, 0, - 1.6,-0.2, -2.5, 0.2, - 0, 0.3, 0, 0, - 0, 1.6, 0.7, 0.1, - 0, 0, 0.3, 0, - 0, 0, -7.8, 0.7 - }; - Matrix A_expected = Matrix_(6, 4, data2); - CHECK(assert_equal(A_expected, A, 1e-10)); - - int Stair_expected[] = {2, 4, 6, 6}; - for (int i=0; i<4; i++) - DOUBLES_EQUAL(Stair_expected[i], Stair[i], 1e-7); - delete []Stair; -} - -/* ************************************************************************* */ -TEST(SPQRUtil, houseHolder_denseqr) -{ - double data[] = { -5, 0, 5, 0, 0, 0, -1, - 00,-5, 0, 5, 0, 0, 1.5, - 10, 0, 0, 0,-10,0, 2, - 00, 10,0, 0, 0, -10, -1 }; - - // check in-place householder, with v vectors below diagonal - double data1[] = { 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, - 0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, - 0, 0, 4.4721, 0, -4.4721, 0, 0, - 0, 0, 0, 4.4721, 0, -4.4721, 0.894 }; - Matrix expected1 = Matrix_(4, 7, data1); - Matrix A1 = Matrix_(4, 7, data); - householder_denseqr(A1); - CHECK(assert_equal(expected1, A1, 1e-3)); -} - -/* ************************************************************************* */ -TEST(SPQRUtil, houseHolder_denseqr2) -{ - double data[] = { -5, 0, 5, 0, 0, 0, -1, - 00,-5, 0, 5, 0, 0, 1.5, - 10, 0, 0, 0,-10,0, 2, - 00, 10,0, 0, 0, -10, -1 }; - - // check in-place householder, with v vectors below diagonal - double data1[] = { 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, - 0, -11.1803, 0, 2.2361, 0, 8.9443, 1.565, - 0, 0, -4.4721, 0, 4.4721, 0, 0, - 0, 0, 0, 4.4721, 0, -4.4721, 0.894 }; - Matrix expected1 = Matrix_(4, 7, data1); - Matrix A1 = Matrix_(4, 7, data); - int* Stair = MakeStairs(A1); - householder_denseqr(A1, Stair); - delete[] Stair; - CHECK(assert_equal(expected1, A1, 1e-3)); -} - -/* ************************************************************************* */ -TEST(SPQRUtil, houseHolder_denseqr3) -{ - double data[] = { 1, 1, 9, - 1, 0, 5}; - - // check in-place householder, with v vectors below diagonal - double data1[] = {-sqrt(2), -1/sqrt(2), -7*sqrt(2), - 0, -1/sqrt(2), -4/sqrt(2)}; - Matrix expected1 = Matrix_(2, 3, data1); - Matrix A1 = Matrix_(2, 3, data); - householder_denseqr(A1); - CHECK(assert_equal(expected1, A1, 1e-3)); -} -/* ************************************************************************* */ -TEST(SPQRUtil, houseHolder_denseqr4) -{ - Matrix A = Matrix_(15, 34, - -5.48351, 23.2337, -45.2073, 6.33455,-0.342553,-0.897005, 7.91126, 3.20237, -2.49219, -2.44189,-0.977376, -1.61127, -3.68421,-1.28059, -2.83303, 2.45949, 0.218835, -0.71239,-0.169314,-0.131355, 2.04233, 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.0782689, - -15.8515, 10.2731, 23.2069, 0.0, 2.0891, 9.02822, 2.18705, 6.1083, -10.5157,-0.690031,-0.638592, -2.47301, -1.16601,-1.35043, -5.39516, 0.69744, -1.22492, -1.86158, -5.12896, -7.27133, -18.7928, 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.360876, - 13.5817, 17.9017, 50.0056, 0.0, 0.0, -1.10618, 6.61197, -6.7864, 8.87588, -2.01464, 1.49684, 3.39016, -2.92526, 2.16326, 6.17234, 1.98881, 0.309325, 1.86023, -4.94073, 2.7695, 7.85143, 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.0619088, - 12.797, 4.79759, -15.866, -1.94292, 0.436084, 1.43799, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -8.05369,-0.492175, -7.84104, 1.53703,-0.807928, 0.144945,-0.341446, -2.06456, 2.259, 0.101199, 0.161626,-0.184581, -11.7786, -1.58626, 4.21069,-0.179109, - -9.32565, 6.08188, 4.92746, 0.0,-0.0726655, -0.280519, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.5653, 10.1011, 19.53,-0.948225, -16.6125, -4.1864, 4.82523, 5.36202, 12.5239, 0.410163, 0.493983,-0.122754, -1.62954, -5.42323, 24.9016, 0.868911, - -8.2164, -7.81388, -9.60582, 0.0, 0.0,-0.0461159, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -16.7678, 5.91287, 12.5045, 6.54954, -15.7228, -10.519, -7.66513, 0.958071,0.462615, 0.744471,-0.334807, -1.22164, -3.33139, 9.43355, -14.4208,-0.831651, - -1.1057, 1.99291, 3.61474, 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.0, 0.0, 6.56605, 4.08432, 7.3415, -4.07027, -25.9866, -19.3138, -2.7792, -3.83993,-3.53144, 0.603377,-0.588407,-0.296625, -17.2456, -9.02528, -51.079, -1.49078, - 0.0, 1.5495, 2.63487, 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.0, 0.0, 7.63944, 4.62248, 4.4997, -29.3868, -6.10506, 20.9431, 10.654, -11.8748,-0.904113, -1.14369, 2.23985, 3.24988, 10.8288, 32.2749, 0.665805,-0.840659, - 0.0, 0.0,-0.576351, 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.0, 0.0, -5.60377, 5.9181, 5.34736, 18.6972, -1.10715, 39.0521, -5.25853, -14.9718, 4.29131, 0.480123, 3.42935, -1.58455, 24.3192, -17.98, 4.7336, 0.939854, - 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.0, 0.0, 0.0, 0.0, 0.0, -6.19444,-0.142454, 0.586582, 29.6886, -4.73646, -5.11474, 39.6994, -1.12835,-3.69964, -3.04975,0.0965116, 8.58342, -3.79485, 19.0323, -5.69059, -1.11543, - 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.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.44195, -4.96618, -1.12282, -1.01802, -46.1653, 0.864773, -18.0404, 24.8898, 1.64442, 5.72634,-0.948517, 17.2222, 0.916729, -2.30198, 1.17404, - 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.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.33992, 1.16655, -18.9535, -24.3327, -8.3228, -25.6997,-42.6673, -3.59911,0.0388951, 1.07185, 7.14524, -5.94804, 28.0376,-0.364333, - 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.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.06503, -4.87522, 3.87236, -11.3562, 3.23001, 33.5579, -13.8812, 7.18718, 1.71541,-0.495603,-0.826235, -6.04699, -1.9388, - 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.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.60639, -2.2334, 9.04169, 13.1342,-7.14457, 5.82756, 14.771, -49.7693, -4.22287, 2.58753, 1.22899,-0.752895, - 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.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.86199, 8.53755, -19.6873, 20.8838, 6.09985, -12.3691, -28.4341, 7.05672, 3.02888, 0.594889,-0.214789); - - Matrix expectedR = Matrix_(15, 34, - 28.0225, 0.698206, 13.7437, -2.12682,-0.891379, -4.70403, 0.419395, -7.37109, 10.738,-0.108275, 1.27796, 3.35731,-0.037285, 2.06295, 6.5978,0.0881205, 0.799999, 2.09404, 0.0, 0.0, 0.0,-0.742297, 10.7949, 5.30571, 0.59541, -2.85665,-3.13251, -0.332376,0.0308008, 0.326458, -3.17932, -1.32946,-0.120428, 0.181167, - 0.0, 33.0569, 3.44432, 4.21511, 0.477218, 1.84355, 9.81181, 0.629595, -0.43972, -3.01942,-0.101787,-0.135997, -4.53513,-0.191803, -0.46459, 3.02053,-0.0762487, -0.116055, 0.0, 0.0, 0.0, -3.10672, -1.53799, 1.44251, 2.96932,-0.267359, 2.33355, -0.0960386, 0.262347, 0.366951, -1.68673, -2.46105, 5.55563,0.0637128, - 0.0, 0.0, -72.5995, 3.31723, -0.92697, -3.15842, 0.217843, 3.35038, -2.29212,-0.0760686, -1.19838, -1.9188,-0.128748,-1.47433, -3.06396,0.0986865, 0.462591,-0.738925, 0.0, 0.0, 0.0, 2.39657, 2.3479, 0.508439, -1.45276,-0.738523,-0.534709, 0.04058,-0.0489968, -0.230194, -2.92748,0.0364329, 0.119466,-0.0346618, - 0.0, 0.0, 0.0, 3.25682, -1.182, -4.84344, 2.74062, -2.81233, 5.06869,-0.834871, 0.28589, 1.18891, -1.18948,0.606334, 2.52042, 0.831464, 0.575576, 0.884713, 4.47525,0.0381282, 8.65006, 0.178133, 7.13055, 0.99353, -1.7708, 0.464406,-5.86884, -0.194461,-0.365941,0.0828452, 10.1153, 3.22621, -9.90261,0.0257706, - 0.0, 0.0, 0.0, 0.0, 1.18256, 5.14989, 0.838654, 3.86831, -6.31407,-0.268897,-0.494264, -1.63226,-0.480456,-0.931946, -3.4322, 0.275576,-0.655629, -1.15196, -7.78905, -13.5715, -29.2364, 3.37589, 18.448, 5.11948, -4.08059, -3.2509,-9.52461, -0.362224,-0.457579,-0.0601673, 1.85657, 2.99257, -12.1144,-0.624855, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.450726, -2.52457, 2.18784, -3.04253, 0.770326,-0.443888, -1.07379, 1.12148,-0.662977, -1.98947,-0.762824,-0.151537,-0.615483, 19.9937, -5.17055, -11.2101, -10.0805, 10.6168, 9.36274, 8.17588, -1.78258,-0.858858, -0.75124, 0.443364, 1.48335, 4.46589, -5.72814, 8.27179, 0.551859, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,0.0812029,-0.0832869, 0.0674935,-0.0247479, 0.0283366,0.0480445,-0.036613,0.0351956, 0.0768672,0.0242134,-0.00953749,0.0194382, -5.93603, -5.0025, -6.38014, 18.5158, 22.668, 1.61251, -1.86948, 11.5217, 5.39137, 0.160562,-0.866767, -1.46548, 6.35692, -13.7392, 45.5091, 1.89557, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.0237536, 0.0206287,6.48832e-05,0.00746251, 0.0129591,0.000272306,0.00955241, 0.0216226,-0.000202751, -0.00189634,0.00560094, -5.77613, 5.44125, 4.94628, 21.3185,-0.976758, 36.3015, -6.24453, -13.7772, 4.2347, 0.597408, 3.16863, -1.89053, 22.7518, -21.5891, 3.36502, 0.993638, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.00807513,-1.07967e-06, 0.00194212, 0.00125444, -0.000133756, 0.00168498, 0.000217439,-4.13036e-05, -0.00259827,-0.000661005, 1.19994, 2.30552, 0.746869, -18.6973, 9.7233, 31.6093, 9.52016, -8.27898, 2.32924, -1.18233, 2.47028, 2.54466, 21.2909, 29.1971, 32.4215, 0.342241, - 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.0, 0.0, 0.0, 0.0, 0.0, -6.19444,-0.142454, 0.586582, 29.6886, -4.73646, -5.11474, 39.6994, -1.12835,-3.69964, -3.04975,0.0965116, 8.58342, -3.79485, 19.0323, -5.69059, -1.11543, - 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.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.44195, -4.96618, -1.12282, -1.01802, -46.1653, 0.864773, -18.0404, 24.8898, 1.64442, 5.72634,-0.948517, 17.2222, 0.916729, -2.30198, 1.17404, - 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.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.33992, 1.16655, -18.9535, -24.3327, -8.3228, -25.6997,-42.6673, -3.59911,0.0388951, 1.07185, 7.14524, -5.94804, 28.0376,-0.364333, - 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.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.06503, -4.87522, 3.87236, -11.3562, 3.23001, 33.5579, -13.8812, 7.18718, 1.71541,-0.495603,-0.826235, -6.04699, -1.9388, - 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.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.60639, -2.2334, 9.04169, 13.1342,-7.14457, 5.82756, 14.771, -49.7693, -4.22287, 2.58753, 1.22899,-0.752895, - 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.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.86199, 8.53755, -19.6873, 20.8838, 6.09985, -12.3691, -28.4341, 7.05672, 3.02888, 0.594889,-0.214789); - - Matrix expectedA = Matrix_(15, 34, - -5.48351, 23.2337, -45.2073, 6.33455,-0.342553,-0.897005, 7.91126, 3.20237, -2.49219, -2.44189,-0.977376, -1.61127, -3.68421,-1.28059, -2.83303, 2.45949, 0.218835, -0.71239,-0.169314,-0.131355, 2.04233, 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.0782689, - -15.8515, 10.2731, 23.2069, 0.0, 2.0891, 9.02822, 2.18705, 6.1083, -10.5157,-0.690031,-0.638592, -2.47301, -1.16601,-1.35043, -5.39516, 0.69744, -1.22492, -1.86158, -5.12896, -7.27133, -18.7928, 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.360876, - 13.5817, 17.9017, 50.0056, 0.0, 0.0, -1.10618, 6.61197, -6.7864, 8.87588, -2.01464, 1.49684, 3.39016, -2.92526, 2.16326, 6.17234, 1.98881, 0.309325, 1.86023, -4.94073, 2.7695, 7.85143, 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.0619088, - 12.797, 4.79759, -15.866, -1.94292, 0.436084, 1.43799, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -8.05369,-0.492175, -7.84104, 1.53703,-0.807928, 0.144945,-0.341446, -2.06456, 2.259, 0.101199, 0.161626,-0.184581, -11.7786, -1.58626, 4.21069,-0.179109, - -9.32565, 6.08188, 4.92746, 0.0,-0.0726655, -0.280519, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.5653, 10.1011, 19.53,-0.948225, -16.6125, -4.1864, 4.82523, 5.36202, 12.5239, 0.410163, 0.493983,-0.122754, -1.62954, -5.42323, 24.9016, 0.868911, - -8.2164, -7.81388, -9.60582, 0.0, 0.0,-0.0461159, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -16.7678, 5.91287, 12.5045, 6.54954, -15.7228, -10.519, -7.66513, 0.958071,0.462615, 0.744471,-0.334807, -1.22164, -3.33139, 9.43355, -14.4208,-0.831651, - -1.1057, 1.99291, 3.61474, 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.0, 0.0, 6.56605, 4.08432, 7.3415, -4.07027, -25.9866, -19.3138, -2.7792, -3.83993,-3.53144, 0.603377,-0.588407,-0.296625, -17.2456, -9.02528, -51.079, -1.49078, - 0.0, 1.5495, 2.63487, 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.0, 0.0, 7.63944, 4.62248, 4.4997, -29.3868, -6.10506, 20.9431, 10.654, -11.8748,-0.904113, -1.14369, 2.23985, 3.24988, 10.8288, 32.2749, 0.665805,-0.840659, - 0.0, 0.0,-0.576351, 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.0, 0.0, -5.60377, 5.9181, 5.34736, 18.6972, -1.10715, 39.0521, -5.25853, -14.9718, 4.29131, 0.480123, 3.42935, -1.58455, 24.3192, -17.98, 4.7336, 0.939854, - 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.0, 0.0, 0.0, 0.0, 0.0, -6.19444,-0.142454, 0.586582, 29.6886, -4.73646, -5.11474, 39.6994, -1.12835,-3.69964, -3.04975,0.0965116, 8.58342, -3.79485, 19.0323, -5.69059, -1.11543, - 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.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.44195, -4.96618, -1.12282, -1.01802, -46.1653, 0.864773, -18.0404, 24.8898, 1.64442, 5.72634,-0.948517, 17.2222, 0.916729, -2.30198, 1.17404, - 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.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.33992, 1.16655, -18.9535, -24.3327, -8.3228, -25.6997,-42.6673, -3.59911,0.0388951, 1.07185, 7.14524, -5.94804, 28.0376,-0.364333, - 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.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.06503, -4.87522, 3.87236, -11.3562, 3.23001, 33.5579, -13.8812, 7.18718, 1.71541,-0.495603,-0.826235, -6.04699, -1.9388, - 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.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -5.60639, -2.2334, 9.04169, 13.1342,-7.14457, 5.82756, 14.771, -49.7693, -4.22287, 2.58753, 1.22899,-0.752895, - 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.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.86199, 8.53755, -19.6873, 20.8838, 6.09985, -12.3691, -28.4341, 7.05672, 3.02888, 0.594889,-0.214789); - - Matrix actualR = A; - householder_denseqr(actualR); - - int* Stair = MakeStairs(A); - CHECK(assert_equal(expectedA, A)); - - Matrix actualRstair = A; - householder_denseqr(actualRstair, Stair); - delete[] Stair; - - CHECK(assert_equal(expectedR, actualR, 1e-3)); - CHECK(assert_equal(expectedR, actualRstair, 1e-3)); -} -#endif - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/inference/tests/testBinaryBayesNet.cpp b/gtsam/inference/tests/testBinaryBayesNet.cpp deleted file mode 100644 index 80c35ed30..000000000 --- a/gtsam/inference/tests/testBinaryBayesNet.cpp +++ /dev/null @@ -1,86 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testBinaryBayesNet.cpp - * @brief Unit tests for BinaryBayesNet - * @author Manohar Paluri - */ - -// STL/C++ -#include -#include -#include -#include -#include -#include - -#include // for operator += -using namespace boost::assign; - -#define GTSAM_MAGIC_KEY - -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -/** A Bayes net made from binary conditional probability tables */ -typedef BayesNet BinaryBayesNet; - - -double probability( BinaryBayesNet & bbn, SymbolMap & config) -{ - double result = 1.0; - BinaryBayesNet::const_iterator it = bbn.begin(); - while( it != bbn.end() ){ - result *= (*it)->probability(config); - it++; - } - return result; -} - -/************************************************************************** */ -TEST( BinaryBayesNet, constructor ) -{ - // small Bayes Net x <- y - // p(y) = 0.2 - // p(x|y=0) = 0.3 - // p(x|y=1) = 0.6 - - SymbolMap config; - config["y"] = false; - config["x"] = false; - // unary conditional for y - boost::shared_ptr py(new BinaryConditional("y",0.2)); - DOUBLES_EQUAL(0.8,py->probability(config),0.01); - - // single parent conditional for x - vector cpt; - cpt += 0.3, 0.6 ; // array index corresponds to binary parent values structure - boost::shared_ptr px_y(new BinaryConditional("x","y",cpt)); - DOUBLES_EQUAL(0.7,px_y->probability(config),0.01); - - // push back conditionals in topological sort order (parents last) - BinaryBayesNet bbn; - bbn.push_back(py); - bbn.push_back(px_y); - - // Test probability of 00,01,10,11 - DOUBLES_EQUAL(0.56,probability(bbn,config),0.01); // P(y=0)P(x=0|y=0) = 0.8 * 0.7 = 0.56; -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} -/* ************************************************************************* */ diff --git a/gtsam/linear/BayesNetPreconditioner.cpp b/gtsam/linear/BayesNetPreconditioner.cpp deleted file mode 100644 index 6b6d638d9..000000000 --- a/gtsam/linear/BayesNetPreconditioner.cpp +++ /dev/null @@ -1,91 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * BayesNetPreconditioner.cpp - * Created on: Dec 31, 2009 - * @Author: Frank Dellaert - */ - -#include -#include - -namespace gtsam { - - /* ************************************************************************* */ - BayesNetPreconditioner::BayesNetPreconditioner(const GaussianFactorGraph& Ab, - const GaussianBayesNet& Rd) : - Ab_(Ab), Rd_(Rd) { - } - - /* ************************************************************************* */ - // R*x = y by solving x=inv(R)*y - VectorValues BayesNetPreconditioner::backSubstitute(const VectorValues& y) const { - return gtsam::backSubstitute(Rd_, y); - } - - /* ************************************************************************* */ - // gy=inv(L)*gx by solving L*gy=gx. - VectorValues BayesNetPreconditioner::backSubstituteTranspose( - const VectorValues& gx) const { - return gtsam::backSubstituteTranspose(Rd_, gx); - } - - /* ************************************************************************* */ - double BayesNetPreconditioner::error(const VectorValues& y) const { - return Ab_.error(x(y)); - } - - /* ************************************************************************* */ - // gradient is inv(R')*A'*(A*inv(R)*y-b), - VectorValues BayesNetPreconditioner::gradient(const VectorValues& y) const { - VectorValues gx = VectorValues::zero(y); - Errors e = Ab_.errors(x(y)); - Ab_.transposeMultiplyAdd(1.0,e,gx); - return gtsam::backSubstituteTranspose(Rd_, gx); - } - - /* ************************************************************************* */ - // Apply operator * - Errors BayesNetPreconditioner::operator*(const VectorValues& y) const { - return Ab_ * x(y); - } - - /* ************************************************************************* */ - // In-place version that overwrites e - // TODO: version that takes scratch space for x - void BayesNetPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const { - VectorValues x = y; - backSubstituteInPlace(Rd_,x); - Ab_.multiplyInPlace(x,e); - } - - /* ************************************************************************* */ - // Apply operator inv(R')*A'*e - VectorValues BayesNetPreconditioner::operator^(const Errors& e) const { - VectorValues x = Ab_ ^ e; // x = A'*e2 - return gtsam::backSubstituteTranspose(Rd_, x); - } - - /* ************************************************************************* */ - // y += alpha*inv(R')*A'*e - void BayesNetPreconditioner::transposeMultiplyAdd(double alpha, - const Errors& e, VectorValues& y) const { - VectorValues x = VectorValues::zero(y); - Ab_.transposeMultiplyAdd(1.0,e,x); // x += A'*e - axpy(alpha, gtsam::backSubstituteTranspose(Rd_, x), y); // y += alpha*inv(R')*x - } - - /* ************************************************************************* */ - -} // namespace gtsam - - diff --git a/gtsam/linear/BayesNetPreconditioner.h b/gtsam/linear/BayesNetPreconditioner.h deleted file mode 100644 index 904eedc3f..000000000 --- a/gtsam/linear/BayesNetPreconditioner.h +++ /dev/null @@ -1,79 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * BayesNetPreconditioner.h - * Created on: Dec 31, 2009 - * @Author: Frank Dellaert - */ - -#ifndef BAYESNETPRECONDITIONER_H_ -#define BAYESNETPRECONDITIONER_H_ - -#include -#include - -namespace gtsam { - - /** - * Upper-triangular preconditioner R for the system |A*x-b|^2 - * The new system will be |A*inv(R)*y-b|^2, i.e., R*x=y - * This class can solve for x=inv(R)*y by back-substituting R*x=y - * and also apply the chain rule gy=inv(R')*gx by solving R'*gy=gx. - * This is not used currently, just to debug operators below - */ - class BayesNetPreconditioner { - - // The original system - const GaussianFactorGraph& Ab_; - - // The preconditioner - const GaussianBayesNet& Rd_; - - public: - - /** Constructor */ - BayesNetPreconditioner(const GaussianFactorGraph& Ab, - const GaussianBayesNet& Rd); - - // R*x = y by solving x=inv(R)*y - VectorValues backSubstitute(const VectorValues& y) const; - - // gy=inv(L)*gx by solving L*gy=gx. - VectorValues backSubstituteTranspose(const VectorValues& gx) const; - - /* x = inv(R)*y */ - inline VectorValues x(const VectorValues& y) const { - return backSubstitute(y); - } - - /* error, given y */ - double error(const VectorValues& y) const; - - /** gradient */ - VectorValues gradient(const VectorValues& y) const; - - /** Apply operator A */ - Errors operator*(const VectorValues& y) const; - - /** In-place version that overwrites e */ - void multiplyInPlace(const VectorValues& y, Errors& e) const; - - /** Apply operator A' */ - VectorValues operator^(const Errors& e) const; - - /** BLAS level 2 equivalent y += alpha*inv(R')*A'*e */ - void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& y) const; - }; - -} // namespace gtsam - -#endif /* BAYESNETPRECONDITIONER_H_ */ diff --git a/gtsam/linear/Makefile.am b/gtsam/linear/Makefile.am index 857641c81..1363d148b 100644 --- a/gtsam/linear/Makefile.am +++ b/gtsam/linear/Makefile.am @@ -18,8 +18,6 @@ check_PROGRAMS += tests/testNoiseModel tests/testErrors tests/testSampler # Vector Configurations headers += VectorValues.h check_PROGRAMS += tests/testVectorValues -#sources += VectorMap.cpp VectorBTree.cpp -#check_PROGRAMS += tests/testVectorMap tests/testVectorBTree # Solvers sources += GaussianSequentialSolver.cpp GaussianMultifrontalSolver.cpp @@ -38,12 +36,9 @@ headers += iterative-inl.h sources += iterative.cpp SubgraphPreconditioner.cpp headers += IterativeSolver.h IterativeOptimizationParameters.h headers += SubgraphSolver.h SubgraphSolver-inl.h -#sources += BayesNetPreconditioner.cpp -#check_PROGRAMS += tests/testBayesNetPreconditioner # Timing tests noinst_PROGRAMS = tests/timeGaussianFactor tests/timeFactorOverhead tests/timeSLAMlike -#noinst_PROGRAMS += tests/timeVectorValues #---------------------------------------------------------------------------------------------------- # Create a libtool library that is not installed diff --git a/gtsam/linear/VectorBTree.cpp b/gtsam/linear/VectorBTree.cpp deleted file mode 100644 index f34e6ae5a..000000000 --- a/gtsam/linear/VectorBTree.cpp +++ /dev/null @@ -1,219 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file VectorValues.cpp - * @brief Factor Graph Values - * @brief VectorValues - * @author Frank Dellaert - */ - -#include -#include -#include - -using namespace std; - -namespace gtsam { - - /* ************************************************************************* */ - // Check if two VectorValuess are compatible, throw exception if not - static void check_compatible(const string& s, const VectorBTree& a, - const VectorBTree& b) { - if (!a.compatible(b)) - throw invalid_argument(s + ": incompatible VectorBTrees"); - } - - /* ************************************************************************* */ - // Check if two VectorBTrees are cloned, throw exception if not - // The 2 configs need exactly the same ranges tree - static void check_cloned(const string& s, const VectorBTree& a, - const VectorBTree& b) { - if (!a.cloned(b)) - throw invalid_argument(s + ": only defined for cloned VectorBTrees"); - } - - /* ************************************************************************* */ - void VectorBTree::print(const string& name) const { - odprintf("VectorBTree %s\n", name.c_str()); - odprintf("size: %d\n", size_); - BOOST_FOREACH(const Pair& p, ranges_) { - const Range& r = p.second; - odprintf("%s: %d:%d", string(p.first).c_str(), r.start(), r.size()); - gtsam::print(get(r)); - } - } - - /* ************************************************************************* */ - bool VectorBTree::equals(const VectorBTree& expected, double tol) const { - BOOST_FOREACH(const Pair& p, ranges_) { - const Symbol& j = p.first; - if (!expected.contains(j)) return false; - if (!equal_with_abs_tol(expected[j], get(p.second), tol)) - return false; - } - return true; - } - - /* ************************************************************************* */ - VectorBTree& VectorBTree::insert(const Symbol& j, const Vector& v) { - if (contains(j)) throw invalid_argument("VectorBTree::insert: key already exists"); - // calculate new range from current dimensionality and dim(a) - size_t n1 = values_.size(), n2 = n1 + v.size(); - ranges_ = ranges_.add(j,Range(n1,n2)); - // resize vector and COPY a into it - values_.resize(n2,true); - std::copy(v.begin(),v.end(),values_.begin()+n1); - // increment size - ++size_; - return *this; - } - - /* ************************************************************************* */ - VectorBTree& VectorBTree::insertAdd(const Symbol& j, const Vector& v) { - if (!contains(j)) return insert(j,v); - const Range& r = ranges_.find(j); - SubVector(values_,r) += v; - return *this; - } - - /* ************************************************************************* */ - void VectorBTree::insert(const VectorBTree& config) { - BOOST_FOREACH(const Pair& p, config.ranges_) - insert(p.first,config.get(p.second)); - } - - /* ************************************************************************* */ - void VectorBTree::insertAdd(const VectorBTree& config) { - BOOST_FOREACH(const Pair& p, config.ranges_) - insertAdd(p.first,config.get(p.second)); - } - - /* ************************************************************************* */ - std::vector VectorBTree::get_names() const { - std::vector names; - BOOST_FOREACH(const Pair& p, ranges_) - names.push_back(p.first); - return names; - } - - /* ************************************************************************* */ - SubVector VectorBTree::operator[](const Symbol& j) { - const Range& r = ranges_.find(j); - return SubVector(values_,r); - } - - /* ************************************************************************* */ - ConstSubVector VectorBTree::operator[](const Symbol& j) const { - const Range& r = ranges_.find(j); - return ConstSubVector(values_,r); - } - - /* ************************************************************************* */ - double VectorBTree::max() const { - double m = -std::numeric_limits::infinity(); - BOOST_FOREACH(const Pair& p, ranges_) - m = std::max(m, gtsam::max(get(p.second))); - return m; - } - - /* ************************************************************************* */ - VectorBTree VectorBTree::scale(double s) const { - VectorBTree scaled = *this; - scaled.values_ *= s; - return scaled; - } - - /* ************************************************************************* */ - VectorBTree VectorBTree::operator*(double s) const { - return scale(s); - } - - /* ************************************************************************* */ - VectorBTree VectorBTree::operator-() const { - VectorBTree result = *this; - result.values_ = - values_; - return result; - } - - /* ************************************************************************* */ - void VectorBTree::operator+=(const VectorBTree& b) { - check_compatible("VectorBTree:operator+=", *this, b); - values_ += b.values_; - } - - /* ************************************************************************* */ - VectorBTree VectorBTree::operator+(const VectorBTree& b) const { - check_compatible("VectorBTree:operator+", *this, b); - VectorBTree result = *this; - result += b; - return result; - } - - /* ************************************************************************* */ - void VectorBTree::operator-=(const VectorBTree& b) { - check_compatible("VectorBTree:operator-=", *this, b); - values_ -= b.values_; - } - - /* ************************************************************************* */ - VectorBTree VectorBTree::operator-(const VectorBTree& b) const { - VectorBTree result = *this; - result -= b; - return result; - } - - /* ************************************************************************* */ - double VectorBTree::dot(const VectorBTree& b) const { - check_compatible("VectorBTree:dot", *this, b); - return gtsam::dot(values_,b.values_); - } - - /* ************************************************************************* */ - VectorBTree& VectorBTree::zero() { - std::fill(values_.begin(), values_.end(), 0.0); - return *this; - } - - /* ************************************************************************* */ - VectorBTree VectorBTree::zero(const VectorBTree& x) { - VectorBTree cloned(x); - return cloned.zero(); - } - - /* ************************************************************************* */ - VectorBTree expmap(const VectorBTree& original, const VectorBTree& delta) { - check_compatible("VectorBTree:expmap", original, delta); - return original + delta; - } - - /* ************************************************************************* */ - VectorBTree expmap(const VectorBTree& original, const Vector& delta) - { - VectorBTree result = original; - result.values_ += delta; - return result; - } - - /* ************************************************************************* */ - void scal(double alpha, VectorBTree& x) { - scal(alpha, x.values_); - } - - /* ************************************************************************* */ - void axpy(double alpha, const VectorBTree& x, VectorBTree& y) { - check_cloned("VectorBTree:axpy", x, y); - axpy(alpha, x.values_, y.values_); - } - - /* ************************************************************************* */ - -} // gtsam diff --git a/gtsam/linear/VectorBTree.h b/gtsam/linear/VectorBTree.h deleted file mode 100644 index 0417dab52..000000000 --- a/gtsam/linear/VectorBTree.h +++ /dev/null @@ -1,272 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file VectorBTree.h - * @brief Factor Graph Valuesuration - * @author Frank Dellaert - */ - -// \callgraph - -#pragma once - -#include -#include - -#include -#include -#include -#include - -namespace gtsam { - - /** Factor Graph Valuesuration */ - class VectorBTree: public Testable { - - private: - - /** dictionary from Symbol to Range */ - typedef boost::numeric::ublas::range Range; - typedef BTree Ranges; - typedef Ranges::value_type Pair; - Ranges ranges_; - - /** Actual vector */ - Vector values_; - - /** size_ is number of vectors */ - size_t size_; - - /** private get from symbol pair */ - Vector get(const Range& r) const { - return sub(values_,r.start(),r.start()+r.size()); - } - -public: - - /** - * Default constructor - */ - VectorBTree() : - size_(0) { - } - - /** - * Copy constructor - */ - VectorBTree(const VectorBTree& c) : - ranges_(c.ranges_), values_(c.values_), size_(c.size_) { - } - - /** - * Construct with a single vector - */ - VectorBTree(const Symbol& j, const Vector& a) : - size_(0) { - insert(j, a); - } - - virtual ~VectorBTree() { - } - - /** print */ - void print(const std::string& name = "") const; - - /** equals, for unit testing */ - bool equals(const VectorBTree& expected, double tol = 1e-9) const; - - /** Insert a value into the values structure with a given index: O(n) */ - VectorBTree& insert(const Symbol& j, const Vector& v); - - /** Insert or add a value with given index: O(n) if does not exist */ - VectorBTree& insertAdd(const Symbol& j, const Vector& v); - - /** Insert a config into another config, replace if key already exists */ - void insert(const VectorBTree& config); - - /** Insert a config into another config, add if key already exists */ - void insertAdd(const VectorBTree& config); - - /** Nr of vectors */ - inline size_t size() const { return size_; } - - /** Total dimensionality */ - inline size_t dim() const { return values_.size(); } - - /** Check whether Symbol j exists in config */ - inline bool contains(const Symbol& j) const { return ranges_.mem(j); } - - /** return all the nodes in the graph **/ - std::vector get_names() const; - - /** Vector access in VectorBtree is via the SubVector type */ - SubVector operator[](const Symbol& j); - ConstSubVector operator[](const Symbol& j) const; - - /** [set] and [get] provided for access via MATLAB */ - void set(const Symbol& j, const Vector& v) { (*this)[j] = v; } - inline const Vector get(const Symbol& j) const { return (*this)[j];} - - /** max of the vectors */ - double max() const; - - /** - * Check if compatible with other config, which is only - * guaranteed if vectors are inserted in exactly the same order, - * or if one config was created from the other using assignment. - * In the latter case, comparison is O(1), otherwise can be O(n). - */ - inline bool compatible(const VectorBTree& other) const { - return ranges_ == other.ranges_; - } - - /** - * O(1) check if structure of config is *physically* the same. - * i.e., configs were created through some assignment chain. - */ - inline bool cloned(const VectorBTree& other) const { - return ranges_.same(other.ranges_); - } - - /** Math operators */ - VectorBTree scale(double s) const; - VectorBTree operator*(double s) const; - VectorBTree operator-() const; - void operator+=(const VectorBTree &b); - VectorBTree operator+(const VectorBTree &b) const; - void operator-=(const VectorBTree &b); - VectorBTree operator-(const VectorBTree &b) const; - double dot(const VectorBTree& b) const; - - /** Set all vectors to zero */ - VectorBTree& zero(); - - /** Create a clone of x with exactly same structure, except with zero values */ - static VectorBTree zero(const VectorBTree& x); - - /** - * Add a delta config, needed for use in NonlinearOptimizer - * For VectorBTree, this is just addition. - */ - friend VectorBTree expmap(const VectorBTree& original, const VectorBTree& delta); - - /** - * Add a delta vector (not a config) - * Will use the ordering that map uses to loop over vectors - */ - friend VectorBTree expmap(const VectorBTree& original, const Vector& delta); - - /** - * BLAS Level 1 scal: x <- alpha*x - */ - friend void scal(double alpha, VectorBTree& x); - - /** - * BLAS Level 1 axpy: y <- alpha*x + y - * UNSAFE !!!! Only works if x and y laid out in exactly same shape - * Used in internal loop in iterative for fast conjugate gradients - * Consider using other functions if this is not in hotspot - */ - friend void axpy(double alpha, const VectorBTree& x, VectorBTree& y); - - /** @brief Const iterator */ - class const_iterator { - - public: - - // traits for playing nice with STL - typedef ptrdiff_t difference_type; // correct ? - typedef std::forward_iterator_tag iterator_category; - typedef std::pair value_type; - typedef const value_type* pointer; - typedef const value_type& reference; - - bool operator==(const const_iterator& __x) const { return it_ == __x.it_;} - bool operator!=(const const_iterator& __x) const { return it_ != __x.it_;} - - reference operator*() const { return value_;} - pointer operator->() const { return &value_;} - - const_iterator& operator++() { increment(); return *this; } - const_iterator operator++(int) { - const_iterator __tmp = *this; increment(); return __tmp; - } - - private: - - Ranges::const_iterator it_, end_; - const VectorBTree& config_; - value_type value_; - - const_iterator(const VectorBTree& config, const Ranges::const_iterator& it) : - it_(it), end_(config_.ranges_.end()), config_(config) { - update(); - } - - void update() { - if (it_ != end_) value_ = std::make_pair(it_->first, config_.get(it_->second)); - } - - void increment() { it_++; update();} - - friend class VectorBTree; - }; // const_iterator - - // We do not have a non-const iterator right now - typedef const_iterator iterator; - - /** return iterators */ - const_iterator begin() const { return const_iterator(*this,ranges_.begin());} - const_iterator end () const { return const_iterator(*this,ranges_.end());} - -#ifdef UNTESTED - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) - { - ar & BOOST_SERIALIZATION_NVP(values); - } - }; // VectorBTree - -#endif - - }; // VectorBTree - - /** scalar product */ - inline VectorBTree operator*(double s, const VectorBTree& x) { - return x * s; - } - - /** dim function (for iterative::CGD) */ - inline double dim(const VectorBTree& x) { - return x.dim(); - } - - /** max of the vectors */ - inline double max(const VectorBTree& x) { - return x.max(); - } - - /* dot product */ - inline double dot(const VectorBTree& a, const VectorBTree& b) { - return a.dot(b); - } - - /** print with optional string */ - inline void print(const VectorBTree& v, const std::string& s = "") { - v.print(s); - } - -} // gtsam diff --git a/gtsam/linear/VectorMap.cpp b/gtsam/linear/VectorMap.cpp deleted file mode 100644 index 2b28770f0..000000000 --- a/gtsam/linear/VectorMap.cpp +++ /dev/null @@ -1,247 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file VectorMap.cpp - * @brief Factor Graph Values - * @brief VectorMap - * @author Carlos Nieto - * @author Christian Potthast - */ - -#include -#include -#include - -// trick from some reading group -#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) - -using namespace std; - -namespace gtsam { - -/* ************************************************************************* */ -void check_size(Index key, const Vector & vj, const Vector & dj) { - if (dj.size()!=vj.size()) { - cout << "For key \"" << key << "\"" << endl; - cout << "vj.size = " << vj.size() << endl; - cout << "dj.size = " << dj.size() << endl; - throw(std::invalid_argument("VectorMap::+ mismatched dimensions")); - } -} - -/* ************************************************************************* */ -std::vector VectorMap::get_names() const { - std::vector names; - for(const_iterator it=values.begin(); it!=values.end(); it++) - names.push_back(it->first); - return names; -} - -/* ************************************************************************* */ -VectorMap& VectorMap::insert(Index name, const Vector& v) { - values.insert(std::make_pair(name,v)); - return *this; -} - -/* ************************************************************************* */ -VectorMap& VectorMap::insertAdd(Index j, const Vector& a) { - Vector& vj = values[j]; - if (vj.size()==0) vj = a; else vj += a; - return *this; -} - -/* ************************************************************************* */ -void VectorMap::insert(const VectorMap& config) { - for (const_iterator it = config.begin(); it!=config.end(); it++) - insert(it->first, it->second); -} - -/* ************************************************************************* */ -void VectorMap::insertAdd(const VectorMap& config) { - for (const_iterator it = config.begin(); it!=config.end(); it++) - insertAdd(it->first,it->second); -} - -/* ************************************************************************* */ -size_t VectorMap::dim() const { - size_t result=0; - for (const_iterator it = begin(); it != end(); it++) - result += it->second.size(); - return result; -} - -/* ************************************************************************* */ -const Vector& VectorMap::operator[](Index name) const { - return values.at(name); -} - -/* ************************************************************************* */ -Vector& VectorMap::operator[](Index name) { - return values.at(name); -} - -/* ************************************************************************* */ -VectorMap VectorMap::scale(double s) const { - VectorMap scaled; - for (const_iterator it = begin(); it != end(); it++) - scaled.insert(it->first, s*it->second); - return scaled; -} - -/* ************************************************************************* */ -VectorMap VectorMap::operator*(double s) const { - return scale(s); -} - -/* ************************************************************************* */ -VectorMap VectorMap::operator-() const { - VectorMap result; - for (const_iterator it = begin(); it != end(); it++) - result.insert(it->first, - it->second); - return result; -} - -/* ************************************************************************* */ -void VectorMap::operator+=(const VectorMap& b) { - insertAdd(b); -} - -/* ************************************************************************* */ -VectorMap VectorMap::operator+(const VectorMap& b) const { - VectorMap result = *this; - result += b; - return result; -} - -/* ************************************************************************* */ -VectorMap VectorMap::operator-(const VectorMap& b) const { - VectorMap result; - for (const_iterator it = begin(); it != end(); it++) - result.insert(it->first, it->second - b[it->first]); - return result; -} - -/* ************************************************************************* */ -VectorMap& VectorMap::zero() { - for (iterator it = begin(); it != end(); it++) - std::fill(it->second.begin(), it->second.end(), 0.0); - return *this; -} - -/* ************************************************************************* */ -VectorMap VectorMap::zero(const VectorMap& x) { - VectorMap cloned(x); - return cloned.zero(); -} - -/* ************************************************************************* */ -Vector VectorMap::vector() const { - Vector result(dim()); - - size_t cur_dim = 0; - Index j; Vector vj; - FOREACH_PAIR(j, vj, values) { - subInsert(result, vj, cur_dim); - cur_dim += vj.size(); - } - return result; -} - -/* ************************************************************************* */ -VectorMap expmap(const VectorMap& original, const VectorMap& delta) -{ - VectorMap newValues; - Index j; Vector vj; // rtodo: copying vector? - FOREACH_PAIR(j, vj, original.values) { - if (delta.contains(j)) { - const Vector& dj = delta[j]; - check_size(j,vj,dj); - newValues.insert(j, vj + dj); - } else { - newValues.insert(j, vj); - } - } - return newValues; -} - -/* ************************************************************************* */ -VectorMap expmap(const VectorMap& original, const Vector& delta) -{ - VectorMap newValues; - size_t i = 0; - Index j; Vector vj; // rtodo: copying vector? - FOREACH_PAIR(j, vj, original.values) { - size_t mj = vj.size(); - Vector dj = sub(delta, i, i+mj); - newValues.insert(j, vj + dj); - i += mj; - } - return newValues; -} - -/* ************************************************************************* */ -void VectorMap::print(const string& name) const { - odprintf("VectorMap %s\n", name.c_str()); - odprintf("size: %d\n", values.size()); - for (const_iterator it = begin(); it != end(); it++) { - odprintf("%d:", it->first); - gtsam::print(it->second); - } -} - -/* ************************************************************************* */ -bool VectorMap::equals(const VectorMap& expected, double tol) const { - if( values.size() != expected.size() ) return false; - - // iterate over all nodes - for (const_iterator it = begin(); it != end(); it++) { - Vector vExpected = expected[it->first]; - if(!equal_with_abs_tol(vExpected,it->second,tol)) - return false; - } - return true; -} - -/* ************************************************************************* */ -double VectorMap::dot(const VectorMap& b) const { - double result = 0.0; // rtodo: copying vector - for (const_iterator it = begin(); it != end(); it++) - result += gtsam::dot(it->second,b[it->first]); - return result; -} - -/* ************************************************************************* */ -double dot(const VectorMap& a, const VectorMap& b) { - return a.dot(b); -} - -/* ************************************************************************* */ -void scal(double alpha, VectorMap& x) { - for (VectorMap::iterator xj = x.begin(); xj != x.end(); xj++) - scal(alpha, xj->second); -} - -/* ************************************************************************* */ -void axpy(double alpha, const VectorMap& x, VectorMap& y) { - VectorMap::const_iterator xj = x.begin(); - for (VectorMap::iterator yj = y.begin(); yj != y.end(); yj++, xj++) - axpy(alpha, xj->second, yj->second); -} - -/* ************************************************************************* */ -void print(const VectorMap& v, const std::string& s){ - v.print(s); -} - -/* ************************************************************************* */ - -} // gtsam diff --git a/gtsam/linear/VectorMap.h b/gtsam/linear/VectorMap.h deleted file mode 100644 index c251cd064..000000000 --- a/gtsam/linear/VectorMap.h +++ /dev/null @@ -1,183 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file VectorMap.h - * @brief Factor Graph Valuesuration - * @author Carlos Nieto - * @author Christian Potthast - */ - -// \callgraph - -#pragma once - -#include -#include - -#include -#include -#include - -namespace gtsam { - - /** Factor Graph Valuesuration */ - class VectorMap : public Testable { - - protected: - /** Map from string indices to values */ - std::map values; - - public: - typedef std::map::iterator iterator; - typedef std::map::const_iterator const_iterator; - - VectorMap() {} - VectorMap(const VectorMap& cfg_in): values(cfg_in.values) {} - VectorMap(Index j, const Vector& a) { insert(j,a); } - - virtual ~VectorMap() {} - - /** return all the nodes in the graph **/ - std::vector get_names() const; - - /** convert into a single large vector */ - Vector vector() const; - - /** Insert a value into the values structure with a given index */ - VectorMap& insert(Index name, const Vector& v); - - /** Insert or add a value with given index */ - VectorMap& insertAdd(Index j, const Vector& v); - - /** Insert a config into another config */ - void insert(const VectorMap& config); - - /** Insert a config into another config, add if key already exists */ - void insertAdd(const VectorMap& config); - - const_iterator begin() const {return values.begin();} - const_iterator end() const {return values.end();} - iterator begin() {return values.begin();} - iterator end() {return values.end();} - - /** Vector access in VectorMap is via a Vector reference */ - Vector& operator[](Index j); - const Vector& operator[](Index j) const; - - /** [set] and [get] provided for access via MATLAB */ - inline Vector& get(Index j) { return (*this)[j];} - void set(Index j, const Vector& v) { (*this)[j] = v; } - inline const Vector& get(Index j) const { return (*this)[j];} - - bool contains(Index name) const { - const_iterator it = values.find(name); - return (it!=values.end()); - } - - /** Nr of vectors */ - size_t size() const { return values.size();} - - /** Total dimensionality */ - size_t dim() const; - - /** max of the vectors */ - inline double max() const { - double m = -std::numeric_limits::infinity(); - for(const_iterator it=begin(); it!=end(); it++) - m = std::max(m, gtsam::max(it->second)); - return m; - } - - /** Scale */ - VectorMap scale(double s) const; - VectorMap operator*(double s) const; - - /** Negation */ - VectorMap operator-() const; - - /** Add in place */ - void operator+=(const VectorMap &b); - - /** Add */ - VectorMap operator+(const VectorMap &b) const; - - /** Subtract */ - VectorMap operator-(const VectorMap &b) const; - - /** print */ - void print(const std::string& name = "") const; - - /** equals, for unit testing */ - bool equals(const VectorMap& expected, double tol=1e-9) const; - - void clear() {values.clear();} - - /** Dot product */ - double dot(const VectorMap& b) const; - - /** Set all vectors to zero */ - VectorMap& zero(); - - /** Create a clone of x with exactly same structure, except with zero values */ - static VectorMap zero(const VectorMap& x); - - /** - * Add a delta config, needed for use in NonlinearOptimizer - * For VectorMap, this is just addition. - */ - friend VectorMap expmap(const VectorMap& original, const VectorMap& delta); - - /** - * Add a delta vector (not a config) - * Will use the ordering that map uses to loop over vectors - */ - friend VectorMap expmap(const VectorMap& original, const Vector& delta); - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) - { - ar & BOOST_SERIALIZATION_NVP(values); - } - }; // VectorMap - - /** scalar product */ - inline VectorMap operator*(double s, const VectorMap& x) {return x*s;} - - /** Dot product */ - double dot(const VectorMap&, const VectorMap&); - - /** - * BLAS Level 1 scal: x <- alpha*x - */ - void scal(double alpha, VectorMap& x); - - /** - * BLAS Level 1 axpy: y <- alpha*x + y - * UNSAFE !!!! Only works if x and y laid out in exactly same shape - * Used in internal loop in iterative for fast conjugate gradients - * Consider using other functions if this is not in hotspot - */ - void axpy(double alpha, const VectorMap& x, VectorMap& y); - - /** dim function (for iterative::CGD) */ - inline double dim(const VectorMap& x) { return x.dim();} - - /** max of the vectors */ - inline double max(const VectorMap& x) { return x.max();} - - /** print with optional string */ - void print(const VectorMap& v, const std::string& s = ""); - -} // gtsam diff --git a/gtsam/linear/tests/testBayesNetPreconditioner.cpp b/gtsam/linear/tests/testBayesNetPreconditioner.cpp deleted file mode 100644 index 2621ad1f3..000000000 --- a/gtsam/linear/tests/testBayesNetPreconditioner.cpp +++ /dev/null @@ -1,79 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testBayesNetConditioner.cpp - * @brief Unit tests for BayesNetConditioner - * @author Frank Dellaert - **/ - -#include -#include -#include - -#define GTSAM_MAGIC_KEY - -#include -#include -#include - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ -TEST( BayesNetPreconditioner, operators ) -{ - // Build a simple Bayes net - // small Bayes Net x <- y, x=2D, y=1D - // 1 2 3 x1 0 - // 0 1 2 * x2 = 0 - // 0 0 1 x3 1 - - // Create a scalar Gaussian on y - GaussianBayesNet bn = scalarGaussian("y", 1, 0.1); - - // Add a conditional node with one parent |Rx+Sy-d| - Matrix R11 = Matrix_(2, 2, 1.0, 2.0, 0.0, 1.0), S12 = Matrix_(2, 1, 3.0, 2.0); - Vector d = zero(2); - Vector sigmas = Vector_(2, 0.1, 0.1); - push_front(bn, "x", d, R11, "y", S12, sigmas); - - // Create Precondioner class - GaussianFactorGraph dummy; - BayesNetPreconditioner P(dummy,bn); - - // inv(R1)*d should equal solution [1;-2;1] - VectorValues D; - D.insert("x", d); - D.insert("y", Vector_(1, 1.0 / 0.1)); // corrected by sigma - VectorValues expected1; - expected1.insert("x", Vector_(2, 1.0, -2.0)); - expected1.insert("y", Vector_(1, 1.0)); - VectorValues actual1 = P.backSubstitute(D); - CHECK(assert_equal(expected1,actual1)); - - // inv(R1')*ones should equal ? - VectorValues ones; - ones.insert("x", Vector_(2, 1.0, 1.0)); - ones.insert("y", Vector_(1, 1.0)); - VectorValues expected2; - expected2.insert("x", Vector_(2, 0.1, -0.1)); - expected2.insert("y", Vector_(1, 0.0)); - VectorValues actual2 = P.backSubstituteTranspose(ones); - CHECK(assert_equal(expected2,actual2)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/linear/tests/testVectorBTree.cpp b/gtsam/linear/tests/testVectorBTree.cpp deleted file mode 100644 index b12871c91..000000000 --- a/gtsam/linear/tests/testVectorBTree.cpp +++ /dev/null @@ -1,287 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testVectorBTree.cpp - * @brief Unit tests for Factor Graph Values - * @author Frank Dellaert - **/ - -#include -#include -#include - -#include -using namespace boost::assign; // bring 'operator+=()' into scope - -#include -#include -#include - -using namespace std; -using namespace gtsam; - -static Symbol l1('l',1), x1('x',1), x2('x',2); -static double inf = std::numeric_limits::infinity(); - -/* ************************************************************************* */ -VectorBTree smallVectorBTree() { - VectorBTree c; - c.insert(l1, Vector_(2, 0.0, -1.0)); - c.insert(x1, Vector_(2, 0.0, 0.0)); - c.insert(x2, Vector_(2, 1.5, 0.0)); - return c; -} - -/* ************************************************************************* */ -TEST( VectorBTree, constructor_insert_get ) -{ - VectorBTree expected; - Vector v = Vector_(3, 5.0, 6.0, 7.0); - expected.insert(x1, v); - VectorBTree actual(x1, v); - LONGS_EQUAL(1,actual.size()) - CHECK(assert_equal(expected,actual)) - CHECK(equal_with_abs_tol(v,actual[x1])) - CHECK(equal_with_abs_tol(v,actual.get(x1))) -} - -/* ************************************************************************* */ -TEST( VectorBTree, dim) { - VectorBTree c = smallVectorBTree(); - LONGS_EQUAL(6,c.dim()); -} - -/* ************************************************************************* */ -TEST( VectorBTree, insertAdd) { - VectorBTree expected; - expected.insert(l1, Vector_(2, 0.0, -2.0)); - expected.insert(x1, Vector_(2, 0.0, 0.0)); - expected.insert(x2, Vector_(2, 3.0, 0.0)); - VectorBTree actual = smallVectorBTree(); - actual.insertAdd(actual); - CHECK(assert_equal(expected,actual)) -} - -/* ************************************************************************* */ -TEST( VectorBTree, zero) { - VectorBTree expected; - expected.insert(l1, Vector_(2, 0.0, 0.0)); - expected.insert(x1, Vector_(2, 0.0, 0.0)); - expected.insert(x2, Vector_(2, 0.0, 0.0)); - VectorBTree actual = smallVectorBTree(); - CHECK(assert_equal(expected,VectorBTree::zero(actual))); - CHECK(assert_equal(expected,actual.zero())); -} - -/* ************************************************************************* */ -TEST( VectorBTree, insert_config) { - VectorBTree expected = smallVectorBTree(); - VectorBTree actual, toAdd = smallVectorBTree(); - actual.insert(toAdd); - CHECK(assert_equal(expected,actual)) -} - -/* ************************************************************************* */ -TEST( VectorBTree, get_names) { - VectorBTree c = smallVectorBTree(); - std::vector expected, actual = c.get_names(); - expected += l1, x1, x2; - CHECK(expected==actual) -} - -/* ************************************************************************* */ -TEST( VectorBTree, const_iterator) { - VectorBTree c = smallVectorBTree(); - VectorBTree::const_iterator it = c.begin(); - CHECK(assert_equal(l1,it->first)); - CHECK(assert_equal(Vector_(2, 0.0,-1.0),(it++)->second)); - CHECK(assert_equal(x1,it->first)); - CHECK(assert_equal(Vector_(2, 0.0, 0.0),(it++)->second)); - CHECK(assert_equal(x2,it->first)); - CHECK(assert_equal(Vector_(2, 1.5, 0.0),(it++)->second)); - CHECK(it==c.end()); -} - -/* ************************************************************************* */ -TEST( VectorBTree, equals ) -{ - VectorBTree cfg1; - cfg1.insert(x1, Vector_(3, 5.0, 6.0, 7.0)); - CHECK(cfg1.equals(cfg1)); - CHECK(cfg1.compatible(cfg1)); - CHECK(cfg1.cloned(cfg1)); - - VectorBTree cfg2; - cfg2.insert(x1, Vector_(3, 5.0, 6.0, 7.0)); - CHECK(cfg1.equals(cfg2)); - CHECK(cfg1.compatible(cfg2)); - CHECK(!cfg1.cloned(cfg2)); - - VectorBTree cfg3 = cfg1; - CHECK(cfg1.equals(cfg3)); - CHECK(cfg1.compatible(cfg3)); - CHECK(cfg1.cloned(cfg3)); - - VectorBTree cfg4; - cfg4.insert(x1, Vector_(3, 5.0, 6.0, 8.0)); - CHECK(!cfg1.equals(cfg4)); - CHECK(cfg1.compatible(cfg4)); - CHECK(!cfg1.cloned(cfg4)); -} - -/* ************************************************************************* */ -TEST( VectorBTree, equals_nan ) - { - VectorBTree cfg1, cfg2; - Vector v1 = Vector_(3, 5.0, 6.0, 7.0); - Vector v2 = Vector_(3, inf, inf, inf); - cfg1.insert(x1, v1); - cfg2.insert(x1, v2); - CHECK(!cfg1.equals(cfg2)); - CHECK(!cfg2.equals(cfg1)); - } - -/* ************************************************************************* */ -TEST( VectorBTree, contains) -{ - VectorBTree fg; - Vector v = Vector_(3, 5.0, 6.0, 7.0); - fg.insert(x1, v); - CHECK(fg.contains(x1)); - CHECK(!fg.contains(x2)); -} - -/* ************************************************************************* */ -TEST( VectorBTree, max) { - VectorBTree c = smallVectorBTree(); - DOUBLES_EQUAL(1.5,c.max(),1e-9); -} - -/* ************************************************************************* */ -TEST( VectorBTree, scale) { - VectorBTree cfg; - cfg.insert(x1, Vector_(2, 1.0, 2.0)); - cfg.insert(x2, Vector_(2,-1.0,-2.0)); - - VectorBTree actual = cfg.scale(2.0); - - VectorBTree expected; - expected.insert(x1, Vector_(2, 2.0, 4.0)); - expected.insert(x2, Vector_(2,-2.0,-4.0)); - - CHECK(assert_equal(actual, expected)); -} - -/* ************************************************************************* */ -TEST( VectorBTree, plus) -{ - VectorBTree c; - Vector vx = Vector_(3, 5.0, 6.0, 7.0), vy = Vector_(2, 8.0, 9.0); - c.insert(x1,vx).insert(x2,vy); - - VectorBTree delta; - Vector dx = Vector_(3, 1.0, 1.0, 1.0), dy = Vector_(2, -1.0, -1.0); - delta.insert(x1, dx).insert(x2,dy); - CHECK(delta.compatible(c)); - - // operator + - VectorBTree expected; - Vector wx = Vector_(3, 6.0, 7.0, 8.0), wy = Vector_(2, 7.0, 8.0); - expected.insert(x1, wx).insert(x2,wy); - CHECK(assert_equal(expected,c+delta)); - - // operator - - VectorBTree expected2; - Vector wx2 = Vector_(3, -5.0, -6.0, -7.0), wy2 = Vector_(2, -8.0, -9.0); - expected2.insert(x1, wx2).insert(x2,wy2); - CHECK(assert_equal(expected2,-c)); - - // expmap - VectorBTree actual = expmap(c,delta); - CHECK(assert_equal(expected,actual)); - - // in-place (although + already tests that, really) - c += delta; - CHECK(assert_equal(expected,c)); -} - -/* ************************************************************************* */ -TEST( VectorBTree, operators) { - VectorBTree c; c.insert(x1, Vector_(2, 1.1, 2.2)); - VectorBTree expected1; expected1.insert(x1, Vector_(2, 2.2, 4.4)); - CHECK(assert_equal(expected1,c*2)); - CHECK(assert_equal(expected1,c+c)); - VectorBTree expected2; expected2.insert(x1, Vector_(2, 0.0, 0.0)); - CHECK(assert_equal(expected2,c-c)); -} - -/* ************************************************************************* */ -TEST( VectorBTree, dot) { - VectorBTree c = smallVectorBTree(); - DOUBLES_EQUAL(3.25,dot(c,c),1e-9); -} - -/* ************************************************************************* */ -TEST( VectorBTree, expmap) -{ - VectorBTree c = smallVectorBTree(); - Vector v = Vector_(6, 0.0,-1.0, 0.0, 0.0, 1.5, 0.0); // l1, x1, x2 - CHECK(assert_equal(expmap(c,c),expmap(c,v))); -} - -/* ************************************************************************* */ -TEST( VectorBTree, scal) { - VectorBTree x,expected; - x.insert(x1,Vector_(3, 1.0, 2.0, 3.0)); - x.insert(x2,Vector_(2, 4.0, 5.0)); - expected.insert(x1,Vector_(3, 10.0, 20.0, 30.0)); - expected.insert(x2,Vector_(2, 40.0, 50.0)); - scal(10,x); - CHECK(assert_equal(expected,x)); -} - -/* ************************************************************************* */ -TEST( VectorBTree, axpy) { - VectorBTree x; - x.insert(x1,Vector_(3, 1.0, 1.0, 1.0)); - x.insert(x2,Vector_(2, -1.0, -1.0)); - - // axpy will only work on cloned configs - enforced for speed - VectorBTree y = VectorBTree::zero(x); - y[x1] = Vector_(3, 5.0, 6.0, 7.0); - y[x2] = Vector_(2, 8.0, 9.0); -// axpy(10,x,y); -// -// // Check result -// VectorBTree expected; -// expected.insert(x1,Vector_(3, 15.0, 16.0, 17.0)); -// expected.insert(x2,Vector_(2, -2.0, -1.0)); -// CHECK(assert_equal(expected,y)); -} - -/* ************************************************************************* */ -TEST( VectorBTree, subVector) { - VectorBTree c; c.insert(x1, Vector_(2, 1.1, 2.2)); - SubVector cx = c[x1]; - for (size_t i = 0; i < 2; i++) - cx(i) = cx(i)*2.0; - VectorBTree expected; expected.insert(x1, Vector_(2, 2.2, 4.4)); - CHECK(assert_equal(expected,c)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} - -/* ************************************************************************* */ diff --git a/gtsam/linear/tests/testVectorMap.cpp b/gtsam/linear/tests/testVectorMap.cpp deleted file mode 100644 index 79263bd0e..000000000 --- a/gtsam/linear/tests/testVectorMap.cpp +++ /dev/null @@ -1,222 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testVectorMap.cpp - * @brief Unit tests for Factor Graph Values - * @author Carlos Nieto - **/ - -/*STL/C++*/ -#include -#include - -#define GTSAM_MAGIC_KEY - -#include -#include -#include - -using namespace std; -using namespace gtsam; - -static const Index l1=0, x1=1, x2=2; -static const Index _a_=3, _x_=4, _y_=5, _g_=6, _p_=7; - -/* ************************************************************************* */ -VectorMap smallVectorMap() { - VectorMap c; - c.insert(l1, Vector_(2, 0.0, -1.0)); - c.insert(x1, Vector_(2, 0.0, 0.0)); - c.insert(x2, Vector_(2, 1.5, 0.0)); - return c; -} - -/* ************************************************************************* */ -TEST( VectorMap, equals1 ) - { - VectorMap expected; - Vector v = Vector_(3, 5.0, 6.0, 7.0); - expected.insert(_a_,v); - VectorMap actual; - actual.insert(_a_,v); - CHECK(assert_equal(expected,actual)); - CHECK(assert_equal(expected[_a_],actual.get(_a_))) -} - -/* ************************************************************************* */ -TEST( VectorMap, equals2 ) - { - VectorMap cfg1, cfg2; - Vector v1 = Vector_(3, 5.0, 6.0, 7.0); - Vector v2 = Vector_(3, 5.0, 6.0, 8.0); - cfg1.insert(_x_, v1); - cfg2.insert(_x_, v2); - CHECK(!cfg1.equals(cfg2)); - CHECK(!cfg2.equals(cfg1)); - } - -/* ************************************************************************* */ -TEST( VectorMap, fullVector) -{ - VectorMap c = smallVectorMap(); - Vector actual = c.vector(); - Vector expected = Vector_(6, 0.0, -1.0, 0.0, 0.0, 1.5, 0.0); - CHECK(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ - -#include -double inf = std::numeric_limits::infinity(); - -TEST( VectorMap, equals_nan ) - { - VectorMap cfg1, cfg2; - Vector v1 = Vector_(3, 5.0, 6.0, 7.0); - Vector v2 = Vector_(3, inf, inf, inf); - cfg1.insert(_x_, v1); - cfg2.insert(_x_, v2); - CHECK(!cfg1.equals(cfg2)); - CHECK(!cfg2.equals(cfg1)); - } - -/* ************************************************************************* */ -TEST( VectorMap, contains) -{ - VectorMap fg; - Vector v = Vector_(3, 5.0, 6.0, 7.0); - fg.insert(_a_, v); - CHECK(fg.contains(_a_)); - CHECK(!fg.contains(_g_)); -} - -/* ************************************************************************* */ -TEST( VectorMap, expmap) -{ - VectorMap c = smallVectorMap(); - Vector v = Vector_(6, 0.0,-1.0, 0.0, 0.0, 1.5, 0.0); // l1, x1, x2 - CHECK(assert_equal(expmap(c,c),expmap(c,v))); -} - -/* ************************************************************************* */ -TEST( VectorMap, plus) -{ - VectorMap c; - Vector vx = Vector_(3, 5.0, 6.0, 7.0), vy = Vector_(2, 8.0, 9.0); - c += VectorMap(_x_,vx); - c += VectorMap(_y_,vy); - - VectorMap delta; - Vector dx = Vector_(3, 1.0, 1.0, 1.0), dy = Vector_(2, -1.0, -1.0); - delta.insert(_x_, dx).insert(_y_,dy); - - VectorMap expected; - Vector wx = Vector_(3, 6.0, 7.0, 8.0), wy = Vector_(2, 7.0, 8.0); - expected.insert(_x_, wx).insert(_y_,wy); - - // functional - VectorMap actual = expmap(c,delta); - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( VectorMap, scale) { - VectorMap cfg; - cfg.insert(_x_, Vector_(2, 1.0, 2.0)); - cfg.insert(_y_, Vector_(2,-1.0,-2.0)); - - VectorMap actual = cfg.scale(2.0); - - VectorMap expected; - expected.insert(_x_, Vector_(2, 2.0, 4.0)); - expected.insert(_y_, Vector_(2,-2.0,-4.0)); - - CHECK(assert_equal(actual, expected)); -} - -/* ************************************************************************* */ -TEST( VectorMap, axpy) { - VectorMap x,y,expected; - x += VectorMap(_x_,Vector_(3, 1.0, 1.0, 1.0)); - x += VectorMap(_y_,Vector_(2, -1.0, -1.0)); - y += VectorMap(_x_,Vector_(3, 5.0, 6.0, 7.0)); - y += VectorMap(_y_,Vector_(2, 8.0, 9.0)); - expected += VectorMap(_x_,Vector_(3, 15.0, 16.0, 17.0)); - expected += VectorMap(_y_,Vector_(2, -2.0, -1.0)); - axpy(10,x,y); - CHECK(assert_equal(expected,y)); -} - -/* ************************************************************************* */ -TEST( VectorMap, scal) { - VectorMap x,expected; - x += VectorMap(_x_,Vector_(3, 1.0, 2.0, 3.0)); - x += VectorMap(_y_,Vector_(2, 4.0, 5.0)); - expected += VectorMap(_x_,Vector_(3, 10.0, 20.0, 30.0)); - expected += VectorMap(_y_,Vector_(2, 40.0, 50.0)); - scal(10,x); - CHECK(assert_equal(expected,x)); -} - -/* ************************************************************************* */ -TEST( VectorMap, update_with_large_delta) { - // this test ensures that if the update for delta is larger than - // the size of the config, it only updates existing variables - VectorMap init, delta; - init.insert(_x_, Vector_(2, 1.0, 2.0)); - init.insert(_y_, Vector_(2, 3.0, 4.0)); - delta.insert(_x_, Vector_(2, 0.1, 0.1)); - delta.insert(_y_, Vector_(2, 0.1, 0.1)); - delta.insert(_p_, Vector_(2, 0.1, 0.1)); - - VectorMap actual = expmap(init,delta); - VectorMap expected; - expected.insert(_x_, Vector_(2, 1.1, 2.1)); - expected.insert(_y_, Vector_(2, 3.1, 4.1)); - - CHECK(assert_equal(actual, expected)); -} - -/* ************************************************************************* */ -TEST( VectorMap, dot) { - VectorMap c = smallVectorMap(); - DOUBLES_EQUAL(3.25,dot(c,c),1e-9); -} - -/* ************************************************************************* */ -TEST( VectorMap, dim) { - VectorMap c = smallVectorMap(); - LONGS_EQUAL(6,c.dim()); -} - -/* ************************************************************************* */ -TEST( VectorMap, operators) { - VectorMap c; c.insert(_x_, Vector_(2, 1.1, 2.2)); - VectorMap expected1; expected1.insert(_x_, Vector_(2, 2.2, 4.4)); - CHECK(assert_equal(expected1,c*2)); - CHECK(assert_equal(expected1,c+c)); - VectorMap expected2; expected2.insert(_x_, Vector_(2, 0.0, 0.0)); - CHECK(assert_equal(expected2,c-c)); -} - -/* ************************************************************************* */ -TEST( VectorMap, getReference) { - VectorMap c; c.insert(_x_, Vector_(2, 1.1, 2.2)); - Vector& cx = c[_x_]; - cx = cx*2.0; - VectorMap expected; expected.insert(_x_, Vector_(2, 2.2, 4.4)); - CHECK(assert_equal(expected,c)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam/linear/tests/timeVectorValues.cpp b/gtsam/linear/tests/timeVectorValues.cpp deleted file mode 100644 index bfb553ae8..000000000 --- a/gtsam/linear/tests/timeVectorValues.cpp +++ /dev/null @@ -1,81 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * @file timeVectorValues.cpp - * @brief Performs timing and profiling for VectorValues operations - * @author Frank Dellaert - */ - -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -#define TIME1(STATEMENT) { boost::timer t; \ - STATEMENT; \ - double time = t.elapsed(); \ - cout << "Average elapsed time :" << 10e3 * time / n << "ms." << endl; } - -#define TIME(STATEMENT) TIME1(for (size_t j = 0; j < n; ++j) STATEMENT;) - -/* ************************************************************************* */ -int main(int argc, char ** argv) { - - // Time assignment operator - cout << "Starting operator=() Timing" << endl; - - // n = number of times to loop - // m = number of vectors - // r = rows per vector - size_t n = 100, m = 30000, r = 3, alpha = 0.1; - - { - cout << "Vector:" << endl; - Vector v = zero(m * r), w = zero(m * r); - TIME(v=w); - TIME(v+=w); - TIME(v+=alpha*w); - TIME(axpy(alpha,v,w)); - } - - { - // Create 2 VectorBTrees and one VectorMap - VectorBTree p, q; - VectorMap old; - cout << "Creating VectorBTree:" << endl; - TIME1(for (size_t i = 0; i < m; ++i) { - Vector v = zero(r); - Symbol key('x', i); - p.insert(key, v); - q.insert(key, v); - old.insert(key, v); - }) - - cout << "VectorBTree:" << endl; - TIME(p=q); - TIME(p+=q); - TIME(p+=alpha*q); - TIME(axpy(alpha,p,q)); - - cout << "VectorBTree get:" << endl; - TIME1(for (size_t i = 0; i < m; ++i) p[Symbol('x', i)]); - - cout << "VectorMap get:" << endl; - TIME1(for (size_t i = 0; i < m; ++i) old[Symbol('x', i)]); -} - - return 0; -} -/* ************************************************************************* */ diff --git a/gtsam/nonlinear/tests/testOrdering.cpp b/gtsam/nonlinear/tests/testOrdering.cpp index 477279da0..ac4a57b40 100644 --- a/gtsam/nonlinear/tests/testOrdering.cpp +++ b/gtsam/nonlinear/tests/testOrdering.cpp @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + /** * @file testOrdering * @author Alex Cunningham diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am index 6fcd5ad12..9f9f1270d 100644 --- a/gtsam/slam/Makefile.am +++ b/gtsam/slam/Makefile.am @@ -11,17 +11,12 @@ sources = check_PROGRAMS = # simulated2D example -headers += Simulated2DValues.h -headers += Simulated2DPosePrior.h Simulated2DPointPrior.h -headers += Simulated2DOdometry.h Simulated2DMeasurement.h +# headers += Simulated2DValues.h headers += simulated2DConstraints.h sources += simulated2D.cpp smallExample.cpp check_PROGRAMS += tests/testSimulated2D # simulated2DOriented example -headers += Simulated2DOrientedValues.h -headers += Simulated2DOrientedPosePrior.h -headers += Simulated2DOrientedOdometry.h sources += simulated2DOriented.cpp check_PROGRAMS += tests/testSimulated2DOriented @@ -37,8 +32,7 @@ headers += BetweenConstraint.h BoundingConstraint.h # 2D Pose SLAM sources += pose2SLAM.cpp dataset.cpp -#sources += Pose2SLAMOptimizer.cpp -check_PROGRAMS += tests/testPose2Factor tests/testPose2Values tests/testPose2SLAM tests/testPose2Prior +check_PROGRAMS += tests/testPose2SLAM tests/testPose2SLAMwSPCG # 2D SLAM using Bearing and Range headers += BearingFactor.h RangeFactor.h BearingRangeFactor.h @@ -47,12 +41,12 @@ check_PROGRAMS += tests/testPlanarSLAM # 3D Pose constraints sources += pose3SLAM.cpp -check_PROGRAMS += tests/testPose3Factor tests/testPose3Values tests/testPose3SLAM +check_PROGRAMS += tests/testPose3SLAM # Visual SLAM headers += GeneralSFMFactor.h ProjectionFactor.h sources += visualSLAM.cpp -check_PROGRAMS += tests/testVSLAMFactor tests/testVSLAMGraph tests/testVSLAMValues tests/testGeneralSFMFactor tests/testGeneralSFMFactor_Cal3Bundler +check_PROGRAMS += tests/testProjectionFactor tests/testVSLAM tests/testGeneralSFMFactor tests/testGeneralSFMFactor_Cal3Bundler # StereoFactor headers += StereoFactor.h diff --git a/gtsam/slam/Pose2SLAMOptimizer.cpp b/gtsam/slam/Pose2SLAMOptimizer.cpp deleted file mode 100644 index c0ad89af9..000000000 --- a/gtsam/slam/Pose2SLAMOptimizer.cpp +++ /dev/null @@ -1,86 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * Pose2SLAMOptimizer.cpp - * - * Created on: Jan 22, 2010 - * Author: dellaert - */ - -#include -#include -#include -#include - -using namespace std; -namespace gtsam { - - using namespace pose2SLAM; - - /* ************************************************************************* */ - Pose2SLAMOptimizer::Pose2SLAMOptimizer(const string& dataset_name, - const string& path) { - - static int maxID = 0; - static bool addNoise = false; - - string filename; - boost::optional noiseModel; - boost::tie(filename, noiseModel) = dataset(dataset_name); - - // read graph and initial estimate - boost::tie(graph_, theta_) = load2D(filename, noiseModel, maxID, addNoise); - graph_->addPrior(theta_->begin()->first, theta_->begin()->second, - noiseModel::Unit::Create(3)); - - // initialize non-linear solver - solver_.initialize(*graph_, *theta_); - - linearize(); - } - - /* ************************************************************************* */ - void Pose2SLAMOptimizer::print(const string& str) const { - GTSAM_PRINT(*graph_); - GTSAM_PRINT(*theta_); - //TODO - //GTSAM_PRINT(solver_); - GTSAM_PRINT(*system_); - } - - /* ************************************************************************* */ - void Pose2SLAMOptimizer::update(const Vector& x) { - VectorValues X = system_->assembleValues(x, *solver_.ordering()); - *theta_ = theta_->expmap(X); - linearize(); - } - - /* ************************************************************************* */ - void Pose2SLAMOptimizer::updatePreconditioned(const Vector& y) { - Vector x; - update(x); - } - - /* ************************************************************************* */ - Vector Pose2SLAMOptimizer::optimize() const { - VectorValues X = solver_.optimize(*system_); - std::list vs; - BOOST_FOREACH(const Symbol& key, *solver_.ordering()) - vs.push_back(X[key]); - return concatVectors(vs); - } - - /* ************************************************************************* */ - double Pose2SLAMOptimizer::error() const { - return graph_->error(*theta_); - } -} diff --git a/gtsam/slam/Pose2SLAMOptimizer.h b/gtsam/slam/Pose2SLAMOptimizer.h deleted file mode 100644 index 905e7f392..000000000 --- a/gtsam/slam/Pose2SLAMOptimizer.h +++ /dev/null @@ -1,123 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * Pose2SLAMOptimizer.h - * - * Created on: Jan 22, 2010 - * Author: dellaert - */ - -#ifndef POSE2SLAMOPTIMIZER_H_ -#define POSE2SLAMOPTIMIZER_H_ - -#include - -#include -#include -#include - -namespace gtsam { - - /** - * Optimizer class for use in MATLAB - * Keeps a Pose2Values estimate - * Returns all relevant matrices so MATLAB can optimize :-) - */ - class Pose2SLAMOptimizer { - private: - - /** Non-linear factor graph */ - boost::shared_ptr graph_; - - /** Current non-linear estimate */ - boost::shared_ptr theta_; - - /** Non-linear solver */ - typedef SubgraphSolver SPCG_Solver; - SPCG_Solver solver_; - - /** Linear Solver */ - boost::shared_ptr system_; - - public: - /** - * Create optimizer: finds spanning tree and ordering - */ - Pose2SLAMOptimizer(const std::string& dataset = "", - const std::string& path = ""); - - /** - * print the object - */ - void print(const std::string& str = "") const; - - /** - * Virtual destructor - */ - virtual ~Pose2SLAMOptimizer() { - } - - /** - * return graph pointer - */ - boost::shared_ptr graph() const { - return graph_; - } - - /** - * return the current linearization point - */ - boost::shared_ptr theta() const { - return theta_; - } - - /** - * linearize around current theta - */ - void linearize() { - system_ = solver_.linearize(*graph_, *theta_); - } - - /** - * Optimize to get a - */ - Vector optimize() const; - - double error() const; - - /** - * Return matrices associated with optimization problem - * around current non-linear estimate theta - * Returns [IJS] sparse representation - */ - Matrix a1() const { return system_->A1(*solver_.ordering()); } - Matrix a2() const { return system_->A2(*solver_.ordering()); } - Vector b1() const { return system_->b1(); } - Vector b2() const { return system_->b2(); } - std::pair Ab1() const { return system_->Ab1(*solver_.ordering()); } - std::pair Ab2() const { return system_->Ab2(*solver_.ordering()); } - - /** - * update estimate with pure delta config x - */ - void update(const Vector& x); - - /** - * update estimate with pre-conditioned delta config y - */ - void updatePreconditioned(const Vector& y); - - }; - -} - -#endif /* POSE2SLAMOPTIMIZER_H_ */ diff --git a/gtsam/slam/Simulated2DMeasurement.h b/gtsam/slam/Simulated2DMeasurement.h deleted file mode 100644 index b85351fc1..000000000 --- a/gtsam/slam/Simulated2DMeasurement.h +++ /dev/null @@ -1,29 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * Simulated2DMeasurement.h - * - * Re-created on Feb 22, 2010 for compatibility with MATLAB - * Author: Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - typedef simulated2D::Measurement Simulated2DMeasurement; - -} - diff --git a/gtsam/slam/Simulated2DOdometry.h b/gtsam/slam/Simulated2DOdometry.h deleted file mode 100644 index 498e23fcc..000000000 --- a/gtsam/slam/Simulated2DOdometry.h +++ /dev/null @@ -1,29 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * Simulated2DOdometry.h - * - * Re-created on Feb 22, 2010 for compatibility with MATLAB - * Author: Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - typedef simulated2D::Odometry Simulated2DOdometry; - -} - diff --git a/gtsam/slam/Simulated2DOrientedOdometry.h b/gtsam/slam/Simulated2DOrientedOdometry.h deleted file mode 100644 index dd5afac7b..000000000 --- a/gtsam/slam/Simulated2DOrientedOdometry.h +++ /dev/null @@ -1,29 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * Simulated2DOrientedOdometry.h - * - * Re-created on Feb 22, 2010 for compatibility with MATLAB - * Author: Kai Ni - */ - -#pragma once - -#include -#include - -namespace gtsam { - - typedef simulated2DOriented::Odometry Simulated2DOrientedOdometry; - -} - diff --git a/gtsam/slam/Simulated2DOrientedPosePrior.h b/gtsam/slam/Simulated2DOrientedPosePrior.h deleted file mode 100644 index be8407b6c..000000000 --- a/gtsam/slam/Simulated2DOrientedPosePrior.h +++ /dev/null @@ -1,30 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * Simulated2DOrientedPosePrior.h - * - * Re-created on Feb 22, 2010 for compatibility with MATLAB - * Author: Kai Ni - */ - -#pragma once - -#include -#include - -namespace gtsam { - - /** Create a prior on a pose Point2 with key 'x1' etc... */ - typedef simulated2DOriented::GenericPosePrior Simulated2DOrientedPosePrior; - -} - diff --git a/gtsam/slam/Simulated2DOrientedValues.h b/gtsam/slam/Simulated2DOrientedValues.h deleted file mode 100644 index 20bd7bffa..000000000 --- a/gtsam/slam/Simulated2DOrientedValues.h +++ /dev/null @@ -1,60 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * Simulated2DValues.h - * - * Re-created on Feb 22, 2010 for compatibility with MATLAB - * Author: Frank Dellaert - */ - -#pragma once - -#include - -namespace gtsam { - - class Simulated2DOrientedValues: public simulated2DOriented::Values { - public: - typedef boost::shared_ptr sharedPoint; - typedef boost::shared_ptr sharedPose; - - Simulated2DOrientedValues() { - } - - void insertPose(const simulated2DOriented::PoseKey& i, const Pose2& p) { - insert(i, p); - } - - void insertPoint(const simulated2DOriented::PointKey& j, const Point2& p) { - insert(j, p); - } - - int nrPoses() const { - return this->first_.size(); - } - - int nrPoints() const { - return this->second_.size(); - } - - sharedPose pose(const simulated2DOriented::PoseKey& i) { - return sharedPose(new Pose2((*this)[i])); - } - - sharedPoint point(const simulated2DOriented::PointKey& j) { - return sharedPoint(new Point2((*this)[j])); - } - - }; - -} // namespace gtsam - diff --git a/gtsam/slam/Simulated2DPointPrior.h b/gtsam/slam/Simulated2DPointPrior.h deleted file mode 100644 index 3610f9c1b..000000000 --- a/gtsam/slam/Simulated2DPointPrior.h +++ /dev/null @@ -1,30 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * Simulated2DPointPrior.h - * - * Re-created on Feb 22, 2010 for compatibility with MATLAB - * Author: Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - /** Create a prior on a landmark Point2 with key 'l1' etc... */ - typedef simulated2D::GenericPrior Simulated2DPointPrior; - -} - diff --git a/gtsam/slam/Simulated2DPosePrior.h b/gtsam/slam/Simulated2DPosePrior.h deleted file mode 100644 index a0b123243..000000000 --- a/gtsam/slam/Simulated2DPosePrior.h +++ /dev/null @@ -1,30 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * Simulated2DPosePrior.h - * - * Re-created on Feb 22, 2010 for compatibility with MATLAB - * Author: Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - /** Create a prior on a pose Point2 with key 'x1' etc... */ - typedef simulated2D::GenericPrior Simulated2DPosePrior; - -} - diff --git a/gtsam/slam/Simulated2DValues.h b/gtsam/slam/Simulated2DValues.h deleted file mode 100644 index 1d11cd4a4..000000000 --- a/gtsam/slam/Simulated2DValues.h +++ /dev/null @@ -1,59 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * Simulated2DValues.h - * - * Re-created on Feb 22, 2010 for compatibility with MATLAB - * Author: Frank Dellaert - */ - -#pragma once - -#include - -namespace gtsam { - - class Simulated2DValues: public simulated2D::Values { - public: - typedef boost::shared_ptr sharedPoint; - - Simulated2DValues() { - } - - void insertPose(const simulated2D::PoseKey& i, const Point2& p) { - insert(i, p); - } - - void insertPoint(const simulated2D::PointKey& j, const Point2& p) { - insert(j, p); - } - - int nrPoses() const { - return this->first_.size(); - } - - int nrPoints() const { - return this->second_.size(); - } - - sharedPoint pose(const simulated2D::PoseKey& i) { - return sharedPoint(new Point2((*this)[i])); - } - - sharedPoint point(const simulated2D::PointKey& j) { - return sharedPoint(new Point2((*this)[j])); - } - - }; - -} // namespace gtsam - diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index 4ff226bf3..e7cac8552 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -63,7 +63,7 @@ namespace gtsam { // Optimizer typedef NonlinearOptimizer OptimizerSequential; - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer Optimizer; } // pose2SLAM diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h index 8f374f904..17c2d21a7 100644 --- a/gtsam/slam/simulated2D.h +++ b/gtsam/slam/simulated2D.h @@ -34,7 +34,40 @@ namespace gtsam { typedef TypedSymbol PointKey; typedef LieValues PoseValues; typedef LieValues PointValues; - typedef TupleValues2 Values; + +// typedef TupleValues2 Values; + class Values: public TupleValues2 { + public: + typedef TupleValues2 Base; + typedef boost::shared_ptr sharedPoint; + + Values() {} + Values(const Base& base) : Base(base) {} + + void insertPose(const simulated2D::PoseKey& i, const Point2& p) { + insert(i, p); + } + + void insertPoint(const simulated2D::PointKey& j, const Point2& p) { + insert(j, p); + } + + int nrPoses() const { + return this->first_.size(); + } + + int nrPoints() const { + return this->second_.size(); + } + + sharedPoint pose(const simulated2D::PoseKey& i) { + return sharedPoint(new Point2((*this)[i])); + } + + sharedPoint point(const simulated2D::PointKey& j) { + return sharedPoint(new Point2((*this)[j])); + } + }; /** * Prior on a single pose, and optional derivative version diff --git a/gtsam/slam/tests/testPose2Factor.cpp b/gtsam/slam/tests/testPose2Factor.cpp deleted file mode 100644 index 282fb4ba9..000000000 --- a/gtsam/slam/tests/testPose2Factor.cpp +++ /dev/null @@ -1,155 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testPose2Factor.cpp - * @brief Unit tests for Pose2Factor Class - * @authors Frank Dellaert, Viorela Ila - **/ - -#include - -#define GTSAM_MAGIC_KEY - -#include -#include -#include - -using namespace std; -using namespace gtsam; - -// Common measurement covariance -static double sx=0.5, sy=0.5,st=0.1; -static noiseModel::Gaussian::shared_ptr covariance( - noiseModel::Gaussian::Covariance(Matrix_(3, 3, - sx*sx, 0.0, 0.0, - 0.0, sy*sy, 0.0, - 0.0, 0.0, st*st - ))); - -/* ************************************************************************* */ -// Very simple test establishing Ax-b \approx z-h(x) -TEST( Pose2Factor, error ) -{ - // Choose a linearization point - Pose2 p1; // robot at origin - Pose2 p2(1, 0, 0); // robot at (1,0) - Pose2Values x0; - x0.insert(1, p1); - x0.insert(2, p2); - - // Create factor - Pose2 z = p1.between(p2); - Pose2Factor factor(1, 2, z, covariance); - - // Actual linearization - Ordering ordering(*x0.orderingArbitrary()); - boost::shared_ptr linear = - boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); - - // Check error at x0, i.e. delta = zero ! - VectorValues delta(x0.dims(ordering)); - delta[ordering["x1"]] = zero(3); - delta[ordering["x2"]] = zero(3); - Vector error_at_zero = Vector_(3,0.0,0.0,0.0); - CHECK(assert_equal(error_at_zero,factor.unwhitenedError(x0))); - CHECK(assert_equal(-error_at_zero, linear->error_vector(delta))); - - // Check error after increasing p2 - VectorValues plus = delta; - plus[ordering["x2"]] = Vector_(3, 0.1, 0.0, 0.0); - Pose2Values x1 = x0.expmap(plus, ordering); - Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 ! - CHECK(assert_equal(error_at_plus,factor.whitenedError(x1))); - CHECK(assert_equal(error_at_plus,linear->error_vector(plus))); -} - -/* ************************************************************************* */ -// common Pose2Factor for tests below -static Pose2 measured(2,2,M_PI_2); -static Pose2Factor factor(1,2,measured, covariance); - -/* ************************************************************************* */ -TEST( Pose2Factor, rhs ) -{ - // Choose a linearization point - Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) - Pose2 p2(-1,4.1,M_PI); // robot at (-1,4.1) looking at negative (ground truth is at -1,4) - Pose2Values x0; - x0.insert(1,p1); - x0.insert(2,p2); - - // Actual linearization - Ordering ordering(*x0.orderingArbitrary()); - boost::shared_ptr linear = - boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); - - // Check RHS - Pose2 hx0 = p1.between(p2); - CHECK(assert_equal(Pose2(2.1, 2.1, M_PI_2),hx0)); - Vector expected_b = Vector_(3, -0.1/sx, 0.1/sy, 0.0); - CHECK(assert_equal(expected_b,-factor.whitenedError(x0))); - CHECK(assert_equal(expected_b,linear->getb())); -} - -/* ************************************************************************* */ -// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector -// Hence i.e., b = approximates z-h(x0) = error_vector(x0) -LieVector h(const Pose2& p1,const Pose2& p2) { - return LieVector(covariance->whiten(factor.evaluateError(p1,p2))); -} - -/* ************************************************************************* */ -TEST( Pose2Factor, linearize ) -{ - // Choose a linearization point at ground truth - Pose2 p1(1,2,M_PI_2); - Pose2 p2(-1,4,M_PI); - Pose2Values x0; - x0.insert(1,p1); - x0.insert(2,p2); - - // expected linearization - Matrix expectedH1 = covariance->Whiten(Matrix_(3,3, - 0.0,-1.0,-2.0, - 1.0, 0.0,-2.0, - 0.0, 0.0,-1.0 - )); - Matrix expectedH2 = covariance->Whiten(Matrix_(3,3, - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0 - )); - Vector expected_b = Vector_(3, 0.0, 0.0, 0.0); - - // expected linear factor - Ordering ordering(*x0.orderingArbitrary()); - SharedDiagonal probModel1 = noiseModel::Unit::Create(3); - JacobianFactor expected(ordering["x1"], expectedH1, ordering["x2"], expectedH2, expected_b, probModel1); - - // Actual linearization - boost::shared_ptr actual = - boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); - CHECK(assert_equal(expected,*actual)); - - // Numerical do not work out because BetweenFactor is approximate ? - Matrix numericalH1 = numericalDerivative21(h, p1, p2); - CHECK(assert_equal(expectedH1,numericalH1)); - Matrix numericalH2 = numericalDerivative22(h, p1, p2); - CHECK(assert_equal(expectedH2,numericalH2)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testPose2Prior.cpp b/gtsam/slam/tests/testPose2Prior.cpp deleted file mode 100644 index 1a5be817a..000000000 --- a/gtsam/slam/tests/testPose2Prior.cpp +++ /dev/null @@ -1,103 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testPose2Prior.cpp - * @brief Unit tests for Pose2Prior Class - * @authors Frank Dellaert, Viorela Ila - **/ - -#include - -#define GTSAM_MAGIC_KEY - -#include -#include -#include - -using namespace std; -using namespace gtsam; - -// Common measurement covariance -static double sx=0.5, sy=0.5,st=0.1; -static SharedGaussian sigmas = sharedSigmas(Vector_(3,sx,sy,st)); - -/* ************************************************************************* */ -// Very simple test establishing Ax-b \approx z-h(x) -TEST( Pose2Prior, error ) -{ - // Choose a linearization point - Pose2 p1(1, 0, 0); // robot at (1,0) - Pose2Values x0; - x0.insert(1, p1); - - // Create factor - Pose2Prior factor(1, p1, sigmas); - - // Actual linearization - Ordering ordering(*x0.orderingArbitrary()); - boost::shared_ptr linear = - boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); - - // Check error at x0, i.e. delta = zero ! - VectorValues delta(x0.dims(ordering)); - delta.makeZero(); - delta[ordering["x1"]] = zero(3); - Vector error_at_zero = Vector_(3,0.0,0.0,0.0); - CHECK(assert_equal(error_at_zero,factor.whitenedError(x0))); - CHECK(assert_equal(-error_at_zero,linear->error_vector(delta))); - - // Check error after increasing p2 - VectorValues addition(x0.dims(ordering)); - addition.makeZero(); - addition[ordering["x1"]] = Vector_(3, 0.1, 0.0, 0.0); - VectorValues plus = delta + addition; - Pose2Values x1 = x0.expmap(plus, ordering); - Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 ! - CHECK(assert_equal(error_at_plus,factor.whitenedError(x1))); - CHECK(assert_equal(error_at_plus,linear->error_vector(plus))); -} - -/* ************************************************************************* */ -// common Pose2Prior for tests below -static Pose2 prior(2,2,M_PI_2); -static Pose2Prior factor(1,prior, sigmas); - -/* ************************************************************************* */ -// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector -// Hence i.e., b = approximates z-h(x0) = error_vector(x0) -LieVector h(const Pose2& p1) { - return LieVector(sigmas->whiten(factor.evaluateError(p1))); -} - -/* ************************************************************************* */ -TEST( Pose2Prior, linearize ) -{ - // Choose a linearization point at ground truth - Pose2Values x0; - x0.insert(1,prior); - - // Actual linearization - Ordering ordering(*x0.orderingArbitrary()); - boost::shared_ptr actual = - boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); - - // Test with numerical derivative - Matrix numericalH = numericalDerivative11(h, prior); - CHECK(assert_equal(numericalH,actual->getA(actual->find(ordering["x1"])))); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index e3c0b7ffc..29d0a2b0f 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -24,10 +24,10 @@ using namespace boost::assign; #define GTSAM_MAGIC_KEY +#include #include #include #include -//#include using namespace std; using namespace gtsam; @@ -291,6 +291,224 @@ TEST(Pose2Graph, optimize2) { // LONGS_EQUAL(1, C.size()); //} +using namespace pose2SLAM; + +/* ************************************************************************* */ +TEST( Pose2Values, pose2Circle ) +{ + // expected is 4 poses tangent to circle with radius 1m + Pose2Values expected; + expected.insert(0, Pose2( 1, 0, M_PI_2)); + expected.insert(1, Pose2( 0, 1, - M_PI )); + expected.insert(2, Pose2(-1, 0, - M_PI_2)); + expected.insert(3, Pose2( 0, -1, 0 )); + + Pose2Values actual = pose2SLAM::circle(4,1.0); + CHECK(assert_equal(expected,actual)); +} + +/* ************************************************************************* */ +TEST( Pose2Values, expmap ) +{ + // expected is circle shifted to right + Pose2Values expected; + expected.insert(0, Pose2( 1.1, 0, M_PI_2)); + expected.insert(1, Pose2( 0.1, 1, - M_PI )); + expected.insert(2, Pose2(-0.9, 0, - M_PI_2)); + expected.insert(3, Pose2( 0.1, -1, 0 )); + + // Note expmap coordinates are in local coordinates, so shifting to right requires thought !!! + Pose2Values circle(pose2SLAM::circle(4,1.0)); + Ordering ordering(*circle.orderingArbitrary()); + VectorValues delta(circle.dims(ordering)); + delta[ordering[Key(0)]] = Vector_(3, 0.0,-0.1,0.0); + delta[ordering[Key(1)]] = Vector_(3, -0.1,0.0,0.0); + delta[ordering[Key(2)]] = Vector_(3, 0.0,0.1,0.0); + delta[ordering[Key(3)]] = Vector_(3, 0.1,0.0,0.0); + Pose2Values actual = circle.expmap(delta, ordering); + CHECK(assert_equal(expected,actual)); +} + +// Common measurement covariance +static SharedGaussian sigmas = sharedSigmas(Vector_(3,sx,sy,st)); + +/* ************************************************************************* */ +// Very simple test establishing Ax-b \approx z-h(x) +TEST( Pose2Prior, error ) +{ + // Choose a linearization point + Pose2 p1(1, 0, 0); // robot at (1,0) + Pose2Values x0; + x0.insert(1, p1); + + // Create factor + Pose2Prior factor(1, p1, sigmas); + + // Actual linearization + Ordering ordering(*x0.orderingArbitrary()); + boost::shared_ptr linear = + boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); + + // Check error at x0, i.e. delta = zero ! + VectorValues delta(x0.dims(ordering)); + delta.makeZero(); + delta[ordering["x1"]] = zero(3); + Vector error_at_zero = Vector_(3,0.0,0.0,0.0); + CHECK(assert_equal(error_at_zero,factor.whitenedError(x0))); + CHECK(assert_equal(-error_at_zero,linear->error_vector(delta))); + + // Check error after increasing p2 + VectorValues addition(x0.dims(ordering)); + addition.makeZero(); + addition[ordering["x1"]] = Vector_(3, 0.1, 0.0, 0.0); + VectorValues plus = delta + addition; + Pose2Values x1 = x0.expmap(plus, ordering); + Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 ! + CHECK(assert_equal(error_at_plus,factor.whitenedError(x1))); + CHECK(assert_equal(error_at_plus,linear->error_vector(plus))); +} + +/* ************************************************************************* */ +// common Pose2Prior for tests below +static gtsam::Pose2 priorVal(2,2,M_PI_2); +static Pose2Prior priorFactor(1,priorVal, sigmas); + +/* ************************************************************************* */ +// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector +// Hence i.e., b = approximates z-h(x0) = error_vector(x0) +LieVector hprior(const Pose2& p1) { + return LieVector(sigmas->whiten(priorFactor.evaluateError(p1))); +} + +/* ************************************************************************* */ +TEST( Pose2Prior, linearize ) +{ + // Choose a linearization point at ground truth + Pose2Values x0; + x0.insert(1,priorVal); + + // Actual linearization + Ordering ordering(*x0.orderingArbitrary()); + boost::shared_ptr actual = + boost::dynamic_pointer_cast(priorFactor.linearize(x0, ordering)); + + // Test with numerical derivative + Matrix numericalH = numericalDerivative11(hprior, priorVal); + CHECK(assert_equal(numericalH,actual->getA(actual->find(ordering["x1"])))); +} + +/* ************************************************************************* */ +// Very simple test establishing Ax-b \approx z-h(x) +TEST( Pose2Factor, error ) +{ + // Choose a linearization point + Pose2 p1; // robot at origin + Pose2 p2(1, 0, 0); // robot at (1,0) + Pose2Values x0; + x0.insert(1, p1); + x0.insert(2, p2); + + // Create factor + Pose2 z = p1.between(p2); + Pose2Factor factor(1, 2, z, covariance); + + // Actual linearization + Ordering ordering(*x0.orderingArbitrary()); + boost::shared_ptr linear = + boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); + + // Check error at x0, i.e. delta = zero ! + VectorValues delta(x0.dims(ordering)); + delta[ordering["x1"]] = zero(3); + delta[ordering["x2"]] = zero(3); + Vector error_at_zero = Vector_(3,0.0,0.0,0.0); + CHECK(assert_equal(error_at_zero,factor.unwhitenedError(x0))); + CHECK(assert_equal(-error_at_zero, linear->error_vector(delta))); + + // Check error after increasing p2 + VectorValues plus = delta; + plus[ordering["x2"]] = Vector_(3, 0.1, 0.0, 0.0); + Pose2Values x1 = x0.expmap(plus, ordering); + Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 ! + CHECK(assert_equal(error_at_plus,factor.whitenedError(x1))); + CHECK(assert_equal(error_at_plus,linear->error_vector(plus))); +} + +/* ************************************************************************* */ +// common Pose2Factor for tests below +static Pose2 measured(2,2,M_PI_2); +static Pose2Factor factor(1,2,measured, covariance); + +/* ************************************************************************* */ +TEST( Pose2Factor, rhs ) +{ + // Choose a linearization point + Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) + Pose2 p2(-1,4.1,M_PI); // robot at (-1,4.1) looking at negative (ground truth is at -1,4) + Pose2Values x0; + x0.insert(1,p1); + x0.insert(2,p2); + + // Actual linearization + Ordering ordering(*x0.orderingArbitrary()); + boost::shared_ptr linear = + boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); + + // Check RHS + Pose2 hx0 = p1.between(p2); + CHECK(assert_equal(Pose2(2.1, 2.1, M_PI_2),hx0)); + Vector expected_b = Vector_(3, -0.1/sx, 0.1/sy, 0.0); + CHECK(assert_equal(expected_b,-factor.whitenedError(x0))); + CHECK(assert_equal(expected_b,linear->getb())); +} + +/* ************************************************************************* */ +// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector +// Hence i.e., b = approximates z-h(x0) = error_vector(x0) +LieVector h(const Pose2& p1,const Pose2& p2) { + return LieVector(covariance->whiten(factor.evaluateError(p1,p2))); +} + +/* ************************************************************************* */ +TEST( Pose2Factor, linearize ) +{ + // Choose a linearization point at ground truth + Pose2 p1(1,2,M_PI_2); + Pose2 p2(-1,4,M_PI); + Pose2Values x0; + x0.insert(1,p1); + x0.insert(2,p2); + + // expected linearization + Matrix expectedH1 = covariance->Whiten(Matrix_(3,3, + 0.0,-1.0,-2.0, + 1.0, 0.0,-2.0, + 0.0, 0.0,-1.0 + )); + Matrix expectedH2 = covariance->Whiten(Matrix_(3,3, + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 + )); + Vector expected_b = Vector_(3, 0.0, 0.0, 0.0); + + // expected linear factor + Ordering ordering(*x0.orderingArbitrary()); + SharedDiagonal probModel1 = noiseModel::Unit::Create(3); + JacobianFactor expected(ordering["x1"], expectedH1, ordering["x2"], expectedH2, expected_b, probModel1); + + // Actual linearization + boost::shared_ptr actual = + boost::dynamic_pointer_cast(factor.linearize(x0, ordering)); + CHECK(assert_equal(expected,*actual)); + + // Numerical do not work out because BetweenFactor is approximate ? + Matrix numericalH1 = numericalDerivative21(h, p1, p2); + CHECK(assert_equal(expectedH1,numericalH1)); + Matrix numericalH2 = numericalDerivative22(h, p1, p2); + CHECK(assert_equal(expectedH2,numericalH2)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/tests/testPose2SLAMwSPCG.cpp b/gtsam/slam/tests/testPose2SLAMwSPCG.cpp new file mode 100644 index 000000000..339f3f578 --- /dev/null +++ b/gtsam/slam/tests/testPose2SLAMwSPCG.cpp @@ -0,0 +1,72 @@ +/** + * @file testPose2SLAMwSPCG + * @author Alex Cunningham + */ + +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace pose2SLAM; + +const double tol = 1e-5; + +/* ************************************************************************* */ +TEST(testPose2SLAMwSPCG, example1) { + + /* generate synthetic data */ + const SharedGaussian sigma(noiseModel::Unit::Create(0.1)); + Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9); + + // create a 3 by 3 grid + // x3 x6 x9 + // x2 x5 x8 + // x1 x4 x7 + Graph graph; + graph.addConstraint(x1,x2,Pose2(0,2,0),sigma) ; + graph.addConstraint(x2,x3,Pose2(0,2,0),sigma) ; + graph.addConstraint(x4,x5,Pose2(0,2,0),sigma) ; + graph.addConstraint(x5,x6,Pose2(0,2,0),sigma) ; + graph.addConstraint(x7,x8,Pose2(0,2,0),sigma) ; + graph.addConstraint(x8,x9,Pose2(0,2,0),sigma) ; + graph.addConstraint(x1,x4,Pose2(2,0,0),sigma) ; + graph.addConstraint(x4,x7,Pose2(2,0,0),sigma) ; + graph.addConstraint(x2,x5,Pose2(2,0,0),sigma) ; + graph.addConstraint(x5,x8,Pose2(2,0,0),sigma) ; + graph.addConstraint(x3,x6,Pose2(2,0,0),sigma) ; + graph.addConstraint(x6,x9,Pose2(2,0,0),sigma) ; + graph.addPrior(x1, Pose2(0,0,0), sigma) ; + + Values initial; + initial.insert(x1, Pose2( 0, 0, 0)); + initial.insert(x2, Pose2( 0, 2.1, 0.01)); + initial.insert(x3, Pose2( 0, 3.9,-0.01)); + initial.insert(x4, Pose2(2.1,-0.1, 0)); + initial.insert(x5, Pose2(1.9, 2.1, 0.02)); + initial.insert(x6, Pose2(2.0, 3.9,-0.02)); + initial.insert(x7, Pose2(4.0, 0.1, 0.03 )); + initial.insert(x8, Pose2(3.9, 2.1, 0.01)); + initial.insert(x9, Pose2(4.1, 3.9,-0.01)); + + Values expected; + expected.insert(x1, Pose2(0.0, 0.0, 0.0)); + expected.insert(x2, Pose2(0.0, 2.0, 0.0)); + expected.insert(x3, Pose2(0.0, 4.0, 0.0)); + expected.insert(x4, Pose2(2.0, 0.0, 0.0)); + expected.insert(x5, Pose2(2.0, 2.0, 0.0)); + expected.insert(x6, Pose2(2.0, 4.0, 0.0)); + expected.insert(x7, Pose2(4.0, 0.0, 0.0 )); + expected.insert(x8, Pose2(4.0, 2.0, 0.0)); + expected.insert(x9, Pose2(4.0, 4.0, 0.0)); + + Values actual = optimizeSPCG(graph, initial); + + EXPECT(assert_equal(expected, actual, tol)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testPose2Values.cpp b/gtsam/slam/tests/testPose2Values.cpp deleted file mode 100644 index 5701eeb02..000000000 --- a/gtsam/slam/tests/testPose2Values.cpp +++ /dev/null @@ -1,67 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testPose2Values.cpp - * @authors Frank Dellaert - **/ - -#include - -#include -#include - -using namespace std; -using namespace gtsam; -using namespace gtsam::pose2SLAM; - -/* ************************************************************************* */ -TEST( Pose2Values, pose2Circle ) -{ - // expected is 4 poses tangent to circle with radius 1m - Pose2Values expected; - expected.insert(0, Pose2( 1, 0, M_PI_2)); - expected.insert(1, Pose2( 0, 1, - M_PI )); - expected.insert(2, Pose2(-1, 0, - M_PI_2)); - expected.insert(3, Pose2( 0, -1, 0 )); - - Pose2Values actual = pose2SLAM::circle(4,1.0); - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( Pose2Values, expmap ) -{ - // expected is circle shifted to right - Pose2Values expected; - expected.insert(0, Pose2( 1.1, 0, M_PI_2)); - expected.insert(1, Pose2( 0.1, 1, - M_PI )); - expected.insert(2, Pose2(-0.9, 0, - M_PI_2)); - expected.insert(3, Pose2( 0.1, -1, 0 )); - - // Note expmap coordinates are in local coordinates, so shifting to right requires thought !!! - Pose2Values circle(pose2SLAM::circle(4,1.0)); - Ordering ordering(*circle.orderingArbitrary()); - VectorValues delta(circle.dims(ordering)); - delta[ordering[Key(0)]] = Vector_(3, 0.0,-0.1,0.0); - delta[ordering[Key(1)]] = Vector_(3, -0.1,0.0,0.0); - delta[ordering[Key(2)]] = Vector_(3, 0.0,0.1,0.0); - delta[ordering[Key(3)]] = Vector_(3, 0.1,0.0,0.0); - Pose2Values actual = circle.expmap(delta, ordering); - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testPose3Factor.cpp b/gtsam/slam/tests/testPose3Factor.cpp deleted file mode 100644 index c8502f302..000000000 --- a/gtsam/slam/tests/testPose3Factor.cpp +++ /dev/null @@ -1,56 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testPose3Pose.cpp - * @brief Unit tests for Pose3Factor Class - * @authors Frank Dellaert - **/ - -#include -#include -using namespace boost::assign; - -#include -#include - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ -TEST( Pose3Factor, error ) -{ - // Create example - Pose3 t1; // origin - Pose3 t2(Rot3::rodriguez(0.1,0.2,0.3),Point3(0,1,0)); - Pose3 z(Rot3::rodriguez(0.2,0.2,0.3),Point3(0,1.1,0));; - - // Create factor - SharedGaussian I6(noiseModel::Unit::Create(6)); - Pose3Factor factor(1,2, z, I6); - - // Create config - Pose3Values x; - x.insert(1,t1); - x.insert(2,t2); - - // Get error h(x)-z -> logmap(z,h(x)) = logmap(z,between(t1,t2)) - Vector actual = factor.unwhitenedError(x); - Vector expected = z.logmap(t1.between(t2)); - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index e07f632b6..81f83a2d7 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -121,6 +121,29 @@ TEST(Pose3Graph, partial_prior_height) { EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); } +/* ************************************************************************* */ +TEST( Pose3Factor, error ) +{ + // Create example + Pose3 t1; // origin + Pose3 t2(Rot3::rodriguez(0.1,0.2,0.3),Point3(0,1,0)); + Pose3 z(Rot3::rodriguez(0.2,0.2,0.3),Point3(0,1.1,0));; + + // Create factor + SharedGaussian I6(noiseModel::Unit::Create(6)); + Pose3Factor factor(1,2, z, I6); + + // Create config + Pose3Values x; + x.insert(1,t1); + x.insert(2,t2); + + // Get error h(x)-z -> logmap(z,h(x)) = logmap(z,between(t1,t2)) + Vector actual = factor.unwhitenedError(x); + Vector expected = z.logmap(t1.between(t2)); + CHECK(assert_equal(expected,actual)); +} + /* ************************************************************************* */ TEST(Pose3Graph, partial_prior_xy) { typedef PartialPriorFactor Partial; @@ -140,7 +163,6 @@ TEST(Pose3Graph, partial_prior_xy) { EXPECT(assert_equal(expA, actA)); Graph graph; -// graph.add(priorXY); // FAIL - on compile, can't initialize a reference? graph.push_back(Partial::shared_ptr(new Partial(priorXY))); Values values; @@ -151,6 +173,57 @@ TEST(Pose3Graph, partial_prior_xy) { EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); } +// The world coordinate system has z pointing up, y north, x east +// The vehicle has X forward, Y right, Z down +Rot3 R1(Point3( 0, 1, 0), Point3( 1, 0, 0), Point3(0, 0, -1)); +Rot3 R2(Point3(-1, 0, 0), Point3( 0, 1, 0), Point3(0, 0, -1)); +Rot3 R3(Point3( 0,-1, 0), Point3(-1, 0, 0), Point3(0, 0, -1)); +Rot3 R4(Point3( 1, 0, 0), Point3( 0,-1, 0), Point3(0, 0, -1)); + +/* ************************************************************************* */ +TEST( Pose3Values, pose3Circle ) +{ + // expected is 4 poses tangent to circle with radius 1m + Pose3Values expected; + expected.insert(0, Pose3(R1, Point3( 1, 0, 0))); + expected.insert(1, Pose3(R2, Point3( 0, 1, 0))); + expected.insert(2, Pose3(R3, Point3(-1, 0, 0))); + expected.insert(3, Pose3(R4, Point3( 0,-1, 0))); + + Pose3Values actual = pose3SLAM::circle(4,1.0); + CHECK(assert_equal(expected,actual)); +} + +/* ************************************************************************* */ +TEST( Pose3Values, expmap ) +{ + Pose3Values expected; +#ifdef CORRECT_POSE3_EXMAP + expected.insert(0, Pose3(R1, Point3( 1.0, 0.1, 0))); + expected.insert(1, Pose3(R2, Point3(-0.1, 1.0, 0))); + expected.insert(2, Pose3(R3, Point3(-1.0,-0.1, 0))); + expected.insert(3, Pose3(R4, Point3( 0.1,-1.0, 0))); +#else + // expected is circle shifted to East + expected.insert(0, Pose3(R1, Point3( 1.1, 0, 0))); + expected.insert(1, Pose3(R2, Point3( 0.1, 1, 0))); + expected.insert(2, Pose3(R3, Point3(-0.9, 0, 0))); + expected.insert(3, Pose3(R4, Point3( 0.1,-1, 0))); +#endif + + // Note expmap coordinates are in global coordinates with non-compose expmap + // so shifting to East requires little thought, different from with Pose2 !!! + + Ordering ordering(*expected.orderingArbitrary()); + VectorValues delta(expected.dims(ordering), Vector_(24, + 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)); + Pose3Values actual = pose3SLAM::circle(4,1.0).expmap(delta, ordering); + CHECK(assert_equal(expected,actual)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/tests/testPose3Values.cpp b/gtsam/slam/tests/testPose3Values.cpp deleted file mode 100644 index b44d432b0..000000000 --- a/gtsam/slam/tests/testPose3Values.cpp +++ /dev/null @@ -1,81 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testPose3Values.cpp - * @authors Frank Dellaert - **/ - -#include - -#include -#include - -using namespace std; -using namespace gtsam; - -// The world coordinate system has z pointing up, y north, x east -// The vehicle has X forward, Y right, Z down -Rot3 R1(Point3( 0, 1, 0), Point3( 1, 0, 0), Point3(0, 0, -1)); -Rot3 R2(Point3(-1, 0, 0), Point3( 0, 1, 0), Point3(0, 0, -1)); -Rot3 R3(Point3( 0,-1, 0), Point3(-1, 0, 0), Point3(0, 0, -1)); -Rot3 R4(Point3( 1, 0, 0), Point3( 0,-1, 0), Point3(0, 0, -1)); - -/* ************************************************************************* */ -TEST( Pose3Values, pose3Circle ) -{ - // expected is 4 poses tangent to circle with radius 1m - Pose3Values expected; - expected.insert(0, Pose3(R1, Point3( 1, 0, 0))); - expected.insert(1, Pose3(R2, Point3( 0, 1, 0))); - expected.insert(2, Pose3(R3, Point3(-1, 0, 0))); - expected.insert(3, Pose3(R4, Point3( 0,-1, 0))); - - Pose3Values actual = pose3SLAM::circle(4,1.0); - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( Pose3Values, expmap ) -{ - Pose3Values expected; -#ifdef CORRECT_POSE3_EXMAP - expected.insert(0, Pose3(R1, Point3( 1.0, 0.1, 0))); - expected.insert(1, Pose3(R2, Point3(-0.1, 1.0, 0))); - expected.insert(2, Pose3(R3, Point3(-1.0,-0.1, 0))); - expected.insert(3, Pose3(R4, Point3( 0.1,-1.0, 0))); -#else - // expected is circle shifted to East - expected.insert(0, Pose3(R1, Point3( 1.1, 0, 0))); - expected.insert(1, Pose3(R2, Point3( 0.1, 1, 0))); - expected.insert(2, Pose3(R3, Point3(-0.9, 0, 0))); - expected.insert(3, Pose3(R4, Point3( 0.1,-1, 0))); -#endif - - // Note expmap coordinates are in global coordinates with non-compose expmap - // so shifting to East requires little thought, different from with Pose2 !!! - - Ordering ordering(*expected.orderingArbitrary()); - VectorValues delta(expected.dims(ordering), Vector_(24, - 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)); - Pose3Values actual = pose3SLAM::circle(4,1.0).expmap(delta, ordering); - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testVSLAMFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp similarity index 93% rename from gtsam/slam/tests/testVSLAMFactor.cpp rename to gtsam/slam/tests/testProjectionFactor.cpp index 9dd849aa2..4efd252a0 100644 --- a/gtsam/slam/tests/testVSLAMFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -9,18 +9,18 @@ * -------------------------------------------------------------------------- */ -/********************************************************** - Written by Frank Dellaert, Nov 2009 - **********************************************************/ +/** + * @file testProjectionFactor.cpp + * @brief Unit tests for ProjectionFactor Class + * @authors Frank Dellaert + * @date Nov 2009 + */ #include #define GTSAM_MAGIC_KEY -#include #include -#include -#include using namespace std; using namespace gtsam; diff --git a/gtsam/slam/tests/testSimulated2D.cpp b/gtsam/slam/tests/testSimulated2D.cpp index 13cc150c5..0e26af1d3 100644 --- a/gtsam/slam/tests/testSimulated2D.cpp +++ b/gtsam/slam/tests/testSimulated2D.cpp @@ -21,7 +21,7 @@ #include #include -#include +//#include using namespace gtsam; using namespace std; @@ -30,7 +30,7 @@ using namespace simulated2D; /* ************************************************************************* */ TEST( simulated2D, Simulated2DValues ) { - Simulated2DValues actual; + Values actual; actual.insertPose(1,Point2(1,1)); actual.insertPoint(2,Point2(2,2)); CHECK(assert_equal(actual,actual,1e-9)); diff --git a/gtsam/slam/tests/testSimulated2DOriented.cpp b/gtsam/slam/tests/testSimulated2DOriented.cpp index 6eabae0ea..ac41e77e5 100644 --- a/gtsam/slam/tests/testSimulated2DOriented.cpp +++ b/gtsam/slam/tests/testSimulated2DOriented.cpp @@ -21,9 +21,9 @@ #include #include -#include +//#include +#include #include -#include using namespace gtsam; using namespace std; @@ -32,7 +32,7 @@ using namespace simulated2DOriented; /* ************************************************************************* */ TEST( simulated2DOriented, Simulated2DValues ) { - Simulated2DValues actual; + simulated2D::Values actual; actual.insertPose(1,Point2(1,1)); actual.insertPoint(2,Point2(2,2)); CHECK(assert_equal(actual,actual,1e-9)); @@ -65,7 +65,7 @@ TEST( simulated2DOriented, constructor ) { Pose2 measurement(0.2, 0.3, 0.1); SharedDiagonal model(Vector_(3, 1., 1., 1.)); - Simulated2DOrientedOdometry factor(measurement, model, PoseKey(1), PoseKey(2)); + Odometry factor(measurement, model, PoseKey(1), PoseKey(2)); Values config; config.insert(PoseKey(1), Pose2(1., 0., 0.2)); diff --git a/gtsam/slam/tests/testVSLAMGraph.cpp b/gtsam/slam/tests/testVSLAM.cpp similarity index 87% rename from gtsam/slam/tests/testVSLAMGraph.cpp rename to gtsam/slam/tests/testVSLAM.cpp index fb1560b04..e1fb3fa0e 100644 --- a/gtsam/slam/tests/testVSLAMGraph.cpp +++ b/gtsam/slam/tests/testVSLAM.cpp @@ -189,6 +189,31 @@ TEST( Graph, CHECK_ORDERING) CHECK(assert_equal(*initialEstimate,*(afterOneIteration.values()))); } +/* ************************************************************************* */ +TEST( Values, update_with_large_delta) { + // this test ensures that if the update for delta is larger than + // the size of the config, it only updates existing variables + Values init; + init.insert(1, Pose3()); + init.insert(1, Point3(1.0, 2.0, 3.0)); + + Values expected; + expected.insert(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); + expected.insert(1, Point3(1.1, 2.1, 3.1)); + + Ordering largeOrdering; + Values largeValues = init; + largeValues.insert(2, Pose3()); + largeOrdering += "x1","l1","x2"; + VectorValues delta(largeValues.dims(largeOrdering)); + delta[largeOrdering["x1"]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1); + delta[largeOrdering["l1"]] = Vector_(3, 0.1, 0.1, 0.1); + delta[largeOrdering["x2"]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1); + Values actual = init.expmap(delta, largeOrdering); + + CHECK(assert_equal(expected,actual)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testVSLAMValues.cpp b/gtsam/slam/tests/testVSLAMValues.cpp deleted file mode 100644 index 5b75e70ce..000000000 --- a/gtsam/slam/tests/testVSLAMValues.cpp +++ /dev/null @@ -1,56 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * @file testValues.cpp - * @brief Tests for the Visual SLAM values structure class - * @author Alex Cunningham - */ - -#include - -#define GTSAM_MAGIC_KEY - -#include -#include - -using namespace std; -using namespace gtsam; -using namespace gtsam::visualSLAM; - -/* ************************************************************************* */ -TEST( Values, update_with_large_delta) { - // this test ensures that if the update for delta is larger than - // the size of the config, it only updates existing variables - Values init; - init.insert(1, Pose3()); - init.insert(1, Point3(1.0, 2.0, 3.0)); - - Values expected; - expected.insert(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); - expected.insert(1, Point3(1.1, 2.1, 3.1)); - - Ordering largeOrdering; - Values largeValues = init; - largeValues.insert(2, Pose3()); - largeOrdering += "x1","l1","x2"; - VectorValues delta(largeValues.dims(largeOrdering)); - delta[largeOrdering["x1"]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1); - delta[largeOrdering["l1"]] = Vector_(3, 0.1, 0.1, 0.1); - delta[largeOrdering["x2"]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1); - Values actual = init.expmap(delta, largeOrdering); - - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/tests/Makefile.am b/tests/Makefile.am index 979686ce5..0ba01a619 100644 --- a/tests/Makefile.am +++ b/tests/Makefile.am @@ -8,12 +8,10 @@ AUTOMAKE_OPTIONS = nostdinc check_PROGRAMS = -#check_PROGRAMS = testBayesNetPreconditioner testSubgraphPreconditioner check_PROGRAMS += testGaussianBayesNet testGaussianFactor testGaussianFactorGraph check_PROGRAMS += testGaussianISAM check_PROGRAMS += testGraph check_PROGRAMS += testInference -#check_PROGRAMS += testIterative check_PROGRAMS += testGaussianJunctionTree check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorGraph check_PROGRAMS += testNonlinearOptimizer diff --git a/tests/testBayesNetPreconditioner.cpp b/tests/testBayesNetPreconditioner.cpp deleted file mode 100644 index 274a100bd..000000000 --- a/tests/testBayesNetPreconditioner.cpp +++ /dev/null @@ -1,101 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testBayesNetConditioner.cpp - * @brief Unit tests for BayesNetConditioner - * @author Frank Dellaert - **/ - -#include -#include -#include - -#define GTSAM_MAGIC_KEY - -#include -#include -#include - -using namespace std; -using namespace gtsam; - -#include -using namespace example; - -/* ************************************************************************* */ -TEST( BayesNetPreconditioner, conjugateGradients ) -{ - // Build a planar graph - GaussianFactorGraph Ab; - VectorValues xtrue; - size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b - - // Get the spanning tree and corresponding ordering - GaussianFactorGraph Ab1, Ab2; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab); - - // Eliminate the spanning tree to build a prior - Ordering ordering = planarOrdering(N); - GaussianBayesNet Rc1 = Ab1.eliminate(ordering); // R1*x-c1 - VectorValues xbar = optimize(Rc1); // xbar = inv(R1)*c1 - - // Create BayesNet-preconditioned system - BayesNetPreconditioner system(Ab,Rc1); - - // Create zero config y0 and perturbed config y1 - VectorValues y0; - Vector z2 = zero(2); - BOOST_FOREACH(const Symbol& j, ordering) y0.insert(j,z2); - - VectorValues y1 = y0; - y1["x2003"] = Vector_(2, 1.0, -1.0); - VectorValues x1 = system.x(y1); - - // Check gradient for y0 - VectorValues expectedGradient0; - expectedGradient0.insert("x1001", Vector_(2,-1000.,-1000.)); - expectedGradient0.insert("x1002", Vector_(2, 0., -300.)); - expectedGradient0.insert("x1003", Vector_(2, 0., -300.)); - expectedGradient0.insert("x2001", Vector_(2, -100., 200.)); - expectedGradient0.insert("x2002", Vector_(2, -100., 0.)); - expectedGradient0.insert("x2003", Vector_(2, -100., -200.)); - expectedGradient0.insert("x3001", Vector_(2, -100., 100.)); - expectedGradient0.insert("x3002", Vector_(2, -100., 0.)); - expectedGradient0.insert("x3003", Vector_(2, -100., -100.)); - VectorValues actualGradient0 = system.gradient(y0); - CHECK(assert_equal(expectedGradient0,actualGradient0)); -#ifdef VECTORBTREE - CHECK(actualGradient0.cloned(y0)); -#endif - - // Solve using PCG - bool verbose = false; - double epsilon = 1e-6; // had to crank this down !!! - size_t maxIterations = 100; - VectorValues actual_y = gtsam::conjugateGradients(system, y1, verbose, epsilon, epsilon, maxIterations); - VectorValues actual_x = system.x(actual_y); - CHECK(assert_equal(xtrue,actual_x)); - - // Compare with non preconditioned version: - VectorValues actual2 = conjugateGradientDescent(Ab, x1, verbose, epsilon, - maxIterations); - CHECK(assert_equal(xtrue,actual2)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp deleted file mode 100644 index 33c580f06..000000000 --- a/tests/testIterative.cpp +++ /dev/null @@ -1,196 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testIterative.cpp - * @brief Unit tests for iterative methods - * @author Frank Dellaert - **/ - -#include // for operator += -using namespace boost::assign; - -#include - -// TODO: DANGEROUS, create shared pointers -#define GTSAM_MAGIC_GAUSSIAN 3 -#define GTSAM_MAGIC_KEY - -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace example; - -static bool verbose = false; - -/* ************************************************************************* */ -TEST( Iterative, steepestDescent ) -{ - // Expected solution - Ordering ord; - ord += "l1", "x1", "x2"; - GaussianFactorGraph fg = createGaussianFactorGraph(); - VectorValues expected = fg.optimize(ord); // destructive - - // Do gradient descent - GaussianFactorGraph fg2 = createGaussianFactorGraph(); - VectorValues zero = createZeroDelta(); - VectorValues actual = steepestDescent(fg2, zero, verbose); - CHECK(assert_equal(expected,actual,1e-2)); -} - -/* ************************************************************************* */ -TEST( Iterative, conjugateGradientDescent ) -{ - // Expected solution - Ordering ord; - ord += "l1", "x1", "x2"; - GaussianFactorGraph fg = createGaussianFactorGraph(); - VectorValues expected = fg.optimize(ord); // destructive - - // create graph and get matrices - GaussianFactorGraph fg2 = createGaussianFactorGraph(); - Matrix A; - Vector b; - Vector x0 = gtsam::zero(6); - boost::tie(A, b) = fg2.matrix(ord); - Vector expectedX = Vector_(6, -0.1, 0.1, -0.1, -0.1, 0.1, -0.2); - - // Do conjugate gradient descent, System version - System Ab(A, b); - Vector actualX = conjugateGradientDescent(Ab, x0, verbose); - CHECK(assert_equal(expectedX,actualX,1e-9)); - - // Do conjugate gradient descent, Matrix version - Vector actualX2 = conjugateGradientDescent(A, b, x0, verbose); - CHECK(assert_equal(expectedX,actualX2,1e-9)); - - // Do conjugate gradient descent on factor graph - VectorValues zero = createZeroDelta(); - VectorValues actual = conjugateGradientDescent(fg2, zero, verbose); - CHECK(assert_equal(expected,actual,1e-2)); - - // Test method - VectorValues actual2 = fg2.conjugateGradientDescent(zero, verbose); - CHECK(assert_equal(expected,actual2,1e-2)); -} - -/* ************************************************************************* */ -/*TEST( Iterative, conjugateGradientDescent_hard_constraint ) -{ - typedef Pose2Values::Key Key; - - Pose2Values config; - config.insert(1, Pose2(0.,0.,0.)); - config.insert(2, Pose2(1.5,0.,0.)); - - Pose2Graph graph; - Matrix cov = eye(3); - graph.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(2), Pose2(1.,0.,0.), cov))); - graph.addHardConstraint(1, config[1]); - - VectorValues zeros; - zeros.insert("x1",zero(3)); - zeros.insert("x2",zero(3)); - - GaussianFactorGraph fg = graph.linearize(config); - VectorValues actual = conjugateGradientDescent(fg, zeros, true, 1e-3, 1e-5, 10); - - VectorValues expected; - expected.insert("x1", zero(3)); - expected.insert("x2", Vector_(-0.5,0.,0.)); - CHECK(assert_equal(expected, actual)); -}*/ - -/* ************************************************************************* */ -TEST( Iterative, conjugateGradientDescent_soft_constraint ) -{ - Pose2Values config; - config.insert(1, Pose2(0.,0.,0.)); - config.insert(2, Pose2(1.5,0.,0.)); - - Pose2Graph graph; - graph.addPrior(1, Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); - graph.addConstraint(1,2, Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); - - VectorValues zeros; - zeros.insert("x1",zero(3)); - zeros.insert("x2",zero(3)); - - boost::shared_ptr fg = graph.linearize(config); - VectorValues actual = conjugateGradientDescent(*fg, zeros, verbose, 1e-3, 1e-5, 100); - - VectorValues expected; - expected.insert("x1", zero(3)); - expected.insert("x2", Vector_(3,-0.5,0.,0.)); - CHECK(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST( Iterative, subgraphPCG ) -{ - typedef Pose2Values::Key Key; - - Pose2Values theta_bar; - theta_bar.insert(1, Pose2(0.,0.,0.)); - theta_bar.insert(2, Pose2(1.5,0.,0.)); - - Pose2Graph graph; - graph.addPrior(1, Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); - graph.addConstraint(1,2, Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); - - // generate spanning tree and create ordering - PredecessorMap tree = graph.findMinimumSpanningTree(); - list keys = predecessorMap2Keys(tree); - list symbols; - symbols.resize(keys.size()); - std::transform(keys.begin(), keys.end(), symbols.begin(), key2symbol); - Ordering ordering(symbols); - - Key root = keys.back(); - Pose2Graph T, C; - graph.split(tree, T, C); - - // build the subgraph PCG system - boost::shared_ptr Ab1_ = T.linearize(theta_bar); - SubgraphPreconditioner::sharedFG Ab1 = T.linearize(theta_bar); - SubgraphPreconditioner::sharedFG Ab2 = C.linearize(theta_bar); - SubgraphPreconditioner::sharedBayesNet Rc1 = Ab1_->eliminate_(ordering); - SubgraphPreconditioner::sharedValues xbar = optimize_(*Rc1); - SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbar); - - VectorValues zeros = VectorValues::zero(*xbar); - - // Solve the subgraph PCG - VectorValues ybar = conjugateGradients (system, zeros, verbose, 1e-5, 1e-5, 100); - VectorValues actual = system.x(ybar); - - VectorValues expected; - expected.insert("x1", zero(3)); - expected.insert("x2", Vector_(3, -0.5, 0., 0.)); - CHECK(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp deleted file mode 100644 index 1b79eec93..000000000 --- a/tests/testSubgraphPreconditioner.cpp +++ /dev/null @@ -1,205 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testSubgraphConditioner.cpp - * @brief Unit tests for SubgraphPreconditioner - * @author Frank Dellaert - **/ - -#include -#include -#include -using namespace boost::assign; - -#include - -#define GTSAM_MAGIC_KEY - -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace example; - -/* ************************************************************************* */ -TEST( SubgraphPreconditioner, planarGraph ) -{ - // Check planar graph construction - GaussianFactorGraph A; - VectorValues xtrue; - boost::tie(A, xtrue) = planarGraph(3); - LONGS_EQUAL(13,A.size()); - LONGS_EQUAL(9,xtrue.size()); - DOUBLES_EQUAL(0,A.error(xtrue),1e-9); // check zero error for xtrue - - // Check canonical ordering - Ordering expected, ordering = planarOrdering(3); - expected += "x3003", "x2003", "x1003", "x3002", "x2002", "x1002", "x3001", "x2001", "x1001"; - CHECK(assert_equal(expected,ordering)); - - // Check that xtrue is optimal - GaussianBayesNet R1 = A.eliminate(ordering); - VectorValues actual = optimize(R1); - CHECK(assert_equal(xtrue,actual)); -} - -/* ************************************************************************* */ -TEST( SubgraphPreconditioner, splitOffPlanarTree ) -{ - // Build a planar graph - GaussianFactorGraph A; - VectorValues xtrue; - boost::tie(A, xtrue) = planarGraph(3); - - // Get the spanning tree and constraints, and check their sizes - GaussianFactorGraph T, C; - boost::tie(T, C) = splitOffPlanarTree(3, A); - LONGS_EQUAL(9,T.size()); - LONGS_EQUAL(4,C.size()); - - // Check that the tree can be solved to give the ground xtrue - Ordering ordering = planarOrdering(3); - GaussianBayesNet R1 = T.eliminate(ordering); - VectorValues xbar = optimize(R1); - CHECK(assert_equal(xtrue,xbar)); -} - -/* ************************************************************************* */ -TEST( SubgraphPreconditioner, system ) -{ - // Build a planar graph - GaussianFactorGraph Ab; - VectorValues xtrue; - size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b - - // Get the spanning tree and corresponding ordering - GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); - SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); - SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); - - // Eliminate the spanning tree to build a prior - Ordering ordering = planarOrdering(N); - SubgraphPreconditioner::sharedBayesNet Rc1 = Ab1_.eliminate_(ordering); // R1*x-c1 - SubgraphPreconditioner::sharedValues xbar = optimize_(*Rc1); // xbar = inv(R1)*c1 - - // Create Subgraph-preconditioned system - SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbar); - - // Create zero config - VectorValues zeros = VectorValues::zero(*xbar); - - // Set up y0 as all zeros - VectorValues y0 = zeros; - - // y1 = perturbed y0 - VectorValues y1 = zeros; - y1["x2003"] = Vector_(2, 1.0, -1.0); - - // Check corresponding x values - VectorValues expected_x1 = xtrue, x1 = system.x(y1); - expected_x1["x2003"] = Vector_(2, 2.01, 2.99); - expected_x1["x3003"] = Vector_(2, 3.01, 2.99); - CHECK(assert_equal(xtrue, system.x(y0))); - CHECK(assert_equal(expected_x1,system.x(y1))); - - // Check errors - DOUBLES_EQUAL(0,Ab.error(xtrue),1e-9); - DOUBLES_EQUAL(3,Ab.error(x1),1e-9); - DOUBLES_EQUAL(0,system.error(y0),1e-9); - DOUBLES_EQUAL(3,system.error(y1),1e-9); - - // Test gradient in x - VectorValues expected_gx0 = zeros; - VectorValues expected_gx1 = zeros; - CHECK(assert_equal(expected_gx0,Ab.gradient(xtrue))); - expected_gx1["x1003"] = Vector_(2, -100., 100.); - expected_gx1["x2002"] = Vector_(2, -100., 100.); - expected_gx1["x2003"] = Vector_(2, 200., -200.); - expected_gx1["x3002"] = Vector_(2, -100., 100.); - expected_gx1["x3003"] = Vector_(2, 100., -100.); - CHECK(assert_equal(expected_gx1,Ab.gradient(x1))); - - // Test gradient in y - VectorValues expected_gy0 = zeros; - VectorValues expected_gy1 = zeros; - expected_gy1["x1003"] = Vector_(2, 2., -2.); - expected_gy1["x2002"] = Vector_(2, -2., 2.); - expected_gy1["x2003"] = Vector_(2, 3., -3.); - expected_gy1["x3002"] = Vector_(2, -1., 1.); - expected_gy1["x3003"] = Vector_(2, 1., -1.); - CHECK(assert_equal(expected_gy0,system.gradient(y0))); - CHECK(assert_equal(expected_gy1,system.gradient(y1))); - - // Check it numerically for good measure - // TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1) -// Vector numerical_g1 = numericalGradient (error, y1, 0.001); -// Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2., -// 3., -3., 0., 0., -1., 1., 1., -1.); -// CHECK(assert_equal(expected_g1,numerical_g1)); -} - -/* ************************************************************************* */ -TEST( SubgraphPreconditioner, conjugateGradients ) -{ - // Build a planar graph - GaussianFactorGraph Ab; - VectorValues xtrue; - size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b - - // Get the spanning tree and corresponding ordering - GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); - SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); - SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); - - // Eliminate the spanning tree to build a prior - Ordering ordering = planarOrdering(N); - SubgraphPreconditioner::sharedBayesNet Rc1 = Ab1_.eliminate_(ordering); // R1*x-c1 - SubgraphPreconditioner::sharedValues xbar = optimize_(*Rc1); // xbar = inv(R1)*c1 - - // Create Subgraph-preconditioned system - SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbar); - - // Create zero config y0 and perturbed config y1 - VectorValues y0 = VectorValues::zero(*xbar); - - VectorValues y1 = y0; - y1["x2003"] = Vector_(2, 1.0, -1.0); - VectorValues x1 = system.x(y1); - - // Solve for the remaining constraints using PCG - bool verbose = false; - double epsilon = 1e-3; - size_t maxIterations = 100; - VectorValues actual = gtsam::conjugateGradients(system, y1, verbose, epsilon, epsilon, maxIterations); - CHECK(assert_equal(y0,actual)); - - // Compare with non preconditioned version: - VectorValues actual2 = conjugateGradientDescent(Ab, x1, verbose, epsilon, - epsilon, maxIterations); - CHECK(assert_equal(xtrue,actual2,1e-4)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */