This document details backend optimization strategies, focusing on manual implementation of the non-linear least squares solver. We address common issues like the H (Hessian) matrix being rank-deficient and discuss methods for constructing and solving the linear system efficiently.

Non-linear Least Squares Solver Flow

Handling Rank-Deficient H Matrix

Two primary approaches exist to handle a rank-deficient H matrix:

1. Levenberg-Marquardt (LM) Algorithm: Introduce a damping factor to ensure the matrix is full-rank. While this allows for a solution, the result might drift towards the null space. Relative pose relationships remain consistent, but the overall solution might require an additional transformation to align with the ground truth.
2. Prior Constraints: Incorporate prior information to enhance system observability. Libraries like g2o and Ceres often add an identity matrix to the information matrix of the first pose (H[11] += I). This is analogous to the LM approach, differing in the additive term (I vs. μI). Crucially, this enforces I*Δx=0, preventing the first pose's incremental change (Δx) from being updated.

// Illustrative example for fixing vertices (equivalent to solution 3)
// All Pose vertices
std::vector<std::shared_ptr<VertexPose>> vertexCams_vec;
for (size_t i = 0; i < cameras.size(); ++i) {
    auto vertexCam = std::make_shared<VertexPose>();
    Eigen::VectorXd pose(7);
    // Initialize pose with translation and quaternion
    pose << cameras[i].twc, cameras[i].qwc.x(), cameras[i].qwc.y(), cameras[i].qwc.z(), cameras[i].qwc.w(); 
    vertexCam->SetParameters(pose);

    // Uncomment to fix the first two poses
    // if (i < 2) {
    //     vertexCam->SetFixed();
    // }

    problem.AddVertex(vertexCam);
    vertexCams_vec.push_back(vertexCam);
}

H Matrix Construction

The process of building the H matrix involves several steps:

Determining Dimensions

1. Dimension Calculation: Define the total dimension of optimization variables n = 6p + 3l, where p is the number of poses and l is the number of landmarks. Determine the residual dimension m. For each residual, compute the Jacobian J (dimensions m_i * n). This allows for computing individual H blocks (n * n). The final H matrix is formed by accumulating these blocks from all residuals.

The total dimension ordering_generic_ represents the sum of LocalDimension() for all optimization variables, which dictates the size of the H matrix.

// Calculate total dimension of optimization variables for H matrix construction
void Problem::SetOrdering() {
    ordering_poses_ = 0;
    ordering_generic_ = 0;
    ordering_landmarks_ = 0;
    int pose_dimension_count = 0; // Debugging counter

    // Vertices are stored in a map, ordered by ID
    for (const auto& vertex_pair : verticies_) {
        auto vertex = vertex_pair.second;
        ordering_generic_ += vertex->LocalDimension(); // Total dimension of all optimizable variables

        if (IsPoseVertex(vertex)) {
            pose_dimension_count += vertex->LocalDimension();
        }

        if (problemType_ == ProblemType::SLAM_PROBLEM) {
            AddOrderingSLAM(vertex); // Separate ordering for SLAM problems
        }

        if (IsPoseVertex(vertex)) {
            std::cout << "Vertex ID: " << vertex->Id() << ", Ordering ID: " << vertex->OrderingId() << std::endl;
        }
    }

    if (problemType_ == ProblemType::SLAM_PROBLEM) {
        // Adjust landmark ordering to appear after poses
        ulong total_pose_dimension = ordering_poses_;
        for (auto const& [id, landmarkVertex] : idx_landmark_vertices_) {
            landmarkVertex->SetOrderingId(landmarkVertex->OrderingId() + total_pose_dimension);
        }
    }
}

ordering_poses_ tracks the starting index for pose variables within the H matrix. The AddOrderingSLAM function manages this by assigning sequential ordering IDs to pose and landmark vertices and sorting them into idx_pose_vertices_ and idx_landmark_vertices_ respectively.

