in_hand_object_modeling.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <ros/node_handle.h>
#include <cstdlib>
#include <robot_mask/GetMask.h>
#include <string.h>
#include "cv_bridge/CvBridge.h"
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <boost/thread.hpp>
#include <sensor_msgs/Image.h>
#include <message_filters/subscriber.h>
#include <sensor_msgs/image_encodings.h>
#include <siftfast/siftfast.h>

using namespace message_filters;
using namespace cv::flann;
using namespace cv;

class LatestMatchingTFMessage
{
private:
    ros::Subscriber tf_sub_;
    ros::Subscriber image_sub_;
    std::string source_frame_, camera_frame_;
    int fovy_, width_, height_, count_ ;
    ros::NodeHandle nh_;
    bool valid_, save_images_;
public:
    ros::ServiceClient client ;
    robot_mask::GetMask srv;
    tf::tfMessageConstPtr tf_state , last_matching_tf;
    std::string tf_topic_, image_topic_;
    sensor_msgs::CvBridge bridge_;
    cv::Mat mask_, original_image_ , final_image_ , image_, image_roi_ , image_roi2_;
    Rect rect_;
    std::string mode_;
    double radius_;
    int min_nn_ , knn_ ;

    LatestMatchingTFMessage(ros::NodeHandle &nh, const std::string &source_frame)
    : source_frame_(source_frame),
      nh_ (nh),
      valid_(false)
    {
        //subscribe the tf_topic and image_topic
        nh_.param("tf_topic", tf_topic_, std::string("/tf"));
        nh_.param("image_topic", image_topic_, std::string("/camera/rgb/image_mono"));
        tf_sub_ = nh_.subscribe(tf_topic_, 1, &LatestMatchingTFMessage::TFcallback, this);
        image_sub_ = nh_.subscribe(image_topic_, 1, &LatestMatchingTFMessage::Imagecallback, this);

        //parameters
        nh_.param("fovy", fovy_, 25);
        nh_.param("width", width_, 640);
        nh_.param("height", height_, 480);
        nh_.param("camera_frame", camera_frame_, std::string("openni_rgb_optical_frame"));
        nh_.param("save_images", save_images_, true);
        nh_.param("mode", mode_ , std::string("knn_search"));
        nh_.param("min_nn", min_nn_ , 2);
        nh_.param("knn", knn_ , 2);
        srv.request.width = width_;
        srv.request.height = height_;
        srv.request.fovy = fovy_;
        srv.request.camera_frame = camera_frame_;
        count_ = 0;
        radius_ = 30;

        //instantiate an autogenerated service class, and assign values into its request member.
        client = nh.serviceClient<robot_mask::GetMask>("/robot_mask/get_mask");
        //Mat::Mat(int rows, int cols, int type)
        mask_.create(srv.request.height, srv.request.width, CV_8UC1);
        final_image_.create(srv.request.height, srv.request.width, CV_8UC1);

    }

    void TFcallback(const tf::tfMessageConstPtr &msg )
    {
        for (uint i = 0; i < msg->transforms.size(); i++)
        {
            //std::cerr << "frame " << i << " " <<  msg->transforms[i].header.frame_id << std::endl;
            if(!valid_)
            {
                if(msg->transforms[i].header.frame_id == source_frame_)
                {
                    last_matching_tf = msg;
                    valid_ = true;
                }
            }
        }
    }


