| 177 | |
| 178 | |
| 179 | ScanNode* ScanGraph::addNode(Pointcloud* scan, pose6d pose) { |
| 180 | if (scan != 0) { |
| 181 | nodes.push_back(new ScanNode(scan, pose, (unsigned int) nodes.size())); |
| 182 | return nodes.back(); |
| 183 | } |
| 184 | else { |
| 185 | OCTOMAP_ERROR("scan is invalid.\n"); |
| 186 | return NULL; |
| 187 | } |
| 188 | } |
| 189 | |
| 190 | |
| 191 | ScanEdge* ScanGraph::addEdge(ScanNode* first, ScanNode* second, pose6d constraint) { |