Ray:更新vslam 摄像头加IMU代码

Workspace/Application/IMUDriver/IMUReader.cc

@@ -36,7 +36,7 @@ IMUReader::~IMUReader() {
 }
 
 // 获取 IMU 数据状态
-int IMUReader::IMUReadUartData() 
+int IMUReader::IMUReadData() 
 {
     uint8_t tmpdata[24];
 
@@ -64,14 +64,31 @@ int IMUReader::IMUReadUartData()
     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();
 
-    // 从 tmpdata 中提取数据
+    IMUData newData;
-    data.gyroX   = *(float *)(tmpdata+0);
+    newData.vGyro.x = *(float *)(tmpdata+0);
-    data.gyroY   = *(float *)(tmpdata+4);
+    newData.vGyro.y = *(float *)(tmpdata+4);
-    data.gyroZ   = *(float *)(tmpdata+8);
+    newData.vGyro.z  = *(float *)(tmpdata+8);
-    data.accelX  = *(float *)(tmpdata+12);
+    newData.vAcc.x = *(float *)(tmpdata+12);
-    data.accelY  = *(float *)(tmpdata+16);
+    newData.vAcc.y = *(float *)(tmpdata+16);
-    data.accelZ  = *(float *)(tmpdata+20);
+    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);
@@ -79,16 +96,22 @@ int IMUReader::IMUReadUartData()
     return 1;
 }
 
+
+
 // 获取 IMU 数据状态
 int IMUReader::ReadData(IMUData& data) {
-    // 模拟获取实际数据的过程，这里使用随机数
+ 
-    data.accelX = this->data.accelX;
+    data.vAcc.x= this->data.vAcc.x;
-    data.accelY = this->data.accelY;
+    data.vAcc.y = this->data.vAcc.y;
-    data.accelZ = this->data.accelZ;
+    data.vAcc.z = this->data.vAcc.z;
-    data.gyroX = this->data.gyroX;
+    data.vGyro.x= this->data.vGyro.x;
-    data.gyroY = this->data.gyroY;
+    data.vGyro.y = this->data.vGyro.y;
-    data.gyroZ = this->data.gyroZ;
+    data.vGyro.z = this->data.vGyro.z;
-    // 返回状态，这里简单地返回 0 表示成功
+    data.timestamp = this->data.timestamp;
+
+    
+
+  
     return 0;
 }
 
@@ -108,12 +131,8 @@ void IMUReader::IMUInit(std::string ComNumber) {
 // 读取 IMU 数据的循环
 void IMUReader::IMUReadDataThread() {
     while (isRunning) {
-        int status = IMUReadUartData();
+        int status = IMUReadData();
         // 处理获取的数据，这里简单地
         // Add delay fun.
     }
-}
-
-float IMUReader::getRandomFloat(){
-    return 1.22f;
 }

Workspace/Application/IMUDriver/IMUReader.h

@@ -1,4 +1,3 @@
-// IMUReader.h
 #pragma once
 
 #include <iostream>
@@ -7,15 +6,20 @@
 #include <string>
 #include <boost/asio.hpp>
 #include <boost/array.hpp>
+#include <vector>
+#include <opencv2/core/core.hpp>
+
+
+using namespace std;
 
 // 结构体定义 IMU 数据
 struct IMUData {
-    float accelX;
+   
-    float accelY;
+    cv::Point3f vAcc;
-    float accelZ;
+    cv::Point3f vGyro;
-    float gyroX;
+    double timestamp;
-    float gyroY;
+   
-    float gyroZ;
+
 };
 
 class IMUReader {
@@ -34,7 +38,13 @@ public:
 
     // 获取 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;
@@ -43,11 +53,12 @@ private:
     bool        isRunning; // 控制是否退出循环的标志变量
     std::thread dataThread; // 数据读取线程
     IMUData     data;
+    mutable std::mutex dataMutex;
+   
+    
     // 读取 IMU 数据的循环
-    int IMUReadUartData();
 
     void IMUReadDataThread(void);
 
-    // 辅助函数，生成随机浮点数（模拟实际获取的传感器数据）
+   
-    float getRandomFloat();
 };

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();
+
+}