Added multicameras support for F2F odometry / OpticalFlow

introlab · Oct 25, 2023 · 64962e8 · 64962e8
1 parent 71bb0cf
commit 64962e8
Show file tree

Hide file tree

Showing 3 changed files with 63 additions and 11 deletions.
diff --git a/corelib/src/RegistrationVis.cpp b/corelib/src/RegistrationVis.cpp
@@ -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());
 					}
 				}
 

diff --git a/corelib/src/odometry/OdometryF2F.cpp b/corelib/src/odometry/OdometryF2F.cpp
@@ -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;
 	}
 

diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp
@@ -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);
+							}
 						}
 					}
 				}