i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +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/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());
}
}

8
corelib/src/odometry/OdometryF2F.cpp
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;
}