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

Refactoring Registration: added RegistrationInfo class, pipeline registration. Camera source: create laser scan from depth image. Removed OdometryICP as the same behavior can be achieved with OdometryF2F and "Reg/Strategy" = 1 (ICP) or 2 (Vis+ICP).

Browse Source
This commit is contained in:
matlabbe
2016-01-06 17:26:55 -05:00
parent 1c66ad1db2
commit 28d9bbb85d
35 changed files with 1209 additions and 1362 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
corelib/include/rtabmap/core/CameraEvent.h
View File

@@ -48,7 +48,7 @@ public:
UEvent(kCodeData),
data_(image, seq, stamp)
{
cameraInfo_.cameraName_ = cameraName;
cameraInfo_.cameraName = cameraName;
}
CameraEvent() :
@@ -66,7 +66,7 @@ public:
UEvent(kCodeData),
data_(data)
{
cameraInfo_.cameraName_ = cameraName;
cameraInfo_.cameraName = cameraName;
}
CameraEvent(const SensorData & data, const CameraInfo & cameraInfo) :
UEvent(kCodeData),
@@ -77,7 +77,7 @@ public:
// Image or descriptors
const SensorData & data() const {return data_;}
const std::string & cameraName() const {return cameraInfo_.cameraName_;}
const std::string & cameraName() const {return cameraInfo_.cameraName;}
const CameraInfo & info() const {return cameraInfo_;}
virtual ~CameraEvent() {}

22
corelib/include/rtabmap/core/CameraInfo.h
View File

@@ -37,20 +37,22 @@ class CameraInfo
public:
CameraInfo() :
cameraName_(""),
id_(0),
timeCapture_(0.0),
timeDisparity_(0.0),
timeMirroring_(0.0)
cameraName(""),
id(0),
timeCapture(0.0f),
timeDisparity(0.0f),
timeMirroring(0.0f),
timeScanFromDepth(0.0f)
{
}
virtual ~CameraInfo() {}
std::string cameraName_;
int id_;
float timeCapture_;
float timeDisparity_;
float timeMirroring_;
std::string cameraName;
int id;
float timeCapture;
float timeDisparity;
float timeMirroring;
float timeScanFromDepth;
};
} // namespace rtabmap

5
corelib/include/rtabmap/core/CameraThread.h
View File

@@ -55,6 +55,8 @@ public:
void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
void setScanFromDepth(bool enabled, int decimation=4, float maxDepth=4.0f)
{_scanFromDepth = enabled; _scanDecimation=decimation; _scanMaxDepth = maxDepth;}
//getters
bool isPaused() const {return !this->isRunning();}
@@ -72,6 +74,9 @@ private:
bool _mirroring;
bool _colorOnly;
bool _stereoToDepth;
bool _scanFromDepth;
int _scanDecimation;
float _scanMaxDepth;
StereoDense * _stereoDense;
};

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

@@ -51,7 +51,8 @@ class VWDictionary;
class VisualWord;
class Feature2D;
class Statistics;
class RegistrationVis;
class Registration;
class RegistrationInfo;
class RegistrationIcp;
class Stereo;
@@ -178,15 +179,13 @@ public:
std::multimap<int, Link> & links,
bool lookInDatabase = false);
Transform computeVisualTransform(int fromId, int toId, std::string * rejectedMsg = 0, int * inliers = 0, float * variance = 0);
Transform computeIcpTransform(int fromId, int toId, Transform guess, std::string * rejectedMsg = 0, int * correspondences = 0, float * variance = 0, float * correspondencesRatio = 0);
Transform computeTransform(int fromId, int toId, RegistrationInfo * info = 0);
Transform computeIcpTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0);
Transform computeIcpTransformMulti(
int newId,
int oldId,
const std::map<int, Transform> & poses,
std::string * rejectedMsg = 0,
int * inliers = 0,
float * variance = 0);
RegistrationInfo * info = 0);
private:
void preUpdate();
@@ -264,7 +263,7 @@ private:
bool _tfIdfLikelihoodUsed;
bool _parallelized;
RegistrationVis * _registrationVis;
Registration * _registrationPipeline;
RegistrationIcp * _registrationIcp;
};

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

@@ -50,7 +50,7 @@ public:
public:
static Odometry * create(const ParametersMap & parameters);
static Odometry * create(Type & type, const ParametersMap & parameters);
static Odometry * create(Type & type, const ParametersMap & parameters = ParametersMap());
public:
virtual ~Odometry();

6
corelib/include/rtabmap/core/OdometryF2F.h
View File

@@ -29,10 +29,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#define ODOMETRYF2F_H_
#include <rtabmap/core/Odometry.h>
#include <rtabmap/core/RegistrationVis.h>
#include <rtabmap/core/Signature.h>
namespace rtabmap {
class Registration;
class RTABMAP_EXP OdometryF2F : public Odometry
{
public:
@@ -51,7 +53,7 @@ private:
int keyFrameThr_;
bool guessFromMotion_;
RegistrationVis registration_;
Registration * registrationPipeline_;
Signature refFrame_;
Transform motionSinceLastKeyFrame_;
};

68
corelib/include/rtabmap/core/OdometryICP.h
View File

@@ -1,68 +0,0 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef ODOMETRYICP_H_
#define ODOMETRYICP_H_
#include <rtabmap/core/Odometry.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
namespace rtabmap {
class RTABMAP_EXP OdometryICP : public Odometry
{
public:
OdometryICP(int decimation = 4,
float voxelSize = 0.005f,
int samples = 0,
float maxCorrespondenceDistance = 0.05f,
int maxIterations = 30,
float correspondenceRatio = 0.7f,
bool pointToPlane = true,
const ParametersMap & odometryParameter = rtabmap::ParametersMap());
virtual void reset(const Transform & initialPose = Transform::getIdentity());
private:
virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);
private:
int _decimation;
float _voxelSize;
float _samples;
float _maxCorrespondenceDistance;
int _maxIterations;
float _correspondenceRatio;
bool _pointToPlane;
pcl::PointCloud<pcl::PointNormal>::Ptr _previousCloudNormal; // for point ot plane
pcl::PointCloud<pcl::PointXYZ>::Ptr _previousCloud; // for point to point
};
}
#endif /* ODOMETRYICP_H_ */

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

