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 -->
<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>
</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
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);

22
loop_fusion/src/pose_graph_node.cpp
View File

@@ -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);