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

Added LccBow/VarianceFromInliersCount parameter (linked with Odom/VarianceFromInliersCount in standalone gui)

Browse Source
This commit is contained in:
matlabbe
2015-10-02 10:40:18 -04:00
parent ee76a02117
commit fbf967066a
13 changed files with 117 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -281,6 +281,7 @@ private:
int _bowEstimationType;
double _bowPnPReprojError;
int _bowPnPFlags;
bool _bowVarianceFromInliersCount;
float _icpMaxTranslation;
float _icpMaxRotation;
int _icpDecimation;

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

@@ -88,6 +88,7 @@ private:
int _estimationType;
double _pnpReprojError;
int _pnpFlags;
bool _varianceFromInliersCount;
Transform _pose;
int _resetCurrentCount;
double previousStamp_;

3
corelib/include/rtabmap/core/OdometryThread.h
View File

@@ -41,7 +41,7 @@ class Odometry;
class RTABMAP_EXP OdometryThread : public UThread, public UEventsHandler {
public:
// take ownership of Odometry
OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize = 1, bool varianceFromInliersCount = false);
OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize = 1);
virtual ~OdometryThread();
protected:
@@ -63,7 +63,6 @@ private:
std::list<SensorData> _dataBuffer;
Odometry * _odometry;
unsigned int _dataBufferMaxSize;
bool _varianceFromInliersCount;
bool _resetOdometry;
};

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

@@ -331,7 +331,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features).");
RTABMAP_PARAM(Odom, ImageBufferSize, unsigned int, 1, "Data buffer size (0 min inf).");
RTABMAP_PARAM(Odom, VarianceFromInliersCount, bool, true, "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
RTABMAP_PARAM(Odom, VarianceFromInliersCount, bool, false, "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
RTABMAP_PARAM(Odom, PnPReprojError, double, 5.0, "PnP reprojection error.");
RTABMAP_PARAM(Odom, PnPFlags, int, 1, "PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
RTABMAP_PARAM(Odom, ParticleFiltering, bool, false, "Particle filtering to smooth the odometry trajectory.");
@@ -378,6 +378,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(LccBow, EpipolarGeometryVar, float, 0.02, "Epipolar geometry maximum variance to accept the loop closure.");
RTABMAP_PARAM(LccBow, PnPReprojError, double, 5.0, "PnP reprojection error.");
RTABMAP_PARAM(LccBow, PnPFlags, int, 1, "PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
RTABMAP_PARAM(LccBow, VarianceFromInliersCount, bool, false, "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
RTABMAP_PARAM_COND(LccReextract, Activated, bool, RTABMAP_NONFREE, false, true, "Activate re-extracting features on global loop closure.");
RTABMAP_PARAM(LccReextract, NNType, int, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4.");
RTABMAP_PARAM(LccReextract, NNDR, float, 0.8, "NNDR: nearest neighbor distance ratio.");

7
corelib/src/Memory.cpp
View File

@@ -108,6 +108,7 @@ Memory::Memory(const ParametersMap & parameters) :
_bowEstimationType(Parameters::defaultLccBowEstimationType()),
_bowPnPReprojError(Parameters::defaultLccBowPnPReprojError()),
_bowPnPFlags(Parameters::defaultLccBowPnPFlags()),
_bowVarianceFromInliersCount(Parameters::defaultLccBowVarianceFromInliersCount()),
_icpMaxTranslation(Parameters::defaultLccIcpMaxTranslation()),
_icpMaxRotation(Parameters::defaultLccIcpMaxRotation()),
@@ -450,6 +451,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kLccBowEpipolarGeometryVar(), _bowEpipolarGeometryVar);
Parameters::parse(parameters, Parameters::kLccBowPnPReprojError(), _bowPnPReprojError);
Parameters::parse(parameters, Parameters::kLccBowPnPFlags(), _bowPnPFlags);
Parameters::parse(parameters, Parameters::kLccBowVarianceFromInliersCount(), _bowVarianceFromInliersCount);
Parameters::parse(parameters, Parameters::kLccIcpMaxTranslation(), _icpMaxTranslation);
Parameters::parse(parameters, Parameters::kLccIcpMaxRotation(), _icpMaxRotation);
Parameters::parse(parameters, Parameters::kLccIcp3Decimation(), _icpDecimation);
@@ -2207,6 +2209,11 @@ Transform Memory::computeVisualTransform(
}
}
if(_bowVarianceFromInliersCount)
{
variance = inliersCount > 0?1.0/double(inliersCount):1.0;
}
if(rejectedMsg)
{
*rejectedMsg = msg;

7
corelib/src/Odometry.cpp
View File

@@ -54,6 +54,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_estimationType(Parameters::defaultOdomEstimationType()),
_pnpReprojError(Parameters::defaultOdomPnPReprojError()),
_pnpFlags(Parameters::defaultOdomPnPFlags()),
_varianceFromInliersCount(Parameters::defaultOdomVarianceFromInliersCount()),
_resetCurrentCount(0),
previousStamp_(0),
previousTransform_(Transform::getIdentity()),
@@ -73,6 +74,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kOdomPnPReprojError(), _pnpReprojError);
Parameters::parse(parameters, Parameters::kOdomPnPFlags(), _pnpFlags);
UASSERT(_pnpFlags>=0 && _pnpFlags <=2);
Parameters::parse(parameters, Parameters::kOdomVarianceFromInliersCount(), _varianceFromInliersCount);
Parameters::parse(parameters, Parameters::kOdomParticleFiltering(), _particleFiltering);
Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize);
Parameters::parse(parameters, Parameters::kOdomParticleNoiseT(), _particleNoiseT);
@@ -269,6 +271,11 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
{
distanceTravelled_ += t.getNorm();
info->distanceTravelled = distanceTravelled_;
if(_varianceFromInliersCount)
{
info->variance = info->inliers > 0?1.0/double(info->inliers):1.0;
}
}
return _pose *= t; // updated

4
corelib/src/OdometryICP.cpp
View File

@@ -72,6 +72,7 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo *
bool hasConverged = false;
double variance = 0;
unsigned int minPoints = 100;
int correspondences = 0;
if(!data.depthOrRightRaw().empty())
{
if(data.depthOrRightRaw().type() == CV_8UC1)
@@ -121,7 +122,6 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo *
hasConverged,
*newCloudRegistered);
int correspondences = 0;
util3d::computeVarianceAndCorrespondences(
newCloudRegistered,
_previousCloudNormal,
@@ -164,7 +164,6 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo *
hasConverged,
*newCloudRegistered);
int correspondences = 0;
util3d::computeVarianceAndCorrespondences(
newCloudRegistered,
_previousCloud,
@@ -202,6 +201,7 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo *
if(info)
{
info->variance = variance;
info->inliers = correspondences;
}
UINFO("Odom update time = %fs hasConverged=%s variance=%f cloud=%d",

15
corelib/src/OdometryThread.cpp
View File

@@ -34,10 +34,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize, bool varianceFromInliersCount) :
OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize) :
_odometry(odometry),
_dataBufferMaxSize(dataBufferMaxSize),
_varianceFromInliersCount(varianceFromInliersCount),
_resetOdometry(false)
{
UASSERT(_odometry != 0);
@@ -99,17 +98,7 @@ void OdometryThread::mainLoop()
OdometryInfo info;
Transform pose = _odometry->process(data, &info);
// a null pose notify that odometry could not be computed
double variance;
if(_varianceFromInliersCount)
{
variance = info.inliers > 0?1.0/double(info.inliers):1.0;
}
else
{
variance = info.variance>0?info.variance:1.0;
}
double variance = info.variance>0?info.variance:1;
this->post(new OdometryEvent(data, pose, variance, variance, info));
}
}

1
corelib/src/Rtabmap.cpp
View File

@@ -655,6 +655,7 @@ int Rtabmap::triggerNewMap()
UINFO("New map triggered, new map = %d", mapId);
_optimizedPoses.clear();
_constraints.clear();
_lastLocalizationNodeId = 0;
}
return mapId;
}

