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

Added Stereo and StereoDense base classes (with Stereo->StereoOpticalFlow and StereoDense->StereoBM) to handle easily stereo parameters. Added stereoEval tool to test Stereo/OpticalFlow=false or true. Added StereoBM parameters. Refactoring of the Preferences dialog (tree view order and some titles)

Browse Source
This commit is contained in:
matlabbe
2015-12-11 17:44:15 -05:00
parent 818eda005f
commit 9cc94ce1e6
44 changed files with 6866 additions and 4311 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -125,89 +125,5 @@ private:
Transform localTransform_;
};
class RTABMAP_EXP StereoCameraModel
{
public:
StereoCameraModel() {}
StereoCameraModel(
const std::string & name,
const cv::Size & imageSize1,
const cv::Mat & K1, const cv::Mat & D1, const cv::Mat & R1, const cv::Mat & P1,
const cv::Size & imageSize2,
const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2,
const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F,
const Transform & localTransform = Transform::getIdentity()) :
left_(name+"_left", imageSize1, K1, D1, R1, P1, localTransform),
right_(name+"_right", imageSize2, K2, D2, R2, P2, localTransform),
name_(name),
R_(R),
T_(T),
E_(E),
F_(F)
{
}
//minimal
StereoCameraModel(
double fx,
double fy,
double cx,
double cy,
double baseline,
const Transform & localTransform = Transform::getIdentity()) :
left_(fx, fy, cx, cy, localTransform),
right_(fx, fy, cx, cy, localTransform, baseline*-fx)
{
}
//minimal to be saved
StereoCameraModel(
const std::string & name,
double fx,
double fy,
double cx,
double cy,
double baseline,
const Transform & localTransform = Transform::getIdentity()) :
left_(name+"_left", fx, fy, cx, cy, localTransform),
right_(name+"_right", fx, fy, cx, cy, localTransform, baseline*-fx),
name_(name)
{
}
virtual ~StereoCameraModel() {}
bool isValid() const {return left_.isValid() && right_.isValid() && baseline() > 0.0;}
void setName(const std::string & name);
const std::string & name() const {return name_;}
bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
bool save(const std::string & directory, bool ignoreStereoTransform = true) const;
double baseline() const {return -right_.Tx()/right_.fx();}
const cv::Mat & R() const {return R_;} //extrinsic rotation matrix
const cv::Mat & T() const {return T_;} //extrinsic translation matrix
const cv::Mat & E() const {return E_;} //extrinsic essential matrix
const cv::Mat & F() const {return F_;} //extrinsic fundamental matrix
void scale(double scale);
void setLocalTransform(const Transform & transform) {left_.setLocalTransform(transform);}
const Transform & localTransform() const {return left_.localTransform();}
Transform stereoTransform() const;
const CameraModel & left() const {return left_;}
const CameraModel & right() const {return right_;}
private:
CameraModel left_;
CameraModel right_;
std::string name_;
cv::Mat R_;
cv::Mat T_;
cv::Mat E_;
cv::Mat F_;
};
} /* namespace rtabmap */
#endif /* CAMERAMODEL_H_ */

5
corelib/include/rtabmap/core/CameraThread.h
View File

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <rtabmap/core/Parameters.h>
#include <rtabmap/utilite/UThread.h>
#include <rtabmap/utilite/UEventsSender.h>
@@ -36,6 +37,7 @@ namespace rtabmap
{
class Camera;
class StereoDense;
/**
* Class CameraThread
@@ -47,7 +49,7 @@ class RTABMAP_EXP CameraThread :
{
public:
// ownership transferred
CameraThread(Camera * camera);
CameraThread(Camera * camera, const ParametersMap & parameters = ParametersMap());
virtual ~CameraThread();
void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
@@ -70,6 +72,7 @@ private:
bool _mirroring;
bool _colorOnly;
bool _stereoToDepth;
StereoDense * _stereoDense;
};
} // namespace rtabmap

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

@@ -102,7 +102,7 @@ public:
kFeatureGfttOrb=8, //new 0.10.11
kFeatureFastOrb=9}; //new 0.11.0
static Feature2D * create(Feature2D::Type & type, const ParametersMap & parameters);
static Feature2D * create(Feature2D::Type type, const ParametersMap & parameters);
static void filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,

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

@@ -54,6 +54,7 @@ class Feature2D;
class Statistics;
class RegistrationVis;
class RegistrationIcp;
class Stereo;
class RTABMAP_EXP Memory
{
@@ -273,11 +274,7 @@ private:
RegistrationIcp * _registrationIcp;
// Stereo stuff
int _stereoFlowWinSize;
int _stereoFlowIterations;
double _stereoFlowEpsilon;
int _stereoFlowMaxLevel;
float _stereoMaxSlope;
Stereo * _stereo;
int _subPixWinSize;
int _subPixIterations;

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

@@ -43,6 +43,7 @@ namespace rtabmap {
class Feature2D;
class OdometryInfo;
class ParticleFilter;
class Stereo;
class RTABMAP_EXP Odometry
{
@@ -143,16 +144,14 @@ private:
private:
//Parameters:
int keyFrameThr_;
int flowWinSize_;
int flowIterations_;
double flowEps_;
int flowMaxLevel_;
bool flowGuessFromMotion_;
int stereoWinSize_;
int stereoIterations_;
double stereoEps_;
int stereoMaxLevel_;
float stereoMaxSlope_;
Stereo * stereo_;
int subPixWinSize_;
int subPixIterations_;
@@ -163,6 +162,8 @@ private:
cv::Mat refFrame_;
std::vector<cv::Point2f> refCorners_;
pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3D_;
Transform motionSinceLastKeyFrame_;
};
class RTABMAP_EXP OdometryMono : public Odometry
@@ -181,11 +182,7 @@ private:
double flowEps_;
int flowMaxLevel_;
int stereoWinSize_;
int stereoIterations_;
double stereoEps_;
int stereoMaxLevel_;
float stereoMaxSlope_;
Stereo * stereo_;
Memory * memory_;
int localHistoryMaxSize_;

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

@@ -347,11 +347,13 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(OdomMono, MinTranslation, float, 0.02, "Minimum translation to add new points to local map. On initialization, translation x 5 is used as the minimum.");
RTABMAP_PARAM(OdomMono, MaxVariance, float, 0.01, "Maximum variance to add new points to local map.");
// Odometry common stuff between BOW and Optical Flow approaches
// Odometry Optical Flow
RTABMAP_PARAM(OdomFlow, KeyFrameThr, int, 0, "Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.");
RTABMAP_PARAM(OdomFlow, WinSize, int, 16, "Used for optical flow approach. See cv::calcOpticalFlowPyrLK().");
RTABMAP_PARAM(OdomFlow, Iterations, int, 30, "Used for optical flow approach. See cv::calcOpticalFlowPyrLK().");
RTABMAP_PARAM(OdomFlow, Eps, double, 0.01, "Used for optical flow approach. See cv::calcOpticalFlowPyrLK().");
RTABMAP_PARAM(OdomFlow, MaxLevel, int, 3, "Used for optical flow approach. See cv::calcOpticalFlowPyrLK().");
RTABMAP_PARAM(OdomFlow, GuessMotion, bool, true, "Guess optical flow from the last motion computed.");
// Common registration parameters
RTABMAP_PARAM(Reg, VarianceFromInliersCount, bool, false, "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
@@ -391,11 +393,26 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Icp, PointToPlaneNormalNeighbors, int, 20, "Number of neighbors to compute normals for point to plane.");
// Stereo disparity
RTABMAP_PARAM(Stereo, WinSize, int, 16, "See cv::calcOpticalFlowPyrLK().");
RTABMAP_PARAM(Stereo, Iterations, int, 30, "See cv::calcOpticalFlowPyrLK().");
RTABMAP_PARAM(Stereo, Eps, double, 0.01, "See cv::calcOpticalFlowPyrLK().");
RTABMAP_PARAM(Stereo, MaxLevel, int, 3, "See cv::calcOpticalFlowPyrLK().");
RTABMAP_PARAM(Stereo, MaxSlope, float, 0.1, "The maximum slope for each stereo pairs.");
RTABMAP_PARAM(Stereo, WinWidth, int, 15, "Window width.");
RTABMAP_PARAM(Stereo, WinHeight, int, 3, "Window height.");
RTABMAP_PARAM(Stereo, Iterations, int, 30, "Maximum iterations.");
RTABMAP_PARAM(Stereo, MaxLevel, int, 3, "Maximum pyramid level.");
RTABMAP_PARAM(Stereo, MinDisparity, int, 0, "Minimum disparity.");
RTABMAP_PARAM(Stereo, MaxDisparity, int, 64, "Maximum disparity.");
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, "[Stereo/OpticalFlow = false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.");
RTABMAP_PARAM(Stereo, Eps, double, 0.01, "[Stereo/OpticalFlow = true] Epsilon stop criterion.");
RTABMAP_PARAM(Stereo, MaxSlope, float, 0.1, "[Stereo/OpticalFlow = true] The maximum slope for each stereo pairs.");
RTABMAP_PARAM(StereoBM, BlockSize, int, 15, "See cv::StereoBM");
RTABMAP_PARAM(StereoBM, MinDisparity, int, 0, "See cv::StereoBM");
RTABMAP_PARAM(StereoBM, NumDisparities, int, 64, "See cv::StereoBM");
RTABMAP_PARAM(StereoBM, PreFilterSize, int, 9, "See cv::StereoBM");
RTABMAP_PARAM(StereoBM, PreFilterCap, int, 31, "See cv::StereoBM");
RTABMAP_PARAM(StereoBM, UniquenessRatio, int, 15, "See cv::StereoBM");
RTABMAP_PARAM(StereoBM, TextureThreshold, int, 10, "See cv::StereoBM");
RTABMAP_PARAM(StereoBM, SpeckleWindowSize, int, 100, "See cv::StereoBM");
RTABMAP_PARAM(StereoBM, SpeckleRange, int, 4, "See cv::StereoBM");
public:
virtual ~Parameters();
@@ -421,12 +438,14 @@ public:
static void parse(const ParametersMap & parameters, const std::string & key, float & value);
static void parse(const ParametersMap & parameters, const std::string & key, double & value);
static void parse(const ParametersMap & parameters, const std::string & key, std::string & value);
static void parse(const ParametersMap & parameters, ParametersMap & parametersOut);
static std::string getVersion();
static std::string getDefaultDatabaseName();
static bool isFeatureParameter(const std::string & param);
static ParametersMap getDefaultOdometryParameters(bool stereo = false);
static ParametersMap getDefaultParameters(const std::string & group);
static void readINI(const std::string & configFile, ParametersMap & parameters);
static void writeINI(const std::string & configFile, const ParametersMap & parameters);

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

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/RtabmapExp.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/CameraModel.h>
#include <rtabmap/core/StereoCameraModel.h>
#include <rtabmap/core/Transform.h>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>

89
corelib/include/rtabmap/core/Stereo.h Normal file
View File

@@ -0,0 +1,89 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef STEREO_H_
#define STEREO_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <rtabmap/core/Parameters.h>
#include <opencv2/core/core.hpp>
namespace rtabmap {
class RTABMAP_EXP Stereo {
public:
Stereo(const ParametersMap & parameters = ParametersMap());
virtual ~Stereo() {}
virtual void parseParameters(const ParametersMap & parameters);
virtual std::vector<cv::Point2f> computeCorrespondences(
const cv::Mat & leftImage,
const cv::Mat & rightImage,
const std::vector<cv::Point2f> & leftCorners,
std::vector<unsigned char> & status) const;
cv::Size winSize() const {return cv::Size(winWidth_, winHeight_);}
int iterations() const {return iterations_;}
int maxLevel() const {return maxLevel_;}
int minDisparity() const {return minDisparity_;}
int maxDisparity() const {return maxDisparity_;}
bool winSSD() const {return winSSD_;}
private:
int winWidth_;
int winHeight_;
int iterations_;
int maxLevel_;
int minDisparity_;
int maxDisparity_;
bool winSSD_;
};
class RTABMAP_EXP StereoOpticalFlow : public Stereo {
public:
StereoOpticalFlow(const ParametersMap & parameters = ParametersMap());
virtual ~StereoOpticalFlow() {}
virtual void parseParameters(const ParametersMap & parameters);
virtual std::vector<cv::Point2f> computeCorrespondences(
const cv::Mat & leftImage,
const cv::Mat & rightImage,
const std::vector<cv::Point2f> & leftCorners,
std::vector<unsigned char> & status) const;
float epsilon() const {return epsilon_;}
float maxSlope() const {return maxSlope_;}
private:
float epsilon_;
float maxSlope_;
};
} /* namespace rtabmap */
#endif /* STEREO_H_ */

143
corelib/include/rtabmap/core/StereoCameraModel.h Normal file
View File

@@ -0,0 +1,143 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef STEREOCAMERAMODEL_H_
#define STEREOCAMERAMODEL_H_
#include <rtabmap/core/CameraModel.h>
namespace rtabmap {
class RTABMAP_EXP StereoCameraModel
{
public:
StereoCameraModel() {}
StereoCameraModel(
const std::string & name,
const cv::Size & imageSize1,
const cv::Mat & K1, const cv::Mat & D1, const cv::Mat & R1, const cv::Mat & P1,
const cv::Size & imageSize2,
const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2,
const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F,
const Transform & localTransform = Transform::getIdentity()) :
left_(name+"_left", imageSize1, K1, D1, R1, P1, localTransform),
right_(name+"_right", imageSize2, K2, D2, R2, P2, localTransform),
name_(name),
R_(R),
T_(T),
E_(E),
F_(F)
{
}
StereoCameraModel(
const std::string & name,
const CameraModel & leftCameraModel,
const CameraModel & rightCameraModel,
const cv::Mat & R = cv::Mat(),
const cv::Mat & T = cv::Mat(),
const cv::Mat & E = cv::Mat(),
const cv::Mat & F = cv::Mat()) :
left_(leftCameraModel),
right_(rightCameraModel),
name_(name),
R_(R),
T_(T),
E_(E),
F_(F)
{
left_.setName(name+"_left");
right_.setName(name+"_right");
}
//minimal
StereoCameraModel(
double fx,
double fy,
double cx,
double cy,
double baseline,
const Transform & localTransform = Transform::getIdentity()) :
left_(fx, fy, cx, cy, localTransform),
right_(fx, fy, cx, cy, localTransform, baseline*-fx)
{
}
//minimal to be saved
StereoCameraModel(
const std::string & name,
double fx,
double fy,
double cx,
double cy,
double baseline,
const Transform & localTransform = Transform::getIdentity()) :
left_(name+"_left", fx, fy, cx, cy, localTransform),
right_(name+"_right", fx, fy, cx, cy, localTransform, baseline*-fx),
name_(name)
{
}
virtual ~StereoCameraModel() {}
bool isValid() const {return left_.isValid() && right_.isValid() && baseline() > 0.0;}
void setName(const std::string & name);
const std::string & name() const {return name_;}
bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
bool save(const std::string & directory, bool ignoreStereoTransform = true) const;
double baseline() const {return -right_.Tx()/right_.fx();}
float computeDepth(float disparity) const;
float computeDisparity(float depth) const; // m
float computeDisparity(unsigned short depth) const; // mm
const cv::Mat & R() const {return R_;} //extrinsic rotation matrix
const cv::Mat & T() const {return T_;} //extrinsic translation matrix
const cv::Mat & E() const {return E_;} //extrinsic essential matrix
const cv::Mat & F() const {return F_;} //extrinsic fundamental matrix
void scale(double scale);
void setLocalTransform(const Transform & transform) {left_.setLocalTransform(transform);}
const Transform & localTransform() const {return left_.localTransform();}
Transform stereoTransform() const;
const CameraModel & left() const {return left_;}
const CameraModel & right() const {return right_;}
private:
CameraModel left_;
CameraModel right_;
std::string name_;
cv::Mat R_;
cv::Mat T_;
cv::Mat E_;
cv::Mat F_;
};
} // rtabmap
#endif /* STEREOCAMERAMODEL_H_ */

76
corelib/include/rtabmap/core/StereoDense.h Normal file
View File

@@ -0,0 +1,76 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef STEREODENSE_H_
#define STEREODENSE_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <rtabmap/core/Parameters.h>
#include <opencv2/core/core.hpp>
namespace rtabmap {
class RTABMAP_EXP StereoDense {
public:
virtual ~StereoDense() {}
virtual void parseParameters(const ParametersMap & parameters) {}
virtual cv::Mat computeDisparity(
const cv::Mat & leftImage,
const cv::Mat & rightImage) const = 0;
protected:
StereoDense(const ParametersMap & parameters = ParametersMap()) {}
};
class RTABMAP_EXP StereoBM : public StereoDense {
public:
StereoBM(int blockSize, int numDisparities);
StereoBM(const ParametersMap & parameters = ParametersMap());
virtual ~StereoBM() {}
virtual void parseParameters(const ParametersMap & parameters);
virtual cv::Mat computeDisparity(
const cv::Mat & leftImage,
const cv::Mat & rightImage) const;
private:
int blockSize_; //15
int minDisparity_; //0
int numDisparities_; //64
int preFilterSize_; //9
int preFilterCap_; //31
int uniquenessRatio_; //15
int textureThreshold_; //10
int speckleWindowSize_; //100
int speckleRange_; //4
};
} /* namespace rtabmap */
#endif /* STEREODENSE_H_ */

37
corelib/include/rtabmap/core/util2d.h
View File

@@ -39,20 +39,36 @@ namespace rtabmap
namespace util2d
{
cv::Mat RTABMAP_EXP disparityFromStereoImages(
// SSD: Sum of Squared Differences
float RTABMAP_EXP ssd(const cv::Mat & windowLeft, const cv::Mat & windowRight);
// SAD: Sum of Absolute intensity Differences
float RTABMAP_EXP sad(const cv::Mat & windowLeft, const cv::Mat & windowRight);
std::vector<cv::Point2f> RTABMAP_EXP calcStereoCorrespondences(
const cv::Mat & leftImage,
const cv::Mat & rightImage,
int type = CV_32FC1); // CV_32FC1 or CV_16SC1
const std::vector<cv::Point2f> & leftCorners,
std::vector<unsigned char> & status,
cv::Size winSize = cv::Size(6,3),
int maxLevel = 3,
int iterations = 5,
int minDisparity = 0,
int maxDisparity = 64,
bool ssdApproach = true); // SSD by default, otherwise it is SAD
// exactly as cv::calcOpticalFlowPyrLK but it should be called with pyramid (from cv::buildOpticalFlowPyramid()) and delta drops the y error.
void RTABMAP_EXP calcOpticalFlowPyrLKStereo( cv::InputArray _prevImg, cv::InputArray _nextImg,
cv::InputArray _prevPts, cv::InputOutputArray _nextPts,
cv::OutputArray _status, cv::OutputArray _err,
cv::Size winSize = cv::Size(15,3), int maxLevel = 3,
cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01),
int flags = 0, double minEigThreshold = 1e-4 );
cv::Mat RTABMAP_EXP disparityFromStereoImages(
const cv::Mat & leftImage,
const cv::Mat & rightImage,
const std::vector<cv::Point2f> & leftCorners,
int flowWinSize = 9,
int flowMaxLevel = 4,
int flowIterations = 20,
double flowEps = 0.02,
float maxCorrespondencesSlope = 0.1f);
int type = CV_32FC1); // CV_32FC1 or CV_16SC1
cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
float fx, float baseline,
@@ -70,11 +86,10 @@ cv::Mat RTABMAP_EXP depthFromStereoImages(
double flowEps = 0.02);
cv::Mat RTABMAP_EXP disparityFromStereoCorrespondences(
const cv::Mat & leftImage,
const cv::Size & disparitySize,
const std::vector<cv::Point2f> & leftCorners,
const std::vector<cv::Point2f> & rightCorners,
const std::vector<unsigned char> & mask,
float maxSlope = 0.1f);
const std::vector<unsigned char> & mask);
cv::Mat RTABMAP_EXP depthFromStereoCorrespondences(
const cv::Mat & leftImage,

13
corelib/include/rtabmap/core/util3d.h
View File

@@ -87,22 +87,19 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDisparity(
const cv::Mat & imageDisparity,
float cx, float cy,
float fx, float baseline,
const StereoCameraModel & model,
int decimation = 1);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDisparityRGB(
const cv::Mat & imageRgb,
const cv::Mat & imageDisparity,
float cx, float cy,
float fx, float baseline,
const StereoCameraModel & model,
int decimation = 1);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromStereoImages(
const cv::Mat & imageLeft,
const cv::Mat & imageRight,
float cx, float cy,
float fx, float baseline,
const StereoCameraModel & model,
int decimation = 1);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
@@ -136,12 +133,12 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::
pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
const cv::Point2f & pt,
float disparity,
float cx, float cy, float fx, float baseline);
const StereoCameraModel & model);
pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
const cv::Point2f & pt,
const cv::Mat & disparity,
float cx, float cy, float fx, float baseline);
const StereoCameraModel & model);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(
const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds);

