From 9422b11149bc8a9f403a7346f7a55129c12a4c41 Mon Sep 17 00:00:00 2001 From: ray <2954701669@qq.com> Date: Wed, 27 Dec 2023 11:39:39 +0800 Subject: [PATCH] =?UTF-8?q?Ray:=E6=9B=B4=E6=96=B0vslam=20=E6=91=84?= =?UTF-8?q?=E5=83=8F=E5=A4=B4=E5=8A=A0IMU=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../IMUDriver/{IMUReader.cpp => IMUReader.cc} | 61 ++++++++----- Workspace/Application/IMUDriver/IMUReader.h | 31 ++++--- .../Application/mono_inertial_euroc_real.cc | 90 +++++++++++++++++++ 3 files changed, 151 insertions(+), 31 deletions(-) rename Workspace/Application/IMUDriver/{IMUReader.cpp => IMUReader.cc} (68%) create mode 100644 Workspace/Application/mono_inertial_euroc_real.cc diff --git a/Workspace/Application/IMUDriver/IMUReader.cpp b/Workspace/Application/IMUDriver/IMUReader.cc similarity index 68% rename from Workspace/Application/IMUDriver/IMUReader.cpp rename to Workspace/Application/IMUDriver/IMUReader.cc index 5888b68..4ff959b 100644 --- a/Workspace/Application/IMUDriver/IMUReader.cpp +++ b/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::system_clock::now().time_since_epoch() ).count(); - // 从 tmpdata 中提取数据 - data.gyroX = *(float *)(tmpdata+0); - data.gyroY = *(float *)(tmpdata+4); - data.gyroZ = *(float *)(tmpdata+8); - data.accelX = *(float *)(tmpdata+12); - data.accelY = *(float *)(tmpdata+16); - data.accelZ = *(float *)(tmpdata+20); + 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); @@ -79,16 +96,22 @@ int IMUReader::IMUReadUartData() return 1; } + + // 获取 IMU 数据状态 int IMUReader::ReadData(IMUData& data) { - // 模拟获取实际数据的过程，这里使用随机数 - data.accelX = this->data.accelX; - data.accelY = this->data.accelY; - data.accelZ = this->data.accelZ; - data.gyroX = this->data.gyroX; - data.gyroY = this->data.gyroY; - data.gyroZ = this->data.gyroZ; - // 返回状态，这里简单地返回 0 表示成功 + + 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; } @@ -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; } \ No newline at end of file diff --git a/Workspace/Application/IMUDriver/IMUReader.h b/Workspace/Application/IMUDriver/IMUReader.h index 2a48db1..b115cbc 100644 --- a/Workspace/Application/IMUDriver/IMUReader.h +++ b/Workspace/Application/IMUDriver/IMUReader.h @@ -1,4 +1,3 @@ -// IMUReader.h #pragma once #include @@ -7,15 +6,20 @@ #include #include #include +#include +#include + + +using namespace std; // 结构体定义 IMU 数据 struct IMUData { - float accelX; - float accelY; - float accelZ; - float gyroX; - float gyroY; - float gyroZ; + + cv::Point3f vAcc; + cv::Point3f vGyro; + double timestamp; + + }; class IMUReader { @@ -34,7 +38,13 @@ public: // 获取 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; @@ -43,11 +53,12 @@ private: bool isRunning; // 控制是否退出循环的标志变量 std::thread dataThread; // 数据读取线程 IMUData data; + mutable std::mutex dataMutex; + + // 读取 IMU 数据的循环 - int IMUReadUartData(); void IMUReadDataThread(void); - // 辅助函数，生成随机浮点数（模拟实际获取的传感器数据） - float getRandomFloat(); + }; diff --git a/Workspace/Application/mono_inertial_euroc_real.cc b/Workspace/Application/mono_inertial_euroc_real.cc new file mode 100644 index 0000000..a2ba10b --- /dev/null +++ b/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