compass.cpp

Go to the documentation of this file.

#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <cv_bridge/cv_bridge.h>
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>
#include <sensor_msgs/image_encodings.h>
#include <image_transport/image_transport.h>
#include <vector>
#include <cmath>

#define _USE_MATH_DEFINES

using namespace std;
using namespace cv;

namespace enc = sensor_msgs::image_encodings;

class VisualCompass
{
  ros::NodeHandle nh_;
  std::string compass_image; 
  std::string compass_output_topic; 
  bool textout; 
  double dt; 
  image_transport::ImageTransport it_;
  image_transport::Subscriber image_sub_;
  ros::Publisher odom_pub;
  static const int up = 25; 
  static const int down = 45; 
  static const int computed_rows = down-up; 
  Mat I; 
  Mat J; 
  double angle_deg; 
  ros::Time current_time, last_time;

public:
  VisualCompass()
    : it_(nh_)
  {
    if (nh_.getParam("compass_image", compass_image)) {
      ROS_INFO("compass_image set to: %s", compass_image.c_str());
    } else {
      compass_image = "/viz/pano/image";
      ROS_INFO("using the default value for compass_image: %s", compass_image.c_str());
    }
    if (nh_.getParam("compass_output_topic", compass_output_topic)) {
      ROS_INFO("compass_output_topic set to: %s", compass_output_topic.c_str());
    } else {
      compass_output_topic = "/vcompass_pose";
      ROS_INFO("using the default value for compass_output_topic: %s", compass_output_topic.c_str());
    }
    if (nh_.getParam("compass_textout", textout)) {
      ROS_INFO("compass_textout set to: %d", textout);
    } else {
      textout = false;
    }
    
    angle_deg = 0;

    image_sub_ = it_.subscribe(compass_image, 1, &VisualCompass::imageCb, this);
    odom_pub = nh_.advertise<nav_msgs::Odometry>(compass_output_topic, 10);
  }
  ~VisualCompass(){};

private:  
  double deg2rad(double deg){
    return (deg*M_PI) / 180;
  }
  
  std::pair<double, double> corel_row(const Mat & I, const Mat & J)
  {
    float max = 0;
    double angle = 0;
    Mat R;
    Mat F;
    Mat G;
    Mat paddedI;                          //expand input image to optimal size
    int nI = getOptimalDFTSize( I.cols ); // on the border add zero pixels
    copyMakeBorder(I, paddedI, 0, 0, 0, nI - I.cols, BORDER_CONSTANT, Scalar::all(0));
    Mat planesI[] = {Mat_<float>(paddedI), Mat::zeros(paddedI.size(), CV_32F)};
    Mat complexI;
    merge(planesI, 2, complexI);         // Add to the expanded another plane with zeros
    dft(complexI, F, DFT_ROWS + DFT_COMPLEX_OUTPUT); // this way the result may fit in the source matrix
    Mat paddedJ;                          //expand input image to optimal size
    int nJ = getOptimalDFTSize( J.cols ); // on the border add zero pixels
    copyMakeBorder(J, paddedJ, 0, 0, 0, nJ - J.cols, BORDER_CONSTANT, Scalar::all(0));
    Mat planesJ[] = {Mat_<float>(paddedJ), Mat::zeros(paddedJ.size(), CV_32F)};
    Mat complexJ;
    merge(planesJ, 2, complexJ);         // Add to the expanded another plane with zeros
    dft(complexJ, G, DFT_ROWS + DFT_COMPLEX_OUTPUT);
    mulSpectrums(F, G, R, DFT_ROWS, true );
    dft(R, R, DFT_REAL_OUTPUT + DFT_SCALE, I.cols);

    for (int i = 0; i < 2*R.cols; i = i+2)
    {
      if (R.at<float>(0,i) > max)
      {
        max = R.at<float>(0,i);
        angle = i;
      }
    }
    angle = angle*180/I.cols; // 180 because of complex input
    if (angle > 180) angle = angle - 360;
    return std::make_pair(-angle,max/10000000); // coeficient reduction for better accunacy in the following proccessing
  }
  
  void imageCb(const sensor_msgs::ImageConstPtr& Image)
  {
    cv_bridge::CvImagePtr cv_ptrActual;
    try
    {
      cv_ptrActual = cv_bridge::toCvCopy(Image, enc::MONO8);
      I = cv_ptrActual->image;
      current_time = Image->header.stamp;
      if (J.empty())
      {
      J = I;
      last_time = current_time;
          dt = 1;
    } else {
        dt = (current_time - last_time).toSec();
      }
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

    pair<double,double> result;
    double mean = 0; 
    double sw = 0; 
    
    for(int k = (I.rows * up / 100); k < (I.rows * down / 100); k++){
        result = corel_row(I.row(k),J.row(k));
        mean += result.first*result.second;
        sw += result.second;
    }
    
    mean = mean/sw;
    angle_deg += mean;
    if (textout) ROS_INFO("%f", angle_deg);
    
    //odometry message
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(deg2rad(angle_deg));
    nav_msgs::Odometry odom;
    odom.header.stamp = current_time;
    odom.header.frame_id = "visual_compass";
    
    //set the position
    odom.pose.pose.position.x = 0.0;
    odom.pose.pose.position.y = 0.0;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation = odom_quat;
//    pose.orientation.z = sin(deg2rad(angle_deg)/2);
//    pose.orientation.w = cos(deg2rad(angle_deg)/2);

    //set the velocity
    odom.child_frame_id = "base_link";
    odom.twist.twist.linear.x = 0;
    odom.twist.twist.linear.y = 0;
    odom.twist.twist.angular.z = deg2rad(mean)/dt;

    //publish the message
    odom_pub.publish(odom);
    last_time = current_time;
    J = I;
  }
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "visual_compass");
    VisualCompass vc;
    ros::spin();
    return 0;
}