i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

Odometry: when "Rtabmap/ImagesAlreadyRectified" is false and odometry strategy doesn't support raw images, added rectification for RGB-D data (only rgb rectified though) for convenience.

Browse Source
This commit is contained in:
matlabbe
2021-06-01 14:33:17 -04:00
parent 12c46286f6
commit 630cb6a59f
4 changed files with 73 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/CameraModel.h
View File

@@ -81,7 +81,7 @@ public:
virtual ~CameraModel() {}
void initRectificationMap();
bool initRectificationMap();
bool isRectificationMapInitialized() const {return !mapX_.empty() && !mapY_.empty();}
bool isValidForProjection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0;}

1
corelib/include/rtabmap/core/Odometry.h
View File

@@ -121,6 +121,7 @@ private:
std::vector<ParticleFilter *> particleFilters_;
cv::KalmanFilter kalmanFilter_;
StereoCameraModel stereoModel_;
std::vector<CameraModel> models_;
std::map<double, Transform> imus_;
protected:

3
corelib/src/CameraModel.cpp
View File

@@ -152,7 +152,7 @@ CameraModel::CameraModel(
K_.at<double>(1,2) = cy;
}
void CameraModel::initRectificationMap()
bool CameraModel::initRectificationMap()
{
UASSERT(imageSize_.height > 0 && imageSize_.width > 0);
UASSERT(D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 6 || D_.cols == 8));
@@ -182,6 +182,7 @@ void CameraModel::initRectificationMap()
// RadialTangential
cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
}
return isRectificationMapInitialized();
}
void CameraModel::setImageSize(const cv::Size & size)

72
corelib/src/Odometry.cpp
View File

@@ -338,8 +338,10 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
else
{
UERROR("Odometry approach chosen cannot process raw images (not rectified images) and we cannot rectify them. "
"Make sure images are rectified, and set %s parameter back to true.",
UERROR("Odometry approach chosen cannot process raw images (not rectified images) "
"and we cannot rectify them as the rectification map failed to initialize (valid calibration?). "
"Make sure images are rectified and set %s parameter back to true, or make sure "
"calibration is valid for rectification.",
Parameters::kRtabmapImagesAlreadyRectified().c_str());
}
}
@@ -352,10 +354,74 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
false);
}
}
else if(!data.cameraModels().empty())
{
bool valid = true;
if(data.cameraModels().size() != models_.size())
{
models_.clear();
valid = false;
}
else
{
for(size_t i=0; i<data.cameraModels().size() && valid; ++i)
{
valid = models_[i].isRectificationMapInitialized() &&
models_[i].imageSize() == data.cameraModels()[i].imageSize();
}
}
if(!valid)
{
models_ = data.cameraModels();
valid = true;
for(size_t i=0; i<models_.size() && valid; ++i)
{
valid = models_[i].initRectificationMap();
}
if(valid)
{
UWARN("%s parameter is set to false but the selected odometry approach cannot "
"process raw images. We will rectify them for convenience (only "
"rgb is rectified, we assume depth image is already rectified!).",
Parameters::kRtabmapImagesAlreadyRectified().c_str());
}
else
{
UERROR("Odometry approach chosen cannot process raw images (not rectified images) "
"and we cannot rectify them as the rectification map failed to initialize (valid calibration?). "
"Make sure images are rectified and set %s parameter back to true, or "
"make sure calibration is valid for rectification",
Parameters::kRtabmapImagesAlreadyRectified().c_str());
models_.clear();
}
}
if(valid)
{
// Note that only RGB image is rectified, the depth image is assumed to be already registered to rectified RGB camera.
if(models_.size()==1)
{
data.setRGBDImage(models_[0].rectifyImage(data.imageRaw()), data.depthRaw(), models_, false);
}
else
{
UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
cv::Mat rectifiedImages = data.imageRaw().clone();
for(size_t i=0; i<models_.size() && valid; ++i)
{
cv::Mat rectifiedImage = models_[i].rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
rectifiedImage.copyTo(cv::Mat(rectifiedImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
}
data.setRGBDImage(rectifiedImages, data.depthRaw(), models_, false);
}
}
}
else
{
UERROR("Odometry approach chosen cannot process raw images (not rectified images). Make sure images "
"are rectified, and set %s parameter back to true.",
"are rectified, and set %s parameter back to true, or make sure that calibration is valid "
"for rectification so we can rectifiy them for convenience",
Parameters::kRtabmapImagesAlreadyRectified().c_str());
}
}