52 lines
1.3 KiB
C++
52 lines
1.3 KiB
C++
/*
|
|
* Pose2SLAMOptimizer.cpp
|
|
*
|
|
* Created on: Jan 22, 2010
|
|
* Author: dellaert
|
|
*/
|
|
|
|
#include "Pose2SLAMOptimizer.h"
|
|
#include "pose2SLAM.h"
|
|
#include "dataset.h"
|
|
#include "SubgraphPreconditioner-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, maxID, noiseModel, addNoise);
|
|
graph_->addPrior(theta_->begin()->first, theta_->begin()->second,
|
|
noiseModel::Unit::Create(3));
|
|
|
|
// initialize non-linear solver
|
|
solver_.initialize(*graph_, *theta_);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
void Pose2SLAMOptimizer::update(const Vector& x) {
|
|
VectorConfig X; // TODO
|
|
update(X);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
void Pose2SLAMOptimizer::updatePreconditioned(const Vector& y) {
|
|
Vector x;
|
|
update(x);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
}
|