trajectory_to_markers_alg.cpp

Go to the documentation of this file.

#include "trajectory_to_markers_alg.h"

TrajectoryToMarkersAlgorithm::TrajectoryToMarkersAlgorithm(void)
{
  nLoops_ = 0;
}

TrajectoryToMarkersAlgorithm::~TrajectoryToMarkersAlgorithm(void)
{
}

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

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

void TrajectoryToMarkersAlgorithm::initialize_markers()
{
  // trajectory line initialization
  trajectory_marker_.header.stamp = ros::Time::now();
  trajectory_marker_.header.frame_id = "/world";
  trajectory_marker_.type = visualization_msgs::Marker::LINE_STRIP;
  trajectory_marker_.action = visualization_msgs::Marker::ADD;
  
  trajectory_marker_.color.a = config_.line_color_a;
  trajectory_marker_.color.r = config_.line_color_r;
  trajectory_marker_.color.g = config_.line_color_g;
  trajectory_marker_.color.b = config_.line_color_b;
  
  trajectory_marker_.ns = "/trajectory";
  trajectory_marker_.id = 1;
  
  trajectory_marker_.scale.x = config_.line_width;
  
  // loop line initialization
  loops_marker_.header.stamp = ros::Time::now();
  loops_marker_.header.frame_id = "/world";
  loops_marker_.type = visualization_msgs::Marker::LINE_LIST;
  loops_marker_.action = visualization_msgs::Marker::ADD;
  
  loops_marker_.color.a = config_.line_loop_color_a;
  loops_marker_.color.r = config_.line_loop_color_r;
  loops_marker_.color.g = config_.line_loop_color_g;
  loops_marker_.color.b = config_.line_loop_color_b;
  
  loops_marker_.ns = "/loops";
  loops_marker_.id = 1;
  
  loops_marker_.scale.x = config_.line_loop_width;
  
  // actual marker initialization
  actual_marker_.header.stamp = ros::Time::now();
  actual_marker_.header.frame_id = "/world";
  actual_marker_.type = visualization_msgs::Marker::SPHERE;
  actual_marker_.action = visualization_msgs::Marker::ADD;
  
  actual_marker_.color.a = config_.actual_marker_color_a;
  actual_marker_.color.r = config_.actual_marker_color_r;
  actual_marker_.color.g = config_.actual_marker_color_g;
  actual_marker_.color.b = config_.actual_marker_color_b;
  
  actual_marker_.ns = "/actual";
  actual_marker_.id = 1;
}

