Skip to content

Add the inequality factor under the Augmented Lagrange Method #2233

@Anas-Abdelkarim

Description

@Anas-Abdelkarim

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

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions