i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +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
corelib/include/rtabmap/core/Memory.h
+1
View File
@@ -281,6 +281,7 @@ private:
int _bowEstimationType;
double _bowPnPReprojError;
int _bowPnPFlags;
bool _bowVarianceFromInliersCount;
float _icpMaxTranslation;
float _icpMaxRotation;
int _icpDecimation;
corelib/include/rtabmap/core/Odometry.h
+1
View File
@@ -88,6 +88,7 @@ private:
int _estimationType;
double _pnpReprojError;
int _pnpFlags;
bool _varianceFromInliersCount;
Transform _pose;
int _resetCurrentCount;
double previousStamp_;
corelib/include/rtabmap/core/OdometryThread.h
+1 -2
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;
};
corelib/include/rtabmap/core/Parameters.h
+2 -1
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.");
corelib/src/Memory.cpp
+7
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;
corelib/src/Odometry.cpp
+7
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
corelib/src/OdometryICP.cpp
+2 -2
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",
corelib/src/OdometryThread.cpp
+2 -13
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));
}
}
corelib/src/Rtabmap.cpp
+1
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;
}