i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-08 04:20:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'devel' of https://github.com/introlab/rtabmap

Browse Source
This commit is contained in:
matlabbe
2017-09-19 14:40:19 -04:00
parent 68fb5d7252 fd18c0b2e9
commit b737df9c40
61 changed files with 2653 additions and 1063 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1 -1
View File
@@ -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})
cmake_modules/FindORB_SLAM2.cmake
+7 -5
View File
@@ -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
corelib/include/rtabmap/core/CameraRGB.h
+4 -1
View File
@@ -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
corelib/include/rtabmap/core/CameraThread.h
+4 -1
View File
@@ -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;
corelib/include/rtabmap/core/Memory.h
+2
View File
@@ -282,7 +282,9 @@ private:
int _imagePostDecimation;
bool _compressionParallelized;
float _laserScanDownsampleStepSize;
float _laserScanVoxelSize;
int _laserScanNormalK;
int _laserScanNormalRadius;
bool _reextractLoopClosureFeatures;
float _rehearsalMaxDistance;
float _rehearsalMaxAngle;
corelib/include/rtabmap/core/Odometry.h
+2
View File
@@ -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<ParticleFilter *> particleFilters_;
cv::KalmanFilter kalmanFilter_;
corelib/include/rtabmap/core/OdometryDVO.h
+2
View File
@@ -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_;
corelib/include/rtabmap/core/OdometryEvent.h
+11 -11
View File
@@ -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<double>(0,0)) && _info.covariance.at<double>(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.covariance.at<double>(1,1)) && _info.covariance.at<double>(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.covariance.at<double>(2,2)) && _info.covariance.at<double>(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.covariance.at<double>(3,3)) && _info.covariance.at<double>(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.covariance.at<double>(4,4)) && _info.covariance.at<double>(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.covariance.at<double>(5,5)) && _info.covariance.at<double>(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<double>(0,0)) && _info.reg.covariance.at<double>(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(1,1)) && _info.reg.covariance.at<double>(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(2,2)) && _info.reg.covariance.at<double>(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(3,3)) && _info.reg.covariance.at<double>(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(4,4)) && _info.reg.covariance.at<double>(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)");
UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(5,5)) && _info.reg.covariance.at<double>(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<float> velocity() const {
if(_info.interval>0.0)
{
corelib/include/rtabmap/core/OdometryF2M.h
+1
View File
@@ -64,6 +64,7 @@ private:
float scanKeyFrameThr_;
int scanMaximumMapSize_;
float scanSubtractRadius_;
float scanSubtractAngle_;
int bundleAdjustment_;
int bundleMaxFrames_;
corelib/include/rtabmap/core/OdometryFovis.h
+3 -1
View File
@@ -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_;
};
corelib/include/rtabmap/core/OdometryInfo.h
+4 -14
View File
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <map>
#include "rtabmap/core/Transform.h"
#include "rtabmap/core/RegistrationInfo.h"
#include <opencv2/features2d/features2d.hpp>
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<int, cv::KeyPoint> words;
std::vector<int> wordMatches;
std::vector<int> wordInliers;
std::map<int, cv::Point3f> localMap;
cv::Mat localScanMap;
corelib/include/rtabmap/core/OdometryORBSLAM2.h
+2
View File
@@ -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_;
};
corelib/include/rtabmap/core/OdometryViso2.h
+2
View File
@@ -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_;
corelib/include/rtabmap/core/Parameters.h
+8 -3
View File
@@ -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.");
corelib/include/rtabmap/core/RegistrationIcp.h
+3 -1
View File
@@ -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;
corelib/include/rtabmap/core/RegistrationInfo.h
+18 -1
View File
@@ -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;
};
}
corelib/include/rtabmap/core/Statistics.h
+5 -1
View File
@@ -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);
corelib/include/rtabmap/core/util3d.h
+7 -2
View File
@@ -192,15 +192,20 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImages(
// return CV_32FC3 (x,y,z)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
// return CV_32FC6 (x,y,z,normal_z,normal_y,normalz)
// return CV_32FC6 (x,y,z,normal_x,normal_y,normal_z)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform());
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
// return CV_32FC4 (x,y,z,rgb)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform());
// return CV_32FC7 (x,y,z,rgb,normal_z,normal_y,normalz)
// return CV_32FC7 (x,y,z,rgb,normal_x,normal_y,normal_z)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
// return CV_32FC7 (x,y,z,rgb,normal_x,normal_y,normal_z)
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform = Transform());
// return CV_32FC2 (x,y)
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
// return CV_32FC5 (x,y,normal_x, normal_y, normal_z)
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform());
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
// For laserScan of type CV_32FC2, z is set to null.
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform = Transform());
// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null.
corelib/include/rtabmap/core/util3d_filtering.h
+6
View File
@@ -141,6 +141,12 @@ pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP passThrough(
float min,
float max,
bool negative = false);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP passThrough(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const std::string & axis,
float min,
float max,
bool negative = false);
pcl::IndicesPtr RTABMAP_EXP cropBox(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
corelib/include/rtabmap/core/util3d_surface.h
+45 -7
View File
@@ -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<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int normalKSearch = 20,
int searchK = 20,
float searchRadius = 0.0f,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
int normalKSearch = 20,
int searchK = 20,
float searchRadius = 0.0f,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::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<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::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<pcl::Normal>::Ptr computeFastOrganizedNormals(
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals2D(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int searchK = 5,
float searchRadius = 0.0f,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeFastOrganizedNormals2D(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int searchK = 5,
float searchRadius = 0.0f,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeFastOrganizedNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float maxDepthChangeFactor = 0.02f,
float normalSmoothingSize = 10.0f,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeFastOrganizedNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::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<pcl::Normal> & normals,
bool is2d = false,
cv::Mat * pcaEigenVectors = 0,
cv::Mat * pcaEigenValues = 0);
float RTABMAP_EXP computeNormalsComplexity(
const pcl::PointCloud<pcl::PointNormal> & cloud,
bool is2d = false,
cv::Mat * pcaEigenVectors = 0,
cv::Mat * pcaEigenValues = 0);
float RTABMAP_EXP computeNormalsComplexity(
const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
bool is2d = false,
cv::Mat * pcaEigenVectors = 0,
cv::Mat * pcaEigenValues = 0);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float searchRadius = 0.0f,
corelib/src/CMakeLists.txt
+2 -2
View File
@@ -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)
corelib/src/CameraRGB.cpp
+4 -2
View File
@@ -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<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK);
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, _scanNormalsRadius);
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*cloud, *normals, *cloudNormals);
scan = util3d::laserScanFromPointCloud(*cloudNormals, _scanLocalTransform.inverse());
corelib/src/CameraThread.cpp
+6 -5
View File
@@ -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<int>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(
pcl::PointCloud<pcl::PointXYZRGB>::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<pcl::PointXYZ>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
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<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, viewPoint);
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, _scanNormalsRadius, viewPoint);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::concatenateFields(*cloud, *normals, *cloudNormals);
scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());
}
corelib/src/Graph.cpp
+1 -1
View File
@@ -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]];
}
}
corelib/src/Memory.cpp
+92 -20
View File
@@ -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<int, Transform>::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<Signature*> depthToLoad;
@@ -2436,6 +2452,8 @@ Transform Memory::computeIcpTransformMulti(
std::string msg;
int maxPoints = fromScan.cols;
pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
bool is2D = true;
for(std::map<int, Transform>::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<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(
scan,
s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second);
if(scan.cols > maxPoints)
if(!scan.empty())
{
maxPoints = scan.cols;
if(scan.channels() != 2 && scan.channels() != 5)
{
is2D = false;
}
if(scan.channels() >= 5)
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormal = util3d::laserScanToPointCloudNormal(
scan,
s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second);
*assembledToNormalClouds += *cloudNormal;
}
else
{
pcl::PointCloud<pcl::PointXYZ>::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<int> 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<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan);
float x,y,z;
data.laserScanInfo().localTransform().getTranslation(x,y,z);
pcl::PointCloud<pcl::Normal>::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<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(laserScan);
cloud = util3d::voxelize(cloud, _laserScanVoxelSize);
laserScan = util3d::laserScanFromPointCloud(*cloud);
}
else
{
pcl::PointCloud<pcl::PointXYZ>::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);
corelib/src/OccupancyGrid.cpp
+1 -1
View File
@@ -207,7 +207,7 @@ void OccupancyGrid::createLocalMap(
UDEBUG("scan channels=%d, occupancyFromCloud_=%d normalsSegmentation_=%d grid3D_=%d",
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
corelib/src/Odometry.cpp
+4 -1
View File
@@ -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
}
corelib/src/OdometryDVO.cpp
+3 -2
View File
@@ -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());
corelib/src/OdometryF2F.cpp
+16 -8
View File
@@ -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(),
&regInfo);
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",
corelib/src/OdometryF2M.cpp
+51 -26
View File
@@ -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(),
&regInfo);
if(transform.isNull() && !guess.isNull() && regPipeline_->isImageRequired())
{
tmpMap = *map_;
@@ -591,7 +597,7 @@ Transform OdometryF2M::computeTransform(
if(lastFrame_->sensorData().laserScanRaw().cols)
{
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan);
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan, tmpMap.sensorData().laserScanInfo().localTransform());
pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());
pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>);
@@ -604,7 +610,7 @@ Transform OdometryF2M::computeTransform(
mapCloudNormals,
pcl::IndicesPtr(new std::vector<int>),
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<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());
scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
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();
}
}
corelib/src/OdometryFovis.cpp
+7 -5
View File
@@ -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())
{
corelib/src/OdometryMono.cpp
+6 -6
View File
@@ -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; i<inliersV.size(); ++i)
{
info->wordInliers[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_;
corelib/src/OdometryORBSLAM2.cpp
+13 -10
View File
@@ -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<cv::KeyPoint> & 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<ORB_SLAM2::MapPoint*> mapPoints = orbslam2_->mpMap->GetAllMapPoints();
for (unsigned int i = 0; i < mapPoints.size(); ++i)
corelib/src/OdometryViso2.cpp
+7 -4
View File
@@ -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())
corelib/src/OptimizerG2O.cpp
+125 -15
View File
@@ -37,19 +37,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/util3d_motion_estimation.h>
#include <rtabmap/core/util3d.h>
#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<SlamBlockSolver::PoseMatrixType> SlamLinearEigenSolver;
#ifdef RTABMAP_G2O
typedef g2o::LinearSolverPCG<SlamBlockSolver::PoseMatrixType> SlamLinearPCGSolver;
#ifdef G2O_HAVE_CSPARSE
typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearCSparseSolver;
@@ -79,14 +88,15 @@ typedef g2o::LinearSolverCholmod<SlamBlockSolver::PoseMatrixType> 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<int, Transform> OptimizerG2O::optimize(
@@ -612,8 +631,12 @@ std::map<int, Transform> 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<int, Transform> OptimizerG2O::optimizeBA(
std::set<int> * outliers)
{
std::map<int, Transform> optimizedPoses;
#ifdef RTABMAP_G2O
#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
UDEBUG("Optimizing graph...");
optimizedPoses.clear();
@@ -638,6 +661,9 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
g2o::BlockSolver_6_3::LinearSolverType * linearSolver = 0;
#ifdef RTABMAP_ORB_SLAM2
linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
#else
if(solver_ == 3)
{
//eigen
@@ -663,14 +689,17 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
//pcg
linearSolver = new g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType>();
}
#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<int, Transform> 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<int, Transform> 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<int, Transform> OptimizerG2O::optimizeBA(
++iter;
}
#ifndef RTABMAP_ORB_SLAM2
UDEBUG("fill edges to g2o...");
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
@@ -759,6 +798,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
}
}
}
#endif
UDEBUG("fill 3D points to g2o...");
const int stepVertexId = poses.rbegin()->first+1;
@@ -775,7 +815,7 @@ std::map<int, Transform> 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<int, cv::Point3f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
@@ -786,25 +826,62 @@ std::map<int, Transform> 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<g2o::VertexSE3Expmap*>(optimizer.vertex(camId));
std::map<int, CameraModel>::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<g2o::VertexCam*>(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<int, Transform> 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<int, Transform> 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<g2o::EdgeStereoSE3ProjectXYZ*>(*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<g2o::EdgeProjectP2SC*>(*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<int, Transform> OptimizerG2O::optimizeBA(
// update poses
for(std::map<int, Transform>::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());
corelib/src/Parameters.cpp
+5 -1
View File
@@ -225,6 +225,10 @@ const std::map<std::string, std::pair<bool, std::string> > & 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<std::string, std::pair<bool, std::string> > & 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())));
corelib/src/RegistrationIcp.cpp
+425 -212
View File
@@ -36,16 +36,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UTimer.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/vtk_io.h>
#include <pcl/conversions.h>
#ifdef RTABMAP_POINTMATCHER
#include <fstream>
#include "pointmatcher/PointMatcher.h"
typedef PointMatcher<float> PM;
typedef PM::DataPoints DP;
DP pclToDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud)
DP pclToDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud, bool is2D)
{
UDEBUG("");
typedef DP::Label Label;
@@ -65,8 +64,11 @@ DP pclToDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud)
isFeature.push_back(true);
featLabels.push_back(Label("y", 1));
isFeature.push_back(true);
featLabels.push_back(Label("z", 1));
isFeature.push_back(true);
if(!is2D)
{
featLabels.push_back(Label("z", 1));
isFeature.push_back(true);
}
featLabels.push_back(Label("pad", 1));
// create cloud
@@ -74,20 +76,21 @@ DP pclToDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud)
cloud.getFeatureViewByName("pad").setConstant(1);
// fill cloud
View viewX(cloud.getFeatureViewByName("x"));
View viewY(cloud.getFeatureViewByName("y"));
View viewZ(cloud.getFeatureViewByName("z"));
View view(cloud.getFeatureViewByName("x"));
for(unsigned int i=0; i<pclCloud->size(); ++i)
{
viewX(0, i) = pclCloud->at(i).x;
viewY(0, i) = pclCloud->at(i).y;
viewZ(0, i) = pclCloud->at(i).z;
view(0, i) = pclCloud->at(i).x;
view(1, i) = pclCloud->at(i).y;
if(!is2D)
{
view(2, i) = pclCloud->at(i).z;
}
}
return cloud;
}
DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud)
DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud, bool is2D)
{
UDEBUG("");
typedef DP::Label Label;
@@ -107,8 +110,11 @@ DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud)
isFeature.push_back(true);
featLabels.push_back(Label("y", 1));
isFeature.push_back(true);
featLabels.push_back(Label("z", 1));
isFeature.push_back(true);
if(!is2D)
{
featLabels.push_back(Label("z", 1));
isFeature.push_back(true);
}
descLabels.push_back(Label("normals", 3));
isFeature.push_back(false);
@@ -122,17 +128,18 @@ DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud)
cloud.getFeatureViewByName("pad").setConstant(1);
// fill cloud
View viewX(cloud.getFeatureViewByName("x"));
View viewY(cloud.getFeatureViewByName("y"));
View viewZ(cloud.getFeatureViewByName("z"));
View view(cloud.getFeatureViewByName("x"));
View viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
View viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
View viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
for(unsigned int i=0; i<pclCloud->size(); ++i)
{
viewX(0, i) = pclCloud->at(i).x;
viewY(0, i) = pclCloud->at(i).y;
viewZ(0, i) = pclCloud->at(i).z;
view(0, i) = pclCloud->at(i).x;
view(1, i) = pclCloud->at(i).y;
if(!is2D)
{
view(2, i) = pclCloud->at(i).z;
}
viewNormalX(0, i) = pclCloud->at(i).normal_x;
viewNormalY(0, i) = pclCloud->at(i).normal_y;
viewNormalZ(0, i) = pclCloud->at(i).normal_z;
@@ -153,14 +160,13 @@ void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointXYZ> & pclCloud)
pclCloud.is_dense = true;
// fill cloud
ConstView viewX(cloud.getFeatureViewByName("x"));
ConstView viewY(cloud.getFeatureViewByName("y"));
ConstView viewZ(cloud.getFeatureViewByName("z"));
ConstView view(cloud.getFeatureViewByName("x"));
bool is3D = cloud.featureExists("z");
for(unsigned int i=0; i<pclCloud.size(); ++i)
{
pclCloud.at(i).x = viewX(0, i);
pclCloud.at(i).y = viewY(0, i);
pclCloud.at(i).z = viewZ(0, i);
pclCloud.at(i).x = view(0, i);
pclCloud.at(i).y = view(1, i);
pclCloud.at(i).z = is3D?view(2, i):0;
}
}
@@ -176,17 +182,16 @@ void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointNormal> & pclCloud)
pclCloud.is_dense = true;
// fill cloud
ConstView viewX(cloud.getFeatureViewByName("x"));
ConstView viewY(cloud.getFeatureViewByName("y"));
ConstView viewZ(cloud.getFeatureViewByName("z"));
ConstView view(cloud.getFeatureViewByName("x"));
bool is3D = cloud.featureExists("z");
ConstView viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
ConstView viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
ConstView viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
for(unsigned int i=0; i<pclCloud.size(); ++i)
{
pclCloud.at(i).x = viewX(0, i);
pclCloud.at(i).y = viewY(0, i);
pclCloud.at(i).z = viewZ(0, i);
pclCloud.at(i).x = view(0, i);
pclCloud.at(i).y = view(1, i);
pclCloud.at(i).z = is3D?view(2, i):0;
pclCloud.at(i).normal_x = viewNormalX(0, i);
pclCloud.at(i).normal_y = viewNormalY(0, i);
pclCloud.at(i).normal_z = viewNormalZ(0, i);
@@ -225,7 +230,9 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration
_epsilon(Parameters::defaultIcpEpsilon()),
_correspondenceRatio(Parameters::defaultIcpCorrespondenceRatio()),
_pointToPlane(Parameters::defaultIcpPointToPlane()),
_pointToPlaneNormalNeighbors(Parameters::defaultIcpPointToPlaneNormalNeighbors()),
_pointToPlaneK(Parameters::defaultIcpPointToPlaneK()),
_pointToPlaneRadius(Parameters::defaultIcpPointToPlaneRadius()),
_pointToPlaneMinComplexity(Parameters::defaultIcpPointToPlaneMinComplexity()),
_libpointmatcher(Parameters::defaultIcpPM()),
_libpointmatcherConfig(Parameters::defaultIcpPMConfig()),
_libpointmatcherOutlierRatio(Parameters::defaultIcpPMOutlierRatio()),
@@ -257,7 +264,10 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kIcpEpsilon(), _epsilon);
Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), _correspondenceRatio);
Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneNormalNeighbors(), _pointToPlaneNormalNeighbors);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), _pointToPlaneK);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneRadius(), _pointToPlaneRadius);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneMinComplexity(), _pointToPlaneMinComplexity);
UASSERT(_pointToPlaneMinComplexity >= 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<float>::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<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform);
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toLocalTransform);
UDEBUG("Conversion time = %f s", timer.ticks());
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
#ifdef RTABMAP_POINTMATCHER
if(_libpointmatcher)
cv::Mat complexityVectorsFrom, complexityVectorsTo;
double fromComplexity = util3d::computeNormalsComplexity(fromScan, &complexityVectorsFrom);
double toComplexity = util3d::computeNormalsComplexity(toScan, &complexityVectorsTo);
float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
info.icpStructuralComplexity = complexity;
if(complexity < _pointToPlaneMinComplexity)
{
// 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<double>(T.template cast<double>(), 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 = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): %f < %f (%s). PointToPoint is done instead.", complexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str());
}
else
#endif
{
icpT = util3d::icpPointToPlane(
fromCloudNormals,
toCloudNormals,
_maxCorrespondenceDistance,
_maxIterations,
hasConverged,
*fromCloudNormalsRegistered,
_epsilon,
this->force3DoF());
}
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform);
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toLocalTransform);
if(!icpT.isNull() && hasConverged)
{
util3d::computeVarianceAndCorrespondences(
fromCloudNormalsRegistered,
toCloudNormals,
_maxCorrespondenceDistance,
variance,
correspondences);
}
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, fromLocalTransform);
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess * toLocalTransform);
UDEBUG("Conversion time = %f s", timer.ticks());
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
pcl::PointCloud<pcl::PointXYZ>::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<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
if(_pointToPlane) // ICP Point To Plane, only in 3D
{
pcl::PointCloud<pcl::Normal>::Ptr normals;
normals = util3d::computeNormals(fromCloudFiltered, _pointToPlaneNormalNeighbors);
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*fromCloudFiltered, *normals, *fromCloudNormals);
normals = util3d::computeNormals(toCloudFiltered, _pointToPlaneNormalNeighbors);
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*toCloudFiltered, *normals, *toCloudNormals);
std::vector<int> 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<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
#ifdef RTABMAP_POINTMATCHER
if(_libpointmatcher)
{
// Load point clouds
DP data = pclToDP(fromCloudNormals);
DP ref = pclToDP(toCloudNormals);
// 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<double>(T.template cast<double>(), 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<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
#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<double>(T.template cast<double>(), 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<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, fromLocalTransform);
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess * toLocalTransform);
UDEBUG("Conversion time = %f s", timer.ticks());
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
pcl::PointCloud<pcl::PointXYZ>::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<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
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<pcl::Normal>::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<pcl::Normal>::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<toComplexity?fromComplexity:toComplexity;
info.icpStructuralComplexity = complexity;
if(complexity < _pointToPlaneMinComplexity)
{
tooLowComplexityForPlaneToPlane = true;
complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): %f < %f (%s). PointToPoint is done instead.", complexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str());
}
else
{
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*fromCloudFiltered, *normalsFrom, *fromCloudNormals);
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*toCloudFiltered, *normalsTo, *toCloudNormals);
std::vector<int> 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<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
#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<double>(T.template cast<double>(), 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<double>(T.template cast<double>(), 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<float>(0,0), complexityVectors.at<float>(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<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2));
Eigen::Vector3f n2(complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(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,
corelib/src/Rtabmap.cpp
+12 -11
View File
@@ -1116,14 +1116,17 @@ bool Rtabmap::process(
else
{
UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str());
if(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0)
if(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(0,0) != 1.0 && info.covariance.at<double>(5,5) > 0.0 && info.covariance.at<double>(5,5) != 1.0)
{
_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv()));
}
}
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<int> & 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<int, std::map<int, Transform> > Rtabmap::getPaths(std::map<int, Transfo
if(valid)
{
UDEBUG("%d <- %d", nearestId, jter->first);
//UDEBUG("%d <- %d", nearestId, jter->first);
path.insert(*jter);
poses.erase(jter);
}
corelib/src/RtabmapThread.cpp
+10 -10
View File
@@ -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<double>(0,0)>=9999))
odomEvent.info().reg.covariance.at<double>(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<double>(0,0));
UWARN("Odometry is reset (high variance (%f >=9999 detected). Increment map id!", odomEvent.info().reg.covariance.at<double>(0,0));
}
pushNewState(kStateTriggeringMap);
covariance_ = cv::Mat();
}
if(uIsFinite(odomEvent.info().covariance.at<double>(0,0)) &&
odomEvent.info().covariance.at<double>(0,0) != 1.0 &&
odomEvent.info().covariance.at<double>(0,0)>0.0)
if(uIsFinite(odomEvent.info().reg.covariance.at<double>(0,0)) &&
odomEvent.info().reg.covariance.at<double>(0,0) != 1.0 &&
odomEvent.info().reg.covariance.at<double>(0,0)>0.0)
{
// Use largest covariance error (to be independent of the odometry frame rate)
if(covariance_.empty() || odomEvent.info().covariance.at<double>(0,0) > covariance_.at<double>(0,0))
if(covariance_.empty() || odomEvent.info().reg.covariance.at<double>(0,0) > covariance_.at<double>(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
corelib/src/SensorData.cpp
+5 -5
View File
@@ -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();
corelib/src/util3d.cpp
+151 -49
View File
@@ -1363,6 +1363,46 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
return laserScan;
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & 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<cloud.size(); ++i)
{
float * ptr = laserScan.ptr<float>(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<pcl::PointXYZRGBNormal> & cloud, const Transform & transform)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
@@ -1419,12 +1459,79 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud,
return laserScan;
}
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5));
bool nullTransform = transform.isNull();
for(unsigned int i=0; i<cloud.size(); ++i)
{
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
{
pcl::PointNormal pt = util3d::transformPoint(cloud.at(i), transform);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.normal_x;
ptr[3] = pt.normal_y;
ptr[4] = pt.normal_z;
}
else
{
const pcl::PointNormal & pt = cloud.at(i);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.normal_x;
ptr[3] = pt.normal_y;
ptr[4] = pt.normal_z;
}
}
return laserScan;
}
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform)
{
UASSERT(cloud.size() == normals.size());
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5));
bool nullTransform = transform.isNull() || transform.isIdentity();
for(unsigned int i=0; i<cloud.size(); ++i)
{
float * ptr = laserScan.ptr<float>(0, i);
if(!nullTransform)
{
pcl::PointNormal pt;
pt.x = cloud.at(i).x;
pt.y = cloud.at(i).y;
pt.z = cloud.at(i).z;
pt.normal_x = normals.at(i).normal_x;
pt.normal_y = normals.at(i).normal_y;
pt.normal_z = normals.at(i).normal_z;
pt = util3d::transformPoint(pt, transform);
ptr[0] = pt.x;
ptr[1] = pt.y;
ptr[2] = pt.normal_x;
ptr[3] = pt.normal_y;
ptr[4] = pt.normal_z;
}
else
{
ptr[0] = cloud.at(i).x;
ptr[1] = cloud.at(i).y;
ptr[2] = normals.at(i).normal_x;
ptr[3] = normals.at(i).normal_y;
ptr[4] = normals.at(i).normal_z;
}
}
return laserScan;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
output->resize(laserScan.cols);
output->is_dense = true;
bool nullTransform = transform.isNull();
Eigen::Affine3f transform3f = transform.toEigen3f();
for(int i=0; i<laserScan.cols; ++i)
@@ -1440,10 +1547,11 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserS
pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat & laserScan, const Transform & transform)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
output->resize(laserScan.cols);
output->is_dense = true;
bool nullTransform = transform.isNull();
for(int i=0; i<laserScan.cols; ++i)
{
@@ -1458,10 +1566,11 @@ pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat
pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const cv::Mat & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
output->resize(laserScan.cols);
output->is_dense = true;
bool nullTransform = transform.isNull() || transform.isIdentity();
Eigen::Affine3f transform3f = transform.toEigen3f();
for(int i=0; i<laserScan.cols; ++i)
@@ -1477,10 +1586,11 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const cv::Mat &
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(const cv::Mat & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
output->resize(laserScan.cols);
output->is_dense = true;
bool nullTransform = transform.isNull() || transform.isIdentity();
for(int i=0; i<laserScan.cols; ++i)
{
@@ -1496,12 +1606,12 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(cons
pcl::PointXYZ laserScanToPoint(const cv::Mat & laserScan, int index)
{
UASSERT(!laserScan.empty() && index < laserScan.cols);
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
pcl::PointXYZ output;
const float * ptr = laserScan.ptr<float>(0, index);
output.x = ptr[0];
output.y = ptr[1];
if(laserScan.channels() >= 3)
if(laserScan.channels() >= 3 && laserScan.channels() != 5)
{
output.z = ptr[2];
}
@@ -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<float>(0, index);
output.x = ptr[0];
output.y = ptr[1];
if(laserScan.channels() >= 3)
if(laserScan.channels() >= 3 && laserScan.channels() != 5)
{
output.z = ptr[2];
}
if(laserScan.channels() == 6)
if(laserScan.channels() == 5)
{
output.normal_x = ptr[2];
output.normal_y = ptr[3];
output.normal_z = ptr[4];
}
else if(laserScan.channels() == 6)
{
output.normal_x = ptr[3];
output.normal_y = ptr[4];
@@ -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<float>(0, index);
output.x = ptr[0];
output.y = ptr[1];
if(laserScan.channels() >= 3)
if(laserScan.channels() >= 3 && laserScan.channels() != 5)
{
output.z = ptr[2];
}
@@ -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<float>(0, index);
output.x = ptr[0];
output.y = ptr[1];
if(laserScan.channels() >= 3)
if(laserScan.channels() >= 3 && laserScan.channels() != 5)
{
output.z = ptr[2];
}
if(laserScan.channels() == 6)
if(laserScan.channels() == 5)
{
output.normal_x = ptr[2];
output.normal_y = ptr[3];
output.normal_z = ptr[4];
}
else if(laserScan.channels() == 6)
{
output.normal_x = ptr[3];
output.normal_y = ptr[4];
@@ -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<float>(0, 0);
min.x = max.x = ptr[0];
min.y = max.y = ptr[1];
min.z = max.z = laserScan.channels() >= 3?ptr[2]:0.0f;
bool is3d = laserScan.channels() >= 3 && laserScan.channels() != 5;
min.z = max.z = is3d?ptr[2]:0.0f;
for(int i=1; i<laserScan.cols; ++i)
{
ptr = laserScan.ptr<float>(0, i);
@@ -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<double>(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<cv::Vec2f>();
const cv::Vec3f* vec3Ptr = laserScan.ptr<cv::Vec3f>();
const cv::Vec4f* vec4Ptr = laserScan.ptr<cv::Vec4f>();
const cv::Vec6f* vec6Ptr = laserScan.ptr<cv::Vec6f>();
const float* vec7Ptr = laserScan.ptr<float>();
int count = 0;
for(int i=0; i<laserScan.cols; ++i)
{
const float* ptr = laserScan.ptr<float>(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);
corelib/src/util3d_filtering.cpp
+20
View File
@@ -369,6 +369,26 @@ pcl::PointCloud<pcl::PointNormal>::Ptr passThrough(
return output;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr passThrough(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::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<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::PassThrough<pcl::PointXYZRGBNormal> 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<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
corelib/src/util3d_registration.cpp
+4 -4
View File
@@ -244,8 +244,8 @@ void computeVarianceAndCorrespondences(
correspondencesOut = 0;
pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
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<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
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);
corelib/src/util3d_surface.cpp
+412 -8
View File
@@ -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<pcl::Normal>::Ptr normals;
if(laserScan.channels() < 4)
{
pcl::PointCloud<pcl::PointXYZ>::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<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(laserScan);
normals = util3d::computeNormals(cloud, searchK, searchRadius);
return util3d::laserScanFromPointCloud(*cloud, *normals);
}
}
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int normalKSearch,
int searchK,
float searchRadius,
const Eigen::Vector3f & viewPoint)
{
pcl::IndicesPtr indices(new std::vector<int>);
return computeNormals(cloud, indices, normalKSearch, viewPoint);
return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
int normalKSearch,
int searchK,
float searchRadius,
const Eigen::Vector3f & viewPoint)
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
@@ -2032,7 +2066,8 @@ pcl::PointCloud<pcl::Normal>::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<pcl::Normal>::Ptr computeNormals(
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
int normalKSearch,
int searchK,
float searchRadius,
const Eigen::Vector3f & viewPoint)
{
pcl::IndicesPtr indices(new std::vector<int>);
return computeNormals(cloud, indices, normalKSearch, viewPoint);
return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
int normalKSearch,
int searchK,
float searchRadius,
const Eigen::Vector3f & viewPoint)
{
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
@@ -2077,13 +2114,182 @@ pcl::PointCloud<pcl::Normal>::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<pcl::Normal>::Ptr computeNormals2D(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int searchK,
float searchRadius,
const Eigen::Vector3f & viewPoint)
{
UASSERT(searchK>0 || searchRadius>0.0f);
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud);
normals->resize(cloud->size());
float bad_point = std::numeric_limits<float>::quiet_NaN ();
// assuming that points are ordered
for(unsigned int i=0; i<cloud->size(); ++i)
{
const pcl::PointXYZ & pt = cloud->at(i);
std::vector<Eigen::Vector3f> neighborNormals;
Eigen::Vector3f direction;
direction[0] = viewPoint[0] - pt.x;
direction[1] = viewPoint[1] - pt.y;
direction[2] = viewPoint[2] - pt.z;
std::vector<int> k_indices;
std::vector<float> 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; j<k_indices.size(); ++j)
{
if(k_indices.at(j) != (int)i)
{
const pcl::PointXYZ & pt2 = cloud->at(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; j<neighborNormals.size(); ++j)
{
meanNormal+=neighborNormals[j];
}
meanNormal /= (float)neighborNormals.size();
meanNormal.normalize();
normals->at(i).normal_x = meanNormal[0];
normals->at(i).normal_y = meanNormal[1];
normals->at(i).normal_z = meanNormal[2];
}
}
return normals;
}
pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2D(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int searchK,
float searchRadius,
const Eigen::Vector3f & viewPoint)
{
UASSERT(searchK>0);
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
normals->resize(cloud->size());
searchRadius *= searchRadius; // squared distance
float bad_point = std::numeric_limits<float>::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<Eigen::Vector3f> 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; j<neighborNormals.size(); ++j)
{
meanNormal+=neighborNormals[j];
}
meanNormal /= (float)neighborNormals.size();
meanNormal.normalize();
normals->at(i).normal_x = meanNormal[0];
normals->at(i).normal_y = meanNormal[1];
normals->at(i).normal_z = meanNormal[2];
}
}
return normals;
}
pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float maxDepthChangeFactor,
@@ -2132,6 +2338,204 @@ pcl::PointCloud<pcl::Normal>::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<int>(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<float>(0, i);
if(scan.channels() == 5)
{
if(uIsFinite(ptrScan[2]) && uIsFinite(ptrScan[3]))
{
float * ptr = data_normals.ptr<float>(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<float>(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<float>(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<float>(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<pcl::PointNormal> & cloud,
bool is2d,
cv::Mat * pcaEigenVectors,
cv::Mat * pcaEigenValues)
{
//Construct a buffer used by the pca analysis
int sz = static_cast<int>(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<float>(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<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
}
return 0.0f;
}
float computeNormalsComplexity(
const pcl::PointCloud<pcl::Normal> & normals,
bool is2d,
cv::Mat * pcaEigenVectors,
cv::Mat * pcaEigenValues)
{
//Construct a buffer used by the pca analysis
int sz = static_cast<int>(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<float>(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<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
}
return 0.0f;
}
float computeNormalsComplexity(
const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
bool is2d,
cv::Mat * pcaEigenVectors,
cv::Mat * pcaEigenValues)
{
//Construct a buffer used by the pca analysis
int sz = static_cast<int>(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<float>(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<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
}
return 0.0f;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float searchRadius,
corelib/src/util3d_transforms.cpp
+66 -41
View File
@@ -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<laserScan.cols; ++i)
{
const float * ptr = laserScan.ptr<float>(0, i);
float * out = output.ptr<float>(0, i);
if(laserScan.type() == CV_32FC2)
{
pcl::PointXYZ pt(
laserScan.at<cv::Vec2f>(i)[0],
laserScan.at<cv::Vec2f>(i)[1], 0);
pt = util3d::transformPoint(pt, transform);
output.at<cv::Vec2f>(i)[0] = pt.x;
output.at<cv::Vec2f>(i)[1] = pt.y;
pcl::PointXYZ pt(ptr[0], ptr[1], 0);
pt = pcl::transformPoint(pt, transform3f);
out[0] = pt.x;
out[1] = pt.y;
}
else if(laserScan.type() == CV_32FC3)
else if(laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4))
{
pcl::PointXYZ pt(
laserScan.at<cv::Vec3f>(i)[0],
laserScan.at<cv::Vec3f>(i)[1],
laserScan.at<cv::Vec3f>(i)[2]);
pt = util3d::transformPoint(pt, transform);
output.at<cv::Vec3f>(i)[0] = pt.x;
output.at<cv::Vec3f>(i)[1] = pt.y;
output.at<cv::Vec3f>(i)[2] = pt.z;
const float * ptr = laserScan.ptr<float>(0, i);
pcl::PointXYZ pt(ptr[0], ptr[1], ptr[2]);
pt = pcl::transformPoint(pt, transform3f);
out[0] = pt.x;
out[1] = pt.y;
out[2] = pt.z;
}
else if(laserScan.type() == CV_32FC(4))
{
pcl::PointXYZ pt(
laserScan.at<cv::Vec4f>(i)[0],
laserScan.at<cv::Vec4f>(i)[1],
laserScan.at<cv::Vec4f>(i)[2]);
pt = util3d::transformPoint(pt, transform);
output.at<cv::Vec4f>(i)[0] = pt.x;
output.at<cv::Vec4f>(i)[1] = pt.y;
output.at<cv::Vec4f>(i)[2] = pt.z;
}
else
else if(laserScan.type() == CV_32FC(5))
{
pcl::PointNormal pt;
pt.x=laserScan.at<cv::Vec6f>(i)[0];
pt.y=laserScan.at<cv::Vec6f>(i)[1];
pt.z=laserScan.at<cv::Vec6f>(i)[2];
pt.normal_x=laserScan.at<cv::Vec6f>(i)[3];
pt.normal_y=laserScan.at<cv::Vec6f>(i)[4];
pt.normal_z=laserScan.at<cv::Vec6f>(i)[5];
pt.x=ptr[0];
pt.y=ptr[1];
pt.z=0;
pt.normal_x=ptr[2];
pt.normal_y=ptr[3];
pt.normal_z=ptr[4];
pt = util3d::transformPoint(pt, transform);
output.at<cv::Vec6f>(i)[0] = pt.x;
output.at<cv::Vec6f>(i)[1] = pt.y;
output.at<cv::Vec6f>(i)[2] = pt.z;
output.at<cv::Vec6f>(i)[3] = pt.normal_x;
output.at<cv::Vec6f>(i)[4] = pt.normal_y;
output.at<cv::Vec6f>(i)[5] = pt.normal_z;
out[0] = pt.x;
out[1] = pt.y;
out[2] = pt.normal_x;
out[3] = pt.normal_y;
out[4] = pt.normal_z;
}
else 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<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
ret.normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
ret.normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
ret.rgb = point.rgb;
return ret;
}
guilib/include/rtabmap/gui/CloudViewer.h
+11
View File
@@ -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<std::string, Transform> _addedClouds; // include cloud, scan, meshes
guilib/include/rtabmap/gui/PreferencesDialog.h
+3 -1
View File
@@ -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();
guilib/src/CloudViewer.cpp
+146 -22
View File
@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/gui/CloudViewer.h"
#include <rtabmap/core/Version.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UMath.h>
@@ -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<pcl::PointNormal>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointNormal>);
pcl::fromPCLPointCloud2 (*binaryCloud, *cloud_xyz);
std::string idNormals = id + "-normals";
if(_visualizer->addPointCloudNormals<pcl::PointNormal>(cloud_xyz, _normalsStep, _normalsScale, idNormals, 0))
{
_visualizer->updatePointCloudPose(idNormals, pose.toEigen3f());
_addedClouds.insert(idNormals, pose);
}
}
// add random color channel
pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::Ptr colorHandler;
colorHandler.reset (new pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2> (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<std::string> ids = _addedClouds.keys();
for(QList<std::string>::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();
guilib/src/DatabaseViewer.cpp
+37 -17
View File
@@ -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<pcl::PointXYZRGBNormal>::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<pcl::PointNormal>::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<pcl::PointXYZRGB>::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<pcl::PointXYZ>::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<pcl::PointNormal>::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<pcl::PointNormal>::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);
}
}
guilib/src/EditDepthArea.h
+6 -6
View File
@@ -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);
guilib/src/ExportCloudsDialog.cpp
+9 -5
View File
@@ -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<pcl::PointXYZ>::Ptr cloudWithoutNormals(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*assembledCloud, *cloudWithoutNormals);
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value());
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), _ui->doubleSpinBox_normalRadiusSearch->value());
UASSERT(assembledCloud->size() == normals->size());
for(unsigned int i=0; i<normals->size(); ++i)
@@ -2521,7 +2525,7 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
viewPoint[2] = data.stereoCameraModel().localTransform().z();
}
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), viewPoint);
pcl::PointCloud<pcl::Normal>::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<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::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<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::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<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), viewPoint);
pcl::PointCloud<pcl::Normal>::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<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::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);
}
guilib/src/GraphViewer.cpp
+1 -1
View File
@@ -673,7 +673,7 @@ void GraphViewer::updatePosterior(const std::map<int, float> & posterior)
std::map<int,float>::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
guilib/src/MainWindow.cpp
+242 -179
View File
@@ -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<int, cv::KeyPoint> kpInliers;
for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
{
kpInliers.insert(*odom.info().words.find(odom.info().wordInliers[i]));
kpInliers.insert(*odom.info().words.find(odom.info().reg.inliersIDs[i]));
}
_ui->imageView_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; i<odom.info().wordMatches.size(); ++i)
for(unsigned int i=0; i<odom.info().reg.matchesIDs.size(); ++i)
{
_ui->imageView_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; i<odom.info().wordInliers.size(); ++i)
for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
{
_ui->imageView_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<double>(0,0)), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().covariance.at<double>(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<double>(5,5)), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().covariance.at<double>(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<double>(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<double>(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<double>(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<double>(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<pcl::PointCloud<pcl::PointXYZRGB>::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<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), viewPoint);
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), _preferencesDialog->getNormalRadiusSearch(), viewPoint);
pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
}
else
@@ -2790,7 +2796,7 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
if(_preferencesDialog->getNormalKSearch() > 0 && cloudWithNormals->size() == 0)
{
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), viewPoint);
pcl::PointCloud<pcl::Normal>::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<pcl::PointXYZ>::Ptr cloud;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB;
pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals;
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudRGBWithNormals;
if(scan.channels() == 7 && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
{
pcl::PointCloud<pcl::PointNormal>::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<pcl::PointNormal>::Ptr cloudTransformed = util3d::transformPointCloud(cloud, pose);
pcl::PointCloud<pcl::PointXYZRGBNormal>::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<int>::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<pcl::PointNormal>::Ptr cloudTransformed = util3d::transformPointCloud(cloudWithNormals, pose);
cloudTransformed = rtabmap::util3d::passThrough(
cloudTransformed,
"z",
_preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
_preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::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<pcl::PointXYZ>::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<pcl::PointXYZRGB>::Ptr cloudTransformed = util3d::transformPointCloud(cloudRGB, pose);
cloudTransformed = rtabmap::util3d::passThrough(
cloudTransformed,
"z",
_preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
_preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::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<pcl::PointXYZ>::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<pcl::PointNormal>::Ptr cloudWithNormals;
if(scan.channels() > 2 && // don't compute normals for 2D scans
cloud->size() &&
_preferencesDialog->getScanNormalKSearch() > 0)
{
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _preferencesDialog->getScanNormalKSearch());
cloudWithNormals.reset(new pcl::PointCloud<pcl::PointNormal>);
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<pcl::Normal>::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::PointNormal>);
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::PointXYZRGBNormal>);
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<std::string, Transform> 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)
guilib/src/OdometryViewer.cpp
+8 -8
View File
@@ -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; i<odom.info().wordMatches.size(); ++i)
for(unsigned int i=0; i<odom.info().reg.matchesIDs.size(); ++i)
{
imageView_->setFeatureColor(odom.info().wordMatches[i], Qt::red); // outliers
imageView_->setFeatureColor(odom.info().reg.matchesIDs[i], Qt::red); // outliers
}
for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
{
imageView_->setFeatureColor(odom.info().wordInliers[i], Qt::green); // inliers
imageView_->setFeatureColor(odom.info().reg.inliersIDs[i], Qt::green); // inliers
}
}
}
guilib/src/PreferencesDialog.cpp
+56 -18
View File
@@ -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())
guilib/src/ui/exportCloudsDialog.ui
+62 -45
View File
@@ -23,9 +23,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-2684</y>
<width>773</width>
<height>4103</height>
<y>0</y>
<width>778</width>
<height>4058</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_13">
@@ -52,21 +52,21 @@
</property>
</widget>
</item>
<item row="8" column="0">
<item row="9" column="0">
<widget class="QCheckBox" name="checkBox_regenerate">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="9" column="0">
<item row="10" column="0">
<widget class="QCheckBox" name="checkBox_filtering">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="8" column="1">
<item row="9" column="1">
<widget class="QLabel" name="label_regenerate">
<property name="text">
<string>Regenerate clouds. This can be used to regenerate the point clouds at higher density than those used for online visualization.</string>
@@ -76,7 +76,14 @@
</property>
</widget>
</item>
<item row="7" column="1">
<item row="12" column="0">
<widget class="QCheckBox" name="checkBox_gainCompensation">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_voxel">
<property name="text">
<string>Voxel size. Set 0 to disable. When organized meshes are assembled, this is the radius in which the vertices of the polygons are merged.</string>
@@ -86,13 +93,6 @@
</property>
</widget>
</item>
<item row="11" column="0">
<widget class="QCheckBox" name="checkBox_gainCompensation">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_binaryFile_2">
<property name="text">
@@ -103,7 +103,7 @@
</property>
</widget>
</item>
<item row="11" column="1">
<item row="12" column="1">
<widget class="QLabel" name="label_gainCompensation">
<property name="text">
<string>Gain compensation. Normalize brightness of images.</string>
@@ -133,7 +133,7 @@
</property>
</widget>
</item>
<item row="12" column="1">
<item row="13" column="1">
<widget class="QLabel" name="label_binaryFile_12">
<property name="text">
<string>Meshing.</string>
@@ -143,7 +143,14 @@
</property>
</widget>
</item>
<item row="9" column="1">
<item row="11" column="0">
<widget class="QCheckBox" name="checkBox_smoothing">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="10" column="1">
<widget class="QLabel" name="label_binaryFile_9">
<property name="text">
<string>Cloud filtering. Remove sparse points that are far from surfaces.</string>
@@ -153,8 +160,8 @@
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QCheckBox" name="checkBox_smoothing">
<item row="13" column="0">
<widget class="QCheckBox" name="checkBox_meshing">
<property name="text">
<string/>
</property>
@@ -170,14 +177,7 @@
</property>
</widget>
</item>
<item row="12" column="0">
<widget class="QCheckBox" name="checkBox_meshing">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="10" column="1">
<item row="11" column="1">
<widget class="QLabel" name="label_binaryFile_10">
<property name="text">
<string>Cloud smoothing using Moving Least Squares algorithm (MLS).</string>
@@ -187,7 +187,7 @@
</property>
</widget>
</item>
<item row="7" column="0">
<item row="8" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize_assembled">
<property name="suffix">
<string> m</string>
@@ -206,6 +206,13 @@
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QCheckBox" name="checkBox_assemble">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_normal">
<property name="text">
@@ -216,23 +223,6 @@
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="checkBox_binary">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QCheckBox" name="checkBox_assemble">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QComboBox" name="comboBox_frame">
<item>
@@ -257,6 +247,16 @@
</item>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="checkBox_binary">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_binaryFile_11">
<property name="text">
@@ -277,6 +277,23 @@
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_normal_2">
<property name="text">
<string>Set the search radius for the normal estimation.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_normalRadiusSearch">
<property name="singleStep">
<double>0.010000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
<item>
guilib/src/ui/preferencesDialog.ui
+453 -245
View File
@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<y>-629</y>
<width>678</width>
<height>2739</height>
</rect>
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>21</number>
<number>18</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -512,6 +512,31 @@ Show a yellow background when the number of odometry inliers goes under this thr
<layout class="QVBoxLayout" name="verticalLayout_112">
<item>
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,1">
<item row="3" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_maxDepth_odom">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>1</number>
</property>
<property name="maximum">
<double>100.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLineEdit" name="lineEdit_roiRatios_odom"/>
</item>
<item row="5" column="0">
<widget class="QLineEdit" name="lineEdit_roiRatios"/>
</item>
<item row="0" column="0">
<widget class="QLabel" name="label_154">
<property name="text">
@@ -597,7 +622,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="12" column="0">
<item row="13" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_opacity">
<property name="suffix">
<string/>
@@ -616,7 +641,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="12" column="1">
<item row="13" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_odom">
<property name="suffix">
<string/>
@@ -635,7 +660,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="12" column="2">
<item row="13" column="2">
<widget class="QLabel" name="label_155">
<property name="text">
<string>Opacity.</string>
@@ -648,7 +673,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="13" column="2">
<item row="14" column="2">
<widget class="QLabel" name="label_157">
<property name="text">
<string>Point size (1..64).</string>
@@ -712,25 +737,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_maxDepth_odom">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>1</number>
</property>
<property name="maximum">
<double>100.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="3" column="2">
<widget class="QLabel" name="label_132">
<property name="text">
@@ -798,12 +804,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QLineEdit" name="lineEdit_roiRatios"/>
</item>
<item row="5" column="1">
<widget class="QLineEdit" name="lineEdit_roiRatios_odom"/>
</item>
<item row="5" column="2">
<widget class="QLabel" name="label_353">
<property name="text">
@@ -858,7 +858,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="13" column="0">
<item row="14" column="0">
<widget class="QSpinBox" name="spinBox_ptsize">
<property name="minimum">
<number>1</number>
@@ -871,7 +871,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="13" column="1">
<item row="14" column="1">
<widget class="QSpinBox" name="spinBox_ptsize_odom">
<property name="minimum">
<number>1</number>
@@ -1010,6 +1010,26 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="12" column="2">
<widget class="QLabel" name="label_427">
<property name="text">
<string>Normal radius search. If not 0, normals will be computed and added to created cloud for visualization (keys 7, 8 and 9).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="12" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_normalRadiusSearch">
<property name="singleStep">
<double>0.010000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
<item>
@@ -1117,7 +1137,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
<string>Laser Scan</string>
</property>
<layout class="QGridLayout" name="gridLayout_79" columnstretch="0,0,1">
<item row="8" column="1">
<item row="9" column="1">
<widget class="QSpinBox" name="spinBox_ptsize_odom_scan">
<property name="minimum">
<number>1</number>
@@ -1127,7 +1147,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="8" column="2">
<item row="9" column="2">
<widget class="QLabel" name="label_158">
<property name="text">
<string>Scan point size (1..64).</string>
@@ -1169,7 +1189,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="7" column="2">
<item row="8" column="2">
<widget class="QLabel" name="label_156">
<property name="text">
<string>Scan opacity.</string>
@@ -1195,7 +1215,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="8" column="0">
<item row="9" column="0">
<widget class="QSpinBox" name="spinBox_ptsize_scan">
<property name="minimum">
<number>1</number>
@@ -1235,7 +1255,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="7" column="0">
<item row="8" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_scan">
<property name="suffix">
<string/>
@@ -1267,7 +1287,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="7" column="1">
<item row="8" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_odom_scan">
<property name="suffix">
<string/>
@@ -1440,6 +1460,26 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="7" column="2">
<widget class="QLabel" name="label_428">
<property name="text">
<string>Normal radius search. If not 0, normals will be computed and added to created cloud for visualization (keys 7, 8 and 9).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_normalRadiusSearch_scan">
<property name="singleStep">
<double>0.010000000000000</double>
</property>
</widget>
</item>
</layout>
</widget>
</item>
@@ -5283,6 +5323,29 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_425">
<property name="text">
<string>Search radius for normals computation (0=disabled). Useful if the ICP registration approach is point to plane.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cameraImages_scanNormalsRadius">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;KITTI: 130 000 points&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
</layout>
@@ -5995,6 +6058,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<layout class="QVBoxLayout" name="verticalLayout_10">
<item>
<layout class="QGridLayout" name="gridLayout_42" columnstretch="0,1">
<item row="14" column="0">
<widget class="QSpinBox" name="spinBox_imagePreDecimation">
<property name="minimum">
<number>-16</number>
</property>
<property name="maximum">
<number>16</number>
</property>
</widget>
</item>
<item row="15" column="0">
<widget class="QSpinBox" name="spinBox_imagePostDecimation">
<property name="minimum">
<number>-16</number>
</property>
<property name="maximum">
<number>16</number>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QSpinBox" name="general_spinBox_maxStMemSize">
<property name="minimum">
@@ -6053,32 +6136,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QSpinBox" name="general_spinBox_maxRetrieved">
<property name="maximum">
<number>999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>2</number>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_retrieved_3">
<property name="text">
<string>Bad signatures are ignored.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QCheckBox" name="general_checkBox_initWMWithAllNodes">
<property name="text">
@@ -6089,16 +6146,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="9" column="1">
<widget class="QLabel" name="label_retrieved_5">
<property name="text">
<string>Keep raw sensor data. Only useful to save loop closure computation time when features re-extraction is enabled. Disable to save RAM memory.</string>
<item row="2" column="0">
<widget class="QSpinBox" name="general_spinBox_maxRetrieved">
<property name="maximum">
<number>999</number>
</property>
<property name="wordWrap">
<bool>true</bool>
<property name="singleStep">
<number>1</number>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
<property name="value">
<number>2</number>
</property>
</widget>
</item>
@@ -6122,10 +6179,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_retrieved_11">
<item row="6" column="1">
<widget class="QLabel" name="label_retrieved_3">
<property name="text">
<string>Create map labels. The first node of a map will be labelled as &quot;map#&quot; where # is the map ID.</string>
<string>Bad signatures are ignored.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -6145,26 +6202,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="16" column="0">
<widget class="QSpinBox" name="general_spinBox_laserScanDownsample">
<property name="minimumSize">
<size>
<width>50</width>
<height>0</height>
</size>
<item row="9" column="1">
<widget class="QLabel" name="label_retrieved_5">
<property name="text">
<string>Keep raw sensor data. Only useful to save loop closure computation time when features re-extraction is enabled. Disable to save RAM memory.</string>
</property>
<property name="minimum">
<number>1</number>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="maximum">
<number>9999</number>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="11" column="1">
<widget class="QLabel" name="label_retrieved_12">
<item row="5" column="1">
<widget class="QLabel" name="label_retrieved_11">
<property name="text">
<string>Raw descriptors kept in memory.</string>
<string>Create map labels. The first node of a map will be labelled as &quot;map#&quot; where # is the map ID.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -6184,6 +6238,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="11" column="1">
<widget class="QLabel" name="label_retrieved_12">
<property name="text">
<string>Raw descriptors kept in memory.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="14" column="1">
<widget class="QLabel" name="label_retrieved_13">
<property name="text">
@@ -6233,6 +6300,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QCheckBox" name="general_checkBox_badSignaturesIgnored">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="general_checkBox_transferSortingByWeightId">
<property name="text">
@@ -6256,16 +6333,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="15" column="0">
<widget class="QSpinBox" name="spinBox_imagePostDecimation">
<property name="minimum">
<number>-16</number>
</property>
<property name="maximum">
<number>16</number>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_29">
<property name="text">
@@ -6279,6 +6346,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="12" column="0">
<widget class="QCheckBox" name="general_checkBox_saveDepth16bits">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_ratioRecent">
<property name="text">
@@ -6292,16 +6369,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QCheckBox" name="general_checkBox_badSignaturesIgnored">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="15" column="1">
<widget class="QLabel" name="label_retrieved_6">
<property name="text">
@@ -6328,21 +6395,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="16" column="1">
<widget class="QLabel" name="label_retrieved_8">
<property name="text">
<string>If &gt; 1, downsample the laser scans when creating a location. This feature can be used to save laser scans already downsampled.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="12" column="0">
<widget class="QCheckBox" name="general_checkBox_saveDepth16bits">
<item row="13" column="0">
<widget class="QCheckBox" name="general_checkBox_compressionParallelized">
<property name="text">
<string/>
</property>
@@ -6351,45 +6405,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="14" column="0">
<widget class="QSpinBox" name="spinBox_imagePreDecimation">
<property name="minimum">
<number>-16</number>
</property>
<property name="maximum">
<number>16</number>
</property>
</widget>
</item>
<item row="17" column="0">
<widget class="QSpinBox" name="general_spinBox_laserScanNormalK">
<property name="minimumSize">
<size>
<width>50</width>
<height>0</height>
</size>
</property>
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>99</number>
</property>
<property name="value">
<number>0</number>
</property>
</widget>
</item>
<item row="17" column="1">
<widget class="QLabel" name="label_retrieved_14">
<item row="10" column="0">
<widget class="QCheckBox" name="general_checkBox_saveIntermediateNodeData">
<property name="text">
<string>If &gt; 0 and laser scans are 3D without normals, normals will be computed with K search neighbors when creating a signature.</string>
<string/>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
@@ -6406,16 +6428,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="13" column="0">
<widget class="QCheckBox" name="general_checkBox_compressionParallelized">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="10" column="1">
<widget class="QLabel" name="label_retrieved_16">
<property name="text">
@@ -6429,18 +6441,139 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QCheckBox" name="general_checkBox_saveIntermediateNodeData">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QGroupBox" name="groupBox_27">
<property name="title">
<string>Laser scan filtering</string>
</property>
<layout class="QGridLayout" name="gridLayout_10" columnstretch="0,1">
<item row="0" column="0">
<widget class="QSpinBox" name="general_spinBox_laserScanDownsample">
<property name="minimumSize">
<size>
<width>50</width>
<height>0</height>
</size>
</property>
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>9999</number>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_retrieved_8">
<property name="text">
<string>Downsampling step. If &gt; 1, downsample the laser scans when creating a location. This feature can be used to save laser scans already downsampled.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="general_doubleSpinBox_laserScanNormalRadius">
<property name="minimumSize">
<size>
<width>50</width>
<height>0</height>
</size>
</property>
<property name="suffix">
<string> m</string>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QSpinBox" name="general_spinBox_laserScanNormalK">
<property name="minimumSize">
<size>
<width>50</width>
<height>0</height>
</size>
</property>
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>99</number>
</property>
<property name="value">
<number>0</number>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_retrieved_14">
<property name="text">
<string>Normal K. If &gt; 0 and laser scans don't have normals, normals will be computed with K search neighbors when creating a signature.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_retrieved_17">
<property name="text">
<string>Normal Radius. If &gt; 0 and laser scans don't have normals, normals will be computed with radius search when creating a signature.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_retrieved_18">
<property name="text">
<string>Voxel size. If &gt; 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.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="general_doubleSpinBox_laserScanVoxelSize">
<property name="minimumSize">
<size>
<width>50</width>
<height>0</height>
</size>
</property>
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_rehearsal2">
<property name="title">
@@ -9947,7 +10080,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item>
<widget class="QStackedWidget" name="stackedWidget_odometryType">
<property name="currentIndex">
<number>5</number>
<number>0</number>
</property>
<widget class="QWidget" name="page_52">
<layout class="QVBoxLayout" name="verticalLayout_77">
@@ -10043,6 +10176,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QSpinBox" name="spinBox_odom_f2m_scanMaxSize">
<property name="maximum">
<number>999999999</number>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_194">
<property name="text">
@@ -10069,14 +10209,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QSpinBox" name="spinBox_odom_f2m_scanMaxSize">
<property name="maximum">
<number>999999999</number>
</property>
</widget>
</item>
<item row="4" column="1">
<item row="5" column="1">
<widget class="QLabel" name="label_357">
<property name="text">
<string>[Visual] Local bundle adjustment. See Optimizer panel. This will not work if Optical Flow correspondences strategy is selected in Visual Registration panel.</string>
@@ -10089,7 +10222,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="5" column="1">
<item row="6" column="0">
<widget class="QSpinBox" name="odom_f2m_bundleMaxFrames">
<property name="maximum">
<number>999999</number>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_358">
<property name="text">
<string>[Visual] Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).</string>
@@ -10102,7 +10242,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="4" column="0">
<item row="5" column="0">
<widget class="QComboBox" name="odom_f2m_bundleStrategy">
<property name="sizeAdjustPolicy">
<enum>QComboBox::AdjustToContents</enum>
@@ -10124,10 +10264,32 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</item>
</widget>
</item>
<item row="5" column="0">
<widget class="QSpinBox" name="odom_f2m_bundleMaxFrames">
<item row="4" column="1">
<widget class="QLabel" name="label_430">
<property name="text">
<string>[Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when Radius above is &gt;0). 0 means any angle.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_odom_f2m_scanAngle">
<property name="suffix">
<string> deg</string>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="maximum">
<number>999999</number>
<double>180.000000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
@@ -13471,6 +13633,61 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</item>
<item>
<layout class="QGridLayout" name="gridLayout_48" columnstretch="0,1">
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icpMaxCorrespondenceDistance">
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.001000000000000</double>
</property>
<property name="maximum">
<double>9999.989999999999782</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_125">
<property name="text">
<string>Uniform sampling voxel size. Set to 0 to disable.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QSpinBox" name="loopClosure_icpDownsamplingStep">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>1</number>
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icpPointToPlaneNormalsRadius">
<property name="singleStep">
<double>0.010000000000000</double>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QCheckBox" name="loopClosure_icpPointToPlane">
<property name="text">
@@ -13572,35 +13789,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_125">
<property name="text">
<string>Uniform sampling voxel size. Set to 0 to disable.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QSpinBox" name="loopClosure_icpDownsamplingStep">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>1</number>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icpVoxelSize">
<property name="suffix">
@@ -13697,29 +13885,10 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icpMaxCorrespondenceDistance">
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.001000000000000</double>
</property>
<property name="maximum">
<double>9999.989999999999782</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_144">
<property name="text">
<string>Point to plane ICP. Only for ICP 3D.</string>
<string>Point to plane ICP.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -13764,6 +13933,45 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="10" column="1">
<widget class="QLabel" name="label_426">
<property name="text">
<string>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.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="11" column="1">
<widget class="QLabel" name="label_429">
<property name="text">
<string>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.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="11" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_icpPointToPlaneNormalsMinComplexity">
<property name="decimals">
<number>3</number>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
<item>
package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package>
<name>rtabmap</name>
<version>0.13.2</version>
<version>0.13.3</version>
<description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
<maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
tools/KittiDataset/main.cpp
+22 -9
View File
@@ -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(" *");
tools/OdometryViewer/main.cpp
+6 -3
View File
@@ -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();
tools/RgbdDataset/main.cpp
+5 -5
View File
@@ -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(" *");