i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-08 11:07:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed build with cvsba and ceres

Browse Source
This commit is contained in:
matlabbe
2023-01-28 18:49:58 -08:00
parent 128d3b0d0a
commit e4edee33eb
3 changed files with 39 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/optimizer/OptimizerCeres.h
+1 -1
View File
@@ -65,7 +65,7 @@ public:
int rootId, int rootId,
const std::map<int, Transform> & poses, const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links, const std::multimap<int, Link> & links,
const std::map<int, CameraModel> & models, // in case of stereo, Tx should be set const std::map<int, std::vector<CameraModel> > & models, // in case of stereo, Tx should be set
std::map<int, cv::Point3f> & points3DMap, std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)> const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
std::set<int> * outliers = 0); std::set<int> * outliers = 0);
corelib/src/optimizer/OptimizerCVSBA.cpp
+14 -8
View File
@@ -87,23 +87,29 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{ {
// Get camera model // Get camera model
std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first); std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(iter->first);
UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection()); UASSERT(iterModel != models.end());
if(iterModel->second.size() != 1)
{
UERROR("Multi-camera BA not implemented for cvsba, only single camera.");
return std::map<int, Transform>();
}
UASSERT(iterModel->second[0].isValidForProjection());
frameIdToIndex.insert(std::make_pair(iter->first, oi)); frameIdToIndex.insert(std::make_pair(iter->first, oi));
cameraMatrix[oi] = iterModel->second.K(); cameraMatrix[oi] = iterModel->second[0].K();
if(iterModel->second.D().cols != 5) if(iterModel->second[0].D().cols != 5)
{ {
distCoeffs[oi] = cv::Mat::zeros(1, 5, CV_64FC1); distCoeffs[oi] = cv::Mat::zeros(1, 5, CV_64FC1);
UWARN("Camera model %d: Distortion coefficients are not 5, setting all them to 0 (assuming no distortion)", iter->first); UWARN("Camera model %d: Distortion coefficients are not 5, setting all them to 0 (assuming no distortion)", iter->first);
} }
else else
{ {
distCoeffs[oi] = iterModel->second.D(); distCoeffs[oi] = iterModel->second[0].D();
} }
Transform t = (iter->second * iterModel->second.localTransform()).inverse(); Transform t = (iter->second * iterModel->second[0].localTransform()).inverse();
R[oi] = (cv::Mat_<double>(3,3) << R[oi] = (cv::Mat_<double>(3,3) <<
(double)t.r11(), (double)t.r12(), (double)t.r13(), (double)t.r11(), (double)t.r12(), (double)t.r13(),
@@ -172,13 +178,13 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
if(this->isSlam2d()) if(this->isSlam2d())
{ {
t = (models.at(iter->first).localTransform() * t).inverse(); t = (models.at(iter->first)[0].localTransform() * t).inverse();
t = iter->second.inverse() * t; t = iter->second.inverse() * t;
iter->second *= t.to3DoF(); iter->second *= t.to3DoF();
} }
else else
{ {
iter->second = (models.at(iter->first).localTransform() * t).inverse(); iter->second = (models.at(iter->first)[0].localTransform() * t).inverse();
} }
++i; ++i;
corelib/src/optimizer/OptimizerCeres.cpp
+24 -12
View File
@@ -317,7 +317,7 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
int rootId, int rootId,
const std::map<int, Transform> & posesIn, const std::map<int, Transform> & posesIn,
const std::multimap<int, Link> & links, 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, std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/Disparity>) const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/Disparity>)
std::set<int> * outliers) std::set<int> * outliers)
@@ -354,10 +354,16 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
++iter) ++iter)
{ {
// Get camera model // Get camera model
std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first); std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(iter->first);
UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection()); UASSERT(iterModel != models.end());
if(iterModel->second.size() != 1)
{
UERROR("Multi-camera BA not implemented for Ceres, only single camera.");
return std::map<int, Transform>();
}
UASSERT(iterModel->second[0].isValidForProjection());
const Transform & t = (iter->second * iterModel->second.localTransform()).inverse(); const Transform & t = (iter->second * iterModel->second[0].localTransform()).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) << cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)t.r11(), (double)t.r12(), (double)t.r13(), (double)t.r11(), (double)t.r12(), (double)t.r13(),
(double)t.r21(), (double)t.r22(), (double)t.r23(), (double)t.r21(), (double)t.r22(), (double)t.r23(),
@@ -403,15 +409,21 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
jter!=iter->second.end(); jter!=iter->second.end();
++jter) ++jter)
{ {
std::map<int, CameraModel>::const_iterator iterModel = models.find(jter->first); std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(jter->first);
UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection()); UASSERT(iterModel != models.end());
if(iterModel->second.size() != 1)
{
UERROR("Multi-camera BA not implemented for Ceres, only single camera.");
return std::map<int, Transform>();
}
UASSERT(iterModel->second[0].isValidForProjection());
baProblem.camera_index_[oi] = camIdToIndex.at(jter->first); baProblem.camera_index_[oi] = camIdToIndex.at(jter->first);
baProblem.point_index_[oi] = pointIdToIndex.at(iter->first); baProblem.point_index_[oi] = pointIdToIndex.at(iter->first);
baProblem.observations_[4*oi] = jter->second.kpt.pt.x - iterModel->second.cx(); baProblem.observations_[4*oi] = jter->second.kpt.pt.x - iterModel->second[0].cx();
baProblem.observations_[4*oi+1] = jter->second.kpt.pt.y - iterModel->second.cy(); baProblem.observations_[4*oi+1] = jter->second.kpt.pt.y - iterModel->second[0].cy();
baProblem.observations_[4*oi+2] = iterModel->second.fx(); baProblem.observations_[4*oi+2] = iterModel->second[0].fx();
baProblem.observations_[4*oi+3] = iterModel->second.fy(); baProblem.observations_[4*oi+3] = iterModel->second[0].fy();
++oi; ++oi;
} }
} }
@@ -480,13 +492,13 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
if(this->isSlam2d()) if(this->isSlam2d())
{ {
t = (models.at(iter->first).localTransform() * t).inverse(); t = (models.at(iter->first)[0].localTransform() * t).inverse();
t = iter->second.inverse() * t; t = iter->second.inverse() * t;
iter->second *= t.to3DoF(); iter->second *= t.to3DoF();
} }
else else
{ {
iter->second = (models.at(iter->first).localTransform() * t).inverse(); iter->second = (models.at(iter->first)[0].localTransform() * t).inverse();
} }
} }