i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

Export: optimized camera projection RAM usage. Added --texture_angle and --cam_projection_decimation options.

Browse Source
This commit is contained in:
matlabbe
2021-11-21 17:10:57 -05:00
parent 090ae0c444
commit 67710ef94c
3 changed files with 362 additions and 210 deletions
Download Patch File Download Diff File Expand all files Collapse all files

104
corelib/src/util3d.cpp
View File

@@ -2823,7 +2823,13 @@ void fillProjectedCloudHoles(cv::Mat & registeredDepth, bool verticalDirection,
}
}
struct ProjectionInfo {
class ProjectionInfo {
public:
ProjectionInfo():
nodeID(-1),
cameraIndex(-1),
distance(-1)
{}
int nodeID;
int cameraIndex;
pcl::PointXY uv;
@@ -2845,6 +2851,12 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
bool distanceToCamPolicy,
const ProgressState * state)
{
UINFO("cloud=%d points", (int)cloud.size());
UINFO("cameraPoses=%d", (int)cameraPoses.size());
UINFO("cameraModels=%d", (int)cameraModels.size());
UINFO("maxDistance=%f", maxDistance);
UINFO("maxAngle=%f", maxAngle);
UINFO("distanceToCamPolicy=%s", distanceToCamPolicy?"true":"false");
std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
if (cloud.empty() || cameraPoses.empty() || cameraModels.empty())
@@ -2859,7 +2871,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
return pointToPixel;
}
std::vector<std::vector<ProjectionInfo> > invertedIndex(cloud.size()); // For each point: list of cameras
std::vector<ProjectionInfo> invertedIndex(cloud.size()); // For each point: list of cameras
int cameraProcessed = 0;
for(std::map<int, Transform>::const_iterator pter = cameraPoses.lower_bound(0); pter!=cameraPoses.end(); ++pter)
{
@@ -2899,7 +2911,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
// re-project in camera frame
float z = ptScan.z;
bool set = false;
if(z > 0.0f)
if(z > 0.0f && (maxDistance<=0 || z<maxDistance))
{
float invZ = 1.0f/z;
float dx = (fx*ptScan.x)*invZ + cx;
@@ -2957,8 +2969,39 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
info.cameraIndex = i;
info.uv.x = float(u)/float(imageSize.width);
info.uv.y = float(v)/float(imageSize.height);
info.distance = zReg[0]/1000.0f;
invertedIndex[zReg[1]].push_back(info);
const Transform & cam = cameraPoses.at(info.nodeID);
const PointT & pt = cloud.at(zReg[1]);
Eigen::Vector4f camDir(cam.x()-pt.x, cam.y()-pt.y, cam.z()-pt.z, 0);
Eigen::Vector4f normal(pt.normal_x, pt.normal_y, pt.normal_z, 0);
float angleToCam = maxAngle<=0?0:pcl::getAngle3D(normal, camDir);
float distanceToCam = zReg[0]/1000.0f;
if( (maxAngle<=0 || (camDir.dot(normal) > 0 && angleToCam < maxAngle)) && // is facing camera? is point normal perpendicular to camera?
(maxDistance<=0 || distanceToCam<maxDistance)) // is point not too far from camera?
{
float vx = info.uv.x-0.5f;
float vy = info.uv.y-0.5f;
float distanceToCenter = vx*vx+vy*vy;
float distance = distanceToCenter;
if(distanceToCamPolicy)
{
distance = distanceToCam;
}
info.distance = distance;
if(invertedIndex[zReg[1]].distance != -1.0f)
{
if(distance <= invertedIndex[zReg[1]].distance)
{
invertedIndex[zReg[1]] = info;
}
}
else
{
invertedIndex[zReg[1]] = info;
}
}
}
}
}
@@ -2991,50 +3034,14 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
// For each point
for(size_t i=0; i<invertedIndex.size(); ++i)
{
if((i+1)%10000 == 0)
{
UDEBUG("Point %d/%d", i+1, (int)cloud.size());
if(state && !state->callback(uFormat("%d/%d points projected to cameras (out of %d points)", colorized, i+1, (int)cloud.size())))
{
//cancelled!
UWARN("Projecting to camera cancelled!");
pointToPixel.clear();
return pointToPixel;
}
}
const PointT & pt = cloud.at(i);
int nodeID = -1;
int cameraIndex = -1;
float smallestWeight = std::numeric_limits<float>::max();
pcl::PointXY uv_coords;
for (size_t j = 0; j<invertedIndex[i].size(); ++j)
if(invertedIndex[i].distance > -1.0f)
{
const Transform & cam = cameraPoses.at(invertedIndex[i][j].nodeID);
Eigen::Vector4f camDir(cam.x()-pt.x, cam.y()-pt.y, cam.z()-pt.z, 0);
Eigen::Vector4f normal(pt.normal_x, pt.normal_y, pt.normal_z, 0);
float angleToCam = maxAngle<=0?0:pcl::getAngle3D(normal, camDir);
float distanceToCam = invertedIndex[i][j].distance;
if( (maxAngle<=0 || (camDir.dot(normal) > 0 && angleToCam < maxAngle)) && // is facing camera? is point normal perpendicular to camera?
(maxDistance<=0 || distanceToCam<maxDistance)) // is point not too far from camera?
{
float vx = invertedIndex[i][j].uv.x-0.5f;
float vy = invertedIndex[i][j].uv.y-0.5f;
float distanceToCenter = vx*vx+vy*vy;
float distance = distanceToCenter;
if(distanceToCamPolicy)
{
distance = distanceToCam;
}
if(distance <= smallestWeight)
{
nodeID = invertedIndex[i][j].nodeID;
cameraIndex = invertedIndex[i][j].cameraIndex;
smallestWeight = distance;
uv_coords = invertedIndex[i][j].uv;
}
}
nodeID = invertedIndex[i].nodeID;
cameraIndex = invertedIndex[i].cameraIndex;
uv_coords = invertedIndex[i].uv;
}
if(nodeID>-1 && cameraIndex> -1)
@@ -3046,7 +3053,12 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
}
}
UINFO("Process %d points...done! (%d [%d%%] projected in cameras)", (int)cloud.size(), colorized, colorized*100/cloud.size());
msg = uFormat("Process %d points...done! (%d [%d%%] projected in cameras)", (int)cloud.size(), colorized, colorized*100/cloud.size());
UINFO(msg.c_str());
if(state)
{
state->callback(msg);
}
return pointToPixel;
}