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

Updated default Vis/EstimationType=1 (PnP). Added all fovis and viso2 parameters.

Browse Source
This commit is contained in:
matlabbe
2017-05-30 18:29:49 -04:00
parent f6e21bd6c8
commit 21e716b08a
12 changed files with 1633 additions and 85 deletions
Download Patch File Download Diff File Expand all files Collapse all files

102
corelib/src/OdometryFovis.cpp
View File

@@ -45,8 +45,14 @@ OdometryFovis::OdometryFovis(const ParametersMap & parameters) :
rect_(0),
stereoCalib_(0),
depthImage_(0),
stereoDepth_(0)
stereoDepth_(0),
lost_(false)
{
fovisParameters_ = Parameters::filterParameters(parameters, "OdomFovis");
if(parameters.find(Parameters::kOdomVisKeyFrameThr()) != parameters.end())
{
fovisParameters_.insert(*parameters.find(Parameters::kOdomVisKeyFrameThr()));
}
}
OdometryFovis::~OdometryFovis()
@@ -104,6 +110,7 @@ void OdometryFovis::reset(const Transform & initialPose)
delete stereoDepth_;
stereoDepth_ = 0;
}
lost_ = false;
#endif
}
@@ -150,7 +157,45 @@ Transform OdometryFovis::computeTransform(
UFATAL("Not supported color type!");
}
fovis::VisualOdometryOptions options = fovis::VisualOdometry::getDefaultOptions();
fovis::VisualOdometryOptions options;
if(fovis_ == 0 || (data.cameraModels().size() != 1 && stereoDepth_ == 0))
{
options = fovis::VisualOdometry::getDefaultOptions();
ParametersMap defaults = Parameters::getDefaultParameters("OdomFovis");
options["feature-window-size"] = uValue(fovisParameters_, Parameters::kOdomFovisFeatureWindowSize(), defaults.at(Parameters::kOdomFovisFeatureWindowSize()));
options["max-pyramid-level"] = uValue(fovisParameters_, Parameters::kOdomFovisMaxPyramidLevel(), defaults.at(Parameters::kOdomFovisMaxPyramidLevel()));
options["min-pyramid-level"] = uValue(fovisParameters_, Parameters::kOdomFovisMinPyramidLevel(), defaults.at(Parameters::kOdomFovisMinPyramidLevel()));
options["target-pixels-per-feature"] = uValue(fovisParameters_, Parameters::kOdomFovisTargetPixelsPerFeature(), defaults.at(Parameters::kOdomFovisTargetPixelsPerFeature()));
options["fast-threshold"] = uValue(fovisParameters_, Parameters::kOdomFovisFastThreshold(), defaults.at(Parameters::kOdomFovisFastThreshold()));
options["use-adaptive-threshold"] = uValue(fovisParameters_, Parameters::kOdomFovisUseAdaptiveThreshold(), defaults.at(Parameters::kOdomFovisUseAdaptiveThreshold()));
options["fast-threshold-adaptive-gain"] = uValue(fovisParameters_, Parameters::kOdomFovisFastThresholdAdaptiveGain(), defaults.at(Parameters::kOdomFovisFastThresholdAdaptiveGain()));
options["use-homography-initialization"] = uValue(fovisParameters_, Parameters::kOdomFovisUseHomographyInitialization(), defaults.at(Parameters::kOdomFovisUseHomographyInitialization()));
options["ref-frame-change-threshold"] = uValue(fovisParameters_, Parameters::kOdomVisKeyFrameThr(), uNumber2Str(Parameters::defaultOdomVisKeyFrameThr()));
// OdometryFrame
options["use-bucketing"] = uValue(fovisParameters_, Parameters::kOdomFovisUseBucketing(), defaults.at(Parameters::kOdomFovisUseBucketing()));
options["bucket-width"] = uValue(fovisParameters_, Parameters::kOdomFovisBucketWidth(), defaults.at(Parameters::kOdomFovisBucketWidth()));
options["bucket-height"] = uValue(fovisParameters_, Parameters::kOdomFovisBucketHeight(), defaults.at(Parameters::kOdomFovisBucketHeight()));
options["max-keypoints-per-bucket"] = uValue(fovisParameters_, Parameters::kOdomFovisMaxKeypointsPerBucket(), defaults.at(Parameters::kOdomFovisMaxKeypointsPerBucket()));
options["use-image-normalization"] = uValue(fovisParameters_, Parameters::kOdomFovisUseImageNormalization(), defaults.at(Parameters::kOdomFovisUseImageNormalization()));
// MotionEstimator
options["inlier-max-reprojection-error"] = uValue(fovisParameters_, Parameters::kOdomFovisInlierMaxReprojectionError(), defaults.at(Parameters::kOdomFovisInlierMaxReprojectionError()));
options["clique-inlier-threshold"] = uValue(fovisParameters_, Parameters::kOdomFovisCliqueInlierThreshold(), defaults.at(Parameters::kOdomFovisCliqueInlierThreshold()));
options["min-features-for-estimate"] = uValue(fovisParameters_, Parameters::kOdomFovisMinFeaturesForEstimate(), defaults.at(Parameters::kOdomFovisMinFeaturesForEstimate()));
options["max-mean-reprojection-error"] = uValue(fovisParameters_, Parameters::kOdomFovisMaxMeanReprojectionError(), defaults.at(Parameters::kOdomFovisMaxMeanReprojectionError()));
options["use-subpixel-refinement"] = uValue(fovisParameters_, Parameters::kOdomFovisUseSubpixelRefinement(), defaults.at(Parameters::kOdomFovisUseSubpixelRefinement()));
options["feature-search-window"] = uValue(fovisParameters_, Parameters::kOdomFovisFeatureSearchWindow(), defaults.at(Parameters::kOdomFovisFeatureSearchWindow()));
options["update-target-features-with-refined"] = uValue(fovisParameters_, Parameters::kOdomFovisUpdateTargetFeaturesWithRefined(), defaults.at(Parameters::kOdomFovisUpdateTargetFeaturesWithRefined()));
// StereoDepth
options["stereo-require-mutual-match"] = uValue(fovisParameters_, Parameters::kOdomFovisStereoRequireMutualMatch(), defaults.at(Parameters::kOdomFovisStereoRequireMutualMatch()));
options["stereo-max-dist-epipolar-line"] = uValue(fovisParameters_, Parameters::kOdomFovisStereoMaxDistEpipolarLine(), defaults.at(Parameters::kOdomFovisStereoMaxDistEpipolarLine()));
options["stereo-max-refinement-displacement"] = uValue(fovisParameters_, Parameters::kOdomFovisStereoMaxRefinementDisplacement(), defaults.at(Parameters::kOdomFovisStereoMaxRefinementDisplacement()));
options["stereo-max-disparity"] = uValue(fovisParameters_, Parameters::kOdomFovisStereoMaxDisparity(), defaults.at(Parameters::kOdomFovisStereoMaxDisparity()));
}
fovis::DepthSource * depthSource = 0;
cv::Mat depth;
cv::Mat right;
@@ -283,22 +328,35 @@ Transform OdometryFovis::computeTransform(
fovis_ = new fovis::VisualOdometry(rect_, options);
}
fovis_->processFrame(gray.data, depthSource);
// get the motion estimate for this frame to the previous frame.
t = Transform::fromEigen3d(fovis_->getMotionEstimate());
fovis::MotionEstimateStatusCode statusCode = fovis::SUCCESS;
if(fovis_->getMotionEstimator())
{
statusCode = fovis_->getMotionEstimator()->getMotionEstimateStatus();
}
cv::Mat covariance;
fovis::MotionEstimateStatusCode statusCode = fovis_->getMotionEstimator()->getMotionEstimateStatus();
if(statusCode > fovis::SUCCESS)
{
UWARN("Fovis error status: %s", fovis::MotionEstimateStatusCodeStrings[statusCode]);
t.setNull();
lost_ = true;
covariance = cv::Mat::eye(6,6, CV_64FC1)*9999.0;
}
else if(lost_)
{
lost_ = false;
// we are not lost anymore but we don't know where we are now according to last valid pose
covariance = cv::Mat::eye(6,6, CV_64FC1)*9999.0;
}
else
{
const Eigen::MatrixXd& cov = fovis_->getMotionEstimator()->getMotionEstimateCov();
if(cov.cols() == 6 && cov.rows() == 6 && cov(0,0) > 0.0)
{
covariance = cv::Mat::eye(6,6, CV_64FC1);
memcpy(covariance.data, cov.data(), 36*sizeof(double));
covariance *= 100.0; // to be in the same scale than loop closure detection
}
}
if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
@@ -314,12 +372,28 @@ Transform OdometryFovis::computeTransform(
info->features = fovis_->getTargetFrame()->getNumDetectedKeypoints();
info->matches = fovis_->getMotionEstimator()->getNumMatches();
info->inliers = fovis_->getMotionEstimator()->getNumInliers();
const Eigen::MatrixXd& cov = fovis_->getMotionEstimator()->getMotionEstimateCov();
if(cov.cols() == 6 && cov.rows() == 6 && cov(0,0) > 0.0)
info->covariance = covariance;
if(this->isInfoDataFilled())
{
info->covariance = cv::Mat::eye(6,6, CV_64FC1);
memcpy(info->covariance.data, cov.data(), 36*sizeof(double));
info->covariance *= 100.0; // to be in the same scale than loop closure detection
const fovis::FeatureMatch * matches = fovis_->getMotionEstimator()->getMatches();
int numMatches = fovis_->getMotionEstimator()->getNumMatches();
if(matches && numMatches>0)
{
info->refCorners.resize(numMatches);
info->newCorners.resize(numMatches);
info->cornerInliers.resize(numMatches);
int oi=0;
for (int i = 0; i < numMatches; ++i)
{
info->refCorners[i].x = matches[i].ref_keypoint->base_uv[0];
info->refCorners[i].y = matches[i].ref_keypoint->base_uv[1];
info->newCorners[i].x = matches[i].target_keypoint->base_uv[0];
info->newCorners[i].y = matches[i].target_keypoint->base_uv[1];
info->cornerInliers[oi++] = i;
}
info->cornerInliers.resize(oi);
}
}
}

