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

CameraThread: Added stereo to depth option. Added parameter "Mem/SaveDepth16Format".

Browse Source
This commit is contained in:
matlabbe
2015-08-27 17:16:12 -04:00
parent 0651d5dfbd
commit ce2bbd8feb
16 changed files with 343 additions and 230 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/CameraThread.h
+2
View File
@@ -52,6 +52,7 @@ public:
void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
//getters
bool isPaused() const {return !this->isRunning();}
@@ -68,6 +69,7 @@ private:
Camera * _camera;
bool _mirroring;
bool _colorOnly;
bool _stereoToDepth;
};
} // namespace rtabmap
corelib/include/rtabmap/core/Memory.h
+1
View File
@@ -232,6 +232,7 @@ private:
float _similarityThreshold;
bool _rawDataKept;
bool _binDataKept;
bool _saveDepth16Format;
bool _notLinkedNodesKeptInDb;
bool _incrementalMemory;
int _maxStMemSize;
corelib/include/rtabmap/core/Parameters.h
+1
View File
@@ -186,6 +186,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Mem, RehearsalSimilarity, float, 0.6, "Rehearsal similarity.");
RTABMAP_PARAM(Mem, ImageKept, bool, false, "Keep raw images in RAM.");
RTABMAP_PARAM(Mem, BinDataKept, bool, true, "Keep binary data in db.");
RTABMAP_PARAM(Mem, SaveDepth16Format, bool, true, "Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
RTABMAP_PARAM(Mem, NotLinkedNodesKept, bool, true, "Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
RTABMAP_PARAM(Mem, STMSize, unsigned int, 10, "Short-term memory size.");
RTABMAP_PARAM(Mem, IncrementalMemory, bool, true, "SLAM mode, otherwise it is Localization mode.");
corelib/include/rtabmap/core/util2d.h
+9 -1
View File
@@ -41,7 +41,8 @@ namespace util2d
cv::Mat RTABMAP_EXP disparityFromStereoImages(
const cv::Mat & leftImage,
const cv::Mat & rightImage);
const cv::Mat & rightImage,
int type = CV_32FC1); // CV_32FC1 or CV_16SC1
cv::Mat RTABMAP_EXP disparityFromStereoImages(
const cv::Mat & leftImage,
@@ -53,6 +54,10 @@ cv::Mat RTABMAP_EXP disparityFromStereoImages(
double flowEps = 0.02,
float maxCorrespondencesSlope = 0.1f);
cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
float fx, float baseline,
int type = CV_32FC1); // CV_32FC1 or CV_16UC1
cv::Mat RTABMAP_EXP depthFromStereoImages(
const cv::Mat & leftImage,
const cv::Mat & rightImage,
@@ -78,6 +83,9 @@ cv::Mat RTABMAP_EXP depthFromStereoCorrespondences(
const std::vector<unsigned char> & mask,
float fx, float baseline);
cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F);
cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U);
float RTABMAP_EXP getDepth(
const cv::Mat & depthImage,
float x, float y,
corelib/include/rtabmap/core/util3d.h
-7
View File
@@ -127,9 +127,6 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
float maxDepth = 0,
const Transform & localTransform = Transform::getIdentity());
cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F);
cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U);
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan);
@@ -147,10 +144,6 @@ pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
const cv::Mat & disparity,
float cx, float cy, float fx, float baseline);
cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
float fx, float baseline,
int type = CV_32FC1);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(
const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP concatenateClouds(
corelib/src/CameraThread.cpp
+14 -1
View File
@@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/CameraEvent.h"
#include "rtabmap/core/CameraRGBD.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/util3d.h"
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/ULogger.h>
@@ -40,7 +42,8 @@ namespace rtabmap
CameraThread::CameraThread(Camera * camera) :
_camera(camera),
_mirroring(false),
_colorOnly(false)
_colorOnly(false),
_stereoToDepth(false)
{
UASSERT(_camera != 0);
}
@@ -96,6 +99,16 @@ void CameraThread::mainLoop()
data.setDepthOrRightRaw(tmpDepth);
}
}
if(_stereoToDepth && data.stereoCameraModel().isValid() && !data.rightRaw().empty())
{
cv::Mat depth = util2d::depthFromDisparity(
util2d::disparityFromStereoImages(data.imageRaw(), data.rightRaw()),
data.stereoCameraModel().left().fx(),
data.stereoCameraModel().baseline());
data.setCameraModel(data.stereoCameraModel().left());
data.setDepthOrRightRaw(depth);
data.setStereoCameraModel(StereoCameraModel());
}
this->post(new CameraEvent(data, _camera->getSerial()));
}
corelib/src/Compression.cpp
+18 -1
View File
@@ -88,7 +88,16 @@ std::vector<unsigned char> compressImage(const cv::Mat & image, const std::strin
std::vector<unsigned char> bytes;
if(!image.empty())
{
cv::imencode(format, image, bytes);
if(image.type() == CV_32FC1)
{
//save in 8bits-4channel
cv::Mat bgra(image.size(), CV_8UC4, image.data);
cv::imencode(format, bgra, bytes);
}
else
{
cv::imencode(format, image, bytes);
}
}
return bytes;
}
@@ -114,6 +123,10 @@ cv::Mat uncompressImage(const cv::Mat & bytes)
#else
image = cv::imdecode(bytes, -1);
#endif
if(image.type() == CV_8UC4)
{
image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
}
}
return image;
}
@@ -128,6 +141,10 @@ cv::Mat uncompressImage(const std::vector<unsigned char> & bytes)
#else
image = cv::imdecode(bytes, -1);
#endif
if(image.type() == CV_8UC4)
{
image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
}
}
return image;
}
corelib/src/Memory.cpp
+5 -4
View File
@@ -68,6 +68,7 @@ Memory::Memory(const ParametersMap & parameters) :
_similarityThreshold(Parameters::defaultMemRehearsalSimilarity()),
_rawDataKept(Parameters::defaultMemImageKept()),
_binDataKept(Parameters::defaultMemBinDataKept()),
_saveDepth16Format(Parameters::defaultMemSaveDepth16Format()),
_notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()),
_incrementalMemory(Parameters::defaultMemIncrementalMemory()),
_maxStMemSize(Parameters::defaultMemSTMSize()),
@@ -399,6 +400,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kMemImageKept(), _rawDataKept);
Parameters::parse(parameters, Parameters::kMemBinDataKept(), _binDataKept);
Parameters::parse(parameters, Parameters::kMemSaveDepth16Format(), _saveDepth16Format);
Parameters::parse(parameters, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb);
Parameters::parse(parameters, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
Parameters::parse(parameters, Parameters::kMemGenerateIds(), _generateIds);
@@ -4257,10 +4259,10 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
std::vector<unsigned char> imageBytes;
std::vector<unsigned char> depthBytes;
if(!depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1)
if(_saveDepth16Format && !depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1)
{
UWARN("Keeping raw data in database: depth type is 32FC1, use 16UC1 depth format to avoid a conversion.");
depthOrRightImage = util3d::cvtDepthFromFloat(depthOrRightImage);
UWARN("Save depth data to 16 bits format: depth type detected is 32FC1, use 16UC1 depth format to avoid this conversion.");
depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage);
}
rtabmap::CompressionThread ctImage(image, std::string(".jpg"));
@@ -4347,7 +4349,6 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
s->sensorData().setUserDataRaw(data.userDataRaw());
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f);
UDEBUG("time compressing data (id=%d) %fs", id, t);
corelib/src/util2d.cpp
+107 -10
View File
@@ -42,12 +42,13 @@ namespace util2d
cv::Mat disparityFromStereoImages(
const cv::Mat & leftImage,
const cv::Mat & rightImage)
const cv::Mat & rightImage,
int type)
{
UASSERT(!leftImage.empty() && !rightImage.empty() &&
(leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1 &&
leftImage.cols == rightImage.cols &&
leftImage.rows == rightImage.rows);
UASSERT(!leftImage.empty() && !rightImage.empty());
UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows);
UASSERT((leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1);
UASSERT(type == CV_32FC1 || type == CV_16SC1);
cv::Mat leftMono;
if(leftImage.channels() == 3)
@@ -70,7 +71,7 @@ cv::Mat disparityFromStereoImages(
stereo.state->textureThreshold = 10;
stereo.state->speckleWindowSize = 100;
stereo.state->speckleRange = 4;
stereo(leftMono, rightImage, disparity, CV_16SC1);
stereo(leftMono, rightImage, disparity, type);
#else
cv::Ptr<cv::StereoBM> stereo = cv::StereoBM::create();
stereo->setBlockSize(15);
@@ -97,10 +98,9 @@ cv::Mat disparityFromStereoImages(
double flowEps,
float maxCorrespondencesSlope)
{
UASSERT(!leftImage.empty() && !rightImage.empty() &&
leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
leftImage.cols == rightImage.cols &&
leftImage.rows == rightImage.rows);
UASSERT(!leftImage.empty() && !rightImage.empty());
UASSERT(leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1);
UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows);
// Find features in the new left image
std::vector<unsigned char> status;
@@ -122,6 +122,51 @@ cv::Mat disparityFromStereoImages(
return disparityFromStereoCorrespondences(leftImage, leftCorners, rightCorners, status, maxCorrespondencesSlope);
}
cv::Mat depthFromDisparity(const cv::Mat & disparity,
float fx, float baseline,
int type)
{
UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
UASSERT(type == CV_32FC1 || type == CV_16UC1);
cv::Mat depth = cv::Mat::zeros(disparity.rows, disparity.cols, type);
int countOverMax = 0;
for (int i = 0; i < disparity.rows; i++)
{
for (int j = 0; j < disparity.cols; j++)
{
float disparity_value = disparity.type() == CV_16SC1?float(disparity.at<short>(i,j))/16.0f:disparity.at<float>(i,j);
if (disparity_value > 0.0f)
{
// baseline * focal / disparity
float d = baseline * fx / disparity_value;
if(d>0)
{
if(depth.type() == CV_32FC1)
{
depth.at<float>(i,j) = d;
}
else
{
if(d*1000.0f <= (float)USHRT_MAX)
{
depth.at<unsigned short>(i,j) = (unsigned short)(d*1000.0f);
}
else
{
++countOverMax;
}
}
}
}
}
}
if(countOverMax)
{
UWARN("Depth conversion error, %d depth values ignored because they are over the maximum depth allowed (65535 mm).", countOverMax);
}
return depth;
}
cv::Mat depthFromStereoImages(
const cv::Mat & leftImage,
const cv::Mat & rightImage,
@@ -209,6 +254,58 @@ cv::Mat depthFromStereoCorrespondences(
return depth;
}
cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
{
UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1);
cv::Mat depth16U;
if(!depth32F.empty())
{
depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1);
int countOverMax = 0;
for(int i=0; i<depth32F.rows; ++i)
{
for(int j=0; j<depth32F.cols; ++j)
{
float depth = (depth32F.at<float>(i,j)*1000.0f);
unsigned short depthMM = 0;
if(depth > 0 && depth <= (float)USHRT_MAX)
{
depthMM = (unsigned short)depth;
}
else if(depth > (float)USHRT_MAX)
{
++countOverMax;
}
depth16U.at<unsigned short>(i, j) = depthMM;
}
}
if(countOverMax)
{
UWARN("Depth conversion error, %d depth values ignored because they are over the maximum depth allowed (65535 mm).", countOverMax);
}
}
return depth16U;
}
cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
{
UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1);
cv::Mat depth32F;
if(!depth16U.empty())
{
depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1);
for(int i=0; i<depth16U.rows; ++i)
{
for(int j=0; j<depth16U.cols; ++j)
{
float depth = float(depth16U.at<unsigned short>(i,j))/1000.0f;
depth32F.at<float>(i, j) = depth;
}
}
}
return depth32F;
}
float getDepth(
const cv::Mat & depthImage,
float x, float y,
corelib/src/util3d.cpp
+93 -166
View File
@@ -286,6 +286,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
float fx, float fy,
int decimation)
{
UDEBUG("");
UASSERT(imageRgb.rows == imageDepth.rows && imageRgb.cols == imageDepth.cols);
UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
UASSERT_MSG(imageDepth.rows % decimation == 0, uFormat("imageDepth.rows=%d decimation=%d", imageDepth.rows, decimation).c_str());
@@ -504,14 +505,13 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
float voxelSize,
int samples)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
{
//depth
UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols);
int subImageWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
{
if(sensorData.cameraModels()[i].isValid())
@@ -627,117 +627,118 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
float voxelSize,
int samples)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
UASSERT(!sensorData.imageRaw().empty());
UASSERT((!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) ||
(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid()));
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
if(!sensorData.imageRaw().empty())
if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
{
if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
//depth
UDEBUG("");
UASSERT(int((sensorData.imageRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.imageRaw().cols);
UASSERT(sensorData.depthRaw().size() == sensorData.imageRaw().size());
int subImageWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size();
for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
{
//depth
UASSERT(int((sensorData.imageRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.imageRaw().cols);
UASSERT(sensorData.depthRaw().size() == sensorData.imageRaw().size());
int subImageWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size();
cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
if(sensorData.cameraModels()[i].isValid())
{
if(sensorData.cameraModels()[i].isValid())
if(subImageWidth % decimation != 0 || sensorData.depthRaw().rows % decimation != 0)
{
if(subImageWidth % decimation != 0 || sensorData.depthRaw().rows % decimation != 0)
UWARN("Image size (%d,%d) modulus decimation (%d) is not null "
"for the cloud creation! Setting decimation to 1...",
subImageWidth, sensorData.depthRaw().rows, decimation);
decimation = 1;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudFromDepthRGB(
cv::Mat(sensorData.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.imageRaw().rows)),
cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)),
sensorData.cameraModels()[i].cx(),
sensorData.cameraModels()[i].cy(),
sensorData.cameraModels()[i].fx(),
sensorData.cameraModels()[i].fy(),
decimation);
if(tmp->size())
{
bool filtered = false;
if(tmp->size() && maxDepth)
{
UWARN("Image size (%d,%d) modulus decimation (%d) is not null "
"for the cloud creation! Setting decimation to 1...",
subImageWidth, sensorData.depthRaw().rows, decimation);
decimation = 1;
tmp = util3d::passThrough(tmp, "z", 0, maxDepth);
filtered = true;
}
if(tmp->size() && voxelSize)
{
tmp = util3d::voxelize(tmp, voxelSize);
filtered = true;
}
if(tmp->size() && samples)
{
tmp = util3d::sampling(tmp, samples);
filtered = true;
}
if(tmp->size() && !filtered)
{
tmp = util3d::removeNaNFromPointCloud(tmp);
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudFromDepthRGB(
cv::Mat(sensorData.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.imageRaw().rows)),
cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)),
sensorData.cameraModels()[i].cx(),
sensorData.cameraModels()[i].cy(),
sensorData.cameraModels()[i].fx(),
sensorData.cameraModels()[i].fy(),
decimation);
if(tmp->size())
{
bool filtered = false;
if(tmp->size() && maxDepth)
{
tmp = util3d::passThrough(tmp, "z", 0, maxDepth);
filtered = true;
}
if(tmp->size() && voxelSize)
{
tmp = util3d::voxelize(tmp, voxelSize);
filtered = true;
}
if(tmp->size() && samples)
{
tmp = util3d::sampling(tmp, samples);
filtered = true;
}
if(tmp->size() && !filtered)
{
tmp = util3d::removeNaNFromPointCloud(tmp);
}
if(tmp->size())
{
tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
}
*cloud += *tmp;
tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
}
*cloud += *tmp;
}
else
{
UERROR("Camera model %d is invalid", i);
}
}
else
{
UERROR("Camera model %d is invalid", i);
}
}
if(cloud->size() && voxelSize)
{
cloud = util3d::voxelize(cloud, voxelSize);
}
}
else if(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid())
{
//stereo
UDEBUG("");
cloud = cloudFromStereoImages(sensorData.imageRaw(),
sensorData.rightRaw(),
sensorData.stereoCameraModel().left().cx(),
sensorData.stereoCameraModel().left().cy(),
sensorData.stereoCameraModel().left().fx(),
sensorData.stereoCameraModel().baseline(),
decimation);
if(cloud->size())
{
bool filtered = false;
if(cloud->size() && maxDepth)
{
cloud = util3d::passThrough(cloud, "z", 0, maxDepth);
filtered = true;
}
if(cloud->size() && voxelSize)
{
cloud = util3d::voxelize(cloud, voxelSize);
filtered = true;
}
if(cloud->size() && !filtered)
{
cloud = util3d::removeNaNFromPointCloud(cloud);
}
}
else if(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid())
{
//stereo
cloud = cloudFromStereoImages(sensorData.imageRaw(),
sensorData.rightRaw(),
sensorData.stereoCameraModel().left().cx(),
sensorData.stereoCameraModel().left().cy(),
sensorData.stereoCameraModel().left().fx(),
sensorData.stereoCameraModel().baseline(),
decimation);
if(cloud->size())
{
bool filtered = false;
if(cloud->size() && maxDepth)
{
cloud = util3d::passThrough(cloud, "z", 0, maxDepth);
filtered = true;
}
if(cloud->size() && voxelSize)
{
cloud = util3d::voxelize(cloud, voxelSize);
filtered = true;
}
if(cloud->size() && !filtered)
{
cloud = util3d::removeNaNFromPointCloud(cloud);
}
if(cloud->size())
{
cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
}
cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
}
}
}
@@ -779,50 +780,6 @@ pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
return scan;
}
cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
{
UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1);
cv::Mat depth16U;
if(!depth32F.empty())
{
depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1);
for(int i=0; i<depth32F.rows; ++i)
{
for(int j=0; j<depth32F.cols; ++j)
{
float depth = (depth32F.at<float>(i,j)*1000.0f);
unsigned short depthMM = 0;
if(depth <= (float)USHRT_MAX)
{
depthMM = (unsigned short)depth;
}
depth16U.at<unsigned short>(i, j) = depthMM;
}
}
}
return depth16U;
}
cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
{
UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1);
cv::Mat depth32F;
if(!depth16U.empty())
{
depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1);
for(int i=0; i<depth16U.rows; ++i)
{
for(int j=0; j<depth16U.cols; ++j)
{
float depth = float(depth16U.at<unsigned short>(i,j))/1000.0f;
depth32F.at<float>(i, j) = depth;
}
}
}
return depth32F;
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
@@ -914,36 +871,6 @@ pcl::PointXYZ projectDisparityTo3D(
return pcl::PointXYZ(bad_point, bad_point, bad_point);
}
cv::Mat depthFromDisparity(const cv::Mat & disparity,
float fx, float baseline,
int type)
{
UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
UASSERT(type == CV_32FC1 || type == CV_16U);
cv::Mat depth = cv::Mat::zeros(disparity.rows, disparity.cols, type);
for (int i = 0; i < disparity.rows; i++)
{
for (int j = 0; j < disparity.cols; j++)
{
float disparity_value = disparity.type() == CV_16SC1?float(disparity.at<short>(i,j))/16.0f:disparity.at<float>(i,j);
if (disparity_value > 0.0f)
{
// baseline * focal / disparity
float d = baseline * fx / disparity_value;
if(depth.type() == CV_32FC1)
{
depth.at<float>(i,j) = d;
}
else
{
depth.at<unsigned short>(i,j) = (unsigned short)(d*1000.0f);
}
}
}
}
return depth;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
guilib/include/rtabmap/gui/PreferencesDialog.h
+1
View File
@@ -189,6 +189,7 @@ public:
int getSourceDatabaseStartPos() const; //Database group
bool getSourceDatabaseStampsUsed() const;//Database group
bool isSourceRGBDColorOnly() const;
bool isSourceStereoDepthGenerated() const;
Transform getSourceLocalTransform() const; //Openni group
Camera * createCamera(bool useRawImages = false); // return camera should be deleted if not null
guilib/src/MainWindow.cpp
+1
View File
@@ -2859,6 +2859,7 @@ void MainWindow::startDetection()
_camera = new CameraThread(camera);
_camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
_camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
_camera->setStereoToDepth(_preferencesDialog->isSourceStereoDepthGenerated());
//Create odometry thread if rgbd slam
if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str()))
guilib/src/PreferencesDialog.cpp
+11
View File
@@ -392,6 +392,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->checkBox_stereoVideo_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkbox_stereo_depthGenerated, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate()));
connect(_ui->pushButton_calibrate_simple, SIGNAL(clicked()), this, SLOT(calibrateSimple()));
connect(_ui->toolButton_openniOniPath, SIGNAL(clicked()), this, SLOT(selectSourceOniPath()));
@@ -444,6 +445,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
// Memory
_ui->general_checkBox_keepRawData->setObjectName(Parameters::kMemImageKept().c_str());
_ui->general_checkBox_keepBinaryData->setObjectName(Parameters::kMemBinDataKept().c_str());
_ui->general_checkBox_saveDepth16bits->setObjectName(Parameters::kMemSaveDepth16Format().c_str());
_ui->general_checkBox_keepNotLinkedNodes->setObjectName(Parameters::kMemNotLinkedNodesKept().c_str());
_ui->general_spinBox_maxStMemSize->setObjectName(Parameters::kMemSTMSize().c_str());
_ui->doubleSpinBox_similarityThreshold->setObjectName(Parameters::kMemRehearsalSimilarity().c_str());
@@ -1105,6 +1107,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
}
_ui->checkbox_rgbd_colorOnly->setChecked(false);
_ui->checkbox_stereo_depthGenerated->setChecked(false);
_ui->openni2_autoWhiteBalance->setChecked(true);
_ui->openni2_autoExposure->setChecked(true);
_ui->openni2_exposure->setValue(0);
@@ -1367,6 +1370,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
settings.beginGroup("stereo");
_ui->comboBox_cameraStereo->setCurrentIndex(settings.value("driver", _ui->comboBox_cameraStereo->currentIndex()).toInt());
_ui->checkbox_stereo_depthGenerated->setChecked(settings.value("depthGenerated", _ui->checkbox_stereo_depthGenerated->isChecked()).toBool());
settings.endGroup(); // stereo
settings.beginGroup("rgb");
@@ -1678,6 +1682,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.beginGroup("stereo");
settings.setValue("driver", _ui->comboBox_cameraStereo->currentIndex());
settings.setValue("depthGenerated", _ui->checkbox_stereo_depthGenerated->isChecked());
settings.endGroup(); // stereo
settings.beginGroup("rgb");
@@ -3447,6 +3452,10 @@ bool PreferencesDialog::isSourceRGBDColorOnly() const
{
return _ui->checkbox_rgbd_colorOnly->isChecked();
}
bool PreferencesDialog::isSourceStereoDepthGenerated() const
{
return _ui->checkbox_stereo_depthGenerated->isChecked();
}
Camera * PreferencesDialog::createCamera(bool useRawImages)
{
@@ -3826,6 +3835,7 @@ void PreferencesDialog::testOdometry(int type)
CameraThread cameraThread(camera); // take ownership of camera
cameraThread.setMirroringEnabled(isSourceMirroring());
cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked());
cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked());
UEventsManager::createPipe(&cameraThread, &odomThread, "CameraEvent");
UEventsManager::createPipe(&odomThread, odomViewer, "OdometryEvent");
UEventsManager::createPipe(odomViewer, &odomThread, "OdometryResetEvent");
@@ -3892,6 +3902,7 @@ void PreferencesDialog::testCamera()
CameraThread cameraThread(camera);
cameraThread.setMirroringEnabled(isSourceMirroring());
cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked());
cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked());
UEventsManager::createPipe(&cameraThread, window, "CameraEvent");
cameraThread.start();
guilib/src/ui/preferencesDialog.ui
+78 -38
View File
@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-542</y>
<y>-392</y>
<width>755</width>
<height>1591</height>
</rect>
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>1</number>
<number>3</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -1769,7 +1769,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item>
<widget class="QStackedWidget" name="stackedWidget_src">
<property name="currentIndex">
<number>0</number>
<number>1</number>
</property>
<widget class="QWidget" name="page_41">
<layout class="QVBoxLayout" name="verticalLayout_64">
@@ -2387,6 +2387,23 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QCheckBox" name="checkbox_stereo_depthGenerated">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QLabel" name="label_247">
<property name="text">
<string>Generate disparity image and convert it to depth. The resulting output is a RGB-D image instead of stereo images.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
<item>
@@ -3880,7 +3897,45 @@ 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="8" column="0">
<item row="4" column="1">
<widget class="QLabel" name="label_retrieved_2">
<property name="text">
<string>True=Generate location Ids, False=use input image ids.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QDoubleSpinBox" name="general_doubleSpinBox_laserScanVoxel">
<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="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QSpinBox" name="spinBox_imageDecimation">
<property name="minimum">
<number>1</number>
@@ -3997,19 +4052,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_retrieved_2">
<property name="text">
<string>True=Generate location Ids, False=use input image ids.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QCheckBox" name="general_checkBox_badSignaturesIgnored">
<property name="text">
@@ -4066,7 +4108,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="8" column="1">
<item row="9" column="1">
<widget class="QLabel" name="label_retrieved_6">
<property name="text">
<string>Image decimation. This feature can be used to save images in lower resolution (size/decimation).</string>
@@ -4102,7 +4144,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="9" column="1">
<item row="10" column="1">
<widget class="QLabel" name="label_retrieved_8">
<property name="text">
<string>If &gt; 0.0, voxelize laser scans when creating a location. This feature can be used to save laser scans already voxelized.</string>
@@ -4115,28 +4157,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QDoubleSpinBox" name="general_doubleSpinBox_laserScanVoxel">
<property name="minimumSize">
<size>
<width>50</width>
<height>0</height>
</size>
<item row="8" column="1">
<widget class="QLabel" name="label_retrieved_9">
<property name="text">
<string>Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).</string>
</property>
<property name="suffix">
<string> m</string>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="decimals">
<number>3</number>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</widget>
</item>
<item row="8" column="0">
<widget class="QCheckBox" name="general_checkBox_saveDepth16bits">
<property name="text">
<string/>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
@@ -6298,7 +6338,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item>
<widget class="QLabel" name="label_83">
<property name="text">
<string>Rigid transformations between nodes are saved on the neighbor links of the RTAB-Map's graph. On loop closures, a new constraint is added to the graph and TORO optimizes the graph. RGB-D images must be sent to work (see Source-&gt;RGB-D Camera).</string>
<string>Rigid transformations between nodes are saved on the neighbor links of the RTAB-Map's graph. On loop closures, a new constraint is added to the graph and TORO optimizes the graph.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
tools/CameraRGBD/main.cpp
+2 -1
View File
@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/CameraRGBD.h"
#include "rtabmap/core/CameraStereo.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/utilite/ULogger.h"
@@ -322,7 +323,7 @@ int main(int argc, char * argv[])
cv::Mat depth = data.depthRaw();
if(depth.type() == CV_32FC1)
{
depth = rtabmap::util3d::cvtDepthFromFloat(depth);
depth = rtabmap::util2d::cvtDepthFromFloat(depth);
}
if(rgb.cols == depth.cols && rgb.rows == depth.rows &&
tools/OdometryViewer/main.cpp
-1
View File
@@ -807,7 +807,6 @@ int main (int argc, char * argv[])
if(camera->isCalibrated())
{
rtabmap::CameraThread cameraThread(camera);
cameraThread.setColorOnly(true);
odomThread.start();
cameraThread.start();