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

Added parameter "Rtabmap/RectifyOnlyFeatures". Added support of fisheye distortion model.

Browse Source
This commit is contained in:
matlabbe
2018-09-24 14:36:54 -04:00
parent f903ffb927
commit c341648a44
14 changed files with 425 additions and 146 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -75,6 +75,7 @@ public:
virtual ~CameraModel() {}
void initRectificationMap();
bool isRectificationMapInitialized() {return !mapX_.empty() && !mapY_.empty();}
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;}

3
corelib/include/rtabmap/core/Memory.h
View File

@@ -302,6 +302,7 @@ private:
int _visMaxFeatures;
int _visCorType;
bool _imagesAlreadyRectified;
bool _rectifyOnlyFeatures;
bool _covOffDiagonalIgnored;
int _idCount;
@@ -312,6 +313,8 @@ private:
bool _linksChanged; // False by default, become true when links are modified.
int _signaturesAdded;
GPS _gpsOrigin;
std::vector<CameraModel> _rectCameraModels;
StereoCameraModel _rectStereoCameraModel;
std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
std::set<int> _stMem; // id

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

@@ -188,6 +188,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Rtabmap, StartNewMapOnLoopClosure, bool, false, "Start a new map only if there is a global loop closure with a previous map.");
RTABMAP_PARAM(Rtabmap, StartNewMapOnGoodSignature, bool, false, uFormat("Start a new map only if the first signature is not bad (i.e., has enough features, see %s).", kKpBadSignRatio().c_str()));
RTABMAP_PARAM(Rtabmap, ImagesAlreadyRectified, bool, true, "Images are already rectified. By default RTAB-Map assumes that received images are rectified. If they are not, they can be rectified by RTAB-Map if this parameter is false.");
RTABMAP_PARAM(Rtabmap, RectifyOnlyFeatures, bool, false, uFormat("If \"%s\" is false and this parameter is true, the whole RGB image will not be rectified, only the features. Warning: As projection of RGB-D image to point cloud is assuming that images are rectified, the generated point cloud map will have wrong colors if this parameter is true.", kRtabmapImagesAlreadyRectified().c_str()));
// Hypotheses selection
RTABMAP_PARAM(Rtabmap, LoopThr, float, 0.11, "Loop closing threshold.");

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

@@ -137,6 +137,7 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(TimingMem, Descriptors_extraction, ms);
RTABMAP_STATS(TimingMem, Rectification, ms);
RTABMAP_STATS(TimingMem, Keypoints_3D, ms);
RTABMAP_STATS(TimingMem, Keypoints_3D_motion, ms);
RTABMAP_STATS(TimingMem, Joining_dictionary_update, ms);
RTABMAP_STATS(TimingMem, Add_new_words, ms);
RTABMAP_STATS(TimingMem, Compressing_data, ms);

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

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

