forked from logzhan/ORB-SLAM3-UESTC
90 lines
2.1 KiB
C++
90 lines
2.1 KiB
C++
|
#include<iostream>
|
||
|
|
#include<algorithm>
|
||
|
|
#include<fstream>
|
||
|
|
#include<chrono>
|
||
|
|
#include <ctime>
|
||
|
|
#include <sstream>
|
||
|
|
#include "IMUReader.h"
|
||
|
|
#include<opencv2/core/core.hpp>
|
||
|
|
#include<System.h>
|
||
|
|
#include "ImuTypes.h"
|
||
|
|
|
||
|
|
using namespace std;
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
int main(int argc, char *argv[])
|
||
|
|
{
|
||
|
|
|
||
|
|
if(argc < 3)
|
||
|
|
{
|
||
|
|
cerr << endl << "Usage: ./mono_inertial_euroc path_to_vocabulary path_to_setting" << endl;
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR, true);
|
||
|
|
|
||
|
|
vector<ORB_SLAM3::IMU::Point> vImuMeas;
|
||
|
|
|
||
|
|
|
||
|
|
vImuMeas.clear();
|
||
|
|
int fps = 30;
|
||
|
|
IMUReader imuReader ("/dev/ttyUSB0");
|
||
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(1000/fps));
|
||
|
|
|
||
|
|
cv::VideoCapture cap = cv::VideoCapture(0);
|
||
|
|
cap.set(cv::CAP_PROP_FRAME_WIDTH, 640);
|
||
|
|
cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480);
|
||
|
|
|
||
|
|
if (!cap.isOpened()) {
|
||
|
|
std::cerr << "Error: Could not open camera." << std::endl;
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
while(1)
|
||
|
|
{
|
||
|
|
cv::Mat im;
|
||
|
|
cap >> im;
|
||
|
|
if(im.empty())
|
||
|
|
{
|
||
|
|
cerr << endl << "Error: Couldn't capture a frame"
|
||
|
|
<< endl;
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
vImuMeas.clear();
|
||
|
|
auto currentTime = std::chrono::high_resolution_clock::now();
|
||
|
|
auto timestamp = std::chrono::duration_cast<std::chrono::nanoseconds>(currentTime.time_since_epoch());
|
||
|
|
double tframe = static_cast<double>(timestamp.count());
|
||
|
|
|
||
|
|
// IMUData data;
|
||
|
|
|
||
|
|
while (!imuReader.imubuffer.empty()) {
|
||
|
|
IMUData myImuData = imuReader.imubuffer.front();
|
||
|
|
ORB_SLAM3::IMU::Point slamImuData(myImuData.vAcc, myImuData.vGyro, myImuData.timestamp);
|
||
|
|
|
||
|
|
// 添加到 SLAM 系统定义的向量中
|
||
|
|
vImuMeas.push_back(slamImuData);
|
||
|
|
|
||
|
|
imuReader.imubuffer.erase(imuReader.imubuffer.begin());
|
||
|
|
}
|
||
|
|
cout << "Accumulated IMU Data Size: " << vImuMeas.size() << endl;
|
||
|
|
|
||
|
|
// 清空 imu 缓冲区
|
||
|
|
|
||
|
|
// imuReader.imubuffer.clear();
|
||
|
|
|
||
|
|
// SLAM.TrackMonocular(im, tframe, vImuMeas);
|
||
|
|
// vImuMeas.clear();
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
// Stop all threads
|
||
|
|
// SLAM.Shutdown();
|
||
|
|
|
||
|
|
}
|