优化过程中符号表达式产生NaN值的调试问题

问题背景 #

在Linux上使用预编译Drake v1.14.0的C++ API，构建MathematicalProgram拟合Xⁿ（X={0,1}）上的指数族分布。设置了两个约束：

• 总概率约束：log∑ₓ∈ₓexp[xᵀφx]=0
• 某变量的边际概率约束

优化过程中触发异常：NaN is detected during Symbolic computation。已采用logsumexp技巧避免直接求和溢出，但需要排查表达式求值时的变量值，定位NaN来源。

调试尝试的问题 #

尝试继承drake::symbolic::Expression实现NoisyExpression类，重写Evaluate()方法打印环境参数，但重载方法未进入调用链，无法输出变量值。

原因分析与解决方案 #

1. 继承Expression无效的原因 #

Drake的symbolic::Expression是值类型，内部采用pimpl模式实现，不支持多态。当你将NoisyExpression对象传递给AddConstraint时，会发生对象切片，Solver仅处理基类Expression对象，因此重载的Evaluate方法不会被执行。

2. 替代调试方案 #

（1）自定义约束回调函数监控变量

使用AddConstraint的自定义函数版本，手动计算表达式并打印变量值：

// 替换原总概率约束
program.AddConstraint(
    [&](const Eigen::VectorXd& vars) -> double {
        double diag_val = vars(0);
        double off_diag_val = vars(1);
        double bias_val = vars(2);
        // 打印当前变量值
        std::cout << "Evaluating total log prob constraint with: "
                  << "diag=" << diag_val << ", off_diag=" << off_diag_val << ", bias=" << bias_val << std::endl;
        // 构建环境并计算表达式
        drake::symbolic::Environment env;
        env.insert(diag, diag_val);
        env.insert(off_diag, off_diag_val);
        env.insert(bias, bias_val);
        return compute_total_log_prob(NUM_BEACONS, diag, off_diag, bias).Evaluate(env);
    },
    0.0, 0.0, // 约束上下界：等于0
    {diag, off_diag, bias}); // 关联变量

// 边际概率约束同理
program.AddConstraint(
    [&](const Eigen::VectorXd& vars) -> double {
        double diag_val = vars(0);
        double off_diag_val = vars(1);
        double bias_val = vars(2);
        std::cout << "Evaluating marginal log prob constraint with: "
                  << "diag=" << diag_val << ", off_diag=" << off_diag_val << ", bias=" << bias_val << std::endl;
        drake::symbolic::Environment env;
        env.insert(diag, diag_val);
        env.insert(off_diag, off_diag_val);
        env.insert(bias, bias_val);
        return compute_marginal_log_prob(NUM_BEACONS, diag, off_diag, bias).Evaluate(env);
    },
    std::log(P_BEACON), std::log(P_BEACON),
    {diag, off_diag, bias});

（2）修复logsumexp的边界情况

当sum因指数下溢变为0时，log(sum)会产生NaN，给sum添加极小epsilon避免：

auto logsumexp(const auto &terms) {
    if (terms.size() == 1) {
        return terms[0];
    }
    const auto max_elem =
        std::accumulate(terms.begin() + 1, terms.end(), *terms.begin(), drake::symbolic::max);

    const auto sum = std::accumulate(
        terms.begin() + 1, terms.end(), exp(terms[0] - max_elem),
        [&max_elem](const auto &accum, const auto &term) { return accum + exp(term - max_elem); });

    // 添加epsilon避免log(0)
    return log(sum + 1e-12) + max_elem;
}

（3）约束变量数值范围

优化过程中变量值过大可能导致指数溢出，添加边界约束限制变量范围：

program.AddBoundingBoxConstraint(-10.0, 10.0, diag);
program.AddBoundingBoxConstraint(-10.0, 10.0, off_diag);
program.AddBoundingBoxConstraint(-20.0, 0.0, bias); // bias是log概率，应为负数

3. 修正测试用例的逻辑冲突 #

当前代码中AddLinearEqualityConstraint(bias, std::log(P_NO_BEACONS))将bias设为log(0.1)，但注释期望bias为log(0.25)，这会导致约束冲突，加剧NaN问题，需统一两者的取值。

原最小示例代码 #

#include <iostream>
#include <numeric>

#include "drake/solvers/mathematical_program.h"
#include "drake/solvers/solve.h"
#include "gtest/gtest.h"

namespace robot::experimental::beacon_sim {
class NoisyExpression : public drake::symbolic::Expression {
   public:
    NoisyExpression(const drake::symbolic::Expression &other, const bool is_noisy)
        : drake::symbolic::Expression(other), is_noisy_(is_noisy) {}

