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

Added Depth information to ImageView's keypoint items

Browse Source
This commit is contained in:
Mathieu Labbe
2015-05-05 11:39:30 -04:00
parent 0b5d6c84b1
commit b2bfa91153
10 changed files with 76 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -121,6 +121,12 @@ std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
const std::list<int> & wordIds,
const std::vector<cv::KeyPoint> & keypoints);
float RTABMAP_EXP getDepth(
const cv::Mat & depthImage,
float x, float y,
bool smoothing,
float maxZError = 0.02f);
pcl::PointXYZ RTABMAP_EXP projectDepthTo3D(
const cv::Mat & depthImage,
float x, float y,

34
corelib/src/util3d.cpp
View File

@@ -770,19 +770,15 @@ void findCorrespondences(
inliers2.resize(oi);
}
pcl::PointXYZ projectDepthTo3D(
float getDepth(
const cv::Mat & depthImage,
float x, float y,
float cx, float cy,
float fx, float fy,
bool smoothing,
float maxZError)
{
UASSERT(!depthImage.empty());
UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
pcl::PointXYZ pt;
float bad_point = std::numeric_limits<float>::quiet_NaN ();
int u = int(x+0.5f);
int v = int(y+0.5f);
@@ -790,8 +786,7 @@ pcl::PointXYZ projectDepthTo3D(
{
UERROR("!(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows) cond failed! returning bad point. (x=%f (u=%d), y=%f (v=%d), cols=%d, rows=%d)",
x,u,y,v,depthImage.cols, depthImage.rows);
pt.x = pt.y = pt.z = bad_point;
return pt;
return 0;
}
bool isInMM = depthImage.type() == CV_16UC1; // is in mm?
@@ -845,7 +840,30 @@ pcl::PointXYZ projectDepthTo3D(
// mean
depth = (depth+sumDepths)/sumWeights;
}
}
else
{
depth = 0;
}
return depth;
}
pcl::PointXYZ projectDepthTo3D(
const cv::Mat & depthImage,
float x, float y,
float cx, float cy,
float fx, float fy,
bool smoothing,
float maxZError)
{
UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
pcl::PointXYZ pt;
float bad_point = std::numeric_limits<float>::quiet_NaN ();
float depth = getDepth(depthImage, x, y, smoothing, maxZError);
if(depth)
{
// Use correct principal point from calibration
cx = cx > 0.0f ? cx : float(depthImage.cols/2) - 0.5f; //cameraInfo.K.at(2)
cy = cy > 0.0f ? cy : float(depthImage.rows/2) - 0.5f; //cameraInfo.K.at(5)