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

make check passes with dense qr using lapack geqrf

release/4.3a0
Richard Roberts 2010-10-22 03:51:10 +00:00
parent 58f23eb6ad
commit 1d94dacca0
14 changed files with 56 additions and 127 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
base/Matrix.cpp
View File

@ -674,6 +674,21 @@ void householder(Matrix &A) {
}
}
#endif
void householderColMajor(MatrixColMajor &A) {
__CLPK_integer m = A.size1();
__CLPK_integer n = A.size2();
double tau[n];
double work_optimal_size;
__CLPK_integer lwork = -1;
__CLPK_integer info;
dgeqrf_(&m, &n, A.data().begin(), &m, tau, &work_optimal_size, &lwork, &info);
lwork = (__CLPK_integer)work_optimal_size;
double work[lwork];
dgeqrf_(&m, &n, A.data().begin(), &m, tau, work, &lwork, &info);
}
#endif
///* ************************************************************************* */

16
base/Matrix.h
View File

@ -264,15 +264,21 @@ void householder_(Matrix& A, size_t k);
*/
void householder(Matrix& A, size_t k);
/**
* Householder tranformation, zeros below diagonal
* @param k number of columns to zero out below diagonal
* @return nothing: in place !!!
*/
#ifdef GT_USE_LAPACK
#ifdef YA_BLAS
/**
* Householder tranformation, zeros below diagonal
* @return nothing: in place !!!
*/
void householder(Matrix& A);
#endif
/**
* Householder tranformation directly on a column-major matrix. Does not zero
* below the diagonal, so it will contain Householder vectors.
* @return nothing: in place !!!
*/
void householderColMajor(MatrixColMajor& A);
#endif
/**

1
examples/PlanarSLAMSelfContained_advanced.cpp
View File

@ -38,6 +38,7 @@
#include <gtsam/nonlinear/TupleValues-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
// Main typedefs

1
examples/SimpleRotation.cpp
View File

@ -43,7 +43,6 @@ using namespace gtsam;
typedef TypedSymbol<Rot2, 'x'> Key;
typedef LieValues<Key> Values;
typedef NonlinearFactorGraph<Values> Graph;
typedef Factorization<Graph,Values> Solver;
typedef NonlinearOptimizer<Graph,Values> Optimizer;
const double degree = M_PI / 180;

74
linear/Factorization.h
View File

@ -1,74 +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 Factorization
* @brief Template Linear solver class that uses a Gaussian Factor Graph
* @author Kai Ni
* @author Frank Dellaert
*/
// $Id: GaussianFactorGraph.h,v 1.24 2009/08/14 20:48:51 acunning Exp $
#pragma once
#include <stdexcept>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/inference/inference-inl.h>
namespace gtsam {
class Ordering;
/**
* A linear system solver using factorization
*/
template <class NONLINEARGRAPH, class VALUES>
class Factorization {
private:
boost::shared_ptr<Ordering> ordering_;
public:
Factorization(boost::shared_ptr<Ordering> ordering)
: ordering_(ordering) {
if (!ordering) throw std::invalid_argument("Factorization constructor: ordering = NULL");
}
/**
* solve for the optimal displacement in the tangent space, and then solve
* the resulted linear system
*/
VectorValues optimize(GaussianFactorGraph& fg) const {
return *GaussianSequentialSolver(fg).optimize();
}
/**
* linearize the non-linear graph around the current config
*/
boost::shared_ptr<GaussianFactorGraph> linearize(const NONLINEARGRAPH& g, const VALUES& config) const {
return g.linearize(config, *ordering_);
}
/**
* Does not do anything here in Factorization.
*/
boost::shared_ptr<Factorization> prepareLinear(const GaussianFactorGraph& fg) const {
return boost::shared_ptr<Factorization>(new Factorization(*this));
}
/** expmap the Values given the stored Ordering */
VALUES expmap(const VALUES& config, const VectorValues& delta) const {
return config.expmap(delta, *ordering_);
}
};
}

1
linear/GaussianJunctionTree.cpp
View File