// TrajectoryToMarkersAlgorithm Public API
bool TrajectoryToMarkersAlgorithm::drawCovariances(const iri_nav_msgs::Trajectory& msg)
{
  //ROS_INFO("TRAJECTORY TO MARKERS: draw Covariances");
  bool new_trajectory = false;
  bool LoopClosure = (msg.loops.size() > nLoops_ * 2);
  
  // Space reserve
  if (marker_array_.markers.capacity() == marker_array_.markers.size()) marker_array_.markers.reserve(marker_array_.markers.size() + 100);
  if (loop_state_.capacity() == loop_state_.size()) loop_state_.reserve(loop_state_.size() + 100);
  
  // NEW NOT REDUNDANT POSE --> update trajectory
  if (msg.steps_2_states.back() != -1)
  {
    //ROS_INFO("TRAJECTORY TO MARKERS: NOT REDUNDANT");
    new_trajectory = true;
    loop_state_.push_back(LoopClosure);
    
    // LOOP CLOSURE --> Recompute all markers
    if (LoopClosure)
    {
      //ROS_INFO("TRAJECTORY TO MARKERS: Loop closed");
      //ROS_INFO("TRAJECTORY TO MARKERS: nLoops_ = %i | msg.loops.size() / 2 = %i", nLoops_, msg.loops.size() / 2);
      
      // loops_marker_ (position of old loops and adding new(s) loop(s))
      //ROS_INFO("All loops:");
      nLoops_ = uint(msg.loops.size()) / 2;
      loops_marker_.points.clear();
      for (uint i = 0; i < nLoops_; i++)
      {
        uint from = msg.steps_2_states.at(msg.loops.at(2 * i));
        uint with = msg.steps_2_states.at(msg.loops.at(2 * i + 1));
        
        //ROS_INFO("states %i with %i", from, with);
        
        loops_marker_.points.push_back(msg.poses.at(from).pose.pose.position);
        loops_marker_.points.push_back(msg.poses.at(with).pose.pose.position);
        loop_state_.at(from) = true;
        loop_state_.at(with) = true;
      }
      
      // marker_array_ & trajectory_marker_array_ (covariances and poses)
      marker_array_.markers.clear();
      trajectory_marker_.points.clear();
      //ROS_INFO("msg.poses.size() = %i - loop_state_.size() = %i", msg.poses.size(), loop_state_.size());
      for (uint i = 0; i < msg.poses.size(); i++)
      {
        Eigen::Matrix2f covs = get_ith_cov(msg, i);
        float theta_cov = get_ith_theta_cov(msg, i);
        
        visualization_msgs::Marker aux_marker = create_marker(i, msg.header, covs, theta_cov, msg.poses.at(i).pose.pose.position, loop_state_.at(i));
        marker_array_.markers.push_back(aux_marker);
        
        trajectory_marker_.points.push_back(msg.poses.at(i).pose.pose.position);
      }
    }
    // NOT LOOP CLOSURE - NOT REDUNDANT POSE --> Add to trajectory
    else
    {
      //ROS_INFO("TRAJECTORY TO MARKERS: msg.poses.size() = %i", msg.poses.size());
      //marker_array_
      Eigen::Matrix2f covs = get_ith_cov(msg, msg.steps_2_states.back());
      float theta_cov = get_ith_theta_cov(msg, msg.steps_2_states.back());
      uint id = marker_array_.markers.size() + 1;
      
      visualization_msgs::Marker aux_marker = create_marker(id, msg.header, covs, theta_cov, msg.poses.back().pose.pose.position, false);
      marker_array_.markers.push_back(aux_marker);
      
     
      trajectory_marker_.points.push_back(msg.poses.back().pose.pose.position);     
    }
  }
  // FIRST POSE
  else if(marker_array_.markers.size() == 0)
  {
    ROS_INFO("FIRST POSE MARKER");
    //marker_array_
    Eigen::Matrix2f covs = get_ith_cov(msg, 0);
    float theta_cov = get_ith_theta_cov(msg, 0);
    uint id = 1;
    
    visualization_msgs::Marker aux_marker = create_marker(id, msg.header, covs, theta_cov, msg.poses.back().pose.pose.position, false);
    marker_array_.markers.push_back(aux_marker);
    
    //trajectory_marker_
    trajectory_marker_.points.push_back(msg.poses.at(0).pose.pose.position);
    new_trajectory = true;
    
    //boolean vector of loopclosure
    loop_state_.push_back(false);
  }
  
  
  // UPDATE ACTUAL MARKER
  uint StateIdx = msg.poses.size() - 1;
  Eigen::Matrix2f covs = get_ith_cov(msg, StateIdx);
  float theta_cov = get_ith_theta_cov(msg, StateIdx);
    
  change_actual_marker(covs, theta_cov, msg.poses.back().pose.pose.position, LoopClosure);

  return new_trajectory;
}

visualization_msgs::MarkerArray TrajectoryToMarkersAlgorithm::get_marker_array() const
{
  visualization_msgs::MarkerArray all_markers_array_(marker_array_);
  all_markers_array_.markers.reserve(all_markers_array_.markers.size() + 2);
  all_markers_array_.markers.push_back(loops_marker_);
  all_markers_array_.markers.push_back(trajectory_marker_);
  
  return all_markers_array_;
}

visualization_msgs::Marker TrajectoryToMarkersAlgorithm::get_actual_marker() const
{
  return actual_marker_;
}

visualization_msgs::Marker TrajectoryToMarkersAlgorithm::create_marker(const uint& id, const std_msgs::Header& header, const Eigen::Matrix2f& covs, const float& theta_cov, const geometry_msgs::Point& position, const bool& loopClosure) const
{
  visualization_msgs::Marker current_marker_;
  current_marker_.header = header;
  current_marker_.type = visualization_msgs::Marker::SPHERE;
  current_marker_.action = visualization_msgs::Marker::ADD;
  
  if (loopClosure)
  {
    current_marker_.color.a = config_.marker_loop_color_a;
    current_marker_.color.r = config_.marker_loop_color_r;
    current_marker_.color.g = config_.marker_loop_color_g;
    current_marker_.color.b = config_.marker_loop_color_b;
  }
  else
  {
    current_marker_.color.a = config_.marker_color_a;
    current_marker_.color.r = config_.marker_color_r;
    current_marker_.color.g = config_.marker_color_g;
    current_marker_.color.b = config_.marker_color_b;
  }
  current_marker_.ns = "/positions";
  current_marker_.id = id;
  //ROS_INFO("step %u", id);
  
  //ROS_INFO("cov:\n%f\t%f \n%f\t%f",covs(0,0),covs(0,1),covs(1,0),covs(1,1));
  
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2f> eig(covs);

  const Eigen::Vector2f& eigValues (eig.eigenvalues());
  const Eigen::Matrix2f& eigVectors (eig.eigenvectors());

  float angle = (atan2(eigVectors(1, 0), eigVectors(0, 0)));

  double lengthMajor = sqrt(eigValues[0]);
  double lengthMinor = sqrt(eigValues[1]);

  //ROS_INFO("eigValues: %f %f",eigValues[0],eigValues[1]);
  //ROS_INFO("lengthMajor/Minor: %f %f",lengthMajor,lengthMinor);

  current_marker_.scale.x = lengthMajor + 0.001;
  current_marker_.scale.y = lengthMinor + 0.001;
  current_marker_.scale.z = 2 * sqrt(theta_cov) + 0.001;
  
  current_marker_.pose.position.x = position.x;
  current_marker_.pose.position.y = position.y;
  current_marker_.pose.orientation = tf::createQuaternionMsgFromYaw(angle);
  //current_marker_.pose.orientation.w = cos(angle*0.5);
  //current_marker_.pose.orientation.z = sin(angle*0.5);
  
  return current_marker_;
}