48
corelib/src/OdometryViso2.cpp
View File

@@ -62,6 +62,7 @@ OdometryViso2::OdometryViso2(const ParametersMap & parameters) :
reference_motion_(Transform::getIdentity())
{
Parameters::parse(parameters, Parameters::kOdomVisKeyFrameThr(), ref_frame_inlier_threshold_);
viso2Parameters_ = Parameters::filterParameters(parameters, "OdomViso2");
}
OdometryViso2::~OdometryViso2()
@@ -162,6 +163,30 @@ Transform OdometryViso2::computeTransform(
params.calib.cu = data.stereoCameraModel().left().cx();
params.calib.cv = data.stereoCameraModel().left().cy();
params.calib.f = data.stereoCameraModel().left().fx();
Parameters::parse(viso2Parameters_, Parameters::kOdomViso2RansacIters(), params.ransac_iters);
Parameters::parse(viso2Parameters_, Parameters::kOdomViso2InlierThreshold(), params.inlier_threshold);
Parameters::parse(viso2Parameters_, Parameters::kOdomViso2Reweighting(), params.reweighting);
Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchNmsN(), params.match.nms_n);
Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchNmsTau(), params.match.nms_tau);
Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchBinsize(), params.match.match_binsize);
Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchRadius(), params.match.match_radius);
Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchDispTolerance(), params.match.match_disp_tolerance);
Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchOutlierDispTolerance(), params.match.outlier_disp_tolerance);
Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchOutlierFlowTolerance(), params.match.outlier_flow_tolerance);
bool multistage = Parameters::defaultOdomViso2MatchMultiStage();
bool halfResolution = Parameters::defaultOdomViso2MatchHalfResolution();
Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchMultiStage(), multistage);
Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchHalfResolution() , halfResolution);
params.match.multi_stage = multistage?1:0;
params.match.half_resolution = halfResolution?1:0;
Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchRefinement(), params.match.refinement);
Parameters::parse(viso2Parameters_, Parameters::kOdomViso2BucketMaxFeatures(), params.bucket.max_features);
Parameters::parse(viso2Parameters_, Parameters::kOdomViso2BucketWidth(), params.bucket.bucket_width);
Parameters::parse(viso2Parameters_, Parameters::kOdomViso2BucketHeight(), params.bucket.bucket_height);
viso2_ = new VisualOdometryStereo(params);
viso2_->process(leftGray.data, rightGray.data, dims);
@@ -245,17 +270,20 @@ Transform OdometryViso2::computeTransform(
info->covariance = covariance;
}
std::vector<Matcher::p_match> matches = viso2_->getMatches();
info->refCorners.resize(matches.size());
info->newCorners.resize(matches.size());
info->cornerInliers.resize(matches.size());
for (size_t i = 0; i < matches.size(); ++i)
if(this->isInfoDataFilled())
{
info->refCorners[i].x = matches[i].u1c;
info->refCorners[i].y = matches[i].v1c;
info->newCorners[i].x = matches[i].u1p;
info->newCorners[i].y = matches[i].v1p;
info->cornerInliers[i] = i;
std::vector<Matcher::p_match> matches = viso2_->getMatches();
info->refCorners.resize(matches.size());
info->newCorners.resize(matches.size());
info->cornerInliers.resize(matches.size());
for (size_t i = 0; i < matches.size(); ++i)
{
info->refCorners[i].x = matches[i].u1p;
info->refCorners[i].y = matches[i].v1p;
info->newCorners[i].x = matches[i].u1c;
info->newCorners[i].y = matches[i].v1c;
info->cornerInliers[i] = i;
}
}
}