Skip to content

Commit

Permalink
Add imu data and timestamps more efficiently
Browse files Browse the repository at this point in the history
  • Loading branch information
@DavidPetkovsek
DavidPetkovsek committed Jun 20, 2023
1 parent b7ba529 commit 98c94b9
Show file tree
Hide file tree
Showing 4 changed files with 28 additions and 34 deletions.
6 changes: 3 additions & 3 deletions include/MORB_SLAM/System.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,18 +93,18 @@ class System
// Proccess the given stereo frame. Images must be synchronized and rectified.
// Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
// Returns the camera pose (empty if tracking fails).
Sophus::SE3f TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp, const std::vector<IMU::Point>& vImuMeas = std::vector<IMU::Point>(), std::string filename="");
Sophus::SE3f TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, double timestamp, const std::vector<IMU::Point>& vImuMeas = std::vector<IMU::Point>(), std::string filename="");

// Process the given rgbd frame. Depthmap must be registered to the RGB frame.
// Input image: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
// Input depthmap: Float (CV_32F).
// Returns the camera pose (empty if tracking fails).
Sophus::SE3f TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp, const std::vector<IMU::Point>& vImuMeas = std::vector<IMU::Point>(), std::string filename="");
Sophus::SE3f TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, double timestamp, const std::vector<IMU::Point>& vImuMeas = std::vector<IMU::Point>(), std::string filename="");

// Proccess the given monocular frame and optionally imu data
// Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
// Returns the camera pose (empty if tracking fails).
Sophus::SE3f TrackMonocular(const cv::Mat &im, const double &timestamp, const std::vector<IMU::Point>& vImuMeas = std::vector<IMU::Point>(), std::string filename="");
Sophus::SE3f TrackMonocular(const cv::Mat &im, double timestamp, const std::vector<IMU::Point>& vImuMeas = std::vector<IMU::Point>(), std::string filename="");


