ScanGraph.cpp
Go to the documentation of this file.
00001 /*
00002  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
00003  * http://octomap.github.com/
00004  *
00005  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
00006  * All rights reserved.
00007  * License: New BSD
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions are met:
00011  *
00012  *     * Redistributions of source code must retain the above copyright
00013  *       notice, this list of conditions and the following disclaimer.
00014  *     * Redistributions in binary form must reproduce the above copyright
00015  *       notice, this list of conditions and the following disclaimer in the
00016  *       documentation and/or other materials provided with the distribution.
00017  *     * Neither the name of the University of Freiburg nor the names of its
00018  *       contributors may be used to endorse or promote products derived from
00019  *       this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00022  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00023  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00024  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00025  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00026  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00027  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00028  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00029  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00030  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  * POSSIBILITY OF SUCH DAMAGE.
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     // file structure:    pointcloud | pose | id
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;  // export pose for human editor
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     // read rotation from euler angles
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     // file structure:    first_id | second_id | constraint | weight
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     // file structure:    first_id | second_id | constraint | weight
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       //      OCTOMAP_DEBUG("ScanGraph::AddEdge %d --> %d\n", first->id, second->id);
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       // check all nodes
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     // file structure:    n | node_1 | ... | node_n | m | edge_1 | ... | edge_m
00327 
00328     // write nodes  ---------------------------------
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     // write edges  ---------------------------------
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     // read nodes  ---------------------------------
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     // read edges  ---------------------------------
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         // skip empty and comment lines:
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           //std::cout << "Pose "<< pose << " found.\n";
00460           currentNode->pose = pose;
00461         } else{
00462           if (currentNode == NULL){
00463             // TODO: allow "simple" pc files by setting initial Scan Pose to (0,0,0)
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           //std::cout << "Point "<< x << "," <<y <<"," <<z << " found.\n";
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     // file structure:    n | edge_1 | ... | edge_n
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     // constraints and nodes are inconsistent, rewire graph
00564 //     for (unsigned int i=0; i<this->edges.size(); i++) delete edges[i];
00565 //     this->edges.clear();
00566 
00567 
00568 //     ScanGraph::iterator first_it = this->begin();
00569 //     ScanGraph::iterator second_it = first_it+1;
00570 
00571 //     for ( ; second_it != this->end(); first_it++, second_it++) {
00572 //       ScanNode* first = (*first_it);
00573 //       ScanNode* second = (*second_it);
00574 //       octomath::Pose6D c =  (first->pose).inv() * second->pose;
00575 //       this->addEdge(first, second, c);
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     // for all node in graph...
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 } // end namespace
00618 
00619 
00620 


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Aug 27 2015 14:13:14