diff --git a/ORB-SLAM3-UESTC/Workspace/Application/IMUDriver/IMUReader.cc b/ORB-SLAM3-UESTC/Workspace/Application/IMUDriver/IMUReader.cc new file mode 100755 index 0000000..48cda73 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Application/IMUDriver/IMUReader.cc @@ -0,0 +1,138 @@ +// IMUReader.cpp +#include +#include +#include "IMUReader.h" + +// 构造函数 +IMUReader::IMUReader() { + isRunning = true; + // 在构造函数中进行初始化工作 + IMUInit("COM0"); + // 创建一个新线程运行数据读取循环 + dataThread = std::thread(&IMUReader::IMUReadDataThread, this); +} + +// 构造函数 +IMUReader::IMUReader(std::string ComNumber) { + isRunning = true; + // 在构造函数中进行初始化工作 + IMUInit(ComNumber); + // 创建一个新线程运行数据读取循环 + dataThread = std::thread(&IMUReader::IMUReadDataThread, this); +} + +// 析构函数 +IMUReader::~IMUReader() { + // 在析构函数中通过改变 isExist 变量为 false 退出循环 + isRunning = false; + // 等待数据读取线程结束 + if (dataThread.joinable()) { + dataThread.join(); + } + // realease serial object. + if(serial != NULL){ + delete serial; + } +} + +// 获取 IMU 数据状态 +int IMUReader::IMUReadData() +{ + uint8_t tmpdata[24]; + + // 读取串口数据 + size_t bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 1)); + + // check frame head + if(tmpdata[0] != 0xfc){ + return 0; + } + // check frame type + bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 1)); + if(tmpdata[0] != 0x40){ + return 0; + } + + bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 1)); + // check imu data len + if(tmpdata[0] != 0x38){ + return 0; + } + + bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 1)); + bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 1)); + bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 1)); + bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 1)); + bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 24)); + + + // data.vGyro.x = *(float *)(tmpdata+0); + // data.vGyro.y = *(float *)(tmpdata+4); + // data.vGyro.z = *(float *)(tmpdata+8); + // data.vAcc.x = *(float *)(tmpdata+12); + // data.vAcc.y = *(float *)(tmpdata+16); + // data.vAcc.z = *(float *)(tmpdata+20); + // data.timestamp = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch() ).count(); + + IMUData newData; + newData.vGyro.x = *(float *)(tmpdata+0); + newData.vGyro.y = *(float *)(tmpdata+4); + newData.vGyro.z = *(float *)(tmpdata+8); + newData.vAcc.x = *(float *)(tmpdata+12); + newData.vAcc.y = *(float *)(tmpdata+16); + newData.vAcc.z = *(float *)(tmpdata+20); + newData.timestamp = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()).count(); + + std::lock_guard lock(dataMutex); + imubuffer.push_back(newData); + + + + + // printf("gyro = %f,%f,%f, Acc = %f,%f,%f\n", data.gyroX,data.gyroY,data.gyroZ, + // data.accelX, data.accelY, data.accelZ); + // 返回状态,这里简单地返回 0 表示成功 + return 1; +} + + + +// 获取 IMU 数据状态 +int IMUReader::ReadData(IMUData& data) { + + data.vAcc.x= this->data.vAcc.x; + data.vAcc.y = this->data.vAcc.y; + data.vAcc.z = this->data.vAcc.z; + data.vGyro.x= this->data.vGyro.x; + data.vGyro.y = this->data.vGyro.y; + data.vGyro.z = this->data.vGyro.z; + data.timestamp = this->data.timestamp; + + + + + return 0; +} + +// 初始化 IMU +void IMUReader::IMUInit(std::string ComNumber) { + // 这里可以进行具体的初始化工作 + std::cout << "IMU Initialization..." << ComNumber << std::endl; + serial = new boost::asio::serial_port(io, ComNumber); + + if(serial == NULL){ + return; + } + serial->set_option(boost::asio::serial_port_base::baud_rate(921600)); // 设置波特率 + serial->set_option(boost::asio::serial_port_base::character_size(8)); // 设置数据位 +} + +// 读取 IMU 数据的循环 +void IMUReader::IMUReadDataThread() { + while (isRunning) { + int status = IMUReadData(); + // 处理获取的数据,这里简单地 + // Add delay fun. + } +} \ No newline at end of file diff --git a/ORB-SLAM3-UESTC/Workspace/Application/IMUDriver/IMUReader.h b/ORB-SLAM3-UESTC/Workspace/Application/IMUDriver/IMUReader.h new file mode 100755 index 0000000..fad4440 --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Application/IMUDriver/IMUReader.h @@ -0,0 +1,64 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + + +using namespace std; + +// 结构体定义 IMU 数据 +struct IMUData { + + cv::Point3f vAcc; + cv::Point3f vGyro; + double timestamp; + + +}; + +class IMUReader { +public: + // 构造函数 + IMUReader(std::string ComNumber); + + // 构造函数 + IMUReader(); + + // 析构函数 + ~IMUReader(); + + // 初始化 IMU + void IMUInit(std::string ComNumber); + + // 获取 IMU 数据状态 + int ReadData(IMUData& data); + int IMUReadData(); + std::vector imubuffer; + + IMUData getdata() const{ + std::unique_lock lock(dataMutex); + return data; + }; +private: + boost::asio::io_service io; + boost::asio::serial_port* serial; + // boost::asio::serial_port serial(io, "/dev/ttyUSB0"); // 替换成你的串口设备路径 + + bool isRunning; // 控制是否退出循环的标志变量 + std::thread dataThread; // 数据读取线程 + IMUData data; + mutable std::mutex dataMutex; + + + // 读取 IMU 数据的循环 + + void IMUReadDataThread(void); + + +}; \ No newline at end of file diff --git a/ORB-SLAM3-UESTC/Workspace/Application/mono_inertial_euroc_real.cc b/ORB-SLAM3-UESTC/Workspace/Application/mono_inertial_euroc_real.cc new file mode 100644 index 0000000..a2ba10b --- /dev/null +++ b/ORB-SLAM3-UESTC/Workspace/Application/mono_inertial_euroc_real.cc @@ -0,0 +1,90 @@ +#include +#include +#include +#include +#include +#include +#include "IMUReader.h" +#include +#include +#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 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(currentTime.time_since_epoch()); + double tframe = static_cast(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(); + +} \ No newline at end of file