2
guilib/include/rtabmap/gui/PreferencesDialog.h
View File

@@ -208,7 +208,7 @@ public:
double getSimThr() const;
int getOdomStrategy() const;
int getOdomBufferSize() const;
bool getOdomVarianceFromInliersCount() const;
bool getLccBowVarianceFromInliersCount() const;
QString getCameraInfoDir() const; // "workinfDir/camera_info"
//

4
guilib/src/MainWindow.cpp
View File

@@ -2931,7 +2931,7 @@ void MainWindow::startDetection()
{
odom = new OdometryBOW(parameters);
}
_odomThread = new OdometryThread(odom, _preferencesDialog->getOdomBufferSize(), _preferencesDialog->getOdomVarianceFromInliersCount());
_odomThread = new OdometryThread(odom, _preferencesDialog->getOdomBufferSize());
UEventsManager::addHandler(_odomThread);
UEventsManager::createPipe(_camera, _odomThread, "CameraEvent");
@@ -2968,7 +2968,7 @@ void MainWindow::startDetection()
{
odom = new OdometryBOW(parameters);
}
_odomThread = new OdometryThread(odom, _preferencesDialog->getOdomBufferSize(), _preferencesDialog->getOdomVarianceFromInliersCount());
_odomThread = new OdometryThread(odom, _preferencesDialog->getOdomBufferSize());
UEventsManager::addHandler(_odomThread);
_odomThread->start();

21
guilib/src/PreferencesDialog.cpp
View File

