Added stereo multi-camera support (#884)

* 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 <mathieu@robust.ai>
2026-09-02 17:40:23 +08:00 · 2022-07-20 15:20:14 -04:00
parent 71a28bb570
commit 5943a8b065
64 changed files with 2205 additions and 1010 deletions
--- a/corelib/src/optimizer/OptimizerCVSBA.cpp
+++ b/corelib/src/optimizer/OptimizerCVSBA.cpp
@@ -57,7 +57,7 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
 		int rootId,
 		const std::map<int, Transform> & posesIn,
 		const std::multimap<int, Link> & links,
-		const std::map<int, CameraModel> & models,
+		const std::map<int, std::vector<CameraModel> > & models,
 		std::map<int, cv::Point3f> & points3DMap,
 		const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/Disparity>)
 		std::set<int> * outliers)
--- a/corelib/src/optimizer/OptimizerG2O.cpp
+++ b/corelib/src/optimizer/OptimizerG2O.cpp
@@ -110,6 +110,8 @@ enum {
    PARAM_OFFSET=0,
 };

+#define MULTICAM_OFFSET 10 // 10 means max 10 cameras per pose
+
 namespace rtabmap {

 bool OptimizerG2O::available()
@@ -1395,7 +1397,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 		int rootId,
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & links,
-		const std::map<int, CameraModel> & models,
+		const std::map<int, std::vector<CameraModel> > & models,
 		std::map<int, cv::Point3f> & points3DMap,
 		const std::map<int, std::map<int, FeatureBA> > & wordReferences,
 		std::set<int> * outliers)
@@ -1488,50 +1490,55 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			if(iter->first > 0)
 			{
 				// Get camera model
-				std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first);
-				UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
+				std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(iter->first);
+				UASSERT(iterModel != models.end() && !iterModel->second.empty());
+				for(size_t i=0; i<iterModel->second.size(); ++i)
+				{
+					UASSERT(iterModel->second[i].isValidForProjection());

-				Transform camPose = iter->second * iterModel->second.localTransform();
+					Transform camPose = iter->second * iterModel->second[i].localTransform();

-				// Add node's pose
-				UASSERT(!camPose.isNull());
+					// Add node's pose
+					UASSERT(!camPose.isNull());
 #ifdef RTABMAP_ORB_SLAM
-				g2o::VertexSE3Expmap * vCam = new g2o::VertexSE3Expmap();
+					g2o::VertexSE3Expmap * vCam = new g2o::VertexSE3Expmap();
 #else
-				g2o::VertexCam * vCam = new g2o::VertexCam();
+					g2o::VertexCam * vCam = new g2o::VertexCam();
 #endif

-				Eigen::Affine3d a = camPose.toEigen3d();
+					Eigen::Affine3d a = camPose.toEigen3d();
 #ifdef RTABMAP_ORB_SLAM
-				a = a.inverse();
-				vCam->setEstimate(g2o::SE3Quat(a.linear(), a.translation()));
+					a = a.inverse();
+					vCam->setEstimate(g2o::SE3Quat(a.linear(), a.translation()));
 #else
-				g2o::SBACam cam(Eigen::Quaterniond(a.linear()), a.translation());
-				cam.setKcam(
-						iterModel->second.fx(),
-						iterModel->second.fy(),
-						iterModel->second.cx(),
-						iterModel->second.cy(),
-						iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_); // baseline in meters
-				vCam->setEstimate(cam);
+					g2o::SBACam cam(Eigen::Quaterniond(a.linear()), a.translation());
+					cam.setKcam(
+							iterModel->second[i].fx(),
+							iterModel->second[i].fy(),
+							iterModel->second[i].cx(),
+							iterModel->second[i].cy(),
+							iterModel->second[i].Tx()<0.0?-iterModel->second[i].Tx()/iterModel->second[i].fx():baseline_); // baseline in meters
+					vCam->setEstimate(cam);
 #endif
-				vCam->setId(iter->first);
+					vCam->setId(iter->first*MULTICAM_OFFSET + i);

-				// negative root means that all other poses should be fixed instead of the root
-				vCam->setFixed((rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId));
+					// negative root means that all other poses should be fixed instead of the root
+					vCam->setFixed((rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId));

-				/*UDEBUG("cam %d (fixed=%d) fx=%f fy=%f cx=%f cy=%f Tx=%f baseline=%f t=%s",
-						iter->first,
-						vCam->fixed()?1:0,
-						iterModel->second.fx(),
-						iterModel->second.fy(),
-						iterModel->second.cx(),
-						iterModel->second.cy(),
-						iterModel->second.Tx(),
-						iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_,
-						camPose.prettyPrint().c_str());*/
+					/*UDEBUG("camPose %d (camid=%d) (fixed=%d) fx=%f fy=%f cx=%f cy=%f Tx=%f baseline=%f t=%s",
+							iter->first,
+							vCam->id(),
+							vCam->fixed()?1:0,
+							iterModel->second[i].fx(),
+							iterModel->second[i].fy(),
+							iterModel->second[i].cx(),
+							iterModel->second[i].cy(),
+							iterModel->second[i].Tx(),
+							iterModel->second[i].Tx()<0.0?-iterModel->second[i].Tx()/iterModel->second[i].fx():baseline_,
+							camPose.prettyPrint().c_str());*/

-				UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert vertex %d!?", iter->first).c_str());
+					UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert cam vertex %d (pose=%d)!?", vCam->id(), iter->first).c_str());
+				}
 			}
 		}