void Problem::AddOrderingSLAM(const std::shared_ptr<myslam::backend::Vertex>& v) {
    if (IsPoseVertex(v)) {
        v->SetOrderingId(ordering_poses_);
        idx_pose_vertices_.insert({v->Id(), v});
        ordering_poses_ += v->LocalDimension(); // Accumulate pose dimensions (e.g., 6 for SE(3))
    } else if (IsLandmarkVertex(v)) {
        v->SetOrderingId(ordering_landmarks_);
        ordering_landmarks_ += v->LocalDimension(); // Accumulate landmark dimensions (e.g., 3 for XYZ)
        idx_landmark_vertices_.insert({v->Id(), v});
    }
}

Building the Hessian Matrix

The Hessian matrix is constructed by iterating through each edge (representing a constraint or measurement).

For an edge connecting two vertices (a binary edge), we retrieve its Jacobian matrices with respect to each connected vertex. The code avoids direct multiplication of J^T * Information * J by calculating smaller Jacobian blocks and placing them into the correct positions in the global H matrix using indices derived from the vertices' ordering IDs.

void Problem::MakeHessian() {
    TicToc timer_hessian;
    ulong matrix_size = ordering_generic_; // Total dimension of optimization variables
    MatXX H(MatXX::Zero(matrix_size, matrix_size)); // Hessian matrix
    VecX b(VecX::Zero(matrix_size)); // Residual vector (b)

    for (auto const& [edge_id, edge] : edges_) {
        edge->ComputeResidual();
        edge->ComputeJacobians();

        const auto& jacobians = edge->Jacobians();
        const auto& verticies = edge->Verticies();

        // Ensure Jacobian count matches vertex count for the edge
        assert(jacobians.size() == verticies.size());

        // Update Hessian and residual vector
        for (size_t i = 0; i < verticies.size(); ++i) {
            auto vertex_i = verticies[i];
            if (vertex_i->IsFixed()) continue; // Skip fixed vertices

            const auto& jacobian_i = jacobians[i];
            ulong index_i = vertex_i->OrderingId();
            ulong dimension_i = vertex_i->LocalDimension();

            // Calculate J_i^T * Information for residual update
            MatXX JtW = jacobian_i.transpose() * edge->Information();

            // Update Hessian blocks
            for (size_t j = i; j < verticies.size(); ++j) {
                auto vertex_j = verticies[j];
                if (vertex_j->IsFixed()) continue;

                const auto& jacobian_j = jacobians[j];
                ulong index_j = vertex_j->OrderingId();
                ulong dimension_j = vertex_j->LocalDimension();

                assert(vertex_j->OrderingId() != -1);
                MatXX hessian_block = JtW * jacobian_j;

                // Add to upper-left block
                H.block(index_i, index_j, dimension_i, dimension_j).noalias() += hessian_block;

                // Add to lower-right block for symmetry (if not diagonal)
                if (j != i) {
                    H.block(index_j, index_i, dimension_j, dimension_i).noalias() += hessian_block.transpose();
                }
            }
            // Update residual vector 'b'
            b.segment(index_i, dimension_i).noalias() -= JtW * edge->Residual();
        }
    }
    Hessian_ = H;
    b_ = b;
    total_hessian_cost_ += timer_hessian.toc();

    // Incorporate prior constraints if they exist
    if (prior_error_.rows() > 0) {
        // Adjust residual for prior
        b_prior_ -= H_prior_ * delta_x_.head(ordering_poses_); 
    }
    // Add prior Hessian to the top-left block of the main Hessian
    Hessian_.topLeftCorner(ordering_poses_, ordering_poses_) += H_prior_;
    // Add prior residuals to the top part of the main residual vector
    b_.head(ordering_poses_) += b_prior_;

    // Initialize the delta_x (update vector) to zero
    delta_x_ = VecX::Zero(matrix_size);
}

The code uses H.block(index_i, index_j, dim_i, dim_j).noalias() += hessian; to efficiently update the Hessian. For symmetric matrices, calculating the upper triangle is sufficient; the lower triangle is filled by transposing the computed block.

The residual vector b is updated within the outer loop for each vertex i. Note that b is only computed once per vertex i in the outer loop.

