pe.cpp

Go to the documentation of this file.

//part of the source code of master thesis of Jiri Divis, source code written by Jiri Divis
#include "utils.h"
#include "pose_graph.h"
#include "tests/testutils.h"
#include "data_dump.h"


#include "ros/ros.h"
#include "tf/transform_listener.h"
#include "sensor_msgs/CameraInfo.h"
#include "sensor_msgs/CompressedImage.h"
#include "nav_msgs/Odometry.h"
#include "cv_bridge/cv_bridge.h"
#include "image_transport/image_transport.h"
#include "vis.h"
#include "vo.h"
#include "emat_estimation.h"


#include <vector>
#include <string>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include "graph_opt.h"
#include <boost/filesystem.hpp>

#include "vslam_utils/common.h"

//#include "camera_model.h"

namespace vslam{


std::string image_mask_file;

std::string image_sample_file;

boost::filesystem::path debug_output_path;






template<
        template<template<class> class, class> class VisualOdometryTT, 
        template<class> class NodeBuilderTT,
        class PoseGraphT> 
class VORosInterface{
    protected:
        typedef VisualOdometryTT<NodeBuilderTT, PoseGraphT> VOType;

        ros::NodeHandle & nhandle;
        ros::NodeHandle & nhandle_priv;
        image_transport::ImageTransport it;
        shared_ptr<VOType> vo;

        image_transport::Subscriber sub;
        tf::TransformListener listener; 
        ros::Publisher odometry_pub;

    public:
        VORosInterface(
                ros::NodeHandle & _nhandle, ros::NodeHandle & _nhandle_priv, shared_ptr<VOType> _vo) : 
                nhandle(_nhandle), nhandle_priv(_nhandle_priv), it(_nhandle), vo(_vo){
            sub=it.subscribe("/viz/pano_vodom/image", 2, &VORosInterface::chatterCallback, 
                    this);//, image_transport::TransportHints("compressed"));
            ROS_INFO("subscribed to topic /viz/pano_vodom/image");

            odometry_pub = nhandle.advertise<nav_msgs::Odometry>(
                    "visual_odom", 50);
        }

    protected:
        int seqFromTimestamp(ros::Time t){
            const int range = (int)std::pow(10,4);
            int seq;
            seq = (t.sec % range)*10;
            seq = seq + (t.nsec) / (int)std::pow(10, 8);
            return seq;
        }

        void confirmResolutionOrExit(const cv::Mat & image){
            int w = image.size().width;
            int h = image.size().height;
            if(w!=1600 || h!=800){
                ROS_ERROR_STREAM("Incorrect resolution of input images detected (width: " << 
                        w << ", height: " << h << ")!");
                ros::shutdown();
            }
        }

        void chatterCallback( const sensor_msgs::ImageConstPtr & it_image){
            ROS_INFO("new image recieved");
            cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(it_image, "rgb8");

            bool debug_images;
            bool trajectory_file;
            nhandle_priv.param<bool>("debug_images", debug_images, false);
            nhandle_priv.param<bool>("trajectory_file", trajectory_file, false);
            confirmResolutionOrExit(cv_image->image);

            shared_ptr<SE3> T_wc = vo->processPose(
                    cv_image->image, 
                    seqFromTimestamp(it_image->header.stamp), 
                    it_image->header.stamp.toNSec(),
                    debug_images, 
                    trajectory_file);

            if(T_wc){
                SE3 T_cw = coordFrameConventionLocalToROS(T_wc->inverse());
                geometry_msgs::Pose pose = getPoseFromTcwInSE3(T_cw);

                nav_msgs::Odometry odom;
                odom.pose.pose=pose;
                odom.header=it_image->header;
                if(nhandle_priv.hasParam("odometry_frame_id")){
                    nhandle_priv.getParam("odometry_frame_id", odom.header.frame_id);
                } else{
                    ROS_WARN("odometry_frame_id parameter not set. exitting.");
                    ros::shutdown();
                }

                odometry_pub.publish(odom);
            }
        }
};





typedef CovisibilityKeyframeVisualOdometry<OneDetectorNodeBuilder, PoseGraph> VOType1;
//typedef KeyframeVisualOdometry<OneDetectorNodeBuilder, PoseGraph> VOType1;
//typedef SimpleVisualOdometry<OneDetectorNodeBuilder, PoseGraph> VOType1;
//typedef SimpleOptimizedVisualOdometry<OneDetectorNodeBuilder, PoseGraph> VOType1;

shared_ptr<OneDetectorNodeBuilder<PoseGraph> > buildOrbNBuilder(){
    shared_ptr<cv::ORB> orb;
    //orb.reset(new cv::ORB(10000, 1.15, 4)); 
    orb.reset(new cv::ORB(10000, 1.15, 1)); 
    shared_ptr<OneDetectorNodeBuilder<PoseGraph> > nbuilder;
    nbuilder.reset(new CustomOneDetectorNodeBuilderFeature2D<PoseGraph>(orb));
    return nbuilder;
}

shared_ptr<OneDetectorNodeBuilder<PoseGraph> > buildFASTbRIEFNBuilder(){
    shared_ptr<cv::FeatureDetector> detector;
    shared_ptr<cv::DescriptorExtractor> extractor;
    detector.reset(new cv::DynamicAdaptedFeatureDetector(
            new cv::FastAdjuster(20, false), 2000, 10000, 5));
    extractor.reset(new cv::BriefDescriptorExtractor(64));
    shared_ptr<OneDetectorNodeBuilder<PoseGraph> > nbuilder;
    nbuilder.reset(new CustomOneDetectorNodeBuilder<PoseGraph>(detector, extractor));
    return nbuilder;
}

shared_ptr<VOType1> buildVOObject(){
    shared_ptr<OneDetectorNodeBuilder<PoseGraph> > nbuilder;
    shared_ptr<EdgeBuilder<PoseGraph> > ebuilder;
    {
        nbuilder=buildOrbNBuilder();
        shared_ptr<RobustFeatureMatcher> rmatcher = constructRMatcher(
                shared_ptr<cv::DescriptorMatcher>(new cv::BFMatcher(cv::NORM_HAMMING)),
                shared_ptr<EMatrixModelBuilder>( 
                new LiHartley5ptFrom6ptsBuilderStd((M_PI / 180.0)*1.0)));
        rmatcher->setRatio(0.80);
        rmatcher->setRansacIterationsNum(300);
        ebuilder.reset(new MonoFtrClassEdgeBuilder<PoseGraph>(rmatcher));
    }
    shared_ptr<VOType1> vo;
    vo.reset(new VOType1(nbuilder, ebuilder, 0.8, 3, 
            shared_ptr<VodomLogger<PoseGraph> >(new VertexDebugInfoLogger<PoseGraph>())));
    return vo;
}

}//namespace vslam

int main(int argc, char** argv){
    using namespace vslam;
    ros::init(argc, argv, "visual_odometry");
    ros::NodeHandle nhandle_priv("~");
    ros::NodeHandle nhandle;

    //reading of requred parameters from parameter server.
    getROSParamOrExit(nhandle_priv, "image_mask_file", image_mask_file);
    getROSParamOrExit(nhandle_priv, "image_sample_file", image_sample_file);
    getROSParamAsPathOrExit(nhandle_priv, "debug_output_dir", debug_output_path);

    shared_ptr<VOType1> vo = buildVOObject();
    VORosInterface<CovisibilityKeyframeVisualOdometry, OneDetectorNodeBuilder, PoseGraph> 
            interface(nhandle, nhandle_priv, vo);
    
    ros::spin();
    return 0;
}