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;
}