    void Imagecallback(const sensor_msgs::ImageConstPtr& image )
    {
        ROS_INFO("[LatestMatchingTFMessage:] Image received in frame: %s with height %d and width %d",
                image->header.frame_id.c_str(), image->height, image->width);
        if(valid_)
        {
            srv.request.tf_state = *last_matching_tf;
            ROS_INFO("[LatestMatchingTFMessage:] waiting for service server...");
            if(!client.waitForExistence (ros::Duration (1)))
            {
                ROS_ERROR("[LatestMatchingTFMessage:] Could not find service.");
            }
            //convert the ros::image in cv::image
            try
            {
                original_image_ = bridge_.imgMsgToCv(image);
                ROS_INFO("[LatestMatchingTFMessage:] Image converted");
            }
            catch (sensor_msgs::CvBridgeException error)
            {
                ROS_ERROR("error");
            }
            ROS_INFO("[LatestMatchingTFMessage:] The server was found - calling now");
            // calls the service
            if (client.call(srv))
            {
                //fill the cv:: Mat mask image with the data from srv.response.mask[]
                //copy the first 640 elements from mask array into first row of cv::Mat and so on..
                for (unsigned int i=0; i < srv.response.mask.size(); i++)
                {
                    if (srv.response.mask[i] == 1)
                        mask_.at<uint8_t>(i/mask_.cols, i%mask_.cols) = 0;
                    else
                        mask_.at<uint8_t>(i/mask_.cols, i%mask_.cols) = 255;
                }
                //make the padding around the gripper mask
                for (int i=0; i < mask_.rows; i++)
                {
                    for ( int j = 0; j < mask_.cols; j++)
                    {
                        if (mask_.at<uint8_t>(i,j-1) == mask_.at<uint8_t>(i,j))
                            mask_.at<uint8_t>(i,j-1) = mask_.at<uint8_t>(i,j);
                        else
                        {
                            //increase the mask with 15 pixels
                            mask_.at<uint8_t>(i,j-1) = 0;
                            mask_.at<uint8_t>(i,j-2) = 0;
                            mask_.at<uint8_t>(i,j-3) = 0;
                            mask_.at<uint8_t>(i,j-4) = 0;
                            mask_.at<uint8_t>(i,j-5) = 0;
                            mask_.at<uint8_t>(i,j-6) = 0;
                            mask_.at<uint8_t>(i,j-7) = 0;
                            mask_.at<uint8_t>(i,j-8) = 0;
                            mask_.at<uint8_t>(i,j-9) = 0;
                            mask_.at<uint8_t>(i,j-10) = 0;
                            mask_.at<uint8_t>(i,j-11) = 0;
                            mask_.at<uint8_t>(i,j-12) = 0;
                            mask_.at<uint8_t>(i,j-13) = 0;
                            mask_.at<uint8_t>(i,j-14) = 0;
                            mask_.at<uint8_t>(i,j-15) = 0;
                        }
                    }
                }
            }
            else
            {
                ROS_ERROR("[LatestMatchingTFMessage:] Failed to call service robot_mask");
                valid_ = false;
                return;
            }
            ROS_INFO("after service call");
            //getting the image without the robot hand
            final_image_.zeros(srv.request.height, srv.request.width, CV_8UC1);
            //--Calculates per-element bit-wise conjunction of two arrays in a final one
            bitwise_and(original_image_, mask_, final_image_);
            valid_ = false ;
        }
        else
        {
            valid_ = false;
            return;
        }
        if(mode_ == "manual")
        {
            Keypoint template_keypoints = extract_keypoints(final_image_ ,true);
            Keypoint template_keypoints1 = template_keypoints;
            //the coordinates for the rect if we know the top left point(262,133) and bottom right point(388,400)of box
            rect_.x = 262;
            rect_.y = 133;
            rect_.width = 126;
            rect_.height = 267;

            image_roi_ = extract_roi(final_image_, template_keypoints, rect_);
            visualize(final_image_ , template_keypoints1);

            cv::namedWindow("Original Image", CV_WINDOW_AUTOSIZE);
            cv::namedWindow("Mask", CV_WINDOW_AUTOSIZE);
            cv::namedWindow("Mask Image", CV_WINDOW_AUTOSIZE);
            cv::namedWindow("Keypoints Image", CV_WINDOW_AUTOSIZE);
            cv::namedWindow("Rect Image", CV_WINDOW_NORMAL);

            /* //display windows on the screen
            cv::imshow("Original Image", original_image_);
            cv::imshow("Mask", mask_);
            cv::imshow("Mask Image", final_image_);
            cv::imshow("Keypoints Image", image_);
            cv::imshow("Rect Image", image_roi_);
            cv::waitKey(20);*/

            //save the images
            if (save_images_)
            {
                count_++;
                std::stringstream ss;
                ss << count_;
                cv::imwrite(ss.str() + ".png", original_image_);
                cv::imwrite(ss.str() + "a.png", final_image_ );
                cv::imwrite(ss.str() + "b.png", image_roi_);
                cv::imwrite(ss.str() + "c.png", image_ );
            }
        }
        else if(mode_ == "radius_search")
        {
            cv::Mat query , indices , dists, output_image;
            query.create(0, 2, CV_32FC1);
            Keypoint template_keypoints2 = extract_keypoints(final_image_ , true);

            //copy all the keypoints in a temporary matrix(q) and then push in them in query matrix
            int query_rows = 0;
            for (int k=0; template_keypoints2 != NULL; ++k, template_keypoints2 = template_keypoints2->next)
            {
                cv::Mat q(1, 2, CV_32FC1);
                q.at<float>(0, 0) = template_keypoints2->col;
                q.at<float>(0, 1) = template_keypoints2->row;
                query.push_back(q);
                query_rows = k;
            }
            //printing all the elements of query matrix
            /*ROS_INFO("query: %d %d ", query.rows, query.cols);
            for (int i = 0; i < query.rows; i++)
            {
                std::cerr << "query:";
                for (int j = 0; j < query.cols; j++)
                {
                    std::cerr << query.at<float>(i, j) << " ";
                }
                std::cerr << std::endl;
            }
            std::cerr << "end query" << std::endl;
            return;*/

            //resize query and indices matrix to actual sizes of template_keypoints (1 keypoint can has k-1 neighbors)
            indices.create(0, query_rows, CV_32SC1);
            dists.create(0, query_rows, CV_32FC1);
            ROS_INFO("indices: %d %d ", indices.rows, indices.cols);

            //search for neighbors
            radiusSearch(query, indices, dists, radius_);

            cv::Mat min_indices(0, indices.cols, CV_32SC1);
            //the inliers vector contains the index rows associated with the query matrix
            std::vector<int> inliers;
            //copy in min_indices all the keypoints from the indices matrix that have less than min_nn neighbors

            //prepare inliers for radiusSearch function
            for (int i = 0; i < indices.rows; i++)
            {
                //check if the indices have min_nn neighbors
                if (indices.at<int>(i, min_nn_) != 0)
                {
                    continue;
                }
                else
                {
                    inliers.push_back(i);
                }
            }

            ROS_INFO("image_final %d, %d", final_image_.rows, final_image_.cols);
            output_image.create(final_image_.rows , final_image_.cols, CV_8UC3);

            // cvtColor -Converts image from one color space to another
            cvtColor(final_image_, output_image, CV_GRAY2RGB );

            image_roi2_ = extract_roi(inliers, query, output_image);
            visualize(inliers, query, output_image);

            /*// display windows on the screen
            cv::imshow("Outliers_removed_Image", output_image);
            cv::imshow("Region_of_Interest_Image", image_roi2_);
            waitKey(5);
             * */

            //save the images
            if (save_images_)
            {
                count_++;
                std::stringstream ss;
                ss << count_;
                cv::imwrite(ss.str() + ".png", output_image);
                cv::imwrite(ss.str() + "a.png", image_roi2_);
            }
        }
        else if(mode_ == "knn_search")
        {
            cv::Mat query , indices , dists, output_image;
            query.create(0, 2, CV_32FC1);
            Keypoint template_keypoints2 = extract_keypoints(final_image_ , true);

            //copy all the keypoints in a temporary matrix(q) and then push in them in query matrix
            int query_rows = 0;
            for (int k=0; template_keypoints2 != NULL; ++k, template_keypoints2 = template_keypoints2->next)
            {
                cv::Mat q(1, 2, CV_32FC1);
                q.at<float>(0, 0) = template_keypoints2->col;
                q.at<float>(0, 1) = template_keypoints2->row;
                query.push_back(q);
                //query_rows is the total number of features
                query_rows = k;
            }
            //printing all the elements of query matrix
            ROS_INFO("query: %d %d ", query.rows, query.cols);

            indices.create(0, query_rows, CV_32SC1);
            dists.create(0, query_rows, CV_32FC1);
            ROS_INFO("indices: %d %d ", indices.rows, indices.cols);

            //search for neighbors
            knnSearch(query, indices, dists, min_nn_);

            // cv::Mat min_indices(0, indices.cols, CV_32SC1);
            //the inliers vector contains the index rows associated with the query matrix
            std::vector<int> inliers;
            //copy in min_indices all the keypoints from the indices matrix that have more than knn_ neighbors

            //prepare inliers for knnSearch function
            for (int i = 0; i < dists.rows; i++)
            {
                bool has_outliers = false;
                for (int j = 0; j < dists.cols; j++)
                {
                    //check if there is any neighbor outside of radius_
                    if (dists.at<float>(i,j) != 0 && dists.at<float>(i,j) > radius_)
                    {
                        has_outliers = true;
                    }
                }
                if (!has_outliers)
                    inliers.push_back(i);
                has_outliers = false;
            }

            ROS_INFO("image_final %d, %d", final_image_.rows, final_image_.cols);

            output_image.create(final_image_.rows , final_image_.cols, CV_8UC3);
            cvtColor(final_image_, output_image, CV_GRAY2RGB );

            image_roi2_ = extract_roi(inliers, query, output_image);
            visualize(inliers, query, output_image);

            /* //display windows on the screen
            cv::imshow("Outliers_removed_Image", output_image);
            cv::imshow("Region_of_Interest_Image", image_roi2_);
            waitKey(5);
             */

            //save the images
            if (save_images_)
            {
                count_++;
                std::stringstream ss;
                ss << count_;
                cv::imwrite(ss.str() + ".png", output_image);
                cv::imwrite(ss.str() + "a.png", image_roi2_);
            }
        }
        else
            return;
    }

