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

DBViewer: added camera projection on scan option (GUI params). Fixed some deprecated warnings.

Browse Source
This commit is contained in:
matlabbe
2021-04-19 18:14:20 -04:00
parent 93bfad626e
commit 3b7c6cd1f4
4 changed files with 409 additions and 293 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
corelib/src/util3d.cpp
View File

@@ -3294,10 +3294,9 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
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 = pcl::getAngle3D(normal, camDir);
float angleToCam = maxAngle<=0?0:pcl::getAngle3D(normal, camDir);
float distanceToCam = invertedIndex[i][j].distance;
if(camDir.dot(normal) > 0 && // is facing camera?
(maxAngle<=0 || angleToCam < maxAngle) && // is point normal perpendicular to camera?
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;

24
corelib/src/util3d_filtering.cpp
View File

@@ -174,7 +174,7 @@ LaserScan commonFiltering(
if(cloud->size() && (normalK > 0 || normalRadius>0.0f))
{
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, normalK, normalRadius);
scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), LaserScan::kXYZRGBNormal, scan.localTransform());
scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), scan.localTransform());
UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
}
else
@@ -183,7 +183,7 @@ LaserScan commonFiltering(
{
UWARN("Voxel filter is applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.");
}
scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), LaserScan::kXYZRGB, scan.localTransform());
scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), scan.localTransform());
}
}
}
@@ -208,17 +208,17 @@ LaserScan commonFiltering(
normals = util3d::computeNormals2D(cloud, normalK, normalRadius);
if(voxelSize == 0.0f && scan.angleIncrement() > 0.0f)
{
scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), LaserScan::kXYINormal, scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement(), scan.localTransform());
scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement(), scan.localTransform());
}
else
{
scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), LaserScan::kXYINormal, scan.localTransform());
scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), scan.localTransform());
}
}
else
{
normals = util3d::computeNormals(cloud, normalK, normalRadius);
scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), LaserScan::kXYZINormal, scan.localTransform());
scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), scan.localTransform());
}
UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
}
@@ -230,11 +230,11 @@ LaserScan commonFiltering(
}
if(scan.is2d())
{
scan = LaserScan(laserScan2dFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), LaserScan::kXYI, scan.localTransform());
scan = LaserScan(laserScan2dFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), scan.localTransform());
}
else
{
scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), LaserScan::kXYZI, scan.localTransform());
scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), scan.localTransform());
}
}
}
@@ -260,17 +260,17 @@ LaserScan commonFiltering(
normals = util3d::computeNormals2D(cloud, normalK, normalRadius);
if(voxelSize == 0.0f && scan.angleIncrement() > 0.0f)
{
scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), LaserScan::kXYNormal, scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement(), scan.localTransform());
scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement(), scan.localTransform());
}
else
{
scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), LaserScan::kXYNormal, scan.localTransform());
scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), scan.localTransform());
}
}
else
{
normals = util3d::computeNormals(cloud, normalK, normalRadius);
scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), LaserScan::kXYZNormal, scan.localTransform());
scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), scan.localTransform());
}
UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
}
@@ -282,11 +282,11 @@ LaserScan commonFiltering(
}
if(scan.is2d())
{
scan = LaserScan(laserScan2dFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), LaserScan::kXY, scan.localTransform());
scan = LaserScan(laserScan2dFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), scan.localTransform());
}
else
{
scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), LaserScan::kXYZ, scan.localTransform());
scan = LaserScan(laserScanFromPointCloud(*cloud), scanMaxPts, scan.rangeMax(), scan.localTransform());
}
}
}