30
corelib/include/rtabmap/core/util3d_features.h
View File

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/calib3d/calib3d.hpp>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/CameraModel.h>
#include <rtabmap/core/StereoCameraModel.h>
#include <list>
#include <map>
@@ -60,34 +61,11 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDisparity(
const cv::Mat & disparity,
const StereoCameraModel & stereoCameraMode);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & leftImage,
const cv::Mat & rightImage,
float fx,
float baseline,
float cx,
float cy,
Transform localTransform = Transform::getIdentity(),
int flowWinSize = 9,
int flowMaxLevel = 4,
int flowIterations = 20,
double flowEps = 0.02,
double maxCorrespondencesSlope = 0.0);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
const std::vector<cv::Point2f> & leftCorners,
const cv::Mat & leftImage,
const cv::Mat & rightImage,
float fx,
float baseline,
float cx,
float cy,
Transform localTransform = Transform::getIdentity(),
int flowWinSize = 9,
int flowMaxLevel = 4,
int flowIterations = 20,
double flowEps = 0.02,
double maxCorrespondencesSlope = 0.0);
const std::vector<cv::Point2f> & rightCorners,
const StereoCameraModel & model,
const std::vector<unsigned char> & mask = std::vector<unsigned char>());
std::multimap<int, pcl::PointXYZ> RTABMAP_EXP generateWords3DMono(
const std::multimap<int, cv::KeyPoint> & kpts,

4
corelib/src/CMakeLists.txt
View File

@@ -60,6 +60,10 @@ SET(SRC_FILES
OdometryMono.cpp
OdometryICP.cpp
Stereo.cpp
StereoDense.cpp
StereoCameraModel.cpp
toro3d/posegraph3.cpp
toro3d/treeoptimizer3_iteration.cpp
toro3d/treeoptimizer3.cpp

147
corelib/src/CameraModel.cpp
View File

@@ -362,151 +362,4 @@ cv::Mat CameraModel::rectifyDepth(const cv::Mat & raw) const
}
}
//
//StereoCameraModel
//
void StereoCameraModel::setName(const std::string & name)
{
name_=name;
left_.setName(name_+"_left");
right_.setName(name_+"_right");
}
bool StereoCameraModel::load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform)
{
name_ = cameraName;
if(left_.load(directory, cameraName+"_left") && right_.load(directory, cameraName+"_right"))
{
if(ignoreStereoTransform)
{
return true;
}
//load rotation, translation
R_ = cv::Mat();
T_ = cv::Mat();
std::string filePath = directory+"/"+cameraName+"_pose.yaml";
if(UFile::exists(filePath))
{
UINFO("Reading stereo calibration file \"%s\"", filePath.c_str());
cv::FileStorage fs(filePath, cv::FileStorage::READ);
name_ = (int)fs["camera_name"];
// import from ROS calibration format
cv::FileNode n = fs["rotation_matrix"];
int rows = (int)n["rows"];
int cols = (int)n["cols"];
std::vector<double> data;
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 3);
R_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
n = fs["translation_matrix"];
rows = (int)n["rows"];
cols = (int)n["cols"];
data.clear();
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 1);
T_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
n = fs["essential_matrix"];
rows = (int)n["rows"];
cols = (int)n["cols"];
data.clear();
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 3);
E_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
n = fs["fundamental_matrix"];
rows = (int)n["rows"];
cols = (int)n["cols"];
data.clear();
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 3);
F_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
fs.release();
return true;
}
else
{
UWARN("Could not load stereo calibration file \"%s\".", filePath.c_str());
}
}
return false;
}
bool StereoCameraModel::save(const std::string & directory, bool ignoreStereoTransform) const
{
if(left_.save(directory) && right_.save(directory))
{
if(ignoreStereoTransform)
{
return true;
}
std::string filePath = directory+"/"+name_+"_pose.yaml";
if(!filePath.empty() && !name_.empty() && !R_.empty() && !T_.empty())
{
UINFO("Saving stereo calibration to file \"%s\"", filePath.c_str());
cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
// export in ROS calibration format
fs << "camera_name" << name_;
fs << "rotation_matrix" << "{";
fs << "rows" << R_.rows;
fs << "cols" << R_.cols;
fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
fs << "}";
fs << "translation_matrix" << "{";
fs << "rows" << T_.rows;
fs << "cols" << T_.cols;
fs << "data" << std::vector<double>((double*)T_.data, ((double*)T_.data)+(T_.rows*T_.cols));
fs << "}";
fs << "essential_matrix" << "{";
fs << "rows" << E_.rows;
fs << "cols" << E_.cols;
fs << "data" << std::vector<double>((double*)E_.data, ((double*)E_.data)+(E_.rows*E_.cols));
fs << "}";
fs << "fundamental_matrix" << "{";
fs << "rows" << F_.rows;
fs << "cols" << F_.cols;
fs << "data" << std::vector<double>((double*)F_.data, ((double*)F_.data)+(F_.rows*F_.cols));
fs << "}";
fs.release();
return true;
}
}
return false;
}
void StereoCameraModel::scale(double scale)
{
left_.scale(scale);
right_.scale(scale);
}
Transform StereoCameraModel::stereoTransform() const
{
if(!R_.empty() && !T_.empty())
{
return Transform(
R_.at<double>(0,0), R_.at<double>(0,1), R_.at<double>(0,2), T_.at<double>(0),
R_.at<double>(1,0), R_.at<double>(1,1), R_.at<double>(1,2), T_.at<double>(1),
R_.at<double>(2,0), R_.at<double>(2,1), R_.at<double>(2,2), T_.at<double>(2));
}
return Transform();
}
} /* namespace rtabmap */