@@ -1563,11 +1570,12 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(

 						Eigen::MatrixXd information = Eigen::MatrixXd::Identity(3, 3) * 1.0/(gravitySigma()*gravitySigma());

-						g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1);
+						g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1*MULTICAM_OFFSET);
 						EdgeSBACamGravity* priorEdge(new EdgeSBACamGravity());
-						std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first);
-						UASSERT(iterModel != models.end() && !iterModel->second.localTransform().isNull());
-						priorEdge->setCameraInvLocalTransform(iterModel->second.localTransform().inverse().toEigen3d().linear());
+						std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(iter->first);
+						// Gravity constraint added only to first camera of a pose
+						UASSERT(iterModel != models.end() && !iterModel->second.empty() && !iterModel->second[0].localTransform().isNull());
+						priorEdge->setCameraInvLocalTransform(iterModel->second[0].localTransform().inverse().toEigen3d().linear());
 						priorEdge->setMeasurement(m);
 						priorEdge->setInformation(information);
 						priorEdge->vertices()[0] = v1;
@@ -1592,15 +1600,17 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 					}

 					// between cameras, not base_link
-					Transform camLink = models.at(id1).localTransform().inverse()*iter->second.transform()*models.at(id2).localTransform();
-					//UDEBUG("added edge %d->%d (in cam frame=%s)",
-					//		id1,
-					//		id2,
-					//		camLink.prettyPrint().c_str());
+					Transform camLink = models.at(id1)[0].localTransform().inverse()*iter->second.transform()*models.at(id2)[0].localTransform();
+					/*UDEBUG("added edge %d->%d (camIDs %d->%d) (in cam frame=%s)",
+							id1,
+							id2,
+							id1*MULTICAM_OFFSET,
+							id2*MULTICAM_OFFSET,
+							camLink.prettyPrint().c_str());*/
 #ifdef RTABMAP_ORB_SLAM
 					EdgeSE3Expmap * e = new EdgeSE3Expmap();
-					g2o::VertexSE3Expmap* v1 = (g2o::VertexSE3Expmap*)optimizer.vertex(id1);
-					g2o::VertexSE3Expmap* v2 = (g2o::VertexSE3Expmap*)optimizer.vertex(id2);
+					g2o::VertexSE3Expmap* v1 = (g2o::VertexSE3Expmap*)optimizer.vertex(id1*MULTICAM_OFFSET);
+					g2o::VertexSE3Expmap* v2 = (g2o::VertexSE3Expmap*)optimizer.vertex(id2*MULTICAM_OFFSET);

 					Transform camPose1 = Transform::fromEigen3d(v1->estimate()).inverse();
 					Transform camPose2Inv = Transform::fromEigen3d(v2->estimate());
@@ -1608,8 +1618,8 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 					camLink =  camPose1 * camPose1 * camLink * camPose2Inv * camPose2Inv;
 #else
 					g2o::EdgeSBACam * e = new g2o::EdgeSBACam();
-					g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1);
-					g2o::VertexCam* v2 = (g2o::VertexCam*)optimizer.vertex(id2);
+					g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1*MULTICAM_OFFSET);
+					g2o::VertexCam* v2 = (g2o::VertexCam*)optimizer.vertex(id2*MULTICAM_OFFSET);
 #endif
 					UASSERT(v1 != 0);
 					UASSERT(v2 != 0);