    void radiusSearch(Mat& query, Mat& indices, Mat& dists, double radius)
    {
        //index is for accessing the coordinates of keypoints from the query matrix
        Index* index;
        //populate index tree
        // When passing an object of LinearIndexParams type, the index will perform a linear, brute-force search.
        index = new Index (query, LinearIndexParams() );
        //        radiusSearch can only search one feature at a time for range search
        for( int i = 0; i < query.rows; i++ )
        {
            //ptr is a pointer for specify the right row for these matrix
            Mat q(1, query.cols, CV_32FC1, query.ptr<float>(i) );
            Mat in;
            in = Mat::zeros(1, indices.cols, CV_32SC1);
            Mat di;
            di = Mat::zeros(1, dists.cols,CV_32FC1);
            //radius is the maximum distance to the nearest neighbors
            index->radiusSearch( q, in, di, radius_ , SearchParams() );
            indices.push_back(in);
            dists.push_back(di);
        }
        //delete the allocated memory for index
        delete index;
    }


    void knnSearch(Mat& query, Mat& indices, Mat& dists, int knn)
    {
        Index* index;
        index = new Index (query, LinearIndexParams() );
        for( int i = 0; i < query.rows; i++ )
        {
            Mat q(1, query.cols, CV_32FC1, query.ptr<float>(i) );
            Mat in;
            in = Mat::zeros(1, indices.cols, CV_32SC1);
            Mat di;
            di = Mat::zeros(1, dists.cols,CV_32FC1);
            index->knnSearch( q, in, di, knn, SearchParams() );
            indices.push_back(in);
            dists.push_back(di);
        }
        delete index;
    }


