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

OdometryICP: added p2p (point to point) option

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1631 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-08-05 00:35:21 +00:00
parent a0da05a352
commit 52af62394e
3 changed files with 88 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -99,6 +99,7 @@ public:
float maxCorrespondenceDistance = 0.05f,
int maxIterations = 30,
float maxFitness = 0.01f,
bool pointToPlane = true,
const ParametersMap & odometryParameter = rtabmap::ParametersMap());
void reset();
@@ -112,8 +113,10 @@ private:
float _maxCorrespondenceDistance;
int _maxIterations;
float _maxFitness;
bool _pointToPlane;
pcl::PointCloud<pcl::PointNormal>::Ptr _previousCloud;
pcl::PointCloud<pcl::PointNormal>::Ptr _previousCloudNormal; // for point ot plane
pcl::PointCloud<pcl::PointXYZ>::Ptr _previousCloud; // for point to point
};
// return true if odometry is correctly computed

109
corelib/src/Odometry.cpp
View File

@@ -355,6 +355,7 @@ OdometryICP::OdometryICP(int decimation,
float maxCorrespondenceDistance,
int maxIterations,
float maxFitness,
bool pointToPlane,
const ParametersMap & odometryParameter) :
Odometry(odometryParameter),
_decimation(decimation),
@@ -363,14 +364,17 @@ OdometryICP::OdometryICP(int decimation,
_maxCorrespondenceDistance(maxCorrespondenceDistance),
_maxIterations(maxIterations),
_maxFitness(maxFitness),
_previousCloud(new pcl::PointCloud<pcl::PointNormal>)
_pointToPlane(pointToPlane),
_previousCloudNormal(new pcl::PointCloud<pcl::PointNormal>),
_previousCloud(new pcl::PointCloud<pcl::PointXYZ>)
{
}
void OdometryICP::reset()
{
Odometry::reset();
_previousCloud.reset(new pcl::PointCloud<pcl::PointNormal>);
_previousCloudNormal.reset(new pcl::PointCloud<pcl::PointNormal>);
_previousCloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
}
// return not null transform if odometry is correctly computed
@@ -396,44 +400,81 @@ Transform OdometryICP::computeTransform(Image & image, int * quality)
_samples,
image.localTransform());
pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ);
std::vector<int> indices;
newCloud = util3d::removeNaNNormalsFromPointCloud(newCloud);
if(newCloudXYZ->size() != newCloud->size())
if(_pointToPlane)
{
UWARN("removed nan normals...");
}
pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ);
if(_previousCloud->size() > minPoints && newCloud->size() > minPoints)
{
Transform transform = util3d::icpPointToPlane(newCloud,
_previousCloud,
_maxCorrespondenceDistance,
_maxIterations,
hasConverged,
fitness);
//pcl::io::savePCDFile("old.pcd", *_previousCloud);
//pcl::io::savePCDFile("new.pcd", *newCloud);
//pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudTransformed = util3d::transformPointCloud(newCloud, transform);
//pcl::io::savePCDFile("newicp.pcd", *newCloudTransformed);
if(hasConverged && (_maxFitness == 0 || fitness < _maxFitness))
std::vector<int> indices;
newCloud = util3d::removeNaNNormalsFromPointCloud(newCloud);
if(newCloudXYZ->size() != newCloud->size())
{
output = transform;
_previousCloud = newCloud;
UWARN("removed nan normals...");
}
else
if(_previousCloudNormal->size() > minPoints && newCloud->size() > minPoints)
{
UWARN("Transform not valid (hasConverged=%s fitness = %f < %f)",
hasConverged?"true":"false", fitness, _maxFitness);
Transform transform = util3d::icpPointToPlane(newCloud,
_previousCloudNormal,
_maxCorrespondenceDistance,
_maxIterations,
hasConverged,
fitness);
//pcl::io::savePCDFile("old.pcd", *_previousCloud);
//pcl::io::savePCDFile("new.pcd", *newCloud);
//pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudTransformed = util3d::transformPointCloud(newCloud, transform);
//pcl::io::savePCDFile("newicp.pcd", *newCloudTransformed);
if(hasConverged && (_maxFitness == 0 || fitness < _maxFitness))
{
output = transform;
_previousCloudNormal = newCloud;
}
else
{
UWARN("Transform not valid (hasConverged=%s fitness = %f < %f)",
hasConverged?"true":"false", fitness, _maxFitness);
}
}
else if(newCloud->size() > minPoints)
{
output.setIdentity();
_previousCloudNormal = newCloud;
}
}
else if(newCloud->size() > minPoints)
else
{
output.setIdentity();
_previousCloud = newCloud;
//point to point
if(_previousCloud->size() > minPoints && newCloudXYZ->size() > minPoints)
{
Transform transform = util3d::icp(newCloudXYZ,
_previousCloud,
_maxCorrespondenceDistance,
_maxIterations,
hasConverged,
fitness);
//pcl::io::savePCDFile("old.pcd", *_previousCloudNormal);
//pcl::io::savePCDFile("new.pcd", *newCloud);
//pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudTransformed = util3d::transformPointCloud(newCloud, transform);
//pcl::io::savePCDFile("newicp.pcd", *newCloudTransformed);
if(hasConverged && (_maxFitness == 0 || fitness < _maxFitness))
{
output = transform;
_previousCloud = newCloudXYZ;
}
else
{
UWARN("Transform not valid (hasConverged=%s fitness = %f < %f)",
hasConverged?"true":"false", fitness, _maxFitness);
}
}
else if(newCloudXYZ->size() > minPoints)
{
output.setIdentity();
_previousCloud = newCloudXYZ;
}
}
}
else
@@ -445,7 +486,7 @@ Transform OdometryICP::computeTransform(Image & image, int * quality)
timer.elapsed(),
hasConverged?"true":"false",
fitness,
(int)_previousCloud->size());
(int)(_pointToPlane?_previousCloudNormal->size():_previousCloud->size()));
return output;
}
@@ -511,7 +552,7 @@ void OdometryThread::mainLoop()
getImage(image);
if(!image.empty())
{
int quality = 0;
int quality = -1;
Transform pose = _odometry->process(image, &quality);
image.setPose(pose); // a null pose notify that odometry could not be computed
this->post(new OdometryEvent(image, quality));

10
tools/OdometryViewer/main.cpp
View File

@@ -45,6 +45,7 @@ void showUsage()
" -v # ICP voxel size (default 0.005)\n"
" -s # ICP samples (default 0, not used if voxel is set.)\n"
" -f #.# ICP fitness (default 0.01)\n"
" -p2p ICP point to point (default point to plane)"
"\n"
" -debug Log debug messages\n"
"\n"
@@ -88,6 +89,7 @@ int main (int argc, char * argv[])
float sec = 0.0f;
bool gpu = false;
int localHistory = 0;
bool p2p = false;
for(int i=1; i<argc; ++i)
{
@@ -493,6 +495,11 @@ int main (int argc, char * argv[])
icp = true;
continue;
}
if(strcmp(argv[i], "-p2p") == 0)
{
p2p = true;
continue;
}
if(strcmp(argv[i], "-debug") == 0)
{
ULogger::setLevel(ULogger::kDebug);
@@ -642,8 +649,9 @@ int main (int argc, char * argv[])
UINFO("Cloud voxel size = %f", voxel);
UINFO("Cloud samples = %d", samples);
UINFO("Cloud fitness = %f", fitness);
UINFO("Cloud point to plane = %s", p2p?"false":"true");
odom = new rtabmap::OdometryICP(decimation, voxel, samples, distance, iterations, fitness);
odom = new rtabmap::OdometryICP(decimation, voxel, samples, distance, iterations, fitness, !p2p);
}
rtabmap::OdometryThread odomThread(odom);
rtabmap::OdometryViewer odomViewer(maxClouds, 2, 0.0, 50);