Efficiency improvements for dual solver. Added changes for matrix-weighted slam tests

Author: holmesco

_scripts/wafr_problems.py

@@ -29,7 +29,12 @@ def load_problem(fname="_examples/mw_loc_3d_small.pkl", id=8):
     if id is not None:
         df = df.loc[id]
     # convert to Homogeneous QCQP
-    problem = convert_to_qcqp(df)
+    if "hom_qcqp" in df:
+        problem = df["hom_qcqp"]
+    elif "homQCQP" in df:
+        problem = df["homQCQP"]
+    else:
+        problem = convert_to_qcqp(df)
 
     return problem, df
 
@@ -80,11 +85,21 @@ def process_problems(fname="_examples/mw_loc_3d_small.pkl"):
 def test_problem(
     fname="_examples/mw_loc_3d_small.pkl",
     form="primal",
+    rm_dep_constr=False,
+    merge_cliques=True,
+    tol=1e-8,
     id=8,
 ):
     problem, df = load_problem(fname=fname, id=id)
+    if rm_dep_constr:
+        problem.remove_dependent_constraints()
+        if merge_cliques:
+            merge_function = problem.merge_cosmo
+        else:
+            merge_function = None
+        problem.clique_decomposition(merge_function=merge_function)
 
-    problem.plot_asg(remove_vars=["h"], html="asg.html")
+    problem.plot_asg(remove_vars=[problem.h], html="asg.html")
     problem.plot_ctree(html="ctree.html")
 
     # Solve standard SDP
