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

Added util3d_features.h doc/tests

Browse Source
This commit is contained in:
matlabbe
2025-06-22 12:43:13 -07:00
parent cb6cdcae90
commit 3ca3c37ee7
5 changed files with 417 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/util3d.cpp
+2
View File
@@ -2688,6 +2688,7 @@ cv::Point3f projectDisparityTo3D(
float disparity,
const StereoCameraModel & model)
{
printf("disparity=%f\n", disparity);
if(disparity > 0.0f && model.baseline() > 0.0f && model.left().fx() > 0.0f)
{
//Z = baseline * f / (d + cx1-cx0);
@@ -2697,6 +2698,7 @@ cv::Point3f projectDisparityTo3D(
c = model.right().cx() - model.left().cx();
}
float W = model.baseline()/(disparity + c);
printf("W=%f\n", W);
return cv::Point3f((pt.x - model.left().cx())*W,
(pt.y - model.left().cy())*model.left().fx()/model.left().fy()*W, model.left().fx()*W);
}
corelib/src/util3d_features.cpp
+14 -13
View File
@@ -51,19 +51,6 @@ namespace rtabmap
namespace util3d
{
std::vector<cv::Point3f> generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
const CameraModel & cameraModel,
float minDepth,
float maxDepth)
{
UASSERT(cameraModel.isValidForProjection());
std::vector<CameraModel> models;
models.push_back(cameraModel);
return generateKeypoints3DDepth(keypoints, depth, models, minDepth, maxDepth);
}
std::vector<cv::Point3f> generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
@@ -119,6 +106,19 @@ std::vector<cv::Point3f> generateKeypoints3DDepth(
return keypoints3d;
}
std::vector<cv::Point3f> generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
const CameraModel & cameraModel,
float minDepth,
float maxDepth)
{
UASSERT(cameraModel.isValidForProjection());
std::vector<CameraModel> models;
models.push_back(cameraModel);
return generateKeypoints3DDepth(keypoints, depth, models, minDepth, maxDepth);
}
std::vector<cv::Point3f> generateKeypoints3DDisparity(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & disparity,
@@ -415,6 +415,7 @@ std::multimap<int, cv::KeyPoint> aggregate(
const std::list<int> & wordIds,
const std::vector<cv::KeyPoint> & keypoints)
{
UASSERT(wordIds.size() == keypoints.size());
std::multimap<int, cv::KeyPoint> words;
std::vector<cv::KeyPoint>::const_iterator kpIter = keypoints.begin();
for(std::list<int>::const_iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)