Skip to content

Commit

Permalink
Added Stereo/Gpu Vis/CorFlowGpu parameters (optical flow gpu integrat…
Browse files Browse the repository at this point in the history 
…ion for F2F odom and stereo correspondences)
  • Loading branch information
@matlabbe
matlabbe committed Sep 5, 2024
1 parent 02106f2 commit 6ca8735
Show file tree
Hide file tree
Showing 9 changed files with 470 additions and 189 deletions.
4 changes: 3 additions & 1 deletion corelib/include/rtabmap/core/Parameters.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -287,7 +287,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(SIFT, Sigma, double, 1.6, "The sigma of the Gaussian applied to the input image at the octave #0. If your image is captured with a weak camera with soft lenses, you might want to reduce the number.");
RTABMAP_PARAM(SIFT, PreciseUpscale, bool, false, "Whether to enable precise upscaling in the scale pyramid (OpenCV >= 4.8).");
RTABMAP_PARAM(SIFT, RootSIFT, bool, false, "Apply RootSIFT normalization of the descriptors.");
RTABMAP_PARAM(SIFT, Gpu, bool, false, "CudaSift: Use GPU version of SIFT. This option is enabled only RTAB-Map is built with CudaSift dependency and GPUs are detected.");
RTABMAP_PARAM(SIFT, Gpu, bool, false, "CudaSift: Use GPU version of SIFT. This option is enabled only if RTAB-Map is built with CudaSift dependency and GPUs are detected.");
RTABMAP_PARAM(SIFT, GaussianThreshold, float, 2.0, "CudaSift: Threshold on difference of Gaussians for feature pruning. The higher the threshold, the less features are produced by the detector.");
RTABMAP_PARAM(SIFT, Upscale, bool, false, "CudaSift: Whether to enable upscaling.");

Expand Down Expand Up @@ -718,6 +718,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(Vis, CorFlowIterations, int, 30, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorFlowEps, float, 0.01, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorFlowMaxLevel, int, 3, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorFlowGpu, bool, false, uFormat("[%s=1] Enable GPU version of the optical flow approach (only available if OpenCV is built with CUDA).", kVisCorType().c_str()));
#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
RTABMAP_PARAM(Vis, BundleAdjustment, int, 1, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
#else
Expand Down Expand Up @@ -795,6 +796,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(Stereo, OpticalFlow, bool, true, "Use optical flow to find stereo correspondences, otherwise a simple block matching approach is used.");
RTABMAP_PARAM(Stereo, SSD, bool, true, uFormat("[%s=false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.", kStereoOpticalFlow().c_str()));
RTABMAP_PARAM(Stereo, Eps, double, 0.01, uFormat("[%s=true] Epsilon stop criterion.", kStereoOpticalFlow().c_str()));
RTABMAP_PARAM(Stereo, Gpu, bool, false, uFormat("[%s=true] Enable GPU version of the optical flow approach (only available if OpenCV is built with CUDA).", kStereoOpticalFlow().c_str()));

RTABMAP_PARAM(Stereo, DenseStrategy, int, 0, "0=cv::StereoBM, 1=cv::StereoSGBM");

Expand Down
1 change: 1 addition & 0 deletions corelib/include/rtabmap/core/RegistrationVis.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,7 @@ class RTABMAP_CORE_EXPORT RegistrationVis : public Registration
int _flowIterations;
float _flowEps;
int _flowMaxLevel;
bool _flowGpu;
float _nndr;
int _nnType;
bool _gmsWithRotation;
Expand Down
15 changes: 15 additions & 0 deletions corelib/include/rtabmap/core/SensorData.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -314,6 +314,13 @@ class RTABMAP_CORE_EXPORT SensorData

bool isPointVisibleFromCameras(const cv::Point3f & pt) const; // assuming point is in robot frame

#ifdef HAVE_OPENCV_CUDEV
const cv::cuda::GpuMat & imageRawGpu() const {return _imageRawGpu;}
void setImageRawGpu(const cv::cuda::GpuMat & image) {_imageRawGpu = image;}
const cv::cuda::GpuMat & depthOrRightRawGpu() const {return _depthOrRightRawGpu;}
void setDepthOrRightRawGpu(const cv::cuda::GpuMat & image) {_depthOrRightRawGpu = image;}
#endif

private:
int _id;
double _stamp;
Expand Down Expand Up @@ -365,6 +372,14 @@ class RTABMAP_CORE_EXPORT SensorData
GPS gps_;

IMU imu_;

#ifdef HAVE_OPENCV_CUDEV
// Temporary buffers used for some optimizations,
// particulary to avoid host<->device copies if same
// data are re-used
cv::cuda::GpuMat _imageRawGpu;
cv::cuda::GpuMat _depthOrRightRawGpu;
#endif
};

}
Expand Down
24 changes: 24 additions & 0 deletions corelib/include/rtabmap/core/Stereo.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,13 +49,21 @@ class RTABMAP_CORE_EXPORT Stereo {
const cv::Mat & rightImage,
const std::vector<cv::Point2f> & leftCorners,
std::vector<unsigned char> & status) const;
#ifdef HAVE_OPENCV_CUDEV
virtual std::vector<cv::Point2f> computeCorrespondences(
const cv::cuda::GpuMat & leftImage,
const cv::cuda::GpuMat & rightImage,
const std::vector<cv::Point2f> & leftCorners,
std::vector<unsigned char> & status) const;
#endif