void TrajectoryToMarkersAlgorithm::change_actual_marker(const Eigen::Matrix2f& covs, const float& theta_cov, const geometry_msgs::Point& position, const bool& loopClosure)
{  
  if (loopClosure)
  {
    actual_marker_.color.a = config_.marker_loop_color_a;
    actual_marker_.color.r = config_.marker_loop_color_r;
    actual_marker_.color.g = config_.marker_loop_color_g;
    actual_marker_.color.b = config_.marker_loop_color_b;
  }
  else
  {
    actual_marker_.color.a = config_.actual_marker_color_a;
    actual_marker_.color.r = config_.actual_marker_color_r;
    actual_marker_.color.g = config_.actual_marker_color_g;
    actual_marker_.color.b = config_.actual_marker_color_b;
  }
    
  //ROS_INFO("cov:\n%f\t%f \n%f\t%f",covs(0,0),covs(0,1),covs(1,0),covs(1,1));
  
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2f> eig(covs);

  const Eigen::Vector2f& eigValues (eig.eigenvalues());
  const Eigen::Matrix2f& eigVectors (eig.eigenvectors());

  float angle = (atan2(eigVectors(1, 0), eigVectors(0, 0)));

  double lengthMajor = sqrt(eigValues[0]);
  double lengthMinor = sqrt(eigValues[1]);

  //ROS_INFO("eigValues: %f %f",eigValues[0],eigValues[1]);
  //ROS_INFO("lengthMajor/Minor: %f %f",lengthMajor,lengthMinor);

  actual_marker_.scale.x = lengthMajor + 0.001;
  actual_marker_.scale.y = lengthMinor + 0.001;
  actual_marker_.scale.z = 2 * sqrt(theta_cov) + 0.001;
  
  actual_marker_.pose.position.x = position.x;
  actual_marker_.pose.position.y = position.y;
  actual_marker_.pose.orientation = tf::createQuaternionMsgFromYaw(angle);
  //current_marker_.pose.orientation.w = cos(angle*0.5);
  //current_marker_.pose.orientation.z = sin(angle*0.5);
}

visualization_msgs::Marker TrajectoryToMarkersAlgorithm::create_loop_marker(const uint& id, const std_msgs::Header& header, const geometry_msgs::Point& position1, const geometry_msgs::Point& position2) const
{
  
  visualization_msgs::Marker current_marker_;
  current_marker_.header = header;
  current_marker_.type = visualization_msgs::Marker::LINE_STRIP;
  current_marker_.action = visualization_msgs::Marker::ADD;
  
  
  current_marker_.color.a = config_.line_loop_color_a;
  current_marker_.color.r = config_.line_loop_color_r;
  current_marker_.color.g = config_.line_loop_color_g;
  current_marker_.color.b = config_.line_loop_color_b;
  
  current_marker_.ns = "/loops";
  current_marker_.id = id;
  //ROS_INFO("step %i", id);
  
  current_marker_.points.push_back(position1);
  current_marker_.points.push_back(position2);
  
  current_marker_.scale.x = config_.line_loop_width;
  
  return current_marker_;
}

Eigen::Matrix2f TrajectoryToMarkersAlgorithm::get_ith_cov(const iri_nav_msgs::Trajectory& msg, const uint i) const
{
  //ROS_INFO("get cov: msg.covariances.size() = %i - idx = %i", msg.covariances.size(), 9 * i +4);
  Eigen::Matrix2f covs;
  covs(0, 0) = msg.poses.at(i).pose.covariance.at(0);
  covs(0, 1) = msg.poses.at(i).pose.covariance.at(1);
  covs(1, 0) = msg.poses.at(i).pose.covariance.at(3);
  covs(1, 1) = msg.poses.at(i).pose.covariance.at(4);

  return covs;
}

float TrajectoryToMarkersAlgorithm::get_ith_theta_cov(const iri_nav_msgs::Trajectory& msg, const uint i) const
{
  //ROS_INFO("get theta cov: msg.covariances.size() = %i - idx = %i", msg.covariances.size(), 9 * i +8);
  
  return msg.poses.at(i).pose.covariance.at(8);
}