예제 #1
0
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;
}
예제 #2
0
파일: hogman3d.cpp 프로젝트: weeks-yu/WReg
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;
}