graph_opt.h

Go to the documentation of this file.

//part of the source code of master thesis, source code produced by Jiri Divis
#ifndef VSLAM_GRAPH_OPT_H_INCLUDED
#define VSLAM_GRAPH_OPT_H_INCLUDED
#include "utils.h"
#include "pose_graph.h"

#include "g2o_types/vertex_pose.h"
#include "g2o_types/vertex_landmark.h"
#include "g2o_types/edge_pose_landmark.h"

#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/factory.h>
#include <g2o/core/optimization_algorithm_factory.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/solvers/pcg/linear_solver_pcg.h>
#include "g2o/solvers/dense/linear_solver_dense.h"
#if defined G2O_HAVE_CHOLMOD
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#elif defined G2O_HAVE_CSPARSE
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#endif


#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <iostream>
#include <cmath>
#include <tr1/functional>



namespace vslam{
/*------------------------------------------------------------------------------------------*/
/*------------------------------------------------------------------------------------------*/
/*------------------------------------------------------------------------------------------*/
namespace g2o_types{}
/*------------------------------------------------------------------------------------------*/

template<class G> class G2OGraphBuilder{
    public:
        typedef typename graph_traits<G>::vertex_descriptor VDT;

        const int first_landmark_id;
        const double max_error;

        G2OGraphBuilder(int _first_landmark_id, double _max_error) : 
                first_landmark_id(_first_landmark_id), max_error(_max_error) {
            
        }

        void operator()(g2o::SparseOptimizer& optimizer, G& g, Points<G>& points, VDT startv){
            buildGraphImpl(optimizer, g, points, startv);
        }

    private:
        virtual void buildGraphImpl(
                g2o::SparseOptimizer& optimizer, G& g, Points<G>& points, VDT startv) = 0;
};





template<class G> class G2OConstActiveDistanceBuilder: public G2OGraphBuilder<G>{
    protected:
        typedef typename graph_traits<G>::vertex_iterator VertexItT;
        typedef typename graph_traits<G>::vertex_descriptor VertexD;

        const int active_window; 
    public:
        G2OConstActiveDistanceBuilder(int _first_landmark_id, double _max_error, int _active_window) : 
                G2OGraphBuilder<G>(_first_landmark_id, _max_error), active_window(_active_window) { }

    private:

        struct Instance{
            g2o::SparseOptimizer& optimizer;
            G& g;
            Points<G>& points;
            vector<bool> point_added;
            std::vector<VertexD> added_vertices;
            vector<int> distances;

            Instance(g2o::SparseOptimizer& _opt, G& _g, Points<G>& _points, VertexD current_pose) : 
                    optimizer(_opt), g(_g), points(_points), point_added(_points.size(), false) {
                typedef boost::iterator_property_map< 
                        vector<int>::iterator, 
                        typename boost::property_map< 
                                G, 
                                boost::vertex_index_t>::type> DistancePMap;
                vector<int> _distances(num_vertices(g));
                DistancePMap dmap(_distances.begin(), get(boost::vertex_index, g));
                boost::dijkstra_shortest_paths(g, current_pose, 
                        boost::weight_map(AllWeightsOneWeightMap<G>()).distance_map(dmap));
                distances=_distances;
            }
        };

        class PoseAgeCompare {
            private:
                const G& g;
            public:
                PoseAgeCompare(const G& _g) : g(_g) {}

                bool operator()(VertexD a, VertexD b)
                {   
                    return get(vertex_timestamp, g, a) > get(vertex_timestamp, g, b);
                }   
        };

        void addLandmarkObservations(int lm_idx, Instance& instance){
            using namespace g2o_types;
            typedef std::pair<VertexD, int> FT;
            const Point<G> & lm = instance.points.getPoint(lm_idx);
            int added_fixed=0;
            for(int i=0; i< static_cast<int>(lm.seen_from.size()); i++){
                FT fid = lm.seen_from[i];
                double dist = instance.distances[get(boost::vertex_index, instance.g, fid.first)];
                bool add=false;
                if(dist <= active_window){
                    add=true;
                } else{
                    if(dist <= 20){
                        if(added_fixed%1 == 0) add=true;
                        added_fixed++;
                    }
                }
                if(add){
                    vector<cv::KeyPoint> & keypoints = get(vertex_kp, instance.g, fid.first);
                    EdgeSE3PointXYZ<G>* observation =  new EdgeSE3PointXYZ<G>;
                    addPose(fid.first, instance, false);
                    observation->setVertex(
                            0,  instance.optimizer.vertex(get(vertex_seq, instance.g, fid.first)));
                    observation->setVertex(
                            1,  instance.optimizer.vertex(this->first_landmark_id+lm_idx));
                    observation->setMeasurement(
                            Vector2d(keypoints[fid.second].pt.x, keypoints[fid.second].pt.y));
                    observation->setInformation(Eigen::Matrix2d::Identity());
                    observation->computeError();
                    if((observation->error()).norm() <= this->max_error){
                        instance.optimizer.addEdge(observation);
                    } else{
                        ROS_WARN_STREAM_NAMED("Optimization", "encountered high error constraint(" 
                                << (observation->error()).norm() << ")"); 
                        break;
                    }
                }
            }
        }

        void addVisibleLandmarks(VertexD vertex, Instance& instance){
            using namespace g2o_types;
            vector<cv::KeyPoint> & keypoints = get(vertex_kp, instance.g, vertex);
            vector<int> & fixed_points = get(vertex_fix_idx, instance.g, vertex);
            for(size_t i = 0; i<keypoints.size(); i++){
                if(fixed_points[i] != BAD_INDEX && 
                        instance.points.getPoint(fixed_points[i]).initialized){
                    if(!instance.point_added[fixed_points[i]]){
                        VertexPointXYZ<G>* feature_landmark = 
                                new VertexPointXYZ<G>(fixed_points[i], instance.points);
                        feature_landmark->setId(this->first_landmark_id+fixed_points[i]);
                        feature_landmark->setEstimate(
                                instance.points.getPoint(fixed_points[i]).global);
                        feature_landmark->setFixed(false);
                        instance.optimizer.addVertex(feature_landmark);
                        instance.point_added[fixed_points[i]]=true;
                        addLandmarkObservations(fixed_points[i], instance);
                    }
                }
            }
        }

        void addPose(VertexD vertex, Instance& instance, bool add_lms){
            using namespace g2o_types;
            if(!instance.optimizer.vertex(get(vertex_seq, instance.g, vertex))){
                VertexSE3<G>* camera_pose=
                    new VertexSE3<G>(instance.g, vertex);
                camera_pose->setId(get(vertex_seq, instance.g, vertex));
                camera_pose->setEstimate(get(vertex_transform, instance.g, vertex));
                camera_pose->setFixed(true);
                instance.optimizer.addVertex(camera_pose);
                if(add_lms) addVisibleLandmarks(vertex, instance);
                ROS_INFO_STREAM_NAMED("Optimization", "Added vertex " << 
                         get(vertex_seq, instance.g, vertex) << " keyframe flag: " << 
                         get(vertex_keyframe_flag, instance.g, vertex));
                instance.added_vertices.push_back(vertex);
            }
        }

        void setPoseFixed(VertexD vertex, Instance& instance, bool fixed){
            instance.optimizer.vertex(get(vertex_seq, instance.g, vertex))->setFixed(fixed);
        }

        virtual void buildGraphImpl(
                g2o::SparseOptimizer& optimizer, G& g, Points<G>& points, VertexD startv){
            Instance instance(optimizer, g, points, startv);

            VertexItT vertex_it, vertex_it_end;
            tie(vertex_it, vertex_it_end) = vertices(instance.g);
            while(vertex_it != vertex_it_end){
                int dist = instance.distances[get(boost::vertex_index, instance.g, *vertex_it)];
                if(dist <= active_window){
                    addPose(*vertex_it, instance, true);
                    setPoseFixed(*vertex_it, instance, false);
                    instance.added_vertices.push_back(*vertex_it);
                    ROS_INFO_STREAM_NAMED("Optimization", "Marked vertex " << 
                             get(vertex_seq, instance.g, *vertex_it) << " as optimizable.");
                }
                ++vertex_it;
            }
            std::sort(instance.added_vertices.begin(), instance.added_vertices.end(), 
                    PoseAgeCompare(instance.g));
            int tmp = static_cast<int>(instance.added_vertices.size());
            for(int i=tmp-2; i< tmp ; i++){
                setPoseFixed(instance.added_vertices[i], instance, true);
            }
        }
};


template<class G> class G2ORunner2{
    protected:
        typedef typename graph_traits<G>::vertex_descriptor VertexD;
        g2o::SparseOptimizer optimizer;

    public:
        G2ORunner2(){
            using namespace g2o;
            typedef BlockSolver< BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> >  SlamBlockSolver;
            typedef LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;
            //typedef LinearSolverPCG<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;
            //typedef LinearSolverCholmod<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;
            //typedef LinearSolverDense<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;

            SlamLinearSolver* linearSolver = new SlamLinearSolver();
            linearSolver->setBlockOrdering(false);
            SlamBlockSolver* blockSolver = new SlamBlockSolver(linearSolver);
            blockSolver->setSchur(false);
            //OptimizationAlgorithmGaussNewton* solver = 
            //        new OptimizationAlgorithmGaussNewton(blockSolver);
            OptimizationAlgorithmLevenberg* solver = 
                    new OptimizationAlgorithmLevenberg(blockSolver);

            optimizer.setAlgorithm(solver);
            optimizer.setComputeBatchStatistics(false);
        }

    public:

        virtual void doOptimalization(
                G& _g, 
                Points<G>& _points,
                G2OGraphBuilder<G> & builder,
                VertexD startv){
            using namespace g2o_types;
            PerformanceLogger<G> pl(_g, startv, "opt");

            ROS_INFO_NAMED("Optimization", "Building pose graph ... ");
            builder(optimizer,_g,_points,startv);
 
            optimizer.setVerbose(false);
            ROS_INFO_NAMED("Optimization", 
                    "Running 15 iterations of the optimalization algorithm ... ");
            optimizer.initializeOptimization();
            //optimizer.setSchur(true);
            optimizer.optimize(15);

            g2o::OptimizableGraph::EdgeContainer all_edges = optimizer.activeEdges();
            double max_error=0;
            BOOST_FOREACH(g2o::OptimizableGraph::Edge* edge_tmp, all_edges){
                EdgeSE3PointXYZ<G>* edge = 
                        dynamic_cast<EdgeSE3PointXYZ<G>*>(edge_tmp);
                edge->computeError();
                if(edge->error().norm() > max_error) max_error=edge->error().norm();
                double err_tolerance = builder.max_error;
                if((edge->error()).norm() >= err_tolerance){
                    ROS_WARN_STREAM_NAMED("Optimization", "high error edge detecded, error norm: " 
                        << (edge->error()).norm() << " errror tolerance: " << err_tolerance);
                }
            }
            ROS_INFO_STREAM_NAMED("Optimization", "Optimization done. Final chi2 is " <<
                optimizer.chi2() << "; Maximal error is "<<max_error << "; Number of edges: " <<
                optimizer.activeEdges().size());
            get(vertex_debug_info, _g, startv).double_propmap["opt_chi2"]= optimizer.chi2();
            get(vertex_debug_info, _g, startv).double_propmap["max_opt_error"]= max_error;
            optimizer.clear(); //clears g2o graph ands saves optimalization results
        }
};



}//namespace vslam



#endif