data_dump.h

Go to the documentation of this file.

//part of the source code of master thesis, source code produced by Jiri Divis
#ifndef VSLAM_DATA_DUMP_H_INCLUDED
#define VSLAM_DATA_DUMP_H_INCLUDED

#include "utils.h"
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/depth_first_search.hpp>
#include "vslam_utils/csv_matrix_io.h"
#include <boost/filesystem.hpp>
#define VSLAM_DUMP_DELIM ';'

namespace vslam{

/*------------------------------------------------------------------------------------------*
 * scene serialization
 *------------------------------------------------------------------------------------------*/

template<class PointsIt, class TrajectoryIt>
void readTrajectory(std::istream & in, PointsIt points,  TrajectoryIt trajectory){
    using namespace vslam_utils;
    int num_pt, traj_sz;
    std::string useless;
    in >> useless;
    in >> num_pt;
    Vector3d vec;
    Matrix3d mat;
    for(int i=0; i < num_pt; i++){
        readVec(in, vec);
        *points=vec;
        ++points;
    }
    in >> useless;
    in >> traj_sz;
    for(int i=0; i < traj_sz; i++){
        readMatrix(in, mat);
        readVec(in, vec);
        *trajectory=SE3(mat, vec);
        ++trajectory;
    }
}

template<class PointsIt, class TrajectoryIt>
void writeTrajectory(std::ostream & out, PointsIt points_begin, PointsIt points_end,
        TrajectoryIt trajectory_begin, TrajectoryIt trajectory_end, int id){
    using namespace vslam_utils;
    writePoints(out, points_begin, points_end);
    out << "trajectory" << endl;
    out << count_if(trajectory_begin, trajectory_end, truePred<SE3>) << endl;
    while(trajectory_begin != trajectory_end){
        writeMatrix(out, trajectory_begin->rotation_matrix());
        writeDelim(out);
        writeMatrix(out, trajectory_begin->translation());
        ++trajectory_begin;
        out << endl;
    }
}



/*------------------------------------------------------------------------------------------*
 * trajectory dump for visualization
 *------------------------------------------------------------------------------------------*/


namespace details{
    ImageMatch preparePoseArrow(const SE3& T_cw){
        Vector3d vec1, vec2;
        vec1 = T_cw * Vector3d(0,0,0);
        vec2 = T_cw * Vector3d(0,0,1);
        return (make_pair(vec1, vec2-vec1));
    }

    template<class G> class DrawGraphDFSVisitor : public boost::default_dfs_visitor{
        protected:
            ImageMatchVec& line_segments;
            ImageMatchVec& pose_arrows;
        public:
            DrawGraphDFSVisitor(ImageMatchVec& _line_segments, ImageMatchVec& _pose_arrows) : 
                    line_segments(_line_segments), pose_arrows(_pose_arrows) {}

            void examine_edge(typename graph_traits<G>::edge_descriptor e, const G& g){
                typename graph_traits<G>::vertex_descriptor v1, v2;
                v1 = source(e, g);
                v2 = target(e, g);
                typename boost::property_map<G,vertex_seq_t>::const_type seq_pmap = 
                        get(vertex_seq, g);
                if(seq_pmap[v1] < seq_pmap[v2]){
                    SE3 T_v2_w = get(vertex_transform, g, v2).inverse();
                    SE3 T_v1_w = get(vertex_transform, g, v1).inverse();
                    line_segments.push_back(
                            make_pair(T_v1_w * Vector3d::Zero(), T_v2_w * Vector3d::Zero()));
                }
            }

            void discover_vertex(typename graph_traits<G>::vertex_descriptor v, const G& g){
                pose_arrows.push_back(preparePoseArrow(get(vertex_transform, g, v).inverse()));
            }
    };
}

template<class G> ImageMatchVec votrack(G& g){
    vector<Vector3d> poses; //XXX alignment!

    int last_seq=-1;
    while(true){
        int min_seq=10000;
        typename graph_traits<G>::vertex_iterator vertex_it, vertex_it_end, tmp_it;
        tie(vertex_it, vertex_it_end) = vertices(g);

        while(vertex_it!=vertex_it_end){
            int seq = get(vertex_seq, g, *vertex_it);
            if( seq > last_seq && seq < min_seq){
                min_seq = seq;
                tmp_it=vertex_it;
            }
            ++vertex_it;
        }
        if(min_seq==10000) break;
        last_seq=min_seq;
        poses.push_back(get(vertex_transform, g, *tmp_it).inverse()*Vector3d(0,0,0));
    }

    ImageMatchVec line_segments;
    if(poses.size() >= 2){
        vector<Vector3d>::const_iterator it = poses.begin() + 1;
        while(it!=poses.end()){
            line_segments.push_back(make_pair(*(it-1), *it));
            ++it;
        }
    }
    return line_segments; 
}






template<class G> void drawGraph(G& g){
    using namespace vslam_utils;
    ImageMatchVec line_segments, pose_arrows, line_segments2;
    details::DrawGraphDFSVisitor<G> vis(line_segments, pose_arrows);
    boost::depth_first_search(g, boost::visitor(vis));
    boost::filesystem::path p(debug_output_path.native());
    p /= "scene.csv";
    std::ofstream f(p.c_str());
    writeVectorPairs(f, line_segments.begin(), line_segments.end());
    writeVectorPairs(f, pose_arrows.begin(), pose_arrows.end());

    line_segments2 = votrack(g);
    writeVectorPairs(f, line_segments2.begin(), line_segments2.end());
}

template<class TrIt> void writeTrajectory(
        std::ostream& out, TrIt trajectory_begin, TrIt trajectory_end){
    using namespace vslam_utils;
    TrIt tr_it = trajectory_begin + 1;
    ImageMatchVec tmp;
    ImageMatchVec tmp2;
    tmp2.push_back(preparePoseArrow(*trajectory_begin));
    while(tr_it!=trajectory_end){
        SE3 T_c1w = (tr_it-1)->inverse();
        SE3 T_c2w = tr_it->inverse();
        tmp.push_back(make_pair(T_c1w*Vector3d::Zero(), T_c2w*Vector3d::Zero()));
        tmp2.push_back(preparePoseArrow(tr_it->inverse()));
        ++tr_it;
    }
    writeVectorPairs(out, tmp.begin(), tmp.end());
    writeVectorPairs(out, tmp2.begin(), tmp2.end());
}

/*------------------------------------------------------------------------------------------*
 * data dumps mooved from testutils
 *------------------------------------------------------------------------------------------*/
template<class PtIt> void displayMatches(
        std::ostream& out, PtIt points_begin, PtIt points_end, const SE3& T_cw){
    using namespace vslam_utils;
    PtIt pt_it = points_begin;
    ImageMatchVec tmp;
    while(pt_it != points_end){
        tmp.push_back(make_pair(T_cw*Vector3d::Zero(), T_cw*(*pt_it)));
        ++pt_it;
    }
    writeVectorPairs(out, tmp.begin(), tmp.end());
}

template<class PtIt> void  displayScene(std::ostream& out, PtIt points_begin, PtIt points_end){
    writePoints(out, points_begin, points_end);
}
}//namespace vslam


#endif