pi/ov_secondary_linux
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

merging

Browse Source
This commit is contained in:
admin1
2022-08-30 19:11:24 +03:00
parent 6dfe0a8dc5
commit 60e64db720
3 changed files with 22 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
loop_fusion/launch/posegraph.launch
View File

@@ -10,10 +10,10 @@
<!-- INPUT: keyframe poses / marginalized poses --> <!-- 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/extrinsic" to="/ov_msckf/loop_extrinsic"/>
<remap from="/vins_estimator/keyframe_point" to="/ov_msckf/loop_feats"/> <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"/> <remap from="/vins_estimator/intrinsics" to="/ov_msckf/loop_intrinsics"/>
<!-- INPUT: current system pose and points --> <!-- INPUT: current system pose and points -->
@@ -31,4 +31,4 @@
</launch> </launch>

16
loop_fusion/src/keyframe.cpp
View File

@@ -16,14 +16,14 @@ static void reduceVector(vector<Derived> &v, vector<uchar> status)
{ {
// changed by PI // changed by PI
int j = 0; int j = 0;
int range = 0; // int range = 0;
if ( int(status.size()) <= int(v.size()) ){ // if ( int(status.size()) <= int(v.size()) ){
range = (int)status.size(); // range = (int)status.size();
} // }
else{ // else{
range = (int)v.size(); // range = (int)v.size();
} // }
for (int i = 0; i < range; i++) for (int i = 0; i < int(v.size()); i++)
if (status[i]) if (status[i])
v[j++] = v[i]; v[j++] = v[i];
v.resize(j); v.resize(j);

22
loop_fusion/src/pose_graph_node.cpp
View File

@@ -589,19 +589,19 @@ int main(int argc, char **argv)
// Get camera information // Get camera information
printf("[POSEGRAPH]: waiting for camera info topic...\n"); // 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)); // auto msg1 = ros::topic::waitForMessage<sensor_msgs::CameraInfo>("/ov_msckf/camera_info", ros::Duration(ros::DURATION_MAX));
intrinsics_callback(msg1); // intrinsics_callback(msg1);
printf("[POSEGRAPH]: received camera info message!\n"); // printf("[POSEGRAPH]: received camera info message!\n");
std::cout << m_camera.get()->parametersToString() << std::endl; // std::cout << m_camera.get()->parametersToString() << std::endl;
// Get camera to imu information // Get camera to imu information
printf("[POSEGRAPH]: waiting for camera to imu extrinsics topic...\n"); // 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)); // auto msg2 = ros::topic::waitForMessage<nav_msgs::Odometry>("/ov_msckf/odomimu", ros::Duration(ros::DURATION_MAX));
extrinsic_callback(msg2); // extrinsic_callback(msg2);
printf("[POSEGRAPH]: received camera to imu extrinsics message!\n"); // printf("[POSEGRAPH]: received camera to imu extrinsics message!\n");
std::cout << qic.transpose() << std::endl; // std::cout << qic.transpose() << std::endl;
std::cout << tic.transpose() << std::endl; // std::cout << tic.transpose() << std::endl;
// Setup the rest of the publishers // Setup the rest of the publishers
ros::Subscriber sub_vio1 = n.subscribe("/vins_estimator/odometry", 2000, vio_callback); ros::Subscriber sub_vio1 = n.subscribe("/vins_estimator/odometry", 2000, vio_callback);