00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #include <iomanip>
00035 #include <fstream>
00036 #include <sstream>
00037 #include <stdlib.h>
00038
00039 #include <octomap/math/Pose6D.h>
00040 #include <octomap/ScanGraph.h>
00041
00042 namespace octomap {
00043
00044
00045 ScanNode::~ScanNode(){
00046 if (scan != NULL){
00047 delete scan;
00048 scan = NULL;
00049 }
00050 }
00051
00052 std::ostream& ScanNode::writeBinary(std::ostream &s) const {
00053
00054
00055
00056 scan->writeBinary(s);
00057 pose.writeBinary(s);
00058 s.write((char*)&id, sizeof(id));
00059
00060 return s;
00061 }
00062
00063 std::istream& ScanNode::readBinary(std::istream &s) {
00064
00065 this->scan = new Pointcloud();
00066 this->scan->readBinary(s);
00067
00068 this->pose.readBinary(s);
00069
00070 s.read((char*)&this->id, sizeof(this->id));
00071
00072 return s;
00073 }
00074
00075
00076 std::ostream& ScanNode::writePoseASCII(std::ostream &s) const {
00077 s << " " << this->id;
00078 s << " ";
00079 this->pose.trans().write(s);
00080 s << " ";
00081 this->pose.rot().toEuler().write(s);
00082 s << std::endl;
00083 return s;
00084 }
00085
00086 std::istream& ScanNode::readPoseASCII(std::istream &s) {
00087 unsigned int read_id;
00088 s >> read_id;
00089 if (read_id != this->id)
00090 OCTOMAP_ERROR("ERROR while reading ScanNode pose from ASCII. id %d does not match real id %d.\n", read_id, this->id);
00091
00092 this->pose.trans().read(s);
00093
00094
00095 point3d rot;
00096 rot.read(s);
00097 this->pose.rot() = octomath::Quaternion(rot);
00098 return s;
00099 }
00100
00101
00102 std::ostream& ScanEdge::writeBinary(std::ostream &s) const {
00103
00104
00105
00106 s.write((char*)&first->id, sizeof(first->id));
00107 s.write((char*)&second->id, sizeof(second->id));
00108 constraint.writeBinary(s);
00109 s.write((char*)&weight, sizeof(weight));
00110 return s;
00111 }
00112
00113 std::istream& ScanEdge::readBinary(std::istream &s, ScanGraph& graph) {
00114 unsigned int first_id, second_id;
00115 s.read((char*)&first_id, sizeof(first_id));
00116 s.read((char*)&second_id, sizeof(second_id));
00117
00118 this->first = graph.getNodeByID(first_id);
00119 if (this->first == NULL) OCTOMAP_ERROR("ERROR while reading ScanEdge. first node not found.\n");
00120 this->second = graph.getNodeByID(second_id);
00121 if (this->second == NULL) OCTOMAP_ERROR("ERROR while reading ScanEdge. second node not found.\n");
00122
00123 this->constraint.readBinary(s);
00124 s.read((char*)&weight, sizeof(weight));
00125
00126 return s;
00127 }
00128
00129
00130 std::ostream& ScanEdge::writeASCII(std::ostream &s) const {
00131
00132
00133
00134 s << " " << first->id << " " << second->id;
00135 s << " ";
00136 constraint.write(s);
00137 s << " " << weight;
00138 s << std::endl;
00139 return s;
00140 }
00141
00142 std::istream& ScanEdge::readASCII(std::istream &s, ScanGraph& graph) {
00143
00144 unsigned int first_id, second_id;
00145 s >> first_id;
00146 s >> second_id;
00147
00148 this->first = graph.getNodeByID(first_id);
00149 if (this->first == NULL) OCTOMAP_ERROR("ERROR while reading ScanEdge. first node %d not found.\n", first_id);
00150 this->second = graph.getNodeByID(second_id);
00151 if (this->second == NULL) OCTOMAP_ERROR("ERROR while reading ScanEdge. second node %d not found.\n", second_id);
00152
00153 this->constraint.read(s);
00154 s >> weight;
00155 return s;
00156 }
00157
00158
00159 ScanGraph::~ScanGraph() {
00160 this->clear();
00161 }
00162
00163 void ScanGraph::clear() {
00164 for (unsigned int i=0; i<nodes.size(); i++) {
00165 delete nodes[i];
00166 }
00167 nodes.clear();
00168 for (unsigned int i=0; i<edges.size(); i++) {
00169 delete edges[i];
00170 }
00171 edges.clear();
00172 }
00173
00174
00175 ScanNode* ScanGraph::addNode(Pointcloud* scan, pose6d pose) {
00176 if (scan != 0) {
00177 nodes.push_back(new ScanNode(scan, pose, nodes.size()));
00178 return nodes.back();
00179 }
00180 else {
00181 OCTOMAP_ERROR("scan is invalid.\n");
00182 return NULL;
00183 }
00184 }
00185
00186
00187 ScanEdge* ScanGraph::addEdge(ScanNode* first, ScanNode* second, pose6d constraint) {
00188
00189 if ((first != 0) && (second != 0)) {
00190 edges.push_back(new ScanEdge(first, second, constraint));
00191
00192 return edges.back();
00193 }
00194 else {
00195 OCTOMAP_ERROR("addEdge:: one or both nodes invalid.\n");
00196 return NULL;
00197 }
00198 }
00199
00200
00201 ScanEdge* ScanGraph::addEdge(unsigned int first_id, unsigned int second_id) {
00202
00203 if ( this->edgeExists(first_id, second_id)) {
00204 OCTOMAP_ERROR("addEdge:: Edge exists!\n");
00205 return NULL;
00206 }
00207
00208 ScanNode* first = getNodeByID(first_id);
00209 ScanNode* second = getNodeByID(second_id);
00210
00211 if ((first != 0) && (second != 0)) {
00212 pose6d constr = first->pose.inv() * second->pose;
00213 return this->addEdge(first, second, constr);
00214 }
00215 else {
00216 OCTOMAP_ERROR("addEdge:: one or both scans invalid.\n");
00217 return NULL;
00218 }
00219 }
00220
00221
00222 void ScanGraph::connectPrevious() {
00223 if (nodes.size() >= 2) {
00224 ScanNode* first = nodes[nodes.size()-2];
00225 ScanNode* second = nodes[nodes.size()-1];
00226 pose6d c = (first->pose).inv() * second->pose;
00227 this->addEdge(first, second, c);
00228 }
00229 }
00230
00231
00232 void ScanGraph::exportDot(std::string filename) {
00233 std::ofstream outfile (filename.c_str());
00234 outfile << "graph ScanGraph" << std::endl;
00235 outfile << "{" << std::endl;
00236 for (unsigned int i=0; i<edges.size(); i++) {
00237 outfile << (edges[i]->first)->id
00238 << " -- "
00239 << (edges[i]->second)->id
00240 << " [label="
00241 << std::fixed << std::setprecision(2) << edges[i]->constraint.transLength()
00242 << "]" << std::endl;
00243 }
00244 outfile << "}" << std::endl;
00245 outfile.close();
00246 }
00247
00248 ScanNode* ScanGraph::getNodeByID(unsigned int id) {
00249 for (unsigned int i = 0; i < nodes.size(); i++) {
00250 if (nodes[i]->id == id) return nodes[i];
00251 }
00252 return NULL;
00253 }
00254
00255 bool ScanGraph::edgeExists(unsigned int first_id, unsigned int second_id) {
00256
00257 for (unsigned int i=0; i<edges.size(); i++) {
00258 if (
00259 (((edges[i]->first)->id == first_id) && ((edges[i]->second)->id == second_id))
00260 ||
00261 (((edges[i]->first)->id == second_id) && ((edges[i]->second)->id == first_id))) {
00262 return true;
00263 }
00264 }
00265 return false;
00266 }
00267
00268 std::vector<unsigned int> ScanGraph::getNeighborIDs(unsigned int id) {
00269 std::vector<unsigned int> res;
00270 ScanNode* node = getNodeByID(id);
00271 if (node) {
00272
00273 for (unsigned int i = 0; i < nodes.size(); i++) {
00274 if (node->id == nodes[i]->id) continue;
00275 if (edgeExists(id, nodes[i]->id)) {
00276 res.push_back(nodes[i]->id);
00277 }
00278 }
00279 }
00280 return res;
00281 }
00282
00283 std::vector<ScanEdge*> ScanGraph::getOutEdges(ScanNode* node) {
00284 std::vector<ScanEdge*> res;
00285 if (node) {
00286 for (std::vector<ScanEdge*>::iterator it = edges.begin(); it != edges.end(); it++) {
00287 if ((*it)->first == node) {
00288 res.push_back(*it);
00289 }
00290 }
00291 }
00292 return res;
00293 }
00294
00295 std::vector<ScanEdge*> ScanGraph::getInEdges(ScanNode* node) {
00296 std::vector<ScanEdge*> res;
00297 if (node) {
00298 for (std::vector<ScanEdge*>::iterator it = edges.begin(); it != edges.end(); it++) {
00299 if ((*it)->second == node) {
00300 res.push_back(*it);
00301 }
00302 }
00303 }
00304 return res;
00305 }
00306
00307 void ScanGraph::transformScans() {
00308 for(ScanGraph::iterator it=this->begin(); it != this->end(); it++) {
00309 ((*it)->scan)->transformAbsolute((*it)->pose);
00310 }
00311 }
00312
00313 bool ScanGraph::writeBinary(const std::string& filename) const {
00314 std::ofstream binary_outfile( filename.c_str(), std::ios_base::binary);
00315 if (!binary_outfile.is_open()){
00316 OCTOMAP_ERROR_STR("Filestream to "<< filename << " not open, nothing written.");
00317 return false;
00318 }
00319 writeBinary(binary_outfile);
00320 binary_outfile.close();
00321 return true;
00322 }
00323
00324 std::ostream& ScanGraph::writeBinary(std::ostream &s) const {
00325
00326
00327
00328
00329 unsigned int graph_size = this->size();
00330 if (graph_size) OCTOMAP_DEBUG("writing %d nodes to binary file...\n", graph_size);
00331 s.write((char*)&graph_size, sizeof(graph_size));
00332
00333 for (ScanGraph::const_iterator it = this->begin(); it != this->end(); it++) {
00334 (*it)->writeBinary(s);
00335 }
00336
00337 if (graph_size) OCTOMAP_DEBUG("done.\n");
00338
00339
00340 unsigned int num_edges = this->edges.size();
00341 if (num_edges) OCTOMAP_DEBUG("writing %d edges to binary file...\n", num_edges);
00342 s.write((char*)&num_edges, sizeof(num_edges));
00343
00344 for (ScanGraph::const_edge_iterator it = this->edges_begin(); it != this->edges_end(); it++) {
00345 (*it)->writeBinary(s);
00346 }
00347
00348 if (num_edges) OCTOMAP_DEBUG(" done.\n");
00349
00350 return s;
00351 }
00352
00353 bool ScanGraph::readBinary(const std::string& filename) {
00354 std::ifstream binary_infile(filename.c_str(), std::ios_base::binary);
00355 if (!binary_infile.is_open()){
00356 OCTOMAP_ERROR_STR("Filestream to "<< filename << " not open, nothing read.");
00357 return false;
00358 }
00359 readBinary(binary_infile);
00360 binary_infile.close();
00361 return true;
00362 }
00363
00364 std::istream& ScanGraph::readBinary(std::ifstream &s) {
00365 if (!s.is_open()){
00366 OCTOMAP_ERROR_STR("Could not read from input filestream in ScanGraph::readBinary, exiting!");
00367 exit(0);
00368 } else if (!s.good()){
00369 OCTOMAP_WARNING_STR("Input filestream not \"good\" in ScanGraph::readBinary");
00370 }
00371 this->clear();
00372
00373
00374 unsigned int graph_size = 0;
00375 s.read((char*)&graph_size, sizeof(graph_size));
00376 if (graph_size) OCTOMAP_DEBUG("reading %d nodes from binary file...\n", graph_size);
00377
00378 if (graph_size > 0) {
00379 this->nodes.reserve(graph_size);
00380
00381 for (unsigned int i=0; i<graph_size; i++) {
00382
00383 ScanNode* node = new ScanNode();
00384 node->readBinary(s);
00385 if (!s.fail()) {
00386 this->nodes.push_back(node);
00387 }
00388 else {
00389 OCTOMAP_ERROR("ScanGraph::readBinary: ERROR.\n" );
00390 break;
00391 }
00392 }
00393 }
00394 if (graph_size) OCTOMAP_DEBUG("done.\n");
00395
00396
00397 unsigned int num_edges = 0;
00398 s.read((char*)&num_edges, sizeof(num_edges));
00399 if (num_edges) OCTOMAP_DEBUG("reading %d edges from binary file...\n", num_edges);
00400
00401 if (num_edges > 0) {
00402 this->edges.reserve(num_edges);
00403
00404 for (unsigned int i=0; i<num_edges; i++) {
00405
00406 ScanEdge* edge = new ScanEdge();
00407 edge->readBinary(s, *this);
00408 if (!s.fail()) {
00409 this->edges.push_back(edge);
00410 }
00411 else {
00412 OCTOMAP_ERROR("ScanGraph::readBinary: ERROR.\n" );
00413 break;
00414 }
00415 }
00416 }
00417 if (num_edges) OCTOMAP_DEBUG("done.\n");
00418 return s;
00419 }
00420
00421 void ScanGraph::readPlainASCII(const std::string& filename){
00422 std::ifstream infile(filename.c_str());
00423 if (!infile.is_open()){
00424 OCTOMAP_ERROR_STR("Filestream to "<< filename << " not open, nothing read.");
00425 return;
00426 }
00427 readPlainASCII(infile);
00428 infile.close();
00429 }
00430
00431 std::istream& ScanGraph::readPlainASCII(std::istream& s){
00432 std::string currentLine;
00433 ScanNode* currentNode = NULL;
00434 while (true){
00435 getline(s, currentLine);
00436 if (s.good() && !s.eof()){
00437 std::stringstream ss;
00438 ss << currentLine;
00439
00440 if (currentLine.size() == 0
00441 || (currentLine.compare(0,1, "#") == 0)
00442 || (currentLine.compare(0,1, " ") == 0)){
00443
00444 continue;
00445 } else if(currentLine.compare(0,4,"NODE")==0){
00446 if (currentNode){
00447 this->nodes.push_back(currentNode);
00448 this->connectPrevious();
00449 OCTOMAP_DEBUG_STR("ScanNode "<< currentNode->pose << " done, size: "<< currentNode->scan->size());
00450 }
00451
00452 currentNode = new ScanNode();
00453 currentNode->scan = new Pointcloud();
00454
00455 float x, y, z, roll, pitch, yaw;
00456 std::string tmp;
00457 ss >> tmp >> x >> y >> z >> roll >> pitch >> yaw;
00458 pose6d pose(x, y, z, roll, pitch, yaw);
00459
00460 currentNode->pose = pose;
00461 } else{
00462 if (currentNode == NULL){
00463
00464 OCTOMAP_ERROR_STR("Error parsing log file, no Scan to add point to!");
00465 break;
00466 }
00467 float x, y, z;
00468 ss >> x >> y >> z;
00469
00470
00471 currentNode->scan->push_back(x,y,z);
00472 }
00473 } else{
00474 if (currentNode){
00475 this->nodes.push_back(currentNode);
00476 this->connectPrevious();
00477 OCTOMAP_DEBUG_STR("Final ScanNode "<< currentNode->pose << " done, size: "<< currentNode->scan->size());
00478 }
00479 break;
00480 }
00481 }
00482
00483 return s;
00484 }
00485
00486 std::ostream& ScanGraph::writeEdgesASCII(std::ostream &s) const {
00487
00488
00489
00490 OCTOMAP_DEBUG_STR("Writing " << this->edges.size() << " edges to ASCII file...");
00491
00492 s << " " << this->edges.size();
00493 s << std::endl;
00494
00495 for (ScanGraph::const_edge_iterator it = this->edges_begin(); it != this->edges_end(); it++) {
00496 (*it)->writeASCII(s);
00497 }
00498 s << std::endl;
00499 OCTOMAP_DEBUG_STR("Done.");
00500
00501 return s;
00502 }
00503
00504
00505 std::istream& ScanGraph::readEdgesASCII(std::istream &s) {
00506
00507 unsigned int num_edges = 0;
00508 s >> num_edges;
00509 OCTOMAP_DEBUG("Reading %d edges from ASCII file...\n", num_edges);
00510
00511 if (num_edges > 0) {
00512
00513 for (unsigned int i=0; i<this->edges.size(); i++) delete edges[i];
00514 this->edges.clear();
00515
00516 this->edges.reserve(num_edges);
00517
00518 for (unsigned int i=0; i<num_edges; i++) {
00519
00520 ScanEdge* edge = new ScanEdge();
00521 edge->readASCII(s, *this);
00522 if (!s.fail()) {
00523 this->edges.push_back(edge);
00524 }
00525 else {
00526 OCTOMAP_ERROR("ScanGraph::readBinary: ERROR.\n" );
00527 break;
00528 }
00529 }
00530 }
00531
00532 OCTOMAP_DEBUG("done.\n");
00533
00534 return s;
00535 }
00536
00537
00538 std::ostream& ScanGraph::writeNodePosesASCII(std::ostream &s) const {
00539
00540 OCTOMAP_DEBUG("Writing %d node poses to ASCII file...\n", this->size());
00541
00542 for (ScanGraph::const_iterator it = this->begin(); it != this->end(); it++) {
00543 (*it)->writePoseASCII(s);
00544 }
00545 s << std::endl;
00546 OCTOMAP_DEBUG("done.\n");
00547
00548 return s;
00549 }
00550
00551 std::istream& ScanGraph::readNodePosesASCII(std::istream &s) {
00552
00553 for (ScanGraph::const_iterator it = this->begin(); it != this->end(); it++) {
00554 (*it)->readPoseASCII(s);
00555 }
00556
00557 for (ScanGraph::edge_iterator it = this->edges_begin(); it != this->edges_end(); it++) {
00558 ScanNode* first = (*it)->first;
00559 ScanNode* second = (*it)->second;
00560 (*it)->constraint = (first->pose).inv() * second->pose;
00561 }
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579 return s;
00580 }
00581
00582
00583 void ScanGraph::cropEachScan(point3d lowerBound, point3d upperBound) {
00584
00585 for (ScanGraph::iterator it = this->begin(); it != this->end(); it++) {
00586 ((*it)->scan)->crop(lowerBound, upperBound);
00587 }
00588 }
00589
00590
00591 void ScanGraph::crop(point3d lowerBound, point3d upperBound) {
00592
00593
00594
00595 for (ScanGraph::iterator it = this->begin(); it != this->end(); it++) {
00596 pose6d scan_pose = (*it)->pose;
00597 Pointcloud* pc = new Pointcloud((*it)->scan);
00598 pc->transformAbsolute(scan_pose);
00599 pc->crop(lowerBound, upperBound);
00600 pc->transform(scan_pose.inv());
00601 delete (*it)->scan;
00602 (*it)->scan = pc;
00603 }
00604 }
00605
00606 unsigned int ScanGraph::getNumPoints(unsigned int max_id) const {
00607 unsigned int retval = 0;
00608
00609 for (ScanGraph::const_iterator it = this->begin(); it != this->end(); it++) {
00610 retval += (*it)->scan->size();
00611 if ((max_id > 0) && ((*it)->id == max_id)) break;
00612 }
00613 return retval;
00614 }
00615
00616
00617 }
00618
00619
00620