-
Notifications
You must be signed in to change notification settings - Fork 864
Description
I am trying to solve an optimization problem with multiple inequality constraints, but I can only add one constraint and get the solver to work. When I try to add a second inequality constraint, however, I get an error. In my code below, I can add this:
constraints.emplace_shared (x[0] - Double_(11.0), sigma);".
or
constraints.emplace_shared (Double_(10.0) - x[0], sigma);
In summary, to compile successfully, I need to comment out one of them. Can you help?
Error
ex_ineq: /gtsam_lib/repos/gtsam/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h:261: void Eigen::DenseBase::resize(Eigen::Index, Eigen::Index) [with Derived = Eigen::Block<Eigen::Matrix<double, -1, 1>, -1, -1, true>; Eigen::Index = long int]: Assertion `rows == this->rows() && cols == this->cols() && "DenseBase::resize() does not actually allow to resize."' failed.
Aborted (core dumped)
Code
#include <gtsam/constrained/ConstrainedOptProblem.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/constrained/AugmentedLagrangianOptimizer.h>
#include <gtsam/constrained/NonlinearEqualityConstraint.h>
#include
#include
namespace constrained_example {
using namespace gtsam;
/// Exponential function e^x.
static double exp_func(const double& x, gtsam::OptionalJacobian<1,1> H1 = {}) {
double result = std::exp(x);
if (H1) H1->setConstant(result);
return result;
}
/// Exponential expression e^x.
static Double_ exp(const Double_& x) { return Double_(exp_func, x); }
/// Pow functor used for pow function.
class PowFunctor {
private:
double c_;
public:
PowFunctor(const double& c) : c_(c) {}
double operator()(const double& x, gtsam::OptionalJacobian<1,1> H1 = {}) const {
if (H1) H1->setConstant(c_ * std::pow(x, c_ - 1));
return std::pow(x, c_);
}
};
/// Pow function.
static Double_ pow(const Double_& x, const double& c) {
PowFunctor pow_functor(c);
return Double_(pow_functor, x);
}
/// Plus between Double expression and double.
static Double_ operator+(const Double_& x, const double& d) { return x + Double_(d); }
/// Negative sign operator.
static Double_ operator-(const Double_& x) { return Double_(0.0) - x; }
// --- Dynamically generate keys and expressions ---
constexpr int N = 4; // Number of keys
static std::vector x_keys;
static std::vector<Double_> x;
static void init_keys() {
if (!x_keys.empty()) return; // Prevent multiple initialization
x_keys.reserve(N);
x.reserve(N);
for (int i = 0; i < N; ++i) {
x_keys.emplace_back('x', i);
x.emplace_back(x_keys.back());
}
}
// Cost function
static NonlinearFactorGraph Cost() {
init_keys();
NonlinearFactorGraph graph;
for (const auto& key : x_keys) {
auto cost_noise = noiseModel::Isotropic::Sigma(1, 1.0 / 2.0);
graph.addPrior(key, 1.5, cost_noise);
}
return graph;
}
// Equality constraints (example: sum of squares = 1)
static NonlinearEqualityConstraints EqConstraints() {
init_keys();
NonlinearEqualityConstraints constraints;
return constraints;
}
// Inequality constraints (example: first two variables)
static NonlinearInequalityConstraints IneqConstraints() {
init_keys();
NonlinearInequalityConstraints constraints;
double sigma =1;
// Upper bound constraint for x[0]
constraints.emplace_shared(x[0] - Double_(11.0), sigma);
// Lower bound constraint for x[0]
constraints.emplace_shared(Double_(10.0) - x[0], sigma);
return constraints;
}
// Initial values
static Values InitValues() {
init_keys();
Values values;
for (int i = 0; i < N; ++i) {
values.insert(x_keys[i], 0.5*(i+1)); // example initialization
}
return values;
}
// Optimal values (for checking)
static Values OptimalValues() {
init_keys();
Values values;
double norm_factor = std::sqrt(N + 1.0);
for (int i = 0; i < N; ++i) {
values.insert(x_keys[i], (i+1)/norm_factor); // example "optimal"
}
return values;
}
} // namespace constrained_example
// --- main ---
int main() {
using namespace constrained_example;
// Keys
auto costs = Cost();
auto eqConstraints = EqConstraints();
auto ineqConstraints = IneqConstraints();
auto init_values = InitValues();
auto optimal_values = OptimalValues();
ConstrainedOptProblem problem(costs, eqConstraints, ineqConstraints);
auto params = std::make_shared<AugmentedLagrangianParams>();
params->verbose = true;
AugmentedLagrangianOptimizer optimizer(problem, init_values, params);
Values results = optimizer.optimize();
results.print("Final results:\n");
return 0;
}
Environment
Ubuntu