diff --git a/include/constants.h b/include/constants.h index 0107be4..80cdade 100644 --- a/include/constants.h +++ b/include/constants.h @@ -28,6 +28,7 @@ extern "C" { #define DAQP_DEFAULT_REFACTOR_TOL 1e-9 #define DAQP_DEFAULT_EPS_PROX 1e-6 + // MACROS #define DAQP_ARSUM(x) ((x)*(x+1)/2) #define DAQP_R_OFFSET(X,Y) (((2*Y-X-1)*X)/2) diff --git a/interfaces/daqp-eigen/CMakeLists.txt b/interfaces/daqp-eigen/CMakeLists.txt index 756b471..db9e189 100644 --- a/interfaces/daqp-eigen/CMakeLists.txt +++ b/interfaces/daqp-eigen/CMakeLists.txt @@ -18,6 +18,7 @@ add_executable(03_update tests/03_update.cpp) add_executable(04_slack_sign tests/04_slack_sign.cpp) add_executable(05_warmstart tests/05_warmstart.cpp) add_executable(06_general_hessian tests/06_general_hessian.cpp) +add_executable(07_symmetrize_hessian tests/07_symmetrize_hessian.cpp) set(TARGETS 00_basic_qp @@ -27,6 +28,7 @@ set(TARGETS 04_slack_sign 05_warmstart 06_general_hessian + 07_symmetrize_hessian ) foreach(TARGET ${TARGETS}) diff --git a/interfaces/daqp-eigen/tests/07_symmetrize_hessian.cpp b/interfaces/daqp-eigen/tests/07_symmetrize_hessian.cpp new file mode 100644 index 0000000..1420a80 --- /dev/null +++ b/interfaces/daqp-eigen/tests/07_symmetrize_hessian.cpp @@ -0,0 +1,108 @@ +#include +#include +#include + +// Verify that DAQP symmetrizes H before factorization. +// +// Key properties tested: +// 1. A fully symmetric H gives the correct reference solution (regression). +// 2. A lower-triangular H is no longer silently treated as a diagonal matrix. +// DAQP always packs 0.5*(H+H^T) into its internal Rinv buffer before +// factorization, so the off-diagonal structure is preserved regardless of +// which triangle the caller fills in. +// 3. A nearly-symmetric H (tiny numerical noise in one triangle) gives the +// same solution as the perfectly symmetric reference. +// 4. H is never mutated — it is user-owned and strictly read-only. +int main() { + double precision = 1e-6; + + // Symmetric positive-definite Hessian (full storage) + Eigen::MatrixXd H_full(2, 2); + H_full << 4, 2, + 2, 3; + + Eigen::VectorXd f = (Eigen::VectorXd(2) << 1, -1).finished(); + + // Simple bounds only (no general constraints), so ms == m. + Eigen::Matrix A(0, 2); + Eigen::VectorXd bu = (Eigen::VectorXd(2) << 5, 5).finished(); + Eigen::VectorXd bl = (Eigen::VectorXd(2) << -5, -5).finished(); + Eigen::VectorXi sense = Eigen::VectorXi::Zero(2); + Eigen::VectorXi break_points = Eigen::VectorXi::Zero(0); + + // 1. Reference: solve with full symmetric H. + // Unconstrained optimum: x* = -H^{-1}*f = [-0.625, 0.75] + EigenDAQPResult ref = daqp_solve(H_full, f, A, bu, bl, sense, break_points); + if (ref.exitflag <= 0) { + std::cerr << "Reference solve failed with exitflag " << ref.exitflag << std::endl; + return 1; + } + + // 2. Diagonal-only H – used as the "wrong" baseline. + // Before the fix, providing a lower-triangular H would produce exactly + // the same result as this because the upper triangle was read as zeros. + Eigen::MatrixXd H_diag = Eigen::MatrixXd::Zero(2, 2); + H_diag(0, 0) = H_full(0, 0); + H_diag(1, 1) = H_full(1, 1); + EigenDAQPResult res_diag = daqp_solve(H_diag, f, A, bu, bl, sense, break_points); + if (res_diag.exitflag <= 0) { + std::cerr << "Diagonal solve failed with exitflag " << res_diag.exitflag << std::endl; + return 1; + } + + // 3. Lower-triangular H (upper triangle set to zero). + // DAQP always packs 0.5*(H+H^T) before factorization, so the + // off-diagonal entries (0.5*lower value) are correctly included. + // Result must differ from the diagonal-only solve. + Eigen::MatrixXd H_lower = H_full.triangularView(); + Eigen::MatrixXd H_lower_copy = H_lower; // snapshot to check H is not mutated + EigenDAQPResult res_lower = daqp_solve(H_lower, f, A, bu, bl, sense, break_points); + if (res_lower.exitflag <= 0) { + std::cerr << "Lower-triangle solve failed with exitflag " << res_lower.exitflag + << std::endl; + return 1; + } + bool lower_not_mutated = H_lower.isApprox(H_lower_copy); + + // 4. Nearly-symmetric H: full H with tiny noise in one off-diagonal entry. + // Symmetrization should average the two triangles and give the same + // solution as the perfectly symmetric reference. + Eigen::MatrixXd H_noisy = H_full; + H_noisy(0, 1) += 1e-8; // tiny asymmetry + EigenDAQPResult res_noisy = daqp_solve(H_noisy, f, A, bu, bl, sense, break_points); + if (res_noisy.exitflag <= 0) { + std::cerr << "Noisy-H solve failed with exitflag " << res_noisy.exitflag << std::endl; + return 1; + } + + // --- Checks --- + + // Full symmetric H gives the reference solution. + bool ref_ok = ref.get_primal().isApprox( + (Eigen::VectorXd(2) << -0.625, 0.75).finished(), precision); + + // After symmetrization, lower-triangular H must NOT equal the diagonal result. + // (Before the fix they would be identical.) + bool lower_not_diagonal = !res_lower.get_primal().isApprox( + res_diag.get_primal(), precision); + + if (!lower_not_mutated) + std::cerr << "H was mutated by daqp_solve (should be read-only)!" << std::endl; + + // Nearly-symmetric H: symmetrized result matches the reference. + bool noisy_ok = res_noisy.get_primal().isApprox(ref.get_primal(), 1e-5); + + if (!ref_ok) + std::cerr << "Reference solution incorrect: " + << ref.get_primal().transpose() << std::endl; + if (!lower_not_diagonal) + std::cerr << "Lower-triangle H was incorrectly treated as diagonal!\n" + << "lower: " << res_lower.get_primal().transpose() << "\n" + << "diag: " << res_diag.get_primal().transpose() << std::endl; + if (!noisy_ok) + std::cerr << "Noisy-H result differs from reference:\n" + << "noisy: " << res_noisy.get_primal().transpose() << "\n" + << "ref: " << ref.get_primal().transpose() << std::endl; + + return (ref_ok && lower_not_diagonal && lower_not_mutated && noisy_ok) ? 0 : 1; +} diff --git a/src/utils.c b/src/utils.c index f65d206..7a3b515 100644 --- a/src/utils.c +++ b/src/utils.c @@ -176,97 +176,103 @@ int daqp_update_ldp(const int mask, DAQPWorkspace *work, DAQPProblem* qp){ } int daqp_update_Rinv(DAQPWorkspace *work, c_float* H, int is_factored){ - int i, j, k, disp, disp2, disp3; + int i, j, k, disp, disp2; const int n = work->n; c_float eps = work->settings->eps_prox; c_float zero_tol = work->settings->zero_tol; + // Reset the semi-proximal mask for this factorization if(work->prox_mask != NULL){ for(i = 0; i < n; i++) work->prox_mask[i] = 0; } work->n_prox = 0; - // Check if Diagonal + // Check if Diagonal — for unfactored H scan only the upper-triangle + // off-diagonals of the n×n row-major matrix (O(n²/2) reads), skipping + // the packing step entirely when H is diagonal. int is_diagonal = 1; - for (i=0, disp=1; i 1e-12 || H[disp] < -1e-12){ - is_diagonal = 0; break; + if(!is_factored){ + for(i = 0, disp = 1; i < n && is_diagonal; i++, disp += i+1){ + for(j = 1; j < n-i; j++, disp++){ + if(H[disp] > zero_tol || H[disp] < -zero_tol){ is_diagonal = 0; break; } + } + } + } else { + for(i = 0, disp = 0; i < n; disp += n-i, i++){ + for(j = 1; j < n-i; j++){ + if(H[disp+j] > zero_tol || H[disp+j] < -zero_tol){ + is_diagonal = 0; break; + } } + if(!is_diagonal) break; } - if(!is_diagonal) break; } - // Diagonal Case + // Diagonal Case — for unfactored H read diagonals directly (no packing needed). if(is_diagonal){ if(work->Rinv != NULL){ work->RinvD = work->Rinv; work->Rinv = NULL; } - - disp = 0; - for(i=0; iprox_mask != NULL) work->prox_mask[i] = 1; work->n_prox++; Hi += eps; } - if (Hi <= zero_tol) return DAQP_EXIT_NONCONVEX; + if(Hi <= zero_tol) return DAQP_EXIT_NONCONVEX; Hi = sqrt(Hi); - disp += n+1; } else { if(Hi <= zero_tol){ if(work->prox_mask != NULL) work->prox_mask[i] = 1; work->n_prox++; Hi = sqrt(Hi*Hi + eps); // Regularization for factors } - disp += n-i; } - work->RinvD[i] = 1/Hi; if(work->scaling != NULL && i < work->ms) work->scaling[i] = Hi; } return 1; } - // Prepare Rinv for dense data + // Not diagonal: ensure Rinv points to allocated data, then pack + // (symmetrize) H into Rinv before Cholesky. if(work->RinvD != NULL){ work->Rinv = work->RinvD; work->RinvD = NULL; } + if(!is_factored){ + for(i = 0, disp = 0; i < n; i++){ + work->Rinv[disp++] = H[i*n+i]; + for(j = i+1; j < n; j++) + work->Rinv[disp++] = (c_float)0.5*(H[i*n+j] + H[j*n+i]); + } + } - // Cholesky - if (is_factored) { + // Cholesky. + if(is_factored){ for(i=0, disp=0; iRinv[disp] = 1/H[disp]; // Store 1/rii - for (j=1, disp++; jRinv[disp] = H[disp]; } } else { - // Standard Cholesky (H -> R), adding eps only where the diagonal - // of the factor would otherwise be non-positive (semi-proximal). - for (i=0, disp=0, disp3=0; iRinv[disp]; // read before overwrite + for(k = 0, disp2 = i; k < i; k++, disp2 += n-k) diag_i -= work->Rinv[disp2] * work->Rinv[disp2]; - - // Only regularize if this direction is singular if(diag_i <= zero_tol){ if(work->prox_mask != NULL) work->prox_mask[i] = 1; work->n_prox++; diag_i += eps; } - - if (diag_i <= zero_tol) return DAQP_EXIT_NONCONVEX; - work->Rinv[disp] = sqrt(diag_i); - - for (j=1; jRinv[disp+j] = H[disp3++]; - for (k=0, disp2=i; kRinv[disp+j] -= work->Rinv[disp2] * work->Rinv[disp2+j]; - work->Rinv[disp+j] /= work->Rinv[disp]; + work->Rinv[disp+j] *= diag_i; } - work->Rinv[disp] = 1/work->Rinv[disp]; + work->Rinv[disp] = diag_i; } } @@ -277,7 +283,8 @@ int daqp_update_Rinv(DAQPWorkspace *work, c_float* H, int is_factored){ for(j=k+1; jRinv[disp2++] *= -work->Rinv[disp]; for(i=k+1, disp++; iRinv[disp] *= work->Rinv[disp2++]; - for(j=1; jRinv[disp+j] -= work->Rinv[disp2++] * work->Rinv[disp]; + for(j=1; jRinv[disp+j] -= work->Rinv[disp2++] * work->Rinv[disp]; } } return 1;