ScanGraph.h
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 #ifndef OCTOMAP_SCANGRAPH_H
00035 #define OCTOMAP_SCANGRAPH_H
00036 
00037 
00038 #include <string>
00039 #include <math.h>
00040 
00041 #include "Pointcloud.h"
00042 #include "octomap_types.h"
00043 
00044 namespace octomap {
00045 
00046   class ScanGraph;
00047 
00048 
00052   class ScanNode {
00053 
00054    public:
00055 
00056     ScanNode (Pointcloud* _scan, pose6d _pose, unsigned int _id)
00057       : scan(_scan), pose(_pose), id(_id) {}
00058     ScanNode ()
00059       : scan(NULL) {}
00060 
00061     ~ScanNode();
00062 
00063     bool operator == (const ScanNode& other) {
00064       return (id == other.id);
00065     }
00066 
00067     std::ostream& writeBinary(std::ostream &s) const;
00068     std::istream& readBinary(std::istream &s);
00069 
00070     std::ostream& writePoseASCII(std::ostream &s) const;
00071     std::istream& readPoseASCII(std::istream &s);
00072 
00073     Pointcloud* scan;
00074     pose6d pose; 
00075     unsigned int id;
00076 
00077   };
00078 
00082   class ScanEdge {
00083 
00084    public:
00085 
00086     ScanEdge(ScanNode* _first, ScanNode* _second, pose6d _constraint)
00087       : first(_first), second(_second), constraint(_constraint), weight(1.0) { }
00088     ScanEdge() {}
00089 
00090     bool operator == (const ScanEdge& other) {
00091       return ( (*first == *(other.first) ) && ( *second == *(other.second) ) );
00092     }
00093 
00094     std::ostream& writeBinary(std::ostream &s) const;
00095     // a graph has to be given to recover ScanNode pointers
00096     std::istream& readBinary(std::istream &s, ScanGraph& graph);
00097 
00098     std::ostream& writeASCII(std::ostream &s) const;
00099     std::istream& readASCII(std::istream &s, ScanGraph& graph);
00100 
00101     ScanNode* first;
00102     ScanNode* second;
00103 
00104     pose6d constraint;
00105     double weight;
00106   };
00107 
00108 
00114   class ScanGraph {
00115 
00116    public:
00117 
00118     ScanGraph() {};
00119     ~ScanGraph();
00120 
00122     void clear();
00123 
00132     ScanNode* addNode(Pointcloud* scan, pose6d pose);
00133 
00143     ScanEdge* addEdge(ScanNode* first, ScanNode* second, pose6d constraint);
00144 
00145     ScanEdge* addEdge(unsigned int first_id, unsigned int second_id);
00146 
00148     ScanNode* getNodeByID(unsigned int id);
00149 
00151     bool edgeExists(unsigned int first_id, unsigned int second_id);
00152 
00154     void connectPrevious();
00155 
00156     std::vector<unsigned int> getNeighborIDs(unsigned int id);
00157     std::vector<ScanEdge*> getOutEdges(ScanNode* node);
00158     // warning: constraints are reversed
00159     std::vector<ScanEdge*> getInEdges(ScanNode* node);
00160 
00161     void exportDot(std::string filename);
00162 
00164     void transformScans();
00165 
00167     void crop(point3d lowerBound, point3d upperBound);
00168 
00170     void cropEachScan(point3d lowerBound, point3d upperBound);
00171 
00172 
00173     typedef std::vector<ScanNode*>::iterator iterator;
00174     typedef std::vector<ScanNode*>::const_iterator const_iterator;
00175     iterator begin() { return nodes.begin(); }
00176     iterator end()   { return nodes.end(); }
00177     const_iterator begin() const { return nodes.begin(); }
00178     const_iterator end() const { return nodes.end(); }
00179 
00180     unsigned int size() const { return nodes.size(); }
00181     unsigned int getNumPoints(unsigned int max_id = -1) const;
00182 
00183     typedef std::vector<ScanEdge*>::iterator edge_iterator;
00184     typedef std::vector<ScanEdge*>::const_iterator const_edge_iterator;
00185     edge_iterator edges_begin() { return edges.begin(); }
00186     edge_iterator edges_end()  { return edges.end(); }
00187     const_edge_iterator edges_begin() const { return edges.begin(); }
00188     const_edge_iterator edges_end() const  { return edges.end(); }
00189 
00190 
00191     std::ostream& writeBinary(std::ostream &s) const;
00192     std::istream& readBinary(std::ifstream &s);
00193     bool writeBinary(const std::string& filename) const;
00194     bool readBinary(const std::string& filename);
00195 
00196 
00197     std::ostream& writeEdgesASCII(std::ostream &s) const;
00198     std::istream& readEdgesASCII(std::istream &s);
00199 
00200     std::ostream& writeNodePosesASCII(std::ostream &s) const;
00201     std::istream& readNodePosesASCII(std::istream &s);
00202 
00219     std::istream& readPlainASCII(std::istream& s);
00220     void readPlainASCII(const std::string& filename);
00221 
00222    protected:
00223 
00224     std::vector<ScanNode*> nodes;
00225     std::vector<ScanEdge*> edges;
00226   };
00227 
00228 }
00229 
00230 
00231 #endif


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Feb 11 2016 23:50:59