Subscribe stereo left and right image and calibration information to publish rectified image

featurematching.cpp

void show_keymatching(const cv::Mat left,const cv::Mat right)
{
    cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create();
    std::vector<cv::KeyPoint> kpl,kpr;
    detector->detect(left, kpl);
    detector->detect(right, kpr);
    
    cv::Ptr<cv::DescriptorExtractor> extractor = cv::ORB::create();
    cv::Mat desl,desr;
    extractor->compute(left, kpl, desl);
    extractor->compute(right, kpr, desr);
    
    // Flann needs the descriptors to be of type CV_32F
    desl.convertTo(desl, CV_32F);
    desr.convertTo(desr, CV_32F);

    cv::FlannBasedMatcher matcher;
    std::vector<cv::DMatch> matches;
    matcher.match( desl, desr, matches );

    double max_dist = 0; double min_dist = 100;
    
    //-- Quick calculation of max and min distances between keypoints
    for( int i = 0; i < desl.rows; i++ )
    {
        double dist = matches[i].distance;
        if( dist < min_dist ) min_dist = dist;
        if( dist > max_dist ) max_dist = dist;
    }

    //-- Use only "good" matches (i.e. whose distance is less than 3*min_dist )
    std::vector< cv::DMatch > good_matches;

    for( int i = 0; i < desl.rows; i++ )
    {
        if( matches[i].distance < 3*min_dist )
        {
            good_matches.push_back( matches[i]);
        }
    }
    

}

package.xml

<package><name>obj_detect</name><version>0.0.0</version><description>The obj_detect package</description><!-- One maintainer tag required, multiple allowed, one person per tag 
--><!-- Example:  -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> 
--><maintainer email="">ubuntu</maintainer><license>Yoshi</license><buildtool_depend>catkin</buildtool_depend><build_depend>sensor_msgs</build_depend><build_depend>roscpp</build_depend><build_depend>rospy</build_depend><build_depend>std_msgs</build_depend><build_depend>image_transport</build_depend><build_depend>cv_bridge</build_depend><build_depend>common_rosdeps</build_depend><build_depend>message_filters</build_depend><run_depend>sensor_msgs</run_depend><run_depend>roscpp</run_depend><run_depend>rospy</run_depend><run_depend>std_msgs</run_depend><run_depend>image_transport</run_depend><run_depend>cv_bridge</run_depend><run_depend>common_rosdeps</run_depend><run_depend>message_filters</run_depend><!-- The export tag contains other, unspecified, tags -->
<export><!-- Other tools can request additional information be placed here 
--></export></package>

Publish_stereo_rectimg.cpp

/*   Author: Yoshi Ri @ Univ of Tokyo 
*            2017 Aug 30
*
*    Subscribing stereo camera image and camera info to get rectified image
*/

#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/quaternion.hpp>

// get synchronized 
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>

// for image processing
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/core/utility.hpp"
#include <stdio.h>

// image decode and sending
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>


/* ------------- Global Variables------------------ */

cv::Mat K1,K2,D1,D2,R1,R2,P1,P2;
cv::Size size;    

bool init_flag = 0;
std_msgs::Header camHeader;

cv::Mat dst_l,dst_r;

/* ----------------------------------------------- */