@ -29,6 +29,7 @@ namespace gtsam {
// explicit template instantiation
template class JunctionTree<GaussianFactorGraph>;
template class ClusterTree<GaussianFactorGraph>;
using namespace std;

2
linear/Makefile.am
View File

@ -25,7 +25,7 @@ check_PROGRAMS += tests/testVectorValues
sources += GaussianSequentialSolver.cpp GaussianMultifrontalSolver.cpp
# Gaussian Factor Graphs
headers += GaussianFactorSet.h Factorization.h
headers += GaussianFactorSet.h
sources += GaussianFactor.cpp GaussianFactorGraph.cpp
sources += GaussianJunctionTree.cpp
sources += GaussianConditional.cpp GaussianBayesNet.cpp

18
linear/NoiseModel.cpp
View File

@ -219,10 +219,10 @@ SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional<std::vector<long>&> firs
// General QR, see also special version in Constrained
SharedDiagonal Gaussian::QRColumnWise(ublas::matrix<double, ublas::column_major>& Ab, vector<long>& firstZeroRows) const {
Matrix Abresult(Ab);
SharedDiagonal result = QR(Abresult, firstZeroRows);
Ab = Abresult;
return result;
WhitenInPlace(Ab);
householderColMajor(Ab);
size_t maxRank = min(Ab.size1(), Ab.size2()-1);
return Unit::Create(maxRank);
// // get size(A) and maxRank
// // TODO: really no rank problems ?
// size_t m = Ab.size1(), n = Ab.size2()-1;
@ -402,13 +402,9 @@ SharedDiagonal Constrained::QR(Matrix& Ab, boost::optional<std::vector<long>&> f
}
SharedDiagonal Constrained::QRColumnWise(ublas::matrix<double, ublas::column_major>& Ab, vector<long>& firstZeroRows) const {
// Matrix AbRowWise(Ab);
// SharedDiagonal result = this->QR(AbRowWise, firstZeroRows);
// Ab = AbRowWise;
// return result;
Matrix Abresult(Ab);
SharedDiagonal result = QR(Abresult, firstZeroRows);
Ab = Abresult;
Matrix AbRowWise(Ab);
SharedDiagonal result = this->QR(AbRowWise, firstZeroRows);
Ab = AbRowWise;
return result;
}

4
nonlinear/NonlinearOptimizer.h
View File

@ -23,8 +23,8 @@
#include <boost/make_shared.hpp>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/Factorization.h>
#include <gtsam/nonlinear/NonlinearOptimizationParameters.h>
namespace gtsam {
@ -63,7 +63,7 @@ namespace gtsam {
*
*
*/
template<class G, class T, class L = GaussianFactorGraph, class GS = GaussianSequentialSolver, class W = NullOptimizerWriter>
template<class G, class T, class L = GaussianFactorGraph, class GS = GaussianMultifrontalSolver, class W = NullOptimizerWriter>
class NonlinearOptimizer {
public:

9
slam/tests/testPose2SLAM.cpp
View File

@ -113,8 +113,7 @@ TEST(Pose2Graph, optimize) {
shared_ptr<Ordering> ordering(new Ordering);
*ordering += "x0","x1";
typedef NonlinearOptimizer<Pose2Graph, Pose2Values> Optimizer;
Optimizer::shared_solver solver(new Optimizer::solver(ordering));
Optimizer optimizer0(fg, initial, solver);
Optimizer optimizer0(fg, initial, ordering);
Optimizer::Parameters::verbosityLevel verbosity = Optimizer::Parameters::SILENT;
//Optimizer::verbosityLevel verbosity = Optimizer::ERROR;
Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity);
@ -153,8 +152,7 @@ TEST(Pose2Graph, optimizeThreePoses) {
*ordering += "x0","x1","x2";
// optimize
pose2SLAM::Optimizer::shared_solver solver(new pose2SLAM::Optimizer::solver(ordering));
pose2SLAM::Optimizer optimizer0(fg, initial, solver);
pose2SLAM::Optimizer optimizer0(fg, initial, ordering);
pose2SLAM::Optimizer::Parameters::verbosityLevel verbosity = pose2SLAM::Optimizer::Parameters::SILENT;
pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity);
@ -197,8 +195,7 @@ TEST(Pose2Graph, optimizeCircle) {
*ordering += "x0","x1","x2","x3","x4","x5";
// optimize
pose2SLAM::Optimizer::shared_solver solver(new pose2SLAM::Optimizer::solver(ordering));
pose2SLAM::Optimizer optimizer0(fg, initial, solver);
pose2SLAM::Optimizer optimizer0(fg, initial, ordering);
pose2SLAM::Optimizer::Parameters::verbosityLevel verbosity = pose2SLAM::Optimizer::Parameters::SILENT;
pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity);

3
slam/tests/testPose3SLAM.cpp
View File

@ -71,8 +71,7 @@ TEST(Pose3Graph, optimizeCircle) {
shared_ptr<Ordering> ordering(new Ordering);
*ordering += "x0","x1","x2","x3","x4","x5";
typedef NonlinearOptimizer<Pose3Graph, Pose3Values> Optimizer;
Optimizer::shared_solver solver(new Optimizer::solver(ordering));
Optimizer optimizer0(fg, initial, solver);
Optimizer optimizer0(fg, initial, ordering);
Optimizer::Parameters::verbosityLevel verbosity = Optimizer::Parameters::SILENT;
// Optimizer::verbosityLevel verbosity = Optimizer::ERROR;
Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity);

9
slam/tests/testVSLAMGraph.cpp
View File

@ -105,8 +105,7 @@ TEST( Graph, optimizeLM)
// Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth
Optimizer::shared_solver solver(new Optimizer::solver(ordering));
Optimizer optimizer(graph, initialEstimate, solver);
Optimizer optimizer(graph, initialEstimate, ordering);
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// Iterate once, and the config should not have changed because we started
@ -143,8 +142,7 @@ TEST( Graph, optimizeLM2)
// Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth
Optimizer::shared_solver solver(new Optimizer::solver(ordering));
Optimizer optimizer(graph, initialEstimate, solver);
Optimizer optimizer(graph, initialEstimate, ordering);
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// Iterate once, and the config should not have changed because we started
@ -179,8 +177,7 @@ TEST( Graph, CHECK_ORDERING)
// Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth
Optimizer::shared_solver solver(new Optimizer::solver(ordering));
Optimizer optimizer(graph, initialEstimate, solver);
Optimizer optimizer(graph, initialEstimate, ordering);
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
// Iterate once, and the config should not have changed because we started

6
tests/testNonlinearEquality.cpp
View File

@ -197,8 +197,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) {
// optimize
boost::shared_ptr<Ordering> ord(new Ordering());
ord->push_back(key1);
PoseOptimizer::shared_solver solver(new PoseOptimizer::solver(ord));
PoseOptimizer optimizer(graph, init, solver);
PoseOptimizer optimizer(graph, init, ord);
double relThresh = 1e-5, absThresh = 1e-5;
PoseOptimizer result = optimizer.levenbergMarquardt(relThresh, absThresh, PoseOptimizer::Parameters::SILENT);
@ -234,8 +233,7 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
// optimize
boost::shared_ptr<Ordering> ord(new Ordering());
ord->push_back(key1);
PoseOptimizer::shared_solver solver(new PoseOptimizer::solver(ord));
PoseOptimizer optimizer(graph, init, solver);
PoseOptimizer optimizer(graph, init, ord);
double relThresh = 1e-5, absThresh = 1e-5;
PoseOptimizer result = optimizer.levenbergMarquardt(relThresh, absThresh, PoseOptimizer::Parameters::SILENT);

16
tests/testNonlinearOptimizer.cpp
View File

@ -69,11 +69,8 @@ TEST( NonlinearOptimizer, linearizeAndOptimizeForDelta )
dx2(1) = -0.2;
expected[ord1["x2"]] = dx2;
Optimizer::shared_solver solver;
// Check one ordering
solver = Optimizer::shared_solver(new Optimizer::solver(Ordering::shared_ptr(new Ordering(ord1))));
Optimizer optimizer1(fg, initial, solver);
Optimizer optimizer1(fg, initial, Optimizer::shared_ordering(new Ordering(ord1)));
VectorValues actual1 = optimizer1.linearizeAndOptimizeForDelta();
CHECK(assert_equal(actual1,expected));
@ -123,8 +120,7 @@ TEST( NonlinearOptimizer, iterateLM )
ord->push_back("x1");
// create initial optimization state, with lambda=0
Optimizer::shared_solver solver(new Optimizer::solver(ord));
Optimizer optimizer(fg, config, solver, 0.);
Optimizer optimizer(fg, config, ord, 0.);
// normal iterate
Optimizer iterated1 = optimizer.iterate();
@ -169,8 +165,7 @@ TEST( NonlinearOptimizer, optimize )
double absoluteThreshold = 1e-5;
// initial optimization state is the same in both cases tested
Optimizer::shared_solver solver(new Optimizer::solver(ord));
Optimizer optimizer(fg, c0, solver);
Optimizer optimizer(fg, c0, ord);
// Gauss-Newton
Optimizer actual1 = optimizer.gaussNewton(relativeThreshold,
@ -240,7 +235,7 @@ TEST( NonlinearOptimizer, SimpleGNOptimizer_noshared )
/* ************************************************************************* */
TEST( NonlinearOptimizer, Factorization )
{
typedef NonlinearOptimizer<Pose2Graph, Pose2Values, GaussianFactorGraph, Factorization<Pose2Graph, Pose2Values> > Optimizer;
typedef NonlinearOptimizer<Pose2Graph, Pose2Values, GaussianFactorGraph, GaussianSequentialSolver > Optimizer;
boost::shared_ptr<Pose2Values> config(new Pose2Values);
config->insert(1, Pose2(0.,0.,0.));
@ -253,9 +248,8 @@ TEST( NonlinearOptimizer, Factorization )
boost::shared_ptr<Ordering> ordering(new Ordering);
ordering->push_back(Pose2Values::Key(1));
ordering->push_back(Pose2Values::Key(2));
Optimizer::shared_solver solver(new Factorization<Pose2Graph, Pose2Values>(ordering));
Optimizer optimizer(graph, config, solver);
Optimizer optimizer(graph, config, ordering);
Optimizer optimized = optimizer.iterateLM();
Pose2Values expected;