9
corelib/src/CameraThread.cpp
View File

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/CameraRGBD.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/StereoDense.h"
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/ULogger.h>
@@ -39,11 +40,12 @@ namespace rtabmap
{
// ownership transferred
CameraThread::CameraThread(Camera * camera) :
CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
_camera(camera),
_mirroring(false),
_colorOnly(false),
_stereoToDepth(false)
_stereoToDepth(false),
_stereoDense(new StereoBM(parameters))
{
UASSERT(_camera != 0);
}
@@ -55,6 +57,7 @@ CameraThread::~CameraThread()
{
delete _camera;
}
delete _stereoDense;
}
void CameraThread::setImageRate(float imageRate)
@@ -105,7 +108,7 @@ void CameraThread::mainLoop()
{
UTimer timer;
cv::Mat depth = util2d::depthFromDisparity(
util2d::disparityFromStereoImages(data.imageRaw(), data.rightRaw()),
_stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
data.stereoCameraModel().left().fx(),
data.stereoCameraModel().baseline());
data.setCameraModel(data.stereoCameraModel().left());

2
corelib/src/Features2d.cpp
View File

@@ -343,7 +343,7 @@ void Feature2D::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kKpWordsPerImage(), maxFeatures_);
}
Feature2D * Feature2D::create(Feature2D::Type & type, const ParametersMap & parameters)
Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parameters)
{
if(RTABMAP_NONFREE == 0)
{

142
corelib/src/Memory.cpp
View File

@@ -55,6 +55,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Statistics.h"
#include "rtabmap/core/Compression.h"
#include "rtabmap/core/Graph.h"
#include "rtabmap/core/Stereo.h"
#include <pcl/io/pcd_io.h>
#include <pcl/common/common.h>
@@ -104,20 +105,16 @@ Memory::Memory(const ParametersMap & parameters) :
_wordsMinDepth(Parameters::defaultKpMinDepth()),
_roiRatios(std::vector<float>(4, 0.0f)),
_stereoFlowWinSize(Parameters::defaultStereoWinSize()),
_stereoFlowIterations(Parameters::defaultStereoIterations()),
_stereoFlowEpsilon(Parameters::defaultStereoEps()),
_stereoFlowMaxLevel(Parameters::defaultStereoMaxLevel()),
_stereoMaxSlope(Parameters::defaultStereoMaxSlope()),
_subPixWinSize(Parameters::defaultKpSubPixWinSize()),
_subPixIterations(Parameters::defaultKpSubPixIterations()),
_subPixEps(Parameters::defaultKpSubPixEps())
{
_feature2D = Feature2D::create(_featureType, parameters);
_featureType = _feature2D->getType();
_vwd = new VWDictionary(parameters);
_registrationVis = new RegistrationVis(parameters);
_registrationIcp = new RegistrationIcp(parameters);
_stereo = new Stereo(parameters);
this->parseParameters(parameters);
}
@@ -377,6 +374,10 @@ Memory::~Memory()
{
delete _registrationIcp;
}
if(_stereo)
{
delete _stereo;
}
if(_postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosed));
}
@@ -417,13 +418,6 @@ void Memory::parseParameters(const ParametersMap & parameters)
_dbDriver->parseParameters(parameters);
}
//stereo
Parameters::parse(parameters, Parameters::kStereoWinSize(), _stereoFlowWinSize);
Parameters::parse(parameters, Parameters::kStereoIterations(), _stereoFlowIterations);
Parameters::parse(parameters, Parameters::kStereoEps(), _stereoFlowEpsilon);
Parameters::parse(parameters, Parameters::kStereoMaxLevel(), _stereoFlowMaxLevel);
Parameters::parse(parameters, Parameters::kStereoMaxSlope(), _stereoMaxSlope);
// Keypoint stuff
if(_vwd)
{
@@ -463,7 +457,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
}
_feature2D = Feature2D::create(detectorStrategy, parameters);
_featureType = detectorStrategy;
_featureType = _feature2D->getType();
}
else if(_feature2D)
{
@@ -477,7 +471,27 @@ void Memory::parseParameters(const ParametersMap & parameters)
if(_registrationIcp)
{
_registrationIcp->parseParameters(parameters);
}
}
//stereo
UASSERT(_stereo != 0);
if((iter=parameters.find(Parameters::kStereoOpticalFlow())) != parameters.end())
{
bool opticalFlow = uStr2Bool(iter->second);
delete _stereo;
if(opticalFlow)
{
_stereo = new StereoOpticalFlow(parameters);
}
else
{
_stereo = new Stereo(parameters);
}
}
else
{
_stereo->parseParameters(parameters);
}
// do this after all parameters are parsed
// SLAM mode vs Localization mode
@@ -3093,7 +3107,6 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
if(!data.depthOrRightRaw().empty() && data.stereoCameraModel().isValid())
{
//stereo
cv::Mat disparity;
bool subPixelOn = false;
if(_subPixWinSize > 0 && _subPixIterations > 0)
{
@@ -3137,27 +3150,35 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
}
//generate a disparity map
disparity = util2d::disparityFromStereoImages(
std::vector<unsigned char> status;
std::vector<cv::Point2f> rightCorners;
rightCorners = _stereo->computeCorrespondences(
imageMono,
data.depthOrRightRaw(),
data.rightRaw(),
leftCorners,
_stereoFlowWinSize,
_stereoFlowMaxLevel,
_stereoFlowIterations,
_stereoFlowEpsilon,
_stereoMaxSlope);
status);
if(_wordsMaxDepth > 0.0f || _wordsMinDepth > 0.0f)
{
UASSERT(status.size() == leftCorners.size() && status.size() == rightCorners.size());
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i] != 0)
{
float d = data.stereoCameraModel().computeDepth(leftCorners[i].x - rightCorners[i].x);
if((_wordsMinDepth > 0.0f && d < _wordsMinDepth) ||
(_wordsMaxDepth > 0.0f && d > _wordsMaxDepth))
{
status[i] = 0;
}
}
}
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemStereo_correspondences(), t*1000.0f);
UDEBUG("generate disparity = %fs", t);
if(_wordsMaxDepth > 0.0f)
{
// disparity = baseline * fx / depth;
float minDisparity = data.stereoCameraModel().baseline() * data.stereoCameraModel().left().fx() / _wordsMaxDepth;
Feature2D::filterKeypointsByDisparity(keypoints, descriptors, disparity, minDisparity);
UDEBUG("filter keypoints by disparity (%d)", (int)keypoints.size());
}
if(keypoints.size())
{
if(!subPixelOn)
@@ -3167,11 +3188,13 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
}
keypoints3D = util3d::generateKeypoints3DDisparity(
keypoints,
disparity,
data.stereoCameraModel());
keypoints3D = util3d::generateKeypoints3DStereo(
leftCorners,
rightCorners,
data.stereoCameraModel(),
status);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t);
@@ -3323,30 +3346,41 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
//generate a disparity map
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(keypoints, leftCorners);
cv::Mat disparity = util2d::disparityFromStereoImages(
std::vector<unsigned char> status;
std::vector<cv::Point2f> rightCorners;
rightCorners = _stereo->computeCorrespondences(
imageMono,
data.depthOrRightRaw(),
data.rightRaw(),
leftCorners,
_stereoFlowWinSize,
_stereoFlowMaxLevel,
_stereoFlowIterations,
_stereoFlowEpsilon,
_stereoMaxSlope);
status);
if(_wordsMaxDepth > 0.0f || _wordsMinDepth > 0.0f)
{
UASSERT(status.size() == leftCorners.size() && status.size() == rightCorners.size());
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i] != 0)
{
float d = data.stereoCameraModel().computeDepth(leftCorners[i].x - rightCorners[i].x);
if((_wordsMinDepth > 0.0f && d < _wordsMinDepth) ||
(_wordsMaxDepth > 0.0f && d > _wordsMaxDepth))
{
status[i] = 0;
}
}
}
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemStereo_correspondences(), t*1000.0f);
UDEBUG("generate disparity = %fs", t);
if(_wordsMaxDepth)
{
// disparity = baseline * fx / depth;
float minDisparity = data.stereoCameraModel().baseline() * data.stereoCameraModel().left().fx() / _wordsMaxDepth;
Feature2D::filterKeypointsByDisparity(keypoints, descriptors, disparity, minDisparity);
}
keypoints3D = util3d::generateKeypoints3DDisparity(
keypoints,
disparity,
data.stereoCameraModel());
keypoints3D = util3d::generateKeypoints3DStereo(
leftCorners,
rightCorners,
data.stereoCameraModel(),
status);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t);

54
corelib/src/OdometryMono.cpp
View File

