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

Odometry: rectify stereo images for convenience if Rtabmap/ImagesAlreadyRectified is false and odometry strategy cannot process raw images

Browse Source
This commit is contained in:
matlabbe
2020-03-02 16:57:14 -05:00
parent 3ea64fbfc3
commit c2dde973a6
4 changed files with 36 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

@@ -115,6 +115,7 @@ private:
std::vector<ParticleFilter *> particleFilters_; std::vector<ParticleFilter *> particleFilters_;
cv::KalmanFilter kalmanFilter_; cv::KalmanFilter kalmanFilter_;
StereoCameraModel stereoModel_;
protected: protected:
Odometry(const rtabmap::ParametersMap & parameters); Odometry(const rtabmap::ParametersMap & parameters);

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

@@ -86,7 +86,7 @@ public:
bool isValidForRectification() const {return left_.isValidForRectification() && right_.isValidForRectification();} bool isValidForRectification() const {return left_.isValidForRectification() && right_.isValidForRectification();}
void initRectificationMap() {left_.initRectificationMap(); right_.initRectificationMap();} void initRectificationMap() {left_.initRectificationMap(); right_.initRectificationMap();}
bool isRectificationMapInitialized() {return left_.isRectificationMapInitialized() && right_.isRectificationMapInitialized();} bool isRectificationMapInitialized() const {return left_.isRectificationMapInitialized() && right_.isRectificationMapInitialized();}
void setName(const std::string & name, const std::string & leftSuffix = "left", const std::string & rightSuffix = "right"); void setName(const std::string & name, const std::string & leftSuffix = "left", const std::string & rightSuffix = "right");
const std::string & name() const {return name_;} const std::string & name() const {return name_;}

37
corelib/src/Odometry.cpp
View File

@@ -283,11 +283,40 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
{ {
UASSERT_MSG(data.id() >= 0, uFormat("Input data should have ID greater or equal than 0 (id=%d)!", data.id()).c_str()); UASSERT_MSG(data.id() >= 0, uFormat("Input data should have ID greater or equal than 0 (id=%d)!", data.id()).c_str());
if(!_imagesAlreadyRectified && !this->canProcessRawImages()) if(!_imagesAlreadyRectified && !this->canProcessRawImages() && !data.imageRaw().empty())
{ {
UERROR("Odometry approach chosen cannot process raw images (not rectified images). Make sure images " if(data.stereoCameraModel().isValidForRectification())
"are rectified, and set %s parameter back to true.", {
Parameters::kRtabmapImagesAlreadyRectified().c_str()); if(!stereoModel_.isRectificationMapInitialized() ||
stereoModel_.left().imageSize() != data.stereoCameraModel().left().imageSize())
{
stereoModel_ = data.stereoCameraModel();
stereoModel_.initRectificationMap();
if(stereoModel_.isRectificationMapInitialized())
{
UWARN("%s parameter is set to false but the selected odometry approach cannot "
"process raw images. We will rectify them for convenience.",
Parameters::kRtabmapImagesAlreadyRectified().c_str());
}
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.",
Parameters::kRtabmapImagesAlreadyRectified().c_str());
}
}
if(stereoModel_.isRectificationMapInitialized())
{
data.setImageRaw(stereoModel_.left().rectifyImage(data.imageRaw()));
data.setDepthOrRightRaw(stereoModel_.right().rectifyImage(data.rightRaw()));
}
}
else
{
UERROR("Odometry approach chosen cannot process raw images (not rectified images). Make sure images "
"are rectified, and set %s parameter back to true.",
Parameters::kRtabmapImagesAlreadyRectified().c_str());
}
} }
// Ground alignment // Ground alignment