v2

ov_eval/src/live_align_trajectory.cpp

@@ -20,6 +20,7 @@
  */
 
 #include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/TransformStamped.h>
 #include <nav_msgs/Path.h>
 #include <ros/ros.h>
 
@@ -30,7 +31,7 @@
 #include "utils/print.h"
 #include "utils/quat_ops.h"
 
-ros::Publisher pub_path;
+ros::Publisher pub_path, pub_T_align;
 void align_and_publish(const nav_msgs::Path::ConstPtr &msg);
 std::vector<double> times_gt;
 std::vector<Eigen::Matrix<double, 7, 1>> poses_gt;
@@ -71,6 +72,7 @@ int main(int argc, char **argv) {
   // Our subscribe and publish nodes
   ros::Subscriber sub = nh.subscribe("/ov_msckf/pathimu", 1, align_and_publish);
   pub_path = nh.advertise<nav_msgs::Path>("/ov_msckf/pathgt", 2);
+  pub_T_align = nh.advertise<geometry_msgs::TransformStamped>("/ov_msckf/T_align", 2);
   ROS_INFO("Subscribing: %s", sub.getTopic().c_str());
   ROS_INFO("Publishing: %s", pub_path.getTopic().c_str());
 
@@ -114,9 +116,43 @@ void align_and_publish(const nav_msgs::Path::ConstPtr &msg) {
   // Perform alignment of the trajectories
   Eigen::Matrix3d R_ESTtoGT;
   Eigen::Vector3d t_ESTinGT;
+  // scale.
   double s_ESTtoGT;
   ov_eval::AlignTrajectory::align_trajectory(poses_temp, gt_poses_temp, R_ESTtoGT, t_ESTinGT, s_ESTtoGT, alignment_type);
+
+  // Added by PI
+  Eigen::Matrix4d T_ESTinGT;
+  T_ESTinGT <<
+            s_ESTtoGT * R_ESTtoGT(0, 0), R_ESTtoGT(0, 1), R_ESTtoGT(0, 2), t_ESTinGT(0),
+            R_ESTtoGT(1, 0), s_ESTtoGT * R_ESTtoGT(1, 1), R_ESTtoGT(1, 2), t_ESTinGT(1),
+            R_ESTtoGT(2, 0), R_ESTtoGT(1, 0), s_ESTtoGT * R_ESTtoGT(1, 0), t_ESTinGT(2),
+            0, 0, 0, 1;
+  // Every time align_and_publish is called, we will publish the values R and t.
+  // Assuming that the current timestamp is the timestamp of the last pose in the queue.
+  geometry_msgs::TransformStamped transformStamped;
+  geometry_msgs::Transform transform;
+  geometry_msgs::Quaternion q_to_transform;
+  geometry_msgs::Vector3 v_to_transform;
+  // End of added by PI
+
   Eigen::Vector4d q_ESTtoGT = ov_core::rot_2_quat(R_ESTtoGT);
+
+  q_to_transform.x = q_ESTtoGT(0);
+  q_to_transform.y = q_ESTtoGT(1);
+  q_to_transform.z = q_ESTtoGT(2);
+  q_to_transform.w = q_ESTtoGT(3);
+
+  v_to_transform.x = t_ESTinGT(0);
+  v_to_transform.x = t_ESTinGT(1);
+  v_to_transform.x = t_ESTinGT(2);
+
+  transform.rotation = q_to_transform;
+  transform.translation = v_to_transform;
+  transformStamped.transform = transform;
+  transformStamped.header = msg->poses[msg->poses.size()-1].header;
+
+  pub_T_align.publish(transformStamped);
+
   PRINT_DEBUG("[TRAJ]: q_ESTtoGT = %.3f, %.3f, %.3f, %.3f | p_ESTinGT = %.3f, %.3f, %.3f | s = %.2f\n", q_ESTtoGT(0), q_ESTtoGT(1),
               q_ESTtoGT(2), q_ESTtoGT(3), t_ESTinGT(0), t_ESTinGT(1), t_ESTinGT(2), s_ESTtoGT);
 

ov_msckf/src/ros/ROS1Visualizer.cpp

@@ -74,6 +74,9 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr<ros::NodeHandle> nh, std::shared_
   it_pub_loop_img_depth_color = it.advertise("/ov_msckf/loop_depth_colored", 2);
 
   // Added by PI on 27.07.2022
+  // В какой топик публикуется ?
+  subs_T_align = nh->subscribe("/ov_msckf/T_align", 1000, &ROS1Visualizer::T_align_callback, this);
+
     std::ifstream fileTraj;
     fileTraj.open("/home/pi/workspace/catkin_ws_ov/src/open_vins/ov_data/merged.txt");
     Eigen::Matrix<double, 7, 1> extractedValues;
@@ -1122,3 +1125,44 @@ void ROS1Visualizer::publish_loopclosure_information() {
   }
 }
 
+void ROS1Visualizer::T_align_callback(const geometry_msgs::TransformStamped &msg){
+    // the main thing
+    const uint pointsLen = cubeVisPoints.size();
+    std::cout << "1" << std::endl;
+    Eigen::MatrixXd points(pointsLen, 4);
+    Eigen::Matrix4d T_align;
+
+    Eigen::Vector4d qRot;
+    qRot << msg.transform.rotation.x, msg.transform.rotation.y, msg.transform.rotation.z, msg.transform.rotation.w;
+    Eigen::Matrix3d mRot = ov_core::quat_2_Rot(qRot);
+
+    for (int i=0;i<3;i++){
+        for (int j=0;j<3;j++){
+            T_align(i,j) = mRot(i,j);
+        }
+    }
+    std::cout << "2" << std::endl;
+    T_align(0,3) = msg.transform.translation.x;
+    T_align(1,3) = msg.transform.translation.y;
+    T_align(2,3) = msg.transform.translation.z;
+    T_align(3,3) = 1;
+    std::cout << "3" << std::endl;
+
+    for (int i=0;i<cubeVisPoints.size();i++){
+        points(i, 0) = cubeVisPoints[0][0];
+        points(i, 1) = cubeVisPoints[0][1];
+        points(i, 2) = cubeVisPoints[0][2];
+        points(i, 3) = 1;
+    }
+    std::cout << "4" << std::endl;
+
+    points = (T_align * points.transpose()).transpose();
+    for (int i=0;i<cubeVisPoints.size();i++){
+        cubeVisPoints[0][0] = points(i, 0);
+        cubeVisPoints[0][1] = points(i, 1);
+        cubeVisPoints[0][2] = points(i, 2);
+    }
+    std::cout << "5" << std::endl;
+
+}
+

ov_msckf/src/ros/ROS1Visualizer.h

@@ -113,6 +113,9 @@ public:
   /// Callback for inertial information
   void callback_inertial(const sensor_msgs::Imu::ConstPtr &msg);
 
+  //// Callback for T_align (added by Ivan Podmogilnyi)
+  void T_align_callback(const geometry_msgs::TransformStamped &msg);
+
   /// Callback for monocular cameras information
   void callback_monocular(const sensor_msgs::ImageConstPtr &msg0, int cam_id0);
 
@@ -167,7 +170,7 @@ protected:
   ros::Publisher vis_pub;
   sensor_msgs::CameraInfo camInfoGlob;
   sensor_msgs::ImagePtr exl_msg_global;
-  ros::Subscriber sub_camera_info;
+  ros::Subscriber sub_camera_info, subs_T_align;
   ros::Publisher pub_poserec;
 
   std::vector<Eigen::Matrix<double, 7, 1>> loadedTrajectory;