worked on 3D model creation
This commit is contained in:
k-a-z-u
@@ -7,229 +7,233 @@
|
||||
#include <functional>
|
||||
#include <mutex>
|
||||
|
||||
template <typename T> class DataMap3 {
|
||||
namespace Ray3D {
|
||||
|
||||
private:
|
||||
template <typename T> class DataMap3 {
|
||||
|
||||
float sx_m;
|
||||
float sy_m;
|
||||
float sz_m;
|
||||
private:
|
||||
|
||||
float x_m;
|
||||
float y_m;
|
||||
float z_m;
|
||||
float sx_m;
|
||||
float sy_m;
|
||||
float sz_m;
|
||||
|
||||
int gridSize_cm;
|
||||
float x_m;
|
||||
float y_m;
|
||||
float z_m;
|
||||
|
||||
BBox3 bbox;
|
||||
int gridSize_cm;
|
||||
|
||||
int nx;
|
||||
int ny;
|
||||
int nz;
|
||||
BBox3 bbox;
|
||||
|
||||
T* data = nullptr;
|
||||
int nx;
|
||||
int ny;
|
||||
int nz;
|
||||
|
||||
public:
|
||||
T* data = nullptr;
|
||||
|
||||
/** ctor */
|
||||
DataMap3() {
|
||||
;
|
||||
}
|
||||
public:
|
||||
|
||||
~DataMap3() {
|
||||
/** ctor */
|
||||
DataMap3() {
|
||||
;
|
||||
}
|
||||
|
||||
// cleanup
|
||||
cleanup();
|
||||
~DataMap3() {
|
||||
|
||||
}
|
||||
// cleanup
|
||||
cleanup();
|
||||
|
||||
DataMap3(const DataMap3&) = delete;
|
||||
DataMap3* operator = (const DataMap3& o) = delete;
|
||||
}
|
||||
|
||||
DataMap3(const DataMap3&) = delete;
|
||||
DataMap3* operator = (const DataMap3& o) = delete;
|
||||
|
||||
|
||||
/** does the map contain the given indices? */
|
||||
bool containsGrid(const int x, const int y, const int z) const {
|
||||
return (x >= 0) && (y >= 0) && (z >= 0) &&
|
||||
(x < nx) && (y < ny) && (z < nz);
|
||||
}
|
||||
/** does the map contain the given indices? */
|
||||
bool containsGrid(const int x, const int y, const int z) const {
|
||||
return (x >= 0) && (y >= 0) && (z >= 0) &&
|
||||
(x < nx) && (y < ny) && (z < nz);
|
||||
}
|
||||
|
||||
/** does the map contain the given coordinate? */
|
||||
bool contain(const float x_m, const float y_m, const float z_m) const {
|
||||
return bbox.contains(Point3(x_m, y_m, z_m));
|
||||
}
|
||||
/** does the map contain the given coordinate? */
|
||||
bool contain(const float x_m, const float y_m, const float z_m) const {
|
||||
return bbox.contains(Point3(x_m, y_m, z_m));
|
||||
}
|
||||
|
||||
void resize(const BBox3 bbox, const int gridSize_cm) {
|
||||
void resize(const BBox3 bbox, const int gridSize_cm) {
|
||||
|
||||
// cleanup
|
||||
cleanup();
|
||||
// cleanup
|
||||
cleanup();
|
||||
|
||||
this->bbox = bbox;
|
||||
this->bbox = bbox;
|
||||
|
||||
// slightly increase to pervent out-of-bounds due to rounding
|
||||
float buffer_m = 1;
|
||||
// slightly increase to pervent out-of-bounds due to rounding
|
||||
float buffer_m = 1;
|
||||
|
||||
// start-offset
|
||||
sx_m = bbox.getMin().x - buffer_m;
|
||||
sy_m = bbox.getMin().y - buffer_m;
|
||||
sz_m = bbox.getMin().z - buffer_m;
|
||||
// start-offset
|
||||
sx_m = bbox.getMin().x - buffer_m;
|
||||
sy_m = bbox.getMin().y - buffer_m;
|
||||
sz_m = bbox.getMin().z - buffer_m;
|
||||
|
||||
// size in meter
|
||||
x_m = (bbox.getMax().x - bbox.getMin().x) + 2*buffer_m;
|
||||
y_m = (bbox.getMax().y - bbox.getMin().y) + 2*buffer_m;
|
||||
z_m = (bbox.getMax().z - bbox.getMin().z) + 2*buffer_m;
|
||||
// size in meter
|
||||
x_m = (bbox.getMax().x - bbox.getMin().x) + 2*buffer_m;
|
||||
y_m = (bbox.getMax().y - bbox.getMin().y) + 2*buffer_m;
|
||||
z_m = (bbox.getMax().z - bbox.getMin().z) + 2*buffer_m;
|
||||
|
||||
// number of elements in the grid
|
||||
this->gridSize_cm = gridSize_cm;
|
||||
nx = (x_m*100) / gridSize_cm;
|
||||
ny = (y_m*100) / gridSize_cm;
|
||||
nz = (z_m*100) / gridSize_cm;
|
||||
// number of elements in the grid
|
||||
this->gridSize_cm = gridSize_cm;
|
||||
nx = (x_m*100) / gridSize_cm;
|
||||
ny = (y_m*100) / gridSize_cm;
|
||||
nz = (z_m*100) / gridSize_cm;
|
||||
|
||||
// allocate and reset all to 0.0
|
||||
data = new T[nx*ny*nz];
|
||||
// allocate and reset all to 0.0
|
||||
data = new T[nx*ny*nz];
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/** get the used grid-size (in cm) */
|
||||
int getGridSize_cm() const {return gridSize_cm;}
|
||||
/** get the used grid-size (in cm) */
|
||||
int getGridSize_cm() const {return gridSize_cm;}
|
||||
|
||||
void set(const float x_m, const float y_m, const float z_m, const T val) {
|
||||
const int ix = std::round( ((x_m-sx_m)) * 100 / gridSize_cm);
|
||||
const int iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm);
|
||||
const int iz = std::round( ((z_m-sz_m)) * 100 / gridSize_cm);
|
||||
setGrid(ix, iy, iz, val);
|
||||
}
|
||||
void set(const float x_m, const float y_m, const float z_m, const T val) {
|
||||
const int ix = std::round( ((x_m-sx_m)) * 100 / gridSize_cm);
|
||||
const int iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm);
|
||||
const int iz = std::round( ((z_m-sz_m)) * 100 / gridSize_cm);
|
||||
setGrid(ix, iy, iz, val);
|
||||
}
|
||||
|
||||
T get(const float x_m, const float y_m, const float z_m) const {
|
||||
const int ix = std::round( ((x_m-sx_m)) * 100 / gridSize_cm );
|
||||
const int iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm );
|
||||
const int iz = std::round( ((z_m-sz_m)) * 100 / gridSize_cm );
|
||||
return getGrid(ix, iy, iz);
|
||||
}
|
||||
T get(const float x_m, const float y_m, const float z_m) const {
|
||||
const int ix = std::round( ((x_m-sx_m)) * 100 / gridSize_cm );
|
||||
const int iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm );
|
||||
const int iz = std::round( ((z_m-sz_m)) * 100 / gridSize_cm );
|
||||
return getGrid(ix, iy, iz);
|
||||
}
|
||||
|
||||
T& getRef(const float x_m, const float y_m, const float z_m) {
|
||||
const int ix = std::round( ((x_m-sx_m)) * 100 / gridSize_cm );
|
||||
const int iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm );
|
||||
const int iz = std::round( ((z_m-sz_m)) * 100 / gridSize_cm );
|
||||
return getGridRef(ix, iy, iz);
|
||||
}
|
||||
T& getRef(const float x_m, const float y_m, const float z_m) {
|
||||
const int ix = std::round( ((x_m-sx_m)) * 100 / gridSize_cm );
|
||||
const int iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm );
|
||||
const int iz = std::round( ((z_m-sz_m)) * 100 / gridSize_cm );
|
||||
return getGridRef(ix, iy, iz);
|
||||
}
|
||||
|
||||
T getGrid(const int ix, const int iy, const int iz) const {
|
||||
Assert::isBetween(ix, 0, nx-1, "x out of range");
|
||||
Assert::isBetween(iy, 0, ny-1, "y out of range");
|
||||
Assert::isBetween(iz, 0, nz-1, "z out of range");
|
||||
const int idx = ix + iy*nx + iz*nx*ny;
|
||||
return data[idx];
|
||||
}
|
||||
T getGrid(const int ix, const int iy, const int iz) const {
|
||||
Assert::isBetween(ix, 0, nx-1, "x out of range");
|
||||
Assert::isBetween(iy, 0, ny-1, "y out of range");
|
||||
Assert::isBetween(iz, 0, nz-1, "z out of range");
|
||||
const int idx = ix + iy*nx + iz*nx*ny;
|
||||
return data[idx];
|
||||
}
|
||||
|
||||
T& getGridRef(const int ix, const int iy, const int iz) {
|
||||
Assert::isBetween(ix, 0, nx-1, "x out of range");
|
||||
Assert::isBetween(iy, 0, ny-1, "y out of range");
|
||||
Assert::isBetween(iz, 0, nz-1, "z out of range");
|
||||
const int idx = ix + iy*nx + iz*nx*ny;
|
||||
return data[idx];
|
||||
}
|
||||
T& getGridRef(const int ix, const int iy, const int iz) {
|
||||
Assert::isBetween(ix, 0, nx-1, "x out of range");
|
||||
Assert::isBetween(iy, 0, ny-1, "y out of range");
|
||||
Assert::isBetween(iz, 0, nz-1, "z out of range");
|
||||
const int idx = ix + iy*nx + iz*nx*ny;
|
||||
return data[idx];
|
||||
}
|
||||
|
||||
void setGrid(const int ix, const int iy, const int iz, const T val) {
|
||||
Assert::isBetween(ix, 0, nx-1, "x out of range");
|
||||
Assert::isBetween(iy, 0, ny-1, "y out of range");
|
||||
Assert::isBetween(iz, 0, nz-1, "z out of range");
|
||||
const int idx = ix + iy*nx + iz*nx*ny;
|
||||
data[idx] = val;
|
||||
}
|
||||
void setGrid(const int ix, const int iy, const int iz, const T val) {
|
||||
Assert::isBetween(ix, 0, nx-1, "x out of range");
|
||||
Assert::isBetween(iy, 0, ny-1, "y out of range");
|
||||
Assert::isBetween(iz, 0, nz-1, "z out of range");
|
||||
const int idx = ix + iy*nx + iz*nx*ny;
|
||||
data[idx] = val;
|
||||
}
|
||||
|
||||
void forEach(std::function<void(float,float,float,T)> func) const {
|
||||
for (int iz = 0; iz < nz; ++iz) {
|
||||
for (int iy = 0; iy < ny; ++iy) {
|
||||
for (int ix = 0; ix < nx; ++ix) {
|
||||
const float x = (ix * gridSize_cm / 100.0f) + sx_m;
|
||||
const float y = (iy * gridSize_cm / 100.0f) + sy_m;
|
||||
const float z = (iz * gridSize_cm / 100.0f) + sz_m;
|
||||
func(x,y,z, getGrid(ix, iy, iz));
|
||||
void forEach(std::function<void(float,float,float,T)> func) const {
|
||||
for (int iz = 0; iz < nz; ++iz) {
|
||||
for (int iy = 0; iy < ny; ++iy) {
|
||||
for (int ix = 0; ix < nx; ++ix) {
|
||||
const float x = (ix * gridSize_cm / 100.0f) + sx_m;
|
||||
const float y = (iy * gridSize_cm / 100.0f) + sy_m;
|
||||
const float z = (iz * gridSize_cm / 100.0f) + sz_m;
|
||||
func(x,y,z, getGrid(ix, iy, iz));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
void dump() {
|
||||
/*
|
||||
void dump() {
|
||||
|
||||
std::ofstream os("/tmp/1.dat");
|
||||
const float s = 1;//gridSize_cm / 100.0f;
|
||||
// for (int y = 0; y < ny; ++y) {
|
||||
// for (int x = 0; x < nx; ++x) {
|
||||
// float rssi = data[x+y*nx];
|
||||
// rssi = (rssi == 0) ? (-100) : (rssi);
|
||||
// os << (x*s) << " " << (y*s) << " " << rssi << "\n";
|
||||
// }
|
||||
// os << "\n";
|
||||
// }
|
||||
for (int y = 0; y < ny; ++y) {
|
||||
for (int x = 0; x < nx; ++x) {
|
||||
float rssi = data[x+y*nx];
|
||||
rssi = (rssi == 0) ? (-100) : (rssi);
|
||||
os << rssi << " ";
|
||||
std::ofstream os("/tmp/1.dat");
|
||||
const float s = 1;//gridSize_cm / 100.0f;
|
||||
// for (int y = 0; y < ny; ++y) {
|
||||
// for (int x = 0; x < nx; ++x) {
|
||||
// float rssi = data[x+y*nx];
|
||||
// rssi = (rssi == 0) ? (-100) : (rssi);
|
||||
// os << (x*s) << " " << (y*s) << " " << rssi << "\n";
|
||||
// }
|
||||
// os << "\n";
|
||||
// }
|
||||
for (int y = 0; y < ny; ++y) {
|
||||
for (int x = 0; x < nx; ++x) {
|
||||
float rssi = data[x+y*nx];
|
||||
rssi = (rssi == 0) ? (-100) : (rssi);
|
||||
os << rssi << " ";
|
||||
}
|
||||
os << "\n";
|
||||
}
|
||||
os << "\n";
|
||||
os.close();
|
||||
|
||||
}
|
||||
os.close();
|
||||
*/
|
||||
|
||||
}
|
||||
*/
|
||||
private:
|
||||
|
||||
private:
|
||||
|
||||
void cleanup() {
|
||||
delete[] data;
|
||||
data = nullptr;
|
||||
}
|
||||
void cleanup() {
|
||||
delete[] data;
|
||||
data = nullptr;
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
struct DataMap3SignalEntry {
|
||||
|
||||
|
||||
|
||||
struct Entry {
|
||||
float rssi;
|
||||
float distanceToAP;
|
||||
Entry(float rssi, float distanceToAP) : rssi(rssi), distanceToAP(distanceToAP) {;}
|
||||
};
|
||||
|
||||
std::vector<Entry> entries;
|
||||
|
||||
void add(const float rssi, const float distanceToAP) {
|
||||
static std::mutex mtx;
|
||||
Entry e(rssi, distanceToAP);
|
||||
mtx.lock();
|
||||
entries.push_back(e);
|
||||
mtx.unlock();
|
||||
}
|
||||
|
||||
float getMaxRSSI() const {
|
||||
auto comp = [] (const Entry& e1, const Entry& e2) {return e1.rssi < e2.rssi;};
|
||||
if (entries.empty()) {return -120;}
|
||||
auto it = std::max_element(entries.begin(), entries.end(), comp);
|
||||
return it->rssi;
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
struct DataMap3SignalEntry {
|
||||
|
||||
class DataMap3Signal : public DataMap3<DataMap3SignalEntry> {
|
||||
|
||||
public:
|
||||
|
||||
/** update average */
|
||||
void update(const float x_m, const float y_m, const float z_m, const float rssi, const float distanceToAP) {
|
||||
struct Entry {
|
||||
float rssi;
|
||||
float distanceToAP;
|
||||
Entry(float rssi, float distanceToAP) : rssi(rssi), distanceToAP(distanceToAP) {;}
|
||||
};
|
||||
|
||||
DataMap3SignalEntry& entry = getRef(x_m, y_m, z_m);
|
||||
entry.add(rssi, distanceToAP);
|
||||
std::vector<Entry> entries;
|
||||
|
||||
}
|
||||
void add(const float rssi, const float distanceToAP) {
|
||||
static std::mutex mtx;
|
||||
Entry e(rssi, distanceToAP);
|
||||
mtx.lock();
|
||||
entries.push_back(e);
|
||||
mtx.unlock();
|
||||
}
|
||||
|
||||
};
|
||||
float getMaxRSSI() const {
|
||||
auto comp = [] (const Entry& e1, const Entry& e2) {return e1.rssi < e2.rssi;};
|
||||
if (entries.empty()) {return -120;}
|
||||
auto it = std::max_element(entries.begin(), entries.end(), comp);
|
||||
return it->rssi;
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
class DataMap3Signal : public DataMap3<DataMap3SignalEntry> {
|
||||
|
||||
public:
|
||||
|
||||
/** update average */
|
||||
void update(const float x_m, const float y_m, const float z_m, const float rssi, const float distanceToAP) {
|
||||
|
||||
DataMap3SignalEntry& entry = getRef(x_m, y_m, z_m);
|
||||
entry.add(rssi, distanceToAP);
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // DATAMAP3_H
|
||||
|
||||
Reference in New Issue
Block a user