Removed files full of broken code, removed simulated2D matlab files, condensed slam unit tests, added the only known test for SPCG in gtsam
Alex Cunningham
parent
b498d64554
commit
36f9ebae90
48
.cproject
48
.cproject
|
|
@ -676,6 +676,30 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="tests/testPose2SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>tests/testPose2SLAM.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="tests/testPose3SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>tests/testPose3SLAM.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="tests/testPose2SLAMwSPCG.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>tests/testPose2SLAMwSPCG.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -2447,6 +2471,30 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="tests/testPose2SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>tests/testPose2SLAM.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="tests/testPose3SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>tests/testPose3SLAM.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="tests/testPose2SLAMwSPCG.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>tests/testPose2SLAMwSPCG.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
|
|||
|
|
@ -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 <cmath>
|
||||
#include <algorithm>
|
||||
#include <stdexcept>
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
|
||||
#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<m; i++) {
|
||||
for(int j=0; j<n; j++)
|
||||
if (isnan(A[j*m+i]))
|
||||
throw std::invalid_argument(msg);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// check Inf in the input matrix
|
||||
void CheckInf(int m, int n, double *A, const char* msg) {
|
||||
for(int i=0; i<m; i++) {
|
||||
for(int j=0; j<n; j++)
|
||||
if (isinf(A[j*m+i]))
|
||||
throw std::invalid_argument(msg);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// remove NaN in the input matrix
|
||||
void RemoveNaN(int m, int n, double *A) {
|
||||
for(int i=0; i<m; i++) {
|
||||
for(int j=0; j<n; j++)
|
||||
if (isnan(A[j*m+i]))
|
||||
A[j*m+i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// check NaN in the input matrix
|
||||
bool HasNaN(int m, int n, double *A) {
|
||||
for(int i=0; i<m; i++) {
|
||||
for(int j=0; j<n; j++)
|
||||
if (isnan(A[j*m+i]))
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// check Inf in the input matrix
|
||||
bool HasInf(int m, int n, double *A) {
|
||||
for(int i=0; i<m; i++) {
|
||||
for(int j=0; j<n; j++)
|
||||
if (isinf(A[j*m+i]))
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* the wrapper for LAPACK dlarft_ and dlarfb_ function
|
||||
*/
|
||||
inline void dlarftb_wrap(int m, int n, int k, int ldc, int ldv, double *V, double *Tau, double *C, double *W,
|
||||
int* numPendingHHs, int* numZeros)
|
||||
{
|
||||
if (m <= 0 || n <= 0 || k <= 0) return ;
|
||||
|
||||
if (m < k) throw std::invalid_argument("dlarftb_wrap: m < k!");
|
||||
dlarft_(&direct, &storev, &m, &k, V, &ldv, Tau, W, &k) ;
|
||||
dlarfb_(&left, &trans, &direct, &storev, &m, &n, &k, V, &ldv,
|
||||
W, &k, C, &ldc, W + k*k, &n);
|
||||
*numPendingHHs = 0;
|
||||
*numZeros = 0;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* check whether there is enough work left
|
||||
*/
|
||||
inline bool NotEnoughLeft(const int numRowLeft, const int numColLeft, const int sizeBlock) {
|
||||
return numRowLeft * (numColLeft - (sizeBlock + 4)) < 5000 || numRowLeft <= sizeBlock / 2
|
||||
|| sizeBlock <= 1;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// write input to the disk for debugging
|
||||
void WriteInput(int m, int n, int numPivotColumns, double *A, int *stairs) {
|
||||
ofstream fs;
|
||||
fs.open ("denseqr.txt", ios::out | ios::trunc);
|
||||
fs << m << " " << n << " " << numPivotColumns << endl;
|
||||
for(int i=0; i<m; i++) {
|
||||
for(int j=0; j<n; j++)
|
||||
fs << A[j*m+i] << "\t";
|
||||
fs << endl;
|
||||
}
|
||||
fs.close();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void DenseQR(int m, int n, int numPivotColumns, double *A, int *stairs, double *workspace) {
|
||||
|
||||
// WriteInput(m, n, numPivotColumns, A, stairs);
|
||||
// CheckNaN(m, n, A, "DenseQR: the input matrix has NaN");
|
||||
// CheckInf(m, n, A, "DenseQR: the input matrix has Inf");
|
||||
|
||||
if (A == NULL) throw std::invalid_argument("DenseQR: A == NULL!");
|
||||
if (stairs == NULL) throw std::invalid_argument("DenseQR: stairs == NULL!");
|
||||
if (workspace == NULL) throw std::invalid_argument("DenseQR: W == NULL!");
|
||||
if (m < 0) throw std::invalid_argument("DenseQR: m < 0");
|
||||
if (n < 0) throw std::invalid_argument("DenseQR: n < 0");
|
||||
if (numPivotColumns < 0 || numPivotColumns > 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
|
||||
|
|
@ -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
|
||||
}
|
||||
|
|
@ -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 <gtsam/base/timing.h>
|
||||
#include <gtsam/base/DenseQRUtil.h>
|
||||
|
||||
#include <string.h> // 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<int> remained;
|
||||
// a[0] = A.data();
|
||||
// for(int i=0; i<m-1; i++) {
|
||||
// a[i+1] = a[i] + n;
|
||||
// remained.push_back(i);
|
||||
// }
|
||||
// remained.push_back(m-1);
|
||||
//
|
||||
// // reorder the rows
|
||||
// int j;
|
||||
// vector<int> sorted;
|
||||
// list<int>::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<n; j++)
|
||||
// Stair[j] = m;
|
||||
//
|
||||
// // copy the new row
|
||||
// Matrix A_new = zeros(m,n);
|
||||
// int offset[m];
|
||||
// offset[0] = 0;
|
||||
// for(int i=1; i<m; i++)
|
||||
// offset[i] = offset[i-1] +n;
|
||||
// vector<int>::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; i<m; i++) {
|
||||
// for(int j=0; j<n; j++)
|
||||
// cout << a[j*m+i] << "\t";
|
||||
// cout << endl;
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// /* ************************************************************************* */
|
||||
// void householder_denseqr(Matrix &A, int* Stair) {
|
||||
//
|
||||
// tic("householder_denseqr");
|
||||
//
|
||||
// int m = A.rows();
|
||||
// int n = A.cols();
|
||||
//
|
||||
// bool allocedStair = false;
|
||||
// if (Stair == NULL) {
|
||||
// allocedStair = true;
|
||||
// Stair = new int[n];
|
||||
// for(int j=0; j<n; j++)
|
||||
// Stair[j] = m;
|
||||
// }
|
||||
//
|
||||
// tic("householder_denseqr: row->col");
|
||||
// // 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; j<n; j++, k0+=m) {
|
||||
// k = k0;
|
||||
// j0 = min(j+1,m);
|
||||
// for(int i=0; i<j0; i++, k++)
|
||||
// A(i,j) = a[k];
|
||||
// }
|
||||
//
|
||||
// toc("householder_denseqr: col->row");
|
||||
//
|
||||
//
|
||||
// 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
|
||||
|
|
@ -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 <gtsam/base/Matrix.h>
|
||||
|
||||
//#ifdef GT_USE_LAPACK
|
||||
//#include <gtsam/base/DenseQR.h>
|
||||
//
|
||||
//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
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 <iostream>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/DenseQRUtil.h>
|
||||
|
||||
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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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 <iostream>
|
||||
#include <sstream>
|
||||
#include <map>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
#include <boost/assign/std/vector.hpp> // for operator +=
|
||||
using namespace boost::assign;
|
||||
|
||||
#define GTSAM_MAGIC_KEY
|
||||
|
||||
#include <gtsam/inference/BinaryConditional.h>
|
||||
#include <gtsam/inference/BayesNet-inl.h>
|
||||
#include <gtsam/inference/Ordering.h>
|
||||
#include <gtsam/inference/SymbolMap.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/** A Bayes net made from binary conditional probability tables */
|
||||
typedef BayesNet<BinaryConditional> BinaryBayesNet;
|
||||
|
||||
|
||||
double probability( BinaryBayesNet & bbn, SymbolMap<bool> & 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<bool> config;
|
||||
config["y"] = false;
|
||||
config["x"] = false;
|
||||
// unary conditional for y
|
||||
boost::shared_ptr<BinaryConditional> py(new BinaryConditional("y",0.2));
|
||||
DOUBLES_EQUAL(0.8,py->probability(config),0.01);
|
||||
|
||||
// single parent conditional for x
|
||||
vector<double> cpt;
|
||||
cpt += 0.3, 0.6 ; // array index corresponds to binary parent values structure
|
||||
boost::shared_ptr<BinaryConditional> 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);}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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 <boost/foreach.hpp>
|
||||
#include <gtsam/linear/BayesNetPreconditioner.h>
|
||||
|
||||
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
|
||||
|
||||
|
||||
|
|
@ -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 <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/GaussianBayesNet.h>
|
||||
|
||||
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_ */
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 <boost/foreach.hpp>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <gtsam/linear/VectorBTree.h>
|
||||
|
||||
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<Symbol> VectorBTree::get_names() const {
|
||||
std::vector<Symbol> 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<double>::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
|
||||
|
|
@ -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 <map>
|
||||
#include <boost/numeric/ublas/storage.hpp>
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
#include <gtsam/base/BTree.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** Factor Graph Valuesuration */
|
||||
class VectorBTree: public Testable<VectorBTree> {
|
||||
|
||||
private:
|
||||
|
||||
/** dictionary from Symbol to Range */
|
||||
typedef boost::numeric::ublas::range Range;
|
||||
typedef BTree<Symbol, Range> 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<Symbol> 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<Symbol,Vector> 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<class ARCHIVE>
|
||||
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
|
||||
|
|
@ -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 <boost/foreach.hpp>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <gtsam/linear/VectorMap.h>
|
||||
|
||||
// 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<Index> VectorMap::get_names() const {
|
||||
std::vector<Index> 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
|
||||
|
|
@ -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 <map>
|
||||
#include <boost/serialization/map.hpp>
|
||||
|
||||
#include <gtsam/base/types.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** Factor Graph Valuesuration */
|
||||
class VectorMap : public Testable<VectorMap> {
|
||||
|
||||
protected:
|
||||
/** Map from string indices to values */
|
||||
std::map<Index,Vector> values;
|
||||
|
||||
public:
|
||||
typedef std::map<Index,Vector>::iterator iterator;
|
||||
typedef std::map<Index,Vector>::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<Index> 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<double>::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<class ARCHIVE>
|
||||
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
|
||||
|
|
@ -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 <boost/foreach.hpp>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#define GTSAM_MAGIC_KEY
|
||||
|
||||
#include <gtsam/inference/Ordering.h>
|
||||
#include <gtsam/linear/BayesNetPreconditioner.h>
|
||||
#include <gtsam/linear/iterative-inl.h>
|
||||
|
||||
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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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 <iostream>
|
||||
#include <sstream>
|
||||
#include <limits>
|
||||
|
||||
#include <boost/assign/std/vector.hpp>
|
||||
using namespace boost::assign; // bring 'operator+=()' into scope
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/linear/VectorBTree.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
static Symbol l1('l',1), x1('x',1), x2('x',2);
|
||||
static double inf = std::numeric_limits<double>::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<Symbol> 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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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 <iostream>
|
||||
#include <sstream>
|
||||
|
||||
#define GTSAM_MAGIC_KEY
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/linear/VectorMap.h>
|
||||
|
||||
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 <limits>
|
||||
double inf = std::numeric_limits<double>::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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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 <iostream>
|
||||
#include <boost/timer.hpp>
|
||||
#include <gtsam/linear/VectorBTree.h>
|
||||
#include <gtsam/linear/VectorMap.h>
|
||||
|
||||
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;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 <gtsam/slam/Pose2SLAMOptimizer.h>
|
||||
#include <gtsam/slam/pose2SLAM.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/linear/SubgraphSolver-inl.h>
|
||||
|
||||
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<SharedDiagonal> 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<Vector> vs;
|
||||
BOOST_FOREACH(const Symbol& key, *solver_.ordering())
|
||||
vs.push_back(X[key]);
|
||||
return concatVectors(vs);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Pose2SLAMOptimizer::error() const {
|
||||
return graph_->error(*theta_);
|
||||
}
|
||||
}
|
||||
|
|
@ -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 <boost/foreach.hpp>
|
||||
|
||||
#include <gtsam/slam/pose2SLAM.h>
|
||||
#include <gtsam/inference/Ordering.h>
|
||||
#include <gtsam/linear/SubgraphSolver.h>
|
||||
|
||||
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<Pose2Graph> graph_;
|
||||
|
||||
/** Current non-linear estimate */
|
||||
boost::shared_ptr<Pose2Values> theta_;
|
||||
|
||||
/** Non-linear solver */
|
||||
typedef SubgraphSolver<Pose2Graph, Pose2Values> SPCG_Solver;
|
||||
SPCG_Solver solver_;
|
||||
|
||||
/** Linear Solver */
|
||||
boost::shared_ptr<SubgraphPreconditioner> 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<const Pose2Graph> graph() const {
|
||||
return graph_;
|
||||
}
|
||||
|
||||
/**
|
||||
* return the current linearization point
|
||||
*/
|
||||
boost::shared_ptr<const Pose2Values> 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<Matrix,Vector> Ab1() const { return system_->Ab1(*solver_.ordering()); }
|
||||
std::pair<Matrix,Vector> 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_ */
|
||||
|
|
@ -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 <gtsam/slam/simulated2D.h>
|
||||
#include <gtsam/slam/Simulated2DValues.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
typedef simulated2D::Measurement Simulated2DMeasurement;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -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 <gtsam/slam/simulated2D.h>
|
||||
#include <gtsam/slam/Simulated2DValues.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
typedef simulated2D::Odometry Simulated2DOdometry;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -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 <gtsam/slam/simulated2DOriented.h>
|
||||
#include <gtsam/slam/Simulated2DOrientedValues.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
typedef simulated2DOriented::Odometry Simulated2DOrientedOdometry;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -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 <gtsam/slam/simulated2DOriented.h>
|
||||
#include <gtsam/slam/Simulated2DOrientedValues.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** Create a prior on a pose Point2 with key 'x1' etc... */
|
||||
typedef simulated2DOriented::GenericPosePrior<Simulated2DOrientedValues, simulated2DOriented::PoseKey> Simulated2DOrientedPosePrior;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -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 <gtsam/slam/simulated2DOriented.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class Simulated2DOrientedValues: public simulated2DOriented::Values {
|
||||
public:
|
||||
typedef boost::shared_ptr<Point2> sharedPoint;
|
||||
typedef boost::shared_ptr<Pose2> 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
|
||||
|
||||
|
|
@ -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 <gtsam/slam/simulated2D.h>
|
||||
#include <gtsam/slam/Simulated2DValues.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** Create a prior on a landmark Point2 with key 'l1' etc... */
|
||||
typedef simulated2D::GenericPrior<Simulated2DValues, simulated2D::PointKey> Simulated2DPointPrior;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -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 <gtsam/slam/simulated2D.h>
|
||||
#include <gtsam/slam/Simulated2DValues.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** Create a prior on a pose Point2 with key 'x1' etc... */
|
||||
typedef simulated2D::GenericPrior<Simulated2DValues, simulated2D::PoseKey> Simulated2DPosePrior;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -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 <gtsam/slam/simulated2D.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class Simulated2DValues: public simulated2D::Values {
|
||||
public:
|
||||
typedef boost::shared_ptr<Point2> 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
|
||||
|
||||
|
|
@ -63,7 +63,7 @@ namespace gtsam {
|
|||
|
||||
// Optimizer
|
||||
typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, GaussianSequentialSolver> OptimizerSequential;
|
||||
typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
|
||||
typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
|
||||
|
||||
} // pose2SLAM
|
||||
|
||||
|
|
|
|||
|
|
@ -34,7 +34,40 @@ namespace gtsam {
|
|||
typedef TypedSymbol<Point2, 'l'> PointKey;
|
||||
typedef LieValues<PoseKey> PoseValues;
|
||||
typedef LieValues<PointKey> PointValues;
|
||||
typedef TupleValues2<PoseValues, PointValues> Values;
|
||||
|
||||
// typedef TupleValues2<PoseValues, PointValues> Values;
|
||||
class Values: public TupleValues2<PoseValues, PointValues> {
|
||||
public:
|
||||
typedef TupleValues2<PoseValues, PointValues> Base;
|
||||
typedef boost::shared_ptr<Point2> 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
|
||||
|
|
|
|||
|
|
@ -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 <CppUnitLite/TestHarness.h>
|
||||
|
||||
#define GTSAM_MAGIC_KEY
|
||||
|
||||
#include <gtsam/base/LieVector.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/slam/pose2SLAM.h>
|
||||
|
||||
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<JacobianFactor> linear =
|
||||
boost::dynamic_pointer_cast<JacobianFactor>(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<JacobianFactor> linear =
|
||||
boost::dynamic_pointer_cast<JacobianFactor>(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<JacobianFactor> actual =
|
||||
boost::dynamic_pointer_cast<JacobianFactor>(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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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 <CppUnitLite/TestHarness.h>
|
||||
|
||||
#define GTSAM_MAGIC_KEY
|
||||
|
||||
#include <gtsam/base/LieVector.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/slam/pose2SLAM.h>
|
||||
|
||||
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<JacobianFactor> linear =
|
||||
boost::dynamic_pointer_cast<JacobianFactor>(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<JacobianFactor> actual =
|
||||
boost::dynamic_pointer_cast<JacobianFactor>(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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -24,10 +24,10 @@ using namespace boost::assign;
|
|||
|
||||
#define GTSAM_MAGIC_KEY
|
||||
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
|
||||
#include <gtsam/inference/FactorGraph-inl.h>
|
||||
#include <gtsam/slam/pose2SLAM.h>
|
||||
//#include <gtsam/slam/Pose2SLAMOptimizer.h>
|
||||
|
||||
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<JacobianFactor> linear =
|
||||
boost::dynamic_pointer_cast<JacobianFactor>(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<JacobianFactor> actual =
|
||||
boost::dynamic_pointer_cast<JacobianFactor>(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<JacobianFactor> linear =
|
||||
boost::dynamic_pointer_cast<JacobianFactor>(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<JacobianFactor> linear =
|
||||
boost::dynamic_pointer_cast<JacobianFactor>(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<JacobianFactor> actual =
|
||||
boost::dynamic_pointer_cast<JacobianFactor>(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;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,72 @@
|
|||
/**
|
||||
* @file testPose2SLAMwSPCG
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam/slam/pose2SLAM.h>
|
||||
#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
|
||||
|
||||
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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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 <iostream>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/slam/pose2SLAM.h>
|
||||
|
||||
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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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 <iostream>
|
||||
#include <boost/assign/std/list.hpp>
|
||||
using namespace boost::assign;
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/slam/pose3SLAM.h>
|
||||
|
||||
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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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<Values, Key> 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;
|
||||
|
|
|
|||
|
|
@ -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 <iostream>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/slam/pose3SLAM.h>
|
||||
|
||||
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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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 <CppUnitLite/TestHarness.h>
|
||||
|
||||
#define GTSAM_MAGIC_KEY
|
||||
|
||||
#include <gtsam/inference/FactorGraph-inl.h>
|
||||
#include <gtsam/slam/visualSLAM.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -21,7 +21,7 @@
|
|||
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/slam/simulated2D.h>
|
||||
#include <gtsam/slam/Simulated2DValues.h>
|
||||
//#include <gtsam/slam/Simulated2DValues.h>
|
||||
|
||||
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));
|
||||
|
|
|
|||
|
|
@ -21,9 +21,9 @@
|
|||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/slam/Simulated2DValues.h>
|
||||
//#include <gtsam/slam/Simulated2DValues.h>
|
||||
#include <gtsam/slam/simulated2D.h>
|
||||
#include <gtsam/slam/simulated2DOriented.h>
|
||||
#include <gtsam/slam/Simulated2DOrientedOdometry.h>
|
||||
|
||||
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));
|
||||
|
|
|
|||
|
|
@ -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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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 <CppUnitLite/TestHarness.h>
|
||||
|
||||
#define GTSAM_MAGIC_KEY
|
||||
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/slam/visualSLAM.h>
|
||||
|
||||
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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 <boost/foreach.hpp>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#define GTSAM_MAGIC_KEY
|
||||
|
||||
#include <gtsam/inference/Ordering.h>
|
||||
#include <gtsam/linear/BayesNetPreconditioner.h>
|
||||
#include <gtsam/linear/iterative-inl.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
#include <gtsam/slam/smallExample.h>
|
||||
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<BayesNetPreconditioner,
|
||||
VectorValues, Errors>(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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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 <boost/assign/std/list.hpp> // for operator +=
|
||||
using namespace boost::assign;
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
// TODO: DANGEROUS, create shared pointers
|
||||
#define GTSAM_MAGIC_GAUSSIAN 3
|
||||
#define GTSAM_MAGIC_KEY
|
||||
|
||||
#include <gtsam/inference/Ordering.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/slam/smallExample.h>
|
||||
#include <gtsam/slam/pose2SLAM.h>
|
||||
#include <gtsam/linear/SubgraphPreconditioner.h>
|
||||
#include <gtsam/inference/FactorGraph-inl.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
|
||||
#include <gtsam/linear/iterative-inl.h>
|
||||
|
||||
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<GaussianFactorGraph> 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<Key> tree = graph.findMinimumSpanningTree<Key, Pose2Factor>();
|
||||
list<Key> keys = predecessorMap2Keys(tree);
|
||||
list<Symbol> symbols;
|
||||
symbols.resize(keys.size());
|
||||
std::transform(keys.begin(), keys.end(), symbols.begin(), key2symbol<Key>);
|
||||
Ordering ordering(symbols);
|
||||
|
||||
Key root = keys.back();
|
||||
Pose2Graph T, C;
|
||||
graph.split<Key, Pose2Factor>(tree, T, C);
|
||||
|
||||
// build the subgraph PCG system
|
||||
boost::shared_ptr<GaussianFactorGraph> 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<SubgraphPreconditioner, VectorValues,
|
||||
Errors> (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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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 <boost/foreach.hpp>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <boost/assign/std/list.hpp>
|
||||
using namespace boost::assign;
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#define GTSAM_MAGIC_KEY
|
||||
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/inference/Ordering.h>
|
||||
#include <gtsam/linear/SubgraphPreconditioner.h>
|
||||
#include <gtsam/linear/iterative-inl.h>
|
||||
#include <gtsam/slam/smallExample.h>
|
||||
|
||||
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<VectorValues> (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<SubgraphPreconditioner,
|
||||
VectorValues, Errors>(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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue