From 2d7be6be4830b6c1974f261065f8982448983b81 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mathieu=20Labb=C3=A9?= Date: Sat, 11 Jul 2015 12:42:46 -0400 Subject: [PATCH] modified how correspondences ratio is computed (icp 2D and 3D), also fixed build with OpenCV3+Cuda --- corelib/include/rtabmap/core/Memory.h | 2 +- .../rtabmap/core/util3d_registration.h | 28 +- corelib/src/CameraRGB.cpp | 2 +- corelib/src/Memory.cpp | 264 +++++++++++------- corelib/src/OdometryICP.cpp | 36 ++- corelib/src/Rtabmap.cpp | 6 +- corelib/src/VWDictionary.cpp | 13 +- corelib/src/util3d_registration.cpp | 258 ++++++----------- guilib/src/DatabaseViewer.cpp | 66 +++-- guilib/src/MainWindow.cpp | 48 ++-- tools/DataRecorder/CMakeLists.txt | 2 + tools/OdometryViewer/CMakeLists.txt | 2 + 12 files changed, 388 insertions(+), 339 deletions(-) diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index be98c74f..f5b334c3 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -272,7 +272,7 @@ private: int _bowRefineIterations; bool _bowForce2D; float _bowEpipolarGeometryVar; - bool _bowEstimationType; + int _bowEstimationType; double _bowPnPReprojError; int _bowPnPFlags; float _icpMaxTranslation; diff --git a/corelib/include/rtabmap/core/util3d_registration.h b/corelib/include/rtabmap/core/util3d_registration.h index c180e3bf..6538ddfb 100644 --- a/corelib/include/rtabmap/core/util3d_registration.h +++ b/corelib/include/rtabmap/core/util3d_registration.h @@ -56,32 +56,42 @@ Transform RTABMAP_EXP transformFromXYZCorrespondences( std::vector * inliers = 0, double * variance = 0); +void RTABMAP_EXP computeVarianceAndCorrespondences( + const pcl::PointCloud::ConstPtr & cloudA, + const pcl::PointCloud::ConstPtr & cloudB, + double maxCorrespondenceDistance, + double & variance, + int & correspondencesOut); +void RTABMAP_EXP computeVarianceAndCorrespondences( + const pcl::PointCloud::ConstPtr & cloudA, + const pcl::PointCloud::ConstPtr & cloudB, + double maxCorrespondenceDistance, + double & variance, + int & correspondencesOut); + Transform RTABMAP_EXP icp( const pcl::PointCloud::ConstPtr & cloud_source, const pcl::PointCloud::ConstPtr & cloud_target, double maxCorrespondenceDistance, int maximumIterations, - bool * hasConverged = 0, - double * variance = 0, - int * correspondences = 0); + bool & hasConverged, + pcl::PointCloud & cloud_source_registered); Transform RTABMAP_EXP icpPointToPlane( const pcl::PointCloud::ConstPtr & cloud_source, const pcl::PointCloud::ConstPtr & cloud_target, double maxCorrespondenceDistance, int maximumIterations, - bool * hasConverged = 0, - double * variance = 0, - int * correspondences = 0); + bool & hasConverged, + pcl::PointCloud & cloud_source_registered); Transform RTABMAP_EXP icp2D( const pcl::PointCloud::ConstPtr & cloud_source, const pcl::PointCloud::ConstPtr & cloud_target, double maxCorrespondenceDistance, int maximumIterations, - bool * hasConverged = 0, - double * variance = 0, - int * correspondences = 0); + bool & hasConverged, + pcl::PointCloud & cloud_source_registered); pcl::PointCloud::Ptr RTABMAP_EXP getICPReadyCloud( const cv::Mat & depth, diff --git a/corelib/src/CameraRGB.cpp b/corelib/src/CameraRGB.cpp index 31c88fd0..55bdd762 100644 --- a/corelib/src/CameraRGB.cpp +++ b/corelib/src/CameraRGB.cpp @@ -115,7 +115,7 @@ unsigned int CameraImages::imagesCount() const { if(_dir) { - return _dir->getFileNames().size(); + return (unsigned int)_dir->getFileNames().size(); } return 0; } diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index deefd3e7..9e743578 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -2348,28 +2348,44 @@ Transform Memory::computeIcpTransform( if(newCloud->size() && oldCloud->size()) { - icpT = util3d::icpPointToPlane(newCloud, + pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud); + icpT = util3d::icpPointToPlane( + newCloud, oldCloud, _icpMaxCorrespondenceDistance, _icpMaxIterations, - &hasConverged, - &variance, - &correspondences); + hasConverged, + *newCloudRegistered); + + util3d::computeVarianceAndCorrespondences( + newCloudRegistered, + oldCloud, + _icpMaxCorrespondenceDistance, + variance, + correspondences); } } else { - icpT = util3d::icp(newCloudXYZ, + pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud); + icpT = util3d::icp( + newCloudXYZ, oldCloudXYZ, _icpMaxCorrespondenceDistance, _icpMaxIterations, - &hasConverged, - &variance, - &correspondences); + hasConverged, + *newCloudRegistered); + + util3d::computeVarianceAndCorrespondences( + newCloudRegistered, + oldCloudXYZ, + _icpMaxCorrespondenceDistance, + variance, + correspondences); } // verify if there are enough correspondences - correspondencesRatio = float(correspondences)/float(newS.sensorData().depthOrRightRaw().total()); + correspondencesRatio = float(correspondences)/float(newCloudXYZ->size()>oldCloudXYZ->size()?newCloudXYZ->size():oldCloudXYZ->size()); UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)", hasConverged?"true":"false", @@ -2456,10 +2472,12 @@ Transform Memory::computeIcpTransform( pcl::PointCloud::Ptr newCloud = util3d::cvMat2Cloud(newS.sensorData().laserScanRaw(), guess); //voxelize + pcl::PointCloud::Ptr oldCloudVoxelized = oldCloud; + pcl::PointCloud::Ptr newCloudVoxelized = newCloud; if(_icp2VoxelSize > _laserScanVoxelSize) { - oldCloud = util3d::voxelize(oldCloud, _icp2VoxelSize); - newCloud = util3d::voxelize(newCloud, _icp2VoxelSize); + oldCloudVoxelized = util3d::voxelize(oldCloud, _icp2VoxelSize); + newCloudVoxelized = util3d::voxelize(newCloud, _icp2VoxelSize); } if(newCloud->size() && oldCloud->size()) @@ -2469,33 +2487,14 @@ Transform Memory::computeIcpTransform( float correspondencesRatio = -1.0f; int correspondences = 0; double variance = 1; - icpT = util3d::icp2D(newCloud, - oldCloud, + pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud()); + icpT = util3d::icp2D( + newCloudVoxelized, + oldCloudVoxelized, _icp2MaxCorrespondenceDistance, _icp2MaxIterations, - &hasConverged, - &variance, - &correspondences); - - // verify if there are enough correspondences - - if(newS.sensorData().laserScanMaxPts()) - { - correspondencesRatio = float(correspondences)/float(newS.sensorData().laserScanMaxPts()); - } - else - { - UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set to 0!", - newS.id()); - } - - UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)", - newS.id(), oldS.id(), - hasConverged?"true":"false", - variance, - correspondences, - (int)(oldCloud->size()>newCloud->size()?oldCloud->size():newCloud->size()), - correspondencesRatio*100.0f); + hasConverged, + *newCloudRegistered); //pcl::io::savePCDFile("oldCloud.pcd", *oldCloud); //pcl::io::savePCDFile("newCloud.pcd", *newCloud); @@ -2507,22 +2506,8 @@ Transform Memory::computeIcpTransform( // UWARN("saved newCloudFinal.pcd"); //} - if(varianceOut) - { - *varianceOut = variance; - } - if(correspondencesOut) - { - *correspondencesOut = correspondences; - } - if(correspondencesRatioOut) - { - *correspondencesRatioOut = correspondencesRatio; - } - if(!icpT.isNull() && - hasConverged && - correspondencesRatio >= _icp2CorrespondenceRatio) + hasConverged) { float ix,iy,iz, iroll,ipitch,iyaw; icpT.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw); @@ -2530,8 +2515,8 @@ Transform Memory::computeIcpTransform( (fabs(ix) > _icpMaxTranslation || fabs(iy) > _icpMaxTranslation || fabs(iz) > _icpMaxTranslation)) - || - (_icpMaxRotation>0.0f && + || + (_icpMaxRotation>0.0f && (fabs(iroll) > _icpMaxRotation || fabs(ipitch) > _icpMaxRotation || fabs(iyaw) > _icpMaxRotation))) @@ -2541,14 +2526,72 @@ Transform Memory::computeIcpTransform( } else { - transform = icpT * guess; - transform = transform.inverse(); + if(_icp2VoxelSize <= _laserScanVoxelSize) + { + newCloud = util3d::transformPointCloud(newCloud, icpT); + } + else + { + newCloud = newCloudRegistered; + } + + pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud); + util3d::computeVarianceAndCorrespondences( + newCloud, + oldCloud, + _icpMaxCorrespondenceDistance, + variance, + correspondences); + + // verify if there are enough correspondences + if(newS.sensorData().laserScanMaxPts()) + { + correspondencesRatio = float(correspondences)/float(newS.sensorData().laserScanMaxPts()); + } + else + { + UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set to 0!", + newS.id()); + } + + UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)", + newS.id(), oldS.id(), + hasConverged?"true":"false", + variance, + correspondences, + (int)(newS.sensorData().laserScanMaxPts()), + correspondencesRatio*100.0f); + + if(varianceOut) + { + *varianceOut = variance; + } + if(correspondencesOut) + { + *correspondencesOut = correspondences; + } + if(correspondencesRatioOut) + { + *correspondencesRatioOut = correspondencesRatio; + } + + if(correspondencesRatio < _icp2CorrespondenceRatio) + { + msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f)", + correspondences, correspondencesRatio, _icp2CorrespondenceRatio); + UINFO(msg.c_str()); + } + else + { + transform = icpT * guess; + transform = transform.inverse(); + } } } else { - msg = uFormat("Cannot compute transform (converged=%s var=%f cor=%d corrRatio=%f/%f)", - hasConverged?"true":"false", variance, correspondences, correspondencesRatio, _icp2CorrespondenceRatio); + msg = uFormat("Cannot compute transform (converged=%s var=%f)", + hasConverged?"true":"false", variance); UINFO(msg.c_str()); } } @@ -2638,49 +2681,28 @@ Transform Memory::computeScanMatchingTransform( newCloud = util3d::cvMat2Cloud(newScan, poses.at(newId)); //voxelize + pcl::PointCloud::Ptr newCloudVoxelized = newCloud; if(newCloud->size() && _icp2VoxelSize > _laserScanVoxelSize) { - newCloud = util3d::voxelize(newCloud, _icp2VoxelSize); + newCloudVoxelized = util3d::voxelize(newCloud, _icp2VoxelSize); } Transform transform; - if(assembledOldClouds->size() && newCloud->size()) + if(assembledOldClouds->size() && newCloudVoxelized->size()) { int correspondences = 0; bool hasConverged = false; - Transform icpT = util3d::icp2D(newCloud, + pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud); + Transform icpT = util3d::icp2D( + newCloudVoxelized, assembledOldClouds, _icp2MaxCorrespondenceDistance, _icp2MaxIterations, - &hasConverged, - variance, - &correspondences); + hasConverged, + *newCloudRegistered); UDEBUG("icpT=%s", icpT.prettyPrint().c_str()); - // verify if there enough correspondences - float correspondencesRatio = 0.0f; - if(newS->sensorData().laserScanMaxPts()) - { - correspondencesRatio = float(correspondences)/float(newS->sensorData().laserScanMaxPts()); - } - else - { - UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set to 0!", - newS->id()); - } - - UDEBUG("variance=%f, correspondences=%d/%d (%f%%) %f", - variance?*variance:-1, - correspondences, - (int)newCloud->size(), - correspondencesRatio*100.0f); - - if(inliers) - { - *inliers = correspondences; - } - //pcl::io::savePCDFile("old.pcd", *assembledOldClouds, true); //pcl::io::savePCDFile("new.pcd", *newCloud, true); //UWARN("local scan matching old.pcd, new.pcd saved!"); @@ -2691,19 +2713,71 @@ Transform Memory::computeScanMatchingTransform( // UWARN("local scan matching newFinal.pcd saved!"); //} - if(!icpT.isNull() && hasConverged && - correspondencesRatio >= _icp2CorrespondenceRatio) + if(!icpT.isNull() && hasConverged) { - transform = poses.at(newId).inverse()*icpT.inverse() * poses.at(oldId); + if(_icp2VoxelSize <= _laserScanVoxelSize) + { + newCloud = util3d::transformPointCloud(newCloud, icpT); + } + else + { + newCloud = newCloudRegistered; + } + + pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud); + double v = 1; + util3d::computeVarianceAndCorrespondences( + newCloud, + assembledOldClouds, + _icpMaxCorrespondenceDistance, + v, + correspondences); + if(variance) + { + *variance = v; + } + + // verify if there enough correspondences + float correspondencesRatio = 0.0f; + if(newS->sensorData().laserScanMaxPts()) + { + correspondencesRatio = float(correspondences)/float(newS->sensorData().laserScanMaxPts()); + } + else + { + UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set to 0!", + newS->id()); + } + + UDEBUG("variance=%f, correspondences=%d/%d (%f%%) %f", + variance?*variance:-1, + correspondences, + (int)newCloud->size(), + correspondencesRatio*100.0f); + + if(inliers) + { + *inliers = correspondences; + } + + if(correspondencesRatio >= _icp2CorrespondenceRatio) + { + transform = poses.at(newId).inverse()*icpT.inverse() * poses.at(oldId); + } + else + { + msg = uFormat("Constraints failed... variance=%f, correspondences=%d/%d (%f%%)", + variance?*variance:-1, + correspondences, + (int)newCloud->size(), + correspondencesRatio); + UINFO(msg.c_str()); + } } else { - msg = uFormat("Constraints failed... hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)", - hasConverged?"true":"false", - variance?*variance:-1, - correspondences, - (int)newCloud->size(), - correspondencesRatio); + msg = uFormat("Constraints failed... hasConverged=%s", + hasConverged?"true":"false"); UINFO(msg.c_str()); } } diff --git a/corelib/src/OdometryICP.cpp b/corelib/src/OdometryICP.cpp index 2ca731a7..90af92df 100644 --- a/corelib/src/OdometryICP.cpp +++ b/corelib/src/OdometryICP.cpp @@ -112,14 +112,22 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo * if(_previousCloudNormal->size() > minPoints && newCloud->size() > minPoints) { - int correspondences = 0; - Transform transform = util3d::icpPointToPlane(newCloud, + pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud); + Transform transform = util3d::icpPointToPlane( + newCloud, _previousCloudNormal, _maxCorrespondenceDistance, _maxIterations, - &hasConverged, - &variance, - &correspondences); + hasConverged, + *newCloudRegistered); + + int correspondences = 0; + util3d::computeVarianceAndCorrespondences( + newCloudRegistered, + _previousCloudNormal, + _maxCorrespondenceDistance, + variance, + correspondences); // verify if there are enough correspondences float correspondencesRatio = float(correspondences)/float(_previousCloudNormal->size()>newCloud->size()?_previousCloudNormal->size():newCloud->size()); @@ -147,14 +155,22 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo * //point to point if(_previousCloud->size() > minPoints && newCloudXYZ->size() > minPoints) { - int correspondences = 0; - Transform transform = util3d::icp(newCloudXYZ, + pcl::PointCloud::Ptr newCloudRegistered(new pcl::PointCloud); + Transform transform = util3d::icp( + newCloudXYZ, _previousCloud, _maxCorrespondenceDistance, _maxIterations, - &hasConverged, - &variance, - &correspondences); + hasConverged, + *newCloudRegistered); + + int correspondences = 0; + util3d::computeVarianceAndCorrespondences( + newCloudRegistered, + _previousCloud, + _maxCorrespondenceDistance, + variance, + correspondences); // verify if there are enough correspondences float correspondencesRatio = float(correspondences)/float(_previousCloud->size()>newCloudXYZ->size()?_previousCloud->size():newCloudXYZ->size()); diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index d6a0b3c5..3cae0da2 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -1553,7 +1553,7 @@ bool Rtabmap::process( iter!=retrievalLocalIds.end() && retrievalLocalIds.size() < _maxLocalRetrieved; ++iter) { - std::map ids = _memory->getNeighborsId(*iter, 2, _maxLocalRetrieved - retrievalLocalIds.size() + 1, true, false); + std::map ids = _memory->getNeighborsId(*iter, 2, _maxLocalRetrieved - (unsigned int)retrievalLocalIds.size() + 1, true, false); for(std::map::reverse_iterator jter=ids.rbegin(); jter!=ids.rend() && retrievalLocalIds.size() < _maxLocalRetrieved; ++jter) @@ -2311,7 +2311,7 @@ bool Rtabmap::process( statistics_.setConstraints(constraints); statistics_.setSignatures(signatures); statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), poses.size()); - localGraphSize = poses.size(); + localGraphSize = (int)poses.size(); } //Start trashing @@ -3053,7 +3053,7 @@ bool Rtabmap::computePath(int targetNode, bool global) { // set goal to latest signature std::string goalStr = uFormat("GOAL:%d", targetNode); - setUserData(0, cv::Mat(1, goalStr.size()+1, CV_8SC1, (void *)goalStr.c_str()).clone()); + setUserData(0, cv::Mat(1, int(goalStr.size()+1), CV_8SC1, (void *)goalStr.c_str()).clone()); } updateGoalIndex(); } diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp index a8242105..fd9fee37 100644 --- a/corelib/src/VWDictionary.cpp +++ b/corelib/src/VWDictionary.cpp @@ -506,15 +506,16 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptors, #ifdef HAVE_OPENCV_CUDAFEATURES2D cv::cuda::GpuMat newDescriptorsGpu(descriptors); cv::cuda::GpuMat lastDescriptorsGpu(_dataTree); + cv::Ptr gpuMatcher; if(type==CV_8U) { - cv::cuda::BruteForceMatcher_GPU gpuMatcher; - gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k); + gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_HAMMING); + gpuMatcher->knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k); } else { - cv::cuda::BruteForceMatcher_GPU > gpuMatcher; - gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k); + gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_L2); + gpuMatcher->knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k); } #endif #endif @@ -742,12 +743,12 @@ std::vector VWDictionary::findNN(const std::list & vws) const if(type==CV_8U) { gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_HAMMING); - gpuMatcher->knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k); + gpuMatcher->knnMatchAsync(newDescriptorsGpu, lastDescriptorsGpu, matches, k); } else { gpuMatcher = cv::cuda::DescriptorMatcher::createBFMatcher(cv::NORM_L2); - gpuMatcher.knnMatch(newDescriptorsGpu, lastDescriptorsGpu, matches, k); + gpuMatcher->knnMatchAsync(newDescriptorsGpu, lastDescriptorsGpu, matches, k); } #endif #endif diff --git a/corelib/src/util3d_registration.cpp b/corelib/src/util3d_registration.cpp index eefe9d5b..d3247b90 100644 --- a/corelib/src/util3d_registration.cpp +++ b/corelib/src/util3d_registration.cpp @@ -222,14 +222,79 @@ Transform transformFromXYZCorrespondences( return Transform(); } +void computeVarianceAndCorrespondences( + const pcl::PointCloud::ConstPtr & cloudA, + const pcl::PointCloud::ConstPtr & cloudB, + double maxCorrespondenceDistance, + double & variance, + int & correspondencesOut) +{ + variance = 1; + correspondencesOut = 0; + pcl::registration::CorrespondenceEstimation::Ptr est; + est.reset(new pcl::registration::CorrespondenceEstimation); + est->setInputTarget(cloudA); + est->setInputSource(cloudB); + pcl::Correspondences correspondences; + est->determineCorrespondences(correspondences, maxCorrespondenceDistance); + + if(correspondences.size()>=3) + { + std::vector distances(correspondences.size()); + for(unsigned int i=0; i> 1]; + variance = (2.1981 * median_error_sqr); + } + + correspondencesOut = (int)correspondences.size(); +} + +void computeVarianceAndCorrespondences( + const pcl::PointCloud::ConstPtr & cloudA, + const pcl::PointCloud::ConstPtr & cloudB, + double maxCorrespondenceDistance, + double & variance, + int & correspondencesOut) +{ + variance = 1; + correspondencesOut = 0; + pcl::registration::CorrespondenceEstimation::Ptr est; + est.reset(new pcl::registration::CorrespondenceEstimation); + est->setInputTarget(cloudA); + est->setInputSource(cloudB); + pcl::Correspondences correspondences; + est->determineCorrespondences(correspondences, maxCorrespondenceDistance); + + if(correspondences.size()>=3) + { + std::vector distances(correspondences.size()); + for(unsigned int i=0; i> 1]; + variance = (2.1981 * median_error_sqr); + } + + correspondencesOut = (int)correspondences.size(); +} + // return transform from source to target (All points must be finite!!!) Transform icp(const pcl::PointCloud::ConstPtr & cloud_source, const pcl::PointCloud::ConstPtr & cloud_target, double maxCorrespondenceDistance, int maximumIterations, - bool * hasConvergedOut, - double * variance, - int * correspondencesOut) + bool & hasConverged, + pcl::PointCloud & cloud_source_registered) { pcl::IterativeClosestPoint icp; // Set the input source and target @@ -247,63 +312,8 @@ Transform icp(const pcl::PointCloud::ConstPtr & cloud_source, //icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance); // Perform the alignment - pcl::PointCloud::Ptr cloud_source_registered(new pcl::PointCloud); - icp.align (*cloud_source_registered); - bool hasConverged = icp.hasConverged(); - - // compute variance - if((correspondencesOut || variance) && hasConverged) - { - pcl::registration::CorrespondenceEstimation::Ptr est; - est.reset(new pcl::registration::CorrespondenceEstimation); - est->setInputTarget(cloud_target); - est->setInputSource(cloud_source_registered); - pcl::Correspondences correspondences; - est->determineCorrespondences(correspondences, maxCorrespondenceDistance); - if(variance) - { - if(correspondences.size()>=3) - { - std::vector distances(correspondences.size()); - for(unsigned int i=0; i> 1]; - *variance = (2.1981 * median_error_sqr); - } - else - { - hasConverged = false; - *variance = -1.0; - } - } - - if(correspondencesOut) - { - *correspondencesOut = (int)correspondences.size(); - } - } - else - { - if(correspondencesOut) - { - *correspondencesOut = 0; - } - if(variance) - { - *variance = -1; - } - } - - if(hasConvergedOut) - { - *hasConvergedOut = hasConverged; - } - + icp.align (cloud_source_registered); + hasConverged = icp.hasConverged(); return Transform::fromEigen4f(icp.getFinalTransformation()); } @@ -313,9 +323,8 @@ Transform icpPointToPlane( const pcl::PointCloud::ConstPtr & cloud_target, double maxCorrespondenceDistance, int maximumIterations, - bool * hasConvergedOut, - double * variance, - int * correspondencesOut) + bool & hasConverged, + pcl::PointCloud & cloud_source_registered) { pcl::IterativeClosestPoint icp; // Set the input source and target @@ -337,63 +346,8 @@ Transform icpPointToPlane( //icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance); // Perform the alignment - pcl::PointCloud::Ptr cloud_source_registered(new pcl::PointCloud); - icp.align (*cloud_source_registered); - bool hasConverged = icp.hasConverged(); - - // compute variance - if((correspondencesOut || variance) && hasConverged) - { - pcl::registration::CorrespondenceEstimation::Ptr est; - est.reset(new pcl::registration::CorrespondenceEstimation); - est->setInputTarget(cloud_target); - est->setInputSource(cloud_source_registered); - pcl::Correspondences correspondences; - est->determineCorrespondences(correspondences, maxCorrespondenceDistance); - if(variance) - { - if(correspondences.size()>=3) - { - std::vector distances(correspondences.size()); - for(unsigned int i=0; i> 1]; - *variance = (2.1981 * median_error_sqr); - } - else - { - hasConverged = false; - *variance = -1.0; - } - } - - if(correspondencesOut) - { - *correspondencesOut = (int)correspondences.size(); - } - } - else - { - if(correspondencesOut) - { - *correspondencesOut = 0; - } - if(variance) - { - *variance = -1; - } - } - - if(hasConvergedOut) - { - *hasConvergedOut = hasConverged; - } - + icp.align (cloud_source_registered); + hasConverged = icp.hasConverged(); return Transform::fromEigen4f(icp.getFinalTransformation()); } @@ -402,9 +356,8 @@ Transform icp2D(const pcl::PointCloud::ConstPtr & cloud_source, const pcl::PointCloud::ConstPtr & cloud_target, double maxCorrespondenceDistance, int maximumIterations, - bool * hasConvergedOut, - double * variance, - int * correspondencesOut) + bool & hasConverged, + pcl::PointCloud & cloud_source_registered) { pcl::IterativeClosestPoint icp; // Set the input source and target @@ -426,63 +379,8 @@ Transform icp2D(const pcl::PointCloud::ConstPtr & cloud_source, //icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance); // Perform the alignment - pcl::PointCloud::Ptr cloud_source_registered(new pcl::PointCloud); - icp.align (*cloud_source_registered); - bool hasConverged = icp.hasConverged(); - - // compute variance - if((correspondencesOut || variance) && hasConverged) - { - pcl::registration::CorrespondenceEstimation::Ptr est; - est.reset(new pcl::registration::CorrespondenceEstimation); - est->setInputTarget(cloud_target); - est->setInputSource(cloud_source_registered); - pcl::Correspondences correspondences; - est->determineCorrespondences(correspondences, maxCorrespondenceDistance); - if(variance) - { - if(correspondences.size()>=3) - { - std::vector distances(correspondences.size()); - for(unsigned int i=0; i> 1]; - *variance = (2.1981 * median_error_sqr); - } - else - { - hasConverged = false; - *variance = -1.0; - } - } - - if(correspondencesOut) - { - *correspondencesOut = (int)correspondences.size(); - } - } - else - { - if(correspondencesOut) - { - *correspondencesOut = 0; - } - if(variance) - { - *variance = -1; - } - } - - if(hasConvergedOut) - { - *hasConvergedOut = hasConverged; - } - + icp.align (cloud_source_registered); + hasConverged = icp.hasConverged(); return Transform::fromEigen4f(icp.getFinalTransformation()); } diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 6556f70a..1fa32b63 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -2764,8 +2764,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update pcl::PointCloud::Ptr cloudA(new pcl::PointCloud); pcl::PointCloud::Ptr cloudB(new pcl::PointCloud); - pcl::PointCloud::Ptr scanA(new pcl::PointCloud); - pcl::PointCloud::Ptr scanB(new pcl::PointCloud); + pcl::PointCloud::Ptr scanAVoxelized(new pcl::PointCloud); + pcl::PointCloud::Ptr scanBVoxelized(new pcl::PointCloud); float correspondenceRatio = 0.0f; if(ui_->checkBox_icp_2d->isChecked()) { @@ -2776,6 +2776,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update if(!oldLaserScan.empty() && !newLaserScan.empty()) { // 2D + pcl::PointCloud::Ptr scanA(new pcl::PointCloud); + pcl::PointCloud::Ptr scanB(new pcl::PointCloud); scanA = util3d::cvMat2Cloud(oldLaserScan); scanB = util3d::cvMat2Cloud(newLaserScan, t); @@ -2785,21 +2787,40 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update scanA = util3d::voxelize(scanA, ui_->doubleSpinBox_icp_voxel->value()); scanB = util3d::voxelize(scanB, ui_->doubleSpinBox_icp_voxel->value()); } + else + { + scanAVoxelized = scanA; + scanBVoxelized = scanB; + } if(scanB->size() && scanA->size()) { - transform = util3d::icp2D(scanB, + pcl::PointCloud::Ptr scanBRegistered(new pcl::PointCloud); + transform = util3d::icp2D( + scanB, scanA, ui_->doubleSpinBox_icp_maxCorrespDistance->value(), ui_->spinBox_icp_iteration->value(), - &hasConverged, - &variance, - &correspondences); + hasConverged, + *scanBRegistered); if(!transform.isNull()) { if(dataTo.laserScanMaxPts()) { + pcl::PointCloud::Ptr scanBTransformed = scanBRegistered; + if(ui_->doubleSpinBox_icp_voxel->value() > 0.0f) + { + scanBTransformed = util3d::transformPointCloud(scanB, transform); + } + + util3d::computeVarianceAndCorrespondences( + scanBTransformed, + scanA, + ui_->doubleSpinBox_icp_maxCorrespDistance->value(), + variance, + correspondences); + correspondenceRatio = float(correspondences)/float(dataTo.laserScanMaxPts()); } else if(ui_->doubleSpinBox_icp_minCorrespondenceRatio->value()) @@ -2844,25 +2865,38 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update UWARN("removed nan normals..."); } - transform = util3d::icpPointToPlane(cloudBNormals, + pcl::PointCloud::Ptr cloudBRegistered(new pcl::PointCloud); + transform = util3d::icpPointToPlane( + cloudBNormals, cloudANormals, ui_->doubleSpinBox_icp_maxCorrespDistance->value(), ui_->spinBox_icp_iteration->value(), - &hasConverged, - &variance, - &correspondences); + hasConverged, + *cloudBRegistered); + util3d::computeVarianceAndCorrespondences( + cloudBRegistered, + cloudANormals, + ui_->doubleSpinBox_icp_maxCorrespDistance->value(), + variance, + correspondences); } else { + pcl::PointCloud::Ptr cloudBRegistered(new pcl::PointCloud); transform = util3d::icp(cloudB, cloudA, ui_->doubleSpinBox_icp_maxCorrespDistance->value(), ui_->spinBox_icp_iteration->value(), - &hasConverged, - &variance, - &correspondences); + hasConverged, + *cloudBRegistered); + util3d::computeVarianceAndCorrespondences( + cloudBRegistered, + cloudA, + ui_->doubleSpinBox_icp_maxCorrespDistance->value(), + variance, + correspondences); } - correspondenceRatio = float(correspondences)/float(dataFrom.imageRaw().total()); + correspondenceRatio = float(correspondences)/float(cloudA->size()>cloudB->size()?cloudA->size():cloudB->size()); } else { @@ -2913,8 +2947,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update if(ui_->dockWidget_constraints->isVisible()) { cloudB = util3d::transformPointCloud(cloudB, transform); - scanB = util3d::transformPointCloud(scanB, transform); - this->updateConstraintView(newLink, true, cloudA, cloudB, scanA, scanB); + scanBVoxelized = util3d::transformPointCloud(scanBVoxelized, transform); + this->updateConstraintView(newLink, true, cloudA, cloudB, scanAVoxelized, scanBVoxelized); } } } diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index b81db148..07668bea 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -3505,22 +3505,22 @@ void MainWindow::postProcessing() } _initProgressDialog->appendText(tr("Refining links...")); - int decimation=8; - float maxDepth=2.0f; - float voxelSize=0.01f; - int samples = 0; - float maxCorrespondences = 0.05f; - float correspondenceRatio = 0.7f; - float icpIterations = 30; + int decimation=Parameters::defaultLccIcp3Decimation(); + float maxDepth=Parameters::defaultLccIcp3MaxDepth(); + float voxelSize=Parameters::defaultLccIcp3VoxelSize(); + int samples = Parameters::defaultLccIcp3Samples(); + float maxCorrespondenceDistance = Parameters::defaultLccIcp3MaxCorrespondenceDistance(); + float correspondenceRatio = Parameters::defaultLccIcp3CorrespondenceRatio(); + float icpIterations = Parameters::defaultLccIcp3Iterations(); Parameters::parse(parameters, Parameters::kLccIcp3Decimation(), decimation); Parameters::parse(parameters, Parameters::kLccIcp3MaxDepth(), maxDepth); Parameters::parse(parameters, Parameters::kLccIcp3VoxelSize(), voxelSize); Parameters::parse(parameters, Parameters::kLccIcp3Samples(), samples); Parameters::parse(parameters, Parameters::kLccIcp3CorrespondenceRatio(), correspondenceRatio); - Parameters::parse(parameters, Parameters::kLccIcp3MaxCorrespondenceDistance(), maxCorrespondences); + Parameters::parse(parameters, Parameters::kLccIcp3MaxCorrespondenceDistance(), maxCorrespondenceDistance); Parameters::parse(parameters, Parameters::kLccIcp3Iterations(), icpIterations); - bool pointToPlane = false; - int pointToPlaneNormalNeighbors = 20; + bool pointToPlane = Parameters::defaultLccIcp3PointToPlane(); + int pointToPlaneNormalNeighbors = Parameters::defaultLccIcp3PointToPlaneNormalNeighbors(); Parameters::parse(parameters, Parameters::kLccIcp3PointToPlane(), pointToPlane); Parameters::parse(parameters, Parameters::kLccIcp3PointToPlaneNormalNeighbors(), pointToPlaneNormalNeighbors); @@ -3605,24 +3605,36 @@ void MainWindow::postProcessing() UWARN("removed nan normals..."); } + pcl::PointCloud::Ptr cloudBRegistered(new pcl::PointCloud); transform = util3d::icpPointToPlane(cloudBNormals, cloudANormals, - maxCorrespondences, + maxCorrespondenceDistance, icpIterations, - &hasConverged, - &variance, - &correspondences); + hasConverged, + *cloudBRegistered); + util3d::computeVarianceAndCorrespondences( + cloudBRegistered, + cloudANormals, + maxCorrespondenceDistance, + variance, + correspondences); } else { UDEBUG(""); + pcl::PointCloud::Ptr cloudBRegistered(new pcl::PointCloud); transform = util3d::icp(cloudB, cloudA, - maxCorrespondences, + maxCorrespondenceDistance, icpIterations, - &hasConverged, - &variance, - &correspondences); + hasConverged, + *cloudBRegistered); + util3d::computeVarianceAndCorrespondences( + cloudBRegistered, + cloudA, + maxCorrespondenceDistance, + variance, + correspondences); } float correspondencesRatio = float(correspondences)/float(cloudB->size()>cloudA->size()?cloudB->size():cloudA->size()); diff --git a/tools/DataRecorder/CMakeLists.txt b/tools/DataRecorder/CMakeLists.txt index 2f85242f..d7361db8 100644 --- a/tools/DataRecorder/CMakeLists.txt +++ b/tools/DataRecorder/CMakeLists.txt @@ -8,6 +8,7 @@ SET(INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/utilite/include ${PROJECT_SOURCE_DIR}/guilib/include ${CMAKE_CURRENT_SOURCE_DIR} + ${OpenCV_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ) @@ -16,6 +17,7 @@ IF("${RTABMAP_QT_VERSION}" STREQUAL "4") ENDIF() SET(LIBRARIES + ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} ${QT_LIBRARIES} ) diff --git a/tools/OdometryViewer/CMakeLists.txt b/tools/OdometryViewer/CMakeLists.txt index dfa5f316..3ec03e4a 100644 --- a/tools/OdometryViewer/CMakeLists.txt +++ b/tools/OdometryViewer/CMakeLists.txt @@ -4,6 +4,7 @@ SET(INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/utilite/include ${PROJECT_SOURCE_DIR}/guilib/include ${CMAKE_CURRENT_SOURCE_DIR} + ${OpenCV_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ) @@ -12,6 +13,7 @@ IF("${RTABMAP_QT_VERSION}" STREQUAL "4") ENDIF() SET(LIBRARIES + ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} ${QT_LIBRARIES} )