object_pose_detection_alg.cpp

Go to the documentation of this file.

#include <list>
#include <time.h>

#include <ros/ros.h>

#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/core/core.hpp>
#include <opencv2/core/types_c.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "object_pose_detection_alg.h"
#include <HoughTransform.hpp>
#include <ransac.hpp>

using namespace std;



ObjectPoseDetectionAlgorithm::ObjectPoseDetectionAlgorithm(void)
{
}

ObjectPoseDetectionAlgorithm::~ObjectPoseDetectionAlgorithm(void)
{
}

void ObjectPoseDetectionAlgorithm::config_update(Config& new_cfg, uint32_t level)
{
        this->lock();

        // save the current configuration
        this->config_=new_cfg;
        //obj_.set_pixel_size(config_.pixel_x)
        
        this->unlock();
}


//My own function to display an image through the screen.
void myDisplayImage(cv::Mat image, std::string title="Window title")
{
        cv::namedWindow(title, CV_WINDOW_NORMAL);
        cv::imshow(title,image);
        cv::waitKey(0);
        cv::destroyWindow(title);
}

//My own function to display matches.
void myPrintMatches(vector<cv::KeyPoint> trainKeyPoints,
                        vector<cv::KeyPoint> testKeyPoints,
                        vector<cv::DMatch> matches)
{
        int m1, m2;
        unsigned int m3;
        m1 = trainKeyPoints.size();
        m2 = testKeyPoints.size();
        m3 = matches.size();
        cout << "Number of train key points: " << m1 << endl;
        cout << "Number of test key points: " << m2 << endl;
        cout << "Number of matches: " << m3 << endl;

        for(unsigned int i=0; i<m3; ++i)
        {
                //cout << matches[i].trainIdx << " -> " << matches[i].queryIdx << endl;
                if(matches[i].trainIdx >= m1 or matches[i].queryIdx >= m2)
                        ROS_ERROR("REFERENCIA DE MATCHES INCORRECTA!");
        }
}

// INITIALIZATION FUNCTIONS ///////////////////////////////////////////////////////////////

bool file_exists(std::string filename)
{
        std::fstream fin;
        fin.open(filename.c_str(),std::ios::in);
        if( fin.is_open() )
        {
                return true;
        }
        fin.close();
        return false;
}

std::vector<std::string> read_file_into_vector(const std::string &initfile)
{
        std::vector<std::string> init;
        std::ifstream pf(initfile.c_str(), std::ios::in);
        if(!pf.is_open()) 
        {
                return init;
        }

        std::string line;
        while(!pf.eof())
        {
                std::getline(pf,line);
                if(line!="") //skipping white lines
                        init.push_back(line);
        }
        return init;
}

std::string trim(const std::string s, bool all=false)
{
        std::string res="";
        if(s[0]==' ')
                res.push_back(s[0]);

        for(uint i=1;i<s.length();i++)
        {
                if(s[i]!=' ')
                        res.push_back(s[i]);
                else if(!all)
                        if(s[i-1]!=' ')
                                res.push_back(s[i]);
        }
        return res;
}

std::string ObjectPoseDetectionAlgorithm::parse_param(const std::vector<std::string>& init, std::string req)
{
        for(std::vector<std::string>::const_iterator line=init.begin(); line!=init.end();line++)
        {
                //std::string trimmedl=trim(*line); //Changed because "eated" the first char of the line.
                std::string trimmedl=*line;
                if(trimmedl[0]=='#')
                        continue;

                std::size_t eqpos=trimmedl.find('=');
                if(eqpos==std::string::npos)
                        continue;

                std::string var=trimmedl.substr(0,eqpos);
                if(var==req)
                {
                        std::string val=trimmedl.substr(eqpos+1);
                        return val;
                }
        }
        return "";
}

bool ObjectPoseDetectionAlgorithm::init_config(const std::string &initfile)
{
        std::vector<std::string> init=read_file_into_vector(initfile);

        //std::string feats_dir_;
        this->feats_dir_=parse_param(init,std::string("feats_dir"));
        if(this->feats_dir_=="")
                this->feats_dir_="train_feats/";

        //std::string train_imas_dir_
        this->train_imas_dir_=parse_param(init,std::string("train_imas_dir"));
        if(this->train_imas_dir_=="")
                this->train_imas_dir_="png/";

        //std::list<std::string> train_files_;
        std::string train_file_list=parse_param(init,std::string("train_file_list"));
        if(train_file_list=="" || !file_exists(train_file_list))
                train_file_list="/home/frigual/Dropbox/PFC/experiments/ortk_inicial/lists/base_train.txt";
        this->train_files_=read_file_into_vector(train_file_list);

        //float distance ratio_
        this->distance_ratio_=fromString<float>(parse_param(init,std::string("distance_ratio")));
        if(this->distance_ratio_<0.01 || this->distance_ratio_>1)
                this->distance_ratio_=0.8;

        //int min_matches
        this->min_matches_=fromString<int>(parse_param(init,std::string("min_matches")));
        if(this->min_matches_<3 || this->min_matches_>25.)
                this->min_matches_=3;

        //std::string test_image_;
        this->test_image_=parse_param(init,std::string("test_image"));
        if(this->test_image_=="")
                this->test_image_="/home/frigual/PFC/datasets/ASL_test_left/img26-L.png";

#ifdef DEBUG
std::cout<<"feats_dir: "<<this->feats_dir_<<std::endl;
std::cout<<"train_imas_dir: "<<this->train_imas_dir_<<std::endl;
std::cout<<"train_files_: "<<train_file_list<<std::endl;
std::cout<<"distance_ratio_: "<<this->distance_ratio_<<std::endl;
std::cout<<"min_matches_: "<<this->min_matches_<<std::endl;
std::cout<<"test_image: "<<this->test_image_<<std::endl;
#endif

        return true;
}