@@ -608,6 +608,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->loopClosure_bowEpipolarGeometryVar->setObjectName(Parameters::kLccBowEpipolarGeometryVar().c_str());
_ui->loopClosure_pnpReprojError->setObjectName(Parameters::kLccBowPnPReprojError().c_str());
_ui->loopClosure_pnpFlags->setObjectName(Parameters::kLccBowPnPFlags().c_str());
_ui->loopClosure_bowVarianceFromInliersCount->setObjectName(Parameters::kLccBowVarianceFromInliersCount().c_str());
_ui->groupBox_reextract->setObjectName(Parameters::kLccReextractActivated().c_str());
_ui->reextract_nn->setObjectName(Parameters::kLccReextractNNType().c_str());
@@ -651,6 +652,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str());
_ui->odom_dataBufferSize->setObjectName(Parameters::kOdomImageBufferSize().c_str());
_ui->odom_varianceFromInliersCount->setObjectName(Parameters::kOdomVarianceFromInliersCount().c_str());
connect(_ui->odom_varianceFromInliersCount, SIGNAL(clicked(bool)), _ui->loopClosure_bowVarianceFromInliersCount, SLOT(setChecked(bool)));
connect(_ui->loopClosure_bowVarianceFromInliersCount, SIGNAL(clicked(bool)), _ui->odom_varianceFromInliersCount, SLOT(setChecked(bool)));
_ui->lineEdit_odom_roi->setObjectName(Parameters::kOdomRoiRatios().c_str());
_ui->odom_estimationType->setObjectName(Parameters::kOdomEstimationType().c_str());
connect(_ui->odom_estimationType, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_odomEstimation, SLOT(setCurrentIndex(int)));
@@ -1938,6 +1941,17 @@ bool PreferencesDialog::validateForm()
_ui->odom_bin_nn->setCurrentIndex(VWDictionary::kNNBruteForce);
}
if(_ui->groupBox_odometry1->isEnabled() &&
_ui->loopClosure_bowVarianceFromInliersCount->isChecked() != _ui->odom_varianceFromInliersCount->isChecked())
{
QMessageBox::warning(this, tr("Parameter warning"),
tr("Odometry %1 variance from inliers count but Loop Closure constraint %2. "
"Applying the same parameter to Loop Closure Constraint.")
.arg(_ui->odom_varianceFromInliersCount->isChecked()?tr("uses"):tr("does not use"))
.arg(_ui->odom_varianceFromInliersCount->isChecked()?tr("does not"):tr("does")));
_ui->loopClosure_bowVarianceFromInliersCount->setChecked(_ui->odom_varianceFromInliersCount->isChecked());
}
return true;
}
@@ -3745,9 +3759,9 @@ int PreferencesDialog::getOdomBufferSize() const
{
return _ui->odom_dataBufferSize->value();
}
bool PreferencesDialog::getOdomVarianceFromInliersCount() const
bool PreferencesDialog::getLccBowVarianceFromInliersCount() const
{
return _ui->odom_varianceFromInliersCount->isChecked();
return _ui->loopClosure_bowVarianceFromInliersCount->isChecked();
}
QString PreferencesDialog::getCameraInfoDir() const
@@ -3888,8 +3902,7 @@ void PreferencesDialog::testOdometry(int type)
OdometryThread odomThread(
odometry, // take ownership of odometry
_ui->odom_dataBufferSize->value(),
_ui->odom_varianceFromInliersCount->isChecked());
_ui->odom_dataBufferSize->value());
odomThread.registerToEventsManager();
OdometryViewer * odomViewer = new OdometryViewer(10,

126
guilib/src/ui/preferencesDialog.ui
View File

@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>23</number>
<number>20</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -7105,32 +7105,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</item>
<item>
<layout class="QGridLayout" name="gridLayout_23" columnstretch="0,1">
<item row="1" column="0">
<widget class="QSpinBox" name="loopClosure_bowMinInliers">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>1000</number>
</property>
<property name="value">
<number>10</number>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_2">
<property name="text">
<string>Minimum visual word correspondences to accept the estimated transformation.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QCheckBox" name="loopClosure_bowForce2D">
<property name="text">
@@ -7138,32 +7112,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QSpinBox" name="loopClosure_bowIterations">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>10000</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>100</number>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_15">
<property name="text">
<string>Maximum RANSAC iterations.</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QComboBox" name="globalDetection_icpType">
<property name="sizeAdjustPolicy">
@@ -7199,6 +7147,58 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QSpinBox" name="loopClosure_bowMinInliers">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>1000</number>
</property>
<property name="value">
<number>10</number>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_2">
<property name="text">
<string>Minimum visual word correspondences to accept the estimated transformation.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QSpinBox" name="loopClosure_bowIterations">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>10000</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>100</number>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_15">
<property name="text">
<string>Maximum RANSAC iterations.</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_57">
<property name="text">
@@ -7244,6 +7244,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_262">
<property name="text">
<string>Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QCheckBox" name="loopClosure_bowVarianceFromInliersCount">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
<item>