    Keypoint extract_keypoints(cv::Mat & image, bool frames_only)
    {
        //        ROS_INFO("IMAGE %d %d", image.rows , image.cols);
        Image sift_image = CreateImage(image.rows, image.cols);

        for (int i = 0; i < image.rows; ++i)
        {
            //copy images from cv:Mat to Image
            // int stride -- how many floats until the next row
            // (used to add padding to make rows aligned to 16 bytes)
            float* pDst = sift_image->pixels + i * sift_image->stride;
            for (int j = 0; j < image.cols; ++j)
                pDst[j] = image.at<uint8_t>(i, j) * (1.0f / 255.0f);
        }
        Keypoint keypoints;
        if (frames_only)
            keypoints = GetKeypointFrames(sift_image);
        else
            keypoints = GetKeypoints(sift_image);
        DestroyAllImages();
        return keypoints;
    }
  

    cv::Mat extract_roi(cv::Mat& image, Keypoint &camera_keypoints, Rect rect )
    {
        std::vector<Point2f> keypoints_rect;
        for (int ii = 0; camera_keypoints != NULL; ++ii, camera_keypoints = camera_keypoints->next)
        {
            if ((camera_keypoints->col > rect.x) &&
                    (camera_keypoints->col < (rect.x + rect.width)) &&
                    (camera_keypoints->row > rect.y) &&
                    (camera_keypoints->row < (rect.y + rect.height)))
            {
                Point2f pt;
                pt.x = camera_keypoints->col;
                pt.y = camera_keypoints->row;
                keypoints_rect.push_back(pt);
            }
        }
        //        ROS_INFO("GET THE KEYPOINTS %ld ", keypoints_rect.size());
        Rect rect1 = boundingRect(Mat(keypoints_rect));
        //        ROS_INFO_STREAM("rect1: " << rect1.x << " " << rect1.y << " " << rect1.width << " " << rect1.height);
        Mat image_roi = image(rect1);
        return image_roi;
    }


