i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Grid: Adjusting viewpoint when projecting into map frame

Browse Source
This commit is contained in:
matlabbe
2017-03-06 13:32:48 -05:00
parent d1e9525052
commit 560fcdc541
1 changed files with 11 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/OccupancyGrid.cpp
+11
View File
@@ -277,6 +277,17 @@ void OccupancyGrid::createLocalMap(
} }
} }
if(projMapFrame_)
{
//we should rotate viewPoint in /map frame
float roll, pitch, yaw;
node.getPose().getEulerAngles(roll, pitch, yaw);
Transform viewpointRotated = Transform(0,0,0,roll,pitch,0) * Transform(viewPoint.x, viewPoint.y, viewPoint.z, 0,0,0);
viewPoint.x = viewpointRotated.x();
viewPoint.y = viewpointRotated.y();
viewPoint.z = viewpointRotated.z();
}
if(cloud->size()) if(cloud->size())
{ {
pcl::IndicesPtr groundIndices(new std::vector<int>); pcl::IndicesPtr groundIndices(new std::vector<int>);