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/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;
}