76 lines
2.0 KiB
C++
76 lines
2.0 KiB
C++
|
/*
|
||
|
|
* Pose2SLAMOptimizer.cpp
|
||
|
|
*
|
||
|
|
* Created on: Jan 22, 2010
|
||
|
|
* Author: dellaert
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "Pose2SLAMOptimizer.h"
|
||
|
|
#include "pose2SLAM.h"
|
||
|
|
#include "dataset.h"
|
||
|
|
#include "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) {
|
||
|
|
VectorConfig X = system_->assembleConfig(x, *solver_.ordering());
|
||
|
|
*theta_ = expmap(*theta_, X);
|
||
|
|
linearize();
|
||
|
|
}
|
||
|
|
|
||
|
|
/* ************************************************************************* */
|
||
|
|
void Pose2SLAMOptimizer::updatePreconditioned(const Vector& y) {
|
||
|
|
Vector x;
|
||
|
|
update(x);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* ************************************************************************* */
|
||
|
|
Vector Pose2SLAMOptimizer::optimize() const {
|
||
|
|
VectorConfig 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_);
|
||
|
|
}
|
||
|
|
}
|