@@ -311,7 +311,6 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, LocalImmunizationRatio, float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer.");
RTABMAP_PARAM(RGBD, ScanMatchingIdsSavedInLinks, bool, true, "Save scan matching IDs in link's user data.");
RTABMAP_PARAM(RGBD, NeighborLinkRefining, bool, false, "When a new node is added to the graph, the transformation of its neighbor link to the previous node is refined using ICP (laser scans required!).");
RTABMAP_PARAM(RGBD, LoopClosureLinkRefining, bool, false, "If the estimated loop closure transformation is refined using ICP (laser scans required!).");
RTABMAP_PARAM(RGBD, LoopClosureReextractFeatures, bool, true, "Extract features even if there are some already in the nodes.");
// Local/Proximity loop closure detection
@@ -361,6 +360,8 @@ class RTABMAP_EXP Parameters
// Common registration parameters
RTABMAP_PARAM(Reg, 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(Reg, Strategy, int, 0, "0=Vis, 1=Icp, 2=VisIcp");
RTABMAP_PARAM(Reg, Force3DoF, bool, false, "Force 3 degrees-of-freedom transform (3Dof: x,y and yaw). Parameters z, roll and pitch will be set to 0.");
// Visual registration parameters
RTABMAP_PARAM(Vis, EstimationType, int, 0, "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
@@ -373,7 +374,6 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Vis, EpipolarGeometryVar, float, 0.02, "[Vis/EstimationType = 2] Epipolar geometry maximum variance to accept the transformation.");
RTABMAP_PARAM(Vis, MinInliers, int, 10, "Minimum feature correspondences to compute/accept the transformation.");
RTABMAP_PARAM(Vis, Iterations, int, 100, "Maximum iterations to compute the transform.");
RTABMAP_PARAM(Vis, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw). Parameters z, roll and pitch will be set to 0.");
RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
RTABMAP_PARAM(Vis, MaxFeatures, int, 1000, "0 no limits.");
RTABMAP_PARAM(Vis, MaxDepth, float, 0.0, "Max depth of the features (0 means no limit).");
@@ -394,7 +394,6 @@ class RTABMAP_EXP Parameters
// ICP registration parameters
RTABMAP_PARAM(Icp, MaxTranslation, float, 0.2, "Maximum ICP translation correction accepted (m).");
RTABMAP_PARAM(Icp, MaxRotation, float, 0.78, "Maximum ICP rotation correction accepted (rad).");
RTABMAP_PARAM(Icp, 2D, bool, true, "If 2D ICP is done (only 3Dof -> x,y,yaw).");
RTABMAP_PARAM(Icp, VoxelSize, float, 0.025, "Uniform sampling voxel size (0=disabled).");
RTABMAP_PARAM(Icp, DownsamplingStep, int, 1, "Downsampling step size (1=no sampling). This is done before uniform sampling.");
RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05, "Max distance for point correspondences.");

77
corelib/include/rtabmap/core/Registration.h
View File

@@ -32,50 +32,75 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/Signature.h>
#include <rtabmap/core/RegistrationInfo.h>
namespace rtabmap {
class RTABMAP_EXP Registration
{
public:
virtual ~Registration() {}
virtual void parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), _varianceFromInliersCount);
}
enum Type {
kTypeUndef = -1,
kTypeVis = 0,
kTypeIcp = 1,
kTypeVisIcp = 2
};
public:
static Registration * create(const ParametersMap & parameters);
static Registration * create(Type & type, const ParametersMap & parameters = ParametersMap());
public:
virtual ~Registration();
virtual void parseParameters(const ParametersMap & parameters);
bool isImageRequired() const;
bool isScanRequired() const;
bool isUserDataRequired() const;
bool varianceFromInliersCount() const {return varianceFromInliersCount_;}
bool force3DoF() const {return force3DoF_;}
// take ownership!
void setChildRegistration(Registration * child);
Transform computeTransformation(
const Signature & from,
const Signature & to,
Transform guess = Transform::getIdentity(),
std::string * rejectedMsg = 0,
std::vector<int> * inliersOut = 0,
float * varianceOut = 0,
float * inliersRatioOut = 0) const
{
Signature fromCopy(from);
Signature toCopy(to);
return computeTransformationMod(fromCopy, toCopy, guess, rejectedMsg, inliersOut, varianceOut, inliersRatioOut);
}
RegistrationInfo * info = 0) const;
Transform computeTransformation(
const SensorData & from,
const SensorData & to,
Transform SensorData = Transform::getIdentity(),
RegistrationInfo * info = 0) const;
virtual Transform computeTransformationMod(
Transform computeTransformationMod(
Signature & from,
Signature & to,
Transform guess = Transform::getIdentity(),
std::string * rejectedMsg = 0,
std::vector<int> * inliersOut = 0,
float * varianceOut = 0,
float * inliersRatioOut = 0) const = 0;
RegistrationInfo * info = 0) const;
protected:
Registration(const ParametersMap & parameters = ParametersMap()) :
_varianceFromInliersCount(Parameters::defaultRegVarianceFromInliersCount())
{
this->parseParameters(parameters);
}
// take ownership of child
Registration(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
protected:
bool _varianceFromInliersCount;
// It is safe to modify the signatures in the implementation, if so, the
// child registration will use these modifications.
virtual Transform computeTransformationImpl(
Signature & from,
Signature & to,
Transform guess,
RegistrationInfo & info) const = 0;
virtual bool isImageRequiredImpl() const = 0;
virtual bool isScanRequiredImpl() const = 0;
virtual bool isUserDataRequiredImpl() const = 0;
private:
bool varianceFromInliersCount_;
bool force3DoF_;
Registration * child_;
};

26
corelib/include/rtabmap/core/RegistrationIcp.h
View File