@@ -31,8 +31,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/util3d_features.h"
#include "rtabmap/core/EpipolarGeometry.h"
#include "rtabmap/core/Stereo.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UConversion.h"
@@ -51,11 +53,6 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
flowIterations_(Parameters::defaultOdomFlowIterations()),
flowEps_(Parameters::defaultOdomFlowEps()),
flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
stereoWinSize_(Parameters::defaultStereoWinSize()),
stereoIterations_(Parameters::defaultStereoIterations()),
stereoEps_(Parameters::defaultStereoEps()),
stereoMaxLevel_(Parameters::defaultStereoMaxLevel()),
stereoMaxSlope_(Parameters::defaultStereoMaxSlope()),
localHistoryMaxSize_(Parameters::defaultOdomBowLocalHistorySize()),
initMinFlow_(Parameters::defaultOdomMonoInitMinFlow()),
initMinTranslation_(Parameters::defaultOdomMonoInitMinTranslation()),
@@ -70,12 +67,6 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kOdomFlowMaxLevel(), flowMaxLevel_);
Parameters::parse(parameters, Parameters::kOdomBowLocalHistorySize(), localHistoryMaxSize_);
Parameters::parse(parameters, Parameters::kStereoWinSize(), stereoWinSize_);
Parameters::parse(parameters, Parameters::kStereoIterations(), stereoIterations_);
Parameters::parse(parameters, Parameters::kStereoEps(), stereoEps_);
Parameters::parse(parameters, Parameters::kStereoMaxLevel(), stereoMaxLevel_);
Parameters::parse(parameters, Parameters::kStereoMaxSlope(), stereoMaxSlope_);
Parameters::parse(parameters, Parameters::kOdomMonoInitMinFlow(), initMinFlow_);
Parameters::parse(parameters, Parameters::kOdomMonoInitMinTranslation(), initMinTranslation_);
Parameters::parse(parameters, Parameters::kOdomMonoMinTranslation(), minTranslation_);
@@ -139,11 +130,23 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
{
UERROR("Error initializing the memory for Mono Odometry.");
}
bool stereoOpticalFlow = Parameters::defaultStereoOpticalFlow();
Parameters::parse(parameters, Parameters::kStereoOpticalFlow(), stereoOpticalFlow);
if(stereoOpticalFlow)
{
stereo_ = new StereoOpticalFlow(parameters);
}
else
{
stereo_ = new Stereo(parameters);
}
}
OdometryMono::~OdometryMono()
{
delete memory_;
delete stereo_;
}
void OdometryMono::reset(const Transform & initialPose)
@@ -735,20 +738,21 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
{
if(refDepthOrRight_.type() == CV_8UC1)
{
newCorners3D = util3d::generateKeypoints3DStereo(
refCorners,
refS->sensorData().imageRaw(),
refDepthOrRight_,
cameraModel.fx(),
data.stereoCameraModel().baseline(),
cameraModel.cx(),
cameraModel.cy(),
Transform::getIdentity(),
stereoWinSize_,
stereoMaxLevel_,
stereoIterations_,
stereoEps_,
stereoMaxSlope_ );
StereoCameraModel m = data.stereoCameraModel();
m.setLocalTransform(Transform::getIdentity());
std::vector<unsigned char> stereoStatus;
std::vector<cv::Point2f> rightCorners;
rightCorners = stereo_->computeCorrespondences(
refS->sensorData().imageRaw(),
refDepthOrRight_,
refCorners,
stereoStatus);
newCorners3D = util3d::generateKeypoints3DStereo(
refCorners,
rightCorners,
m,
stereoStatus);
}
else if(refDepthOrRight_.type() == CV_32FC1 || refDepthOrRight_.type() == CV_16UC1)
{

280
corelib/src/OdometryOpticalFlow.cpp
View File

@@ -30,8 +30,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Features2d.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_features.h"
#include "rtabmap/core/Stereo.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UConversion.h"
@@ -45,33 +47,39 @@ namespace rtabmap {
OdometryOpticalFlow::OdometryOpticalFlow(const ParametersMap & parameters) :
Odometry(parameters),
keyFrameThr_(Parameters::defaultOdomFlowKeyFrameThr()),
flowWinSize_(Parameters::defaultOdomFlowWinSize()),
flowIterations_(Parameters::defaultOdomFlowIterations()),
flowEps_(Parameters::defaultOdomFlowEps()),
flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
stereoWinSize_(Parameters::defaultStereoWinSize()),
stereoIterations_(Parameters::defaultStereoIterations()),
stereoEps_(Parameters::defaultStereoEps()),
stereoMaxLevel_(Parameters::defaultStereoMaxLevel()),
stereoMaxSlope_(Parameters::defaultStereoMaxSlope()),
flowGuessFromMotion_(Parameters::defaultOdomFlowGuessMotion()),
subPixWinSize_(Parameters::defaultVisSubPixWinSize()),
subPixIterations_(Parameters::defaultVisSubPixIterations()),
subPixEps_(Parameters::defaultVisSubPixEps()),
refCorners3D_(new pcl::PointCloud<pcl::PointXYZ>)
refCorners3D_(new pcl::PointCloud<pcl::PointXYZ>),
motionSinceLastKeyFrame_(Transform::getIdentity())
{
Parameters::parse(parameters, Parameters::kOdomFlowKeyFrameThr(), keyFrameThr_);
Parameters::parse(parameters, Parameters::kOdomFlowWinSize(), flowWinSize_);
Parameters::parse(parameters, Parameters::kOdomFlowIterations(), flowIterations_);
Parameters::parse(parameters, Parameters::kOdomFlowEps(), flowEps_);
Parameters::parse(parameters, Parameters::kOdomFlowMaxLevel(), flowMaxLevel_);
Parameters::parse(parameters, Parameters::kStereoWinSize(), stereoWinSize_);
Parameters::parse(parameters, Parameters::kStereoIterations(), stereoIterations_);
Parameters::parse(parameters, Parameters::kStereoEps(), stereoEps_);
Parameters::parse(parameters, Parameters::kStereoMaxLevel(), stereoMaxLevel_);
Parameters::parse(parameters, Parameters::kStereoMaxSlope(), stereoMaxSlope_);
Parameters::parse(parameters, Parameters::kOdomFlowGuessMotion(), flowGuessFromMotion_);
Parameters::parse(parameters, Parameters::kVisSubPixWinSize(), subPixWinSize_);
Parameters::parse(parameters, Parameters::kVisSubPixIterations(), subPixIterations_);
Parameters::parse(parameters, Parameters::kVisSubPixEps(), subPixEps_);
bool stereoOpticalFlow = Parameters::defaultStereoOpticalFlow();
Parameters::parse(parameters, Parameters::kStereoOpticalFlow(), stereoOpticalFlow);
if(stereoOpticalFlow)
{
stereo_ = new StereoOpticalFlow(parameters);
}
else
{
stereo_ = new Stereo(parameters);
}
ParametersMap::const_iterator iter;
Feature2D::Type detectorStrategy = (Feature2D::Type)Parameters::defaultVisFeatureType();
if((iter=parameters.find(Parameters::kVisFeatureType())) != parameters.end())
@@ -106,15 +114,16 @@ OdometryOpticalFlow::OdometryOpticalFlow(const ParametersMap & parameters) :
OdometryOpticalFlow::~OdometryOpticalFlow()
{
delete feature2D_;
delete stereo_;
}
void OdometryOpticalFlow::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
refFrame_ = cv::Mat();
refCorners_.clear();
refCorners3D_->clear();
motionSinceLastKeyFrame_.setIdentity();
}
// return not null transform if odometry is correctly computed
@@ -168,10 +177,9 @@ Transform OdometryOpticalFlow::computeTransform(
uFormat("%d vs %d", (int)refCorners_.size(), (int)refCorners3D_->size()).c_str());
// make guess
bool flowGuessByMotion = true;
cv::Mat K = data.cameraModels().size()?data.cameraModels()[0].K():data.stereoCameraModel().left().K();
Transform localTransform = data.cameraModels().size()?data.cameraModels()[0].localTransform():data.stereoCameraModel().left().localTransform();
Transform guess = (this->previousTransform() * localTransform).inverse();
Transform guess = (motionSinceLastKeyFrame_*this->previousTransform() * localTransform).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
@@ -186,7 +194,7 @@ Transform OdometryOpticalFlow::computeTransform(
objectPoints[i].y = refCorners3D_->at(i).y;
objectPoints[i].z = refCorners3D_->at(i).z;
}
if(flowGuessByMotion && !this->previousTransform().isIdentity())
if(flowGuessFromMotion_ && !(motionSinceLastKeyFrame_*this->previousTransform()).isIdentity())
{
UDEBUG("project points to new image");
cv::projectPoints(objectPoints, rvec, tvec, K, cv::Mat(), newCorners);
@@ -196,7 +204,7 @@ Transform OdometryOpticalFlow::computeTransform(
std::vector<unsigned char> status;
std::vector<float> err;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
int winSize = (newCorners.size()||!flowGuessByMotion)?flowWinSize_:(flowWinSize_*2);
int winSize = (newCorners.size()||!flowGuessFromMotion_)?flowWinSize_:(flowWinSize_*2);
cv::calcOpticalFlowPyrLK(
refFrame_,
newLeftFrame,
@@ -205,7 +213,7 @@ Transform OdometryOpticalFlow::computeTransform(
status,
err,
cv::Size(winSize, winSize),
(newCorners.size()||!flowGuessByMotion)?flowMaxLevel_:flowMaxLevel_*2,
(newCorners.size()||!flowGuessFromMotion_)?flowMaxLevel_:flowMaxLevel_*2,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (newCorners.size()?cv::OPTFLOW_USE_INITIAL_FLOW:0), 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
@@ -246,20 +254,35 @@ Transform OdometryOpticalFlow::computeTransform(
if(!data.rightRaw().empty())
{
// stereo
newCorners3DKept = util3d::generateKeypoints3DStereo(
newCornersKept,
std::vector<unsigned char> stereoStatus;
std::vector<cv::Point2f> rightCorners;
rightCorners = stereo_->computeCorrespondences(
newLeftFrame,
data.rightRaw(),
data.stereoCameraModel().left().fx(),
data.stereoCameraModel().baseline(),
data.stereoCameraModel().left().cx(),
data.stereoCameraModel().left().cy(),
data.stereoCameraModel().left().localTransform(),
stereoWinSize_,
stereoMaxLevel_,
stereoIterations_,
stereoEps_,
stereoMaxSlope_);
newCornersKept,
stereoStatus);
if(this->getMaxDepth() > 0.0f)
{
UASSERT(status.size() == newCornersKept.size() && status.size() == rightCorners.size());
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i] != 0)
{
float d = data.stereoCameraModel().computeDepth(newCornersKept[i].x - rightCorners[i].x);
if(this->getMaxDepth() > 0.0f && d > this->getMaxDepth())
{
status[i] = 0;
}
}
}
}
newCorners3DKept = util3d::generateKeypoints3DStereo(
newCornersKept,
rightCorners,
data.stereoCameraModel(),
stereoStatus);
}
else
{
@@ -433,120 +456,133 @@ Transform OdometryOpticalFlow::computeTransform(
newCorners.clear();
if(!output.isNull())
{
// Copy or generate new keypoints
if(data.keypoints().size())
{
cv::KeyPoint::convert(data.keypoints(), newCorners);
}
else
{
// generate kpts
std::vector<cv::KeyPoint> newKtps;
cv::Rect roi = Feature2D::computeRoi(newLeftFrame, this->getRoiRatios());
newKtps = feature2D_->generateKeypoints(newLeftFrame, roi);
output = motionSinceLastKeyFrame_.inverse() * output;
if(newKtps.size())
// new key-frame?
if(keyFrameThr_ <= 0 || inliers <= keyFrameThr_)
{
// Copy or generate new keypoints
if(data.keypoints().size())
{
cv::KeyPoint::convert(newKtps, newCorners);
if(subPixWinSize_ > 0 && subPixIterations_ > 0)
{
UDEBUG("cv::cornerSubPix() begin");
cv::cornerSubPix(newLeftFrame, newCorners,
cv::Size( subPixWinSize_, subPixWinSize_ ),
cv::Size( -1, -1 ),
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations_, subPixEps_ ) );
UDEBUG("cv::cornerSubPix() end");
}
}
}
if((int)newCorners.size() >= this->getMinInliers())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D(new pcl::PointCloud<pcl::PointXYZ>);
newCorners3D->resize(newCorners.size());
std::vector<cv::Point2f> newCornersFiltered(newCorners.size());
int oi=0;
UTimer corner3dTimer;
if(!data.rightRaw().empty())
{
/// stereo
pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3DTmp = util3d::generateKeypoints3DStereo(
newCorners,
newLeftFrame,
data.rightRaw(),
data.stereoCameraModel().left().fx(),
data.stereoCameraModel().baseline(),
data.stereoCameraModel().left().cx(),
data.stereoCameraModel().left().cy(),
Transform::getIdentity(),
stereoWinSize_,
stereoMaxLevel_,
stereoIterations_,
stereoEps_,
stereoMaxSlope_);
UASSERT(refCorners3DTmp->size() == newCorners.size());
for(unsigned int i=0; i<newCorners.size(); ++i)
{
if(pcl::isFinite(refCorners3DTmp->at(i)) &&
(this->getMaxDepth() == 0.0f || refCorners3DTmp->at(i).z < this->getMaxDepth()))
{
newCorners3D->at(oi) = util3d::transformPoint(refCorners3DTmp->at(i), data.stereoCameraModel().left().localTransform());
newCornersFiltered[oi] = newCorners[i];
++oi;
}
}
cv::KeyPoint::convert(data.keypoints(), newCorners);
}
else
{
// depth
for(unsigned int i=0; i<newCorners.size(); ++i)
// generate kpts
std::vector<cv::KeyPoint> newKtps;
cv::Rect roi = Feature2D::computeRoi(newLeftFrame, this->getRoiRatios());
newKtps = feature2D_->generateKeypoints(newLeftFrame, roi);
if(newKtps.size())
{
if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depthRaw().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depthRaw().rows)))
cv::KeyPoint::convert(newKtps, newCorners);
if(subPixWinSize_ > 0 && subPixIterations_ > 0)
{
pcl::PointXYZ pt = util3d::projectDepthTo3D(
data.depthRaw(),
newCorners[i].x,
newCorners[i].y,
data.cameraModels()[0].cx(),
data.cameraModels()[0].cy(),
data.cameraModels()[0].fx(),
data.cameraModels()[0].fy(),
true);
if(pcl::isFinite(pt) &&
pt.z > 0 &&
(this->getMaxDepth() == 0.0f || pt.z < this->getMaxDepth()))
UDEBUG("cv::cornerSubPix() begin");
cv::cornerSubPix(newLeftFrame, newCorners,
cv::Size( subPixWinSize_, subPixWinSize_ ),
cv::Size( -1, -1 ),
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations_, subPixEps_ ) );
UDEBUG("cv::cornerSubPix() end");
}
}
}
if((int)newCorners.size() >= this->getMinInliers())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D(new pcl::PointCloud<pcl::PointXYZ>);
newCorners3D->resize(newCorners.size());
std::vector<cv::Point2f> newCornersFiltered(newCorners.size());
int oi=0;
UTimer corner3dTimer;
if(!data.rightRaw().empty())
{
// stereo
std::vector<unsigned char> stereoStatus;
std::vector<cv::Point2f> rightCorners;
rightCorners = stereo_->computeCorrespondences(
newLeftFrame,
data.rightRaw(),
newCorners,
stereoStatus);
pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3DTmp = util3d::generateKeypoints3DStereo(
newCorners,
rightCorners,
data.stereoCameraModel(),
stereoStatus);
UASSERT(refCorners3DTmp->size() == newCorners.size());
for(unsigned int i=0; i<newCorners.size(); ++i)
{
if(pcl::isFinite(refCorners3DTmp->at(i)) &&
(this->getMaxDepth() <= 0.0f || data.stereoCameraModel().computeDepth(newCorners[i].x - rightCorners[i].x) <= this->getMaxDepth() ))
{
newCorners3D->at(oi) = util3d::transformPoint(pt, data.cameraModels()[0].localTransform());
newCorners3D->at(oi) = refCorners3DTmp->at(i);
newCornersFiltered[oi] = newCorners[i];
++oi;
}
}
}
}
UDEBUG("Computing 3d corners = %f s", corner3dTimer.ticks());
newCornersFiltered.resize(oi);
newCorners3D->resize(oi);
else
{
// depth
for(unsigned int i=0; i<newCorners.size(); ++i)
{
if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depthRaw().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depthRaw().rows)))
{
pcl::PointXYZ pt = util3d::projectDepthTo3D(
data.depthRaw(),
newCorners[i].x,
newCorners[i].y,
data.cameraModels()[0].cx(),
data.cameraModels()[0].cy(),
data.cameraModels()[0].fx(),
data.cameraModels()[0].fy(),
true);
if(pcl::isFinite(pt) &&
pt.z > 0 &&
(this->getMaxDepth() == 0.0f || pt.z < this->getMaxDepth()))
{
newCorners3D->at(oi) = util3d::transformPoint(pt, data.cameraModels()[0].localTransform());
newCornersFiltered[oi] = newCorners[i];
++oi;
}
}
}
}
UDEBUG("Computing 3d corners = %f s", corner3dTimer.ticks());
newCornersFiltered.resize(oi);
newCorners3D->resize(oi);
if((int)newCornersFiltered.size() >= this->getMinInliers())
{
refFrame_ = newLeftFrame;
refCorners_ = newCornersFiltered;
refCorners3D_ = newCorners3D;
if((int)newCornersFiltered.size() >= this->getMinInliers())
{
refFrame_ = newLeftFrame;
refCorners_ = newCornersFiltered;
refCorners3D_ = newCorners3D;
//reset motion
motionSinceLastKeyFrame_.setIdentity();
}
else
{
UWARN("Too low 3D corners (%d/%d, minCorners=%d), ignoring new frame...",
(int)newCornersFiltered.size(), (int)refCorners3D_->size(), this->getMinInliers());
output.setNull();
}
}
else
{
UWARN("Too low 3D corners (%d/%d, minCorners=%d), ignoring new frame...",
(int)newCornersFiltered.size(), (int)refCorners3D_->size(), this->getMinInliers());
UWARN("Too low 2D corners (%d), ignoring new frame...",
(int)newCorners.size());
output.setNull();
}
}
else
{
UWARN("Too low 2D corners (%d), ignoring new frame...",
(int)newCorners.size());
output.setNull();
motionSinceLastKeyFrame_ *= output;
}
}
@@ -555,7 +591,7 @@ Transform OdometryOpticalFlow::computeTransform(
info->type = 1;
info->variance = variance;
info->inliers = inliers;
info->features = (int)newCorners.size();
info->features = (int)refCorners_.size();
info->matches = correspondences;
}