@@ -1629,8 +1639,60 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			}
 		}

+		UDEBUG("fill hard edges between camera 0 and other cameras (multicam)...");
+		for(std::map<int, std::vector<CameraModel> >::const_iterator iter=models.begin(); iter!=models.end(); ++iter)
+		{
+			int id = iter->first;
+			if(uContains(poses, id))
+			{
+				for(size_t i=1; i<iter->second.size(); ++i)
+				{
+					// add edge
+					// Set large information matrix to keep these links fixed
+					Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity()*9999999;
+
+					// between cameras, not base_link
+					Transform camLink = iter->second[0].localTransform().inverse()*iter->second[i].localTransform();
+#ifdef RTABMAP_ORB_SLAM
+					EdgeSE3Expmap * e = new EdgeSE3Expmap();
+					g2o::VertexSE3Expmap* v1 = (g2o::VertexSE3Expmap*)optimizer.vertex(id*MULTICAM_OFFSET);
+					g2o::VertexSE3Expmap* v2 = (g2o::VertexSE3Expmap*)optimizer.vertex(id*MULTICAM_OFFSET+i);
+
+					Transform camPose1 = Transform::fromEigen3d(v1->estimate()).inverse();
+					Transform camPose2Inv = Transform::fromEigen3d(v2->estimate());
+
+					camLink =  camPose1 * camPose1 * camLink * camPose2Inv * camPose2Inv;
+#else
+					g2o::EdgeSBACam * e = new g2o::EdgeSBACam();
+					g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id*MULTICAM_OFFSET);
+					g2o::VertexCam* v2 = (g2o::VertexCam*)optimizer.vertex(id*MULTICAM_OFFSET+i);
+#endif
+					/*UDEBUG("added edge between subcam 0->%d id:%d->%d (in cam frame=%s)",
+							i,
+							v1->id(),
+							v2->id(),
+							camLink.prettyPrint().c_str());*/
+
+					UASSERT(v1 != 0);
+					UASSERT(v2 != 0);
+					e->setVertex(0, v1);
+					e->setVertex(1, v2);
+					Eigen::Affine3d a = camLink.toEigen3d();
+					e->setMeasurement(g2o::SE3Quat(a.linear(), a.translation()));
+					e->setInformation(information);
+
+					if (!optimizer.addEdge(e))
+					{
+						delete e;
+						UERROR("Map: Failed adding constraint between %d and %d, skipping", v1->id(), v2->id());
+						return optimizedPoses;
+					}
+				}
+			}
+		}
+
 		UDEBUG("fill 3D points to g2o...");
