stereo_visual_odometry_alg_node.cpp

Go to the documentation of this file.

#include "stereo_visual_odometry_alg_node.h"

using namespace std;

StereoVisualOdometryAlgNode::StereoVisualOdometryAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<StereoVisualOdometryAlgorithm>()
{
  //init class attributes if necessary
  this->loop_rate_ = 100;//in [Hz]

  // [init publishers]
  this->pose_with_covariance_publisher_ = this->public_node_handle_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose_with_covariance", 100);
  
  // [init subscribers]
  this->right_image_subscriber_ = this->public_node_handle_.subscribe("right_image", 100, &StereoVisualOdometryAlgNode::right_image_callback, this);
  this->left_image_subscriber_ = this->public_node_handle_.subscribe("left_image", 100, &StereoVisualOdometryAlgNode::left_image_callback, this);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
  
  count_left_images=0;
  count_right_images=0;
  H=cv::Mat::eye(4,4,CV_64F);
  Hcov=cv::Mat::zeros(6,6,CV_64F);
  point=cv::Mat::zeros(4,1,CV_64F);
  point.at<double>(3,0)=1.0;
}

StereoVisualOdometryAlgNode::~StereoVisualOdometryAlgNode(void)
{
  // [free dynamic memory]
}

void StereoVisualOdometryAlgNode::mainNodeThread(void)
{
  
  if ((image1.rows!=0) & (image2.rows!=0) & (image3.rows!=0) & (image4.rows!=0))
  { 
    //ros::Time begin = ros::Time::now();
    //ROS_INFO("Temps calcul: %f %u",begin.toSec(),begin.toNSec());
    
    alg_.compute_H_Hcov(image1,image2,image3,image4,H,Hcov,point);
    
    //ros::Time end = ros::Time::now();
    //double total_s = end.toSec()-begin.toSec();
    
    //ROS_INFO("Temps calcul: %f %u",end.toSec(),end.toNSec());
    //ROS_INFO("Temps calcul: %f",total_s);
    
    image1=image3;
    image2=image4;
    
    image3=cv::Mat::zeros(0,0,CV_64F);
    image4=cv::Mat::zeros(0,0,CV_64F);
    
    double y;
    double p;
    double r;
    
    alg_.extract_ypr(H,y,p,r);
    
    // [fill msg structures]
    this->PoseWithCovarianceStamped_msg_.header.stamp = ros::Time::now();
    this->PoseWithCovarianceStamped_msg_.header.frame_id = "/teo/stereo/left_camera";
    this->PoseWithCovarianceStamped_msg_.pose.pose.position.x=point.at<double>(2,0)/1000.0;
    this->PoseWithCovarianceStamped_msg_.pose.pose.position.y=-point.at<double>(0,0)/1000.0;
    this->PoseWithCovarianceStamped_msg_.pose.pose.position.z=point.at<double>(1,0)/1000.0;
    this->PoseWithCovarianceStamped_msg_.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(r,y,-p);
    //(r,p,y)
    
    double Hcov_array[36]={Hcov.at<double>(0,0), Hcov.at<double>(0,1), Hcov.at<double>(0,2), Hcov.at<double>(0,3), Hcov.at<double>(0,4), Hcov.at<double>(0,5), 
                           Hcov.at<double>(1,0), Hcov.at<double>(1,1), Hcov.at<double>(1,2), Hcov.at<double>(1,3), Hcov.at<double>(1,4), Hcov.at<double>(1,5),
                           Hcov.at<double>(2,0), Hcov.at<double>(2,1), Hcov.at<double>(2,2), Hcov.at<double>(2,3), Hcov.at<double>(2,4), Hcov.at<double>(2,5), 
                           Hcov.at<double>(3,0), Hcov.at<double>(3,1), Hcov.at<double>(3,2), Hcov.at<double>(3,3), Hcov.at<double>(3,4), Hcov.at<double>(3,5),
                           Hcov.at<double>(4,0), Hcov.at<double>(4,1), Hcov.at<double>(4,2), Hcov.at<double>(4,3), Hcov.at<double>(4,4), Hcov.at<double>(4,5), 
                           Hcov.at<double>(5,0), Hcov.at<double>(5,1), Hcov.at<double>(5,2), Hcov.at<double>(5,3), Hcov.at<double>(5,4), Hcov.at<double>(5,5)};  
    for (int i=0;i<36;i++)
    {
      this->PoseWithCovarianceStamped_msg_.pose.covariance[i]=Hcov_array[i];
    }
    
    // [publish messages]
    this->pose_with_covariance_publisher_.publish(this->PoseWithCovarianceStamped_msg_);
  }
  
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]
  
}

/*  [subscriber callbacks] */
void StereoVisualOdometryAlgNode::right_image_callback(const sensor_msgs::Image::ConstPtr& msg) 
{ 
  ROS_INFO("StereoVisualOdometryAlgNode::right_image_callback: New Message Received"); 
  count_right_images=count_right_images+1;
  cout<<"number of right images received: "<<count_right_images<<endl;
  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  //this->right_image_mutex_.enter();
  
  cv_bridge::CvImagePtr img;
  try
  {
    img = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
  }
  
  if (count_right_images==1)
  {
    image2=img->image;
  }
  
  else
  {
    image4=img->image;
  }
    
  //alg_.show_image_alg_test(image2);
  
  //ROS_INFO("imatge data-size %d",msg->data.size());
  
  //cout << msg->data <<endl; 

  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  //this->right_image_mutex_.exit(); 
}

void StereoVisualOdometryAlgNode::left_image_callback(const sensor_msgs::Image::ConstPtr& msg) 
{ 
  ROS_INFO("StereoVisualOdometryAlgNode::left_image_callback: New Message Received");
  count_left_images=count_left_images+1;
  cout<<"number of left images received: "<<count_left_images<<endl;

  //use appropiate mutex to shared variables if necessary 
  //this->alg_.lock(); 
  //this->left_image_mutex_.enter();
  
  cv_bridge::CvImagePtr img;
  try
  {
    img = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
  }
  

  if (count_left_images==1)
  {
    image1=img->image;
  }
  
  else
  {
    image3=img->image;
  }
  
  //alg_.show_image_alg_test(image1);
  //imshow("left image 2", image2);
  //sensor_msgs::Image image2;
  //image2 = *msg;

  //std::cout << msg->data << std::endl; 

  //unlock previously blocked shared variables 
  //this->alg_.unlock(); 
  //this->left_image_mutex_.exit(); 
}

/*  [service callbacks] */

/*  [action callbacks] */

/*  [action requests] */

void StereoVisualOdometryAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();

  this->alg_.unlock();
}

void StereoVisualOdometryAlgNode::addNodeDiagnostics(void)
{
}

/* main function */
int main(int argc,char *argv[])
{
  return algorithm_base::main<StereoVisualOdometryAlgNode>(argc, argv, "stereo_visual_odometry_alg_node");
}