32
corelib/src/Parameters.cpp
View File

@@ -118,6 +118,21 @@ rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo)
return odomParameters;
}
ParametersMap Parameters::getDefaultParameters(const std::string & group)
{
rtabmap::ParametersMap parameters;
const rtabmap::ParametersMap & defaultParameters = rtabmap::Parameters::getDefaultParameters();
for(rtabmap::ParametersMap::const_iterator iter=defaultParameters.begin(); iter!=defaultParameters.end(); ++iter)
{
if(iter->first.compare(group) == 0)
{
parameters.insert(*iter);
}
}
UASSERT_MSG(parameters.size(), uFormat("No parameters found for group %s!", group.c_str()).c_str());
return parameters;
}
const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemovedParameters()
{
if(removedParameters_.empty())
@@ -201,6 +216,8 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
removedParameters_.insert(std::make_pair("RGBD/OptimizeSlam2D", std::make_pair(true, Parameters::kOptimizerSlam2D())));
removedParameters_.insert(std::make_pair("RGBD/OptimizeVarianceIgnored", std::make_pair(true, Parameters::kOptimizerVarianceIgnored())));
removedParameters_.insert(std::make_pair("Stereo/WinSize", std::make_pair(true, Parameters::kStereoWinWidth())));
// before 0.11.0
removedParameters_.insert(std::make_pair("GFTT/MaxCorners", std::make_pair(true, Parameters::kVisMaxFeatures())));
removedParameters_.insert(std::make_pair("LccBow/MaxDepth", std::make_pair(true, Parameters::kVisMaxDepth())));
@@ -270,7 +287,7 @@ void Parameters::parse(const ParametersMap & parameters, const std::string & key
ParametersMap::const_iterator iter = parameters.find(key);
if(iter != parameters.end())
{
value = atoi(iter->second.c_str());
value = uStr2Int(iter->second.c_str());
}
}
void Parameters::parse(const ParametersMap & parameters, const std::string & key, unsigned int & value)
@@ -278,7 +295,7 @@ void Parameters::parse(const ParametersMap & parameters, const std::string & key
ParametersMap::const_iterator iter = parameters.find(key);
if(iter != parameters.end())
{
value = atoi(iter->second.c_str());
value = uStr2Int(iter->second.c_str());
}
}
void Parameters::parse(const ParametersMap & parameters, const std::string & key, float & value)
@@ -305,6 +322,17 @@ void Parameters::parse(const ParametersMap & parameters, const std::string & key
value = iter->second;
}
}
void Parameters::parse(const ParametersMap & parameters, ParametersMap & parametersOut)
{
for(ParametersMap::iterator iter=parametersOut.begin(); iter!=parametersOut.end(); ++iter)
{
ParametersMap::const_iterator jter = parameters.find(iter->first);
if(jter != parameters.end())
{
iter->second = jter->second;
}
}
}
void Parameters::readINI(const std::string & configFile, ParametersMap & parameters)

134
corelib/src/Stereo.cpp Normal file
View File

@@ -0,0 +1,134 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/Stereo.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/utilite/ULogger.h>
#include <opencv2/video/tracking.hpp>
namespace rtabmap {
Stereo::Stereo(const ParametersMap & parameters) :
winWidth_(Parameters::defaultStereoWinWidth()),
winHeight_(Parameters::defaultStereoWinHeight()),
iterations_(Parameters::defaultStereoIterations()),
maxLevel_(Parameters::defaultStereoMaxLevel()),
minDisparity_(Parameters::defaultStereoMinDisparity()),
maxDisparity_(Parameters::defaultStereoMaxDisparity()),
winSSD_(Parameters::defaultStereoSSD())
{
}
void Stereo::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kStereoWinWidth(), winWidth_);
Parameters::parse(parameters, Parameters::kStereoWinHeight(), winHeight_);
Parameters::parse(parameters, Parameters::kStereoIterations(), iterations_);
Parameters::parse(parameters, Parameters::kStereoMaxLevel(), maxLevel_);
Parameters::parse(parameters, Parameters::kStereoMinDisparity(), minDisparity_);
Parameters::parse(parameters, Parameters::kStereoMaxDisparity(), maxDisparity_);
Parameters::parse(parameters, Parameters::kStereoSSD(), winSSD_);
}
std::vector<cv::Point2f> Stereo::computeCorrespondences(
const cv::Mat & leftImage,
const cv::Mat & rightImage,
const std::vector<cv::Point2f> & leftCorners,
std::vector<unsigned char> & status) const
{
std::vector<cv::Point2f> rightCorners;
UDEBUG("util2d::calcStereoCorrespondences() begin");
rightCorners = util2d::calcStereoCorrespondences(
leftImage,
rightImage,
leftCorners,
status,
cv::Size(winWidth_, winHeight_),
maxLevel_,
iterations_,
minDisparity_,
maxDisparity_,
winSSD_);
UDEBUG("util2d::calcStereoCorrespondences() end");
return rightCorners;
}
StereoOpticalFlow::StereoOpticalFlow(const ParametersMap & parameters) :
Stereo(parameters),
epsilon_(Parameters::defaultStereoEps()),
maxSlope_(Parameters::defaultStereoMaxSlope())
{
this->parseParameters(parameters);
}
void StereoOpticalFlow::parseParameters(const ParametersMap & parameters)
{
Stereo::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kStereoEps(), epsilon_);
Parameters::parse(parameters, Parameters::kStereoMaxSlope(), maxSlope_);
}
std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
const cv::Mat & leftImage,
const cv::Mat & rightImage,
const std::vector<cv::Point2f> & leftCorners,
std::vector<unsigned char> & status) const
{
std::vector<cv::Point2f> rightCorners;
UDEBUG("util2d::calcOpticalFlowPyrLKStereo() begin");
std::vector<float> err;
util2d::calcOpticalFlowPyrLKStereo(
leftImage,
rightImage,
leftCorners,
rightCorners,
status,
err,
this->winSize(),
this->maxLevel(),
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, this->iterations(), epsilon_),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
UDEBUG("util2d::calcOpticalFlowPyrLKStereo() end");
UASSERT(leftCorners.size() == rightCorners.size() && status.size() == leftCorners.size());
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i]!=0)
{
float disparity = leftCorners[i].x - rightCorners[i].x;
float slope = fabs((leftCorners[i].y-rightCorners[i].y) / (leftCorners[i].x-rightCorners[i].x));
if(disparity < float(this->minDisparity()) || disparity > float(this->maxDisparity()) ||
(maxSlope_ > 0.0f && fabs(leftCorners[i].y-rightCorners[i].y) > 1.0f && slope > maxSlope_))
{
status[i] = 0;
}
}
}
return rightCorners;
}
} /* namespace rtabmap */

214
corelib/src/StereoCameraModel.cpp Normal file
View File

@@ -0,0 +1,214 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/StereoCameraModel.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UConversion.h>
#include <opencv2/imgproc/imgproc.hpp>
namespace rtabmap {
void StereoCameraModel::setName(const std::string & name)
{
name_=name;
left_.setName(name_+"_left");
right_.setName(name_+"_right");
}
bool StereoCameraModel::load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform)
{
name_ = cameraName;
if(left_.load(directory, cameraName+"_left") && right_.load(directory, cameraName+"_right"))
{
if(ignoreStereoTransform)
{
return true;
}
//load rotation, translation
R_ = cv::Mat();
T_ = cv::Mat();
std::string filePath = directory+"/"+cameraName+"_pose.yaml";
if(UFile::exists(filePath))
{
UINFO("Reading stereo calibration file \"%s\"", filePath.c_str());
cv::FileStorage fs(filePath, cv::FileStorage::READ);
name_ = (int)fs["camera_name"];
// import from ROS calibration format
cv::FileNode n = fs["rotation_matrix"];
int rows = (int)n["rows"];
int cols = (int)n["cols"];
std::vector<double> data;
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 3);
R_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
n = fs["translation_matrix"];
rows = (int)n["rows"];
cols = (int)n["cols"];
data.clear();
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 1);
T_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
n = fs["essential_matrix"];
rows = (int)n["rows"];
cols = (int)n["cols"];
data.clear();
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 3);
E_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
n = fs["fundamental_matrix"];
rows = (int)n["rows"];
cols = (int)n["cols"];
data.clear();
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 3);
F_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
fs.release();
return true;
}
else
{
UWARN("Could not load stereo calibration file \"%s\".", filePath.c_str());
}
}
return false;
}
bool StereoCameraModel::save(const std::string & directory, bool ignoreStereoTransform) const
{
if(left_.save(directory) && right_.save(directory))
{
if(ignoreStereoTransform)
{
return true;
}
std::string filePath = directory+"/"+name_+"_pose.yaml";
if(!filePath.empty() && !name_.empty() && !R_.empty() && !T_.empty())
{
UINFO("Saving stereo calibration to file \"%s\"", filePath.c_str());
cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
// export in ROS calibration format
fs << "camera_name" << name_;
fs << "rotation_matrix" << "{";
fs << "rows" << R_.rows;
fs << "cols" << R_.cols;
fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
fs << "}";
fs << "translation_matrix" << "{";
fs << "rows" << T_.rows;
fs << "cols" << T_.cols;
fs << "data" << std::vector<double>((double*)T_.data, ((double*)T_.data)+(T_.rows*T_.cols));
fs << "}";
fs << "essential_matrix" << "{";
fs << "rows" << E_.rows;
fs << "cols" << E_.cols;
fs << "data" << std::vector<double>((double*)E_.data, ((double*)E_.data)+(E_.rows*E_.cols));
fs << "}";
fs << "fundamental_matrix" << "{";
fs << "rows" << F_.rows;
fs << "cols" << F_.cols;
fs << "data" << std::vector<double>((double*)F_.data, ((double*)F_.data)+(F_.rows*F_.cols));
fs << "}";
fs.release();
return true;
}
}
return false;
}
void StereoCameraModel::scale(double scale)
{
left_.scale(scale);
right_.scale(scale);
}
float StereoCameraModel::computeDepth(float disparity) const
{
//depth = baseline * f / (disparity + cx1-cx0);
UASSERT(this->isValid());
if(disparity == 0.0f)
{
return 0.0f;
}
return baseline() * left().fx() / (disparity + right().cx() - left().cx());
}
float StereoCameraModel::computeDisparity(float depth) const
{
// disparity = (baseline * fx / depth) - (cx1-cx0);
UASSERT(this->isValid());
if(depth == 0.0f)
{
return 0.0f;
}
return baseline() * left().fx() / depth - right().cx() + left().cx();
}
float StereoCameraModel::computeDisparity(unsigned short depth) const
{
// disparity = (baseline * fx / depth) - (cx1-cx0);
UASSERT(this->isValid());
if(depth == 0)
{
return 0.0f;
}
return baseline() * left().fx() / (float(depth)/1000.0f) - right().cx() + left().cx();
}
Transform StereoCameraModel::stereoTransform() const
{
if(!R_.empty() && !T_.empty())
{
return Transform(
R_.at<double>(0,0), R_.at<double>(0,1), R_.at<double>(0,2), T_.at<double>(0),
R_.at<double>(1,0), R_.at<double>(1,1), R_.at<double>(1,2), T_.at<double>(1),
R_.at<double>(2,0), R_.at<double>(2,1), R_.at<double>(2,2), T_.at<double>(2));
}
return Transform();
}
} /* namespace rtabmap */

122
corelib/src/StereoDense.cpp Normal file
View File