Initializing LM Damping Parameter (μ)

The initial damping parameter μ_0 for the LM algorithm is set using the maximum diagonal element of the Hessian:

μ_0 = τ * max{(J^T * J)_ii}

void Problem::ComputeLambdaInitLM() {
    // ... (existing code for finding max diagonal element) ...
    double tau = 1e-5; // Damping factor
    currentLambda_ = tau * maxDiagonal;
}

Solving the Linear System

After constructing H and b, and setting the LM parameter, the linear system H * Δx = b can be solved.

Non-SLAM Problems: Matrix Inversion

For dense H matrices typical in non-SLAM problems, direct matrix inversion is feasible.

if (problemType_ == ProblemType::GENERIC_PROBLEM) {
    // Non-SLAM problems: Solve directly using inversion
    MatXX H_augmented = Hessian_;
    // Add damping factor to the diagonal
    for (ulong i = 0; i < matrix_size; ++i) {
        H_augmented(i, i) += currentLambda_;
    }
    // Solve for delta_x
    // delta_x_ = PCGSolver(H_augmented, b_, H_augmented.rows() * 2); // PCG if preferred
    delta_x_ = Hessian_.inverse() * b_; // Direct inversion
}

SLAM Problems: Schur Complement

SLAM problems often result in sparse H matrices. Utilizing this sparsity, the Schur complement method is employed for efficient marginalization and solving.

Given the partitioned system:

$$ \begin{bmatrix} H ext{\textsubscript{pp}} & H ext{\textsubscript{pm}} \ H ext{\textsubscript{mp}} & H ext{\textsubscript{mm}} \end{bmatrix} \begin{bmatrix} \Delta x ext{\textsubscript{p}}^{\ast} \ \Delta x ext{\textsubscript{m}}^{\ast} \end{bmatrix} = \begin{bmatrix} b ext{\textsubscript{p}} \ b ext{\textsubscript{m}} \end{bmatrix} $$

where p denotes poses and m denotes landmarks:

① Partitioning by Pose and Landmark

The H matrix dimensions are (p + m) * (p + m) and Δx is (p + m) * 1.

• ordering_poses_: Total dimension of pose variables (e.g., 6 * num_poses).
• ordering_landmarks_: Total dimension of landmark variables (e.g., 3 * num_landmarks).

// SLAM problems use Schur complement for solving
int pose_block_size = ordering_poses_;       // Size of the pose block (H_pp, H_pm, H_mp)
int landmark_block_size = ordering_landmarks_; // Size of the landmark block (H_mm)

// Extract blocks for Schur complement calculation
// H_mm: Landmark-Landmark block
MatXX H_mm = Hessian_.block(pose_block_size, pose_block_size, landmark_block_size, landmark_block_size);
// H_pm: Pose-Landmark block
MatXX H_pm = Hessian_.block(0, pose_block_size, pose_block_size, landmark_block_size);
// H_mp: Landmark-Pose block
MatXX H_mp = Hessian_.block(pose_block_size, 0, landmark_block_size, pose_block_size);

// Extract residual blocks
VecX b_p = b_.segment(0, pose_block_size);
VecX b_m = b_.segment(pose_block_size, landmark_block_size);

② Schur Complement

The Schur complement is derived to eliminate landmark variables, resulting in a smaller system for poses.

1. Invert H_mm: Since H_mm is typically diagonal (especially with inverse depth parameterization), its inverse can be computed efficiently by inverting diagonal blocks or individual elements.

   // Invert the landmark-landmark block (H_mm)
   // Assuming H_mm is block-diagonal or diagonal for efficiency
   MatXX H_mm_inv(MatXX::Zero(landmark_block_size, landmark_block_size));
   for (auto const& [id, landmarkVertex] : idx_landmark_vertices_) {
       // Adjust index to be relative to the landmark block
       int idx_relative = landmarkVertex->OrderingId() - pose_block_size;
       int dim = landmarkVertex->LocalDimension(); // Dimension of this landmark's parameters (e.g., 3 for XYZ)
       // Invert the corresponding block
       H_mm_inv.block(idx_relative, idx_relative, dim, dim) = H_mm.block(idx_relative, idx_relative, dim, dim).inverse();
   }
   

