i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

CID-SIMS support (#1676)

Browse Source 
* cid-sims dataset support

* Added cli tool to test CID-SIMS dataset

* removed debug log
This commit is contained in:
matlabbe
2026-03-28 14:33:26 -07:00
committed by GitHub
parent 5b985f69be
commit f94a963463
13 changed files with 1115 additions and 301 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
corelib/src/CameraModel.cpp
View File

@@ -353,6 +353,22 @@ bool CameraModel::load(const std::string & filePath)
data[0], data[1], data[2], data[3],
data[4], data[5], data[6], data[7],
data[8], data[9], data[10], data[11]);
Transform detCheck = localTransform_.clone();
localTransform_.normalizeRotation(); /// Normalize by default
float det = detCheck.toEigen3f().linear().determinant();
if(fabs(det - 1.0f) > 0.0001)
{
std::stringstream streamBefore, streamAfter;
streamBefore << detCheck << std::endl;
streamAfter << localTransform_ << std::endl;
UWARN("The camera model's local_transform from \"%s\" doesn't "
"have a normalized rotation matrix (dertminant=%f). We will normalize "
"it for convenience.\nWas:\n%sNow\n%s",
filePath.c_str(),
det,
streamBefore.str().c_str(),
streamAfter.str().c_str());
}
}
else
{

17
corelib/src/IMUThread.cpp
View File

@@ -135,8 +135,20 @@ void IMUThread::mainLoop()
std::stringstream stream(line);
std::string s;
std::getline(stream, s, ',');
std::string nanoseconds = s.substr(s.size() - 9, 9);
std::string seconds = s.substr(0, s.size() - 9);
double stamp = 0.0;
if(s.find('.') != std::string::npos)
{
// Normal [epoch] timestamp
stamp = uStr2Double(s);
}
else
{
// Assume EuRoC format
std::string nanoseconds = s.substr(s.size() - 9, 9);
std::string seconds = s.substr(0, s.size() - 9);
stamp = double(uStr2Int(seconds)) + double(uStr2Int(nanoseconds))*1e-9;
}
cv::Vec3d gyr;
for (int j = 0; j < 3; ++j) {
@@ -150,7 +162,6 @@ void IMUThread::mainLoop()
acc[j] = uStr2Double(s);
}
double stamp = double(uStr2Int(seconds)) + double(uStr2Int(nanoseconds))*1e-9;
if(previousStamp_>0 && stamp > previousStamp_)
{
captureDelay_ = stamp - previousStamp_;

4
corelib/src/OdometryThread.cpp
View File

@@ -188,7 +188,7 @@ void OdometryThread::addData(const SensorEvent & event)
"(%f), skipping that frame (imu buffer size=%ld). "
"When using async IMU, make sure IMU is published faster "
"than camera/lidar (assuming IMU latency is very small compared to camera/lidar)."
"Current camera/lidar delay is %fs.",
"Current camera/lidar delay with system time is %fs.",
event.data().stamp(), _oldestAsyncImuStamp, _imuBuffer.size(), UTimer::now() - event.data().stamp());
notify = false;
}
@@ -197,7 +197,7 @@ void OdometryThread::addData(const SensorEvent & event)
"(%f), skipping that frame (imu buffer size=%ld). "
"When using async IMU, make sure IMU is published faster "
"than camera/lidar (assuming IMU latency is very small compared to camera/lidar). "
"Current camera/lidar delay is %fs.",
"Current camera/lidar delay with system time is %fs.",
event.data().stamp(), _newestAsyncImuStamp, _imuBuffer.size(), UTimer::now() - event.data().stamp());
notify = false;
}

25
corelib/src/Transform.cpp
View File

@@ -52,6 +52,26 @@ Transform::Transform(
r11, r12, r13, o14,
r21, r22, r23, o24,
r31, r32, r33, o34);
if( r11>0.0f || r12>0.0f || r13>0.0f ||
r21>0.0f || r22>0.0f || r23>0.0f ||
r31>0.0f || r32>0.0f || r33>0.0f)
{
Eigen::Matrix3f m;
m << r11, r12, r13,
r21, r22, r23,
r31, r32, r33;
float d = m.determinant();
if(fabs(d-1.0f) > 0.0001)
{
UWARN("Created transform doesn't have normalized rotation. Any transformation with this transform can cause unexpected results!"
" Determinant([%f %f %f;%f %f %f;%f %f %f])=%f",
r11, r12, r13,
r21, r22, r23,
r31, r32, r33,
d);
}
}
}
Transform::Transform(const cv::Mat & transformationMatrix)
@@ -509,6 +529,11 @@ Transform Transform::fromString(const std::string & string)
numbers[4], numbers[5], numbers[6], numbers[7],
numbers[8], numbers[9], numbers[10], numbers[11]);
}
// Always normalize
if(!t.isNull())
{
t.normalizeRotation();
}
return t;
}

81
corelib/src/camera/CameraImages.cpp
View File

