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

fixed laserScanFromDepthImage order (counterclockwise)

Browse Source
This commit is contained in:
matlabbe
2017-02-24 15:26:42 -05:00
parent b0170d1f6f
commit 3cf21a7e7f
2 changed files with 10 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
corelib/include/rtabmap/core/util3d.h
View File

@@ -167,6 +167,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
const ParametersMap & stereoParameters = ParametersMap(),
const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
/**
* Simulate a laser scan rotating counterclockwise, using middle line of the depth image.
*/
pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
const cv::Mat & depthImage,
float fx,
@@ -176,6 +179,11 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
float maxDepth = 0,
float minDepth = 0,
const Transform & localTransform = Transform::getIdentity());
/**
* Simulate a laser scan rotating counterclockwise, using middle line of the depth images.
* The last value of the scan is the most left value of the first depth image. The first value of the scan is the most right value of the last depth image.
*
*/
pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImages(
const cv::Mat & depthImages,
const std::vector<CameraModel> & cameraModels,

4
corelib/src/util3d.cpp
View File

@@ -1109,7 +1109,7 @@ pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
{
scan.resize(depthImage.cols);
int oi = 0;
for(int i=0; i<depthImage.cols; ++i)
for(int i=depthImage.cols-1; i>=0; --i)
{
pcl::PointXYZ pt = util3d::projectDepthTo3D(depthImage, i, middle, cx, cy, fx, fy, false);
if(pcl::isFinite(pt) && pt.z >= minDepth && (maxDepth == 0 || pt.z < maxDepth))
@@ -1135,7 +1135,7 @@ pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImages(
pcl::PointCloud<pcl::PointXYZ> scan;
UASSERT(int((depthImages.cols/cameraModels.size())*cameraModels.size()) == depthImages.cols);
int subImageWidth = depthImages.cols/cameraModels.size();
for(unsigned int i=0; i<cameraModels.size(); ++i)
for(int i=(int)cameraModels.size()-1; i>=0; --i)
{
if(cameraModels[i].isValidForProjection())
{