From 60e64db720e7c965abdaa52e16fc9a506875cd64 Mon Sep 17 00:00:00 2001
From: admin1 <admin1@drivecast.ru>
Date: Tue, 30 Aug 2022 19:11:24 +0300
Subject: [PATCH] merging

---
 loop_fusion/launch/posegraph.launch |  6 +++---
 loop_fusion/src/keyframe.cpp        | 16 ++++++++--------
 loop_fusion/src/pose_graph_node.cpp | 22 +++++++++++-----------
 3 files changed, 22 insertions(+), 22 deletions(-)

diff --git a/loop_fusion/launch/posegraph.launch b/loop_fusion/launch/posegraph.launch
index c058286..8b41a9f 100644
--- a/loop_fusion/launch/posegraph.launch
+++ b/loop_fusion/launch/posegraph.launch
@@ -10,10 +10,10 @@
 
 
         <!-- INPUT: keyframe poses / marginalized poses -->
-        <remap from="/cam0/image_raw"                  to="/stereo/left/image_raw"/>
+        <remap from="/cam0/image_raw"                  to="/cam0/image_raw"/>
         <remap from="/vins_estimator/extrinsic"        to="/ov_msckf/loop_extrinsic"/>
         <remap from="/vins_estimator/keyframe_point"   to="/ov_msckf/loop_feats"/>
-        <remap from="/vins_estimator/keyframe_pose"    to="/ov_msckf/odomimu"/>
+        <remap from="/vins_estimator/keyframe_pose"    to="/ov_msckf/loop_pose"/>
         <remap from="/vins_estimator/intrinsics"       to="/ov_msckf/loop_intrinsics"/>
 
         <!-- INPUT: current system pose and points -->
@@ -31,4 +31,4 @@
 
 
 
-</launch>
\ No newline at end of file
+</launch>
diff --git a/loop_fusion/src/keyframe.cpp b/loop_fusion/src/keyframe.cpp
index eb7d1e0..dfdc7f1 100644
--- a/loop_fusion/src/keyframe.cpp
+++ b/loop_fusion/src/keyframe.cpp
@@ -16,14 +16,14 @@ static void reduceVector(vector<Derived> &v, vector<uchar> status)
 {
     // changed by PI
     int j = 0;
-    int range = 0;
-    if ( int(status.size()) <= int(v.size()) ){
-        range = (int)status.size();
-    }
-    else{
-        range = (int)v.size();
-    }
-    for (int i = 0; i < range; i++)
+//    int range = 0;
+//    if ( int(status.size()) <= int(v.size()) ){
+//        range = (int)status.size();
+//    }
+//    else{
+//        range = (int)v.size();
+//    }
+    for (int i = 0; i < int(v.size()); i++)
         if (status[i])
             v[j++] = v[i];
     v.resize(j);
diff --git a/loop_fusion/src/pose_graph_node.cpp b/loop_fusion/src/pose_graph_node.cpp
index 12c83db..73307bb 100644
--- a/loop_fusion/src/pose_graph_node.cpp
+++ b/loop_fusion/src/pose_graph_node.cpp
@@ -589,19 +589,19 @@ int main(int argc, char **argv)
 
 
     // Get camera information
-    printf("[POSEGRAPH]: waiting for camera info topic...\n");
-    auto msg1 = ros::topic::waitForMessage<sensor_msgs::CameraInfo>("/ov_msckf/camera_info", ros::Duration(ros::DURATION_MAX));
-    intrinsics_callback(msg1);
-    printf("[POSEGRAPH]: received camera info message!\n");
-    std::cout << m_camera.get()->parametersToString() << std::endl;
+//    printf("[POSEGRAPH]: waiting for camera info topic...\n");
+//    auto msg1 = ros::topic::waitForMessage<sensor_msgs::CameraInfo>("/ov_msckf/camera_info", ros::Duration(ros::DURATION_MAX));
+//    intrinsics_callback(msg1);
+//    printf("[POSEGRAPH]: received camera info message!\n");
+//    std::cout << m_camera.get()->parametersToString() << std::endl;
 
     // Get camera to imu information
-    printf("[POSEGRAPH]: waiting for camera to imu extrinsics topic...\n");
-    auto msg2 = ros::topic::waitForMessage<nav_msgs::Odometry>("/ov_msckf/odomimu", ros::Duration(ros::DURATION_MAX));
-    extrinsic_callback(msg2);
-    printf("[POSEGRAPH]: received camera to imu extrinsics message!\n");
-    std::cout << qic.transpose() << std::endl;
-    std::cout << tic.transpose() << std::endl;
+//    printf("[POSEGRAPH]: waiting for camera to imu extrinsics topic...\n");
+//    auto msg2 = ros::topic::waitForMessage<nav_msgs::Odometry>("/ov_msckf/odomimu", ros::Duration(ros::DURATION_MAX));
+//    extrinsic_callback(msg2);
+//    printf("[POSEGRAPH]: received camera to imu extrinsics message!\n");
+//    std::cout << qic.transpose() << std::endl;
+//    std::cout << tic.transpose() << std::endl;
 
     // Setup the rest of the publishers
     ros::Subscriber sub_vio1 = n.subscribe("/vins_estimator/odometry", 2000, vio_callback);