    cv::Mat extract_roi(std::vector<int> inliers, cv::Mat query, cv::Mat &output_image  )
    {
        cv::Mat image;
        std::vector<Point2f> keypoints_rect;
        for (uint ii=0; ii < inliers.size() ; ++ii)
        {
            Point2f pt;
            pt.x = query.at<float>(inliers.at(ii),0);
            pt.y = query.at<float>(inliers.at(ii),1);
            keypoints_rect.push_back(pt);
        }
        ROS_INFO("GET THE KEYPOINTS %ld ", keypoints_rect.size());

        if (keypoints_rect.size()!= 0)
        {
            Rect rect1 = boundingRect(Mat(keypoints_rect));
            ROS_INFO_STREAM("rect1: " << rect1.x << " " << rect1.y << " " << rect1.width << " " << rect1.height);

            cv::Mat image_roi2 = output_image(rect1);
            return image_roi2;
        }
        else return image; //an empty region
    }


    void visualize(cv::Mat camera_image_in, Keypoint &camera_keypoints)
    {
        image_.create(camera_image_in.cols, camera_image_in.rows, CV_8UC1);
        // cvtColor -Converts image from one color space to another
        cvtColor(camera_image_in, image_, CV_GRAY2RGB);
        // display camera image keypoints
        for (int ii=0; camera_keypoints != NULL; ++ii, camera_keypoints = camera_keypoints->next)
        {
            circle(image_, cvPoint((int) (camera_keypoints->col),  (int) (camera_keypoints->row)), 3,
                    cvScalar(255, 0, 0));
        }
    }


    void visualize(std::vector<int> inliers, cv::Mat query, cv::Mat &output_image )
    {
        // display camera image keypoints
        for (uint ii=0; ii < inliers.size() ; ++ii)
        {
            //the inliers vector contains the rows associated with the query matrix
            //the 0 and 1 are the coloums from the query matrix with template1_keypoints
            circle(output_image, cvPoint((int) (query.at<float>(inliers.at(ii),0)),  (int) (query.at<float>(inliers.at(ii),1))), 3,
                    cvScalar(255, 0, 0));
        }
    }
};

int main(int argc, char **argv)
{
    //get_mask_client - the node name for client , get_mask - node for the server
    ros::init(argc, argv, "get_mask_client");
    ros::NodeHandle n("~");
    LatestMatchingTFMessage tf(n, "/base_footprint");
    //enters a loop, calling message callbacks as fast as possible
    ros::spin();
}