@@ -0,0 +1,122 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/StereoDense.h>
#include <rtabmap/utilite/ULogger.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
namespace rtabmap {
StereoBM::StereoBM(int blockSize, int numDisparities) :
blockSize_(blockSize),
minDisparity_(Parameters::defaultStereoBMMinDisparity()),
numDisparities_(numDisparities),
preFilterSize_(Parameters::defaultStereoBMPreFilterSize()),
preFilterCap_(Parameters::defaultStereoBMPreFilterCap()),
uniquenessRatio_(Parameters::defaultStereoBMUniquenessRatio()),
textureThreshold_(Parameters::defaultStereoBMTextureThreshold()),
speckleWindowSize_(Parameters::defaultStereoBMSpeckleWindowSize()),
speckleRange_(Parameters::defaultStereoBMSpeckleRange())
{
}
StereoBM::StereoBM(const ParametersMap & parameters) :
StereoDense(parameters),
blockSize_(Parameters::defaultStereoBMBlockSize()),
minDisparity_(Parameters::defaultStereoBMMinDisparity()),
numDisparities_(Parameters::defaultStereoBMNumDisparities()),
preFilterSize_(Parameters::defaultStereoBMPreFilterSize()),
preFilterCap_(Parameters::defaultStereoBMPreFilterCap()),
uniquenessRatio_(Parameters::defaultStereoBMUniquenessRatio()),
textureThreshold_(Parameters::defaultStereoBMTextureThreshold()),
speckleWindowSize_(Parameters::defaultStereoBMSpeckleWindowSize()),
speckleRange_(Parameters::defaultStereoBMSpeckleRange())
{
this->parseParameters(parameters);
}
void StereoBM::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kStereoBMBlockSize(), blockSize_);
Parameters::parse(parameters, Parameters::kStereoBMMinDisparity(), minDisparity_);
Parameters::parse(parameters, Parameters::kStereoBMNumDisparities(), numDisparities_);
Parameters::parse(parameters, Parameters::kStereoBMPreFilterSize(), preFilterSize_);
Parameters::parse(parameters, Parameters::kStereoBMPreFilterCap(), preFilterCap_);
Parameters::parse(parameters, Parameters::kStereoBMUniquenessRatio(), uniquenessRatio_);
Parameters::parse(parameters, Parameters::kStereoBMTextureThreshold(), textureThreshold_);
Parameters::parse(parameters, Parameters::kStereoBMPreFilterSize(), speckleWindowSize_);
Parameters::parse(parameters, Parameters::kStereoBMSpeckleRange(), speckleRange_);
}
cv::Mat StereoBM::computeDisparity(
const cv::Mat & leftImage,
const cv::Mat & rightImage) const
{
UASSERT(!leftImage.empty() && !rightImage.empty());
UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows);
UASSERT((leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1);
cv::Mat leftMono;
if(leftImage.channels() == 3)
{
cv::cvtColor(leftImage, leftMono, CV_BGR2GRAY);
}
else
{
leftMono = leftImage;
}
cv::Mat disparity;
#if CV_MAJOR_VERSION < 3
cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET);
stereo.state->SADWindowSize = blockSize_;
stereo.state->minDisparity = minDisparity_;
stereo.state->numberOfDisparities = numDisparities_;
stereo.state->preFilterSize = preFilterSize_;
stereo.state->preFilterCap = preFilterCap_;
stereo.state->uniquenessRatio = uniquenessRatio_;
stereo.state->textureThreshold = textureThreshold_;
stereo.state->speckleWindowSize = speckleWindowSize_;
stereo.state->speckleRange = speckleRange_;
stereo(leftMono, rightImage, disparity, CV_16SC1);
#else
cv::Ptr<cv::StereoBM> stereo = cv::StereoBM::create();
stereo->setBlockSize(blockSize_);
stereo->setMinDisparity(minDisparity_);
stereo->setNumDisparities(numDisparities_);
stereo->setPreFilterSize(preFilterSize_);
stereo->setPreFilterCap(preFilterCap_);
stereo->setUniquenessRatio(uniquenessRatio_);
stereo->setTextureThreshold(textureThreshold_);
stereo->setSpeckleWindowSize(speckleWindowSize_);
stereo->setSpeckleRange(speckleRange_);
stereo->compute(leftMono, rightImage, disparity);
#endif
return disparity;
}
} /* namespace rtabmap */

733
corelib/src/util2d.cpp
View File

