From 52a4e8964fdb8a19a5cdaa4a432f5259fa2b8cf9 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 25 Aug 2017 18:02:40 -0400 Subject: [PATCH 01/17] 0.13.3: scan2d with normals support/registration --- CMakeLists.txt | 2 +- corelib/include/rtabmap/core/CameraRGB.h | 5 +- corelib/include/rtabmap/core/CameraThread.h | 5 +- corelib/include/rtabmap/core/Memory.h | 1 + corelib/include/rtabmap/core/Parameters.h | 6 +- .../include/rtabmap/core/RegistrationIcp.h | 3 +- corelib/include/rtabmap/core/util3d.h | 2 + .../include/rtabmap/core/util3d_filtering.h | 6 + corelib/include/rtabmap/core/util3d_surface.h | 32 +- corelib/src/CameraRGB.cpp | 6 +- corelib/src/CameraThread.cpp | 5 +- corelib/src/Memory.cpp | 13 +- corelib/src/Parameters.cpp | 6 +- corelib/src/RegistrationIcp.cpp | 60 +++- corelib/src/util3d.cpp | 40 +++ corelib/src/util3d_filtering.cpp | 20 ++ corelib/src/util3d_surface.cpp | 221 ++++++++++++- guilib/include/rtabmap/gui/CloudViewer.h | 11 + .../include/rtabmap/gui/PreferencesDialog.h | 3 + guilib/src/CloudViewer.cpp | 166 ++++++++-- guilib/src/ExportCloudsDialog.cpp | 14 +- guilib/src/MainWindow.cpp | 250 ++++++++------ guilib/src/PreferencesDialog.cpp | 37 ++- guilib/src/ui/exportCloudsDialog.ui | 107 +++--- guilib/src/ui/preferencesDialog.ui | 313 ++++++++++++------ package.xml | 2 +- tools/KittiDataset/main.cpp | 21 +- tools/OdometryViewer/main.cpp | 9 +- 28 files changed, 1029 insertions(+), 337 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bb96ce0f..2c4951d5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") ####################### SET(RTABMAP_MAJOR_VERSION 0) SET(RTABMAP_MINOR_VERSION 13) -SET(RTABMAP_PATCH_VERSION 2) +SET(RTABMAP_PATCH_VERSION 3) SET(RTABMAP_VERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) diff --git a/corelib/include/rtabmap/core/CameraRGB.h b/corelib/include/rtabmap/core/CameraRGB.h index 8d624164..4fa68b8b 100644 --- a/corelib/include/rtabmap/core/CameraRGB.h +++ b/corelib/include/rtabmap/core/CameraRGB.h @@ -85,7 +85,8 @@ public: int maxScanPts = 0, int downsampleStep = 1, float voxelSize = 0.0f, - int normalsK = 0, // compute normals if > 0 + int normalsK = 0, // compute normals if > 0 + float normalsRadius = 0, // compute normals if > 0 const Transform & localTransform=Transform::getIdentity()) { _scanPath = dir; @@ -93,6 +94,7 @@ public: _scanMaxPts = maxScanPts; _scanDownsampleStep = downsampleStep; _scanNormalsK = normalsK; + _scanNormalsRadius = normalsRadius; _scanVoxelSize = voxelSize; if(_scanDownsampleStep>1) { @@ -158,6 +160,7 @@ private: int _scanDownsampleStep; float _scanVoxelSize; int _scanNormalsK; + float _scanNormalsRadius; bool _depthFromScan; int _depthFromScanFillHoles; // <0:horizontal 0:disabled >0:vertical diff --git a/corelib/include/rtabmap/core/CameraThread.h b/corelib/include/rtabmap/core/CameraThread.h index 61cd1366..1c5bebaa 100644 --- a/corelib/include/rtabmap/core/CameraThread.h +++ b/corelib/include/rtabmap/core/CameraThread.h @@ -73,13 +73,15 @@ public: int decimation=4, float maxDepth=4.0f, float voxelSize = 0.0f, - int normalsK = 0) + int normalsK = 0, + int normalsRadius = 0.0f) { _scanFromDepth = enabled; _scanDecimation=decimation; _scanMaxDepth = maxDepth; _scanVoxelSize = voxelSize; _scanNormalsK = normalsK; + _scanNormalsRadius = normalsRadius; } void postUpdate(SensorData * data, CameraInfo * info = 0) const; @@ -107,6 +109,7 @@ private: float _scanMinDepth; float _scanVoxelSize; int _scanNormalsK; + float _scanNormalsRadius; StereoDense * _stereoDense; clams::DiscreteDepthDistortionModel * _distortionModel; bool _bilateralFiltering; diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index 93d4953e..4f418879 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -283,6 +283,7 @@ private: bool _compressionParallelized; float _laserScanDownsampleStepSize; int _laserScanNormalK; + int _laserScanNormalRadius; bool _reextractLoopClosureFeatures; float _rehearsalMaxDistance; float _rehearsalMaxAngle; diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index affcb602..5a17768d 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -211,7 +211,8 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Mem, ImagePostDecimation, int, 1, "Image decimation (>=1) of saved data in created signatures (after features extraction). Decimation is done from the original image. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value)."); RTABMAP_PARAM(Mem, CompressionParallelized, bool, true, "Compression of sensor data is multi-threaded."); RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1, "If > 1, downsample the laser scans when creating a signature."); - RTABMAP_PARAM(Mem, LaserScanNormalK, int, 0, "If > 0 and laser scans are 3D without normals, normals will be computed with K search neighbors when creating a signature."); + RTABMAP_PARAM(Mem, LaserScanNormalK, int, 0, "If > 0 and laser scans don't have normals, normals will be computed with K search neighbors when creating a signature."); + RTABMAP_PARAM(Mem, LaserScanNormalRadius, int, 0, "If > 0 and laser scans don't have normals, normals will be computed with radius search neighbors when creating a signature."); RTABMAP_PARAM(Mem, UseOdomFeatures, bool, false, "Use odometry features."); // KeypointMemory (Keypoint-based) @@ -510,7 +511,8 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Icp, Epsilon, float, 0, "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution."); RTABMAP_PARAM(Icp, CorrespondenceRatio, float, 0.2, "Ratio of matching correspondences to accept the transform."); RTABMAP_PARAM(Icp, PointToPlane, bool, false, "Use point to plane ICP."); - RTABMAP_PARAM(Icp, PointToPlaneNormalNeighbors, int, 20, "Number of neighbors to compute normals for point to plane."); + RTABMAP_PARAM(Icp, PointToPlaneK, int, 20, "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals."); + RTABMAP_PARAM(Icp, PointToPlaneRadius, float, 0.0, "Search radius to compute normals for point to plane if the cloud doesn't have already normals."); // libpointmatcher RTABMAP_PARAM(Icp, PM, bool, false, "Use libpointmatcher for ICP registration instead of PCL's implementation."); diff --git a/corelib/include/rtabmap/core/RegistrationIcp.h b/corelib/include/rtabmap/core/RegistrationIcp.h index 546292c7..51960421 100644 --- a/corelib/include/rtabmap/core/RegistrationIcp.h +++ b/corelib/include/rtabmap/core/RegistrationIcp.h @@ -64,7 +64,8 @@ private: float _epsilon; float _correspondenceRatio; bool _pointToPlane; - int _pointToPlaneNormalNeighbors; + int _pointToPlaneK; + float _pointToPlaneRadius; bool _libpointmatcher; std::string _libpointmatcherConfig; float _libpointmatcherOutlierRatio; diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h index 73a149ad..87233169 100644 --- a/corelib/include/rtabmap/core/util3d.h +++ b/corelib/include/rtabmap/core/util3d.h @@ -198,6 +198,8 @@ cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud // return CV_32FC4 (x,y,z,rgb) cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform()); // return CV_32FC7 (x,y,z,rgb,normal_z,normal_y,normalz) +cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::PointCloud & normals, const Transform & transform = Transform()); +// return CV_32FC7 (x,y,z,rgb,normal_z,normal_y,normalz) cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform()); // return CV_32FC2 (x,y) cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform()); diff --git a/corelib/include/rtabmap/core/util3d_filtering.h b/corelib/include/rtabmap/core/util3d_filtering.h index ad8569e5..e71338dc 100644 --- a/corelib/include/rtabmap/core/util3d_filtering.h +++ b/corelib/include/rtabmap/core/util3d_filtering.h @@ -141,6 +141,12 @@ pcl::PointCloud::Ptr RTABMAP_EXP passThrough( float min, float max, bool negative = false); +pcl::PointCloud::Ptr RTABMAP_EXP passThrough( + const pcl::PointCloud::Ptr & cloud, + const std::string & axis, + float min, + float max, + bool negative = false); pcl::IndicesPtr RTABMAP_EXP cropBox( const pcl::PointCloud::Ptr & cloud, diff --git a/corelib/include/rtabmap/core/util3d_surface.h b/corelib/include/rtabmap/core/util3d_surface.h index 3ebbce7f..4c43e078 100644 --- a/corelib/include/rtabmap/core/util3d_surface.h +++ b/corelib/include/rtabmap/core/util3d_surface.h @@ -212,32 +212,50 @@ cv::Mat RTABMAP_EXP mergeTextures( bool exposureFusion = false, //Exposure fusion can be used only with OpenCV3 const ProgressState * state = 0); - +cv::Mat RTABMAP_EXP computeNormals( + const cv::Mat & laserScan, + int searchK, + float searchRadius); pcl::PointCloud::Ptr RTABMAP_EXP computeNormals( const pcl::PointCloud::Ptr & cloud, - int normalKSearch = 20, + int searchK = 20, + float searchRadius = 0.0f, const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); pcl::PointCloud::Ptr RTABMAP_EXP computeNormals( const pcl::PointCloud::Ptr & cloud, - int normalKSearch = 20, + int searchK = 20, + float searchRadius = 0.0f, const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); pcl::PointCloud::Ptr RTABMAP_EXP computeNormals( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, - int normalKSearch = 20, + int searchK = 20, + float searchRadius = 0.0f, const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); pcl::PointCloud::Ptr RTABMAP_EXP computeNormals( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, - int normalKSearch = 20, + int searchK = 20, + float searchRadius = 0.0f, const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); -pcl::PointCloud::Ptr computeFastOrganizedNormals( +pcl::PointCloud::Ptr RTABMAP_EXP computeNormals2D( + const pcl::PointCloud::Ptr & cloud, + int searchK = 5, + float searchRadius = 0.0f, + const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); +pcl::PointCloud::Ptr RTABMAP_EXP computeFastOrganizedNormals2D( + const pcl::PointCloud::Ptr & cloud, + int searchK = 5, + float searchRadius = 0.0f, + const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); + +pcl::PointCloud::Ptr RTABMAP_EXP computeFastOrganizedNormals( const pcl::PointCloud::Ptr & cloud, float maxDepthChangeFactor = 0.02f, float normalSmoothingSize = 10.0f, const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); -pcl::PointCloud::Ptr computeFastOrganizedNormals( +pcl::PointCloud::Ptr RTABMAP_EXP computeFastOrganizedNormals( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, float maxDepthChangeFactor = 0.02f, diff --git a/corelib/src/CameraRGB.cpp b/corelib/src/CameraRGB.cpp index de9b6258..232fb30a 100644 --- a/corelib/src/CameraRGB.cpp +++ b/corelib/src/CameraRGB.cpp @@ -70,6 +70,7 @@ CameraImages::CameraImages() : _scanDownsampleStep(1), _scanVoxelSize(0.0f), _scanNormalsK(0), + _scanNormalsRadius(0), _depthFromScan(false), _depthFromScanFillHoles(1), _depthFromScanFillHolesFromBorder(false), @@ -99,6 +100,7 @@ CameraImages::CameraImages(const std::string & path, _scanDownsampleStep(1), _scanVoxelSize(0.0f), _scanNormalsK(0), + _scanNormalsRadius(0), _depthFromScan(false), _depthFromScanFillHoles(1), _depthFromScanFillHolesFromBorder(false), @@ -685,9 +687,9 @@ SensorData CameraImages::captureImage(CameraInfo * info) cloud = util3d::voxelize(cloud, _scanVoxelSize); UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d", _scanVoxelSize, previousSize, (int)cloud->size()); } - if(_scanNormalsK > 0 && cloud->size()) + if((_scanNormalsK > 0 || _scanNormalsRadius) && cloud->size()) { - pcl::PointCloud::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK); + pcl::PointCloud::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, _scanNormalsRadius); pcl::PointCloud::Ptr cloudNormals(new pcl::PointCloud); pcl::concatenateFields(*cloud, *normals, *cloudNormals); scan = util3d::laserScanFromPointCloud(*cloudNormals, _scanLocalTransform.inverse()); diff --git a/corelib/src/CameraThread.cpp b/corelib/src/CameraThread.cpp index 1c1e5b1c..ce20bfd2 100644 --- a/corelib/src/CameraThread.cpp +++ b/corelib/src/CameraThread.cpp @@ -58,6 +58,7 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) : _scanMinDepth(0.0f), _scanVoxelSize(0.0f), _scanNormalsK(0), + _scanNormalsRadius(0.0f), _stereoDense(new StereoBM(parameters)), _distortionModel(0), _bilateralFiltering(false), @@ -323,10 +324,10 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const if(cloud->size()) { - if(_scanNormalsK>0) + if(_scanNormalsK>0 || _scanNormalsRadius>0.0f) { Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z()); - pcl::PointCloud::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, viewPoint); + pcl::PointCloud::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, _scanNormalsRadius, viewPoint); pcl::PointCloud::Ptr cloudNormals(new pcl::PointCloud); pcl::concatenateFields(*cloud, *normals, *cloudNormals); scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse()); diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index c0367929..2e4abb9d 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -89,6 +89,7 @@ Memory::Memory(const ParametersMap & parameters) : _compressionParallelized(Parameters::defaultMemCompressionParallelized()), _laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()), _laserScanNormalK(Parameters::defaultMemLaserScanNormalK()), + _laserScanNormalRadius(Parameters::defaultMemLaserScanNormalRadius()), _reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()), _rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()), _rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()), @@ -440,6 +441,7 @@ void Memory::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kMemCompressionParallelized(), _compressionParallelized); Parameters::parse(parameters, Parameters::kMemLaserScanDownsampleStepSize(), _laserScanDownsampleStepSize); Parameters::parse(parameters, Parameters::kMemLaserScanNormalK(), _laserScanNormalK); + Parameters::parse(parameters, Parameters::kMemLaserScanNormalRadius(), _laserScanNormalRadius); Parameters::parse(parameters, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures); Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance); Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle); @@ -3718,13 +3720,12 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p if(stats) stats->addStatistic(Statistics::kTimingMemScan_downsampling(), t*1000.0f); UDEBUG("time downsampling scan = %fs", t); } - if(!laserScan.empty() && _laserScanNormalK > 0 && laserScan.channels() == 3 && !isIntermediateNode) + if(!laserScan.empty() && + (_laserScanNormalK > 0 || _laserScanNormalRadius>0.0f) && + laserScan.channels() > 1 && laserScan.channels() < 5 && + !isIntermediateNode) { - pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloud(laserScan); - float x,y,z; - data.laserScanInfo().localTransform().getTranslation(x,y,z); - pcl::PointCloud::Ptr normals = util3d::computeNormals(cloud, _laserScanNormalK, Eigen::Vector3f(x,y,z)); - laserScan = util3d::laserScanFromPointCloud(*cloud, *normals); + laserScan = util3d::computeNormals(laserScan, _laserScanNormalK, _laserScanNormalRadius); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemScan_normals(), t*1000.0f); UDEBUG("time normals scan = %fs", t); diff --git a/corelib/src/Parameters.cpp b/corelib/src/Parameters.cpp index f873e986..8d868274 100644 --- a/corelib/src/Parameters.cpp +++ b/corelib/src/Parameters.cpp @@ -225,6 +225,10 @@ const std::map > & Parameters::getRemo { // removed parameters + // 0.13.3 + removedParameters_.insert(std::make_pair("Icp/PointToPlaneNormalNeighbors", std::make_pair(true, Parameters::kIcpPointToPlaneK()))); + + // 0.13.1 removedParameters_.insert(std::make_pair("Rtabmap/VhStrategy", std::make_pair(true, Parameters::kVhEpEnabled()))); @@ -326,7 +330,7 @@ const std::map > & Parameters::getRemo removedParameters_.insert(std::make_pair("LccIcp3/Iterations", std::make_pair(false, Parameters::kIcpIterations()))); removedParameters_.insert(std::make_pair("LccIcp3/CorrespondenceRatio", std::make_pair(false, Parameters::kIcpCorrespondenceRatio()))); removedParameters_.insert(std::make_pair("LccIcp3/PointToPlane", std::make_pair(true, Parameters::kIcpPointToPlane()))); - removedParameters_.insert(std::make_pair("LccIcp3/PointToPlaneNormalNeighbors", std::make_pair(true, Parameters::kIcpPointToPlaneNormalNeighbors()))); + removedParameters_.insert(std::make_pair("LccIcp3/PointToPlaneNormalNeighbors", std::make_pair(true, Parameters::kIcpPointToPlaneK()))); removedParameters_.insert(std::make_pair("LccIcp2/MaxCorrespondenceDistance", std::make_pair(true, Parameters::kIcpMaxCorrespondenceDistance()))); removedParameters_.insert(std::make_pair("LccIcp2/Iterations", std::make_pair(true, Parameters::kIcpIterations()))); diff --git a/corelib/src/RegistrationIcp.cpp b/corelib/src/RegistrationIcp.cpp index c6f1e2a2..fae10208 100644 --- a/corelib/src/RegistrationIcp.cpp +++ b/corelib/src/RegistrationIcp.cpp @@ -36,8 +36,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include -#include #include #ifdef RTABMAP_POINTMATCHER @@ -225,7 +223,8 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration _epsilon(Parameters::defaultIcpEpsilon()), _correspondenceRatio(Parameters::defaultIcpCorrespondenceRatio()), _pointToPlane(Parameters::defaultIcpPointToPlane()), - _pointToPlaneNormalNeighbors(Parameters::defaultIcpPointToPlaneNormalNeighbors()), + _pointToPlaneK(Parameters::defaultIcpPointToPlaneK()), + _pointToPlaneRadius(Parameters::defaultIcpPointToPlaneRadius()), _libpointmatcher(Parameters::defaultIcpPM()), _libpointmatcherConfig(Parameters::defaultIcpPMConfig()), _libpointmatcherOutlierRatio(Parameters::defaultIcpPMOutlierRatio()), @@ -257,7 +256,8 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kIcpEpsilon(), _epsilon); Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), _correspondenceRatio); Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane); - Parameters::parse(parameters, Parameters::kIcpPointToPlaneNormalNeighbors(), _pointToPlaneNormalNeighbors); + Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), _pointToPlaneK); + Parameters::parse(parameters, Parameters::kIcpPointToPlaneRadius(), _pointToPlaneRadius); Parameters::parse(parameters, Parameters::kIcpPM(), _libpointmatcher); Parameters::parse(parameters, Parameters::kIcpPMConfig(), _libpointmatcherConfig); @@ -342,7 +342,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters) UASSERT_MSG(_maxIterations > 0, uFormat("value=%d", _maxIterations).c_str()); UASSERT(_epsilon >= 0.0f); UASSERT_MSG(_correspondenceRatio >=0.0f && _correspondenceRatio <=1.0f, uFormat("value=%f", _correspondenceRatio).c_str()); - UASSERT_MSG(_pointToPlaneNormalNeighbors > 0, uFormat("value=%d", _pointToPlaneNormalNeighbors).c_str()); + UASSERT_MSG(!_pointToPlane || (_pointToPlane && (_pointToPlaneK > 0 || _pointToPlaneRadius > 0.0f)), uFormat("_pointToPlaneK=%d _pointToPlaneRadius=%f", _pointToPlaneK, _pointToPlaneRadius).c_str()); } Transform RegistrationIcp::computeTransformationImpl( @@ -354,7 +354,8 @@ Transform RegistrationIcp::computeTransformationImpl( UDEBUG("Guess transform = %s", guess.prettyPrint().c_str()); UDEBUG("Voxel size=%f", _voxelSize); UDEBUG("PointToPlane=%d", _pointToPlane?1:0); - UDEBUG("Normal neighborhood=%d", _pointToPlaneNormalNeighbors); + UDEBUG("Normal neighborhood=%d", _pointToPlaneK); + UDEBUG("Normal radius=%d", _pointToPlaneRadius); UDEBUG("Max correspondence distance=%f", _maxCorrespondenceDistance); UDEBUG("Max Iterations=%d", _maxIterations); UDEBUG("Correspondence Ratio=%f", _correspondenceRatio); @@ -406,8 +407,8 @@ Transform RegistrationIcp::computeTransformationImpl( if( _pointToPlane && _voxelSize == 0.0f && - fromScan.channels() == 6 && - toScan.channels() == 6) + fromScan.channels() >= 6 && + toScan.channels() >= 6) { //special case if we have already normals computed and there is no filtering pcl::PointCloud::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform); @@ -450,6 +451,9 @@ Transform RegistrationIcp::computeTransformationImpl( else #endif { + fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals); + toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals); + icpT = util3d::icpPointToPlane( fromCloudNormals, toCloudNormals, @@ -497,15 +501,41 @@ Transform RegistrationIcp::computeTransformationImpl( } pcl::PointCloud::Ptr fromCloudRegistered(new pcl::PointCloud()); - if(_pointToPlane) // ICP Point To Plane, only in 3D + if(_pointToPlane && // ICP Point To Plane + !((fromScan.channels() == 2 || toScan.channels() == 2) && !_libpointmatcher)) // PCL crashes if 2D { pcl::PointCloud::Ptr normals; + Eigen::Vector3f viewpointFrom(fromLocalTransform.x(), fromLocalTransform.y(), fromLocalTransform.z()); + Transform toT = guess * toLocalTransform; + Eigen::Vector3f viewpointTo(toT.x(), toT.y(), toT.z()); - normals = util3d::computeNormals(fromCloudFiltered, _pointToPlaneNormalNeighbors); + if(fromScan.channels() == 2) + { + normals = util3d::computeFastOrganizedNormals2D( + fromCloudFiltered, + _pointToPlaneK, + _pointToPlaneRadius, + viewpointFrom); + } + else + { + normals = util3d::computeNormals(fromCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointFrom); + } pcl::PointCloud::Ptr fromCloudNormals(new pcl::PointCloud); pcl::concatenateFields(*fromCloudFiltered, *normals, *fromCloudNormals); - normals = util3d::computeNormals(toCloudFiltered, _pointToPlaneNormalNeighbors); + if(toScan.channels() == 2) + { + normals = util3d::computeFastOrganizedNormals2D( + toCloudFiltered, + _pointToPlaneK, + _pointToPlaneRadius, + viewpointTo); + } + else + { + normals = util3d::computeNormals(toCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointTo); + } pcl::PointCloud::Ptr toCloudNormals(new pcl::PointCloud); pcl::concatenateFields(*toCloudFiltered, *normals, *toCloudNormals); @@ -517,7 +547,7 @@ Transform RegistrationIcp::computeTransformationImpl( 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)); - UDEBUG("Compute normals time = %f s", timer.ticks()); + UDEBUG("Compute normals (%d,%d) time = %f s", (int)fromCloudNormals->size(), (int)toCloudNormals->size(), timer.ticks()); if(toCloudNormals->size() && fromCloudNormals->size()) { @@ -569,7 +599,6 @@ Transform RegistrationIcp::computeTransformationImpl( this->force3DoF()); } - if(!icpT.isNull() && hasConverged) { util3d::computeVarianceAndCorrespondences( @@ -583,6 +612,11 @@ Transform RegistrationIcp::computeTransformationImpl( } else // ICP Point to Point { + if(_pointToPlane && ((fromScan.channels() == 2 || toScan.channels() == 2) && !_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()); + } + if(_voxelSize > 0.0f) { // update output scans diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index f9279fa8..f73cda49 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -1363,6 +1363,46 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud & cloud, return laserScan; } +cv::Mat laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::PointCloud & normals, const Transform & transform) +{ + UASSERT(cloud.size() == normals.size()); + cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7)); + bool nullTransform = transform.isNull() || transform.isIdentity(); + for(unsigned int i=0; i(0, i); + if(!nullTransform) + { + pcl::PointXYZRGBNormal 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.z; + ptr[4] = pt.normal_x; + ptr[5] = pt.normal_y; + ptr[6] = pt.normal_z; + } + else + { + ptr[0] = cloud.at(i).x; + ptr[1] = cloud.at(i).y; + ptr[2] = cloud.at(i).z; + ptr[4] = normals.at(i).normal_x; + ptr[5] = normals.at(i).normal_y; + ptr[6] = normals.at(i).normal_z; + } + int * ptrInt = (int*)ptr; + ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16); + } + return laserScan; +} + cv::Mat laserScanFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform) { cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7)); diff --git a/corelib/src/util3d_filtering.cpp b/corelib/src/util3d_filtering.cpp index 8d307505..7be53070 100644 --- a/corelib/src/util3d_filtering.cpp +++ b/corelib/src/util3d_filtering.cpp @@ -369,6 +369,26 @@ pcl::PointCloud::Ptr passThrough( return output; } +pcl::PointCloud::Ptr passThrough( + const pcl::PointCloud::Ptr & cloud, + const std::string & axis, + float min, + float max, + bool negative) +{ + UASSERT_MSG(max > min, uFormat("cloud=%d, max=%f min=%f axis=%s", (int)cloud->size(), max, min, axis.c_str()).c_str()); + UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0); + + pcl::PointCloud::Ptr output(new pcl::PointCloud); + pcl::PassThrough filter; + filter.setNegative(negative); + filter.setFilterFieldName(axis); + filter.setFilterLimits(min, max); + filter.setInputCloud(cloud); + filter.filter(*output); + return output; +} + pcl::IndicesPtr cropBox( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, diff --git a/corelib/src/util3d_surface.cpp b/corelib/src/util3d_surface.cpp index e1648110..b4f53643 100644 --- a/corelib/src/util3d_surface.cpp +++ b/corelib/src/util3d_surface.cpp @@ -1994,18 +1994,51 @@ cv::Mat mergeTextures( return globalTextures; } +cv::Mat computeNormals( + const cv::Mat & laserScan, + int searchK, + float searchRadius) +{ + if(laserScan.empty() || laserScan.channels()<2 || laserScan.channels()>4) + { + return laserScan; + } + + pcl::PointCloud::Ptr normals; + if(laserScan.channels() < 4) + { + pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloud(laserScan); + if(laserScan.channels() == 2) + { + normals = util3d::computeNormals2D(cloud, searchK, searchRadius); + } + else + { + normals = util3d::computeNormals(cloud, searchK, searchRadius); + } + return util3d::laserScanFromPointCloud(*cloud, *normals); + } + else // 4 channels + { + pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloudRGB(laserScan); + normals = util3d::computeNormals(cloud, searchK, searchRadius); + return util3d::laserScanFromPointCloud(*cloud, *normals); + } +} pcl::PointCloud::Ptr computeNormals( const pcl::PointCloud::Ptr & cloud, - int normalKSearch, + int searchK, + float searchRadius, const Eigen::Vector3f & viewPoint) { pcl::IndicesPtr indices(new std::vector); - return computeNormals(cloud, indices, normalKSearch, viewPoint); + return computeNormals(cloud, indices, searchK, searchRadius, viewPoint); } pcl::PointCloud::Ptr computeNormals( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, - int normalKSearch, + int searchK, + float searchRadius, const Eigen::Vector3f & viewPoint) { pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); @@ -2032,7 +2065,8 @@ pcl::PointCloud::Ptr computeNormals( // n.setIndices(indices); //} n.setSearchMethod (tree); - n.setKSearch (normalKSearch); + n.setKSearch (searchK); + n.setRadiusSearch (searchRadius); n.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]); n.compute (*normals); @@ -2041,16 +2075,18 @@ pcl::PointCloud::Ptr computeNormals( pcl::PointCloud::Ptr computeNormals( const pcl::PointCloud::Ptr & cloud, - int normalKSearch, + int searchK, + float searchRadius, const Eigen::Vector3f & viewPoint) { pcl::IndicesPtr indices(new std::vector); - return computeNormals(cloud, indices, normalKSearch, viewPoint); + return computeNormals(cloud, indices, searchK, searchRadius, viewPoint); } pcl::PointCloud::Ptr computeNormals( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, - int normalKSearch, + int searchK, + float searchRadius, const Eigen::Vector3f & viewPoint) { pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); @@ -2077,13 +2113,182 @@ pcl::PointCloud::Ptr computeNormals( // n.setIndices(indices); //} n.setSearchMethod (tree); - n.setKSearch (normalKSearch); + n.setKSearch (searchK); + n.setRadiusSearch(searchRadius); n.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]); n.compute (*normals); return normals; } +pcl::PointCloud::Ptr computeNormals2D( + const pcl::PointCloud::Ptr & cloud, + int searchK, + float searchRadius, + const Eigen::Vector3f & viewPoint) +{ + UASSERT(searchK>0 || searchRadius>0.0f); + pcl::PointCloud::Ptr normals (new pcl::PointCloud); + + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + tree->setInputCloud (cloud); + + normals->resize(cloud->size()); + + float bad_point = std::numeric_limits::quiet_NaN (); + + // assuming that points are ordered + for(unsigned int i=0; isize(); ++i) + { + const pcl::PointXYZ & pt = cloud->at(i); + std::vector neighborNormals; + Eigen::Vector3f direction; + direction[0] = viewPoint[0] - pt.x; + direction[1] = viewPoint[1] - pt.y; + direction[2] = viewPoint[2] - pt.z; + + std::vector k_indices; + std::vector k_sqr_distances; + if(searchRadius>0.0f) + { + tree->radiusSearch(cloud->at(i), searchRadius, k_indices, k_sqr_distances, searchK); + } + else + { + tree->nearestKSearch(cloud->at(i), searchK, k_indices, k_sqr_distances); + } + + for(unsigned int j=0; jat(k_indices.at(j)); + Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z); + Eigen::Vector3f up = v.cross(direction); + Eigen::Vector3f n = up.cross(v); + n.normalize(); + neighborNormals.push_back(n); + } + } + + if(neighborNormals.empty()) + { + normals->at(i).normal_x = bad_point; + normals->at(i).normal_y = bad_point; + normals->at(i).normal_z = bad_point; + } + else + { + Eigen::Vector3f meanNormal(0,0,0); + for(unsigned int j=0; jat(i).normal_x = meanNormal[0]; + normals->at(i).normal_y = meanNormal[1]; + normals->at(i).normal_z = meanNormal[2]; + } + } + + return normals; +} + +pcl::PointCloud::Ptr computeFastOrganizedNormals2D( + const pcl::PointCloud::Ptr & cloud, + int searchK, + float searchRadius, + const Eigen::Vector3f & viewPoint) +{ + UASSERT(searchK>0); + pcl::PointCloud::Ptr normals (new pcl::PointCloud); + + normals->resize(cloud->size()); + searchRadius *= searchRadius; // squared distance + + float bad_point = std::numeric_limits::quiet_NaN (); + + // assuming that points are ordered + for(int i=0; i<(int)cloud->size(); ++i) + { + int li = i-searchK; + if(li<0) + { + li=0; + } + int hi = i+searchK; + if(hi>=(int)cloud->size()) + { + hi=(int)cloud->size()-1; + } + + // get points before not too far + const pcl::PointXYZ & pt = cloud->at(i); + std::vector neighborNormals; + Eigen::Vector3f direction; + direction[0] = viewPoint[0] - cloud->at(i).x; + direction[1] = viewPoint[1] - cloud->at(i).y; + direction[2] = viewPoint[2] - cloud->at(i).z; + for(int j=i-1; j>=li; --j) + { + const pcl::PointXYZ & pt2 = cloud->at(j); + Eigen::Vector3f vd(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z); + if(searchRadius<=0.0f || (vd[0]*vd[0] + vd[1]*vd[1] + vd[2]*vd[2]) < searchRadius) + { + Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z); + Eigen::Vector3f up = v.cross(direction); + Eigen::Vector3f n = up.cross(v); + n.normalize(); + neighborNormals.push_back(n); + } + else + { + break; + } + } + for(int j=i+1; j<=hi; ++j) + { + const pcl::PointXYZ & pt2 = cloud->at(j); + Eigen::Vector3f vd(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z); + if(searchRadius<=0.0f || (vd[0]*vd[0] + vd[1]*vd[1] + vd[2]*vd[2]) < searchRadius) + { + Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z); + Eigen::Vector3f up = v[2]==0.0f?Eigen::Vector3f(0,0,1):v.cross(direction); + Eigen::Vector3f n = up.cross(v); + n.normalize(); + neighborNormals.push_back(n); + } + else + { + break; + } + } + + if(neighborNormals.empty()) + { + normals->at(i).normal_x = bad_point; + normals->at(i).normal_y = bad_point; + normals->at(i).normal_z = bad_point; + } + else + { + Eigen::Vector3f meanNormal(0,0,0); + for(unsigned int j=0; jat(i).normal_x = meanNormal[0]; + normals->at(i).normal_y = meanNormal[1]; + normals->at(i).normal_z = meanNormal[2]; + } + } + + return normals; +} + pcl::PointCloud::Ptr computeFastOrganizedNormals( const pcl::PointCloud::Ptr & cloud, float maxDepthChangeFactor, diff --git a/guilib/include/rtabmap/gui/CloudViewer.h b/guilib/include/rtabmap/gui/CloudViewer.h index 92019d62..4556236a 100644 --- a/guilib/include/rtabmap/gui/CloudViewer.h +++ b/guilib/include/rtabmap/gui/CloudViewer.h @@ -274,8 +274,14 @@ public: void setCameraFree(); void setCameraLockZ(bool enabled = true); void setGridShown(bool shown); + void setNormalsShown(bool shown); void setGridCellCount(unsigned int count); void setGridCellSize(float size); + bool isNormalsShown() const; + int getNormalsStep() const; + float getNormalsScale() const; + void setNormalsStep(int step); + void setNormalsScale(float scale); public slots: void setDefaultBackgroundColor(const QColor & color); @@ -318,6 +324,9 @@ private: QAction * _aShowGrid; QAction * _aSetGridCellCount; QAction * _aSetGridCellSize; + QAction * _aShowNormals; + QAction * _aSetNormalsStep; + QAction * _aSetNormalsScale; QAction * _aSetBackgroundColor; QAction * _aSetRenderingRate; QAction * _aSetLighting; @@ -336,6 +345,8 @@ private: QColor _frustumColor; unsigned int _gridCellCount; float _gridCellSize; + int _normalsStep; + float _normalsScale; cv::Vec3d _lastCameraOrientation; cv::Vec3d _lastCameraPose; QMap _addedClouds; // include cloud, scan, meshes diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 78ad8f34..0d11e8e1 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -164,9 +164,11 @@ public: double getCeilingFilteringHeight() const; double getFloorFilteringHeight() const; int getNormalKSearch() const; + double getNormalRadiusSearch() const; double getScanCeilingFilteringHeight() const; double getScanFloorFilteringHeight() const; int getScanNormalKSearch() const; + double getScanNormalRadiusSearch() const; bool isCloudsShown(int index) const; // 0=map, 1=odom bool isOctomapUpdated() const; bool isOctomapShown() const; @@ -241,6 +243,7 @@ public: double getSourceScanFromDepthMaxDepth() const; double getSourceScanVoxelSize() const; int getSourceScanNormalsK() const; + double getSourceScanNormalsRadius() const; Transform getSourceLocalTransform() const; //Openni group Transform getLaserLocalTransform() const; // directory images Camera * createCamera(bool useRawImages = false, bool useColor = true); // return camera should be deleted if not null diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index c40548ab..00ef8032 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/gui/CloudViewer.h" #include +#include #include #include #include @@ -190,6 +191,9 @@ CloudViewer::CloudViewer(QWidget *parent) : _aShowGrid(0), _aSetGridCellCount(0), _aSetGridCellSize(0), + _aShowNormals(0), + _aSetNormalsStep(0), + _aSetNormalsScale(0), _aSetBackgroundColor(0), _aSetRenderingRate(0), _aSetLighting(0), @@ -203,6 +207,8 @@ CloudViewer::CloudViewer(QWidget *parent) : _frustumColor(Qt::gray), _gridCellCount(50), _gridCellSize(1), + _normalsStep(1), + _normalsScale(0.2), _lastCameraOrientation(0,0,0), _lastCameraPose(0,0,0), _defaultBgColor(Qt::black), @@ -310,6 +316,10 @@ void CloudViewer::createMenu() _aShowGrid->setCheckable(true); _aSetGridCellCount = new QAction("Set cell count...", this); _aSetGridCellSize = new QAction("Set cell size...", this); + _aShowNormals = new QAction("Show normals", this); + _aShowNormals->setCheckable(true); + _aSetNormalsStep = new QAction("Set normals step...", this); + _aSetNormalsScale = new QAction("Set normals scale...", this); _aSetBackgroundColor = new QAction("Set background color...", this); _aSetRenderingRate = new QAction("Set rendering rate...", this); _aSetLighting = new QAction("Lighting", this); @@ -352,12 +362,18 @@ void CloudViewer::createMenu() gridMenu->addAction(_aSetGridCellCount); gridMenu->addAction(_aSetGridCellSize); + QMenu * normalsMenu = new QMenu("Normals", this); + normalsMenu->addAction(_aShowNormals); + normalsMenu->addAction(_aSetNormalsStep); + normalsMenu->addAction(_aSetNormalsScale); + //menus _menu = new QMenu(this); _menu->addMenu(cameraMenu); _menu->addMenu(trajectoryMenu); _menu->addMenu(frustumMenu); _menu->addMenu(gridMenu); + _menu->addMenu(normalsMenu); _menu->addAction(_aSetBackgroundColor); _menu->addAction(_aSetRenderingRate); _menu->addAction(_aSetLighting); @@ -400,6 +416,10 @@ void CloudViewer::saveSettings(QSettings & settings, const QString & group) cons settings.setValue("grid_cell_count", this->getGridCellCount()); settings.setValue("grid_cell_size", (double)this->getGridCellSize()); + settings.setValue("normals", this->isNormalsShown()); + settings.setValue("normals_step", this->getNormalsStep()); + settings.setValue("normals_scale", (double)this->getNormalsScale()); + settings.setValue("trajectory_shown", this->isTrajectoryShown()); settings.setValue("trajectory_size", this->getTrajectorySize()); @@ -439,6 +459,10 @@ void CloudViewer::loadSettings(QSettings & settings, const QString & group) this->setGridCellCount(settings.value("grid_cell_count", this->getGridCellCount()).toUInt()); this->setGridCellSize(settings.value("grid_cell_size", this->getGridCellSize()).toFloat()); + this->setNormalsShown(settings.value("normals", this->isNormalsShown()).toBool()); + this->setNormalsStep(settings.value("normals_step", this->getNormalsStep()).toInt()); + this->setNormalsScale(settings.value("normals_scale", this->getNormalsScale()).toFloat()); + this->setTrajectoryShown(settings.value("trajectory_shown", this->isTrajectoryShown()).toBool()); this->setTrajectorySize(settings.value("trajectory_size", this->getTrajectorySize()).toUInt()); @@ -473,10 +497,21 @@ bool CloudViewer::updateCloudPose( if(_addedClouds.contains(id)) { UDEBUG("Updating pose %s to %s", id.c_str(), pose.prettyPrint().c_str()); - if(_addedClouds.find(id).value() == pose || - _visualizer->updatePointCloudPose(id, pose.toEigen3f())) + bool samePose = _addedClouds.find(id).value() == pose; + Eigen::Affine3f posef = pose.toEigen3f(); + if(samePose || + _visualizer->updatePointCloudPose(id, posef)) { _addedClouds.find(id).value() = pose; + if(!samePose) + { + std::string idNormals = id+"-normals"; + if(_addedClouds.find(idNormals)!=_addedClouds.end()) + { + _visualizer->updatePointCloudPose(idNormals, posef); + _addedClouds.find(idNormals).value() = pose; + } + } return true; } } @@ -501,6 +536,18 @@ bool CloudViewer::addCloud( Eigen::Vector4f origin(pose.x(), pose.y(), pose.z(), 0.0f); Eigen::Quaternionf orientation = Eigen::Quaternionf(pose.toEigen3f().rotation()); + if(haveNormals && _aShowNormals->isChecked()) + { + pcl::PointCloud::Ptr cloud_xyz (new pcl::PointCloud); + pcl::fromPCLPointCloud2 (*binaryCloud, *cloud_xyz); + std::string idNormals = id + "-normals"; + if(_visualizer->addPointCloudNormals(cloud_xyz, _normalsStep, _normalsScale, idNormals, 0)) + { + _visualizer->updatePointCloudPose(idNormals, pose.toEigen3f()); + _addedClouds.insert(idNormals, pose); + } + } + // add random color channel pcl::visualization::PointCloudColorHandler::Ptr colorHandler; colorHandler.reset (new pcl::visualization::PointCloudColorHandlerRandom (binaryCloud)); @@ -1626,7 +1673,9 @@ void CloudViewer::removeAllClouds() bool CloudViewer::removeCloud(const std::string & id) { bool success = _visualizer->removePointCloud(id); + _visualizer->removePointCloud(id+"-normals"); _addedClouds.remove(id); // remove after visualizer + _addedClouds.remove(id+"-normals"); return success; } @@ -1929,6 +1978,12 @@ void CloudViewer::setCloudVisibility(const std::string & id, bool isVisible) if(iter != cloudActorMap->end()) { iter->second.actor->SetVisibility(isVisible?1:0); + + iter = cloudActorMap->find(id+"-normals"); + if(iter != cloudActorMap->end()) + { + iter->second.actor->SetVisibility(isVisible&&_aShowNormals->isChecked()?1:0); + } } else { @@ -1992,20 +2047,6 @@ void CloudViewer::setCameraLockZ(bool enabled) _lastCameraOrientation= _lastCameraPose = cv::Vec3f(0,0,0); _aLockViewZ->setChecked(enabled); } - -void CloudViewer::setGridShown(bool shown) -{ - _aShowGrid->setChecked(shown); - if(shown) - { - this->addGrid(); - } - else - { - this->removeGrid(); - } -} - bool CloudViewer::isCameraTargetLocked() const { return _aLockCamera->isChecked(); @@ -2022,6 +2063,23 @@ bool CloudViewer::isCameraLockZ() const { return _aLockViewZ->isChecked(); } +double CloudViewer::getRenderingRate() const +{ + return _renderingRate; +} + +void CloudViewer::setGridShown(bool shown) +{ + _aShowGrid->setChecked(shown); + if(shown) + { + this->addGrid(); + } + else + { + this->removeGrid(); + } +} bool CloudViewer::isGridShown() const { return _aShowGrid->isChecked(); @@ -2034,11 +2092,6 @@ float CloudViewer::getGridCellSize() const { return _gridCellSize; } -double CloudViewer::getRenderingRate() const -{ - return _renderingRate; -} - void CloudViewer::setGridCellCount(unsigned int count) { if(count > 0) @@ -2110,6 +2163,54 @@ void CloudViewer::removeGrid() _gridLines.clear(); } +void CloudViewer::setNormalsShown(bool shown) +{ + _aShowNormals->setChecked(shown); + QList ids = _addedClouds.keys(); + for(QList::iterator iter = ids.begin(); iter!=ids.end(); ++iter) + { + std::string idNormals = *iter + "-normals"; + if(_addedClouds.find(idNormals) != _addedClouds.end()) + { + this->setCloudVisibility(idNormals, this->getCloudVisibility(*iter) && shown); + } + } +} +bool CloudViewer::isNormalsShown() const +{ + return _aShowNormals->isChecked(); +} +int CloudViewer::getNormalsStep() const +{ + return _normalsStep; +} +float CloudViewer::getNormalsScale() const +{ + return _normalsScale; +} +void CloudViewer::setNormalsStep(int step) +{ + if(step > 0) + { + _normalsStep = step; + } + else + { + UERROR("Cannot set normals step <= 0, step=%d", step); + } +} +void CloudViewer::setNormalsScale(float scale) +{ + if(scale > 0) + { + _normalsScale= scale; + } + else + { + UERROR("Cannot set normals scale <= 0, value=%f", scale); + } +} + Eigen::Vector3f rotatePointAroundAxe( const Eigen::Vector3f & point, const Eigen::Vector3f & axis, @@ -2405,6 +2506,29 @@ void CloudViewer::handleAction(QAction * a) this->setGridCellSize(value); } } + else if(a == _aShowNormals) + { + this->setNormalsShown(_aShowNormals->isChecked()); + this->update(); + } + else if(a == _aSetNormalsStep) + { + bool ok; + int value = QInputDialog::getInt(this, tr("Set normals step"), tr("Step"), _normalsStep, 1, 10000, 1, &ok); + if(ok) + { + this->setNormalsStep(value); + } + } + else if(a == _aSetNormalsScale) + { + bool ok; + double value = QInputDialog::getDouble(this, tr("Set normals scale"), tr("Scale (m)"), _normalsScale, 0.01, 10, 2, &ok); + if(ok) + { + this->setNormalsScale(value); + } + } else if(a == _aSetBackgroundColor) { QColor color = this->getDefaultBackgroundColor(); diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index e15ee71a..3c121fe3 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -88,6 +88,7 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) : connect(_ui->checkBox_fromDepth, SIGNAL(stateChanged(int)), this, SLOT(updateReconstructionFlavor())); connect(_ui->checkBox_binary, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); connect(_ui->spinBox_normalKSearch, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged())); + connect(_ui->doubleSpinBox_normalRadiusSearch, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->comboBox_pipeline, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged())); connect(_ui->comboBox_pipeline, SIGNAL(currentIndexChanged(int)), this, SLOT(updateReconstructionFlavor())); connect(_ui->comboBox_meshingApproach, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged())); @@ -255,6 +256,7 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou settings.setValue("from_depth", _ui->checkBox_fromDepth->isChecked()); settings.setValue("binary", _ui->checkBox_binary->isChecked()); settings.setValue("normals_k", _ui->spinBox_normalKSearch->value()); + settings.setValue("normals_radius", _ui->doubleSpinBox_normalRadiusSearch->value()); settings.setValue("regenerate", _ui->checkBox_regenerate->isChecked()); settings.setValue("regenerate_decimation", _ui->spinBox_decimation->value()); @@ -371,6 +373,7 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou _ui->checkBox_fromDepth->setChecked(settings.value("from_depth", _ui->checkBox_fromDepth->isChecked()).toBool()); _ui->checkBox_binary->setChecked(settings.value("binary", _ui->checkBox_binary->isChecked()).toBool()); _ui->spinBox_normalKSearch->setValue(settings.value("normals_k", _ui->spinBox_normalKSearch->value()).toInt()); + _ui->doubleSpinBox_normalRadiusSearch->setValue(settings.value("normals_radius", _ui->doubleSpinBox_normalRadiusSearch->value()).toDouble()); _ui->checkBox_regenerate->setChecked(settings.value("regenerate", _ui->checkBox_regenerate->isChecked()).toBool()); _ui->spinBox_decimation->setValue(settings.value("regenerate_decimation", _ui->spinBox_decimation->value()).toInt()); @@ -487,6 +490,7 @@ void ExportCloudsDialog::restoreDefaults() _ui->checkBox_fromDepth->setChecked(true); _ui->checkBox_binary->setChecked(true); _ui->spinBox_normalKSearch->setValue(20); + _ui->doubleSpinBox_normalRadiusSearch->setValue(0.0); _ui->checkBox_regenerate->setChecked(_dbDriver!=0?true:false); _ui->spinBox_decimation->setValue(1); @@ -1384,7 +1388,7 @@ bool ExportCloudsDialog::getExportedClouds( // recompute normals pcl::PointCloud::Ptr cloudWithoutNormals(new pcl::PointCloud); pcl::copyPointCloud(*assembledCloud, *cloudWithoutNormals); - pcl::PointCloud::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value()); + pcl::PointCloud::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), _ui->doubleSpinBox_normalRadiusSearch->value()); UASSERT(assembledCloud->size() == normals->size()); for(unsigned int i=0; isize(); ++i) @@ -2521,7 +2525,7 @@ std::map::Ptr, pcl::Indic viewPoint[2] = data.stereoCameraModel().localTransform().z(); } - pcl::PointCloud::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), viewPoint); + pcl::PointCloud::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), _ui->doubleSpinBox_normalRadiusSearch->value(), viewPoint); pcl::concatenateFields(*cloudWithoutNormals, *normals, *cloud); if(_ui->checkBox_subtraction->isChecked() && @@ -2592,7 +2596,7 @@ std::map::Ptr, pcl::Indic } else { - normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), viewPoint); + normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), _ui->doubleSpinBox_normalRadiusSearch->value(), viewPoint); } pcl::concatenateFields(*cloudWithoutNormals, *normals, *cloud); } @@ -2673,7 +2677,7 @@ std::map::Ptr, pcl::Indic _progressDialog->appendText(tr("Cached cloud %1 is not found in cached data, the view point for normal computation will not be set (%2/%3).").arg(iter->first).arg(index).arg(poses.size()), Qt::darkYellow); } - pcl::PointCloud::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), viewPoint); + pcl::PointCloud::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), _ui->doubleSpinBox_normalRadiusSearch->value(), viewPoint); pcl::concatenateFields(*cloudWithoutNormals, *normals, *cloud); } else if(!_ui->checkBox_fromDepth->isChecked() && uContains(cachedScans, iter->first)) @@ -2731,7 +2735,7 @@ std::map::Ptr, pcl::Indic } else { - normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), viewPoint); + normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), _ui->doubleSpinBox_normalRadiusSearch->value(), viewPoint); } pcl::concatenateFields(*cloudWithoutNormals, *normals, *cloud); } diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index e80ca600..90e3c32a 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -2651,9 +2651,9 @@ std::pair::Ptr, pcl::IndicesPtr> MainWindow::c if(_preferencesDialog->getSubtractFilteringAngle() > 0.0f) { //normals required - if(_preferencesDialog->getNormalKSearch() > 0) + if(_preferencesDialog->getNormalKSearch() > 0 || _preferencesDialog->getNormalRadiusSearch() > 0) { - pcl::PointCloud::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), viewPoint); + pcl::PointCloud::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), _preferencesDialog->getNormalRadiusSearch(), viewPoint); pcl::concatenateFields(*cloud, *normals, *cloudWithNormals); } else @@ -2790,7 +2790,7 @@ std::pair::Ptr, pcl::IndicesPtr> MainWindow::c if(_preferencesDialog->getNormalKSearch() > 0 && cloudWithNormals->size() == 0) { - pcl::PointCloud::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), viewPoint); + pcl::PointCloud::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), _preferencesDialog->getNormalRadiusSearch(), viewPoint); pcl::concatenateFields(*cloud, *normals, *cloudWithNormals); } @@ -2880,22 +2880,48 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m scan = util3d::downsample(scan, _preferencesDialog->getDownsamplingStepScan(0)); } - if(scan.channels() == 6) + pcl::PointCloud::Ptr cloud; + pcl::PointCloud::Ptr cloudRGB; + pcl::PointCloud::Ptr cloudWithNormals; + pcl::PointCloud::Ptr cloudRGBWithNormals; + if(scan.channels() == 7 && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0) { - pcl::PointCloud::Ptr cloud; - cloud = util3d::laserScanToPointCloudNormal(scan, iter->sensorData().laserScanInfo().localTransform()); - if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0) + cloudRGBWithNormals = util3d::laserScanToPointCloudRGBNormal(scan, iter->sensorData().laserScanInfo().localTransform()); + } + else if(scan.channels() == 6 && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0) + { + cloudWithNormals = util3d::laserScanToPointCloudNormal(scan, iter->sensorData().laserScanInfo().localTransform()); + } + else if(scan.channels() == 4) + { + cloudRGB = util3d::laserScanToPointCloudRGB(scan, iter->sensorData().laserScanInfo().localTransform()); + } + else + { + cloud = util3d::laserScanToPointCloud(scan, iter->sensorData().laserScanInfo().localTransform()); + } + + if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0) + { + if(cloud.get()) { cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0)); } + if(cloudRGB.get()) + { + cloudRGB = util3d::voxelize(cloudRGB, _preferencesDialog->getCloudVoxelSizeScan(0)); + } + } - // Do ceiling/floor filtering - if(cloud->size() && - (_preferencesDialog->getScanFloorFilteringHeight() != 0.0 || - _preferencesDialog->getScanCeilingFilteringHeight() != 0.0)) + // Do ceiling/floor filtering + if(scan.channels() > 2 && // don't filter 2D scans + (_preferencesDialog->getScanFloorFilteringHeight() != 0.0 || + _preferencesDialog->getScanCeilingFilteringHeight() != 0.0)) + { + if(cloudRGBWithNormals.get()) { // perform in /map frame - pcl::PointCloud::Ptr cloudTransformed = util3d::transformPointCloud(cloud, pose); + pcl::PointCloud::Ptr cloudTransformed = util3d::transformPointCloud(cloudRGBWithNormals, pose); cloudTransformed = rtabmap::util3d::passThrough( cloudTransformed, "z", @@ -2903,53 +2929,35 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits::max():_preferencesDialog->getScanCeilingFilteringHeight()); //transform back in sensor frame - cloud = util3d::transformPointCloud(cloudTransformed, pose.inverse()); + cloudRGBWithNormals = util3d::transformPointCloud(cloudTransformed, pose.inverse()); } + if(cloudWithNormals.get()) + { + // perform in /map frame + pcl::PointCloud::Ptr cloudTransformed = util3d::transformPointCloud(cloudWithNormals, pose); + cloudTransformed = rtabmap::util3d::passThrough( + cloudTransformed, + "z", + _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits::min():_preferencesDialog->getScanFloorFilteringHeight(), + _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits::max():_preferencesDialog->getScanCeilingFilteringHeight()); - QColor color = Qt::gray; - if(mapId >= 0) - { - color = (Qt::GlobalColor)(mapId+3 % 12 + 7 ); + //transform back in sensor frame + cloudWithNormals = util3d::transformPointCloud(cloudTransformed, pose.inverse()); } - if(!_cloudViewer->addCloud(scanName, cloud, pose, color)) + if(cloudRGB.get()) { - UERROR("Adding cloud %d to viewer failed!", nodeId); - } - else - { - if(nodeId > 0) - { - if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0) - { - //reconvert the voxelized cloud - scan = util3d::laserScanFromPointCloud(*cloud); - } - else - { - scan = util3d::transformLaserScan(scan, iter->sensorData().laserScanInfo().localTransform()); - } - _createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame - } - _cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0)); - _cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0)); - } - } - else - { - pcl::PointCloud::Ptr cloud; - cloud = util3d::laserScanToPointCloud(scan, iter->sensorData().laserScanInfo().localTransform()); - bool filtered = false; - if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0) - { - cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0)); - filtered = true; - } + // perform in /map frame + pcl::PointCloud::Ptr cloudTransformed = util3d::transformPointCloud(cloudRGB, pose); + cloudTransformed = rtabmap::util3d::passThrough( + cloudTransformed, + "z", + _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits::min():_preferencesDialog->getScanFloorFilteringHeight(), + _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits::max():_preferencesDialog->getScanCeilingFilteringHeight()); - // Do ceiling/floor filtering - if(scan.channels() > 2 && // don't filter 2D scans - cloud->size() && - (_preferencesDialog->getScanFloorFilteringHeight() != 0.0 || - _preferencesDialog->getScanCeilingFilteringHeight() != 0.0)) + //transform back in sensor frame + cloudRGB = util3d::transformPointCloud(cloudTransformed, pose.inverse()); + } + if(cloud.get()) { // perform in /map frame pcl::PointCloud::Ptr cloudTransformed = util3d::transformPointCloud(cloud, pose); @@ -2961,78 +2969,103 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m //transform back in sensor frame cloud = util3d::transformPointCloud(cloudTransformed, pose.inverse()); - filtered = true; } + } - pcl::PointCloud::Ptr cloudWithNormals; - if(scan.channels() > 2 && // don't compute normals for 2D scans - cloud->size() && - _preferencesDialog->getScanNormalKSearch() > 0) - { - pcl::PointCloud::Ptr normals = util3d::computeNormals(cloud, _preferencesDialog->getScanNormalKSearch()); - cloudWithNormals.reset(new pcl::PointCloud); - pcl::concatenateFields(*cloud, *normals, *cloudWithNormals); - filtered = true; - } + if( (cloud.get() || cloudRGB.get()) && + (_preferencesDialog->getScanNormalKSearch() > 0 || _preferencesDialog->getScanNormalRadiusSearch() > 0.0)) + { + Eigen::Vector3f scanViewpoint( + iter->sensorData().laserScanInfo().localTransform().x(), + iter->sensorData().laserScanInfo().localTransform().y(), + iter->sensorData().laserScanInfo().localTransform().z()); - QColor color = Qt::gray; - if(mapId >= 0) + pcl::PointCloud::Ptr normals; + if(cloud->size()) { - color = (Qt::GlobalColor)(mapId+3 % 12 + 7 ); - } - if(cloudWithNormals.get()) - { - if(!_cloudViewer->addCloud(scanName, cloudWithNormals, pose, color)) + if(scan.channels() == 2) { - UERROR("Adding cloud %d to viewer failed!", nodeId); + normals = util3d::computeFastOrganizedNormals2D(cloud, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint); } else { - if(nodeId > 0) - { - //reconvert the voxelized cloud - scan = util3d::laserScanFromPointCloud(*cloudWithNormals); - _createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame - } - - _cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0)); - _cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0)); + normals = util3d::computeNormals(cloud, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint); } + cloudWithNormals.reset(new pcl::PointCloud); + pcl::concatenateFields(*cloud, *normals, *cloudWithNormals); + cloud.reset(); } else { - if(!_cloudViewer->addCloud(scanName, cloud, pose, color)) + UASSERT(cloudRGB->size()); // Assuming 4 channels cannot be 2D + normals = util3d::computeNormals(cloudRGB, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint); + cloudRGBWithNormals.reset(new pcl::PointCloud); + pcl::concatenateFields(*cloudRGB, *normals, *cloudRGBWithNormals); + cloudRGB.reset(); + } + } + + QColor color = Qt::gray; + if(mapId >= 0) + { + color = (Qt::GlobalColor)(mapId+3 % 12 + 7 ); + } + bool added = false; + if(cloudRGBWithNormals.get()) + { + added = _cloudViewer->addCloud(scanName, cloudRGBWithNormals, pose, color); + if(added && nodeId > 0) + { + scan = util3d::laserScanFromPointCloud(*cloudRGBWithNormals); + } + } + else if(cloudWithNormals.get()) + { + added = _cloudViewer->addCloud(scanName, cloudWithNormals, pose, color); + if(added && nodeId > 0) + { + scan = util3d::laserScanFromPointCloud(*cloudWithNormals); + } + } + else if(cloudRGB.get()) + { + added = _cloudViewer->addCloud(scanName, cloudWithNormals, pose, color); + if(added && nodeId > 0) + { + scan = util3d::laserScanFromPointCloud(*cloudWithNormals); + } + } + else + { + UASSERT(cloud.get()); + added = _cloudViewer->addCloud(scanName, cloud, pose, color); + if(added && nodeId > 0) + { + if(scan.channels() == 2) { - UERROR("Adding cloud %d to viewer failed!", nodeId); + scan = util3d::laserScan2dFromPointCloud(*cloud); } else { - if(nodeId > 0) - { - if(filtered) - { - //reconvert the voxelized cloud - if(scan.channels() == 2) - { - scan = util3d::laserScan2dFromPointCloud(*cloud); - } - else - { - scan = util3d::laserScanFromPointCloud(*cloud); - } - } - else - { - scan = util3d::transformLaserScan(scan, iter->sensorData().laserScanInfo().localTransform()); - } - _createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame - } - - _cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0)); - _cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0)); + scan = util3d::laserScanFromPointCloud(*cloud); } } } + + if(!added) + { + UERROR("Adding cloud %d to viewer failed!", nodeId); + } + else + { + if(nodeId > 0) + { + _createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame + } + + _cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0)); + _cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0)); + } } } @@ -4496,7 +4529,8 @@ void MainWindow::startDetection() _preferencesDialog->getSourceScanFromDepthDecimation(), _preferencesDialog->getSourceScanFromDepthMaxDepth(), _preferencesDialog->getSourceScanVoxelSize(), - _preferencesDialog->getSourceScanNormalsK()); + _preferencesDialog->getSourceScanNormalsK(), + _preferencesDialog->getSourceScanNormalsRadius()); if(_preferencesDialog->isDepthFilteringAvailable()) { if(_preferencesDialog->isBilateralFiltering()) diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index cce1694c..78d3cfa8 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -410,9 +410,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->doubleSpinBox_ceilingFilterHeight, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->doubleSpinBox_floorFilterHeight, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->spinBox_normalKSearch, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); + connect(_ui->doubleSpinBox_normalRadiusSearch, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->doubleSpinBox_ceilingFilterHeight_scan, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->doubleSpinBox_floorFilterHeight_scan, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->spinBox_normalKSearch_scan, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); + connect(_ui->doubleSpinBox_normalRadiusSearch_scan, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->checkBox_showGraphs, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->checkBox_showFrustums, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); @@ -585,6 +587,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->doubleSpinBox_cameraImages_scanVoxelSize, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->spinBox_cameraImages_scanNormalsK, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->doubleSpinBox_cameraImages_scanNormalsRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); //Rtabmap basic connect(_ui->general_doubleSpinBox_timeThr, SIGNAL(valueChanged(double)), _ui->general_doubleSpinBox_timeThr_2, SLOT(setValue(double))); @@ -650,6 +653,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->spinBox_imagePostDecimation->setObjectName(Parameters::kMemImagePostDecimation().c_str()); _ui->general_spinBox_laserScanDownsample->setObjectName(Parameters::kMemLaserScanDownsampleStepSize().c_str()); _ui->general_spinBox_laserScanNormalK->setObjectName(Parameters::kMemLaserScanNormalK().c_str()); + _ui->general_doubleSpinBox_laserScanNormalRadius->setObjectName(Parameters::kMemLaserScanNormalRadius().c_str()); _ui->checkBox_useOdomFeatures->setObjectName(Parameters::kMemUseOdomFeatures().c_str()); // Database @@ -857,7 +861,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->loopClosure_icpEpsilon->setObjectName(Parameters::kIcpEpsilon().c_str()); _ui->loopClosure_icpRatio->setObjectName(Parameters::kIcpCorrespondenceRatio().c_str()); _ui->loopClosure_icpPointToPlane->setObjectName(Parameters::kIcpPointToPlane().c_str()); - _ui->loopClosure_icpPointToPlaneNormals->setObjectName(Parameters::kIcpPointToPlaneNormalNeighbors().c_str()); + _ui->loopClosure_icpPointToPlaneNormals->setObjectName(Parameters::kIcpPointToPlaneK().c_str()); + _ui->loopClosure_icpPointToPlaneNormalsRadius->setObjectName(Parameters::kIcpPointToPlaneRadius().c_str()); _ui->groupBox_libpointmatcher->setObjectName(Parameters::kIcpPM().c_str()); _ui->lineEdit_IcpPMConfigPath->setObjectName(Parameters::kIcpPMConfig().c_str()); @@ -1374,10 +1379,12 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->doubleSpinBox_ceilingFilterHeight->setValue(0); _ui->doubleSpinBox_floorFilterHeight->setValue(0); _ui->spinBox_normalKSearch->setValue(10); + _ui->doubleSpinBox_normalRadiusSearch->setValue(0.0); _ui->doubleSpinBox_ceilingFilterHeight_scan->setValue(0); _ui->doubleSpinBox_floorFilterHeight_scan->setValue(0); _ui->spinBox_normalKSearch_scan->setValue(0); + _ui->doubleSpinBox_normalRadiusSearch_scan->setValue(0.0); _ui->checkBox_showGraphs->setChecked(true); _ui->checkBox_showFrustums->setChecked(false); @@ -1542,6 +1549,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->setValue(4.0); _ui->doubleSpinBox_cameraImages_scanVoxelSize->setValue(0.025f); _ui->spinBox_cameraImages_scanNormalsK->setValue(20); + _ui->doubleSpinBox_cameraImages_scanNormalsRadius->setValue(0.0); _ui->groupBox_depthFromScan->setChecked(false); _ui->groupBox_depthFromScan_fillHoles->setChecked(true); @@ -1769,9 +1777,11 @@ void PreferencesDialog::readGuiSettings(const QString & filePath) _ui->doubleSpinBox_ceilingFilterHeight->setValue(settings.value("cloudCeilingHeight", _ui->doubleSpinBox_ceilingFilterHeight->value()).toDouble()); _ui->doubleSpinBox_floorFilterHeight->setValue(settings.value("cloudFloorHeight", _ui->doubleSpinBox_floorFilterHeight->value()).toDouble()); _ui->spinBox_normalKSearch->setValue(settings.value("normalKSearch", _ui->spinBox_normalKSearch->value()).toInt()); + _ui->doubleSpinBox_normalRadiusSearch->setValue(settings.value("normalRadiusSearch", _ui->doubleSpinBox_normalRadiusSearch->value()).toDouble()); _ui->doubleSpinBox_ceilingFilterHeight_scan->setValue(settings.value("scanCeilingHeight", _ui->doubleSpinBox_ceilingFilterHeight_scan->value()).toDouble()); _ui->doubleSpinBox_floorFilterHeight_scan->setValue(settings.value("scanFloorHeight", _ui->doubleSpinBox_floorFilterHeight_scan->value()).toDouble()); _ui->spinBox_normalKSearch_scan->setValue(settings.value("scanNormalKSearch", _ui->spinBox_normalKSearch_scan->value()).toInt()); + _ui->doubleSpinBox_normalRadiusSearch_scan->setValue(settings.value("scanNormalRadiusSearch", _ui->doubleSpinBox_normalRadiusSearch_scan->value()).toDouble()); _ui->checkBox_showGraphs->setChecked(settings.value("showGraphs", _ui->checkBox_showGraphs->isChecked()).toBool()); _ui->checkBox_showFrustums->setChecked(settings.value("showFrustums", _ui->checkBox_showFrustums->isChecked()).toBool()); @@ -1933,6 +1943,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->setValue(settings.value("maxDepth", _ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value()).toDouble()); _ui->doubleSpinBox_cameraImages_scanVoxelSize->setValue(settings.value("voxelSize", _ui->doubleSpinBox_cameraImages_scanVoxelSize->value()).toDouble()); _ui->spinBox_cameraImages_scanNormalsK->setValue(settings.value("normalsK", _ui->spinBox_cameraImages_scanNormalsK->value()).toInt()); + _ui->doubleSpinBox_cameraImages_scanNormalsRadius->setValue(settings.value("normalsRadius", _ui->doubleSpinBox_cameraImages_scanNormalsRadius->value()).toDouble()); settings.endGroup();//ScanFromDepth settings.beginGroup("DepthFromScan"); @@ -2155,9 +2166,11 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const settings.setValue("cloudCeilingHeight", _ui->doubleSpinBox_ceilingFilterHeight->value()); settings.setValue("cloudFloorHeight", _ui->doubleSpinBox_floorFilterHeight->value()); settings.setValue("normalKSearch", _ui->spinBox_normalKSearch->value()); + settings.setValue("normalRadiusSearch", _ui->doubleSpinBox_normalRadiusSearch->value()); settings.setValue("scanCeilingHeight", _ui->doubleSpinBox_ceilingFilterHeight_scan->value()); settings.setValue("scanFloorHeight", _ui->doubleSpinBox_floorFilterHeight_scan->value()); settings.setValue("scanNormalKSearch", _ui->spinBox_normalKSearch_scan->value()); + settings.setValue("scanNormalRadiusSearch", _ui->doubleSpinBox_normalRadiusSearch_scan->value()); settings.setValue("showGraphs", _ui->checkBox_showGraphs->isChecked()); settings.setValue("showFrustums", _ui->checkBox_showFrustums->isChecked()); @@ -2321,6 +2334,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.setValue("maxDepth", _ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value()); settings.setValue("voxelSize", _ui->doubleSpinBox_cameraImages_scanVoxelSize->value()); settings.setValue("normalsK", _ui->spinBox_cameraImages_scanNormalsK->value()); + settings.setValue("normalsRadius", _ui->doubleSpinBox_cameraImages_scanNormalsRadius->value()); settings.endGroup(); settings.beginGroup("DepthFromScan"); @@ -4286,6 +4300,10 @@ int PreferencesDialog::getNormalKSearch() const { return _ui->spinBox_normalKSearch->value(); } +double PreferencesDialog::getNormalRadiusSearch() const +{ + return _ui->doubleSpinBox_normalRadiusSearch->value(); +} double PreferencesDialog::getScanCeilingFilteringHeight() const { return _ui->doubleSpinBox_ceilingFilterHeight_scan->value(); @@ -4298,6 +4316,10 @@ int PreferencesDialog::getScanNormalKSearch() const { return _ui->spinBox_normalKSearch_scan->value(); } +double PreferencesDialog::getScanNormalRadiusSearch() const +{ + return _ui->doubleSpinBox_normalRadiusSearch_scan->value(); +} bool PreferencesDialog::isGraphsShown() const { @@ -4636,6 +4658,10 @@ int PreferencesDialog::getSourceScanNormalsK() const { return _ui->spinBox_cameraImages_scanNormalsK->value(); } +double PreferencesDialog::getSourceScanNormalsRadius() const +{ + return _ui->doubleSpinBox_cameraImages_scanNormalsRadius->value(); +} Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor) { @@ -4743,6 +4769,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor) _ui->spinBox_cameraImages_scanDownsampleStep->value(), _ui->doubleSpinBox_cameraImages_scanVoxelSize->value(), _ui->spinBox_cameraImages_scanNormalsK->value(), + _ui->doubleSpinBox_cameraImages_scanNormalsRadius->value(), this->getLaserLocalTransform()); ((CameraRGBDImages*)camera)->setTimestamps( _ui->checkBox_cameraImages_timestamps->isChecked(), @@ -4788,6 +4815,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor) _ui->spinBox_cameraImages_scanDownsampleStep->value(), _ui->doubleSpinBox_cameraImages_scanVoxelSize->value(), _ui->spinBox_cameraImages_scanNormalsK->value(), + _ui->doubleSpinBox_cameraImages_scanNormalsRadius->value(), this->getLaserLocalTransform()); ((CameraStereoImages*)camera)->setTimestamps( _ui->checkBox_cameraImages_timestamps->isChecked(), @@ -4893,6 +4921,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor) _ui->spinBox_cameraImages_scanDownsampleStep->value(), _ui->doubleSpinBox_cameraImages_scanVoxelSize->value(), _ui->spinBox_cameraImages_scanNormalsK->value(), + _ui->doubleSpinBox_cameraImages_scanNormalsRadius->value(), this->getLaserLocalTransform()); ((CameraImages*)camera)->setDepthFromScan( _ui->groupBox_depthFromScan->isChecked(), @@ -5138,7 +5167,8 @@ void PreferencesDialog::testOdometry() _ui->spinBox_cameraScanFromDepth_decimation->value(), _ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value(), _ui->doubleSpinBox_cameraImages_scanVoxelSize->value(), - _ui->spinBox_cameraImages_scanNormalsK->value()); + _ui->spinBox_cameraImages_scanNormalsK->value(), + _ui->doubleSpinBox_cameraImages_scanNormalsRadius->value()); if(isDepthFilteringAvailable()) { if(_ui->groupBox_bilateral->isChecked()) @@ -5186,7 +5216,8 @@ void PreferencesDialog::testCamera() _ui->spinBox_cameraScanFromDepth_decimation->value(), _ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value(), _ui->doubleSpinBox_cameraImages_scanVoxelSize->value(), - _ui->spinBox_cameraImages_scanNormalsK->value()); + _ui->spinBox_cameraImages_scanNormalsK->value(), + _ui->doubleSpinBox_cameraImages_scanNormalsRadius->value()); if(isDepthFilteringAvailable()) { if(_ui->groupBox_bilateral->isChecked()) diff --git a/guilib/src/ui/exportCloudsDialog.ui b/guilib/src/ui/exportCloudsDialog.ui index b87e173a..45dbca5d 100644 --- a/guilib/src/ui/exportCloudsDialog.ui +++ b/guilib/src/ui/exportCloudsDialog.ui @@ -23,9 +23,9 @@ 0 - -2684 - 773 - 4103 + 0 + 778 + 4058 @@ -52,21 +52,21 @@ - + - + - + Regenerate clouds. This can be used to regenerate the point clouds at higher density than those used for online visualization. @@ -76,7 +76,14 @@ - + + + + + + + + Voxel size. Set 0 to disable. When organized meshes are assembled, this is the radius in which the vertices of the polygons are merged. @@ -86,13 +93,6 @@ - - - - - - - @@ -103,7 +103,7 @@ - + Gain compensation. Normalize brightness of images. @@ -133,7 +133,7 @@ - + Meshing. @@ -143,7 +143,14 @@ - + + + + + + + + Cloud filtering. Remove sparse points that are far from surfaces. @@ -153,8 +160,8 @@ - - + + @@ -170,14 +177,7 @@ - - - - - - - - + Cloud smoothing using Moving Least Squares algorithm (MLS). @@ -187,7 +187,7 @@ - + m @@ -206,6 +206,13 @@ + + + + + + + @@ -216,23 +223,6 @@ - - - - - - - true - - - - - - - - - - @@ -257,6 +247,16 @@ + + + + + + + true + + + @@ -277,6 +277,23 @@ + + + + Set the search radius for the normal estimation. + + + true + + + + + + + 0.010000000000000 + + + diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 50c96e14..09a3ca15 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - 0 + -831 678 2739 @@ -95,7 +95,7 @@ QFrame::Raised - 21 + 1 @@ -512,6 +512,31 @@ Show a yellow background when the number of odometry inliers goes under this thr + + + + m + + + 1 + + + 100.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + + + @@ -597,7 +622,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - + @@ -616,7 +641,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - + @@ -635,7 +660,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - + Opacity. @@ -648,7 +673,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - + Point size (1..64). @@ -712,25 +737,6 @@ Show a yellow background when the number of odometry inliers goes under this thr - - - - m - - - 1 - - - 100.000000000000000 - - - 0.100000000000000 - - - 0.000000000000000 - - - @@ -798,12 +804,6 @@ Show a yellow background when the number of odometry inliers goes under this thr - - - - - - @@ -858,7 +858,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - + 1 @@ -871,7 +871,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - + 1 @@ -1010,6 +1010,26 @@ Show a yellow background when the number of odometry inliers goes under this thr + + + + Normal radius search. If not 0, normals will be computed and added to created cloud for visualization (keys 7, 8 and 9). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0.010000000000000 + + + @@ -1117,7 +1137,7 @@ Show a yellow background when the number of odometry inliers goes under this thr Laser Scan - + 1 @@ -1127,7 +1147,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - + Scan point size (1..64). @@ -1169,7 +1189,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - + Scan opacity. @@ -1195,7 +1215,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - + 1 @@ -1235,7 +1255,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - + @@ -1267,7 +1287,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - + @@ -1440,6 +1460,26 @@ Show a yellow background when the number of odometry inliers goes under this thr + + + + Normal radius search. If not 0, normals will be computed and added to created cloud for visualization (keys 7, 8 and 9). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0.010000000000000 + + + @@ -5283,6 +5323,29 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + Search radius for normals computation (0=disabled). Useful if the ICP registration approach is point to plane. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> + + + 0.010000000000000 + + + @@ -5995,6 +6058,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + -16 + + + 16 + + + @@ -6066,19 +6139,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - Bad signatures are ignored. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - @@ -6089,10 +6149,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Keep raw sensor data. Only useful to save loop closure computation time when features re-extraction is enabled. Disable to save RAM memory. + Bad signatures are ignored. true @@ -6122,10 +6182,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Create map labels. The first node of a map will be labelled as "map#" where # is the map ID. + Keep raw sensor data. Only useful to save loop closure computation time when features re-extraction is enabled. Disable to save RAM memory. true @@ -6145,6 +6205,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + Create map labels. The first node of a map will be labelled as "map#" where # is the map ID. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + @@ -6161,6 +6234,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + + false + + + @@ -6174,16 +6257,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - false - - - @@ -6256,13 +6329,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - -16 + + + + - - 16 + + false @@ -6292,16 +6365,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - false - - - @@ -6315,6 +6378,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + + false + + + @@ -6341,16 +6414,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - false - - - @@ -6380,10 +6443,20 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + + false + + + - If > 0 and laser scans are 3D without normals, normals will be computed with K search neighbors when creating a signature. + If > 0 and laser scans don't have normals, normals will be computed with K search neighbors when creating a signature. true @@ -6406,8 +6479,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + @@ -6429,13 +6502,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - + If > 0 and laser scans don't have normals, normals will be computed with radius search when creating a signature. - - false + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + 50 + 0 + + + + 0.010000000000000 @@ -13764,6 +13853,26 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare + + + + Search radius to compute normals for point to plane. Normals won't be recomputed if uniform sampling is disabled and that there are already normals in the laser scans. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0.010000000000000 + + + diff --git a/package.xml b/package.xml index 1b1f6912..ce2093c7 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap - 0.13.2 + 0.13.3 RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe diff --git a/tools/KittiDataset/main.cpp b/tools/KittiDataset/main.cpp index 1b06660f..f16a77c4 100644 --- a/tools/KittiDataset/main.cpp +++ b/tools/KittiDataset/main.cpp @@ -60,6 +60,7 @@ void showUsage() " --scan_step # Scan downsample step (default=10).\n" " --scan_voxel #.# Scan voxel size (default 0.3 m).\n" " --scan_k Scan normal K (default 20).\n" + " --scan_radius Scan normal radius (default 0).\n" " --map_update # Do map update each X odometry frames (default=10, which\n" " gives 1 Hz map update assuming images are at 10 Hz).\n\n" "%s\n" @@ -104,6 +105,7 @@ int main(int argc, char * argv[]) int scanStep = 10; float scanVoxel = 0.3f; int scanNormalK = 20; + float scanNormalRadius = 0.0f; std::string gtPath; if(argc < 2) { @@ -153,6 +155,15 @@ int main(int argc, char * argv[]) showUsage(); } } + else if(std::strcmp(argv[i], "--scan_radius") == 0) + { + scanNormalRadius = atof(argv[++i]); + if(scanNormalRadius < 0.0f) + { + printf("scanNormalRadius should be >= 0\n"); + showUsage(); + } + } else if(std::strcmp(argv[i], "--gt") == 0) { gtPath = argv[++i]; @@ -233,10 +244,11 @@ int main(int argc, char * argv[]) if(scan) { pathScan = path+"/velodyne"; - printf(" Scan: %s\n", pathScan.c_str()); - printf(" Scan step: %d\n", scanStep); - printf(" Scan voxel: %fm\n", scanVoxel); - printf(" Scan normal k: %d\n", scanNormalK); + printf(" Scan: %s\n", pathScan.c_str()); + printf(" Scan step: %d\n", scanStep); + printf(" Scan voxel: %fm\n", scanVoxel); + printf(" Scan normal k: %d\n", scanNormalK); + printf(" Scan normal radius: %f\n", scanNormalRadius); } if(!parameters.empty()) { @@ -338,6 +350,7 @@ int main(int argc, char * argv[]) scanStep, scanVoxel, scanNormalK, + scanNormalRadius, Transform(-0.27f, 0.0f, 0.08, 0.0f, 0.0f, 0.0f)); } diff --git a/tools/OdometryViewer/main.cpp b/tools/OdometryViewer/main.cpp index bf768be8..cc233a2b 100644 --- a/tools/OdometryViewer/main.cpp +++ b/tools/OdometryViewer/main.cpp @@ -191,6 +191,7 @@ int main (int argc, char * argv[]) float maxDepth = 4.0f; float voxelSize = rtabmap::Parameters::defaultIcpVoxelSize(); int normalsK = 0; + float normalsRadius = 0.0f; if(regStrategy == 1 || regStrategy == 2) { // icp requires scans @@ -203,8 +204,10 @@ int main (int argc, char * argv[]) rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlane(), pointToPlane); if(pointToPlane) { - normalsK = rtabmap::Parameters::defaultIcpPointToPlaneNormalNeighbors(); - rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlaneNormalNeighbors(), normalsK); + normalsK = rtabmap::Parameters::defaultIcpPointToPlaneK(); + rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlaneK(), normalsK); + normalsRadius = rtabmap::Parameters::defaultIcpPointToPlaneRadius(); + rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlaneRadius(), normalsRadius); } uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpDownsamplingStep(), "1")); @@ -310,7 +313,7 @@ int main (int argc, char * argv[]) { rtabmap::CameraThread cameraThread(camera, parameters); - cameraThread.setScanFromDepth(icp, decimation<1?1:decimation, maxDepth, voxelSize, normalsK); + cameraThread.setScanFromDepth(icp, decimation<1?1:decimation, maxDepth, voxelSize, normalsK, normalsRadius); odomThread.start(); cameraThread.start(); From 423b47a5ff452dabbc833dedc8b14f70d221ed81 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 27 Aug 2017 19:58:49 -0400 Subject: [PATCH 02/17] New 2d scan type: CV_32FC5 (x,y,normal_x,normal_y,normal_z) --- corelib/include/rtabmap/core/util3d.h | 9 +- corelib/src/Memory.cpp | 45 ++++++--- corelib/src/OccupancyGrid.cpp | 2 +- corelib/src/RegistrationIcp.cpp | 126 ++++++++++++++++---------- corelib/src/Rtabmap.cpp | 2 +- corelib/src/util3d.cpp | 113 +++++++++++++++++++---- corelib/src/util3d_surface.cpp | 3 +- corelib/src/util3d_transforms.cpp | 84 +++++++++-------- guilib/src/DatabaseViewer.cpp | 4 +- guilib/src/EditDepthArea.h | 12 +-- guilib/src/MainWindow.cpp | 13 ++- guilib/src/ui/preferencesDialog.ui | 85 +++-------------- 12 files changed, 292 insertions(+), 206 deletions(-) diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h index 87233169..28b9f629 100644 --- a/corelib/include/rtabmap/core/util3d.h +++ b/corelib/include/rtabmap/core/util3d.h @@ -192,17 +192,20 @@ pcl::PointCloud RTABMAP_EXP laserScanFromDepthImages( // return CV_32FC3 (x,y,z) cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & 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 & cloud, const Transform & transform = Transform()); cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & cloud, const pcl::PointCloud & normals, const Transform & transform = Transform()); // return CV_32FC4 (x,y,z,rgb) cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud & 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 & cloud, const pcl::PointCloud & 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 & cloud, const Transform & transform = Transform()); // return CV_32FC2 (x,y) cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform()); +// return CV_32FC5 (x,y,normal_x, normal_y, normal_z) +cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform = Transform()); +cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud & cloud, const pcl::PointCloud & normals, const Transform & transform = Transform()); // For laserScan of type CV_32FC2, z is set to null. pcl::PointCloud::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. diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 2e4abb9d..ace57d42 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -2438,6 +2438,7 @@ Transform Memory::computeIcpTransformMulti( std::string msg; int maxPoints = fromScan.cols; pcl::PointCloud::Ptr assembledToClouds(new pcl::PointCloud); + bool is2D = true; for(std::map::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::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::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); diff --git a/corelib/src/OccupancyGrid.cpp b/corelib/src/OccupancyGrid.cpp index f7fe3dd2..0af41863 100644 --- a/corelib/src/OccupancyGrid.cpp +++ b/corelib/src/OccupancyGrid.cpp @@ -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 diff --git a/corelib/src/RegistrationIcp.cpp b/corelib/src/RegistrationIcp.cpp index fae10208..a9f8e3e2 100644 --- a/corelib/src/RegistrationIcp.cpp +++ b/corelib/src/RegistrationIcp.cpp @@ -39,11 +39,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #ifdef RTABMAP_POINTMATCHER +#include #include "pointmatcher/PointMatcher.h" typedef PointMatcher PM; typedef PM::DataPoints DP; -DP pclToDP(const pcl::PointCloud::Ptr & pclCloud) +DP pclToDP(const pcl::PointCloud::Ptr & pclCloud, bool is2D) { UDEBUG(""); typedef DP::Label Label; @@ -63,8 +64,11 @@ DP pclToDP(const pcl::PointCloud::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::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; isize(); ++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::Ptr & pclCloud) +DP pclToDP(const pcl::PointCloud::Ptr & pclCloud, bool is2D) { UDEBUG(""); typedef DP::Label Label; @@ -105,8 +110,11 @@ DP pclToDP(const pcl::PointCloud::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::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; isize(); ++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 & 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) 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; ioutlierFilters.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::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::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform); pcl::PointCloud::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toLocalTransform); + fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals); + toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals); + + UDEBUG("Conversion time = %f s", timer.ticks()); pcl::PointCloud::Ptr fromCloudNormalsRegistered(new pcl::PointCloud()); #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::Ptr fromCloudRegistered(new pcl::PointCloud()); 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::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::Ptr fromCloudNormals(new pcl::PointCloud); 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; diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index facc0eb8..61fa3fef 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -1116,7 +1116,7 @@ bool Rtabmap::process( else { UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str()); - if(info.covariance.at(0,0) > 0.0 && info.covariance.at(5,5) > 0.0) + if(!info.covariance.empty() && info.covariance.at(0,0) > 0.0 && info.covariance.at(0,0) != 1.0 && info.covariance.at(5,5) > 0.0 && info.covariance.at(5,5) != 1.0) { _memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv())); } diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index f73cda49..a05a0714 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -1459,9 +1459,75 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud & cloud, return laserScan; } +cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud & cloud, const Transform & transform) +{ + cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5)); + bool nullTransform = transform.isNull(); + for(unsigned int i=0; i(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 & cloud, const pcl::PointCloud & 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(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::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::Ptr output(new pcl::PointCloud); output->resize(laserScan.cols); @@ -1480,7 +1546,7 @@ pcl::PointCloud::Ptr laserScanToPointCloud(const cv::Mat & laserS pcl::PointCloud::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::Ptr output(new pcl::PointCloud); output->resize(laserScan.cols); @@ -1498,7 +1564,7 @@ pcl::PointCloud::Ptr laserScanToPointCloudNormal(const cv::Mat pcl::PointCloud::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::Ptr output(new pcl::PointCloud); output->resize(laserScan.cols); @@ -1517,7 +1583,7 @@ pcl::PointCloud::Ptr laserScanToPointCloudRGB(const cv::Mat & pcl::PointCloud::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::Ptr output(new pcl::PointCloud); output->resize(laserScan.cols); @@ -1536,12 +1602,12 @@ pcl::PointCloud::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(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(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(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(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(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(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]; diff --git a/corelib/src/util3d_surface.cpp b/corelib/src/util3d_surface.cpp index b4f53643..f407c12e 100644 --- a/corelib/src/util3d_surface.cpp +++ b/corelib/src/util3d_surface.cpp @@ -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 { diff --git a/corelib/src/util3d_transforms.cpp b/corelib/src/util3d_transforms.cpp index 15626527..2f2c886c 100644 --- a/corelib/src/util3d_transforms.cpp +++ b/corelib/src/util3d_transforms.cpp @@ -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(0, i); + float * out = output.ptr(0, i); if(laserScan.type() == CV_32FC2) { - pcl::PointXYZ pt( - laserScan.at(i)[0], - laserScan.at(i)[1], 0); - pt = util3d::transformPoint(pt, transform); - output.at(i)[0] = pt.x; - output.at(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(i)[0], - laserScan.at(i)[1], - laserScan.at(i)[2]); - pt = util3d::transformPoint(pt, transform); - output.at(i)[0] = pt.x; - output.at(i)[1] = pt.y; - output.at(i)[2] = pt.z; + const float * ptr = laserScan.ptr(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(i)[0], - laserScan.at(i)[1], - laserScan.at(i)[2]); - pt = util3d::transformPoint(pt, transform); - output.at(i)[0] = pt.x; - output.at(i)[1] = pt.y; - output.at(i)[2] = pt.z; - } - else + else if(laserScan.type() == CV_32FC(5)) { pcl::PointNormal pt; - pt.x=laserScan.at(i)[0]; - pt.y=laserScan.at(i)[1]; - pt.z=laserScan.at(i)[2]; - pt.normal_x=laserScan.at(i)[3]; - pt.normal_y=laserScan.at(i)[4]; - pt.normal_z=laserScan.at(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(i)[0] = pt.x; - output.at(i)[1] = pt.y; - output.at(i)[2] = pt.z; - output.at(i)[3] = pt.normal_x; - output.at(i)[4] = pt.normal_y; - output.at(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; } } } diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index a3a54a9a..ae2ac6d9 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -3757,7 +3757,7 @@ void DatabaseViewer::updateConstraintView( constraintsViewer_->removeCloud("scan1"); if(!dataFrom.laserScanRaw().empty()) { - if(dataFrom.laserScanRaw().channels() == 6) + if(dataFrom.laserScanRaw().channels() >= 5) { pcl::PointCloud::Ptr scan; scan = rtabmap::util3d::laserScanToPointCloudNormal(dataFrom.laserScanRaw(), dataFrom.laserScanInfo().localTransform()); @@ -3780,7 +3780,7 @@ void DatabaseViewer::updateConstraintView( } if(!dataTo.laserScanRaw().empty()) { - if(dataTo.laserScanRaw().channels() == 6) + if(dataTo.laserScanRaw().channels() >= 5) { pcl::PointCloud::Ptr scan; scan = rtabmap::util3d::laserScanToPointCloudNormal(dataTo.laserScanRaw(), t*dataTo.laserScanInfo().localTransform()); diff --git a/guilib/src/EditDepthArea.h b/guilib/src/EditDepthArea.h index fba98515..b2921a33 100644 --- a/guilib/src/EditDepthArea.h +++ b/guilib/src/EditDepthArea.h @@ -57,12 +57,12 @@ public slots: void resetChanges(); protected: - void mousePressEvent(QMouseEvent *event) override; - void mouseMoveEvent(QMouseEvent *event) override; - void mouseReleaseEvent(QMouseEvent *event) override; - void paintEvent(QPaintEvent *event) override; - void resizeEvent(QResizeEvent *event) override; - void contextMenuEvent(QContextMenuEvent * e) override; + virtual void mousePressEvent(QMouseEvent *event); + virtual void mouseMoveEvent(QMouseEvent *event); + virtual void mouseReleaseEvent(QMouseEvent *event); + virtual void paintEvent(QPaintEvent *event); + virtual void resizeEvent(QResizeEvent *event); + virtual void contextMenuEvent(QContextMenuEvent * e); private: void drawLineTo(const QPoint &endPoint); diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 90e3c32a..b32b27f3 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -2888,7 +2888,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m { 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()); } @@ -2983,7 +2983,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m pcl::PointCloud::Ptr normals; if(cloud->size()) { - if(scan.channels() == 2) + if(scan.channels() == 2 || scan.channels() == 5) { 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); 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()) diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 09a3ca15..cdd5246f 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,25 +63,16 @@ 0 - -831 + 0 678 - 2739 + 2736 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -95,7 +86,7 @@ QFrame::Raised - 1 + 21 @@ -4570,16 +4561,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki Directory of images (optional settings) - - 0 - - - 0 - - - 0 - - + 0 @@ -12580,16 +12562,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - 0 - - - 0 - - - 0 - - + 0 @@ -12729,16 +12702,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - - 0 - - - 0 - - - 0 - - + 0 @@ -12896,16 +12860,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -12985,16 +12940,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -13106,16 +13052,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -13808,7 +13745,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - Point to plane ICP. Only for ICP 3D. + Point to plane ICP. true From b70ffb6331540e51ac4f4288bd22b7a94929c485 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 28 Aug 2017 16:15:22 -0400 Subject: [PATCH 03/17] fixed multi scans matching with normals, removed some debug logs --- corelib/src/Graph.cpp | 2 +- corelib/src/Memory.cpp | 67 +++++++++++------- corelib/src/RegistrationIcp.cpp | 6 +- corelib/src/Rtabmap.cpp | 16 ++--- guilib/src/CloudViewer.cpp | 2 +- guilib/src/DatabaseViewer.cpp | 24 +++---- guilib/src/GraphViewer.cpp | 2 +- guilib/src/MainWindow.cpp | 119 ++++++++++++++++++-------------- 8 files changed, 135 insertions(+), 103 deletions(-) diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index 542e9fdb..433fa900 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -1603,7 +1603,7 @@ int findNearestNode( kdTree->nearestKSearch(pt, 1, ind, dist); if(ind.size() && dist.size() && ind[0] >= 0) { - UDEBUG("Nearest node = %d: %f", ids[ind[0]], dist[0]); + //UDEBUG("Nearest node = %d: %f", ids[ind[0]], dist[0]); id = ids[ind[0]]; } } diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index ace57d42..e8406d3c 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -2406,6 +2406,18 @@ Transform Memory::computeIcpTransformMulti( UASSERT(uContains(poses, toId) && uContains(_signatures, toId)); UDEBUG("Guess=%s", (poses.at(fromId).inverse() * poses.at(toId)).prettyPrint().c_str()); + if(ULogger::level() == ULogger::kDebug) + { + std::string ids; + for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) + { + if(iter->first != fromId) + { + ids += uNumber2Str(iter->first) + " "; + } + } + UDEBUG("%d vs %s", fromId, ids.c_str()); + } // make sure that all laser scans are loaded std::list depthToLoad; @@ -2438,6 +2450,7 @@ Transform Memory::computeIcpTransformMulti( std::string msg; int maxPoints = fromScan.cols; pcl::PointCloud::Ptr assembledToClouds(new pcl::PointCloud); + pcl::PointCloud::Ptr assembledToNormalClouds(new pcl::PointCloud); bool is2D = true; for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) { @@ -2454,14 +2467,26 @@ Transform Memory::computeIcpTransformMulti( { is2D = false; } - pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloud( - scan, - s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second); + + if(scan.channels() >= 5) + { + pcl::PointCloud::Ptr cloudNormal = util3d::laserScanToPointCloudNormal( + scan, + s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second); + *assembledToNormalClouds += *cloudNormal; + } + else + { + pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloud( + scan, + s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second); + *assembledToClouds += *cloud; + } + if(scan.cols > maxPoints) { maxPoints = scan.cols; } - *assembledToClouds += *cloud; } } else @@ -2470,28 +2495,22 @@ Transform Memory::computeIcpTransformMulti( } } } - if(assembledToClouds->size()) - { - 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 - } + cv::Mat assembledScan; + if(assembledToNormalClouds->size()) + { + assembledScan = is2D?util3d::laserScan2dFromPointCloud(*assembledToNormalClouds):util3d::laserScanFromPointCloud(*assembledToNormalClouds); } + else if(assembledToClouds->size()) + { + assembledScan = is2D?util3d::laserScan2dFromPointCloud(*assembledToClouds):util3d::laserScanFromPointCloud(*assembledToClouds); + } + // scans are in base frame but for 2d scans, set the height so that correspondences matching works + assembledData.setLaserScanRaw(assembledScan, + LaserScanInfo( + fromS->sensorData().laserScanInfo().maxPoints()?fromS->sensorData().laserScanInfo().maxPoints():maxPoints, + fromS->sensorData().laserScanInfo().maxRange(), + is2D?Transform(0,0,fromS->sensorData().laserScanInfo().localTransform().z(),0,0,0):Transform::getIdentity())); Transform guess = poses.at(fromId).inverse() * poses.at(toId); std::vector inliersV; diff --git a/corelib/src/RegistrationIcp.cpp b/corelib/src/RegistrationIcp.cpp index a9f8e3e2..ac7acdfb 100644 --- a/corelib/src/RegistrationIcp.cpp +++ b/corelib/src/RegistrationIcp.cpp @@ -425,8 +425,8 @@ Transform RegistrationIcp::computeTransformationImpl( if( _pointToPlane && _voxelSize == 0.0f && - fromScan.channels() >= 6 && - toScan.channels() >= 6 && + fromScan.channels() >= 5 && + toScan.channels() >= 5 && !((fromScan.channels() == 5 || toScan.channels() == 5) && !_libpointmatcher)) // PCL crashes if 2D) { //special case if we have already normals computed and there is no filtering @@ -454,8 +454,8 @@ Transform RegistrationIcp::computeTransformationImpl( PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP); UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)"); T = icp(data, ref); - UDEBUG("libpointmatcher icp...done!"); icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim(T.template cast(), 4)))); + UDEBUG("libpointmatcher icp...done! T=%s", icpT.prettyPrint().c_str()); float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio(); UDEBUG("match ratio: %f", matchRatio); diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 61fa3fef..6aaf2265 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -1228,9 +1228,7 @@ bool Rtabmap::process( //============================================================ if(_proximityByTime && rehearsedId == 0 && // don't do it if rehearsal happened - signature->getWords3().size() && _memory->isIncremental() && // don't do it in localization mode - !signature->isBadSignature() && signature->getWeight()>=0) { const std::set & stm = _memory->getStMem(); @@ -1511,7 +1509,7 @@ bool Rtabmap::process( ++immunizedGlobally; } - UDEBUG("nt=%d m=%d immunized=1", iter->first, iter->second); + //UDEBUG("nt=%d m=%d immunized=1", iter->first, iter->second); } neighbors.erase(iter++); } @@ -1557,7 +1555,7 @@ bool Rtabmap::process( { ++nbDirectNeighborsInDb; } - UDEBUG("nt=%d m=%d", iter->first, iter->second); + //UDEBUG("nt=%d m=%d", iter->first, iter->second); } neighbors.erase(iter++); } @@ -1698,7 +1696,7 @@ bool Rtabmap::process( { ++immunizedLocally; } - UDEBUG("local node %d on path immunized=1", iter->first); + //UDEBUG("local node %d on path immunized=1", iter->first); } } } @@ -1745,7 +1743,7 @@ bool Rtabmap::process( { ++immunizedLocally; } - UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first); + //UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first); } } } @@ -2006,7 +2004,7 @@ bool Rtabmap::process( //find the nearest pose on the path int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id())); UASSERT(nearestId > 0); - UDEBUG("Path %d (size=%d) distance=%fm", nearestId, (int)path.size(), _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId))); + //UDEBUG("Path %d (size=%d) distance=%fm", nearestId, (int)path.size(), _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId))); // nearest pose must be close and not linked to current location if(!signature->hasLink(nearestId) && @@ -2101,7 +2099,7 @@ bool Rtabmap::process( } else { - UDEBUG("Path %d ignored", nearestId); + //UDEBUG("Path %d ignored", nearestId); } } } @@ -2996,7 +2994,7 @@ std::map > Rtabmap::getPaths(std::mapfirst); + //UDEBUG("%d <- %d", nearestId, jter->first); path.insert(*jter); poses.erase(jter); } diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index 00ef8032..0edff4a7 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -496,7 +496,7 @@ bool CloudViewer::updateCloudPose( { if(_addedClouds.contains(id)) { - UDEBUG("Updating pose %s to %s", id.c_str(), pose.prettyPrint().c_str()); + //UDEBUG("Updating pose %s to %s", id.c_str(), pose.prettyPrint().c_str()); bool samePose = _addedClouds.find(id).value() == pose; Eigen::Affine3f posef = pose.toEigen3f(); if(samePose || diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index ae2ac6d9..e5603b6f 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -4020,19 +4020,19 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) UINFO("rotational_min=%f", rotational_min); UINFO("rotational_max=%f", rotational_max); - ui_->toolBox_statistics->updateStat("GT/translational_rmse/", translational_rmse, false); - ui_->toolBox_statistics->updateStat("GT/translational_mean/", translational_mean, false); - ui_->toolBox_statistics->updateStat("GT/translational_median/", translational_median, false); - ui_->toolBox_statistics->updateStat("GT/translational_std/", translational_std, false); - ui_->toolBox_statistics->updateStat("GT/translational_min/", translational_min, false); - ui_->toolBox_statistics->updateStat("GT/translational_max/", translational_max, false); + ui_->toolBox_statistics->updateStat("GT/translational rmse/", translational_rmse, false); + ui_->toolBox_statistics->updateStat("GT/translational mean/", translational_mean, false); + ui_->toolBox_statistics->updateStat("GT/translational median/", translational_median, false); + ui_->toolBox_statistics->updateStat("GT/translational std/", translational_std, false); + ui_->toolBox_statistics->updateStat("GT/translational min/", translational_min, false); + ui_->toolBox_statistics->updateStat("GT/translational max/", translational_max, false); - ui_->toolBox_statistics->updateStat("GT/rotational_rmse/", rotational_rmse, false); - ui_->toolBox_statistics->updateStat("GT/rotational_mean/", rotational_mean, false); - ui_->toolBox_statistics->updateStat("GT/rotational_median/", rotational_median, false); - ui_->toolBox_statistics->updateStat("GT/rotational_std/", rotational_std, false); - ui_->toolBox_statistics->updateStat("GT/rotational_min/", rotational_min, false); - ui_->toolBox_statistics->updateStat("GT/rotational_max/", rotational_max, false); + ui_->toolBox_statistics->updateStat("GT/rotational rmse/", rotational_rmse, false); + ui_->toolBox_statistics->updateStat("GT/rotational mean/", rotational_mean, false); + ui_->toolBox_statistics->updateStat("GT/rotational median/", rotational_median, false); + ui_->toolBox_statistics->updateStat("GT/rotational std/", rotational_std, false); + ui_->toolBox_statistics->updateStat("GT/rotational min/", rotational_min, false); + ui_->toolBox_statistics->updateStat("GT/rotational max/", rotational_max, false); } } diff --git a/guilib/src/GraphViewer.cpp b/guilib/src/GraphViewer.cpp index 98e5f70f..2380ccdf 100644 --- a/guilib/src/GraphViewer.cpp +++ b/guilib/src/GraphViewer.cpp @@ -673,7 +673,7 @@ void GraphViewer::updatePosterior(const std::map & posterior) std::map::const_iterator jter = posterior.find(iter.key()); if(jter != posterior.end()) { - UDEBUG("id=%d max=%f hyp=%f color = %f", iter.key(), max, jter->second, (1-jter->second/max)*240.0f/360.0f); + //UDEBUG("id=%d max=%f hyp=%f color = %f", iter.key(), max, jter->second, (1-jter->second/max)*240.0f/360.0f); iter.value()->setColor(QColor::fromHsvF((1-jter->second/max)*240.0f/360.0f, 1, 1, 1)); //0=red 240=blue } else diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index b32b27f3..82568ee3 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -3309,35 +3309,35 @@ Transform MainWindow::alignPosesToGroundTruth( if((_preferencesDialog->isTimeUsedInFigures() && stamp > 0.0) || (refId && refId>=0)) { - _ui->statsToolBox->updateStat("GT/translational_rmse/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_rmse, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("GT/translational_mean/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_mean, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("GT/translational_median/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_median, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("GT/translational_std/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_std, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("GT/translational_min/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_min, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("GT/translational_max/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_max, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/translational rmse/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_rmse, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/translational mean/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_mean, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/translational median/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_median, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/translational std/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_std, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/translational min/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_min, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/translational max/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_max, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("GT/rotational_rmse/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_rmse, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("GT/rotational_mean/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_mean, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("GT/rotational_median/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_median, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("GT/rotational_std/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_std, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("GT/rotational_min/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_min, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("GT/rotational_max/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_max, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/rotational rmse/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_rmse, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/rotational mean/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_mean, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/rotational median/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_median, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/rotational std/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_std, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/rotational min/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_min, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/rotational max/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_max, _preferencesDialog->isCacheSavedInFigures()); } else { - _ui->statsToolBox->updateStat("GT/translational_rmse/", translational_rmse, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("GT/translational_mean/", translational_mean, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("GT/translational_median/", translational_median, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("GT/translational_std/", translational_std, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("GT/translational_min/", translational_min, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("GT/translational_max/", translational_max, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/translational rmse/", translational_rmse, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/translational mean/", translational_mean, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/translational median/", translational_median, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/translational std/", translational_std, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/translational min/", translational_min, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/translational max/", translational_max, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("GT/rotational_rmse/", rotational_rmse, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("GT/rotational_mean/", rotational_mean, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("GT/rotational_median/", rotational_median, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("GT/rotational_std/", rotational_std, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("GT/rotational_min/", rotational_min, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("GT/rotational_max/", rotational_max, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/rotational rmse/", rotational_rmse, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/rotational mean/", rotational_mean, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/rotational median/", rotational_median, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/rotational std/", rotational_std, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/rotational min/", rotational_min, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("GT/rotational max/", rotational_max, _preferencesDialog->isCacheSavedInFigures()); } } } @@ -3348,43 +3348,58 @@ void MainWindow::updateNodeVisibility(int nodeId, bool visible) { UINFO("Update visibility %d", nodeId); QMap viewerClouds = _cloudViewer->getAddedClouds(); - if(_preferencesDialog->isCloudsShown(0)) + Transform pose; + if(_currentGTPosesMap.size() && + _ui->actionAnchor_clouds_to_ground_truth->isChecked() && + _currentGTPosesMap.find(nodeId)!=_currentGTPosesMap.end()) { - std::string cloudName = uFormat("cloud%d", nodeId); - if(visible && !viewerClouds.contains(cloudName) && _cachedSignatures.contains(nodeId) && _currentPosesMap.find(nodeId) != _currentPosesMap.end()) - { - createAndAddCloudToMap(nodeId, _currentPosesMap.find(nodeId)->second, uValue(_currentMapIds, nodeId, -1)); - } - else if(viewerClouds.contains(cloudName)) - { - if(visible && _currentPosesMap.find(nodeId) != _currentPosesMap.end()) - { - //make sure the transformation was done - _cloudViewer->updateCloudPose(cloudName, _currentPosesMap.find(nodeId)->second); - } - _cloudViewer->setCloudVisibility(cloudName, visible); - } + pose = _currentGTPosesMap.at(nodeId); + } + else if(_currentPosesMap.find(nodeId) != _currentPosesMap.end()) + { + pose = _currentPosesMap.at(nodeId); } - if(_preferencesDialog->isScansShown(0)) + if(!pose.isNull() || !visible) { - std::string scanName = uFormat("scan%d", nodeId); - if(visible && !viewerClouds.contains(scanName) && _cachedSignatures.contains(nodeId) && _currentPosesMap.find(nodeId) != _currentPosesMap.end()) + if(_preferencesDialog->isCloudsShown(0)) { - createAndAddScanToMap(nodeId, _currentPosesMap.find(nodeId)->second, uValue(_currentMapIds, nodeId, -1)); - } - else if(viewerClouds.contains(scanName)) - { - if(visible && _currentPosesMap.find(nodeId) != _currentPosesMap.end()) + std::string cloudName = uFormat("cloud%d", nodeId); + if(visible && !viewerClouds.contains(cloudName) && _cachedSignatures.contains(nodeId)) { - //make sure the transformation was done - _cloudViewer->updateCloudPose(scanName, _currentPosesMap.find(nodeId)->second); + createAndAddCloudToMap(nodeId, pose, uValue(_currentMapIds, nodeId, -1)); + } + else if(viewerClouds.contains(cloudName)) + { + if(visible) + { + //make sure the transformation was done + _cloudViewer->updateCloudPose(cloudName, pose); + } + _cloudViewer->setCloudVisibility(cloudName, visible); } - _cloudViewer->setCloudVisibility(scanName, visible); } - } - _cloudViewer->update(); + if(_preferencesDialog->isScansShown(0)) + { + std::string scanName = uFormat("scan%d", nodeId); + if(visible && !viewerClouds.contains(scanName) && _cachedSignatures.contains(nodeId)) + { + createAndAddScanToMap(nodeId, pose, uValue(_currentMapIds, nodeId, -1)); + } + else if(viewerClouds.contains(scanName)) + { + if(visible) + { + //make sure the transformation was done + _cloudViewer->updateCloudPose(scanName, pose); + } + _cloudViewer->setCloudVisibility(scanName, visible); + } + } + + _cloudViewer->update(); + } } void MainWindow::updateGraphView() From 7ec58c63e93ab6083baadc114c25f3faf8ff6bb0 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 28 Aug 2017 17:37:57 -0400 Subject: [PATCH 04/17] SensorData/Memory: fixed assert for scan channels 5 an 7 --- corelib/src/Memory.cpp | 2 +- corelib/src/SensorData.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index e8406d3c..eed46118 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -3310,7 +3310,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p data.depthOrRightRaw().rows, data.depthOrRightRaw().type(), CV_16UC1, CV_32FC1, CV_8UC1).c_str()); - UASSERT(data.laserScanRaw().empty() || data.laserScanRaw().type() == CV_32FC2 || data.laserScanRaw().type() == CV_32FC3 || data.laserScanRaw().type() == CV_32FC(4) || data.laserScanRaw().type() == CV_32FC(6)); + UASSERT(data.laserScanRaw().empty() || data.laserScanRaw().type() == CV_32FC2 || data.laserScanRaw().type() == CV_32FC3 || data.laserScanRaw().type() == CV_32FC(4) || data.laserScanRaw().type() == CV_32FC(5) || data.laserScanRaw().type() == CV_32FC(6) || data.laserScanRaw().type() == CV_32FC(7)); if(!data.depthOrRightRaw().empty() && data.cameraModels().size() == 0 && diff --git a/corelib/src/SensorData.cpp b/corelib/src/SensorData.cpp index af5cf7d9..7a45b953 100644 --- a/corelib/src/SensorData.cpp +++ b/corelib/src/SensorData.cpp @@ -195,7 +195,7 @@ SensorData::SensorData( _depthOrRightRaw = depth; } - if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6)) + if(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)) { _laserScanRaw = laserScan; } @@ -300,7 +300,7 @@ SensorData::SensorData( _depthOrRightRaw = depth; } - if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6)) + if(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)) { _laserScanRaw = laserScan; } @@ -406,7 +406,7 @@ SensorData::SensorData( _depthOrRightRaw = right; } - if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6)) + if(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)) { _laserScanRaw = laserScan; } @@ -496,7 +496,7 @@ void SensorData::setOccupancyGrid( if(!ground.empty()) { - if(ground.type() == CV_32FC2 || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(6)) + if(ground.type() == CV_32FC2 || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(5) || ground.type() == CV_32FC(6) || ground.type() == CV_32FC(7)) { _groundCellsRaw = ground; ctGround.start(); @@ -509,7 +509,7 @@ void SensorData::setOccupancyGrid( } if(!obstacles.empty()) { - if(obstacles.type() == CV_32FC2 || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(6)) + if(obstacles.type() == CV_32FC2 || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(5) || obstacles.type() == CV_32FC(6) || obstacles.type() == CV_32FC(7)) { _obstacleCellsRaw = obstacles; ctObstacles.start(); From 4f55b56d6b2b88961cb8b8f8ea83923986e7a651 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 31 Aug 2017 18:01:46 -0400 Subject: [PATCH 05/17] Odometry: 2D scans + normals support --- corelib/include/rtabmap/core/Odometry.h | 2 + corelib/src/Odometry.cpp | 5 +- corelib/src/OdometryF2F.cpp | 10 ++- corelib/src/OdometryF2M.cpp | 59 ++++++++++++------ corelib/src/RegistrationIcp.cpp | 82 +++++++++++++++++++------ corelib/src/util3d_registration.cpp | 8 +-- guilib/src/DatabaseViewer.cpp | 3 +- 7 files changed, 121 insertions(+), 48 deletions(-) diff --git a/corelib/include/rtabmap/core/Odometry.h b/corelib/include/rtabmap/core/Odometry.h index d378979f..64baec3d 100644 --- a/corelib/include/rtabmap/core/Odometry.h +++ b/corelib/include/rtabmap/core/Odometry.h @@ -68,6 +68,7 @@ public: bool isInfoDataFilled() const {return _fillInfoData;} const Transform & previousVelocityTransform() const {return previousVelocityTransform_;} double previousStamp() const {return previousStamp_;} + unsigned int framesProcessed() const {return framesProcessed_;} private: virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0; @@ -98,6 +99,7 @@ private: Transform previousVelocityTransform_; Transform previousGroundTruthPose_; float distanceTravelled_; + unsigned int framesProcessed_; std::vector particleFilters_; cv::KalmanFilter kalmanFilter_; diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index 8098389d..dd6709ff 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -102,7 +102,8 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) : _pose(Transform::getIdentity()), _resetCurrentCount(0), previousStamp_(0), - distanceTravelled_(0) + distanceTravelled_(0), + framesProcessed_(0) { Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown); @@ -168,6 +169,7 @@ void Odometry::reset(const Transform & initialPose) _resetCurrentCount = 0; previousStamp_ = 0; distanceTravelled_ = 0; + framesProcessed_ = 0; if(_force3DoF || particleFilters_.size()) { float x,y,z, roll,pitch,yaw; @@ -544,6 +546,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet distanceTravelled_ += t.getNorm(); info->distanceTravelled = distanceTravelled_; } + ++framesProcessed_; return _pose *= t; // update } diff --git a/corelib/src/OdometryF2F.cpp b/corelib/src/OdometryF2F.cpp index 51520518..73a0fea5 100644 --- a/corelib/src/OdometryF2F.cpp +++ b/corelib/src/OdometryF2F.cpp @@ -83,6 +83,7 @@ Transform OdometryF2F::computeTransform( return output; } + bool addKeyFrame = false; RegistrationInfo regInfo; UASSERT(!this->getPose().isNull()); @@ -99,8 +100,8 @@ Transform OdometryF2F::computeTransform( output = registrationPipeline_->computeTransformationMod( tmpRefFrame, newFrame, - // special case for ICP-only odom, set guess to identity if we just started - !guess.isNull()?motionSinceLastKeyFrame*guess:!registrationPipeline_->isImageRequired()&&this->getPose().isIdentity()?Transform::getIdentity():Transform(), + // special case for ICP-only odom, set guess to identity if we just started or reset + !guess.isNull()?motionSinceLastKeyFrame*guess:!registrationPipeline_->isImageRequired()&&this->framesProcessed()<2?motionSinceLastKeyFrame:Transform(), ®Info); if(output.isNull() && !guess.isNull() && registrationPipeline_->isImageRequired()) @@ -185,7 +186,7 @@ Transform OdometryF2F::computeTransform( { UDEBUG("Update key frame"); int features = newFrame.getWordsDescriptors().size(); - if(features == 0) + if(registrationPipeline_->isImageRequired() && features == 0) { newFrame = Signature(data); // this will generate features only for the first frame or if optical flow was used (no 3d words) @@ -209,6 +210,8 @@ Transform OdometryF2F::computeTransform( //reset motion lastKeyFramePose_.setNull(); + + addKeyFrame = true; } else { @@ -249,6 +252,7 @@ Transform OdometryF2F::computeTransform( info->icpInliersRatio = regInfo.icpInliersRatio; info->matches = regInfo.matches; info->features = newFrame.sensorData().keypoints().size(); + info->keyFrameAdded = addKeyFrame; } UINFO("Odom update time = %fs lost=%s inliers=%d, ref frame corners=%d, transform accepted=%s", diff --git a/corelib/src/OdometryF2M.cpp b/corelib/src/OdometryF2M.cpp index 9103d8c6..5a5d5d32 100644 --- a/corelib/src/OdometryF2M.cpp +++ b/corelib/src/OdometryF2M.cpp @@ -186,9 +186,10 @@ Transform OdometryF2M::computeTransform( Transform transform = regPipeline_->computeTransformationMod( tmpMap, *lastFrame_, - // special case for ICP-only odom, set guess to identity if we just started - !guess.isNull()?this->getPose()*guess:!regPipeline_->isImageRequired()&&this->getPose().isIdentity()?Transform::getIdentity():Transform(), + // special case for ICP-only odom, set guess to identity if we just started or reset + !guess.isNull()?this->getPose()*guess:!regPipeline_->isImageRequired()&&this->framesProcessed()<2?this->getPose():Transform(), ®Info); + if(transform.isNull() && !guess.isNull() && regPipeline_->isImageRequired()) { tmpMap = *map_; @@ -591,7 +592,7 @@ Transform OdometryF2M::computeTransform( if(lastFrame_->sensorData().laserScanRaw().cols) { - pcl::PointCloud::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan); + pcl::PointCloud::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan, tmpMap.sensorData().laserScanInfo().localTransform()); pcl::PointCloud::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform()); pcl::IndicesPtr frameCloudNormalsIndices(new std::vector); @@ -623,17 +624,6 @@ Transform OdometryF2M::computeTransform( newPoints, scanMaximumMapSize_); - if(newPoints < 20) - { - UWARN("The number of new scan points added to local odometry " - "map is low (%d), you may want to decrease the parameter \"%s\" " - "(current value=%f and ICP inliers ratio is %f)", - newPoints, - Parameters::kOdomScanKeyFrameThr().c_str(), - scanKeyFrameThr_, - regInfo.icpInliersRatio); - } - if(scansBuffer_.size() > 1 && int(mapCloudNormals->size() + newPoints) > scanMaximumMapSize_) { @@ -691,7 +681,16 @@ Transform OdometryF2M::computeTransform( *mapCloudNormals += *scansBuffer_.back().first; } } - mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals); + if(mapScan.channels() == 2 || mapScan.channels() == 5) + { + Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0); + mapScan = util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint); + } + else + { + Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0); + mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint); + } modified=true; } } @@ -702,7 +701,18 @@ Transform OdometryF2M::computeTransform( { *map_ = tmpMap; - map_->sensorData().setLaserScanRaw(mapScan, LaserScanInfo(0, 0)); + if(mapScan.channels() == 2 || mapScan.channels() == 5) + { + + map_->sensorData().setLaserScanRaw(mapScan, + LaserScanInfo(0, 0.0f, Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanInfo().localTransform().z(),0,0,0))); + } + else + { + map_->sensorData().setLaserScanRaw(mapScan, + LaserScanInfo(0, 0.0f, newFramePose.translation())); + } + map_->setWords(mapWords); map_->setWords3(mapPoints); map_->setWordsDescriptors(mapDescriptors); @@ -717,7 +727,7 @@ Transform OdometryF2M::computeTransform( if(this->isInfoDataFilled()) { info->localMap = uMultimapToMap(tmpMap.getWords3()); - info->localScanMap = tmpMap.sensorData().laserScanRaw(); + info->localScanMap = util3d::transformLaserScan(tmpMap.sensorData().laserScanRaw(), tmpMap.sensorData().laserScanInfo().localTransform()); } } } @@ -831,7 +841,18 @@ Transform OdometryF2M::computeTransform( frameValid = true; pcl::PointCloud::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform()); scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector))); - map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), LaserScanInfo(0,0)); + if(lastFrame_->sensorData().laserScanRaw().channels() == 2 || lastFrame_->sensorData().laserScanRaw().channels() == 5) + { + Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0); + map_->sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint), + LaserScanInfo(0, 0.0f, Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanInfo().localTransform().z(),0,0,0))); + } + else + { + Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0); + map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint), + LaserScanInfo(0, 0.0f, newFramePose.translation())); + } addKeyFrame = true; } else @@ -854,7 +875,7 @@ Transform OdometryF2M::computeTransform( if(this->isInfoDataFilled()) { info->localMap = uMultimapToMap(map_->getWords3()); - info->localScanMap = map_->sensorData().laserScanRaw(); + info->localScanMap = util3d::transformLaserScan(map_->sensorData().laserScanRaw(), map_->sensorData().laserScanInfo().localTransform()); } } } diff --git a/corelib/src/RegistrationIcp.cpp b/corelib/src/RegistrationIcp.cpp index ac7acdfb..99c8c2cf 100644 --- a/corelib/src/RegistrationIcp.cpp +++ b/corelib/src/RegistrationIcp.cpp @@ -505,18 +505,24 @@ Transform RegistrationIcp::computeTransformationImpl( pcl::PointCloud::Ptr toCloudFiltered = toCloud; if(_voxelSize > 0.0f) { - int pointsBeforeFiltering = fromCloudFiltered->size(); + float pointsBeforeFiltering = (float)fromCloudFiltered->size(); fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _voxelSize); - maxLaserScansFrom = maxLaserScansFrom * fromCloudFiltered->size() / pointsBeforeFiltering; + float ratioFrom = float(fromCloudFiltered->size()) / pointsBeforeFiltering; + maxLaserScansFrom = int(float(maxLaserScansFrom) * ratioFrom); - pointsBeforeFiltering = toCloudFiltered->size(); + pointsBeforeFiltering = (float)toCloudFiltered->size(); toCloudFiltered = util3d::voxelize(toCloudFiltered, _voxelSize); - maxLaserScansTo = maxLaserScansTo * toCloudFiltered->size() / pointsBeforeFiltering; + float ratioTo = float(toCloudFiltered->size()) / pointsBeforeFiltering; + maxLaserScansTo = int(float(maxLaserScansTo) * ratioTo); - UDEBUG("Voxel filtering time (voxel=%f m, ratioFrom=%f ratioTo=%f) = %f s", + UDEBUG("Voxel filtering time (voxel=%f m, ratioFrom=%f->%d/%d ratioTo=%f->%d/%d) = %f s", _voxelSize, - float(fromCloudFiltered->size()) / float(pointsBeforeFiltering), - float(toCloudFiltered->size()) / float(pointsBeforeFiltering), + ratioFrom, + (int)fromCloudFiltered->size(), + maxLaserScansFrom, + ratioTo, + (int)toCloudFiltered->size(), + maxLaserScansTo, timer.ticks()); } @@ -531,11 +537,22 @@ Transform RegistrationIcp::computeTransformationImpl( if(fromScan.channels() == 2 || fromScan.channels() == 5) { - normals = util3d::computeFastOrganizedNormals2D( - fromCloudFiltered, - _pointToPlaneK, - _pointToPlaneRadius, - viewpointFrom); + if(_voxelSize > 0.0f) + { + normals = util3d::computeNormals2D( + fromCloudFiltered, + _pointToPlaneK, + _pointToPlaneRadius, + viewpointFrom); + } + else + { + normals = util3d::computeFastOrganizedNormals2D( + fromCloudFiltered, + _pointToPlaneK, + _pointToPlaneRadius, + viewpointFrom); + } } else { @@ -546,11 +563,22 @@ Transform RegistrationIcp::computeTransformationImpl( if(toScan.channels() == 2 || toScan.channels() == 5) { - normals = util3d::computeFastOrganizedNormals2D( - toCloudFiltered, - _pointToPlaneK, - _pointToPlaneRadius, - viewpointTo); + if(_voxelSize > 0.0f) + { + normals = util3d::computeNormals2D( + toCloudFiltered, + _pointToPlaneK, + _pointToPlaneRadius, + viewpointTo); + } + else + { + normals = util3d::computeFastOrganizedNormals2D( + toCloudFiltered, + _pointToPlaneK, + _pointToPlaneRadius, + viewpointTo); + } } else { @@ -564,7 +592,7 @@ Transform RegistrationIcp::computeTransformationImpl( fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals); // update output scans - if(fromScan.channels() == 2 || toScan.channels() == 5) + if(fromScan.channels() == 2 || fromScan.channels() == 5) { fromSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform)); } @@ -654,8 +682,22 @@ Transform RegistrationIcp::computeTransformationImpl( if(_voxelSize > 0.0f) { // update output scans - fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform)); - toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform)); + if(fromScan.channels() == 2 || fromScan.channels() == 5) + { + fromSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform)); + } + else + { + fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform)); + } + if(toScan.channels() == 2 || toScan.channels() == 5) + { + toSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*toCloudFiltered, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform)); + } + else + { + toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform)); + } } #ifdef RTABMAP_POINTMATCHER diff --git a/corelib/src/util3d_registration.cpp b/corelib/src/util3d_registration.cpp index 73cf184f..86a59078 100644 --- a/corelib/src/util3d_registration.cpp +++ b/corelib/src/util3d_registration.cpp @@ -244,8 +244,8 @@ void computeVarianceAndCorrespondences( correspondencesOut = 0; pcl::registration::CorrespondenceEstimation::Ptr est; est.reset(new pcl::registration::CorrespondenceEstimation); - est->setInputTarget(cloudB); - est->setInputSource(cloudA); + est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB); + est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA); pcl::Correspondences correspondences; est->determineCorrespondences(correspondences, maxCorrespondenceDistance); @@ -277,8 +277,8 @@ void computeVarianceAndCorrespondences( correspondencesOut = 0; pcl::registration::CorrespondenceEstimation::Ptr est; est.reset(new pcl::registration::CorrespondenceEstimation); - est->setInputTarget(cloudB); - est->setInputSource(cloudA); + est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB); + est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA); pcl::Correspondences correspondences; est->determineCorrespondences(correspondences, maxCorrespondenceDistance); diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index e5603b6f..60fb5352 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -2469,7 +2469,8 @@ void DatabaseViewer::update(int value, labelPose->setText(QString("%1xyz=(%2,%3,%4)\nrpy=(%5,%6,%7)").arg(odomPose.isIdentity()?"* ":"").arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw)); if(s!=0.0) { - stamp->setText(QDateTime::fromMSecsSinceEpoch(s*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz")); + stamp->setText(QString::number(s, 'f')); + stamp->setToolTip(QDateTime::fromMSecsSinceEpoch(s*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz")); } if(data.cameraModels().size() || data.stereoCameraModel().isValidForProjection()) { From 39283a5526c83d5ed0aeec30988b72853dcea9bd Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 1 Sep 2017 18:12:19 -0400 Subject: [PATCH 06/17] RegistrationIcp: libpointmatcher setting PointToPLane force2D parameter --- corelib/src/RegistrationIcp.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/corelib/src/RegistrationIcp.cpp b/corelib/src/RegistrationIcp.cpp index 99c8c2cf..1c453904 100644 --- a/corelib/src/RegistrationIcp.cpp +++ b/corelib/src/RegistrationIcp.cpp @@ -331,9 +331,15 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters) 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")); + params["force2D"] = force3DoF()?"1":"0"; + icp->errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create("PointToPlaneErrorMinimizer", params)); + params.clear(); + } + else + { + icp->errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer")); + } icp->transformationCheckers.clear(); params["maxIterationCount"] = uNumber2Str(_maxIterations); From 380fc2cbde4878b151dac5b6313361f7ee23a606 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 6 Sep 2017 11:01:39 -0400 Subject: [PATCH 07/17] PreferencesDialog: Fixed odom stack widget index always set to ORB-SLAM2 on loading --- guilib/include/rtabmap/gui/PreferencesDialog.h | 1 - guilib/src/PreferencesDialog.cpp | 14 -------------- guilib/src/ui/preferencesDialog.ui | 10 +++++----- 3 files changed, 5 insertions(+), 20 deletions(-) diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 0d11e8e1..0d6607cb 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -301,7 +301,6 @@ private slots: void addParameter(double value); void addParameter(const QString & value); void updatePredictionPlot(); - void updateOdometryVisibility(); void updateKpROI(); void updateStereoDisparityVisibility(); void useOdomFeatures(); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 78d3cfa8..fb66de3f 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -907,7 +907,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : //Odometry _ui->odom_strategy->setObjectName(Parameters::kOdomStrategy().c_str()); connect(_ui->odom_strategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_odometryType, SLOT(setCurrentIndex(int))); - connect(_ui->odom_strategy, SIGNAL(currentIndexChanged(int)), this, SLOT(updateOdometryVisibility())); _ui->odom_strategy->setCurrentIndex(Parameters::defaultOdomStrategy()); _ui->odom_countdown->setObjectName(Parameters::kOdomResetCountdown().c_str()); _ui->odom_holonomic->setObjectName(Parameters::kOdomHolonomic().c_str()); @@ -1628,7 +1627,6 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) if(groupBox->objectName() == _ui->groupBox_odometry1->objectName()) { _ui->odom_registration->setCurrentIndex(3); - updateOdometryVisibility(); } } } @@ -3950,18 +3948,6 @@ void PreferencesDialog::setupKpRoiPanel() _ui->doubleSpinBox_kp_roi3->setValue(strings[3].toDouble()*100.0); } -void PreferencesDialog::updateOdometryVisibility() -{ - UASSERT(_ui->odom_strategy->count() == 6); - _ui->groupBox_odomF2M->setVisible(_ui->odom_strategy->currentIndex()==0); - _ui->groupBox_odomF2F->setVisible(_ui->odom_strategy->currentIndex()==1); - _ui->groupBox_odomFovis->setVisible(_ui->odom_strategy->currentIndex()==2); - _ui->groupBox_odomViso2->setVisible(_ui->odom_strategy->currentIndex()==3); - _ui->groupBox_odomDVO->setVisible(_ui->odom_strategy->currentIndex()==4); - _ui->groupBox_odomORBSLAM2->setVisible(_ui->odom_strategy->currentIndex()==5); - _ui->groupBox_odomMono->setVisible(_ui->odom_strategy->currentIndex()==6); -} - void PreferencesDialog::updateKpROI() { QStringList strings; diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index cdd5246f..8c235384 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,9 +63,9 @@ 0 - 0 - 678 - 2736 + -531 + 673 + 2749 @@ -86,7 +86,7 @@ QFrame::Raised - 21 + 18 @@ -10018,7 +10018,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - 5 + 0 From aca005c2872db23e032acc8f121384156de90518 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 11 Sep 2017 13:17:53 -0400 Subject: [PATCH 08/17] Added parameter "IcpPointToPlaneMaxComplexity". Added util3d::computeNormalsComplexity(). OdomInfo has now RegistrationInfo field to avoid duplicating members. --- corelib/include/rtabmap/core/OdometryEvent.h | 22 +- corelib/include/rtabmap/core/OdometryInfo.h | 18 +- corelib/include/rtabmap/core/Parameters.h | 1 + .../include/rtabmap/core/RegistrationIcp.h | 1 + .../include/rtabmap/core/RegistrationInfo.h | 19 +- corelib/include/rtabmap/core/Statistics.h | 5 +- corelib/include/rtabmap/core/util3d_surface.h | 12 + corelib/src/OdometryDVO.cpp | 2 +- corelib/src/OdometryF2F.cpp | 14 +- corelib/src/OdometryF2M.cpp | 11 +- corelib/src/OdometryFovis.cpp | 6 +- corelib/src/OdometryMono.cpp | 12 +- corelib/src/OdometryORBSLAM2.cpp | 17 +- corelib/src/OdometryViso2.cpp | 6 +- corelib/src/RegistrationIcp.cpp | 359 ++++++++++-------- corelib/src/Rtabmap.cpp | 5 +- corelib/src/RtabmapThread.cpp | 20 +- corelib/src/util3d.cpp | 43 +-- corelib/src/util3d_surface.cpp | 147 +++++++ guilib/src/MainWindow.cpp | 44 ++- guilib/src/OdometryViewer.cpp | 16 +- guilib/src/PreferencesDialog.cpp | 1 + guilib/src/ui/preferencesDialog.ui | 134 ++++--- tools/KittiDataset/main.cpp | 10 +- tools/RgbdDataset/main.cpp | 10 +- 25 files changed, 581 insertions(+), 354 deletions(-) diff --git a/corelib/include/rtabmap/core/OdometryEvent.h b/corelib/include/rtabmap/core/OdometryEvent.h index 8e186ab8..e33e8bbb 100644 --- a/corelib/include/rtabmap/core/OdometryEvent.h +++ b/corelib/include/rtabmap/core/OdometryEvent.h @@ -41,7 +41,7 @@ class OdometryEvent : public UEvent public: OdometryEvent() { - _info.covariance = cv::Mat::eye(6,6,CV_64FC1); + _info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1); } OdometryEvent( const SensorData & data, @@ -51,17 +51,17 @@ public: _pose(pose), _info(info) { - if(_info.covariance.empty()) + if(_info.reg.covariance.empty()) { - _info.covariance = cv::Mat::eye(6,6,CV_64FC1); + _info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1); } - UASSERT(_info.covariance.cols == 6 && _info.covariance.rows == 6 && _info.covariance.type() == CV_64FC1); - UASSERT_MSG(uIsFinite(_info.covariance.at(0,0)) && _info.covariance.at(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)"); - UASSERT_MSG(uIsFinite(_info.covariance.at(1,1)) && _info.covariance.at(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)"); - UASSERT_MSG(uIsFinite(_info.covariance.at(2,2)) && _info.covariance.at(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)"); - UASSERT_MSG(uIsFinite(_info.covariance.at(3,3)) && _info.covariance.at(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)"); - UASSERT_MSG(uIsFinite(_info.covariance.at(4,4)) && _info.covariance.at(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)"); - UASSERT_MSG(uIsFinite(_info.covariance.at(5,5)) && _info.covariance.at(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)"); + UASSERT(_info.reg.covariance.cols == 6 && _info.reg.covariance.rows == 6 && _info.reg.covariance.type() == CV_64FC1); + UASSERT_MSG(uIsFinite(_info.reg.covariance.at(0,0)) && _info.reg.covariance.at(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)"); + UASSERT_MSG(uIsFinite(_info.reg.covariance.at(1,1)) && _info.reg.covariance.at(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)"); + UASSERT_MSG(uIsFinite(_info.reg.covariance.at(2,2)) && _info.reg.covariance.at(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)"); + UASSERT_MSG(uIsFinite(_info.reg.covariance.at(3,3)) && _info.reg.covariance.at(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)"); + UASSERT_MSG(uIsFinite(_info.reg.covariance.at(4,4)) && _info.reg.covariance.at(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)"); + UASSERT_MSG(uIsFinite(_info.reg.covariance.at(5,5)) && _info.reg.covariance.at(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)"); } virtual ~OdometryEvent() {} virtual std::string getClassName() const {return "OdometryEvent";} @@ -69,7 +69,7 @@ public: SensorData & data() {return _data;} const SensorData & data() const {return _data;} const Transform & pose() const {return _pose;} - const cv::Mat & covariance() const {return _info.covariance;} + const cv::Mat & covariance() const {return _info.reg.covariance;} std::vector velocity() const { if(_info.interval>0.0) { diff --git a/corelib/include/rtabmap/core/OdometryInfo.h b/corelib/include/rtabmap/core/OdometryInfo.h index b382d927..5c278c56 100644 --- a/corelib/include/rtabmap/core/OdometryInfo.h +++ b/corelib/include/rtabmap/core/OdometryInfo.h @@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "rtabmap/core/Transform.h" +#include "rtabmap/core/RegistrationInfo.h" #include namespace rtabmap { @@ -39,9 +40,6 @@ class OdometryInfo public: OdometryInfo() : lost(true), - matches(0), - inliers(0), - icpInliersRatio(0.0f), features(0), localMapSize(0), localScanMapSize(0), @@ -62,10 +60,7 @@ public: { OdometryInfo output; output.lost = lost; - output.matches = matches; - output.inliers = inliers; - output.icpInliersRatio = icpInliersRatio; - output.covariance = covariance.clone(); + output.reg = reg.copyWithoutData(); output.features = features; output.localMapSize = localMapSize; output.localScanMapSize = localScanMapSize; @@ -87,10 +82,7 @@ public: } bool lost; - int matches; - int inliers; - float icpInliersRatio; - cv::Mat covariance; + RegistrationInfo reg; int features; int localMapSize; int localScanMapSize; @@ -108,12 +100,10 @@ public: Transform transformGroundTruth; float distanceTravelled; - int type; // 0=F2M, 1=F2F + int type; // F2M std::multimap words; - std::vector wordMatches; - std::vector wordInliers; std::map localMap; cv::Mat localScanMap; diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 5a17768d..a06fd9ff 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -513,6 +513,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Icp, PointToPlane, bool, false, "Use point to plane ICP."); RTABMAP_PARAM(Icp, PointToPlaneK, int, 20, "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals."); RTABMAP_PARAM(Icp, PointToPlaneRadius, float, 0.0, "Search radius to compute normals for point to plane if the cloud doesn't have already normals."); + RTABMAP_PARAM(Icp, PointToPlaneMinComplexity, float, 0.0, "Minimum structural complexity (0.0=low, 1.0=high) of the scan to do point to plane registration, otherwise point to point registration is done instead."); // libpointmatcher RTABMAP_PARAM(Icp, PM, bool, false, "Use libpointmatcher for ICP registration instead of PCL's implementation."); diff --git a/corelib/include/rtabmap/core/RegistrationIcp.h b/corelib/include/rtabmap/core/RegistrationIcp.h index 51960421..05fe86b8 100644 --- a/corelib/include/rtabmap/core/RegistrationIcp.h +++ b/corelib/include/rtabmap/core/RegistrationIcp.h @@ -66,6 +66,7 @@ private: bool _pointToPlane; int _pointToPlaneK; float _pointToPlaneRadius; + float _pointToPlaneMinComplexity; bool _libpointmatcher; std::string _libpointmatcherConfig; float _libpointmatcherOutlierRatio; diff --git a/corelib/include/rtabmap/core/RegistrationInfo.h b/corelib/include/rtabmap/core/RegistrationInfo.h index 27dadf10..35471aae 100644 --- a/corelib/include/rtabmap/core/RegistrationInfo.h +++ b/corelib/include/rtabmap/core/RegistrationInfo.h @@ -39,10 +39,26 @@ public: matches(0), icpInliersRatio(0), icpTranslation(0.0f), - icpRotation(0.0f) + icpRotation(0.0f), + icpStructuralComplexity(0.0f) + { } + RegistrationInfo copyWithoutData() const + { + RegistrationInfo output; + output.covariance = covariance.clone(); + output.rejectedMsg = rejectedMsg; + output.inliers = inliers; + output.matches = matches; + output.icpInliersRatio = icpInliersRatio; + output.icpTranslation = icpTranslation; + output.icpRotation = icpRotation; + output.icpStructuralComplexity = icpStructuralComplexity; + return output; + } + cv::Mat covariance; std::string rejectedMsg; @@ -56,6 +72,7 @@ public: float icpInliersRatio; float icpTranslation; float icpRotation; + float icpStructuralComplexity; }; } diff --git a/corelib/include/rtabmap/core/Statistics.h b/corelib/include/rtabmap/core/Statistics.h index 7d5f7fad..cd6def47 100644 --- a/corelib/include/rtabmap/core/Statistics.h +++ b/corelib/include/rtabmap/core/Statistics.h @@ -77,7 +77,10 @@ class RTABMAP_EXP Statistics RTABMAP_STATS(NeighborLinkRefining, Accepted,); RTABMAP_STATS(NeighborLinkRefining, Inliers,); - RTABMAP_STATS(NeighborLinkRefining, Inliers_ratio,); + RTABMAP_STATS(NeighborLinkRefining, ICP_inliers_ratio,); + RTABMAP_STATS(NeighborLinkRefining, ICP_rotation, rad); + RTABMAP_STATS(NeighborLinkRefining, ICP_translation, m); + RTABMAP_STATS(NeighborLinkRefining, ICP_complexity,); RTABMAP_STATS(NeighborLinkRefining, Variance,); RTABMAP_STATS(NeighborLinkRefining, Pts,); diff --git a/corelib/include/rtabmap/core/util3d_surface.h b/corelib/include/rtabmap/core/util3d_surface.h index 4c43e078..4f723896 100644 --- a/corelib/include/rtabmap/core/util3d_surface.h +++ b/corelib/include/rtabmap/core/util3d_surface.h @@ -262,6 +262,18 @@ pcl::PointCloud::Ptr RTABMAP_EXP computeFastOrganizedNormals( float normalSmoothingSize = 10.0f, const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); +float RTABMAP_EXP computeNormalsComplexity( + const cv::Mat & scan); +float RTABMAP_EXP computeNormalsComplexity( + const pcl::PointCloud & normals, + bool is2d = false); +float RTABMAP_EXP computeNormalsComplexity( + const pcl::PointCloud & cloud, + bool is2d = false); +float RTABMAP_EXP computeNormalsComplexity( + const pcl::PointCloud & cloud, + bool is2d = false); + pcl::PointCloud::Ptr RTABMAP_EXP mls( const pcl::PointCloud::Ptr & cloud, float searchRadius = 0.0f, diff --git a/corelib/src/OdometryDVO.cpp b/corelib/src/OdometryDVO.cpp index c05a16de..edff9dd4 100644 --- a/corelib/src/OdometryDVO.cpp +++ b/corelib/src/OdometryDVO.cpp @@ -270,7 +270,7 @@ Transform OdometryDVO::computeTransform( if(info) { info->type = (int)kTypeDVO; - info->covariance = covariance; + info->reg.covariance = covariance; } UINFO("Odom update time = %fs", timer.elapsed()); diff --git a/corelib/src/OdometryF2F.cpp b/corelib/src/OdometryF2F.cpp index 73a0fea5..be1ed402 100644 --- a/corelib/src/OdometryF2F.cpp +++ b/corelib/src/OdometryF2F.cpp @@ -246,13 +246,17 @@ Transform OdometryF2F::computeTransform( if(info) { - info->type = 1; - info->covariance = regInfo.covariance; - info->inliers = regInfo.inliers; - info->icpInliersRatio = regInfo.icpInliersRatio; - info->matches = regInfo.matches; + info->type = kTypeF2F; info->features = newFrame.sensorData().keypoints().size(); info->keyFrameAdded = addKeyFrame; + if(this->isInfoDataFilled()) + { + info->reg = regInfo; + } + else + { + info->reg = regInfo.copyWithoutData(); + } } UINFO("Odom update time = %fs lost=%s inliers=%d, ref frame corners=%d, transform accepted=%s", diff --git a/corelib/src/OdometryF2M.cpp b/corelib/src/OdometryF2M.cpp index 5a5d5d32..f5c571c6 100644 --- a/corelib/src/OdometryF2M.cpp +++ b/corelib/src/OdometryF2M.cpp @@ -894,10 +894,6 @@ Transform OdometryF2M::computeTransform( if(info) { - info->covariance = regInfo.covariance; - info->inliers = regInfo.inliers; - info->matches = regInfo.matches; - info->icpInliersRatio = regInfo.icpInliersRatio; info->features = nFeatures; info->localKeyFrames = (int)bundlePoses_.size(); info->keyFrameAdded = addKeyFrame; @@ -907,8 +903,11 @@ Transform OdometryF2M::computeTransform( if(this->isInfoDataFilled()) { - info->wordMatches = regInfo.matchesIDs; - info->wordInliers = regInfo.inliersIDs; + info->reg = regInfo; + } + else + { + info->reg = regInfo.copyWithoutData(); } } diff --git a/corelib/src/OdometryFovis.cpp b/corelib/src/OdometryFovis.cpp index 44f48ba8..0109f52a 100644 --- a/corelib/src/OdometryFovis.cpp +++ b/corelib/src/OdometryFovis.cpp @@ -381,9 +381,9 @@ Transform OdometryFovis::computeTransform( info->type = (int)kTypeFovis; info->keyFrameAdded = fovis_->getChangeReferenceFrames(); info->features = fovis_->getTargetFrame()->getNumDetectedKeypoints(); - info->matches = fovis_->getMotionEstimator()->getNumMatches(); - info->inliers = fovis_->getMotionEstimator()->getNumInliers(); - info->covariance = covariance; + info->reg.matches = fovis_->getMotionEstimator()->getNumMatches(); + info->reg.inliers = fovis_->getMotionEstimator()->getNumInliers(); + info->reg.covariance = covariance; if(this->isInfoDataFilled()) { diff --git a/corelib/src/OdometryMono.cpp b/corelib/src/OdometryMono.cpp index 8115cb13..7358b3b7 100644 --- a/corelib/src/OdometryMono.cpp +++ b/corelib/src/OdometryMono.cpp @@ -352,7 +352,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu if(this->isInfoDataFilled() && info) { - info->wordMatches.insert(info->wordMatches.end(), matches.begin(), matches.end()); + info->reg.matchesIDs.insert(info->reg.matchesIDs.end(), matches.begin(), matches.end()); } correspondences = (int)matches.size(); @@ -397,10 +397,10 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu if(this->isInfoDataFilled() && info && inliersV.size()) { - info->wordInliers.resize(inliersV.size()); + info->reg.inliersIDs.resize(inliersV.size()); for(unsigned int i=0; iwordInliers[i] = matches[inliersV[i]]; // index and ID should match (index starts at 0, ID starts at 1) + info->reg.inliersIDs[i] = matches[inliersV[i]]; // index and ID should match (index starts at 0, ID starts at 1) } } @@ -976,7 +976,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu if(info) { // a very high variance tells that the new pose is not linked with the previous one - info->covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0; + info->reg.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0; } // generate kpts @@ -1013,8 +1013,8 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu if(this->isInfoDataFilled() && info) { //info->variance = variance; - info->inliers = inliers; - info->matches = correspondences; + info->reg.inliers = inliers; + info->reg.matches = correspondences; info->features = nFeatures; info->localMapSize = (int)localMap_.size(); info->localMap = localMap_; diff --git a/corelib/src/OdometryORBSLAM2.cpp b/corelib/src/OdometryORBSLAM2.cpp index 6568e87f..2c7523f5 100644 --- a/corelib/src/OdometryORBSLAM2.cpp +++ b/corelib/src/OdometryORBSLAM2.cpp @@ -891,15 +891,15 @@ Transform OdometryORBSLAM2::computeTransform( { info->lost = t.isNull(); info->type = (int)kTypeORBSLAM2; - info->covariance = covariance; + info->reg.covariance = covariance; info->localMapSize = totalMapPoints; info->localKeyFrames = totalKfs; if(this->isInfoDataFilled() && orbslam2_->mpTracker && orbslam2_->mpMap) { const std::vector & kpts = orbslam2_->mpTracker->mCurrentFrame.mvKeys; - info->wordMatches.resize(kpts.size()); - info->wordInliers.resize(kpts.size()); + info->reg.matchesIDs.resize(kpts.size()); + info->reg.inliersIDs.resize(kpts.size()); int oi = 0; for (unsigned int i = 0; i < kpts.size(); ++i) { @@ -915,14 +915,15 @@ Transform OdometryORBSLAM2::computeTransform( info->words.insert(std::make_pair(wordId, kpts[i])); if(orbslam2_->mpTracker->mCurrentFrame.mvpMapPoints[i] != 0) { - info->wordMatches[oi] = wordId; - info->wordInliers[oi] = wordId; + info->reg.matchesIDs[oi] = wordId; + info->reg.inliersIDs[oi] = wordId; ++oi; } } - info->wordMatches.resize(oi); - info->wordInliers.resize(oi); - info->inliers = oi; + info->reg.matchesIDs.resize(oi); + info->reg.inliersIDs.resize(oi); + info->reg.inliers = oi; + info->reg.matches = oi; std::vector mapPoints = orbslam2_->mpMap->GetAllMapPoints(); for (unsigned int i = 0; i < mapPoints.size(); ++i) diff --git a/corelib/src/OdometryViso2.cpp b/corelib/src/OdometryViso2.cpp index ce8dfaf0..ee0fcdd8 100644 --- a/corelib/src/OdometryViso2.cpp +++ b/corelib/src/OdometryViso2.cpp @@ -273,11 +273,11 @@ Transform OdometryViso2::computeTransform( { info->type = (int)kTypeViso2; info->keyFrameAdded = !keep_reference_frame_; - info->matches = viso2_->getNumberOfMatches(); - info->inliers = viso2_->getNumberOfInliers(); + info->reg.matches = viso2_->getNumberOfMatches(); + info->reg.inliers = viso2_->getNumberOfInliers(); if(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1) { - info->covariance = covariance; + info->reg.covariance = covariance; } if(this->isInfoDataFilled()) diff --git a/corelib/src/RegistrationIcp.cpp b/corelib/src/RegistrationIcp.cpp index 1c453904..c70912eb 100644 --- a/corelib/src/RegistrationIcp.cpp +++ b/corelib/src/RegistrationIcp.cpp @@ -232,6 +232,7 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration _pointToPlane(Parameters::defaultIcpPointToPlane()), _pointToPlaneK(Parameters::defaultIcpPointToPlaneK()), _pointToPlaneRadius(Parameters::defaultIcpPointToPlaneRadius()), + _pointToPlaneMinComplexity(Parameters::defaultIcpPointToPlaneMinComplexity()), _libpointmatcher(Parameters::defaultIcpPM()), _libpointmatcherConfig(Parameters::defaultIcpPMConfig()), _libpointmatcherOutlierRatio(Parameters::defaultIcpPMOutlierRatio()), @@ -265,6 +266,8 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane); Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), _pointToPlaneK); Parameters::parse(parameters, Parameters::kIcpPointToPlaneRadius(), _pointToPlaneRadius); + Parameters::parse(parameters, Parameters::kIcpPointToPlaneMinComplexity(), _pointToPlaneMinComplexity); + UASSERT(_pointToPlaneMinComplexity >= 0.0f && _pointToPlaneMinComplexity <= 1.0f); Parameters::parse(parameters, Parameters::kIcpPM(), _libpointmatcher); Parameters::parse(parameters, Parameters::kIcpPMConfig(), _libpointmatcherConfig); @@ -428,6 +431,8 @@ Transform RegistrationIcp::computeTransformationImpl( float correspondencesRatio = 0.0f; int correspondences = 0; double variance = 1.0; + bool transformComputed = false; + bool tooLowComplexityForPlaneToPlane = false; if( _pointToPlane && _voxelSize == 0.0f && @@ -436,72 +441,88 @@ Transform RegistrationIcp::computeTransformationImpl( !((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::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform); - pcl::PointCloud::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toLocalTransform); - fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals); - toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals); - - - UDEBUG("Conversion time = %f s", timer.ticks()); - pcl::PointCloud::Ptr fromCloudNormalsRegistered(new pcl::PointCloud()); -#ifdef RTABMAP_POINTMATCHER - if(_libpointmatcher) + double fromComplexity = util3d::computeNormalsComplexity(fromScan); + double toComplexity = util3d::computeNormalsComplexity(toScan); + UWARN("%d->%d %f %f", fromSignature.id(), toSignature.id(), fromComplexity, toComplexity); + float complexity = fromComplexity(T.template cast(), 4)))); - UDEBUG("libpointmatcher icp...done! T=%s", icpT.prettyPrint().c_str()); - - float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio(); - UDEBUG("match ratio: %f", matchRatio); - - if(!icpT.isNull()) - { - fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT); - hasConverged = true; - } - } - catch(const std::exception & e) - { - UWARN("libpointmatcher has failed: %s", e.what()); - } + tooLowComplexityForPlaneToPlane = true; + UWARN("ICP PointToPlane ignored as structural complexity is too low: %f < %f (%s). PointToPoint is done instead.", complexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str()); } else -#endif { - icpT = util3d::icpPointToPlane( - fromCloudNormals, - toCloudNormals, - _maxCorrespondenceDistance, - _maxIterations, - hasConverged, - *fromCloudNormalsRegistered, - _epsilon, - this->force3DoF()); - } + pcl::PointCloud::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform); + pcl::PointCloud::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toLocalTransform); - if(!icpT.isNull() && hasConverged) - { - util3d::computeVarianceAndCorrespondences( - fromCloudNormalsRegistered, - toCloudNormals, - _maxCorrespondenceDistance, - variance, - correspondences); + fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals); + toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals); + + + UDEBUG("Conversion time = %f s", timer.ticks()); + pcl::PointCloud::Ptr fromCloudNormalsRegistered(new pcl::PointCloud()); +#ifdef RTABMAP_POINTMATCHER + if(_libpointmatcher) + { + // Load point clouds + 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; + try + { + UASSERT(_libpointmatcherICP != 0); + PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP); + UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)"); + T = icp(data, ref); + icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim(T.template cast(), 4)))); + UDEBUG("libpointmatcher icp...done! T=%s", icpT.prettyPrint().c_str()); + + float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio(); + UDEBUG("match ratio: %f", matchRatio); + + if(!icpT.isNull()) + { + fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT); + hasConverged = true; + } + } + catch(const std::exception & e) + { + UWARN("libpointmatcher has failed: %s", e.what()); + } + } + else +#endif + { + icpT = util3d::icpPointToPlane( + fromCloudNormals, + toCloudNormals, + _maxCorrespondenceDistance, + _maxIterations, + hasConverged, + *fromCloudNormalsRegistered, + _epsilon, + this->force3DoF()); + } + + if(!icpT.isNull() && hasConverged) + { + util3d::computeVarianceAndCorrespondences( + fromCloudNormalsRegistered, + toCloudNormals, + _maxCorrespondenceDistance, + variance, + correspondences); + } + transformComputed = true; } } - else + + if(!transformComputed) { pcl::PointCloud::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, fromLocalTransform); pcl::PointCloud::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess * toLocalTransform); @@ -534,44 +555,43 @@ Transform RegistrationIcp::computeTransformationImpl( pcl::PointCloud::Ptr fromCloudRegistered(new pcl::PointCloud()); if(_pointToPlane && // ICP Point To Plane + !tooLowComplexityForPlaneToPlane && // if previously rejected above !((fromScan.channels() == 2 || fromScan.channels() == 5 || toScan.channels() == 2 || toScan.channels() == 5) && !_libpointmatcher)) // PCL crashes if 2D { - pcl::PointCloud::Ptr normals; Eigen::Vector3f viewpointFrom(fromLocalTransform.x(), fromLocalTransform.y(), fromLocalTransform.z()); + pcl::PointCloud::Ptr normalsFrom; + if(fromScan.channels() == 2 || fromScan.channels() == 5) + { + if(_voxelSize > 0.0f) + { + normalsFrom = util3d::computeNormals2D( + fromCloudFiltered, + _pointToPlaneK, + _pointToPlaneRadius, + viewpointFrom); + } + else + { + normalsFrom = util3d::computeFastOrganizedNormals2D( + fromCloudFiltered, + _pointToPlaneK, + _pointToPlaneRadius, + viewpointFrom); + } + } + else + { + normalsFrom = util3d::computeNormals(fromCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointFrom); + } + Transform toT = guess * toLocalTransform; Eigen::Vector3f viewpointTo(toT.x(), toT.y(), toT.z()); - - if(fromScan.channels() == 2 || fromScan.channels() == 5) - { - if(_voxelSize > 0.0f) - { - normals = util3d::computeNormals2D( - fromCloudFiltered, - _pointToPlaneK, - _pointToPlaneRadius, - viewpointFrom); - } - else - { - normals = util3d::computeFastOrganizedNormals2D( - fromCloudFiltered, - _pointToPlaneK, - _pointToPlaneRadius, - viewpointFrom); - } - } - else - { - normals = util3d::computeNormals(fromCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointFrom); - } - pcl::PointCloud::Ptr fromCloudNormals(new pcl::PointCloud); - pcl::concatenateFields(*fromCloudFiltered, *normals, *fromCloudNormals); - + pcl::PointCloud::Ptr normalsTo; if(toScan.channels() == 2 || toScan.channels() == 5) { if(_voxelSize > 0.0f) { - normals = util3d::computeNormals2D( + normalsTo = util3d::computeNormals2D( toCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, @@ -579,7 +599,7 @@ Transform RegistrationIcp::computeTransformationImpl( } else { - normals = util3d::computeFastOrganizedNormals2D( + normalsTo = util3d::computeFastOrganizedNormals2D( toCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, @@ -588,99 +608,116 @@ Transform RegistrationIcp::computeTransformationImpl( } else { - normals = util3d::computeNormals(toCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointTo); + normalsTo = util3d::computeNormals(toCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointTo); } - pcl::PointCloud::Ptr toCloudNormals(new pcl::PointCloud); - pcl::concatenateFields(*toCloudFiltered, *normals, *toCloudNormals); - std::vector indices; - toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals); - fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals); - - // update output scans - if(fromScan.channels() == 2 || fromScan.channels() == 5) + double fromComplexity = util3d::computeNormalsComplexity(*normalsFrom, fromScan.channels() == 2 || fromScan.channels() == 5); + double toComplexity = util3d::computeNormalsComplexity(*normalsTo, toScan.channels() == 2 || toScan.channels() == 5); + float complexity = fromComplexity::Ptr fromCloudNormals(new pcl::PointCloud); + pcl::concatenateFields(*fromCloudFiltered, *normalsFrom, *fromCloudNormals); - UDEBUG("Compute normals (%d,%d) time = %f s", (int)fromCloudNormals->size(), (int)toCloudNormals->size(), timer.ticks()); + pcl::PointCloud::Ptr toCloudNormals(new pcl::PointCloud); + pcl::concatenateFields(*toCloudFiltered, *normalsTo, *toCloudNormals); - if(toCloudNormals->size() && fromCloudNormals->size()) - { - pcl::PointCloud::Ptr fromCloudNormalsRegistered(new pcl::PointCloud()); + std::vector indices; + toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals); + fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals); + + // update output scans + if(fromScan.channels() == 2 || fromScan.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()); + + if(toCloudNormals->size() && fromCloudNormals->size()) + { + pcl::PointCloud::Ptr fromCloudNormalsRegistered(new pcl::PointCloud()); #ifdef RTABMAP_POINTMATCHER - if(_libpointmatcher) - { - // Load point clouds - 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; - try + if(_libpointmatcher) { - UASSERT(_libpointmatcherICP != 0); - PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP); - UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)"); - T = icp(data, ref); - UDEBUG("libpointmatcher icp...done!"); - icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim(T.template cast(), 4)))); + // Load point clouds + DP data = pclToDP(fromCloudNormals, fromScan.channels() == 2 || fromScan.channels() == 5); + DP ref = pclToDP(toCloudNormals, toScan.channels() == 2 || toScan.channels() == 5); - float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio(); - UDEBUG("match ratio: %f", matchRatio); - - if(!icpT.isNull()) + // Compute the transformation to express data in ref + PM::TransformationParameters T; + try { - fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT); - hasConverged = true; + UASSERT(_libpointmatcherICP != 0); + PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP); + UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)"); + T = icp(data, ref); + UDEBUG("libpointmatcher icp...done!"); + icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim(T.template cast(), 4)))); + + float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio(); + UDEBUG("match ratio: %f", matchRatio); + + if(!icpT.isNull()) + { + fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT); + hasConverged = true; + } + } + catch(const std::exception & e) + { + UWARN("libpointmatcher has failed: %s", e.what()); } } - catch(const std::exception & e) + else +#endif { - UWARN("libpointmatcher has failed: %s", e.what()); + icpT = util3d::icpPointToPlane( + fromCloudNormals, + toCloudNormals, + _maxCorrespondenceDistance, + _maxIterations, + hasConverged, + *fromCloudNormalsRegistered, + _epsilon, + this->force3DoF()); + } + + if(!icpT.isNull() && hasConverged) + { + util3d::computeVarianceAndCorrespondences( + fromCloudNormalsRegistered, + toCloudNormals, + _maxCorrespondenceDistance, + variance, + correspondences); } } - else -#endif - { - icpT = util3d::icpPointToPlane( - fromCloudNormals, - toCloudNormals, - _maxCorrespondenceDistance, - _maxIterations, - hasConverged, - *fromCloudNormalsRegistered, - _epsilon, - this->force3DoF()); - } - - if(!icpT.isNull() && hasConverged) - { - util3d::computeVarianceAndCorrespondences( - fromCloudNormalsRegistered, - toCloudNormals, - _maxCorrespondenceDistance, - variance, - correspondences); - } + transformComputed = true; } } - else // ICP Point to Point + + if(!transformComputed) // ICP Point to Point { - if(_pointToPlane && ((fromScan.channels() == 2 || fromScan.channels() == 5 || toScan.channels() == 2 || toScan.channels() == 5) && !_libpointmatcher)) + if(_pointToPlane && !tooLowComplexityForPlaneToPlane && ((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()); } @@ -707,7 +744,7 @@ Transform RegistrationIcp::computeTransformationImpl( } #ifdef RTABMAP_POINTMATCHER - if(_libpointmatcher) + if(_libpointmatcher && !_pointToPlane) // don't use libpointmatcher if it is configured for point to plane { // Load point clouds DP data = pclToDP(fromCloudFiltered, fromScan.channels() == 2 || fromScan.channels() == 5); diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 6aaf2265..1937b8af 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -1123,7 +1123,10 @@ bool Rtabmap::process( } statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0); statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), info.inliers); - statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers_ratio(), info.icpInliersRatio); + statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_inliers_ratio(), info.icpInliersRatio); + statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_rotation(), info.icpRotation); + statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_translation(), info.icpTranslation); + statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_complexity(), info.icpStructuralComplexity); statistics_.addStatistic(Statistics::kNeighborLinkRefiningPts(), signature->sensorData().laserScanRaw().cols); } } diff --git a/corelib/src/RtabmapThread.cpp b/corelib/src/RtabmapThread.cpp index 02245fae..68ab0c20 100644 --- a/corelib/src/RtabmapThread.cpp +++ b/corelib/src/RtabmapThread.cpp @@ -336,7 +336,7 @@ bool RtabmapThread::handleEvent(UEvent* event) if (!e->info().odomPose.isNull() || (_rtabmap->getMemory() && !_rtabmap->getMemory()->isIncremental())) { OdometryInfo infoCov; - infoCov.covariance = e->info().odomCovariance; + infoCov.reg.covariance = e->info().odomCovariance; this->addData(OdometryEvent(e->data(), e->info().odomPose, infoCov)); } else @@ -347,7 +347,7 @@ bool RtabmapThread::handleEvent(UEvent* event) else { OdometryInfo infoCov; - infoCov.covariance = e->info().odomCovariance; + infoCov.reg.covariance = e->info().odomCovariance; this->addData(OdometryEvent(e->data(), e->info().odomPose, infoCov)); } @@ -570,7 +570,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent) } if(!lastPose_.isIdentity() && (odomEvent.pose().isIdentity() || - odomEvent.info().covariance.at(0,0)>=9999)) + odomEvent.info().reg.covariance.at(0,0)>=9999)) { if(odomEvent.pose().isIdentity()) { @@ -578,20 +578,20 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent) } else { - UWARN("Odometry is reset (high variance (%f >=9999 detected). Increment map id!", odomEvent.info().covariance.at(0,0)); + UWARN("Odometry is reset (high variance (%f >=9999 detected). Increment map id!", odomEvent.info().reg.covariance.at(0,0)); } pushNewState(kStateTriggeringMap); covariance_ = cv::Mat(); } - if(uIsFinite(odomEvent.info().covariance.at(0,0)) && - odomEvent.info().covariance.at(0,0) != 1.0 && - odomEvent.info().covariance.at(0,0)>0.0) + if(uIsFinite(odomEvent.info().reg.covariance.at(0,0)) && + odomEvent.info().reg.covariance.at(0,0) != 1.0 && + odomEvent.info().reg.covariance.at(0,0)>0.0) { // Use largest covariance error (to be independent of the odometry frame rate) - if(covariance_.empty() || odomEvent.info().covariance.at(0,0) > covariance_.at(0,0)) + if(covariance_.empty() || odomEvent.info().reg.covariance.at(0,0) > covariance_.at(0,0)) { - covariance_ = odomEvent.info().covariance; + covariance_ = odomEvent.info().reg.covariance; } } @@ -614,7 +614,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent) covariance_ = cv::Mat::eye(6,6,CV_64FC1); } OdometryInfo odomInfo = odomEvent.info().copyWithoutData(); - odomInfo.covariance = covariance_; + odomInfo.reg.covariance = covariance_; if(ignoreFrame) { // set negative id so rtabmap will detect it as an intermediate node diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index a05a0714..fe3ad546 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -1808,7 +1808,7 @@ cv::Mat projectCloudToCamera( { UASSERT(!cameraTransform.isNull()); UASSERT(!laserScan.empty()); - UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || 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)); UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3); float fx = cameraMatrixK.at(0,0); @@ -1819,46 +1819,25 @@ cv::Mat projectCloudToCamera( cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1); Transform t = cameraTransform.inverse(); - const cv::Vec2f* vec2Ptr = laserScan.ptr(); - const cv::Vec3f* vec3Ptr = laserScan.ptr(); - const cv::Vec4f* vec4Ptr = laserScan.ptr(); - const cv::Vec6f* vec6Ptr = laserScan.ptr(); - const float* vec7Ptr = laserScan.ptr(); - int count = 0; for(int i=0; i(0, i); + // Get 3D from laser scan cv::Point3f ptScan; - if(laserScan.type() == CV_32FC2) + if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC(5)) { - ptScan.x = vec2Ptr[i][0]; - ptScan.y = vec2Ptr[i][1]; + // 2D scans + ptScan.x = ptr[0]; + ptScan.y = ptr[1]; ptScan.z = 0; } - else if(laserScan.type() == CV_32FC3) + else // 3D scans { - ptScan.x = vec3Ptr[i][0]; - ptScan.y = vec3Ptr[i][1]; - ptScan.z = vec3Ptr[i][2]; - } - else if(laserScan.type() == CV_32FC(4)) - { - ptScan.x = vec4Ptr[i][0]; - ptScan.y = vec4Ptr[i][1]; - ptScan.z = vec4Ptr[i][2]; - } - else if(laserScan.type() == CV_32FC(6)) - { - ptScan.x = vec6Ptr[i][0]; - ptScan.y = vec6Ptr[i][1]; - ptScan.z = vec6Ptr[i][2]; - } - else // 7f - { - ptScan.x = (vec7Ptr+i*7)[0]; - ptScan.y = (vec7Ptr+i*7)[1]; - ptScan.z = (vec7Ptr+i*7)[2]; + ptScan.x = ptr[0]; + ptScan.y = ptr[1]; + ptScan.z = ptr[2]; } ptScan = util3d::transformPoint(ptScan, t); diff --git a/corelib/src/util3d_surface.cpp b/corelib/src/util3d_surface.cpp index f407c12e..13365fec 100644 --- a/corelib/src/util3d_surface.cpp +++ b/corelib/src/util3d_surface.cpp @@ -2338,6 +2338,153 @@ pcl::PointCloud::Ptr computeFastOrganizedNormals( return normals; } +float computeNormalsComplexity(const cv::Mat & scan) +{ + if(!scan.empty() && (scan.channels() == 5 || scan.channels() == 6 || scan.channels() == 7)) + { + //Construct a buffer used by the pca analysis + int sz = static_cast(scan.cols*2); + bool is2d = scan.channels() == 5; + cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1); + int oi = 0; + for (int i = 0; i < scan.cols; ++i) + { + const float * ptrScan = scan.ptr(0, i); + if(scan.channels() == 5) + { + if(uIsFinite(ptrScan[2]) && uIsFinite(ptrScan[3])) + { + float * ptr = data_normals.ptr(oi++, 0); + ptr[0] = ptrScan[2]; + ptr[1] = ptrScan[3]; + } + } + else if(scan.channels() == 6) + { + if(uIsFinite(ptrScan[3]) && uIsFinite(ptrScan[4]) && uIsFinite(ptrScan[5])) + { + float * ptr = data_normals.ptr(oi++, 0); + ptr[0] = ptrScan[3]; + ptr[1] = ptrScan[4]; + ptr[2] = ptrScan[5]; + } + } + else + { + if(uIsFinite(ptrScan[4]) && uIsFinite(ptrScan[5]) && uIsFinite(ptrScan[6])) + { + float * ptr = data_normals.ptr(oi++, 0); + ptr[0] = ptrScan[4]; + ptr[1] = ptrScan[5]; + ptr[2] = ptrScan[6]; + } + } + } + if(oi>1) + { + cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW); + + // Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity + return pca_analysis.eigenvalues.at(0, is2d?1:2)*(is2d?2.0f:3.0f); + } + } + else if(!scan.empty()) + { + UERROR("Scan doesn't have normals!"); + } + return 0.0f; +} + +float computeNormalsComplexity(const pcl::PointCloud & cloud, bool is2d) +{ + //Construct a buffer used by the pca analysis + int sz = static_cast(cloud.size()*2); + cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1); + int oi = 0; + for (unsigned int i = 0; i < cloud.size(); ++i) + { + const pcl::PointNormal & pt = cloud.at(i); + if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z)) + { + float * ptr = data_normals.ptr(oi++, 0); + ptr[0] = pt.normal_x; + ptr[1] = pt.normal_y; + if(!is2d) + { + ptr[2] = pt.normal_z; + } + } + } + if(oi>1) + { + cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW); + + // Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity + return pca_analysis.eigenvalues.at(0, is2d?1:2)*(is2d?2.0f:3.0f); + } + return 0.0f; +} + +float computeNormalsComplexity(const pcl::PointCloud & normals, bool is2d) +{ + //Construct a buffer used by the pca analysis + int sz = static_cast(normals.size()*2); + cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1); + int oi = 0; + for (unsigned int i = 0; i < normals.size(); ++i) + { + const pcl::Normal & pt = normals.at(i); + if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z)) + { + float * ptr = data_normals.ptr(oi++, 0); + ptr[0] = pt.normal_x; + ptr[1] = pt.normal_y; + if(!is2d) + { + ptr[2] = pt.normal_z; + } + } + } + if(oi>1) + { + cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW); + + // Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity + return pca_analysis.eigenvalues.at(0, is2d?1:2)*(is2d?2.0f:3.0f); + } + return 0.0f; +} + +float computeNormalsComplexity(const pcl::PointCloud & cloud, bool is2d) +{ + //Construct a buffer used by the pca analysis + int sz = static_cast(cloud.size()*2); + cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1); + int oi = 0; + for (unsigned int i = 0; i < cloud.size(); ++i) + { + const pcl::PointXYZRGBNormal & pt = cloud.at(i); + if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z)) + { + float * ptr = data_normals.ptr(oi++, 0); + ptr[0] = pt.normal_x; + ptr[1] = pt.normal_y; + if(!is2d) + { + ptr[2] = pt.normal_z; + } + } + } + if(oi>1) + { + cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW); + + // Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity + return pca_analysis.eigenvalues.at(0, is2d?1:2)*(is2d?2.0f:3.0f); + } + return 0.0f; +} + pcl::PointCloud::Ptr mls( const pcl::PointCloud::Ptr & cloud, float searchRadius, diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 82568ee3..289bf455 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -560,6 +560,9 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _ui->statsToolBox->updateStat("Odometry/Inliers/", false); _ui->statsToolBox->updateStat("Odometry/InliersRatio/", false); _ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", false); + _ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", false); + _ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", false); + _ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", false); _ui->statsToolBox->updateStat("Odometry/StdDevLin/", false); _ui->statsToolBox->updateStat("Odometry/StdDevAng/", false); _ui->statsToolBox->updateStat("Odometry/VarianceLin/", false); @@ -812,7 +815,7 @@ bool MainWindow::handleEvent(UEvent* anEvent) if (_odomThread == 0 && _camera->camera()->odomProvided() && _preferencesDialog->isRGBDMode()) { OdometryInfo odomInfo; - odomInfo.covariance = cameraEvent->info().odomCovariance; + odomInfo.reg.covariance = cameraEvent->info().odomCovariance; if (!_processingOdometry && !_processingStatistics) { _processingOdometry = true; // if we receive too many odometry events! @@ -927,11 +930,11 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI pose = _lastOdomPose; lost = true; } - else if(odom.info().inliers>0 && + else if(odom.info().reg.inliers>0 && _preferencesDialog->getOdomQualityWarnThr() && - odom.info().inliers < _preferencesDialog->getOdomQualityWarnThr()) + odom.info().reg.inliers < _preferencesDialog->getOdomQualityWarnThr()) { - UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().inliers, _preferencesDialog->getOdomQualityWarnThr()); + UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().reg.inliers, _preferencesDialog->getOdomQualityWarnThr()); lostStateChanged = _cloudViewer->getBackgroundColor() == Qt::darkRed; _cloudViewer->setBackgroundColor(Qt::darkYellow); _ui->imageView_odometry->setBackgroundColor(Qt::darkYellow); @@ -1202,9 +1205,9 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI if(_preferencesDialog->isOdomOnlyInliersShown()) { std::multimap kpInliers; - for(unsigned int i=0; iimageView_odometry->setFeatures( kpInliers, @@ -1271,13 +1274,13 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI { if(_ui->imageView_odometry->isFeaturesShown() && !_preferencesDialog->isOdomOnlyInliersShown()) { - for(unsigned int i=0; iimageView_odometry->setFeatureColor(odom.info().wordMatches[i], Qt::red); // outliers + _ui->imageView_odometry->setFeatureColor(odom.info().reg.matchesIDs[i], Qt::red); // outliers } - for(unsigned int i=0; iimageView_odometry->setFeatureColor(odom.info().wordInliers[i], Qt::green); // inliers + _ui->imageView_odometry->setFeatureColor(odom.info().reg.inliersIDs[i], Qt::green); // inliers } } } @@ -1323,15 +1326,18 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI } //Process info - _ui->statsToolBox->updateStat("Odometry/Inliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().inliers, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("Odometry/InliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().inliers)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().icpInliersRatio, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().matches, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("Odometry/StdDevLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().covariance.at(0,0)), _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().covariance.at(0,0), _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("Odometry/StdDevAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().covariance.at(5,5)), _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().covariance.at(5,5), _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/Inliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.inliers, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/InliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().reg.inliers)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpInliersRatio, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpRotation, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpTranslation, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpStructuralComplexity, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.matches, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().reg.matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/StdDevLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().reg.covariance.at(0,0)), _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.covariance.at(0,0), _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/StdDevAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().reg.covariance.at(5,5)), _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.covariance.at(5,5), _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeEstimation*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/Features/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().features, _preferencesDialog->isCacheSavedInFigures()); diff --git a/guilib/src/OdometryViewer.cpp b/guilib/src/OdometryViewer.cpp index eff1662e..55b73d42 100644 --- a/guilib/src/OdometryViewer.cpp +++ b/guilib/src/OdometryViewer.cpp @@ -179,7 +179,7 @@ void OdometryViewer::clear() void OdometryViewer::processData(const rtabmap::OdometryEvent & odom) { processingData_ = true; - int quality = odom.info().inliers; + int quality = odom.info().reg.inliers; bool lost = false; bool lostStateChanged = false; @@ -193,11 +193,11 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom) lost = true; } - else if(odom.info().inliers>0 && + else if(odom.info().reg.inliers>0 && qualityWarningThr_ && - odom.info().inliers < qualityWarningThr_) + odom.info().reg.inliers < qualityWarningThr_) { - UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().inliers, qualityWarningThr_); + UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().reg.inliers, qualityWarningThr_); lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed; imageView_->setBackgroundColor(Qt::darkYellow); cloudView_->setBackgroundColor(Qt::darkYellow); @@ -425,13 +425,13 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom) { if(imageView_->isFeaturesShown()) { - for(unsigned int i=0; isetFeatureColor(odom.info().wordMatches[i], Qt::red); // outliers + imageView_->setFeatureColor(odom.info().reg.matchesIDs[i], Qt::red); // outliers } - for(unsigned int i=0; isetFeatureColor(odom.info().wordInliers[i], Qt::green); // inliers + imageView_->setFeatureColor(odom.info().reg.inliersIDs[i], Qt::green); // inliers } } } diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index fb66de3f..b9a8bdae 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -863,6 +863,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->loopClosure_icpPointToPlane->setObjectName(Parameters::kIcpPointToPlane().c_str()); _ui->loopClosure_icpPointToPlaneNormals->setObjectName(Parameters::kIcpPointToPlaneK().c_str()); _ui->loopClosure_icpPointToPlaneNormalsRadius->setObjectName(Parameters::kIcpPointToPlaneRadius().c_str()); + _ui->loopClosure_icpPointToPlaneNormalsMinComplexity->setObjectName(Parameters::kIcpPointToPlaneMinComplexity().c_str()); _ui->groupBox_libpointmatcher->setObjectName(Parameters::kIcpPM().c_str()); _ui->lineEdit_IcpPMConfigPath->setObjectName(Parameters::kIcpPMConfig().c_str()); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 8c235384..273fbfae 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,9 +63,9 @@ 0 - -531 - 673 - 2749 + -215 + 678 + 2736 @@ -86,7 +86,7 @@ QFrame::Raised - 18 + 21 @@ -13497,6 +13497,61 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare + + + + 3 + + + 0.001000000000000 + + + 9999.989999999999782 + + + 0.010000000000000 + + + 0.100000000000000 + + + + + + + Uniform sampling voxel size. Set to 0 to disable. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + 999999 + + + 1 + + + 1 + + + + + + + 0.010000000000000 + + + @@ -13598,35 +13653,6 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - - - - Uniform sampling voxel size. Set to 0 to disable. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 1 - - - 999999 - - - 1 - - - 1 - - - @@ -13723,25 +13749,6 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - - - - 3 - - - 0.001000000000000 - - - 9999.989999999999782 - - - 0.010000000000000 - - - 0.100000000000000 - - - @@ -13803,8 +13810,27 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - - + + + + Minimum structural complexity (0.0=low, 1.0=high) of the scan to do point to plane registration, otherwise point to point registration is done instead. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 3 + + + 1.000000000000000 + 0.010000000000000 diff --git a/tools/KittiDataset/main.cpp b/tools/KittiDataset/main.cpp index f16a77c4..dbe069c9 100644 --- a/tools/KittiDataset/main.cpp +++ b/tools/KittiDataset/main.cpp @@ -400,8 +400,8 @@ int main(int argc, char * argv[]) if(odomInfo.interval>0.0) speed = odomInfo.transform.x()/odomInfo.interval*3.6; externalStats.insert(std::make_pair("Odometry/Speed/kph", speed)); - externalStats.insert(std::make_pair("Odometry/Inliers/ms", odomInfo.inliers)); - externalStats.insert(std::make_pair("Odometry/Features/ms", odomInfo.features)); + externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers)); + externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features)); bool processData = true; if(iteration % mapUpdate != 0) @@ -413,11 +413,11 @@ int main(int argc, char * argv[]) } if(covariance.empty()) { - covariance = odomInfo.covariance; + covariance = odomInfo.reg.covariance; } else { - covariance += odomInfo.covariance; + covariance += odomInfo.reg.covariance; } timer.restart(); @@ -431,7 +431,7 @@ int main(int argc, char * argv[]) ++iteration; printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms", - iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f)); + iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f)); if(processData && rtabmap.getLoopClosureId()>0) { printf(" *"); diff --git a/tools/RgbdDataset/main.cpp b/tools/RgbdDataset/main.cpp index 70b2fa90..b5f1a4ce 100644 --- a/tools/RgbdDataset/main.cpp +++ b/tools/RgbdDataset/main.cpp @@ -226,8 +226,8 @@ int main(int argc, char * argv[]) Transform pose = odom.process(data, &odomInfo); externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f)); externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f)); - externalStats.insert(std::make_pair("Odometry/Inliers/ms", odomInfo.inliers)); - externalStats.insert(std::make_pair("Odometry/Features/ms", odomInfo.features)); + externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers)); + externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features)); bool processData = true; if(detectionRate>0.0f && @@ -251,11 +251,11 @@ int main(int argc, char * argv[]) } if(covariance.empty()) { - covariance = odomInfo.covariance; + covariance = odomInfo.reg.covariance; } else { - covariance += odomInfo.covariance; + covariance += odomInfo.reg.covariance; } timer.restart(); @@ -269,7 +269,7 @@ int main(int argc, char * argv[]) ++iteration; printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d)=%dms, slam=%dms", - iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f)); + iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f)); if(processData && rtabmap.getLoopClosureId()>0) { printf(" *"); From 1ae15911fc5d213c0e64ebe596d9f14e6f10d5be Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 11 Sep 2017 14:00:08 -0400 Subject: [PATCH 09/17] Fixed compilation warning of not used variables when some odom approaches are not supported --- corelib/include/rtabmap/core/OdometryDVO.h | 2 ++ corelib/include/rtabmap/core/OdometryFovis.h | 4 +++- corelib/include/rtabmap/core/OdometryORBSLAM2.h | 2 ++ corelib/include/rtabmap/core/OdometryViso2.h | 2 ++ corelib/src/OdometryDVO.cpp | 3 ++- corelib/src/OdometryFovis.cpp | 6 ++++-- corelib/src/OdometryORBSLAM2.cpp | 6 ++++-- corelib/src/OdometryViso2.cpp | 5 ++++- 8 files changed, 23 insertions(+), 7 deletions(-) diff --git a/corelib/include/rtabmap/core/OdometryDVO.h b/corelib/include/rtabmap/core/OdometryDVO.h index e75939f1..2411cf77 100644 --- a/corelib/include/rtabmap/core/OdometryDVO.h +++ b/corelib/include/rtabmap/core/OdometryDVO.h @@ -53,10 +53,12 @@ private: virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0); private: +#ifdef RTABMAP_DVO dvo::DenseTracker * dvo_; dvo::core::RgbdImagePyramid * reference_; dvo::core::RgbdCameraPyramid * camera_; bool lost_; +#endif Transform motionFromKeyFrame_; Transform previousLocalTransform_; diff --git a/corelib/include/rtabmap/core/OdometryFovis.h b/corelib/include/rtabmap/core/OdometryFovis.h index 7b835ef5..23185173 100644 --- a/corelib/include/rtabmap/core/OdometryFovis.h +++ b/corelib/include/rtabmap/core/OdometryFovis.h @@ -53,13 +53,15 @@ private: virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0); private: +#ifdef RTABMAP_FOVIS fovis::VisualOdometry * fovis_; fovis::Rectification * rect_; fovis::StereoCalibration * stereoCalib_; fovis::DepthImage * depthImage_; fovis::StereoDepth * stereoDepth_; - ParametersMap fovisParameters_; bool lost_; +#endif + ParametersMap fovisParameters_; Transform previousLocalTransform_; }; diff --git a/corelib/include/rtabmap/core/OdometryORBSLAM2.h b/corelib/include/rtabmap/core/OdometryORBSLAM2.h index 5dab11a7..01626b5b 100644 --- a/corelib/include/rtabmap/core/OdometryORBSLAM2.h +++ b/corelib/include/rtabmap/core/OdometryORBSLAM2.h @@ -51,9 +51,11 @@ private: virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0); private: +#ifdef RTABMAP_ORB_SLAM2 ORBSLAM2System * orbslam2_; ORB_SLAM2::System * system_; bool firstFrame_; +#endif Transform originLocalTransform_; }; diff --git a/corelib/include/rtabmap/core/OdometryViso2.h b/corelib/include/rtabmap/core/OdometryViso2.h index 47e194e7..52658a11 100644 --- a/corelib/include/rtabmap/core/OdometryViso2.h +++ b/corelib/include/rtabmap/core/OdometryViso2.h @@ -47,12 +47,14 @@ private: virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0); private: +#ifdef RTABMAP_VISO2 VisualOdometryStereo * viso2_; int ref_frame_change_method_; // Reference frame method (defautl 0): 0=under inliers threshold, 1=min pixel motion, int ref_frame_inlier_threshold_; // method 0. Change the reference frame if the number of inliers is low double ref_frame_motion_threshold_; // method 1. Change the reference frame if last motion is small bool lost_; bool keep_reference_frame_; +#endif Transform reference_motion_; Transform previousLocalTransform_; ParametersMap viso2Parameters_; diff --git a/corelib/src/OdometryDVO.cpp b/corelib/src/OdometryDVO.cpp index edff9dd4..77fbc067 100644 --- a/corelib/src/OdometryDVO.cpp +++ b/corelib/src/OdometryDVO.cpp @@ -28,7 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/OdometryDVO.h" #include "rtabmap/core/OdometryInfo.h" #include "rtabmap/core/util2d.h" -#include "rtabmap/core/Version.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UTimer.h" #include "rtabmap/utilite/UStl.h" @@ -43,10 +42,12 @@ namespace rtabmap { OdometryDVO::OdometryDVO(const ParametersMap & parameters) : Odometry(parameters), +#ifdef RTABMAP_DVO dvo_(0), reference_(0), camera_(0), lost_(false), +#endif motionFromKeyFrame_(Transform::getIdentity()) { } diff --git a/corelib/src/OdometryFovis.cpp b/corelib/src/OdometryFovis.cpp index 0109f52a..081be1ef 100644 --- a/corelib/src/OdometryFovis.cpp +++ b/corelib/src/OdometryFovis.cpp @@ -28,7 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/OdometryFovis.h" #include "rtabmap/core/OdometryInfo.h" #include "rtabmap/core/util2d.h" -#include "rtabmap/core/Version.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UTimer.h" #include "rtabmap/utilite/UStl.h" @@ -40,13 +39,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { OdometryFovis::OdometryFovis(const ParametersMap & parameters) : - Odometry(parameters), + Odometry(parameters) +#ifdef RTABMAP_FOVIS + , fovis_(0), rect_(0), stereoCalib_(0), depthImage_(0), stereoDepth_(0), lost_(false) +#endif { fovisParameters_ = Parameters::filterParameters(parameters, "OdomFovis"); if(parameters.find(Parameters::kOdomVisKeyFrameThr()) != parameters.end()) diff --git a/corelib/src/OdometryORBSLAM2.cpp b/corelib/src/OdometryORBSLAM2.cpp index 2c7523f5..0be97494 100644 --- a/corelib/src/OdometryORBSLAM2.cpp +++ b/corelib/src/OdometryORBSLAM2.cpp @@ -28,7 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/OdometryORBSLAM2.h" #include "rtabmap/core/OdometryInfo.h" #include "rtabmap/core/util2d.h" -#include "rtabmap/core/Version.h" #include "rtabmap/core/util3d_transforms.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UTimer.h" @@ -746,10 +745,13 @@ public: namespace rtabmap { OdometryORBSLAM2::OdometryORBSLAM2(const ParametersMap & parameters) : - Odometry(parameters), + Odometry(parameters) +#ifdef RTABMAP_ORB_SLAM2 + , orbslam2_(0), system_(0), firstFrame_(true) +#endif { #ifdef RTABMAP_ORB_SLAM2 orbslam2_ = new ORBSLAM2System(parameters); diff --git a/corelib/src/OdometryViso2.cpp b/corelib/src/OdometryViso2.cpp index ee0fcdd8..aee0b67a 100644 --- a/corelib/src/OdometryViso2.cpp +++ b/corelib/src/OdometryViso2.cpp @@ -28,7 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/OdometryViso2.h" #include "rtabmap/core/OdometryInfo.h" #include "rtabmap/core/util2d.h" -#include "rtabmap/core/Version.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UTimer.h" #include "rtabmap/utilite/UStl.h" @@ -53,15 +52,19 @@ namespace rtabmap { OdometryViso2::OdometryViso2(const ParametersMap & parameters) : Odometry(parameters), +#ifdef RTABMAP_VISO2 viso2_(0), ref_frame_change_method_(0), ref_frame_inlier_threshold_(Parameters::defaultOdomVisKeyFrameThr()), ref_frame_motion_threshold_(5.0), lost_(false), keep_reference_frame_(false), +#endif reference_motion_(Transform::getIdentity()) { +#ifdef RTABMAP_VISO2 Parameters::parse(parameters, Parameters::kOdomVisKeyFrameThr(), ref_frame_inlier_threshold_); +#endif viso2Parameters_ = Parameters::filterParameters(parameters, "OdomViso2"); } From 77bdff1e4b8e8240d72cf4141c51c44c682a6aa8 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 11 Sep 2017 14:03:57 -0400 Subject: [PATCH 10/17] removed a debug log --- corelib/src/RegistrationIcp.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/corelib/src/RegistrationIcp.cpp b/corelib/src/RegistrationIcp.cpp index c70912eb..af060db6 100644 --- a/corelib/src/RegistrationIcp.cpp +++ b/corelib/src/RegistrationIcp.cpp @@ -444,7 +444,6 @@ Transform RegistrationIcp::computeTransformationImpl( double fromComplexity = util3d::computeNormalsComplexity(fromScan); double toComplexity = util3d::computeNormalsComplexity(toScan); - UWARN("%d->%d %f %f", fromSignature.id(), toSignature.id(), fromComplexity, toComplexity); float complexity = fromComplexity Date: Mon, 11 Sep 2017 14:44:39 -0400 Subject: [PATCH 11/17] GUI: update logger filter ids after starting the camera. Update default value of parameters "Odom/ScanKeyFrameThr=0.9" and "Icp/PointToPlaneMinComplexity=0.02" --- corelib/include/rtabmap/core/Parameters.h | 4 ++-- guilib/src/MainWindow.cpp | 2 ++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index a06fd9ff..5de16847 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -387,7 +387,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Odom, GuessMotion, bool, false, "Guess next transformation from the last motion computed."); RTABMAP_PARAM(Odom, KeyFrameThr, float, 0.3, "[Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame."); RTABMAP_PARAM(Odom, VisKeyFrameThr, int, 100, "[Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame."); - RTABMAP_PARAM(Odom, ScanKeyFrameThr, float, 0.7, "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame."); + RTABMAP_PARAM(Odom, ScanKeyFrameThr, float, 0.9, "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame."); RTABMAP_PARAM(Odom, ImageDecimation, int, 1, "Decimation of the images before registration. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value)."); RTABMAP_PARAM(Odom, AlignWithGround, bool, false, "Align odometry with the ground on initialization."); @@ -513,7 +513,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Icp, PointToPlane, bool, false, "Use point to plane ICP."); RTABMAP_PARAM(Icp, PointToPlaneK, int, 20, "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals."); RTABMAP_PARAM(Icp, PointToPlaneRadius, float, 0.0, "Search radius to compute normals for point to plane if the cloud doesn't have already normals."); - RTABMAP_PARAM(Icp, PointToPlaneMinComplexity, float, 0.0, "Minimum structural complexity (0.0=low, 1.0=high) of the scan to do point to plane registration, otherwise point to point registration is done instead."); + RTABMAP_PARAM(Icp, PointToPlaneMinComplexity, float, 0.02, "Minimum structural complexity (0.0=low, 1.0=high) of the scan to do point to plane registration, otherwise point to point registration is done instead."); // libpointmatcher RTABMAP_PARAM(Icp, PM, bool, false, "Use libpointmatcher for ICP registration instead of PCL's implementation."); diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 289bf455..dd5ec215 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -7108,6 +7108,7 @@ void MainWindow::changeState(MainWindow::State newState) if(_camera) { _camera->start(); + ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads()); } break; @@ -7139,6 +7140,7 @@ void MainWindow::changeState(MainWindow::State newState) if(_camera) { _camera->start(); + ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads()); } } else if(_state == kDetecting) From 16ffcc76843182ad873562b70a0abc30c685d8e4 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 11 Sep 2017 20:59:36 -0400 Subject: [PATCH 12/17] DatabaseViewer: just show single axis on constraint viewer if built with PCL < 1.7.2 --- guilib/src/DatabaseViewer.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 60fb5352..e95a2e0e 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -3805,8 +3805,8 @@ void DatabaseViewer::updateConstraintView( } //update coordinate - constraintsViewer_->addOrUpdateCoordinate("from_coordinate", pose, 0.2); +#if PCL_VERSION_COMPARE(>=, 1, 7, 2) constraintsViewer_->addOrUpdateCoordinate("to_coordinate", pose*t, 0.2); constraintsViewer_->removeCoordinate("to_coordinate_gt"); if(uContains(groundTruthPoses_, link.from()) && uContains(groundTruthPoses_, link.to())) @@ -3814,6 +3814,7 @@ void DatabaseViewer::updateConstraintView( constraintsViewer_->addOrUpdateCoordinate("to_coordinate_gt", pose*(groundTruthPoses_.at(link.from()).inverse()*groundTruthPoses_.at(link.to())), 0.1); } +#endif constraintsViewer_->clearTrajectory(); From b2fb7d5d5ba4436e17714c50df75448bb84c50a4 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 17 Sep 2017 13:28:25 -0400 Subject: [PATCH 13/17] Supporting g2o from ORB_SLAM2 --- cmake_modules/FindORB_SLAM2.cmake | 12 +-- corelib/src/CMakeLists.txt | 4 +- corelib/src/OptimizerG2O.cpp | 140 ++++++++++++++++++++++++++---- guilib/src/PreferencesDialog.cpp | 20 ++++- 4 files changed, 153 insertions(+), 23 deletions(-) diff --git a/cmake_modules/FindORB_SLAM2.cmake b/cmake_modules/FindORB_SLAM2.cmake index 095b9d78..41be0194 100644 --- a/cmake_modules/FindORB_SLAM2.cmake +++ b/cmake_modules/FindORB_SLAM2.cmake @@ -9,13 +9,15 @@ find_path(ORB_SLAM2_INCLUDE_DIR NAMES System.h PATHS $ENV{ORB_SLAM2_ROOT_DIR}/include) find_library(ORB_SLAM2_LIBRARY NAMES ORB_SLAM2 PATHS $ENV{ORB_SLAM2_ROOT_DIR}/lib) -find_library(g2o_LIBRARY NAMES g2o PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/g2o/lib) +find_path(g2o_INCLUDE_DIR NAMES g2o/core/sparse_optimizer.h PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/g2o NO_DEFAULT_PATH) +find_library(g2o_LIBRARY NAMES g2o PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/g2o/lib NO_DEFAULT_PATH) +find_library(DBoW2_LIBRARY NAMES DBoW2 PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/DBoW2/lib NO_DEFAULT_PATH) -IF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND g2o_LIBRARY) +IF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY) SET(ORB_SLAM2_FOUND TRUE) - SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIR} $ENV{ORB_SLAM2_ROOT_DIR}) - SET(ORB_SLAM2_LIBRARIES ${g2o_LIBRARY} ${ORB_SLAM2_LIBRARY}) -ENDIF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND g2o_LIBRARY) + SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIR} ${g2o_INCLUDE_DIR} $ENV{ORB_SLAM2_ROOT_DIR}) + SET(ORB_SLAM2_LIBRARIES ${g2o_LIBRARY} ${ORB_SLAM2_LIBRARY} ${DBoW2_LIBRARY}) +ENDIF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY) IF (ORB_SLAM2_FOUND) # show which ORB_SLAM2 was found only if not quiet diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 6e951eac..65badfa7 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -325,12 +325,12 @@ ENDIF(dvo_core_FOUND) IF(ORB_SLAM2_FOUND) SET(INCLUDE_DIRS + ${ORB_SLAM2_INCLUDE_DIRS} #before so that g2o includes are taken from ORB_SLAM2 directory before the official g2o one ${INCLUDE_DIRS} - ${ORB_SLAM2_INCLUDE_DIRS} ) SET(LIBRARIES + ${ORB_SLAM2_LIBRARIES} ${LIBRARIES} - ${ORB_SLAM2_LIBRARIES} ) ENDIF(ORB_SLAM2_FOUND) diff --git a/corelib/src/OptimizerG2O.cpp b/corelib/src/OptimizerG2O.cpp index 9146c12f..646d607d 100644 --- a/corelib/src/OptimizerG2O.cpp +++ b/corelib/src/OptimizerG2O.cpp @@ -37,19 +37,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#ifdef RTABMAP_G2O -#include "g2o/config.h" +#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2) #include "g2o/core/sparse_optimizer.h" #include "g2o/core/block_solver.h" #include "g2o/core/factory.h" #include "g2o/core/optimization_algorithm_factory.h" #include "g2o/core/optimization_algorithm_gauss_newton.h" #include "g2o/core/optimization_algorithm_levenberg.h" +#include "g2o/core/robust_kernel_impl.h" #include "g2o/core/linear_solver.h" + +#ifdef RTABMAP_G2O #include "g2o/types/sba/types_sba.h" +#include "g2o/solvers/eigen/linear_solver_eigen.h" +#include "g2o/config.h" #include "g2o/types/slam2d/types_slam2d.h" #include "g2o/types/slam3d/types_slam3d.h" -#include "g2o/core/robust_kernel_impl.h" #ifdef G2O_HAVE_CSPARSE #include "g2o/solvers/csparse/linear_solver_csparse.h" #endif @@ -57,14 +60,20 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifdef G2O_HAVE_CHOLMOD #include "g2o/solvers/cholmod/linear_solver_cholmod.h" #endif -#include "g2o/solvers/eigen/linear_solver_eigen.h" - enum { PARAM_OFFSET=0, }; +#endif // RTABMAP_G2O + +#ifdef RTABMAP_ORB_SLAM2 +#include "g2o/types/types_sba.h" +#include "g2o/types/types_six_dof_expmap.h" +#include "g2o/solvers/linear_solver_eigen.h" +#endif typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver; typedef g2o::LinearSolverEigen SlamLinearEigenSolver; +#ifdef RTABMAP_G2O typedef g2o::LinearSolverPCG SlamLinearPCGSolver; #ifdef G2O_HAVE_CSPARSE typedef g2o::LinearSolverCSparse SlamLinearCSparseSolver; @@ -79,14 +88,15 @@ typedef g2o::LinearSolverCholmod SlamLinearChol #include "vertigo/g2o/edge_se3Switchable.h" #include "vertigo/g2o/vertex_switchLinear.h" #endif +#endif -#endif // end RTABMAP_G2O +#endif // end defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2) namespace rtabmap { bool OptimizerG2O::available() { -#ifdef RTABMAP_G2O +#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2) return true; #else return false; @@ -123,6 +133,13 @@ void OptimizerG2O::parseParameters(const ParametersMap & parameters) UASSERT(pixelVariance_ > 0.0); UASSERT(baseline_ >= 0.0); +#ifdef RTABMAP_ORB_SLAM2 + if(solver_ != 3) + { + UWARN("g2o built with ORB_SLAM2 has only Eigen solver available, using Eigen=3 instead of %d.", solver_); + solver_ = 3; + } +#else #ifndef G2O_HAVE_CHOLMOD if(solver_ == 2) { @@ -138,6 +155,8 @@ void OptimizerG2O::parseParameters(const ParametersMap & parameters) solver_ = 1; } #endif +#endif + } std::map OptimizerG2O::optimize( @@ -612,8 +631,12 @@ std::map OptimizerG2O::optimize( UWARN("This method should be called at least with 1 pose!"); } UDEBUG("Optimizing graph...end!"); +#else +#ifdef RTABMAP_ORB_SLAM2 + UERROR("G2O graph optimization cannot be used with g2o built from ORB_SLAM2, only SBA is available."); #else UERROR("Not built with G2O support!"); +#endif #endif return optimizedPoses; } @@ -628,7 +651,7 @@ std::map OptimizerG2O::optimizeBA( std::set * outliers) { std::map optimizedPoses; -#ifdef RTABMAP_G2O +#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2) UDEBUG("Optimizing graph..."); optimizedPoses.clear(); @@ -638,6 +661,9 @@ std::map OptimizerG2O::optimizeBA( optimizer.setVerbose(ULogger::level()==ULogger::kDebug); g2o::BlockSolver_6_3::LinearSolverType * linearSolver = 0; +#ifdef RTABMAP_ORB_SLAM2 + linearSolver = new g2o::LinearSolverEigen(); +#else if(solver_ == 3) { //eigen @@ -663,14 +689,17 @@ std::map OptimizerG2O::optimizeBA( //pcg linearSolver = new g2o::LinearSolverPCG(); } +#endif g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver); +#ifndef RTABMAP_ORB_SLAM2 if(optimizer_ == 1) { optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(solver_ptr)); } else +#endif { optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(solver_ptr)); } @@ -686,9 +715,17 @@ std::map OptimizerG2O::optimizeBA( // Add node's pose UASSERT(!camPose.isNull()); +#ifdef RTABMAP_ORB_SLAM2 + g2o::VertexSE3Expmap * vCam = new g2o::VertexSE3Expmap(); +#else g2o::VertexCam * vCam = new g2o::VertexCam(); +#endif Eigen::Affine3d a = camPose.toEigen3d(); +#ifdef RTABMAP_ORB_SLAM2 + a = a.inverse(); + vCam->setEstimate(g2o::SE3Quat(a.rotation(), a.translation())); +#else g2o::SBACam cam(Eigen::Quaterniond(a.rotation()), a.translation()); cam.setKcam( iterModel->second.fx(), @@ -697,6 +734,7 @@ std::map OptimizerG2O::optimizeBA( iterModel->second.cy(), iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_); // baseline in meters vCam->setEstimate(cam); +#endif vCam->setId(iter->first); // negative root means that all other poses should be fixed instead of the root @@ -718,6 +756,7 @@ std::map OptimizerG2O::optimizeBA( ++iter; } +#ifndef RTABMAP_ORB_SLAM2 UDEBUG("fill edges to g2o..."); for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) { @@ -759,6 +798,7 @@ std::map OptimizerG2O::optimizeBA( } } } +#endif UDEBUG("fill 3D points to g2o..."); const int stepVertexId = poses.rbegin()->first+1; @@ -775,7 +815,7 @@ std::map OptimizerG2O::optimizeBA( vpt3d->setMarginalized(true); optimizer.addVertex(vpt3d); - //UDEBUG("Added 3D point %d (%f,%f,%f)", vpt3d->id()-stepVertexId, pt3d.x, pt3d.y, pt3d.z); + UDEBUG("Added 3D point %d (%f,%f,%f)", vpt3d->id()-stepVertexId, pt3d.x, pt3d.y, pt3d.z); // set observations for(std::map::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter) @@ -786,25 +826,62 @@ std::map OptimizerG2O::optimizeBA( const cv::Point3f & pt = jter->second; double depth = pt.z; - //UDEBUG("Added observation pt=%d to cam=%d (%f,%f) d=%f", vpt3d->id()-stepVertexId, camId, pt.x, pt.y, depth); + UDEBUG("Added observation pt=%d to cam=%d (%f,%f) d=%f", vpt3d->id()-stepVertexId, camId, pt.x, pt.y, depth); g2o::OptimizableGraph::Edge * e; + double baseline = 0.0; +#ifdef RTABMAP_ORB_SLAM2 + g2o::VertexSE3Expmap* vcam = dynamic_cast(optimizer.vertex(camId)); + std::map::const_iterator iterModel = models.find(camId); + + cv::Point3f t = util3d::transformPoint(pt3d, Transform::fromEigen3d(vcam->estimate()).inverse()); + UDEBUG("in cam %d frame=(%f,%f,%f)", camId, t.x, t.y, t.z); + + cv::Point3f t2 = util3d::transformPoint(pt3d, (poses.at(camId)*iterModel->second.localTransform()).inverse()); + UDEBUG("in cam2 %d frame=(%f,%f,%f)",camId, t2.x, t2.y, t2.z); + + g2o::Vector3d t3 = vcam->estimate().map(g2o::Vector3d(pt3d.x, pt3d.y, pt3d.z)); + UDEBUG("in cam3 %d frame=(%f,%f,%f)",camId, t3[0], t3[1], t3[2]); + + cv::Point3f t4 = util3d::transformPoint(pt3d, (poses.at(camId)*iterModel->second.localTransform())); + UDEBUG("in cam4 %d frame=(%f,%f,%f)",camId, t4.x, t4.y, t4.z); + + UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection()); + baseline = iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_; +#else g2o::VertexCam* vcam = dynamic_cast(optimizer.vertex(camId)); + baseline = vcam->estimate().baseline; +#endif double variance = pixelVariance_; - if(uIsFinite(depth) && depth > 0.0 && vcam->estimate().baseline > 0.0) + if(uIsFinite(depth) && depth > 0.0 && baseline > 0.0) { // stereo edge +#ifdef RTABMAP_ORB_SLAM2 + g2o::EdgeStereoSE3ProjectXYZ* es = new g2o::EdgeStereoSE3ProjectXYZ(); + float disparity = baseline * iterModel->second.fx() / depth; + Eigen::Vector3d obs( pt.x, pt.y, pt.x-disparity); + es->setMeasurement(obs); + //variance *= log(exp(1)+disparity); + es->setInformation(Eigen::Matrix3d::Identity() / variance); + es->fx = iterModel->second.fx(); + es->fy = iterModel->second.fy(); + es->cx = iterModel->second.cx(); + es->cy = iterModel->second.cy(); + es->bf = baseline*es->fx; + e = es; +#else g2o::EdgeProjectP2SC* es = new g2o::EdgeProjectP2SC(); - float disparity = vcam->estimate().baseline * vcam->estimate().Kcam(0,0) / depth; + float disparity = baseline * vcam->estimate().Kcam(0,0) / depth; Eigen::Vector3d obs( pt.x, pt.y, pt.x-disparity); es->setMeasurement(obs); //variance *= log(exp(1)+disparity); es->setInformation(Eigen::Matrix3d::Identity() / variance); e = es; +#endif } else { - if(vcam->estimate().baseline > 0.0) + if(baseline > 0.0) { UWARN("Stereo camera model detected but current " "observation (pt=%d to cam=%d) has null depth (%f m), adding " @@ -812,15 +889,28 @@ std::map OptimizerG2O::optimizeBA( vpt3d->id()-stepVertexId, camId, depth); } // mono edge +#ifdef RTABMAP_ORB_SLAM2 + g2o::EdgeSE3ProjectXYZ* em = new g2o::EdgeSE3ProjectXYZ(); + Eigen::Vector2d obs( pt.x, pt.y); + em->setMeasurement(obs); + em->setInformation(Eigen::Matrix2d::Identity() / variance); + em->fx = iterModel->second.fx(); + em->fy = iterModel->second.fy(); + em->cx = iterModel->second.cx(); + em->cy = iterModel->second.cy(); + e = em; + +#else g2o::EdgeProjectP2MC* em = new g2o::EdgeProjectP2MC(); Eigen::Vector2d obs( pt.x, pt.y); em->setMeasurement(obs); em->setInformation(Eigen::Matrix2d::Identity() / variance); e = em; +#endif } e->setVertex(0, vpt3d); e->setVertex(1, vcam); - + UDEBUG(""); if(robustKernelDelta_ > 0.0) { g2o::RobustKernelHuber* kernel = new g2o::RobustKernelHuber; @@ -875,8 +965,20 @@ std::map OptimizerG2O::optimizeBA( { (*iter)->setLevel(1); ++outliersCount; - double d = ((g2o::EdgeProjectP2SC*)(*iter))->measurement()[0]-((g2o::EdgeProjectP2SC*)(*iter))->measurement()[2]; + double d = 0.0; +#ifdef RTABMAP_ORB_SLAM2 + if(dynamic_cast(*iter) != 0) + { + d = ((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[0]-((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[2]; + } + UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2()); +#else + if(dynamic_cast(*iter) != 0) + { + d = ((g2o::EdgeProjectP2SC*)(*iter))->measurement()[0]-((g2o::EdgeProjectP2SC*)(*iter))->measurement()[2]; + } UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeProjectP2SC*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2()); +#endif const cv::Point3f & pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId); ((g2o::VertexSBAPointXYZ*)(*iter)->vertex(0))->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z)); @@ -909,11 +1011,19 @@ std::map OptimizerG2O::optimizeBA( // update poses for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) { +#ifdef RTABMAP_ORB_SLAM2 + const g2o::VertexSE3Expmap* v = (const g2o::VertexSE3Expmap*)optimizer.vertex(iter->first); +#else const g2o::VertexCam* v = (const g2o::VertexCam*)optimizer.vertex(iter->first); +#endif if(v) { Transform t = Transform::fromEigen3d(v->estimate()); +#ifdef RTABMAP_ORB_SLAM2 + t=t.inverse(); +#endif + // remove model local transform t *= models.at(iter->first).localTransform().inverse(); UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str()); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index b9a8bdae..4d3c2fb1 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -211,6 +211,13 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->loopClosure_bundle->setItemData(1, 0, Qt::UserRole - 1); _ui->groupBoxx_g2o->setEnabled(false); } +#ifdef RTABMAP_ORB_SLAM2 + else + { + // only graph optimization is disabled, g2o (from ORB_SLAM2) is valid only for SBA + _ui->graphOptimization_type->setItemData(1, 0, Qt::UserRole - 1); + } +#endif if(!OptimizerG2O::isCSparseAvailable()) { _ui->comboBox_g2o_solver->setItemData(0, 0, Qt::UserRole - 1); @@ -2434,6 +2441,7 @@ bool PreferencesDialog::validateForm() "with TORO. GTSAM is set instead for graph optimization strategy.")); _ui->graphOptimization_type->setCurrentIndex(Optimizer::kTypeGTSAM); } +#ifndef RTABMAP_ORB_SLAM2 else if(Optimizer::isAvailable(Optimizer::kTypeG2O)) { QMessageBox::warning(this, tr("Parameter warning"), @@ -2441,8 +2449,13 @@ bool PreferencesDialog::validateForm() "with TORO. g2o is set instead for graph optimization strategy.")); _ui->graphOptimization_type->setCurrentIndex(Optimizer::kTypeG2O); } +#endif } +#ifdef RTABMAP_ORB_SLAM2 + if(_ui->graphOptimization_type->currentIndex() == 1) +#else if(_ui->graphOptimization_type->currentIndex() == 1 && !Optimizer::isAvailable(Optimizer::kTypeG2O)) +#endif { if(Optimizer::isAvailable(Optimizer::kTypeGTSAM)) { @@ -2461,6 +2474,7 @@ bool PreferencesDialog::validateForm() } if(_ui->graphOptimization_type->currentIndex() == 2 && !Optimizer::isAvailable(Optimizer::kTypeGTSAM)) { +#ifndef RTABMAP_ORB_SLAM2 if(Optimizer::isAvailable(Optimizer::kTypeG2O)) { QMessageBox::warning(this, tr("Parameter warning"), @@ -2468,7 +2482,9 @@ bool PreferencesDialog::validateForm() "with GTSAM. g2o is set instead for graph optimization strategy.")); _ui->graphOptimization_type->setCurrentIndex(Optimizer::kTypeG2O); } - else if(Optimizer::isAvailable(Optimizer::kTypeTORO)) + else +#endif + if(Optimizer::isAvailable(Optimizer::kTypeTORO)) { QMessageBox::warning(this, tr("Parameter warning"), tr("Selected graph optimization strategy (GTSAM) is not available. RTAB-Map is not built " @@ -3487,7 +3503,9 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string ok = false; } #endif +#ifndef RTABMAP_ORB_SLAM2 if(!Optimizer::isAvailable(Optimizer::kTypeG2O)) +#endif { if(valueInt==1 && combo->objectName().toStdString().compare(Parameters::kOptimizerStrategy()) == 0) { From 2aa56c8d49df19c7d157aafd6edcf41641bdd8c5 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 18 Sep 2017 15:52:39 -0400 Subject: [PATCH 14/17] MainWindow: Fixed seg fault on scan rgb without normals --- guilib/src/MainWindow.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index dd5ec215..bce8b410 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -3042,10 +3042,10 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m } else if(cloudRGB.get()) { - added = _cloudViewer->addCloud(scanName, cloudWithNormals, pose, color); + added = _cloudViewer->addCloud(scanName, cloudRGB, pose, color); if(added && nodeId > 0) { - scan = util3d::laserScanFromPointCloud(*cloudWithNormals); + scan = util3d::laserScanFromPointCloud(*cloudRGB); } } else @@ -3064,7 +3064,6 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m } } } - if(!added) { UERROR("Adding cloud %d to viewer failed!", nodeId); From a70996f079ef2ab5c57163bbd7f4dc98bc7348d8 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 18 Sep 2017 17:00:15 -0400 Subject: [PATCH 15/17] util3d: Fixed color not copied on laser scan conversion --- corelib/src/util3d_transforms.cpp | 6 +++++- guilib/src/DatabaseViewer.cpp | 20 +++++++++++++++++++- guilib/src/MainWindow.cpp | 6 +++--- 3 files changed, 27 insertions(+), 5 deletions(-) diff --git a/corelib/src/util3d_transforms.cpp b/corelib/src/util3d_transforms.cpp index 2f2c886c..9cd97f6a 100644 --- a/corelib/src/util3d_transforms.cpp +++ b/corelib/src/util3d_transforms.cpp @@ -193,7 +193,9 @@ pcl::PointXYZRGB transformPoint( const pcl::PointXYZRGB & pt, const Transform & transform) { - return pcl::transformPoint(pt, transform.toEigen3f()); + pcl::PointXYZRGB ptRGB = pcl::transformPoint(pt, transform.toEigen3f()); + ptRGB.rgb = pt.rgb; + return ptRGB; } pcl::PointNormal transformPoint( const pcl::PointNormal & point, @@ -227,6 +229,8 @@ pcl::PointXYZRGBNormal transformPoint( ret.normal_x = static_cast (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2)); ret.normal_y = static_cast (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2)); ret.normal_z = static_cast (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2)); + + ret.rgb = point.rgb; return ret; } diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index e95a2e0e..c4c3e6f3 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -2698,7 +2698,16 @@ void DatabaseViewer::update(int value, //add scan if(ui_->checkBox_showScan->isChecked() && data.laserScanRaw().cols) { - if(data.laserScanRaw().channels() == 6) + if(data.laserScanRaw().channels() == 7) + { + pcl::PointCloud::Ptr scan = util3d::laserScanToPointCloudRGBNormal(data.laserScanRaw(), data.laserScanInfo().localTransform()); + if(ui_->doubleSpinBox_voxelSize->value() > 0.0) + { + scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value()); + } + cloudViewer_->addCloud("scan", scan, pose, Qt::yellow); + } + else if(data.laserScanRaw().channels() == 6) { pcl::PointCloud::Ptr scan = util3d::laserScanToPointCloudNormal(data.laserScanRaw(), data.laserScanInfo().localTransform()); if(ui_->doubleSpinBox_voxelSize->value() > 0.0) @@ -2707,6 +2716,15 @@ void DatabaseViewer::update(int value, } cloudViewer_->addCloud("scan", scan, pose, Qt::yellow); } + else if(data.laserScanRaw().channels() == 4) + { + pcl::PointCloud::Ptr scan = util3d::laserScanToPointCloudRGB(data.laserScanRaw(), data.laserScanInfo().localTransform()); + if(ui_->doubleSpinBox_voxelSize->value() > 0.0) + { + scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value()); + } + cloudViewer_->addCloud("scan", scan, pose, Qt::yellow); + } else { pcl::PointCloud::Ptr scan = util3d::laserScanToPointCloud(data.laserScanRaw(), data.laserScanInfo().localTransform()); diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index bce8b410..da4602a6 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -2920,7 +2920,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m } // Do ceiling/floor filtering - if(scan.channels() > 2 && // don't filter 2D scans + if((scan.channels() > 2 && scan.channels() != 5) && // don't filter 2D scans (_preferencesDialog->getScanFloorFilteringHeight() != 0.0 || _preferencesDialog->getScanCeilingFilteringHeight() != 0.0)) { @@ -2987,7 +2987,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m iter->sensorData().laserScanInfo().localTransform().z()); pcl::PointCloud::Ptr normals; - if(cloud->size()) + if(cloud.get() && cloud->size()) { if(scan.channels() == 2 || scan.channels() == 5) { @@ -3003,7 +3003,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m } else { - UASSERT(cloudRGB->size()); // Assuming 4 channels cannot be 2D + UASSERT(cloudRGB.get() && cloudRGB->size()); // Assuming 4 channels cannot be 2D normals = util3d::computeNormals(cloudRGB, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint); cloudRGBWithNormals.reset(new pcl::PointCloud); pcl::concatenateFields(*cloudRGB, *normals, *cloudRGBWithNormals); From cf6478b6335459c1a71289c22d5fa5c47ea1ad87 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 18 Sep 2017 21:49:57 -0400 Subject: [PATCH 16/17] util3d::transformLaserScan(): supporting 7 channels --- corelib/src/CameraThread.cpp | 6 +++--- corelib/src/util3d_transforms.cpp | 21 +++++++++++++++++++-- 2 files changed, 22 insertions(+), 5 deletions(-) diff --git a/corelib/src/CameraThread.cpp b/corelib/src/CameraThread.cpp index ce20bfd2..fa4151d3 100644 --- a/corelib/src/CameraThread.cpp +++ b/corelib/src/CameraThread.cpp @@ -298,7 +298,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const UASSERT(_scanDecimation >= 1); UTimer timer; pcl::IndicesPtr validIndices(new std::vector); - pcl::PointCloud::Ptr cloud = util3d::cloudFromSensorData( + pcl::PointCloud::Ptr cloud = util3d::cloudRGBFromSensorData( data, _scanDecimation, _scanMaxDepth, @@ -317,7 +317,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const } else if(!cloud->is_dense) { - pcl::PointCloud::Ptr denseCloud(new pcl::PointCloud); + pcl::PointCloud::Ptr denseCloud(new pcl::PointCloud); pcl::copyPointCloud(*cloud, *validIndices, *denseCloud); cloud = denseCloud; } @@ -328,7 +328,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const { Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z()); pcl::PointCloud::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, _scanNormalsRadius, viewPoint); - pcl::PointCloud::Ptr cloudNormals(new pcl::PointCloud); + pcl::PointCloud::Ptr cloudNormals(new pcl::PointCloud); pcl::concatenateFields(*cloud, *normals, *cloudNormals); scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse()); } diff --git a/corelib/src/util3d_transforms.cpp b/corelib/src/util3d_transforms.cpp index 9cd97f6a..cd3d3501 100644 --- a/corelib/src/util3d_transforms.cpp +++ b/corelib/src/util3d_transforms.cpp @@ -38,7 +38,7 @@ 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(5) || 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) || laserScan.type() == CV_32FC(7)); cv::Mat output = laserScan.clone(); @@ -81,7 +81,7 @@ cv::Mat transformLaserScan(const cv::Mat & laserScan, const Transform & transfor out[3] = pt.normal_y; out[4] = pt.normal_z; } - else // 6 and 7 channels + else if(laserScan.type() == CV_32FC(6)) { pcl::PointNormal pt; pt.x=ptr[0]; @@ -98,6 +98,23 @@ cv::Mat transformLaserScan(const cv::Mat & laserScan, const Transform & transfor out[4] = pt.normal_y; out[5] = pt.normal_z; } + else // 7 channels + { + pcl::PointNormal pt; + pt.x=ptr[0]; + pt.y=ptr[1]; + pt.z=ptr[2]; + pt.normal_x=ptr[4]; + pt.normal_y=ptr[5]; + pt.normal_z=ptr[6]; + pt = util3d::transformPoint(pt, transform); + out[0] = pt.x; + out[1] = pt.y; + out[2] = pt.z; + out[4] = pt.normal_x; + out[5] = pt.normal_y; + out[6] = pt.normal_z; + } } } return output; From fd18c0b2e97423f6ad6e5d966d0c5724c5dac66f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 19 Sep 2017 14:15:17 -0400 Subject: [PATCH 17/17] Added Mem/LaserScanVoxelSize and OdomF2M/ScanSubtractAngle parameters. util3d::computeNormalsComplexity() now returns PCA's eigen vectors and values optionally. RegistrationIcp: detecting complexity of environment when PointToPLane is used, if too low, PointToPoint is done and movements are limited to main direction of the normals. --- corelib/include/rtabmap/core/Memory.h | 1 + corelib/include/rtabmap/core/OdometryF2M.h | 1 + corelib/include/rtabmap/core/Parameters.h | 4 +- corelib/include/rtabmap/core/Statistics.h | 1 + corelib/include/rtabmap/core/util3d_surface.h | 16 +- corelib/src/Memory.cpp | 31 ++ corelib/src/OdometryF2M.cpp | 7 +- corelib/src/RegistrationIcp.cpp | 79 ++- corelib/src/util3d.cpp | 4 + corelib/src/util3d_surface.cpp | 59 ++- guilib/src/PreferencesDialog.cpp | 2 + guilib/src/ui/preferencesDialog.ui | 474 +++++++++++------- 12 files changed, 491 insertions(+), 188 deletions(-) diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index 4f418879..e323e91f 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -282,6 +282,7 @@ private: int _imagePostDecimation; bool _compressionParallelized; float _laserScanDownsampleStepSize; + float _laserScanVoxelSize; int _laserScanNormalK; int _laserScanNormalRadius; bool _reextractLoopClosureFeatures; diff --git a/corelib/include/rtabmap/core/OdometryF2M.h b/corelib/include/rtabmap/core/OdometryF2M.h index 1b28e739..df739c54 100644 --- a/corelib/include/rtabmap/core/OdometryF2M.h +++ b/corelib/include/rtabmap/core/OdometryF2M.h @@ -64,6 +64,7 @@ private: float scanKeyFrameThr_; int scanMaximumMapSize_; float scanSubtractRadius_; + float scanSubtractAngle_; int bundleAdjustment_; int bundleMaxFrames_; diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 5de16847..59a30a99 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -211,8 +211,9 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Mem, ImagePostDecimation, int, 1, "Image decimation (>=1) of saved data in created signatures (after features extraction). Decimation is done from the original image. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value)."); RTABMAP_PARAM(Mem, CompressionParallelized, bool, true, "Compression of sensor data is multi-threaded."); RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1, "If > 1, downsample the laser scans when creating a signature."); + RTABMAP_PARAM(Mem, LaserScanVoxelSize, float, 0.0, uFormat("If > 0 m, voxel filtering is done on laser scans when creating a signature. If the laser scan had normals, they will be removed. To recompute the normals, make sure to use \"%s\" or \"%s\" parameters.", kMemLaserScanNormalK().c_str(), kMemLaserScanNormalRadius().c_str()).c_str()); RTABMAP_PARAM(Mem, LaserScanNormalK, int, 0, "If > 0 and laser scans don't have normals, normals will be computed with K search neighbors when creating a signature."); - RTABMAP_PARAM(Mem, LaserScanNormalRadius, int, 0, "If > 0 and laser scans don't have normals, normals will be computed with radius search neighbors when creating a signature."); + RTABMAP_PARAM(Mem, LaserScanNormalRadius, int, 0, "If > 0 m and laser scans don't have normals, normals will be computed with radius search neighbors when creating a signature."); RTABMAP_PARAM(Mem, UseOdomFeatures, bool, false, "Use odometry features."); // KeypointMemory (Keypoint-based) @@ -396,6 +397,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(OdomF2M, MaxNewFeatures, int, 0, "[Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit."); RTABMAP_PARAM(OdomF2M, ScanMaxSize, int, 2000, "[Geometry] Maximum local scan map size."); RTABMAP_PARAM(OdomF2M, ScanSubtractRadius, float, 0.05, "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans."); + RTABMAP_PARAM(OdomF2M, ScanSubtractAngle, float, 45, uFormat("[Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when \"%s\">0). 0 means any angle.", kOdomF2MScanSubtractRadius().c_str()).c_str()); RTABMAP_PARAM(OdomF2M, BundleAdjustment, int, 0, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba."); RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 0, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map)."); diff --git a/corelib/include/rtabmap/core/Statistics.h b/corelib/include/rtabmap/core/Statistics.h index cd6def47..c81eee43 100644 --- a/corelib/include/rtabmap/core/Statistics.h +++ b/corelib/include/rtabmap/core/Statistics.h @@ -134,6 +134,7 @@ class RTABMAP_EXP Statistics RTABMAP_STATS(TimingMem, Compressing_data, ms); RTABMAP_STATS(TimingMem, Post_decimation, ms); RTABMAP_STATS(TimingMem, Scan_downsampling, ms); + RTABMAP_STATS(TimingMem, Scan_voxel_filtering, ms); RTABMAP_STATS(TimingMem, Scan_normals, ms); RTABMAP_STATS(TimingMem, Occupancy_grid, ms); diff --git a/corelib/include/rtabmap/core/util3d_surface.h b/corelib/include/rtabmap/core/util3d_surface.h index 4f723896..c8253f8d 100644 --- a/corelib/include/rtabmap/core/util3d_surface.h +++ b/corelib/include/rtabmap/core/util3d_surface.h @@ -263,16 +263,24 @@ pcl::PointCloud::Ptr RTABMAP_EXP computeFastOrganizedNormals( const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); float RTABMAP_EXP computeNormalsComplexity( - const cv::Mat & scan); + const cv::Mat & scan, + cv::Mat * pcaEigenVectors = 0, + cv::Mat * pcaEigenValues = 0); float RTABMAP_EXP computeNormalsComplexity( const pcl::PointCloud & normals, - bool is2d = false); + bool is2d = false, + cv::Mat * pcaEigenVectors = 0, + cv::Mat * pcaEigenValues = 0); float RTABMAP_EXP computeNormalsComplexity( const pcl::PointCloud & cloud, - bool is2d = false); + bool is2d = false, + cv::Mat * pcaEigenVectors = 0, + cv::Mat * pcaEigenValues = 0); float RTABMAP_EXP computeNormalsComplexity( const pcl::PointCloud & cloud, - bool is2d = false); + bool is2d = false, + cv::Mat * pcaEigenVectors = 0, + cv::Mat * pcaEigenValues = 0); pcl::PointCloud::Ptr RTABMAP_EXP mls( const pcl::PointCloud::Ptr & cloud, diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index eed46118..eec614cb 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -88,6 +88,7 @@ Memory::Memory(const ParametersMap & parameters) : _imagePostDecimation(Parameters::defaultMemImagePostDecimation()), _compressionParallelized(Parameters::defaultMemCompressionParallelized()), _laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()), + _laserScanVoxelSize(Parameters::defaultMemLaserScanVoxelSize()), _laserScanNormalK(Parameters::defaultMemLaserScanNormalK()), _laserScanNormalRadius(Parameters::defaultMemLaserScanNormalRadius()), _reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()), @@ -440,6 +441,7 @@ void Memory::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kMemImagePostDecimation(), _imagePostDecimation); Parameters::parse(parameters, Parameters::kMemCompressionParallelized(), _compressionParallelized); Parameters::parse(parameters, Parameters::kMemLaserScanDownsampleStepSize(), _laserScanDownsampleStepSize); + Parameters::parse(parameters, Parameters::kMemLaserScanVoxelSize(), _laserScanVoxelSize); Parameters::parse(parameters, Parameters::kMemLaserScanNormalK(), _laserScanNormalK); Parameters::parse(parameters, Parameters::kMemLaserScanNormalRadius(), _laserScanNormalRadius); Parameters::parse(parameters, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures); @@ -3760,6 +3762,35 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p if(stats) stats->addStatistic(Statistics::kTimingMemScan_downsampling(), t*1000.0f); UDEBUG("time downsampling scan = %fs", t); } + if(!laserScan.empty() && _laserScanVoxelSize > 0.0f && !isIntermediateNode) + { + float pointsBeforeFiltering = laserScan.cols; + if(laserScan.channels() == 4 || laserScan.channels() == 7) + { + pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloudRGB(laserScan); + cloud = util3d::voxelize(cloud, _laserScanVoxelSize); + laserScan = util3d::laserScanFromPointCloud(*cloud); + } + else + { + pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloud(laserScan); + cloud = util3d::voxelize(cloud, _laserScanVoxelSize); + if(laserScan.channels() == 2 || laserScan.channels() == 5) + { + laserScan = util3d::laserScan2dFromPointCloud(*cloud); + } + else + { + laserScan = util3d::laserScanFromPointCloud(*cloud); + } + } + float ratio = float(laserScan.cols) / pointsBeforeFiltering; + maxLaserScanMaxPts = int(float(maxLaserScanMaxPts) * ratio); + + t = timer.ticks(); + if(stats) stats->addStatistic(Statistics::kTimingMemScan_voxel_filtering(), t*1000.0f); + UDEBUG("time voxel filtering scan = %fs", t); + } if(!laserScan.empty() && (_laserScanNormalK > 0 || _laserScanNormalRadius>0.0f) && laserScan.channels() > 1 && laserScan.channels() < 5 && diff --git a/corelib/src/OdometryF2M.cpp b/corelib/src/OdometryF2M.cpp index f5c571c6..552611e4 100644 --- a/corelib/src/OdometryF2M.cpp +++ b/corelib/src/OdometryF2M.cpp @@ -65,6 +65,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) : scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()), scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()), scanSubtractRadius_(Parameters::defaultOdomF2MScanSubtractRadius()), + scanSubtractAngle_(Parameters::defaultOdomF2MScanSubtractAngle()), bundleAdjustment_(Parameters::defaultOdomF2MBundleAdjustment()), bundleMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()), map_(new Signature(-1)), @@ -80,6 +81,10 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) : Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_); Parameters::parse(parameters, Parameters::kOdomF2MScanMaxSize(), scanMaximumMapSize_); Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractRadius(), scanSubtractRadius_); + if(Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractAngle(), scanSubtractAngle_)) + { + scanSubtractAngle_ *= M_PI/180.0f; + } Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustment(), bundleAdjustment_); Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxFrames(), bundleMaxFrames_); UASSERT(bundleMaxFrames_ >= 0); @@ -605,7 +610,7 @@ Transform OdometryF2M::computeTransform( mapCloudNormals, pcl::IndicesPtr(new std::vector), scanSubtractRadius_, - 0.0f); + scanSubtractAngle_); newPoints = frameCloudNormalsIndices->size(); } else diff --git a/corelib/src/RegistrationIcp.cpp b/corelib/src/RegistrationIcp.cpp index af060db6..4bb2cf33 100644 --- a/corelib/src/RegistrationIcp.cpp +++ b/corelib/src/RegistrationIcp.cpp @@ -433,6 +433,7 @@ Transform RegistrationIcp::computeTransformationImpl( double variance = 1.0; bool transformComputed = false; bool tooLowComplexityForPlaneToPlane = false; + cv::Mat complexityVectors; if( _pointToPlane && _voxelSize == 0.0f && @@ -442,14 +443,16 @@ Transform RegistrationIcp::computeTransformationImpl( { //special case if we have already normals computed and there is no filtering - double fromComplexity = util3d::computeNormalsComplexity(fromScan); - double toComplexity = util3d::computeNormalsComplexity(toScan); + cv::Mat complexityVectorsFrom, complexityVectorsTo; + double fromComplexity = util3d::computeNormalsComplexity(fromScan, &complexityVectorsFrom); + double toComplexity = util3d::computeNormalsComplexity(toScan, &complexityVectorsTo); float complexity = fromComplexity 0.0f) + if(_voxelSize > 0.0f || !tooLowComplexityForPlaneToPlane) { // update output scans if(fromScan.channels() == 2 || fromScan.channels() == 5) @@ -743,7 +748,7 @@ Transform RegistrationIcp::computeTransformationImpl( } #ifdef RTABMAP_POINTMATCHER - if(_libpointmatcher && !_pointToPlane) // don't use libpointmatcher if it is configured for point to plane + if(_libpointmatcher) { // Load point clouds DP data = pclToDP(fromCloudFiltered, fromScan.channels() == 2 || fromScan.channels() == 5); @@ -756,7 +761,30 @@ Transform RegistrationIcp::computeTransformationImpl( UASSERT(_libpointmatcherICP != 0); PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP); UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)"); - T = icp(data, ref); + if(_pointToPlane) + { + // temporary set PointToPointErrorMinimizer + PM::ICP & icpTmp = icp; + icpTmp.errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer")); + + for(PM::OutlierFilters::iterator iter=icpTmp.outlierFilters.begin(); iter!=icpTmp.outlierFilters.end();) + { + if((*iter)->className.compare("SurfaceNormalOutlierFilter") == 0) + { + iter = icpTmp.outlierFilters.erase(iter); + } + else + { + ++iter; + } + } + + T = icpTmp(data, ref); + } + else + { + T = icp(data, ref); + } UDEBUG("libpointmatcher icp...done!"); icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim(T.template cast(), 4)))); @@ -790,6 +818,39 @@ Transform RegistrationIcp::computeTransformationImpl( if(!icpT.isNull() && hasConverged) { + if(tooLowComplexityForPlaneToPlane) + { + Transform guessInv = guess.inverse(); + Transform t = guessInv * icpT.inverse() * guess; + Eigen::Vector3f v(t.x(), t.y(), t.z()); + if(complexityVectors.cols == 2) + { + // limit translation in direction of the first eigen vector + Eigen::Vector3f n(complexityVectors.at(0,0), complexityVectors.at(0,1), 0.0f); + float a = v.dot(n); + v = n*a; + } + else if(complexityVectors.rows == 3) + { + // limit translation in direction of the first and second eigen vectors + Eigen::Vector3f n1(complexityVectors.at(0,0), complexityVectors.at(0,1), complexityVectors.at(0,2)); + Eigen::Vector3f n2(complexityVectors.at(1,0), complexityVectors.at(1,1), complexityVectors.at(1,2)); + float a = v.dot(n1); + float b = v.dot(n2); + v = n1*a; + v += n2*b; + } + else + { + UWARN("not supposed to be here!"); + v = Eigen::Vector3f(0,0,0); + } + float roll, pitch, yaw; + t.getEulerAngles(roll, pitch, yaw); + t = Transform(v[0], v[1], v[2], roll, pitch, yaw); + icpT = guess * t.inverse() * guessInv; + } + util3d::computeVarianceAndCorrespondences( fromCloudRegistered, toCloudFiltered, diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index fe3ad546..63bf4d43 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -1531,6 +1531,7 @@ pcl::PointCloud::Ptr laserScanToPointCloud(const cv::Mat & laserS pcl::PointCloud::Ptr output(new pcl::PointCloud); output->resize(laserScan.cols); + output->is_dense = true; bool nullTransform = transform.isNull(); Eigen::Affine3f transform3f = transform.toEigen3f(); for(int i=0; i::Ptr laserScanToPointCloudNormal(const cv::Mat pcl::PointCloud::Ptr output(new pcl::PointCloud); output->resize(laserScan.cols); + output->is_dense = true; bool nullTransform = transform.isNull(); for(int i=0; i::Ptr laserScanToPointCloudRGB(const cv::Mat & pcl::PointCloud::Ptr output(new pcl::PointCloud); output->resize(laserScan.cols); + output->is_dense = true; bool nullTransform = transform.isNull() || transform.isIdentity(); Eigen::Affine3f transform3f = transform.toEigen3f(); for(int i=0; i::Ptr laserScanToPointCloudRGBNormal(cons pcl::PointCloud::Ptr output(new pcl::PointCloud); output->resize(laserScan.cols); + output->is_dense = true; bool nullTransform = transform.isNull() || transform.isIdentity(); for(int i=0; i::Ptr computeFastOrganizedNormals( return normals; } -float computeNormalsComplexity(const cv::Mat & scan) +float computeNormalsComplexity( + const cv::Mat & scan, + cv::Mat * pcaEigenVectors, + cv::Mat * pcaEigenValues) { if(!scan.empty() && (scan.channels() == 5 || scan.channels() == 6 || scan.channels() == 7)) { @@ -2384,6 +2387,15 @@ float computeNormalsComplexity(const cv::Mat & scan) { cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW); + if(pcaEigenVectors) + { + *pcaEigenVectors = pca_analysis.eigenvectors; + } + if(pcaEigenValues) + { + *pcaEigenValues = pca_analysis.eigenvalues; + } + // Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity return pca_analysis.eigenvalues.at(0, is2d?1:2)*(is2d?2.0f:3.0f); } @@ -2395,7 +2407,11 @@ float computeNormalsComplexity(const cv::Mat & scan) return 0.0f; } -float computeNormalsComplexity(const pcl::PointCloud & cloud, bool is2d) +float computeNormalsComplexity( + const pcl::PointCloud & cloud, + bool is2d, + cv::Mat * pcaEigenVectors, + cv::Mat * pcaEigenValues) { //Construct a buffer used by the pca analysis int sz = static_cast(cloud.size()*2); @@ -2419,13 +2435,26 @@ float computeNormalsComplexity(const pcl::PointCloud & cloud, { cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW); + if(pcaEigenVectors) + { + *pcaEigenVectors = pca_analysis.eigenvectors; + } + if(pcaEigenValues) + { + *pcaEigenValues = pca_analysis.eigenvalues; + } + // Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity return pca_analysis.eigenvalues.at(0, is2d?1:2)*(is2d?2.0f:3.0f); } return 0.0f; } -float computeNormalsComplexity(const pcl::PointCloud & normals, bool is2d) +float computeNormalsComplexity( + const pcl::PointCloud & normals, + bool is2d, + cv::Mat * pcaEigenVectors, + cv::Mat * pcaEigenValues) { //Construct a buffer used by the pca analysis int sz = static_cast(normals.size()*2); @@ -2449,13 +2478,26 @@ float computeNormalsComplexity(const pcl::PointCloud & normals, boo { cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW); + if(pcaEigenVectors) + { + *pcaEigenVectors = pca_analysis.eigenvectors; + } + if(pcaEigenValues) + { + *pcaEigenValues = pca_analysis.eigenvalues; + } + // Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity return pca_analysis.eigenvalues.at(0, is2d?1:2)*(is2d?2.0f:3.0f); } return 0.0f; } -float computeNormalsComplexity(const pcl::PointCloud & cloud, bool is2d) +float computeNormalsComplexity( + const pcl::PointCloud & cloud, + bool is2d, + cv::Mat * pcaEigenVectors, + cv::Mat * pcaEigenValues) { //Construct a buffer used by the pca analysis int sz = static_cast(cloud.size()*2); @@ -2479,6 +2521,15 @@ float computeNormalsComplexity(const pcl::PointCloud & c { cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW); + if(pcaEigenVectors) + { + *pcaEigenVectors = pca_analysis.eigenvectors; + } + if(pcaEigenValues) + { + *pcaEigenValues = pca_analysis.eigenvalues; + } + // Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity return pca_analysis.eigenvalues.at(0, is2d?1:2)*(is2d?2.0f:3.0f); } diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 4d3c2fb1..a16c0a60 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -659,6 +659,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->spinBox_imagePreDecimation->setObjectName(Parameters::kMemImagePreDecimation().c_str()); _ui->spinBox_imagePostDecimation->setObjectName(Parameters::kMemImagePostDecimation().c_str()); _ui->general_spinBox_laserScanDownsample->setObjectName(Parameters::kMemLaserScanDownsampleStepSize().c_str()); + _ui->general_doubleSpinBox_laserScanVoxelSize->setObjectName(Parameters::kMemLaserScanVoxelSize().c_str()); _ui->general_spinBox_laserScanNormalK->setObjectName(Parameters::kMemLaserScanNormalK().c_str()); _ui->general_doubleSpinBox_laserScanNormalRadius->setObjectName(Parameters::kMemLaserScanNormalRadius().c_str()); _ui->checkBox_useOdomFeatures->setObjectName(Parameters::kMemUseOdomFeatures().c_str()); @@ -932,6 +933,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->spinBox_odom_f2m_maxNewFeatures->setObjectName(Parameters::kOdomF2MMaxNewFeatures().c_str()); _ui->spinBox_odom_f2m_scanMaxSize->setObjectName(Parameters::kOdomF2MScanMaxSize().c_str()); _ui->doubleSpinBox_odom_f2m_scanRadius->setObjectName(Parameters::kOdomF2MScanSubtractRadius().c_str()); + _ui->doubleSpinBox_odom_f2m_scanAngle->setObjectName(Parameters::kOdomF2MScanSubtractAngle().c_str()); _ui->odom_f2m_bundleStrategy->setObjectName(Parameters::kOdomF2MBundleAdjustment().c_str()); _ui->odom_f2m_bundleMaxFrames->setObjectName(Parameters::kOdomF2MBundleAdjustmentMaxFrames().c_str()); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 273fbfae..576f4aad 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,16 +63,25 @@ 0 - -215 + -629 678 - 2736 + 2739 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -86,7 +95,7 @@ QFrame::Raised - 21 + 18 @@ -4561,7 +4570,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki Directory of images (optional settings) - + + 0 + + + 0 + + + 0 + + 0 @@ -6040,6 +6058,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + -16 + + + 16 + + + @@ -6108,19 +6136,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 999 - - - 1 - - - 2 - - - @@ -6131,16 +6146,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - Bad signatures are ignored. + + + + 999 - - true + + 1 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 2 @@ -6164,10 +6179,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Keep raw sensor data. Only useful to save loop closure computation time when features re-extraction is enabled. Disable to save RAM memory. + Bad signatures are ignored. true @@ -6187,6 +6202,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + Keep raw sensor data. Only useful to save loop closure computation time when features re-extraction is enabled. Disable to save RAM memory. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + @@ -6200,22 +6228,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - 50 - 0 - - - - 1 - - - 9999 - - - @@ -6288,6 +6300,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + + false + + + @@ -6311,16 +6333,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - false - - - @@ -6334,6 +6346,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + + false + + + @@ -6360,16 +6382,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - false - - - @@ -6383,48 +6395,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - If > 1, downsample the laser scans when creating a location. This feature can be used to save laser scans already downsampled. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - -16 - - - 16 - - - - - - - - 50 - 0 - - - - 0 - - - 99 - - - 0 - - - @@ -6435,16 +6405,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - If > 0 and laser scans don't have normals, normals will be computed with K search neighbors when creating a signature. + - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + false @@ -6461,16 +6428,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - false - - - @@ -6484,34 +6441,139 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - If > 0 and laser scans don't have normals, normals will be computed with radius search when creating a signature. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - 50 - 0 - - - - 0.010000000000000 - - - + + + + Laser scan filtering + + + + + + + 50 + 0 + + + + 1 + + + 9999 + + + + + + + Downsampling step. If > 1, downsample the laser scans when creating a location. This feature can be used to save laser scans already downsampled. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + 50 + 0 + + + + m + + + 0.010000000000000 + + + + + + + + 50 + 0 + + + + 0 + + + 99 + + + 0 + + + + + + + Normal K. If > 0 and laser scans don't have normals, normals will be computed with K search neighbors when creating a signature. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Normal Radius. If > 0 and laser scans don't have normals, normals will be computed with radius search when creating a signature. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Voxel size. If > 0 m, voxel filtering is done on laser scans when creating a signature. If the laser scan had normals, they will be removed. To recompute the normals, make sure to use Normal K or Normal Radius parameters below. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + 50 + 0 + + + + m + + + 3 + + + 0.010000000000000 + + + + + + @@ -10114,6 +10176,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + 999999999 + + + @@ -10140,14 +10209,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 999999999 - - - - + [Visual] Local bundle adjustment. See Optimizer panel. This will not work if Optical Flow correspondences strategy is selected in Visual Registration panel. @@ -10160,7 +10222,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + + + + 999999 + + + + [Visual] Maximum frames used for bundle adjustment (0=inf or all current frames in the local map). @@ -10173,7 +10242,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + QComboBox::AdjustToContents @@ -10195,10 +10264,32 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + [Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when Radius above is >0). 0 means any angle. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + deg + + + 0 + - 999999 + 180.000000000000000 + + + 0.000000000000000 @@ -12562,7 +12653,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + + 0 + + + 0 + + + 0 + + 0 @@ -12702,7 +12802,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + + 0 + + + 0 + + + 0 + + 0 @@ -12860,7 +12969,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -12940,7 +13058,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -13052,7 +13179,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - + + 0 + + + 0 + + + 0 + + 0