-		const int stepVertexId = poses.rbegin()->first+1;
+		const int stepVertexId = poses.rbegin()->first*MULTICAM_OFFSET+MULTICAM_OFFSET;
 		int negVertexOffset = stepVertexId;
 		if(wordReferences.size() && wordReferences.rbegin()->first>0)
 		{
@@ -1669,22 +1731,24 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 				// set observations
 				for(std::map<int, FeatureBA>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
 				{
-					int camId = jter->first;
-					if(poses.find(camId) != poses.end() && optimizer.vertex(camId) != 0)
+					int poseId = jter->first;
+					int camIndex = jter->second.cameraIndex;
+					int camId = poseId*MULTICAM_OFFSET+camIndex;
+					if(poses.find(poseId) != poses.end() && optimizer.vertex(camId) != 0)
 					{
 						const FeatureBA & pt = jter->second;
 						double depth = pt.depth;

-						//UDEBUG("Added observation pt=%d to cam=%d (%d,%d) depth=%f", vpt3d->id()-stepVertexId, camId, (int)pt.kpt.pt.x, (int)pt.kpt.pt.y, depth);
+						//UDEBUG("Added observation pt=%d to cam=%d (%d,%d) depth=%f (camIndex=%d)", vpt3d->id()-stepVertexId, camId, (int)pt.kpt.pt.x, (int)pt.kpt.pt.y, depth, camIndex);

 						g2o::OptimizableGraph::Edge * e;
 						double baseline = 0.0;
 #ifdef RTABMAP_ORB_SLAM
 						g2o::VertexSE3Expmap* vcam = dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(camId));
-						std::map<int, CameraModel>::const_iterator iterModel = models.find(camId);
+						std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(poseId);

-						UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
-						baseline = iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_;
+						UASSERT(iterModel != models.end() && camIndex<iterModel->second.size() && iterModel->second[camIndex].isValidForProjection());
+						baseline = iterModel->second[camIndex].Tx()<0.0?-iterModel->second[camIndex].Tx()/iterModel->second[camIndex].fx():baseline_;
 #else
 						g2o::VertexCam* vcam = dynamic_cast<g2o::VertexCam*>(optimizer.vertex(camId));
 						baseline = vcam->estimate().baseline;
@@ -1695,15 +1759,15 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 							// stereo edge
 #ifdef RTABMAP_ORB_SLAM
 							g2o::EdgeStereoSE3ProjectXYZ* es = new g2o::EdgeStereoSE3ProjectXYZ();
-							float disparity = baseline * iterModel->second.fx() / depth;
+							float disparity = baseline * iterModel->second[camIndex].fx() / depth;
 							Eigen::Vector3d obs( pt.kpt.pt.x, pt.kpt.pt.y, pt.kpt.pt.x-disparity);
 							es->setMeasurement(obs);
 							//variance *= log(exp(1)+disparity);
 							es->setInformation(Eigen::Matrix3d::Identity() / variance);
-							es->fx = iterModel->second.fx();
-							es->fy = iterModel->second.fy();
-							es->cx = iterModel->second.cx();
-							es->cy = iterModel->second.cy();
+							es->fx = iterModel->second[camIndex].fx();
+							es->fy = iterModel->second[camIndex].fy();
+							es->cx = iterModel->second[camIndex].cx();
+							es->cy = iterModel->second[camIndex].cy();
 							es->bf = baseline*es->fx;
 							e = es;
 #else
@@ -1731,10 +1795,10 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 							Eigen::Vector2d obs( pt.kpt.pt.x, pt.kpt.pt.y);
 							em->setMeasurement(obs);
 							em->setInformation(Eigen::Matrix2d::Identity() / variance);
-							em->fx = iterModel->second.fx();
-							em->fy = iterModel->second.fy();
-							em->cx = iterModel->second.cx();
-							em->cy = iterModel->second.cy();
+							em->fx = iterModel->second[camIndex].fx();
+							em->fy = iterModel->second[camIndex].fy();
+							em->cx = iterModel->second[camIndex].cx();
+							em->cy = iterModel->second[camIndex].cy();
 							e = em;

 #else
@@ -1809,13 +1873,13 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 						{
 							d = ((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[0]-((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[2];
 						}
-						UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
+						//UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
 #else
 						if(dynamic_cast<g2o::EdgeProjectP2SC*>(*iter) != 0)
 						{
 							d = ((g2o::EdgeProjectP2SC*)(*iter))->measurement()[0]-((g2o::EdgeProjectP2SC*)(*iter))->measurement()[2];
 						}
-						UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeProjectP2SC*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
+						//UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeProjectP2SC*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
 #endif

 						cv::Point3f pt3d;
@@ -1858,10 +1922,11 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 		{
 			if(iter->first > 0)
 			{
+				int camId = iter->first*MULTICAM_OFFSET;
 #ifdef RTABMAP_ORB_SLAM
-				const g2o::VertexSE3Expmap* v = (const g2o::VertexSE3Expmap*)optimizer.vertex(iter->first);
+				const g2o::VertexSE3Expmap* v = (const g2o::VertexSE3Expmap*)optimizer.vertex(camId);
 #else
-				const g2o::VertexCam* v = (const g2o::VertexCam*)optimizer.vertex(iter->first);
+				const g2o::VertexCam* v = (const g2o::VertexCam*)optimizer.vertex(camId);
 #endif
 				if(v)
 				{
@@ -1872,7 +1937,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 #endif

 					// remove model local transform
-					t *= models.at(iter->first).localTransform().inverse();
+					t *= models.at(iter->first)[0].localTransform().inverse();

 					//UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str());
 					if(t.isNull())
@@ -1896,7 +1961,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 				}
 				else
 				{
-					UERROR("Vertex (pose) %d not found!?", iter->first);
+					UERROR("Vertex (pose) %d (cam=%d) not found!?", iter->first, camId);
 				}
 			}
 		}