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

Refactoring Registration: added RegistrationInfo class, pipeline registration. Camera source: create laser scan from depth image. Removed OdometryICP as the same behavior can be achieved with OdometryF2F and "Reg/Strategy" = 1 (ICP) or 2 (Vis+ICP).

Browse Source
This commit is contained in:
matlabbe
2016-01-06 17:26:55 -05:00
parent 1c66ad1db2
commit 28d9bbb85d
35 changed files with 1209 additions and 1362 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/CameraThread.cpp
+28 -4
View File
@@ -45,6 +45,9 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
_mirroring(false),
_colorOnly(false),
_stereoToDepth(false),
_scanFromDepth(false),
_scanDecimation(4),
_scanMaxDepth(4.0f),
_stereoDense(new StereoBM(parameters))
{
UASSERT(_camera != 0);
@@ -103,7 +106,7 @@ void CameraThread::mainLoop()
cv::flip(data.depthRaw(), tmpDepth, 1);
data.setDepthOrRightRaw(tmpDepth);
}
info.timeMirroring_ = timer.ticks();
info.timeMirroring = timer.ticks();
}
if(_stereoToDepth && data.stereoCameraModel().isValid() && !data.rightRaw().empty())
{
@@ -115,10 +118,31 @@ void CameraThread::mainLoop()
data.setCameraModel(data.stereoCameraModel().left());
data.setDepthOrRightRaw(depth);
data.setStereoCameraModel(StereoCameraModel());
info.timeDisparity_ = timer.ticks();
UINFO("Computing disparity = %f s", info.timeDisparity_);
info.timeDisparity = timer.ticks();
UINFO("Computing disparity = %f s", info.timeDisparity);
}
info.cameraName_ = _camera->getSerial();
if(_scanFromDepth &&
data.cameraModels().size() &&
data.cameraModels().at(0).isValid() &&
!data.depthRaw().empty())
{
if(data.laserScanRaw().empty())
{
UASSERT(_scanDecimation >= 1);
UTimer timer;
cv::Mat scan = util3d::laserScanFromPointCloud(*util3d::cloudFromSensorData(data, _scanDecimation, _scanMaxDepth));
data.setLaserScanRaw(scan, (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation), _scanMaxDepth);
info.timeScanFromDepth = timer.ticks();
UINFO("Computing scan from depth = %f s", info.timeScanFromDepth);
}
else
{
UWARN("Option to create laser scan from depth image is enabled, but "
"there is already a laser scan in the captured sensor data. Scan from "
"depth will not be created.");
}
}
info.cameraName = _camera->getSerial();
this->post(new CameraEvent(data, info));
}
else if(!this->isKilled())