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

Added stereo multi-camera support (#884)

Browse Source 
* Integrated OpenGV

* Fixed build without opengv

* Cmake: moved OpenGV dependency status under solvers group

* Added multi-stereocamera models support

* Fixed OpenGV 0 sample error when one of the camera doesn't have features. Fixed g2o BA id offset with multi-camera.

* Fixed multicam 3d points generated from stereo correspondences

* db: Fixed multi stereo models not loaded correctly

* gui: fixed stereo rectification option, RegVis: fixed projection error with old databases (image size not set in calibration)

* OdomF2M: Fixed map.at error when bundle adjustment is not used

* depthai: added imu firmware update option for convenience

* Fixed various refactor errors

* Moved "large number stereo correspondences rejected" warning outside computeCorrespondences function for multicam

* Added error log if ba correspondences are computed with empty signatures

* fixed compilation errors with latest opencv

Co-authored-by: mathieu86 <[email protected]>
This commit is contained in:
matlabbe
2022-07-20 15:20:14 -04:00
committed by GitHub
co-authored by mathieu86
parent 71a28bb570
commit 5943a8b065
64 changed files with 2205 additions and 1010 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Features2d.cpp
+115 -14
View File
@@ -792,7 +792,9 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
std::vector<cv::Point3f> keypoints3D;
if(keypoints.size())
{
if(!data.rightRaw().empty() && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection())
if(!data.rightRaw().empty() && !data.imageRaw().empty() &&
!data.stereoCameraModels().empty() &&
data.stereoCameraModels()[0].isValidForProjection())
{
//stereo
cv::Mat imageMono;
@@ -808,22 +810,121 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(keypoints, leftCorners);
std::vector<unsigned char> status;
std::vector<cv::Point2f> rightCorners;
rightCorners = _stereo->computeCorrespondences(
imageMono,
data.rightRaw(),
leftCorners,
status);
keypoints3D = util3d::generateKeypoints3DStereo(
leftCorners,
rightCorners,
data.stereoCameraModel(),
status,
_minDepth,
_maxDepth);
if(data.stereoCameraModels().size() == 1)
{
std::vector<unsigned char> status;
rightCorners = _stereo->computeCorrespondences(
imageMono,
data.rightRaw(),
leftCorners,
status);
if(ULogger::level() >= ULogger::kWarning)
{
int rejected = 0;
for(size_t i=0; i<status.size(); ++i)
{
if(status[i]==0)
{
++rejected;
}
}
if(rejected > (int)status.size()/2)
{
UWARN("A large number (%d/%d) of stereo correspondences are rejected! "
"Optical flow may have failed because images are not calibrated, "
"the background is too far (no disparity between the images), "
"maximum disparity may be too small (%f) or that exposure between "
"left and right images is too different.",
rejected,
(int)status.size(),
_stereo->maxDisparity());
}
}
keypoints3D = util3d::generateKeypoints3DStereo(
leftCorners,
rightCorners,
data.stereoCameraModels()[0],
status,
_minDepth,
_maxDepth);
}
else
{
int subImageWith = imageMono.cols / data.stereoCameraModels().size();
UASSERT(imageMono.cols % subImageWith == 0);
std::vector<std::vector<cv::Point2f> > subLeftCorners(data.stereoCameraModels().size());
std::vector<std::vector<int> > subIndex(data.stereoCameraModels().size());
// Assign keypoints per camera
for(size_t i=0; i<leftCorners.size(); ++i)
{
int cameraIndex = int(leftCorners[i].x / subImageWith);
leftCorners[i].x -= cameraIndex*subImageWith;
subLeftCorners[cameraIndex].push_back(leftCorners[i]);
subIndex[cameraIndex].push_back(i);
}
keypoints3D.resize(keypoints.size());
int total = 0;
int rejected = 0;
for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
{
if(!subLeftCorners[i].empty())
{
std::vector<unsigned char> status;
rightCorners = _stereo->computeCorrespondences(
imageMono.colRange(cv::Range(subImageWith*i, subImageWith*(i+1))),
data.rightRaw().colRange(cv::Range(subImageWith*i, subImageWith*(i+1))),
subLeftCorners[i],
status);
std::vector<cv::Point3f> subKeypoints3D = util3d::generateKeypoints3DStereo(
subLeftCorners[i],
rightCorners,
data.stereoCameraModels()[i],
status,
_minDepth,
_maxDepth);
if(ULogger::level() >= ULogger::kWarning)
{
for(size_t i=0; i<status.size(); ++i)
{
if(status[i]==0)
{
++rejected;
}
}
total+=status.size();
}
UASSERT(subIndex[i].size() == subKeypoints3D.size());
for(size_t j=0; j<subKeypoints3D.size(); ++j)
{
keypoints3D[subIndex[i][j]] = subKeypoints3D[j];
}
}
}
if(ULogger::level() >= ULogger::kWarning)
{
if(rejected > total/2)
{
UWARN("A large number (%d/%d) of stereo correspondences are rejected! "
"Optical flow may have failed because images are not calibrated, "
"the background is too far (no disparity between the images), "
"maximum disparity may be too small (%f) or that exposure between "
"left and right images is too different.",
rejected,
total,
_stereo->maxDisparity());
}
}
}
}
else if(!data.depthRaw().empty() && data.cameraModels().size())
{