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

RegistrationIcp: Added laserScanToDP()

Browse Source
This commit is contained in:
matlabbe
2018-02-17 07:48:10 -05:00
parent edbed67afe
commit 077b3ab59e
2 changed files with 146 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files

126
corelib/src/RegistrationIcp.cpp
View File

@@ -148,6 +148,116 @@ DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud, bool is2D)
return cloud; return cloud;
} }
DP laserScanToDP(const rtabmap::LaserScan & scan)
{
UDEBUG("");
typedef DP::Label Label;
typedef DP::Labels Labels;
typedef DP::View View;
if (scan.isEmpty())
return DP();
// fill labels
// conversions of descriptor fields from pcl
// see http://www.ros.org/wiki/pcl/Overview
Labels featLabels;
Labels descLabels;
featLabels.push_back(Label("x", 1));
featLabels.push_back(Label("y", 1));
if(!scan.is2d())
{
featLabels.push_back(Label("z", 1));
}
featLabels.push_back(Label("pad", 1));
if(scan.hasNormals())
{
descLabels.push_back(Label("normals", 3));
}
if(scan.hasIntensity())
{
descLabels.push_back(Label("intensity", 1));
}
// create cloud
DP cloud(featLabels, descLabels, scan.size());
cloud.getFeatureViewByName("pad").setConstant(1);
// fill cloud
int nx = scan.getNormalsOffset();
int ny = nx+1;
int nz = ny+1;
int offsetI = scan.getIntensityOffset();
bool hasLocalTransform = !scan.localTransform().isNull() && !scan.localTransform().isIdentity();
View view(cloud.getFeatureViewByName("x"));
View viewNormalX(nx!=-1?cloud.getDescriptorRowViewByName("normals",0):view);
View viewNormalY(nx!=-1?cloud.getDescriptorRowViewByName("normals",1):view);
View viewNormalZ(nx!=-1?cloud.getDescriptorRowViewByName("normals",2):view);
View viewIntensity(offsetI!=-1?cloud.getDescriptorRowViewByName("intensity",0):view);
for(unsigned int i=0; i<scan.size(); ++i)
{
const float * ptr = scan.data().ptr<float>(0, i);
if(hasLocalTransform)
{
if(nx == -1)
{
cv::Point3f pt(ptr[0], ptr[1], scan.is2d()?0:ptr[2]);
pt = rtabmap::util3d::transformPoint(pt, scan.localTransform());
view(0, i) = pt.x;
view(1, i) = pt.y;
if(!scan.is2d())
{
view(2, i) = pt.z;
}
}
else if(uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz]))
{
pcl::PointNormal pt;
pt.x=ptr[0];
pt.y=ptr[1];
pt.z=scan.is2d()?0:ptr[2];
pt.normal_x=ptr[nx];
pt.normal_y=ptr[ny];
pt.normal_z=ptr[nz];
pt = rtabmap::util3d::transformPoint(pt, scan.localTransform());
view(0, i) = pt.x;
view(1, i) = pt.y;
if(!scan.is2d())
{
view(2, i) = pt.z;
}
viewNormalX(0, i) = pt.normal_x;
viewNormalY(0, i) = pt.normal_y;
viewNormalZ(0, i) = pt.normal_z;
}
}
else if(nx!=-1 || (uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz])))
{
view(0, i) = ptr[0];
view(1, i) = ptr[1];
if(!scan.is2d())
{
view(2, i) = ptr[2];
}
if(nx!=-1)
{
viewNormalX(0, i) = ptr[nx];
viewNormalY(0, i) = ptr[ny];
viewNormalZ(0, i) = ptr[nz];
}
}
if(offsetI!=-1)
{
viewIntensity(0, i) = ptr[offsetI];
}
}
return cloud;
}
void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointXYZ> & pclCloud) void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointXYZ> & pclCloud)
{ {
UDEBUG(""); UDEBUG("");
@@ -469,8 +579,8 @@ Transform RegistrationIcp::computeTransformationImpl(
if(_libpointmatcher) if(_libpointmatcher)
{ {
// Load point clouds // Load point clouds
DP data = pclToDP(fromCloudNormals, fromScan.is2d()); DP data = laserScanToDP(fromScan);
DP ref = pclToDP(toCloudNormals, toScan.is2d()); DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.maxRange(), toScan.format(), guess * toScan.localTransform()));
// Compute the transformation to express data in ref // Compute the transformation to express data in ref
PM::TransformationParameters T; PM::TransformationParameters T;
@@ -681,6 +791,8 @@ Transform RegistrationIcp::computeTransformationImpl(
toScan.localTransform())); toScan.localTransform()));
} }
UDEBUG("Compute normals (%d,%d) time = %f s", (int)fromCloudNormals->size(), (int)toCloudNormals->size(), timer.ticks()); UDEBUG("Compute normals (%d,%d) time = %f s", (int)fromCloudNormals->size(), (int)toCloudNormals->size(), timer.ticks());
fromScan = fromSignature.sensorData().laserScanRaw();
toScan = toSignature.sensorData().laserScanRaw();
if(toCloudNormals->size() && fromCloudNormals->size()) if(toCloudNormals->size() && fromCloudNormals->size())
{ {
@@ -690,8 +802,8 @@ Transform RegistrationIcp::computeTransformationImpl(
if(_libpointmatcher) if(_libpointmatcher)
{ {
// Load point clouds // Load point clouds
DP data = pclToDP(fromCloudNormals, fromScan.is2d()); DP data = laserScanToDP(fromScan);
DP ref = pclToDP(toCloudNormals, toScan.is2d()); DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.maxRange(), toScan.format(), guess*toScan.localTransform()));
// Compute the transformation to express data in ref // Compute the transformation to express data in ref
PM::TransformationParameters T; PM::TransformationParameters T;
@@ -797,14 +909,16 @@ Transform RegistrationIcp::computeTransformationImpl(
LaserScan::kXYZ, LaserScan::kXYZ,
toScan.localTransform())); toScan.localTransform()));
} }
fromScan = fromSignature.sensorData().laserScanRaw();
toScan = toSignature.sensorData().laserScanRaw();
} }
#ifdef RTABMAP_POINTMATCHER #ifdef RTABMAP_POINTMATCHER
if(_libpointmatcher) if(_libpointmatcher)
{ {
// Load point clouds // Load point clouds
DP data = pclToDP(fromCloudFiltered, fromScan.is2d()); DP data = laserScanToDP(fromScan);
DP ref = pclToDP(toCloudFiltered, toScan.is2d()); DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.maxRange(), toScan.format(), guess*toScan.localTransform()));
// Compute the transformation to express data in ref // Compute the transformation to express data in ref
PM::TransformationParameters T; PM::TransformationParameters T;

