<commit_before>#include <ros/ros.h>

#include "tracking.h"
#include "tools.h"

using namespace tools;

namespace slam
{

  Tracking::Tracking(Publisher *f_pub, Graph *graph)
                    : f_pub_(f_pub), graph_(graph), frame_id_(0), jump_detected_(false), secs_to_filter_(10.0)
  {}

  void Tracking::run()
  {
    // Init
    state_ = NOT_INITIALIZED;
    last_fixed_frame_pose_.setIdentity();

    ros::NodeHandle nh;
    ros::NodeHandle nhp("~");

    pose_pub_ = nhp.advertise<nav_msgs::Odometry>("odometry", 1);

    image_transport::ImageTransport it(nh);

    image_transport::SubscriberFilter left_sub, right_sub;
    message_filters::Subscriber<sensor_msgs::CameraInfo> left_info_sub, right_info_sub;
    message_filters::Subscriber<nav_msgs::Odometry> odom_sub;
    message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub;

    // Message sync
    boost::shared_ptr<Sync> sync;
    odom_sub      .subscribe(nh, params_.odom_topic, 20);
    left_sub      .subscribe(it, params_.camera_topic+"/left/image_rect_color", 3);
    right_sub     .subscribe(it, params_.camera_topic+"/right/image_rect_color", 3);
    left_info_sub .subscribe(nh, params_.camera_topic+"/left/camera_info",  3);
    right_info_sub.subscribe(nh, params_.camera_topic+"/right/camera_info", 3);
    // cloud_sub     .subscribe(nh, params_.camera_topic+"/points2", 5);
    sync.reset(new Sync(SyncPolicy(5), odom_sub, left_sub, right_sub, left_info_sub, right_info_sub) );
    sync->registerCallback(bind(&Tracking::msgsCallback, this, _1, _2, _3, _4, _5));

    // Create directory to store the keyframes
    string keyframes_dir = WORKING_DIRECTORY + "keyframes";
    if (fs::is_directory(keyframes_dir))
      fs::remove_all(keyframes_dir);
    fs::path dir1(keyframes_dir);
    if (!fs::create_directory(dir1))
      ROS_ERROR("[Localization:] ERROR -> Impossible to create the keyframes directory.");

    // Create directory to store the pointclouds
    string pointclouds_dir = WORKING_DIRECTORY + "pointclouds";
    if (fs::is_directory(pointclouds_dir))
      fs::remove_all(pointclouds_dir);
    fs::path dir2(pointclouds_dir);
    if (!fs::create_directory(dir2))
      ROS_ERROR("[Localization:] ERROR -> Impossible to create the pointclouds directory.");

    ros::spin();
  }

