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

Refactored Odometry class:

Browse Source 
-new odometry parameters
-new Optical flow strategy
-Stereo data support 
Refactored SensorData class to support stereo images
Updated default parameters (mostly odometry ones)
util3d: new methods to handle/reconstruct 3D clouds from disparity image / stereo images
PreferencesDialog: added Odometry/BOW and Odometry/OpticalFLow panels.
DatabaseViewer: fixed a crash when database is empty. Added cloud reconstruction of stereo images if saved in database
Camera: added 1 second delay to avoid dark images at the starting


git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1849 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-13 19:10:22 +00:00
parent a3f7415821
commit 8b67633b34
25 changed files with 2108 additions and 498 deletions
Download Patch File Download Diff File Expand all files Collapse all files
guilib/src/DatabaseViewer.cpp
+113 -20
View File
@@ -139,7 +139,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
connect(ui_->checkBox_initGuess, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
ui_->constraintsViewer->setCameraLockZ(false);
ui_->constraintsViewer->updateCameraPosition(Transform::getIdentity());
}
DatabaseViewer::~DatabaseViewer()
@@ -470,9 +470,11 @@ void DatabaseViewer::updateIds()
ui_->horizontalSlider_neighbors->setEnabled(false);
}
updateLoopClosuresSlider();
updateGraphView();
if(ids_.size())
{
updateLoopClosuresSlider();
updateGraphView();
}
}
void DatabaseViewer::generateGraph()
@@ -769,12 +771,35 @@ void DatabaseViewer::view3DMap()
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cv::Mat imageMat = rtabmap::util3d::uncompressImage(image);
cv::Mat depthMat = rtabmap::util3d::uncompressImage(depth);
cloud = rtabmap::util3d::cloudFromDepthRGB(
UASSERT(imageMat.empty() || imageMat.type()==CV_8UC3 || imageMat.type() == CV_8UC1);
UASSERT(depthMat.empty() || depthMat.type()==CV_8UC1 || depthMat.type() == CV_16UC1 || depthMat.type() == CV_32FC1);
if(depthMat.type() == CV_8UC1)
{
cv::Mat leftImg;
if(imageMat.channels() == 3)
{
cv::cvtColor(imageMat, leftImg, CV_BGR2GRAY);
}
else
{
leftImg = imageMat;
}
cloud = rtabmap::util3d::cloudFromDisparityRGB(
imageMat,
depthMat,
util3d::disparityFromStereoImages(leftImg, depthMat),
cx, cy,
fx, fy,
decimation);
}
else
{
cloud = rtabmap::util3d::cloudFromDepthRGB(
imageMat,
depthMat,
cx, cy,
fx, fy,
decimation);
}
if(maxDepth)
{
@@ -872,12 +897,35 @@ void DatabaseViewer::generate3DMap()
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cv::Mat imageMat = rtabmap::util3d::uncompressImage(image);
cv::Mat depthMat = rtabmap::util3d::uncompressImage(depth);
cloud = rtabmap::util3d::cloudFromDepthRGB(
UASSERT(imageMat.empty() || imageMat.type()==CV_8UC3 || imageMat.type() == CV_8UC1);
UASSERT(depthMat.empty() || depthMat.type()==CV_8UC1 || depthMat.type() == CV_16UC1 || depthMat.type() == CV_32FC1);
if(depthMat.type() == CV_8UC1)
{
cv::Mat leftImg;
if(imageMat.channels() == 3)
{
cv::cvtColor(imageMat, leftImg, CV_BGR2GRAY);
}
else
{
leftImg = imageMat;
}
cloud = rtabmap::util3d::cloudFromDisparityRGB(
imageMat,
depthMat,
util3d::disparityFromStereoImages(leftImg, depthMat),
cx, cy,
fx, fy,
decimation);
}
else
{
cloud = rtabmap::util3d::cloudFromDepthRGB(
imageMat,
depthMat,
cx, cy,
fx, fy,
decimation);
}
if(maxDepth)
{
@@ -1316,6 +1364,8 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link)
cv::Mat imageA = rtabmap::util3d::uncompressImage(imageBytesA);
cv::Mat depthA = rtabmap::util3d::uncompressImage(depthBytesA);
cv::Mat depth2dA = rtabmap::util3d::uncompressData(depth2dBytesA);
UASSERT(imageA.empty() || imageA.type()==CV_8UC3 || imageA.type() == CV_8UC1);
UASSERT(depthA.empty() || depthA.type()==CV_8UC1 || depthA.type() == CV_16UC1 || depthA.type() == CV_32FC1);
std::vector<unsigned char> imageBytesB, depthBytesB, depth2dBytesB;
memory_->getImageDepth(link.to(), imageBytesB, depthBytesB, depth2dBytesB, fxB, fyB, cxB, cyB, localTransformB);
@@ -1325,23 +1375,66 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link)
//cloud 3d
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA;
cloudA = rtabmap::util3d::cloudFromDepthRGB(
imageA,
depthA,
cxA, cyA,
fxA, fyA,
1);
if(depthA.type() == CV_8UC1)
{
cv::Mat leftImg;
if(imageA.channels() == 3)
{
cv::cvtColor(imageA, leftImg, CV_BGR2GRAY);
}
else
{
leftImg = imageA;
}
cv::Mat disparity = util3d::disparityFromStereoImages(leftImg, depthA);
cloudA = rtabmap::util3d::cloudFromDisparityRGB(
imageA,
disparity,
cxA, cyA,
fxA, fyA,
1);
}
else
{
cloudA = rtabmap::util3d::cloudFromDepthRGB(
imageA,
depthA,
cxA, cyA,
fxA, fyA,
1);
}
cloudA = rtabmap::util3d::removeNaNFromPointCloud(cloudA);
cloudA = rtabmap::util3d::transformPointCloud(cloudA, localTransformA);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
cloudB = rtabmap::util3d::cloudFromDepthRGB(
imageB,
depthB,
cxB, cyB,
fxB, fyB,
1);
if(depthB.type() == CV_8UC1)
{
cv::Mat leftImg;
if(imageB.channels() == 3)
{
cv::cvtColor(imageB, leftImg, CV_BGR2GRAY);
}
else
{
leftImg = imageB;
}
cloudB = rtabmap::util3d::cloudFromDisparityRGB(
imageB,
util3d::disparityFromStereoImages(leftImg, depthB),
cxB, cyB,
fxB, fyB,
1);
}
else
{
cloudB = rtabmap::util3d::cloudFromDepthRGB(
imageB,
depthB,
cxB, cyB,
fxB, fyB,
1);
}
cloudB = rtabmap::util3d::removeNaNFromPointCloud(cloudB);
cloudB = rtabmap::util3d::transformPointCloud(cloudB, t*localTransformB);