@@ -94,23 +109,25 @@ def test_problem(
     # Solve decomposed SDP
     methods = dict(objective="split", constraint="split")
     clq_list, info = solve_dsdp(
-        problem, form=form, tol=1e-8, decomp_methods=methods, verbose=True
+        problem, form=form, tol=tol, decomp_methods=methods, verbose=True
     )
     Y, ranks, factor_dict = problem.get_mr_completion(clq_list)
 
     # Check solution
     assert Y.shape[1] == 1, ValueError("Decomposed solution is not rank 1")
     x = []
     for varname in df.var_sizes.keys():
-        x.append(factor_dict[varname].flatten())
+        x.append(factor_dict[varname][:, 0].flatten())
     x = np.concatenate(x)
     if factor_dict["h"][0, 0] < 0:
         x = -x
 
     cost_decomp = x @ df.cost @ x
     cost_local = df.x_gt_init @ df.cost @ df.x_gt_init
-    tol = 1e-3
-    assert cost_decomp <= cost_local + tol, ValueError("Decomposed SDP has higher cost")
+    rtol = 1e-5
+    assert cost_decomp <= cost_local * (1 + rtol), ValueError(
+        "Decomposed SDP has higher cost"
+    )
 
     if cost_local - cost_decomp > tol:
         print(f"Cost difference: {cost_local-cost_decomp}")
@@ -127,7 +144,7 @@ def test_problem(
     else:
         print("Cost difference is within tolerance. checking solution")
         np.testing.assert_allclose(
-            x, df.x_gt_init, atol=1e-6, err_msg="solution doesn't match"
+            x, df.x_gt_init, atol=5e-5, err_msg="solution doesn't match"
         )
 
 
@@ -175,7 +192,17 @@ def plot_results(dataset="mw_loc_3d"):
     # test_problem(fname="_examples/mw_loc_3d_small.pkl")
     # test_problem(fname="_examples/mw_loc_3d.pkl", form="dual", id=4)
     # test_problem(fname="_examples/rangeonlyloc2d_no_const-vel_small.pkl")
-    # test_problem(fname="_examples/rangeonlyloc2d_no_const-vel.pkl", id=27)
+    test_problem(
+        fname="_examples/rangeonlyloc2d_no_const-vel.pkl", form="dual", tol=1e-12, id=9
+    )
+    # test_problem(
+    #     fname="_examples/mw_slam_10pose_r1.pkl",
+    #     rm_dep_constr=True,
+    #     form="dual",
+    #     merge_cliques=False,
+    #     tol=1e-7,
+    #     id=0,
+    # )
 
     # Process Problems
     # process_problems(fname="_examples/mw_loc_3d_small.pkl")
@@ -185,7 +212,7 @@ def plot_results(dataset="mw_loc_3d"):
 
     # Run Speed test
     # run_speed_tests(dataset="mw_loc_3d", tol=1e-6)
-    run_speed_tests(dataset="rangeonlyloc2d_no_const-vel", tol=1e-8)
+    # run_speed_tests(dataset="rangeonlyloc2d_no_const-vel", tol=1e-8)
 
     # Plot Results
     # plot_results()

cert_tools/hom_qcqp.py

@@ -603,18 +603,23 @@ def get_mr_completion(self, clique_mats, var_list=None, rank_tol=1e5, debug=Fals
                 Yval = np.vstack([factor_dict[var] for var in clique.separator])
                 # Find the orthogonal transformation between cliques using the polar decomposition
                 Q, H = polar(V.T @ Yval)
-                U_Q = U @ Q
+
                 if debug:
                     V_Q = V @ Q
+                    U_Q = U @ Q
                     y = np.vstack([Yval, U_Q])
                     np.testing.assert_allclose(
                         y @ y.T, clique_mats[i], atol=1e-5, rtol=1e-4
                     )
-
+                # Retrieve "rotated" residuals
                 for key in clique.residual:
-                    # NOTE: Assumes that separator comes before residual in variable ordering
-                    inds = clique._get_indices(key) - (max(sep_inds) + 1)
-                    factor_dict[key] = U_Q[inds, :]
+                    inds = clique._get_indices(key)
+                    factor_dict[key] = factor[inds, :] @ Q
+
+        # Check for and fix inverted solution
+        if np.any(factor_dict[self.h] < 0):
+            for key, value in factor_dict.items():
+                factor_dict[key] = -value
 
         # Construct full factor
         if var_list is None:

cert_tools/sparse_solvers.py

@@ -223,6 +223,7 @@ def solve_dsdp_dual(
         tol (float, optional): Tolerance for solver. Defaults to TOL.
         adjust (bool, optional): If true, adjust the cost matrix. Defaults to False.
     """
+    T0 = time()
     # List of constraints with homogenizing constraint.
     A_h = PolyMatrix()
     A_h[problem.h, problem.h] = 1
@@ -233,62 +234,65 @@ def solve_dsdp_dual(
 
     # CLIQUE VARIABLES
     cliques = problem.cliques
-    zvars = [M.variable(fu.Domain.inPSDCone(c.size)) for c in cliques]
+    cvars = [M.variable(fu.Domain.inPSDCone(c.size)) for c in cliques]
 
     # LAGRANGE VARIABLES
-    y = [M.variable(fu.Domain.unbounded(1)) for i in range(len(As))]
+    y = [M.variable(f"y{i}") for i in range(len(As))]
 
     # OBJECTIVE
     if verbose:
         print("Adding Objective")
     M.objective(fu.ObjectiveSense.Minimize, y[-1])
 
-    # AFFINE CONSTRAINTS: C + sum(Ai*y_i) - sum(Z_k) = 0
+    # CONSTRUCT CERTIFICATE
+    if verbose:
+        print("Building Certificate Matrix")
+    cert_mat_list = []
+    # get constant cost matrix
+    C_mat = problem.C.get_matrix(problem.var_sizes)
+    C_fusion = fu.Expr.constTerm(sparse_to_fusion(C_mat))
+    cert_mat_list.append(C_fusion)
+    # get constraint-multiplier products
+    for i, A in enumerate(As):
+        A_mat = A.get_matrix(problem.var_sizes)
+        A_fusion = sparse_to_fusion(A_mat)
+        cert_mat_list.append(fu.Expr.mul(A_fusion, y[i]))
+    # sum into certificate
+    H = fu.Expr.add(cert_mat_list)
+
+    # AFFINE CONSTRAINTS:
+    # H_ij = C_ij + sum(Ai*y_i)_ij - sum(Z_k)_ij = 0
     # Get a list of edges in the aggregate sparsity pattern (including main diagonal)
     if verbose:
         print("Generating Affine Constraints")
     edges = [e.tuple for e in problem.asg.es]
     edges += [(v.index, v.index) for v in problem.asg.vs]
+
     # Generate one matrix constraint per edge. This links the cliques to the
     for edge_id in edges:
         # Get variables in edge from graph
         var0 = problem.asg.vs["name"][edge_id[0]]
         var1 = problem.asg.vs["name"][edge_id[1]]
-        mat_list = []
-        # Get component of Cost matrix
-        C_mat = problem.C[var0, var1]
-        if not np.all(C_mat == 0):
-            c_mat = -C_mat.reshape(-1)
-        else:
-            c_mat = 0.0
-
-        # Get component of Constraint matrices
-        for i, A in enumerate(As):
-            A_mat = A[var0, var1]
-            if not np.all(A_mat == 0):
-                if not sp.issparse(A_mat):
-                    A_mat = sp.coo_array(A_mat)
-                a_mat = sparse_to_fusion(A_mat.reshape(-1, 1))
-                mat_list.append(fu.Expr.mul(a_mat, y[i]))
-
-        # Component of clique variables
-        for k, clique in enumerate(problem.cliques):
+        # Get component of certificate matrix
+        row_inds = problem._get_indices(var0)
+        col_inds = problem._get_indices(var1)
+        inds = get_block_inds(row_inds, col_inds, var0 == var1)
+        sum_list = [H.pick(inds)]
+        # Find the cliques that are involved with these variables
+        clique_inds = problem.var_clique_map[var0] & problem.var_clique_map[var1]
+        cliques = [problem.cliques[i] for i in clique_inds]
+        # get components of clique variables
+        for clique in cliques:
             if var0 in clique.var_list and var1 in clique.var_list:
-                ind1 = clique._get_indices(var0)
-                ind2 = clique._get_indices(var1)
-                inds = []
-                for i in ind1:
-                    for j in ind2:
-                        inds.append([i, j])
-                mat_list.append(-zvars[k].pick(inds))
-
+                row_inds = clique._get_indices(var0)
+                col_inds = clique._get_indices(var1)
+                inds = get_block_inds(row_inds, col_inds, var0 == var1)
+                sum_list.append(-cvars[clique.index].pick(inds))
         # Add the list together
-        matsum = fu.Expr.add(mat_list)
-        M.constraint(f"e_{var0}_{var1}", matsum, fu.Domain.equalsTo(c_mat))
+        matsumvec = fu.Expr.add(sum_list)
+        M.constraint(f"e_{var0}_{var1}", matsumvec, fu.Domain.equalsTo(0.0))
 
     # SOLVE
-    if verbose:
-        print("Starting problem solve")
     M.setSolverParam("intpntSolveForm", "dual")
     # record problem
     if verbose:
@@ -306,50 +310,49 @@ def solve_dsdp_dual(
         M.setLogHandler(f)
 
     M.acceptedSolutionStatus(fu.AccSolutionStatus.Anything)
-    T0 = time()
-    M.solve()
     T1 = time()
+    M.solve()
+    T2 = time()
+
+    # Store information
+    info = {
+        "success": False,
+        "cost": -np.inf,
+        "runtime": T2 - T1,
+        "preprocess_time": T1 - T0,
+        "msg": str(M.getProblemStatus()),
+    }
 
     # EXTRACT SOLN
-    if M.getProblemStatus() in [
-        fu.ProblemStatus.PrimalAndDualFeasible,
-        fu.ProblemStatus.Unknown,
-    ]:
+    status = M.getProblemStatus()
+    if status == fu.ProblemStatus.PrimalAndDualFeasible:
         # Get MOSEK cost
         cost = M.primalObjValue()
-        if cost < 0:
-            print("cost is negative! sanity check:")
-        clq_list = [zvar.dual().reshape(zvar.shape) for zvar in zvars]
-        dual = [zvar.level().reshape(zvar.shape) for zvar in zvars]
+        clq_list = [cvar.dual().reshape(cvar.shape) for cvar in cvars]
+        dual = [cvar.level().reshape(cvar.shape) for cvar in cvars]
         mults = [y_i.level() for y_i in y]
-        info = {
-            "success": True,
-            "cost": cost,
-            "time": T1 - T0,
-            "msg": M.getProblemStatus(),
-            "dual": dual,
-            "mults": mults,
-        }
-    elif M.getProblemStatus() is fu.ProblemStatus.DualInfeasible:
-        clq_list = []
-        info = {
-            "success": False,
-            "cost": -np.inf,
-            "time": T1 - T0,
-            "msg": "dual infeasible",
-        }
+        info["success"] = True
+        info["dual"] = dual
+        info["cost"] = cost
+        info["mults"] = mults
     else:
-        print("Unknown status:", M.getProblemStatus())
-        clq_list = []
-        info = {
-            "success": False,
-            "cost": -np.inf,
-            "time": T1 - T0,
-            "msg": M.getProblemStatus(),
-        }
+        print("Solve Failed - Mosek Status: " + str(status))
     return clq_list, info
 
 
+def get_block_inds(row_inds, col_inds, triu=False):
+    """Helper function for getting a grid of indices based on row and column indices. Only selects upper triangle if triu is set to true"""
+    inds = []
+    for row in range(len(row_inds)):
+        if triu:
+            colstart = row
+        else:
+            colstart = 0
+        for col in range(colstart, len(col_inds)):
+            inds.append([row_inds[row], col_inds[col]])
+    return np.array(inds)
+
+
 def solve_dsdp_primal(
     problem: HomQCQP,
     reduce_constrs=None,