Skip to content

Commit

Permalink
PERF: Introduce particle data structure for PSO
Browse files Browse the repository at this point in the history 
* For any given particle p, this update removes any recomputation of the
  NCC fitness value.
* Replaces arrays with vectors
* This drops the per frame opt time from ~9 to ~18 seconds to around ~3
  seconds
  • Loading branch information
@NicerNewerCar
NicerNewerCar committed Oct 26, 2023
1 parent 73eb359 commit e032505
Show file tree
Hide file tree
Showing 3 changed files with 108 additions and 84 deletions.
88 changes: 46 additions & 42 deletions libautoscoper/src/PSO.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,10 +1,10 @@
#include "PSO.hpp"
#include <iostream>
#include <string>

#include <vector>

// New Particle Swarm Optimization
float host_fitness_function(float x[])
float host_fitness_function(std::vector<float> x)
{
double xyzypr_manip[6] = { 0 };
for (int i = 0; i <= NUM_OF_DIMENSIONS - 1; i++)
Expand Down Expand Up @@ -46,18 +46,30 @@ float getRandomClamped()
return (float)rand() / (float)RAND_MAX;
}

void pso(float *positions, float *velocities, float *pBests, float *gBest, unsigned int MAX_EPOCHS, unsigned int MAX_STALL)
void pso(std::vector<Particle*>* particles, Particle* gBest, unsigned int MAX_EPOCHS, unsigned int MAX_STALL)
{
int stall_iter = 0;
float tempParticle1[NUM_OF_DIMENSIONS];
float tempParticle2[NUM_OF_DIMENSIONS];

bool do_this = true;
unsigned int counter = 0;
//for (int iter = 0; iter < (signed int)MAX_EPOCHS; iter++)

float OMEGA = 0.8f;

// Make a copy of the particles, this will be the initial pBest
std::vector<Particle*> pBest;
Particle* pBestTemp = new Particle();
for (Particle* p : *particles) {
*pBestTemp = *p;
pBest.push_back(pBestTemp);
}
pBestTemp = nullptr;
delete pBestTemp;

// Calc NCC for gBest
gBest->ncc_val = host_fitness_function(gBest->position);

// Init pointers
Particle* currentBest = new Particle();
Particle* p = new Particle();
Particle* curPBest = new Particle();
while (do_this)
{
//std::cout << "OMEGA: " << OMEGA << std::endl;
Expand All @@ -66,55 +78,39 @@ void pso(float *positions, float *velocities, float *pBests, float *gBest, unsig
do_this = false;
}

float currentBest = host_fitness_function(gBest);
*currentBest = *gBest; // We want this to be a copy not a pointer

for (int i = 0; i < NUM_OF_PARTICLES*NUM_OF_DIMENSIONS; i++)
for (int i = 0; i < NUM_OF_PARTICLES; i++)
{
float rp = getRandomClamped();
float rg = getRandomClamped();
p = particles->at(i); // We want these to be pointers not copies
curPBest = pBest.at(i);

velocities[i] = OMEGA * velocities[i] + c1 * rp*(pBests[i] - positions[i]) + c2 * rg*(gBest[i%NUM_OF_DIMENSIONS] - positions[i]);
// Update the velocities and positions
p->updateVelocityAndPosition(curPBest, gBest, OMEGA);

positions[i] += velocities[i];
}

OMEGA = OMEGA * 0.9f;
// Get the NCC of the current particle
p->ncc_val = host_fitness_function(p->position);

for (int i = 0; i < NUM_OF_PARTICLES*NUM_OF_DIMENSIONS; i += NUM_OF_DIMENSIONS)
{
for (int j = 0; j < NUM_OF_DIMENSIONS; j++)
{
tempParticle1[j] = positions[i + j];
tempParticle2[j] = pBests[i + j];
// Update the pBest if the current particle is better
if (p->ncc_val < curPBest->ncc_val) {
*curPBest = *p;
}

if (host_fitness_function(tempParticle1) < host_fitness_function(tempParticle2))
{
for (int j = 0; j < NUM_OF_DIMENSIONS; j++)
{
pBests[i + j] = positions[i + j];
}

if (host_fitness_function(tempParticle1) < host_fitness_function(gBest))
{
//cout << "Current Best is: " ;
for (int j = 0; j < NUM_OF_DIMENSIONS; j++)
{
gBest[j] = pBests[i + j];
}
}
// Update the gBest if the current particle is better
if (p->ncc_val < gBest->ncc_val) {
*gBest = *p;
}
}

float epochBest = host_fitness_function(gBest);
OMEGA = OMEGA * 0.9f;

std::cout << "Current Best NCC: " << epochBest << std::endl;
std::cout << "Current Best NCC: " << gBest->ncc_val << std::endl;
//std::cout << "Stall: " << stall_iter << std::endl;
if (abs(epochBest - currentBest) < 1e-4f)
if (abs(gBest->ncc_val - currentBest->ncc_val) < 1e-4f)
{
//std::cout << "Increased Stall Iter" << std::endl;
stall_iter++;
} else if (abs(epochBest - currentBest) > 0.001f)
} else if (abs(gBest->ncc_val - currentBest->ncc_val) > 0.001f)
{
//std::cout << "Zeroed Stall Iter" << std::endl;
stall_iter = 0;
Expand All @@ -127,5 +123,13 @@ void pso(float *positions, float *velocities, float *pBests, float *gBest, unsig

counter++;
}
// Clean up pointers
p = nullptr;
delete p;
curPBest = nullptr;
delete curPBest;
currentBest = nullptr;
delete currentBest;

std::cout << "Total #Epoch of: " << counter << std::endl;
}
44 changes: 42 additions & 2 deletions libautoscoper/src/PSO.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <vector>
const int NUM_OF_PARTICLES = 100;
const int NUM_OF_DIMENSIONS = 6;
const float c1 = 1.5f;
Expand All @@ -24,6 +25,45 @@ void intializeRandom();

float getRandom(float low, float high);
float getRandomClamped();
float host_fitness_function(float x[]);
float host_fitness_function(std::vector<float> x);

void pso(float *positions, float *velocities, float *pBests, float *gBest, unsigned int MAX_EPOCHS, unsigned int MAX_STALL);
struct Particle {
float ncc_val;
std::vector<float> position;
std::vector<float> velocity;

// Copy constructor
Particle(const Particle& p) {
ncc_val = p.ncc_val;
position = p.position;
velocity = p.velocity;
}
// Default constructor
Particle() {
ncc_val = FLT_MAX;
velocity = *(new std::vector<float>(NUM_OF_DIMENSIONS, 0.f));
}
Particle(const std::vector<float>& pos) {
ncc_val = FLT_MAX;
position = pos;
velocity = *(new std::vector<float>(NUM_OF_DIMENSIONS, 0.f));
}
// Assignment operator
Particle& operator=(const Particle& p) {
ncc_val = p.ncc_val;
position = p.position;
velocity = p.velocity;
return *this;
}
void updateVelocityAndPosition(Particle* pBest, Particle* gBest, float OMEGA) {
for (int i = 0; i < NUM_OF_DIMENSIONS; i++) {
float rp = getRandomClamped();
float rg = getRandomClamped();

this->velocity.at(i) = OMEGA * velocity.at(i) + c1 * rp * (pBest->position.at(i) - this->position.at(i)) + c2 * rg * (gBest->position.at(i) - this->position.at(i));
this->position.at(i) += this->velocity.at(i);
}
}
};

void pso(std::vector<Particle*>* particles, Particle* gBest, unsigned int MAX_EPOCHS, unsigned int MAX_STALL);
60 changes: 20 additions & 40 deletions libautoscoper/src/Tracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -419,61 +419,41 @@ void Tracker::optimize(int frame, int dFrame, int repeats, int opt_method, unsig
unsigned int MAX_EPOCHS = max_iter;
unsigned int MAX_STALL = max_stall_iter;

float positions[NUM_OF_PARTICLES*NUM_OF_DIMENSIONS];
float velocities[NUM_OF_PARTICLES*NUM_OF_DIMENSIONS];
float pBests[NUM_OF_PARTICLES*NUM_OF_DIMENSIONS];
float gBest[NUM_OF_DIMENSIONS];
std::vector<Particle*> particles;
Particle* gBest = new Particle();

srand((unsigned)time(NULL));

for (int i = 0; i < NUM_OF_PARTICLES*NUM_OF_DIMENSIONS; i++)
{
if (i >= NUM_OF_DIMENSIONS) {
positions[i] = getRandom(START_RANGE_MIN, START_RANGE_MAX);
}
// First point will be the initial position
else {
//if (j > 1)
//{
positions[i] = (float)0.0;
//}
//else
//{
// positions[i] = (float)0.2; // Noise for refinements
//}
}
pBests[i] = positions[i];
velocities[i] = 0;
}
Particle* p = new Particle({ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f });
particles.push_back(p); // First particle is the initial position
*gBest = *p; // gBest is initially the initial position

for (int i = 0; i < NUM_OF_DIMENSIONS; i++)
for (int i = 0; i < NUM_OF_PARTICLES-1; i++)
{
gBest[i] = pBests[i];
p = new Particle();
for (int j = 0; j < NUM_OF_DIMENSIONS; j++) {
p->position.push_back(getRandom(START_RANGE_MIN, START_RANGE_MAX));
}
particles.push_back(p);
}
p = nullptr;
delete p;

clock_t cpu_begin = clock();

pso(positions, velocities, pBests, gBest, MAX_EPOCHS, MAX_STALL);
//cuda_pso(positions, velocities, pBests, gBest, MAX_EPOCHS);

pso(&particles, gBest, MAX_EPOCHS, MAX_STALL);
clock_t cpu_end = clock();

printf("Time elapsed:%10.3lf s\n", (double)(cpu_end - cpu_begin) / CLOCKS_PER_SEC);

std::cout << "Pose change from initial position: ";
for (int i = 0; i < NUM_OF_DIMENSIONS; i++)
{
if (i == NUM_OF_DIMENSIONS - 1)
{
std::cout << gBest[i] << std::endl;
}
else {
std::cout << gBest[i] << ",";
}
xyzypr_manip[i] = gBest[i];
for (int i = 0; i < NUM_OF_DIMENSIONS-1; i++) {
std::cout << gBest->position[i] << ", ";
xyzypr_manip[i] = gBest->position[i];
}
std::cout << gBest->position[NUM_OF_DIMENSIONS-1] << std::endl;
xyzypr_manip[NUM_OF_DIMENSIONS-1] = gBest->position[NUM_OF_DIMENSIONS-1];

printf("Minimum NCC from PSO = %f\n", host_fitness_function(gBest));
printf("Minimum NCC from PSO = %f\n", gBest->ncc_val);

manip = CoordFrame::from_xyzAxis_angle(xyzypr_manip);
// PSO End
Expand Down

0 comments on commit e032505

Please sign in to comment.