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

0.15.1: CameraImages: added ground truth time diff, fixed memory leak when loading binary scans. CameraRGBDImages and CameraStereoImages: fixed start id. Feature2D: added grid rows and cols parameters (Kp/GridRows, Kp/GridCols, Vis/GridRows, Vis/GridCols). OdomInfo: publish bundle frames. OdometryORBSLAM2: added OdomORBSLAM2/Fps and OdomORBSLAM2/MaxFeatures parameters. Registration: added Reg/RepeatOnce parameter and removed variance normalization. For util2d::getDepth() and util3d::projectDepthTo3D(), maxZError parameter is now depthErrorRatio to be dependent of the sensor range. Database: save image width and height from stereo calibration. OptimizerG2O: fixed SBA optimization when using g2o built from ORBSLAM2 library. OptimizerGTSAM: to increase optimization stability, all rotations in information matrix are divided by 100000. Added rtabmap-report tool.

Browse Source
This commit is contained in:
matlabbe
2017-11-30 16:52:03 -05:00
parent 821c1c938e
commit 4452e637ad
46 changed files with 1892 additions and 802 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Registration.cpp
+22 -50
View File
@@ -32,6 +32,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
double Registration::COVARIANCE_EPSILON = 0.000000001;
Registration * Registration::create(const ParametersMap & parameters)
{
int regTypeInt = Parameters::defaultRegStrategy();
@@ -61,8 +63,7 @@ Registration * Registration::create(Registration::Type & type, const ParametersM
}
Registration::Registration(const ParametersMap & parameters, Registration * child) :
varianceFromInliersCount_(Parameters::defaultRegVarianceFromInliersCount()),
covarianceNormalized_(Parameters::defaultRegVarianceNormalized()),
repeatOnce_(Parameters::defaultRegRepeatOnce()),
force3DoF_(Parameters::defaultRegForce3DoF()),
child_(child)
{
@@ -78,9 +79,9 @@ Registration::~Registration()
}
void Registration::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), varianceFromInliersCount_);
Parameters::parse(parameters, Parameters::kRegVarianceNormalized(), covarianceNormalized_);
Parameters::parse(parameters, Parameters::kRegRepeatOnce(), repeatOnce_);
Parameters::parse(parameters, Parameters::kRegForce3DoF(), force3DoF_);
if(child_)
{
child_->parseParameters(parameters);
@@ -194,26 +195,29 @@ Transform Registration::computeTransformationMod(
}
Transform t = computeTransformationImpl(from, to, guess, info);
if(repeatOnce_ && guess.isNull() && !t.isNull())
{
// redo with guess to get a more accurate transform
t = computeTransformationImpl(from, to, t, info);
}
if(info.covariance.empty())
{
info.covariance = cv::Mat::eye(6,6,CV_64FC1);
}
if(varianceFromInliersCount_)
{
if(info.icpInliersRatio)
{
info.covariance = cv::Mat::eye(6,6,CV_64FC1)*(info.icpInliersRatio > 0?1.0/double(info.icpInliersRatio):1.0);
}
else
{
info.covariance = cv::Mat::eye(6,6,CV_64FC1)*(info.inliers > 0?1.0/double(info.inliers):1.0);
}
}
normalizeCovariance(info.covariance, t);
if(info.covariance.at<double>(0,0)<=COVARIANCE_EPSILON)
info.covariance.at<double>(0,0) = COVARIANCE_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(1,1)<=COVARIANCE_EPSILON)
info.covariance.at<double>(1,1) = COVARIANCE_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(2,2)<=COVARIANCE_EPSILON)
info.covariance.at<double>(2,2) = COVARIANCE_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(3,3)<=COVARIANCE_EPSILON)
info.covariance.at<double>(3,3) = COVARIANCE_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(4,4)<=COVARIANCE_EPSILON)
info.covariance.at<double>(4,4) = COVARIANCE_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(5,5)<=COVARIANCE_EPSILON)
info.covariance.at<double>(5,5) = COVARIANCE_EPSILON; // epsilon if exact transform
if(child_)
{
@@ -239,36 +243,4 @@ Transform Registration::computeTransformationMod(
return t;
}
void Registration::normalizeCovariance(cv::Mat & covariance, const Transform & transform) const
{
UASSERT(covariance.cols == 6 && covariance.rows == 6);
if(covarianceNormalized_)
{
// normalize variance
float norm = transform.getNorm();
covariance.at<double>(0,0) *= norm;
covariance.at<double>(1,1) *= norm;
covariance.at<double>(2,2) *= norm;
float angle = transform.getAngle()/10.0;
covariance.at<double>(3,3) *= angle;
covariance.at<double>(4,4) *= angle;
covariance.at<double>(5,5) *= angle;
}
double epsilon = 0.000001;
if(covariance.at<double>(0,0)<=epsilon)
covariance.at<double>(0,0) = epsilon; // epsilon if exact transform
if(covariance.at<double>(1,1)<=epsilon)
covariance.at<double>(1,1) = epsilon; // epsilon if exact transform
if(covariance.at<double>(2,2)<=epsilon)
covariance.at<double>(2,2) = epsilon; // epsilon if exact transform
if(covariance.at<double>(3,3)<=epsilon)
covariance.at<double>(3,3) = epsilon; // epsilon if exact transform
if(covariance.at<double>(4,4)<=epsilon)
covariance.at<double>(4,4) = epsilon; // epsilon if exact transform
if(covariance.at<double>(5,5)<=epsilon)
covariance.at<double>(5,5) = epsilon; // epsilon if exact transform
}
}