Skip to content

Commit

Permalink
Merge branch 'dev' into triangulation
Browse files Browse the repository at this point in the history
  • Loading branch information
@Karlkorv
Karlkorv authored Jul 18, 2024
2 parents 223699b + eaf029e commit 6c9847c
Show file tree
Hide file tree
Showing 16 changed files with 512 additions and 344 deletions.
2 changes: 1 addition & 1 deletion config.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ N_SENSORS_TMP: 256

# Beamforming
N_FRAMES: 4 # How many "frames" that are stored in the ringbuffer
N_SAMPLES: 256 #16384 # 512 # Each frame, must be a power of 2
N_SAMPLES: 512 # Warning cannot be larger than 512 since pagesize is 4096 bytes and size of float is 4 bytes
N_OFFSET: N_SAMPLES / 2
BUFFER_LENGTH: N_FRAMES * N_SAMPLES

Expand Down
196 changes: 125 additions & 71 deletions src/algorithms/gradient_ascend.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
GradientParticle::GradientParticle(Antenna &antenna, Streams *streams) : antenna(antenna), streams(streams) {
this->epsilon = 1e-9f;
this->delta = TO_RADIANS(7);
this->tracking = false;
random();
}

Expand All @@ -19,31 +20,22 @@ void GradientParticle::jump() {
directionCurrent.theta = clip(directionCurrent.theta, 0.0, M_PI / 2.0);
}