2. Compute Pose Update Equation: The Schur complement reduces the system to:

   $$ \left( H ext{\textsubscript{pp}} - H ext{\textsubscript{pm}} H ext{\textsubscript{mm}}^{-1} H ext{\textsubscript{mp}} \right) \Delta x ext{\textsubscript{p}}^{\ast} = b ext{\textsubscript{p}} - H ext{\textsubscript{pm}} H ext{\textsubscript{mm}}^{-1} b ext{\textsubscript{m}} $$

   // Compute the Schur complement for the pose block (H_pp_schur_)
   MatXX temp_H_pm = H_pm * H_mm_inv;
   H_pp_schur_ = Hessian_.block(0, 0, pose_block_size, pose_block_size) - temp_H_pm * H_mp;
   // Compute the corresponding residual vector for poses (b_pp_schur_)
   b_pp_schur_ = b_p - temp_H_pm * b_m;
   

③ Solve Linear System

1. Solve for Pose Updates (Δx_p^*): Solve the reduced system for the pose increments.

   VecX delta_x_p(VecX::Zero(pose_block_size));
   // Add LM damping to the pose block's Schur complement
   for (ulong i = 0; i < pose_block_size; ++i) {
       H_pp_schur_(i, i) += currentLambda_;
   }
   
   // Solve the reduced linear system for poses
   // Using PCG solver for demonstration, though direct solve might be faster for small systems
   int iterations = H_pp_schur_.rows() * 2; // Example iteration count for PCG
   delta_x_p = PCGSolver(H_pp_schur_, b_pp_schur_, iterations);
   // Store the pose updates
   delta_x_.head(pose_block_size) = delta_x_p;
   

2. Solve for Landmark Updates (Δx_m^*): Back-substitute the pose updates to find the landmark increments using the original equations:

   H_mm * Δx_m^* = b_m - H_mp * Δx_p^*

   // Solve for landmark updates using the calculated pose updates
   VecX delta_x_landmarks(landmark_block_size);
   delta_x_landmarks = H_mm_inv * (b_m - H_mp * delta_x_p);
   // Store the landmark updates
   delta_x_.tail(landmark_block_size) = delta_x_landmarks;
   

Marginalization Test

Marginalization is a technique to remove variables from the optimization, incorporating their effect as a prior. This section demonstrates moving variables to the bottom-right corner of the H matrix, simulating marginalization.

Moving Marginalized Variables

To isolate a variable for marginalization, it's moved to the bottom-right block of the H matrix.

1. Move Row i to the End: The row containing the variable is extracted and placed at the matrix's end.

   // Example: Move row 'idx' of dimension 'dim' to the end of a 'reserve_size' matrix
   int idx = 1;            // Index of the variable to marginalize
   int dim = 1;            // Dimension of the variable
   int reserve_size = 3;   // Total dimension of variables
   
   // Move row 'idx' to the end
   Eigen::MatrixXd temp_rows = H_marg.block(idx, 0, dim, reserve_size);
   Eigen::MatrixXd temp_botRows = H_marg.block(idx + dim, 0, reserve_size - idx - dim, reserve_size);
   H_marg.block(idx, 0, reserve_size - idx - dim, reserve_size) = temp_botRows;
   H_marg.block(reserve_size - dim, 0, dim, reserve_size) = temp_rows;
   

2. Move Column i to the Right: Similarly, the corresponding column is moved to the rightmost position.

   // Move column 'idx' to the right end
   Eigen::MatrixXd temp_cols = H_marg.block(0, idx, reserve_size, dim);
   Eigen::MatrixXd temp_rightCols = H_marg.block(0, idx + dim, reserve_size, reserve_size - idx - dim);
   H_marg.block(0, idx, reserve_size, reserve_size - idx - dim) = temp_rightCols;
   H_marg.block(0, reserve_size - dim, reserve_size, dim) = temp_cols;
   

