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

Merged master to devel

Browse Source
This commit is contained in:
matlabbe
2017-06-10 10:34:52 -04:00
parent e321e1bb2c dfc425a7c5
commit fa2199c3ea
12 changed files with 127 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Parameters.h
+3 -3
View File
@@ -228,7 +228,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Kp, DetectorStrategy, int, 0, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=FREAK.");
#else
RTABMAP_PARAM(Kp, DetectorStrategy, int, 2, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=FREAK.");
#endif
#endif
RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed, bool, true, "Use of the td-idf strategy to compute the likelihood.");
RTABMAP_PARAM(Kp, Parallelized, bool, true, "If the dictionary update and signature creation were parallelized.");
RTABMAP_PARAM_STR(Kp, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
@@ -327,7 +327,7 @@ class RTABMAP_EXP Parameters
// Local/Proximity loop closure detection
RTABMAP_PARAM(RGBD, ProximityByTime, bool, false, "Detection over all locations in STM.");
RTABMAP_PARAM(RGBD, ProximityBySpace, bool, true, "Detection over locations (in Working Memory or STM) near in space.");
RTABMAP_PARAM(RGBD, ProximityBySpace, bool, true, "Detection over locations (in Working Memory) near in space.");
RTABMAP_PARAM(RGBD, ProximityMaxGraphDepth, int, 50, "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
RTABMAP_PARAM(RGBD, ProximityMaxPaths, int, 3, "Maximum paths compared (from the most recent) for proximity detection by space. 0 means no limit.");
RTABMAP_PARAM(RGBD, ProximityPathFilteringRadius, float, 0.5, "Path filtering radius to reduce the number of nodes to compare in a path. A path should also be inside that radius to be considered for proximity detection.");
@@ -335,7 +335,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed, bool, true, "When comparing to a local path, merge the scan using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
RTABMAP_PARAM(RGBD, ProximityAngle, float, 45, "Maximum angle (degrees) for visual proximity detection.");
// Graph optimization
// Graph optimization
#ifdef RTABMAP_GTSAM
RTABMAP_PARAM(Optimizer, Strategy, int, 2, "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
RTABMAP_PARAM(Optimizer, Iterations, int, 20, "Optimization iterations.");
corelib/include/rtabmap/core/clams/discrete_depth_distortion_model.h
+1
View File
@@ -32,6 +32,7 @@ RTAB-Map integration: Mathieu Labbe
#include <assert.h>
#include <vector>
#include <set>
#include <Eigen/Core>
#include <opencv2/opencv.hpp>
#include <rtabmap/utilite/UMutex.h>
corelib/src/CameraStereo.cpp
+18 -10
View File
@@ -1029,7 +1029,8 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
sl::Pose pose;
zed_->getPosition(pose);
int trackingConfidence = pose.pose_confidence;
if (trackingConfidence)
// FIXME What does pose_confidence == -1 mean?
if (trackingConfidence>0)
{
info->odomPose = zedPoseToTransform(pose);
if (!info->odomPose.isNull())
@@ -1037,24 +1038,31 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
//transform x->forward, y->left, z->up
Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
info->odomPose = opticalTransform * info->odomPose * opticalTransform.inverse();
}
if (lost_)
{
info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose
lost_ = false;
UDEBUG("Init %s (var=%f)", info->odomPose.prettyPrint().c_str(), 9999.0f);
if (lost_)
{
info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose
lost_ = false;
UDEBUG("Init %s (var=%f)", info->odomPose.prettyPrint().c_str(), 9999.0f);
}
else
{
info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence);
UDEBUG("Run %s (var=%f)", info->odomPose.prettyPrint().c_str(), 1.0f / float(trackingConfidence));
}
}
else
{
info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence);
UDEBUG("Run %s (var=%f)", info->odomPose.prettyPrint().c_str(), 1.0f / float(trackingConfidence));
info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
lost_ = true;
UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
}
}
else
{
info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
lost_ = true;
UWARN("ZED lost!");
UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
}
}
}
corelib/src/pcl18/surface/impl/texture_mapping.hpp
+1 -1
View File
@@ -1327,7 +1327,7 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
cv::Mat depth = cameras[current_cam].depth;
bool currentDepthSet = false;
float maxDepthError = max_depth_error_==0.0f?std::sqrt(iter->second.longestEdgeSqrd) : max_depth_error_;
float maxDepthError = max_depth_error_==0.0f?std::sqrt(iter->second.longestEdgeSqrd)*2.0f : max_depth_error_;
if(!cameras[current_cam].depth.empty() && maxDepthError > 0.0f)
{
float d1 = depth.type() == CV_32FC1?
corelib/src/util3d.cpp
+2 -5
View File
@@ -990,12 +990,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
decimation = 1;
}
UASSERT(!sensorData.imageRaw().empty());
UASSERT((!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) ||
(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValidForProjection()));
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
if(!sensorData.imageRaw().empty() && !sensorData.depthRaw().empty() && sensorData.cameraModels().size())
{
//depth
UDEBUG("");
@@ -1090,7 +1087,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
}
}
}
else if(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValidForProjection())
else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValidForProjection())
{
//stereo
UDEBUG("");