i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-09 04:50:20 +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
corelib/src/RegistrationVis.cpp
+44 -1
View File
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/RegistrationVis.h>
#include <rtabmap/core/util3d_motion_estimation.h>
#include <rtabmap/core/util3d_features.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/VWDictionary.h>
#include <rtabmap/core/util2d.h>
@@ -488,6 +489,7 @@ Transform RegistrationVis::computeTransformationImpl(
if(!imageFrom.empty() && !imageTo.empty())
{
UASSERT(!toSignature.sensorData().cameraModels().empty() || !toSignature.sensorData().stereoCameraModels().empty());
std::vector<cv::Point2f> cornersFrom;
cv::KeyPoint::convert(kptsFrom, cornersFrom);
std::vector<cv::Point2f> cornersTo;
@@ -510,7 +512,48 @@ Transform RegistrationVis::computeTransformationImpl(
}
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());
}
}
corelib/src/odometry/OdometryF2F.cpp
+4 -4
View File
@@ -74,15 +74,15 @@ Transform OdometryF2F::computeTransform(
UTimer timer;
Transform output;
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;
}
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;
}
guilib/src/MainWindow.cpp
+15 -6
View File
@@ -1713,6 +1713,11 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
//draw lines
UASSERT(odom.info().refCorners.size() == odom.info().newCorners.size());
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)
{
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())
{
_ui->imageView_odometry->addLine(
odom.info().newCorners[i].x,
odom.info().newCorners[i].y,
odom.info().refCorners[i].x,
odom.info().refCorners[i].y,
inliers.find(i) != inliers.end()?Qt::blue:Qt::yellow);
// just draw lines in same camera
if(subImageWidth==0 || int(odom.info().refCorners[i].x/subImageWidth) == int(odom.info().newCorners[i].x/subImageWidth))
{
_ui->imageView_odometry->addLine(
odom.info().newCorners[i].x,
odom.info().newCorners[i].y,
odom.info().refCorners[i].x,
odom.info().refCorners[i].y,
inliers.find(i) != inliers.end()?Qt::blue:Qt::yellow);
}
}
}
}