@@ -39,33 +39,25 @@ namespace rtabmap {
class RTABMAP_EXP RegistrationIcp : public Registration
{
public:
RegistrationIcp(const ParametersMap & parameters = ParametersMap());
// take ownership of child
RegistrationIcp(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
virtual ~RegistrationIcp() {}
virtual void parseParameters(const ParametersMap & parameters);
virtual Transform computeTransformationMod(
protected:
virtual Transform computeTransformationImpl(
Signature & from,
Signature & to,
Transform guess = Transform::getIdentity(),
std::string * rejectedMsg = 0,
std::vector<int> * inliersOut = 0,
float * varianceOut = 0,
float * inliersRatioOut = 0) const;
Transform computeTransformation(
const SensorData & from,
const SensorData & to,
Transform guess = Transform::getIdentity(),
std::string * rejectedMsg = 0,
std::vector<int> * inliersOut = 0,
float * varianceOut = 0,
float * inliersRatioOut = 0) const;
Transform guess,
RegistrationInfo & info) const;
virtual bool isImageRequiredImpl() const {return false;}
virtual bool isScanRequiredImpl() const {return true;}
virtual bool isUserDataRequiredImpl() const {return false;}
private:
float _maxTranslation;
float _maxRotation;
bool _icp2D;
float _voxelSize;
int _downsamplingStep;
float _maxCorrespondenceDistance;

33
corelib/include/rtabmap/core/RegistrationInfo.h Normal file
View File

@@ -0,0 +1,33 @@
/*
* RegistrationInfo.h
*
* Created on: Jan 5, 2016
* Author: mathieu
*/
#ifndef REGISTRATIONINFO_H_
#define REGISTRATIONINFO_H_
namespace rtabmap {
class RegistrationInfo
{
public:
RegistrationInfo() :
variance(0),
inliers(0),
inliersRatio(0)
{
}
float variance;
int inliers;
float inliersRatio;
std::vector<int> inliersIndexes_;
std::string rejectedMsg_;
};
}
#endif /* REGISTRATIONINFO_H_ */

25
corelib/include/rtabmap/core/RegistrationVis.h
View File

@@ -39,31 +39,32 @@ namespace rtabmap {
class RTABMAP_EXP RegistrationVis : public Registration
{
public:
RegistrationVis(const ParametersMap & parameters = ParametersMap());
// take ownership of child
RegistrationVis(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
virtual ~RegistrationVis();
virtual void parseParameters(const ParametersMap & parameters);
virtual Transform computeTransformationMod(
Signature & from,
Signature & to,
Transform guess = Transform::getIdentity(), // guess is ignored for RegistrationVis
std::string * rejectedMsg = 0,
std::vector<int> * inliersOut = 0,
float * varianceOut = 0,
float * inliersRatioOut = 0) const;
float getBowInlierDistance() const {return _inlierDistance;}
int getBowIterations() const {return _iterations;}
int getBowMinInliers() const {return _minInliers;}
bool getBowForce2D() const {return _force2D;}
protected:
virtual Transform computeTransformationImpl(
Signature & from,
Signature & to,
Transform guess,
RegistrationInfo & info) const;
virtual bool isImageRequiredImpl() const {return true;}
virtual bool isScanRequiredImpl() const {return false;}
virtual bool isUserDataRequiredImpl() const {return false;}
private:
int _minInliers;
float _inlierDistance;
int _iterations;
int _refineIterations;
bool _force2D;
float _epipolarGeometryVar;
int _estimationType;
bool _forwardEstimateOnly;

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

@@ -184,7 +184,6 @@ private:
float _rgbdLinearUpdate;
float _rgbdAngularUpdate;
float _newMapOdomChangeDistance;
bool _loopClosureRefining;
bool _neighborLinkRefining;
bool _proximityByTime;
bool _proximityBySpace;

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

@@ -97,6 +97,7 @@ public:
Transform inverse() const;
Transform rotation() const;
Transform translation() const;
Transform to3DoF() const;
void getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const;
void getEulerAngles(float & roll, float & pitch, float & yaw) const;

2
corelib/src/CMakeLists.txt
View File

@@ -50,6 +50,7 @@ SET(SRC_FILES
OptimizerGTSAM.cpp
OptimizerCVSBA.cpp
Registration.cpp
RegistrationIcp.cpp
RegistrationVis.cpp
@@ -57,7 +58,6 @@ SET(SRC_FILES
OdometryThread.cpp
OdometryLocalMap.cpp
OdometryMono.cpp
OdometryICP.cpp
OdometryF2F.cpp
Stereo.cpp

4
corelib/src/Camera.cpp
View File

@@ -102,8 +102,8 @@ SensorData Camera::takeImage(CameraInfo * info)
}
if(info)
{
info->id_ = data.id();
info->timeCapture_ = captureTime;
info->id = data.id();
info->timeCapture = captureTime;
}
return data;
}

32
corelib/src/CameraThread.cpp
View File

@@ -45,6 +45,9 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
_mirroring(false),
_colorOnly(false),
_stereoToDepth(false),
_scanFromDepth(false),
_scanDecimation(4),
_scanMaxDepth(4.0f),
_stereoDense(new StereoBM(parameters))
{
UASSERT(_camera != 0);
@@ -103,7 +106,7 @@ void CameraThread::mainLoop()
cv::flip(data.depthRaw(), tmpDepth, 1);
data.setDepthOrRightRaw(tmpDepth);
}
info.timeMirroring_ = timer.ticks();
info.timeMirroring = timer.ticks();
}
if(_stereoToDepth && data.stereoCameraModel().isValid() && !data.rightRaw().empty())
{
@@ -115,10 +118,31 @@ void CameraThread::mainLoop()
data.setCameraModel(data.stereoCameraModel().left());
data.setDepthOrRightRaw(depth);
data.setStereoCameraModel(StereoCameraModel());
info.timeDisparity_ = timer.ticks();
UINFO("Computing disparity = %f s", info.timeDisparity_);
info.timeDisparity = timer.ticks();
UINFO("Computing disparity = %f s", info.timeDisparity);
}
info.cameraName_ = _camera->getSerial();
if(_scanFromDepth &&
data.cameraModels().size() &&
data.cameraModels().at(0).isValid() &&
!data.depthRaw().empty())
{
if(data.laserScanRaw().empty())
{
UASSERT(_scanDecimation >= 1);
UTimer timer;
cv::Mat scan = util3d::laserScanFromPointCloud(*util3d::cloudFromSensorData(data, _scanDecimation, _scanMaxDepth));
data.setLaserScanRaw(scan, (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation), _scanMaxDepth);
info.timeScanFromDepth = timer.ticks();
UINFO("Computing scan from depth = %f s", info.timeScanFromDepth);
}
else
{
UWARN("Option to create laser scan from depth image is enabled, but "
"there is already a laser scan in the captured sensor data. Scan from "
"depth will not be created.");
}
}
info.cameraName = _camera->getSerial();
this->post(new CameraEvent(data, info));
}
else if(!this->isKilled())

107
corelib/src/Memory.cpp
View File

@@ -41,6 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/VisualWord.h"
#include "rtabmap/core/Features2d.h"
#include "rtabmap/core/RegistrationIcp.h"
#include "rtabmap/core/Registration.h"
#include "rtabmap/core/RegistrationVis.h"
#include "rtabmap/core/DBDriver.h"
#include "rtabmap/core/util3d_features.h"
@@ -102,7 +103,7 @@ Memory::Memory(const ParametersMap & parameters) :
{
_feature2D = Feature2D::create(parameters);
_vwd = new VWDictionary(parameters);
_registrationVis = new RegistrationVis(parameters);
_registrationPipeline = Registration::create(parameters);
_registrationIcp = new RegistrationIcp(parameters);
this->parseParameters(parameters);
}
@@ -365,9 +366,9 @@ Memory::~Memory()
{
delete _vwd;
}
if(_registrationVis)
if(_registrationPipeline)
{
delete _registrationVis;
delete _registrationPipeline;
}
if(_registrationIcp)
{
@@ -447,10 +448,27 @@ void Memory::parseParameters(const ParametersMap & parameters)
_feature2D->parseParameters(parameters);
}
if(_registrationVis)
Registration::Type regStrategy = Registration::kTypeUndef;
if((iter=parameters.find(Parameters::kRegStrategy())) != parameters.end())
{
_registrationVis->parseParameters(parameters);
regStrategy = (Registration::Type)std::atoi((*iter).second.c_str());
}
if(regStrategy!=Registration::kTypeUndef)
{
UDEBUG("new registration strategy %d", int(regStrategy));
if(_registrationPipeline)
{
delete _registrationPipeline;
_registrationPipeline = 0;
}
_registrationPipeline = Registration::create(regStrategy, parameters_);
}
else if(_registrationPipeline)
{
_registrationPipeline->parseParameters(parameters);
}
if(_registrationIcp)
{
_registrationIcp->parseParameters(parameters);
@@ -2012,12 +2030,10 @@ void Memory::removeLink(int oldId, int newId)
}
// compute transform fromId -> toId
Transform Memory::computeVisualTransform(
Transform Memory::computeTransform(
int fromId,
int toId,
std::string * rejectedMsg,
int * inliers,
float * variance)
RegistrationInfo * info)
{
const Signature * fromS = this->getSignature(fromId);
const Signature * toS = this->getSignature(toId);
@@ -2026,37 +2042,49 @@ Transform Memory::computeVisualTransform(
if(fromS && toS)
{
// compute transform fromId -> toId
std::vector<int> inliersV;
if(_reextractLoopClosureFeatures)
// make sure we have all data needed
if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired()) ||
_registrationPipeline->isScanRequired() ||
_registrationPipeline->isUserDataRequired())
{
getNodeData(fromS->id(), true);
getNodeData(toS->id(), true);
}
// compute transform fromId -> toId
std::vector<int> inliersV;
if(_reextractLoopClosureFeatures || (fromS->getWords().size() && toS->getWords().size()))
{
Signature tmpFrom = *fromS;
Signature tmpTo = *toS;
tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
tmpTo.setWords3(std::multimap<int, cv::Point3f>());
transform = _registrationVis->computeTransformation(tmpFrom, tmpTo, Transform::getIdentity(), rejectedMsg, &inliersV, variance);
}
else if(fromS->getWords().size() && toS->getWords().size())
{
transform = _registrationVis->computeTransformation(*fromS, *toS, Transform::getIdentity(), rejectedMsg, &inliersV, variance);
}
if(inliers)
{
*inliers = (int)inliersV.size();
if(_reextractLoopClosureFeatures)
{
tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
tmpTo.setWords3(std::multimap<int, cv::Point3f>());
}
Transform guess = Transform::getIdentity();
if(!_registrationPipeline->isImageRequired())
{
// no visual in the pipeline, make visual registration for guess
RegistrationVis regVis(parameters_);
guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
}
if(!guess.isNull())
{
transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
}
}
}
else
{
std::string msg = uFormat("Did not find nodes %d and/or %d", fromId, toId);
if(rejectedMsg)
if(info)
{
*rejectedMsg = msg;
info->rejectedMsg_ = msg;
}
UWARN(msg.c_str());
}
@@ -2068,10 +2096,7 @@ Transform Memory::computeIcpTransform(
int fromId,
int toId,
Transform guess,
std::string * rejectedMsg,
int * inliers,
float * variance,
float * inliersRatio)
RegistrationInfo * info)
{
Signature * fromS = this->_getSignature(fromId);
Signature * toS = this->_getSignature(toId);
@@ -2107,18 +2132,14 @@ Transform Memory::computeIcpTransform(
// compute transform fromId -> toId
std::vector<int> inliersV;
t = _registrationIcp->computeTransformation(fromS->sensorData(), toS->sensorData(), guess, rejectedMsg, &inliersV, variance, inliersRatio);
if(inliers)
{
*inliers = (int)inliersV.size();
}
t = _registrationIcp->computeTransformation(fromS->sensorData(), toS->sensorData(), guess, info);
}
else
{
std::string msg = uFormat("Did not find nodes %d and/or %d", fromId, toId);
if(rejectedMsg)
if(info)
{
*rejectedMsg = msg;
info->rejectedMsg_ = msg;
}
UWARN(msg.c_str());
}
@@ -2130,9 +2151,7 @@ Transform Memory::computeIcpTransformMulti(
int fromId,
int toId,
const std::map<int, Transform> & poses,
std::string * rejectedMsg,
int * inliers,
float * variance)
RegistrationInfo * info)
{
UASSERT(uContains(poses, fromId) && uContains(_signatures, fromId));
UASSERT(uContains(poses, toId) && uContains(_signatures, toId));
@@ -2192,11 +2211,7 @@ Transform Memory::computeIcpTransformMulti(
Transform guess = poses.at(fromId).inverse() * poses.at(toId);
std::vector<int> inliersV;
t = _registrationIcp->computeTransformation(fromS->sensorData(), assembledData, guess, rejectedMsg, &inliersV, variance);
if(inliers)
{
*inliers = (int)inliersV.size();
}
t = _registrationIcp->computeTransformation(fromS->sensorData(), assembledData, guess, info);
}
return t;

4
corelib/src/Odometry.cpp
View File

@@ -70,7 +70,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_minDepth(Parameters::defaultVisMinDepth()),
_maxDepth(Parameters::defaultVisMaxDepth()),
_resetCountdown(Parameters::defaultOdomResetCountdown()),
_force2D(Parameters::defaultVisForce2D()),
_force2D(Parameters::defaultRegForce3DoF()),
_holonomic(Parameters::defaultOdomHolonomic()),
_filteringStrategy(Parameters::defaultOdomFilteringStrategy()),
_particleSize(Parameters::defaultOdomParticleSize()),
@@ -100,7 +100,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kVisMinDepth(), _minDepth);
Parameters::parse(parameters, Parameters::kVisMaxDepth(), _maxDepth);
Parameters::parse(parameters, Parameters::kVisRoiRatios(), _roiRatios);
Parameters::parse(parameters, Parameters::kVisForce2D(), _force2D);
Parameters::parse(parameters, Parameters::kRegForce3DoF(), _force2D);
Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic);
Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);

63
corelib/src/OdometryF2F.cpp
View File

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/OdometryF2F.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/Registration.h"
#include "rtabmap/core/EpipolarGeometry.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
@@ -37,15 +38,16 @@ OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
Odometry(parameters),
keyFrameThr_(Parameters::defaultOdomFlowKeyFrameThr()),
guessFromMotion_(Parameters::defaultOdomFlowGuessMotion()),
registration_(parameters),
motionSinceLastKeyFrame_(Transform::getIdentity())
{
registrationPipeline_ = Registration::create(parameters);
Parameters::parse(parameters, Parameters::kOdomFlowKeyFrameThr(), keyFrameThr_);
Parameters::parse(parameters, Parameters::kOdomFlowGuessMotion(), guessFromMotion_);
}
OdometryF2F::~OdometryF2F()
{
delete registrationPipeline_;
}
void OdometryF2F::reset(const Transform & initialPose)
@@ -74,20 +76,16 @@ Transform OdometryF2F::computeTransform(
return output;
}
float variance = 0;
std::vector<int> inliers;
RegistrationInfo regInfo;
Signature newFrame(data);
if(refFrame_.getWords().size())
if(refFrame_.sensorData().isValid())
{
std::string rejectedMsg;
output = registration_.computeTransformationMod(
output = registrationPipeline_->computeTransformationMod(
refFrame_,
newFrame,
guessFromMotion_?motionSinceLastKeyFrame_*this->previousTransform():Transform(),
&rejectedMsg,
&inliers,
&variance);
&regInfo);
if(info && this->isInfoDataFilled())
{
@@ -106,11 +104,11 @@ Transform OdometryF2F::computeTransform(
idToIndex.insert(std::make_pair(iter->first, i));
++i;
}
info->cornerInliers.resize(inliers.size(), 1);
info->cornerInliers.resize(regInfo.inliersIndexes_.size(), 1);
i=0;
for(; i<(int)inliers.size(); ++i)
for(; i<(int)regInfo.inliersIndexes_.size(); ++i)
{
info->cornerInliers[i] = idToIndex.at(inliers[i]);
info->cornerInliers[i] = idToIndex.at(regInfo.inliersIndexes_[i]);
}
}
@@ -127,17 +125,24 @@ Transform OdometryF2F::computeTransform(
motionSinceLastKeyFrame_ *= output;
// new key-frame?
if(keyFrameThr_ <= 0 || (int)inliers.size() <= keyFrameThr_)
if(keyFrameThr_ <= 0 || (int)regInfo.inliers <= keyFrameThr_)
{
UDEBUG("Update key frame");
// only generate features for the first frame
Signature newRefFrame(data);
Signature dummy;
registration_.computeTransformationMod(
newRefFrame,
dummy);
if((int)newRefFrame.getWords().size() >= this->getMinInliers())
int features = -1;
if(registrationPipeline_->isImageRequired())
{
// this will generate features only for the first frame
Signature dummy;
registrationPipeline_->computeTransformationMod(
newRefFrame,
dummy);
features = (int)newRefFrame.getWords().size();
}
if((features < 0 || features >= this->getMinInliers()) &&
(!registrationPipeline_->isScanRequired() || newRefFrame.sensorData().laserScanRaw().cols))
{
refFrame_ = newRefFrame;
@@ -146,23 +151,33 @@ Transform OdometryF2F::computeTransform(
}
else
{
UWARN("Too low 2D corners (%d), keeping last key frame...",
(int)newRefFrame.getWords().size());
if(features >= 0 && features < this->getMinInliers())
{
UWARN("Too low 2D features (%d), keeping last key frame...", features);
}
if(registrationPipeline_->isScanRequired() && newRefFrame.sensorData().laserScanRaw().cols==0)
{
UWARN("Too low scan points (%d), keeping last key frame...", newRefFrame.sensorData().laserScanRaw().cols);
}
}
}
}
else if(!regInfo.rejectedMsg_.empty())
{
UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg_.c_str());
}
if(info)
{
info->type = 1;
info->variance = variance;
info->inliers = (int)inliers.size();
info->variance = regInfo.variance;
info->inliers = regInfo.inliers;
}
UINFO("Odom update time = %fs lost=%s inliers=%d, ref frame corners=%d, transform accepted=%s",
timer.elapsed(),
output.isNull()?"true":"false",
(int)inliers.size(),
(int)regInfo.inliers,
(int)refFrame_.getWords().size(),
!output.isNull()?"true":"false");

216
corelib/src/OdometryICP.cpp
View File

@@ -1,216 +0,0 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/OdometryICP.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/util3d_surface.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
namespace rtabmap {
OdometryICP::OdometryICP(int decimation,
float voxelSize,
int samples,
float maxCorrespondenceDistance,
int maxIterations,
float correspondenceRatio,
bool pointToPlane,
const ParametersMap & odometryParameter) :
Odometry(odometryParameter),
_decimation(decimation),
_voxelSize(voxelSize),
_samples(samples),
_maxCorrespondenceDistance(maxCorrespondenceDistance),
_maxIterations(maxIterations),
_correspondenceRatio(correspondenceRatio),
_pointToPlane(pointToPlane),
_previousCloudNormal(new pcl::PointCloud<pcl::PointNormal>),
_previousCloud(new pcl::PointCloud<pcl::PointXYZ>)
{
}
void OdometryICP::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
_previousCloudNormal.reset(new pcl::PointCloud<pcl::PointNormal>);
_previousCloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
}
// return not null transform if odometry is correctly computed
Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo * info)
{
UTimer timer;
Transform output;
bool hasConverged = false;
double variance = 0;
unsigned int minPoints = 100;
int correspondences = 0;
if(!data.depthOrRightRaw().empty())
{
if(data.depthOrRightRaw().type() == CV_8UC1)
{
UERROR("ICP 3D cannot be done on stereo images!");
return output;
}
if(!(data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()))
{
UERROR("ICP 3D cannot be done without calibration or on multi-camera!");
return output;
}
const CameraModel & cameraModel = data.cameraModels()[0];
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
data.depthOrRightRaw(),
cameraModel.fx(),
cameraModel.fy(),
cameraModel.cx(),
cameraModel.cy(),
_decimation,
this->getMaxDepth(),
_voxelSize,
_samples,
cameraModel.localTransform());
if(_pointToPlane)
{
pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ);
std::vector<int> indices;
newCloud = util3d::removeNaNNormalsFromPointCloud(newCloud);
if(newCloudXYZ->size() != newCloud->size())
{
UWARN("removed nan normals...");
}
if(_previousCloudNormal->size() > minPoints && newCloud->size() > minPoints)
{
pcl::PointCloud<pcl::PointNormal>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointNormal>);
Transform transform = util3d::icpPointToPlane(
newCloud,
_previousCloudNormal,
_maxCorrespondenceDistance,
_maxIterations,
hasConverged,
*newCloudRegistered);
util3d::computeVarianceAndCorrespondences(
newCloudRegistered,
_previousCloudNormal,
_maxCorrespondenceDistance,
variance,
correspondences);
// verify if there are enough correspondences
float correspondencesRatio = float(correspondences)/float(_previousCloudNormal->size()>newCloud->size()?_previousCloudNormal->size():newCloud->size());
if(!transform.isNull() && hasConverged &&
correspondencesRatio >= _correspondenceRatio)
{
output = transform;
_previousCloudNormal = newCloud;
}
else
{
UWARN("Transform not valid (hasConverged=%s variance = %f)",
hasConverged?"true":"false", variance);
}
}
else if(newCloud->size() > minPoints)
{
output.setIdentity();
_previousCloudNormal = newCloud;
}
}
else
{
//point to point
if(_previousCloud->size() > minPoints && newCloudXYZ->size() > minPoints)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>);
Transform transform = util3d::icp(
newCloudXYZ,
_previousCloud,
_maxCorrespondenceDistance,
_maxIterations,
hasConverged,
*newCloudRegistered);
util3d::computeVarianceAndCorrespondences(
newCloudRegistered,
_previousCloud,
_maxCorrespondenceDistance,
variance,
correspondences);
// verify if there are enough correspondences
float correspondencesRatio = float(correspondences)/float(_previousCloud->size()>newCloudXYZ->size()?_previousCloud->size():newCloudXYZ->size());
if(!transform.isNull() && hasConverged &&
correspondencesRatio >= _correspondenceRatio)
{
output = transform;
_previousCloud = newCloudXYZ;
}
else
{
UWARN("Transform not valid (hasConverged=%s variance = %f)",
hasConverged?"true":"false", variance);
}
}
else if(newCloudXYZ->size() > minPoints)
{
output.setIdentity();
_previousCloud = newCloudXYZ;
}
}
}
else
{
UERROR("Depth is empty?!?");
}
if(info)
{
info->variance = variance;
info->inliers = correspondences;
}
UINFO("Odom update time = %fs hasConverged=%s variance=%f cloud=%d",
timer.elapsed(),
hasConverged?"true":"false",
variance,
(int)(_pointToPlane?_previousCloudNormal->size():_previousCloud->size()));
return output;
}
} // namespace rtabmap

