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

Projection map frame is still doing roll/pitch transformation (without z)

Browse Source
This commit is contained in:
matlabbe
2016-07-17 17:06:36 -04:00
parent 536136af77
commit 237ab2be45
1 changed files with 6 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
guilib/src/MainWindow.cpp
+6 -11
View File
@@ -2587,11 +2587,8 @@ void MainWindow::createAndAddProjectionMap(
// add pose rotation without yaw
float roll, pitch, yaw;
if(_preferencesDialog->projMapFrame())
{
pose.getEulerAngles(roll, pitch, yaw);
voxelCloud = util3d::transformPointCloud(voxelCloud, Transform(0,0, pose.z(), roll, pitch, 0));
}
pose.getEulerAngles(roll, pitch, yaw);
voxelCloud = util3d::transformPointCloud(voxelCloud, Transform(0,0, _preferencesDialog->projMapFrame()?pose.z():0, roll, pitch, 0));
if(_preferencesDialog->projMaxObstaclesHeight())
{
@@ -2637,12 +2634,10 @@ void MainWindow::createAndAddProjectionMap(
if(_octomap->addedNodes().empty() ||
nodeId > _octomap->addedNodes().rbegin()->first)
{
if(_preferencesDialog->projMapFrame())
{
Transform tinv = Transform(0,0,pose.z(), roll, pitch, 0).inverse();
groundCloud = util3d::transformPointCloud(groundCloud, tinv);
obstaclesCloud = util3d::transformPointCloud(obstaclesCloud, tinv);
}
Transform tinv = Transform(0,0,_preferencesDialog->projMapFrame()?pose.z():0, roll, pitch, 0).inverse();
groundCloud = util3d::transformPointCloud(groundCloud, tinv);
obstaclesCloud = util3d::transformPointCloud(obstaclesCloud, tinv);
if(_preferencesDialog->isOctomapGroundAnObstacle())
{
*obstaclesCloud += *groundCloud;