62
corelib/src/CameraModel.cpp
View File

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UStl.h>
#include <opencv2/imgproc/imgproc.hpp>
namespace rtabmap {
@@ -57,7 +58,7 @@ CameraModel::CameraModel(
localTransform_(localTransform)
{
UASSERT(K_.empty() || (K_.rows == 3 && K_.cols == 3 && K_.type() == CV_64FC1));
UASSERT(D_.empty() || (D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 8) && D_.type() == CV_64FC1));
UASSERT(D_.empty() || (D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 6 || D_.cols == 8) && D_.type() == CV_64FC1));
UASSERT(R_.empty() || (R_.rows == 3 && R_.cols == 3 && R_.type() == CV_64FC1));
UASSERT(P_.empty() || (P_.rows == 3 && P_.cols == 4 && P_.type() == CV_64FC1));
}
@@ -153,12 +154,27 @@ CameraModel::CameraModel(
void CameraModel::initRectificationMap()
{
UASSERT(imageSize_.height > 0 && imageSize_.width > 0);
UASSERT(D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 8));
UASSERT(D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 6 || D_.cols == 8));
UASSERT(R_.rows == 3 && R_.cols == 3);
UASSERT(P_.rows == 3 && P_.cols == 4);
// init rectification map
UINFO("Initialize rectify map");
cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
if(D_.cols == 6)
{
// Equidistant / FishEye
// get only k parameters (k1,k2,p1,p2,k3,k4)
cv::Mat D(1, 4, CV_64FC1);
D.at<double>(0,0) = D_.at<double>(0,1);
D.at<double>(0,1) = D_.at<double>(0,1);
D.at<double>(0,2) = D_.at<double>(0,4);
D.at<double>(0,3) = D_.at<double>(0,5);
cv::fisheye::initUndistortRectifyMap(K_, D, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
}
else
{
// RadialTangential
cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
}
}
void CameraModel::setImageSize(const cv::Size & size)
@@ -256,7 +272,20 @@ bool CameraModel::load(const std::string & directory, const std::string & camera
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 1 && (cols == 4 || cols == 5 || cols == 8));
D_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
std::string distortionModel = (std::string)n["distortion_model"];
if(uStrContains(distortionModel, "fisheye") ||
uStrContains(distortionModel, "equidistant"))
{
D_ = cv::Mat::zeros(1,6,CV_64FC1);
D_.at<double>(0,0) = data[0];
D_.at<double>(0,1) = data[1];
D_.at<double>(0,4) = data[2];
D_.at<double>(0,5) = data[3];
}
else
{
D_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
}
}
else
{
@@ -347,20 +376,33 @@ bool CameraModel::save(const std::string & directory) const
if(!D_.empty())
{
cv::Mat D = D_;
if(D_.cols == 6)
{
D = cv::Mat(1,4,CV_64FC1);
D.at<double>(0,0) = D_.at<double>(0,0);
D.at<double>(0,1) = D_.at<double>(0,1);
D.at<double>(0,2) = D_.at<double>(0,4);
D.at<double>(0,3) = D_.at<double>(0,5);
}
fs << "distortion_coefficients" << "{";
fs << "rows" << D_.rows;
fs << "cols" << D_.cols;
fs << "data" << std::vector<double>((double*)D_.data, ((double*)D_.data)+(D_.rows*D_.cols));
fs << "rows" << D.rows;
fs << "cols" << D.cols;
fs << "data" << std::vector<double>((double*)D.data, ((double*)D.data)+(D.rows*D.cols));
fs << "}";
// compaibility with ROS
if(D_.cols > 5)
if(D_.cols == 6)
{
fs << "distortion_model" << "rational_polynomial";
fs << "distortion_model" << "fisheye"; // equidistant
}
else if(D.cols > 5)
{
fs << "distortion_model" << "rational_polynomial"; // rad tan
}
else
{
fs << "distortion_model" << "plumb_bob";
fs << "distortion_model" << "plumb_bob"; // rad tan
}
}

164
corelib/src/Memory.cpp
View File

@@ -104,6 +104,7 @@ Memory::Memory(const ParametersMap & parameters) :
_visMaxFeatures(Parameters::defaultVisMaxFeatures()),
_visCorType(Parameters::defaultVisCorType()),
_imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
_rectifyOnlyFeatures(Parameters::defaultRtabmapRectifyOnlyFeatures()),
_covOffDiagonalIgnored(Parameters::defaultMemCovOffDiagIgnored()),
_idCount(kIdStart),
_idMapCount(kIdStart),
@@ -486,6 +487,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
uInsert(params, ParametersPair(Parameters::kVisCorType(), "0"));
}
Parameters::parse(params, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
Parameters::parse(params, Parameters::kRtabmapRectifyOnlyFeatures(), _rectifyOnlyFeatures);
Parameters::parse(params, Parameters::kMemCovOffDiagIgnored(), _covOffDiagonalIgnored);
@@ -1506,6 +1508,8 @@ void Memory::clear()
_memoryChanged = false;
_linksChanged = false;
_gpsOrigin = GPS();
_rectCameraModels.clear();
_rectStereoCameraModel = StereoCameraModel();
if(_dbDriver)
{
@@ -3685,25 +3689,41 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
}
}
if(!_imagesAlreadyRectified && !data.imageRaw().empty())
bool imagesRectified = _imagesAlreadyRectified;
// Stereo must be always rectified because of the stereo correspondence approach
if(!imagesRectified && !data.imageRaw().empty() && !(_rectifyOnlyFeatures && data.rightRaw().empty()))
{
if(!data.depthRaw().empty())
{
UERROR("RGB-D images should be already rectified! Make sure they are and set %s parameter back to true.",
Parameters::kRtabmapImagesAlreadyRectified().c_str());
return 0;
}
// we assume that once rtabmap is receiving data, the calibration won't change over time
if(data.cameraModels().size())
{
// Note that only RGB image is rectified, the depth image is assumed to be already registered to rectified RGB camera.
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().size(), data.imageRaw().type());
bool initRectMaps = _rectCameraModels.empty();
if(initRectMaps)
{
_rectCameraModels.resize(data.cameraModels().size());
}
for(unsigned int i=0; i<data.cameraModels().size(); ++i)
{
if(data.cameraModels()[i].isValidForRectification())
{
cv::Mat rectifiedImage = data.cameraModels()[i].rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
if(initRectMaps)
{
_rectCameraModels[i] = data.cameraModels()[i];
if(!_rectCameraModels[i].isRectificationMapInitialized())
{
UWARN("Initializing rectification maps for camera %d (only done for the first image received)...", i);
_rectCameraModels[i].initRectificationMap();
UWARN("Initializing rectification maps for camera %d (only done for the first image received)... done!", i);
}
}
UASSERT(_rectCameraModels[i].imageWidth() == data.cameraModels()[i].imageWidth() &&
_rectCameraModels[i].imageHeight() == data.cameraModels()[i].imageHeight());
cv::Mat rectifiedImage = _rectCameraModels[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)));
imagesRectified = true;
}
else
{
@@ -3718,8 +3738,21 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
}
else if(data.stereoCameraModel().isValidForRectification())
{
data.setImageRaw(data.stereoCameraModel().left().rectifyImage(data.imageRaw()));
data.setDepthOrRightRaw(data.stereoCameraModel().right().rectifyImage(data.rightRaw()));
if(!_rectStereoCameraModel.isValidForRectification())
{
_rectStereoCameraModel = data.stereoCameraModel();
if(!_rectStereoCameraModel.isRectificationMapInitialized())
{
UWARN("Initializing rectification maps (only done for the first image received)...");
_rectStereoCameraModel.initRectificationMap();
UWARN("Initializing rectification maps (only done for the first image received)...done!");
}
}
UASSERT(_rectStereoCameraModel.left().imageWidth() == data.stereoCameraModel().left().imageWidth());
UASSERT(_rectStereoCameraModel.left().imageHeight() == data.stereoCameraModel().left().imageHeight());
data.setImageRaw(_rectStereoCameraModel.left().rectifyImage(data.imageRaw()));
data.setDepthOrRightRaw(_rectStereoCameraModel.right().rectifyImage(data.rightRaw()));
imagesRectified = true;
}
else
{
@@ -3836,6 +3869,111 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
else if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
(!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModel().isValidForProjection()))
{
if(!imagesRectified && decimatedData.cameraModels().size())
{
std::vector<cv::KeyPoint> keypointsValid;
keypointsValid.reserve(keypoints.size());
cv::Mat descriptorsValid;
descriptorsValid.reserve(descriptors.rows);
//undistort keypoints before projection (RGB-D)
if(decimatedData.cameraModels().size() == 1)
{
std::vector<cv::Point2f> pointsIn, pointsOut;
cv::KeyPoint::convert(keypoints,pointsIn);
if(decimatedData.cameraModels()[0].D_raw().cols == 6)
{
// Equidistant / FishEye
// get only k parameters (k1,k2,p1,p2,k3,k4)
cv::Mat D(1, 4, CV_64FC1);
D.at<double>(0,0) = decimatedData.cameraModels()[0].D_raw().at<double>(0,1);
D.at<double>(0,1) = decimatedData.cameraModels()[0].D_raw().at<double>(0,1);
D.at<double>(0,2) = decimatedData.cameraModels()[0].D_raw().at<double>(0,4);
D.at<double>(0,3) = decimatedData.cameraModels()[0].D_raw().at<double>(0,5);
cv::fisheye::undistortPoints(pointsIn, pointsOut,
decimatedData.cameraModels()[0].K_raw(),
D,
decimatedData.cameraModels()[0].R(),
decimatedData.cameraModels()[0].P());
}
else
{
//RadialTangential
cv::undistortPoints(pointsIn, pointsOut,
decimatedData.cameraModels()[0].K_raw(),
decimatedData.cameraModels()[0].D_raw(),
decimatedData.cameraModels()[0].R(),
decimatedData.cameraModels()[0].P());
}
UASSERT(pointsOut.size() == keypoints.size());
for(unsigned int i=0; i<pointsOut.size(); ++i)
{
if(pointsOut.at(i).x>=0 && pointsOut.at(i).x<decimatedData.cameraModels()[0].imageWidth() &&
pointsOut.at(i).y>=0 && pointsOut.at(i).y<decimatedData.cameraModels()[0].imageHeight())
{
keypointsValid.push_back(keypoints.at(i));
keypointsValid.back().pt.x = pointsOut.at(i).x;
keypointsValid.back().pt.y = pointsOut.at(i).y;
descriptorsValid.push_back(descriptors.row(i));
}
}
}
else
{
UASSERT(int((decimatedData.imageRaw().cols/decimatedData.cameraModels().size())*decimatedData.cameraModels().size()) == decimatedData.imageRaw().cols);
float subImageWidth = decimatedData.imageRaw().cols/decimatedData.cameraModels().size();
for(unsigned int i=0; i<keypoints.size(); ++i)
{
int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)decimatedData.cameraModels().size(),
uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
cameraIndex, (int)decimatedData.cameraModels().size(), keypoints[i].pt.x, subImageWidth, decimatedData.cameraModels()[0].imageWidth()).c_str());
std::vector<cv::Point2f> pointsIn, pointsOut;
pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y));
if(decimatedData.cameraModels()[cameraIndex].D_raw().cols == 6)
{
// Equidistant / FishEye
// get only k parameters (k1,k2,p1,p2,k3,k4)
cv::Mat D(1, 4, CV_64FC1);
D.at<double>(0,0) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
D.at<double>(0,1) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
D.at<double>(0,2) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,4);
D.at<double>(0,3) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,5);
cv::fisheye::undistortPoints(pointsIn, pointsOut,
decimatedData.cameraModels()[cameraIndex].K_raw(),
D,
decimatedData.cameraModels()[cameraIndex].R(),
decimatedData.cameraModels()[cameraIndex].P());
}
else
{
//RadialTangential
cv::undistortPoints(pointsIn, pointsOut,
decimatedData.cameraModels()[cameraIndex].K_raw(),
decimatedData.cameraModels()[cameraIndex].D_raw(),
decimatedData.cameraModels()[cameraIndex].R(),
decimatedData.cameraModels()[cameraIndex].P());
}
if(pointsOut[0].x>=0 && pointsOut[0].x<decimatedData.cameraModels()[cameraIndex].imageWidth() &&
pointsOut[0].y>=0 && pointsOut[0].y<decimatedData.cameraModels()[cameraIndex].imageHeight())
{
keypointsValid.push_back(keypoints.at(i));
keypointsValid.back().pt.x = pointsOut[0].x + subImageWidth*cameraIndex;
keypointsValid.back().pt.y = pointsOut[0].y;
descriptorsValid.push_back(descriptors.row(i));
}
}
}
keypoints = keypointsValid;
descriptors = descriptorsValid;
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f);
UDEBUG("time rectification = %fs", t);
}
keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
@@ -3891,6 +4029,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
imageMono = data.imageRaw();
}
UASSERT_MSG(imagesRectified, "Cannot extract descriptors on not rectified image from keypoints which assumed to be undistorted");
descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
}
t = timer.ticks();
@@ -4177,8 +4317,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
t = timer.ticks();
UASSERT(words3D.size() == words.size());
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)words3D.size(), t);
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D_motion(), t*1000.0f);
UDEBUG("time keypoints 3D by motion (%d) = %fs", (int)words3D.size(), t);
}
}