void ObjectPoseDetectionAlgorithm::compute_train_set()
{
        unsigned int counter = 0;

        // For every train image...
        for(std::vector<std::string>::const_iterator file=this->train_files_.begin(); file!=this->train_files_.end(); file++)
        {
                // Skip comments in file.
                if( (*file)[0] == '#')
                        continue;

                // Read image.
                std::string path = train_imas_dir_ + *file + ".png";
                cv::Mat image = cv::imread(path,0);
                if(image.empty())
                {
                        cout << "Can not read image: " << path << endl;
                        exit(-1);
                }
                this->train_imas_.push_back(image);

                // Extract key points.
                std::vector<cv::KeyPoint> keypoints;
                this->detector->detect( image, keypoints );

                // Compute descriptors.
                cv::Mat descriptors;
                this->descriptorExtractor->compute( image, keypoints, descriptors );

                // Save the features. (The folder to save the features has to be previously created)
                cv::FileStorage fs(this->feats_dir_ + *file + ".yml.gz", cv::FileStorage::WRITE);
                cv::write(fs, "keypoints", keypoints);
                cv::write(fs, "descriptors", descriptors);

                // Push the features to the class variables.
                this->train_kpoints_.push_back(keypoints);
                this->train_desc_.push_back(descriptors);

                counter++;
        }

        // Train the matcher with all the descriptors.
        if(not this->descriptorMatcher->empty())
                this->descriptorMatcher->clear();
        this->descriptorMatcher->add(train_desc_);
        this->descriptorMatcher->train();

        ROS_INFO("Computed the features of %d images.", counter);
}


//Unused
void ObjectPoseDetectionAlgorithm::load_train_set()
{
        std::vector<cv::KeyPoint> keypoints;
        cv::Mat descriptors;
        unsigned int counter = 0;

        // For every training image...
        for(std::vector<std::string>::const_iterator file=this->train_files_.begin(); 
                        file!=this->train_files_.end(); 
                        file++)
        {
                // Push the image to the class variable.
                cv::Mat image = cv::imread(train_imas_dir_+*file+".png",0);
                this->train_imas_.push_back(image);

                // Read the keypoints and descriptors from the disk.
                cv::FileStorage fs2(feats_dir_+*file+".yml.gz", cv::FileStorage::READ);
                cv::read(fs2["keypoints"], keypoints);
                cv::read(fs2["descriptors"], descriptors);

                // Push them to the class variables.
                this->train_kpoints_.push_back(keypoints);
                this->train_desc_.push_back(descriptors);

                counter++;
        }

        // Train the matcher with all the descriptors.
        this->descriptorMatcher->add(train_desc_);
        this->descriptorMatcher->train();

        ROS_INFO("Loaded the features of %d images.", counter);
}


