65 lines
1.3 KiB
C++
Executable File
65 lines
1.3 KiB
C++
Executable File
#ifndef MYINITIALIZER3_H
|
|
#define MYINITIALIZER3_H
|
|
|
|
#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
|
|
#include "MyState.h"
|
|
|
|
#include <Indoor/grid/Grid.h>
|
|
#include <Indoor/math/Distributions.h>
|
|
|
|
class MyInitializer : public K::ParticleFilterInitializer<MyState> {
|
|
|
|
private:
|
|
|
|
Grid<MyGridNode>& grid;
|
|
|
|
int x_cm;
|
|
int y_cm;
|
|
int z_cm;
|
|
|
|
int heading;
|
|
|
|
|
|
|
|
public:
|
|
|
|
/** q0 = random */
|
|
MyInitializer(Grid<MyGridNode>& grid) : grid(grid), heading(0) {
|
|
|
|
}
|
|
|
|
/** q0 = given */
|
|
MyInitializer(Grid<MyGridNode>& grid, int x_cm, int y_cm, int z_cm, int heading) :
|
|
grid(grid), x_cm(x_cm), y_cm(y_cm), z_cm(z_cm), heading(heading) {
|
|
|
|
}
|
|
|
|
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
|
|
|
|
Distribution::Uniform<int> distIdx(0, grid.getNumNodes()-1);
|
|
Distribution::Uniform<float> distHead(0, M_PI*2);
|
|
|
|
for (K::Particle<MyState>& p : particles) {
|
|
|
|
const int idx = distIdx.draw();
|
|
MyGridNode& n = grid[idx];
|
|
|
|
//p.state.pCur = Point3(x_cm, y_cm, z_cm);
|
|
//GridPoint gp(p.state.pCur.x, p.state.pCur.y, p.state.pCur.z);
|
|
//p.state.walkState.node = &grid.getNodeFor(gp);
|
|
|
|
p.state.pCur = (Point3) n;
|
|
p.state.walkState.node = &n;
|
|
|
|
p.state.pOld = p.state.pCur;
|
|
p.state.walkState.heading = distHead.draw();
|
|
p.state.hPa = 0;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // MYINITIALIZER_H
|