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

export: when --images and --cam_projection are used, we export depth image from lidar projection to RGB frames

Browse Source
This commit is contained in:
matlabbe
2022-12-04 17:23:33 -08:00
parent 8a8c360f89
commit 64cb7fb6fe
1 changed files with 41 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
tools/Export/main.cpp
View File

@@ -1483,6 +1483,46 @@ int main(int argc, char * argv[])
cameraModelsProj = cameraModels;
}
if(exportImages)
{
printf("Camera projection... projecting cloud to individual cameras (--images option)\n");
// projectCloudToCamera requires PCLPointCloud2
pcl::PCLPointCloud2::Ptr cloud2(new pcl::PCLPointCloud2);
if(!cloudToExport->empty())
{
pcl::toPCLPointCloud2(*cloudToExport, *cloud2);
}
else if(!cloudIToExport->empty())
{
pcl::toPCLPointCloud2(*cloudIToExport, *cloud2);
}
std::string dir = outputDirectory+"/"+baseName+"_depth_from_scan";
if(!UDirectory::exists(dir)) {
UDirectory::makeDir(dir);
}
for(std::map<int, std::vector<rtabmap::CameraModel> >::iterator iter=cameraModelsProj.begin();
iter!=cameraModelsProj.end();
++iter)
{
cv::Mat depth(iter->second.front().imageHeight(), iter->second.front().imageWidth()*iter->second.size(), CV_32FC1);
for(size_t i=0; i<iter->second.size(); ++i)
{
cv::Mat subDepth = util3d::projectCloudToCamera(
iter->second.at(i).imageSize(),
iter->second.at(i).K(),
cloud2,
robotPoses.at(iter->first) * iter->second.at(i).localTransform());
subDepth.copyTo(depth(cv::Range::all(), cv::Range(i*iter->second.front().imageWidth(), (i+1)*iter->second.front().imageWidth())));
}
depth = rtabmap::util2d::cvtDepthFromFloat(depth);
std::string outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f",cameraStamps.at(iter->first)))+".png";
cv::imwrite(outputPath, depth);
}
}
cv::Mat projMask;
if(!cameraProjMask.empty())
{
@@ -1495,6 +1535,7 @@ int main(int argc, char * argv[])
}
}
printf("Camera projection... projecting cloud to all cameras\n");
pointToCamId.resize(!cloudToExport->empty()?cloudToExport->size():cloudIToExport->size());
std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
if(!cloudToExport->empty())