diff --git a/libautoscoper/src/PSO.cpp b/libautoscoper/src/PSO.cpp index 9ea0c148..be6808d4 100644 --- a/libautoscoper/src/PSO.cpp +++ b/libautoscoper/src/PSO.cpp @@ -1,10 +1,10 @@ #include "PSO.hpp" #include #include - +#include // New Particle Swarm Optimization -float host_fitness_function(float x[]) +float host_fitness_function(std::vector x) { double xyzypr_manip[6] = { 0 }; for (int i = 0; i <= NUM_OF_DIMENSIONS - 1; i++) @@ -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* 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 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; @@ -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; @@ -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; } diff --git a/libautoscoper/src/PSO.hpp b/libautoscoper/src/PSO.hpp index f7dc0b37..a88b039a 100644 --- a/libautoscoper/src/PSO.hpp +++ b/libautoscoper/src/PSO.hpp @@ -12,6 +12,7 @@ #include #include #include +#include const int NUM_OF_PARTICLES = 100; const int NUM_OF_DIMENSIONS = 6; const float c1 = 1.5f; @@ -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 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 position; + std::vector 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(NUM_OF_DIMENSIONS, 0.f)); + } + Particle(const std::vector& pos) { + ncc_val = FLT_MAX; + position = pos; + velocity = *(new std::vector(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* particles, Particle* gBest, unsigned int MAX_EPOCHS, unsigned int MAX_STALL); diff --git a/libautoscoper/src/Tracker.cpp b/libautoscoper/src/Tracker.cpp index a1fe8f13..823c826a 100644 --- a/libautoscoper/src/Tracker.cpp +++ b/libautoscoper/src/Tracker.cpp @@ -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 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