@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2014, cv::Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
@@ -8,7 +8,7 @@ modification, are permitted provided that the following conditions are met:
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
documentation and/or other cv::Materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
@@ -29,9 +29,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UStl.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <map>
namespace rtabmap
{
@@ -39,6 +44,675 @@ namespace rtabmap
namespace util2d
{
// SSD: Sum of Squared Differences
float ssd(const cv::Mat & windowLeft, const cv::Mat & windowRight)
{
UASSERT_MSG(windowLeft.type() == CV_8UC1 || windowLeft.type() == CV_32FC1 || windowLeft.type() == CV_16SC2, uFormat("Type=%d", windowLeft.type()).c_str());
UASSERT(windowLeft.type() == windowRight.type());
UASSERT_MSG(windowLeft.rows == windowRight.rows, uFormat("%d vs %d", windowLeft.rows, windowRight.rows).c_str());
UASSERT_MSG(windowLeft.cols == windowRight.cols, uFormat("%d vs %d", windowLeft.cols, windowRight.cols).c_str());
float score = 0.0f;
for(int v=0; v<windowLeft.rows; ++v)
{
for(int u=0; u<windowLeft.cols; ++u)
{
float s = 0.0f;
if(windowLeft.type() == CV_8UC1)
{
s = float(windowLeft.at<unsigned char>(v,u))-float(windowRight.at<unsigned char>(v,u));
}
else if(windowLeft.type() == CV_32FC1)
{
s = windowLeft.at<float>(v,u)-windowRight.at<float>(v,u);
}
else if(windowLeft.type() == CV_16SC2)
{
float sL = float(windowLeft.at<cv::Vec2s>(v,u)[0])*0.5f+float(windowLeft.at<cv::Vec2s>(v,u)[1])*0.5f;
float sR = float(windowRight.at<cv::Vec2s>(v,u)[0])*0.5f+float(windowRight.at<cv::Vec2s>(v,u)[1])*0.5f;
s = sL - sR;
}
score += s*s;
}
}
return score;
}
// SAD: Sum of Absolute intensity Differences
float sad(const cv::Mat & windowLeft, const cv::Mat & windowRight)
{
UASSERT_MSG(windowLeft.type() == CV_8UC1 || windowLeft.type() == CV_32FC1 || windowLeft.type() == CV_16SC2, uFormat("Type=%d", windowLeft.type()).c_str());
UASSERT(windowLeft.type() == windowRight.type());
UASSERT_MSG(windowLeft.rows == windowRight.rows, uFormat("%d vs %d", windowLeft.rows, windowRight.rows).c_str());
UASSERT_MSG(windowLeft.cols == windowRight.cols, uFormat("%d vs %d", windowLeft.cols, windowRight.cols).c_str());
float score = 0.0f;
for(int v=0; v<windowLeft.rows; ++v)
{
for(int u=0; u<windowLeft.cols; ++u)
{
if(windowLeft.type() == CV_8UC1)
{
score += fabs(float(windowLeft.at<unsigned char>(v,u))-float(windowRight.at<unsigned char>(v,u)));
}
else if(windowLeft.type() == CV_32FC1)
{
score += fabs(windowLeft.at<float>(v,u)-windowRight.at<float>(v,u));
}
else if(windowLeft.type() == CV_16SC2)
{
float sL = float(windowLeft.at<cv::Vec2s>(v,u)[0])*0.5f+float(windowLeft.at<cv::Vec2s>(v,u)[1])*0.5f;
float sR = float(windowRight.at<cv::Vec2s>(v,u)[0])*0.5f+float(windowRight.at<cv::Vec2s>(v,u)[1])*0.5f;
score += fabs(sL - sR);
}
}
}
return score;
}
std::vector<cv::Point2f> calcStereoCorrespondences(
const cv::Mat & leftImage,
const cv::Mat & rightImage,
const std::vector<cv::Point2f> & leftCorners,
std::vector<unsigned char> & status,
cv::Size winSize,
int maxLevel,
int iterations,
int minDisparity,
int maxDisparity,
bool ssdApproach)
{
UDEBUG("winSize=(%d,%d)", winSize.width, winSize.height);
UDEBUG("maxLevel=%d", maxLevel);
UDEBUG("minDisparity=%d", minDisparity);
UDEBUG("maxDisparity=%d", maxDisparity);
UDEBUG("iterations=%d", iterations);
UDEBUG("ssdApproach=%d", ssdApproach?1:0);
// window should be odd
if(winSize.width%2 == 0)
{
winSize.width+=1;
}
if(winSize.height%2 == 0)
{
winSize.height+=1;
}
cv::Size halfWin((winSize.width-1)/2, (winSize.height-1)/2);
UTimer timer;
double pyramidTime = 0.0;
double disparityTime = 0.0;
double subpixelTime = 0.0;
std::vector<cv::Point2f> rightCorners(leftCorners.size());
std::vector<cv::Mat> leftPyramid, rightPyramid;
maxLevel = cv::buildOpticalFlowPyramid( leftImage, leftPyramid, winSize, maxLevel, false);
maxLevel = cv::buildOpticalFlowPyramid( rightImage, rightPyramid, winSize, maxLevel, false);
pyramidTime = timer.ticks();
status = std::vector<unsigned char>(leftCorners.size(), 0);
int totalIterations = 0;
int noSubPixel = 0;
for(unsigned int i=0; i<leftCorners.size(); ++i)
{
int oi=0;
float bestScore = -1.0f;
float secondBest = -1.0f;
int bestScoreIndex = -1;
int tmpMinDisparity = minDisparity;
int tmpMaxDisparity = maxDisparity;
int iterations = 0;
std::vector<float> scores;
for(int level=maxLevel; level>=0; --level)
{
UASSERT(level < (int)leftPyramid.size());
cv::Point2i center(int(leftCorners[i].x/float(1<<level)), int(leftCorners[i].y/float(1<<level)));
oi=0;
bestScore = -1.0f;
secondBest = -1.0f;
bestScoreIndex = -1;
int localMaxDisparity = -tmpMaxDisparity / (1<<level);
int localMinDisparity = -tmpMinDisparity / (1<<level);
if(center.x-halfWin.width-(level==0?1:0) >=0 && center.x+halfWin.width+(level==0?1:0) < leftPyramid[level].cols &&
center.y-halfWin.height >=0 && center.y+halfWin.height < leftPyramid[level].rows)
{
cv::Mat windowLeft(leftPyramid[level],
cv::Range(center.y-halfWin.height,center.y+halfWin.height+1),
cv::Range(center.x-halfWin.width,center.x+halfWin.width+1));
int minCol = center.x+localMaxDisparity-halfWin.width-1;
if(minCol < 0)
{
localMaxDisparity -= minCol;
}
int maxCol = center.x+localMinDisparity+halfWin.width+1;
if(maxCol >= leftPyramid[level].cols)
{
localMinDisparity += maxCol-leftPyramid[level].cols-1;
}
scores = std::vector<float>(localMinDisparity-localMaxDisparity+1, 0.0f);
for(int d=localMinDisparity; d>localMaxDisparity; --d)
{
++iterations;
cv::Mat windowRight(rightPyramid[level],
cv::Range(center.y-halfWin.height,center.y+halfWin.height+1),
cv::Range(center.x+d-halfWin.width,center.x+d+halfWin.width+1));
scores[oi] = ssdApproach?ssd(windowLeft, windowRight):sad(windowLeft, windowRight);
if(scores[oi] > 0 && (bestScore < 0.0f || scores[oi] < bestScore))
{
secondBest = bestScore;
bestScoreIndex = oi;
bestScore = scores[oi];
}
++oi;
}
if(bestScoreIndex>=0)
{
if(level>0)
{
tmpMaxDisparity = tmpMinDisparity+(bestScoreIndex+1)*(1<<level);
tmpMaxDisparity+=tmpMaxDisparity%level;
if(tmpMaxDisparity > maxDisparity)
{
tmpMaxDisparity = maxDisparity;
}
tmpMinDisparity = tmpMinDisparity+(bestScoreIndex-1)*(1<<level);
tmpMinDisparity -= tmpMinDisparity%level;
if(tmpMinDisparity < minDisparity)
{
tmpMinDisparity = minDisparity;
}
}
}
}
}
disparityTime+=timer.ticks();
totalIterations+=iterations;
if(bestScoreIndex>=0)
{
//subpixel refining
int d = -(tmpMinDisparity+bestScoreIndex);
cv::Mat windowLeft(winSize, CV_32FC1);
cv::Mat windowRight(winSize, CV_32FC1);
cv::getRectSubPix(leftPyramid[0],
winSize,
leftCorners[i],
windowLeft,
windowLeft.type());
if(leftCorners[i].x != float(int(leftCorners[i].x)))
{
//recompute bestScore if the pt is not integer
cv::getRectSubPix(rightPyramid[0],
winSize,
cv::Point2f(leftCorners[i].x+float(d), leftCorners[i].y),
windowRight,
windowRight.type());
bestScore = ssdApproach?ssd(windowLeft, windowRight):sad(windowLeft, windowRight);
}
float xc = leftCorners[i].x+float(d);
float vc = bestScore;
float step = 0.5f;
std::map<float, float> cache;
bool reject = false;
for(int it=0; it<iterations; ++it)
{
float x1 = xc-step;
float x2 = xc+step;
float v1 = uValue(cache, x1, 0.0f);
float v2 = uValue(cache, x2, 0.0f);
if(v1 == 0.0f)
{
cv::getRectSubPix(rightPyramid[0],
winSize,
cv::Point2f(x1, leftCorners[i].y),
windowRight,
windowRight.type());
v1 = ssdApproach?ssd(windowLeft, windowRight):sad(windowLeft, windowRight);
}
if(v2 == 0.0f)
{
cv::getRectSubPix(rightPyramid[0],
winSize,
cv::Point2f(x2, leftCorners[i].y),
windowRight,
windowRight.type());
v2 = ssdApproach?ssd(windowLeft, windowRight):sad(windowLeft, windowRight);
}
float previousXc = xc;
float previousVc = vc;
xc = v1<vc&&v1<v2?x1:v2<vc&&v2<v1?x2:xc;
vc = v1<vc&&v1<v2?v1:v2<vc&&v2<v1?v2:vc;
if(previousXc == xc)
{
step /= 2.0f;
}
else
{
cache.insert(std::make_pair(previousXc, previousVc));
}
if(xc < leftCorners[i].x+float(d)-1.0f || xc > leftCorners[i].x+float(d)+1.0f)
{
reject = true;
break;
}
}
if(leftCorners[i].x+float(d) == xc)
{
++noSubPixel;
}
rightCorners[i] = cv::Point2f(xc, leftCorners[i].y);
status[i] = reject?0:1;
}
subpixelTime+=timer.ticks();
}
UDEBUG("noSubPixel=%d", noSubPixel);
UDEBUG("totalIterations=%d", totalIterations);
UDEBUG("Time pyramid = %f s", pyramidTime);
UDEBUG("Time disparity = %f s", disparityTime);
UDEBUG("Time sub-pixel = %f s", subpixelTime);
return rightCorners;
}
typedef float acctype;
typedef float itemtype;
#define CV_DESCALE(x,n) (((x) + (1 << ((n)-1))) >> (n))
//
// Adapted from OpenCV cv::calcOpticalFlowPyrLK() to force
// only optical flow on x-axis (assuming that prevImg is the left
// image and nextImg is the right image):
// https://github.com/Itseez/opencv/blob/ddf82d0b154873510802ef75c53e628cd7b2cb13/modules/video/src/lkpyramid.cpp#L1088
//
// The difference is on this line:
// https://github.com/Itseez/opencv/blob/ddf82d0b154873510802ef75c53e628cd7b2cb13/modules/video/src/lkpyramid.cpp#L683-L684
// - cv::Point2f delta( (float)((A12*b2 - A22*b1) * D), (float)((A12*b1 - A11*b2) * D));
// + cv::Point2f delta( (float)((A12*b2 - A22*b1) * D), 0); //<--- note the 0 for y
//
void calcOpticalFlowPyrLKStereo( cv::InputArray _prevImg, cv::InputArray _nextImg,
cv::InputArray _prevPts, cv::InputOutputArray _nextPts,
cv::OutputArray _status, cv::OutputArray _err,
cv::Size winSize, int maxLevel,
cv::TermCriteria criteria,
int flags, double minEigThreshold )
{
cv::Mat prevPtsMat = _prevPts.getMat();
const int derivDepth = cv::DataType<short>::depth;
CV_Assert( maxLevel >= 0 && winSize.width > 2 && winSize.height > 2 );
int level=0, i, npoints;
CV_Assert( (npoints = prevPtsMat.checkVector(2, CV_32F, true)) >= 0 );
if( npoints == 0 )
{
_nextPts.release();
_status.release();
_err.release();
return;
}
if( !(flags & cv::OPTFLOW_USE_INITIAL_FLOW) )
_nextPts.create(prevPtsMat.size(), prevPtsMat.type(), -1, true);
cv::Mat nextPtsMat = _nextPts.getMat();
CV_Assert( nextPtsMat.checkVector(2, CV_32F, true) == npoints );
const cv::Point2f* prevPts = prevPtsMat.ptr<cv::Point2f>();
cv::Point2f* nextPts = nextPtsMat.ptr<cv::Point2f>();
_status.create((int)npoints, 1, CV_8U, -1, true);
cv::Mat statusMat = _status.getMat(), errMat;
CV_Assert( statusMat.isContinuous() );
uchar* status = statusMat.ptr();
float* err = 0;
for( i = 0; i < npoints; i++ )
status[i] = true;
if( _err.needed() )
{
_err.create((int)npoints, 1, CV_32F, -1, true);
errMat = _err.getMat();
CV_Assert( errMat.isContinuous() );
err = errMat.ptr<float>();
}
std::vector<cv::Mat> prevPyr, nextPyr;
int levels1 = -1;
int lvlStep1 = 1;
int levels2 = -1;
int lvlStep2 = 1;
if(_prevImg.kind() != cv::_InputArray::STD_VECTOR_MAT)
{
//create pyramid
maxLevel = cv::buildOpticalFlowPyramid(_prevImg, prevPyr, winSize, maxLevel, true);
}
else if(_prevImg.kind() == cv::_InputArray::STD_VECTOR_MAT)
{
_prevImg.getMatVector(prevPyr);
}
levels1 = int(prevPyr.size()) - 1;
CV_Assert(levels1 >= 0);
if (levels1 % 2 == 1 && prevPyr[0].channels() * 2 == prevPyr[1].channels() && prevPyr[1].depth() == derivDepth)
{
lvlStep1 = 2;
levels1 /= 2;
}
// ensure that pyramid has required padding
if(levels1 > 0)
{
cv::Size fullSize;
cv::Point ofs;
prevPyr[lvlStep1].locateROI(fullSize, ofs);
CV_Assert(ofs.x >= winSize.width && ofs.y >= winSize.height
&& ofs.x + prevPyr[lvlStep1].cols + winSize.width <= fullSize.width
&& ofs.y + prevPyr[lvlStep1].rows + winSize.height <= fullSize.height);
}
if(levels1 < maxLevel)
maxLevel = levels1;
if(_nextImg.kind() != cv::_InputArray::STD_VECTOR_MAT)
{
//create pyramid
maxLevel = cv::buildOpticalFlowPyramid(_nextImg, nextPyr, winSize, maxLevel, false);
}
else if(_nextImg.kind() == cv::_InputArray::STD_VECTOR_MAT)
{
_nextImg.getMatVector(nextPyr);
}
levels2 = int(nextPyr.size()) - 1;
CV_Assert(levels2 >= 0);
if (levels2 % 2 == 1 && nextPyr[0].channels() * 2 == nextPyr[1].channels() && nextPyr[1].depth() == derivDepth)
{
lvlStep2 = 2;
levels2 /= 2;
}
// ensure that pyramid has required padding
if(levels2 > 0)
{
cv::Size fullSize;
cv::Point ofs;
nextPyr[lvlStep2].locateROI(fullSize, ofs);
CV_Assert(ofs.x >= winSize.width && ofs.y >= winSize.height
&& ofs.x + nextPyr[lvlStep2].cols + winSize.width <= fullSize.width
&& ofs.y + nextPyr[lvlStep2].rows + winSize.height <= fullSize.height);
}
if(levels2 < maxLevel)
maxLevel = levels2;
if( (criteria.type & cv::TermCriteria::COUNT) == 0 )
criteria.maxCount = 30;
else
criteria.maxCount = std::min(std::max(criteria.maxCount, 0), 100);
if( (criteria.type & cv::TermCriteria::EPS) == 0 )
criteria.epsilon = 0.01;
else
criteria.epsilon = std::min(std::max(criteria.epsilon, 0.), 10.);
criteria.epsilon *= criteria.epsilon;
// for all pyramids
for( level = maxLevel; level >= 0; level-- )
{
cv::Mat derivI = prevPyr[level * lvlStep1 + 1];
CV_Assert(prevPyr[level * lvlStep1].size() == nextPyr[level * lvlStep2].size());
CV_Assert(prevPyr[level * lvlStep1].type() == nextPyr[level * lvlStep2].type());
const cv::Mat & prevImg = prevPyr[level * lvlStep1];
const cv::Mat & prevDeriv = derivI;
const cv::Mat & nextImg = nextPyr[level * lvlStep2];
// for all corners
{
cv::Point2f halfWin((winSize.width-1)*0.5f, (winSize.height-1)*0.5f);
const cv::Mat& I = prevImg;
const cv::Mat& J = nextImg;
const cv::Mat& derivI = prevDeriv;
int j, cn = I.channels(), cn2 = cn*2;
cv::AutoBuffer<short> _buf(winSize.area()*(cn + cn2));
int derivDepth = cv::DataType<short>::depth;
cv::Mat IWinBuf(winSize, CV_MAKETYPE(derivDepth, cn), (short*)_buf);
cv::Mat derivIWinBuf(winSize, CV_MAKETYPE(derivDepth, cn2), (short*)_buf + winSize.area()*cn);
for( int ptidx = 0; ptidx < npoints; ptidx++ )
{
cv::Point2f prevPt = prevPts[ptidx]*(float)(1./(1 << level));
cv::Point2f nextPt;
if( level == maxLevel )
{
if( flags & cv::OPTFLOW_USE_INITIAL_FLOW )
nextPt = nextPts[ptidx]*(float)(1./(1 << level));
else
nextPt = prevPt;
}
else
nextPt = nextPts[ptidx]*2.f;
nextPts[ptidx] = nextPt;
cv::Point2i iprevPt, inextPt;
prevPt -= halfWin;
iprevPt.x = cvFloor(prevPt.x);
iprevPt.y = cvFloor(prevPt.y);
if( iprevPt.x < -winSize.width || iprevPt.x >= derivI.cols ||
iprevPt.y < -winSize.height || iprevPt.y >= derivI.rows )
{
if( level == 0 )
{
if( status )
status[ptidx] = false;
if( err )
err[ptidx] = 0;
}
continue;
}
float a = prevPt.x - iprevPt.x;
float b = prevPt.y - iprevPt.y;
const int W_BITS = 14, W_BITS1 = 14;
const float FLT_SCALE = 1.f/(1 << 20);
int iw00 = cvRound((1.f - a)*(1.f - b)*(1 << W_BITS));
int iw01 = cvRound(a*(1.f - b)*(1 << W_BITS));
int iw10 = cvRound((1.f - a)*b*(1 << W_BITS));
int iw11 = (1 << W_BITS) - iw00 - iw01 - iw10;
int dstep = (int)(derivI.step/derivI.elemSize1());
int stepI = (int)(I.step/I.elemSize1());
int stepJ = (int)(J.step/J.elemSize1());
acctype iA11 = 0, iA12 = 0, iA22 = 0;
float A11, A12, A22;
// extract the patch from the first image, compute covariation cv::Matrix of derivatives
int x, y;
for( y = 0; y < winSize.height; y++ )
{
const uchar* src = I.ptr() + (y + iprevPt.y)*stepI + iprevPt.x*cn;
const short* dsrc = derivI.ptr<short>() + (y + iprevPt.y)*dstep + iprevPt.x*cn2;
short* Iptr = IWinBuf.ptr<short>(y);
short* dIptr = derivIWinBuf.ptr<short>(y);
x = 0;
for( ; x < winSize.width*cn; x++, dsrc += 2, dIptr += 2 )
{
int ival = CV_DESCALE(src[x]*iw00 + src[x+cn]*iw01 +
src[x+stepI]*iw10 + src[x+stepI+cn]*iw11, W_BITS1-5);
int ixval = CV_DESCALE(dsrc[0]*iw00 + dsrc[cn2]*iw01 +
dsrc[dstep]*iw10 + dsrc[dstep+cn2]*iw11, W_BITS1);
int iyval = CV_DESCALE(dsrc[1]*iw00 + dsrc[cn2+1]*iw01 + dsrc[dstep+1]*iw10 +
dsrc[dstep+cn2+1]*iw11, W_BITS1);
Iptr[x] = (short)ival;
dIptr[0] = (short)ixval;
dIptr[1] = (short)iyval;
iA11 += (itemtype)(ixval*ixval);
iA12 += (itemtype)(ixval*iyval);
iA22 += (itemtype)(iyval*iyval);
}
}
A11 = iA11*FLT_SCALE;
A12 = iA12*FLT_SCALE;
A22 = iA22*FLT_SCALE;
float D = A11*A22 - A12*A12;
float minEig = (A22 + A11 - std::sqrt((A11-A22)*(A11-A22) +
4.f*A12*A12))/(2*winSize.width*winSize.height);
if( err && (flags & cv::OPTFLOW_LK_GET_MIN_EIGENVALS) != 0 )
err[ptidx] = (float)minEig;
if( minEig < minEigThreshold || D < FLT_EPSILON )
{
if( level == 0 && status )
status[ptidx] = false;
continue;
}
D = 1.f/D;
nextPt -= halfWin;
cv::Point2f prevDelta;
for( j = 0; j < criteria.maxCount; j++ )
{
inextPt.x = cvFloor(nextPt.x);
inextPt.y = cvFloor(nextPt.y);
if( inextPt.x < -winSize.width || inextPt.x >= J.cols ||
inextPt.y < -winSize.height || inextPt.y >= J.rows )
{
if( level == 0 && status )
status[ptidx] = false;
break;
}
a = nextPt.x - inextPt.x;
b = nextPt.y - inextPt.y;
iw00 = cvRound((1.f - a)*(1.f - b)*(1 << W_BITS));
iw01 = cvRound(a*(1.f - b)*(1 << W_BITS));
iw10 = cvRound((1.f - a)*b*(1 << W_BITS));
iw11 = (1 << W_BITS) - iw00 - iw01 - iw10;
acctype ib1 = 0, ib2 = 0;
float b1, b2;
for( y = 0; y < winSize.height; y++ )
{
const uchar* Jptr = J.ptr() + (y + inextPt.y)*stepJ + inextPt.x*cn;
const short* Iptr = IWinBuf.ptr<short>(y);
const short* dIptr = derivIWinBuf.ptr<short>(y);
x = 0;
for( ; x < winSize.width*cn; x++, dIptr += 2 )
{
int diff = CV_DESCALE(Jptr[x]*iw00 + Jptr[x+cn]*iw01 +
Jptr[x+stepJ]*iw10 + Jptr[x+stepJ+cn]*iw11,
W_BITS1-5) - Iptr[x];
ib1 += (itemtype)(diff*dIptr[0]);
ib2 += (itemtype)(diff*dIptr[1]);
}
}
b1 = ib1*FLT_SCALE;
b2 = ib2*FLT_SCALE;
cv::Point2f delta( (float)((A12*b2 - A22*b1) * D),
0);//(float)((A12*b1 - A11*b2) * D)); // MODIFICATION
//delta = -delta;
nextPt += delta;
nextPts[ptidx] = nextPt + halfWin;
if( delta.ddot(delta) <= criteria.epsilon )
break;
if( j > 0 && std::abs(delta.x + prevDelta.x) < 0.01 &&
std::abs(delta.y + prevDelta.y) < 0.01 )
{
nextPts[ptidx] -= delta*0.5f;
break;
}
prevDelta = delta;
}
if( status[ptidx] && err && level == 0 && (flags & cv::OPTFLOW_LK_GET_MIN_EIGENVALS) == 0 )
{
cv::Point2f nextPoint = nextPts[ptidx] - halfWin;
cv::Point inextPoint;
inextPoint.x = cvFloor(nextPoint.x);
inextPoint.y = cvFloor(nextPoint.y);
if( inextPoint.x < -winSize.width || inextPoint.x >= J.cols ||
inextPoint.y < -winSize.height || inextPoint.y >= J.rows )
{
if( status )
status[ptidx] = false;
continue;
}
float aa = nextPoint.x - inextPoint.x;
float bb = nextPoint.y - inextPoint.y;
iw00 = cvRound((1.f - aa)*(1.f - bb)*(1 << W_BITS));
iw01 = cvRound(aa*(1.f - bb)*(1 << W_BITS));
iw10 = cvRound((1.f - aa)*bb*(1 << W_BITS));
iw11 = (1 << W_BITS) - iw00 - iw01 - iw10;
float errval = 0.f;
for( y = 0; y < winSize.height; y++ )
{
const uchar* Jptr = J.ptr() + (y + inextPoint.y)*stepJ + inextPoint.x*cn;
const short* Iptr = IWinBuf.ptr<short>(y);
for( x = 0; x < winSize.width*cn; x++ )
{
int diff = CV_DESCALE(Jptr[x]*iw00 + Jptr[x+cn]*iw01 +
Jptr[x+stepJ]*iw10 + Jptr[x+stepJ+cn]*iw11,
W_BITS1-5) - Iptr[x];
errval += std::abs((float)diff);
}
}
err[ptidx] = errval * 1.f/(32*winSize.width*cn*winSize.height);
}
}
}
}
}
cv::Mat disparityFromStereoImages(
const cv::Mat & leftImage,
@@ -88,40 +762,6 @@ cv::Mat disparityFromStereoImages(
return disparity;
}
cv::Mat disparityFromStereoImages(
const cv::Mat & leftImage,
const cv::Mat & rightImage,
const std::vector<cv::Point2f> & leftCorners,
int flowWinSize,
int flowMaxLevel,
int flowIterations,
double flowEps,
float maxCorrespondencesSlope)
{
UASSERT(!leftImage.empty() && !rightImage.empty());
UASSERT(leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1);
UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows);
// Find features in the new left image
std::vector<unsigned char> status;
std::vector<float> err;
std::vector<cv::Point2f> rightCorners;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
cv::calcOpticalFlowPyrLK(
leftImage,
rightImage,
leftCorners,
rightCorners,
status,
err,
cv::Size(flowWinSize, flowWinSize), flowMaxLevel,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations, flowEps),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
return disparityFromStereoCorrespondences(leftImage, leftCorners, rightCorners, status, maxCorrespondencesSlope);
}
cv::Mat depthFromDisparity(const cv::Mat & disparity,
float fx, float baseline,
int type)
@@ -205,25 +845,22 @@ cv::Mat depthFromStereoImages(
}
cv::Mat disparityFromStereoCorrespondences(
const cv::Mat & leftImage,
const cv::Size & disparitySize,
const std::vector<cv::Point2f> & leftCorners,
const std::vector<cv::Point2f> & rightCorners,
const std::vector<unsigned char> & mask,
float maxSlope)
const std::vector<unsigned char> & mask)
{
UASSERT(!leftImage.empty() && leftCorners.size() == rightCorners.size());
UASSERT(leftCorners.size() == rightCorners.size());
UASSERT(mask.size() == 0 || mask.size() == leftCorners.size());
cv::Mat disparity = cv::Mat::zeros(leftImage.rows, leftImage.cols, CV_32FC1);
cv::Mat disparity = cv::Mat::zeros(disparitySize, CV_32FC1);
for(unsigned int i=0; i<leftCorners.size(); ++i)
{
if(mask.size() == 0 || mask[i])
if(mask.empty() || mask[i])
{
float d = leftCorners[i].x - rightCorners[i].x;
float slope = fabs((leftCorners[i].y - rightCorners[i].y) / (leftCorners[i].x - rightCorners[i].x));
if(d > 0.0f && (maxSlope <= 0 || fabs(leftCorners[i].y-rightCorners[i].y) <= 1.0f || slope <= maxSlope))
{
disparity.at<float>(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f)) = d;
}
cv::Point2i dispPt(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f));
UASSERT(dispPt.x >= 0 && dispPt.x < disparitySize.width);
UASSERT(dispPt.y >= 0 && dispPt.y < disparitySize.height);
disparity.at<float>(dispPt.y, dispPt.x) = leftCorners[i].x - rightCorners[i].x;
}
}
return disparity;