  void Tracking::msgsCallback(const nav_msgs::Odometry::ConstPtr& odom_msg,
                              const sensor_msgs::ImageConstPtr& l_img_msg,
                              const sensor_msgs::ImageConstPtr& r_img_msg,
                              const sensor_msgs::CameraInfoConstPtr& l_info_msg,
                              const sensor_msgs::CameraInfoConstPtr& r_info_msg)
  {

    tf::Transform c_odom_robot = Tools::odomTotf(*odom_msg);
    double timestamp = l_img_msg->header.stamp.toSec();

    cv::Mat l_img, r_img;
    Tools::imgMsgToMat(*l_img_msg, *r_img_msg, l_img, r_img);

    if (state_ == NOT_INITIALIZED)
    {
      // Transformation between odometry and camera
      if (!getOdom2CameraTf(*odom_msg, *l_img_msg, odom2camera_))
      {
        ROS_WARN("[Localization:] Impossible to transform odometry to camera frame.");
        return;
      }

      // Camera parameters
      cv::Mat camera_matrix;
      Tools::getCameraModel(*l_info_msg, *r_info_msg, camera_model_, camera_matrix);

      // Set graph properties
      graph_->setCamera2Odom(odom2camera_.inverse());
      graph_->setCameraMatrix(camera_matrix);
      graph_->setCameraModel(camera_model_.left());

      // The initial frame
      c_frame_ = Frame(l_img, r_img, camera_model_, timestamp);

      // For the first frame, its estimated pose will coincide with odometry
      tf::Transform c_odom_camera = c_odom_robot * odom2camera_;
      c_frame_.setCameraPose(c_odom_camera);

      prev_corrected_odom_robot_ = c_odom_robot;

      state_ = INITIALIZING;
    }
    else
    {
      PointCloudRGB::Ptr pcl_cloud(new PointCloudRGB);
      // fromROSMsg(*cloud_msg, *pcl_cloud);

      // The current frame
      c_frame_ = Frame(l_img, r_img, camera_model_, timestamp);

      // Set the odometry of this frame
      tf::Transform c_odom_camera = c_odom_robot * odom2camera_;
      c_frame_.setCameraPose(c_odom_camera);

      // Need new keyframe
      needNewKeyFrame(pcl_cloud);
    }

    // Correct robot pose with the last graph information
    tf::Transform c_odom_camera = c_odom_robot * odom2camera_;
    tf::Transform corrected_odom_robot = graph_->correctOdometry(c_odom_camera) * odom2camera_.inverse();

    // Detect a big jump
    double jump = Tools::poseDiff3D(corrected_odom_robot, prev_corrected_odom_robot_);
    if (!jump_detected_ && jump > 0.8)
    {
      jump_time_ = ros::WallTime::now();
      jump_detected_ = true;
    }
    if (jump_detected_ && ( ros::WallTime::now().toSec() - jump_time_.toSec() > secs_to_filter_ )    )
    {
      jump_detected_ = false;
    }

    tf::Transform pose = corrected_odom_robot;
    if (jump_detected_)
    {
      // Filter big jumps
      double m = 1/(10*secs_to_filter_);
      double factor = m * (ros::WallTime::now().toSec() - jump_time_.toSec());

      double c_x = pose.getOrigin().x();
      double c_y = pose.getOrigin().y();
      double c_z = pose.getOrigin().z();

      double p_x = prev_corrected_odom_robot_.getOrigin().x();
      double p_y = prev_corrected_odom_robot_.getOrigin().y();
      double p_z = prev_corrected_odom_robot_.getOrigin().z();

      double x = factor * c_x + (1-factor) * p_x;
      double y = factor * c_y + (1-factor) * p_y;
      double z = factor * c_z + (1-factor) * p_z;

      ROS_INFO_STREAM("X: " << x);

      tf::Vector3 filtered_pose(x, y, z);
      pose.setOrigin(filtered_pose);
    }

    // Publish
    nav_msgs::Odometry pose_msg = *odom_msg;
    tf::poseTFToMsg(pose, pose_msg.pose.pose);
    pose_pub_.publish(pose_msg);
<commit_msg>Remove log<commit_after>#include <ros/ros.h>

#include "tracking.h"
#include "tools.h"

using namespace tools;

namespace slam
{

  Tracking::Tracking(Publisher *f_pub, Graph *graph)
                    : f_pub_(f_pub), graph_(graph), frame_id_(0), jump_detected_(false), secs_to_filter_(10.0)
  {}

  void Tracking::run()
  {
    // Init
    state_ = NOT_INITIALIZED;
    last_fixed_frame_pose_.setIdentity();

    ros::NodeHandle nh;
    ros::NodeHandle nhp("~");

    pose_pub_ = nhp.advertise<nav_msgs::Odometry>("odometry", 1);

    image_transport::ImageTransport it(nh);

    image_transport::SubscriberFilter left_sub, right_sub;
    message_filters::Subscriber<sensor_msgs::CameraInfo> left_info_sub, right_info_sub;
    message_filters::Subscriber<nav_msgs::Odometry> odom_sub;
    message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub;

    // Message sync
    boost::shared_ptr<Sync> sync;
    odom_sub      .subscribe(nh, params_.odom_topic, 20);
    left_sub      .subscribe(it, params_.camera_topic+"/left/image_rect_color", 3);
    right_sub     .subscribe(it, params_.camera_topic+"/right/image_rect_color", 3);
    left_info_sub .subscribe(nh, params_.camera_topic+"/left/camera_info",  3);
    right_info_sub.subscribe(nh, params_.camera_topic+"/right/camera_info", 3);
    // cloud_sub     .subscribe(nh, params_.camera_topic+"/points2", 5);
    sync.reset(new Sync(SyncPolicy(5), odom_sub, left_sub, right_sub, left_info_sub, right_info_sub) );
    sync->registerCallback(bind(&Tracking::msgsCallback, this, _1, _2, _3, _4, _5));

    // Create directory to store the keyframes
    string keyframes_dir = WORKING_DIRECTORY + "keyframes";
    if (fs::is_directory(keyframes_dir))
      fs::remove_all(keyframes_dir);
    fs::path dir1(keyframes_dir);
    if (!fs::create_directory(dir1))
      ROS_ERROR("[Localization:] ERROR -> Impossible to create the keyframes directory.");

    // Create directory to store the pointclouds
    string pointclouds_dir = WORKING_DIRECTORY + "pointclouds";
    if (fs::is_directory(pointclouds_dir))
      fs::remove_all(pointclouds_dir);
    fs::path dir2(pointclouds_dir);
    if (!fs::create_directory(dir2))
      ROS_ERROR("[Localization:] ERROR -> Impossible to create the pointclouds directory.");

    ros::spin();
  }

  void Tracking::msgsCallback(const nav_msgs::Odometry::ConstPtr& odom_msg,
                              const sensor_msgs::ImageConstPtr& l_img_msg,
                              const sensor_msgs::ImageConstPtr& r_img_msg,
                              const sensor_msgs::CameraInfoConstPtr& l_info_msg,
                              const sensor_msgs::CameraInfoConstPtr& r_info_msg)
  {

    tf::Transform c_odom_robot = Tools::odomTotf(*odom_msg);
    double timestamp = l_img_msg->header.stamp.toSec();

    cv::Mat l_img, r_img;
    Tools::imgMsgToMat(*l_img_msg, *r_img_msg, l_img, r_img);

    if (state_ == NOT_INITIALIZED)
    {
      // Transformation between odometry and camera
      if (!getOdom2CameraTf(*odom_msg, *l_img_msg, odom2camera_))
      {
        ROS_WARN("[Localization:] Impossible to transform odometry to camera frame.");
        return;
      }

      // Camera parameters
      cv::Mat camera_matrix;
      Tools::getCameraModel(*l_info_msg, *r_info_msg, camera_model_, camera_matrix);

      // Set graph properties
      graph_->setCamera2Odom(odom2camera_.inverse());
      graph_->setCameraMatrix(camera_matrix);
      graph_->setCameraModel(camera_model_.left());

      // The initial frame
      c_frame_ = Frame(l_img, r_img, camera_model_, timestamp);

      // For the first frame, its estimated pose will coincide with odometry
      tf::Transform c_odom_camera = c_odom_robot * odom2camera_;
      c_frame_.setCameraPose(c_odom_camera);

      prev_corrected_odom_robot_ = c_odom_robot;

      state_ = INITIALIZING;
    }
    else
    {
      PointCloudRGB::Ptr pcl_cloud(new PointCloudRGB);
      // fromROSMsg(*cloud_msg, *pcl_cloud);

      // The current frame
      c_frame_ = Frame(l_img, r_img, camera_model_, timestamp);

      // Set the odometry of this frame
      tf::Transform c_odom_camera = c_odom_robot * odom2camera_;
      c_frame_.setCameraPose(c_odom_camera);

      // Need new keyframe
      needNewKeyFrame(pcl_cloud);
    }

    // Correct robot pose with the last graph information
    tf::Transform c_odom_camera = c_odom_robot * odom2camera_;
    tf::Transform corrected_odom_robot = graph_->correctOdometry(c_odom_camera) * odom2camera_.inverse();

    // Detect a big jump
    double jump = Tools::poseDiff3D(corrected_odom_robot, prev_corrected_odom_robot_);
    if (!jump_detected_ && jump > 0.8)
    {
      jump_time_ = ros::WallTime::now();
      jump_detected_ = true;
    }
    if (jump_detected_ && ( ros::WallTime::now().toSec() - jump_time_.toSec() > secs_to_filter_ )    )
    {
      jump_detected_ = false;
    }

    tf::Transform pose = corrected_odom_robot;
    if (jump_detected_)
    {
      // Filter big jumps
      double m = 1/(10*secs_to_filter_);
      double factor = m * (ros::WallTime::now().toSec() - jump_time_.toSec());

      double c_x = pose.getOrigin().x();
      double c_y = pose.getOrigin().y();
      double c_z = pose.getOrigin().z();

      double p_x = prev_corrected_odom_robot_.getOrigin().x();
      double p_y = prev_corrected_odom_robot_.getOrigin().y();
      double p_z = prev_corrected_odom_robot_.getOrigin().z();

      double x = factor * c_x + (1-factor) * p_x;
      double y = factor * c_y + (1-factor) * p_y;
      double z = factor * c_z + (1-factor) * p_z;

      tf::Vector3 filtered_pose(x, y, z);
      pose.setOrigin(filtered_pose);
    }

    // Publish
    nav_msgs::Odometry pose_msg = *odom_msg;
    tf::poseTFToMsg(pose, pose_msg.pose.pose);
    pose_pub_.publish(pose_msg);

{'fraction_non_alphanumeric': 0.12038069107433765, 'fraction_numerical': 0.007202263568550115, 'mean_word_length': 3.3800225309800975, 'pattern_counts': {'":': 0, '<': 17, '<?xml version=': 0, '>': 35, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 2, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1628957', 'n_tokens_mistral': 4086, 'n_tokens_neox': 3859, 'n_words': 725}