int main(int argc, char** argv)
{
OptimizableGraph::initMultiThreading();
int maxIterations;
bool verbose;
string inputFilename;
string gnudump;
string outputfilename;
string solverProperties;
string strSolver;
string loadLookup;
bool initialGuess;
bool initialGuessOdometry;
bool marginalize;
bool listTypes;
bool listSolvers;
bool listRobustKernels;
bool incremental;
bool guiOut;
int gaugeId;
string robustKernel;
bool computeMarginals;
bool printSolverProperties;
double huberWidth;
double gain;
int maxIterationsWithGain;
//double lambdaInit;
int updateGraphEachN = 10;
string statsFile;
string summaryFile;
bool nonSequential;
// command line parsing
std::vector<int> gaugeList;
CommandArgs arg;
arg.param("i", maxIterations, 5, "perform n iterations, if negative consider the gain");
arg.param("gain", gain, 1e-6, "the gain used to stop optimization (default = 1e-6)");
arg.param("ig",maxIterationsWithGain, std::numeric_limits<int>::max(), "Maximum number of iterations with gain enabled (default: inf)");
arg.param("v", verbose, false, "verbose output of the optimization process");
arg.param("guess", initialGuess, false, "initial guess based on spanning tree");
arg.param("guessOdometry", initialGuessOdometry, false, "initial guess based on odometry");
arg.param("inc", incremental, false, "run incremetally");
arg.param("update", updateGraphEachN, 10, "updates after x odometry nodes");
arg.param("guiout", guiOut, false, "gui output while running incrementally");
arg.param("marginalize", marginalize, false, "on or off");
arg.param("printSolverProperties", printSolverProperties, false, "print the properties of the solver");
arg.param("solverProperties", solverProperties, "", "set the internal properties of a solver,\n\te.g., initialLambda=0.0001,maxTrialsAfterFailure=2");
arg.param("gnudump", gnudump, "", "dump to gnuplot data file");
arg.param("robustKernel", robustKernel, "", "use this robust error function");
arg.param("robustKernelWidth", huberWidth, -1., "width for the robust Kernel (only if robustKernel)");
arg.param("computeMarginals", computeMarginals, false, "computes the marginal covariances of something. FOR TESTING ONLY");
arg.param("gaugeId", gaugeId, -1, "force the gauge");
arg.param("o", outputfilename, "", "output final version of the graph");
arg.param("solver", strSolver, "gn_var", "specify which solver to use underneat\n\t {gn_var, lm_fix3_2, gn_fix6_3, lm_fix7_3}");
#ifndef G2O_DISABLE_DYNAMIC_LOADING_OF_LIBRARIES
string dummy;
arg.param("solverlib", dummy, "", "specify a solver library which will be loaded");
arg.param("typeslib", dummy, "", "specify a types library which will be loaded");
#endif
arg.param("stats", statsFile, "", "specify a file for the statistics");
arg.param("listTypes", listTypes, false, "list the registered types");
arg.param("listRobustKernels", listRobustKernels, false, "list the registered robust kernels");
arg.param("listSolvers", listSolvers, false, "list the available solvers");
arg.param("renameTypes", loadLookup, "", "create a lookup for loading types into other types,\n\t TAG_IN_FILE=INTERNAL_TAG_FOR_TYPE,TAG2=INTERNAL2\n\t e.g., VERTEX_CAM=VERTEX_SE3:EXPMAP");
arg.param("gaugeList", gaugeList, std::vector<int>(), "set the list of gauges separated by commas without spaces \n e.g: 1,2,3,4,5 ");
arg.param("summary", summaryFile, "", "append a summary of this optimization run to the summary file passed as argument");
arg.paramLeftOver("graph-input", inputFilename, "", "graph file which will be processed", true);
arg.param("nonSequential", nonSequential, false, "apply the robust kernel only on loop closures and not odometries");
arg.parseArgs(argc, argv);
if (verbose) {
cout << "# Used Compiler: " << G2O_CXX_COMPILER << endl;
}
#ifndef G2O_DISABLE_DYNAMIC_LOADING_OF_LIBRARIES
// registering all the types from the libraries
DlWrapper dlTypesWrapper;
loadStandardTypes(dlTypesWrapper, argc, argv);
// register all the solvers
DlWrapper dlSolverWrapper;
loadStandardSolver(dlSolverWrapper, argc, argv);
#else
if (verbose)
cout << "# linked version of g2o" << endl;
#endif
OptimizationAlgorithmFactory* solverFactory = OptimizationAlgorithmFactory::instance();
if (listSolvers) {
solverFactory->listSolvers(cout);
}
if (listTypes) {
Factory::instance()->printRegisteredTypes(cout, true);
}
if (listRobustKernels) {
std::vector<std::string> kernels;
RobustKernelFactory::instance()->fillKnownKernels(kernels);
cout << "Robust Kernels:" << endl;
for (size_t i = 0; i < kernels.size(); ++i) {
cout << kernels[i] << endl;
}
}
SparseOptimizer optimizer;
optimizer.setVerbose(verbose);
optimizer.setForceStopFlag(&hasToStop);
SparseOptimizerTerminateAction* terminateAction = 0;
if (maxIterations < 0) {
cerr << "# setup termination criterion based on the gain of the iteration" << endl;
maxIterations = maxIterationsWithGain;
terminateAction = new SparseOptimizerTerminateAction;
terminateAction->setGainThreshold(gain);
terminateAction->setMaxIterations(maxIterationsWithGain);
optimizer.addPostIterationAction(terminateAction);
}
// allocating the desired solver + testing whether the solver is okay
OptimizationAlgorithmProperty solverProperty;
optimizer.setAlgorithm(solverFactory->construct(strSolver, solverProperty));
if (! optimizer.solver()) {
cerr << "Error allocating solver. Allocating \"" << strSolver << "\" failed!" << endl;
return 0;
}
if (solverProperties.size() > 0) {
bool updateStatus = optimizer.solver()->updatePropertiesFromString(solverProperties);
if (! updateStatus) {
cerr << "Failure while updating the solver properties from the given string" << endl;
}
}
if (solverProperties.size() > 0 || printSolverProperties) {
optimizer.solver()->printProperties(cerr);
}
// Loading the input data
if (loadLookup.size() > 0) {
optimizer.setRenamedTypesFromString(loadLookup);
}
if (inputFilename.size() == 0) {
cerr << "No input data specified" << endl;
return 0;
} else if (inputFilename == "-") {
cerr << "Read input from stdin" << endl;
if (!optimizer.load(cin)) {
cerr << "Error loading graph" << endl;
return 2;
}
} else {
cerr << "Read input from " << inputFilename << endl;
ifstream ifs(inputFilename.c_str());
if (!ifs) {
cerr << "Failed to open file" << endl;
return 1;
}
if (!optimizer.load(ifs)) {
cerr << "Error loading graph" << endl;
return 2;
}
}
cerr << "Loaded " << optimizer.vertices().size() << " vertices" << endl;
cerr << "Loaded " << optimizer.edges().size() << " edges" << endl;
if (optimizer.vertices().size() == 0) {
cerr << "Graph contains no vertices" << endl;
return 1;
}
set<int> vertexDimensions = optimizer.dimensions();
if (! optimizer.isSolverSuitable(solverProperty, vertexDimensions)) {
cerr << "The selected solver is not suitable for optimizing the given graph" << endl;
return 3;
}
assert (optimizer.solver());
//optimizer.setMethod(str2method(strMethod));
//optimizer.setUserLambdaInit(lambdaInit);
// check for vertices to fix to remove DoF
bool gaugeFreedom = optimizer.gaugeFreedom();
OptimizableGraph::Vertex* gauge=0;
if (gaugeList.size()){
cerr << "Fixing gauges: ";
for (size_t i=0; i<gaugeList.size(); i++){
int id=gaugeList[i];
OptimizableGraph::Vertex* v=optimizer.vertex(id);
if (!v){
cerr << "fatal, not found the vertex of id " << id << " in the gaugeList. Aborting";
return -1;
} else {
if (i==0)
gauge = v;
cerr << v->id() << " ";
v->setFixed(1);
}
}
cerr << endl;
gaugeFreedom = false;
} else {
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 or already fixed vertex" << endl;
}
// if schur, we wanna marginalize the landmarks...
if (marginalize || solverProperty.requiresMarginalize) {
int maxDim = *vertexDimensions.rbegin();
int minDim = *vertexDimensions.begin();
if (maxDim != minDim) {
cerr << "# Preparing Marginalization of the Landmarks ... ";
for (HyperGraph::VertexIDMap::iterator it=optimizer.vertices().begin(); it!=optimizer.vertices().end(); it++){
OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second);
if (v->dimension() != maxDim) {
v->setMarginalized(true);
}
}
cerr << "done." << endl;
}
}
if (robustKernel.size() > 0) {
AbstractRobustKernelCreator* creator = RobustKernelFactory::instance()->creator(robustKernel);
cerr << "# Preparing robust error function ... ";
if (creator) {
if (nonSequential) {
for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it);
if (e->vertices().size() >= 2 && std::abs(e->vertex(0)->id() - e->vertex(1)->id()) != 1) {
e->setRobustKernel(creator->construct());
if (huberWidth > 0)
e->robustKernel()->setDelta(huberWidth);
}
}
} else {
for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it);
e->setRobustKernel(creator->construct());
if (huberWidth > 0)
e->robustKernel()->setDelta(huberWidth);
}
}
cerr << "done." << endl;
} else {
cerr << "Unknown Robust Kernel: " << robustKernel << endl;
}
}
// 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 (incremental) {
cerr << CL_RED("# Note: this variant performs batch steps in each time step") << endl;
cerr << CL_RED("# For a variant which updates the Cholesky factor use the binary g2o_incremental") << endl;
int incIterations = maxIterations;
if (! arg.parsedParam("i")) {
cerr << "# Setting default number of iterations" << endl;
incIterations = 1;
}
int updateDisplayEveryN = updateGraphEachN;
int maxDim = 0;
cerr << "# incremental settings" << endl;
cerr << "#\t solve every " << updateGraphEachN << endl;
cerr << "#\t iterations " << incIterations << endl;
SparseOptimizer::VertexIDMap vertices = optimizer.vertices();
for (SparseOptimizer::VertexIDMap::const_iterator it = vertices.begin(); it != vertices.end(); ++it) {
const SparseOptimizer::Vertex* v = static_cast<const SparseOptimizer::Vertex*>(it->second);
maxDim = max(maxDim, v->dimension());
}
vector<SparseOptimizer::Edge*> edges;
for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it);
edges.push_back(e);
}
optimizer.edges().clear();
optimizer.vertices().clear();
optimizer.setVerbose(false);
// sort the edges in a way that inserting them makes sense
sort(edges.begin(), edges.end(), IncrementalEdgesCompare());
double cumTime = 0.;
int vertexCount=0;
int lastOptimizedVertexCount = 0;
int lastVisUpdateVertexCount = 0;
bool freshlyOptimized=false;
bool firstRound = true;
HyperGraph::VertexSet verticesAdded;
HyperGraph::EdgeSet edgesAdded;
for (vector<SparseOptimizer::Edge*>::iterator it = edges.begin(); it != edges.end(); ++it) {
SparseOptimizer::Edge* e = *it;
int doInit = 0;
SparseOptimizer::Vertex* v1 = optimizer.vertex(e->vertices()[0]->id());
SparseOptimizer::Vertex* v2 = optimizer.vertex(e->vertices()[1]->id());
if (! v1) {
SparseOptimizer::Vertex* v = v1 = dynamic_cast<SparseOptimizer::Vertex*>(e->vertices()[0]);
bool v1Added = optimizer.addVertex(v);
//cerr << "adding" << v->id() << "(" << v->dimension() << ")" << endl;
assert(v1Added);
if (! v1Added)
cerr << "Error adding vertex " << v->id() << endl;
else
verticesAdded.insert(v);
doInit = 1;
if (v->dimension() == maxDim)
vertexCount++;
}
if (! v2) {
SparseOptimizer::Vertex* v = v2 = dynamic_cast<SparseOptimizer::Vertex*>(e->vertices()[1]);
bool v2Added = optimizer.addVertex(v);
//cerr << "adding" << v->id() << "(" << v->dimension() << ")" << endl;
assert(v2Added);
if (! v2Added)
cerr << "Error adding vertex " << v->id() << endl;
else
verticesAdded.insert(v);
doInit = 2;
if (v->dimension() == maxDim)
vertexCount++;
}
// adding the edge and initialization of the vertices
{
//cerr << " adding edge " << e->vertices()[0]->id() << " " << e->vertices()[1]->id() << endl;
if (! optimizer.addEdge(e)) {
cerr << "Unable to add edge " << e->vertices()[0]->id() << " -> " << e->vertices()[1]->id() << endl;
} else {
edgesAdded.insert(e);
}
if (doInit) {
OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);
switch (doInit){
case 1: // initialize v1 from v2
{
HyperGraph::VertexSet toSet;
toSet.insert(to);
if (e->initialEstimatePossible(toSet, from) > 0.) {
//cerr << "init: "
//<< to->id() << "(" << to->dimension() << ") -> "
//<< from->id() << "(" << from->dimension() << ") " << endl;
e->initialEstimate(toSet, from);
} else {
assert(0 && "Added unitialized variable to the graph");
}
break;
}
case 2:
{
HyperGraph::VertexSet fromSet;
fromSet.insert(from);
if (e->initialEstimatePossible(fromSet, to) > 0.) {
//cerr << "init: "
//<< from->id() << "(" << from->dimension() << ") -> "
//<< to->id() << "(" << to->dimension() << ") " << endl;
e->initialEstimate(fromSet, to);
} else {
assert(0 && "Added unitialized variable to the graph");
}
break;
}
default: cerr << "doInit wrong value\n";
}
}
}
freshlyOptimized=false;
{
//cerr << "Optimize" << endl;
if (vertexCount - lastOptimizedVertexCount >= updateGraphEachN) {
if (firstRound) {
if (!optimizer.initializeOptimization()){
cerr << "initialization failed" << endl;
return 0;
}
} else {
if (! optimizer.updateInitialization(verticesAdded, edgesAdded)) {
cerr << "updating initialization failed" << endl;
return 0;
}
}
verticesAdded.clear();
edgesAdded.clear();
double ts = get_monotonic_time();
int currentIt=optimizer.optimize(incIterations, !firstRound);
double dts = get_monotonic_time() - ts;
cumTime += dts;
firstRound = false;
//optimizer->setOptimizationTime(cumTime);
if (verbose) {
double chi2 = optimizer.chi2();
cerr << "nodes= " << optimizer.vertices().size() << "\t edges= " << optimizer.edges().size() << "\t chi2= " << chi2
<< "\t time= " << dts << "\t iterations= " << currentIt << "\t cumTime= " << cumTime << endl;
}
lastOptimizedVertexCount = vertexCount;
freshlyOptimized = true;
if (guiOut) {
if (vertexCount - lastVisUpdateVertexCount >= updateDisplayEveryN) {
dumpEdges(cout, optimizer);
lastVisUpdateVertexCount = vertexCount;
}
}
}
if (! verbose)
cerr << ".";
}
} // for all edges
if (! freshlyOptimized) {
double ts = get_monotonic_time();
int currentIt=optimizer.optimize(incIterations, !firstRound);
double dts = get_monotonic_time() - ts;
cumTime += dts;
//optimizer->setOptimizationTime(cumTime);
if (verbose) {
double chi2 = optimizer.chi2();
cerr << "nodes= " << optimizer.vertices().size() << "\t edges= " << optimizer.edges().size() << "\t chi2= " << chi2
<< "\t time= " << dts << "\t iterations= " << currentIt << "\t cumTime= " << cumTime << endl;
}
}
} else {
// BATCH optimization
if (statsFile!=""){
// allocate buffer for statistics;
optimizer.setComputeBatchStatistics(true);
}
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
double loadChi = optimizer.chi2();
cerr << "Initial chi2 = " << FIXED(loadChi) << endl;
if (initialGuess) {
optimizer.computeInitialGuess();
} else if (initialGuessOdometry) {
EstimatePropagatorCostOdometry costFunction(&optimizer);
optimizer.computeInitialGuess(costFunction);
}
double initChi = optimizer.chi2();
signal(SIGINT, sigquit_handler);
int result=optimizer.optimize(maxIterations);
if (maxIterations > 0 && result==OptimizationAlgorithm::Fail){
cerr << "Cholesky failed, result might be invalid" << endl;
} else if (computeMarginals){
std::vector<std::pair<int, int> > blockIndices;
for (size_t i=0; i<optimizer.activeVertices().size(); i++){
OptimizableGraph::Vertex* v=optimizer.activeVertices()[i];
if (v->hessianIndex()>=0){
blockIndices.push_back(make_pair(v->hessianIndex(), v->hessianIndex()));
}
if (v->hessianIndex()>0){
blockIndices.push_back(make_pair(v->hessianIndex()-1, v->hessianIndex()));
}
}
SparseBlockMatrix<MatrixXd> spinv;
if (optimizer.computeMarginals(spinv, blockIndices)) {
for (size_t i=0; i<optimizer.activeVertices().size(); i++){
OptimizableGraph::Vertex* v=optimizer.activeVertices()[i];
cerr << "Vertex id:" << v->id() << endl;
if (v->hessianIndex()>=0){
cerr << "inv block :" << v->hessianIndex() << ", " << v->hessianIndex()<< endl;
cerr << *(spinv.block(v->hessianIndex(), v->hessianIndex()));
cerr << endl;
}
if (v->hessianIndex()>0){
cerr << "inv block :" << v->hessianIndex()-1 << ", " << v->hessianIndex()<< endl;
cerr << *(spinv.block(v->hessianIndex()-1, v->hessianIndex()));
cerr << endl;
}
}
}
}
optimizer.computeActiveErrors();
double finalChi=optimizer.chi2();
if (summaryFile!="") {
PropertyMap summary;
summary.makeProperty<StringProperty>("filename", inputFilename);
summary.makeProperty<IntProperty>("n_vertices", optimizer.vertices().size());
summary.makeProperty<IntProperty>("n_edges", optimizer.edges().size());
int nLandmarks=0;
int nPoses=0;
int maxDim = *vertexDimensions.rbegin();
for (HyperGraph::VertexIDMap::iterator it=optimizer.vertices().begin(); it!=optimizer.vertices().end(); it++){
OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second);
if (v->dimension() != maxDim) {
nLandmarks++;
} else
nPoses++;
}
set<string> edgeTypes;
for (HyperGraph::EdgeSet::iterator it=optimizer.edges().begin(); it!=optimizer.edges().end(); it++){
edgeTypes.insert(Factory::instance()->tag(*it));
}
stringstream edgeTypesString;
for (std::set<string>::iterator it=edgeTypes.begin(); it!=edgeTypes.end(); it++){
edgeTypesString << *it << " ";
}
summary.makeProperty<IntProperty>("n_poses", nPoses);
summary.makeProperty<IntProperty>("n_landmarks", nLandmarks);
summary.makeProperty<StringProperty>("edge_types", edgeTypesString.str());
summary.makeProperty<DoubleProperty>("load_chi", loadChi);
summary.makeProperty<StringProperty>("solver", strSolver);
summary.makeProperty<BoolProperty>("robustKernel", robustKernel.size() > 0);
summary.makeProperty<DoubleProperty>("init_chi", initChi);
summary.makeProperty<DoubleProperty>("final_chi", finalChi);
summary.makeProperty<IntProperty>("maxIterations", maxIterations);
summary.makeProperty<IntProperty>("realIterations", result);
ofstream os;
os.open(summaryFile.c_str(), ios::app);
summary.writeToCSV(os);
}
if (statsFile!=""){
cerr << "writing stats to file \"" << statsFile << "\" ... ";
ofstream os(statsFile.c_str());
const BatchStatisticsContainer& bsc = optimizer.batchStatistics();
for (int i=0; i<maxIterations; i++) {
os << bsc[i] << endl;
}
cerr << "done." << endl;
}
}
// saving again
if (gnudump.size() > 0) {
bool gnuPlotStatus = saveGnuplot(gnudump, optimizer);
if (! gnuPlotStatus) {
cerr << "Error while writing gnuplot files" << endl;
}
}
if (outputfilename.size() > 0) {
if (outputfilename == "-") {
cerr << "saving to stdout";
optimizer.save(cout);
} else {
cerr << "saving " << outputfilename << " ... ";
optimizer.save(outputfilename.c_str());
}
cerr << "done." << endl;
}
// destroy all the singletons
//Factory::destroy();
//OptimizationAlgorithmFactory::destroy();
//HyperGraphActionLibrary::destroy();
return 0;
}
int main (int argc , char ** argv){
int maxIterations;
bool verbose;
bool robustKernel;
double lambdaInit;
CommandArgs arg;
bool fixSensor;
bool fixPlanes;
bool fixFirstPose;
bool fixTrajectory;
bool planarMotion;
bool listSolvers;
string strSolver;
cerr << "graph" << endl;
arg.param("i", maxIterations, 5, "perform n iterations");
arg.param("v", verbose, false, "verbose output of the optimization process");
arg.param("solver", strSolver, "lm_var", "select one specific solver");
arg.param("lambdaInit", lambdaInit, 0, "user specified lambda init for levenberg");
arg.param("robustKernel", robustKernel, false, "use robust error functions");
arg.param("fixSensor", fixSensor, false, "fix the sensor position on the robot");
arg.param("fixTrajectory", fixTrajectory, false, "fix the trajectory");
arg.param("fixFirstPose", fixFirstPose, false, "fix the first robot pose");
arg.param("fixPlanes", fixPlanes, false, "fix the planes (do localization only)");
arg.param("planarMotion", planarMotion, false, "robot moves on a plane");
arg.param("listSolvers", listSolvers, false, "list the solvers");
arg.parseArgs(argc, argv);
SparseOptimizer* g=new SparseOptimizer();
ParameterSE3Offset* odomOffset=new ParameterSE3Offset();
odomOffset->setId(0);
g->addParameter(odomOffset);
OptimizationAlgorithmFactory* solverFactory = OptimizationAlgorithmFactory::instance();
OptimizationAlgorithmProperty solverProperty;
OptimizationAlgorithm* solver = solverFactory->construct(strSolver, solverProperty);
g->setAlgorithm(solver);
if (listSolvers){
solverFactory->listSolvers(cerr);
return 0;
}
if (! g->solver()){
cerr << "Error allocating solver. Allocating \"" << strSolver << "\" failed!" << endl;
cerr << "available solvers: " << endl;
solverFactory->listSolvers(cerr);
cerr << "--------------" << endl;
return 0;
}
cerr << "sim" << endl;
Simulator* sim = new Simulator(g);
cerr << "robot" << endl;
Robot* r=new Robot(g);
cerr << "planeSensor" << endl;
Matrix3d R=Matrix3d::Identity();
R <<
0, 0, 1,
-1, 0, 0,
0, -1, 0;
Isometry3d sensorPose=Isometry3d::Identity();
sensorPose.matrix().block<3,3>(0,0) = R;
sensorPose.translation()= Vector3d(.3 , 0.5 , 1.2);
PlaneSensor* ps = new PlaneSensor(r, 0, sensorPose);
ps->_nplane << 0.03, 0.03, 0.005;
r->_sensors.push_back(ps);
sim->_robots.push_back(r);
cerr << "p1" << endl;
Plane3D plane;
PlaneItem* pi =new PlaneItem(g,1);
plane.fromVector(Eigen::Vector4d(0.,0.,1.,5.));
static_cast<VertexPlane*>(pi->vertex())->setEstimate(plane);
pi->vertex()->setFixed(fixPlanes);
sim->_world.insert(pi);
plane.fromVector(Eigen::Vector4d(1.,0.,0.,5.));
pi =new PlaneItem(g,2);
static_cast<VertexPlane*>(pi->vertex())->setEstimate(plane);
pi->vertex()->setFixed(fixPlanes);
sim->_world.insert(pi);
cerr << "p2" << endl;
pi =new PlaneItem(g,3);
plane.fromVector(Eigen::Vector4d(0.,1.,0.,5.));
static_cast<VertexPlane*>(pi->vertex())->setEstimate(plane);
pi->vertex()->setFixed(fixPlanes);
sim->_world.insert(pi);
Quaterniond q, iq;
if (planarMotion) {
r->_planarMotion = true;
r->_nmovecov << 0.01, 0.0025, 1e-9, 0.001, 0.001, 0.025;
q = Quaterniond(AngleAxisd(0.2, Vector3d::UnitZ()).toRotationMatrix());
iq = Quaterniond(AngleAxisd(-0.2, Vector3d::UnitZ()).toRotationMatrix());
} else {
r->_planarMotion = false;
//r->_nmovecov << 0.1, 0.005, 1e-9, 0.05, 0.001, 0.001;
r->_nmovecov << 0.1, 0.005, 1e-9, 0.001, 0.001, 0.05;
q = Quaterniond((AngleAxisd(M_PI/10, Vector3d::UnitZ()) * AngleAxisd(0.1, Vector3d::UnitY())).toRotationMatrix());
iq = Quaterniond((AngleAxisd(-M_PI/10, Vector3d::UnitZ()) * AngleAxisd(0.1, Vector3d::UnitY())).toRotationMatrix());
}
Isometry3d delta=Isometry3d::Identity();
sim->_lastVertexId=4;
Isometry3d startPose=Isometry3d::Identity();
startPose.matrix().block<3,3>(0,0) = AngleAxisd(-0.75*M_PI, Vector3d::UnitZ()).toRotationMatrix();
sim->move(0,startPose);
int k =20;
int l = 2;
double delta_t = 0.2;
for (int j=0; j<l; j++) {
Vector3d tr(1.,0.,0.);
delta.matrix().block<3,3>(0,0) = q.toRotationMatrix();
if (j==(l-1)){
delta.matrix().block<3,3>(0,0) = Matrix3d::Identity();
}
delta.translation()=tr*(delta_t*j);
Isometry3d iDelta = delta.inverse();
for (int a=0; a<2; a++){
for (int i=0; i<k; i++){
cerr << "m";
if (a==0)
sim->relativeMove(0,delta);
else
sim->relativeMove(0,iDelta);
cerr << "s";
sim->sense(0);
}
}
}
for (int j=0; j<l; j++) {
Vector3d tr(1.,0.,0.);
delta.matrix().block<3,3>(0,0) = iq.toRotationMatrix();
if (j==l-1){
delta.matrix().block<3,3>(0,0) = Matrix3d::Identity();
}
delta.translation()=tr*(delta_t*j);
Isometry3d iDelta = delta.inverse();
for (int a=0; a<2; a++){
for (int i=0; i<k; i++){
cerr << "m";
if (a==0)
sim->relativeMove(0,delta);
else
sim->relativeMove(0,iDelta);
cerr << "s";
sim->sense(0);
}
}
}
ofstream os("test_gt.g2o");
g->save(os);
if (fixSensor) {
ps->_offsetVertex->setFixed(true);
} else {
Vector6d noffcov;
noffcov << 0.1,0.1,0.1,0.5, 0.5, 0.5;
ps->_offsetVertex->setEstimate(ps->_offsetVertex->estimate() * sample_noise_from_se3(noffcov));
ps->_offsetVertex->setFixed(false);
}
if (fixFirstPose){
OptimizableGraph::Vertex* gauge = g->vertex(4);
if (gauge)
gauge->setFixed(true);
} // else {
// // multiply all vertices of the robot by this standard quantity
// Quaterniond q(AngleAxisd(1, Vector3d::UnitZ()).toRotationMatrix());
// Vector3d tr(1,0,0);
// Isometry3d delta;
// delta.matrix().block<3,3>(0,0)=q.toRotationMatrix();
// delta.translation()=tr;
// for (size_t i=0; i< g->vertices().size(); i++){
// VertexSE3 *v = dynamic_cast<VertexSE3 *>(g->vertex(i));
// if (v && v->id()>0){
// v->setEstimate (v->estimate()*delta);
// }
// }
// }
ofstream osp("test_preopt.g2o");
g->save(osp);
//g->setMethod(SparseOptimizer::LevenbergMarquardt);
g->initializeOptimization();
g->setVerbose(verbose);
g->optimize(maxIterations);
if (! fixSensor ){
SparseBlockMatrix<MatrixXd> spinv;
std::pair<int, int> indexParams;
indexParams.first = ps->_offsetVertex->hessianIndex();
indexParams.second = ps->_offsetVertex->hessianIndex();
std::vector<std::pair <int, int> > blockIndices;
blockIndices.push_back(indexParams);
if (!g->computeMarginals(spinv, blockIndices)){
cerr << "error in computing the covariance" << endl;
} else {
MatrixXd m = *spinv.block(ps->_offsetVertex->hessianIndex(), ps->_offsetVertex->hessianIndex());
cerr << "Param covariance" << endl;
cerr << m << endl;
cerr << "OffsetVertex: " << endl;
ps->_offsetVertex->write(cerr);
cerr << endl;
cerr << "rotationDeterminant: " << m.block<3,3>(0,0).determinant() << endl;
cerr << "translationDeterminant: " << m.block<3,3>(3,3).determinant() << endl;
cerr << endl;
}
}
ofstream os1("test_postOpt.g2o");
g->save(os1);
}
void MarginalCovarianceCholesky::computeCovariance(SparseBlockMatrix<MatrixXD>& spinv, const std::vector<int>& rowBlockIndices, const std::vector< std::pair<int, int> >& blockIndices)
{
// allocate the sparse
spinv = SparseBlockMatrix<MatrixXD>(&rowBlockIndices[0],
&rowBlockIndices[0],
rowBlockIndices.size(),
rowBlockIndices.size(), true);
_map.clear();
vector<MatrixElem> elemsToCompute;
for (size_t i = 0; i < blockIndices.size(); ++i) {
int blockRow=blockIndices[i].first;
int blockCol=blockIndices[i].second;
assert(blockRow>=0);
assert(blockRow < (int)rowBlockIndices.size());
assert(blockCol>=0);
assert(blockCol < (int)rowBlockIndices.size());
int rowBase=spinv.rowBaseOfBlock(blockRow);
int colBase=spinv.colBaseOfBlock(blockCol);
MatrixXD *block=spinv.block(blockRow, blockCol, true);
assert(block);
for (int iRow=0; iRow<block->rows(); ++iRow)
for (int iCol=0; iCol<block->cols(); ++iCol){
int rr=rowBase+iRow;
int cc=colBase+iCol;
int r = _perm ? _perm[rr] : rr; // apply permutation
int c = _perm ? _perm[cc] : cc;
if (r > c)
swap(r, c);
elemsToCompute.push_back(MatrixElem(r, c));
}
}
// sort the elems to reduce the number of recursive calls
sort(elemsToCompute.begin(), elemsToCompute.end());
// compute the inverse elements we need
for (size_t i = 0; i < elemsToCompute.size(); ++i) {
const MatrixElem& me = elemsToCompute[i];
computeEntry(me.r, me.c);
}
// set the marginal covariance
for (size_t i = 0; i < blockIndices.size(); ++i) {
int blockRow=blockIndices[i].first;
int blockCol=blockIndices[i].second;
int rowBase=spinv.rowBaseOfBlock(blockRow);
int colBase=spinv.colBaseOfBlock(blockCol);
MatrixXD *block=spinv.block(blockRow, blockCol);
assert(block);
for (int iRow=0; iRow<block->rows(); ++iRow)
for (int iCol=0; iCol<block->cols(); ++iCol){
int rr=rowBase+iRow;
int cc=colBase+iCol;
int r = _perm ? _perm[rr] : rr; // apply permutation
int c = _perm ? _perm[cc] : cc;
if (r > c)
swap(r, c);
int idx = computeIndex(r, c);
LookupMap::const_iterator foundIt = _map.find(idx);
assert(foundIt != _map.end());
(*block)(iRow, iCol) = foundIt->second;
}
}
}
void BSplineMotionError<SPLINE_T>::buildHessianImplementation(SparseBlockMatrix & outHessian, Eigen::VectorXd & outRhs, bool /* useMEstimator */) {
// get the coefficients:
Eigen::MatrixXd coeff = _splineDV->spline().coefficients();
// create a column vector of spline coefficients
int dim = coeff.rows();
int seg = coeff.cols();
// build a vector of coefficients:
Eigen::VectorXd c(dim*seg);
// rows are spline dimension
for(int i = 0; i < seg; i++) {
c.block(i*dim,0,dim,1) = coeff.block(0, i, dim,1);
}
// right hand side:
Eigen::VectorXd b_u(_Q.rows()); // number of rows of Q:
b_u.setZero();
/* std::cout <<"b" << std::endl;
for(int i = 0 ; i < b_u->rows(); i++)
std::cout << (*b_u)(i) << std::endl;
std::cout <<"/b" << std::endl; */
_Q.multiply(&b_u, c);
// place the hessian elements in the correct place:
// build hessian:
for(size_t i = 0; i < numDesignVariables(); i++)
{
if( designVariable(i)->isActive()) {
// get the block index
int colBlockIndex = designVariable(i)->blockIndex();
int rows = designVariable(i)->minimalDimensions();
int rowBase = outHessian.colBaseOfBlock(colBlockIndex);
// <- this is our column index
//_numberOfSplineDesignVariables
for(size_t j = 0; j <= i; j++) // upper triangle should be sufficient
{
if (designVariable(j)->isActive()) {
int rowBlockIndex = designVariable(j)->blockIndex();
// select the corresponding block in _Q:
Eigen::MatrixXd* Qblock = _Q.block(i,j, false); // get block and do NOT allocate.
if (Qblock) { // check if block exists
// get the Hessian Block
const bool allocateIfMissing = true;
Eigen::MatrixXd *Hblock = outHessian.block(rowBlockIndex, colBlockIndex, allocateIfMissing);
*Hblock += *Qblock; // insert!
}
}
}
outRhs.segment(rowBase, rows) -= b_u.segment(i*rows, rows);
}
}
//std::cout << "OutHessian" << outHessian.toDense() << std::endl;
// show outRhs:
// for (int i = 0; i < outRhs.rows(); i++)
// std::cout << outRhs(i) << " : " << (*b_u)(i) << std::endl;
}