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/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/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..e323e91f 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -282,7 +282,9 @@ private: int _imagePostDecimation; bool _compressionParallelized; float _laserScanDownsampleStepSize; + float _laserScanVoxelSize; int _laserScanNormalK; + int _laserScanNormalRadius; bool _reextractLoopClosureFeatures; float _rehearsalMaxDistance; float _rehearsalMaxAngle; 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/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/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/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/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/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/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/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index affcb602..59a30a99 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -211,7 +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, 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, 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 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) @@ -386,7 +388,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."); @@ -395,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)."); @@ -510,7 +513,9 @@ 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."); + 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/corelib/include/rtabmap/core/RegistrationIcp.h b/corelib/include/rtabmap/core/RegistrationIcp.h index 546292c7..05fe86b8 100644 --- a/corelib/include/rtabmap/core/RegistrationIcp.h +++ b/corelib/include/rtabmap/core/RegistrationIcp.h @@ -64,7 +64,9 @@ private: float _epsilon; float _correspondenceRatio; bool _pointToPlane; - int _pointToPlaneNormalNeighbors; + 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..c81eee43 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,); @@ -131,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.h b/corelib/include/rtabmap/core/util3d.h index 73a149ad..28b9f629 100644 --- a/corelib/include/rtabmap/core/util3d.h +++ b/corelib/include/rtabmap/core/util3d.h @@ -192,15 +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_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/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..c8253f8d 100644 --- a/corelib/include/rtabmap/core/util3d_surface.h +++ b/corelib/include/rtabmap/core/util3d_surface.h @@ -212,38 +212,76 @@ 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, float normalSmoothingSize = 10.0f, const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0)); +float RTABMAP_EXP computeNormalsComplexity( + const cv::Mat & scan, + cv::Mat * pcaEigenVectors = 0, + cv::Mat * pcaEigenValues = 0); +float RTABMAP_EXP computeNormalsComplexity( + const pcl::PointCloud & normals, + bool is2d = false, + cv::Mat * pcaEigenVectors = 0, + cv::Mat * pcaEigenValues = 0); +float RTABMAP_EXP computeNormalsComplexity( + const pcl::PointCloud & cloud, + bool is2d = false, + cv::Mat * pcaEigenVectors = 0, + cv::Mat * pcaEigenValues = 0); +float RTABMAP_EXP computeNormalsComplexity( + const pcl::PointCloud & cloud, + bool is2d = false, + cv::Mat * pcaEigenVectors = 0, + cv::Mat * pcaEigenValues = 0); + pcl::PointCloud::Ptr RTABMAP_EXP mls( const pcl::PointCloud::Ptr & cloud, float searchRadius = 0.0f, 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/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..fa4151d3 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), @@ -297,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, @@ -316,18 +317,18 @@ 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; } 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 cloudNormals(new pcl::PointCloud); + 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/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 c0367929..eec614cb 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -88,7 +88,9 @@ 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()), _rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()), _rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()), @@ -439,7 +441,9 @@ 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); Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance); Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle); @@ -2404,6 +2408,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; @@ -2436,6 +2452,8 @@ 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) { if(iter->first != fromId) @@ -2445,14 +2463,33 @@ 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; + } + + 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 { @@ -2460,15 +2497,22 @@ Transform Memory::computeIcpTransformMulti( } } } - if(assembledToClouds->size()) + + cv::Mat assembledScan; + if(assembledToNormalClouds->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 + 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; @@ -3268,7 +3312,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 && @@ -3718,13 +3762,41 @@ 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() && _laserScanVoxelSize > 0.0f && !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); + 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 && + !isIntermediateNode) + { + 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/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/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/OdometryDVO.cpp b/corelib/src/OdometryDVO.cpp index c05a16de..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()) { } @@ -270,7 +271,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 51520518..be1ed402 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 { @@ -243,12 +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 9103d8c6..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); @@ -186,9 +191,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 +597,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); @@ -604,7 +610,7 @@ Transform OdometryF2M::computeTransform( mapCloudNormals, pcl::IndicesPtr(new std::vector), scanSubtractRadius_, - 0.0f); + scanSubtractAngle_); newPoints = frameCloudNormalsIndices->size(); } else @@ -623,17 +629,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 +686,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 +706,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 +732,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 +846,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 +880,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()); } } } @@ -873,10 +899,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; @@ -886,8 +908,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..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()) @@ -381,9 +383,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..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); @@ -891,15 +893,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 +917,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..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"); } @@ -273,11 +276,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/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/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..4bb2cf33 100644 --- a/corelib/src/RegistrationIcp.cpp +++ b/corelib/src/RegistrationIcp.cpp @@ -36,16 +36,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include -#include #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; @@ -65,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 @@ -74,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; @@ -107,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); @@ -122,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; @@ -153,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; i= 0.0f && _pointToPlaneMinComplexity <= 1.0f); Parameters::parse(parameters, Parameters::kIcpPM(), _libpointmatcher); Parameters::parse(parameters, Parameters::kIcpPMConfig(), _libpointmatcherConfig); @@ -319,8 +329,20 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters) icp->outlierFilters.clear(); icp->outlierFilters.push_back(PM::get().OutlierFilterRegistrar.create("TrimmedDistOutlierFilter", params)); params.clear(); + if(_pointToPlane) + { + params["maxAngle"] = uNumber2Str(_maxRotation<=0.0f?M_PI:_maxRotation); + icp->outlierFilters.push_back(PM::get().OutlierFilterRegistrar.create("SurfaceNormalOutlierFilter", params)); + params.clear(); - icp->errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create(_pointToPlane?"PointToPlaneErrorMinimizer":"PointToPointErrorMinimizer")); + 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); @@ -332,6 +354,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 @@ -342,7 +369,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 +381,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); @@ -403,199 +431,46 @@ Transform RegistrationIcp::computeTransformationImpl( float correspondencesRatio = 0.0f; int correspondences = 0; double variance = 1.0; + bool transformComputed = false; + bool tooLowComplexityForPlaneToPlane = false; + cv::Mat complexityVectors; 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 - pcl::PointCloud::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform); - pcl::PointCloud::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toLocalTransform); - UDEBUG("Conversion time = %f s", timer.ticks()); - pcl::PointCloud::Ptr fromCloudNormalsRegistered(new pcl::PointCloud()); -#ifdef RTABMAP_POINTMATCHER - if(_libpointmatcher) + cv::Mat complexityVectorsFrom, complexityVectorsTo; + double fromComplexity = util3d::computeNormalsComplexity(fromScan, &complexityVectorsFrom); + double toComplexity = util3d::computeNormalsComplexity(toScan, &complexityVectorsTo); + float complexity = fromComplexity(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()); - } + tooLowComplexityForPlaneToPlane = true; + complexityVectors = fromComplexityforce3DoF()); - } + 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); - } - } - else - { - pcl::PointCloud::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, fromLocalTransform); - pcl::PointCloud::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess * toLocalTransform); - UDEBUG("Conversion time = %f s", timer.ticks()); - - pcl::PointCloud::Ptr fromCloudFiltered = fromCloud; - pcl::PointCloud::Ptr toCloudFiltered = toCloud; - if(_voxelSize > 0.0f) - { - int pointsBeforeFiltering = fromCloudFiltered->size(); - fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _voxelSize); - maxLaserScansFrom = maxLaserScansFrom * fromCloudFiltered->size() / pointsBeforeFiltering; - - pointsBeforeFiltering = toCloudFiltered->size(); - toCloudFiltered = util3d::voxelize(toCloudFiltered, _voxelSize); - maxLaserScansTo = maxLaserScansTo * toCloudFiltered->size() / pointsBeforeFiltering; - - UDEBUG("Voxel filtering time (voxel=%f m, ratioFrom=%f ratioTo=%f) = %f s", - _voxelSize, - float(fromCloudFiltered->size()) / float(pointsBeforeFiltering), - float(toCloudFiltered->size()) / float(pointsBeforeFiltering), - timer.ticks()); - } - - pcl::PointCloud::Ptr fromCloudRegistered(new pcl::PointCloud()); - if(_pointToPlane) // ICP Point To Plane, only in 3D - { - pcl::PointCloud::Ptr normals; - - normals = util3d::computeNormals(fromCloudFiltered, _pointToPlaneNormalNeighbors); - pcl::PointCloud::Ptr fromCloudNormals(new pcl::PointCloud); - pcl::concatenateFields(*fromCloudFiltered, *normals, *fromCloudNormals); - - normals = util3d::computeNormals(toCloudFiltered, _pointToPlaneNormalNeighbors); - 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 - 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()); - - if(toCloudNormals->size() && fromCloudNormals->size()) - { - pcl::PointCloud::Ptr fromCloudNormalsRegistered(new pcl::PointCloud()); - -#ifdef RTABMAP_POINTMATCHER - if(_libpointmatcher) - { - // Load point clouds - DP data = pclToDP(fromCloudNormals); - DP ref = pclToDP(toCloudNormals); - - // 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); - 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()); - } - } - else -#endif - { - icpT = util3d::icpPointToPlane( - fromCloudNormals, - toCloudNormals, - _maxCorrespondenceDistance, - _maxIterations, - hasConverged, - *fromCloudNormalsRegistered, - _epsilon, - this->force3DoF()); - } + toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals); - if(!icpT.isNull() && hasConverged) - { - util3d::computeVarianceAndCorrespondences( - fromCloudNormalsRegistered, - toCloudNormals, - _maxCorrespondenceDistance, - variance, - correspondences); - } - } - } - else // ICP Point to Point - { - 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)); - } - + 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(fromCloudFiltered); - DP ref = pclToDP(toCloudFiltered); + 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; @@ -605,6 +480,311 @@ 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); + 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; + } + } + + if(!transformComputed) + { + pcl::PointCloud::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, fromLocalTransform); + pcl::PointCloud::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess * toLocalTransform); + UDEBUG("Conversion time = %f s", timer.ticks()); + + pcl::PointCloud::Ptr fromCloudFiltered = fromCloud; + pcl::PointCloud::Ptr toCloudFiltered = toCloud; + if(_voxelSize > 0.0f) + { + float pointsBeforeFiltering = (float)fromCloudFiltered->size(); + fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _voxelSize); + float ratioFrom = float(fromCloudFiltered->size()) / pointsBeforeFiltering; + maxLaserScansFrom = int(float(maxLaserScansFrom) * ratioFrom); + + pointsBeforeFiltering = (float)toCloudFiltered->size(); + toCloudFiltered = util3d::voxelize(toCloudFiltered, _voxelSize); + float ratioTo = float(toCloudFiltered->size()) / pointsBeforeFiltering; + maxLaserScansTo = int(float(maxLaserScansTo) * ratioTo); + + UDEBUG("Voxel filtering time (voxel=%f m, ratioFrom=%f->%d/%d ratioTo=%f->%d/%d) = %f s", + _voxelSize, + ratioFrom, + (int)fromCloudFiltered->size(), + maxLaserScansFrom, + ratioTo, + (int)toCloudFiltered->size(), + maxLaserScansTo, + timer.ticks()); + } + + 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 + { + 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()); + pcl::PointCloud::Ptr normalsTo; + if(toScan.channels() == 2 || toScan.channels() == 5) + { + if(_voxelSize > 0.0f) + { + normalsTo = util3d::computeNormals2D( + toCloudFiltered, + _pointToPlaneK, + _pointToPlaneRadius, + viewpointTo); + } + else + { + normalsTo = util3d::computeFastOrganizedNormals2D( + toCloudFiltered, + _pointToPlaneK, + _pointToPlaneRadius, + viewpointTo); + } + } + else + { + normalsTo = util3d::computeNormals(toCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointTo); + } + + cv::Mat complexityVectorsFrom, complexityVectorsTo; + double fromComplexity = util3d::computeNormalsComplexity(*normalsFrom, fromScan.channels() == 2 || fromScan.channels() == 5, &complexityVectorsFrom); + double toComplexity = util3d::computeNormalsComplexity(*normalsTo, toScan.channels() == 2 || toScan.channels() == 5, &complexityVectorsTo); + float complexity = fromComplexity::Ptr fromCloudNormals(new pcl::PointCloud); + pcl::concatenateFields(*fromCloudFiltered, *normalsFrom, *fromCloudNormals); + + pcl::PointCloud::Ptr toCloudNormals(new pcl::PointCloud); + pcl::concatenateFields(*toCloudFiltered, *normalsTo, *toCloudNormals); + + 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 + { + 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()); + } + } + 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; + } + } + + if(!transformComputed) // ICP Point to Point + { + 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()); + } + + if(_voxelSize > 0.0f || !tooLowComplexityForPlaneToPlane) + { + // update output scans + 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 + if(_libpointmatcher) + { + // Load point clouds + 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; + 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.)"); + 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)))); @@ -638,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/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index facc0eb8..1937b8af 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -1116,14 +1116,17 @@ 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())); } } 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); } } @@ -1228,9 +1231,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 +1512,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 +1558,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 +1699,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 +1746,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 +2007,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 +2102,7 @@ bool Rtabmap::process( } else { - UDEBUG("Path %d ignored", nearestId); + //UDEBUG("Path %d ignored", nearestId); } } } @@ -2996,7 +2997,7 @@ std::map > Rtabmap::getPaths(std::mapfirst); + //UDEBUG("%d <- %d", nearestId, jter->first); path.insert(*jter); poses.erase(jter); } 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/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(); diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index f9279fa8..63bf4d43 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)); @@ -1419,12 +1459,79 @@ 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); + output->is_dense = true; bool nullTransform = transform.isNull(); Eigen::Affine3f transform3f = transform.toEigen3f(); for(int i=0; i::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); + output->is_dense = true; bool nullTransform = transform.isNull(); for(int i=0; i::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); + output->is_dense = true; bool nullTransform = transform.isNull() || transform.isIdentity(); Eigen::Affine3f transform3f = transform.toEigen3f(); for(int i=0; i::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); + output->is_dense = true; bool nullTransform = transform.isNull() || transform.isIdentity(); for(int i=0; i::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]; } @@ -1511,16 +1621,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]; @@ -1538,12 +1654,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]; } @@ -1566,16 +1682,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]; @@ -1606,12 +1728,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); @@ -1622,7 +1745,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]; @@ -1689,7 +1812,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); @@ -1700,46 +1823,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_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_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/corelib/src/util3d_surface.cpp b/corelib/src/util3d_surface.cpp index e1648110..6fc96dd5 100644 --- a/corelib/src/util3d_surface.cpp +++ b/corelib/src/util3d_surface.cpp @@ -1994,18 +1994,52 @@ 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); + return util3d::laserScan2dFromPointCloud(*cloud, *normals); + } + 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 +2066,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 +2076,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 +2114,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, @@ -2132,6 +2338,204 @@ pcl::PointCloud::Ptr computeFastOrganizedNormals( return normals; } +float computeNormalsComplexity( + const cv::Mat & scan, + cv::Mat * pcaEigenVectors, + cv::Mat * pcaEigenValues) +{ + 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); + + 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); + } + } + else if(!scan.empty()) + { + UERROR("Scan doesn't have normals!"); + } + return 0.0f; +} + +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); + 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); + + 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, + cv::Mat * pcaEigenVectors, + cv::Mat * pcaEigenValues) +{ + //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); + + 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, + cv::Mat * pcaEigenVectors, + cv::Mat * pcaEigenValues) +{ + //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); + + 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; +} + pcl::PointCloud::Ptr mls( const pcl::PointCloud::Ptr & cloud, float searchRadius, diff --git a/corelib/src/util3d_transforms.cpp b/corelib/src/util3d_transforms.cpp index 15626527..cd3d3501 100644 --- a/corelib/src/util3d_transforms.cpp +++ b/corelib/src/util3d_transforms.cpp @@ -38,61 +38,82 @@ 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) || laserScan.type() == CV_32FC(7)); 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 if(laserScan.type() == CV_32FC(6)) + { + pcl::PointNormal pt; + pt.x=ptr[0]; + pt.y=ptr[1]; + pt.z=ptr[2]; + pt.normal_x=ptr[3]; + pt.normal_y=ptr[4]; + pt.normal_z=ptr[5]; + pt = util3d::transformPoint(pt, transform); + out[0] = pt.x; + out[1] = pt.y; + out[2] = pt.z; + out[3] = pt.normal_x; + out[4] = pt.normal_y; + out[5] = pt.normal_z; + } + else // 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; } } } @@ -189,7 +210,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, @@ -223,6 +246,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/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..0d6607cb 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 @@ -298,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/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index c40548ab..0edff4a7 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()); @@ -472,11 +496,22 @@ 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())) + //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 || + _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/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index a3a54a9a..c4c3e6f3 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()) { @@ -2697,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) @@ -2706,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()); @@ -3757,7 +3776,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 +3799,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()); @@ -3804,8 +3823,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())) @@ -3813,6 +3832,7 @@ void DatabaseViewer::updateConstraintView( constraintsViewer_->addOrUpdateCoordinate("to_coordinate_gt", pose*(groundTruthPoses_.at(link.from()).inverse()*groundTruthPoses_.at(link.to())), 0.1); } +#endif constraintsViewer_->clearTrajectory(); @@ -4020,19 +4040,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/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/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/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 e80ca600..da4602a6 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()); @@ -2651,9 +2657,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 +2796,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 +2886,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() == 5 || 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 && scan.channels() != 5) && // 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 +2935,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 +2975,109 @@ 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.get() && cloud->size()) { - color = (Qt::GlobalColor)(mapId+3 % 12 + 7 ); - } - if(cloudWithNormals.get()) - { - if(!_cloudViewer->addCloud(scanName, cloudWithNormals, pose, color)) + if(scan.channels() == 2 || scan.channels() == 5) { - 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.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); + 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) + { + if(scan.channels() == 5) { - UERROR("Adding cloud %d to viewer failed!", nodeId); + scan = util3d::laserScan2dFromPointCloud(*cloudWithNormals); } 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(*cloudWithNormals); } } } + else if(cloudRGB.get()) + { + added = _cloudViewer->addCloud(scanName, cloudRGB, pose, color); + if(added && nodeId > 0) + { + scan = util3d::laserScanFromPointCloud(*cloudRGB); + } + } + else + { + UASSERT(cloud.get()); + added = _cloudViewer->addCloud(scanName, cloud, pose, color); + if(added && nodeId > 0) + { + if(scan.channels() == 2) + { + scan = util3d::laserScan2dFromPointCloud(*cloud); + } + else + { + 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)); + } } } @@ -3269,35 +3314,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()); } } } @@ -3308,43 +3353,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() @@ -4496,7 +4556,8 @@ void MainWindow::startDetection() _preferencesDialog->getSourceScanFromDepthDecimation(), _preferencesDialog->getSourceScanFromDepthMaxDepth(), _preferencesDialog->getSourceScanVoxelSize(), - _preferencesDialog->getSourceScanNormalsK()); + _preferencesDialog->getSourceScanNormalsK(), + _preferencesDialog->getSourceScanNormalsRadius()); if(_preferencesDialog->isDepthFilteringAvailable()) { if(_preferencesDialog->isBilateralFiltering()) @@ -7046,6 +7107,7 @@ void MainWindow::changeState(MainWindow::State newState) if(_camera) { _camera->start(); + ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads()); } break; @@ -7077,6 +7139,7 @@ void MainWindow::changeState(MainWindow::State newState) if(_camera) { _camera->start(); + ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads()); } } else if(_state == kDetecting) 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 cce1694c..a16c0a60 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); @@ -410,9 +417,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 +594,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))); @@ -649,7 +659,9 @@ 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()); // Database @@ -857,7 +869,9 @@ 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->loopClosure_icpPointToPlaneNormalsMinComplexity->setObjectName(Parameters::kIcpPointToPlaneMinComplexity().c_str()); _ui->groupBox_libpointmatcher->setObjectName(Parameters::kIcpPM().c_str()); _ui->lineEdit_IcpPMConfigPath->setObjectName(Parameters::kIcpPMConfig().c_str()); @@ -902,7 +916,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()); @@ -920,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()); @@ -1374,10 +1388,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 +1558,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); @@ -1620,7 +1637,6 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) if(groupBox->objectName() == _ui->groupBox_odometry1->objectName()) { _ui->odom_registration->setCurrentIndex(3); - updateOdometryVisibility(); } } } @@ -1769,9 +1785,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 +1951,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 +2174,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 +2342,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"); @@ -2421,6 +2443,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"), @@ -2428,8 +2451,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)) { @@ -2448,6 +2476,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"), @@ -2455,7 +2484,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 " @@ -3474,7 +3505,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) { @@ -3936,18 +3969,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; @@ -4286,6 +4307,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 +4323,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 +4665,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 +4776,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 +4822,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 +4928,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 +5174,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 +5223,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..576f4aad 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - 0 + -629 678 2739 @@ -95,7 +95,7 @@ QFrame::Raised - 21 + 18 @@ -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,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + -16 + + + 16 + + + + + + + -16 + + + 16 + + + @@ -6053,32 +6136,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 999 - - - 1 - - - 2 - - - - - - - Bad signatures are ignored. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - @@ -6089,16 +6146,16 @@ 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. + + + + 999 - - true + + 1 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 2 @@ -6122,10 +6179,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. + Bad signatures are ignored. true @@ -6145,26 +6202,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - 50 - 0 - + + + + Keep raw sensor data. Only useful to save loop closure computation time when features re-extraction is enabled. Disable to save RAM memory. - - 1 + + true - - 9999 + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + - Raw descriptors kept in memory. + Create map labels. The first node of a map will be labelled as "map#" where # is the map ID. true @@ -6184,6 +6238,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + Raw descriptors kept in memory. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + @@ -6233,6 +6300,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + + false + + + @@ -6256,16 +6333,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - -16 - - - 16 - - - @@ -6279,6 +6346,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + + false + + + @@ -6292,16 +6369,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - false - - - @@ -6328,21 +6395,8 @@ 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 - - - - - + + @@ -6351,45 +6405,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - -16 - - - 16 - - - - - - - - 50 - 0 - - - - 0 - - - 99 - - - 0 - - - - - + + - If > 0 and laser scans are 3D without normals, normals will be computed with K search neighbors when creating a signature. + - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + false @@ -6406,16 +6428,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - false - - - @@ -6429,18 +6441,139 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - false - - - + + + + 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 + + + + + + @@ -9947,7 +10080,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - 5 + 0 @@ -10043,6 +10176,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + 999999999 + + + @@ -10069,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. @@ -10089,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). @@ -10102,7 +10242,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + QComboBox::AdjustToContents @@ -10124,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 @@ -13471,6 +13633,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 + + + @@ -13572,35 +13789,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 - - - @@ -13697,29 +13885,10 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - - - - 3 - - - 0.001000000000000 - - - 9999.989999999999782 - - - 0.010000000000000 - - - 0.100000000000000 - - - - Point to plane ICP. Only for ICP 3D. + Point to plane ICP. true @@ -13764,6 +13933,45 @@ 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 + + + + + + + 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/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..dbe069c9 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)); } @@ -387,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) @@ -400,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(); @@ -418,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/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(); 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(" *");