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

Added util3d::projectCloudToCamera() with PCLPointcloud2 interface, update util2d::fillDeptHoles() to support CV_32FC1

Browse Source
This commit is contained in:
matlabbe
2017-02-23 00:15:43 -05:00
parent a1c036a7e2
commit 8de6cb1ca0
4 changed files with 282 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

99
corelib/src/util2d.cpp
View File

@@ -1414,18 +1414,29 @@ cv::Mat registerDepth(
return registered;
}
cv::Mat fillDepthHoles(const cv::Mat & registeredDepth, int maximumHoleSize, float errorRatio)
cv::Mat fillDepthHoles(const cv::Mat & depth, int maximumHoleSize, float errorRatio)
{
UASSERT(registeredDepth.type() == CV_16UC1);
UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1);
UASSERT(maximumHoleSize > 0);
cv::Mat output = registeredDepth.clone();
for(int y=0; y<registeredDepth.rows-2; ++y)
cv::Mat output = depth.clone();
bool isMM = depth.type() == CV_16UC1;
for(int y=0; y<depth.rows-2; ++y)
{
for(int x=0; x<registeredDepth.cols-2; ++x)
for(int x=0; x<depth.cols-2; ++x)
{
float a = registeredDepth.at<unsigned short>(y, x);
float bRight = registeredDepth.at<unsigned short>(y, x+1);
float bDown = registeredDepth.at<unsigned short>(y+1, x);
float a, bRight, bDown;
if(isMM)
{
a = depth.at<unsigned short>(y, x);
bRight = depth.at<unsigned short>(y, x+1);
bDown = depth.at<unsigned short>(y+1, x);
}
else
{
a = depth.at<float>(y, x);
bRight = depth.at<float>(y, x+1);
bDown = depth.at<float>(y+1, x);
}
if(a > 0.0f && (bRight == 0.0f || bDown == 0.0f))
{
@@ -1437,13 +1448,13 @@ cv::Mat fillDepthHoles(const cv::Mat & registeredDepth, int maximumHoleSize, flo
// horizontal
if(!horizontalSet)
{
if(x+1+h >= registeredDepth.cols)
if(x+1+h >= depth.cols)
{
horizontalSet = true;
}
else
{
float c = registeredDepth.at<unsigned short>(y, x+1+h);
float c = isMM?depth.at<unsigned short>(y, x+1+h):depth.at<float>(y, x+1+h);
if(c == 0)
{
// ignore this size
@@ -1456,16 +1467,36 @@ cv::Mat fillDepthHoles(const cv::Mat & registeredDepth, int maximumHoleSize, flo
{
//linear interpolation
float slope = (c-a)/float(h+1);
for(int z=x+1; z<x+1+h; ++z)
if(isMM)
{
if(output.at<unsigned short>(y, z) == 0)
for(int z=x+1; z<x+1+h; ++z)
{
output.at<unsigned short>(y, z) = (unsigned short)(a+(slope*float(z-x)));
unsigned short & value = output.at<unsigned short>(y, z);
if(value == 0)
{
value = (unsigned short)(a+(slope*float(z-x)));
}
else
{
// average with the previously set value
value = (value+(unsigned short)(a+(slope*float(z-x))))/2;
}
}
else
}
else
{
for(int z=x+1; z<x+1+h; ++z)
{
// average with the previously set value
output.at<unsigned short>(y, z) = (output.at<unsigned short>(y, z)+(unsigned short)(a+(slope*float(z-x))))/2;
float & value = output.at<float>(y, z);
if(value == 0)
{
value = a+(slope*float(z-x));
}
else
{
// average with the previously set value
value = (value+(a+(slope*float(z-x))))/2;
}
}
}
}
@@ -1478,13 +1509,13 @@ cv::Mat fillDepthHoles(const cv::Mat & registeredDepth, int maximumHoleSize, flo
// vertical
if(!verticalSet)
{
if(y+1+h >= registeredDepth.rows)
if(y+1+h >= depth.rows)
{
verticalSet = true;
}
else
{
float c = registeredDepth.at<unsigned short>(y+1+h, x);
float c = isMM?depth.at<unsigned short>(y+1+h, x):depth.at<float>(y+1+h, x);
if(c == 0)
{
// ignore this size
@@ -1497,16 +1528,36 @@ cv::Mat fillDepthHoles(const cv::Mat & registeredDepth, int maximumHoleSize, flo
{
//linear interpolation
float slope = (c-a)/float(h+1);
for(int z=y+1; z<y+1+h; ++z)
if(isMM)
{
if(output.at<unsigned short>(z, x) == 0)
for(int z=y+1; z<y+1+h; ++z)
{
output.at<unsigned short>(z, x) = (unsigned short)(a+(slope*float(z-y)));
unsigned short & value = output.at<unsigned short>(z, x);
if(value == 0)
{
value = (unsigned short)(a+(slope*float(z-y)));
}
else
{
// average with the previously set value
value = (value+(unsigned short)(a+(slope*float(z-y))))/2;
}
}
else
}
else
{
for(int z=y+1; z<y+1+h; ++z)
{
// average with the previously set value
output.at<unsigned short>(z, x) = (output.at<unsigned short>(z, x)+(unsigned short)(a+(slope*float(z-y))))/2;
float & value = output.at<float>(z, x);
if(value == 0)
{
value = (a+(slope*float(z-y)));
}
else
{
// average with the previously set value
value = (value+(a+(slope*float(z-y))))/2;
}
}
}
}

221
corelib/src/util3d.cpp
View File

@@ -1483,6 +1483,10 @@ cv::Mat projectCloudToCamera(
cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
Transform t = cameraTransform.inverse();
const cv::Vec2f* vec2Ptr = laserScan.ptr<cv::Vec2f>();
const cv::Vec3f* vec3Ptr = laserScan.ptr<cv::Vec3f>();
const cv::Vec6f* vec6Ptr = laserScan.ptr<cv::Vec6f>();
int count = 0;
for(int i=0; i<laserScan.cols; ++i)
{
@@ -1490,38 +1494,61 @@ cv::Mat projectCloudToCamera(
cv::Point3f ptScan;
if(laserScan.type() == CV_32FC2)
{
ptScan.x = laserScan.at<cv::Vec2f>(i)[0];
ptScan.y = laserScan.at<cv::Vec2f>(i)[1];
ptScan.x = vec2Ptr[i][0];
ptScan.y = vec2Ptr[i][1];
ptScan.z = 0;
}
else if(laserScan.type() == CV_32FC3)
{
ptScan.x = laserScan.at<cv::Vec3f>(i)[0];
ptScan.y = laserScan.at<cv::Vec3f>(i)[1];
ptScan.z = laserScan.at<cv::Vec3f>(i)[2];
ptScan.x = vec3Ptr[i][0];
ptScan.y = vec3Ptr[i][1];
ptScan.z = vec3Ptr[i][2];
}
else
{
ptScan.x = laserScan.at<cv::Vec6f>(i)[0];
ptScan.y = laserScan.at<cv::Vec6f>(i)[1];
ptScan.z = laserScan.at<cv::Vec6f>(i)[2];
ptScan.x = vec6Ptr[i][0];
ptScan.y = vec6Ptr[i][1];
ptScan.z = vec6Ptr[i][2];
}
ptScan = util3d::transformPoint(ptScan, t);
// re-project in camera frame
float z = ptScan.z;
float invZ = 1.0f/z;
int dx = (fx*ptScan.x)*invZ + cx;
int dy = (fy*ptScan.y)*invZ + cy;
if(z > 0.0f && uIsInBounds(dx, 0, registered.cols) && uIsInBounds(dy, 0, registered.rows))
bool set = false;
if(z > 0.0f)
{
++count;
float &zReg = registered.at<float>(dy, dx);
if(zReg == 0 || z < zReg)
float invZ = 1.0f/z;
float dx = (fx*ptScan.x)*invZ + cx;
float dy = (fy*ptScan.y)*invZ + cy;
int dx_low = dx;
int dy_low = dy;
int dx_high = dx + 0.5f;
int dy_high = dy + 0.5f;
if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows))
{
zReg = z;
float &zReg = registered.at<float>(dy_low, dx_low);
if(zReg == 0 || z < zReg)
{
zReg = z;
}
set = true;
}
if((dx_low != dx_high || dy_low != dy_high) &&
uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows))
{
float &zReg = registered.at<float>(dy_high, dx_high);
if(zReg == 0 || z < zReg)
{
zReg = z;
}
set = true;
}
}
if(set)
{
count++;
}
}
UDEBUG("Points in camera=%d/%d", count, laserScan.cols);
@@ -1556,26 +1583,170 @@ cv::Mat projectCloudToCamera(
// re-project in camera frame
float z = ptScan.z;
bool set = false;
if(z > 0.0f)
{
float invZ = 1.0f/z;
int dx = (fx*ptScan.x)*invZ + cx;
if(uIsInBounds(dx, 0, registered.cols))
float dx = (fx*ptScan.x)*invZ + cx;
float dy = (fy*ptScan.y)*invZ + cy;
int dx_low = dx;
int dy_low = dy;
int dx_high = dx + 0.5f;
int dy_high = dy + 0.5f;
if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows))
{
int dy = (fy*ptScan.y)*invZ + cy;
if(uIsInBounds(dy, 0, registered.rows))
set = true;
float &zReg = registered.at<float>(dy_low, dx_low);
if(zReg == 0 || z < zReg)
{
++count;
float &zReg = registered.at<float>(dy, dx);
if(zReg == 0 || z < zReg)
zReg = z;
}
}
if((dx_low != dx_high || dy_low != dy_high) &&
uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows))
{
set = true;
float &zReg = registered.at<float>(dy_high, dx_high);
if(zReg == 0 || z < zReg)
{
zReg = z;
}
}
}
if(set)
{
count++;
}
}
UDEBUG("Points in camera=%d/%d", count, (int)laserScan->size());
return registered;
}
cv::Mat projectCloudToCamera(
const cv::Size & imageSize,
const cv::Mat & cameraMatrixK,
const pcl::PCLPointCloud2::Ptr laserScan, // assuming points are already in /base_link coordinate
const rtabmap::Transform & cameraTransform) // /base_link -> /camera_link
{
UASSERT(!cameraTransform.isNull());
UASSERT(!laserScan->data.empty());
UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
float fx = cameraMatrixK.at<double>(0,0);
float fy = cameraMatrixK.at<double>(1,1);
float cx = cameraMatrixK.at<double>(0,2);
float cy = cameraMatrixK.at<double>(1,2);
cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
Transform t = cameraTransform.inverse();
pcl::MsgFieldMap field_map;
pcl::createMapping<pcl::PointXYZ> (laserScan->fields, field_map);
int count = 0;
if(field_map.size() == 1)
{
for (uint32_t row = 0; row < laserScan->height; ++row)
{
const uint8_t* row_data = &laserScan->data[row * laserScan->row_step];
for (uint32_t col = 0; col < laserScan->width; ++col)
{
const uint8_t* msg_data = row_data + col * laserScan->point_step;
pcl::PointXYZ ptScan;
memcpy (&ptScan, msg_data + field_map.front().serialized_offset, field_map.front().size);
ptScan = util3d::transformPoint(ptScan, t);
// re-project in camera frame
float z = ptScan.z;
bool set = false;
if(z > 0.0f)
{
float invZ = 1.0f/z;
float dx = (fx*ptScan.x)*invZ + cx;
float dy = (fy*ptScan.y)*invZ + cy;
int dx_low = dx;
int dy_low = dy;
int dx_high = dx + 0.5f;
int dy_high = dy + 0.5f;
if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows))
{
zReg = z;
set = true;
float &zReg = registered.at<float>(dy_low, dx_low);
if(zReg == 0 || z < zReg)
{
zReg = z;
}
}
if((dx_low != dx_high || dy_low != dy_high) &&
uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows))
{
set = true;
float &zReg = registered.at<float>(dy_high, dx_high);
if(zReg == 0 || z < zReg)
{
zReg = z;
}
}
}
if(set)
{
count++;
}
}
}
}
UDEBUG("Points in camera=%d/%d", count, (int)laserScan->size());
else
{
UERROR("field map pcl::pointXYZ not found!");
}
/*
int count = 0;
for(int i=0; i<(int)laserScan->size(); ++i)
{
// Get 3D from laser scan
pcl::PointXYZ ptScan = laserScan->at(i);
ptScan = util3d::transformPoint(ptScan, t);
// re-project in camera frame
float z = ptScan.z;
bool set = false;
if(z > 0.0f)
{
float invZ = 1.0f/z;
float dx = (fx*ptScan.x)*invZ + cx;
float dy = (fy*ptScan.y)*invZ + cy;
int dx_low = dx;
int dy_low = dy;
int dx_high = dx + 0.5f;
int dy_high = dy + 0.5f;
if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows))
{
set = true;
float &zReg = registered.at<float>(dy_low, dx_low);
if(zReg == 0 || z < zReg)
{
zReg = z;
}
}
if((dx_low != dx_high || dy_low != dy_high) &&
uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows))
{
set = true;
float &zReg = registered.at<float>(dy_high, dx_high);
if(zReg == 0 || z < zReg)
{
zReg = z;
}
}
}
if(set)
{
count++;
}
}
*/
UDEBUG("Points in camera=%d/%d", count, (int)laserScan->data.size());
return registered;
}