| 44 | } |
| 45 | |
| 46 | void PointcloudDrawer::setScanGraph(const ScanGraph& graph){ |
| 47 | clear(); |
| 48 | |
| 49 | // count points first: |
| 50 | for (octomap::ScanGraph::const_iterator it = graph.begin(); it != graph.end(); it++) { |
| 51 | m_numberPoints += (*it)->scan->size(); |
| 52 | } |
| 53 | |
| 54 | m_pointsArray = new GLfloat[3*m_numberPoints]; |
| 55 | |
| 56 | unsigned i = 0; |
| 57 | for (octomap::ScanGraph::const_iterator graph_it = graph.begin(); graph_it != graph.end(); graph_it++) { |
| 58 | octomap::Pointcloud* scan = new Pointcloud((*graph_it)->scan); |
| 59 | scan->transformAbsolute((*graph_it)->pose); |
| 60 | |
| 61 | for (Pointcloud::iterator pc_it = scan->begin(); pc_it != scan->end(); ++pc_it){ |
| 62 | m_pointsArray[3*i] = pc_it->x(); |
| 63 | m_pointsArray[3*i +1] = pc_it->y(); |
| 64 | m_pointsArray[3*i +2] = pc_it->z(); |
| 65 | |
| 66 | i++; |
| 67 | } |
| 68 | delete scan; |
| 69 | } |
| 70 | } |
| 71 | |
| 72 | void PointcloudDrawer::clear(){ |
| 73 | |