6
corelib/src/Parameters.cpp
View File

@@ -158,7 +158,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
removedParameters_.insert(std::make_pair("Odom/RefineIterations", std::make_pair(true, Parameters::kVisRefineIterations())));
removedParameters_.insert(std::make_pair("Odom/MaxDepth", std::make_pair(true, Parameters::kVisMaxDepth())));
removedParameters_.insert(std::make_pair("Odom/RoiRatios", std::make_pair(true, Parameters::kVisRoiRatios())));
removedParameters_.insert(std::make_pair("Odom/Force2D", std::make_pair(true, Parameters::kVisForce2D())));
removedParameters_.insert(std::make_pair("Odom/Force2D", std::make_pair(true, Parameters::kRegForce3DoF())));
removedParameters_.insert(std::make_pair("Odom/VarianceFromInliersCount", std::make_pair(true, Parameters::kRegVarianceFromInliersCount())));
removedParameters_.insert(std::make_pair("Odom/PnPReprojError", std::make_pair(true, Parameters::kVisPnPReprojError())));
removedParameters_.insert(std::make_pair("Odom/PnPFlags", std::make_pair(true, Parameters::kVisPnPFlags())));
@@ -188,13 +188,13 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
removedParameters_.insert(std::make_pair("LccBow/MinInliers", std::make_pair(false, Parameters::kVisMinInliers())));
removedParameters_.insert(std::make_pair("LccBow/Iterations", std::make_pair(false, Parameters::kVisIterations())));
removedParameters_.insert(std::make_pair("LccBow/RefineIterations", std::make_pair(false, Parameters::kVisRefineIterations())));
removedParameters_.insert(std::make_pair("LccBow/Force2D", std::make_pair(false, Parameters::kVisForce2D())));
removedParameters_.insert(std::make_pair("LccBow/Force2D", std::make_pair(false, Parameters::kRegForce3DoF())));
removedParameters_.insert(std::make_pair("LccBow/VarianceFromInliersCount", std::make_pair(false, Parameters::kRegVarianceFromInliersCount())));
removedParameters_.insert(std::make_pair("LccBow/PnPReprojError", std::make_pair(false, Parameters::kVisPnPReprojError())));
removedParameters_.insert(std::make_pair("LccBow/PnPFlags", std::make_pair(false, Parameters::kVisPnPFlags())));
removedParameters_.insert(std::make_pair("LccBow/EpipolarGeometryVar", std::make_pair(true, Parameters::kVisEpipolarGeometryVar())));
removedParameters_.insert(std::make_pair("LccIcp/Type", std::make_pair(true, Parameters::kRGBDLoopClosureLinkRefining())));
removedParameters_.insert(std::make_pair("LccIcp/Type", std::make_pair(false, Parameters::kRegStrategy())));
removedParameters_.insert(std::make_pair("LccIcp3/Decimation", std::make_pair(false, "")));
removedParameters_.insert(std::make_pair("LccIcp3/MaxDepth", std::make_pair(false, "")));