// This stops local mapping thread (map building) and performs only camera tracking.
Expand Down
2 changes: 1 addition & 1 deletion include/MORB_SLAM/Tracking.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,7 @@ class Tracking {
Sophus::SE3f GrabImageMonocular(const cv::Mat& im, const double& timestamp,
const std::string &filename, const Camera_ptr &cam);

void GrabImuData(const IMU::Point& imuMeasurement);
void GrabImuData(const std::vector<IMU::Point>& imuMeasurements);

void SetLocalMapper(LocalMapping* pLocalMapper);
void SetLoopClosing(LoopClosing* pLoopClosing);
Expand Down
15 changes: 6 additions & 9 deletions src/System.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -253,7 +253,7 @@ System::System(const std::string& strVocFile, const std::string& strSettingsFile
}

Sophus::SE3f System::TrackStereo(const cv::Mat& imLeft, const cv::Mat& imRight,
const double& timestamp,
double timestamp,
const std::vector<IMU::Point>& vImuMeas,
std::string filename) {
if (mSensor != CameraType::STEREO && mSensor != CameraType::IMU_STEREO) {
Expand Down Expand Up @@ -315,8 +315,7 @@ Sophus::SE3f System::TrackStereo(const cv::Mat& imLeft, const cv::Mat& imRight,
}

if (mSensor == CameraType::IMU_STEREO)
for (size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
mpTracker->GrabImuData(vImuMeas[i_imu]);
mpTracker->GrabImuData(vImuMeas);

// std::cout << "start GrabImageStereo" << std::endl;
Sophus::SE3f Tcw = mpTracker->GrabImageStereo(imLeftToFeed, imRightToFeed,
Expand All @@ -335,7 +334,7 @@ Sophus::SE3f System::TrackStereo(const cv::Mat& imLeft, const cv::Mat& imRight,
}

Sophus::SE3f System::TrackRGBD(const cv::Mat& im, const cv::Mat& depthmap,
const double& timestamp,
double timestamp,
const std::vector<IMU::Point>& vImuMeas,
std::string filename) {
if (mSensor != CameraType::RGBD && mSensor != CameraType::IMU_RGBD) {
Expand Down Expand Up @@ -389,8 +388,7 @@ Sophus::SE3f System::TrackRGBD(const cv::Mat& im, const cv::Mat& depthmap,
}

if (mSensor == CameraType::IMU_RGBD)
for (size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
mpTracker->GrabImuData(vImuMeas[i_imu]);
mpTracker->GrabImuData(vImuMeas);

Sophus::SE3f Tcw =
mpTracker->GrabImageRGBD(imToFeed, imDepthToFeed, timestamp, filename, cameras[0]); // for now we know cameras[0] is providing the image
Expand All @@ -402,7 +400,7 @@ Sophus::SE3f System::TrackRGBD(const cv::Mat& im, const cv::Mat& depthmap,
return Tcw;
}

Sophus::SE3f System::TrackMonocular(const cv::Mat& im, const double& timestamp,
Sophus::SE3f System::TrackMonocular(const cv::Mat& im, double timestamp,
const std::vector<IMU::Point>& vImuMeas,
std::string filename) {
// {
Expand Down Expand Up @@ -460,8 +458,7 @@ Sophus::SE3f System::TrackMonocular(const cv::Mat& im, const double& timestamp,
}

if (mSensor == CameraType::IMU_MONOCULAR)
for (size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
mpTracker->GrabImuData(vImuMeas[i_imu]);
mpTracker->GrabImuData(vImuMeas);

Sophus::SE3f Tcw =
mpTracker->GrabImageMonocular(imToFeed, timestamp, filename, cameras[0]); // for now we know cameras[0] is providing the image
Expand Down
39 changes: 18 additions & 21 deletions src/Tracking.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1519,9 +1519,10 @@ Sophus::SE3f Tracking::GrabImageMonocular(const cv::Mat& im,
return mCurrentFrame.GetPose();
}

void Tracking::GrabImuData(const IMU::Point& imuMeasurement) {
void Tracking::GrabImuData(const std::vector<IMU::Point>& imuMeasurements) {
std::unique_lock<std::mutex> lock(mMutexImuQueue);
mlQueueImuData.push_back(imuMeasurement);
for(auto &point : imuMeasurements)
mlQueueImuData.emplace_back(point); // copy ctor
}

void Tracking::PreintegrateIMU() {
Expand All @@ -1540,32 +1541,28 @@ void Tracking::PreintegrateIMU() {
return;
}

while (true) {
std::unique_lock<std::mutex> lock(mMutexImuQueue);
bool bSleep = false;
// std::cout << "mImuPer " << mImuPer << std::endl;
{
if (!mlQueueImuData.empty()) {
IMU::Point* m = &mlQueueImuData.front();
std::cout.precision(17);
if (m->t < mCurrentFrame.mpPrevFrame->mTimeStamp - mImuPer) {
mlQueueImuData.pop_front();
} else if (m->t < mCurrentFrame.mTimeStamp - mImuPer) {
mvImuFromLastFrame.push_back(*m);
mlQueueImuData.pop_front();
} else {
mvImuFromLastFrame.push_back(*m);
std::unique_lock<std::mutex> lock(mMutexImuQueue);
auto itr = mlQueueImuData.begin();
auto lastItr = itr;
for(; itr != mlQueueImuData.end(); ++itr){
IMU::Point &point = *itr;
if(point.t < mCurrentFrame.mpPrevFrame->mTimeStamp - mImuPer) {
// pass
}else if(point.t < mCurrentFrame.mTimeStamp - mImuPer){
mvImuFromLastFrame.emplace_back(point);
}else{
break;
}
} else {
break;
bSleep = true;
lastItr = itr;
}
}
if (bSleep) usleep(500);
if(!mvImuFromLastFrame.empty())
mlQueueImuData.erase(mlQueueImuData.begin(), lastItr);
}

const int n = mvImuFromLastFrame.size() - 1;
if (n == 0) {
if (n <= 0) {
std::cout << "Empty IMU measurements vector!!!\n";
return;
}
Expand Down

0 comments on commit 98c94b9

Please sign in to comment.