Marginalization via Schur Complement

This process effectively computes a prior Hessian (H_prior) representing the influence of the marginalized variables on the remaining ones.

 /// Perform Schur complement for marginalization
 double epsilon = 1e-8;
 int marginalized_dim = dim;
 int remaining_dim = reserve_size - dim;   // Dimension of remaining variables

 // Extract the block corresponding to marginalized variables (Amm)
 Eigen::MatrixXd Amm = 0.5 * (H_marg.block(remaining_dim, remaining_dim, marginalized_dim, marginalized_dim) + H_marg.block(remaining_dim, remaining_dim, marginalized_dim, marginalized_dim).transpose());

 // Compute inverse of Amm (e.g., using Eigen's SelfAdjointEigenSolver for robustness)
 Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(Amm);
 Eigen::MatrixXd Amm_inv = saes.eigenvectors() * Eigen::VectorXd(
         (saes.eigenvalues().array() > epsilon).select(saes.eigenvalues().array().inverse(), 0)).asDiagonal() *
                           saes.eigenvectors().transpose();

 // Extract other blocks for Schur complement calculation
 // Arm: Remaining-Marginalized block
 Eigen::MatrixXd Arm = H_marg.block(0, remaining_dim, remaining_dim, marginalized_dim);
 // Amr: Marginalized-Remaining block
 Eigen::MatrixXd Amr = H_marg.block(remaining_dim, 0, marginalized_dim, remaining_dim);
 // Arr: Remaining-Remaining block
 Eigen::MatrixXd Arr = H_marg.block(0, 0, remaining_dim, remaining_dim);

 // Compute the prior Hessian for the remaining variables
 Eigen::MatrixXd temp_Arm = Arm * Amm_inv;
 Eigen::MatrixXd H_prior = Arr - temp_Arm * Amr;

Summary: Preventing Optimization Drift

Adding Prior Constraints

Impose strong prior constraints on specific variables. This is achieved by assigning a very large weight to the corresponding information matrix in a prior edge, effectively setting the update Δx for that variable to zero.

Fixing Poses or Landmarks

Directyl fix certain camera poses or landmark positions to prevent them from being optimized. For monocular Bundle Adjustment (BA) problems, common strategies include fixing one camera pose and one feature, or fixing two camera poses. This ensures that the optimization state does not drift uncontrollably.

// Example of fixing vertices during problem construction
if (i < 2) { // Fix the first two camera poses
    vertexCam->SetFixed();
}

Supporting Structures

Frame Structure

Represents a single camera frame, storing its pose (rotation Rwc and translation twc, quaternion qwc) and observed features.

/*
 * Frame: Stores pose and observations for each camera frame.
 */
struct Frame {
    Frame(Eigen::Matrix3d R, Eigen::Vector3d t) : Rwc(R), qwc(R), twc(t) {};
    Eigen::Matrix3d Rwc; // Rotation from camera to world
    Eigen::Quaterniond qwc; // Quaternion representation of Rwc
    Eigen::Vector3d twc; // Translation of camera center in world coordinates

    // Map of feature ID to its observed 2D projection in this frame
    std::unordered_map<int, Eigen::Vector3d> featurePerId; 
};

Generating Synthetic Data

This function simulates camera poses and 3D landmark positions in a world frame, along with corresponding feature observations in each frame. This is useful for testing the optimization pipeline.

/*
 * Generate synthetic world-frame data: camera poses, feature points, and observations per frame.
 */
