image0TimeList;int year, month, day;int hour, minute;double second;while (fscanf(file, "%d-%d-%d %d:%d:%lf", &year, &month, &day, &hour, &minute, &second) != EOF){image0TimeList.push_back(hour * 60 * 60 + minute * 60 + second);}std::fclose(file);init_cam_time = image0TimeList[0]; printf("init_cam_time: .5f \n", init_cam_time);double baseTime;baseTime = min(init_imu_time,init_gps_time,init_cam_time);printf("baseTime: .5f \n", baseTime);//4、读取配置参数和发布主题readParameters(config_file);estimator.setParameter();registerPub(n);//5、VIO的结果输出保存文件FILE* outFile;outFile = fopen((OUTPUT_FOLDER + "/vio.txt").c_str(),"w");if(outFile == NULL)printf("Output vio path dosen't exist: %s\n", OUTPUT_FOLDER.c_str());string leftImagePath, rightImagePath;cv::Mat imLeft, imRight;//6、遍历整个图像for (size_t i = 0; i < image0TimeList.size(); i++){ int num_imu = 0;if(ros::ok()){printf("process image %d\n", (int)i);stringstream ss;ss << setfill('0') << setw(10) << i;leftImagePath = dataPath + "image_00/data/" + ss.str() + ".png";rightImagePath = dataPath + "image_01/data/" + ss.str() + ".png";printf("%s\n", leftImagePath.c_str() );printf("%s\n", rightImagePath.c_str() );double imgTime = 0; imLeft= cv::imread(leftImagePath, CV_LOAD_IMAGE_GRAYSCALE );imRight = cv::imread(rightImagePath, CV_LOAD_IMAGE_GRAYSCALE );imgTime = image0TimeList[i] - baseTime;printf("image time: .5f \n", imgTime);//读取GPS信息std::getline(csv_GPSfile, line_gps);readGPSdata(line_gps, gpsObs);sensor_msgs::NavSatFix gps_position;gps_position.header.frame_id = "NED";gps_position.header.stamp = ros::Time(imgTime);gps_position.latitude= gpsObs.position[0];gps_position.longitude = gpsObs.position[1];gps_position.altitude= gpsObs.position[2];gps_position.position_covariance[0] = gpsObs.pos_accuracy;pubGPS.publish(gps_position);//---------------------加上IMU-------------------------////读取imu的信息while (std::getline(csv_IMUfile, line_imu)){num_imu++;printf("process imu %d\n",num_imu);readIMUdata(line_imu, imuObs);double imuTime = imuObs.time - baseTime;printf("imu time: .5f \n", imuTime);Vector3d acc = imuObs.acc;Vector3d gyr = imuObs.gyr;estimator.inputIMU(imuTime, acc, gyr);if (imuTime >= imgTime) //简单的时间同步,IMU的时间戳是不大于图像的{break;}}//---------------------加上IMU-------------------------//if(STEREO)//双目为1，否则为0{estimator.inputImage(imgTime,imLeft, imRight);}elseestimator.inputImage(imgTime, imLeft);Eigen::Matrix pose;estimator.getPoseInWorldFrame(pose);if(outFile != NULL)fprintf (outFile, "%f %f %f %f %f %f %f %f %f %f %f %f \n",pose(0,0), pose(0,1), pose(0,2),pose(0,3),pose(1,0), pose(1,1), pose(1,2),pose(1,3),pose(2,0), pose(2,1), pose(2,2),pose(2,3));//cv::imshow("leftImage", imLeft);//cv::imshow("rightImage", imRight);//cv::waitKey(2);}elsebreak;}if(outFile != NULL)fclose (outFile);return 0;}void readIMUdata(const std::string &line, IMU_MSG &imuObs){std::stringstreamlineStream(line);std::stringdataRecord[7];std::getline(lineStream, dataRecord[0], ' ');//这里的数据间是空格，如果有逗号，用'，'std::getline(lineStream, dataRecord[1], ' ');std::getline(lineStream, dataRecord[2], ' ');std::getline(lineStream, dataRecord[3], ' ');std::getline(lineStream, dataRecord[4], ' ');std::getline(lineStream, dataRecord[5], ' ');std::getline(lineStream, dataRecord[6], ' ');imuObs.time = std::stod(dataRecord[0]); //时间：s;imuObs.acc[0] = std::stod(dataRecord[1]);imuObs.acc[1] = std::stod(dataRecord[2]);imuObs.acc[2] = std::stod(dataRecord[3]);imuObs.gyr[0] = std::stod(dataRecord[4]);imuObs.gyr[1] = std::stod(dataRecord[5]);imuObs.gyr[2] = std::stod(dataRecord[6]);}void readGPSdata(const std::string &line, GPS_MSG &gpsObs){std::stringstreamlineStream(line);std::stringdataRecord[7];std::getline(lineStream, dataRecord[0], ' ');//这里的数据间是空格，如果有逗号，用'，'std::getline(lineStream, dataRecord[1], ' ');std::getline(lineStream, dataRecord[2], ' ');std::getline(lineStream, dataRecord[3], ' ');std::getline(lineStream, dataRecord[4], ' ');gpsObs.time = std::stod(dataRecord[0]); //时间：s;gpsObs.position[0] = std::stod(dataRecord[1]);gpsObs.position[1] = std::stod(dataRecord[2]);gpsObs.position[2] = std::stod(dataRecord[3]);gpsObs.pos_accuracy = std::stod(dataRecord[4]);}