void GradientParticle::nearby() {
#if 1
std::vector<Spherical> near = directionCurrent.nearby(delta);
#else
// North
directionNearby[NORTH].theta = directionCurrent.theta + delta;
directionNearby[NORTH].phi = directionCurrent.phi;
void GradientParticle::step(double rate) {
update();

// East
directionNearby[EAST].theta = directionCurrent.theta;
directionNearby[EAST].phi = directionCurrent.phi + delta;// / sin(epsilon + directionCurrent.theta);
Spherical newDirection;

// South
directionNearby[SOUTH].theta = directionCurrent.theta - delta;
directionNearby[SOUTH].phi = directionCurrent.phi;
newDirection.theta = directionCurrent.theta + rate * directionGradient.theta;
newDirection.phi = directionCurrent.phi + (rate * directionGradient.phi) / sin(1e-9 + directionCurrent.theta);

// West
directionNearby[WEST].theta = directionCurrent.theta;
directionNearby[WEST].phi = directionCurrent.phi - delta;// / sin(epsilon + directionCurrent.theta);
#endif
//for (Spherical& spherical : near) {
// spherical.phi = wrapAngle(spherical.phi);
// spherical.theta = clip(spherical.theta, 0.0, M_PI / 2.0);
// directionNearby[i] =
//}
newDirection.phi = wrapAngle(newDirection.phi);
newDirection.theta = clip(newDirection.theta, 0.0, M_PI / 2.0);

directionCurrent = newDirection;
}

void GradientParticle::nearby() {
std::vector<Spherical> near = directionCurrent.nearby(delta);
for (int i = 0; i < 4; i++) {
directionNearby[i] = near[i];
directionNearby[i].phi = wrapAngle(directionNearby[i].phi);
Expand Down Expand Up @@ -79,12 +71,8 @@ void GradientParticle::update() {
int i = antenna.index[s];
float fraction = fractional_delays[n][i];

//std::cout << "Using index: " << i << std::endl;

int offset = offset_delays[n][i];

//std::cout << offset << std::endl;

float *signal = streams->get_signal(i, offset);
delay(&out[0], signal, fraction);
//delay_corrected(&out[0], signal, fraction, antenna.power_correction_mask[s]);
Expand All @@ -97,19 +85,17 @@ void GradientParticle::update() {
power[n] /= (float) N_SAMPLES;
}

float thetaPower = fmax(fabs(power[NORTH]), fabs(power[SOUTH]));
float phiPower = fmax(fabs(power[EAST]), fabs(power[WEST]));

float thetaPower = ((power[NORTH] + power[SOUTH]) / 2.0);
float phiPower = ((power[NORTH] + power[SOUTH]) / 2.0);
directionGradient.theta = (double) - ((power[NORTH] - power[SOUTH]) / thetaPower); /// (2.0 * delta));
directionGradient.theta = (double) ((power[SOUTH] - power[NORTH]) / thetaPower);/// (2.0 * delta));
directionGradient.phi = (double) ((power[EAST] - power[WEST]) / phiPower);/// (2.0 * delta));
magnitude = (power[NORTH] + power[EAST] + power[SOUTH] + power[WEST]) / 4.0;

//std::cout << "theta=" << directionGradient.theta << " phi=" << directionGradient.phi<< std::endl;
gradient = fabs(directionGradient.theta) + fabs(directionGradient.phi);
}


SphericalGradient::SphericalGradient(Pipeline *pipeline, Antenna &antenna, bool *running, std::size_t swarm_size, std::size_t iterations) : Worker(pipeline, running), swarm_size(swarm_size), iterations(iterations) {
this->antenna = antenna;
SphericalGradient::SphericalGradient(Pipeline *pipeline, Antenna &antenna, bool *running, std::size_t swarm_size, std::size_t iterations) : Worker(pipeline, running), antenna(antenna), swarm_size(swarm_size), iterations(iterations) {
this->streams = pipeline->getStreams();
thread_loop = std::thread(&SphericalGradient::loop, this);
}
Expand All @@ -119,7 +105,8 @@ void SphericalGradient::initialize_particles() {
particles.clear();
for (int i = 0; i < swarm_size; i++) {
particles.emplace_back(this->antenna, this->streams);
//GradientParticle &particle = particles.back();
GradientParticle &particle = particles.back();
particle.delta = TO_RADIANS(10.0);//1.0 + drandom() * 1.0);
}
}

Expand All @@ -135,6 +122,41 @@ void SphericalGradient::draw_heatmap(cv::Mat *heatmap) {

lock.lock();

#if 1
for (GradientParticle &particle: currentTrackers) {
if (!particle.tracking) {
continue;
}
// If we convert to sphere with radius 0.5 we don't have to normalize it other than add
// 0.5 to get the sphere in the first sector in the cartesian coordinate system
Cartesian position = Cartesian::convert(particle.directionCurrent, 1.0);

// Use the position values to plot over the heatmap
int x = (int) (x_res * (position.x / 2.0 + 0.5));
int y = (int) (y_res * (position.y / 2.0 + 0.5));

float gradient = 1.0 - clip(particle.gradient, 0.0, 2.0) / 2.0;

heatmap->at<uchar>(y, x) = (255);

int m = 255;//(int) clip(particle.magnitude / 1e-6, 0.0, 255.0);

cv::Mat &frame = *heatmap;

cv::circle(frame, cv::Point(x, y), 1 + (int) (gradient * 10.0), cv::Scalar(m, m, m), cv::FILLED, 8, 0);


heatmap->at<uchar>(y, x) = 255;
}
#else
float maxValue = 0.0;
for (GradientParticle &particle: particles) {
if (particle.magnitude > maxValue) {
maxValue = particle.magnitude;
}
}
//maxValue = 20 * std::log10(maxValue * 1e5);
for (GradientParticle &particle: particles) {
// If we convert to sphere with radius 0.5 we don't have to normalize it other than add
// 0.5 to get the sphere in the first sector in the cartesian coordinate system
Expand All @@ -144,47 +166,51 @@ void SphericalGradient::draw_heatmap(cv::Mat *heatmap) {
int x = (int) (x_res * (position.x / 2.0 + 0.5));
int y = (int) (y_res * (position.y / 2.0 + 0.5));
cv::circle(*heatmap, cv::Point(y, x), 3, cv::Scalar(255, 255, 255), cv::FILLED, 8, 0);
float gradient = 1.0 - clip(particle.gradient, 0.0, 2.0) / 2.0;
heatmap->at<uchar>(x,y) = (255);
float mag = particle.magnitude; //20 * std::log10(particle.magnitude * 1e5);
int m = (int) (mag / maxValue * 255.0);
cv::circle(*heatmap, cv::Point(y, x), 1 + (int)(gradient * 10.0), cv::Scalar(m,m,m), cv::FILLED, 8, 0);
heatmap->at<uchar>(x, y) = 255;
}
#endif

lock.unlock();
}

void SphericalGradient::loop() {
std::cout << "Starting loop" << std::endl;
double learning_rate;
constexpr double start_rate = 5e-3;
constexpr double start_rate = 1e-1;
// Place particles on dome
int it = 0;
initialize_particles();

int n_trackers = 9;

for (int i = 0; i < n_trackers; i++) {
currentTrackers.emplace_back(this->antenna, this->streams);
GradientParticle& particle = currentTrackers.back();
particle.delta = TO_RADIANS(3);
}

while (looping && pipeline->isRunning()) {

// Wait for incoming data
pipeline->barrier();

lock.lock();

// if (it % 40 == 0) {
// // Place particles on dome
// std::cout << "Replacing" << std::endl;
// for (auto &particle: particles) {
// particle.jump();
// }
// //initialize_particles();
// }
//
// if (it % 500 == 0) {
// // Place particles on dome
// //std::cout << "Replacing" << std::endl;
// //for (auto &particle: particles) {
// // particle.jump();
// //}
// initialize_particles();
// }
// it++;

if (it % 100 == 0) {
initialize_particles();
}
it++;



Expand All @@ -197,26 +223,54 @@ void SphericalGradient::loop() {
//std::cout << "Too slow: " << i + 1 << "/" << iterations << std::endl;
break;// Too slow!
}
for (auto &particle: particles) {

particle.update();


Spherical newDirection;
learning_rate = ((double)(iterations - i) / (double)iterations);
particle.delta = TO_RADIANS(learning_rate * 10.0);

learning_rate *= start_rate;
newDirection.theta = particle.directionCurrent.theta + learning_rate * particle.directionGradient.theta;
newDirection.phi = particle.directionCurrent.phi + (learning_rate * particle.directionGradient.phi)/ sin(1e-9+particle.directionCurrent.theta);

newDirection.phi = wrapAngle(newDirection.phi);
newDirection.theta = clip(newDirection.theta, 0.0, M_PI / 2.0);
int n_tracking = 0;
for (auto &particle : currentTrackers) {
if (particle.tracking) {
for (int i = 0; i < 5; i++)
particle.step(start_rate / 100.0);
if (particle.gradient > 1.0) {
particle.tracking = false;
} else {
n_tracking++;
}
}
}

particle.directionCurrent = newDirection;
for (int m = 0; m < n_trackers; m++) {
if (!currentTrackers[m].tracking) {
continue;
}

for (int n = m+1; n < n_trackers; n++) {
if (!currentTrackers[n].tracking) {
continue;
}
if (currentTrackers[m].directionCurrent.angle(currentTrackers[n].directionCurrent) < M_PI / 20.0) {
currentTrackers[m].tracking = false;
}
}
}
for (auto &particle: particles) {
particle.step(start_rate);
if (n_tracking < n_trackers) {
if (particle.gradient < 1e-4) {
for (auto &tracker : currentTrackers) {
if (!tracker.tracking && tracker.directionCurrent.angle(particle.directionCurrent) > M_PI / 20.0) {
tracker.tracking = true;
tracker.directionCurrent = particle.directionCurrent;
}
}
}
}
}
}

tracking.clear();
for (auto &tracker : currentTrackers) {
if (tracker.tracking) {
tracking.emplace_back(tracker.directionCurrent, tracker.magnitude, 1 / tracker.gradient);
}
}
lock.unlock();
}

Expand Down
7 changes: 5 additions & 2 deletions src/algorithms/gradient_ascend.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,11 +12,13 @@

class GradientParticle {
public:
bool tracking;
Spherical directionCurrent;
Spherical directionNearby[4];
Spherical directionGradient;
Spherical directionBest;
float magnitude;
float gradient;
Antenna &antenna;
Streams *streams;

Expand All @@ -30,6 +32,7 @@ class GradientParticle {

void nearby();
void jump();
void step(double rate);

void random();
void update();
Expand Down Expand Up @@ -72,9 +75,9 @@ class SphericalGradient : public Worker {
std::size_t iterations;
std::size_t swarm_size;
Streams *streams;
Antenna antenna;
Antenna &antenna;
std::vector<GradientParticle> particles;

std::vector<GradientParticle> currentTrackers;
};


Expand Down
3 changes: 0 additions & 3 deletions src/algorithms/pso_seeker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,9 +9,6 @@

#define USE_LUT 0

#define SWARM_SIZE 100
#define SWARM_ITERATIONS


//#define IN_FIRST_SECTOR(i) ((i < 28) && )

Expand Down
Loading

0 comments on commit 6c9847c

Please sign in to comment.