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

Added multicameras support for F2F odometry / OpticalFlow

Browse Source
This commit is contained in:
matlabbe
2023-10-25 14:54:20 -07:00
parent 71bb0cf226
commit 64962e8e3d
3 changed files with 63 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
corelib/src/RegistrationVis.cpp
View File

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/RegistrationVis.h> #include <rtabmap/core/RegistrationVis.h>
#include <rtabmap/core/util3d_motion_estimation.h> #include <rtabmap/core/util3d_motion_estimation.h>
#include <rtabmap/core/util3d_features.h> #include <rtabmap/core/util3d_features.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d.h> #include <rtabmap/core/util3d.h>
#include <rtabmap/core/VWDictionary.h> #include <rtabmap/core/VWDictionary.h>
#include <rtabmap/core/util2d.h> #include <rtabmap/core/util2d.h>
@@ -488,6 +489,7 @@ Transform RegistrationVis::computeTransformationImpl(
if(!imageFrom.empty() && !imageTo.empty()) if(!imageFrom.empty() && !imageTo.empty())
{ {
UASSERT(!toSignature.sensorData().cameraModels().empty() || !toSignature.sensorData().stereoCameraModels().empty());
std::vector<cv::Point2f> cornersFrom; std::vector<cv::Point2f> cornersFrom;
cv::KeyPoint::convert(kptsFrom, cornersFrom); cv::KeyPoint::convert(kptsFrom, cornersFrom);
std::vector<cv::Point2f> cornersTo; std::vector<cv::Point2f> cornersTo;
@@ -510,7 +512,48 @@ Transform RegistrationVis::computeTransformationImpl(
} }
else else
{ {
UERROR("Optical flow guess with multi-cameras is not implemented, guess ignored..."); UTimer t;
int nCameras = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels().size():toSignature.sensorData().stereoCameraModels().size();
cornersTo = cornersFrom;
// compute inverse transforms one time
std::vector<Transform> inverseTransforms(nCameras);
for(int c=0; c<nCameras; ++c)
{
Transform localTransform = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[c].localTransform():toSignature.sensorData().stereoCameraModels()[c].left().localTransform();
inverseTransforms[c] = (guess * localTransform).inverse();
UDEBUG("inverse transforms: cam %d -> %s", c, inverseTransforms[c].prettyPrint().c_str());
}
// Project 3D points in each camera
int inFrame = 0;
UASSERT(kptsFrom3D.size() == cornersTo.size());
int subImageWidth = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].imageWidth():toSignature.sensorData().stereoCameraModels()[0].left().imageWidth();
UASSERT(subImageWidth>0);
for(size_t i=0; i<kptsFrom3D.size(); ++i)
{
// Start from camera having the reference corner first (in case there is overlap between the cameras)
int startIndex = cornersFrom[i].x/subImageWidth;
UASSERT(startIndex < nCameras);
for(int c=startIndex; (c+1)%nCameras != 0; ++c)
{
const CameraModel & model = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[c]:toSignature.sensorData().stereoCameraModels()[c].left();
cv::Point3f ptsInCamFrame = util3d::transformPoint(kptsFrom3D[i], inverseTransforms[c]);
if(ptsInCamFrame.z > 0)
{
float u,v;
model.reproject(ptsInCamFrame.x, ptsInCamFrame.y, ptsInCamFrame.z, u, v);
if(model.inFrame(u,v))
{
cornersTo[i].x = u+model.imageWidth()*c;
cornersTo[i].y = v;
++inFrame;
break;
}
}
}
}
UDEBUG("Pprojected %d/%ld points inside %d cameras (time=%fs)",
inFrame, cornersTo.size(), nCameras, t.ticks());
} }
} }

8
corelib/src/odometry/OdometryF2F.cpp
View File

@@ -74,15 +74,15 @@ Transform OdometryF2F::computeTransform(
UTimer timer; UTimer timer;
Transform output; Transform output;
if(!data.rightRaw().empty() && if(!data.rightRaw().empty() &&
(data.stereoCameraModels().size() != 1 || !data.stereoCameraModels()[0].isValidForProjection())) (data.stereoCameraModels().empty() || !data.stereoCameraModels()[0].isValidForProjection()))
{ {
UERROR("Calibrated stereo camera required (multi-cameras not supported)"); UERROR("Calibrated stereo camera required.");
return output; return output;
} }
if(!data.depthRaw().empty() && if(!data.depthRaw().empty() &&
(data.cameraModels().size() != 1 || !data.cameraModels()[0].isValidForProjection())) (data.cameraModels().empty() || !data.cameraModels()[0].isValidForProjection()))
{ {
UERROR("Calibrated camera required (multi-cameras not supported)."); UERROR("Calibrated camera required.");
return output; return output;
} }

21
guilib/src/MainWindow.cpp
View File

@@ -1713,6 +1713,11 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
//draw lines //draw lines
UASSERT(odom.info().refCorners.size() == odom.info().newCorners.size()); UASSERT(odom.info().refCorners.size() == odom.info().newCorners.size());
std::set<int> inliers(odom.info().cornerInliers.begin(), odom.info().cornerInliers.end()); std::set<int> inliers(odom.info().cornerInliers.begin(), odom.info().cornerInliers.end());
int subImageWidth = 0;
if(data->cameraModels().size()>1 || data->stereoCameraModels().size()>1)
{
subImageWidth = data->cameraModels().size()?data->cameraModels()[0].imageWidth():data->stereoCameraModels()[0].left().imageWidth();
}
for(unsigned int i=0; i<odom.info().refCorners.size(); ++i) for(unsigned int i=0; i<odom.info().refCorners.size(); ++i)
{ {
if(_ui->imageView_odometry->isFeaturesShown() && inliers.find(i) != inliers.end()) if(_ui->imageView_odometry->isFeaturesShown() && inliers.find(i) != inliers.end())
@@ -1721,12 +1726,16 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
} }
if(_ui->imageView_odometry->isLinesShown()) if(_ui->imageView_odometry->isLinesShown())
{ {
_ui->imageView_odometry->addLine( // just draw lines in same camera
odom.info().newCorners[i].x, if(subImageWidth==0 || int(odom.info().refCorners[i].x/subImageWidth) == int(odom.info().newCorners[i].x/subImageWidth))
odom.info().newCorners[i].y, {
odom.info().refCorners[i].x, _ui->imageView_odometry->addLine(
odom.info().refCorners[i].y, odom.info().newCorners[i].x,
inliers.find(i) != inliers.end()?Qt::blue:Qt::yellow); odom.info().newCorners[i].y,
odom.info().refCorners[i].x,
odom.info().refCorners[i].y,
inliers.find(i) != inliers.end()?Qt::blue:Qt::yellow);
}
} }
} }
} }