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

Refactored OdometryOpticalFlow (added optical flow guess using previous odometry transform, merged stereo/depth stuff)

Browse Source
This commit is contained in:
matlabbe
2015-06-15 14:44:44 -04:00
parent b8dccc2228
commit dfbf6e721e
14 changed files with 785 additions and 671 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/util3d_features.cpp
+40 -7
View File
@@ -124,15 +124,46 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
int flowWinSize,
int flowMaxLevel,
int flowIterations,
double flowEps)
double flowEps,
double maxCorrespondencesSlope)
{
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(keypoints, leftCorners);
return generateKeypoints3DStereo(
leftCorners,
leftImage,
rightImage,
fx,
baseline,
cx,
cy,
transform,
flowWinSize,
flowMaxLevel,
flowIterations,
flowEps,
maxCorrespondencesSlope);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
const std::vector<cv::Point2f> & leftCorners,
const cv::Mat & leftImage,
const cv::Mat & rightImage,
float fx,
float baseline,
float cx,
float cy,
const Transform & transform,
int flowWinSize,
int flowMaxLevel,
int flowIterations,
double flowEps,
double maxCorrespondencesSlope)
{
UASSERT(!leftImage.empty() && !rightImage.empty() &&
leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
leftImage.rows == rightImage.rows && leftImage.cols == rightImage.cols);
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(keypoints, leftCorners);
// Find features in the new left image
std::vector<unsigned char> status;
std::vector<float> err;
@@ -151,16 +182,18 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
UDEBUG("cv::calcOpticalFlowPyrLK() end");
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
keypoints3d->resize(keypoints.size());
keypoints3d->resize(leftCorners.size());
float bad_point = std::numeric_limits<float>::quiet_NaN ();
UASSERT(status.size() == keypoints.size());
UASSERT(status.size() == leftCorners.size());
for(unsigned int i=0; i<status.size(); ++i)
{
pcl::PointXYZ pt(bad_point, bad_point, bad_point);
if(status[i])
{
float disparity = leftCorners[i].x - rightCorners[i].x;
if(disparity > 0.0f)
float slope = fabs((leftCorners[i].y-rightCorners[i].y) / (leftCorners[i].x-rightCorners[i].x));
if(disparity > 0.0f &&
(maxCorrespondencesSlope <=0 || fabs(leftCorners[i].y-rightCorners[i].y) <= 1.0f || slope <= maxCorrespondencesSlope))
{
pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
leftCorners[i],