void GetSimDataInWordFrame(std::vector<Frame>& cameraPoses, std::vector<Eigen::Vector3d>& points) {
    int num_features = 20;  // Number of features, assuming all are visible in each frame
    int num_poses = 3;     // Number of camera poses

    double radius = 8.0;
    for (int n = 0; n < num_poses; ++n) {
        double theta = n * 2.0 * M_PI / (num_poses * 4.0); // Along a 1/4 circle arc
        // Rotation around the Z-axis
        Eigen::Matrix3d R;
        R = Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitZ());
        Eigen::Vector3d t = Eigen::Vector3d(
            radius * cos(theta) - radius,
            radius * sin(theta),
            1.0 * sin(2.0 * theta)
        );
        cameraPoses.push_back(Frame(R, t));
    }

    // Generate random 3D points for features
    std::default_random_engine generator;
    // Noise model based on 2 pixels / focal length (assuming focal length = 1)
    std::normal_distribution<double> noise_pdf(0., 2.0 / 1000.0); 
    for (int j = 0; j < num_features; ++j) {
        std::uniform_real_distribution<double> xy_rand(-4.0, 4.0);
        std::uniform_real_distribution<double> z_rand(4.0, 8.0);

        Eigen::Vector3d Pw(xy_rand(generator), xy_rand(generator), z_rand(generator));
        points.push_back(Pw);

        // Generate observations for each frame
        for (int i = 0; i < num_poses; ++i) {
            // Project 3D point into camera frame: Pc = Rwc * (Pw - twc)
            Eigen::Vector3d Pc = cameraPoses[i].Rwc.transpose() * (Pw - cameraPoses[i].twc);
            Pc = Pc / Pc.z();  // Normalize to image plane coordinates
            // Add noise
            Pc[0] += noise_pdf(generator);
            Pc[1] += noise_pdf(generator);
            cameraPoses[i].featurePerId.insert({j, Pc});
        }
    }
}

Binary Edge: Pose and Landmark Optimization Test

This section demonstrates setting up and solving a SLAM problem with binary edges (connecting poses and landmarks).

   // 1. Initialize the problem
   Problem problem(Problem::ProblemType::SLAM_PROBLEM);

   // 2. Create Pose vertices
   std::vector<std::shared_ptr<VertexPose> > vertexCams_vec;
   for (size_t i = 0; i < cameras.size(); ++i) {
       auto vertexCam = std::make_shared<VertexPose>();
       Eigen::VectorXd pose(7);
       // Initialize pose with translation and quaternion (qcw is camera-to-world)
       pose << cameras[i].twc, cameras[i].qcw.x(), cameras[i].qcw.y(), cameras[i].qcw.z(), cameras[i].qcw.w(); 
       vertexCam->SetParameters(pose);
       problem.AddVertex(vertexCam);
       vertexCams_vec.push_back(vertexCam);
   }

   // 3. Create Landmark vertices
   std::vector<std::shared_ptr<VertexPointXYZ>> allPoints;
   for (size_t i = 0; i < points.size(); ++i) {
       Eigen::Vector3d Pw = points[i]; // Assuming initial frame (frame 0) has ground truth points
       auto verterxPoint = std::make_shared<VertexPointXYZ>();
       verterxPoint->SetParameters(Pw);
       problem.AddVertex(verterxPoint);
       allPoints.push_back(verterxPoint);

       // 4. Create Reprojection Error edges (binary edges)
       // Start from the second frame (index 1) for observations
       for (size_t j = 1; j < cameras.size(); ++j) {
           Eigen::Vector3d observed_pt = cameras[j].featurePerId.at(i);
           // Create a reprojection error edge connecting landmark and pose
           auto edge = std::make_shared<EdgeReprojectionXYZ>(observed_pt);

           std::vector<std::shared_ptr<Vertex> > edge_vertices;
           edge_vertices.push_back(verterxPoint);      // Landmark vertex
           edge_vertices.push_back(vertexCams_vec[j]); // Pose vertex
           edge->SetVertex(edge_vertices);
           problem.AddEdge(edge);
       }
   }

   problem.Solve(5); // Solve the optimization problem for 5 iterations

   std::cout << "------------ Pose Translation After Optimization ----------------" << std::endl;
   for (size_t i = 0; i < vertexCams_vec.size(); ++i) {
       std::cout << "Translation (optimized) Frame " << i << ": " << vertexCams_vec[i]->Parameters().head(3).transpose()
                 << " || Ground Truth: " << cameras[i].twc.transpose() << std::endl;
   }

The results show that the first frame's pose remains stable, and the optimized values are close to the ground truth. The exact behavior can depend on the specific configuration and data.