cv::Size winSize() const {return cv::Size(winWidth_, winHeight_);}
int iterations() const {return iterations_;}
int maxLevel() const {return maxLevel_;}
float minDisparity() const {return minDisparity_;}
float maxDisparity() const {return maxDisparity_;}
bool winSSD() const {return winSSD_;}
virtual bool isGpuEnabled() const {return false;}

private:
int winWidth_;
Expand All @@ -78,11 +86,27 @@ class RTABMAP_CORE_EXPORT StereoOpticalFlow : public Stereo {
const cv::Mat & rightImage,
const std::vector<cv::Point2f> & leftCorners,
std::vector<unsigned char> & status) const;

#ifdef HAVE_OPENCV_CUDEV
virtual std::vector<cv::Point2f> computeCorrespondences(
const cv::cuda::GpuMat & leftImage,
const cv::cuda::GpuMat & rightImage,
const std::vector<cv::Point2f> & leftCorners,
std::vector<unsigned char> & status) const;
#endif

float epsilon() const {return epsilon_;}
virtual bool isGpuEnabled() const;

private:
void updateStatus(
const std::vector<cv::Point2f> & leftCorners,
const std::vector<cv::Point2f> & rightCorners,
std::vector<unsigned char> & status) const;

private:
float epsilon_;
bool gpu_;
};

} /* namespace rtabmap */
Expand Down
77 changes: 62 additions & 15 deletions corelib/src/Features2d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -865,15 +865,36 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
data.stereoCameraModels()[0].isValidForProjection())
{
//stereo
cv::Mat imageMono;
// convert to grayscale
if(data.imageRaw().channels() > 1)
cv::Mat imageLeft = data.imageRaw();
cv::Mat imageRight = data.rightRaw();
#ifdef HAVE_OPENCV_CUDEV
cv::cuda::GpuMat d_imageLeft;
cv::cuda::GpuMat d_imageRight;
if(_stereo->isGpuEnabled())
{
cv::cvtColor(data.imageRaw(), imageMono, cv::COLOR_BGR2GRAY);
d_imageLeft = data.imageRawGpu();
if(d_imageLeft.empty()) {
d_imageLeft = cv::cuda::GpuMat(imageLeft);
}
// convert to grayscale
if(d_imageLeft.channels() > 1) {
cv::cuda::GpuMat tmp;
cv::cuda::cvtColor(d_imageLeft, tmp, cv::COLOR_BGR2GRAY);
d_imageLeft = tmp;
}
d_imageRight = data.depthOrRightRawGpu();
if(d_imageRight.empty()) {
d_imageRight = cv::cuda::GpuMat(imageRight);
}
}
else
#endif
{
imageMono = data.imageRaw();
// convert to grayscale (right image should be already grayscale)
if(imageLeft.channels() > 1)
{
cv::cvtColor(data.imageRaw(), imageLeft, cv::COLOR_BGR2GRAY);
}
}

std::vector<cv::Point2f> leftCorners;
Expand All @@ -884,11 +905,24 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
if(data.stereoCameraModels().size() == 1)
{
std::vector<unsigned char> status;
rightCorners = _stereo->computeCorrespondences(
imageMono,
data.rightRaw(),
leftCorners,
status);
#ifdef HAVE_OPENCV_CUDEV
if(_stereo->isGpuEnabled())
{
rightCorners = _stereo->computeCorrespondences(
d_imageLeft,
d_imageRight,
leftCorners,
status);
}
#endif
else
{
rightCorners = _stereo->computeCorrespondences(
imageLeft,
imageRight,
leftCorners,
status);
}

if(ULogger::level() >= ULogger::kWarning)
{
Expand Down Expand Up @@ -923,8 +957,8 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
}
else
{
int subImageWith = imageMono.cols / data.stereoCameraModels().size();
UASSERT(imageMono.cols % subImageWith == 0);
int subImageWith = imageLeft.cols / data.stereoCameraModels().size();
UASSERT(imageLeft.cols % subImageWith == 0);
std::vector<std::vector<cv::Point2f> > subLeftCorners(data.stereoCameraModels().size());
std::vector<std::vector<int> > subIndex(data.stereoCameraModels().size());
// Assign keypoints per camera
Expand All @@ -944,11 +978,24 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
if(!subLeftCorners[i].empty())
{
std::vector<unsigned char> status;
rightCorners = _stereo->computeCorrespondences(
imageMono.colRange(cv::Range(subImageWith*i, subImageWith*(i+1))),
data.rightRaw().colRange(cv::Range(subImageWith*i, subImageWith*(i+1))),
#ifdef HAVE_OPENCV_CUDEV
if(_stereo->isGpuEnabled())
{
rightCorners = _stereo->computeCorrespondences(
d_imageLeft.colRange(cv::Range(subImageWith*i, subImageWith*(i+1))),
d_imageRight.colRange(cv::Range(subImageWith*i, subImageWith*(i+1))),
subLeftCorners[i],
status);
}
#endif
else
{
rightCorners = _stereo->computeCorrespondences(
imageLeft.colRange(cv::Range(subImageWith*i, subImageWith*(i+1))),
imageRight.colRange(cv::Range(subImageWith*i, subImageWith*(i+1))),
subLeftCorners[i],
status);
}

std::vector<cv::Point3f> subKeypoints3D = util3d::generateKeypoints3DStereo(
subLeftCorners[i],
Expand Down
74 changes: 61 additions & 13 deletions corelib/src/RegistrationVis.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,6 +83,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
_flowIterations(Parameters::defaultVisCorFlowIterations()),
_flowEps(Parameters::defaultVisCorFlowEps()),
_flowMaxLevel(Parameters::defaultVisCorFlowMaxLevel()),
_flowGpu(Parameters::defaultVisCorFlowGpu()),
_nndr(Parameters::defaultVisCorNNDR()),
_nnType(Parameters::defaultVisCorNNType()),
_gmsWithRotation(Parameters::defaultGMSWithRotation()),
Expand Down Expand Up @@ -143,6 +144,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), _flowIterations);
Parameters::parse(parameters, Parameters::kVisCorFlowEps(), _flowEps);
Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), _flowMaxLevel);
Parameters::parse(parameters, Parameters::kVisCorFlowGpu(), _flowGpu);
Parameters::parse(parameters, Parameters::kVisCorNNDR(), _nndr);
Parameters::parse(parameters, Parameters::kVisCorNNType(), _nnType);
Parameters::parse(parameters, Parameters::kGMSWithRotation(), _gmsWithRotation);
Expand All @@ -164,6 +166,14 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
UASSERT_MSG(_inlierDistance > 0.0f, uFormat("value=%f", _inlierDistance).c_str());
UASSERT_MSG(_iterations > 0, uFormat("value=%d", _iterations).c_str());

#ifndef HAVE_OPENCV_CUDAOPTFLOW
if(_flowGpu)
{
UERROR("%s is enabled but RTAB-Map is not built with OpenCV CUDA, disabling it.", Parameters::kVisCorFlowGpu().c_str());
_flowGpu = false;
}
#endif

if(_nnType == 6)
{
// verify that we have Python3 support
Expand Down Expand Up @@ -471,18 +481,59 @@ Transform RegistrationVis::computeTransformationImpl(
if(_correspondencesApproach == 1) //Optical Flow
{
UDEBUG("");
// convert to grayscale
if(imageFrom.channels() > 1)
#ifdef HAVE_OPENCV_CUDAOPTFLOW
cv::cuda::GpuMat d_imageFrom;
cv::cuda::GpuMat d_imageTo;
if (_flowGpu)
{
cv::Mat tmp;
cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
imageFrom = tmp;
UDEBUG("GPU optical flow: preparing GPU image data...");
d_imageFrom = fromSignature.sensorData().imageRawGpu();
if(d_imageFrom.empty() && !imageFrom.empty()) {
d_imageFrom = cv::cuda::GpuMat(imageFrom);
}
// convert to grayscale
if(d_imageFrom.channels() > 1) {
cv::cuda::GpuMat tmp;
cv::cuda::cvtColor(d_imageFrom, tmp, cv::COLOR_BGR2GRAY);
d_imageFrom = tmp;
}
if(fromSignature.sensorData().imageRawGpu().empty())
{
fromSignature.sensorData().setImageRawGpu(d_imageFrom); // buffer it
}

d_imageTo = toSignature.sensorData().imageRawGpu();
if(d_imageTo.empty() && !imageTo.empty()) {
d_imageTo = cv::cuda::GpuMat(imageTo);
}
// convert to grayscale
if(d_imageTo.channels() > 1) {
cv::cuda::GpuMat tmp;
cv::cuda::cvtColor(d_imageTo, tmp, cv::COLOR_BGR2GRAY);
d_imageTo = tmp;
}
if(toSignature.sensorData().imageRawGpu().empty())
{
toSignature.sensorData().setImageRawGpu(d_imageTo); // buffer it
}
UDEBUG("GPU optical flow: preparing GPU image data... done!");
}
if(imageTo.channels() > 1)
else
#endif
{
cv::Mat tmp;
cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
imageTo = tmp;
// convert to grayscale
if(imageFrom.channels() > 1)
{
cv::Mat tmp;
cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
imageFrom = tmp;
}
if(imageTo.channels() > 1)
{
cv::Mat tmp;
cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
imageTo = tmp;
}
}

std::vector<cv::Point3f> kptsFrom3D;
Expand Down Expand Up @@ -573,12 +624,9 @@ Transform RegistrationVis::computeTransformationImpl(
UDEBUG("guessSet = %d", guessSet?1:0);
std::vector<unsigned char> status;
#ifdef HAVE_OPENCV_CUDAOPTFLOW
bool gpu = false;
if (gpu)
if (_flowGpu)
{
UDEBUG("cv::cuda::SparsePyrLKOpticalFlow transfer host to device begin");
cv::cuda::GpuMat d_imageFrom(imageFrom);
cv::cuda::GpuMat d_imageTo(imageTo);
cv::cuda::GpuMat d_cornersFrom(cornersFrom);
cv::cuda::GpuMat d_cornersTo(cornersTo);
UDEBUG("cv::cuda::SparsePyrLKOpticalFlow transfer host to device end");
Expand Down
Loading

0 comments on commit 6ca8735

Please sign in to comment.