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 @@
-
+
-
+
@@ -31,4 +31,4 @@
-
\ No newline at end of file
+
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 &v, vector 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("/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("/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("/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("/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);