vws_alg.cpp

Go to the documentation of this file.

#include "vws_alg.h"

VwsAlgorithm::VwsAlgorithm(void)
{
}

VwsAlgorithm::~VwsAlgorithm(void)
{
}

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

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

// VwsAlgorithm Public API
cv::Mat VwsAlgorithm::descriptors_srv_to_mat(iri_perception_msgs::DescriptorsToVws::Request &req)
{
//         cv::Mat descriptors;
        cv::Mat descriptors;
        descriptors = cv::Mat::zeros(req.descriptor_set.num, req.descriptor_set.len, CV_32FC1); // (rows, cols)
        
        for(int i=0; i < req.descriptor_set.num; i++) {
                for(int j=0; j< req.descriptor_set.len; j++) {
                        descriptors.at<float>(i,j) = (float)req.descriptor_set.descriptors[i].descriptor[j];
                }
        }
        return descriptors;
}


iri_perception_msgs::GeoVwSet VwsAlgorithm::get_geo_vw_from_descriptors(iri_perception_msgs::DescriptorsToVws::Request &req)
{
        iri_perception_msgs::GeoVwSet result;
        std::vector<cv::DMatch> matches;
        cv::Mat descriptors;
        
        /*  ---------------------
        
        Header header
        int32 num_orient_bins
        int32 num_spa_bins
        int32 num
        int32 len
        int32 width
        int32 height
        iri_perception_msgs/Descriptor[] descriptors
        
        */
        
        descriptors = descriptors_srv_to_mat(req);
        
        ROS_INFO("MATCHING! %d, %d", descriptors.rows, centroids_.rows);
        
        cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce");
        matcher->match( descriptors, centroids_, matches);
        
        for ( std::vector<cv::DMatch>::iterator it=matches.begin() ; it < matches.end(); it++ ) {
                iri_perception_msgs::GeoVw geo_vw;
                geo_vw.x = req.descriptor_set.descriptors[it->queryIdx].u;
                geo_vw.y = req.descriptor_set.descriptors[it->queryIdx].v;
                geo_vw.word = it->trainIdx;
                result.geo_vws.push_back(geo_vw);
        }
        
        return result;
}