47
corelib/src/util3d.cpp
View File

@@ -358,8 +358,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
const cv::Mat & imageDisparity,
float cx, float cy,
float fx, float baseline,
const StereoCameraModel & model,
int decimation)
{
UASSERT(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1);
@@ -382,7 +381,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
for(int w = 0; w < imageDisparity.cols && w/decimation < (int)cloud->width; w+=decimation)
{
float disp = float(imageDisparity.at<short>(h,w))/16.0f;
cloud->at((h/decimation)*cloud->width + (w/decimation)) = projectDisparityTo3D(cv::Point2f(w, h), disp, cx, cy, fx, baseline);
cloud->at((h/decimation)*cloud->width + (w/decimation)) = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
}
}
}
@@ -393,7 +392,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
for(int w = 0; w < imageDisparity.cols && w/decimation < (int)cloud->width; w+=decimation)
{
float disp = imageDisparity.at<float>(h,w);
cloud->at((h/decimation)*cloud->width + (w/decimation)) = projectDisparityTo3D(cv::Point2f(w, h), disp, cx, cy, fx, baseline);
cloud->at((h/decimation)*cloud->width + (w/decimation)) = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
}
}
}
@@ -403,8 +402,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
const cv::Mat & imageRgb,
const cv::Mat & imageDisparity,
float cx, float cy,
float fx, float baseline,
const StereoCameraModel & model,
int decimation)
{
UASSERT(!imageRgb.empty() && !imageDisparity.empty());
@@ -453,7 +451,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
}
float disp = imageDisparity.type()==CV_16SC1?float(imageDisparity.at<short>(h,w))/16.0f:imageDisparity.at<float>(h,w);
pcl::PointXYZ ptXYZ = projectDisparityTo3D(cv::Point2f(w, h), disp, cx, cy, fx, baseline);
pcl::PointXYZ ptXYZ = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
pt.x = ptXYZ.x;
pt.y = ptXYZ.y;
pt.z = ptXYZ.z;
@@ -465,8 +463,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
const cv::Mat & imageLeft,
const cv::Mat & imageRight,
float cx, float cy,
float fx, float baseline,
const StereoCameraModel & model,
int decimation)
{
UASSERT(!imageLeft.empty() && !imageRight.empty());
@@ -479,14 +476,14 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
cv::Mat leftColor = imageLeft;
cv::Mat rightMono = imageRight;
StereoCameraModel modelDecimation = model;
if(leftColor.rows % decimation != 0 ||
leftColor.cols % decimation != 0)
{
leftColor = util2d::decimate(leftColor, decimation);
rightMono = util2d::decimate(rightMono, decimation);
fx /= float(decimation);
cx /= float(decimation);
cy /= float(decimation);
modelDecimation.scale(1/float(decimation));
decimation = 1;
}
@@ -503,8 +500,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
return cloudFromDisparityRGB(
leftColor,
util2d::disparityFromStereoImages(leftMono, rightMono),
cx, cy,
fx, baseline,
modelDecimation,
decimation);
}
@@ -595,10 +591,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
}
cloud = cloudFromDisparity(
util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw()),
sensorData.stereoCameraModel().left().cx(),
sensorData.stereoCameraModel().left().cy(),
sensorData.stereoCameraModel().left().fx(),
sensorData.stereoCameraModel().baseline(),
sensorData.stereoCameraModel(),
decimation);
if(cloud->size())
@@ -720,10 +713,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
UDEBUG("");
cloud = cloudFromStereoImages(sensorData.imageRaw(),
sensorData.rightRaw(),
sensorData.stereoCameraModel().left().cx(),
sensorData.stereoCameraModel().left().cy(),
sensorData.stereoCameraModel().left().fx(),
sensorData.stereoCameraModel().baseline(),
sensorData.stereoCameraModel(),
decimation);
if(cloud->size())
@@ -871,12 +861,13 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserS
pcl::PointXYZ projectDisparityTo3D(
const cv::Point2f & pt,
float disparity,
float cx, float cy, float fx, float baseline)
const StereoCameraModel & model)
{
if(disparity > 0.0f && baseline > 0.0f && fx > 0.0f)
if(disparity != 0.0f && model.baseline() > 0.0f && model.left().fx() > 0.0f)
{
float W = disparity/baseline;// + (right_.cx() - left_.cx()) / Tx;
return pcl::PointXYZ((pt.x - cx)/W, (pt.y - cy)/W, fx/W);
//Z = baseline * f / (d + cx1-cx0);
float W = model.baseline()/(disparity + model.right().cx() - model.left().cx());
return pcl::PointXYZ((pt.x - model.left().cx())*W, (pt.y - model.left().cy())*W, model.left().fx()*W);
}
float bad_point = std::numeric_limits<float>::quiet_NaN ();
return pcl::PointXYZ(bad_point, bad_point, bad_point);
@@ -885,7 +876,7 @@ pcl::PointXYZ projectDisparityTo3D(
pcl::PointXYZ projectDisparityTo3D(
const cv::Point2f & pt,
const cv::Mat & disparity,
float cx, float cy, float fx, float baseline)
const StereoCameraModel & model)
{
UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
int u = int(pt.x+0.5f);
@@ -895,7 +886,7 @@ pcl::PointXYZ projectDisparityTo3D(
uIsInBounds(v, 0, disparity.rows))
{
float d = disparity.type() == CV_16SC1?float(disparity.at<short>(v,u))/16.0f:disparity.at<float>(v,u);
return projectDisparityTo3D(pt, d, cx, cy, fx, baseline);
return projectDisparityTo3D(pt, d, model);
}
return pcl::PointXYZ(bad_point, bad_point, bad_point);
}

98
corelib/src/util3d_features.cpp
View File

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_features.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_correspondences.h"
@@ -111,10 +112,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDisparity(
pcl::PointXYZ pt = util3d::projectDisparityTo3D(
keypoints[i].pt,
disparity,
stereoCameraModel.left().cx(),
stereoCameraModel.left().cy(),
stereoCameraModel.left().fx(),
stereoCameraModel.baseline());
stereoCameraModel);
if(pcl::isFinite(pt) &&
!stereoCameraModel.left().localTransform().isNull() &&
@@ -127,105 +125,39 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDisparity(
return keypoints3d;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & leftImage,
const cv::Mat & rightImage,
float fx,
float baseline,
float cx,
float cy,
Transform localTransform,
int flowWinSize,
int flowMaxLevel,
int flowIterations,
double flowEps,
double maxCorrespondencesSlope)
{
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(keypoints, leftCorners);
return generateKeypoints3DStereo(
leftCorners,
leftImage,
rightImage,
fx,
baseline,
cx,
cy,
localTransform,
flowWinSize,
flowMaxLevel,
flowIterations,
flowEps,
maxCorrespondencesSlope);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
const std::vector<cv::Point2f> & leftCorners,
const cv::Mat & leftImage,
const cv::Mat & rightImage,
float fx,
float baseline,
float cx,
float cy,
Transform localTransform,
int flowWinSize,
int flowMaxLevel,
int flowIterations,
double flowEps,
double maxCorrespondencesSlope)
const std::vector<cv::Point2f> & rightCorners,
const StereoCameraModel & model,
const std::vector<unsigned char> & mask)
{
UASSERT(!leftImage.empty() && !rightImage.empty() &&
leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
leftImage.rows == rightImage.rows && leftImage.cols == rightImage.cols);
UASSERT(fx > 0.0f && baseline > 0.0f);
// Find features in the new left image
std::vector<unsigned char> status;
std::vector<float> err;
std::vector<cv::Point2f> rightCorners;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
cv::calcOpticalFlowPyrLK(
leftImage,
rightImage,
leftCorners,
rightCorners,
status,
err,
cv::Size(flowWinSize, flowWinSize), flowMaxLevel,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations, flowEps),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
UASSERT(leftCorners.size() == rightCorners.size());
UASSERT(mask.size() == 0 || leftCorners.size() == mask.size());
UASSERT(model.left().fx()> 0.0f && model.baseline() > 0.0f);
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
keypoints3d->resize(leftCorners.size());
float bad_point = std::numeric_limits<float>::quiet_NaN ();
UASSERT(status.size() == leftCorners.size());
for(unsigned int i=0; i<status.size(); ++i)
for(unsigned int i=0; i<leftCorners.size(); ++i)
{
pcl::PointXYZ pt(bad_point, bad_point, bad_point);
if(status[i])
if(mask.empty() || mask[i])
{
float disparity = leftCorners[i].x - rightCorners[i].x;
float slope = fabs((leftCorners[i].y-rightCorners[i].y) / (leftCorners[i].x-rightCorners[i].x));
if(disparity > 0.0f &&
(maxCorrespondencesSlope <=0 || fabs(leftCorners[i].y-rightCorners[i].y) <= 1.0f || slope <= maxCorrespondencesSlope))
if(disparity != 0.0f)
{
pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
leftCorners[i],
disparity,
cx,
cy,
fx,
baseline);
model);
if(pcl::isFinite(tmpPt))
{
pt = tmpPt;
if(!localTransform.isNull() &&
!localTransform.isIdentity())
if(!model.localTransform().isNull() &&
!model.localTransform().isIdentity())
{
pt = util3d::transformPoint(pt, localTransform);
pt = util3d::transformPoint(pt, model.localTransform());
}
}
}