ray

ORB-SLAM3-UESTC/Workspace/Application/IMUDriver/IMUReader.cc

@@ -0,0 +1,138 @@
+// IMUReader.cpp
+#include <iostream>
+#include <string>
+#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::nanoseconds>(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::nanoseconds>(
+        std::chrono::system_clock::now().time_since_epoch()).count();
+    
+    std::lock_guard<std::mutex> 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.
+    }
+}

ORB-SLAM3-UESTC/Workspace/Application/IMUDriver/IMUReader.h

@@ -0,0 +1,64 @@
+#pragma once
+
+#include <iostream>
+#include <thread>
+#include <chrono>
+#include <string>
+#include <boost/asio.hpp>
+#include <boost/array.hpp>
+#include <vector>
+#include <opencv2/core/core.hpp>
+
+
+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<IMUData> imubuffer;
+
+    IMUData getdata() const{
+        std::unique_lock<std::mutex> 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);
+
+   
+};

ORB-SLAM3-UESTC/Workspace/Application/mono_inertial_euroc_real.cc

@@ -0,0 +1,90 @@
+#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();
+
+}