190
corelib/src/Registration.cpp Normal file
View File

@@ -0,0 +1,190 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/RegistrationVis.h>
#include <rtabmap/core/RegistrationIcp.h>
#include <rtabmap/utilite/ULogger.h>
namespace rtabmap {
Registration * Registration::create(const ParametersMap & parameters)
{
int regTypeInt = Parameters::defaultRegStrategy();
Parameters::parse(parameters, Parameters::kRegStrategy(), regTypeInt);
Registration::Type type = (Registration::Type)regTypeInt;
return create(type, parameters);
}
Registration * Registration::create(Registration::Type & type, const ParametersMap & parameters)
{
UDEBUG("type=%d", (int)type);
Registration * reg = 0;
switch(type)
{
case Registration::kTypeIcp:
reg = new RegistrationIcp(parameters);
break;
case Registration::kTypeVisIcp:
reg = new RegistrationVis(parameters, new RegistrationIcp(parameters));
break;
default: // kTypeVis
reg = new RegistrationVis(parameters);
type = Registration::kTypeVis;
break;
}
return reg;
}
Registration::Registration(const ParametersMap & parameters, Registration * child) :
varianceFromInliersCount_(Parameters::defaultRegVarianceFromInliersCount()),
force3DoF_(Parameters::defaultRegForce3DoF()),
child_(child)
{
this->parseParameters(parameters);
}
Registration::~Registration()
{
if(child_)
{
delete child_;
}
}
void Registration::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), varianceFromInliersCount_);
Parameters::parse(parameters, Parameters::kRegForce3DoF(), force3DoF_);
if(child_)
{
child_->parseParameters(parameters);
}
}
bool Registration::isImageRequired() const
{
bool val = isImageRequiredImpl();
if(!val && child_)
{
val = child_->isImageRequired();
}
return val;
}
bool Registration::isScanRequired() const
{
bool val = isScanRequiredImpl();
if(!val && child_)
{
val = child_->isScanRequired();
}
return val;
}
bool Registration::isUserDataRequired() const
{
bool val = isUserDataRequiredImpl();
if(!val && child_)
{
val = child_->isUserDataRequired();
}
return val;
}
void Registration::setChildRegistration(Registration * child)
{
if(child_)
{
delete child_;
}
child_ = child;
}
Transform Registration::computeTransformation(
const Signature & from,
const Signature & to,
Transform guess,
RegistrationInfo * infoOut) const
{
Signature fromCopy(from);
Signature toCopy(to);
return computeTransformationMod(fromCopy, toCopy, guess, infoOut);
}
Transform Registration::computeTransformation(
const SensorData & from,
const SensorData & to,
Transform guess,
RegistrationInfo * infoOut) const
{
Signature fromCopy(from);
Signature toCopy(to);
return computeTransformationMod(fromCopy, toCopy, guess, infoOut);
}
Transform Registration::computeTransformationMod(
Signature & from,
Signature & to,
Transform guess,
RegistrationInfo * infoOut) const
{
RegistrationInfo info;
Transform t = computeTransformationImpl(from, to, guess, info);
if(child_)
{
if(!t.isNull())
{
t = child_->computeTransformationMod(from, to, force3DoF_?t.to3DoF():t, &info);
}
}
else if(!t.isNull() && force3DoF_)
{
t = t.to3DoF();
}
if(varianceFromInliersCount_)
{
if(info.inliersRatio)
{
info.variance = info.inliersRatio > 0?1.0/double(info.inliersRatio):1.0;
}
else
{
info.variance = info.inliers > 0?1.0f/float(info.inliers):1.0f;
}
info.variance = info.variance>0.0f?info.variance:0.0001f; // epsilon if exact transform
}
if(infoOut)
{
*infoOut = info;
}
return t;
}
}

