v2

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;
+
+}
+