i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-11 12:29:50 +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
corelib/src/util3d.cpp
+3 -1
View File
@@ -2863,11 +2863,13 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
int cameraProcessed = 0; int cameraProcessed = 0;
for(std::map<int, Transform>::const_iterator pter = cameraPoses.lower_bound(0); pter!=cameraPoses.end(); ++pter) 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()) if(iter!=cameraModels.end() && !iter->second.empty())
{ {
for(size_t i=0; i<iter->second.size(); ++i) 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()); Transform cameraTransform = (pter->second * iter->second[i].localTransform());
UASSERT(!cameraTransform.isNull()); UASSERT(!cameraTransform.isNull());
cv::Mat cameraMatrixK = iter->second[i].K(); cv::Mat cameraMatrixK = iter->second[i].K();