// Just like ortk_detect_callback, but to call manually.
// Reads ObjectPoseDetectionAlgorithm::test_image_ to know the path of the image.
void ObjectPoseDetectionAlgorithm::ortk_detect()
{
        cv::Mat testImage = cv::imread(test_image_, 0);

        // Detect keypoints from test image.
        std::vector<cv::KeyPoint> testKeypoints;
        this->detector->detect( testImage, testKeypoints );

        // Extract descriptors.
        cv::Mat testDescriptors;
        this->descriptorExtractor->compute( testImage, testKeypoints, testDescriptors );

        // Compute matches between test images and all the train library.
        vector< vector<cv::DMatch> > matcherOutput;
        this->descriptorMatcher->knnMatch(testDescriptors, matcherOutput, 2);

        ROS_INFO("Detected %d matches from test image to train library.", (int) matcherOutput.size());

        // Now we want to identify which set of matches belong to each image.
        vector< vector<cv::DMatch> > totalMatches(train_imas_.size());
        for(vector< vector<cv::DMatch> >::iterator it = matcherOutput.begin(); it != matcherOutput.end(); ++it)
                // Only if it's much better than the others.
                if( (*it)[0].distance / (*it)[1].distance < this->distance_ratio_ )
                        totalMatches[(*it)[0].imgIdx].push_back( (*it)[0] );

        // For every train image...
        for(unsigned int iImage = 0; iImage < train_imas_.size(); ++iImage)
        {
                vector<cv::DMatch> initialMatches = totalMatches[iImage];
                cv::Mat trainImage = train_imas_[iImage];
                ROS_INFO("For the #%d image, %d initial matches have been found.", iImage, (int) initialMatches.size());

                // If enough matches...
                if(initialMatches.size() >= this->min_matches_)
                {
                        // GHT
                        //   Declare output variables.
                        HoughTransform hough;
                        vector<HoughTransform::Hypothesis> myHypotheses;
                        //   Call GHT
                        int numHoughHypotheses = hough.computeTransform(trainImage.cols,
                                                trainImage.rows,
                                                train_kpoints_[iImage],
                                                testKeypoints,
                                                initialMatches,
                                                myHypotheses);
                        //   Print results.
                        ROS_INFO("We have %d hough hypotheses", numHoughHypotheses);
                        if(numHoughHypotheses > 0)
                        {
                                //   Display matches.
                                cv::Mat displayHough;
                                for(uint iHyp = 0; iHyp < 1; ++iHyp)
                                {
                                        cv::drawMatches(testImage, testKeypoints, train_imas_[iImage], train_kpoints_[iImage], myHypotheses[iHyp].matches, displayHough, cv::Scalar( 0, 255, 0), cv::Scalar( 0, 255, 0), vector<char>(), 6);
                                        myDisplayImage(displayHough, "GHT matches");
                                }

                                // RANSAC
                                //   Declare output variables.
                                list<cv::Mat> affineSolutions;
                                list<pair<float, int> > errorMeasures;
                                vector< vector<cv::DMatch> > ransacMatches;
                                //   Call RANSAC and IRLS.
                                computeAffHyp(train_kpoints_[iImage], testKeypoints, myHypotheses, affineSolutions, errorMeasures, true, true, ransacMatches);
                                //   Display matches.
                                for(uint iSolution = 0; iSolution<ransacMatches.size(); ++iSolution)
                                {
                                        cv::Mat displayRansac;
                                        cv::drawMatches(testImage,
                                                                testKeypoints,
                                                                train_imas_[iImage],
                                                                train_kpoints_[iImage],
                                                                ransacMatches[iSolution],
                                                                displayRansac,
                                                                cv::Scalar( 0, 255, 0),
                                                                cv::Scalar( 0, 255, 0),
                                                                vector<char>(),
                                                                6);
                                        myDisplayImage(displayRansac, "Ransac matches");
                                }
                                //   Display resulting bounding boxes.
                                displayBox(train_imas_[iImage], testImage, affineSolutions);
                        }
                }
        }
}


ObjectPoseDetectionAlgorithm::ObjectPoseDetectionAlgorithm()
{
/*      if(argc != 5)
        {
                cout << "Usage: rosrun ortk ortk pathToConfigFile detectorType descriptorType matcherType\n\n"
                     << "Possible detectorType values: see in documentation on createFeatureDetector().\n"
                     << "Possible descriptorType values: see in documentation on createDescriptorExtractor().\n"
                     << "Possible matcherType values: see in documentation on createDescriptorMatcher().\n\n"
                     << "Example: rosrun ortk ortk ~/PFC/experiments/ortk_inicial/config_file.txt SIFT SIFT FlannBased\n"
                     << "config-file.txt example:\n"
                     << "train_file_list=~/base_train.txt\n"
                     << "train_imas_dir=~/tex_book/\n"
                     << "feats_dir=~/features/\n";
                ROS_ERROR("Error: Wrong number of params!");
                exit(-1);
        }
*/
        // Parameters configuration.
        //   Config file.
        std::string initfile(argv[1]);
        if(!init_config(initfile))
        {
                ROS_ERROR("Error: Bad config file");
                exit(-1);
        }
        //   Feature Detector
        this->detector = cv::FeatureDetector::create( argv[2] );
        if(detector.empty())
        {
                ROS_ERROR("Error: Couldn't create feature detector.");
                exit(-1);
        }
        //   Descriptor Extractor
        this->descriptorExtractor = cv::DescriptorExtractor::create( argv[3] );
        if(descriptorExtractor.empty())
        {
                ROS_ERROR("Error: Couldn't create descriptor extractor.");
                exit(-1);
        }
        //   Descriptor Matcher
        this->descriptorMatcher = cv::DescriptorMatcher::create( argv[4] );
        if(descriptorMatcher.empty())
        {
                ROS_ERROR("Error: Couldn't create descriptor matcher.");
                exit(-1);
        }

        // Loading training data.
        this->compute_train_set();
        //this->load_train_set(); // Not used since we always compute the features.
}