甘肃龙网

  1. 首页 > 生活 > >

VINS( 四 )

生活百科坐标


我看了博客上也有人遇到这个问题，和他们交流他们也不知道问题所在，好在还是弄出来了！对后面的同学一定很有帮助！
运行程序 打开第一个终端
打开第二个终端
进入工作区间内，分别输入：
cd /
//单独一个端口，编译后关闭，没有修改程序不用编译，可以用于检查程序问题
devel/setup.bash
vins .
打开第三个终端
进入工作区间内，分别输入：
cd /
devel/setup.bash
vinssrc/VINS-///g.yaml /home////
单目
vinssrc/VINS-///.yaml /home////
打开第四个终端
进入工作区间内，分别输入：
cd /
devel/setup.bash
情况一、单目+IMU
个人感觉效果还是可以的！我们再来看看 双目+IMU
情况二、双目+IMU
评价：一个字，惨不忍睹！
我有点想不明白了，单目+IMU 跑kitti数据可以，为什么吗 双目+IMU 就不行？
你要是说是程序有问题，但是 程序跑RUROC的双目+IMU时，也是好的呀！
而且跑kitti数据的双目程序也是好的呀，这样一排除，个人感觉还是出在了外参上面，如果发现我错误的小伙伴别忘了留言提醒我！
对了，我看了相关数据介绍的论文了，好像GPS/IMU是与相机不同步，可是不同步为什么 单目+IMU效果是好的？
要么都不同步呗，这个我无法理解！注意咱的思路，后面验证我是对的！
上面问题已经解决了！过程很艰辛，但是我想说的是我的思路很好，不然估计还是解决不了！处理的方式上面已经给了！
修正后，双目+IMU+GPS 的效果如下：
完美！
对应的程序如下： 添加的主程序：.cpp
/*说明：本程序尝试：相机+IMU+GPS，其实VINS-fusion 已经具备这个条件了，主要在于结合；本次程序是rosNodeTest.cpp 和 KITTIGPS.cpp 的结合；通过读取不同的配置文件，可以决定是单目相机还是双目相机；先尝试用Kitti的数据，在尝试用自己采集的数据！*/#include #include #include #include #include #include #include #include #include #include #include #include #include #include "estimator/estimator.h"#include "utility/visualization.h"#include "camera_imu_gps.h"using namespace std;using namespace Eigen;Estimator estimator;ros::Publisher pubGPS;std::mutex m_buf;void readIMUdata(const std::string &line, IMU_MSG &imuObs);void readGPSdata(const std::string &line, GPS_MSG &gpsObs);void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg){map>> featureFrame;for (unsigned int i = 0; i < feature_msg->points.size(); i++){int feature_id = feature_msg->channels[0].values[i];int camera_id = feature_msg->channels[1].values[i];double x = feature_msg->points[i].x;double y = feature_msg->points[i].y;double z = feature_msg->points[i].z;double p_u = feature_msg->channels[2].values[i];double p_v = feature_msg->channels[3].values[i];double velocity_x = feature_msg->channels[4].values[i];double velocity_y = feature_msg->channels[5].values[i];if(feature_msg->channels.size() > 5){double gx = feature_msg->channels[6].values[i];double gy = feature_msg->channels[7].values[i];double gz = feature_msg->channels[8].values[i];pts_gt[feature_id] = Eigen::Vector3d(gx, gy, gz);//printf("receive pts gt %d %f %f %f\n", feature_id, gx, gy, gz);}ROS_ASSERT(z == 1);Eigen::Matrix xyz_uv_velocity;xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;featureFrame[feature_id].emplace_back(camera_id,xyz_uv_velocity);}double t = feature_msg->header.stamp.toSec();estimator.inputFeature(t, featureFrame);return;}void restart_callback(const std_msgs::BoolConstPtr &restart_msg){if (restart_msg->data =http://www.kingceram.com/post/= true){ROS_WARN("restart the estimator!");estimator.clearState();estimator.setParameter();}return;}void imu_switch_callback(const std_msgs::BoolConstPtr &switch_msg){if (switch_msg->data =http://www.kingceram.com/post/= true){ROS_WARN("use IMU!");estimator.changeSensorType(1, STEREO);}else{ROS_WARN("disable IMU!");estimator.changeSensorType(0, STEREO);}return;}void cam_switch_callback(const std_msgs::BoolConstPtr &switch_msg){if (switch_msg->data =http://www.kingceram.com/post/= true){ROS_WARN("use stereo!");estimator.changeSensorType(USE_IMU, 1);}else{ROS_WARN("use mono camera (left)!");estimator.changeSensorType(USE_IMU, 0);}return;}int main(int argc, char** argv){ros::init(argc, argv, "vins_estimator");ros::NodeHandle n("~");ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);pubGPS = n.advertise("/gps", 1000);ros::Subscriber sub_feature = n.subscribe("/feature_tracker/feature", 2000, feature_callback);ros::Subscriber sub_restart = n.subscribe("/vins_restart", 100, restart_callback);ros::Subscriber sub_imu_switch = n.subscribe("/vins_imu_switch", 100, imu_switch_callback);ros::Subscriber sub_cam_switch = n.subscribe("/vins_cam_switch", 100, cam_switch_callback);if(argc != 3){printf("please intput: rosrun vins camera_IMU_GPS_test [config file] [data folder] \n""for example: rosrun vins camera_IMU_GPS_test ""~/catkin_ws/src/VINS-Fusion/config/test_data/stereo_imu_gps_config.yaml ""/home/hltt3838/kitti_data/2011_10_03_drive_0042_sync/ \n");return 1;}string config_file = argv[1];//stereo_imu_gps_config.yamlprintf("config_file: %s\n", argv[1]);string sequence = argv[2];//---/home/hltt3838/kitti_data/2011_10_03_drive_0042_sync/printf("read sequence: %s\n", argv[2]);string dataPath = sequence + "/";//1、读取imu的 txt 文件，一行一行读取double init_imu_time;IMU_MSG imuObs;std::string line_imu;std::string imuPath = dataPath + "imu_data_100hz/imu.txt";std::ifstreamcsv_IMUfile(imuPath); if (!csv_IMUfile){printf("cannot find imu Path \n" );return 0;}std::getline(csv_IMUfile, line_imu); //header, 获得的第一个IMU数据readIMUdata(line_imu, imuObs);init_imu_time = imuObs.time;printf("init_imu_time： .5f \n", init_imu_time);//2、读取GPS的 txt 文件，一行一行读取double init_gps_time;GPS_MSG gpsObs;std::string line_gps;std::string gpsPath = dataPath + "gps_data_10hz/gps.txt";std::ifstreamcsv_GPSfile(gpsPath);if (!csv_GPSfile){printf("cannot find gps Path \n" );return 0;}std::getline(csv_GPSfile, line_gps); //header, 获得的第一个gps数据readGPSdata(line_gps, gpsObs);init_gps_time = gpsObs.time;printf("init_gps_time： .5f \n", init_gps_time); //3、读取图像时间，整个文件读取，存放到 imageTimeList,两个相机对齐了，没有进行判断//Cam0double init_cam_time;FILE* file;file = std::fopen((dataPath + "image_00/timestamps.txt").c_str() , "r");if(file == NULL){printf("cannot find file: %simage_00/timestamps.txt \n", dataPath.c_str());ROS_BREAK();return 0;}vector


上一篇：电视机品牌排行榜前十名 电视机十大品牌排行榜

下一篇：2023年中国高校计算机大赛