void BaseMultiEdge<D, E>::constructQuadraticForm()
{
const InformationType& omega = _information;
Matrix<double, D, 1> omega_r = - omega * _error;
for (size_t i = 0; i < _vertices.size(); ++i) {
OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
bool istatus = !(from->fixed());
if (istatus) {
const MatrixXd& A = _jacobianOplus[i];
MatrixXd AtO = A.transpose() * omega;
int fromDim = from->dimension();
Map<MatrixXd> fromMap(from->hessianData(), fromDim, fromDim);
Map<VectorXd> fromB(from->bData(), fromDim);
// ii block in the hessian
#ifdef G2O_OPENMP
from->lockQuadraticForm();
#endif
fromMap.noalias() += AtO * A;
fromB.noalias() += A.transpose() * omega_r;
// compute the off-diagonal blocks ij for all j
for (size_t j = i+1; j < _vertices.size(); ++j) {
OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(_vertices[j]);
#ifdef G2O_OPENMP
to->lockQuadraticForm();
#endif
bool jstatus = !(to->fixed());
if (jstatus) {
const MatrixXd& B = _jacobianOplus[j];
int idx = computeUpperTriangleIndex(i, j);
assert(idx < (int)_hessian.size());
HessianHelper& hhelper = _hessian[idx];
if (hhelper.transposed) { // we have to write to the block as transposed
hhelper.matrix.noalias() += B.transpose() * AtO.transpose();
} else {
hhelper.matrix.noalias() += AtO * B;
}
}
#ifdef G2O_OPENMP
to->unlockQuadraticForm();
#endif
}
#ifdef G2O_OPENMP
from->unlockQuadraticForm();
#endif
}
}
}
bool SparseOptimizer::buildIndexMapping
(SparseOptimizer::VertexContainer& vlist)
{
if (! vlist.size())
{
_ivMap.clear();
return false;
}
_ivMap.resize(vlist.size());
size_t i = 0;
// Recorre todos los vertices dandoles un indice.
// Si el vertice es fijo, su indice sera -1
// Para los vertices no fijos, les da un indice incremental.
// Primero se les da a los vertices no marginalizables y luego a los que si.
// Al final _ivMap contendra todos los vertices no fijos con los vertices
// no marginalizables en las primeras posiciones de _ivMap
for (int k=0; k<2; k++)
for (VertexContainer::iterator it=vlist.begin(); it!=vlist.end(); it++)
{
OptimizableGraph::Vertex* v = *it;
if (! v->fixed())
{
if (static_cast<int>(v->marginalized()) == k)
{
v->setTempIndex(i);
_ivMap[i]=v;
i++;
}
}else v->setTempIndex(-1);
}
_ivMap.resize(i);
return true;
}
bool SparseOptimizer::buildIndexMapping(SparseOptimizer::VertexContainer& vlist){
if (! vlist.size()){
_ivMap.clear();
return false;
}
_ivMap.resize(vlist.size());
size_t i = 0;
for (int k=0; k<2; k++)
for (VertexContainer::iterator it=vlist.begin(); it!=vlist.end(); ++it){
OptimizableGraph::Vertex* v = *it;
if (! v->fixed()){
if (static_cast<int>(v->marginalized()) == k){
v->setHessianIndex(i);
_ivMap[i]=v;
i++;
}
}
else {
v->setHessianIndex(-1);
}
}
_ivMap.resize(i);
return true;
}
void SparseOptimizer::computeInitialGuess(EstimatePropagatorCost& costFunction)
{
OptimizableGraph::VertexSet emptySet;
std::set<Vertex*> backupVertices;
HyperGraph::VertexSet fixedVertices; // these are the root nodes where to start the initialization
for (EdgeContainer::iterator it = _activeEdges.begin(); it != _activeEdges.end(); ++it) {
OptimizableGraph::Edge* e = *it;
for (size_t i = 0; i < e->vertices().size(); ++i) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(e->vertex(i));
if (!v)
continue;
if (v->fixed())
fixedVertices.insert(v);
else { // check for having a prior which is able to fully initialize a vertex
for (EdgeSet::const_iterator vedgeIt = v->edges().begin(); vedgeIt != v->edges().end(); ++vedgeIt) {
OptimizableGraph::Edge* vedge = static_cast<OptimizableGraph::Edge*>(*vedgeIt);
if (vedge->vertices().size() == 1 && vedge->initialEstimatePossible(emptySet, v) > 0.) {
//cerr << "Initialize with prior for " << v->id() << endl;
vedge->initialEstimate(emptySet, v);
fixedVertices.insert(v);
}
}
}
if (v->hessianIndex() == -1) {
std::set<Vertex*>::const_iterator foundIt = backupVertices.find(v);
if (foundIt == backupVertices.end()) {
v->push();
backupVertices.insert(v);
}
}
}
}
EstimatePropagator estimatePropagator(this);
estimatePropagator.propagate(fixedVertices, costFunction);
// restoring the vertices that should not be initialized
for (std::set<Vertex*>::iterator it = backupVertices.begin(); it != backupVertices.end(); ++it) {
Vertex* v = *it;
v->pop();
}
if (verbose()) {
computeActiveErrors();
cerr << "iteration= -1\t chi2= " << activeChi2()
<< "\t time= 0.0"
<< "\t cumTime= 0.0"
<< "\t (using initial guess from " << costFunction.name() << ")" << endl;
}
}
void BaseMultiEdge<D, E>::linearizeOplus()
{
#ifdef G2O_OPENMP
for (size_t i = 0; i < _vertices.size(); ++i) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
v->lockQuadraticForm();
}
#endif
const double delta = 1e-9;
const double scalar = 1.0 / (2*delta);
ErrorVector errorBak;
ErrorVector errorBeforeNumeric = _error;
for (size_t i = 0; i < _vertices.size(); ++i) {
//Xi - estimate the jacobian numerically
OptimizableGraph::Vertex* vi = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
if (vi->fixed())
continue;
const int vi_dim = vi->dimension();
#ifdef _MSC_VER
double* add_vi = new double[vi_dim];
#else
double add_vi[vi_dim];
#endif
std::fill(add_vi, add_vi + vi_dim, 0.0);
if (_jacobianOplus[i].rows() != _dimension || _jacobianOplus[i].cols() != vi_dim)
_jacobianOplus[i].resize(_dimension, vi_dim);
// add small step along the unit vector in each dimension
for (int d = 0; d < vi_dim; ++d) {
vi->push();
add_vi[d] = delta;
vi->oplus(add_vi);
computeError();
errorBak = _error;
vi->pop();
vi->push();
add_vi[d] = -delta;
vi->oplus(add_vi);
computeError();
errorBak -= _error;
vi->pop();
add_vi[d] = 0.0;
_jacobianOplus[i].col(d) = scalar * errorBak;
} // end dimension
#ifdef _MSC_VER
delete[] add_vi;
#endif
}
_error = errorBeforeNumeric;
#ifdef G2O_OPENMP
for (int i = (int)(_vertices.size()) - 1; i >= 0; --i) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
v->unlockQuadraticForm();
}
#endif
}
int main(int argc, char** argv)
{
bool fixLaser;
int maxIterations;
bool verbose;
string inputFilename;
string outputfilename;
string rawFilename;
string odomTestFilename;
string dumpGraphFilename;
// command line parsing
CommandArgs commandLineArguments;
commandLineArguments.param("i", maxIterations, 10, "perform n iterations");
commandLineArguments.param("v", verbose, false, "verbose output of the optimization process");
commandLineArguments.param("o", outputfilename, "", "output final version of the graph");
commandLineArguments.param("test", odomTestFilename, "", "apply odometry calibration to some test data");
commandLineArguments.param("dump", dumpGraphFilename, "", "write the graph to the disk");
commandLineArguments.param("fixLaser", fixLaser, false, "keep the laser offset fixed during optimization");
commandLineArguments.paramLeftOver("gm2dl-input", inputFilename, "", "gm2dl file which will be processed");
commandLineArguments.paramLeftOver("raw-log", rawFilename, "", "raw log file containing the odometry");
commandLineArguments.parseArgs(argc, argv);
SparseOptimizer optimizer;
optimizer.setVerbose(verbose);
optimizer.setForceStopFlag(&hasToStop);
allocateSolverForSclam(optimizer);
// loading
if (! Gm2dlIO::readGm2dl(inputFilename, optimizer, false)) {
cerr << "Error while loading gm2dl file" << endl;
}
DataQueue robotLaserQueue;
int numLaserOdom = Gm2dlIO::readRobotLaser(rawFilename, robotLaserQueue);
if (numLaserOdom == 0) {
cerr << "No raw information read" << endl;
return 0;
}
cerr << "Read " << numLaserOdom << " laser readings from file" << endl;
bool gaugeFreedom = optimizer.gaugeFreedom();
OptimizableGraph::Vertex* gauge = optimizer.findGauge();
if (gaugeFreedom) {
if (! gauge) {
cerr << "# cannot find a vertex to fix in this thing" << endl;
return 2;
} else {
cerr << "# graph is fixed by node " << gauge->id() << endl;
gauge->setFixed(true);
}
} else {
cerr << "# graph is fixed by priors" << endl;
}
addOdometryCalibLinksDifferential(optimizer, robotLaserQueue);
// sanity check
HyperDijkstra d(&optimizer);
UniformCostFunction f;
d.shortestPaths(gauge, &f);
//cerr << PVAR(d.visited().size()) << endl;
if (d.visited().size()!=optimizer.vertices().size()) {
cerr << CL_RED("Warning: d.visited().size() != optimizer.vertices().size()") << endl;
cerr << "visited: " << d.visited().size() << endl;
cerr << "vertices: " << optimizer.vertices().size() << endl;
if (1)
for (SparseOptimizer::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
if (d.visited().count(v) == 0) {
cerr << "\t unvisited vertex " << it->first << " " << (void*)v << endl;
v->setFixed(true);
}
}
}
for (SparseOptimizer::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
if (v->fixed()) {
cerr << "\t fixed vertex " << it->first << endl;
}
}
VertexSE2* laserOffset = dynamic_cast<VertexSE2*>(optimizer.vertex(Gm2dlIO::ID_LASERPOSE));
VertexOdomDifferentialParams* odomParamsVertex = dynamic_cast<VertexOdomDifferentialParams*>(optimizer.vertex(Gm2dlIO::ID_ODOMCALIB));
if (fixLaser) {
cerr << "Fix position of the laser offset" << endl;
laserOffset->setFixed(true);
}
signal(SIGINT, sigquit_handler);
cerr << "Doing full estimation" << endl;
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
cerr << "Initial chi2 = " << FIXED(optimizer.chi2()) << endl;
int i=optimizer.optimize(maxIterations);
if (maxIterations > 0 && !i){
cerr << "optimize failed, result might be invalid" << endl;
}
if (laserOffset) {
cerr << "Calibrated laser offset (x, y, theta):" << laserOffset->estimate().toVector().transpose() << endl;
}
if (odomParamsVertex) {
cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " << odomParamsVertex->estimate().transpose() << endl;
}
cerr << "vertices: " << optimizer.vertices().size() << endl;
cerr << "edges: " << optimizer.edges().size() << endl;
if (dumpGraphFilename.size() > 0) {
cerr << "Writing " << dumpGraphFilename << " ... ";
optimizer.save(dumpGraphFilename.c_str());
cerr << "done." << endl;
}
// optional input of a seperate file for applying the odometry calibration
if (odomTestFilename.size() > 0) {
DataQueue testRobotLaserQueue;
int numTestOdom = Gm2dlIO::readRobotLaser(odomTestFilename, testRobotLaserQueue);
if (numTestOdom == 0) {
cerr << "Unable to read test data" << endl;
} else {
ofstream rawStream("odometry_raw.txt");
ofstream calibratedStream("odometry_calibrated.txt");
const Vector3d& odomCalib = odomParamsVertex->estimate();
RobotLaser* prev = dynamic_cast<RobotLaser*>(testRobotLaserQueue.buffer().begin()->second);
SE2 prevCalibratedPose = prev->odomPose();
for (DataQueue::Buffer::const_iterator it = testRobotLaserQueue.buffer().begin(); it != testRobotLaserQueue.buffer().end(); ++it) {
RobotLaser* cur = dynamic_cast<RobotLaser*>(it->second);
assert(cur);
double dt = cur->timestamp() - prev->timestamp();
SE2 motion = prev->odomPose().inverse() * cur->odomPose();
// convert to velocity measurment
MotionMeasurement motionMeasurement(motion.translation().x(), motion.translation().y(), motion.rotation().angle(), dt);
VelocityMeasurement velocityMeasurement = OdomConvert::convertToVelocity(motionMeasurement);
// apply calibration
VelocityMeasurement calibratedVelocityMeasurment = velocityMeasurement;
calibratedVelocityMeasurment.setVl(odomCalib(0) * calibratedVelocityMeasurment.vl());
calibratedVelocityMeasurment.setVr(odomCalib(1) * calibratedVelocityMeasurment.vr());
MotionMeasurement mm = OdomConvert::convertToMotion(calibratedVelocityMeasurment, odomCalib(2));
// combine calibrated odometry with the previous pose
SE2 remappedOdom;
remappedOdom.fromVector(mm.measurement());
SE2 calOdomPose = prevCalibratedPose * remappedOdom;
// write output
rawStream << prev->odomPose().translation().x() << " " << prev->odomPose().translation().y() << " " << prev->odomPose().rotation().angle() << endl;
calibratedStream << calOdomPose.translation().x() << " " << calOdomPose.translation().y() << " " << calOdomPose.rotation().angle() << endl;
prevCalibratedPose = calOdomPose;
prev = cur;
}
}
}
if (outputfilename.size() > 0) {
Gm2dlIO::updateLaserData(optimizer);
cerr << "Writing " << outputfilename << " ... ";
bool writeStatus = Gm2dlIO::writeGm2dl(outputfilename, optimizer);
cerr << (writeStatus ? "done." : "failed") << endl;
}
return 0;
}