void imgCallback(const sensor_msgs::ImageConstPtr& limage, const sensor_msgs::CameraInfoConstPtr& lcam_info,const sensor_msgs::ImageConstPtr& rimage, const sensor_msgs::CameraInfoConstPtr& rcam_info)
{   
    // catch the image into cv_ptr_image
    cv_bridge::CvImagePtr cv_ptr_left,cv_ptr_right;
    cv_ptr_left = cv_bridge::toCvCopy(limage, sensor_msgs::image_encodings::MONO8);
    cv_ptr_right = cv_bridge::toCvCopy(rimage, sensor_msgs::image_encodings::MONO8);
    
    camHeader.stamp = lcam_info->header.stamp;
    float tempK1[9],tempK2[9],tempD1[5],tempD2[5],tempR1[9],tempR2[9],tempP1[12],tempP2[12];
    // Initialize camera info
    if(!init_flag){
        for (int i = 0; i < 9; i++) {
           tempK1[i] = lcam_info->K[i];
           tempK2[i] = rcam_info->K[i];
           tempR1[i] = lcam_info->R[i];
           tempR2[i] = rcam_info->R[i];       
        }    
        for (int i = 0; i < 5; i++) {
           tempD1[i] = lcam_info->D[i];
           tempD2[i] = rcam_info->D[i];
        }
        for (int i = 0; i < 12; i++) {
           tempP1[i] = lcam_info->P[i];
           tempP2[i] = rcam_info->P[i];
        }

        K1 = cv::Mat(3, 3, CV_32F, &tempK1);
        K2 = cv::Mat(3, 3, CV_32F, &tempK2);
        D1 = cv::Mat(1,5, CV_32F, &tempD1);
        D2 = cv::Mat(1,5, CV_32F, &tempD2);
        R1 = cv::Mat(3, 3, CV_32F, &tempR1);
        R2 = cv::Mat(3, 3, CV_32F, &tempR2);
        P1 = cv::Mat(3, 4, CV_32F, &tempP1);
        P2 = cv::Mat(3, 4, CV_32F, &tempP2);
        size = cv_ptr_left->image.size();
        
        init_flag = 1;
        printf("Finish Initialization! \n");
    }
    
    // get undistorted image 
    cv::Mat lmap1,lmap2,rmap1,rmap2;
    initUndistortRectifyMap(K1,D1,R1,P1.rowRange(0,3),size,CV_16SC2,lmap1,lmap2);//?
    initUndistortRectifyMap(K2,D2,R2,P2.rowRange(0,3),size,CV_16SC2,rmap1,rmap2);
    remap(cv_ptr_left->image,dst_l,lmap1,lmap2,CV_INTER_LINEAR);
    remap(cv_ptr_right->image,dst_r,rmap1,rmap2,CV_INTER_LINEAR);
    
    cv::imshow("undistort left",dst_l);
    cv::imshow("raw img",cv_ptr_left->image);

    cv::waitKey(1);
}



int main(int argc, char **argv)
{
  // init ros
    ros::init(argc, argv, "Stereo_matcher");
    ros::NodeHandle nh;
    uint64_t ptsSeq = 0, camdataSeq = 0;

  // Use message filters to get multiple messages
    message_filters::Subscriber<sensor_msgs::Image> limage_sub(nh, "/stereo/left/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::Image> rimage_sub(nh, "/stereo/right/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::CameraInfo> linfo_sub(nh, "/stereo/left/camera_info", 1);
    message_filters::Subscriber<sensor_msgs::CameraInfo> rinfo_sub(nh, "/stereo/right/camera_info", 1);
    message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::CameraInfo> sync(limage_sub, linfo_sub, rimage_sub, rinfo_sub, 10);
    sync.registerCallback(boost::bind(&imgCallback, _1, _2, _3, _4));
  
  // publishers
  
	image_transport::ImageTransport it(nh);
  	image_transport::Publisher stereo_left_rect_pub = it.advertise("stereo/left/image_rect", 1);
  	image_transport::Publisher stereo_right_rect_pub = it.advertise("stereo/right/image_rect", 1);
	ros::Rate loop_rate(30);  
  	while (ros::ok()) {
		// ROS spin
		ros::spinOnce();
		
		// get number of subscribers
        int stereo_left_subscribers = stereo_left_rect_pub.getNumSubscribers();
		int stereo_right_subscribers = stereo_left_rect_pub.getNumSubscribers();
        bool run_loop = stereo_left_subscribers +  stereo_right_subscribers ;
        
        // if some one is listening to you
        if(run_loop){
			
			//Fill the camera image header
		    // std_msgs::Header camHeader; // globally prepared
	 		camHeader.seq = ++camdataSeq;
			//camHeader.stamp = img_timestamp; //stamp is already prepared
			camHeader.frame_id = "perceptin_camera_rectified";
			
			//Publish camera images 
			sensor_msgs::ImagePtr imageLeftMsg = cv_bridge::CvImage(std_msgs::Header(camHeader), "mono8", dst_l).toImageMsg();
			sensor_msgs::ImagePtr imageRightMsg = cv_bridge::CvImage(std_msgs::Header(camHeader), "mono8", dst_r).toImageMsg();
			stereo_left_rect_pub.publish(imageLeftMsg);
			stereo_right_rect_pub.publish(imageRightMsg);
    
        }else{	
		    loop_rate.sleep();
		}
    }

    ros::spin();

    return 0;
}