<commit_before>#include <cmath>
#include <string>

#include <ros/ros.h>
#include <geometry_msgs/Point.h>
#include <rviz_visual_tools/rviz_visual_tools.h>
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_ros/transform_listener.h>

#include <Eigen/Geometry>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "body_angle_visualizer");
  ros::NodeHandle n {};
  ros::Publisher pub {n.advertise<geometry_msgs::Point>("body_direction", 1)};
  ros::Rate r {5};
  tf2_ros::Buffer tfBuffer {};
  tf2_ros::TransformListener tfListener {tfBuffer};
  rviz_visual_tools::RvizVisualTools rvt {"openni_depth_frame", "rviz_visual_markers"};

  ros::NodeHandle pn {"~"};
  int target_number {1};
  pn.getParam("target_number", target_number);
  std::string root_name {"openni_depth_frame"};
  pn.getParam("root", root_name);

  const auto head_name {"head_" + std::to_string(target_number)};
  const auto torso_name {"torso_" + std::to_string(target_number)};

  while (ros::ok()) {
    try {
      const auto head_pos {tf2::transformToEigen(tfBuffer.lookupTransform(root_name, head_name, ros::Time{0}))};
      const auto torso_pos {tf2::transformToEigen(tfBuffer.lookupTransform(root_name, torso_name, ros::Time{0}))};
      const auto stand_vec {head_pos.translation() - torso_pos.translation()};
      const auto stand_quaternion {Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), stand_vec)};

      pub.publish(tf2::toMsg(stand_vec));

      rvt.deleteAllMarkers();
      rvt.publishArrow(Eigen::Affine3d{stand_quaternion}, rviz_visual_tools::BLUE, rviz_visual_tools::LARGE);

      Eigen::Affine3d text_pos {};
      text_pos.translation() = stand_vec;
      rvt.publishText(text_pos, std::to_string(stand_vec.normalized()(1)), rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE);

      rvt.trigger();
    } catch (tf2::TransformException &e) {<commit_msg>Update fiting for kinect coordinate system<commit_after>#include <cmath>
#include <string>

#include <ros/ros.h>
#include <geometry_msgs/Point.h>
#include <rviz_visual_tools/rviz_visual_tools.h>
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_ros/transform_listener.h>

#include <Eigen/Geometry>
#include <Eigen/SVD>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "body_angle_visualizer");
  ros::NodeHandle n {};

  int target_number {1};
  std::string root_name {"openni_depth_frame"};
  {
    ros::NodeHandle pn {"~"};
    pn.getParam("target_number", target_number);
    pn.getParam("root", root_name);
  }

  const auto head_name {"head_" + std::to_string(target_number)};
  const auto torso_name {"torso_" + std::to_string(target_number)};

  ros::Publisher pub {n.advertise<geometry_msgs::Point>("body_direction", 1)};
  ros::Rate r {5};
  tf2_ros::Buffer tfBuffer {};
  tf2_ros::TransformListener tfListener {tfBuffer};
  rviz_visual_tools::RvizVisualTools rvt {root_name, "rviz_visual_markers"};

  while (ros::ok()) {
    try {
      const auto head_pos {tf2::transformToEigen(tfBuffer.lookupTransform(root_name, head_name, ros::Time{0}))};
      const auto torso_pos {tf2::transformToEigen(tfBuffer.lookupTransform(root_name, torso_name, ros::Time{0}))};
      const auto torso_yrp {torso_pos.rotation().eulerAngles(1, 2, 0)};
      const auto stand_vec {head_pos.translation() - torso_pos.translation()};
      const auto stand_quaternion {Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), stand_vec)};

      pub.publish(tf2::toMsg(stand_vec));

      rvt.deleteAllMarkers();
      rvt.publishArrow(Eigen::Affine3d{stand_quaternion}, rviz_visual_tools::BLUE, rviz_visual_tools::LARGE);

      Eigen::Affine3d text_pos {};
      text_pos.translation() = stand_vec;
      rvt.publishText(text_pos, std::to_string(torso_yrp(1)), rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE);

      rvt.trigger();
    } catch (tf2::TransformException &e) {
{'fraction_non_alphanumeric': 0.15032509752925877, 'fraction_numerical': 0.010143042912873863, 'mean_word_length': 5.980036297640654, 'pattern_counts': {'":': 0, '<': 22, '<?xml version=': 0, '>': 22, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3499411', 'n_tokens_mistral': 1459, 'n_tokens_neox': 1388, 'n_words': 225}