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

3D projection: adding pose rotation (roll, pitch) before projection

Browse Source
This commit is contained in:
matlabbe
2016-04-12 17:45:50 -04:00
parent f511896c43
commit fc76e5b8f3
2 changed files with 13 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
guilib/src/DatabaseViewer.cpp
View File

@@ -3315,10 +3315,13 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
ui_->doubleSpinBox_projMaxDepth->value(), ui_->doubleSpinBox_projMaxDepth->value(),
ui_->doubleSpinBox_projMinDepth->value(), ui_->doubleSpinBox_projMinDepth->value(),
validIndices.get()); validIndices.get());
if(ui_->doubleSpinBox_gridCellSize->value()) UASSERT(ui_->doubleSpinBox_gridCellSize->value() > 0);
{
cloud = util3d::voxelize(cloud, validIndices, ui_->doubleSpinBox_gridCellSize->value()); cloud = util3d::voxelize(cloud, validIndices, ui_->doubleSpinBox_gridCellSize->value());
}
// add pose rotation without yaw
float roll, pitch, yaw;
graphFiltered.at(ids[i]).getEulerAngles(roll, pitch, yaw);
cloud = util3d::transformPointCloud(cloud, Transform(0,0,0, roll, pitch, 0));
if(cloud->size()) if(cloud->size())
{ {

6
guilib/src/MainWindow.cpp
View File

@@ -2191,6 +2191,12 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
int minClusterSize = 20; int minClusterSize = 20;
cv::Mat ground, obstacles; cv::Mat ground, obstacles;
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelCloud = util3d::voxelize(cloud, indices, cellSize); pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelCloud = util3d::voxelize(cloud, indices, cellSize);
// add pose rotation without yaw
float roll, pitch, yaw;
pose.getEulerAngles(roll, pitch, yaw);
voxelCloud = util3d::transformPointCloud(voxelCloud, Transform(0,0,0, roll, pitch, 0));
util3d::occupancy2DFromCloud3D<pcl::PointXYZRGBNormal>( util3d::occupancy2DFromCloud3D<pcl::PointXYZRGBNormal>(
voxelCloud, voxelCloud,
ground, ground,