vis.h

Go to the documentation of this file.

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


#include "utils.h"
#include "pose_graph.h"
//#include "feature_track_graph.h"

void drawRaysOnEpipolarPlane(
        const Matrix3d & E,
        const ImageMatchVec & matches,
        cv::Mat & img1,
        cv::Mat & img2);

//void drawKeypoints(const vector<cv::KeyPoint> & keypoints, cv::Mat & image);
//void drawKeypoints(const ImageCoordinateVec & keypoints, cv::Mat & image);

namespace vslam{
/*------------------------------------------------------------------------------------------*/
/*------------------------------------------------------------------------------------------*/


/*template<class PG> void drawVOTracks(
        const cv::Mat& image_in, cv::Mat& image_out, typename graph_traits<PG>::vertex_descriptor vd,
        const Points<PG>& points, const PG& pg){
    typename pg_traits<PG>::ftr_idx_iterator ftr_it, ftr_it_end;
    tie(ftr_it, ftr_it_end) = node_feature_idxs(vd, pg);
    while(ftr_it != ftr_it_end){
        FeatureTrackGraph<PG> ftg(pg, points, vd, *ftr_it);
        typedef typename graph_traits<FeatureTrackGraph<PG> >::edge_iterator EI;
        EI edge_it, edge_it_end;
        tie(edge_it, edge_it_end) = edges(ftg);
        while(edge_it != edge_it_end){
            typename graph_traits<PG>::vertex_descriptor sv, tv;
            int sidx, tidx;
            
            typename graph_traits<PG>::vertex_descriptor sv, tv
            sv=cvtToVertexDesc<PG>(get(vertex_pgvd, ftg, source(*edge_it, ftg)));
            int sidx = get(vertex_ftr_idx, ftg, source(*edge_it, ftg));
            tv = cvtToVertexDesc<PG>(get(vertex_pgvd, ftg, target(*edge_it, ftg)));
            int tidx = get(vertex_ftr_idx, ftg, target(*edge_it, ftg));

            cv::KeyPoint skp = get(vertex_kp, pg, sv)[sidx];
            cv::KeyPoint dkp = get(vertex_kp, pg, tv)[tidx];
            cv::Point center1(skp.pt.x, skp.pt.y);
            cv::Point center2(dkp.pt.x, dkp.pt.y);
            cv::line(image_out, center1, center2, cv::Scalar(0,0,255), 1, CV_AA);
            ++edge_it;
        }
        ++ftr_it;
    }
}*/

template<class G> void edgeVisualization(const cv::Mat& img1, const cv::Mat& img2, G& graph, 
        typename graph_traits<G>::vertex_descriptor vd1,
        typename graph_traits<G>::vertex_descriptor vd2){
    cv::Mat image1, image2;
    image1=img1.clone();
    image2=img2.clone();
    SE3 computedT = get(edge_transform, graph, edge(vd1, vd2, graph).first);
    drawMatchedNodeKeypoints(image1, vd1, vd2, graph, image1);
    drawMatchedNodeKeypoints(image2, vd2, vd1, graph, image2);
    ImageMatchVec matches = computeMatches(vd1, vd2, graph);
    drawRaysOnEpipolarPlane(sE3ToE(computedT), matches, image1, image2);
    cv::imwrite(seqencedName(get(vertex_seq, graph, vd1), "epilines.jpg"), image1);
    cv::imwrite(seqencedName(get(vertex_seq, graph, vd2), "epilines.jpg"), image2);
}





template<class G> void drawNodeKeypoints( typename graph_traits<G>::vertex_descriptor vd, 
        const G& g, const cv::Mat& image, cv::Mat& out_image){
    cv::drawKeypoints(image, get(vertex_kp, g, vd), out_image, cv::Scalar(0,255,0));
}


template<class G> void drawMatchedNodeKeypoints(
        const cv::Mat& image,
        typename graph_traits<G>::vertex_descriptor vd_img, 
        typename graph_traits<G>::vertex_descriptor matched_vd,
        const G& g,
        cv::Mat out_image){
    const vector<cv::KeyPoint>&  kpts = get(vertex_kp, g, vd_img);
    list<int> mkp_idxs = transformF<int>(
            mapToList(get(edge_kp_idx_map,g,edge(vd_img, matched_vd, g).first)), 
            getFirst);
    list<int> mkp_inv_idxs= inverse_idxs(mkp_idxs, makeSeq(0, (int)kpts.size() - 1, 1)); 
    vector<cv::KeyPoint> mkpts = switchCont<vector>(transformF<cv::KeyPoint>(
            getByIdx(indexBy(makeSeq(0, (int)kpts.size() - 1, 1), kpts), mkp_idxs), 
            getSecond));
    vector<cv::KeyPoint> nkpts = switchCont<vector>(transformF<cv::KeyPoint>(
            getByIdx(indexBy(makeSeq(0, (int)kpts.size() - 1, 1), kpts), mkp_inv_idxs), 
            getSecond));

    cv::drawKeypoints(image, mkpts, out_image, cv::Scalar(0,255,0));//, 
            //cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
    cv::drawKeypoints(image, nkpts, out_image, cv::Scalar(150,150,150));//, 
            //cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
}

template<class G>
void drawMatches(const cv::Mat & srci, typename graph_traits<G>::vertex_descriptor srcv, 
        const cv::Mat & desti, typename graph_traits<G>::vertex_descriptor destv, const G& g){
    cv::Mat imageMatches;
    
    vector<cv::DMatch> dmatches;
    const map<int, int> & src_dst_map = get(edge_kp_idx_map, g, edge(srcv, destv, g).first);
    std::transform(src_dst_map.begin(), src_dst_map.end(), back_inserter(dmatches), dmatchFromPair);

    cv::drawMatches(
            srci, get(vertex_kp, g, srcv),  // 1st image and its keypoints
            desti, get(vertex_kp, g, destv),  // 2nd image and its keypoints
            dmatches,                   // the matches
            imageMatches,               // the image produced
            cv::Scalar(255,255,255)); // color of the lines
    std::ostringstream s;
    s << get(vertex_seq, g, srcv) << "to" << get(vertex_seq, g, destv) << "_" << "matches.jpg";
    cv::imwrite(s.str(), imageMatches);
}

template<class G>
void drawMatches2(cv::Mat & srci, typename graph_traits<G>::vertex_descriptor srcv, 
        cv::Mat & desti, typename graph_traits<G>::vertex_descriptor destv, const G& g){
    
    vector<cv::DMatch> dmatches;
    const map<int, int> & src_dst_map = get(edge_kp_idx_map, g, edge(srcv, destv, g).first);
    std::transform(src_dst_map.begin(), src_dst_map.end(), back_inserter(dmatches), dmatchFromPair);

    typename property_map<G, vertex_kp_t>::const_type vkpmap = get(vertex_kp, g);

    for(size_t i=0; i<dmatches.size(); i++){
        cv::Point2d pt1 = vkpmap[srcv][dmatches[i].queryIdx].pt;
        cv::Point2d pt2 = vkpmap[destv][dmatches[i].trainIdx].pt;
        cv::line(srci, pt1, pt2, CV_RGB(0,0,255), 1, 4);
        cv::line(desti, pt1, pt2, CV_RGB(0,0,255), 1, 4);
    }
}

template<class G>
void drawMatches3(cv::Mat & srci, typename graph_traits<G>::vertex_descriptor srcv, 
        cv::Mat & desti, typename graph_traits<G>::vertex_descriptor destv, const G& g, 
        const Points<G>& points){
    
    vector<cv::DMatch> dmatches;
    const map<int, int> & src_dst_map = get(edge_kp_idx_map, g, edge(srcv, destv, g).first);
    std::transform(src_dst_map.begin(), src_dst_map.end(), back_inserter(dmatches), dmatchFromPair);

    typename property_map<G, vertex_kp_t>::const_type vkpmap = get(vertex_kp, g);
    typename property_map<G, vertex_fix_idx_t>::const_type vfixmap = get(vertex_fix_idx, g);

    for(size_t i=0; i<dmatches.size(); i++){
        int v1idx = dmatches[i].queryIdx;
        int v2idx = dmatches[i].trainIdx;
        if(vfixmap[srcv][v1idx]!=BAD_INDEX && points.getPoint(vfixmap[srcv][v1idx]).initialized){ 
            if(vfixmap[destv][v2idx]!=BAD_INDEX && points.getPoint(vfixmap[destv][v2idx]).initialized){
                cv::Point2d pt1 = vkpmap[srcv][v1idx].pt;
                cv::Point2d pt2 = vkpmap[destv][v2idx].pt;
                cv::line(srci, pt1, pt2, CV_RGB(0,0,255), 1, 4);
                cv::line(desti, pt1, pt2, CV_RGB(0,0,255), 1, 4);
            }
        }
    }
}


inline void drawMyKpt(cv::Mat & image, cv::Point pt, cv::Scalar color){
    cv::circle(image,pt,3,cv::Scalar(255,255,255),1, CV_AA);
    cv::circle(image,pt,2,cv::Scalar(255,255,255),2, CV_AA);
    cv::circle(image,pt,2,color, 1, CV_AA);
    cv::circle(image,pt,1,color, 2, CV_AA);
}


template<class G>
void drawLandmarkReprojections(
        cv::Mat & image, 
        typename graph_traits<G>::vertex_descriptor vd, 
        const G& g, 
        const Points<G>& points){
    typedef std::vector<cv::KeyPoint> TKP;
    typedef std::vector<int> TIDX;
    const TKP& kpts = get(vertex_kp, g, vd);
    const TIDX& landmark_indexes = get(vertex_fix_idx, g, vd);
    
    TKP::const_iterator kp_it = kpts.begin();
    TIDX::const_iterator li_it = landmark_indexes.begin();
    while(kp_it != kpts.end()){
        assert(li_it!=landmark_indexes.end());
        if(*li_it!=BAD_INDEX){
            Point<G> pt = points.getPoint(*li_it);
            if(pt.initialized){
                drawMyKpt(image, kp_it->pt, cv::Scalar(255,255,255));
                const SE3& T_wc = get(vertex_transform, g, vd);
                Vector2d reproj_pt0 = PanoImCoord::fromHomogenous(T_wc*pt.global).getPixel();
                cv::Point reproj_pt(reproj_pt0(0), reproj_pt0(1));
                //drawMyKpt(image, reproj_pt);
                cv::line(image, kp_it->pt, reproj_pt,cv::Scalar(255, 255, 255), 2, CV_AA);
                cv::line(image, kp_it->pt, reproj_pt,cv::Scalar(0, 0, 0), 1, CV_AA);
            } else{
                drawMyKpt(image, kp_it->pt, cv::Scalar(255,0,0));
            }
        }
        kp_it++; li_it++;
    }
}
}
#endif