Split and organize first pass.
This commit is contained in:
parent
6ab8166ffc
commit
a4498cd33d
|
|
@ -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
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>)
|
||||
|
||||
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
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>)
|
||||
|
|
@ -27,7 +30,7 @@ install(
|
|||
)
|
||||
install(
|
||||
TARGETS
|
||||
optimizer
|
||||
simplex_solver
|
||||
EXPORT export_${PROJECT_NAME}
|
||||
ARCHIVE DESTINATION lib/${PROJECT_NAME}
|
||||
LIBRARY DESTINATION lib/${PROJECT_NAME}
|
||||
|
|
|
|||
|
|
@ -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_
|
||||
|
|
@ -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 <vector>
|
||||
|
||||
namespace j7s
|
||||
{
|
||||
|
||||
class SimplexSolver
|
||||
{
|
||||
public:
|
||||
SimplexSolver(const CostFunction & costFunction, const std::vector<double> initSimplex);
|
||||
|
||||
IterationState update();
|
||||
|
||||
Coordinate bestCoord() const;
|
||||
|
||||
private:
|
||||
const CostFunction m_costFunction;
|
||||
std::vector<Coordinate> m_currentSimplex;
|
||||
|
||||
// Helper functions.
|
||||
double newPoint();
|
||||
std::vector<Coordinate> contract();
|
||||
double calcVolume();
|
||||
};
|
||||
|
||||
} // namespace j7s
|
||||
|
||||
#endif // J7S__SIMPLEXSOLVER_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_
|
||||
|
|
@ -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 <cmath>
|
||||
|
||||
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
|
||||
|
|
@ -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 <algorithm>
|
||||
#include <cmath>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace j7s
|
||||
{
|
||||
SimplexSolver::SimplexSolver(
|
||||
const CostFunction & costFunction, const std::vector<double> 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<Coordinate> SimplexSolver::contract()
|
||||
{
|
||||
const auto smallest = m_currentSimplex.front();
|
||||
|
||||
std::vector<Coordinate> 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<Coordinate> 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
|
||||
211
src/main.cpp
211
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 <cmath>
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <algorithm>
|
||||
#include <stdexcept>
|
||||
|
||||
#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<double> initSimplex);
|
||||
|
||||
IterationState update();
|
||||
|
||||
Coordinate bestCoord() const;
|
||||
|
||||
private:
|
||||
const CostFunction m_costFunction;
|
||||
std::vector<Coordinate> m_currentSimplex;
|
||||
|
||||
// Helper functions.
|
||||
double newPoint();
|
||||
std::vector<Coordinate> contract();
|
||||
double calcVolume();
|
||||
};
|
||||
|
||||
SimplexSolver::SimplexSolver(const CostFunction & costFunction, const std::vector<double> 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<Coordinate> SimplexSolver::contract()
|
||||
{
|
||||
const auto smallest = m_currentSimplex.front();
|
||||
|
||||
std::vector<Coordinate> 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<Coordinate> 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<double> init_simplex = {-10, 0, 10};
|
||||
SimplexSolver solver(cost, init_simplex);
|
||||
j7s::SimplexSolver solver(cost, init_simplex);
|
||||
|
||||
IterationState state = IterationState::OK;
|
||||
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
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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
|
||||
Loading…
Reference in New Issue