From a4498cd33d28f423e47d9fe65cf1b41680c2f831 Mon Sep 17 00:00:00 2001 From: James Pace Date: Wed, 6 Jul 2022 12:10:43 +0000 Subject: [PATCH] Split and organize first pass. --- CMakeLists.txt | 11 +- .../{optimizer.hpp => CostFunction.hpp} | 19 +- include/j7s-optimization/SimplexSolver.hpp | 42 ++++ include/j7s-optimization/common.hpp | 41 ++++ src/CostFunction.cpp | 33 +++ src/SimplexSolver.cpp | 138 ++++++++++++ src/main.cpp | 213 +----------------- src/optimizer.cpp | 23 -- 8 files changed, 284 insertions(+), 236 deletions(-) rename include/j7s-optimization/{optimizer.hpp => CostFunction.hpp} (58%) create mode 100644 include/j7s-optimization/SimplexSolver.hpp create mode 100644 include/j7s-optimization/common.hpp create mode 100644 src/CostFunction.cpp create mode 100644 src/SimplexSolver.cpp delete mode 100644 src/optimizer.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index c834870..e2e7f30 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,14 +8,17 @@ find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros REQUIRED) find_package(rclcpp REQUIRED) -add_library(optimizer src/optimizer.cpp) -target_compile_features(optimizer PUBLIC cxx_std_17) # Require C++17 -target_include_directories(optimizer PUBLIC +add_library(simplex_solver + src/CostFunction.cpp + src/SimplexSolver.cpp) +target_compile_features(simplex_solver PUBLIC cxx_std_17) # Require C++17 +target_include_directories(simplex_solver PUBLIC $ $) add_executable(main src/main.cpp) target_compile_features(main PUBLIC cxx_std_17) # Require C++17 +target_link_libraries(main simplex_solver) target_include_directories(main PUBLIC $ $) @@ -27,7 +30,7 @@ install( ) install( TARGETS - optimizer + simplex_solver EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib/${PROJECT_NAME} LIBRARY DESTINATION lib/${PROJECT_NAME} diff --git a/include/j7s-optimization/optimizer.hpp b/include/j7s-optimization/CostFunction.hpp similarity index 58% rename from include/j7s-optimization/optimizer.hpp rename to include/j7s-optimization/CostFunction.hpp index bd25784..10c0fb6 100644 --- a/include/j7s-optimization/optimizer.hpp +++ b/include/j7s-optimization/CostFunction.hpp @@ -7,20 +7,25 @@ // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -#ifndef J7S__OPTIMIZER_HPP_ -#define J7S__OPTIMIZER_HPP_ +#ifndef J7S__COSTFUNCTION_HPP_ +#define J7S__COSTFUNCTION_HPP_ namespace j7s { - -class Optimizer +class CostFunction { public: - Optimizer(); + CostFunction(double a, double b, double c); + double eval(double input) const; - virtual ~Optimizer(); + double actualBest() const; + +private: + double m_a; + double m_b; + double m_c; }; } // namespace j7s -#endif // J7S__OPTIMIZER_HPP_ +#endif // J7S__COSTFUNCTION_HPP_ diff --git a/include/j7s-optimization/SimplexSolver.hpp b/include/j7s-optimization/SimplexSolver.hpp new file mode 100644 index 0000000..b1511c1 --- /dev/null +++ b/include/j7s-optimization/SimplexSolver.hpp @@ -0,0 +1,42 @@ +// Copyright 2022 James Pace +// All Rights Reserved. +// +// For a license to this software contact +// James Pace at jpace121@gmail.com. +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +#ifndef J7S__SIMPLEXSOLVER_HPP_ +#define J7S__SIMPLEXSOLVER_HPP_ + +#include "j7s-optimization/CostFunction.hpp" +#include "j7s-optimization/common.hpp" + +#include + +namespace j7s +{ + +class SimplexSolver +{ +public: + SimplexSolver(const CostFunction & costFunction, const std::vector initSimplex); + + IterationState update(); + + Coordinate bestCoord() const; + +private: + const CostFunction m_costFunction; + std::vector m_currentSimplex; + + // Helper functions. + double newPoint(); + std::vector contract(); + double calcVolume(); +}; + +} // namespace j7s + +#endif // J7S__SIMPLEXSOLVER_HPP_ diff --git a/include/j7s-optimization/common.hpp b/include/j7s-optimization/common.hpp new file mode 100644 index 0000000..afa505c --- /dev/null +++ b/include/j7s-optimization/common.hpp @@ -0,0 +1,41 @@ +// Copyright 2022 James Pace +// All Rights Reserved. +// +// For a license to this software contact +// James Pace at jpace121@gmail.com. +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +#ifndef J7S__COMMON_HPP_ +#define J7S__COMMON_HPP_ + +namespace j7s +{ + +// The state after a single iteration of the solver. +enum class IterationState +{ + OK, + CONVERGED +}; + +// A coordinate is pair of a cost input and the resulting the cost. +// We save them together to minimize cost function evaluations. +// This isn't the cleanest thing in the world, but it's worth saving +// evaluations. +struct Coordinate +{ + double input; + double cost; + + Coordinate() : input{0.0}, cost{0.0} {}; + Coordinate(double input, double cost) : input{input}, cost{cost} {}; + + // Sort by cost. + bool operator<(const Coordinate & other) const { return (cost < other.cost); } +}; + +} // namespace j7s + +#endif // J7S__COMMON_HPP_ diff --git a/src/CostFunction.cpp b/src/CostFunction.cpp new file mode 100644 index 0000000..d666084 --- /dev/null +++ b/src/CostFunction.cpp @@ -0,0 +1,33 @@ +// Copyright 2022 James Pace +// All Rights Reserved. +// +// For a license to this software contact +// James Pace at jpace121@gmail.com. +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +#include "j7s-optimization/CostFunction.hpp" + +#include + +namespace j7s +{ +CostFunction::CostFunction(double a, double b, double c) : m_a{a}, m_b{b}, m_c{c} {} + +double CostFunction::eval(double input) const +{ + return m_a * std::pow(input, 2) + m_b * input + m_c; +} + +double CostFunction::actualBest() const +{ + // y = a*x**2 + b*x + c + // dy/dx = 2*a*x + b + // 0 = 2*a*x + b + // -b/2*a = x + + return (-m_b / (2 * m_a)); +} + +} // namespace j7s diff --git a/src/SimplexSolver.cpp b/src/SimplexSolver.cpp new file mode 100644 index 0000000..2934412 --- /dev/null +++ b/src/SimplexSolver.cpp @@ -0,0 +1,138 @@ +// Copyright 2022 James Pace +// All Rights Reserved. +// +// For a license to this software contact +// James Pace at jpace121@gmail.com. +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +#include "j7s-optimization/SimplexSolver.hpp" + +#include +#include +#include + +namespace j7s +{ +SimplexSolver::SimplexSolver( + const CostFunction & costFunction, const std::vector initSimplex) : + m_costFunction{costFunction} +{ + m_currentSimplex.reserve(initSimplex.size()); + + for (const auto val : initSimplex) + { + m_currentSimplex.emplace_back(val, m_costFunction.eval(val)); + } + std::sort(m_currentSimplex.begin(), m_currentSimplex.end()); +} + +Coordinate SimplexSolver::bestCoord() const +{ + return m_currentSimplex.front(); +} + +double SimplexSolver::newPoint() +{ + if (m_currentSimplex.size() == 0) + { + throw std::runtime_error("Simplex can't be missing."); + } + + // Calculate sum. + double biggest = m_currentSimplex.back().input; + // Calculate volume. + double sum = 0.0; + // All but the biggest, (would be adding 0...) + for (unsigned int index = 0; index < m_currentSimplex.size() - 1; index++) + { + const double diff = m_currentSimplex[index].input - biggest; + sum += diff; + } + + const double newPoint = sum * (2.0 / m_currentSimplex.size()); + + return newPoint; +} + +std::vector SimplexSolver::contract() +{ + const auto smallest = m_currentSimplex.front(); + + std::vector newVector; + newVector.reserve(m_currentSimplex.size()); + newVector.emplace_back(smallest); + + // TODO: Really check size before I get here... + for (auto it = m_currentSimplex.begin() + 1; it != m_currentSimplex.end(); it++) + { + const auto oldInput = it->input; + const auto newInput = 0.5 * (oldInput + smallest.input); + const auto newCost = m_costFunction.eval(newInput); + newVector.emplace_back(newInput, newCost); + } + std::sort(newVector.begin(), newVector.end()); + + return newVector; +} + +double SimplexSolver::calcVolume() +{ + // TODO: For reals do something like: + // https://math.stackexchange.com/questions/337197/finding-the-volume-of-a-tetrahedron-by-given-vertices + // For now: + // Sort by input and find the difference squared between the first and last. + + const auto inputLess = [](const Coordinate & first, const Coordinate & second) + { return first.input < second.input; }; + + // Copy the vector so we don't sort the original. + std::vector simplexCopy = m_currentSimplex; + std::sort(simplexCopy.begin(), simplexCopy.end(), inputLess); + const auto smallest = simplexCopy.front(); + const auto biggest = simplexCopy.back(); + return std::pow(biggest.input - smallest.input, 2.0); +} + +IterationState SimplexSolver::update() +{ + if (m_currentSimplex.size() < 3) + { + throw std::runtime_error("Simplex can't be a line."); + } + // Check for convergence and potentially early return. + // TODO: Make configurable. + const auto volume = calcVolume(); + if (volume < 1e-4) + { + return IterationState::CONVERGED; + } + + // Do update. + Coordinate potential; + potential.input = newPoint(); + potential.cost = m_costFunction.eval(potential.input); + + // TODO: Understand why looking at second biggest, not biggest. + // Explanation from paper is that it is in order to make + // the new guess of the next iteration different the current + // biggest. + const auto secondBiggest = *(m_currentSimplex.end() - 2); + if (potential.cost < secondBiggest.cost) + { + // Replace the last simplex value with the better one. + *(m_currentSimplex.end() - 1) = potential; + // TODO: DO I need to sort or is it sorted already? + std::sort(m_currentSimplex.begin(), m_currentSimplex.end()); + } + else + { + // Do a contraction. + m_currentSimplex = contract(); + } + + return IterationState::OK; +} + +} // namespace j7s diff --git a/src/main.cpp b/src/main.cpp index 4aa5400..5634abc 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -7,224 +7,33 @@ // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -#include #include -#include -#include -#include -#include "j7s-optimization/optimizer.hpp" - -class CostFunction -{ -public: - CostFunction(double a, double b, double c); - double eval(double input) const; - - double actualBest() const; - -private: - double m_a; - double m_b; - double m_c; -}; - -CostFunction::CostFunction(double a, double b, double c) : m_a{a}, m_b{b}, m_c{c} {} - -double CostFunction::eval(double input) const -{ - return m_a * std::pow(input, 2) + m_b * input + m_c; -} - -double CostFunction::actualBest() const -{ - // y = a*x**2 + b*x + c - // dy/dx = 2*a*x + b - // 0 = 2*a*x + b - // -b/2*a = x - - return (-m_b / (2 * m_a)); -} - -// A coordinate is pair of a cost input and the resulting the cost. -// We save them together to minimize cost function evaluations. -// This isn't the cleanest thing in the world, but it's worth saving -// evaluations. -struct Coordinate -{ - double input; - double cost; - - Coordinate() : input{0.0}, cost{0.0} {}; - Coordinate(double input, double cost) : input{input}, cost{cost} {}; - - // Sort by cost. - bool operator<(const Coordinate & other) const { return (cost < other.cost); } -}; - -enum class IterationState { - OK, - CONVERGED -}; - -class SimplexSolver -{ -public: - SimplexSolver(const CostFunction & costFunction, const std::vector initSimplex); - - IterationState update(); - - Coordinate bestCoord() const; - -private: - const CostFunction m_costFunction; - std::vector m_currentSimplex; - - // Helper functions. - double newPoint(); - std::vector contract(); - double calcVolume(); -}; - -SimplexSolver::SimplexSolver(const CostFunction & costFunction, const std::vector initSimplex): - m_costFunction{costFunction} -{ - m_currentSimplex.reserve(initSimplex.size()); - - for(const auto val : initSimplex) - { - m_currentSimplex.emplace_back(val, m_costFunction.eval(val)); - } - std::sort(m_currentSimplex.begin(), m_currentSimplex.end()); -} - -Coordinate SimplexSolver::bestCoord() const -{ - return m_currentSimplex.front(); -} - -double SimplexSolver::newPoint() -{ - if (m_currentSimplex.size() == 0) - { - throw std::runtime_error("Simplex can't be missing."); - } - - // Calculate sum. - double biggest = m_currentSimplex.back().input; - // Calculate volume. - double sum = 0.0; - // All but the biggest, (would be adding 0...) - for (unsigned int index = 0; index < m_currentSimplex.size() - 1; index++) - { - const double diff = m_currentSimplex[index].input - biggest; - sum += diff; - } - - const double newPoint = sum * (2.0 / m_currentSimplex.size()); - - return newPoint; -} - -std::vector SimplexSolver::contract() -{ - const auto smallest = m_currentSimplex.front(); - - std::vector newVector; - newVector.reserve(m_currentSimplex.size()); - newVector.emplace_back(smallest); - - // TODO: Really check size before I get here... - for(auto it = m_currentSimplex.begin() + 1; it != m_currentSimplex.end(); it++) - { - const auto oldInput = it->input; - const auto newInput = 0.5*(oldInput + smallest.input); - const auto newCost = m_costFunction.eval(newInput); - newVector.emplace_back(newInput, newCost); - } - std::sort(newVector.begin(), newVector.end()); - - return newVector; -} - -double SimplexSolver::calcVolume() -{ - // TODO: For reals do something like: - // https://math.stackexchange.com/questions/337197/finding-the-volume-of-a-tetrahedron-by-given-vertices - // For now: - // Sort by input and find the difference squared between the first and last. - - const auto inputLess = [](const Coordinate & first, const Coordinate & second) - { return first.input < second.input; }; - - // Copy the vector so we don't sort the original. - std::vector simplexCopy = m_currentSimplex; - std::sort(simplexCopy.begin(), simplexCopy.end(), inputLess); - const auto smallest = simplexCopy.front(); - const auto biggest = simplexCopy.back(); - return std::pow(biggest.input - smallest.input, 2.0); -} - -IterationState SimplexSolver::update() -{ - if (m_currentSimplex.size() < 3) - { - throw std::runtime_error("Simplex can't be a line."); - } - // Check for convergence and potentially early return. - // TODO: Make configurable. - const auto volume = calcVolume(); - if(volume < 1e-4) - { - return IterationState::CONVERGED; - } - - // Do update. - Coordinate potential; - potential.input = newPoint(); - potential.cost = m_costFunction.eval(potential.input); - - // TODO: Understand why looking at second biggest, not biggest. - // Explanation from paper is that it is in order to make - // the new guess of the next iteration different the current - // biggest. - const auto secondBiggest = *(m_currentSimplex.end() - 2); - if(potential.cost < secondBiggest.cost) - { - // Replace the last simplex value with the better one. - *(m_currentSimplex.end() - 1) = potential; - // TODO: DO I need to sort or is it sorted already? - std::sort(m_currentSimplex.begin(), m_currentSimplex.end()); - } - else - { - // Do a contraction. - m_currentSimplex = contract(); - } - - return IterationState::OK; -} +#include "j7s-optimization/CostFunction.hpp" +#include "j7s-optimization/SimplexSolver.hpp" +#include "j7s-optimization/common.hpp" int main(int, char **) { - const CostFunction cost(2.0, 3.0, 4.0); + const j7s::CostFunction cost(2.0, 3.0, 4.0); const std::vector init_simplex = {-10, 0, 10}; - SimplexSolver solver(cost, init_simplex); + j7s::SimplexSolver solver(cost, init_simplex); - IterationState state = IterationState::OK; - for(int cnt = 0; cnt < 1000; cnt++) + j7s::IterationState state = j7s::IterationState::OK; + for (int cnt = 0; cnt < 1000; cnt++) { state = solver.update(); - if(state == IterationState::CONVERGED) + if (state == j7s::IterationState::CONVERGED) { break; } } - if(state == IterationState::CONVERGED) + if (state == j7s::IterationState::CONVERGED) { const auto best = solver.bestCoord(); std::cout << "Converged! Best Input: " << best.input << " Cost: " << best.cost << std::endl; - std::cout << "Actual Best: " << cost.actualBest() << " Cost: " << cost.eval(cost.actualBest()) << std::endl; + std::cout << "Actual Best: " << cost.actualBest() + << " Cost: " << cost.eval(cost.actualBest()) << std::endl; } else { diff --git a/src/optimizer.cpp b/src/optimizer.cpp deleted file mode 100644 index 69eb45a..0000000 --- a/src/optimizer.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright 2022 James Pace -// All Rights Reserved. -// -// For a license to this software contact -// James Pace at jpace121@gmail.com. -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -#include "j7s-optimization/optimizer.hpp" - -namespace j7s -{ - -Optimizer::Optimizer() -{ -} - -Optimizer::~Optimizer() -{ -} - -} // namespace j7s