Go to the documentation of this file.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 #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
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
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