i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 16:57:46 +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-28 16:40:08 -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
corelib/include/rtabmap/core/util3d.h
+6 -3
View File
@@ -192,17 +192,20 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImages(
// return CV_32FC3 (x,y,z) // return CV_32FC3 (x,y,z)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
// return CV_32FC6 (x,y,z,normal_z,normal_y,normalz) // return CV_32FC6 (x,y,z,normal_x,normal_y,normal_z)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform());
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
// return CV_32FC4 (x,y,z,rgb) // return CV_32FC4 (x,y,z,rgb)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform());
// return CV_32FC7 (x,y,z,rgb,normal_z,normal_y,normalz) // return CV_32FC7 (x,y,z,rgb,normal_x,normal_y,normal_z)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
// return CV_32FC7 (x,y,z,rgb,normal_z,normal_y,normalz) // return CV_32FC7 (x,y,z,rgb,normal_x,normal_y,normal_z)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform = Transform());
// return CV_32FC2 (x,y) // return CV_32FC2 (x,y)
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
// return CV_32FC5 (x,y,normal_x, normal_y, normal_z)
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform());
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
// For laserScan of type CV_32FC2, z is set to null. // For laserScan of type CV_32FC2, z is set to null.
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform = Transform()); pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform = Transform());
// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null. // For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null.
corelib/src/Memory.cpp
+33 -12
View File
@@ -2438,6 +2438,7 @@ Transform Memory::computeIcpTransformMulti(
std::string msg; std::string msg;
int maxPoints = fromScan.cols; int maxPoints = fromScan.cols;
pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>); 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) for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{ {
if(iter->first != fromId) if(iter->first != fromId)
@@ -2447,14 +2448,21 @@ Transform Memory::computeIcpTransformMulti(
{ {
cv::Mat scan; cv::Mat scan;
s->sensorData().uncompressData(0, 0, &scan); s->sensorData().uncompressData(0, 0, &scan);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud( if(!scan.empty())
scan,
s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second);
if(scan.cols > maxPoints)
{ {
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 else
{ {
@@ -2464,12 +2472,25 @@ Transform Memory::computeIcpTransformMulti(
} }
if(assembledToClouds->size()) if(assembledToClouds->size())
{ {
assembledData.setLaserScanRaw( if(is2D)
util3d::laserScanFromPointCloud(*assembledToClouds), {
LaserScanInfo( assembledData.setLaserScanRaw(
fromS->sensorData().laserScanInfo().maxPoints()?fromS->sensorData().laserScanInfo().maxPoints():maxPoints, util3d::laserScan2dFromPointCloud(*assembledToClouds),
fromS->sensorData().laserScanInfo().maxRange(), LaserScanInfo(
Transform::getIdentity())); // scans are in base frame 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); Transform guess = poses.at(fromId).inverse() * poses.at(toId);
corelib/src/OccupancyGrid.cpp
+1 -1
View File
@@ -207,7 +207,7 @@ void OccupancyGrid::createLocalMap(
UDEBUG("scan channels=%d, occupancyFromCloud_=%d normalsSegmentation_=%d grid3D_=%d", 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); 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"); UDEBUG("2D laser scan");
//2D //2D
corelib/src/RegistrationIcp.cpp
+80 -46
View File
@@ -39,11 +39,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/conversions.h> #include <pcl/conversions.h>
#ifdef RTABMAP_POINTMATCHER #ifdef RTABMAP_POINTMATCHER
#include <fstream>
#include "pointmatcher/PointMatcher.h" #include "pointmatcher/PointMatcher.h"
typedef PointMatcher<float> PM; typedef PointMatcher<float> PM;
typedef PM::DataPoints DP; 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(""); UDEBUG("");
typedef DP::Label Label; typedef DP::Label Label;
@@ -63,8 +64,11 @@ DP pclToDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud)
isFeature.push_back(true); isFeature.push_back(true);
featLabels.push_back(Label("y", 1)); featLabels.push_back(Label("y", 1));
isFeature.push_back(true); isFeature.push_back(true);
featLabels.push_back(Label("z", 1)); if(!is2D)
isFeature.push_back(true); {
featLabels.push_back(Label("z", 1));
isFeature.push_back(true);
}
featLabels.push_back(Label("pad", 1)); featLabels.push_back(Label("pad", 1));
// create cloud // create cloud
@@ -72,20 +76,21 @@ DP pclToDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud)
cloud.getFeatureViewByName("pad").setConstant(1); cloud.getFeatureViewByName("pad").setConstant(1);
// fill cloud // fill cloud
View viewX(cloud.getFeatureViewByName("x")); View view(cloud.getFeatureViewByName("x"));
View viewY(cloud.getFeatureViewByName("y"));
View viewZ(cloud.getFeatureViewByName("z"));
for(unsigned int i=0; i<pclCloud->size(); ++i) for(unsigned int i=0; i<pclCloud->size(); ++i)
{ {
viewX(0, i) = pclCloud->at(i).x; view(0, i) = pclCloud->at(i).x;
viewY(0, i) = pclCloud->at(i).y; view(1, i) = pclCloud->at(i).y;
viewZ(0, i) = pclCloud->at(i).z; if(!is2D)
{
view(2, i) = pclCloud->at(i).z;
}
} }
return cloud; return cloud;
} }
DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud) DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud, bool is2D)
{ {
UDEBUG(""); UDEBUG("");
typedef DP::Label Label; typedef DP::Label Label;
@@ -105,8 +110,11 @@ DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud)
isFeature.push_back(true); isFeature.push_back(true);
featLabels.push_back(Label("y", 1)); featLabels.push_back(Label("y", 1));
isFeature.push_back(true); isFeature.push_back(true);
featLabels.push_back(Label("z", 1)); if(!is2D)
isFeature.push_back(true); {
featLabels.push_back(Label("z", 1));
isFeature.push_back(true);
}
descLabels.push_back(Label("normals", 3)); descLabels.push_back(Label("normals", 3));
isFeature.push_back(false); isFeature.push_back(false);
@@ -120,17 +128,18 @@ DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud)
cloud.getFeatureViewByName("pad").setConstant(1); cloud.getFeatureViewByName("pad").setConstant(1);
// fill cloud // fill cloud
View viewX(cloud.getFeatureViewByName("x")); View view(cloud.getFeatureViewByName("x"));
View viewY(cloud.getFeatureViewByName("y"));
View viewZ(cloud.getFeatureViewByName("z"));
View viewNormalX(cloud.getDescriptorRowViewByName("normals",0)); View viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
View viewNormalY(cloud.getDescriptorRowViewByName("normals",1)); View viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
View viewNormalZ(cloud.getDescriptorRowViewByName("normals",2)); View viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
for(unsigned int i=0; i<pclCloud->size(); ++i) for(unsigned int i=0; i<pclCloud->size(); ++i)
{ {
viewX(0, i) = pclCloud->at(i).x; view(0, i) = pclCloud->at(i).x;
viewY(0, i) = pclCloud->at(i).y; view(1, i) = pclCloud->at(i).y;
viewZ(0, i) = pclCloud->at(i).z; if(!is2D)
{
view(2, i) = pclCloud->at(i).z;
}
viewNormalX(0, i) = pclCloud->at(i).normal_x; viewNormalX(0, i) = pclCloud->at(i).normal_x;
viewNormalY(0, i) = pclCloud->at(i).normal_y; viewNormalY(0, i) = pclCloud->at(i).normal_y;
viewNormalZ(0, i) = pclCloud->at(i).normal_z; 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; pclCloud.is_dense = true;
// fill cloud // fill cloud
ConstView viewX(cloud.getFeatureViewByName("x")); ConstView view(cloud.getFeatureViewByName("x"));
ConstView viewY(cloud.getFeatureViewByName("y")); bool is3D = cloud.featureExists("z");
ConstView viewZ(cloud.getFeatureViewByName("z"));
for(unsigned int i=0; i<pclCloud.size(); ++i) for(unsigned int i=0; i<pclCloud.size(); ++i)
{ {
pclCloud.at(i).x = viewX(0, i); pclCloud.at(i).x = view(0, i);
pclCloud.at(i).y = viewY(0, i); pclCloud.at(i).y = view(1, i);
pclCloud.at(i).z = viewZ(0, 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; pclCloud.is_dense = true;
// fill cloud // fill cloud
ConstView viewX(cloud.getFeatureViewByName("x")); ConstView view(cloud.getFeatureViewByName("x"));
ConstView viewY(cloud.getFeatureViewByName("y")); bool is3D = cloud.featureExists("z");
ConstView viewZ(cloud.getFeatureViewByName("z"));
ConstView viewNormalX(cloud.getDescriptorRowViewByName("normals",0)); ConstView viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
ConstView viewNormalY(cloud.getDescriptorRowViewByName("normals",1)); ConstView viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
ConstView viewNormalZ(cloud.getDescriptorRowViewByName("normals",2)); ConstView viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
for(unsigned int i=0; i<pclCloud.size(); ++i) for(unsigned int i=0; i<pclCloud.size(); ++i)
{ {
pclCloud.at(i).x = viewX(0, i); pclCloud.at(i).x = view(0, i);
pclCloud.at(i).y = viewY(0, i); pclCloud.at(i).y = view(1, i);
pclCloud.at(i).z = viewZ(0, i); pclCloud.at(i).z = is3D?view(2, i):0;
pclCloud.at(i).normal_x = viewNormalX(0, i); pclCloud.at(i).normal_x = viewNormalX(0, i);
pclCloud.at(i).normal_y = viewNormalY(0, i); pclCloud.at(i).normal_y = viewNormalY(0, i);
pclCloud.at(i).normal_z = viewNormalZ(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.clear();
icp->outlierFilters.push_back(PM::get().OutlierFilterRegistrar.create("TrimmedDistOutlierFilter", params)); icp->outlierFilters.push_back(PM::get().OutlierFilterRegistrar.create("TrimmedDistOutlierFilter", params));
params.clear(); 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")); icp->errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create(_pointToPlane?"PointToPlaneErrorMinimizer":"PointToPointErrorMinimizer"));
@@ -332,6 +345,11 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
params["smoothLength"] = uNumber2Str(4); params["smoothLength"] = uNumber2Str(4);
icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("DifferentialTransformationChecker", params)); icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("DifferentialTransformationChecker", params));
params.clear(); 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 #endif
@@ -408,20 +426,25 @@ Transform RegistrationIcp::computeTransformationImpl(
if( _pointToPlane && if( _pointToPlane &&
_voxelSize == 0.0f && _voxelSize == 0.0f &&
fromScan.channels() >= 6 && 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 //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 fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform);
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toLocalTransform); 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()); UDEBUG("Conversion time = %f s", timer.ticks());
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>()); pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
#ifdef RTABMAP_POINTMATCHER #ifdef RTABMAP_POINTMATCHER
if(_libpointmatcher) if(_libpointmatcher)
{ {
// Load point clouds // Load point clouds
DP data = pclToDP(fromCloudNormals); DP data = pclToDP(fromCloudNormals, fromScan.channels() == 5);
DP ref = pclToDP(toCloudNormals); DP ref = pclToDP(toCloudNormals, toScan.channels() == 5);
// Compute the transformation to express data in ref // Compute the transformation to express data in ref
PM::TransformationParameters T; PM::TransformationParameters T;
@@ -451,9 +474,6 @@ Transform RegistrationIcp::computeTransformationImpl(
else else
#endif #endif
{ {
fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
icpT = util3d::icpPointToPlane( icpT = util3d::icpPointToPlane(
fromCloudNormals, fromCloudNormals,
toCloudNormals, toCloudNormals,
@@ -502,14 +522,14 @@ Transform RegistrationIcp::computeTransformationImpl(
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>()); pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
if(_pointToPlane && // ICP Point To Plane 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; pcl::PointCloud<pcl::Normal>::Ptr normals;
Eigen::Vector3f viewpointFrom(fromLocalTransform.x(), fromLocalTransform.y(), fromLocalTransform.z()); Eigen::Vector3f viewpointFrom(fromLocalTransform.x(), fromLocalTransform.y(), fromLocalTransform.z());
Transform toT = guess * toLocalTransform; Transform toT = guess * toLocalTransform;
Eigen::Vector3f viewpointTo(toT.x(), toT.y(), toT.z()); Eigen::Vector3f viewpointTo(toT.x(), toT.y(), toT.z());
if(fromScan.channels() == 2) if(fromScan.channels() == 2 || fromScan.channels() == 5)
{ {
normals = util3d::computeFastOrganizedNormals2D( normals = util3d::computeFastOrganizedNormals2D(
fromCloudFiltered, fromCloudFiltered,
@@ -524,7 +544,7 @@ Transform RegistrationIcp::computeTransformationImpl(
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointNormal>); pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*fromCloudFiltered, *normals, *fromCloudNormals); pcl::concatenateFields(*fromCloudFiltered, *normals, *fromCloudNormals);
if(toScan.channels() == 2) if(toScan.channels() == 2 || toScan.channels() == 5)
{ {
normals = util3d::computeFastOrganizedNormals2D( normals = util3d::computeFastOrganizedNormals2D(
toCloudFiltered, toCloudFiltered,
@@ -544,8 +564,22 @@ Transform RegistrationIcp::computeTransformationImpl(
fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals); fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
// update output scans // update output scans
fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform)); if(fromScan.channels() == 2 || toScan.channels() == 5)
toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform)); {
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()); 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) if(_libpointmatcher)
{ {
// Load point clouds // Load point clouds
DP data = pclToDP(fromCloudNormals); DP data = pclToDP(fromCloudNormals, fromScan.channels() == 2 || fromScan.channels() == 5);
DP ref = pclToDP(toCloudNormals); DP ref = pclToDP(toCloudNormals, toScan.channels() == 2 || toScan.channels() == 5);
// Compute the transformation to express data in ref // Compute the transformation to express data in ref
PM::TransformationParameters T; PM::TransformationParameters T;
@@ -612,7 +646,7 @@ Transform RegistrationIcp::computeTransformationImpl(
} }
else // ICP Point to Point 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()); 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) if(_libpointmatcher)
{ {
// Load point clouds // Load point clouds
DP data = pclToDP(fromCloudFiltered); DP data = pclToDP(fromCloudFiltered, fromScan.channels() == 2 || fromScan.channels() == 5);
DP ref = pclToDP(toCloudFiltered); DP ref = pclToDP(toCloudFiltered, toScan.channels() == 2 || toScan.channels() == 5);
// Compute the transformation to express data in ref // Compute the transformation to express data in ref
PM::TransformationParameters T; PM::TransformationParameters T;
corelib/src/Rtabmap.cpp
+1 -1
View File
@@ -1116,7 +1116,7 @@ bool Rtabmap::process(
else else
{ {
UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str()); 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())); _memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv()));
} }
corelib/src/util3d.cpp
+96 -17
View File
@@ -1459,9 +1459,75 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud,
return laserScan; 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) 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>); pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
output->resize(laserScan.cols); 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) 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>); pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
output->resize(laserScan.cols); 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) 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>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
output->resize(laserScan.cols); 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) 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>); pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
output->resize(laserScan.cols); output->resize(laserScan.cols);
@@ -1536,12 +1602,12 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(cons
pcl::PointXYZ laserScanToPoint(const cv::Mat & laserScan, int index) pcl::PointXYZ laserScanToPoint(const cv::Mat & laserScan, int index)
{ {
UASSERT(!laserScan.empty() && index < laserScan.cols); 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; pcl::PointXYZ output;
const float * ptr = laserScan.ptr<float>(0, index); const float * ptr = laserScan.ptr<float>(0, index);
output.x = ptr[0]; output.x = ptr[0];
output.y = ptr[1]; output.y = ptr[1];
if(laserScan.channels() >= 3) if(laserScan.channels() >= 3 && laserScan.channels() != 5)
{ {
output.z = ptr[2]; 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) pcl::PointNormal laserScanToPointNormal(const cv::Mat & laserScan, int index)
{ {
UASSERT(!laserScan.empty() && index < laserScan.cols); 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; pcl::PointNormal output;
const float * ptr = laserScan.ptr<float>(0, index); const float * ptr = laserScan.ptr<float>(0, index);
output.x = ptr[0]; output.x = ptr[0];
output.y = ptr[1]; output.y = ptr[1];
if(laserScan.channels() >= 3) if(laserScan.channels() >= 3 && laserScan.channels() != 5)
{ {
output.z = ptr[2]; 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_x = ptr[3];
output.normal_y = ptr[4]; 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) 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.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; pcl::PointXYZRGB output;
const float * ptr = laserScan.ptr<float>(0, index); const float * ptr = laserScan.ptr<float>(0, index);
output.x = ptr[0]; output.x = ptr[0];
output.y = ptr[1]; output.y = ptr[1];
if(laserScan.channels() >= 3) if(laserScan.channels() >= 3 && laserScan.channels() != 5)
{ {
output.z = ptr[2]; 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) 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.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; pcl::PointXYZRGBNormal output;
const float * ptr = laserScan.ptr<float>(0, index); const float * ptr = laserScan.ptr<float>(0, index);
output.x = ptr[0]; output.x = ptr[0];
output.y = ptr[1]; output.y = ptr[1];
if(laserScan.channels() >= 3) if(laserScan.channels() >= 3 && laserScan.channels() != 5)
{ {
output.z = ptr[2]; 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_x = ptr[3];
output.normal_y = ptr[4]; 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) void getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max)
{ {
UASSERT(!laserScan.empty()); 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); const float * ptr = laserScan.ptr<float>(0, 0);
min.x = max.x = ptr[0]; min.x = max.x = ptr[0];
min.y = max.y = ptr[1]; 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) for(int i=1; i<laserScan.cols; ++i)
{ {
ptr = laserScan.ptr<float>(0, 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]; if(ptr[1] < min.y) min.y = ptr[1];
else if(ptr[1] > max.y) max.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]; if(ptr[2] < min.z) min.z = ptr[2];
else if(ptr[2] > max.z) max.z = ptr[2]; else if(ptr[2] > max.z) max.z = ptr[2];
corelib/src/util3d_surface.cpp
+2 -1
View File
@@ -2011,12 +2011,13 @@ cv::Mat computeNormals(
if(laserScan.channels() == 2) if(laserScan.channels() == 2)
{ {
normals = util3d::computeNormals2D(cloud, searchK, searchRadius); normals = util3d::computeNormals2D(cloud, searchK, searchRadius);
return util3d::laserScan2dFromPointCloud(*cloud, *normals);
} }
else else
{ {
normals = util3d::computeNormals(cloud, searchK, searchRadius); normals = util3d::computeNormals(cloud, searchK, searchRadius);
return util3d::laserScanFromPointCloud(*cloud, *normals);
} }
return util3d::laserScanFromPointCloud(*cloud, *normals);
} }
else // 4 channels else // 4 channels
{ {
corelib/src/util3d_transforms.cpp
+44 -40
View File
@@ -38,61 +38,65 @@ namespace util3d
cv::Mat transformLaserScan(const cv::Mat & laserScan, const Transform & transform) 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(); cv::Mat output = laserScan.clone();
if(!transform.isNull() && !transform.isIdentity()) if(!transform.isNull() && !transform.isIdentity())
{ {
Eigen::Affine3f transform3f = transform.toEigen3f();
for(int i=0; i<laserScan.cols; ++i) 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) if(laserScan.type() == CV_32FC2)
{ {
pcl::PointXYZ pt( pcl::PointXYZ pt(ptr[0], ptr[1], 0);
laserScan.at<cv::Vec2f>(i)[0], pt = pcl::transformPoint(pt, transform3f);
laserScan.at<cv::Vec2f>(i)[1], 0); out[0] = pt.x;
pt = util3d::transformPoint(pt, transform); out[1] = pt.y;
output.at<cv::Vec2f>(i)[0] = pt.x;
output.at<cv::Vec2f>(i)[1] = pt.y;
} }
else if(laserScan.type() == CV_32FC3) else if(laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4))
{ {
pcl::PointXYZ pt( const float * ptr = laserScan.ptr<float>(0, i);
laserScan.at<cv::Vec3f>(i)[0], pcl::PointXYZ pt(ptr[0], ptr[1], ptr[2]);
laserScan.at<cv::Vec3f>(i)[1], pt = pcl::transformPoint(pt, transform3f);
laserScan.at<cv::Vec3f>(i)[2]); out[0] = pt.x;
pt = util3d::transformPoint(pt, transform); out[1] = pt.y;
output.at<cv::Vec3f>(i)[0] = pt.x; out[2] = pt.z;
output.at<cv::Vec3f>(i)[1] = pt.y;
output.at<cv::Vec3f>(i)[2] = pt.z;
} }
else if(laserScan.type() == CV_32FC(4)) else if(laserScan.type() == CV_32FC(5))
{
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
{ {
pcl::PointNormal pt; pcl::PointNormal pt;
pt.x=laserScan.at<cv::Vec6f>(i)[0]; pt.x=ptr[0];
pt.y=laserScan.at<cv::Vec6f>(i)[1]; pt.y=ptr[1];
pt.z=laserScan.at<cv::Vec6f>(i)[2]; pt.z=0;
pt.normal_x=laserScan.at<cv::Vec6f>(i)[3]; pt.normal_x=ptr[2];
pt.normal_y=laserScan.at<cv::Vec6f>(i)[4]; pt.normal_y=ptr[3];
pt.normal_z=laserScan.at<cv::Vec6f>(i)[5]; pt.normal_z=ptr[4];
pt = util3d::transformPoint(pt, transform); pt = util3d::transformPoint(pt, transform);
output.at<cv::Vec6f>(i)[0] = pt.x; out[0] = pt.x;
output.at<cv::Vec6f>(i)[1] = pt.y; out[1] = pt.y;
output.at<cv::Vec6f>(i)[2] = pt.z; out[2] = pt.normal_x;
output.at<cv::Vec6f>(i)[3] = pt.normal_x; out[3] = pt.normal_y;
output.at<cv::Vec6f>(i)[4] = pt.normal_y; out[4] = pt.normal_z;
output.at<cv::Vec6f>(i)[5] = 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;
} }
} }
} }
guilib/src/DatabaseViewer.cpp
+2 -2
View File
@@ -3757,7 +3757,7 @@ void DatabaseViewer::updateConstraintView(
constraintsViewer_->removeCloud("scan1"); constraintsViewer_->removeCloud("scan1");
if(!dataFrom.laserScanRaw().empty()) if(!dataFrom.laserScanRaw().empty())
{ {
if(dataFrom.laserScanRaw().channels() == 6) if(dataFrom.laserScanRaw().channels() >= 5)
{ {
pcl::PointCloud<pcl::PointNormal>::Ptr scan; pcl::PointCloud<pcl::PointNormal>::Ptr scan;
scan = rtabmap::util3d::laserScanToPointCloudNormal(dataFrom.laserScanRaw(), dataFrom.laserScanInfo().localTransform()); scan = rtabmap::util3d::laserScanToPointCloudNormal(dataFrom.laserScanRaw(), dataFrom.laserScanInfo().localTransform());
@@ -3780,7 +3780,7 @@ void DatabaseViewer::updateConstraintView(
} }
if(!dataTo.laserScanRaw().empty()) if(!dataTo.laserScanRaw().empty())
{ {
if(dataTo.laserScanRaw().channels() == 6) if(dataTo.laserScanRaw().channels() >= 5)
{ {
pcl::PointCloud<pcl::PointNormal>::Ptr scan; pcl::PointCloud<pcl::PointNormal>::Ptr scan;
scan = rtabmap::util3d::laserScanToPointCloudNormal(dataTo.laserScanRaw(), t*dataTo.laserScanInfo().localTransform()); scan = rtabmap::util3d::laserScanToPointCloudNormal(dataTo.laserScanRaw(), t*dataTo.laserScanInfo().localTransform());
guilib/src/EditDepthArea.h
+6 -6
View File
@@ -57,12 +57,12 @@ public slots:
void resetChanges(); void resetChanges();
protected: protected:
void mousePressEvent(QMouseEvent *event) override; virtual void mousePressEvent(QMouseEvent *event);
void mouseMoveEvent(QMouseEvent *event) override; virtual void mouseMoveEvent(QMouseEvent *event);
void mouseReleaseEvent(QMouseEvent *event) override; virtual void mouseReleaseEvent(QMouseEvent *event);
void paintEvent(QPaintEvent *event) override; virtual void paintEvent(QPaintEvent *event);
void resizeEvent(QResizeEvent *event) override; virtual void resizeEvent(QResizeEvent *event);
void contextMenuEvent(QContextMenuEvent * e) override; virtual void contextMenuEvent(QContextMenuEvent * e);
private: private:
void drawLineTo(const QPoint &endPoint); void drawLineTo(const QPoint &endPoint);
guilib/src/MainWindow.cpp
+10 -3
View File
@@ -2888,7 +2888,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
{ {
cloudRGBWithNormals = util3d::laserScanToPointCloudRGBNormal(scan, iter->sensorData().laserScanInfo().localTransform()); cloudRGBWithNormals = util3d::laserScanToPointCloudRGBNormal(scan, iter->sensorData().laserScanInfo().localTransform());
} }
else if(scan.channels() == 6 && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0) else if((scan.channels() == 5 || scan.channels() == 6) && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
{ {
cloudWithNormals = util3d::laserScanToPointCloudNormal(scan, iter->sensorData().laserScanInfo().localTransform()); cloudWithNormals = util3d::laserScanToPointCloudNormal(scan, iter->sensorData().laserScanInfo().localTransform());
} }
@@ -2983,7 +2983,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
pcl::PointCloud<pcl::Normal>::Ptr normals; pcl::PointCloud<pcl::Normal>::Ptr normals;
if(cloud->size()) if(cloud->size())
{ {
if(scan.channels() == 2) if(scan.channels() == 2 || scan.channels() == 5)
{ {
normals = util3d::computeFastOrganizedNormals2D(cloud, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint); normals = util3d::computeFastOrganizedNormals2D(cloud, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
} }
@@ -3024,7 +3024,14 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
added = _cloudViewer->addCloud(scanName, cloudWithNormals, pose, color); added = _cloudViewer->addCloud(scanName, cloudWithNormals, pose, color);
if(added && nodeId > 0) if(added && nodeId > 0)
{ {
scan = util3d::laserScanFromPointCloud(*cloudWithNormals); if(scan.channels() == 5)
{
scan = util3d::laserScan2dFromPointCloud(*cloudWithNormals);
}
else
{
scan = util3d::laserScanFromPointCloud(*cloudWithNormals);
}
} }
} }
else if(cloudRGB.get()) else if(cloudRGB.get())
guilib/src/ui/preferencesDialog.ui
+11 -74
View File
@@ -63,25 +63,16 @@
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
<y>-831</y> <y>0</y>
<width>678</width> <width>678</width>
<height>2739</height> <height>2736</height>
</rect> </rect>
</property> </property>
<layout class="QVBoxLayout" name="verticalLayout_16"> <layout class="QVBoxLayout" name="verticalLayout_16">
<property name="spacing"> <property name="spacing">
<number>0</number> <number>0</number>
</property> </property>
<property name="leftMargin"> <property name="margin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
@@ -95,7 +86,7 @@
<enum>QFrame::Raised</enum> <enum>QFrame::Raised</enum>
</property> </property>
<property name="currentIndex"> <property name="currentIndex">
<number>1</number> <number>21</number>
</property> </property>
<widget class="QWidget" name="page_22"> <widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1"> <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -4570,16 +4561,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>Directory of images (optional settings)</string> <string>Directory of images (optional settings)</string>
</property> </property>
<layout class="QVBoxLayout" name="verticalLayout_93"> <layout class="QVBoxLayout" name="verticalLayout_93">
<property name="leftMargin"> <property name="margin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
@@ -12580,16 +12562,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property> </property>
<widget class="QWidget" name="page_54"> <widget class="QWidget" name="page_54">
<layout class="QVBoxLayout" name="verticalLayout_85"> <layout class="QVBoxLayout" name="verticalLayout_85">
<property name="leftMargin"> <property name="margin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
@@ -12729,16 +12702,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</widget> </widget>
<widget class="QWidget" name="page_55"> <widget class="QWidget" name="page_55">
<layout class="QVBoxLayout" name="verticalLayout_86"> <layout class="QVBoxLayout" name="verticalLayout_86">
<property name="leftMargin"> <property name="margin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
@@ -12896,16 +12860,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<property name="spacing"> <property name="spacing">
<number>0</number> <number>0</number>
</property> </property>
<property name="leftMargin"> <property name="margin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
@@ -12985,16 +12940,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<property name="spacing"> <property name="spacing">
<number>0</number> <number>0</number>
</property> </property>
<property name="leftMargin"> <property name="margin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
@@ -13106,16 +13052,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<property name="spacing"> <property name="spacing">
<number>0</number> <number>0</number>
</property> </property>
<property name="leftMargin"> <property name="margin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number> <number>0</number>
</property> </property>
<item> <item>
@@ -13808,7 +13745,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<item row="8" column="1"> <item row="8" column="1">
<widget class="QLabel" name="label_144"> <widget class="QLabel" name="label_144">
<property name="text"> <property name="text">
<string>Point to plane ICP. Only for ICP 3D.</string> <string>Point to plane ICP.</string>
</property> </property>
<property name="wordWrap"> <property name="wordWrap">
<bool>true</bool> <bool>true</bool>