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

projectCloudToCamerasImpl: fixed bug using wrong camera models

Browse Source
This commit is contained in:
matlabbe
2021-09-08 09:34:21 -04:00
parent 7baedf4c72
commit bc39b19517
1 changed files with 3 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/src/util3d.cpp
View File

@@ -2863,11 +2863,13 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
int cameraProcessed = 0;
for(std::map<int, Transform>::const_iterator pter = cameraPoses.lower_bound(0); pter!=cameraPoses.end(); ++pter)
{
std::map<int, std::vector<CameraModel> >::const_iterator iter=cameraModels.begin();
std::map<int, std::vector<CameraModel> >::const_iterator iter=cameraModels.find(pter->first);
if(iter!=cameraModels.end() && !iter->second.empty())
{
for(size_t i=0; i<iter->second.size(); ++i)
{
UWARN("%d/%d: %s %s", pter->first, i, pter->second.prettyPrint().c_str(), iter->second[i].localTransform().prettyPrint().c_str());
Transform cameraTransform = (pter->second * iter->second[i].localTransform());
UASSERT(!cameraTransform.isNull());
cv::Mat cameraMatrixK = iter->second[i].K();