PoseMap hogman_solver(KinematicGraphType &graph, PoseIndex &poseIndex, PoseMap &observed, PoseMap &predictedEmpty, double sigma_position, double sigma_orientation, double timestamp) { bool visualize = false; bool verbose = false; bool guess = 0; int iterations = 10; GraphOptimizer3D* optimizer = new CholOptimizer3D(); optimizer->verbose() = verbose; optimizer->visualizeToStdout() = visualize; optimizer->guessOnEdges() = guess; optimizer->clear(); // reference tf::Transform root = poseToTransform(observed.begin()->second); int n=1000; // add predicted markers for (PoseMap::iterator it = predictedEmpty.begin(); it != predictedEmpty.end(); it++) { tf::Transform m = poseToTransform(it->second); add_vertex(optimizer, it->first, root.inverseTimes(m)); } // add observed markers for (PoseMap::iterator it = observed.begin(); it != observed.end(); it++) { tf::Transform m = poseToTransform(it->second); add_vertex(optimizer, it->first + n, root.inverseTimes(m)); } for (KinematicGraphType::iterator it = graph.begin(); it != graph.end(); it++) { int idx = poseIndex[it->first.first][it->first.second][timestamp]; Pose& pose_pred = it->second->model.track.pose_projected[idx]; ROS_ASSERT(!(isnan(pose_pred.position.x) || isnan(pose_pred.position.y) || isnan(pose_pred.position.z) || isnan(pose_pred.orientation.x) || isnan(pose_pred.orientation.y) || isnan(pose_pred.orientation.z)|| isnan(pose_pred.orientation.w))); add_edge(optimizer, it->first.first, it->first.second, poseToTransform( pose_pred), sigma_position, sigma_orientation); } // add strong edges between observed markers (actually, the observed markers should be kept fixed) for (PoseMap::iterator it = observed.begin(); it != observed.end(); it++) { if (it == observed.begin()) it++;// skip first // relative transformation between marker 0 and marker it tf::Transform m = poseToTransform(observed.begin()->second).inverseTimes( poseToTransform(it->second)); add_edge(optimizer, observed.begin()->first + n, it->first + n, m, sigma_position / 100, sigma_orientation / 100); } // add weak edges between predicted markers and observed markers (keep them in place) for (PoseMap::iterator it = observed.begin(); it != observed.end(); it++) { // relative transformation between marker 0 and marker it tf::Transform m = tf::Transform::getIdentity(); add_edge(optimizer, it->first, it->first + n, m, sigma_position * 100, sigma_orientation * 100); } optimizer->initialize(0); optimizer->optimize(iterations, false); PoseMap predictedMarkers; GraphOptimizer3D::VertexIDMap &vertices = optimizer->vertices(); for (PoseMap::iterator it = predictedEmpty.begin(); it != predictedEmpty.end(); it++) { const PoseGraph3D::Vertex* v; v= reinterpret_cast<const PoseGraph3D::Vertex*>(vertices[it->first]); if (v == 0) { cerr << "VERTEX not found!!" << endl; } Transformation3 t = v->transformation; Vector6 p = t.toVector(); tf::Transform d = tf::Transform(RPY_to_MAT(p[3], p[4], p[5]), tf::Vector3( p[0], p[1], p[2])); predictedMarkers[it->first] = transformToPose(root * d); } const PoseGraph3D::Vertex* v = reinterpret_cast<const PoseGraph3D::Vertex*>(vertices[observed.begin()->first + n]); Transformation3 t = v->transformation; Vector6 p = t.toVector(); tf::Transform d = tf::Transform(RPY_to_MAT(p[3], p[4], p[5]), tf::Vector3(p[0], p[1], p[2])); for (PoseMap::iterator it = predictedMarkers.begin(); it != predictedMarkers.end(); it++) { // now correct it->second = transformToPose(root * d.inverseTimes(root.inverseTimes( poseToTransform(it->second)))); } delete optimizer; return predictedMarkers; }
int main(int argc, char** argv) { if (argc<2) { printBanner(); return 0; } bool visualize = false; bool overrideCovariances = false; int iterations = 10; bool verbose = false; bool incremental = true; bool guess = 0; int optType = OPT_CHOL; int updateGraphEachN = 10; int updateVisualizationEachN = 25; char* filename = 0; char* gnudump = 0; char* outfilename = 0; GraphOptimizer3D* optimizer = 0; int numLevels = 3; int nodeDistance = 2; // command line for (int c = 1; c < argc; ++c) { if (! strcmp(argv[c],"-chol")) { optimizer = new CholOptimizer3D(); optType = OPT_CHOL; } else if (! strcmp(argv[c],"-hchol")) { optimizer = new HCholOptimizer3D(numLevels, nodeDistance); optType = OPT_HCHOL; } else if (! strcmp(argv[c],"-v")) { verbose=true; } else if (! strcmp(argv[c],"-batch")) { incremental = false; } else if (! strcmp(argv[c],"-update")) { c++; updateGraphEachN = atoi(argv[c]); } else if (! strcmp(argv[c],"-guiout")) { visualize = true; } else if (! strcmp(argv[c],"-o")) { c++; outfilename=argv[c]; } else if (! strcmp(argv[c],"-gnudump")) { c++; gnudump=argv[c]; } else if (! strcmp(argv[c],"-i")) { c++; iterations=atoi(argv[c]); } else if (! strcmp(argv[c],"-guess")) { guess = true; } else if (! strcmp(argv[c],"-oc")) { overrideCovariances = true; } else if (! strcmp(argv[c],"-h")) { printBanner(); return 0; } else { filename=argv[c]; } } if (!optimizer) { optimizer = new HCholOptimizer3D(numLevels, nodeDistance); optType = OPT_HCHOL; } if (verbose && optType==OPT_HCHOL) { cerr << "WARNING: " << endl; cerr << "You selected the verbose option and the hogman mode" << endl; cerr << "This does not make sense and I will ignore this option" <<endl; verbose = false; } if (optType==OPT_HCHOL && ! incremental) { cerr << "WARNING: " << endl; cerr << "You selected the batch mode for hogman." << endl; cerr << "This version of HOGMAN is made for on-line operation, not for off-line." << endl; cerr << "This is an unsupported feature, and it will be slower than standard Cholesky." << endl; return 0; } ifstream is(filename); if (!is) { cerr << "Error opening " << filename << endl; return 1; } cerr << "# Optimizer started, Parameter summary:" << endl; cerr << "# strategy= " << optimizer2string(optType) << endl; cerr << "# verbose= " << verbose << endl; cerr << "# update= " << updateGraphEachN << endl; cerr << "# iterations= " << iterations << endl; cerr << "# verbose= " << verbose << endl; cerr << "# outfile= " << ((outfilename)? outfilename : "not set") << endl; cerr << "# infile= " << ((filename)? filename : "not set") << endl; cerr << "# incemental= " << incremental << endl; cerr << "# initial guess= " << guess << endl; // set the optimizer setting optimizer->verbose() = verbose; optimizer->visualizeToStdout() = visualize; optimizer->guessOnEdges() = incremental; if (incremental) { ofstream stat_fs("stat3d.dat"); int vertexCount=0; optimizer->visualizeToStdout() = false; optimizer->verbose() = false; cerr << "# Loading Edges ... "; LoadedEdgeSet3D loadedEdges; loadEdges3D(loadedEdges, is, overrideCovariances); cerr << "done." << endl; struct timeval ts, te; double cumTime=0; bool addNextEdge=true; bool freshlyOptimized=false; int count=0; for (LoadedEdgeSet3D::const_iterator it = loadedEdges.begin(); it != loadedEdges.end(); ++it) { bool optimize=false; if (addNextEdge && !optimizer->vertices().empty()) { int maxInGraph = optimizer->vertices().rbegin()->first; int idMax = max(it->id1, it->id2); if (maxInGraph < idMax && ! freshlyOptimized) { addNextEdge=false; optimize=true; count++; } else { addNextEdge=true; optimize=false; } } PoseGraph3D::Vertex* v1 = optimizer->vertex(it->id1); PoseGraph3D::Vertex* v2 = optimizer->vertex(it->id2); if (! v1 && addNextEdge) { //cerr << " adding vertex " << it->id1 << endl; v1 = optimizer->addVertex(it->id1, Transformation3(), Matrix6::eye(1.0)); assert(v1); vertexCount++; } if (! v2 && addNextEdge) { //cerr << " adding vertex " << it->id2 << endl; v2 = optimizer->addVertex(it->id2, Transformation3(), Matrix6::eye(1.0)); assert(v2); vertexCount++; } if (addNextEdge) { //cerr << " adding edge " << it->id1 << " " << it->id2 << " " << it->mean << endl; optimizer->addEdge(v1, v2, it->mean, it->informationMatrix); } freshlyOptimized=false; if (optimize) { //cerr << "Optimize" << endl; if (vertexCount >= updateGraphEachN) { gettimeofday(&ts, 0); int currentIt=optimizer->optimize(iterations, true); gettimeofday(&te,0); double dts=(te.tv_sec-ts.tv_sec)+1e-6*(te.tv_usec-ts.tv_usec); 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; } stat_fs << "nodes= " << optimizer->vertices().size() << " edges= " << optimizer->edges().size() << " time= " << dts << " cumTime= " << cumTime << " chi2= " << optimizer->chi2() << endl; vertexCount=0; } // update visualization if (visualize && !(count % updateVisualizationEachN) ) { HCholOptimizer3D* opt=dynamic_cast<HCholOptimizer3D*>(optimizer); if (0 && opt) { opt=opt->level(opt->nLevels()-1); opt->visualizeToStream(cout); } else { optimizer->visualizeToStream(cout); } } if (! verbose) cerr << "."; addNextEdge=true; freshlyOptimized=true; it--; count++; } } // for all edges if (visualize) { // visualize the final state optimizer->visualizeToStream(cout); } cerr << "**** Optimization Done ****" << endl; cerr << "TOTAL TIME= " << cumTime << " s." << endl; cerr << "# final chi=" << optimizer->chi2() << endl; } else { optimizer->guessOnEdges() = guess; struct timeval ts, te; optimizer->load(is, overrideCovariances); if (! optimizer->initialize(0)) { cerr << "error in initialization" << endl; } cerr << "# initial chi=" << optimizer->chi2() << endl; gettimeofday(&ts,0); optimizer->optimize(iterations, false); gettimeofday(&te,0); cerr << "**** Optimization Done ****" << endl; double dts=(te.tv_sec-ts.tv_sec)+1e-6*(te.tv_usec-ts.tv_usec); cerr << "# final chi=" << optimizer->chi2() << endl; cerr << "TOTAL TIME= " << dts << " s." << endl; } if (outfilename) { cerr << "Saving Graph to " << outfilename << " ... "; ofstream fout(outfilename); optimizer->save(fout); fout.close(); cerr << "done." << endl; } if (gnudump) { cerr << "Saving Data to " << gnudump << " ... "; ofstream fout(gnudump); optimizer->saveGnuplot(fout); fout.close(); cerr << "done." << endl; } // clean up delete optimizer; return 0; }