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

New 2d scan type: CV_32FC5 (x,y,normal_x,normal_y,normal_z)

Browse Source
This commit is contained in:
matlabbe
2017-08-27 19:58:49 -04:00
parent 52a4e8964f
commit 423b47a5ff
12 changed files with 292 additions and 206 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
corelib/src/Memory.cpp
View File

@@ -2438,6 +2438,7 @@ Transform Memory::computeIcpTransformMulti(
std::string msg;
int maxPoints = fromScan.cols;
pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
bool is2D = true;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(iter->first != fromId)
@@ -2447,14 +2448,21 @@ Transform Memory::computeIcpTransformMulti(
{
cv::Mat scan;
s->sensorData().uncompressData(0, 0, &scan);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(
scan,
s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second);
if(scan.cols > maxPoints)
if(!scan.empty())
{
maxPoints = scan.cols;
if(scan.channels() != 2 && scan.channels() != 5)
{
is2D = false;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(
scan,
s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second);
if(scan.cols > maxPoints)
{
maxPoints = scan.cols;
}
*assembledToClouds += *cloud;
}
*assembledToClouds += *cloud;
}
else
{
@@ -2464,12 +2472,25 @@ Transform Memory::computeIcpTransformMulti(
}
if(assembledToClouds->size())
{
assembledData.setLaserScanRaw(
util3d::laserScanFromPointCloud(*assembledToClouds),
LaserScanInfo(
fromS->sensorData().laserScanInfo().maxPoints()?fromS->sensorData().laserScanInfo().maxPoints():maxPoints,
fromS->sensorData().laserScanInfo().maxRange(),
Transform::getIdentity())); // scans are in base frame
if(is2D)
{
assembledData.setLaserScanRaw(
util3d::laserScan2dFromPointCloud(*assembledToClouds),
LaserScanInfo(
fromS->sensorData().laserScanInfo().maxPoints()?fromS->sensorData().laserScanInfo().maxPoints():maxPoints,
fromS->sensorData().laserScanInfo().maxRange(),
Transform::getIdentity())); // scans are in base frame
}
else
{
assembledData.setLaserScanRaw(
util3d::laserScanFromPointCloud(*assembledToClouds),
LaserScanInfo(
fromS->sensorData().laserScanInfo().maxPoints()?fromS->sensorData().laserScanInfo().maxPoints():maxPoints,
fromS->sensorData().laserScanInfo().maxRange(),
Transform::getIdentity())); // scans are in base frame
}
}
Transform guess = poses.at(fromId).inverse() * poses.at(toId);

2
corelib/src/OccupancyGrid.cpp
View File

@@ -207,7 +207,7 @@ void OccupancyGrid::createLocalMap(
UDEBUG("scan channels=%d, occupancyFromCloud_=%d normalsSegmentation_=%d grid3D_=%d",
node.sensorData().laserScanRaw().empty()?0:node.sensorData().laserScanRaw().channels(), occupancyFromCloud_?1:0, normalsSegmentation_?1:0, grid3D_?1:0);
if(node.sensorData().laserScanRaw().channels() == 2 && !occupancyFromCloud_)
if((node.sensorData().laserScanRaw().channels() == 2 || node.sensorData().laserScanRaw().channels() == 5) && !occupancyFromCloud_)
{
UDEBUG("2D laser scan");
//2D

126
corelib/src/RegistrationIcp.cpp
View File

@@ -39,11 +39,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/conversions.h>
#ifdef RTABMAP_POINTMATCHER
#include <fstream>
#include "pointmatcher/PointMatcher.h"
typedef PointMatcher<float> PM;
typedef PM::DataPoints DP;
DP pclToDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud)
DP pclToDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud, bool is2D)
{
UDEBUG("");
typedef DP::Label Label;
@@ -63,8 +64,11 @@ DP pclToDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud)
isFeature.push_back(true);
featLabels.push_back(Label("y", 1));
isFeature.push_back(true);
featLabels.push_back(Label("z", 1));
isFeature.push_back(true);
if(!is2D)
{
featLabels.push_back(Label("z", 1));
isFeature.push_back(true);
}
featLabels.push_back(Label("pad", 1));
// create cloud
@@ -72,20 +76,21 @@ DP pclToDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud)
cloud.getFeatureViewByName("pad").setConstant(1);
// fill cloud
View viewX(cloud.getFeatureViewByName("x"));
View viewY(cloud.getFeatureViewByName("y"));
View viewZ(cloud.getFeatureViewByName("z"));
View view(cloud.getFeatureViewByName("x"));
for(unsigned int i=0; i<pclCloud->size(); ++i)
{
viewX(0, i) = pclCloud->at(i).x;
viewY(0, i) = pclCloud->at(i).y;
viewZ(0, i) = pclCloud->at(i).z;
view(0, i) = pclCloud->at(i).x;
view(1, i) = pclCloud->at(i).y;
if(!is2D)
{
view(2, i) = pclCloud->at(i).z;
}
}
return cloud;
}
DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud)
DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud, bool is2D)
{
UDEBUG("");
typedef DP::Label Label;
@@ -105,8 +110,11 @@ DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud)
isFeature.push_back(true);
featLabels.push_back(Label("y", 1));
isFeature.push_back(true);
featLabels.push_back(Label("z", 1));
isFeature.push_back(true);
if(!is2D)
{
featLabels.push_back(Label("z", 1));
isFeature.push_back(true);
}
descLabels.push_back(Label("normals", 3));
isFeature.push_back(false);
@@ -120,17 +128,18 @@ DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud)
cloud.getFeatureViewByName("pad").setConstant(1);
// fill cloud
View viewX(cloud.getFeatureViewByName("x"));
View viewY(cloud.getFeatureViewByName("y"));
View viewZ(cloud.getFeatureViewByName("z"));
View view(cloud.getFeatureViewByName("x"));
View viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
View viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
View viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
for(unsigned int i=0; i<pclCloud->size(); ++i)
{
viewX(0, i) = pclCloud->at(i).x;
viewY(0, i) = pclCloud->at(i).y;
viewZ(0, i) = pclCloud->at(i).z;
view(0, i) = pclCloud->at(i).x;
view(1, i) = pclCloud->at(i).y;
if(!is2D)
{
view(2, i) = pclCloud->at(i).z;
}
viewNormalX(0, i) = pclCloud->at(i).normal_x;
viewNormalY(0, i) = pclCloud->at(i).normal_y;
viewNormalZ(0, i) = pclCloud->at(i).normal_z;
@@ -151,14 +160,13 @@ void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointXYZ> & pclCloud)
pclCloud.is_dense = true;
// fill cloud
ConstView viewX(cloud.getFeatureViewByName("x"));
ConstView viewY(cloud.getFeatureViewByName("y"));
ConstView viewZ(cloud.getFeatureViewByName("z"));
ConstView view(cloud.getFeatureViewByName("x"));
bool is3D = cloud.featureExists("z");
for(unsigned int i=0; i<pclCloud.size(); ++i)
{
pclCloud.at(i).x = viewX(0, i);
pclCloud.at(i).y = viewY(0, i);
pclCloud.at(i).z = viewZ(0, i);
pclCloud.at(i).x = view(0, i);
pclCloud.at(i).y = view(1, i);
pclCloud.at(i).z = is3D?view(2, i):0;
}
}
@@ -174,17 +182,16 @@ void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointNormal> & pclCloud)
pclCloud.is_dense = true;
// fill cloud
ConstView viewX(cloud.getFeatureViewByName("x"));
ConstView viewY(cloud.getFeatureViewByName("y"));
ConstView viewZ(cloud.getFeatureViewByName("z"));
ConstView view(cloud.getFeatureViewByName("x"));
bool is3D = cloud.featureExists("z");
ConstView viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
ConstView viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
ConstView viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
for(unsigned int i=0; i<pclCloud.size(); ++i)
{
pclCloud.at(i).x = viewX(0, i);
pclCloud.at(i).y = viewY(0, i);
pclCloud.at(i).z = viewZ(0, i);
pclCloud.at(i).x = view(0, i);
pclCloud.at(i).y = view(1, i);
pclCloud.at(i).z = is3D?view(2, i):0;
pclCloud.at(i).normal_x = viewNormalX(0, i);
pclCloud.at(i).normal_y = viewNormalY(0, i);
pclCloud.at(i).normal_z = viewNormalZ(0, i);
@@ -319,6 +326,12 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
icp->outlierFilters.clear();
icp->outlierFilters.push_back(PM::get().OutlierFilterRegistrar.create("TrimmedDistOutlierFilter", params));
params.clear();
if(_pointToPlane)
{
params["maxAngle"] = uNumber2Str(_maxRotation<=0.0f?M_PI:_maxRotation);
icp->outlierFilters.push_back(PM::get().OutlierFilterRegistrar.create("SurfaceNormalOutlierFilter", params));
params.clear();
}
icp->errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create(_pointToPlane?"PointToPlaneErrorMinimizer":"PointToPointErrorMinimizer"));
@@ -332,6 +345,11 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
params["smoothLength"] = uNumber2Str(4);
icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("DifferentialTransformationChecker", params));
params.clear();
params["maxRotationNorm"] = uNumber2Str(_maxRotation<=0.0f?M_PI:_maxRotation);
params["maxTranslationNorm"] = uNumber2Str(_maxTranslation<=0.0f?std::numeric_limits<float>::max():_maxTranslation);
icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("BoundTransformationChecker", params));
params.clear();
}
}
#endif
@@ -408,20 +426,25 @@ Transform RegistrationIcp::computeTransformationImpl(
if( _pointToPlane &&
_voxelSize == 0.0f &&
fromScan.channels() >= 6 &&
toScan.channels() >= 6)
toScan.channels() >= 6 &&
!((fromScan.channels() == 5 || toScan.channels() == 5) && !_libpointmatcher)) // PCL crashes if 2D)
{
//special case if we have already normals computed and there is no filtering
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform);
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toLocalTransform);
fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
UDEBUG("Conversion time = %f s", timer.ticks());
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
#ifdef RTABMAP_POINTMATCHER
if(_libpointmatcher)
{
// Load point clouds
DP data = pclToDP(fromCloudNormals);
DP ref = pclToDP(toCloudNormals);
DP data = pclToDP(fromCloudNormals, fromScan.channels() == 5);
DP ref = pclToDP(toCloudNormals, toScan.channels() == 5);
// Compute the transformation to express data in ref
PM::TransformationParameters T;
@@ -451,9 +474,6 @@ Transform RegistrationIcp::computeTransformationImpl(
else
#endif
{
fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
icpT = util3d::icpPointToPlane(
fromCloudNormals,
toCloudNormals,
@@ -502,14 +522,14 @@ Transform RegistrationIcp::computeTransformationImpl(
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
if(_pointToPlane && // ICP Point To Plane
!((fromScan.channels() == 2 || toScan.channels() == 2) && !_libpointmatcher)) // PCL crashes if 2D
!((fromScan.channels() == 2 || fromScan.channels() == 5 || toScan.channels() == 2 || toScan.channels() == 5) && !_libpointmatcher)) // PCL crashes if 2D
{
pcl::PointCloud<pcl::Normal>::Ptr normals;
Eigen::Vector3f viewpointFrom(fromLocalTransform.x(), fromLocalTransform.y(), fromLocalTransform.z());
Transform toT = guess * toLocalTransform;
Eigen::Vector3f viewpointTo(toT.x(), toT.y(), toT.z());
if(fromScan.channels() == 2)
if(fromScan.channels() == 2 || fromScan.channels() == 5)
{
normals = util3d::computeFastOrganizedNormals2D(
fromCloudFiltered,
@@ -524,7 +544,7 @@ Transform RegistrationIcp::computeTransformationImpl(
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*fromCloudFiltered, *normals, *fromCloudNormals);
if(toScan.channels() == 2)
if(toScan.channels() == 2 || toScan.channels() == 5)
{
normals = util3d::computeFastOrganizedNormals2D(
toCloudFiltered,
@@ -544,8 +564,22 @@ Transform RegistrationIcp::computeTransformationImpl(
fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
// update output scans
fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
if(fromScan.channels() == 2 || toScan.channels() == 5)
{
fromSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
}
else
{
fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
}
if(toScan.channels() == 2 || toScan.channels() == 5)
{
toSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*toCloudNormals, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
}
else
{
toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
}
UDEBUG("Compute normals (%d,%d) time = %f s", (int)fromCloudNormals->size(), (int)toCloudNormals->size(), timer.ticks());
@@ -557,8 +591,8 @@ Transform RegistrationIcp::computeTransformationImpl(
if(_libpointmatcher)
{
// Load point clouds
DP data = pclToDP(fromCloudNormals);
DP ref = pclToDP(toCloudNormals);
DP data = pclToDP(fromCloudNormals, fromScan.channels() == 2 || fromScan.channels() == 5);
DP ref = pclToDP(toCloudNormals, toScan.channels() == 2 || toScan.channels() == 5);
// Compute the transformation to express data in ref
PM::TransformationParameters T;
@@ -612,7 +646,7 @@ Transform RegistrationIcp::computeTransformationImpl(
}
else // ICP Point to Point
{
if(_pointToPlane && ((fromScan.channels() == 2 || toScan.channels() == 2) && !_libpointmatcher))
if(_pointToPlane && ((fromScan.channels() == 2 || fromScan.channels() == 5 || toScan.channels() == 2 || toScan.channels() == 5) && !_libpointmatcher))
{
UWARN("ICP PointToPlane ignored for 2d scans with PCL registration (some crash issues). Use libpointmatcher (%s) or disable %s to avoid this warning.", Parameters::kIcpPM().c_str(), Parameters::kIcpPointToPlane().c_str());
}
@@ -628,8 +662,8 @@ Transform RegistrationIcp::computeTransformationImpl(
if(_libpointmatcher)
{
// Load point clouds
DP data = pclToDP(fromCloudFiltered);
DP ref = pclToDP(toCloudFiltered);
DP data = pclToDP(fromCloudFiltered, fromScan.channels() == 2 || fromScan.channels() == 5);
DP ref = pclToDP(toCloudFiltered, toScan.channels() == 2 || toScan.channels() == 5);
// Compute the transformation to express data in ref
PM::TransformationParameters T;

2
corelib/src/Rtabmap.cpp
View File

@@ -1116,7 +1116,7 @@ bool Rtabmap::process(
else
{
UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str());
if(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0)
if(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(0,0) != 1.0 && info.covariance.at<double>(5,5) > 0.0 && info.covariance.at<double>(5,5) != 1.0)
{
_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv()));
}

113
corelib/src/util3d.cpp
View File

@@ -1459,9 +1459,75 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud,
return laserScan;
}
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5));
bool nullTransform = transform.isNull();
for(unsigned int i=0; i<cloud.size(); ++i)
{
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
{
pcl::PointNormal pt = util3d::transformPoint(cloud.at(i), transform);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.normal_x;
ptr[3] = pt.normal_y;
ptr[4] = pt.normal_z;
}
else
{
const pcl::PointNormal & pt = cloud.at(i);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.normal_x;
ptr[3] = pt.normal_y;
ptr[4] = pt.normal_z;
}
}
return laserScan;
}
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform)
{
UASSERT(cloud.size() == normals.size());
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5));
bool nullTransform = transform.isNull() || transform.isIdentity();
for(unsigned int i=0; i<cloud.size(); ++i)
{
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
{
pcl::PointNormal pt;
pt.x = cloud.at(i).x;
pt.y = cloud.at(i).y;
pt.z = cloud.at(i).z;
pt.normal_x = normals.at(i).normal_x;
pt.normal_y = normals.at(i).normal_y;
pt.normal_z = normals.at(i).normal_z;
pt = util3d::transformPoint(pt, transform);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.normal_x;
ptr[3] = pt.normal_y;
ptr[4] = pt.normal_z;
}
else
{
ptr[0] = cloud.at(i).x;
ptr[1] = cloud.at(i).y;
ptr[2] = normals.at(i).normal_x;
ptr[3] = normals.at(i).normal_y;
ptr[4] = normals.at(i).normal_z;
}
}
return laserScan;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
output->resize(laserScan.cols);
@@ -1480,7 +1546,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserS
pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat & laserScan, const Transform & transform)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
output->resize(laserScan.cols);
@@ -1498,7 +1564,7 @@ pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat
pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const cv::Mat & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
output->resize(laserScan.cols);
@@ -1517,7 +1583,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const cv::Mat &
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(const cv::Mat & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
output->resize(laserScan.cols);
@@ -1536,12 +1602,12 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(cons
pcl::PointXYZ laserScanToPoint(const cv::Mat & laserScan, int index)
{
UASSERT(!laserScan.empty() && index < laserScan.cols);
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointXYZ output;
const float * ptr = laserScan.ptr<float>(0, index);
output.x = ptr[0];
output.y = ptr[1];
if(laserScan.channels() >= 3)
if(laserScan.channels() >= 3 && laserScan.channels() != 5)
{
output.z = ptr[2];
}
@@ -1551,16 +1617,22 @@ pcl::PointXYZ laserScanToPoint(const cv::Mat & laserScan, int index)
pcl::PointNormal laserScanToPointNormal(const cv::Mat & laserScan, int index)
{
UASSERT(!laserScan.empty() && index < laserScan.cols);
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointNormal output;
const float * ptr = laserScan.ptr<float>(0, index);
output.x = ptr[0];
output.y = ptr[1];
if(laserScan.channels() >= 3)
if(laserScan.channels() >= 3 && laserScan.channels() != 5)
{
output.z = ptr[2];
}
if(laserScan.channels() == 6)
if(laserScan.channels() == 5)
{
output.normal_x = ptr[2];
output.normal_y = ptr[3];
output.normal_z = ptr[4];
}
else if(laserScan.channels() == 6)
{
output.normal_x = ptr[3];
output.normal_y = ptr[4];
@@ -1578,12 +1650,12 @@ pcl::PointNormal laserScanToPointNormal(const cv::Mat & laserScan, int index)
pcl::PointXYZRGB laserScanToPointRGB(const cv::Mat & laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
{
UASSERT(!laserScan.empty() && index < laserScan.cols);
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointXYZRGB output;
const float * ptr = laserScan.ptr<float>(0, index);
output.x = ptr[0];
output.y = ptr[1];
if(laserScan.channels() >= 3)
if(laserScan.channels() >= 3 && laserScan.channels() != 5)
{
output.z = ptr[2];
}
@@ -1606,16 +1678,22 @@ pcl::PointXYZRGB laserScanToPointRGB(const cv::Mat & laserScan, int index, unsig
pcl::PointXYZRGBNormal laserScanToPointRGBNormal(const cv::Mat & laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
{
UASSERT(!laserScan.empty() && index < laserScan.cols);
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointXYZRGBNormal output;
const float * ptr = laserScan.ptr<float>(0, index);
output.x = ptr[0];
output.y = ptr[1];
if(laserScan.channels() >= 3)
if(laserScan.channels() >= 3 && laserScan.channels() != 5)
{
output.z = ptr[2];
}
if(laserScan.channels() == 6)
if(laserScan.channels() == 5)
{
output.normal_x = ptr[2];
output.normal_y = ptr[3];
output.normal_z = ptr[4];
}
else if(laserScan.channels() == 6)
{
output.normal_x = ptr[3];
output.normal_y = ptr[4];
@@ -1646,12 +1724,13 @@ pcl::PointXYZRGBNormal laserScanToPointRGBNormal(const cv::Mat & laserScan, int
void getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max)
{
UASSERT(!laserScan.empty());
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
const float * ptr = laserScan.ptr<float>(0, 0);
min.x = max.x = ptr[0];
min.y = max.y = ptr[1];
min.z = max.z = laserScan.channels() >= 3?ptr[2]:0.0f;
bool is3d = laserScan.channels() >= 3 && laserScan.channels() != 5;
min.z = max.z = is3d?ptr[2]:0.0f;
for(int i=1; i<laserScan.cols; ++i)
{
ptr = laserScan.ptr<float>(0, i);
@@ -1662,7 +1741,7 @@ void getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max
if(ptr[1] < min.y) min.y = ptr[1];
else if(ptr[1] > max.y) max.y = ptr[1];
if(laserScan.channels() >= 3)
if(is3d)
{
if(ptr[2] < min.z) min.z = ptr[2];
else if(ptr[2] > max.z) max.z = ptr[2];

3
corelib/src/util3d_surface.cpp
View File

@@ -2011,12 +2011,13 @@ cv::Mat computeNormals(
if(laserScan.channels() == 2)
{
normals = util3d::computeNormals2D(cloud, searchK, searchRadius);
return util3d::laserScan2dFromPointCloud(*cloud, *normals);
}
else
{
normals = util3d::computeNormals(cloud, searchK, searchRadius);
return util3d::laserScanFromPointCloud(*cloud, *normals);
}
return util3d::laserScanFromPointCloud(*cloud, *normals);
}
else // 4 channels
{

84
corelib/src/util3d_transforms.cpp
View File

@@ -38,61 +38,65 @@ namespace util3d
cv::Mat transformLaserScan(const cv::Mat & laserScan, const Transform & transform)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6));
cv::Mat output = laserScan.clone();
if(!transform.isNull() && !transform.isIdentity())
{
Eigen::Affine3f transform3f = transform.toEigen3f();
for(int i=0; i<laserScan.cols; ++i)
{
const float * ptr = laserScan.ptr<float>(0, i);
float * out = output.ptr<float>(0, i);
if(laserScan.type() == CV_32FC2)
{
pcl::PointXYZ pt(
laserScan.at<cv::Vec2f>(i)[0],
laserScan.at<cv::Vec2f>(i)[1], 0);
pt = util3d::transformPoint(pt, transform);
output.at<cv::Vec2f>(i)[0] = pt.x;
output.at<cv::Vec2f>(i)[1] = pt.y;
pcl::PointXYZ pt(ptr[0], ptr[1], 0);
pt = pcl::transformPoint(pt, transform3f);
out[0] = pt.x;
out[1] = pt.y;
}
else if(laserScan.type() == CV_32FC3)
else if(laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4))
{
pcl::PointXYZ pt(
laserScan.at<cv::Vec3f>(i)[0],
laserScan.at<cv::Vec3f>(i)[1],
laserScan.at<cv::Vec3f>(i)[2]);
pt = util3d::transformPoint(pt, transform);
output.at<cv::Vec3f>(i)[0] = pt.x;
output.at<cv::Vec3f>(i)[1] = pt.y;
output.at<cv::Vec3f>(i)[2] = pt.z;
const float * ptr = laserScan.ptr<float>(0, i);
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.type() == CV_32FC(4))
{
pcl::PointXYZ pt(
laserScan.at<cv::Vec4f>(i)[0],
laserScan.at<cv::Vec4f>(i)[1],
laserScan.at<cv::Vec4f>(i)[2]);
pt = util3d::transformPoint(pt, transform);
output.at<cv::Vec4f>(i)[0] = pt.x;
output.at<cv::Vec4f>(i)[1] = pt.y;
output.at<cv::Vec4f>(i)[2] = pt.z;
}
else
else if(laserScan.type() == CV_32FC(5))
{
pcl::PointNormal pt;
pt.x=laserScan.at<cv::Vec6f>(i)[0];
pt.y=laserScan.at<cv::Vec6f>(i)[1];
pt.z=laserScan.at<cv::Vec6f>(i)[2];
pt.normal_x=laserScan.at<cv::Vec6f>(i)[3];
pt.normal_y=laserScan.at<cv::Vec6f>(i)[4];
pt.normal_z=laserScan.at<cv::Vec6f>(i)[5];
pt.x=ptr[0];
pt.y=ptr[1];
pt.z=0;
pt.normal_x=ptr[2];
pt.normal_y=ptr[3];
pt.normal_z=ptr[4];
pt = util3d::transformPoint(pt, transform);
output.at<cv::Vec6f>(i)[0] = pt.x;
output.at<cv::Vec6f>(i)[1] = pt.y;
output.at<cv::Vec6f>(i)[2] = pt.z;
output.at<cv::Vec6f>(i)[3] = pt.normal_x;
output.at<cv::Vec6f>(i)[4] = pt.normal_y;
output.at<cv::Vec6f>(i)[5] = pt.normal_z;
out[0] = pt.x;
out[1] = pt.y;
out[2] = pt.normal_x;
out[3] = pt.normal_y;
out[4] = pt.normal_z;
}
else // 6 and 7 channels
{
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;
}
}
}