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

Merge branch 'master' of github.com:introlab/rtabmap into gtest

Browse Source
This commit is contained in:
matlabbe
2025-06-21 13:45:01 -07:00
parent c776b9f056 ee98be9ef7
commit 7b1f37b8ee
84 changed files with 3332 additions and 1443 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/SensorData.h
+54 -2
View File
@@ -78,6 +78,16 @@ public:
double stamp = 0.0,
const cv::Mat & userData = cv::Mat());
// RGB-D constructor + depth confidence
SensorData(
const cv::Mat & rgb,
const cv::Mat & depth,
const cv::Mat & depth_confidence,
const CameraModel & cameraModel,
int id = 0,
double stamp = 0.0,
const cv::Mat & userData = cv::Mat());
// RGB-D constructor + laser scan
SensorData(
const LaserScan & laserScan,
@@ -87,6 +97,17 @@ public:
int id = 0,
double stamp = 0.0,
const cv::Mat & userData = cv::Mat());
// RGB-D constructor + confidence + laser scan
SensorData(
const LaserScan & laserScan,
const cv::Mat & rgb,
const cv::Mat & depth,
const cv::Mat & depthConfidence,
const CameraModel & cameraModel,
int id = 0,
double stamp = 0.0,
const cv::Mat & userData = cv::Mat());
// Multi-cameras RGB-D constructor
SensorData(
@@ -96,6 +117,16 @@ public:
int id = 0,
double stamp = 0.0,
const cv::Mat & userData = cv::Mat());
// Multi-cameras RGB-D constructor + depth confidence
SensorData(
const cv::Mat & rgb,
const cv::Mat & depth,
const cv::Mat & depthConfidence,
const std::vector<CameraModel> & cameraModels,
int id = 0,
double stamp = 0.0,
const cv::Mat & userData = cv::Mat());
// Multi-cameras RGB-D constructor + laser scan
SensorData(
@@ -107,6 +138,17 @@ public:
double stamp = 0.0,
const cv::Mat & userData = cv::Mat());
// Multi-cameras RGB-D constructor + depth confidence + laser scan
SensorData(
const LaserScan & laserScan,
const cv::Mat & rgb,
const cv::Mat & depth,
const cv::Mat & depthConfidence,
const std::vector<CameraModel> & cameraModels,
int id = 0,
double stamp = 0.0,
const cv::Mat & userData = cv::Mat());
// Stereo constructor
SensorData(
const cv::Mat & left,
@@ -160,6 +202,8 @@ public:
_imageCompressed.empty() &&
_depthOrRightRaw.empty() &&
_depthOrRightCompressed.empty() &&
_depthConfidenceRaw.empty() &&
_depthConfidenceCompressed.empty() &&
_laserScanRaw.isEmpty() &&
_laserScanCompressed.isEmpty() &&
_cameraModels.empty() &&
@@ -178,10 +222,12 @@ public:
const cv::Mat & imageCompressed() const {return _imageCompressed;}
const cv::Mat & depthOrRightCompressed() const {return _depthOrRightCompressed;}
const cv::Mat & depthConfidenceCompressed() const {return _depthConfidenceCompressed;}
const LaserScan & laserScanCompressed() const {return _laserScanCompressed;}
const cv::Mat & imageRaw() const {return _imageRaw;}
const cv::Mat & depthOrRightRaw() const {return _depthOrRightRaw;}
const cv::Mat & depthConfidenceRaw() const {return _depthConfidenceRaw;}
const LaserScan & laserScanRaw() const {return _laserScanRaw;}
/**
@@ -190,7 +236,9 @@ public:
* @param clearPreviousData, clear previous raw and compressed images before setting the new ones.
*/
void setRGBDImage(const cv::Mat & rgb, const cv::Mat & depth, const CameraModel & model, bool clearPreviousData = true);
void setRGBDImage(const cv::Mat & rgb, const cv::Mat & depth, const cv::Mat & depth_confidence, const CameraModel & model, bool clearPreviousData = true);
void setRGBDImage(const cv::Mat & rgb, const cv::Mat & depth, const std::vector<CameraModel> & models, bool clearPreviousData = true);
void setRGBDImage(const cv::Mat & rgb, const cv::Mat & depth, const cv::Mat & depth_confidence, const std::vector<CameraModel> & models, bool clearPreviousData = true);
void setStereoImage(const cv::Mat & left, const cv::Mat & right, const StereoCameraModel & stereoCameraModel, bool clearPreviousData = true);
void setStereoImage(const cv::Mat & left, const cv::Mat & right, const std::vector<StereoCameraModel> & stereoCameraModels, bool clearPreviousData = true);
@@ -227,7 +275,8 @@ public:
cv::Mat * userDataRaw = 0,
cv::Mat * groundCellsRaw = 0,
cv::Mat * obstacleCellsRaw = 0,
cv::Mat * emptyCellsRaw = 0);
cv::Mat * emptyCellsRaw = 0,
cv::Mat * depthConfidenceRaw = 0);
void uncompressDataConst(
cv::Mat * imageRaw,
cv::Mat * depthOrRightRaw,
@@ -235,7 +284,8 @@ public:
cv::Mat * userDataRaw = 0,
cv::Mat * groundCellsRaw = 0,
cv::Mat * obstacleCellsRaw = 0,
cv::Mat * emptyCellsRaw = 0) const;
cv::Mat * emptyCellsRaw = 0,
cv::Mat * depthConfidenceRaw = 0) const;
const std::vector<CameraModel> & cameraModels() const {return _cameraModels;}
const std::vector<StereoCameraModel> & stereoCameraModels() const {return _stereoCameraModels;}
@@ -327,10 +377,12 @@ private:
cv::Mat _imageCompressed; // compressed image
cv::Mat _depthOrRightCompressed; // compressed image
cv::Mat _depthConfidenceCompressed; // compressed data
LaserScan _laserScanCompressed; // compressed data
cv::Mat _imageRaw; // CV_8UC1 or CV_8UC3
cv::Mat _depthOrRightRaw; // depth CV_16UC1 or CV_32FC1, right image CV_8UC1 or CV_8UC3
cv::Mat _depthConfidenceRaw; // CV_8UC1
LaserScan _laserScanRaw;
std::vector<CameraModel> _cameraModels;
corelib/include/rtabmap/core/Transform.h
+1 -1
View File
@@ -113,7 +113,7 @@ public:
void getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const;
void getEulerAngles(float & roll, float & pitch, float & yaw) const;
void getTranslation(float & x, float & y, float & z) const;
float getAngle(float x=1.0f, float y=0.0f, float z=0.0f) const;
float getAngle(const Transform & t) const;
float getNorm() const;
float getNormSquared() const;
float getDistance(const Transform & t) const;
corelib/include/rtabmap/core/util2d.h
+12
View File
@@ -513,6 +513,14 @@ cv::Mat RTABMAP_CORE_EXPORT registerDepth(
const cv::Size & colorSize,
const cv::Mat & colorK,
const rtabmap::Transform & transform);
cv::Mat RTABMAP_CORE_EXPORT registerDepth(
const cv::Mat & depth,
const cv::Mat & confidence,
const cv::Mat & depthK,
const cv::Size & colorSize,
const cv::Mat & colorK,
const rtabmap::Transform & transform,
cv::Mat & registeredConfidence);
/**
* @brief Fills holes in the depth image using linear interpolation.
@@ -598,6 +606,10 @@ cv::Mat RTABMAP_CORE_EXPORT fastBilateralFiltering(
float sigmaR = 0.05f,
bool earlyDivision = false);
void RTABMAP_CORE_EXPORT depthBleedingFiltering(
cv::Mat & depth,
float maxDepthError);
/**
* @brief Automatic brightness and contrast optimization with optional histogram clipping.
*
corelib/include/rtabmap/core/util3d.h
+27 -4
View File
@@ -211,6 +211,15 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromDepth(
float maxDepth = 0.0f,
float minDepth = 0.0f,
std::vector<int> * validIndices = 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromDepth(
const cv::Mat & imageDepth,
const cv::Mat & imageDepthConfidence,
const CameraModel & model,
int decimation = 1,
float maxDepth = 0.0f,
float minDepth = 0.0f,
unsigned char confidenceThr = 0,
std::vector<int> * validIndices = 0);
/**
* @brief Creates a point cloud from an RGB image and a depth image.
@@ -276,6 +285,16 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudFromDepthRGB(
float maxDepth = 0.0f,
float minDepth = 0.0f,
std::vector<int> * validIndices = 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudFromDepthRGB(
const cv::Mat & imageRgb,
const cv::Mat & imageDepth,
const cv::Mat & imageDepthConfidence,
const CameraModel & model,
int decimation = 1,
float maxDepth = 0.0f,
float minDepth = 0.0f,
unsigned char confidenceThr = 0, // 0=low, 100=high
std::vector<int> * validIndices = 0);
/**
* @brief Converts a disparity image to a 3D point cloud.
@@ -447,7 +466,8 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> RTABMAP_CORE_EXPORT cloudsFromS
float minDepth = 0.0f,
std::vector<pcl::IndicesPtr> * validIndices = 0,
const ParametersMap & stereoParameters = ParametersMap(),
const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
const std::vector<float> & roiRatios = std::vector<float>(), // ignored for stereo
unsigned char confidenceThr = 0); // ignored for stereo
/**
* @brief Generates a point cloud from sensor data.
@@ -484,7 +504,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData(
float minDepth = 0.0f,
std::vector<int> * validIndices = 0,
const ParametersMap & stereoParameters = ParametersMap(),
const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
const std::vector<float> & roiRatios = std::vector<float>(), // ignored for stereo
unsigned char confidenceThr = 0); // ignored for stereo
/**
* @brief Generates a point cloud with RGB color data from sensor data.
@@ -525,7 +546,8 @@ std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> RTABMAP_CORE_EXPORT cloudsRG
float minDepth = 0.0f,
std::vector<pcl::IndicesPtr > * validIndices = 0,
const ParametersMap & stereoParameters = ParametersMap(),
const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
const std::vector<float> & roiRatios = std::vector<float>(), // ignored for stereo
unsigned char confidenceThr = 0); // ignored for stereo
/**
* @brief Generates a point cloud of type pcl::PointXYZRGB from sensor data.
@@ -560,7 +582,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudRGBFromSensorDat
float minDepth = 0.0f,
std::vector<int> * validIndices = 0,
const ParametersMap & stereoParameters = ParametersMap(),
const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
const std::vector<float> & roiRatios = std::vector<float>(), // ignored for stereo
unsigned char confidenceThr = 0); // ignored for stereo
/**
* @brief Converts the middle row of a depth image into a laser scan (point cloud) using camera intrinsics and a local transformation.
corelib/src/CMakeLists.txt
+1
View File
@@ -816,6 +816,7 @@ CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql.in ${CMA
SET(RESOURCES
${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql
${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_20_0.sql
${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_18_3.sql
${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_18_0.sql
${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_17_0.sql
corelib/src/DBDriverSqlite3.cpp
+49 -6
View File
@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/Compression.h"
#include "DatabaseSchema_sql.h"
#include "DatabaseSchema_0_20_0_sql.h"
#include "DatabaseSchema_0_18_3_sql.h"
#include "DatabaseSchema_0_18_0_sql.h"
#include "DatabaseSchema_0_17_0_sql.h"
@@ -402,6 +403,7 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
schemas.push_back(std::make_pair("0.17.0", DATABASESCHEMA_0_17_0_SQL));
schemas.push_back(std::make_pair("0.18.0", DATABASESCHEMA_0_18_0_SQL));
schemas.push_back(std::make_pair("0.18.3", DATABASESCHEMA_0_18_3_SQL));
schemas.push_back(std::make_pair("0.20.0", DATABASESCHEMA_0_20_0_SQL));
schemas.push_back(std::make_pair(uNumber2Str(RTABMAP_VERSION_MAJOR)+"."+uNumber2Str(RTABMAP_VERSION_MINOR), DATABASESCHEMA_SQL));
for(size_t i=0; i<schemas.size(); ++i)
{
@@ -1317,7 +1319,15 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
if(images)
{
fields << "image, depth, calibration";
if(uStrNumCmp(_version, "0.22.0") >= 0)
{
fields << "image, depth, depth_confidence, calibration";
}
else
{
fields << "image, depth, calibration";
}
if(scan || userData || occupancyGrid)
{
fields << ", ";
@@ -1448,6 +1458,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
cv::Mat imageCompressed;
cv::Mat depthOrRightCompressed;
cv::Mat depthConfidenceCompressed;
std::vector<CameraModel> models;
std::vector<StereoCameraModel> stereoModels;
Transform localTransform = Transform::getIdentity();
@@ -1472,6 +1483,17 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
}
if(uStrNumCmp(_version, "0.22.0") >= 0)
{
//Create the depth image
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>4 && data)
{
depthConfidenceCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
}
}
if(uStrNumCmp(_version, "0.10.0") < 0)
{
data = sqlite3_column_blob(ppStmt, index); // local transform
@@ -1823,7 +1845,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
{
if(models.size())
{
(*iter)->sensorData().setRGBDImage(imageCompressed, depthOrRightCompressed, models);
(*iter)->sensorData().setRGBDImage(imageCompressed, depthOrRightCompressed, depthConfidenceCompressed, models);
}
else
{
@@ -4478,6 +4500,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
{
if(!(*i)->sensorData().imageCompressed().empty() ||
!(*i)->sensorData().depthOrRightCompressed().empty() ||
!(*i)->sensorData().depthConfidenceCompressed().empty() ||
!(*i)->sensorData().laserScanCompressed().isEmpty() ||
!(*i)->sensorData().userDataCompressed().empty() ||
!(*i)->sensorData().cameraModels().empty() ||
@@ -5503,7 +5526,7 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
materialPolygons[p][i] = serializedPolygons.at<int>(t + p*polygonSize + i);
}
}
t+=materialPolygons.size()*polygonSize;
t+=materialPolygons.size()*polygonSize-1;
polygons->push_back(materialPolygons);
}
}
@@ -5520,7 +5543,7 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
UASSERT(serializedTexCoords.total());
for(int t=0; t<serializedTexCoords.cols; ++t)
{
UASSERT(int(serializedTexCoords.at<float>(t)) > 0);
UASSERT_MSG(int(serializedTexCoords.at<float>(t)) > 0, uFormat("serializedTexCoords.at<float>(%d)=%f", t, serializedTexCoords.at<float>(t)).c_str());
#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > materialtexCoords(int(serializedTexCoords.at<float>(t)));
#else
@@ -5533,8 +5556,10 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
{
materialtexCoords[p][0] = serializedTexCoords.at<float>(t + p*2);
materialtexCoords[p][1] = serializedTexCoords.at<float>(t + p*2 + 1);
UASSERT(materialtexCoords[p][0]>=0.0f && materialtexCoords[p][0] <= 1.0f);
UASSERT(materialtexCoords[p][1]>=0.0f && materialtexCoords[p][1] <= 1.0f);
}
t+=materialtexCoords.size()*2;
t+=materialtexCoords.size()*2-1;
texCoords->push_back(materialtexCoords);
}
}
@@ -6186,7 +6211,11 @@ void DBDriverSqlite3::stepScanUpdate(sqlite3_stmt * ppStmt, int nodeId, const La
std::string DBDriverSqlite3::queryStepSensorData() const
{
UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
if(uStrNumCmp(_version, "0.16.0") >= 0)
if(uStrNumCmp(_version, "0.22.0") >= 0)
{
return "INSERT INTO Data(id, image, depth, depth_confidence, calibration, scan_info, scan, user_data, ground_cells, obstacle_cells, empty_cells, cell_size, view_point_x, view_point_y, view_point_z) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.16.0") >= 0)
{
return "INSERT INTO Data(id, image, depth, calibration, scan_info, scan, user_data, ground_cells, obstacle_cells, empty_cells, cell_size, view_point_x, view_point_y, view_point_z) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?,?);";
}
@@ -6250,6 +6279,20 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
//depth confidence
if(uStrNumCmp(_version, "0.22.0") >= 0)
{
if(!sensorData.depthConfidenceCompressed().empty())
{
rc = sqlite3_bind_blob(ppStmt, index++, sensorData.depthConfidenceCompressed().data, (int)sensorData.depthConfidenceCompressed().cols, SQLITE_STATIC);
}
else
{
rc = sqlite3_bind_null(ppStmt, index++);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
// calibration
std::vector<unsigned char> calibrationData;
std::vector<float> calibration;
corelib/src/DBReader.cpp
+16 -2
View File
@@ -607,6 +607,7 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
// update images and local transforms
cv::Mat combinedImages;
cv::Mat combinedDepthImages;
cv::Mat combinedDepthConfidenceImages;
std::vector<CameraModel> combinedModels;
std::vector<StereoCameraModel> combinedStereoModels;
for(size_t i=0; i<_cameraIndices.size(); ++i)
@@ -645,6 +646,18 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
fromROI = cv::Mat(data.depthOrRightRaw(), cv::Rect(_cameraIndices[i]*subImageWidth, 0, subImageWidth, data.depthOrRightRaw().rows));
toROI = cv::Mat(combinedDepthImages, cv::Rect(addedCameras*subImageWidth, 0, subImageWidth, combinedDepthImages.rows));
fromROI.copyTo(toROI);
if(!data.depthConfidenceRaw().empty())
{
UASSERT(data.depthConfidenceRaw().size() == data.depthOrRightRaw().size());
if(combinedDepthConfidenceImages.empty())
{
combinedDepthConfidenceImages = cv::Mat(data.depthConfidenceRaw().rows, subImageWidth*(_cameraIndices.size()-i), data.depthConfidenceRaw().type());
}
fromROI = cv::Mat(data.depthConfidenceRaw(), cv::Rect(_cameraIndices[i]*subImageWidth, 0, subImageWidth, data.depthConfidenceRaw().rows));
toROI = cv::Mat(combinedDepthConfidenceImages, cv::Rect(addedCameras*subImageWidth, 0, subImageWidth, combinedDepthConfidenceImages.rows));
fromROI.copyTo(toROI);
}
}
if(!data.cameraModels().empty())
@@ -671,7 +684,7 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
}
if(!combinedModels.empty())
{
data.setRGBDImage(combinedImages, combinedDepthImages, combinedModels);
data.setRGBDImage(combinedImages, combinedDepthImages, combinedDepthConfidenceImages, combinedModels);
}
else
{
@@ -728,10 +741,11 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
}
data.setLandmarks(landmarks);
UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, Grid=%d, UserData=%d, GlobalPose=%d, GPS=%d, IMU=%d",
UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, Conf=%d, Grid=%d, UserData=%d, GlobalPose=%d, GPS=%d, IMU=%d",
data.laserScanRaw().isEmpty()?0:1,
data.imageRaw().empty()?0:1,
data.depthOrRightRaw().empty()?0:1,
data.depthConfidenceRaw().empty()?0:1,
data.gridCellSize()==0.0f?0:1,
data.userDataRaw().empty()?0:1,
globalPose.isNull()?0:1,
corelib/src/Features2d.cpp
+1 -1
View File
@@ -688,7 +688,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
#ifndef RTABMAP_TORCH
if(type == Feature2D::kFeatureSuperPointTorch)
{
UWARN("SupertPoint Torch feature cannot be used as RTAB-Map is not built with the option enabled. GFTT/ORB is used instead.");
UWARN("SuperPoint Torch feature cannot be used as RTAB-Map is not built with the option enabled. GFTT/ORB is used instead.");
type = Feature2D::kFeatureGfttOrb;
}
#endif
corelib/src/Graph.cpp
+17 -12
View File
@@ -744,9 +744,8 @@ void calcRelativeErrors (
// compute rotational and translational errors
Transform pose_delta_gt = poses_gt[i].inverse()*poses_gt[i+1];
Transform pose_delta_result = poses_result[i].inverse()*poses_result[i+1];
Transform pose_error = pose_delta_result.inverse()*pose_delta_gt;
float r_err = pose_error.getAngle();
float t_err = pose_error.getNorm();
float r_err = pose_delta_result.getAngle(pose_delta_gt);
float t_err = pose_delta_result.getDistance(pose_delta_gt);
// write to file
err.push_back(errors(i,r_err,t_err,0,0));
@@ -993,15 +992,21 @@ void computeMaxGraphErrors(
if(iter->second.type() != Link::kLandmark ||
1.0 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999.0)
{
float opt_roll,opt_pitch,opt_yaw;
float link_roll,link_pitch,link_yaw;
t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
linkT.getEulerAngles(link_roll, link_pitch, link_yaw);
float angularError = uMax3(
force3DoF?0:fabs(opt_roll - link_roll),
force3DoF?0:fabs(opt_pitch - link_pitch),
fabs(opt_yaw - link_yaw));
angularError = angularError>M_PI?2*M_PI-angularError:angularError;
float angularError = 0.0f;
if(force3DoF)
{
float opt_roll,opt_pitch,opt_yaw;
float link_roll,link_pitch,link_yaw;
t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
linkT.getEulerAngles(link_roll, link_pitch, link_yaw);
angularError = fabs(opt_yaw - link_yaw);
angularError = angularError>M_PI?2*M_PI-angularError:angularError;
}
else
{
angularError = t.getAngle(linkT);
}
UASSERT(iter->second.rotVariance(false)>0.0);
float stddevAngular = sqrt(iter->second.rotVariance(false));
float angularErrorRatio = angularError/stddevAngular;
corelib/src/Memory.cpp
+19 -3
View File
@@ -4893,9 +4893,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
else
{
decimationDepth = (int)ceil(float(data.depthRaw().rows) / float(targetSize));
UASSERT(data.depthConfidenceRaw().empty() || data.depthConfidenceRaw().size() == data.depthRaw().size());
}
}
UDEBUG("decimation rgbOrLeft(rows=%d)=%d, depthOrRight(rows=%d)=%d", data.imageRaw().rows, _imagePreDecimation, data.depthOrRightRaw().rows, decimationDepth);
UDEBUG("decimation rgbOrLeft(rows=%d)=%d, depthOrRight(rows=%d)=%d (conf? %d)", data.imageRaw().rows, _imagePreDecimation, data.depthOrRightRaw().rows, decimationDepth, data.depthConfidenceRaw().empty()?0:1);
std::vector<CameraModel> cameraModels = decimatedData.cameraModels();
for(unsigned int i=0; i<cameraModels.size(); ++i)
@@ -4907,6 +4908,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
decimatedData.setRGBDImage(
util2d::decimate(decimatedData.imageRaw(), _imagePreDecimation),
util2d::decimate(decimatedData.depthOrRightRaw(), decimationDepth),
util2d::decimate(decimatedData.depthConfidenceRaw(), decimationDepth),
cameraModels);
}
@@ -5656,6 +5658,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
cv::Mat image = data.imageRaw();
cv::Mat depthOrRightImage = data.depthOrRightRaw();
cv::Mat depthConfidence = data.depthConfidenceRaw();
std::vector<CameraModel> cameraModels = data.cameraModels();
std::vector<StereoCameraModel> stereoCameraModels = data.stereoCameraModels();
@@ -5666,6 +5670,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
{
image = decimatedData.imageRaw();
depthOrRightImage = decimatedData.depthOrRightRaw();
depthConfidence = decimatedData.depthConfidenceRaw();
cameraModels = decimatedData.cameraModels();
stereoCameraModels = decimatedData.stereoCameraModels();
}
@@ -5879,9 +5884,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
Signature * s;
if(this->isBinDataKept() && (!isIntermediateNode || _saveIntermediateNodeData))
{
UDEBUG("Bin data kept: rgb=%d, depth=%d, scan=%d, userData=%d",
UDEBUG("Bin data kept: rgb=%d, depth=%d, conf=%d, scan=%d, userData=%d",
image.empty()?0:1,
depthOrRightImage.empty()?0:1,
depthConfidence.empty()?0:1,
laserScan.isEmpty()?0:1,
data.userDataRaw().empty()?0:1);
@@ -5928,12 +5934,14 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
cv::Mat compressedImage;
cv::Mat compressedDepth;
cv::Mat compressedDepthConfidence;
cv::Mat compressedScan;
cv::Mat compressedUserData;
if(_compressionParallelized)
{
rtabmap::CompressionThread ctImage(image, _rgbCompressionFormat);
rtabmap::CompressionThread ctDepth(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?_depthCompressionFormat:_rgbCompressionFormat);
rtabmap::CompressionThread ctDepthConfidence(depthConfidence);
rtabmap::CompressionThread ctLaserScan(laserScan.data());
rtabmap::CompressionThread ctUserData(data.userDataRaw());
if(!image.empty())
@@ -5944,6 +5952,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
{
ctDepth.start();
}
if(!depthConfidence.empty())
{
ctDepthConfidence.start();
}
if(!laserScan.isEmpty())
{
ctLaserScan.start();
@@ -5954,11 +5966,13 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
}
ctImage.join();
ctDepth.join();
ctDepthConfidence.join();
ctLaserScan.join();
ctUserData.join();
compressedImage = ctImage.getCompressedData();
compressedDepth = ctDepth.getCompressedData();
compressedDepthConfidence = ctDepthConfidence.getCompressedData();
compressedScan = ctLaserScan.getCompressedData();
compressedUserData = ctUserData.getCompressedData();
}
@@ -5966,6 +5980,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
{
compressedImage = compressImage2(image, _rgbCompressionFormat);
compressedDepth = compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?_depthCompressionFormat:_rgbCompressionFormat);
compressedDepthConfidence = compressData2(depthConfidence);
compressedScan = compressData2(laserScan.data());
compressedUserData = compressData2(data.userDataRaw());
}
@@ -6016,6 +6031,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
laserScan.localTransform()),
compressedImage.empty()?data.imageCompressed():compressedImage,
compressedDepth.empty()?data.depthOrRightCompressed():compressedDepth,
compressedDepthConfidence.empty()?data.depthConfidenceCompressed():compressedDepthConfidence,
cameraModels,
id,
0,
@@ -6113,7 +6129,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
// set raw data
if(!cameraModels.empty())
{
s->sensorData().setRGBDImage(image, depthOrRightImage, cameraModels, false);
s->sensorData().setRGBDImage(image, depthOrRightImage, depthConfidence, cameraModels, false);
}
else
{
corelib/src/OdometryInfo.cpp
+2 -3
View File
@@ -191,9 +191,8 @@ std::map<std::string, float> OdometryInfo::statistics(const Transform & pose)
{
if(!transform.isNull())
{
rtabmap::Transform diff = transformGroundTruth.inverse()*transform;
stats.insert(std::make_pair("Odometry/TG_error_lin/m", diff.getNorm()));
stats.insert(std::make_pair("Odometry/TG_error_ang/deg", diff.getAngle()*180.0/CV_PI));
stats.insert(std::make_pair("Odometry/TG_error_lin/m", transformGroundTruth.getDistance(transform)));
stats.insert(std::make_pair("Odometry/TG_error_ang/deg", transformGroundTruth.getAngle(transform)*180.0/CV_PI));
}
transformGroundTruth.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
corelib/src/Rtabmap.cpp
+6 -7
View File
@@ -1701,7 +1701,7 @@ bool Rtabmap::process(
{
distanceToClosestNodeInTheGraph = sqrt(sqrdDistance);
UDEBUG("Last localization pose = %s, closest node=%d (%f m)", newPose.prettyPrint().c_str(), closestNode, distanceToClosestNodeInTheGraph);
angleToClosestNodeInTheGraph = (newPose.inverse() * _optimizedPoses.at(closestNode)).getAngle();
angleToClosestNodeInTheGraph = newPose.getAngle(_optimizedPoses.at(closestNode));
}
}
@@ -3954,7 +3954,7 @@ bool Rtabmap::process(
{
distanceToClosestNodeInTheGraph = _lastLocalizationPose.getDistance(_optimizedPoses.at(closestNode));
UDEBUG("Last localization pose = %s, updated closest node=%d (%f m)", _lastLocalizationPose.prettyPrint().c_str(), closestNode, distanceToClosestNodeInTheGraph);
angleToClosestNodeInTheGraph = (_lastLocalizationPose.inverse() * _optimizedPoses.at(closestNode)).getAngle();
angleToClosestNodeInTheGraph = _lastLocalizationPose.getAngle(_optimizedPoses.at(closestNode));
}
}
_lastLocalizationPose = _optimizedPoses.at(signature->id()); // update
@@ -4103,16 +4103,15 @@ bool Rtabmap::process(
if(!sLoop->getGroundTruthPose().isNull() && !signature->getGroundTruthPose().isNull())
{
Transform transformGT = sLoop->getGroundTruthPose().inverse() * signature->getGroundTruthPose();
Transform error = loopIter->second.transform().inverse() * transformGT;
statistics_.addStatistic(Statistics::kGtLocalization_linear_error(), error.getNorm());
statistics_.addStatistic(Statistics::kGtLocalization_angular_error(), error.getAngle(1,0,0)*180/M_PI);
statistics_.addStatistic(Statistics::kGtLocalization_linear_error(), loopIter->second.transform().getDistance(transformGT));
statistics_.addStatistic(Statistics::kGtLocalization_angular_error(), loopIter->second.transform().getAngle(transformGT)*180/M_PI);
}
}
_distanceTravelledSinceLastLocalization = 0.0f;
statistics_.addStatistic(Statistics::kLoopMapToOdom_norm(), _mapCorrection.getNorm());
statistics_.addStatistic(Statistics::kLoopMapToOdom_angle(), _mapCorrection.getAngle()*180.0f/M_PI);
statistics_.addStatistic(Statistics::kLoopMapToOdom_angle(), _mapCorrection.getAngle(Transform::getIdentity())*180.0f/M_PI);
_mapCorrection.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
statistics_.addStatistic(Statistics::kLoopMapToOdom_x(), x);
statistics_.addStatistic(Statistics::kLoopMapToOdom_y(), y);
@@ -4126,7 +4125,7 @@ bool Rtabmap::process(
{
Transform odomCorrection = (previousMapCorrection*odomPose).inverse()*_mapCorrection*odomPose;
statistics_.addStatistic(Statistics::kLoopOdom_correction_norm(), odomCorrection.getNorm());
statistics_.addStatistic(Statistics::kLoopOdom_correction_angle(), odomCorrection.getAngle()*180.0f/M_PI);
statistics_.addStatistic(Statistics::kLoopOdom_correction_angle(), odomCorrection.getAngle(Transform::getIdentity())*180.0f/M_PI);
odomCorrection.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
statistics_.addStatistic(Statistics::kLoopOdom_correction_x(), x);
statistics_.addStatistic(Statistics::kLoopOdom_correction_y(), y);
corelib/src/SensorData.cpp
+155 -7
View File
@@ -107,6 +107,25 @@ SensorData::SensorData(
setUserData(userData);
}
// RGB-D constructor + confidence + laser scan
SensorData::SensorData(
const LaserScan & laserScan,
const cv::Mat & rgb,
const cv::Mat & depth,
const cv::Mat & depthConfidence,
const CameraModel & cameraModel,
int id,
double stamp,
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_cellSize(0.0f)
{
setRGBDImage(rgb, depth, depthConfidence, cameraModel);
setLaserScan(laserScan);
setUserData(userData);
}
// Multi-cameras RGB-D constructor
SensorData::SensorData(
const cv::Mat & rgb,
@@ -123,6 +142,23 @@ SensorData::SensorData(
setUserData(userData);
}
// Multi-cameras RGB-D constructor + confidence
SensorData::SensorData(
const cv::Mat & rgb,
const cv::Mat & depth,
const cv::Mat & depthConfidence,
const std::vector<CameraModel> & cameraModels,
int id,
double stamp,
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_cellSize(0.0f)
{
setRGBDImage(rgb, depth, depthConfidence, cameraModels);
setUserData(userData);
}
// Multi-cameras RGB-D constructor + laser scan
SensorData::SensorData(
const LaserScan & laserScan,
@@ -141,6 +177,25 @@ SensorData::SensorData(
setUserData(userData);
}
// Multi-cameras RGB-D constructor + confidence + laser scan
SensorData::SensorData(
const LaserScan & laserScan,
const cv::Mat & rgb,
const cv::Mat & depth,
const cv::Mat & depthConfidence,
const std::vector<CameraModel> & cameraModels,
int id,
double stamp,
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_cellSize(0.0f)
{
setRGBDImage(rgb, depth, depthConfidence, cameraModels);
setLaserScan(laserScan);
setUserData(userData);
}
// Stereo constructor
SensorData::SensorData(
const cv::Mat & left,
@@ -234,9 +289,29 @@ void SensorData::setRGBDImage(
models.push_back(model);
setRGBDImage(rgb, depth, models, clearPreviousData);
}
void SensorData::setRGBDImage(
const cv::Mat & rgb,
const cv::Mat & depth,
const cv::Mat & depthConfidence,
const CameraModel & model,
bool clearPreviousData)
{
std::vector<CameraModel> models;
models.push_back(model);
setRGBDImage(rgb, depth, depthConfidence, models, clearPreviousData);
}
void SensorData::setRGBDImage(
const cv::Mat & rgb,
const cv::Mat & depth,
const std::vector<CameraModel> & models,
bool clearPreviousData)
{
setRGBDImage(rgb, depth, cv::Mat(), models, clearPreviousData);
}
void SensorData::setRGBDImage(
const cv::Mat & rgb,
const cv::Mat & depth,
const cv::Mat & depthConfidence,
const std::vector<CameraModel> & models,
bool clearPreviousData)
{
@@ -300,6 +375,30 @@ void SensorData::setRGBDImage(
_depthOrRightRaw = cv::Mat();
_depthOrRightCompressed = cv::Mat();
}
if(depthConfidence.rows == 1)
{
UASSERT(depthConfidence.type() == CV_8UC1); // Bytes
_depthConfidenceCompressed = depthConfidence;
if(clearData)
{
_depthConfidenceRaw = cv::Mat();
}
}
else if(!depthConfidence.empty())
{
UASSERT(depthConfidence.type() == CV_8UC1);
_depthConfidenceRaw = depthConfidence;
if(clearData)
{
_depthConfidenceCompressed = cv::Mat();
}
}
else if(clearData)
{
_depthConfidenceRaw = cv::Mat();
_depthConfidenceCompressed = cv::Mat();
}
}
void SensorData::setStereoImage(
const cv::Mat & left,
@@ -528,7 +627,7 @@ void SensorData::setOccupancyGrid(
void SensorData::uncompressData()
{
cv::Mat tmpA, tmpB, tmpD, tmpE, tmpF, tmpG;
cv::Mat tmpA, tmpB, tmpD, tmpE, tmpF, tmpG, tmpH;
LaserScan tmpC;
uncompressData(_imageCompressed.empty()?0:&tmpA,
_depthOrRightCompressed.empty()?0:&tmpB,
@@ -536,7 +635,8 @@ void SensorData::uncompressData()
_userDataCompressed.empty()?0:&tmpD,
_groundCellsCompressed.empty()?0:&tmpE,
_obstacleCellsCompressed.empty()?0:&tmpF,
_emptyCellsCompressed.empty()?0:&tmpG);
_emptyCellsCompressed.empty()?0:&tmpG,
_depthConfidenceCompressed.empty()?0:&tmpH);
}
void SensorData::uncompressData(
@@ -546,16 +646,27 @@ void SensorData::uncompressData(
cv::Mat * userDataRaw,
cv::Mat * groundCellsRaw,
cv::Mat * obstacleCellsRaw,
cv::Mat * emptyCellsRaw)
cv::Mat * emptyCellsRaw,
cv::Mat * depthConfidenceRaw)
{
UDEBUG("%d data(%d,%d,%d,%d,%d,%d,%d)", this->id(), imageRaw?1:0, depthRaw?1:0, laserScanRaw?1:0, userDataRaw?1:0, groundCellsRaw?1:0, obstacleCellsRaw?1:0, emptyCellsRaw?1:0);
UDEBUG("%d data(%d,%d,%d,%d,%d,%d,%d,%d)",
this->id(),
imageRaw?1:0,
depthRaw?1:0,
laserScanRaw?1:0,
userDataRaw?1:0,
groundCellsRaw?1:0,
obstacleCellsRaw?1:0,
emptyCellsRaw?1:0,
depthConfidenceRaw?1:0);
if(imageRaw == 0 &&
depthRaw == 0 &&
laserScanRaw == 0 &&
userDataRaw == 0 &&
groundCellsRaw == 0 &&
obstacleCellsRaw == 0 &&
emptyCellsRaw == 0)
emptyCellsRaw == 0 &&
depthConfidenceRaw == 0)
{
return;
}
@@ -566,7 +677,8 @@ void SensorData::uncompressData(
userDataRaw,
groundCellsRaw,
obstacleCellsRaw,
emptyCellsRaw);
emptyCellsRaw,
depthConfidenceRaw);
if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
{
@@ -588,6 +700,10 @@ void SensorData::uncompressData(
{
_depthOrRightRaw = *depthRaw;
}
if(depthConfidenceRaw && !depthConfidenceRaw->empty() && _depthConfidenceRaw.empty())
{
_depthConfidenceRaw = *depthConfidenceRaw;
}
if(laserScanRaw && !laserScanRaw->isEmpty() && _laserScanRaw.isEmpty())
{
_laserScanRaw = *laserScanRaw;
@@ -628,7 +744,8 @@ void SensorData::uncompressDataConst(
cv::Mat * userDataRaw,
cv::Mat * groundCellsRaw,
cv::Mat * obstacleCellsRaw,
cv::Mat * emptyCellsRaw) const
cv::Mat * emptyCellsRaw,
cv::Mat * depthConfidenceRaw) const
{
if(imageRaw)
{
@@ -638,6 +755,10 @@ void SensorData::uncompressDataConst(
{
*depthRaw = _depthOrRightRaw;
}
if(depthConfidenceRaw)
{
*depthConfidenceRaw = _depthConfidenceRaw;
}
if(laserScanRaw)
{
*laserScanRaw = _laserScanRaw;
@@ -660,6 +781,7 @@ void SensorData::uncompressDataConst(
}
if( (imageRaw && imageRaw->empty()) ||
(depthRaw && depthRaw->empty()) ||
(depthConfidenceRaw && depthConfidenceRaw->empty()) ||
(laserScanRaw && laserScanRaw->isEmpty()) ||
(userDataRaw && userDataRaw->empty()) ||
(groundCellsRaw && groundCellsRaw->empty()) ||
@@ -668,6 +790,7 @@ void SensorData::uncompressDataConst(
{
rtabmap::CompressionThread ctImage(_imageCompressed, true);
rtabmap::CompressionThread ctDepth(_depthOrRightCompressed, true);
rtabmap::CompressionThread ctDepthConfidence(_depthConfidenceCompressed, false);
rtabmap::CompressionThread ctLaserScan(_laserScanCompressed.data(), false);
rtabmap::CompressionThread ctUserData(_userDataCompressed, false);
rtabmap::CompressionThread ctGroundCells(_groundCellsCompressed, false);
@@ -683,6 +806,11 @@ void SensorData::uncompressDataConst(
UASSERT(_depthOrRightCompressed.type() == CV_8UC1);
ctDepth.start();
}
if(depthConfidenceRaw && depthConfidenceRaw->empty() && !_depthConfidenceCompressed.empty())
{
UASSERT(_depthConfidenceCompressed.type() == CV_8UC1);
ctDepthConfidence.start();
}
if(laserScanRaw && laserScanRaw->isEmpty() && !_laserScanCompressed.isEmpty())
{
UASSERT(_laserScanCompressed.isCompressed());
@@ -710,6 +838,7 @@ void SensorData::uncompressDataConst(
}
ctImage.join();
ctDepth.join();
ctDepthConfidence.join();
ctLaserScan.join();
ctUserData.join();
ctGroundCells.join();
@@ -746,6 +875,21 @@ void SensorData::uncompressDataConst(
}
}
}
if(depthConfidenceRaw && depthConfidenceRaw->empty())
{
*depthConfidenceRaw = ctDepthConfidence.getUncompressedData();
if(depthConfidenceRaw->empty())
{
if(_depthConfidenceCompressed.empty())
{
UWARN("Requested depth confidence data, but the sensor data (%d) doesn't have depth confidence.", this->id());
}
else
{
UERROR("Requested depth confidence data, but failed to uncompress (%d).", this->id());
}
}
}
if(laserScanRaw && laserScanRaw->isEmpty())
{
if(_laserScanCompressed.angleIncrement() > 0.0f)
@@ -815,6 +959,8 @@ unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes
(_imageRaw.empty()?0:_imageRaw.total()*_imageRaw.elemSize()) +
(_depthOrRightCompressed.empty()?0:_depthOrRightCompressed.total()*_depthOrRightCompressed.elemSize()) +
(_depthOrRightRaw.empty()?0:_depthOrRightRaw.total()*_depthOrRightRaw.elemSize()) +
(_depthConfidenceCompressed.empty()?0:_depthConfidenceCompressed.total()*_depthConfidenceCompressed.elemSize()) +
(_depthConfidenceRaw.empty()?0:_depthConfidenceRaw.total()*_depthConfidenceRaw.elemSize()) +
(_userDataCompressed.empty()?0:_userDataCompressed.total()*_userDataCompressed.elemSize()) +
(_userDataRaw.empty()?0:_userDataRaw.total()*_userDataRaw.elemSize()) +
(_laserScanCompressed.empty()?0:_laserScanCompressed.data().total()*_laserScanCompressed.data().elemSize()) +
@@ -836,6 +982,7 @@ void SensorData::clearCompressedData(bool images, bool scan, bool userData)
{
_imageCompressed=cv::Mat();
_depthOrRightCompressed=cv::Mat();
_depthConfidenceCompressed=cv::Mat();
}
if(scan)
{
@@ -852,6 +999,7 @@ void SensorData::clearRawData(bool images, bool scan, bool userData)
{
_imageRaw=cv::Mat();
_depthOrRightRaw=cv::Mat();
_depthConfidenceRaw=cv::Mat();
}
if(scan)
{
corelib/src/Transform.cpp
+2 -4
View File
@@ -273,11 +273,9 @@ void Transform::getTranslation(float & x, float & y, float & z) const
z = this->z();
}
float Transform::getAngle(float x, float y, float z) const
float Transform::getAngle(const Transform & t) const
{
Eigen::Vector3f vA(x,y,z);
Eigen::Vector3f vB = this->toEigen3f().linear()*Eigen::Vector3f(1,0,0);
return pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
return getQuaternionf().angularDistance(t.getQuaternionf());
}
float Transform::getNorm() const
corelib/src/odometry/OdometryDVO.cpp
+1 -1
View File
@@ -217,7 +217,7 @@ Transform OdometryDVO::computeTransform(
t = motionFromKeyFrame_.inverse() * t;
// TODO make parameters?
if(currentMotion.getNorm() > 0.01 || currentMotion.getAngle() > 0.01)
if(currentMotion.getNorm() > 0.01 || currentMotion.getAngle(Transform::getIdentity()) > 0.01)
{
if(info)
{
corelib/src/resources/DatabaseSchema.sql.in
+1
View File
@@ -32,6 +32,7 @@ CREATE TABLE Data (
id INTEGER NOT NULL,
image BLOB, -- compressed image (Grayscale or RGB)
depth BLOB, -- compressed image (Depth or Right image)
depth_confidence BLOB, -- compressed data (low=0 high=100)
calibration BLOB, -- fx, fy, cx, cy, [baseline,] width, height, local_transform
scan BLOB, -- compressed data (Laser scan)
corelib/src/resources/backward_compatibility/DatabaseSchema_0_20_0.sql
+182
View File
@@ -0,0 +1,182 @@
-- *******************************************************************
-- DatabaseSchema: Script for creating the database
-- Usage:
-- $ sqlite3 LTM.db < DatabaseSchema.sql
--
-- *******************************************************************
-- *******************************************************************
-- CLEAN
-- *******************************************************************
/*DROP TABLE Node;*/
-- *******************************************************************
-- CREATE
-- *******************************************************************
CREATE TABLE Node (
id INTEGER NOT NULL,
map_id INTEGER NOT NULL,
weight INTEGER,
stamp FLOAT,
pose BLOB, -- 3x4 float
ground_truth_pose BLOB, -- 3x4 float
velocity BLOB, -- 6 float (vx,vy,vz,vroll,vpitch,vyaw) m/s and rad/s
label TEXT,
gps BLOB, -- 1x6 double: stamp, longitude (DD), latitude (DD), altitude (m), accuracy (m), bearing (North 0->360 deg clockwise)
env_sensors BLOB, -- Variable 3xdouble: (sensorId1, value, stamp, sensorId2, value, stamp, ...)
time_enter DATE,
PRIMARY KEY (id)
);
CREATE TABLE Data (
id INTEGER NOT NULL,
image BLOB, -- compressed image (Grayscale or RGB)
depth BLOB, -- compressed image (Depth or Right image)
calibration BLOB, -- fx, fy, cx, cy, [baseline,] width, height, local_transform
scan BLOB, -- compressed data (Laser scan)
scan_info BLOB, -- scan_max_pts, scan_max_range, scan_format, local_transform
ground_cells BLOB, -- compressed data (occupancy grid)
obstacle_cells BLOB, -- compressed data (occupancy grid)
empty_cells BLOB, -- compressed data (occupancy grid)
cell_size FLOAT,
view_point_x FLOAT,
view_point_y FLOAT,
view_point_z FLOAT,
user_data BLOB, -- compressed data (User data)
time_enter DATE,
PRIMARY KEY (id)
);
CREATE TABLE Link (
from_id INTEGER NOT NULL,
to_id INTEGER NOT NULL,
type INTEGER NOT NULL, -- kNeighbor=0, kGlobalClosure=1, kLocalSpaceClosure=2, kLocalTimeClosure=3, kUserClosure=4, kVirtualClosure=5, kNeighborMerged=6, kPosePrior=7, kLandmark=8
information_matrix BLOB NOT NULL, -- 6x6 double (inverse covariance)
transform BLOB, -- 3x4 float
user_data BLOB, -- compressed data (User data)
FOREIGN KEY (from_id) REFERENCES Node(id),
FOREIGN KEY (to_id) REFERENCES Node(id)
);
--
CREATE TABLE Word (
id INTEGER NOT NULL,
descriptor_size INTEGER NOT NULL,
descriptor BLOB NOT NULL,
time_enter DATE,
PRIMARY KEY (id)
);
CREATE TABLE Feature (
node_id INTEGER NOT NULL,
word_id INTEGER NOT NULL,
pos_x FLOAT NOT NULL,
pos_y FLOAT NOT NULL,
size INTEGER NOT NULL,
dir FLOAT NOT NULL,
response FLOAT NOT NULL,
octave INTEGER NOT NULL,
depth_x FLOAT,
depth_y FLOAT,
depth_z FLOAT,
descriptor_size INTEGER,
descriptor BLOB,
FOREIGN KEY (node_id) REFERENCES Node(id)
);
CREATE TABLE GlobalDescriptor (
node_id INTEGER NOT NULL,
type INTEGER NOT NULL,
info BLOB,
data BLOB NOT NULL,
FOREIGN KEY (node_id) REFERENCES Node(id)
);
--
CREATE TABLE Info (
STM_size INTEGER,
last_sign_added INTEGER,
process_mem_used INTEGER,
database_mem_used INTEGER,
dictionary_size INTEGER,
parameters TEXT,
time_enter DATE
);
CREATE TABLE Statistics (
id INTEGER NOT NULL,
stamp FLOAT,
data BLOB, -- compressed string
wm_state BLOB, -- compressed data
FOREIGN KEY (id) REFERENCES Node(id)
);
CREATE TABLE Admin (
version TEXT,
preview_image BLOB, -- compressed image
opt_cloud BLOB, -- compressed data
opt_ids BLOB, -- Node ids used to generate the optimized cloud/mesh
opt_poses BLOB, -- compressed N*3x4 float
opt_last_localization BLOB, -- 3x4 float
opt_polygons_size INTEGER, -- e.g., 3
opt_polygons BLOB, -- compressed data [length_v0, i0,i1,i3, length_v1, i0,i1,i3]
opt_tex_coords BLOB, -- compressed data [length_v0, u0,v0,u1,v1,u2,v2, length_v1, u0,v0,u1,v1,u2,v2]
opt_tex_materials BLOB, -- compressed image
opt_map BLOB, -- compressed CV_8SC1 occupancy grid
opt_map_x_min FLOAT,
opt_map_y_min FLOAT,
opt_map_resolution FLOAT,
time_enter DATE
);
-- *******************************************************************
-- TRIGGERS
-- *******************************************************************
CREATE TRIGGER insert_Feature BEFORE INSERT ON Feature
WHEN NOT EXISTS (SELECT Node.id FROM Node WHERE Node.id = NEW.node_id)
BEGIN
SELECT RAISE(ABORT, 'Foreign key constraint failed in Feature table');
END;
-- Creating a trigger for time_enter
CREATE TRIGGER insert_Node_timeEnter AFTER INSERT ON Node
BEGIN
UPDATE Node SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid;
END;
CREATE TRIGGER insert_Data_timeEnter AFTER INSERT ON Data
BEGIN
UPDATE Node SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid;
END;
CREATE TRIGGER insert_Word_timeEnter AFTER INSERT ON Word
BEGIN
UPDATE Word SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid;
END;
CREATE TRIGGER insert_Info_timeEnter AFTER INSERT ON Info
BEGIN
UPDATE Info SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid;
END;
-- *******************************************************************
-- INDEXES
-- *******************************************************************
CREATE UNIQUE INDEX IDX_Node_id on Node (id);
CREATE INDEX IDX_Feature_node_id on Feature (node_id);
CREATE INDEX IDX_GlobalDescriptor_node_id on GlobalDescriptor (node_id);
CREATE INDEX IDX_Link_from_id on Link (from_id);
CREATE UNIQUE INDEX IDX_node_label on Node (label);
CREATE UNIQUE INDEX IDX_Statistics_id on Statistics (id);
-- *******************************************************************
-- VERSION
-- *******************************************************************
INSERT INTO Admin(version) VALUES('0.20.0');
corelib/src/superpoint_torch/SuperPoint.cc
+1 -2
View File
@@ -242,8 +242,7 @@ cv::Mat SPDetector::compute(const std::vector<cv::KeyPoint> &keypoints)
auto desc = torch::grid_sampler(desc_, grid, 0, 0, true); // [1, 256, 1, n_keypoints]
// normalize to 1
desc = torch::nn::functional::normalize(desc.reshape({1, desc_.size(1), -1})); //[1, 256, n_keypoints]
desc = desc.squeeze(); //[256, n_keypoints]
desc = torch::nn::functional::normalize(desc.reshape({desc_.size(1), -1}), torch::nn::functional::NormalizeFuncOptions().dim(0)); //[256, n_keypoints]
desc = desc.transpose(0, 1).contiguous(); //[n_keypoints, 256]
if(cuda_)
corelib/src/util2d.cpp
+91 -1
View File
@@ -1351,18 +1351,38 @@ cv::Mat interpolate(const cv::Mat & image, int factor, float depthErrorRatio)
return out;
}
cv::Mat registerDepth(
const cv::Mat & depth,
const cv::Mat & depthK,
const cv::Size & colorSize,
const cv::Mat & colorK,
const rtabmap::Transform & transform)
{
cv::Mat tmp;
return registerDepth(
depth,
cv::Mat(),
depthK,
colorSize,
colorK,
transform,
tmp);
}
cv::Mat registerDepth(
const cv::Mat & depth,
const cv::Mat & confidence,
const cv::Mat & depthK,
const cv::Size & colorSize,
const cv::Mat & colorK,
const rtabmap::Transform & transform)
const rtabmap::Transform & transform,
cv::Mat & registeredConfidence)
{
UASSERT(!transform.isNull());
UASSERT(!depth.empty());
UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1); // mm or m
UASSERT(depthK.type() == CV_64FC1 && depthK.cols == 3 && depthK.cols == 3);
UASSERT(colorK.type() == CV_64FC1 && colorK.cols == 3 && colorK.cols == 3);
UASSERT(confidence.empty() || (confidence.size() == depth.size() && confidence.type()==CV_8UC1));
float fx = depthK.at<double>(0,0);
float fy = depthK.at<double>(1,1);
@@ -1381,10 +1401,19 @@ cv::Mat registerDepth(
Eigen::Vector4f P4,P3;
P4[3] = 1;
cv::Mat registered = cv::Mat::zeros(colorSize, depth.type());
registeredConfidence = cv::Mat();
if(!confidence.empty())
{
registeredConfidence = cv::Mat::zeros(colorSize, confidence.type());
}
bool depthInMM = depth.type() == CV_16UC1;
for(int y=0; y<depth.rows; ++y)
{
const unsigned char * confPtr = 0;
if(!confidence.empty()) {
confPtr = confidence.ptr<unsigned char>(y);
}
for(int x=0; x<depth.cols; ++x)
{
//filtering
@@ -1411,6 +1440,9 @@ cv::Mat registerDepth(
if(zReg == 0 || z16 < zReg)
{
zReg = z16;
if(confPtr) {
registeredConfidence.at<unsigned char>(dy, dx) = confPtr[x];
}
}
}
else
@@ -1419,6 +1451,9 @@ cv::Mat registerDepth(
if(zReg == 0 || z < zReg)
{
zReg = z;
if(confPtr) {
registeredConfidence.at<unsigned char>(dy, dx) = confPtr[x];
}
}
}
}
@@ -1923,6 +1958,61 @@ cv::Mat fastBilateralFiltering(const cv::Mat & depth, float sigmaS, float sigmaR
return output;
}
void depthBleedingFiltering(cv::Mat & depth, float maxDepthError)
{
if(depth.empty())
{
return;
}
UASSERT(depth.type() == CV_32FC1 || depth.type() == CV_16UC1);
// ignore border
depth.row(0).setTo(cv::Scalar(0));
depth.row(depth.rows-1).setTo(cv::Scalar(0));
depth.col(0).setTo(cv::Scalar(0));
depth.col(depth.cols-1).setTo(cv::Scalar(0));
if(depth.type() == CV_32FC1)
{
float * depthPtr = depth.ptr<float>();
for(int v=1; v<depth.rows-1; ++v)
{
for(int u=1; u<depth.cols-1; ++u)
{
int row = depth.cols*v;
float & ref = depthPtr[row + u];
if((fabs(ref - depthPtr[row + u - 1]) > maxDepthError &&
fabs(ref - depthPtr[row + u + 1]) > maxDepthError) ||
(fabs(ref - depthPtr[depth.cols*(v-1) + u]) > maxDepthError &&
fabs(ref - depthPtr[depth.cols*(v+1) + u]) > maxDepthError))
{
ref = 0.0f;
}
}
}
}
else if(depth.type() == CV_16UC1)
{
unsigned short * depthPtr = depth.ptr<unsigned short>();
unsigned short maxDepthErrorMM = (unsigned short)(maxDepthError*1000.0f);
for(int v=1; v<depth.rows-1; ++v)
{
for(int u=1; u<depth.cols-1; ++u)
{
int row = depth.cols*v;
unsigned short & ref = depthPtr[row + u];
if((abs((int)ref - (int)depthPtr[row + u - 1]) > maxDepthErrorMM &&
abs((int)ref - (int)depthPtr[row + u + 1]) > maxDepthErrorMM) ||
(abs((int)ref - (int)depthPtr[depth.cols*(v-1) + u]) > maxDepthErrorMM &&
abs((int)ref - (int)depthPtr[depth.cols*(v+1) + u]) > maxDepthErrorMM))
{
ref = 0;
}
}
}
}
}
/**
* \brief Automatic brightness and contrast optimization with optional histogram clipping
* \param [in]src Input image GRAY or BGR or BGRA
corelib/src/util3d.cpp
+115 -31
View File
@@ -216,12 +216,33 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
return cloudFromDepth(imageDepth, model, decimation, maxDepth, minDepth, validIndices);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
const cv::Mat & imageDepthIn,
const CameraModel & model,
int decimation,
float maxDepth,
float minDepth,
std::vector<int> * validIndices)
{
return cloudFromDepth(
imageDepthIn,
cv::Mat(),
model,
decimation,
maxDepth,
minDepth,
0,
validIndices);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
const cv::Mat & imageDepthIn,
const cv::Mat & imageDepthConfidenceIn,
const CameraModel & model,
int decimation,
float maxDepth,
float minDepth,
unsigned char confidenceThr,
std::vector<int> * validIndices)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -234,8 +255,10 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
UASSERT(model.isValidForProjection());
UASSERT(!imageDepthIn.empty() && (imageDepthIn.type() == CV_16UC1 || imageDepthIn.type() == CV_32FC1));
UASSERT(imageDepthConfidenceIn.empty() || confidenceThr == 0 || (imageDepthConfidenceIn.type() == CV_8UC1 && imageDepthConfidenceIn.size() == imageDepthIn.size()));
cv::Mat imageDepth = imageDepthIn;
cv::Mat imageDepthConfidence = confidenceThr==0?cv::Mat():imageDepthConfidenceIn;
if(model.imageHeight()>0 && model.imageWidth()>0)
{
UASSERT(model.imageHeight() % imageDepthIn.rows == 0 && model.imageWidth() % imageDepthIn.cols == 0);
@@ -262,6 +285,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
{
UDEBUG("Depth interpolation factor=%d", targetSize/imageDepthIn.rows);
imageDepth = util2d::interpolate(imageDepthIn, targetSize/imageDepthIn.rows);
if(!imageDepthConfidence.empty()) {
imageDepthConfidence = util2d::interpolate(imageDepthConfidenceIn, targetSize/imageDepthConfidenceIn.rows);
}
decimation = 1;
}
else if(targetSize == imageDepthIn.rows)
@@ -313,11 +339,13 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
float depthCx = model.cx() * rgbToDepthFactorX;
float depthCy = model.cy() * rgbToDepthFactorY;
UDEBUG("depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d",
UDEBUG("depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) has confidence=%d (thr=%d) decimation=%d",
imageDepth.cols, imageDepth.rows,
model.fx(), model.fy(), model.cx(), model.cy(),
rgbToDepthFactorX,
rgbToDepthFactorY,
imageDepthConfidenceIn.empty()?0:1,
(int)confidenceThr,
decimation);
int oi = 0;
@@ -327,21 +355,21 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
{
pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
if(imageDepthConfidence.empty() || imageDepthConfidence.at<unsigned char>(h,w) >= confidenceThr)
{
pt.x = ptXYZ.x;
pt.y = ptXYZ.y;
pt.z = ptXYZ.z;
if(validIndices)
pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
{
validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
pt.x = ptXYZ.x;
pt.y = ptXYZ.y;
pt.z = ptXYZ.z;
if(validIndices)
{
validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
}
}
}
else
{
pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
}
}
}
@@ -367,13 +395,36 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
return cloudFromDepthRGB(imageRgb, imageDepth, model, decimation, maxDepth, minDepth, validIndices);
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
const cv::Mat & imageRgb,
const cv::Mat & imageDepthIn,
const CameraModel & model,
int decimation,
float maxDepth,
float minDepth,
std::vector<int> * validIndices)
{
return cloudFromDepthRGB(
imageRgb,
imageDepthIn,
cv::Mat(),
model,
decimation,
maxDepth,
minDepth,
0,
validIndices);
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
const cv::Mat & imageRgb,
const cv::Mat & imageDepthIn,
const cv::Mat & imageDepthConfidenceIn,
const CameraModel & model,
int decimation,
float maxDepth,
float minDepth,
unsigned char confidenceThr,
std::vector<int> * validIndices)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
@@ -389,6 +440,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
//UASSERT_MSG(imageRgb.rows % imageDepthIn.rows == 0 && imageRgb.cols % imageDepthIn.cols == 0,
// uFormat("rgb=%dx%d depth=%dx%d", imageRgb.cols, imageRgb.rows, imageDepthIn.cols, imageDepthIn.rows).c_str());
UASSERT(!imageDepthIn.empty() && (imageDepthIn.type() == CV_16UC1 || imageDepthIn.type() == CV_32FC1));
UASSERT(imageDepthConfidenceIn.empty() || confidenceThr==0 || (imageDepthConfidenceIn.type() == CV_8UC1 && imageDepthConfidenceIn.size() == imageDepthIn.size()));
if(decimation < 0)
{
if(imageRgb.rows % decimation != 0 || imageRgb.cols % decimation != 0)
@@ -431,6 +483,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
}
cv::Mat imageDepth = imageDepthIn;
cv::Mat imageDepthConfidence = confidenceThr==0?cv::Mat():imageDepthConfidenceIn;
if(decimation < 0)
{
UDEBUG("Decimation from RGB image (%d)", decimation);
@@ -442,6 +495,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
{
UDEBUG("Depth interpolation factor=%d", targetSize/imageDepthIn.rows);
imageDepth = util2d::interpolate(imageDepthIn, targetSize/imageDepthIn.rows);
if(!imageDepthConfidence.empty()) {
imageDepthConfidence = util2d::interpolate(imageDepthConfidenceIn, targetSize/imageDepthConfidenceIn.rows);
}
decimation = 1;
}
else if(targetSize == imageDepthIn.rows)
@@ -486,12 +542,14 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
float depthCx = model.cx() / rgbToDepthFactorX;
float depthCy = model.cy() / rgbToDepthFactorY;
UDEBUG("rgb=%dx%d depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d",
UDEBUG("rgb=%dx%d depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) has confidence=%d (thr=%d) decimation=%d",
imageRgb.cols, imageRgb.rows,
imageDepth.cols, imageDepth.rows,
model.fx(), model.fy(), model.cx(), model.cy(),
rgbToDepthFactorX,
rgbToDepthFactorY,
imageDepthConfidenceIn.empty()?0:1,
(int)confidenceThr,
decimation);
int oi = 0;
@@ -519,21 +577,21 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
pt.r = v;
}
pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
if (pcl::isFinite(ptXYZ) && ptXYZ.z >= minDepth && (maxDepth <= 0.0f || ptXYZ.z <= maxDepth))
pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
if(imageDepthConfidence.empty() || imageDepthConfidence.at<unsigned char>(h,w) >= confidenceThr)
{
pt.x = ptXYZ.x;
pt.y = ptXYZ.y;
pt.z = ptXYZ.z;
if (validIndices)
pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
if (pcl::isFinite(ptXYZ) && ptXYZ.z >= minDepth && (maxDepth <= 0.0f || ptXYZ.z <= maxDepth))
{
validIndices->at(oi) = (h / decimation)*cloud->width + (w / decimation);
pt.x = ptXYZ.x;
pt.y = ptXYZ.y;
pt.z = ptXYZ.z;
if (validIndices)
{
validIndices->at(oi) = (h / decimation)*cloud->width + (w / decimation);
}
++oi;
}
++oi;
}
else
{
pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
}
}
}
@@ -808,7 +866,8 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
float minDepth,
std::vector<pcl::IndicesPtr> * validIndices,
const ParametersMap & stereoParameters,
const std::vector<float> & roiRatios)
const std::vector<float> & roiRatios,
unsigned char confidenceThr)
{
if(decimation == 0)
{
@@ -821,6 +880,7 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
{
//depth
UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols);
UASSERT(sensorData.depthConfidenceRaw().empty() || confidenceThr==0 || (sensorData.depthConfidenceRaw().type() == CV_8UC1 && sensorData.depthConfidenceRaw().cols == sensorData.depthRaw().cols && sensorData.depthConfidenceRaw().rows == sensorData.depthRaw().rows));
int subImageWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
{
@@ -832,6 +892,10 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
if(sensorData.cameraModels()[i].isValidForProjection())
{
cv::Mat depth = cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows));
cv::Mat depthConfidence;
if(!sensorData.depthConfidenceRaw().empty() && confidenceThr > 0) {
depthConfidence = cv::Mat(sensorData.depthConfidenceRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthConfidenceRaw().rows));
}
CameraModel model = sensorData.cameraModels()[i];
if( roiRatios.size() == 4 &&
(roiRatios[0] > 0.0f ||
@@ -852,6 +916,9 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
roiRgb.height%decimation==0)))
{
depth = cv::Mat(depth, roiDepth);
if(!depthConfidence.empty()) {
depthConfidence = cv::Mat(depthConfidence, roiDepth);
}
if(model.imageWidth() != 0 && model.imageHeight() != 0)
{
model = model.roi(util2d::computeRoi(model.imageSize(), roiRatios));
@@ -880,10 +947,12 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp = util3d::cloudFromDepth(
depth,
depthConfidence,
model,
decimation,
maxDepth,
minDepth,
confidenceThr,
validIndices?validIndices->back().get():0);
if(tmp->size())
@@ -1004,7 +1073,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
float minDepth,
std::vector<int> * validIndices,
const ParametersMap & stereoParameters,
const std::vector<float> & roiRatios)
const std::vector<float> & roiRatios,
unsigned char confidenceThr)
{
std::vector<pcl::IndicesPtr> validIndicesV;
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds = cloudsFromSensorData(
@@ -1014,7 +1084,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
minDepth,
validIndices?&validIndicesV:0,
stereoParameters,
roiRatios);
roiRatios,
confidenceThr);
if(validIndices)
{
@@ -1057,7 +1128,8 @@ std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloudsRGBFromSensorData(
float minDepth,
std::vector<pcl::IndicesPtr> * validIndices,
const ParametersMap & stereoParameters,
const std::vector<float> & roiRatios)
const std::vector<float> & roiRatios,
unsigned char confidenceThr)
{
if(decimation == 0)
{
@@ -1076,6 +1148,7 @@ std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloudsRGBFromSensorData(
//UASSERT_MSG(sensorData.imageRaw().rows % sensorData.depthRaw().rows == 0, uFormat("rgb=%d depth=%d", sensorData.imageRaw().rows, sensorData.depthRaw().rows).c_str());
int subRGBWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size();
int subDepthWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
UASSERT(sensorData.depthConfidenceRaw().empty() || confidenceThr==0 || (sensorData.depthConfidenceRaw().type() == CV_8UC1 && sensorData.depthConfidenceRaw().cols == sensorData.depthRaw().cols && sensorData.depthConfidenceRaw().rows == sensorData.depthRaw().rows));
for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
{
@@ -1088,6 +1161,10 @@ std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloudsRGBFromSensorData(
{
cv::Mat rgb(sensorData.imageRaw(), cv::Rect(subRGBWidth*i, 0, subRGBWidth, sensorData.imageRaw().rows));
cv::Mat depth(sensorData.depthRaw(), cv::Rect(subDepthWidth*i, 0, subDepthWidth, sensorData.depthRaw().rows));
cv::Mat depthConfidence;
if(!sensorData.depthConfidenceRaw().empty() && confidenceThr>0) {
depthConfidence = cv::Mat(sensorData.depthConfidenceRaw(), cv::Rect(subDepthWidth*i, 0, subDepthWidth, sensorData.depthConfidenceRaw().rows));
}
CameraModel model = sensorData.cameraModels()[i];
if( roiRatios.size() == 4 &&
((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||
@@ -1103,6 +1180,9 @@ std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloudsRGBFromSensorData(
roiRgb.height%decimation==0)
{
depth = cv::Mat(depth, roiDepth);
if(!depthConfidence.empty()) {
depthConfidence = cv::Mat(depthConfidence, roiDepth);
}
rgb = cv::Mat(rgb, roiRgb);
model = model.roi(roiRgb);
}
@@ -1126,10 +1206,12 @@ std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloudsRGBFromSensorData(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudFromDepthRGB(
rgb,
depth,
depthConfidence,
model,
decimation,
maxDepth,
minDepth,
confidenceThr,
validIndices?validIndices->back().get():0);
if(tmp->size())
@@ -1232,7 +1314,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
float minDepth,
std::vector<int> * validIndices,
const ParametersMap & stereoParameters,
const std::vector<float> & roiRatios)
const std::vector<float> & roiRatios,
unsigned char confidenceThr)
{
std::vector<pcl::IndicesPtr> validIndicesV;
std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds = cloudsRGBFromSensorData(
@@ -1242,7 +1325,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
minDepth,
validIndices?&validIndicesV:0,
stereoParameters,
roiRatios);
roiRatios,
confidenceThr);
if(validIndices)
{