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

Refactoring: removed some code duplication about transformation estimation and features (2D-3D) extraction.

Browse Source 
Added Feature2D::generateKeypoints3D() for convenience.
Added parameter "Vis/PnPOpenCV2".
Added parameter "Vis/ForwardEstOnly".
Removed OdometryOpticalFlow class, replaced by OdometryF2F (frame-to-frame). To get the same previous OpticalFLow approach, parameter "Vis/CorType" should be set to 1.
Some parameters under group "OdomFlow/..." are now under "Vis/CorFlow...".
In Registration class, add computeTransformationMod() method to modify input signatures.
Added constructor Signature(SensorData) for convenience.
Modified words multimap used with cv::Point3f instead of pcl::PointXYZ to limit the use of PCL headers where they are not really required.
Added Stereo::create() for convenience.
Transform: fixed quaternion constructor where data_ was not initialized. Added parentheses operator for convenience.
DatabaseViewer: loading .rtabmap/rtabmap.ini instead of .rtabmap/dbViewer.ini when used from rtabmap application. Added vertical layout option for convenience.
MainWindow: fixed wrong Odometry speed values
ParametersToolBox: using QStackedWidget instead of a QToolBox for space, added "Restore Defaults" button.
This commit is contained in:
matlabbe
2015-12-21 17:27:05 -05:00
parent 3292fb1146
commit 2e9634cf65
54 changed files with 2838 additions and 2725 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/EpipolarGeometry.h
+10
View File
@@ -86,6 +86,16 @@ public:
static cv::Mat findFFromCalibratedStereoCameras(double fx, double fy, double cx, double cy, double Tx, double Ty);
/**
* if a=[1 2 3 4 6], b=[1 2 4 5 6], results= [(1,1) (2,2) (4,4) (6,6)]
* realPairsCount = 4
*/
static int findPairs(
const std::map<int, cv::KeyPoint> & wordsA,
const std::map<int, cv::KeyPoint> & wordsB,
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs);
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
* realPairsCount = 5
corelib/include/rtabmap/core/Features2d.h
+16 -7
View File
@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/features2d/features2d.hpp>
#include <list>
#include "rtabmap/core/Parameters.h"
#include "rtabmap/core/SensorData.h"
#if CV_MAJOR_VERSION < 3
namespace cv{
@@ -87,6 +88,8 @@ typedef cv::cuda::FastFeatureDetector CV_FAST_GPU;
namespace rtabmap {
class Stereo;
// Feature2D
class RTABMAP_EXP Feature2D {
public:
@@ -104,7 +107,6 @@ public:
static Feature2D * create(const ParametersMap & parameters = ParametersMap());
static Feature2D * create(Feature2D::Type type, const ParametersMap & parameters = ParametersMap()); // for convenience
static Feature2D * create(Feature2D::Type type, int wordsPerImage, const ParametersMap & parameters = ParametersMap()); // for convenience
static void filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
@@ -137,10 +139,13 @@ public:
int getMaxFeatures() const {return maxFeatures_;}
public:
virtual ~Feature2D() {}
virtual ~Feature2D();
std::vector<cv::KeyPoint> generateKeypoints(const cv::Mat & image, const cv::Rect & roi = cv::Rect()) const;
std::vector<cv::KeyPoint> generateKeypoints(const cv::Mat & image) const;
cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
std::vector<cv::Point3f> generateKeypoints3D(
const SensorData & data,
const std::vector<cv::KeyPoint> & keypoints) const;
virtual void parseParameters(const ParametersMap & parameters);
virtual Feature2D::Type getType() const = 0;
@@ -154,6 +159,14 @@ private:
private:
int maxFeatures_;
float _wordsMaxDepth; // 0=inf
float _wordsMinDepth;
std::vector<float> _roiRatios; // size 4
int _subPixWinSize;
int _subPixIterations;
double _subPixEps;
// Stereo stuff
Stereo * _stereo;
};
//SURF
@@ -198,7 +211,6 @@ private:
virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
private:
int nfeatures_;
int nOctaveLayers_;
double contrastThreshold_;
double edgeThreshold_;
@@ -222,7 +234,6 @@ private:
virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
private:
int nFeatures_;
float scaleFactor_;
int nLevels_;
int edgeThreshold_;
@@ -258,7 +269,6 @@ private:
double gpuKeypointsRatio_;
int minThreshold_;
int maxThreshold_;
int maxTotalKeypoints_;
int gridRows_;
int gridCols_;
@@ -337,7 +347,6 @@ private:
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const;
private:
int _maxCorners;
double _qualityLevel;
double _minDistance;
int _blockSize;
corelib/include/rtabmap/core/Memory.h
+1 -16
View File
@@ -42,7 +42,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/utilite/UStl.h"
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <pcl/point_types.h>
namespace rtabmap {
@@ -147,7 +146,7 @@ public:
SensorData getNodeData(int nodeId, bool uncompressedData = false, bool keepLoadedDataInMemory = true);
void getNodeWords(int nodeId,
std::multimap<int, cv::KeyPoint> & words,
std::multimap<int, pcl::PointXYZ> & words3);
std::multimap<int, cv::Point3f> & words3);
SensorData getSignatureDataConst(int locationId) const;
std::set<int> getAllSignatureIds() const;
bool memoryChanged() const {return _memoryChanged;}
@@ -161,8 +160,6 @@ public:
const Feature2D * getFeature2D() const {return _feature2D;}
bool isGraphReduced() const {return _reduceGraph;}
void setRoi(const std::string & roi);
void dumpMemoryTree(const char * fileNameTree) const;
virtual void dumpMemory(std::string directory) const;
virtual void dumpSignatures(const char * fileNameSign, bool words3D) const;
@@ -172,7 +169,6 @@ public:
//keypoint stuff
const VWDictionary * getVWDictionary() const;
Feature2D::Type getFeatureType() const {return _featureType;}
// RGB-D stuff
void getMetricConstraints(
@@ -262,23 +258,12 @@ private:
//Keypoint stuff
VWDictionary * _vwd;
Feature2D * _feature2D;
Feature2D::Type _featureType;
float _badSignRatio;;
bool _tfIdfLikelihoodUsed;
bool _parallelized;
float _wordsMaxDepth; // 0=inf
float _wordsMinDepth;
std::vector<float> _roiRatios; // size 4
RegistrationVis * _registrationVis;
RegistrationIcp * _registrationIcp;
// Stereo stuff
Stereo * _stereo;
int _subPixWinSize;
int _subPixIterations;
double _subPixEps;
};
} // namespace rtabmap
corelib/include/rtabmap/core/Odometry.h
+17 -26
View File
@@ -33,6 +33,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/SensorData.h>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/Signature.h>
#include <rtabmap/core/RegistrationVis.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
@@ -59,11 +61,13 @@ public:
float getInlierDistance() const {return _inlierDistance;}
int getIterations() const {return _iterations;}
int getRefineIterations() const {return _refineIterations;}
float getMinDepth() const {return _minDepth;}
float getMaxDepth() const {return _maxDepth;}
bool isInfoDataFilled() const {return _fillInfoData;}
int getEstimationType() const {return _estimationType;}
double getPnPReprojError() const {return _pnpReprojError;}
int getPnPFlags() const {return _pnpFlags;}
bool getPnPOpenCV2() const {return _pnpOpenCV2;}
const Transform & previousTransform() const {return previousTransform_;}
bool isVarianceFromInliersCount() const {return _varianceFromInliersCount;}
@@ -79,6 +83,7 @@ private:
float _inlierDistance;
int _iterations;
int _refineIterations;
float _minDepth;
float _maxDepth;
int _resetCountdown;
bool _force2D;
@@ -93,6 +98,7 @@ private:
int _estimationType;
double _pnpReprojError;
int _pnpFlags;
bool _pnpOpenCV2;
bool _varianceFromInliersCount;
float _kalmanProcessNoise;
float _kalmanMeasurementNoise;
@@ -119,7 +125,7 @@ public:
virtual ~OdometryBOW();
virtual void reset(const Transform & initialPose = Transform::getIdentity());
const std::map<int, pcl::PointXYZ> & getLocalMap() const {return localMap_;}
const std::map<int, cv::Point3f> & getLocalMap() const {return localMap_;}
const Memory * getMemory() const {return _memory;}
private:
@@ -131,20 +137,18 @@ private:
std::string _fixedLocalMapPath;
Memory * _memory;
std::map<int, pcl::PointXYZ> localMap_;
std::map<int, cv::Point3f> localMap_;
};
class RTABMAP_EXP OdometryOpticalFlow : public Odometry
class RTABMAP_EXP OdometryF2F : public Odometry
{
public:
OdometryOpticalFlow(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
virtual ~OdometryOpticalFlow();
OdometryF2F(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
virtual ~OdometryF2F();
virtual void reset(const Transform & initialPose = Transform::getIdentity());
const cv::Mat & getLastFrame() const {return refFrame_;}
const std::vector<cv::Point2f> & getLastCorners() const {return refCorners_;}
const pcl::PointCloud<pcl::PointXYZ>::Ptr & getLastCorners3D() const {return refCorners3D_;}
const Signature & getRefFrame() const {return refFrame_;}
private:
virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);
@@ -152,27 +156,14 @@ private:
private:
//Parameters:
int keyFrameThr_;
int flowWinSize_;
int flowIterations_;
double flowEps_;
int flowMaxLevel_;
bool flowGuessFromMotion_;
Stereo * stereo_;
int subPixWinSize_;
int subPixIterations_;
double subPixEps_;
Feature2D * feature2D_;
cv::Mat refFrame_;
std::vector<cv::Point2f> refCorners_;
pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3D_;
bool guessFromMotion_;
RegistrationVis registration_;
Signature refFrame_;
Transform motionSinceLastKeyFrame_;
};
class RTABMAP_EXP OdometryMono : public Odometry
{
public:
@@ -202,7 +193,7 @@ private:
cv::Mat refDepthOrRight_;
std::map<int, cv::Point2f> cornersMap_;
std::map<int, cv::Point3f> localMap_;
std::map<int, std::multimap<int, pcl::PointXYZ> > keyFrameWords3D_;
std::map<int, std::map<int, cv::Point3f> > keyFrameWords3D_;
std::map<int, Transform> keyFramePoses_;
float maxVariance_;
};
corelib/include/rtabmap/core/OdometryInfo.h
+3 -3
View File
@@ -64,15 +64,15 @@ public:
Transform transformGroundTruth;
float distanceTravelled;
int type; // 0=BOW, 1=Optical Flow, 2=ICP
int type; // 0=BOW, 1=F2F, 2=ICP, 3=Mono
// BOW odometry
// BOW
std::multimap<int, cv::KeyPoint> words;
std::vector<int> wordMatches;
std::vector<int> wordInliers;
std::map<int, cv::Point3f> localMap;
// Optical Flow odometry
// F2F && Mono
std::vector<cv::Point2f> refCorners;
std::vector<cv::Point2f> newCorners;
std::vector<int> cornerInliers;
corelib/include/rtabmap/core/Parameters.h
+11 -8
View File
@@ -214,7 +214,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Kp, IncrementalFlann, bool, true, "When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary doubles in size).");
RTABMAP_PARAM(Kp, MaxDepth, float, 0.0, "Filter extracted keypoints by depth (0=inf).");
RTABMAP_PARAM(Kp, MinDepth, float, 0.0, "Filter extracted keypoints by depth.");
RTABMAP_PARAM(Kp, WordsPerImage, int, 400, "Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
RTABMAP_PARAM(Kp, MaxFeatures, int, 400, "Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
RTABMAP_PARAM(Kp, BadSignRatio, float, 0.5, "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
RTABMAP_PARAM_COND(Kp, NndrRatio, float, RTABMAP_NONFREE, 0.8, 0.9, "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
RTABMAP_PARAM_COND(Kp, DetectorStrategy, int, RTABMAP_NONFREE, 0, 2, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
@@ -357,10 +357,6 @@ class RTABMAP_EXP Parameters
// 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
@@ -368,16 +364,16 @@ class RTABMAP_EXP Parameters
// Visual registration parameters
RTABMAP_PARAM(Vis, EstimationType, int, 0, "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
RTABMAP_PARAM(Vis, ForwardEstOnly, bool, true, "Forward estimation (A->B). If false, a transformation is also computed in backward direction (B->A), then the two resulting transforms are merged (middle interpolation between the transforms).");
RTABMAP_PARAM(Vis, InlierDistance, float, 0.1, "[Vis/EstimationType = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.");
RTABMAP_PARAM(Vis, RefineIterations, int, 10, "[Vis/EstimationType = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
RTABMAP_PARAM(Vis, PnPReprojError, double, 5.0, "[Vis/EstimationType = 1] PnP reprojection error.");
RTABMAP_PARAM(Vis, PnPFlags, int, 1, "[Vis/EstimationType = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
RTABMAP_PARAM(Vis, PnPFlags, int, 1, "[Vis/EstimationType = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
RTABMAP_PARAM(Vis, PnPOpenCV2, bool, true, "[Vis/EstimationType = 1] Use OpenCV2 solvePnPRansac() in OpenCV3.");
RTABMAP_PARAM(Vis, EpipolarGeometryVar, float, 0.02, "[Vis/EstimationType = 2] Epipolar geometry maximum variance to accept the transformation.");
RTABMAP_PARAM(Vis, MinInliers, int, 10, "Minimum feature correspondences to compute/accept the transformation.");
RTABMAP_PARAM(Vis, Iterations, int, 100, "Maximum iterations to compute the transform.");
RTABMAP_PARAM(Vis, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw). Parameters z, roll and pitch will be set to 0.");
RTABMAP_PARAM(Vis, NNType, int, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4.");
RTABMAP_PARAM(Vis, NNDR, float, 0.8, "NNDR: nearest neighbor distance ratio.");
RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
RTABMAP_PARAM(Vis, MaxFeatures, int, 1000, "0 no limits.");
RTABMAP_PARAM(Vis, MaxDepth, float, 0.0, "Max depth of the features (0 means no limit).");
@@ -386,6 +382,13 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Vis, SubPixWinSize, int, 3, "See cv::cornerSubPix().");
RTABMAP_PARAM(Vis, SubPixIterations, int, 0, "See cv::cornerSubPix(). 0 disables sub pixel refining.");
RTABMAP_PARAM(Vis, SubPixEps, double, 0.02, "See cv::cornerSubPix().");
RTABMAP_PARAM(Vis, CorType, int, 0, "Correspondences computation approach: 0=Features Matching, 1=Optical Flow");
RTABMAP_PARAM(Vis, CorNNType, int, 3, "[Vis/CorrespondenceType=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach.");
RTABMAP_PARAM(Vis, CorNNDR, float, 0.8, "[Vis/CorrespondenceType=0] NNDR: nearest neighbor distance ratio. Used for features matching approach.");
RTABMAP_PARAM(Vis, CorFlowWinSize, int, 16, "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
RTABMAP_PARAM(Vis, CorFlowIterations, int, 30, "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
RTABMAP_PARAM(Vis, CorFlowEps, double, 0.01, "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
RTABMAP_PARAM(Vis, CorFlowMaxLevel, int, 3, "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
// ICP registration parameters
corelib/include/rtabmap/core/Registration.h
+16 -15
View File
@@ -41,29 +41,30 @@ public:
{
Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), _varianceFromInliersCount);
}
virtual Transform computeTransformation(
Transform computeTransformation(
const Signature & from,
const Signature & to,
Transform guess = Transform::getIdentity(),
std::string * rejectedMsg = 0,
int * inliersOut = 0,
std::vector<int> * inliersOut = 0,
float * varianceOut = 0,
float * inliersRatioOut = 0) = 0;
Transform computeTransformation2(
const SensorData & fromData,
const SensorData & toData,
Transform guess = Transform::getIdentity(), // guess is ignored for RegistrationVis
std::string * rejectedMsg = 0,
int * inliersOut = 0,
float * varianceOut = 0,
float * inliersRatioOut = 0)
float * inliersRatioOut = 0) const
{
Signature fromSignature(fromData.id(), -1, 0, 0.0, "", Transform::getIdentity(), fromData);
Signature toSignature(toData.id(), -1, 0, 0.0, "", Transform::getIdentity(), toData);
return computeTransformation(fromSignature, toSignature, guess, rejectedMsg, inliersOut, varianceOut, inliersRatioOut);
Signature fromCopy(from);
Signature toCopy(to);
return computeTransformationMod(fromCopy, toCopy, guess, rejectedMsg, inliersOut, varianceOut, inliersRatioOut);
}
virtual Transform computeTransformationMod(
Signature & from,
Signature & to,
Transform guess = Transform::getIdentity(),
std::string * rejectedMsg = 0,
std::vector<int> * inliersOut = 0,
float * varianceOut = 0,
float * inliersRatioOut = 0) const = 0;
protected:
Registration(const ParametersMap & parameters = ParametersMap()) :
_varianceFromInliersCount(Parameters::defaultRegVarianceFromInliersCount())
corelib/include/rtabmap/core/RegistrationIcp.h
+7 -7
View File
@@ -42,23 +42,23 @@ public:
virtual void parseParameters(const ParametersMap & parameters);
virtual Transform computeTransformation(
const Signature & from,
const Signature & to,
virtual Transform computeTransformationMod(
Signature & from,
Signature & to,
Transform guess = Transform::getIdentity(),
std::string * rejectedMsg = 0,
int * inliersOut = 0,
std::vector<int> * inliersOut = 0,
float * varianceOut = 0,
float * inliersRatioOut = 0);
float * inliersRatioOut = 0) const;
Transform computeTransformation(
const SensorData & from,
const SensorData & to,
Transform guess = Transform::getIdentity(),
std::string * rejectedMsg = 0,
int * inliersOut = 0,
std::vector<int> * inliersOut = 0,
float * varianceOut = 0,
float * inliersRatioOut = 0);
float * inliersRatioOut = 0) const;
private:
float _maxTranslation;
corelib/include/rtabmap/core/RegistrationVis.h
+12 -5
View File
@@ -42,14 +42,14 @@ public:
virtual void parseParameters(const ParametersMap & parameters);
virtual Transform computeTransformation(
const Signature & from,
const Signature & to,
virtual Transform computeTransformationMod(
Signature & from,
Signature & to,
Transform guess = Transform::getIdentity(), // guess is ignored for RegistrationVis
std::string * rejectedMsg = 0,
int * inliersOut = 0,
std::vector<int> * inliersOut = 0,
float * varianceOut = 0,
float * inliersRatioOut = 0);
float * inliersRatioOut = 0) const;
float getBowInlierDistance() const {return _inlierDistance;}
int getBowIterations() const {return _iterations;}
@@ -64,8 +64,15 @@ private:
bool _force2D;
float _epipolarGeometryVar;
int _estimationType;
bool _forwardEstimateOnly;
double _PnPReprojError;
int _PnPFlags;
bool _PnPOpenCV2;
int _correspondencesApproach;
int _flowWinSize;
int _flowIterations;
float _flowEps;
int _flowMaxLevel;
ParametersMap _featureParameters;
};
corelib/include/rtabmap/core/Signature.h
+4 -3
View File
@@ -57,6 +57,7 @@ public:
const std::string & label = std::string(),
const Transform & pose = Transform(),
const SensorData & sensorData = SensorData());
Signature(const SensorData & data);
virtual ~Signature();
/**
@@ -107,10 +108,10 @@ public:
const std::map<int, int> & getWordsChanged() const {return _wordsChanged;}
//metric stuff
void setWords3(const std::multimap<int, pcl::PointXYZ> & words3) {_words3 = words3;}
void setWords3(const std::multimap<int, cv::Point3f> & words3) {_words3 = words3;}
void setPose(const Transform & pose) {_pose = pose;}
const std::multimap<int, pcl::PointXYZ> & getWords3() const {return _words3;}
const std::multimap<int, cv::Point3f> & getWords3() const {return _words3;}
const Transform & getPose() const {return _pose;}
cv::Mat getPoseCovariance() const;
@@ -132,7 +133,7 @@ private:
// times in the signature, it will be 2 times in this list)
// Words match with the CvSeq keypoints and descriptors
std::multimap<int, cv::KeyPoint> _words; // word <id, keypoint>
std::multimap<int, pcl::PointXYZ> _words3; // word <id, keypoint> // in base_link frame (localTransform applied))
std::multimap<int, cv::Point3f> _words3; // word <id, keypoint> // in base_link frame (localTransform applied))
std::map<int, int> _wordsChanged; // <oldId, newId>
bool _enabled;
corelib/include/rtabmap/core/Stereo.h
+3
View File
@@ -36,6 +36,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
class RTABMAP_EXP Stereo {
public:
static Stereo * create(const ParametersMap & parameters = ParametersMap());
public:
Stereo(const ParametersMap & parameters = ParametersMap());
virtual ~Stereo() {}
corelib/include/rtabmap/core/Transform.h
+2
View File
@@ -72,6 +72,8 @@ public:
float & operator[](int index) {return data()[index];}
const float & operator[](int index) const {return data()[index];}
float & operator()(int row, int col) {return data()[row*4 + col];}
const float & operator()(int row, int col) const {return data()[row*4 + col];}
bool isNull() const;
bool isIdentity() const;
corelib/include/rtabmap/core/VWDictionary.h
+1 -1
View File
@@ -91,7 +91,7 @@ public:
void exportDictionary(const char * fileNameReferences, const char * fileNameDescriptors) const;
void clear();
void clear(bool printWarningsIfNotEmpty = true);
std::vector<VisualWord *> getUnusedWords() const;
std::vector<int> getUnusedWordIds() const;
unsigned int getUnusedWordsSize() const {return (int)_unusedWords.size();}
corelib/include/rtabmap/core/util3d.h
+9 -2
View File
@@ -130,16 +130,18 @@ cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ>
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform = Transform());
pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
cv::Point3f RTABMAP_EXP projectDisparityTo3D(
const cv::Point2f & pt,
float disparity,
const StereoCameraModel & model);
pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
cv::Point3f RTABMAP_EXP projectDisparityTo3D(
const cv::Point2f & pt,
const cv::Mat & disparity,
const StereoCameraModel & model);
bool RTABMAP_EXP isFinite(const cv::Point3f & pt);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(
const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP concatenateClouds(
@@ -176,6 +178,11 @@ void RTABMAP_EXP savePCDWords(
const std::multimap<int, pcl::PointXYZ> & words,
const Transform & transform = Transform::getIdentity());
void RTABMAP_EXP savePCDWords(
const std::string & fileName,
const std::multimap<int, cv::Point3f> & words,
const Transform & transform = Transform::getIdentity());
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP loadBINCloud(const std::string & fileName, int dim);
} // namespace util3d
corelib/include/rtabmap/core/util3d_correspondences.h
+8 -8
View File
@@ -50,18 +50,18 @@ void RTABMAP_EXP findCorrespondences(
std::list<std::pair<cv::Point2f, cv::Point2f> > & pairs);
void RTABMAP_EXP findCorrespondences(
const std::multimap<int, pcl::PointXYZ> & words1,
const std::multimap<int, pcl::PointXYZ> & words2,
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
const std::multimap<int, cv::Point3f> & words1,
const std::multimap<int, cv::Point3f> & words2,
std::vector<cv::Point3f> & inliers1,
std::vector<cv::Point3f> & inliers2,
float maxDepth,
std::vector<int> * uniqueCorrespondences = 0);
void RTABMAP_EXP findCorrespondences(
const std::map<int, pcl::PointXYZ> & words1,
const std::map<int, pcl::PointXYZ> & words2,
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
const std::map<int, cv::Point3f> & words1,
const std::map<int, cv::Point3f> & words2,
std::vector<cv::Point3f> & inliers1,
std::vector<cv::Point3f> & inliers2,
float maxDepth,
std::vector<int> * correspondences = 0);
corelib/include/rtabmap/core/util3d_features.h
+9 -10
View File
@@ -30,8 +30,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/RtabmapExp.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/CameraModel.h>
@@ -46,38 +44,39 @@ namespace util3d
{
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDepth(
std::vector<cv::Point3f> RTABMAP_EXP generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
const CameraModel & cameraModel);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDepth(
std::vector<cv::Point3f> RTABMAP_EXP generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
const std::vector<CameraModel> & cameraModels);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDisparity(
std::vector<cv::Point3f> RTABMAP_EXP generateKeypoints3DDisparity(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & disparity,
const StereoCameraModel & stereoCameraMode);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
std::vector<cv::Point3f> RTABMAP_EXP generateKeypoints3DStereo(
const std::vector<cv::Point2f> & leftCorners,
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,
const std::multimap<int, cv::KeyPoint> & previousKpts,
std::map<int, cv::Point3f> RTABMAP_EXP generateWords3DMono(
const std::map<int, cv::KeyPoint> & kpts,
const std::map<int, cv::KeyPoint> & previousKpts,
const CameraModel & cameraModel,
Transform & cameraTransform,
int pnpIterations = 100,
float pnpReprojError = 8.0f,
int pnpFlags = 0, // cv::SOLVEPNP_ITERATIVE
bool pnpOpenCV2 = true,
float ransacParam1 = 3.0f,
float ransacParam2 = 0.99f,
const std::multimap<int, pcl::PointXYZ> & refGuess3D = std::multimap<int, pcl::PointXYZ>(),
const std::map<int, cv::Point3f> & refGuess3D = std::map<int, cv::Point3f>(),
double * variance = 0);
std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
corelib/include/rtabmap/core/util3d_motion_estimation.h
+20 -6
View File
@@ -30,8 +30,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/RtabmapExp.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/CameraModel.h>
@@ -42,22 +40,23 @@ namespace util3d
{
Transform RTABMAP_EXP estimateMotion3DTo2D(
const std::map<int, pcl::PointXYZ> & words3A,
const std::map<int, cv::Point3f> & words3A,
const std::map<int, cv::KeyPoint> & words2B,
const CameraModel & cameraModel,
int minInliers = 10,
int iterations = 100,
double reprojError = 5.,
int flagsPnP = 0,
bool pnpOpenCV2 = true,
const Transform & guess = Transform::getIdentity(),
const std::map<int, pcl::PointXYZ> & words3B = std::map<int, pcl::PointXYZ>(),
const std::map<int, cv::Point3f> & words3B = std::map<int, cv::Point3f>(),
double * varianceOut = 0, // mean reproj error if words3B is not set
std::vector<int> * matchesOut = 0,
std::vector<int> * inliersOut = 0);
Transform RTABMAP_EXP estimateMotion3DTo3D(
const std::map<int, pcl::PointXYZ> & words3A,
const std::map<int, pcl::PointXYZ> & words3B,
const std::map<int, cv::Point3f> & words3A,
const std::map<int, cv::Point3f> & words3B,
int minInliers = 10,
double inliersDistance = 0.1,
int iterations = 100,
@@ -66,6 +65,21 @@ Transform RTABMAP_EXP estimateMotion3DTo3D(
std::vector<int> * matchesOut = 0,
std::vector<int> * inliersOut = 0);
void RTABMAP_EXP solvePnPRansac(
cv::InputArray _opoints,
cv::InputArray _ipoints,
cv::InputArray _cameraMatrix,
cv::InputArray _distCoeffs,
cv::OutputArray _rvec,
cv::OutputArray _tvec,
bool useExtrinsicGuess,
int iterationsCount,
float reprojectionError,
int minInliersCount,
cv::OutputArray _inliers,
int flags,
bool opencv2version);
} // namespace util3d
} // namespace rtabmap
corelib/include/rtabmap/core/util3d_transforms.h
+3
View File
@@ -53,6 +53,9 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const Transform & transform);
cv::Point3f RTABMAP_EXP transformPoint(
const cv::Point3f & pt,
const Transform & transform);
pcl::PointXYZ RTABMAP_EXP transformPoint(
const pcl::PointXYZ & pt,
const Transform & transform);
corelib/src/CMakeLists.txt
+1 -1
View File
@@ -56,9 +56,9 @@ SET(SRC_FILES
Odometry.cpp
OdometryThread.cpp
OdometryBOW.cpp
OdometryOpticalFlow.cpp
OdometryMono.cpp
OdometryICP.cpp
OdometryF2F.cpp
Stereo.cpp
StereoDense.cpp
corelib/src/DBDriverSqlite3.cpp
+5 -5
View File
@@ -1529,8 +1529,8 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
int visualWordId = 0;
cv::KeyPoint kpt;
std::multimap<int, cv::KeyPoint> visualWords;
std::multimap<int, pcl::PointXYZ> visualWords3;
pcl::PointXYZ depth(0,0,0);
std::multimap<int, cv::Point3f> visualWords3;
cv::Point3f depth(0,0,0);
// Process the result if one
rc = sqlite3_step(ppStmt);
@@ -2305,7 +2305,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
if((*i)->getWords3().size())
{
std::multimap<int, cv::KeyPoint>::const_iterator w=(*i)->getWords().begin();
std::multimap<int, pcl::PointXYZ>::const_iterator p=(*i)->getWords3().begin();
std::multimap<int, cv::Point3f>::const_iterator p=(*i)->getWords3().begin();
for(; w!=(*i)->getWords().end(); ++w, ++p)
{
UASSERT(w->first == p->first); // must be same id!
@@ -2316,7 +2316,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
{
for(std::multimap<int, cv::KeyPoint>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
{
stepKeypoint(ppStmt, (*i)->id(), w->first, w->second, pcl::PointXYZ(0,0,0));
stepKeypoint(ppStmt, (*i)->id(), w->first, w->second, cv::Point3f(0,0,0));
}
}
}
@@ -3008,7 +3008,7 @@ std::string DBDriverSqlite3::queryStepKeypoint() const
{
return "INSERT INTO Map_Node_Word(node_id, word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z) VALUES(?,?,?,?,?,?,?,?,?,?);";
}
void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt, int nodeId, int wordId, const cv::KeyPoint & kp, const pcl::PointXYZ & pt) const
void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt, int nodeId, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt) const
{
if(!ppStmt)
{
corelib/src/DBDriverSqlite3.h
+1 -2
View File
@@ -32,7 +32,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/DBDriver.h"
#include <opencv2/features2d/features2d.hpp>
#include "sqlite3/sqlite3.h"
#include <pcl/point_types.h>
namespace rtabmap {
@@ -110,7 +109,7 @@ private:
void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const pcl::PointXYZ & pt) const;
void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt) const;
private:
void loadLinksQuery(std::list<Signature *> & signatures) const;
corelib/src/EpipolarGeometry.cpp
+23
View File
@@ -406,6 +406,29 @@ cv::Mat EpipolarGeometry::findFFromCalibratedStereoCameras(double fx, double fy,
return K.inv().t()*E*K.inv();
}
/**
* if a=[1 2 3 4 6], b=[1 2 4 5 6], results= [(1,1) (2,2) (4,4) (6,6)]
* realPairsCount = 4
*/
int EpipolarGeometry::findPairs(
const std::map<int, cv::KeyPoint> & wordsA,
const std::map<int, cv::KeyPoint> & wordsB,
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs)
{
int realPairsCount = 0;
pairs.clear();
for(std::map<int, cv::KeyPoint>::const_iterator i=wordsA.begin(); i!=wordsA.end(); ++i)
{
std::map<int, cv::KeyPoint>::const_iterator ptB = wordsB.find(i->first);
if(ptB != wordsB.end())
{
pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(i->first, std::pair<cv::KeyPoint, cv::KeyPoint>(i->second, ptB->second)));
++realPairsCount;
}
}
return realPairsCount;
}
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
* realPairsCount = 5
corelib/src/Features2d.cpp
+202 -53
View File
@@ -27,6 +27,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Features2d.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_features.h"
#include "rtabmap/core/Stereo.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/ULogger.h"
@@ -243,7 +245,7 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat &
}
}
}
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, (int)keypoints.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, (int)kptsTmp.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
keypoints = kptsTmp;
if(descriptors.rows)
@@ -335,13 +337,74 @@ cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::vector<float> &
// Feature2D
/////////////////////
Feature2D::Feature2D(const ParametersMap & parameters) :
maxFeatures_(Parameters::defaultKpWordsPerImage())
maxFeatures_(Parameters::defaultKpMaxFeatures()),
_wordsMaxDepth(Parameters::defaultKpMaxDepth()),
_wordsMinDepth(Parameters::defaultKpMinDepth()),
_roiRatios(std::vector<float>(4, 0.0f)),
_subPixWinSize(Parameters::defaultKpSubPixWinSize()),
_subPixIterations(Parameters::defaultKpSubPixIterations()),
_subPixEps(Parameters::defaultKpSubPixEps())
{
_stereo = new Stereo(parameters);
this->parseParameters(parameters);
}
Feature2D::~Feature2D()
{
delete _stereo;
}
void Feature2D::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kKpWordsPerImage(), maxFeatures_);
Parameters::parse(parameters, Parameters::kKpMaxFeatures(), maxFeatures_);
Parameters::parse(parameters, Parameters::kKpMaxDepth(), _wordsMaxDepth);
Parameters::parse(parameters, Parameters::kKpMinDepth(), _wordsMinDepth);
Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), _subPixWinSize);
Parameters::parse(parameters, Parameters::kKpSubPixIterations(), _subPixIterations);
Parameters::parse(parameters, Parameters::kKpSubPixEps(), _subPixEps);
// convert ROI from string to vector
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kKpRoiRatios())) != parameters.end())
{
std::list<std::string> strValues = uSplit(iter->second, ' ');
if(strValues.size() != 4)
{
ULOGGER_ERROR("The number of values must be 4 (roi=\"%s\")", iter->second.c_str());
}
else
{
std::vector<float> tmpValues(4);
unsigned int i=0;
for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
{
tmpValues[i] = uStr2Float(*iter);
++i;
}
if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
{
_roiRatios = tmpValues;
}
else
{
ULOGGER_ERROR("The roi ratios are not valid (roi=\"%s\")", iter->second.c_str());
}
}
}
//stereo
UASSERT(_stereo != 0);
if((iter=parameters.find(Parameters::kStereoOpticalFlow())) != parameters.end())
{
delete _stereo;
_stereo = Stereo::create(parameters);
}
else
{
_stereo->parseParameters(parameters);
}
}
Feature2D * Feature2D::create(const ParametersMap & parameters)
{
@@ -350,12 +413,6 @@ Feature2D * Feature2D::create(const ParametersMap & parameters)
return create((Feature2D::Type)type, parameters);
}
Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parameters)
{
int wordsPerImage = Parameters::defaultKpWordsPerImage();
Parameters::parse(parameters, Parameters::kKpWordsPerImage(), wordsPerImage);
return create(type, wordsPerImage, parameters);
}
Feature2D * Feature2D::create(Feature2D::Type type, int wordsPerImage, const ParametersMap & parameters)
{
if(RTABMAP_NONFREE == 0)
{
@@ -426,54 +483,156 @@ Feature2D * Feature2D::create(Feature2D::Type type, int wordsPerImage, const Par
#endif
}
return feature2D;
}
std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image) const
{
UASSERT(!image.empty());
UASSERT(image.type() == CV_8UC1);
std::vector<cv::KeyPoint> keypoints;
if(!image.empty() && image.channels() == 1 && image.type() == CV_8U)
UTimer timer;
// Get keypoints
cv::Rect roi = Feature2D::computeRoi(image, _roiRatios);
keypoints = this->generateKeypointsImpl(image, roi.width && roi.height?roi:cv::Rect(0,0,image.cols, image.rows));
UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d", timer.ticks(), keypoints.size());
limitKeypoints(keypoints, maxFeatures_);
if(roi.x || roi.y)
{
UTimer timer;
// Get keypoints
keypoints = this->generateKeypointsImpl(image, roi.width && roi.height?roi:cv::Rect(0,0,image.cols, image.rows));
ULOGGER_DEBUG("Keypoints extraction time = %f s, keypoints extracted = %d", timer.ticks(), keypoints.size());
limitKeypoints(keypoints, maxFeatures_);
if(roi.x || roi.y)
// Adjust keypoint position to raw image
for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
{
// Adjust keypoint position to raw image
for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
{
iter->pt.x += roi.x;
iter->pt.y += roi.y;
}
iter->pt.x += roi.x;
iter->pt.y += roi.y;
}
}
else if(image.empty())
if(_subPixWinSize > 0 && _subPixIterations > 0)
{
UERROR("Image is null!");
}
else
{
UERROR("Image format must be mono8. Current has %d channels and type = %d, size=%d,%d",
image.channels(), image.type(), image.cols, image.rows);
std::vector<cv::Point2f> corners;
cv::KeyPoint::convert(keypoints, corners);
cv::cornerSubPix( image, corners,
cv::Size( _subPixWinSize, _subPixWinSize ),
cv::Size( -1, -1 ),
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, _subPixIterations, _subPixEps ) );
for(unsigned int i=0;i<corners.size(); ++i)
{
keypoints[i].pt = corners[i];
}
UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d", timer.ticks(), keypoints.size());
}
return keypoints;
}
cv::Mat Feature2D::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
cv::Mat Feature2D::generateDescriptors(
const cv::Mat & image,
std::vector<cv::KeyPoint> & keypoints) const
{
UASSERT(!image.empty());
UASSERT(image.type() == CV_8UC1);
cv::Mat descriptors = generateDescriptorsImpl(image, keypoints);
UASSERT_MSG(descriptors.rows == (int)keypoints.size(), uFormat("descriptors=%d, keypoints=%d", descriptors.rows, (int)keypoints.size()).c_str());
UDEBUG("Descriptors extracted = %d, remaining kpts=%d", descriptors.rows, (int)keypoints.size());
return descriptors;
}
std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
const SensorData & data,
const std::vector<cv::KeyPoint> & keypoints) const
{
std::vector<cv::Point3f> keypoints3D;
if(!data.depthOrRightRaw().empty() && !data.imageRaw().empty() && data.stereoCameraModel().isValid())
{
//stereo
cv::Mat imageMono;
// convert to grayscale
if(data.imageRaw().channels() > 1)
{
cv::cvtColor(data.imageRaw(), imageMono, cv::COLOR_BGR2GRAY);
}
else
{
imageMono = data.imageRaw();
}
//generate a disparity map
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(keypoints, leftCorners);
std::vector<unsigned char> status;
std::vector<cv::Point2f> rightCorners;
rightCorners = _stereo->computeCorrespondences(
imageMono,
data.rightRaw(),
leftCorners,
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;
}
}
}
}
keypoints3D = util3d::generateKeypoints3DStereo(
leftCorners,
rightCorners,
data.stereoCameraModel(),
status);
}
else if(!data.depthRaw().empty() && data.cameraModels().size())
{
keypoints3D = util3d::generateKeypoints3DDepth(
keypoints,
data.depthOrRightRaw(),
data.cameraModels());
if(_wordsMaxDepth > 0.0f || _wordsMinDepth > 0.0f)
{
UASSERT(keypoints3D.size() == keypoints.size());
bool isInMM = data.depthRaw().type() == CV_16UC1;
float bad_point = std::numeric_limits<float>::quiet_NaN ();
for(unsigned int i=0; i<keypoints.size(); ++i)
{
int u = int(keypoints[i].pt.x+0.5f);
int v = int(keypoints[i].pt.y+0.5f);
bool reject = true;
if(u >=0 && u<data.depthRaw().cols && v >=0 && v<data.depthRaw().rows)
{
float d = isInMM?(float)data.depthRaw().at<uint16_t>(v,u)*0.001f:data.depthRaw().at<float>(v,u);
if(uIsFinite(d) && d>_wordsMinDepth && (_wordsMaxDepth <= 0.0f || d < _wordsMaxDepth))
{
reject = false;
}
}
if(reject)
{
keypoints3D[i].x = bad_point;
keypoints3D[i].y = bad_point;
keypoints3D[i].z = bad_point;
}
}
}
}
return keypoints3D;
}
//////////////////////////
//SURF
//////////////////////////
@@ -605,7 +764,6 @@ cv::Mat SURF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
//SIFT
//////////////////////////
SIFT::SIFT(const ParametersMap & parameters) :
nfeatures_(Parameters::defaultSIFTNFeatures()),
nOctaveLayers_(Parameters::defaultSIFTNOctaveLayers()),
contrastThreshold_(Parameters::defaultSIFTContrastThreshold()),
edgeThreshold_(Parameters::defaultSIFTEdgeThreshold()),
@@ -624,15 +782,14 @@ void SIFT::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kSIFTContrastThreshold(), contrastThreshold_);
Parameters::parse(parameters, Parameters::kSIFTEdgeThreshold(), edgeThreshold_);
Parameters::parse(parameters, Parameters::kSIFTNFeatures(), nfeatures_);
Parameters::parse(parameters, Parameters::kSIFTNOctaveLayers(), nOctaveLayers_);
Parameters::parse(parameters, Parameters::kSIFTSigma(), sigma_);
#if RTABMAP_NONFREE == 1
#if CV_MAJOR_VERSION < 3
_sift = cv::Ptr<CV_SIFT>(new CV_SIFT(nfeatures_, nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_));
_sift = cv::Ptr<CV_SIFT>(new CV_SIFT(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_));
#else
_sift = CV_SIFT::create(nfeatures_, nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_);
_sift = CV_SIFT::create(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_);
#endif
#else
UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
@@ -668,7 +825,6 @@ cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
//ORB
//////////////////////////
ORB::ORB(const ParametersMap & parameters) :
nFeatures_(Parameters::defaultKpWordsPerImage()),
scaleFactor_(Parameters::defaultORBScaleFactor()),
nLevels_(Parameters::defaultORBNLevels()),
edgeThreshold_(Parameters::defaultORBEdgeThreshold()),
@@ -691,7 +847,6 @@ void ORB::parseParameters(const ParametersMap & parameters)
{
Feature2D::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kKpWordsPerImage(), nFeatures_);
Parameters::parse(parameters, Parameters::kORBScaleFactor(), scaleFactor_);
Parameters::parse(parameters, Parameters::kORBNLevels(), nLevels_);
Parameters::parse(parameters, Parameters::kORBEdgeThreshold(), edgeThreshold_);
@@ -727,7 +882,7 @@ void ORB::parseParameters(const ParametersMap & parameters)
if(gpu_)
{
#if CV_MAJOR_VERSION < 3
_gpuOrb = cv::Ptr<CV_ORB_GPU>(new CV_ORB_GPU(nFeatures_, scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_));
_gpuOrb = cv::Ptr<CV_ORB_GPU>(new CV_ORB_GPU(this->getMaxFeatures(), scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_));
_gpuOrb->setFastParams(fastThreshold_, nonmaxSuppresion_);
#else
#ifdef HAVE_OPENCV_CUDAFEATURES2D
@@ -738,9 +893,9 @@ void ORB::parseParameters(const ParametersMap & parameters)
else
{
#if CV_MAJOR_VERSION < 3
_orb = cv::Ptr<CV_ORB>(new CV_ORB(nFeatures_, scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_));
_orb = cv::Ptr<CV_ORB>(new CV_ORB(this->getMaxFeatures(), scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_));
#else
_orb = CV_ORB::create(nFeatures_, scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_);
_orb = CV_ORB::create(this->getMaxFeatures(), scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_);
#endif
}
}
@@ -821,7 +976,6 @@ FAST::FAST(const ParametersMap & parameters) :
gpuKeypointsRatio_(Parameters::defaultFASTGpuKeypointsRatio()),
minThreshold_(Parameters::defaultFASTMinThreshold()),
maxThreshold_(Parameters::defaultFASTMaxThreshold()),
maxTotalKeypoints_(Parameters::defaultKpWordsPerImage()),
gridRows_(Parameters::defaultFASTGridRows()),
gridCols_(Parameters::defaultFASTGridCols())
{
@@ -843,12 +997,9 @@ void FAST::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kFASTMinThreshold(), minThreshold_);
Parameters::parse(parameters, Parameters::kFASTMaxThreshold(), maxThreshold_);
Parameters::parse(parameters, Parameters::kKpWordsPerImage(), maxTotalKeypoints_);
Parameters::parse(parameters, Parameters::kFASTGridRows(), gridRows_);
Parameters::parse(parameters, Parameters::kFASTGridCols(), gridCols_);
UWARN("minThreshold_=%d", minThreshold_);
UASSERT_MSG(threshold_ >= minThreshold_, uFormat("%d vs %d", threshold_, minThreshold_).c_str());
UASSERT_MSG(threshold_ <= maxThreshold_, uFormat("%d vs %d", threshold_, maxThreshold_).c_str());
@@ -893,7 +1044,7 @@ void FAST::parseParameters(const ParametersMap & parameters)
if(gridRows_ > 0 && gridCols_ > 0)
{
cv::Ptr<cv::FeatureDetector> fastAdjuster = cv::Ptr<cv::FastAdjuster>(new cv::FastAdjuster(threshold_, nonmaxSuppression_, minThreshold_, maxThreshold_));
_fast = cv::Ptr<cv::FeatureDetector>(new cv::GridAdaptedFeatureDetector(fastAdjuster, maxTotalKeypoints_, gridRows_, gridCols_));
_fast = cv::Ptr<cv::FeatureDetector>(new cv::GridAdaptedFeatureDetector(fastAdjuster, this->getMaxFeatures(), gridRows_, gridCols_));
}
else
{
@@ -1067,7 +1218,6 @@ cv::Mat FAST_ORB::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv:
//GFTT
//////////////////////////
GFTT::GFTT(const ParametersMap & parameters) :
_maxCorners(Parameters::defaultKpWordsPerImage()),
_qualityLevel(Parameters::defaultGFTTQualityLevel()),
_minDistance(Parameters::defaultGFTTMinDistance()),
_blockSize(Parameters::defaultGFTTBlockSize()),
@@ -1085,7 +1235,6 @@ void GFTT::parseParameters(const ParametersMap & parameters)
{
Feature2D::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kKpWordsPerImage(), _maxCorners);
Parameters::parse(parameters, Parameters::kGFTTQualityLevel(), _qualityLevel);
Parameters::parse(parameters, Parameters::kGFTTMinDistance(), _minDistance);
Parameters::parse(parameters, Parameters::kGFTTBlockSize(), _blockSize);
@@ -1093,9 +1242,9 @@ void GFTT::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kGFTTK(), _k);
#if CV_MAJOR_VERSION < 3
_gftt = cv::Ptr<CV_GFTT>(new CV_GFTT(_maxCorners, _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k));
_gftt = cv::Ptr<CV_GFTT>(new CV_GFTT(this->getMaxFeatures(), _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k));
#else
_gftt = CV_GFTT::create(_maxCorners, _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k);
_gftt = CV_GFTT::create(this->getMaxFeatures(), _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k);
#endif
}
corelib/src/Memory.cpp
+57 -378
View File
@@ -96,24 +96,14 @@ Memory::Memory(const ParametersMap & parameters) :
_linksChanged(false),
_signaturesAdded(0),
_featureType((Feature2D::Type)Parameters::defaultKpDetectorStrategy()),
_badSignRatio(Parameters::defaultKpBadSignRatio()),
_tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()),
_parallelized(Parameters::defaultKpParallelized()),
_wordsMaxDepth(Parameters::defaultKpMaxDepth()),
_wordsMinDepth(Parameters::defaultKpMinDepth()),
_roiRatios(std::vector<float>(4, 0.0f)),
_subPixWinSize(Parameters::defaultKpSubPixWinSize()),
_subPixIterations(Parameters::defaultKpSubPixIterations()),
_subPixEps(Parameters::defaultKpSubPixEps())
_parallelized(Parameters::defaultKpParallelized())
{
_feature2D = Feature2D::create(_featureType, parameters);
_featureType = _feature2D->getType();
_feature2D = Feature2D::create(parameters);
_vwd = new VWDictionary(parameters);
_registrationVis = new RegistrationVis(parameters);
_registrationIcp = new RegistrationIcp(parameters);
_stereo = new Stereo(parameters);
this->parseParameters(parameters);
}
@@ -383,10 +373,6 @@ Memory::~Memory()
{
delete _registrationIcp;
}
if(_stereo)
{
delete _stereo;
}
}
void Memory::parseParameters(const ParametersMap & parameters)
@@ -435,17 +421,6 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kKpTfIdfLikelihoodUsed(), _tfIdfLikelihoodUsed);
Parameters::parse(parameters, Parameters::kKpParallelized(), _parallelized);
Parameters::parse(parameters, Parameters::kKpBadSignRatio(), _badSignRatio);
Parameters::parse(parameters, Parameters::kKpMaxDepth(), _wordsMaxDepth);
Parameters::parse(parameters, Parameters::kKpMinDepth(), _wordsMinDepth);
Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), _subPixWinSize);
Parameters::parse(parameters, Parameters::kKpSubPixIterations(), _subPixIterations);
Parameters::parse(parameters, Parameters::kKpSubPixEps(), _subPixEps);
if((iter=parameters.find(Parameters::kKpRoiRatios())) != parameters.end())
{
this->setRoi((*iter).second);
}
//Keypoint detector
UASSERT(_feature2D != 0);
@@ -461,11 +436,9 @@ void Memory::parseParameters(const ParametersMap & parameters)
{
delete _feature2D;
_feature2D = 0;
_featureType = Feature2D::kFeatureUndef;
}
_feature2D = Feature2D::create(detectorStrategy, parameters);
_featureType = _feature2D->getType();
}
else if(_feature2D)
{
@@ -480,26 +453,6 @@ void Memory::parseParameters(const ParametersMap & parameters)
{
_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
@@ -654,39 +607,6 @@ bool Memory::update(
return true;
}
void Memory::setRoi(const std::string & roi)
{
std::list<std::string> strValues = uSplit(roi, ' ');
if(strValues.size() != 4)
{
ULOGGER_ERROR("The number of values must be 4 (roi=\"%s\")", roi.c_str());
}
else
{
std::vector<float> tmpValues(4);
unsigned int i=0;
for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
{
tmpValues[i] = uStr2Float(*iter);
++i;
}
if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
{
_roiRatios = tmpValues;
}
else
{
ULOGGER_ERROR("The roi ratios are not valid (roi=\"%s\")", roi.c_str());
}
}
}
void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance)
{
UTimer timer;
@@ -2105,6 +2025,7 @@ Transform Memory::computeVisualTransform(
if(fromS && toS)
{
// compute transform fromId -> toId
std::vector<int> inliersV;
if(_reextractLoopClosureFeatures)
{
getNodeData(fromS->id(), true);
@@ -2114,14 +2035,18 @@ Transform Memory::computeVisualTransform(
Signature tmpTo = *toS;
tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
tmpFrom.setWords3(std::multimap<int, pcl::PointXYZ>());
tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
tmpTo.setWords3(std::multimap<int, pcl::PointXYZ>());
return _registrationVis->computeTransformation(tmpFrom, tmpTo, Transform::getIdentity(), rejectedMsg, inliers, variance);
tmpTo.setWords3(std::multimap<int, cv::Point3f>());
transform = _registrationVis->computeTransformation(tmpFrom, tmpTo, Transform::getIdentity(), rejectedMsg, &inliersV, variance);
}
else
{
return _registrationVis->computeTransformation(*fromS, *toS, Transform::getIdentity(), rejectedMsg, inliers, variance);
transform = _registrationVis->computeTransformation(*fromS, *toS, Transform::getIdentity(), rejectedMsg, &inliersV, variance);
}
if(inliers)
{
*inliers = (int)inliersV.size();
}
}
else
@@ -2133,7 +2058,7 @@ Transform Memory::computeVisualTransform(
}
UWARN(msg.c_str());
}
return Transform();
return transform;
}
// compute transform fromId -> toId
@@ -2179,7 +2104,12 @@ Transform Memory::computeIcpTransform(
toS->sensorData().uncompressData(0, 0, &tmp2);
// compute transform fromId -> toId
t = _registrationIcp->computeTransformation(*fromS, *toS, guess, rejectedMsg, inliers, variance, inliersRatio);
std::vector<int> inliersV;
t = _registrationIcp->computeTransformation(fromS->sensorData(), toS->sensorData(), guess, rejectedMsg, &inliersV, variance, inliersRatio);
if(inliers)
{
*inliers = (int)inliersV.size();
}
}
else
{
@@ -2259,8 +2189,12 @@ Transform Memory::computeIcpTransformMulti(
}
Transform guess = poses.at(fromId).inverse() * poses.at(toId);
Signature toS(0, 0, 0, 0, "", toPose, assembledData);
t = _registrationIcp->computeTransformation(*fromS, toS, guess, rejectedMsg, inliers, variance);
std::vector<int> inliersV;
t = _registrationIcp->computeTransformation(fromS->sensorData(), assembledData, guess, rejectedMsg, &inliersV, variance);
if(inliers)
{
*inliers = (int)inliersV.size();
}
}
return t;
@@ -2461,8 +2395,8 @@ void Memory::dumpSignatures(const char * fileNameSign, bool words3D) const
{
if(words3D)
{
const std::multimap<int, pcl::PointXYZ> & ref = ss->getWords3();
for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
const std::multimap<int, cv::Point3f> & ref = ss->getWords3();
for(std::multimap<int, cv::Point3f>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
{
//show only valid point according to current parameters
if(pcl::isFinite(jter->second) &&
@@ -2851,7 +2785,7 @@ SensorData Memory::getNodeData(int nodeId, bool uncompressedData, bool keepLoade
void Memory::getNodeWords(int nodeId,
std::multimap<int, cv::KeyPoint> & words,
std::multimap<int, pcl::PointXYZ> & words3)
std::multimap<int, cv::Point3f> & words3)
{
UDEBUG("nodeId=%d", nodeId);
Signature * s = this->_getSignature(nodeId);
@@ -3092,227 +3026,44 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
preUpdateThread.start();
}
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3D(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<cv::Point3f> keypoints3D;
if(data.keypoints().size() == 0)
{
if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty() && !isIntermediateNode)
{
// Extract features
cv::Mat imageMono;
// convert to grayscale
if(data.imageRaw().channels() > 1)
if(data.imageRaw().channels() == 3)
{
UDEBUG("convert to grayscale...");
cv::cvtColor(data.imageRaw(), imageMono, cv::COLOR_BGR2GRAY);
cv::cvtColor(data.imageRaw(), imageMono, CV_BGR2GRAY);
}
else
{
imageMono = data.imageRaw();
}
UDEBUG("Set ROI...");
cv::Rect roi = Feature2D::computeRoi(imageMono, _roiRatios);
if(!data.depthOrRightRaw().empty() && data.stereoCameraModel().isValid())
{
//stereo
bool subPixelOn = false;
if(_subPixWinSize > 0 && _subPixIterations > 0)
{
subPixelOn = true;
}
UDEBUG("Generating keypoints...");
keypoints = _feature2D->generateKeypoints(imageMono, roi);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
keypoints = _feature2D->generateKeypoints(imageMono);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
if(keypoints.size())
{
// descriptors should be extracted before subpixel
descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(keypoints, leftCorners);
if(subPixelOn)
{
cv::cornerSubPix( imageMono, leftCorners,
cv::Size( _subPixWinSize, _subPixWinSize ),
cv::Size( -1, -1 ),
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, _subPixIterations, _subPixEps ) );
for(unsigned int i=0;i<leftCorners.size(); ++i)
{
keypoints[i].pt = leftCorners[i];
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemSubpixel(), t*1000.0f);
UDEBUG("time subpix left kpts=%fs", t);
}
UASSERT(keypoints.size() == leftCorners.size());
//generate a disparity map
std::vector<unsigned char> status;
std::vector<cv::Point2f> rightCorners;
rightCorners = _stereo->computeCorrespondences(
imageMono,
data.rightRaw(),
leftCorners,
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(keypoints.size())
{
UASSERT(keypoints.size() == descriptors.rows);
UASSERT(leftCorners.size() == keypoints.size());
keypoints3D = util3d::generateKeypoints3DStereo(
leftCorners,
rightCorners,
data.stereoCameraModel(),
status);
UASSERT(keypoints.size() == keypoints3D->size());
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t);
}
}
}
else if(!data.depthOrRightRaw().empty() && data.cameraModels().size())
{
//depth
bool subPixelOn = false;
if(_subPixWinSize > 0 && _subPixIterations > 0)
{
subPixelOn = true;
}
UDEBUG("Generating keypoints...");
keypoints = _feature2D->generateKeypoints(imageMono, roi);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
if(keypoints.size())
{
if(subPixelOn)
{
// descriptors should be extracted before subpixel
descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(keypoints, leftCorners);
cv::cornerSubPix( imageMono, leftCorners,
cv::Size( _subPixWinSize, _subPixWinSize ),
cv::Size( -1, -1 ),
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, _subPixIterations, _subPixEps ) );
for(unsigned int i=0;i<leftCorners.size(); ++i)
{
keypoints[i].pt = leftCorners[i];
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemSubpixel(), t*1000.0f);
UDEBUG("time subpix left kpts=%fs", t);
}
if(_wordsMaxDepth > 0.0f || _wordsMinDepth > 0.0f)
{
Feature2D::filterKeypointsByDepth(keypoints, descriptors, data.depthOrRightRaw(), _wordsMinDepth, _wordsMaxDepth);
UDEBUG("filter keypoints by depth (%d)", (int)keypoints.size());
}
if(keypoints.size())
{
if(!subPixelOn)
{
descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
}
UASSERT(keypoints.size() == descriptors.rows);
keypoints3D = util3d::generateKeypoints3DDepth(
keypoints,
data.depthOrRightRaw(),
data.cameraModels());
UASSERT(keypoints.size() == keypoints3D->size());
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t);
}
}
}
else
{
//RGB only
UDEBUG("Generating keypoints...");
keypoints = _feature2D->generateKeypoints(imageMono, roi);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
if(keypoints.size())
{
descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
if(_subPixWinSize > 0 && _subPixIterations > 0)
{
std::vector<cv::Point2f> corners;
cv::KeyPoint::convert(keypoints, corners);
cv::cornerSubPix( imageMono, corners,
cv::Size( _subPixWinSize, _subPixWinSize ),
cv::Size( -1, -1 ),
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, _subPixIterations, _subPixEps ) );
for(unsigned int i=0;i<corners.size(); ++i)
{
keypoints[i].pt = corners[i];
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemSubpixel(), t*1000.0f);
UDEBUG("time subpix kpts=%fs", t);
}
}
}
descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
{
descriptors = cv::Mat();
}
else if((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValid()) ||
(!data.rightRaw().empty() && data.stereoCameraModel().isValid()))
{
keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
}
}
else if(data.imageRaw().empty())
{
@@ -3332,79 +3083,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
keypoints = data.keypoints();
descriptors = data.descriptors().clone();
// filter by depth
if(!data.depthOrRightRaw().empty() && !data.imageRaw().empty() && data.stereoCameraModel().isValid())
{
//stereo
cv::Mat imageMono;
// convert to grayscale
if(data.imageRaw().channels() > 1)
{
cv::cvtColor(data.imageRaw(), imageMono, cv::COLOR_BGR2GRAY);
}
else
{
imageMono = data.imageRaw();
}
//generate a disparity map
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(keypoints, leftCorners);
std::vector<unsigned char> status;
std::vector<cv::Point2f> rightCorners;
rightCorners = _stereo->computeCorrespondences(
imageMono,
data.rightRaw(),
leftCorners,
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);
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);
}
else if(!data.depthOrRightRaw().empty() && data.cameraModels().size())
{
//depth
if(_wordsMaxDepth > 0.0f || _wordsMinDepth > 0.0f)
{
Feature2D::filterKeypointsByDepth(keypoints, descriptors, _wordsMinDepth, _wordsMaxDepth);
UDEBUG("filter keypoints by depth (%d)", (int)keypoints.size());
}
keypoints3D = util3d::generateKeypoints3DDepth(
keypoints,
data.depthOrRightRaw(),
data.cameraModels());
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t);
}
keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
}
if(_parallelized)
@@ -3439,11 +3118,11 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
}
std::multimap<int, cv::KeyPoint> words;
std::multimap<int, pcl::PointXYZ> words3D;
std::multimap<int, cv::Point3f> words3D;
if(wordIds.size() > 0)
{
UASSERT(wordIds.size() == keypoints.size());
UASSERT(keypoints3D->size() == 0 || keypoints3D->size() == wordIds.size());
UASSERT(keypoints3D.size() == 0 || keypoints3D.size() == wordIds.size());
unsigned int i=0;
for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
{
@@ -3459,9 +3138,9 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
{
words.insert(std::pair<int, cv::KeyPoint>(*iter, keypoints[i]));
}
if(keypoints3D->size())
if(keypoints3D.size())
{
words3D.insert(std::pair<int, pcl::PointXYZ>(*iter, keypoints3D->at(i)));
words3D.insert(std::pair<int, cv::Point3f>(*iter, keypoints3D.at(i)));
}
}
}
@@ -3478,9 +3157,9 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
{
Transform cameraTransform = pose.inverse() * previousS->getPose();
// compute 3D words by epipolar geometry with the previous signature
std::multimap<int, pcl::PointXYZ> inliers = util3d::generateWords3DMono(
words,
previousS->getWords(),
std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
uMultimapToMapUnique(words),
uMultimapToMapUnique(previousS->getWords()),
data.cameraModels()[0],
cameraTransform);
@@ -3488,21 +3167,21 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
float bad_point = std::numeric_limits<float>::quiet_NaN ();
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
std::multimap<int, pcl::PointXYZ>::iterator jter=inliers.find(iter->first);
std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
if(jter != inliers.end())
{
words3D.insert(std::make_pair(iter->first, jter->second));
}
else
{
words3D.insert(std::make_pair(iter->first, pcl::PointXYZ(bad_point,bad_point,bad_point)));
words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
}
}
t = timer.ticks();
UASSERT(words3D.size() == words.size());
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t);
UDEBUG("time keypoints 3D (%d) = %fs", (int)words3D.size(), t);
}
}
corelib/src/Odometry.cpp
+4
View File
@@ -40,6 +40,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_inlierDistance(Parameters::defaultVisInlierDistance()),
_iterations(Parameters::defaultVisIterations()),
_refineIterations(Parameters::defaultVisRefineIterations()),
_minDepth(Parameters::defaultVisMinDepth()),
_maxDepth(Parameters::defaultVisMaxDepth()),
_resetCountdown(Parameters::defaultOdomResetCountdown()),
_force2D(Parameters::defaultVisForce2D()),
@@ -54,6 +55,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_estimationType(Parameters::defaultVisEstimationType()),
_pnpReprojError(Parameters::defaultVisPnPReprojError()),
_pnpFlags(Parameters::defaultVisPnPFlags()),
_pnpOpenCV2(Parameters::defaultVisPnPOpenCV2()),
_varianceFromInliersCount(Parameters::defaultRegVarianceFromInliersCount()),
_kalmanProcessNoise(Parameters::defaultOdomKalmanProcessNoise()),
_kalmanMeasurementNoise(Parameters::defaultOdomKalmanMeasurementNoise()),
@@ -68,6 +70,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kVisInlierDistance(), _inlierDistance);
Parameters::parse(parameters, Parameters::kVisIterations(), _iterations);
Parameters::parse(parameters, Parameters::kVisRefineIterations(), _refineIterations);
Parameters::parse(parameters, Parameters::kVisMinDepth(), _minDepth);
Parameters::parse(parameters, Parameters::kVisMaxDepth(), _maxDepth);
Parameters::parse(parameters, Parameters::kVisRoiRatios(), _roiRatios);
Parameters::parse(parameters, Parameters::kVisForce2D(), _force2D);
@@ -76,6 +79,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);
Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _pnpReprojError);
Parameters::parse(parameters, Parameters::kVisPnPFlags(), _pnpFlags);
Parameters::parse(parameters, Parameters::kVisPnPOpenCV2(), _pnpOpenCV2);
UASSERT(_pnpFlags>=0 && _pnpFlags <=2);
Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), _varianceFromInliersCount);
Parameters::parse(parameters, Parameters::kOdomFilteringStrategy(), _filteringStrategy);
corelib/src/OdometryBOW.cpp
+16 -13
View File
@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_motion_estimation.h"
#include "rtabmap/core/Optimizer.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UConversion.h"
@@ -59,6 +60,7 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kOdomBowFixedLocalMapPath(), _fixedLocalMapPath);
ParametersMap customParameters;
customParameters.insert(ParametersPair(Parameters::kKpMinDepth(), uNumber2Str(this->getMinDepth())));
customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth())));
customParameters.insert(ParametersPair(Parameters::kKpRoiRatios(), this->getRoiRatios()));
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
@@ -66,18 +68,18 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kMemSaveDepth16Format(), "false"));
int nn = Parameters::defaultVisNNType();
float nndr = Parameters::defaultVisNNDR();
int nn = Parameters::defaultVisCorNNType();
float nndr = Parameters::defaultVisCorNNDR();
int featureType = Parameters::defaultVisFeatureType();
int maxFeatures = Parameters::defaultVisMaxFeatures();
Parameters::parse(parameters, Parameters::kVisNNType(), nn);
Parameters::parse(parameters, Parameters::kVisNNDR(), nndr);
Parameters::parse(parameters, Parameters::kVisCorNNType(), nn);
Parameters::parse(parameters, Parameters::kVisCorNNDR(), nndr);
Parameters::parse(parameters, Parameters::kVisFeatureType(), featureType);
Parameters::parse(parameters, Parameters::kVisMaxFeatures(), maxFeatures);
customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(nn)));
customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(featureType)));
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxFeatures)));
customParameters.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(maxFeatures)));
// Memory's stereo parameters, copy from Odometry
int subPixWinSize = Parameters::defaultVisSubPixWinSize();
@@ -150,8 +152,8 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
if(s)
{
// Transform 3D points accordingly to pose and add them to local map
const std::multimap<int, pcl::PointXYZ> & words3D = s->getWords3();
for(std::multimap<int, pcl::PointXYZ>::const_iterator pointsIter=words3D.begin();
const std::multimap<int, cv::Point3f> & words3D = s->getWords3();
for(std::multimap<int, cv::Point3f>::const_iterator pointsIter=words3D.begin();
pointsIter!=words3D.end();
++pointsIter)
{
@@ -255,6 +257,7 @@ Transform OdometryBOW::computeTransform(
this->getIterations(),
this->getPnPReprojError(),
this->getPnPFlags(),
this->getPnPOpenCV2(),
this->getPose(),
uMultimapToMap(newSignature->getWords3()),
isVarianceFromInliersCount()?0:&variance, // don't compute variance if we use inliers
@@ -356,10 +359,10 @@ Transform OdometryBOW::computeTransform(
// keep old word
if(localMap_.find(*iter) == localMap_.end())
{
const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
if(pcl::isFinite(pt))
const cv::Point3f & pt = newSignature->getWords3().find(*iter)->second;
if(util3d::isFinite(pt))
{
pcl::PointXYZ pt2 = util3d::transformPoint(pt, t);
cv::Point3f pt2 = util3d::transformPoint(pt, t);
localMap_.insert(std::make_pair(*iter, pt2));
}
}
@@ -391,10 +394,10 @@ Transform OdometryBOW::computeTransform(
// Only add unique words
if(newSignature->getWords3().count(*iter) == 1)
{
const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
if(pcl::isFinite(pt))
const cv::Point3f & pt = newSignature->getWords3().find(*iter)->second;
if(util3d::isFinite(pt))
{
pcl::PointXYZ pt2 = util3d::transformPoint(pt, t);
cv::Point3f pt2 = util3d::transformPoint(pt, t);
localMap_.insert(std::make_pair(*iter, pt2));
}
else
corelib/src/OdometryF2F.cpp
+172
View File
@@ -0,0 +1,172 @@
/*
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/Odometry.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/EpipolarGeometry.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
namespace rtabmap {
OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
Odometry(parameters),
keyFrameThr_(Parameters::defaultOdomFlowKeyFrameThr()),
guessFromMotion_(Parameters::defaultOdomFlowGuessMotion()),
registration_(parameters),
motionSinceLastKeyFrame_(Transform::getIdentity())
{
Parameters::parse(parameters, Parameters::kOdomFlowKeyFrameThr(), keyFrameThr_);
Parameters::parse(parameters, Parameters::kOdomFlowGuessMotion(), guessFromMotion_);
}
OdometryF2F::~OdometryF2F()
{
}
void OdometryF2F::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
refFrame_ = Signature();
motionSinceLastKeyFrame_.setIdentity();
}
// return not null transform if odometry is correctly computed
Transform OdometryF2F::computeTransform(
const SensorData & data,
OdometryInfo * info)
{
UTimer timer;
Transform output;
if(!data.rightRaw().empty() && !data.stereoCameraModel().isValid())
{
UERROR("Calibrated stereo camera required");
return output;
}
if(!data.depthRaw().empty() &&
(data.cameraModels().size() != 1 || !data.cameraModels()[0].isValid()))
{
UERROR("Calibrated camera required (multi-cameras not supported).");
return output;
}
float variance = 0;
std::vector<int> inliers;
Signature newFrame(data);
if(refFrame_.getWords().size())
{
std::string rejectedMsg;
output = registration_.computeTransformationMod(
refFrame_,
newFrame,
guessFromMotion_?motionSinceLastKeyFrame_*this->previousTransform():Transform::getIdentity(),
&rejectedMsg,
&inliers,
&variance);
if(info && this->isInfoDataFilled())
{
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
EpipolarGeometry::findPairsUnique(refFrame_.getWords(), newFrame.getWords(), pairs);
info->refCorners.resize(pairs.size());
info->newCorners.resize(pairs.size());
std::map<int, int> idToIndex;
int i=0;
for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin();
iter!=pairs.end();
++iter)
{
info->refCorners[i] = iter->second.first.pt;
info->newCorners[i] = iter->second.second.pt;
idToIndex.insert(std::make_pair(iter->first, i));
++i;
}
info->cornerInliers.resize(inliers.size(), 1);
i=0;
for(; i<(int)inliers.size(); ++i)
{
info->cornerInliers[i] = idToIndex.at(inliers[i]);
}
}
}
else
{
//return Identity
output = Transform::getIdentity();
}
if(!output.isNull())
{
output = motionSinceLastKeyFrame_.inverse() * output;
motionSinceLastKeyFrame_ *= output;
// new key-frame?
if(keyFrameThr_ <= 0 || (int)inliers.size() <= keyFrameThr_)
{
UDEBUG("Update key frame");
// only generate features for the first frame
Signature newRefFrame(data);
Signature dummy;
registration_.computeTransformationMod(
newRefFrame,
dummy);
if((int)newRefFrame.getWords().size() >= this->getMinInliers())
{
refFrame_ = newRefFrame;
//reset motion
motionSinceLastKeyFrame_.setIdentity();
}
else
{
UWARN("Too low 2D corners (%d), keeping last key frame...",
(int)newRefFrame.getWords().size());
}
}
}
if(info)
{
info->type = 1;
info->variance = variance;
info->inliers = (int)inliers.size();
}
UINFO("Odom update time = %fs lost=%s inliers=%d, ref frame corners=%d, transform accepted=%s",
timer.elapsed(),
output.isNull()?"true":"false",
(int)inliers.size(),
(int)refFrame_.getWords().size(),
!output.isNull()?"true":"false");
return output;
}
} // namespace rtabmap
corelib/src/OdometryMono.cpp
+69 -62
View File
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_motion_estimation.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/util3d_features.h"
@@ -49,10 +50,10 @@ namespace rtabmap {
OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
Odometry(parameters),
flowWinSize_(Parameters::defaultOdomFlowWinSize()),
flowIterations_(Parameters::defaultOdomFlowIterations()),
flowEps_(Parameters::defaultOdomFlowEps()),
flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
flowWinSize_(Parameters::defaultVisCorFlowWinSize()),
flowIterations_(Parameters::defaultVisCorFlowIterations()),
flowEps_(Parameters::defaultVisCorFlowEps()),
flowMaxLevel_(Parameters::defaultVisCorFlowMaxLevel()),
localHistoryMaxSize_(Parameters::defaultOdomBowLocalHistorySize()),
initMinFlow_(Parameters::defaultOdomMonoInitMinFlow()),
initMinTranslation_(Parameters::defaultOdomMonoInitMinTranslation()),
@@ -61,10 +62,10 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
fundMatrixConfidence_(Parameters::defaultVhEpRansacParam2()),
maxVariance_(Parameters::defaultOdomMonoMaxVariance())
{
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::kVisCorFlowWinSize(), flowWinSize_);
Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), flowIterations_);
Parameters::parse(parameters, Parameters::kVisCorFlowEps(), flowEps_);
Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), flowMaxLevel_);
Parameters::parse(parameters, Parameters::kOdomBowLocalHistorySize(), localHistoryMaxSize_);
Parameters::parse(parameters, Parameters::kOdomMonoInitMinFlow(), initMinFlow_);
@@ -85,18 +86,18 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kKpTfIdfLikelihoodUsed(), "false"));
int nn = Parameters::defaultVisNNType();
float nndr = Parameters::defaultVisNNDR();
int nn = Parameters::defaultVisCorNNType();
float nndr = Parameters::defaultVisCorNNDR();
int featureType = Parameters::defaultVisFeatureType();
int maxFeatures = Parameters::defaultVisMaxFeatures();
Parameters::parse(parameters, Parameters::kVisNNType(), nn);
Parameters::parse(parameters, Parameters::kVisNNDR(), nndr);
Parameters::parse(parameters, Parameters::kVisCorNNType(), nn);
Parameters::parse(parameters, Parameters::kVisCorNNDR(), nndr);
Parameters::parse(parameters, Parameters::kVisFeatureType(), featureType);
Parameters::parse(parameters, Parameters::kVisMaxFeatures(), maxFeatures);
customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(nn)));
customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(featureType)));
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxFeatures)));
customParameters.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(maxFeatures)));
int subPixWinSize = Parameters::defaultVisSubPixWinSize();
int subPixIterations = Parameters::defaultVisSubPixIterations();
@@ -354,7 +355,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
{
//PnPRansac
std::vector<int> inliersV;
cv::solvePnPRansac(
util3d::solvePnPRansac(
objectPoints,
imagePoints,
K,
@@ -364,13 +365,10 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
true,
this->getIterations(),
this->getPnPReprojError(),
#if CV_MAJOR_VERSION < 3
0, // min inliers
#else
0.99, // confidence
#endif
inliersV,
this->getPnPFlags());
this->getPnPFlags(),
this->getPnPOpenCV2());
UDEBUG("inliers=%d/%d", (int)inliersV.size(), (int)objectPoints.size());
@@ -427,15 +425,16 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
{
double variance = 0;
const std::multimap<int, pcl::PointXYZ> & previousGuess = keyFrameWords3D_.find(previousS->id())->second;
std::multimap<int, pcl::PointXYZ> inliers3D = util3d::generateWords3DMono(
previousS->getWords(),
newS->getWords(),
const std::map<int, cv::Point3f> & previousGuess = keyFrameWords3D_.find(previousS->id())->second;
std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
uMultimapToMapUnique(previousS->getWords()),
uMultimapToMapUnique(newS->getWords()),
cameraModel,
cameraTransform,
this->getIterations(),
this->getPnPReprojError(),
this->getPnPFlags(),
this->getPnPOpenCV2(),
fundMatrixReprojError_,
fundMatrixConfidence_,
previousGuess,
@@ -457,7 +456,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
UDEBUG("cameraTransform= %s", cameraTransform.prettyPrint().c_str());
std::multimap<int, cv::Point3f> wordsToAdd;
for(std::multimap<int, pcl::PointXYZ>::iterator iter=inliers3D.begin();
for(std::map<int, cv::Point3f>::iterator iter=inliers3D.begin();
iter != inliers3D.end();
++iter)
{
@@ -467,8 +466,8 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
if(!uContains(localMap_, iter->first))
{
//UDEBUG("Add new point %d to local map", iter->first);
pcl::PointXYZ newPt = util3d::transformPoint(iter->second, newPose);
wordsToAdd.insert(std::make_pair(iter->first, cv::Point3f(newPt.x, newPt.y, newPt.z)));
cv::Point3f newPt = util3d::transformPoint(iter->second, newPose);
wordsToAdd.insert(std::make_pair(iter->first, newPt));
}
}
@@ -722,17 +721,17 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
EpipolarGeometry::triangulatePoints(x_norm, xp_norm, P0, P, cloud, reprojErrors);
pcl::PointCloud<pcl::PointXYZ>::Ptr inliersRef(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr inliersRefGuess(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<cv::Point3f> inliersRef;
std::vector<cv::Point3f> inliersRefGuess;
std::vector<cv::Point2f> imagePoints(cloud->size());
inliersRef->resize(cloud->size());
inliersRefGuess->resize(cloud->size());
inliersRef.resize(cloud->size());
inliersRefGuess.resize(cloud->size());
tmpCornersId.resize(cloud->size());
oi = 0;
UASSERT(newCorners.size() == cloud->size());
pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<cv::Point3f> newCorners3D;
if(!refDepthOrRight_.empty())
{
@@ -776,17 +775,19 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
{
imagePoints[oi] = newCorners[i];
tmpCornersId[oi] = cornerIds[i];
(*inliersRef)[oi] = cloud->at(i);
if(!newCorners3D->empty())
inliersRef[oi].x = cloud->at(i).x;
inliersRef[oi].y = cloud->at(i).y;
inliersRef[oi].z = cloud->at(i).z;
if(!newCorners3D.empty())
{
(*inliersRefGuess)[oi] = newCorners3D->at(i);
inliersRefGuess[oi] = newCorners3D.at(i);
}
++oi;
}
}
imagePoints.resize(oi);
inliersRef->resize(oi);
inliersRefGuess->resize(oi);
inliersRef.resize(oi);
inliersRefGuess.resize(oi);
tmpCornersId.resize(oi);
cornerIds = tmpCornersId;
@@ -795,25 +796,25 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
//estimate scale
float scale = 1;
std::multimap<float, float> scales; // <variance, scale>
if(!newCorners3D->empty()) // scale known
if(!newCorners3D.empty()) // scale known
{
UASSERT(inliersRefGuess->size() == inliersRef->size());
for(unsigned int i=0; i<inliersRef->size(); ++i)
UASSERT(inliersRefGuess.size() == inliersRef.size());
for(unsigned int i=0; i<inliersRef.size(); ++i)
{
if(pcl::isFinite(inliersRefGuess->at(i)))
if(util3d::isFinite(inliersRefGuess.at(i)))
{
float s = inliersRefGuess->at(i).z/inliersRef->at(i).z;
std::vector<float> errorSqrdDists(inliersRef->size());
float s = inliersRefGuess.at(i).z/inliersRef.at(i).z;
std::vector<float> errorSqrdDists(inliersRef.size());
oi = 0;
for(unsigned int j=0; j<inliersRef->size(); ++j)
for(unsigned int j=0; j<inliersRef.size(); ++j)
{
if(cloud->at(j).z>0)
{
pcl::PointXYZ refPt = inliersRef->at(j);
cv::Point3f refPt = inliersRef.at(j);
refPt.x *= s;
refPt.y *= s;
refPt.z *= s;
const pcl::PointXYZ & guess = inliersRefGuess->at(j);
const cv::Point3f & guess = inliersRefGuess.at(j);
errorSqrdDists[oi++] = uNormSquared(refPt.x-guess.x, refPt.y-guess.y, refPt.z-guess.z);
}
}
@@ -850,11 +851,19 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
}
}
else if(inliersRef->size())
else if(inliersRef.size())
{
// find centroid of the cloud and set it to 1 meter
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*inliersRef, centroid);
pcl::PointCloud<pcl::PointXYZ> inliersRefCloud;
inliersRefCloud.resize(inliersRef.size());
for(unsigned int i=0; i<inliersRef.size(); ++i)
{
inliersRefCloud[i].x = inliersRef[i].x;
inliersRefCloud[i].y = inliersRef[i].y;
inliersRefCloud[i].z = inliersRef[i].z;
}
pcl::compute3DCentroid(inliersRefCloud, centroid);
scale = 1.0f / centroid[2];
}
else
@@ -865,17 +874,17 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
if(!reject)
{
//PnPRansac
std::vector<cv::Point3f> objectPoints(inliersRef->size());
for(unsigned int i=0; i<inliersRef->size(); ++i)
std::vector<cv::Point3f> objectPoints(inliersRef.size());
for(unsigned int i=0; i<inliersRef.size(); ++i)
{
objectPoints[i].x = inliersRef->at(i).x * scale;
objectPoints[i].y = inliersRef->at(i).y * scale;
objectPoints[i].z = inliersRef->at(i).z * scale;
objectPoints[i].x = inliersRef.at(i).x * scale;
objectPoints[i].y = inliersRef.at(i).y * scale;
objectPoints[i].z = inliersRef.at(i).z * scale;
}
cv::Mat rvec;
cv::Mat tvec;
std::vector<int> inliersPnP;
cv::solvePnPRansac(
util3d::solvePnPRansac(
objectPoints, // 3D points in ref referential
imagePoints, // 2D points in new referential
K,
@@ -885,13 +894,10 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
false,
this->getIterations(),
this->getPnPReprojError(),
#if CV_MAJOR_VERSION < 3
0, // min inliers
#else
0.99, // confidence
#endif
inliersPnP,
this->getPnPFlags());
this->getPnPFlags(),
this->getPnPOpenCV2());
UDEBUG("PnP inliers = %d / %d", (int)inliersPnP.size(), (int)objectPoints.size());
@@ -914,15 +920,16 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
std::vector<int> wordsId = uKeys(memory_->getLastWorkingSignature()->getWords());
UASSERT(wordsId.size());
UASSERT(cornerIds.size() == objectPoints.size());
std::multimap<int, pcl::PointXYZ> keyFrameWords3D;
std::map<int, cv::Point3f> keyFrameWords3D;
Transform t = this->getPose()*cameraModel.localTransform();
for(unsigned int i=0; i<inliersPnP.size(); ++i)
{
int index =inliersPnP.at(i);
int id = cornerIds[index];
UASSERT(id > 0 && id <= *wordsId.rbegin());
pcl::PointXYZ pt = util3d::transformPoint(
pcl::PointXYZ(objectPoints.at(index).x, objectPoints.at(index).y, objectPoints.at(index).z),
this->getPose()*cameraModel.localTransform());
cv::Point3f pt = util3d::transformPoint(
objectPoints.at(index),
t);
localMap_.insert(std::make_pair(id, cv::Point3f(pt.x, pt.y, pt.z)));
keyFrameWords3D.insert(std::make_pair(id, pt));
}
corelib/src/OdometryOpticalFlow.cpp
-609
View File
@@ -1,609 +0,0 @@
/*
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/Odometry.h"
#include "rtabmap/core/OdometryInfo.h"
#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"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UMath.h"
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/calib3d/calib3d.hpp>
namespace rtabmap {
OdometryOpticalFlow::OdometryOpticalFlow(const ParametersMap & parameters) :
Odometry(parameters),
keyFrameThr_(Parameters::defaultOdomFlowKeyFrameThr()),
flowWinSize_(Parameters::defaultOdomFlowWinSize()),
flowIterations_(Parameters::defaultOdomFlowIterations()),
flowEps_(Parameters::defaultOdomFlowEps()),
flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
flowGuessFromMotion_(Parameters::defaultOdomFlowGuessMotion()),
subPixWinSize_(Parameters::defaultVisSubPixWinSize()),
subPixIterations_(Parameters::defaultVisSubPixIterations()),
subPixEps_(Parameters::defaultVisSubPixEps()),
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::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())
{
detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
}
ParametersMap customParameters;
int maxFeatures = Parameters::defaultVisMaxFeatures();
Parameters::parse(parameters, Parameters::kVisMaxFeatures(), maxFeatures);
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxFeatures)));
// add only feature stuff
for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
std::string group = uSplit(iter->first, '/').front();
if(group.compare("SURF") == 0 ||
group.compare("SIFT") == 0 ||
group.compare("BRIEF") == 0 ||
group.compare("FAST") == 0 ||
group.compare("ORB") == 0 ||
group.compare("FREAK") == 0 ||
group.compare("GFTT") == 0 ||
group.compare("BRISK") == 0)
{
customParameters.insert(*iter);
}
}
feature2D_ = Feature2D::create(detectorStrategy, customParameters);
}
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
Transform OdometryOpticalFlow::computeTransform(
const SensorData & data,
OdometryInfo * info)
{
UTimer timer;
Transform output;
if(!data.rightRaw().empty() && !data.stereoCameraModel().isValid())
{
UERROR("Calibrated stereo camera required");
return output;
}
if(!data.depthRaw().empty() &&
(data.cameraModels().size() != 1 || !data.cameraModels()[0].isValid()))
{
UERROR("Calibrated camera required (multi-cameras not supported).");
return output;
}
double variance = 0;
int inliers = 0;
int correspondences = 0;
if(info)
{
info->type = 1;
}
cv::Mat newLeftFrame;
// convert to grayscale
if(data.imageRaw().channels() > 1)
{
cv::cvtColor(data.imageRaw(), newLeftFrame, cv::COLOR_BGR2GRAY);
}
else
{
newLeftFrame = data.imageRaw().clone();
}
std::vector<cv::Point2f> newCorners;
UDEBUG("lastCorners_.size()=%d lastFrame_=%d depthRight=%d",
(int)refCorners_.size(), refFrame_.empty()?0:1, data.depthOrRightRaw().empty()?0:1);
if(!refFrame_.empty() &&
((data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()) || data.stereoCameraModel().isValid()) &&
refCorners_.size() &&
refCorners3D_->size())
{
UASSERT_MSG(refCorners_.size() == refCorners3D_->size(),
uFormat("%d vs %d", (int)refCorners_.size(), (int)refCorners3D_->size()).c_str());
// make guess
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 = (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(),
(double)guess.r31(), (double)guess.r32(), (double)guess.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
std::vector<cv::Point3f> objectPoints(refCorners3D_->size());
for(unsigned int i=0; i<objectPoints.size(); ++i)
{
objectPoints[i].x = refCorners3D_->at(i).x;
objectPoints[i].y = refCorners3D_->at(i).y;
objectPoints[i].z = refCorners3D_->at(i).z;
}
if(flowGuessFromMotion_ && !(motionSinceLastKeyFrame_*this->previousTransform()).isIdentity())
{
UDEBUG("project points to new image");
cv::projectPoints(objectPoints, rvec, tvec, K, cv::Mat(), newCorners);
}
// Find features in the new left image
std::vector<unsigned char> status;
std::vector<float> err;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
int winSize = flowWinSize_;
cv::calcOpticalFlowPyrLK(
refFrame_,
newLeftFrame,
refCorners_,
newCorners,
status,
err,
cv::Size(winSize, winSize),
(newCorners.size()||!flowGuessFromMotion_)?flowMaxLevel_:3,
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");
pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3DKept(new pcl::PointCloud<pcl::PointXYZ>);
refCorners3DKept->resize(status.size());
std::vector<cv::Point3f> objectPointsKept(status.size());
std::vector<cv::Point2f> refCornersKept(status.size());
std::vector<cv::Point2f> newCornersKept(status.size());
int ki = 0;
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i] &&
uIsInBounds(newCorners[i].x, 0.0f, float(data.depthOrRightRaw().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depthOrRightRaw().rows)))
{
refCorners3DKept->at(ki) = refCorners3D_->at(i);
objectPointsKept[ki] = objectPoints[i];
refCornersKept[ki] = refCorners_[i];
newCornersKept[ki] = newCorners[i];
++ki;
}
}
refCorners3DKept->resize(ki);
objectPointsKept.resize(ki);
refCornersKept.resize(ki);
newCornersKept.resize(ki);
correspondences = ki;
if(correspondences && correspondences >= this->getMinInliers())
{
// get new 3D points
pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3DKept;
if(!isVarianceFromInliersCount() || this->getEstimationType() != 1)
{
// Don't compute the new 3D points if the variance is not required on PnP estimation
if(!data.rightRaw().empty())
{
// stereo
std::vector<unsigned char> stereoStatus;
std::vector<cv::Point2f> rightCorners;
rightCorners = stereo_->computeCorrespondences(
newLeftFrame,
data.rightRaw(),
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
{
//depth
std::vector<cv::KeyPoint> newCornersKeptKpt;
cv::KeyPoint::convert(newCornersKept, newCornersKeptKpt);
newCorners3DKept = util3d::generateKeypoints3DDepth(
newCornersKeptKpt,
data.depthRaw(),
data.cameraModels());
}
UASSERT(newCorners3DKept.get() != 0);
}
std::vector<int> inliersV;
if(this->getEstimationType() == 1) // PnP
{
if(this->isInfoDataFilled() && info)
{
info->refCorners = refCornersKept;
info->newCorners = newCornersKept;
}
//PnPRansac
cv::solvePnPRansac(
objectPointsKept,
newCornersKept,
K,
cv::Mat(),
rvec,
tvec,
true,
this->getIterations(),
this->getPnPReprojError(),
#if CV_MAJOR_VERSION < 3
0, // min inliers
#else
0.99, // confidence
#endif
inliersV,
this->getPnPFlags());
cv::Rodrigues(rvec, R);
Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
inliers = (int)inliersV.size();
if((int)inliersV.size() >= this->getMinInliers())
{
// make it incremental
output = (localTransform * pnp).inverse();
// compute variance from 3D correspondences error
variance = 1;
if(!isVarianceFromInliersCount())
{
UASSERT(objectPointsKept.size() == newCorners3DKept->size());
std::vector<float> errorSqrdDists(inliersV.size());
int oi = 0;
for(unsigned int i=0; i<inliersV.size(); ++i)
{
if(pcl::isFinite(newCorners3DKept->at(inliersV[i])))
{
const cv::Point3f & objPt = objectPointsKept[inliersV[i]];
pcl::PointXYZ newPt = util3d::transformPoint(newCorners3DKept->at(inliersV[i]), output);
errorSqrdDists[oi++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
}
}
errorSqrdDists.resize(oi);
if(errorSqrdDists.size())
{
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
variance = 2.1981 * median_error_sqr;
}
}
}
else
{
UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), this->getMinInliers());
}
if(this->isInfoDataFilled() && info)
{
info->cornerInliers = inliersV;
}
}
else
{
// Get 3D correspondences (remove NaN)
pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesRef(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesNew(new pcl::PointCloud<pcl::PointXYZ>);
correspondencesRef->resize(newCornersKept.size());
correspondencesNew->resize(newCornersKept.size());
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(newCornersKept.size());
info->newCorners.resize(newCornersKept.size());
}
int oi = 0;
UASSERT(newCorners3DKept->size() == newCornersKept.size());
for(unsigned int i=0; i<newCornersKept.size(); ++i)
{
if(pcl::isFinite(newCorners3DKept->at(i)) &&
(this->getMaxDepth() == 0.0f || newCorners3DKept->at(i).z < this->getMaxDepth()))
{
//Add 3D correspondences!
correspondencesRef->at(oi) = refCorners3DKept->at(i);
correspondencesNew->at(oi) = newCorners3DKept->at(i);
if(this->isInfoDataFilled() && info)
{
info->refCorners[oi] = refCornersKept[i];
info->newCorners[oi] = newCornersKept[i];
}
++oi;
}
}
correspondencesRef->resize(oi);
correspondencesNew->resize(oi);
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(oi);
info->newCorners.resize(oi);
}
correspondences = oi;
UDEBUG("Getting correspondences end, kept %d/%d", correspondences, (int)newCornersKept.size());
if(correspondences >= this->getMinInliers())
{
UTimer timerRANSAC;
Transform t = util3d::transformFromXYZCorrespondences(
correspondencesNew,
correspondencesRef,
this->getInlierDistance(),
this->getIterations(),
this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
&inliersV,
&variance);
UDEBUG("time RANSAC = %fs", timerRANSAC.ticks());
inliers = (int)inliersV.size();
if(!t.isNull() && inliers >= this->getMinInliers())
{
output = t;
}
else
{
UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
}
}
else
{
UWARN("Not enough correspondences (%d)", correspondences);
}
}
if(this->isInfoDataFilled() && info)
{
info->cornerInliers = inliersV;
}
}
}
else
{
//return Identity
output = Transform::getIdentity();
}
newCorners.clear();
if(!output.isNull())
{
output = motionSinceLastKeyFrame_.inverse() * output;
// new key-frame?
if(keyFrameThr_ <= 0 || inliers <= keyFrameThr_)
{
// 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);
if(newKtps.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
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) = refCorners3DTmp->at(i);
newCornersFiltered[oi] = newCorners[i];
++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;
//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 2D corners (%d), ignoring new frame...",
(int)newCorners.size());
output.setNull();
}
}
else
{
motionSinceLastKeyFrame_ *= output;
}
}
if(info)
{
info->type = 1;
info->variance = variance;
info->inliers = inliers;
info->features = (int)refCorners_.size();
info->matches = correspondences;
}
UINFO("Odom update time = %fs lost=%s inliers=%d/%d, new corners=%d, transform accepted=%s",
timer.elapsed(),
output.isNull()?"true":"false",
inliers,
correspondences,
(int)newCorners.size(),
!output.isNull()?"true":"false");
return output;
}
} // namespace rtabmap
corelib/src/Parameters.cpp
+17 -7
View File
@@ -141,6 +141,8 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
// removed parameters
// 0.11.0
removedParameters_.insert(std::make_pair("Kp/WordsPerImage", std::make_pair(true, Parameters::kKpMaxFeatures())));
removedParameters_.insert(std::make_pair("Mem/LaserScanVoxelSize", std::make_pair(false, Parameters::kMemLaserScanDownsampleStepSize())));
removedParameters_.insert(std::make_pair("Mem/LocalSpaceLinksKeptInWM", std::make_pair(false, "")));
@@ -161,8 +163,13 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
removedParameters_.insert(std::make_pair("Odom/PnPReprojError", std::make_pair(true, Parameters::kVisPnPReprojError())));
removedParameters_.insert(std::make_pair("Odom/PnPFlags", std::make_pair(true, Parameters::kVisPnPFlags())));
removedParameters_.insert(std::make_pair("OdomBow/NNType", std::make_pair(true, Parameters::kVisNNType())));
removedParameters_.insert(std::make_pair("OdomBow/NNDR", std::make_pair(true, Parameters::kVisNNDR())));
removedParameters_.insert(std::make_pair("OdomBow/NNType", std::make_pair(true, Parameters::kVisCorNNType())));
removedParameters_.insert(std::make_pair("OdomBow/NNDR", std::make_pair(true, Parameters::kVisCorNNDR())));
removedParameters_.insert(std::make_pair("OdomFlow/WinSize", std::make_pair(true, Parameters::kVisCorFlowWinSize())));
removedParameters_.insert(std::make_pair("OdomFlow/Iterations", std::make_pair(true, Parameters::kVisCorFlowIterations())));
removedParameters_.insert(std::make_pair("OdomFlow/Eps", std::make_pair(true, Parameters::kVisCorFlowEps())));
removedParameters_.insert(std::make_pair("OdomFlow/MaxLevel", std::make_pair(true, Parameters::kVisCorFlowMaxLevel())));
removedParameters_.insert(std::make_pair("OdomSubPix/WinSize", std::make_pair(true, Parameters::kVisSubPixWinSize())));
removedParameters_.insert(std::make_pair("OdomSubPix/Iterations", std::make_pair(true, Parameters::kVisSubPixIterations())));
@@ -173,8 +180,8 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
removedParameters_.insert(std::make_pair("LccReextract/MaxWords", std::make_pair(false, Parameters::kVisMaxFeatures())));
removedParameters_.insert(std::make_pair("LccReextract/MaxDepth", std::make_pair(false, Parameters::kVisMaxDepth())));
removedParameters_.insert(std::make_pair("LccReextract/RoiRatios", std::make_pair(false, Parameters::kVisRoiRatios())));
removedParameters_.insert(std::make_pair("LccReextract/NNType", std::make_pair(false, Parameters::kVisNNType())));
removedParameters_.insert(std::make_pair("LccReextract/NNDR", std::make_pair(false, Parameters::kVisNNDR())));
removedParameters_.insert(std::make_pair("LccReextract/NNType", std::make_pair(false, Parameters::kVisCorNNType())));
removedParameters_.insert(std::make_pair("LccReextract/NNDR", std::make_pair(false, Parameters::kVisCorNNDR())));
removedParameters_.insert(std::make_pair("LccBow/EstimationType", std::make_pair(false, Parameters::kVisEstimationType())));
removedParameters_.insert(std::make_pair("LccBow/InlierDistance", std::make_pair(false, Parameters::kVisInlierDistance())));
@@ -235,8 +242,8 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
removedParameters_.insert(std::make_pair("Odom/Type", std::make_pair(true, Parameters::kVisFeatureType())));
removedParameters_.insert(std::make_pair("Odom/MaxWords", std::make_pair(true, Parameters::kVisMaxFeatures())));
removedParameters_.insert(std::make_pair("Odom/LocalHistory", std::make_pair(true, Parameters::kOdomBowLocalHistorySize())));
removedParameters_.insert(std::make_pair("Odom/NearestNeighbor", std::make_pair(true, Parameters::kVisNNType())));
removedParameters_.insert(std::make_pair("Odom/NNDR", std::make_pair(true, Parameters::kVisNNDR())));
removedParameters_.insert(std::make_pair("Odom/NearestNeighbor", std::make_pair(true, Parameters::kVisCorNNType())));
removedParameters_.insert(std::make_pair("Odom/NNDR", std::make_pair(true, Parameters::kVisCorNNDR())));
}
return removedParameters_;
}
@@ -420,7 +427,10 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet
}
uInsert(parameters, ParametersPair(key, iter->second));
if(Parameters::getDefaultParameters().find(key) != Parameters::getDefaultParameters().end())
{
uInsert(parameters, ParametersPair(key, iter->second));
}
}
}
}
corelib/src/RegistrationIcp.cpp
+8 -8
View File
@@ -77,14 +77,14 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
UASSERT_MSG(_pointToPlaneNormalNeighbors > 0, uFormat("value=%d", _pointToPlaneNormalNeighbors).c_str());
}
Transform RegistrationIcp::computeTransformation(
const Signature & fromSignature,
const Signature & toSignature,
Transform RegistrationIcp::computeTransformationMod(
Signature & fromSignature,
Signature & toSignature,
Transform guess,
std::string * rejectedMsg,
int * inliersOut,
std::vector<int> * inliersOut,
float * varianceOut,
float * inliersRatioOut)
float * inliersRatioOut) const
{
return computeTransformation(
fromSignature.sensorData(),
@@ -101,9 +101,9 @@ Transform RegistrationIcp::computeTransformation(
const SensorData & dataTo,
Transform guess,
std::string * rejectedMsg,
int * inliersOut,
std::vector<int> * inliersOut,
float * varianceOut,
float * inliersRatioOut)
float * inliersRatioOut) const
{
UDEBUG("Guess transform = %s", guess.prettyPrint().c_str());
UDEBUG("Voxel size=%f", _voxelSize);
@@ -298,7 +298,7 @@ Transform RegistrationIcp::computeTransformation(
}
if(inliersOut)
{
*inliersOut = correspondences;
inliersOut->push_back(correspondences);
}
if(inliersRatioOut)
{
corelib/src/RegistrationVis.cpp
+570 -205
View File
@@ -30,11 +30,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/RegistrationVis.h>
#include <rtabmap/core/util3d_motion_estimation.h>
#include <rtabmap/core/util3d_features.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/VWDictionary.h>
#include <rtabmap/core/Features2d.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UMath.h>
namespace rtabmap {
@@ -46,8 +49,15 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters) :
_force2D(Parameters::defaultVisForce2D()),
_epipolarGeometryVar(Parameters::defaultVisEpipolarGeometryVar()),
_estimationType(Parameters::defaultVisEstimationType()),
_forwardEstimateOnly(Parameters::defaultVisForwardEstOnly()),
_PnPReprojError(Parameters::defaultVisPnPReprojError()),
_PnPFlags(Parameters::defaultVisPnPFlags())
_PnPFlags(Parameters::defaultVisPnPFlags()),
_PnPOpenCV2(Parameters::defaultVisPnPOpenCV2()),
_correspondencesApproach(Parameters::defaultVisCorType()),
_flowWinSize(Parameters::defaultVisCorFlowWinSize()),
_flowIterations(Parameters::defaultVisCorFlowIterations()),
_flowEps(Parameters::defaultVisCorFlowEps()),
_flowMaxLevel(Parameters::defaultVisCorFlowMaxLevel())
{
_featureParameters = Parameters::getDefaultParameters();
uInsert(_featureParameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // make sure it is incremental
@@ -59,10 +69,10 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters) :
uInsert(_featureParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
uInsert(_featureParameters, ParametersPair(Parameters::kMemGenerateIds(), "true"));
uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), _featureParameters.at(Parameters::kVisNNType())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), _featureParameters.at(Parameters::kVisNNDR())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), _featureParameters.at(Parameters::kVisCorNNType())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), _featureParameters.at(Parameters::kVisCorNNDR())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpDetectorStrategy(), _featureParameters.at(Parameters::kVisFeatureType())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpWordsPerImage(), _featureParameters.at(Parameters::kVisMaxFeatures())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxFeatures(), _featureParameters.at(Parameters::kVisMaxFeatures())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxDepth(), _featureParameters.at(Parameters::kVisMaxDepth())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpMinDepth(), _featureParameters.at(Parameters::kVisMinDepth())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpRoiRatios(), _featureParameters.at(Parameters::kVisRoiRatios())));
@@ -86,6 +96,12 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _epipolarGeometryVar);
Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags);
Parameters::parse(parameters, Parameters::kVisPnPOpenCV2(), _PnPOpenCV2);
Parameters::parse(parameters, Parameters::kVisCorType(), _correspondencesApproach);
Parameters::parse(parameters, Parameters::kVisCorFlowWinSize(), _flowWinSize);
Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), _flowIterations);
Parameters::parse(parameters, Parameters::kVisCorFlowEps(), _flowEps);
Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), _flowMaxLevel);
UASSERT_MSG(_minInliers >= 1, uFormat("value=%d", _minInliers).c_str());
UASSERT_MSG(_inlierDistance > 0.0f, uFormat("value=%f", _inlierDistance).c_str());
@@ -101,13 +117,13 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
}
}
if(uContains(parameters, Parameters::kVisNNType()))
if(uContains(parameters, Parameters::kVisCorNNType()))
{
uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), parameters.at(Parameters::kVisNNType())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), parameters.at(Parameters::kVisCorNNType())));
}
if(uContains(parameters, Parameters::kVisNNDR()))
if(uContains(parameters, Parameters::kVisCorNNDR()))
{
uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), parameters.at(Parameters::kVisNNDR())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), parameters.at(Parameters::kVisCorNNDR())));
}
if(uContains(parameters, Parameters::kVisFeatureType()))
{
@@ -115,7 +131,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
}
if(uContains(parameters, Parameters::kVisMaxFeatures()))
{
uInsert(_featureParameters, ParametersPair(Parameters::kKpWordsPerImage(), parameters.at(Parameters::kVisMaxFeatures())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxFeatures(), parameters.at(Parameters::kVisMaxFeatures())));
}
if(uContains(parameters, Parameters::kVisMaxDepth()))
{
@@ -147,235 +163,593 @@ RegistrationVis::~RegistrationVis()
{
}
Transform RegistrationVis::computeTransformation(
const Signature & fromSignature,
const Signature & toSignature,
Transform guess, // guess is ignored for RegistrationVis
Transform RegistrationVis::computeTransformationMod(
Signature & fromSignature,
Signature & toSignature,
Transform guess, // guess is only used by Optical Flow correspondences (flowMaxLevel is set to 0 when guess is used)
std::string * rejectedMsg,
int * inliersOut,
std::vector<int> * inliersOut,
float * varianceOut,
float * inliersRatioOut)
float * inliersRatioOut) const
{
Transform transform;
UDEBUG("%s=%d", Parameters::kVisMinInliers().c_str(), _minInliers);
UDEBUG("%s=%f", Parameters::kVisInlierDistance().c_str(), _inlierDistance);
UDEBUG("%s=%d", Parameters::kVisIterations().c_str(), _iterations);
UDEBUG("%s=%d", Parameters::kVisForce2D().c_str(), _force2D?1:0);
UDEBUG("%s=%d", Parameters::kVisEstimationType().c_str(), _estimationType);
UDEBUG("%s=%d", Parameters::kVisForwardEstOnly().c_str(), _forwardEstimateOnly);
UDEBUG("%s=%f", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar);
UDEBUG("%s=%f", Parameters::kVisPnPReprojError().c_str(), _PnPReprojError);
UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags);
UDEBUG("%s=%d", Parameters::kVisCorType().c_str(), _correspondencesApproach?1:0);
UDEBUG("%s=%d", Parameters::kVisCorFlowWinSize().c_str(), _flowWinSize);
UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations);
UDEBUG("%s=%f", Parameters::kVisCorFlowEps().c_str(), _flowEps);
UDEBUG("%s=%d", Parameters::kVisCorFlowMaxLevel().c_str(), _flowMaxLevel);
UDEBUG("Input(%d): from=%d words, %d 3D words, %d kpts, %d descriptors",
fromSignature.id(),
(int)fromSignature.getWords().size(),
(int)fromSignature.getWords3().size(),
(int)fromSignature.sensorData().keypoints().size(),
fromSignature.sensorData().descriptors().rows);
UDEBUG("Input(%d): to=%d words, %d 3D words, %d kpts, %d descriptors",
toSignature.id(),
(int)toSignature.getWords().size(),
(int)toSignature.getWords3().size(),
(int)toSignature.sensorData().keypoints().size(),
toSignature.sensorData().descriptors().rows);
std::string msg;
// Guess transform from visual words
int inliersCount= 0;
double variance = 1.0;
// Extract features?
const std::multimap<int, cv::KeyPoint> * wordsFrom = 0;
const std::multimap<int, cv::KeyPoint> * wordsTo = 0;
const std::multimap<int, pcl::PointXYZ> * words3From = 0;
const std::multimap<int, pcl::PointXYZ> * words3To = 0;
std::multimap<int, cv::KeyPoint> extractedWordsFrom, extractedWordsTo;
std::multimap<int, pcl::PointXYZ> extractedWords3From, extractedWords3To;
if(fromSignature.getWords().size() == 0 && toSignature.getWords().size() == 0)
////////////////////
// Find correspondences
////////////////////
if(fromSignature.getWords().size() && fromSignature.getWords3().size() &&
toSignature.getWords().size() && (_estimationType==1 || toSignature.getWords3().size()))
{
// Use the Memory class to extract features
Memory memory(_featureParameters);
// Add signatures
SensorData dataFrom = fromSignature.sensorData();
SensorData dataTo = toSignature.sensorData();
// make sure there are no features already in the SensorData
dataFrom.setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
dataTo.setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
UTimer timeT;
memory.update(dataFrom);
if(memory.getLastWorkingSignature() == 0)
{
UWARN("Failed to extract features for node %d", dataFrom.id());
}
else
{
extractedWordsFrom = memory.getLastWorkingSignature()->getWords();
extractedWords3From = memory.getLastWorkingSignature()->getWords3();
UDEBUG("timeTo = %fs", timeT.ticks());
memory.update(dataTo);
if(memory.getLastWorkingSignature() == 0)
{
UWARN("Failed to extract features for node %d", dataTo.id());
}
else
{
extractedWordsTo = memory.getLastWorkingSignature()->getWords();
extractedWords3To = memory.getLastWorkingSignature()->getWords3();
UDEBUG("timeFrom = %fs", timeT.ticks());
}
}
wordsFrom = &extractedWordsFrom;
wordsTo = &extractedWordsTo;
words3From = &extractedWords3From;
words3To = &extractedWords3To;
// no need to extract new features, we have all the data we need
UDEBUG("");
}
else
{
wordsFrom = &fromSignature.getWords();
wordsTo = &toSignature.getWords();
words3From = &fromSignature.getWords3();
words3To = &toSignature.getWords3();
UDEBUG("");
// just some checks to make sure that input data are ok
UASSERT((fromSignature.getWords().empty() && fromSignature.getWords3().empty())||
(fromSignature.getWords().size() == fromSignature.getWords3().size()));
UASSERT(fromSignature.sensorData().keypoints().size() == fromSignature.sensorData().descriptors().rows ||
fromSignature.sensorData().descriptors().rows == 0);
UASSERT((toSignature.getWords().empty() && toSignature.getWords3().empty())||
(toSignature.getWords().size() == toSignature.getWords3().size()));
UASSERT(toSignature.sensorData().keypoints().size() == toSignature.sensorData().descriptors().rows ||
toSignature.sensorData().descriptors().rows == 0);
UASSERT(fromSignature.sensorData().imageRaw().type() == CV_8UC1 ||
fromSignature.sensorData().imageRaw().type() == CV_8UC3);
UASSERT(toSignature.sensorData().imageRaw().type() == CV_8UC1 ||
toSignature.sensorData().imageRaw().type() == CV_8UC3);
Feature2D * detector = Feature2D::create(_featureParameters);
std::vector<cv::KeyPoint> kptsFrom;
if(fromSignature.getWords().empty())
{
if(fromSignature.sensorData().keypoints().empty())
{
if(fromSignature.sensorData().imageRaw().channels() > 1)
{
cv::Mat tmp;
cv::cvtColor(fromSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
fromSignature.sensorData().setImageRaw(tmp);
}
kptsFrom = detector->generateKeypoints(fromSignature.sensorData().imageRaw());
}
else
{
kptsFrom = fromSignature.sensorData().keypoints();
}
}
else
{
kptsFrom = uValues(fromSignature.getWords());
}
std::multimap<int, cv::KeyPoint> wordsFrom;
std::multimap<int, cv::KeyPoint> wordsTo;
std::multimap<int, cv::Point3f> words3From;
std::multimap<int, cv::Point3f> words3To;
if(_correspondencesApproach == 1) //Optical Flow
{
UDEBUG("");
// convert to grayscale
if(fromSignature.sensorData().imageRaw().channels() > 1)
{
cv::Mat tmp;
cv::cvtColor(fromSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
fromSignature.sensorData().setImageRaw(tmp);
}
if(toSignature.sensorData().imageRaw().channels() > 1)
{
cv::Mat tmp;
cv::cvtColor(toSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
toSignature.sensorData().setImageRaw(tmp);
}
std::vector<cv::Point3f> kptsFrom3D;
if(fromSignature.getWords3().empty())
{
kptsFrom3D = detector->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
}
else
{
kptsFrom3D = uValues(fromSignature.getWords3());
}
if(!toSignature.sensorData().imageRaw().empty())
{
std::vector<cv::Point2f> cornersFrom;
cv::KeyPoint::convert(kptsFrom, cornersFrom);
std::vector<cv::Point2f> cornersTo;
bool guessSet = !guess.isIdentity() && !guess.isNull();
if(guessSet)
{
Transform localTransform = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].localTransform():fromSignature.sensorData().stereoCameraModel().left().localTransform();
Transform guessCameraRef = (guess * localTransform).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
(double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
(double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
cv::Mat K = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].K():fromSignature.sensorData().stereoCameraModel().left().K();
cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), cornersTo);
}
// Find features in the new left image
UDEBUG("guessSet = %d", guessSet?1:0);
std::vector<unsigned char> status;
std::vector<float> err;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
int winSize = _flowWinSize;
cv::calcOpticalFlowPyrLK(
fromSignature.sensorData().imageRaw(),
toSignature.sensorData().imageRaw(),
cornersFrom,
cornersTo,
status,
err,
cv::Size(winSize, winSize),
guessSet?0:_flowMaxLevel,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, _flowIterations, _flowEps),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (guessSet?cv::OPTFLOW_USE_INITIAL_FLOW:0), 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
UASSERT(kptsFrom.size() == kptsFrom3D.size());
std::vector<cv::KeyPoint> kptsTo(kptsFrom.size());
std::vector<cv::Point3f> kptsFrom3DKept(kptsFrom3D.size());
int ki = 0;
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i] &&
uIsInBounds(cornersTo[i].x, 0.0f, float(toSignature.sensorData().depthOrRightRaw().cols)) &&
uIsInBounds(cornersTo[i].y, 0.0f, float(toSignature.sensorData().depthOrRightRaw().rows)))
{
kptsFrom[ki] = cv::KeyPoint(cornersFrom[i], 1);
kptsFrom3DKept[ki] = kptsFrom3D[i];
kptsTo[ki++] = cv::KeyPoint(cornersTo[i], 1);
}
}
kptsFrom.resize(ki);
kptsTo.resize(ki);
kptsFrom3DKept.resize(ki);
std::vector<cv::Point3f> kptsTo3D;
if(_estimationType == 0 || (_estimationType == 1 && !_varianceFromInliersCount) || !_forwardEstimateOnly)
{
kptsTo3D = detector->generateKeypoints3D(toSignature.sensorData(), kptsTo);
}
UASSERT(kptsFrom.size() == kptsFrom3DKept.size());
UASSERT(kptsFrom.size() == kptsTo.size());
UASSERT(kptsTo3D.size() == 0 || kptsTo.size() == kptsTo3D.size());
for(unsigned int i=0; i< kptsFrom3DKept.size(); ++i)
{
wordsFrom.insert(std::make_pair(i, kptsFrom[i]));
words3From.insert(std::make_pair(i, kptsFrom3DKept[i]));
wordsTo.insert(std::make_pair(i, kptsTo[i]));
if(kptsTo3D.size())
{
words3To.insert(std::make_pair(i, kptsTo3D[i]));
}
}
toSignature.sensorData().setFeatures(kptsTo, cv::Mat());
}
else
{
UASSERT(kptsFrom.size() == kptsFrom3D.size());
for(unsigned int i=0; i< kptsFrom3D.size(); ++i)
{
if(util3d::isFinite(kptsFrom3D[i]))
{
wordsFrom.insert(std::make_pair(i, kptsFrom[i]));
words3From.insert(std::make_pair(i, kptsFrom3D[i]));
}
}
toSignature.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
}
fromSignature.sensorData().setFeatures(kptsFrom, cv::Mat());
}
else // Features Matching
{
UDEBUG("");
std::vector<cv::KeyPoint> kptsTo;
if(toSignature.getWords().empty())
{
if(toSignature.sensorData().keypoints().empty() &&
!toSignature.sensorData().imageRaw().empty())
{
if(toSignature.sensorData().imageRaw().channels() > 1)
{
cv::Mat tmp;
cv::cvtColor(toSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
toSignature.sensorData().setImageRaw(tmp);
}
kptsTo = detector->generateKeypoints(toSignature.sensorData().imageRaw());
}
else
{
kptsTo = toSignature.sensorData().keypoints();
}
}
else
{
kptsTo = uValues(toSignature.getWords());
}
// extract descriptors
UDEBUG("kptsFrom=%d", (int)kptsFrom.size());
UDEBUG("kptsTo=%d", (int)kptsTo.size());
cv::Mat descriptorsFrom;
cv::Mat descriptorsTo;
if(fromSignature.getWords().empty() && fromSignature.sensorData().descriptors().rows == kptsFrom.size())
{
descriptorsFrom = fromSignature.sensorData().descriptors();
}
else
{
descriptorsFrom = detector->generateDescriptors(fromSignature.sensorData().imageRaw(), kptsFrom);
}
if(toSignature.getWords().empty() && toSignature.sensorData().descriptors().rows == kptsTo.size())
{
descriptorsTo = toSignature.sensorData().descriptors();
}
else if(!toSignature.sensorData().imageRaw().empty())
{
descriptorsTo = detector->generateDescriptors(toSignature.sensorData().imageRaw(), kptsTo);
}
// create 3D keypoints
std::vector<cv::Point3f> kptsFrom3D;
std::vector<cv::Point3f> kptsTo3D;
if(fromSignature.getWords3().empty())
{
kptsFrom3D = detector->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
}
else
{
kptsFrom3D = uValues(fromSignature.getWords3());
}
if((_estimationType == 0 || (_estimationType == 1 && !_varianceFromInliersCount) || !_forwardEstimateOnly) &&
toSignature.getWords3().empty() &&
!toSignature.sensorData().imageRaw().empty())
{
kptsTo3D = detector->generateKeypoints3D(toSignature.sensorData(), kptsTo);
}
else
{
kptsTo3D = uValues(toSignature.getWords3());
}
// We have all data we need here, so match using the vocabulary
UDEBUG("descriptorsFrom=%d", descriptorsFrom.rows);
VWDictionary dictionary(_featureParameters);
std::list<int> fromWordIds = dictionary.addNewWords(descriptorsFrom, 1);
std::list<int> toWordIds;
UDEBUG("descriptorsTo=%d", descriptorsTo.rows);
if(descriptorsTo.rows)
{
dictionary.update();
toWordIds = dictionary.addNewWords(descriptorsTo, 2);
}
dictionary.clear(false);
std::multiset<int> fromWordIdsSet(fromWordIds.begin(), fromWordIds.end());
std::multiset<int> toWordIdsSet(toWordIds.begin(), toWordIds.end());
UASSERT(kptsFrom3D.size() == kptsFrom.size());
UASSERT(fromWordIds.size() == kptsFrom.size());
int i=0;
for(std::list<int>::iterator iter=fromWordIds.begin(); iter!=fromWordIds.end(); ++iter)
{
if(fromWordIdsSet.count(*iter) == 1)
{
wordsFrom.insert(std::make_pair(*iter, kptsFrom[i]));
words3From.insert(std::make_pair(*iter, kptsFrom3D[i]));
}
++i;
}
UASSERT(kptsTo3D.size() == 0 || kptsTo3D.size() == kptsTo.size());
UASSERT(toWordIds.size() == kptsTo.size());
i=0;
for(std::list<int>::iterator iter=toWordIds.begin(); iter!=toWordIds.end(); ++iter)
{
if(toWordIdsSet.count(*iter) == 1)
{
wordsTo.insert(std::make_pair(*iter, kptsTo[i]));
if(kptsTo3D.size())
{
words3To.insert(std::make_pair(*iter, kptsTo3D[i]));
}
}
++i;
}
//remove doubles
fromSignature.sensorData().setFeatures(kptsFrom, descriptorsFrom);
toSignature.sensorData().setFeatures(kptsTo, descriptorsTo);
}
fromSignature.setWords(wordsFrom);
fromSignature.setWords3(words3From);
toSignature.setWords(wordsTo);
toSignature.setWords3(words3To);
delete detector;
}
if(_estimationType == 2) // Epipolar Geometry
/////////////////////
// Motion estimation
/////////////////////
Transform transform;
float variance = 1.0f;
if(toSignature.getWords().size() || !toSignature.sensorData().imageRaw().empty())
{
if(!toSignature.sensorData().stereoCameraModel().isValid() &&
(toSignature.sensorData().cameraModels().size() != 1 ||
!toSignature.sensorData().cameraModels()[0].isValid()))
Transform transforms[2];
std::vector<int> inliers[2];
double variances[2] = {1.0f};
for(int dir=0; dir<(!_forwardEstimateOnly?2:1); ++dir)
{
UERROR("Calibrated camera required (multi-cameras not supported).");
}
else if((int)wordsFrom->size() >= _minInliers &&
(int)wordsTo->size() >= _minInliers)
{
UASSERT(fromSignature.sensorData().stereoCameraModel().isValid() || (fromSignature.sensorData().cameraModels().size() == 1 && fromSignature.sensorData().cameraModels()[0].isValid()));
const CameraModel & cameraModel = fromSignature.sensorData().stereoCameraModel().isValid()?fromSignature.sensorData().stereoCameraModel().left():fromSignature.sensorData().cameraModels()[0];
// we only need the camera transform, send guess words3 for scale estimation
Transform cameraTransform;
std::multimap<int, pcl::PointXYZ> inliers3D = util3d::generateWords3DMono(
*wordsFrom,
*wordsTo,
cameraModel,
cameraTransform,
_iterations,
_PnPReprojError,
_PnPFlags, // cv::SOLVEPNP_ITERATIVE
1.0f,
0.99f,
*words3From, // for scale estimation
&variance);
inliersCount = (int)inliers3D.size();
if(!cameraTransform.isNull())
// A to B
const Signature * signatureA;
const Signature * signatureB;
if(dir == 0)
{
if((int)inliers3D.size() >= _minInliers)
signatureA = &fromSignature;
signatureB = &toSignature;
}
else
{
signatureA = &toSignature;
signatureB = &fromSignature;
}
if(_estimationType == 2) // Epipolar Geometry
{
UDEBUG("");
if(!signatureB->sensorData().stereoCameraModel().isValid() &&
(signatureB->sensorData().cameraModels().size() != 1 ||
!signatureB->sensorData().cameraModels()[0].isValid()))
{
if(variance <= _epipolarGeometryVar)
UERROR("Calibrated camera required (multi-cameras not supported).");
}
else if((int)signatureA->getWords().size() >= _minInliers &&
(int)signatureB->getWords().size() >= _minInliers)
{
UASSERT(signatureA->sensorData().stereoCameraModel().isValid() || (signatureA->sensorData().cameraModels().size() == 1 && signatureA->sensorData().cameraModels()[0].isValid()));
const CameraModel & cameraModel = signatureA->sensorData().stereoCameraModel().isValid()?signatureA->sensorData().stereoCameraModel().left():signatureA->sensorData().cameraModels()[0];
// we only need the camera transform, send guess words3 for scale estimation
Transform cameraTransform;
std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
uMultimapToMapUnique(signatureA->getWords()),
uMultimapToMapUnique(signatureB->getWords()),
cameraModel,
cameraTransform,
_iterations,
_PnPReprojError,
_PnPFlags, // cv::SOLVEPNP_ITERATIVE
_PnPOpenCV2,
1.0f,
0.99f,
uMultimapToMapUnique(signatureA->getWords3()), // for scale estimation
&variances[dir]);
inliers[dir] = uKeys(inliers3D);
if(!cameraTransform.isNull())
{
transform = cameraTransform;
if((int)inliers3D.size() >= _minInliers)
{
if(variances[dir] <= _epipolarGeometryVar)
{
transforms[dir] = cameraTransform;
}
else
{
msg = uFormat("Variance is too high! (max inlier distance=%f, variance=%f)", _epipolarGeometryVar, variances[dir]);
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("Not enough inliers %d < %d", (int)inliers3D.size(), _minInliers);
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("Variance is too high! (max inlier distance=%f, variance=%f)", _epipolarGeometryVar, variance);
msg = uFormat("No camera transform found");
UINFO(msg.c_str());
}
}
else if(signatureA->getWords().size() == 0)
{
msg = uFormat("No enough features (%d)", (int)signatureA->getWords().size());
UWARN(msg.c_str());
}
else
{
msg = uFormat("No camera model");
UWARN(msg.c_str());
}
}
else if(_estimationType == 1) // PnP
{
UDEBUG("");
if(!signatureB->sensorData().stereoCameraModel().isValid() &&
(signatureB->sensorData().cameraModels().size() != 1 ||
!signatureB->sensorData().cameraModels()[0].isValid()))
{
UERROR("Calibrated camera required (multi-cameras not supported). Id=%d Models=%d StereoModel=%d weight=%d",
signatureB->id(),
(int)signatureB->sensorData().cameraModels().size(),
signatureB->sensorData().stereoCameraModel().isValid()?1:0,
signatureB->getWeight());
}
else
{
UDEBUG("words from3D=%d to2D=%d", (int)signatureA->getWords3().size(), (int)signatureB->getWords().size());
// 3D to 2D
if((int)signatureA->getWords3().size() >= _minInliers &&
(int)signatureB->getWords().size() >= _minInliers)
{
UASSERT(signatureB->sensorData().stereoCameraModel().isValid() || (signatureB->sensorData().cameraModels().size() == 1 && signatureB->sensorData().cameraModels()[0].isValid()));
const CameraModel & cameraModel = signatureB->sensorData().stereoCameraModel().isValid()?signatureB->sensorData().stereoCameraModel().left():signatureB->sensorData().cameraModels()[0];
std::vector<int> inliersV;
transforms[dir] = util3d::estimateMotion3DTo2D(
uMultimapToMapUnique(signatureA->getWords3()),
uMultimapToMapUnique(signatureB->getWords()),
cameraModel,
_minInliers,
_iterations,
_PnPReprojError,
_PnPFlags,
_PnPOpenCV2,
dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
uMultimapToMapUnique(signatureA->getWords3()),
_varianceFromInliersCount?0:&variances[dir],
0,
&inliersV);
inliers[dir] = inliersV;
if(transforms[dir].isNull())
{
msg = uFormat("Not enough inliers %d/%d between %d and %d",
(int)inliers[dir].size(), _minInliers, signatureA->id(), signatureB->id());
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)",
(int)signatureA->getWords3().size(), (int)signatureB->getWords().size(), _minInliers);
UINFO(msg.c_str());
}
}
}
else
{
UDEBUG("");
// 3D -> 3D
if((int)signatureA->getWords3().size() >= _minInliers &&
(int)signatureB->getWords3().size() >= _minInliers)
{
std::vector<int> inliersV;
transforms[dir] = util3d::estimateMotion3DTo3D(
uMultimapToMapUnique(signatureA->getWords3()),
uMultimapToMapUnique(signatureB->getWords3()),
_minInliers,
_inlierDistance,
_iterations,
_refineIterations,
&variances[dir],
0,
&inliersV);
inliers[dir] = inliersV;
if(transforms[dir].isNull())
{
msg = uFormat("Not enough inliers %d/%d between %d and %d",
(int)inliers[dir].size(), _minInliers, signatureA->id(), signatureB->id());
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("Not enough inliers %d < %d", (int)inliers3D.size(), _minInliers);
msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)",
(int)signatureA->getWords3().size(), (int)signatureB->getWords3().size(), _minInliers);
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("No camera transform found");
UINFO(msg.c_str());
}
}
else if(words3From->size() == 0)
{
msg = uFormat("No 3D guess words found");
UWARN(msg.c_str());
}
else
{
msg = uFormat("No camera model");
UWARN(msg.c_str());
}
}
else if(_estimationType == 1) // PnP
{
if(!toSignature.sensorData().stereoCameraModel().isValid() &&
(toSignature.sensorData().cameraModels().size() != 1 ||
!toSignature.sensorData().cameraModels()[0].isValid()))
{
UERROR("Calibrated camera required (multi-cameras not supported). Id=%d Models=%d StereoModel=%d weight=%d",
toSignature.id(),
(int)toSignature.sensorData().cameraModels().size(),
toSignature.sensorData().stereoCameraModel().isValid()?1:0,
toSignature.getWeight());
}
else
{
// 3D to 2D
if((int)words3From->size() >= _minInliers &&
(int)wordsTo->size() >= _minInliers)
{
UASSERT(toSignature.sensorData().stereoCameraModel().isValid() || (toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValid()));
const CameraModel & cameraModel = toSignature.sensorData().stereoCameraModel().isValid()?toSignature.sensorData().stereoCameraModel().left():toSignature.sensorData().cameraModels()[0];
std::vector<int> inliersV;
transform = util3d::estimateMotion3DTo2D(
uMultimapToMap(*words3From),
uMultimapToMap(*wordsTo),
cameraModel,
_minInliers,
_iterations,
_PnPReprojError,
_PnPFlags,
Transform::getIdentity(),
uMultimapToMap(*words3To),
_varianceFromInliersCount?0:&variance,
0,
&inliersV);
inliersCount = (int)inliersV.size();
if(transform.isNull())
if(!transforms[1].isNull())
{
transforms[1] = transforms[1].inverse();
}
UDEBUG("t1=%s", transforms[0].prettyPrint().c_str());
UDEBUG("t2=%s", transforms[1].prettyPrint().c_str());
if(!transforms[1].isNull())
{
if(transforms[0].isNull())
{
transform = transforms[1];
if(inliersOut)
{
msg = uFormat("Not enough inliers %d/%d between %d and %d",
inliersCount, _minInliers, fromSignature.id(), toSignature.id());
UINFO(msg.c_str());
*inliersOut = inliers[1];
}
variance = variances[1];
if(_varianceFromInliersCount)
{
variance = inliers[1].size() > 0?1.0f/float(inliers[1].size()):1.0f;
}
}
else
{
msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)",
(int)words3From->size(), (int)wordsTo->size(), _minInliers);
UINFO(msg.c_str());
}
}
}
else
{
// 3D -> 3D
if((int)words3From->size() >= _minInliers &&
(int)words3To->size() >= _minInliers)
{
std::vector<int> inliersV;
transform = util3d::estimateMotion3DTo3D(
uMultimapToMap(*words3From),
uMultimapToMap(*words3To),
_minInliers,
_inlierDistance,
_iterations,
_refineIterations,
&variance,
0,
&inliersV);
inliersCount = (int)inliersV.size();
if(transform.isNull())
{
msg = uFormat("Not enough inliers %d/%d between %d and %d",
inliersCount, _minInliers, fromSignature.id(), toSignature.id());
UINFO(msg.c_str());
transform = transforms[0].interpolate(0.5f, transforms[1]);
if(inliersOut)
{
*inliersOut = inliers[0];
}
variance = (variances[0]+variances[1])/2.0f;
if(_varianceFromInliersCount)
{
int avg = (inliers[0].size()+inliers[1].size())/2;
variance = avg>0?1.0f/float(avg):1.0f;
}
}
}
else
{
msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)",
(int)words3From->size(), (int)words3To->size(), _minInliers);
UINFO(msg.c_str());
transform = transforms[0];
if(inliersOut)
{
*inliersOut = inliers[0];
}
variance = variances[0];
if(_varianceFromInliersCount)
{
variance = inliers[0].size() > 0?1.0f/float(inliers[0].size()):1.0f;
}
}
}
if(!transform.isNull())
{
UDEBUG("");
// verify if it is a 180 degree transform, well verify > 90
float x,y,z, roll,pitch,yaw;
transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
@@ -395,19 +769,10 @@ Transform RegistrationVis::computeTransformation(
}
}
if(_varianceFromInliersCount)
{
variance = inliersCount > 0?1.0/double(inliersCount):1.0;
}
if(rejectedMsg)
{
*rejectedMsg = msg;
}
if(inliersOut)
{
*inliersOut = inliersCount;
}
if(varianceOut)
{
*varianceOut = variance>0.0f?variance:0.0001; // epsilon if exact transform
corelib/src/Rtabmap.cpp
+1 -1
View File
@@ -3114,7 +3114,7 @@ void Rtabmap::get3DMap(
SensorData data = _memory->getNodeData(*iter);
data.setId(*iter);
std::multimap<int, cv::KeyPoint> words;
std::multimap<int, pcl::PointXYZ> words3;
std::multimap<int, cv::Point3f> words3;
_memory->getNodeWords(*iter, words, words3);
signatures.insert(std::make_pair(*iter,
Signature(*iter,
corelib/src/Signature.cpp
+19 -3
View File
@@ -75,6 +75,22 @@ Signature::Signature(
UASSERT(_sensorData.id() == _id);
}
Signature::Signature(const SensorData & data) :
_id(data.id()),
_mapId(-1),
_stamp(data.stamp()),
_weight(0),
_label(""),
_saved(false),
_modified(true),
_linksModified(true),
_enabled(false),
_pose(Transform::getIdentity()),
_sensorData(data)
{
}
Signature::~Signature()
{
//UDEBUG("id=%d", _id);
@@ -175,7 +191,7 @@ void Signature::changeWordsRef(int oldWordId, int activeWordId)
std::list<cv::KeyPoint> kps = uValues(_words, oldWordId);
if(kps.size())
{
std::list<pcl::PointXYZ> pts = uValues(_words3, oldWordId);
std::list<cv::Point3f> pts = uValues(_words3, oldWordId);
_words.erase(oldWordId);
_words3.erase(oldWordId);
_wordsChanged.insert(std::make_pair(oldWordId, activeWordId));
@@ -183,9 +199,9 @@ void Signature::changeWordsRef(int oldWordId, int activeWordId)
{
_words.insert(std::pair<int, cv::KeyPoint>(activeWordId, (*iter)));
}
for(std::list<pcl::PointXYZ>::const_iterator iter=pts.begin(); iter!=pts.end(); ++iter)
for(std::list<cv::Point3f>::const_iterator iter=pts.begin(); iter!=pts.end(); ++iter)
{
_words3.insert(std::pair<int, pcl::PointXYZ>(activeWordId, (*iter)));
_words3.insert(std::pair<int, cv::Point3f>(activeWordId, (*iter)));
}
}
}
corelib/src/Stereo.cpp
+14
View File
@@ -32,6 +32,20 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
Stereo * Stereo::create(const ParametersMap & parameters)
{
bool opticalFlow = Parameters::defaultStereoOpticalFlow();
Parameters::parse(parameters, Parameters::kStereoOpticalFlow(), opticalFlow);
if(opticalFlow)
{
return new StereoOpticalFlow(parameters);
}
else
{
return new Stereo(parameters);
}
}
Stereo::Stereo(const ParametersMap & parameters) :
winWidth_(Parameters::defaultStereoWinWidth()),
winHeight_(Parameters::defaultStereoWinHeight()),
corelib/src/Transform.cpp
+8 -6
View File
@@ -67,21 +67,22 @@ Transform::Transform(float x, float y, float z, float roll, float pitch, float y
*this = fromEigen3f(t);
}
Transform::Transform(float x, float y, float z, float qx, float qy, float qz, float qw)
Transform::Transform(float x, float y, float z, float qx, float qy, float qz, float qw) :
data_(cv::Mat::zeros(3,4,CV_32FC1))
{
Eigen::Matrix3f rotation = Eigen::Quaternionf(qw, qx, qy, qz).toRotationMatrix();
data()[0] = rotation(0,0);
data()[1] = rotation(0,1);
data()[2] = rotation(0,2);
data()[3] = 0.0f;
data()[3] = x;
data()[4] = rotation(1,0);
data()[5] = rotation(1,1);
data()[6] = rotation(1,2);
data()[7] = 0.0f;
data()[7] = y;
data()[8] = rotation(2,0);
data()[9] = rotation(2,1);
data()[10] = rotation(2,2);
data()[11] = 0.0f;
data()[11] = z;
}
Transform::Transform(float x, float y, float theta)
@@ -92,7 +93,8 @@ Transform::Transform(float x, float y, float theta)
bool Transform::isNull() const
{
return (data()[0] == 0.0f &&
return (data_.empty() ||
(data()[0] == 0.0f &&
data()[1] == 0.0f &&
data()[2] == 0.0f &&
data()[3] == 0.0f &&
@@ -115,7 +117,7 @@ bool Transform::isNull() const
uIsNan(data()[8]) ||
uIsNan(data()[9]) ||
uIsNan(data()[10]) ||
uIsNan(data()[11]);
uIsNan(data()[11]));
}
bool Transform::isIdentity() const
corelib/src/VWDictionary.cpp
+10 -7
View File
@@ -686,16 +686,19 @@ void VWDictionary::update()
UDEBUG("");
}
void VWDictionary::clear()
void VWDictionary::clear(bool printWarningsIfNotEmpty)
{
ULOGGER_DEBUG("");
if(_visualWords.size() && _incrementalDictionary)
if(printWarningsIfNotEmpty)
{
UWARN("Visual dictionary would be already empty here (%d words still in dictionary).", (int)_visualWords.size());
}
if(_notIndexedWords.size())
{
UWARN("Not indexed words should be empty here (%d words still not indexed)", (int)_notIndexedWords.size());
if(_visualWords.size() && _incrementalDictionary)
{
UWARN("Visual dictionary would be already empty here (%d words still in dictionary).", (int)_visualWords.size());
}
if(_notIndexedWords.size())
{
UWARN("Not indexed words should be empty here (%d words still not indexed)", (int)_notIndexedWords.size());
}
}
for(std::map<int, VisualWord *>::iterator i=_visualWords.begin(); i!=_visualWords.end(); ++i)
{
corelib/src/util3d.cpp
+34 -8
View File
@@ -381,7 +381,8 @@ 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, model);
cv::Point3f pt = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(pt.x, pt.y, pt.z);
}
}
}
@@ -392,7 +393,8 @@ 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, model);
cv::Point3f pt = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(pt.x, pt.y, pt.z);
}
}
}
@@ -451,7 +453,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, model);
cv::Point3f ptXYZ = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
pt.x = ptXYZ.x;
pt.y = ptXYZ.y;
pt.z = ptXYZ.z;
@@ -858,7 +860,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserS
}
// inspired from ROS image_geometry/src/stereo_camera_model.cpp
pcl::PointXYZ projectDisparityTo3D(
cv::Point3f projectDisparityTo3D(
const cv::Point2f & pt,
float disparity,
const StereoCameraModel & model)
@@ -867,13 +869,13 @@ pcl::PointXYZ projectDisparityTo3D(
{
//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);
return cv::Point3f((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);
return cv::Point3f(bad_point, bad_point, bad_point);
}
pcl::PointXYZ projectDisparityTo3D(
cv::Point3f projectDisparityTo3D(
const cv::Point2f & pt,
const cv::Mat & disparity,
const StereoCameraModel & model)
@@ -888,7 +890,12 @@ pcl::PointXYZ projectDisparityTo3D(
float d = disparity.type() == CV_16SC1?float(disparity.at<short>(v,u))/16.0f:disparity.at<float>(v,u);
return projectDisparityTo3D(pt, d, model);
}
return pcl::PointXYZ(bad_point, bad_point, bad_point);
return cv::Point3f(bad_point, bad_point, bad_point);
}
bool isFinite(const cv::Point3f & pt)
{
return uIsFinite(pt.x) && uIsFinite(pt.y) && uIsFinite(pt.z);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds)
@@ -961,6 +968,25 @@ void savePCDWords(
}
}
void savePCDWords(
const std::string & fileName,
const std::multimap<int, cv::Point3f> & words,
const Transform & transform)
{
if(words.size())
{
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.resize(words.size());
int i=0;
for(std::multimap<int, cv::Point3f>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
cv::Point3f pt = transformPoint(iter->second, transform);
cloud[i++] = pcl::PointXYZ(pt.x, pt.y, pt.z);
}
pcl::io::savePCDFile(fileName, cloud);
}
}
pcl::PointCloud<pcl::PointXYZ>::Ptr loadBINCloud(const std::string & fileName, int dim)
{
UASSERT(dim > 0);
corelib/src/util3d_correspondences.cpp
+12 -12
View File
@@ -315,10 +315,10 @@ void findCorrespondences(
}
void findCorrespondences(
const std::multimap<int, pcl::PointXYZ> & words1,
const std::multimap<int, pcl::PointXYZ> & words2,
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
const std::multimap<int, cv::Point3f> & words1,
const std::multimap<int, cv::Point3f> & words2,
std::vector<cv::Point3f> & inliers1,
std::vector<cv::Point3f> & inliers2,
float maxDepth,
std::vector<int> * uniqueCorrespondences)
{
@@ -338,8 +338,8 @@ void findCorrespondences(
{
inliers1[oi] = words1.find(*iter)->second;
inliers2[oi] = words2.find(*iter)->second;
if(pcl::isFinite(inliers1[oi]) &&
pcl::isFinite(inliers2[oi]) &&
if(util3d::isFinite(inliers1[oi]) &&
util3d::isFinite(inliers2[oi]) &&
(inliers1[oi].x != 0 || inliers1[oi].y != 0 || inliers1[oi].z != 0) &&
(inliers2[oi].x != 0 || inliers2[oi].y != 0 || inliers2[oi].z != 0) &&
(maxDepth <= 0 || (inliers1[oi].x > 0 && inliers1[oi].x <= maxDepth && inliers2[oi].x>0 &&inliers2[oi].x<=maxDepth)))
@@ -361,10 +361,10 @@ void findCorrespondences(
}
void findCorrespondences(
const std::map<int, pcl::PointXYZ> & words1,
const std::map<int, pcl::PointXYZ> & words2,
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
const std::map<int, cv::Point3f> & words1,
const std::map<int, cv::Point3f> & words2,
std::vector<cv::Point3f> & inliers1,
std::vector<cv::Point3f> & inliers2,
float maxDepth,
std::vector<int> * correspondences)
{
@@ -384,8 +384,8 @@ void findCorrespondences(
{
inliers1[oi] = words1.find(*iter)->second;
inliers2[oi] = words2.find(*iter)->second;
if(pcl::isFinite(inliers1[oi]) &&
pcl::isFinite(inliers2[oi]) &&
if(util3d::isFinite(inliers1[oi]) &&
util3d::isFinite(inliers2[oi]) &&
(inliers1[oi].x != 0 || inliers1[oi].y != 0 || inliers1[oi].z != 0) &&
(inliers2[oi].x != 0 || inliers2[oi].y != 0 || inliers2[oi].z != 0) &&
(maxDepth <= 0 || (inliers1[oi].x > 0 && inliers1[oi].x <= maxDepth && inliers2[oi].x>0 &&inliers2[oi].x<=maxDepth)))
corelib/src/util3d_features.cpp
+52 -51
View File
@@ -31,12 +31,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_correspondences.h"
#include "rtabmap/core/util3d_motion_estimation.h"
#include "rtabmap/core/EpipolarGeometry.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UStl.h>
#include <opencv2/video/tracking.hpp>
@@ -46,7 +48,7 @@ namespace rtabmap
namespace util3d
{
pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
std::vector<cv::Point3f> generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
const CameraModel & cameraModel)
@@ -57,26 +59,26 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
return generateKeypoints3DDepth(keypoints, depth, models);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
std::vector<cv::Point3f> generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
const std::vector<CameraModel> & cameraModels)
{
UASSERT(!depth.empty() && (depth.type() == CV_32FC1 || depth.type() == CV_16UC1));
UASSERT(cameraModels.size());
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<cv::Point3f> keypoints3d;
if(!depth.empty())
{
UASSERT(int((depth.cols/cameraModels.size())*cameraModels.size()) == depth.cols);
float subImageWidth = depth.cols/cameraModels.size();
keypoints3d->resize(keypoints.size());
keypoints3d.resize(keypoints.size());
for(unsigned int i=0; i!=keypoints.size(); ++i)
{
int cameraIndex = int(keypoints[i].pt.x / subImageWidth);
UASSERT_MSG(cameraIndex < (int)cameraModels.size(),
uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f",
cameraIndex, (int)cameraModels.size(), keypoints[i].pt.x, subImageWidth).c_str());
pcl::PointXYZ pt = util3d::projectDepthTo3D(
pcl::PointXYZ ptXYZ = util3d::projectDepthTo3D(
depth,
keypoints[i].pt.x-subImageWidth*cameraIndex,
keypoints[i].pt.y,
@@ -86,46 +88,47 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
cameraModels.at(cameraIndex).fy(),
true);
if(pcl::isFinite(pt) &&
cv::Point3f pt(ptXYZ.x, ptXYZ.y, ptXYZ.z);
if(util3d::isFinite(pt) &&
!cameraModels.at(cameraIndex).localTransform().isNull() &&
!cameraModels.at(cameraIndex).localTransform().isIdentity())
{
pt = util3d::transformPoint(pt, cameraModels.at(cameraIndex).localTransform());
}
keypoints3d->at(i) = pt;
keypoints3d.at(i) = pt;
}
}
return keypoints3d;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDisparity(
std::vector<cv::Point3f> generateKeypoints3DDisparity(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & disparity,
const StereoCameraModel & stereoCameraModel)
{
UASSERT(!disparity.empty() && (disparity.type() == CV_16SC1 || disparity.type() == CV_32F));
UASSERT(stereoCameraModel.isValid());
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
keypoints3d->resize(keypoints.size());
std::vector<cv::Point3f> keypoints3d;
keypoints3d.resize(keypoints.size());
for(unsigned int i=0; i!=keypoints.size(); ++i)
{
pcl::PointXYZ pt = util3d::projectDisparityTo3D(
cv::Point3f pt = util3d::projectDisparityTo3D(
keypoints[i].pt,
disparity,
stereoCameraModel);
if(pcl::isFinite(pt) &&
if(util3d::isFinite(pt) &&
!stereoCameraModel.left().localTransform().isNull() &&
!stereoCameraModel.left().localTransform().isIdentity())
{
pt = util3d::transformPoint(pt, stereoCameraModel.left().localTransform());
}
keypoints3d->at(i) = pt;
keypoints3d.at(i) = pt;
}
return keypoints3d;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
std::vector<cv::Point3f> generateKeypoints3DStereo(
const std::vector<cv::Point2f> & leftCorners,
const std::vector<cv::Point2f> & rightCorners,
const StereoCameraModel & model,
@@ -135,23 +138,23 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
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());
std::vector<cv::Point3f> keypoints3d;
keypoints3d.resize(leftCorners.size());
float bad_point = std::numeric_limits<float>::quiet_NaN ();
for(unsigned int i=0; i<leftCorners.size(); ++i)
{
pcl::PointXYZ pt(bad_point, bad_point, bad_point);
cv::Point3f pt(bad_point, bad_point, bad_point);
if(mask.empty() || mask[i])
{
float disparity = leftCorners[i].x - rightCorners[i].x;
if(disparity != 0.0f)
{
pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
cv::Point3f tmpPt = util3d::projectDisparityTo3D(
leftCorners[i],
disparity,
model);
if(pcl::isFinite(tmpPt))
if(util3d::isFinite(tmpPt))
{
pt = tmpPt;
if(!model.localTransform().isNull() &&
@@ -163,7 +166,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
}
}
keypoints3d->at(i) = pt;
keypoints3d.at(i) = pt;
}
return keypoints3d;
}
@@ -172,23 +175,24 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
// return 3D points in ref referential
// If cameraTransform is not null, it will be used for triangulation instead of the camera transform computed by epipolar geometry
// when refGuess3D is passed and cameraTransform is null, scale will be estimated, returning scaled cloud and camera transform
std::multimap<int, pcl::PointXYZ> generateWords3DMono(
const std::multimap<int, cv::KeyPoint> & refWords,
const std::multimap<int, cv::KeyPoint> & nextWords,
std::map<int, cv::Point3f> generateWords3DMono(
const std::map<int, cv::KeyPoint> & refWords,
const std::map<int, cv::KeyPoint> & nextWords,
const CameraModel & cameraModel,
Transform & cameraTransform,
int pnpIterations,
float pnpReprojError,
int pnpFlags,
bool pnpOpenCV2,
float ransacParam1,
float ransacParam2,
const std::multimap<int, pcl::PointXYZ> & refGuess3D,
const std::map<int, cv::Point3f> & refGuess3D,
double * varianceOut)
{
UASSERT(cameraModel.isValid());
std::multimap<int, pcl::PointXYZ> words3D;
std::map<int, cv::Point3f> words3D;
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
if(EpipolarGeometry::findPairsUnique(refWords, nextWords, pairs) > 8)
if(EpipolarGeometry::findPairs(refWords, nextWords, pairs) > 8)
{
std::vector<unsigned char> status;
cv::Mat F = EpipolarGeometry::findFFromWords(pairs, status, ransacParam1, ransacParam2);
@@ -268,7 +272,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
// triangulate the points
//std::vector<double> reprojErrors;
//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
//std::vector<cv::Point3f> cloud;
//EpipolarGeometry::triangulatePoints(x_norm, xp_norm, P0, P, cloud, reprojErrors);
cv::Mat pts4D;
cv::triangulatePoints(P0, P, x_norm, xp_norm, pts4D);
@@ -282,23 +286,23 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
pts4D.col(i) /= pts4D.at<double>(3,i);
if(pts4D.at<double>(2,i) > 0)
{
words3D.insert(std::make_pair(indexes[i], util3d::transformPoint(pcl::PointXYZ(pts4D.at<double>(0,i), pts4D.at<double>(1,i), pts4D.at<double>(2,i)), cameraModel.localTransform())));
words3D.insert(std::make_pair(indexes[i], util3d::transformPoint(cv::Point3f(pts4D.at<double>(0,i), pts4D.at<double>(1,i), pts4D.at<double>(2,i)), cameraModel.localTransform())));
}
}
if(refGuess3D.size())
{
// scale estimation
pcl::PointCloud<pcl::PointXYZ>::Ptr inliersRef(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr inliersRefGuess(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<cv::Point3f> inliersRef;
std::vector<cv::Point3f> inliersRefGuess;
util3d::findCorrespondences(
words3D,
refGuess3D,
*inliersRef,
*inliersRefGuess,
inliersRef,
inliersRefGuess,
0);
if(inliersRef->size())
if(inliersRef.size())
{
// estimate the scale
float scale = 1.0f;
@@ -306,18 +310,18 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
if(!useCameraTransformGuess)
{
std::multimap<float, float> scales; // <variance, scale>
for(unsigned int i=0; i<inliersRef->size(); ++i)
for(unsigned int i=0; i<inliersRef.size(); ++i)
{
// using x as depth, assuming we are in global referential
float s = inliersRefGuess->at(i).x/inliersRef->at(i).x;
std::vector<float> errorSqrdDists(inliersRef->size());
for(unsigned int j=0; j<inliersRef->size(); ++j)
float s = inliersRefGuess.at(i).x/inliersRef.at(i).x;
std::vector<float> errorSqrdDists(inliersRef.size());
for(unsigned int j=0; j<inliersRef.size(); ++j)
{
pcl::PointXYZ refPt = inliersRef->at(j);
cv::Point3f refPt = inliersRef.at(j);
refPt.x *= s;
refPt.y *= s;
refPt.z *= s;
const pcl::PointXYZ & newPt = inliersRefGuess->at(j);
const cv::Point3f & newPt = inliersRefGuess.at(j);
errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z);
}
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
@@ -333,11 +337,11 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
else
{
//compute variance at scale=1
std::vector<float> errorSqrdDists(inliersRef->size());
for(unsigned int j=0; j<inliersRef->size(); ++j)
std::vector<float> errorSqrdDists(inliersRef.size());
for(unsigned int j=0; j<inliersRef.size(); ++j)
{
const pcl::PointXYZ & refPt = inliersRef->at(j);
const pcl::PointXYZ & newPt = inliersRefGuess->at(j);
const cv::Point3f & refPt = inliersRef.at(j);
const cv::Point3f & newPt = inliersRefGuess.at(j);
errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z);
}
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
@@ -358,8 +362,8 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
int oi=0;
for(unsigned int i=0; i<indexes.size(); ++i)
{
std::multimap<int, pcl::PointXYZ>::iterator iter = words3D.find(indexes[i]);
if(pcl::isFinite(iter->second))
std::multimap<int, cv::Point3f>::iterator iter = words3D.find(indexes[i]);
if(util3d::isFinite(iter->second))
{
iter->second.x *= scale;
iter->second.y *= scale;
@@ -384,7 +388,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
cv::Rodrigues(R, rvec);
cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
std::vector<int> inliersV;
cv::solvePnPRansac(
util3d::solvePnPRansac(
objectPoints,
imagePoints,
K,
@@ -394,13 +398,10 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
true,
pnpIterations,
pnpReprojError,
#if CV_MAJOR_VERSION < 3
0, // min inliers
#else
0.99, // confidence
#endif
inliersV,
pnpFlags);
pnpFlags,
pnpOpenCV2);
UDEBUG("PnP inliers = %d / %d", (int)inliersV.size(), (int)objectPoints.size());
corelib/src/util3d_motion_estimation.cpp
+154 -112
View File
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_correspondences.h"
#include "rtabmap/core/util3d.h"
namespace rtabmap
{
@@ -40,29 +41,23 @@ namespace rtabmap
namespace util3d
{
#if CV_MAJOR_VERSION >= 3
void solvePnPRansac(cv::InputArray _opoints, cv::InputArray _ipoints,
cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs,
cv::OutputArray _rvec, cv::OutputArray _tvec, bool useExtrinsicGuess,
int iterationsCount, float reprojectionError, int minInliersCount,
cv::OutputArray _inliers, int flags);
#endif
Transform estimateMotion3DTo2D(
const std::map<int, pcl::PointXYZ> & words3A,
const std::map<int, cv::Point3f> & words3A,
const std::map<int, cv::KeyPoint> & words2B,
const CameraModel & cameraModel,
int minInliers,
int iterations,
double reprojError,
int flagsPnP,
bool pnpOpenCV2,
const Transform & guess,
const std::map<int, pcl::PointXYZ> & words3B,
const std::map<int, cv::Point3f> & words3B,
double * varianceOut,
std::vector<int> * matchesOut,
std::vector<int> * inliersOut)
{
UASSERT(cameraModel.isValid());
UASSERT(!guess.isNull());
Transform transform;
std::vector<int> matches, inliers;
@@ -79,9 +74,10 @@ Transform estimateMotion3DTo2D(
matches.resize(ids.size());
for(unsigned int i=0; i<ids.size(); ++i)
{
if(words3A.find(ids[i]) != words3A.end())
std::map<int, cv::Point3f>::const_iterator iter=words3A.find(ids[i]);
if(iter != words3A.end() && util3d::isFinite(iter->second))
{
pcl::PointXYZ pt = words3A.find(ids[i])->second;
cv::Point3f pt = words3A.find(ids[i])->second;
objectPoints[oi].x = pt.x;
objectPoints[oi].y = pt.y;
objectPoints[oi].z = pt.z;
@@ -109,11 +105,7 @@ Transform estimateMotion3DTo2D(
cv::Mat tvec = (cv::Mat_<double>(1,3) <<
(double)guessCameraFrame.x(), (double)guessCameraFrame.y(), (double)guessCameraFrame.z());
#if CV_MAJOR_VERSION >= 3
solvePnPRansac(
#else
cv::solvePnPRansac(
#endif
util3d::solvePnPRansac(
objectPoints,
imagePoints,
K,
@@ -125,7 +117,8 @@ Transform estimateMotion3DTo2D(
reprojError,
0, // min inliers
inliers,
flagsPnP);
flagsPnP,
pnpOpenCV2);
if((int)inliers.size() >= minInliers)
{
@@ -143,11 +136,11 @@ Transform estimateMotion3DTo2D(
oi = 0;
for(unsigned int i=0; i<inliers.size(); ++i)
{
std::map<int, pcl::PointXYZ>::const_iterator iter = words3B.find(matches[inliers[i]]);
if(iter != words3B.end() && pcl::isFinite(iter->second))
std::map<int, cv::Point3f>::const_iterator iter = words3B.find(matches[inliers[i]]);
if(iter != words3B.end() && util3d::isFinite(iter->second))
{
const cv::Point3f & objPt = objectPoints[inliers[i]];
pcl::PointXYZ newPt = util3d::transformPoint(iter->second, transform);
cv::Point3f newPt = util3d::transformPoint(iter->second, transform);
errorSqrdDists[oi++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
}
}
@@ -191,8 +184,8 @@ Transform estimateMotion3DTo2D(
}
Transform estimateMotion3DTo3D(
const std::map<int, pcl::PointXYZ> & words3A,
const std::map<int, pcl::PointXYZ> & words3B,
const std::map<int, cv::Point3f> & words3A,
const std::map<int, cv::Point3f> & words3B,
int minInliers,
double inliersDistance,
int iterations,
@@ -202,29 +195,43 @@ Transform estimateMotion3DTo3D(
std::vector<int> * inliersOut)
{
Transform transform;
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>); // previous
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>); // new
std::vector<cv::Point3f> inliers1; // previous
std::vector<cv::Point3f> inliers2; // new
std::vector<int> matches;
util3d::findCorrespondences(
words3A,
words3B,
*inliers1,
*inliers2,
inliers1,
inliers2,
0,
&matches);
UASSERT(inliers1.size() == inliers2.size());
if(varianceOut)
{
*varianceOut = 1.0;
}
if((int)inliers1->size() >= minInliers)
if((int)inliers1.size() >= minInliers)
{
std::vector<int> inliers;
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2cloud(new pcl::PointCloud<pcl::PointXYZ>);
inliers1cloud->resize(inliers1.size());
inliers2cloud->resize(inliers1.size());
for(unsigned int i=0; i<inliers1.size(); ++i)
{
(*inliers1cloud)[i].x = inliers1[i].x;
(*inliers1cloud)[i].y = inliers1[i].y;
(*inliers1cloud)[i].z = inliers1[i].z;
(*inliers2cloud)[i].x = inliers2[i].x;
(*inliers2cloud)[i].y = inliers2[i].y;
(*inliers2cloud)[i].z = inliers2[i].z;
}
Transform t = util3d::transformFromXYZCorrespondences(
inliers2,
inliers1,
inliers2cloud,
inliers1cloud,
inliersDistance,
iterations,
refineIterations>0,
@@ -463,89 +470,124 @@ namespace pnpransac
cv::Mutex PnPSolver::syncMutex;
}
void solvePnPRansac(cv::InputArray _opoints, cv::InputArray _ipoints,
cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs,
cv::OutputArray _rvec, cv::OutputArray _tvec, bool useExtrinsicGuess,
int iterationsCount, float reprojectionError, int minInliersCount,
cv::OutputArray _inliers, int flags)
{
const int _rng_seed = 0;
cv::Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
cv::Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
CV_Assert(opoints.isContinuous());
CV_Assert(opoints.depth() == CV_32F || opoints.depth() == CV_64F);
CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3);
CV_Assert(ipoints.isContinuous());
CV_Assert(ipoints.depth() == CV_32F || ipoints.depth() == CV_64F);
CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2);
_rvec.create(3, 1, CV_64FC1);
_tvec.create(3, 1, CV_64FC1);
cv::Mat rvec = _rvec.getMat();
cv::Mat tvec = _tvec.getMat();
cv::Mat objectPoints = opoints.reshape(3, 1), imagePoints = ipoints.reshape(2, 1);
if (minInliersCount <= 0)
minInliersCount = objectPoints.cols;
pnpransac::Parameters params;
params.iterationsCount = iterationsCount;
params.minInliersCount = minInliersCount;
params.reprojectionError = reprojectionError;
params.useExtrinsicGuess = useExtrinsicGuess;
params.camera.init(cameraMatrix, distCoeffs);
params.flags = flags;
std::vector<int> localInliers;
cv::Mat localRvec, localTvec;
rvec.copyTo(localRvec);
tvec.copyTo(localTvec);
int bestIndex;
// TBB not used
if (objectPoints.cols >= pnpransac::MIN_POINTS_COUNT)
{
pnpransac::PnPSolver solver(objectPoints, imagePoints, params,
localRvec, localTvec, localInliers, bestIndex,
_rng_seed);
solver(0, iterationsCount);
}
if (localInliers.size() >= (size_t)pnpransac::MIN_POINTS_COUNT)
{
if (flags != CV_P3P)
{
int i, pointsCount = (int)localInliers.size();
cv::Mat inlierObjectPoints(1, pointsCount, CV_MAKE_TYPE(opoints.depth(), 3)), inlierImagePoints(1, pointsCount, CV_MAKE_TYPE(ipoints.depth(), 2));
for (i = 0; i < pointsCount; i++)
{
int index = localInliers[i];
cv::Mat colInlierImagePoints = inlierImagePoints(cv::Rect(i, 0, 1, 1));
imagePoints.col(index).copyTo(colInlierImagePoints);
cv::Mat colInlierObjectPoints = inlierObjectPoints(cv::Rect(i, 0, 1, 1));
objectPoints.col(index).copyTo(colInlierObjectPoints);
}
solvePnP(inlierObjectPoints, inlierImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, false, flags);
}
localRvec.copyTo(rvec);
localTvec.copyTo(tvec);
if (_inliers.needed())
cv::Mat(localInliers).copyTo(_inliers);
}
else
{
tvec.setTo(cv::Scalar(0));
cv::Mat R = cv::Mat::eye(3, 3, CV_64F);
Rodrigues(R, rvec);
if( _inliers.needed() )
_inliers.release();
}
return;
}
#endif
void solvePnPRansac(
cv::InputArray _opoints,
cv::InputArray _ipoints,
cv::InputArray _cameraMatrix,
cv::InputArray _distCoeffs,
cv::OutputArray _rvec,
cv::OutputArray _tvec,
bool useExtrinsicGuess,
int iterationsCount,
float reprojectionError,
int minInliersCount,
cv::OutputArray _inliers,
int flags,
bool opencv2version)
{
#if CV_MAJOR_VERSION >= 3
if(opencv2version)
{
const int _rng_seed = 0;
cv::Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
cv::Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
CV_Assert(opoints.isContinuous());
CV_Assert(opoints.depth() == CV_32F || opoints.depth() == CV_64F);
CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3);
CV_Assert(ipoints.isContinuous());
CV_Assert(ipoints.depth() == CV_32F || ipoints.depth() == CV_64F);
CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2);
_rvec.create(3, 1, CV_64FC1);
_tvec.create(3, 1, CV_64FC1);
cv::Mat rvec = _rvec.getMat();
cv::Mat tvec = _tvec.getMat();
cv::Mat objectPoints = opoints.reshape(3, 1), imagePoints = ipoints.reshape(2, 1);
if (minInliersCount <= 0)
minInliersCount = objectPoints.cols;
pnpransac::Parameters params;
params.iterationsCount = iterationsCount;
params.minInliersCount = minInliersCount;
params.reprojectionError = reprojectionError;
params.useExtrinsicGuess = useExtrinsicGuess;
params.camera.init(cameraMatrix, distCoeffs);
params.flags = flags;
std::vector<int> localInliers;
cv::Mat localRvec, localTvec;
rvec.copyTo(localRvec);
tvec.copyTo(localTvec);
int bestIndex;
// TBB not used
if (objectPoints.cols >= pnpransac::MIN_POINTS_COUNT)
{
pnpransac::PnPSolver solver(objectPoints, imagePoints, params,
localRvec, localTvec, localInliers, bestIndex,
_rng_seed);
solver(0, iterationsCount);
}
if (localInliers.size() >= (size_t)pnpransac::MIN_POINTS_COUNT)
{
if (flags != CV_P3P)
{
int i, pointsCount = (int)localInliers.size();
cv::Mat inlierObjectPoints(1, pointsCount, CV_MAKE_TYPE(opoints.depth(), 3)), inlierImagePoints(1, pointsCount, CV_MAKE_TYPE(ipoints.depth(), 2));
for (i = 0; i < pointsCount; i++)
{
int index = localInliers[i];
cv::Mat colInlierImagePoints = inlierImagePoints(cv::Rect(i, 0, 1, 1));
imagePoints.col(index).copyTo(colInlierImagePoints);
cv::Mat colInlierObjectPoints = inlierObjectPoints(cv::Rect(i, 0, 1, 1));
objectPoints.col(index).copyTo(colInlierObjectPoints);
}
solvePnP(inlierObjectPoints, inlierImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, false, flags);
}
localRvec.copyTo(rvec);
localTvec.copyTo(tvec);
if (_inliers.needed())
cv::Mat(localInliers).copyTo(_inliers);
}
else
{
tvec.setTo(cv::Scalar(0));
cv::Mat R = cv::Mat::eye(3, 3, CV_64F);
Rodrigues(R, rvec);
if( _inliers.needed() )
_inliers.release();
}
}
else
#endif
{
cv::solvePnPRansac(
_opoints,
_ipoints,
_cameraMatrix,
_distCoeffs,
_rvec,
_tvec,
useExtrinsicGuess,
iterationsCount,
reprojectionError,
#if CV_MAJOR_VERSION < 3
minInliersCount, // min inliers
#else
0.99, // confidence
#endif
_inliers,
flags);
}
return;
}
}
}
corelib/src/util3d_transforms.cpp
+10
View File
@@ -68,6 +68,16 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformPointCloud(
return output;
}
cv::Point3f transformPoint(
const cv::Point3f & point,
const Transform & transform)
{
cv::Point3f ret = point;
ret.x = transform (0, 0) * point.x + transform (0, 1) * point.y + transform (0, 2) * point.z + transform (0, 3);
ret.y = transform (1, 0) * point.x + transform (1, 1) * point.y + transform (1, 2) * point.z + transform (1, 3);
ret.z = transform (2, 0) * point.x + transform (2, 1) * point.y + transform (2, 2) * point.z + transform (2, 3);
return ret;
}
pcl::PointXYZ transformPoint(
const pcl::PointXYZ & pt,
const Transform & transform)
guilib/include/rtabmap/gui/DatabaseViewer.h
+3 -1
View File
@@ -60,7 +60,7 @@ class RTABMAPGUI_EXP DatabaseViewer : public QMainWindow
Q_OBJECT
public:
DatabaseViewer(QWidget * parent = 0);
DatabaseViewer(const QString & ini = QString(), QWidget * parent = 0);
virtual ~DatabaseViewer();
bool openDatabase(const QString & path);
bool isSavedMaximized() const {return savedMaximized_;}
@@ -111,6 +111,7 @@ private slots:
void updateLoggerLevel();
void updateStereo();
void notifyParametersChanged(const QStringList &);
void setupMainLayout(int vertical);
private:
QString getIniFilePath() const;
@@ -173,6 +174,7 @@ private:
bool savedMaximized_;
bool firstCall_;
QString iniFilePath_;
};
}
guilib/src/DataRecorder.cpp
+1 -1
View File
@@ -71,7 +71,7 @@ bool DataRecorder::init(const QString & path, bool recordInRAM)
{
ParametersMap customParameters;
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "-1")); // desactivate keypoints extraction
customParameters.insert(ParametersPair(Parameters::kKpMaxFeatures(), "-1")); // desactivate keypoints extraction
customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "true")); // to keep images
customParameters.insert(ParametersPair(Parameters::kMemMapLabelsAdded(), "false")); // don't create map labels
customParameters.insert(ParametersPair(Parameters::kMemBadSignaturesIgnored(), "true")); // make usre memory cleanup is done
guilib/src/DatabaseViewer.cpp
+67 -43
View File
@@ -78,11 +78,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
DatabaseViewer::DatabaseViewer(QWidget * parent) :
DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
QMainWindow(parent),
dbDriver_(0),
savedMaximized_(false),
firstCall_(true)
firstCall_(true),
iniFilePath_(ini)
{
pathDatabase_ = QDir::homePath()+"/Documents/RTAB-Map"; //use home directory by default
@@ -99,6 +100,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
ui_->comboBox_logger_level->setVisible(parent==0);
ui_->label_logger_level->setVisible(parent==0);
connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(updateLoggerLevel()));
connect(ui_->checkBox_verticalLayout, SIGNAL(stateChanged(int)), this, SLOT(setupMainLayout(int)));
QString title("RTAB-Map Database Viewer[*]");
this->setWindowTitle(title);
@@ -244,6 +246,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
//connect(ui_->graphicsView_A, SIGNAL(configChanged()), this, SLOT(configModified()));
//connect(ui_->graphicsView_B, SIGNAL(configChanged()), this, SLOT(configModified()));
connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
connect(ui_->checkBox_verticalLayout, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
// Graph view
connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
@@ -297,6 +300,18 @@ DatabaseViewer::~DatabaseViewer()
}
}
void DatabaseViewer::setupMainLayout(int vertical)
{
if(vertical)
{
qobject_cast<QHBoxLayout *>(ui_->horizontalLayout_imageViews->layout())->setDirection(QBoxLayout::TopToBottom);
}
else if(!vertical)
{
qobject_cast<QHBoxLayout *>(ui_->horizontalLayout_imageViews->layout())->setDirection(QBoxLayout::LeftToRight);
}
}
void DatabaseViewer::showCloseButton(bool visible)
{
ui_->buttonBox->setVisible(visible);
@@ -309,6 +324,10 @@ void DatabaseViewer::configModified()
QString DatabaseViewer::getIniFilePath() const
{
if(!iniFilePath_.isEmpty())
{
return iniFilePath_;
}
QString privatePath = QDir::homePath() + "/.rtabmap";
if(!QDir(privatePath).exists())
{
@@ -338,6 +357,7 @@ void DatabaseViewer::readSettings()
savedMaximized_ = settings.value("maximized", false).toBool();
ui_->comboBox_logger_level->setCurrentIndex(settings.value("loggerLevel", ui_->comboBox_logger_level->currentIndex()).toInt());
ui_->checkBox_verticalLayout->setChecked(settings.value("verticalLayout", ui_->checkBox_verticalLayout->isChecked()).toBool());
// GraphViewer settings
ui_->graphViewer->loadSettings(settings, "GraphView");
@@ -409,6 +429,7 @@ void DatabaseViewer::writeSettings()
savedMaximized_ = this->isMaximized();
settings.setValue("loggerLevel", ui_->comboBox_logger_level->currentIndex());
settings.setValue("verticalLayout", ui_->checkBox_verticalLayout->isChecked());
// save GraphViewer settings
ui_->graphViewer->saveSettings(settings, "GraphView");
@@ -2343,10 +2364,10 @@ void DatabaseViewer::updateStereo(const SensorData * data)
// generate kpts
std::vector<cv::KeyPoint> kpts;
cv::Rect roi = Feature2D::computeRoi(leftMono, "0.03 0.03 0.04 0.04");
uInsert(parameters, ParametersPair(Parameters::kKpWordsPerImage(), parameters.at(Parameters::kVisMaxFeatures())));
uInsert(parameters, ParametersPair(Parameters::kKpMaxFeatures(), parameters.at(Parameters::kVisMaxFeatures())));
uInsert(parameters, ParametersPair(Parameters::kKpRoiRatios(), std::string(opticalFlow?"0.03 0.03 0.04 0.04":"0 0 0 0")));
Feature2D * kptDetector = Feature2D::create(parameters);
kpts = kptDetector->generateKeypoints(leftMono, opticalFlow?roi:cv::Rect());
kpts = kptDetector->generateKeypoints(leftMono);
delete kptDetector;
float timeKpt = timer.ticks();
@@ -2378,23 +2399,23 @@ void DatabaseViewer::updateStereo(const SensorData * data)
int negativeDisparityOutliers = 0;
for(unsigned int i=0; i<status.size(); ++i)
{
pcl::PointXYZ pt(bad_point, bad_point, bad_point);
cv::Point3f pt(bad_point, bad_point, bad_point);
if(status[i])
{
float disparity = leftCorners[i].x - rightCorners[i].x;
if(disparity > 0.0f)
{
pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
cv::Point3f tmpPt = util3d::projectDisparityTo3D(
leftCorners[i],
disparity,
data->stereoCameraModel());
if(pcl::isFinite(tmpPt))
if(util3d::isFinite(tmpPt))
{
pt = pcl::transformPoint(tmpPt, data->stereoCameraModel().left().localTransform().toEigen3f());
pt = util3d::transformPoint(tmpPt, data->stereoCameraModel().left().localTransform());
status[i] = 100; //blue
++inliers;
cloud->at(oi++) = pt;
cloud->at(oi++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
}
}
else
@@ -2515,9 +2536,18 @@ void DatabaseViewer::updateWordsMatching()
// Add lines
// Draw lines between corresponding features...
float scaleX = ui_->graphicsView_A->viewScale();
float deltaX = ui_->graphicsView_A->width()/scaleX;
float deltaX = 0;
float deltaY = 0;
if(ui_->checkBox_verticalLayout->isChecked())
{
deltaY = ui_->graphicsView_A->height()/scaleX;
}
else
{
deltaX = ui_->graphicsView_A->width()/scaleX;
}
const KeypointItem * kptA = wordsA.value(ids[i]);
const KeypointItem * kptB = wordsB.value(ids[i]);
ui_->graphicsView_A->addLine(
@@ -2772,18 +2802,18 @@ void DatabaseViewer::updateConstraintView(
cloudFrom->resize(sFrom->getWords3().size());
cloudTo->resize(sTo->getWords3().size());
int i=0;
for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=sFrom->getWords3().begin();
for(std::multimap<int, cv::Point3f>::const_iterator iter=sFrom->getWords3().begin();
iter!=sFrom->getWords3().end();
++iter)
{
cloudFrom->at(i++) = iter->second;
cloudFrom->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
}
i=0;
for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=sTo->getWords3().begin();
for(std::multimap<int, cv::Point3f>::const_iterator iter=sTo->getWords3().begin();
iter!=sTo->getWords3().end();
++iter)
{
cloudTo->at(i++) = iter->second;
cloudTo->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
}
if(cloudFrom->size())
@@ -3624,7 +3654,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
Transform t;
std::string rejectedMsg;
float variance = -1.0f;
int inliers = -1;
std::vector<int> inliers;
// Add sensor data to generate features
SensorData dataFrom;
@@ -3637,9 +3667,18 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
UDEBUG("");
RegistrationVis reg(parameters);
t = reg.computeTransformation2(dataFrom, dataTo, Transform::getIdentity(), &rejectedMsg, &inliers, &variance);
Signature fromS(dataFrom);
Signature toS(dataTo);
t = reg.computeTransformationMod(fromS, toS, Transform::getIdentity(), &rejectedMsg, &inliers, &variance);
UDEBUG("");
if(!silent)
{
ui_->graphicsView_A->setFeatures(fromS.getWords(), dataFrom.depthRaw());
ui_->graphicsView_B->setFeatures(toS.getWords(), dataTo.depthRaw());
updateWordsMatching();
}
if(!t.isNull())
{
Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), t, variance, variance);
@@ -3668,7 +3707,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
}
if(ui_->dockWidget_constraints->isVisible())
{
this->updateConstraintView(newLink);
this->updateConstraintView(newLink, false);
}
}
else if(!silent)
@@ -3702,20 +3741,11 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
UASSERT(!containsLink(linksRefined_, from, to));
ParametersMap parameters = ui_->parameters_toolbox->getParameters();
uInsert(parameters, ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kVisFeatureType())));
uInsert(parameters, ParametersPair(Parameters::kKpNNStrategy(), parameters.at(Parameters::kVisNNType())));
uInsert(parameters, ParametersPair(Parameters::kKpMaxDepth(), parameters.at(Parameters::kVisMaxDepth())));
uInsert(parameters, ParametersPair(Parameters::kKpWordsPerImage(), parameters.at(Parameters::kVisMaxFeatures())));
uInsert(parameters, ParametersPair(Parameters::kMemGenerateIds(), "false"));
uInsert(parameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0"));
Transform t;
std::string rejectedMsg;
float variance = -1.0f;
int inliers = -1;
// create a fake memory to compute the transform
Memory tmpMemory(parameters);
std::vector<int> inliers;
// Add sensor data to generate features
SensorData dataFrom;
@@ -3725,27 +3755,21 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
dbDriver_->getNodeData(to, dataTo);
dataTo.uncompressData();
if(from > to)
{
tmpMemory.update(dataTo);
tmpMemory.update(dataFrom);
}
else
{
tmpMemory.update(dataFrom);
tmpMemory.update(dataTo);
}
t = tmpMemory.computeVisualTransform(from, to, &rejectedMsg, &inliers, &variance);
UDEBUG("");
RegistrationVis reg(parameters);
Signature fromS(dataFrom);
Signature toS(dataTo);
t = reg.computeTransformationMod(fromS, toS, Transform::getIdentity(), &rejectedMsg, &inliers, &variance);
UDEBUG("");
if(!silent)
{
ui_->graphicsView_A->setFeatures(tmpMemory.getSignature(from)->getWords(), dataFrom.depthRaw());
ui_->graphicsView_B->setFeatures(tmpMemory.getSignature(to)->getWords(), dataTo.depthRaw());
ui_->graphicsView_A->setFeatures(fromS.getWords(), dataFrom.depthRaw());
ui_->graphicsView_B->setFeatures(toS.getWords(), dataTo.depthRaw());
updateWordsMatching();
}
if(!t.isNull())
{
// transform is valid, make a link
guilib/src/MainWindow.cpp
+14 -14
View File
@@ -1038,6 +1038,11 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
_ui->statsToolBox->updateStat("Odometry/TFpitch/deg", (float)odom.data().id(), pitch*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/TFyaw/deg", (float)odom.data().id(), yaw*180.0/CV_PI);
}
if(odom.info().interval > 0)
{
_ui->statsToolBox->updateStat("Odometry/Interval/ms", (float)odom.data().id(), odom.info().interval*1000.f);
_ui->statsToolBox->updateStat("Odometry/Speed/kph", (float)odom.data().id(), x/odom.info().interval*3.6f);
}
if(!odom.info().transformGroundTruth.isNull())
{
@@ -1072,11 +1077,6 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
_ui->statsToolBox->updateStat("Odometry/PGyaw/deg", (float)odom.data().id(), yaw*180.0/CV_PI);
}
if(odom.info().interval > 0)
{
_ui->statsToolBox->updateStat("Odometry/Interval/ms", (float)odom.data().id(), odom.info().interval*1000.f);
_ui->statsToolBox->updateStat("Odometry/Speed/kph", (float)odom.data().id(), x/odom.info().interval*3.6f);
}
if(odom.info().distanceTravelled > 0)
{
_ui->statsToolBox->updateStat("Odometry/Distance/m", (float)odom.data().id(), odom.info().distanceTravelled);
@@ -1916,7 +1916,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
int oi=0;
UASSERT(iter->getWords().size() == iter->getWords3().size());
std::multimap<int, cv::KeyPoint>::const_iterator kter=iter->getWords().begin();
for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=iter->getWords3().begin();
for(std::multimap<int, cv::Point3f>::const_iterator jter=iter->getWords3().begin();
jter!=iter->getWords3().end(); ++jter, ++kter, ++oi)
{
(*cloud)[oi].x = jter->second.x;
@@ -2870,7 +2870,7 @@ void MainWindow::editDatabase()
QString path = QFileDialog::getOpenFileName(this, tr("Edit database..."), _preferencesDialog->getWorkingDirectory(), tr("RTAB-Map database files (*.db)"));
if(!path.isEmpty())
{
DatabaseViewer * viewer = new DatabaseViewer(this);
DatabaseViewer * viewer = new DatabaseViewer(_preferencesDialog->getIniFilePath(), this);
viewer->setWindowModality(Qt::WindowModal);
viewer->setAttribute(Qt::WA_DeleteOnClose, true);
viewer->showCloseButton();
@@ -3026,7 +3026,7 @@ void MainWindow::startDetection()
Odometry * odom;
if(_preferencesDialog->getOdomStrategy() == 1)
{
odom = new OdometryOpticalFlow(parameters);
odom = new OdometryF2F(parameters);
}
else if(_preferencesDialog->getOdomStrategy() == 2)
{
@@ -3065,7 +3065,7 @@ void MainWindow::startDetection()
Odometry * odom;
if(_preferencesDialog->getOdomStrategy() == 1)
{
odom = new OdometryOpticalFlow(parameters);
odom = new OdometryF2F(parameters);
}
else if(_preferencesDialog->getOdomStrategy() == 2)
{
@@ -3524,16 +3524,16 @@ void MainWindow::postProcessing()
if(reextractFeatures)
{
signatureFrom.setWords(std::multimap<int, cv::KeyPoint>());
signatureFrom.setWords3(std::multimap<int, pcl::PointXYZ>());
signatureFrom.setWords3(std::multimap<int, cv::Point3f>());
signatureTo.setWords(std::multimap<int, cv::KeyPoint>());
signatureTo.setWords3(std::multimap<int, pcl::PointXYZ>());
signatureTo.setWords3(std::multimap<int, cv::Point3f>());
}
Transform transform;
std::string rejectedMsg;
int inliers = -1;
float variance = -1.0f;
RegistrationVis registration(parameters);
std::vector<int> inliers;
transform = registration.computeTransformation(signatureFrom, signatureTo, Transform(), &rejectedMsg, &inliers, &variance);
if(!transform.isNull())
@@ -3623,7 +3623,7 @@ void MainWindow::postProcessing()
{
std::string rejectedMsg;
float variance = -1.0f;
Transform transform = regIcp.computeTransformation(signatureFrom, signatureTo, iter->second.transform(), &rejectedMsg, 0, &variance);
Transform transform = regIcp.computeTransformation(signatureFrom.sensorData(), signatureTo.sensorData(), iter->second.transform(), &rejectedMsg, 0, &variance);
if(!transform.isNull())
{
@@ -5844,7 +5844,7 @@ std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > MainWindow::getClouds(
{
cloud->resize(s.getWords3().size());
int oi=0;
for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=s.getWords3().begin(); jter!=s.getWords3().end(); ++jter)
for(std::multimap<int, cv::Point3f>::const_iterator jter=s.getWords3().begin(); jter!=s.getWords3().end(); ++jter)
{
(*cloud)[oi].x = jter->second.x;
(*cloud)[oi].y = jter->second.y;
guilib/src/ParametersToolBox.cpp
+32 -11
View File
@@ -36,11 +36,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <QComboBox>
#include <QDoubleSpinBox>
#include <QLineEdit>
#include <QStackedWidget>
#include <QScrollArea>
#include <QLabel>
#include <QGroupBox>
#include <QCheckBox>
#include <QVBoxLayout>
#include <QMessageBox>
#include <QPushButton>
#include <stdio.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UConversion.h>
@@ -49,9 +52,20 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
ParametersToolBox::ParametersToolBox(QWidget *parent) :
QToolBox(parent),
QWidget(parent),
comboBox_(new QComboBox(this)),
stackedWidget_(new QStackedWidget(this)),
parameters_(Parameters::getDefaultParameters())
{
QVBoxLayout * layout = new QVBoxLayout(this);
this->setLayout(layout);
layout->addWidget(comboBox_);
layout->addWidget(stackedWidget_, 1);
QPushButton * resetButton = new QPushButton(this);
resetButton->setText(tr("Restore Defaults"));
layout->addWidget(resetButton);
connect(resetButton, SIGNAL(clicked()), this, SLOT(resetCurrentPage()));
}
ParametersToolBox::~ParametersToolBox()
@@ -66,14 +80,15 @@ QWidget * ParametersToolBox::getParameterWidget(const QString & key)
QStringList ParametersToolBox::resetPage(int index)
{
QStringList paramChanged;
const QObjectList & children = this->widget(index)->children();
const QObjectList & children = stackedWidget_->widget(index)->children().first()->children().first()->children();
for(int j=0; j<children.size();++j)
{
QString key = children.at(j)->objectName();
// ignore working memory
QString group = key.split("/").first();
if(!ignoredGroups_.contains(group))
if(!ignoredGroups_.contains(group) && parameters_.find(key.toStdString())!=parameters_.end())
{
UASSERT_MSG(parameters_.find(key.toStdString()) != parameters_.end(), uFormat("key=%s", key.toStdString().c_str()).c_str());
std::string value = Parameters::getDefaultParameters().at(key.toStdString());
parameters_.at(key.toStdString()) = value;
@@ -125,7 +140,7 @@ QStringList ParametersToolBox::resetPage(int index)
void ParametersToolBox::resetCurrentPage()
{
this->blockSignals(true);
QStringList paramChanged = this->resetPage(this->currentIndex());
QStringList paramChanged = this->resetPage(stackedWidget_->currentIndex());
this->blockSignals(false);
Q_EMIT parametersChanged(paramChanged);
}
@@ -134,7 +149,7 @@ void ParametersToolBox::resetAllPages()
{
QStringList paramChanged;
this->blockSignals(true);
for(int i=0; i< this->count(); ++i)
for(int i=0; i< stackedWidget_->count(); ++i)
{
paramChanged.append(this->resetPage(i));
}
@@ -190,9 +205,9 @@ void ParametersToolBox::updateParametersVisibility()
void ParametersToolBox::setupUi(const QSet<QString> & ignoredGroups)
{
ignoredGroups_ = ignoredGroups;
this->removeItem(0); // remove dummy page used in .ui
QWidget * currentItem = 0;
const ParametersMap & parameters = Parameters::getDefaultParameters();
QStringList groups;
for(ParametersMap::const_iterator iter=parameters.begin();
iter!=parameters.end();
++iter)
@@ -204,14 +219,16 @@ void ParametersToolBox::setupUi(const QSet<QString> & ignoredGroups)
QString name = splitted.last();
if(currentItem == 0 || currentItem->objectName().compare(group) != 0)
{
currentItem = new QWidget(this);
this->addItem(currentItem, group);
groups.push_back(group);
QScrollArea * area = new QScrollArea(this);
stackedWidget_->addWidget(area);
currentItem = new QWidget();
currentItem->setObjectName(group);
QVBoxLayout * layout = new QVBoxLayout(currentItem);
currentItem->setLayout(layout);
layout->setSizeConstraint(QLayout::SetMinimumSize);
layout->setContentsMargins(0,0,0,0);
layout->setSpacing(0);
layout->addSpacerItem(new QSpacerItem(0,0, QSizePolicy::Minimum, QSizePolicy::Expanding));
area->setWidget(currentItem);
addParameter(layout, iter->first, iter->second);
}
@@ -221,6 +238,8 @@ void ParametersToolBox::setupUi(const QSet<QString> & ignoredGroups)
}
}
}
comboBox_->addItems(groups);
connect(comboBox_, SIGNAL(currentIndexChanged(int)), stackedWidget_, SLOT(setCurrentIndex(int)));
updateParametersVisibility();
}
@@ -230,6 +249,8 @@ void ParametersToolBox::updateParameter(const std::string & key, const std::stri
QString group = QString::fromStdString(key).split("/").first();
if(!ignoredGroups_.contains(group))
{
UASSERT_MSG(parameters_.find(key) != parameters_.end(), uFormat("key=\"%s\"", key.c_str()).c_str());
parameters_.at(key) = value;
QWidget * widget = this->findChild<QWidget*>(key.c_str());
QString type = QString::fromStdString(Parameters::getType(key));
if(type.compare("string") == 0)
@@ -340,7 +361,7 @@ void ParametersToolBox::addParameter(QVBoxLayout * layout,
widget->setDecimals(3);
}
if(def>=0.0)
if(def>0.0)
{
widget->setMaximum(def*1000000.0);
}
guilib/src/ParametersToolBox.h
+6 -3
View File
@@ -33,15 +33,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#define PARAMETERSTOOLBOX_H_
#include <rtabmap/core/Parameters.h>
#include <QToolBox>
#include <QWidget>
#include <QSet>
class QVBoxLayout;
class QAbstractButton;
class QStackedWidget;
class QComboBox;
namespace rtabmap {
class ParametersToolBox: public QToolBox
class ParametersToolBox: public QWidget
{
Q_OBJECT
@@ -77,6 +78,8 @@ private:
void updateParametersVisibility();
private:
QComboBox * comboBox_;
QStackedWidget * stackedWidget_;
ParametersMap parameters_;
QSet<QString> ignoredGroups_;
};
guilib/src/PreferencesDialog.cpp
+22 -10
View File
@@ -208,6 +208,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->openni2_exposure->setEnabled(CameraOpenNI2::exposureGainAvailable());
_ui->openni2_gain->setEnabled(CameraOpenNI2::exposureGainAvailable());
#if CV_MAJOR_VERSION < 3
_ui->loopClosure_pnpOpenCV2->setVisible(false);
_ui->label_loopClosure_pnpOpenCV2->setVisible(false);
#endif
// Default Driver
connect(_ui->comboBox_sourceType, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility()));
connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(updateRGBDCameraGroupBoxVisibility()));
@@ -520,7 +525,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->surf_doubleSpinBox_nndrRatio->setObjectName(Parameters::kKpNndrRatio().c_str());
_ui->surf_doubleSpinBox_maxDepth->setObjectName(Parameters::kKpMaxDepth().c_str());
_ui->surf_doubleSpinBox_minDepth->setObjectName(Parameters::kKpMinDepth().c_str());
_ui->surf_spinBox_wordsPerImageTarget->setObjectName(Parameters::kKpWordsPerImage().c_str());
_ui->surf_spinBox_wordsPerImageTarget->setObjectName(Parameters::kKpMaxFeatures().c_str());
_ui->surf_doubleSpinBox_ratioBadSign->setObjectName(Parameters::kKpBadSignRatio().c_str());
_ui->checkBox_kp_tfIdfLikelihoodUsed->setObjectName(Parameters::kKpTfIdfLikelihoodUsed().c_str());
_ui->checkBox_kp_parallelized->setObjectName(Parameters::kKpParallelized().c_str());
@@ -640,14 +645,19 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->loopClosure_estimationType->setObjectName(Parameters::kVisEstimationType().c_str());
connect(_ui->loopClosure_estimationType, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_loopClosureEstimation, SLOT(setCurrentIndex(int)));
_ui->stackedWidget_loopClosureEstimation->setCurrentIndex(Parameters::defaultVisEstimationType());
_ui->loopClosure_forwardEst->setObjectName(Parameters::kVisForwardEstOnly().c_str());
_ui->loopClosure_bowEpipolarGeometryVar->setObjectName(Parameters::kVisEpipolarGeometryVar().c_str());
_ui->loopClosure_pnpReprojError->setObjectName(Parameters::kVisPnPReprojError().c_str());
_ui->loopClosure_pnpFlags->setObjectName(Parameters::kVisPnPFlags().c_str());
_ui->loopClosure_pnpOpenCV2->setObjectName(Parameters::kVisPnPOpenCV2().c_str());
_ui->loopClosure_bowVarianceFromInliersCount->setObjectName(Parameters::kRegVarianceFromInliersCount().c_str());
_ui->loopClosure_reextract->setObjectName(Parameters::kRGBDLoopClosureReextractFeatures().c_str());
_ui->reextract_nn->setObjectName(Parameters::kVisNNType().c_str());
_ui->reextract_nndrRatio->setObjectName(Parameters::kVisNNDR().c_str());
_ui->loopClosure_correspondencesType->setObjectName(Parameters::kVisCorType().c_str());
connect(_ui->loopClosure_correspondencesType, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_loopClosureCorrespondences, SLOT(setCurrentIndex(int)));
_ui->stackedWidget_loopClosureCorrespondences->setCurrentIndex(Parameters::defaultVisCorType());
_ui->reextract_nn->setObjectName(Parameters::kVisCorNNType().c_str());
_ui->reextract_nndrRatio->setObjectName(Parameters::kVisCorNNDR().c_str());
_ui->reextract_type->setObjectName(Parameters::kVisFeatureType().c_str());
_ui->reextract_maxFeatures->setObjectName(Parameters::kVisMaxFeatures().c_str());
_ui->loopClosure_bowMaxDepth->setObjectName(Parameters::kVisMaxDepth().c_str());
@@ -685,10 +695,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
//Odometry Optical Flow
_ui->odom_flow_keyframeThr->setObjectName(Parameters::kOdomFlowKeyFrameThr().c_str());
_ui->odom_flow_winSize->setObjectName(Parameters::kOdomFlowWinSize().c_str());
_ui->odom_flow_maxLevel->setObjectName(Parameters::kOdomFlowMaxLevel().c_str());
_ui->odom_flow_iterations->setObjectName(Parameters::kOdomFlowIterations().c_str());
_ui->odom_flow_eps->setObjectName(Parameters::kOdomFlowEps().c_str());
_ui->odom_flow_winSize_2->setObjectName(Parameters::kVisCorFlowWinSize().c_str());
_ui->odom_flow_maxLevel_2->setObjectName(Parameters::kVisCorFlowMaxLevel().c_str());
_ui->odom_flow_iterations_2->setObjectName(Parameters::kVisCorFlowIterations().c_str());
_ui->odom_flow_eps_2->setObjectName(Parameters::kVisCorFlowEps().c_str());
_ui->odom_flow_guessMotion->setObjectName(Parameters::kOdomFlowGuessMotion().c_str());
//Odometry Mono
@@ -699,6 +709,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
//Odometry particle filter
_ui->odom_filteringStrategy->setObjectName(Parameters::kOdomFilteringStrategy().c_str());
_ui->stackedWidget_odometryFiltering->setCurrentIndex(_ui->odom_filteringStrategy->currentIndex());
connect(_ui->odom_filteringStrategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_odometryFiltering, SLOT(setCurrentIndex(int)));
_ui->spinBox_particleSize->setObjectName(Parameters::kOdomParticleSize().c_str());
_ui->doubleSpinBox_particleNoiseT->setObjectName(Parameters::kOdomParticleNoiseT().c_str());
_ui->doubleSpinBox_particleLambdaT->setObjectName(Parameters::kOdomParticleLambdaT().c_str());
@@ -2410,7 +2422,7 @@ void PreferencesDialog::selectSourceDatabase()
void PreferencesDialog::openDatabaseViewer()
{
DatabaseViewer * viewer = new DatabaseViewer(this);
DatabaseViewer * viewer = new DatabaseViewer(getIniFilePath(), this);
viewer->setWindowModality(Qt::WindowModal);
viewer->setAttribute(Qt::WA_DeleteOnClose, true);
viewer->showCloseButton();
@@ -2723,7 +2735,7 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
}
else if(valueInt==1 &&
(combo->objectName().toStdString().compare(Parameters::kKpNNStrategy()) == 0 ||
combo->objectName().toStdString().compare(Parameters::kVisNNType()) == 0))
combo->objectName().toStdString().compare(Parameters::kVisCorNNType()) == 0))
{
UWARN("Trying to set \"%s\" to KdTree but RTAB-Map isn't built "
@@ -4112,7 +4124,7 @@ void PreferencesDialog::testOdometry()
Odometry * odometry;
if(this->getOdomStrategy() == 1)
{
odometry = new OdometryOpticalFlow(parameters);
odometry = new OdometryF2F(parameters);
}
else if(this->getOdomStrategy() == 2)
{
guilib/src/ui/DatabaseViewer.ui
+367 -380
View File
@@ -17,7 +17,7 @@
<enum>Qt::LeftToRight</enum>
</property>
<widget class="QWidget" name="centralwidget">
<layout class="QVBoxLayout" name="verticalLayout_7" stretch="2,0">
<layout class="QVBoxLayout" name="verticalLayout_5" stretch="1,0,0">
<property name="spacing">
<number>0</number>
</property>
@@ -25,380 +25,370 @@
<number>0</number>
</property>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_3" stretch="1,1">
<property name="rightMargin">
<layout class="QHBoxLayout" name="horizontalLayout_imageViews" stretch="1,1">
<item>
<widget class="rtabmap::ImageView" name="graphicsView_A" native="true"/>
</item>
<item>
<widget class="rtabmap::ImageView" name="graphicsView_B" native="true"/>
</item>
</layout>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_11">
<property name="spacing">
<number>0</number>
</property>
<item>
<layout class="QVBoxLayout" name="verticalLayout_6" stretch="1,0,0">
<property name="spacing">
<number>0</number>
<item row="0" column="0">
<widget class="QScrollArea" name="scrollArea_2">
<property name="horizontalScrollBarPolicy">
<enum>Qt::ScrollBarAsNeeded</enum>
</property>
<item>
<widget class="rtabmap::ImageView" name="graphicsView_A" native="true"/>
</item>
<item>
<widget class="QScrollArea" name="scrollArea_2">
<property name="horizontalScrollBarPolicy">
<enum>Qt::ScrollBarAsNeeded</enum>
</property>
<property name="widgetResizable">
<bool>true</bool>
</property>
<widget class="QWidget" name="scrollAreaWidgetContents_2">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>181</width>
<height>184</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
<item row="0" column="0">
<widget class="QLabel" name="label_parentsA_2">
<property name="text">
<string>Parents</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_parentsA">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_childrenA_2">
<property name="text">
<string>Children</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_childrenA">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_childrenA_4">
<property name="text">
<string>Label</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_labelA">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QLabel" name="label_childrenA_6">
<property name="text">
<string>Stamp</string>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_stampA">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_childrenA_8">
<property name="text">
<string>Weight</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_weightA">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_childrenA_10">
<property name="text">
<string>Map ID</string>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_mapA">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QLabel" name="label_childrenA_12">
<property name="text">
<string>Pose</string>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_poseA">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</widget>
</widget>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout">
<property name="margin">
<number>12</number>
</property>
<item>
<layout class="QVBoxLayout" name="verticalLayout_4">
<item>
<widget class="QLabel" name="label_2">
<property name="text">
<string>Index :</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_3">
<property name="text">
<string>Id :</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QVBoxLayout" name="verticalLayout_3">
<item>
<widget class="QLabel" name="label_indexA">
<property name="text">
<string>indexA</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_idA">
<property name="text">
<string>idA</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QSlider" name="horizontalSlider_A">
<property name="focusPolicy">
<enum>Qt::ClickFocus</enum>
<property name="widgetResizable">
<bool>true</bool>
</property>
<widget class="QWidget" name="scrollAreaWidgetContents_2">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>189</width>
<height>184</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
<item row="0" column="0">
<widget class="QLabel" name="label_parentsA_2">
<property name="text">
<string>Parents</string>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_parentsA">
<property name="text">
<string/>
</property>
<property name="tickPosition">
<enum>QSlider::TicksAbove</enum>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_childrenA_2">
<property name="text">
<string>Children</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_childrenA">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_childrenA_4">
<property name="text">
<string>Label</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_labelA">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QLabel" name="label_childrenA_6">
<property name="text">
<string>Stamp</string>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_stampA">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_childrenA_8">
<property name="text">
<string>Weight</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_weightA">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_childrenA_10">
<property name="text">
<string>Map ID</string>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_mapA">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QLabel" name="label_childrenA_12">
<property name="text">
<string>Pose</string>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_poseA">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</widget>
</widget>
</item>
<item row="0" column="1">
<widget class="QScrollArea" name="scrollArea">
<property name="horizontalScrollBarPolicy">
<enum>Qt::ScrollBarAsNeeded</enum>
</property>
<property name="widgetResizable">
<bool>true</bool>
</property>
<widget class="QWidget" name="scrollAreaWidgetContents">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>187</width>
<height>184</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
<item row="0" column="1">
<widget class="QLabel" name="label_parentsB">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QLabel" name="label_parentsA_4">
<property name="text">
<string>Parents</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_childrenA_3">
<property name="text">
<string>Children</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_childrenB">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_childrenA_5">
<property name="text">
<string>Label</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_labelB">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QLabel" name="label_childrenA_7">
<property name="text">
<string>Stamp</string>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_stampB">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_childrenA_9">
<property name="text">
<string>Weight</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_weightB">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_childrenA_11">
<property name="text">
<string>Map ID</string>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_mapB">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QLabel" name="label_childrenA_13">
<property name="text">
<string>Pose</string>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_poseB">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</widget>
</widget>
</item>
<item row="1" column="0">
<layout class="QHBoxLayout" name="horizontalLayout">
<property name="margin">
<number>12</number>
</property>
<item>
<layout class="QVBoxLayout" name="verticalLayout_4">
<item>
<widget class="QLabel" name="label_2">
<property name="text">
<string>Index :</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_3">
<property name="text">
<string>Id :</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QVBoxLayout" name="verticalLayout_3">
<item>
<widget class="QLabel" name="label_indexA">
<property name="text">
<string>indexA</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_idA">
<property name="text">
<string>idA</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QSlider" name="horizontalSlider_A">
<property name="focusPolicy">
<enum>Qt::ClickFocus</enum>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="tickPosition">
<enum>QSlider::TicksAbove</enum>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QVBoxLayout" name="verticalLayout_5" stretch="1,0,0">
<property name="spacing">
<number>0</number>
<item row="1" column="1">
<layout class="QHBoxLayout" name="horizontalLayout_2">
<property name="margin">
<number>12</number>
</property>
<item>
<widget class="rtabmap::ImageView" name="graphicsView_B" native="true"/>
</item>
<item>
<widget class="QScrollArea" name="scrollArea">
<property name="horizontalScrollBarPolicy">
<enum>Qt::ScrollBarAsNeeded</enum>
</property>
<property name="widgetResizable">
<bool>true</bool>
</property>
<widget class="QWidget" name="scrollAreaWidgetContents">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>181</width>
<height>184</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
<item row="0" column="1">
<widget class="QLabel" name="label_parentsB">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QLabel" name="label_parentsA_4">
<property name="text">
<string>Parents</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_childrenA_3">
<property name="text">
<string>Children</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_childrenB">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_childrenA_5">
<property name="text">
<string>Label</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_labelB">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QLabel" name="label_childrenA_7">
<property name="text">
<string>Stamp</string>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_stampB">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_childrenA_9">
<property name="text">
<string>Weight</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_weightB">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_childrenA_11">
<property name="text">
<string>Map ID</string>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_mapB">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QLabel" name="label_childrenA_13">
<property name="text">
<string>Pose</string>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_poseB">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</widget>
</widget>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_2">
<property name="margin">
<number>12</number>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<item>
<layout class="QVBoxLayout" name="verticalLayout">
<item>
<widget class="QLabel" name="label_5">
<property name="text">
<string>Index :</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_4">
<property name="text">
<string>Id :</string>
</property>
</widget>
</item>
</layout>
<widget class="QLabel" name="label_5">
<property name="text">
<string>Index :</string>
</property>
</widget>
</item>
<item>
<layout class="QVBoxLayout" name="verticalLayout_2">
<item>
<widget class="QLabel" name="label_indexB">
<property name="text">
<string>indexB</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_idB">
<property name="text">
<string>idB</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QSlider" name="horizontalSlider_B">
<property name="focusPolicy">
<enum>Qt::ClickFocus</enum>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="tickPosition">
<enum>QSlider::TicksAbove</enum>
<widget class="QLabel" name="label_4">
<property name="text">
<string>Id :</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QVBoxLayout" name="verticalLayout_2">
<item>
<widget class="QLabel" name="label_indexB">
<property name="text">
<string>indexB</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_idB">
<property name="text">
<string>idB</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QSlider" name="horizontalSlider_B">
<property name="focusPolicy">
<enum>Qt::ClickFocus</enum>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="tickPosition">
<enum>QSlider::TicksAbove</enum>
</property>
</widget>
</item>
</layout>
</item>
</layout>
@@ -829,8 +819,15 @@
<widget class="QWidget" name="dockWidgetContents_3">
<layout class="QVBoxLayout" name="verticalLayout_10">
<item>
<layout class="QHBoxLayout" name="horizontalLayout_11">
<item>
<layout class="QGridLayout" name="gridLayout_4">
<item row="0" column="2">
<widget class="QLabel" name="label_logger_level">
<property name="text">
<string>Logger level</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QComboBox" name="comboBox_logger_level">
<property name="currentIndex">
<number>1</number>
@@ -857,10 +854,17 @@
</item>
</widget>
</item>
<item>
<widget class="QLabel" name="label_logger_level">
<item row="1" column="1">
<widget class="QCheckBox" name="checkBox_verticalLayout">
<property name="text">
<string>Logger level</string>
<string/>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QLabel" name="label_logger_level_2">
<property name="text">
<string>Vertical layout</string>
</property>
</widget>
</item>
@@ -1012,7 +1016,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>261</width>
<width>297</width>
<height>411</height>
</rect>
</property>
@@ -1269,7 +1273,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>201</width>
<width>297</width>
<height>126</height>
</rect>
</property>
@@ -1368,7 +1372,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-48</y>
<y>0</y>
<width>297</width>
<height>134</height>
</rect>
@@ -1613,24 +1617,7 @@
<number>0</number>
</property>
<item>
<widget class="rtabmap::ParametersToolBox" name="parameters_toolbox">
<property name="currentIndex">
<number>0</number>
</property>
<widget class="QWidget" name="page">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>336</width>
<height>76</height>
</rect>
</property>
<attribute name="label">
<string>Page 1</string>
</attribute>
</widget>
</widget>
<widget class="rtabmap::ParametersToolBox" name="parameters_toolbox" native="true"/>
</item>
</layout>
</widget>
@@ -1770,7 +1757,7 @@
</customwidget>
<customwidget>
<class>rtabmap::ParametersToolBox</class>
<extends>QToolBox</extends>
<extends>QWidget</extends>
<header>ParametersToolBox.h</header>
<container>1</container>
</customwidget>
guilib/src/ui/preferencesDialog.ui
+705 -591
View File
File diff suppressed because it is too large Load Diff
tools/OdometryViewer/main.cpp
+6 -5
View File
@@ -104,8 +104,8 @@ int main (int argc, char * argv[])
bool icp = false;
bool flow = false;
bool mono = false;
int nnType = rtabmap::Parameters::defaultVisNNType();
float nndr = rtabmap::Parameters::defaultVisNNDR();
int nnType = rtabmap::Parameters::defaultVisCorNNType();
float nndr = rtabmap::Parameters::defaultVisCorNNDR();
float distance = rtabmap::Parameters::defaultVisInlierDistance();
int maxWords = rtabmap::Parameters::defaultVisMaxFeatures();
int minInliers = rtabmap::Parameters::defaultVisMinInliers();
@@ -658,7 +658,8 @@ int main (int argc, char * argv[])
if(flow)
{
// Optical Flow
odom = new rtabmap::OdometryOpticalFlow(parameters);
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisCorType(), "1"));
odom = new rtabmap::OdometryF2F(parameters);
}
else
{
@@ -666,8 +667,8 @@ int main (int argc, char * argv[])
UINFO("Nearest neighbor = %s", nnName.c_str());
UINFO("Nearest neighbor ratio = %f", nndr);
UINFO("Local history = %d", localHistory);
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisNNType(), uNumber2Str(nnType)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisNNDR(), uNumber2Str(nndr)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisCorNNType(), uNumber2Str(nnType)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisCorNNDR(), uNumber2Str(nndr)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowLocalHistorySize(), uNumber2Str(localHistory)));
if(mono)
tools/StereoEval/main.cpp
+3 -5
View File
@@ -244,11 +244,9 @@ int main(int argc, char * argv[])
// generate kpts
std::vector<cv::KeyPoint> kpts;
cv::Rect roi = Feature2D::computeRoi(leftMono, "0.03 0.03 0.04 0.04");
int type;
Parameters::parse(parameters, Parameters::kKpDetectorStrategy(), type);
Feature2D * kptDetector = Feature2D::create(Feature2D::Type(type), parameters);
kpts = kptDetector->generateKeypoints(leftMono, roi);
uInsert(parameters, ParametersPair(Parameters::kKpRoiRatios(), "0.03 0.03 0.04 0.04"));
Feature2D * kptDetector = Feature2D::create(parameters);
kpts = kptDetector->generateKeypoints(leftMono);
delete kptDetector;
timeKpts = timer.ticks();
utilite/include/rtabmap/utilite/UStl.h
+1 -1
View File
@@ -494,7 +494,7 @@ inline std::map<K, V> uMultimapToMapUnique(const std::multimap<K, V> & m)
if(m.count(*iter) == 1)
{
typename std::multimap<K, V>::const_iterator jter=m.find(*iter);
mapOut.insert(std::pair<K,V>(jter->first, jter->second));
mapOut.insert(mapOut.end(), std::pair<K,V>(jter->first, jter->second));
}
}
return mapOut;