58 s.write((
char*)&
id,
sizeof(
id));
70 s.read((
char*)&this->
id,
sizeof(this->
id));
89 if (read_id != this->
id)
90 OCTOMAP_ERROR(
"ERROR while reading ScanNode pose from ASCII. id %d does not match real id %d.\n", read_id, this->
id);
106 s.write((
char*)&first->id,
sizeof(first->id));
107 s.write((
char*)&second->id,
sizeof(second->id));
108 constraint.writeBinary(s);
109 s.write((
char*)&weight,
sizeof(weight));
114 unsigned int first_id, second_id;
115 s.read((
char*)&first_id,
sizeof(first_id));
116 s.read((
char*)&second_id,
sizeof(second_id));
119 if (this->first == NULL)
OCTOMAP_ERROR(
"ERROR while reading ScanEdge. first node not found.\n");
121 if (this->second == NULL)
OCTOMAP_ERROR(
"ERROR while reading ScanEdge. second node not found.\n");
123 this->constraint.readBinary(s);
124 s.read((
char*)&weight,
sizeof(weight));
134 s <<
" " << first->id <<
" " << second->id;
144 unsigned int first_id, second_id;
149 if (this->first == NULL)
OCTOMAP_ERROR(
"ERROR while reading ScanEdge. first node %d not found.\n", first_id);
151 if (this->second == NULL)
OCTOMAP_ERROR(
"ERROR while reading ScanEdge. second node %d not found.\n", second_id);
153 this->constraint.read(s);
164 for (
unsigned int i=0; i<nodes.size(); i++) {
168 for (
unsigned int i=0; i<edges.size(); i++) {
177 nodes.push_back(
new ScanNode(scan, pose, (
unsigned int) nodes.size()));
189 if ((first != 0) && (second != 0)) {
190 edges.push_back(
new ScanEdge(first, second, constraint));
203 if ( this->edgeExists(first_id, second_id)) {
208 ScanNode* first = getNodeByID(first_id);
209 ScanNode* second = getNodeByID(second_id);
211 if ((first != 0) && (second != 0)) {
213 return this->addEdge(first, second, constr);
223 if (nodes.size() >= 2) {
224 ScanNode* first = nodes[nodes.size()-2];
225 ScanNode* second = nodes[nodes.size()-1];
227 this->addEdge(first, second, c);
233 std::ofstream outfile (filename.c_str());
234 outfile <<
"graph ScanGraph" << std::endl;
235 outfile <<
"{" << std::endl;
236 for (
unsigned int i=0; i<edges.size(); i++) {
237 outfile << (edges[i]->first)->
id 239 << (edges[i]->second)->id
241 << std::fixed << std::setprecision(2) << edges[i]->constraint.transLength()
244 outfile <<
"}" << std::endl;
249 for (
unsigned int i = 0; i < nodes.size(); i++) {
250 if (nodes[i]->
id ==
id)
return nodes[i];
257 for (
unsigned int i=0; i<edges.size(); i++) {
259 (((edges[i]->first)->
id == first_id) && ((edges[i]->second)->
id == second_id))
261 (((edges[i]->first)->
id == second_id) && ((edges[i]->second)->
id == first_id))) {
269 std::vector<unsigned int> res;
273 for (
unsigned int i = 0; i < nodes.size(); i++) {
274 if (node->
id == nodes[i]->id)
continue;
275 if (edgeExists(
id, nodes[i]->
id)) {
276 res.push_back(nodes[i]->
id);
284 std::vector<ScanEdge*> res;
286 for (std::vector<ScanEdge*>::iterator it = edges.begin(); it != edges.end(); it++) {
287 if ((*it)->first == node) {
296 std::vector<ScanEdge*> res;
298 for (std::vector<ScanEdge*>::iterator it = edges.begin(); it != edges.end(); it++) {
299 if ((*it)->second == node) {
309 ((*it)->scan)->transformAbsolute((*it)->pose);
314 std::ofstream binary_outfile( filename.c_str(), std::ios_base::binary);
315 if (!binary_outfile.is_open()){
320 binary_outfile.close();
330 unsigned int graph_size = (
unsigned int) this->size();
331 if (graph_size)
OCTOMAP_DEBUG(
"writing %u nodes to binary file...\n", graph_size);
332 s.write((
char*)&graph_size,
sizeof(graph_size));
335 (*it)->writeBinary(s);
341 unsigned int num_edges = (
unsigned int) this->edges.size();
342 if (num_edges)
OCTOMAP_DEBUG(
"writing %u edges to binary file...\n", num_edges);
343 s.write((
char*)&num_edges,
sizeof(num_edges));
346 (*it)->writeBinary(s);
355 std::ifstream binary_infile(filename.c_str(), std::ios_base::binary);
356 if (!binary_infile.is_open()){
361 binary_infile.close();
367 OCTOMAP_ERROR_STR(
"Could not read from input filestream in ScanGraph::readBinary");
369 }
else if (!s.good()){
375 unsigned int graph_size = 0;
376 s.read((
char*)&graph_size,
sizeof(graph_size));
377 if (graph_size)
OCTOMAP_DEBUG(
"reading %d nodes from binary file...\n", graph_size);
379 if (graph_size > 0) {
380 this->nodes.reserve(graph_size);
382 for (
unsigned int i=0; i<graph_size; i++) {
387 this->nodes.push_back(node);
398 unsigned int num_edges = 0;
399 s.read((
char*)&num_edges,
sizeof(num_edges));
400 if (num_edges)
OCTOMAP_DEBUG(
"reading %d edges from binary file...\n", num_edges);
403 this->edges.reserve(num_edges);
405 for (
unsigned int i=0; i<num_edges; i++) {
410 this->edges.push_back(edge);
423 std::ifstream infile(filename.c_str());
424 if (!infile.is_open()){
428 readPlainASCII(infile);
433 std::string currentLine;
436 getline(s, currentLine);
437 if (s.good() && !s.eof()){
438 std::stringstream ss;
441 if (currentLine.size() == 0
442 || (currentLine.compare(0,1,
"#") == 0)
443 || (currentLine.compare(0,1,
" ") == 0)){
446 }
else if(currentLine.compare(0,4,
"NODE")==0){
448 this->nodes.push_back(currentNode);
449 this->connectPrevious();
456 float x, y, z, roll, pitch, yaw;
458 ss >> tmp >> x >> y >> z >> roll >> pitch >> yaw;
461 currentNode->pose =
pose;
463 if (currentNode == NULL){
476 this->nodes.push_back(currentNode);
477 this->connectPrevious();
491 OCTOMAP_DEBUG_STR(
"Writing " << this->edges.size() <<
" edges to ASCII file...");
493 s <<
" " << this->edges.size();
497 (*it)->writeASCII(s);
508 unsigned int num_edges = 0;
510 OCTOMAP_DEBUG(
"Reading %d edges from ASCII file...\n", num_edges);
514 for (
unsigned int i=0; i<this->edges.size(); i++)
delete edges[i];
517 this->edges.reserve(num_edges);
519 for (
unsigned int i=0; i<num_edges; i++) {
524 this->edges.push_back(edge);
541 OCTOMAP_DEBUG(
"Writing %lu node poses to ASCII file...\n", (
unsigned long) this->size());
544 (*it)->writePoseASCII(s);
555 (*it)->readPoseASCII(s);
561 (*it)->constraint = (first->
pose).inv() * second->
pose;
587 ((*it)->scan)->crop(lowerBound, upperBound);
597 pose6d scan_pose = (*it)->pose;
600 pc->
crop(lowerBound, upperBound);
611 retval += (*it)->scan->size();
612 if ((max_id > 0) && ((*it)->id == max_id))
break;
std::ostream & writeBinary(std::ostream &s) const
Binary output operator.
std::istream & readPoseASCII(std::istream &s)
std::ostream & write(std::ostream &s) const
ScanNode * addNode(Pointcloud *scan, pose6d pose)
std::istream & readBinary(std::istream &s, ScanGraph &graph)
std::istream & readASCII(std::istream &s, ScanGraph &graph)
std::istream & readBinary(std::ifstream &s)
void exportDot(std::string filename)
std::istream & readNodePosesASCII(std::istream &s)
#define OCTOMAP_DEBUG(...)
std::istream & read(std::istream &s)
std::vector< ScanNode * >::const_iterator const_iterator
Vector3 toEuler() const
Conversion to Euler angles.
void transform(pose6d transform)
Apply transform to each point.
ScanNode * getNodeByID(unsigned int id)
will return NULL if node was not found
pose6d pose
6D pose from which the scan was performed
std::istream & readBinary(std::istream &s)
std::istream & readBinary(std::istream &s)
Binary input operator.
void push_back(float x, float y, float z)
std::vector< ScanEdge * > getOutEdges(ScanNode *node)
std::ostream & writeBinary(std::ostream &s) const
void clear()
Clears all nodes and edges, and will delete the corresponding objects.
std::vector< ScanNode * >::iterator iterator
ScanEdge * addEdge(ScanNode *first, ScanNode *second, pose6d constraint)
Pose6D inv() const
Inversion.
std::ostream & writeBinary(std::ostream &s) const
std::ostream & writeBinary(std::ostream &s) const
std::ostream & writeEdgesASCII(std::ostream &s) const
void transformScans()
Transform every scan according to its pose.
std::vector< unsigned int > getNeighborIDs(unsigned int id)
void crop(point3d lowerBound, point3d upperBound)
Cut graph (all containing Pointclouds) to given BBX in global coords.
std::istream & readBinary(std::istream &s)
std::ostream & writeBinary(std::ostream &s) const
std::vector< ScanEdge * >::iterator edge_iterator
This class represents a tree-dimensional pose of an object.
void connectPrevious()
Connect previously added ScanNode to the one before that.
Vector3 & trans()
Translational component.
std::ostream & writeNodePosesASCII(std::ostream &s) const
This class represents a three-dimensional vector.
#define OCTOMAP_WARNING_STR(args)
std::istream & readPlainASCII(std::istream &s)
#define OCTOMAP_ERROR_STR(args)
std::vector< ScanEdge * >::const_iterator const_edge_iterator
#define OCTOMAP_DEBUG_STR(args)
void transformAbsolute(pose6d transform)
Apply transform to each point, undo previous transforms.
std::istream & readEdgesASCII(std::istream &s)
Quaternion & rot()
Rotational component.
void cropEachScan(point3d lowerBound, point3d upperBound)
Cut Pointclouds to given BBX in local coords.
void crop(point3d lowerBound, point3d upperBound)
Crop Pointcloud to given bounding box.
#define OCTOMAP_ERROR(...)
std::vector< ScanEdge * > getInEdges(ScanNode *node)
size_t getNumPoints(unsigned int max_id=-1) const
std::ostream & writePoseASCII(std::ostream &s) const
This class represents a Quaternion.
bool edgeExists(unsigned int first_id, unsigned int second_id)
std::ostream & writeASCII(std::ostream &s) const