    double Evaluate(const drake::symbolic::Environment &env) {
        std::cout << "Evaluate" << std::endl;
        if (is_noisy_) {
            std::cout << "---------------------------------------" << std::endl;
            std::cout << env << std::endl;
        }
        return drake::symbolic::Expression::Evaluate(env);
    }

   private:
    bool is_noisy_;
};

constexpr int n_choose_k(const int n, const int k) {
    int num = 1;
    int den = 1;
    for (int i = 1; i <= k; i++) {
        num *= n + 1 - i;
        den *= i;
    }
    return num / den;
}

auto logsumexp(const auto &terms) {
    if (terms.size() == 1) {
        return terms[0];
    }
    const auto max_elem =
        std::accumulate(terms.begin() + 1, terms.end(), *terms.begin(), drake::symbolic::max);

    const auto sum = std::accumulate(
        terms.begin() + 1, terms.end(), exp(terms[0] - max_elem),
        [&max_elem](const auto &accum, const auto &term) { return accum + exp(term - max_elem); });

    return log(sum) + max_elem;
}

drake::symbolic::Expression compute_total_log_prob(const int n,
                                                   const drake::symbolic::Variable &phi,
                                                   const drake::symbolic::Variable &psi,
                                                   const drake::symbolic::Variable &bias) {
    const auto kth_term = [&phi, &psi, &bias](const int n,
                                              const int k) -> drake::symbolic::Expression {
        return std::log(static_cast<double>(n_choose_k(n, k))) +
               static_cast<double>(k) * phi + static_cast<double>((k - 1) * k / 2) * psi + bias;
    };

    std::vector<drake::symbolic::Expression> terms;
    terms.reserve(n);
    for (int k = 0; k < n; k++) {
        terms.push_back(kth_term(n, k));
    }

    return logsumexp(terms);
}

drake::symbolic::Expression compute_marginal_log_prob(const int n,
                                                      const drake::symbolic::Variable &phi,
                                                      const drake::symbolic::Variable &psi,
                                                      const drake::symbolic::Variable &bias) {
    const auto kth_term = [&phi, &psi, &bias](const int n,
                                              const int k) -> drake::symbolic::Expression {
        return std::log(static_cast<double>(n_choose_k(n, k))) +
               static_cast<double>(k + 1) * phi + static_cast<double>((k + 1) * k / 2) * psi + bias;
    };
    std::vector<drake::symbolic::Expression> terms;
    terms.reserve(n);
    for (int k = 0; k < n; k++) {
        terms.push_back(kth_term(n - 1, k));
    }

    return logsumexp(terms);
}

TEST(CorrelatedBeaconsTest, min_example) {
    drake::solvers::MathematicalProgram program;
    const auto covariance_matrix_vars = program.NewContinuousVariables(2, "covar");
    const auto &diag = covariance_matrix_vars[0];
    const auto &off_diag = covariance_matrix_vars[1];
    const auto bias_var = program.NewContinuousVariables(1, "offset");
    const auto &bias = bias_var[0];
    constexpr double P_NO_BEACONS = 0.1;
    constexpr double P_BEACON = 0.5;
    constexpr int NUM_BEACONS = 2;

    // Try to fit an exponential family distribution to two variables. Since the variables are
    // we would like to have the following output:
    // P((0,0)) = P((0, 1)) = P((1, 0)) = P((1, 1)) = 0.25
    // The exponential family has the form P(x) = exp(x^t @ [[phi, psi], [psi, phi]] @ x + bias)
    // For this test case, we expect that phi = psi = 0 and bias = log(0.25)
    program.AddLinearEqualityConstraint(bias, std::log(P_NO_BEACONS));
    program.AddConstraint(
        NoisyExpression(compute_total_log_prob(NUM_BEACONS, diag, off_diag, bias), false) == 0.0);
    program.AddConstraint(
        NoisyExpression(compute_marginal_log_prob(NUM_BEACONS, diag, off_diag, bias), true) ==
        std::log(P_BEACON));
    program.AddCost(diag * diag + off_diag * off_diag + bias * bias);

    std::cout << program << std::endl;
    const auto result = Solve(program);

    std::cout << result.is_success() << std::endl;
    std::cout << "Diag: " << result.GetSolution(diag) << std::endl;
    std::cout << "Off diag: " << result.GetSolution(off_diag) << std::endl;
    std::cout << "bias: " << result.GetSolution(bias) << std::endl;
}
}  // namespace robot::experimental::beacon_sim

内容的提问来源于stack exchange，提问作者Erick Fuentes