88
corelib/src/util3d_transforms.cpp
View File

@@ -43,79 +43,43 @@ LaserScan transformLaserScan(const LaserScan & laserScan, const Transform & tran
if(!transform.isNull() && !transform.isIdentity()) if(!transform.isNull() && !transform.isIdentity())
{ {
Eigen::Affine3f transform3f = transform.toEigen3f(); Eigen::Affine3f transform3f = transform.toEigen3f();
int nx = laserScan.getNormalsOffset();
int ny = nx+1;
int nz = ny+1;
for(int i=0; i<laserScan.size(); ++i) for(int i=0; i<laserScan.size(); ++i)
{ {
const float * ptr = laserScan.data().ptr<float>(0, i); const float * ptr = laserScan.data().ptr<float>(0, i);
float * out = output.ptr<float>(0, i); float * out = output.ptr<float>(0, i);
if(laserScan.format() == LaserScan::kXY || laserScan.format() == LaserScan::kXYI) if(nx == -1)
{ {
pcl::PointXYZ pt(ptr[0], ptr[1], 0); pcl::PointXYZ pt(ptr[0], ptr[1], laserScan.is2d()?0:ptr[2]);
pt = pcl::transformPoint(pt, transform3f); pt = pcl::transformPoint(pt, transform3f);
out[0] = pt.x; out[0] = pt.x;
out[1] = pt.y; out[1] = pt.y;
} if(!laserScan.is2d())
else if(laserScan.format() == LaserScan::kXYZ || laserScan.format() == LaserScan::kXYZI || laserScan.format() == LaserScan::kXYZRGB) {
{ out[2] = pt.z;
pcl::PointXYZ pt(ptr[0], ptr[1], ptr[2]); }
pt = pcl::transformPoint(pt, transform3f);
out[0] = pt.x;
out[1] = pt.y;
out[2] = pt.z;
}
else if(laserScan.format() == LaserScan::kXYNormal || laserScan.format() == LaserScan::kXYINormal)
{
int nOffset = laserScan.format() == LaserScan::kXYINormal?1:0;
pcl::PointNormal pt;
pt.x=ptr[0];
pt.y=ptr[1];
pt.z=0;
pt.normal_x=ptr[nOffset+2];
pt.normal_y=ptr[nOffset+3];
pt.normal_z=ptr[nOffset+4];
pt = util3d::transformPoint(pt, transform);
out[0] = pt.x;
out[1] = pt.y;
out[nOffset+2] = pt.normal_x;
out[nOffset+3] = pt.normal_y;
out[nOffset+4] = pt.normal_z;
}
else if(laserScan.format() == LaserScan::kXYZNormal)
{
pcl::PointNormal pt;
pt.x=ptr[0];
pt.y=ptr[1];
pt.z=ptr[2];
pt.normal_x=ptr[3];
pt.normal_y=ptr[4];
pt.normal_z=ptr[5];
pt = util3d::transformPoint(pt, transform);
out[0] = pt.x;
out[1] = pt.y;
out[2] = pt.z;
out[3] = pt.normal_x;
out[4] = pt.normal_y;
out[5] = pt.normal_z;
}
else if(laserScan.format() == LaserScan::kXYZINormal || laserScan.format() == LaserScan::kXYZRGBNormal)
{
pcl::PointNormal pt;
pt.x=ptr[0];
pt.y=ptr[1];
pt.z=ptr[2];
pt.normal_x=ptr[4];
pt.normal_y=ptr[5];
pt.normal_z=ptr[6];
pt = util3d::transformPoint(pt, transform);
out[0] = pt.x;
out[1] = pt.y;
out[2] = pt.z;
out[4] = pt.normal_x;
out[5] = pt.normal_y;
out[6] = pt.normal_z;
} }
else else
{ {
UERROR("Unknown laser scan format! (%d)", laserScan.format()); pcl::PointNormal pt;
pt.x=ptr[0];
pt.y=ptr[1];
pt.z=laserScan.is2d()?0:ptr[2];
pt.normal_x=ptr[nx];
pt.normal_y=ptr[ny];
pt.normal_z=ptr[nz];
pt = util3d::transformPoint(pt, transform);
out[0] = pt.x;
out[1] = pt.y;
if(!laserScan.is2d())
{
out[2] = pt.z;
}
out[nx] = pt.normal_x;
out[ny] = pt.normal_y;
out[nz] = pt.normal_z;
} }
} }
} }