63
corelib/src/RegistrationIcp.cpp
View File

@@ -39,10 +39,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
RegistrationIcp::RegistrationIcp(const ParametersMap & parameters) :
RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration * child) :
Registration(parameters, child),
_maxTranslation(Parameters::defaultIcpMaxTranslation()),
_maxRotation(Parameters::defaultIcpMaxRotation()),
_icp2D(Parameters::defaultIcp2D()),
_voxelSize(Parameters::defaultIcpVoxelSize()),
_downsamplingStep(Parameters::defaultIcpDownsamplingStep()),
_maxCorrespondenceDistance(Parameters::defaultIcpMaxCorrespondenceDistance()),
@@ -60,7 +60,6 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kIcpMaxTranslation(), _maxTranslation);
Parameters::parse(parameters, Parameters::kIcpMaxRotation(), _maxRotation);
Parameters::parse(parameters, Parameters::kIcp2D(), _icp2D);
Parameters::parse(parameters, Parameters::kIcpVoxelSize(), _voxelSize);
Parameters::parse(parameters, Parameters::kIcpDownsamplingStep(), _downsamplingStep);
Parameters::parse(parameters, Parameters::kIcpMaxCorrespondenceDistance(), _maxCorrespondenceDistance);
@@ -77,42 +76,18 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
UASSERT_MSG(_pointToPlaneNormalNeighbors > 0, uFormat("value=%d", _pointToPlaneNormalNeighbors).c_str());
}
Transform RegistrationIcp::computeTransformationMod(
Transform RegistrationIcp::computeTransformationImpl(
Signature & fromSignature,
Signature & toSignature,
Transform guess,
std::string * rejectedMsg,
std::vector<int> * inliersOut,
float * varianceOut,
float * inliersRatioOut) const
{
return computeTransformation(
fromSignature.sensorData(),
toSignature.sensorData(),
guess,
rejectedMsg,
inliersOut,
varianceOut,
inliersRatioOut);
}
Transform RegistrationIcp::computeTransformation(
const SensorData & dataFrom,
const SensorData & dataTo,
Transform guess,
std::string * rejectedMsg,
std::vector<int> * inliersOut,
float * varianceOut,
float * inliersRatioOut) const
RegistrationInfo & info) const
{
UDEBUG("Guess transform = %s", guess.prettyPrint().c_str());
UDEBUG("Voxel size=%f", _voxelSize);
UDEBUG("2D=%d", _icp2D?1:0);
UDEBUG("PointToPlane=%d", _pointToPlane?1:0);
UDEBUG("Normal neighborhood=%d", _pointToPlaneNormalNeighbors);
UDEBUG("Max corrrespondence distance=%f", _maxCorrespondenceDistance);
UDEBUG("Max Iterations=%d", _maxIterations);
UDEBUG("Variance from inliers count=%d", _varianceFromInliersCount?1:0);
UDEBUG("Correspondence Ratio=%f", _correspondenceRatio);
UDEBUG("Max translation=%f", _maxTranslation);
UDEBUG("Max rotation=%f", _maxRotation);
@@ -121,6 +96,9 @@ Transform RegistrationIcp::computeTransformation(
std::string msg;
Transform transform;
SensorData & dataFrom = fromSignature.sensorData();
SensorData & dataTo = toSignature.sensorData();
// ICP with guess transform
if(!dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
{
@@ -157,7 +135,7 @@ Transform RegistrationIcp::computeTransformation(
double variance = 1.0;
bool correspondencesComputed = false;
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
if(!_icp2D) // 3D ICP
if(!force3DoF()) // 3D ICP
{
if(_pointToPlane)
{
@@ -287,23 +265,9 @@ Transform RegistrationIcp::computeTransformation(
maxLaserScans>0?maxLaserScans:dataTo.laserScanMaxPts()?dataTo.laserScanMaxPts():(int)(toCloud->size()>fromCloud->size()?toCloud->size():fromCloud->size()),
correspondencesRatio*100.0f);
if(_varianceFromInliersCount)
{
variance = correspondencesRatio > 0?1.0/double(correspondencesRatio):1.0;
}
if(varianceOut)
{
*varianceOut = variance>0.0f?variance:0.0001; // epsilon if exact transform
}
if(inliersOut)
{
inliersOut->push_back(correspondences);
}
if(inliersRatioOut)
{
*inliersRatioOut = correspondencesRatio;
}
info.variance = variance>0.0f?variance:0.0001; // epsilon if exact transform
info.inliers = correspondences;
info.inliersRatio = correspondencesRatio;
if(correspondencesRatio < _correspondenceRatio)
{
@@ -354,10 +318,7 @@ Transform RegistrationIcp::computeTransformation(
}
if(rejectedMsg)
{
*rejectedMsg = msg;
}
info.rejectedMsg_ = msg;
UDEBUG("New transform = %s", transform.prettyPrint().c_str());
return transform;

94
corelib/src/RegistrationVis.cpp
View File

@@ -41,12 +41,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
RegistrationVis::RegistrationVis(const ParametersMap & parameters) :
RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration * child) :
Registration(parameters, child),
_minInliers(Parameters::defaultVisMinInliers()),
_inlierDistance(Parameters::defaultVisInlierDistance()),
_iterations(Parameters::defaultVisIterations()),
_refineIterations(Parameters::defaultVisRefineIterations()),
_force2D(Parameters::defaultVisForce2D()),
_epipolarGeometryVar(Parameters::defaultVisEpipolarGeometryVar()),
_estimationType(Parameters::defaultVisEstimationType()),
_forwardEstimateOnly(Parameters::defaultVisForwardEstOnly()),
@@ -82,7 +82,6 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVisInlierDistance(), _inlierDistance);
Parameters::parse(parameters, Parameters::kVisIterations(), _iterations);
Parameters::parse(parameters, Parameters::kVisRefineIterations(), _refineIterations);
Parameters::parse(parameters, Parameters::kVisForce2D(), _force2D);
Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);
Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _epipolarGeometryVar);
Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
@@ -154,19 +153,15 @@ RegistrationVis::~RegistrationVis()
{
}
Transform RegistrationVis::computeTransformationMod(
Transform RegistrationVis::computeTransformationImpl(
Signature & fromSignature,
Signature & toSignature,
Transform guess, // guess is only used by Optical Flow correspondences (flowMaxLevel is set to 0 when guess is used)
std::string * rejectedMsg,
std::vector<int> * inliersOut,
float * varianceOut,
float * inliersRatioOut) const
RegistrationInfo & info) const
{
UDEBUG("%s=%d", Parameters::kVisMinInliers().c_str(), _minInliers);
UDEBUG("%s=%f", Parameters::kVisInlierDistance().c_str(), _inlierDistance);
UDEBUG("%s=%d", Parameters::kVisIterations().c_str(), _iterations);
UDEBUG("%s=%d", Parameters::kVisForce2D().c_str(), _force2D?1:0);
UDEBUG("%s=%d", Parameters::kVisEstimationType().c_str(), _estimationType);
UDEBUG("%s=%d", Parameters::kVisForwardEstOnly().c_str(), _forwardEstimateOnly);
UDEBUG("%s=%f", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar);
@@ -337,7 +332,7 @@ Transform RegistrationVis::computeTransformationMod(
kptsFrom3DKept.resize(ki);
std::vector<cv::Point3f> kptsTo3D;
if(_estimationType == 0 || (_estimationType == 1 && !_varianceFromInliersCount) || !_forwardEstimateOnly)
if(_estimationType == 0 || (_estimationType == 1 && !varianceFromInliersCount()) || !_forwardEstimateOnly)
{
kptsTo3D = detector->generateKeypoints3D(toSignature.sensorData(), kptsTo);
}
@@ -433,7 +428,7 @@ Transform RegistrationVis::computeTransformationMod(
{
kptsFrom3D = uValues(fromSignature.getWords3());
}
if((_estimationType == 0 || (_estimationType == 1 && !_varianceFromInliersCount) || !_forwardEstimateOnly) &&
if((_estimationType == 0 || (_estimationType == 1 && !varianceFromInliersCount()) || !_forwardEstimateOnly) &&
toSignature.getWords3().empty() &&
!toSignature.sensorData().imageRaw().empty())
{
@@ -503,6 +498,7 @@ Transform RegistrationVis::computeTransformationMod(
/////////////////////
Transform transform;
float variance = 1.0f;
int inliersCount = 0;
if(toSignature.getWords().size() || !toSignature.sensorData().imageRaw().empty())
{
Transform transforms[2];
@@ -629,7 +625,7 @@ Transform RegistrationVis::computeTransformationMod(
_PnPRefineIterations,
dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
uMultimapToMapUnique(signatureA->getWords3()),
_varianceFromInliersCount?0:&variances[dir],
varianceFromInliersCount()?0:&variances[dir],
0,
&inliersV);
inliers[dir] = inliersV;
@@ -696,68 +692,27 @@ Transform RegistrationVis::computeTransformationMod(
if(transforms[0].isNull())
{
transform = transforms[1];
if(inliersOut)
{
*inliersOut = inliers[1];
}
info.inliersIndexes_ = inliers[1];
variance = variances[1];
if(_varianceFromInliersCount)
{
variance = inliers[1].size() > 0?1.0f/float(inliers[1].size()):1.0f;
}
inliersCount = (int)inliers[1].size();
}
else
{
/*if(!guess.isNull())
{
// use the transform nearest of the guess
int index = 0;
if(transforms[0].getDistance(guess) > transforms[1].getDistance(guess))
{
index = 1;
}
transform = transforms[index];
if(inliersOut)
{
*inliersOut = inliers[index];
}
transform = transforms[0].interpolate(0.5f, transforms[1]);
info.inliersIndexes_ = inliers[0];
variance = variances[index];
if(_varianceFromInliersCount)
{
variance = inliers[index].size() > 0?1.0f/float(inliers[index].size()):1.0f;
}
}
else*/
{
transform = transforms[0].interpolate(0.5f, transforms[1]);
if(inliersOut)
{
*inliersOut = inliers[0];
}
variance = (variances[0]+variances[1])/2.0f;
if(_varianceFromInliersCount)
{
int avg = (inliers[0].size()+inliers[1].size())/2;
variance = avg>0?1.0f/float(avg):1.0f;
}
}
variance = (variances[0]+variances[1])/2.0f;
inliersCount = (int)(inliers[0].size()+inliers[1].size())/2;
}
}
else
{
transform = transforms[0];
if(inliersOut)
{
*inliersOut = inliers[0];
}
info.inliersIndexes_ = inliers[0];
variance = variances[0];
if(_varianceFromInliersCount)
{
variance = inliers[0].size() > 0?1.0f/float(inliers[0].size()):1.0f;
}
inliersCount = (int)inliers[0].size();
}
}
@@ -776,21 +731,12 @@ Transform RegistrationVis::computeTransformationMod(
roll, pitch, yaw);
UWARN(msg.c_str());
}
else if(_force2D)
{
UDEBUG("Forcing 2D...");
transform = Transform(x,y,0, 0, 0, yaw);
}
}
if(rejectedMsg)
{
*rejectedMsg = msg;
}
if(varianceOut)
{
*varianceOut = variance>0.0f?variance:0.0001; // epsilon if exact transform
}
info.inliers = inliersCount;
info.rejectedMsg_ = msg;
info.variance = variance>0.0f?variance:0.0001f; // epsilon if exact transform
UDEBUG("transform=%s", transform.prettyPrint().c_str());
return transform;
}

75
corelib/src/Rtabmap.cpp
View File

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/BayesFilter.h"
#include "rtabmap/core/Compression.h"
#include "rtabmap/core/RegistrationInfo.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UFile.h>
@@ -89,7 +90,6 @@ Rtabmap::Rtabmap() :
_rgbdLinearUpdate(Parameters::defaultRGBDLinearUpdate()),
_rgbdAngularUpdate(Parameters::defaultRGBDAngularUpdate()),
_newMapOdomChangeDistance(Parameters::defaultRGBDNewMapOdomChangeDistance()),
_loopClosureRefining(Parameters::defaultRGBDLoopClosureLinkRefining()),
_neighborLinkRefining(Parameters::defaultRGBDNeighborLinkRefining()),
_proximityByTime(Parameters::defaultRGBDProximityByTime()),
_proximityBySpace(Parameters::defaultRGBDProximityBySpace()),
@@ -411,7 +411,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDPlanStuckIterations(), _pathStuckIterations);
Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity);
Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity);
Parameters::parse(parameters, Parameters::kRGBDLoopClosureLinkRefining(), _loopClosureRefining);
UASSERT(_rgbdLinearUpdate >= 0.0f);
UASSERT(_rgbdAngularUpdate >= 0.0f);
@@ -1009,21 +1008,18 @@ bool Rtabmap::process(
{
UINFO("Odometry correction by scan matching");
Transform guess = signature->getLinks().begin()->second.transform().inverse();
float variance = 1.0f;
int inliers = 0;
float inliersRatio = 0;
std::string rejectedMsg;
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, &rejectedMsg, &inliers, &variance, &inliersRatio);
RegistrationInfo info;
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, &info);
if(!t.isNull())
{
UINFO("Scan matching: update neighbor link (%d->%d, variance=%f) from %s to %s",
signature->id(),
oldId,
variance,
info.variance,
signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
t.prettyPrint().c_str());
UASSERT(variance > 0.0);
_memory->updateLink(oldId, signature->id(), t, variance, variance);
UASSERT(info.variance > 0.0);
_memory->updateLink(oldId, signature->id(), t, info.variance, info.variance);
if(_optimizeFromGraphEnd)
{
@@ -1043,17 +1039,17 @@ bool Rtabmap::process(
}
else
{
UINFO("Scan matching rejected: %s", rejectedMsg.c_str());
if(variance > 0)
UINFO("Scan matching rejected: %s", info.rejectedMsg_.c_str());
if(info.variance > 0)
{
double sqrtVar = sqrt(variance);
double sqrtVar = sqrt(info.variance);
_memory->updateLink(signature->id(), oldId, guess, sqrtVar, sqrtVar);
}
}
statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), inliers);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers_ratio(), inliersRatio);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningVariance(), variance);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), info.inliers);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers_ratio(), info.inliersRatio);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningVariance(), info.variance);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningPts(), signature->sensorData().laserScanRaw().cols);
}
}
@@ -1161,13 +1157,9 @@ bool Rtabmap::process(
{
std::string rejectedMsg;
UDEBUG("Check local transform between %d and %d", signature->id(), *iter);
float variance = 1.0f;
int inliers = -1;
Transform transform = _memory->computeVisualTransform(signature->id(), *iter, &rejectedMsg, &inliers, &variance);
if(!transform.isNull() && _loopClosureRefining)
{
transform = _memory->computeIcpTransform(signature->id(), *iter, transform, &rejectedMsg, 0, &variance);
}
RegistrationInfo info;
Transform transform = _memory->computeTransform(signature->id(), *iter, &info);
if(!transform.isNull())
{
UDEBUG("Add local loop closure in TIME (%d->%d) %s",
@@ -1175,8 +1167,8 @@ bool Rtabmap::process(
*iter,
transform.prettyPrint().c_str());
// Add a loop constraint
UASSERT(variance > 0.0);
if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, variance, variance)))
UASSERT(info.variance > 0.0);
if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, info.variance, info.variance)))
{
++localLoopClosuresInTimeFound;
UINFO("Local loop closure found between %d and %d with t=%s",
@@ -1718,19 +1710,14 @@ bool Rtabmap::process(
float variance = 1.0f;
if(_rgbdSlamMode)
{
std::string rejectedMsg;
transform = _memory->computeVisualTransform(signature->id(), _loopClosureHypothesis.first, &rejectedMsg, &loopClosureVisualInliers, &variance);
if(!transform.isNull() && _loopClosureRefining)
{
transform = _memory->computeIcpTransform(signature->id(), _loopClosureHypothesis.first, transform, &rejectedMsg, 0, &variance);
}
RegistrationInfo info;
transform = _memory->computeTransform(signature->id(), _loopClosureHypothesis.first, &info);
loopClosureVisualInliers = info.inliers;
rejectedHypothesis = transform.isNull();
if(rejectedHypothesis)
{
UWARN("Rejected loop closure %d -> %d: %s",
_loopClosureHypothesis.first, signature->id(), rejectedMsg.c_str());
_loopClosureHypothesis.first, signature->id(), info.rejectedMsg_.c_str());
}
}
if(!rejectedHypothesis)
@@ -1822,12 +1809,8 @@ bool Rtabmap::process(
(_proximityFilteringRadius <= 0.0f ||
_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius))
{
float variance = 1.0f;
Transform transform = _memory->computeVisualTransform(signature->id(), nearestId, 0, 0, &variance);
if(!transform.isNull() && _loopClosureRefining)
{
transform = _memory->computeIcpTransform(signature->id(), nearestId, transform, 0, 0, &variance);
}
RegistrationInfo info;
Transform transform = _memory->computeTransform(signature->id(), nearestId, &info);
if(!transform.isNull())
{
if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)
@@ -1836,8 +1819,8 @@ bool Rtabmap::process(
signature->id(),
nearestId,
transform.prettyPrint().c_str());
UASSERT(variance > 0.0);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, variance, variance));
UASSERT(info.variance > 0.0);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, info.variance, info.variance));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
if(_loopClosureHypothesis.first == 0)
@@ -1927,8 +1910,8 @@ bool Rtabmap::process(
//The nearest will be the reference for a loop closure transform
if(signature->getLinks().find(nearestId) == signature->getLinks().end())
{
float variance = 1.0f;
Transform transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, path, 0, 0, &variance);
RegistrationInfo info;
Transform transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, path, &info);
if(!transform.isNull())
{
if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)
@@ -1960,8 +1943,8 @@ bool Rtabmap::process(
}
// set Identify covariance for laser scan matching only
UASSERT(variance>0.0);
double sqrtVar = sqrt(variance);
UASSERT(info.variance>0.0);
double sqrtVar = sqrt(info.variance);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, sqrtVar, sqrtVar, scanMatchingIds));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));

7
corelib/src/Transform.cpp
View File

@@ -173,6 +173,13 @@ Transform Transform::translation() const
0,0,1, data()[11]);
}
Transform Transform::to3DoF() const
{
float x,y,z,roll,pitch,yaw;
this->getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
return Transform(x,y,0, 0,0,yaw);
}
void Transform::getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const
{
pcl::getTranslationAndEulerAngles(toEigen3f(), x, y, z, roll, pitch, yaw);