419 lines
10 KiB
Text
419 lines
10 KiB
Text
|
/*
|
||
|
|
* parcelmanager implementation
|
||
|
|
*
|
||
|
|
* Copyright (C) Uwe Hebbelmann, Sebastian Stock, Andre Schemschat
|
||
|
|
*
|
||
|
|
* Released under the GPL version 3.
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "grid/parcelmanager.h"
|
||
|
|
#include "slam6d/globals.icc"
|
||
|
|
#include "grid/gridWriter.h"
|
||
|
|
#include <fstream>
|
||
|
|
using std::ifstream;
|
||
|
|
using std::ofstream;
|
||
|
|
#include <iostream>
|
||
|
|
using std::cerr;
|
||
|
|
using std::endl;
|
||
|
|
|
||
|
|
/**
|
||
|
|
* Ctor.
|
||
|
|
* Sets the static size information of the parcelinfo-class.
|
||
|
|
* If resume is true, it will try to load the last parcelinfo-file.
|
||
|
|
* If the file was read successfully, it contains the parcels created
|
||
|
|
* during the last run of the programm. (See the resumeflag in the
|
||
|
|
* documentation for more infos)
|
||
|
|
*
|
||
|
|
*
|
||
|
|
* @param width The width of each parcel
|
||
|
|
* @param height The height of each parcel
|
||
|
|
* @param path The path of the files
|
||
|
|
* @param resolution The resolution of a cell
|
||
|
|
* @param resume If true, last parcelinfofile will be loaded
|
||
|
|
*/
|
||
|
|
parcelmanager::parcelmanager(long width, long height,
|
||
|
|
string path, int resolution,
|
||
|
|
bool resume)
|
||
|
|
{
|
||
|
|
this->parcelwidth = width;
|
||
|
|
this->parcelheight = height;
|
||
|
|
this->viewpointX = 0;
|
||
|
|
this->viewpointZ = 0;
|
||
|
|
this->path = path;
|
||
|
|
this->resolution = resolution;
|
||
|
|
|
||
|
|
this->minX = 0;
|
||
|
|
this->maxX = 0;
|
||
|
|
this->minZ = 0;
|
||
|
|
this->maxZ = 0;
|
||
|
|
|
||
|
|
parcelinfo::setParcelsize(width, height);
|
||
|
|
|
||
|
|
// loadParcelinfo if programm should resume
|
||
|
|
if(resume)
|
||
|
|
loadParcelinfo(this->path + PARCELINFOFILE);
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* Dtor.
|
||
|
|
* Calls saveParcelinfo for saving infos about
|
||
|
|
* already created parcel.
|
||
|
|
* It also clears the internal structur (calls clear())
|
||
|
|
*/
|
||
|
|
parcelmanager::~parcelmanager()
|
||
|
|
{
|
||
|
|
saveParcelinfo(this->path + PARCELINFOFILE);
|
||
|
|
|
||
|
|
clear();
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* This methods removes all parcels with a not set
|
||
|
|
* used-flag (in parcelinfo). They are saved to disk
|
||
|
|
* before they are freed.
|
||
|
|
* If all is set, all parcels are saved and free regardless
|
||
|
|
* of their used-flag
|
||
|
|
*
|
||
|
|
* @param all Delete all parcels or not?
|
||
|
|
*/
|
||
|
|
void parcelmanager::freeMemory(bool all)
|
||
|
|
{
|
||
|
|
parcelmap::iterator cur = this->parcels.begin();
|
||
|
|
parcelmap::iterator end = this->parcels.end();
|
||
|
|
|
||
|
|
// iterate through each parcel
|
||
|
|
while(cur != end)
|
||
|
|
{
|
||
|
|
// if parcel is loaded and wasnt used (or all free)
|
||
|
|
if(cur->second != NULL &&
|
||
|
|
(!cur->first->wasUsed() || all))
|
||
|
|
{
|
||
|
|
// saving the parcel using the parcel format
|
||
|
|
// (Must use parcelFormat, otherwise parcel cant be loaded again!)
|
||
|
|
string filename = cur->first->getFilename();
|
||
|
|
parcelWriter writer(filename);
|
||
|
|
|
||
|
|
writer.write(*cur->second);
|
||
|
|
|
||
|
|
delete cur->second;
|
||
|
|
cur->second = NULL;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
// reset used-flag of parcelinfo
|
||
|
|
parcelinfo *p = cur->first;
|
||
|
|
p->resetUsed();
|
||
|
|
|
||
|
|
++cur;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* The method frees all created parcelinfos and clears the map.
|
||
|
|
* First it calls freeMemory(true) to save all parcels in memory
|
||
|
|
*/
|
||
|
|
void parcelmanager::clear()
|
||
|
|
{
|
||
|
|
freeMemory(true);
|
||
|
|
|
||
|
|
parcelmap::iterator it = this->parcels.begin();
|
||
|
|
parcelmap::iterator end = this->parcels.end();
|
||
|
|
|
||
|
|
while(it != end)
|
||
|
|
{
|
||
|
|
delete it->first;
|
||
|
|
++it;
|
||
|
|
}
|
||
|
|
|
||
|
|
this->parcels.clear();
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* The methods loads the parcel specified by info
|
||
|
|
* into memory. The method does check, if the
|
||
|
|
* parcel is already loaded, so calling this method
|
||
|
|
* on an already loaded parcel does nothing.
|
||
|
|
*
|
||
|
|
* @param *info A pointer to the parcelinfo
|
||
|
|
*/
|
||
|
|
void parcelmanager::loadParcel(parcelinfo* info)
|
||
|
|
{
|
||
|
|
if(this->parcels[info] == NULL)
|
||
|
|
this->parcels[info] = parcel::readParcel(info->getFilename());
|
||
|
|
|
||
|
|
updateOuterPoints(this->parcels[info]);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* The method creates a new parcel. It calculates the offset
|
||
|
|
* of the new parcel based on the absolute coordinates it is given.
|
||
|
|
* The new parcel is added to the internal map, along with its
|
||
|
|
* parcelinfo
|
||
|
|
*
|
||
|
|
* @param x The absolute x coordinate
|
||
|
|
* @param z The absolute z coordinate
|
||
|
|
*/
|
||
|
|
void parcelmanager::createParcel(long x, long z)
|
||
|
|
{
|
||
|
|
// calculate offset
|
||
|
|
long offsetX = (long) (x / this->parcelwidth) * this->parcelwidth;
|
||
|
|
if (x < 0)
|
||
|
|
offsetX = offsetX - this->parcelwidth;
|
||
|
|
|
||
|
|
long offsetZ = (long) (z/ this->parcelheight) * this->parcelheight;
|
||
|
|
if (z < 0)
|
||
|
|
offsetZ = offsetZ - this->parcelheight;
|
||
|
|
|
||
|
|
|
||
|
|
// create parcelinfo and parcel
|
||
|
|
string filename = this->path + "parcel" + to_string(offsetX) + to_string(offsetZ) + ".pcl";
|
||
|
|
|
||
|
|
parcelinfo *p = new parcelinfo(offsetX, offsetZ, filename);
|
||
|
|
|
||
|
|
this->parcels[p] = new parcel(offsetX,
|
||
|
|
offsetZ,
|
||
|
|
this->parcelwidth,
|
||
|
|
this->parcelheight);
|
||
|
|
|
||
|
|
// update the new borders
|
||
|
|
updateOuterPoints(this->parcels[p]);
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* Updates the minimal and maximal found limits
|
||
|
|
* @param g The grid to update
|
||
|
|
*/
|
||
|
|
void parcelmanager::updateOuterPoints(const grid *g)
|
||
|
|
{
|
||
|
|
if(g->getOffsetX() < minX)
|
||
|
|
minX = g->getOffsetX();
|
||
|
|
if(g->getOffsetZ() < minZ)
|
||
|
|
minZ = g->getOffsetZ();
|
||
|
|
|
||
|
|
if(g->getOffsetX() + g->getSizeX() > maxX)
|
||
|
|
maxX = g->getOffsetX() + g->getSizeX();
|
||
|
|
if(g->getOffsetZ() + g->getSizeZ() > maxZ)
|
||
|
|
maxZ = g->getOffsetZ() + g->getSizeZ();
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* The method adds the given grid to the needed parcels.
|
||
|
|
* It checks which parcels are affected by the grid, loads them if
|
||
|
|
* they already exist or creates new ones.
|
||
|
|
* Afterwards it adds the information of the grid to each relevant
|
||
|
|
* parcels
|
||
|
|
*
|
||
|
|
* @param g The grid which should be addeda
|
||
|
|
* @param vpX The x-coordiante of the viewpoint
|
||
|
|
* @param vpZ The z-coordinate of the viewpoint
|
||
|
|
*/
|
||
|
|
void parcelmanager::addGrid(const grid* g, long vpX, long vpZ)
|
||
|
|
{
|
||
|
|
bool parcelFound;
|
||
|
|
|
||
|
|
this->viewpointX = vpX;
|
||
|
|
this->viewpointZ = vpZ;
|
||
|
|
|
||
|
|
long iSize = g->getOffsetX() + g->getSizeX();
|
||
|
|
long jSize = g->getOffsetZ() + g->getSizeZ();
|
||
|
|
|
||
|
|
// iterate through all parcels needed in this grid
|
||
|
|
for(int i=g->getOffsetX() ; i < iSize + this->parcelwidth; i += this->parcelwidth)
|
||
|
|
{
|
||
|
|
for(int j=g->getOffsetZ() ; j < jSize + this->parcelheight; j += this->parcelheight)
|
||
|
|
{
|
||
|
|
parcelmap::iterator it = this->parcels.begin();
|
||
|
|
parcelmap::iterator end = this->parcels.end();
|
||
|
|
|
||
|
|
parcelFound = false;
|
||
|
|
|
||
|
|
// for each point, iterate through the existing parcels
|
||
|
|
while(it != end)
|
||
|
|
{
|
||
|
|
// parcel contains point
|
||
|
|
if(it->first->contains(i, j))
|
||
|
|
{
|
||
|
|
// load Parcel if not in memory
|
||
|
|
if(it->second == NULL)
|
||
|
|
loadParcel(it->first);
|
||
|
|
|
||
|
|
// reset the counter
|
||
|
|
it->first->setUsed();
|
||
|
|
parcelFound = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
++it;
|
||
|
|
}
|
||
|
|
|
||
|
|
// if parcel was not found, create new
|
||
|
|
if(parcelFound == false)
|
||
|
|
createParcel(i, j);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// remove all parcels not needed for the scan
|
||
|
|
freeMemory(false);
|
||
|
|
|
||
|
|
|
||
|
|
// Call addGrid for each parcel in memory
|
||
|
|
// The parcel only integrates the points which are
|
||
|
|
// relevant to it
|
||
|
|
parcelmap::iterator it = this->parcels.begin();
|
||
|
|
parcelmap::iterator end = this->parcels.end();
|
||
|
|
|
||
|
|
while(it != end)
|
||
|
|
{
|
||
|
|
if(it->second != NULL)
|
||
|
|
it->second->addGrid(g);
|
||
|
|
|
||
|
|
++it;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* The method saves the internal parcelinfo map to file,
|
||
|
|
* so it can be restored for later use (e.g. for additional scans
|
||
|
|
* based on the same scene)
|
||
|
|
*
|
||
|
|
* @param filename The file where the parcelinfos are stored to
|
||
|
|
*/
|
||
|
|
void parcelmanager::saveParcelinfo(string filename)
|
||
|
|
{
|
||
|
|
ofstream outfile(filename.c_str());
|
||
|
|
|
||
|
|
// Check if stream is open and valid
|
||
|
|
if(!outfile.good())
|
||
|
|
{
|
||
|
|
cerr << "[saveParcelinfo] Fehler, konnte Stream nicht oeffnen" << endl
|
||
|
|
<< "(Filename: " << filename << ")" << endl;
|
||
|
|
exit(1);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// iterate all entries and write them
|
||
|
|
parcelmap::iterator it = this->parcels.begin();
|
||
|
|
parcelmap::iterator end = this->parcels.end();
|
||
|
|
|
||
|
|
while(it != end)
|
||
|
|
{
|
||
|
|
outfile << it->first->getOffsetX() << " "
|
||
|
|
<< it->first->getOffsetZ() << " "
|
||
|
|
<< it->first->getFilename() << endl;
|
||
|
|
|
||
|
|
++it;
|
||
|
|
}
|
||
|
|
|
||
|
|
outfile.close();
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* The method restores the parcelinfos written by saveParcelinfo.
|
||
|
|
*
|
||
|
|
* @param filename The file where the parcelinfos are stored
|
||
|
|
*/
|
||
|
|
void parcelmanager::loadParcelinfo(string filename)
|
||
|
|
{
|
||
|
|
ifstream infile(filename.c_str());
|
||
|
|
|
||
|
|
// Stream ok?
|
||
|
|
if(!infile.good())
|
||
|
|
{
|
||
|
|
cerr << "[loadparcelinfo] Fehler, konnte Stream nicht oeffnen" << endl
|
||
|
|
<< "(filename: " << filename << ")" << endl;
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
// clear old information (should be already empty, but make sure)
|
||
|
|
clear();
|
||
|
|
|
||
|
|
long offsetX;
|
||
|
|
long offsetZ;
|
||
|
|
string file;
|
||
|
|
|
||
|
|
// read all infos
|
||
|
|
while(!infile.eof())
|
||
|
|
{
|
||
|
|
infile >> offsetX;
|
||
|
|
if(infile.eof()) continue;
|
||
|
|
infile >> offsetZ;
|
||
|
|
if(infile.eof()) continue;
|
||
|
|
infile >> file;
|
||
|
|
if(infile.eof()) continue;
|
||
|
|
|
||
|
|
parcelinfo *p = new parcelinfo(offsetX, offsetZ, file);
|
||
|
|
this->parcels[p] = NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
infile.close();
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* This method is able to write the entire world, based on all parcels and create and write lines.
|
||
|
|
* Each parcel is loaded and then written into a single file.
|
||
|
|
* Afterwards ALL parcels are stored to file and freed from memory.
|
||
|
|
* Additionally the world is written in ppm format and lines can be created and written.
|
||
|
|
*
|
||
|
|
* The map is written in a format which can be read by the MapViewer of Group2.
|
||
|
|
* The specification of the format are listed in the documentation
|
||
|
|
* @param file the filename of the world
|
||
|
|
*/
|
||
|
|
void parcelmanager::writeWorld(string file)
|
||
|
|
{
|
||
|
|
worldWriter writer(this->path + file,
|
||
|
|
this->minX, this->maxX, this->minZ, this->maxZ,
|
||
|
|
this->resolution,
|
||
|
|
this->viewpointX, this->viewpointZ);
|
||
|
|
|
||
|
|
|
||
|
|
parcelmap::iterator it = parcels.begin();
|
||
|
|
parcelmap::iterator end = parcels.end();
|
||
|
|
|
||
|
|
while(it != end)
|
||
|
|
{
|
||
|
|
if(it->second == NULL)
|
||
|
|
loadParcel(it->first);
|
||
|
|
|
||
|
|
writer.write(*it->second);
|
||
|
|
|
||
|
|
++it;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* The method creates a grid of the world.
|
||
|
|
*/
|
||
|
|
grid* parcelmanager::createWorldGrid()
|
||
|
|
{
|
||
|
|
grid *g = new grid(this->minX, this->minZ,
|
||
|
|
this->maxX - this->minX + 1,
|
||
|
|
this->maxZ - this->minZ + 1);
|
||
|
|
|
||
|
|
parcelmap::iterator it = parcels.begin();
|
||
|
|
parcelmap::iterator end = parcels.end();
|
||
|
|
|
||
|
|
// Go through all parcels
|
||
|
|
while(it != end)
|
||
|
|
{
|
||
|
|
if(it->second == NULL)
|
||
|
|
loadParcel(it->first);
|
||
|
|
|
||
|
|
for(int i=0; i < it->second->getSizeX(); ++i)
|
||
|
|
{
|
||
|
|
for(int j=0; j < it->second->getSizeZ(); ++j)
|
||
|
|
{
|
||
|
|
g->addPoint(*(it->second->points[i][j]));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
++it;
|
||
|
|
}
|
||
|
|
return g;
|
||
|
|
}
|
||
|
|
|