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

2
corelib/include/rtabmap/core/optimizer/OptimizerCeres.h
View File

@@ -65,7 +65,7 @@ public:
int rootId,
const std::map<int, Transform> & poses,
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,
const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
std::set<int> * outliers = 0);

22
corelib/src/optimizer/OptimizerCVSBA.cpp
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)
{
// 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());
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));
cameraMatrix[oi] = iterModel->second.K();
if(iterModel->second.D().cols != 5)
cameraMatrix[oi] = iterModel->second[0].K();
if(iterModel->second[0].D().cols != 5)
{
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);
}
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) <<
(double)t.r11(), (double)t.r12(), (double)t.r13(),
@@ -172,13 +178,13 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
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;
iter->second *= t.to3DoF();
}
else
{
iter->second = (models.at(iter->first).localTransform() * t).inverse();
iter->second = (models.at(iter->first)[0].localTransform() * t).inverse();
}
++i;

36
corelib/src/optimizer/OptimizerCeres.cpp
View File

@@ -317,7 +317,7 @@ std::map<int, Transform> OptimizerCeres::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)
@@ -354,10 +354,16 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
++iter)
{
// 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());
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) <<
(double)t.r11(), (double)t.r12(), (double)t.r13(),
(double)t.r21(), (double)t.r22(), (double)t.r23(),
@@ -403,15 +409,21 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
jter!=iter->second.end();
++jter)
{
std::map<int, CameraModel>::const_iterator iterModel = models.find(jter->first);
UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(jter->first);
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.point_index_[oi] = pointIdToIndex.at(iter->first);
baProblem.observations_[4*oi] = jter->second.kpt.pt.x - iterModel->second.cx();
baProblem.observations_[4*oi+1] = jter->second.kpt.pt.y - iterModel->second.cy();
baProblem.observations_[4*oi+2] = iterModel->second.fx();
baProblem.observations_[4*oi+3] = iterModel->second.fy();
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[0].cy();
baProblem.observations_[4*oi+2] = iterModel->second[0].fx();
baProblem.observations_[4*oi+3] = iterModel->second[0].fy();
++oi;
}
}
@@ -480,13 +492,13 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
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;
iter->second *= t.to3DoF();
}
else
{
iter->second = (models.at(iter->first).localTransform() * t).inverse();
iter->second = (models.at(iter->first)[0].localTransform() * t).inverse();
}
}