@@ -63,6 +63,7 @@ CameraImages::CameraImages() :
_syncImageRateWithStamps(true),
_odometryFormat(0),
_groundTruthFormat(0),
_groundTruthLocalTransform(Transform::getIdentity()),
_maxPoseTimeDiff(0.02),
_captureDelay(0.0)
{}
@@ -93,6 +94,7 @@ CameraImages::CameraImages(const std::string & path,
_syncImageRateWithStamps(true),
_odometryFormat(0),
_groundTruthFormat(0),
_groundTruthLocalTransform(Transform::getIdentity()),
_maxPoseTimeDiff(0.02),
_captureDelay(0.0)
{
@@ -478,27 +480,43 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
if(success && _odometryPath.size() && odometry_.empty())
{
success = readPoses(odometry_, _stamps, _odometryPath, _odometryFormat, _maxPoseTimeDiff);
if(!success)
{
UERROR("Failed to read odometry poses.");
}
if(success)
{
for(size_t i=0; i<odometry_.size(); ++i)
{
// linear cov = 0.0001
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1) * (i==0?9999.0:0.0001);
if(i!=0)
{
// angular cov = 0.000001
covariance.at<double>(3,3) *= 0.01;
covariance.at<double>(4,4) *= 0.01;
covariance.at<double>(5,5) *= 0.01;
}
covariances_.push_back(covariance);
}
}
}
if(success && _groundTruthPath.size())
{
success = readPoses(groundTruth_, _stamps, _groundTruthPath, _groundTruthFormat, _maxPoseTimeDiff);
}
if(!odometry_.empty())
{
for(size_t i=0; i<odometry_.size(); ++i)
if(!success)
{
// linear cov = 0.0001
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1) * (i==0?9999.0:0.0001);
if(i!=0)
UERROR("Failed to read ground truth poses.");
}
else if(!_groundTruthLocalTransform.isIdentity())
{
Transform gtInv = _groundTruthLocalTransform.inverse();
for(auto pose: groundTruth_)
{
// angular cov = 0.000001
covariance.at<double>(3,3) *= 0.01;
covariance.at<double>(4,4) *= 0.01;
covariance.at<double>(5,5) *= 0.01;
pose = pose*gtInv; // pose of base_link, assuming ground truth frame and base frame are rigidly fixed
}
covariances_.push_back(covariance);
}
}
}
@@ -607,7 +625,7 @@ bool CameraImages::readPoses(
}
if(validPoses != (int)inOutStamps.size())
{
UWARN("%d valid poses of %d stamps", validPoses, (int)inOutStamps.size());
UWARN("%d/%ld valid poses of %ld stamps", validPoses, outputPoses.size(), inOutStamps.size());
}
}
else
@@ -756,32 +774,19 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
if(_stamps.size())
{
stamp = _stamps.front();
_stamps.pop_front();
if(_stamps.size())
{
_captureDelay = _stamps.front() - stamp;
}
UERROR("stamps cannot be used when startAt < 0");
}
if(odometry_.size())
{
odometryPose = odometry_.front();
odometry_.pop_front();
if(covariances_.size())
{
covariance = covariances_.front();
covariances_.pop_front();
}
UERROR("odometry cannot be used when startAt < 0");
}
if(groundTruth_.size())
{
groundTruthPose = groundTruth_.front();
groundTruth_.pop_front();
UERROR("groundTruth cannot be used when startAt < 0");
}
if(_models.size() && !model.isValidForProjection())
{
model = _models.front();
_models.pop_front();
UERROR("models cannot be used when startAt < 0");
}
}
else
@@ -792,6 +797,7 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
{
imageFilePath = _path + imageFileName;
scanFilePath = _scanPath + scanFileName;
size_t stampsSize = _stamps.size();
if(_stamps.size())
{
stamp = _stamps.front();
@@ -803,6 +809,8 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
}
if(odometry_.size())
{
UASSERT_MSG(stampsSize==0 || stampsSize == odometry_.size(),
uFormat("Stamps=%ld odometry=%ld", _stamps.size(), odometry_.size()).c_str());
odometryPose = odometry_.front();
odometry_.pop_front();
if(covariances_.size())
@@ -813,11 +821,15 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
}
if(groundTruth_.size())
{
UASSERT_MSG(stampsSize==0 || stampsSize == groundTruth_.size(),
uFormat("Stamps=%ld groundTruth=%ld", _stamps.size(), groundTruth_.size()).c_str());
groundTruthPose = groundTruth_.front();
groundTruth_.pop_front();
}
if(_models.size() && !model.isValidForProjection())
{
UASSERT_MSG(stampsSize==0 || stampsSize == _models.size(),
uFormat("Stamps=%ld models=%ld", _stamps.size(), _models.size()).c_str());
model = _models.front();
_models.pop_front();
}
@@ -834,6 +846,7 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
imageFilePath = _path + imageFileName;
scanFilePath = _scanPath + scanFileName;
size_t stampsSize = _stamps.size();
if(_stamps.size())
{
stamp = _stamps.front();
@@ -845,6 +858,8 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
}
if(odometry_.size())
{
UASSERT_MSG(stampsSize==0 || stampsSize == odometry_.size(),
uFormat("Stamps=%ld odometry=%ld", stampsSize, odometry_.size()).c_str());
odometryPose = odometry_.front();
odometry_.pop_front();
if(covariances_.size())
@@ -855,11 +870,15 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
}
if(groundTruth_.size())
{
UASSERT_MSG(stampsSize==0 || stampsSize == groundTruth_.size(),
uFormat("Stamps=%ld groundTruth=%ld", _stamps.size(), groundTruth_.size()).c_str());
groundTruthPose = groundTruth_.front();
groundTruth_.pop_front();
}
if(_models.size() && !model.isValidForProjection())
{
UASSERT_MSG(stampsSize==0 || stampsSize == _models.size(),
uFormat("Stamps=%ld models=%ld", _stamps.size(), _models.size()).c_str());
model = _models.front();
_models.pop_front();
}