如何用代码实现同时对三维点位置和相机参数进行非线性优化（LM算法）（三维重建task2-3）

如何用代码实现同时对三维点位置和相机参数进行非线性优化（LM算法）（三维重建task2-3）

这次的代码涉及了比较多的数学原理，如无约束非线性最小优化问题、最速下降法、牛顿法、LM法等等
代码呈上

#include <fstream>
#include <sstream>
#include <sfm/camera_pose.h>
#include<cassert>
#include <iomanip>
#include "sfm/ba_conjugate_gradient.h"
#include "sfm/bundle_adjustment.h"
#include "sfm/ba_sparse_matrix.h"
#include "sfm/ba_dense_vector.h"
#include "sfm/ba_linear_solver.h"
#include "sfm/ba_sparse_matrix.h"
#include "sfm/ba_dense_vector.h"
#include "sfm/ba_cholesky.h"


typedef sfm::ba::SparseMatrix<double> SparseMatrixType;
typedef sfm::ba::DenseVector<double> DenseVectorType;

//global variables
std::vector<sfm::ba::Camera> cameras;
std::vector<sfm::ba::Point3D> points;
std::vector<sfm::ba::Observation> observations;

#define TRUST_REGION_RADIUS_INIT (1000)
#define TRUST_REGION_RADIUS_DECREMENT (1.0 / 10.0)
#define TRUST_REGION_RADIUS_GAIN (10.0)

// lm 算法最多迭代次数
const int lm_max_iterations = 100;
// mean square error
double initial_mse = 0.0;
double final_mse = 0.0;
int num_lm_iterations = 0;
int num_lm_successful_iterations = 0;
int num_lm_unsuccessful_iterations = 0;

// lm 算法终止条件
double lm_mse_threshold = 1e-16;
double lm_delta_threshold = 1e-8;

// 信赖域大小
double trust_region_radius = 1000;
int cg_max_iterations =1000;
//相机参数个数
int camera_block_dim  = 9;

const int num_cam_params = 9;

/**
 * /decription 加载相关数据，包括相机的初始内外参数，三维点，观察点
 * @param file_name
 * @param cams
 * @param pts3D
 * @param observations
 */
void load_data(const std::string& file_name
               ,std::vector<sfm::ba::Camera>&cams
               ,std::vector<sfm::ba::Point3D>&pts3D
               ,std::vector<sfm::ba::Observation> &observations){
   

    /* 加载数据 */
    std::ifstream in(file_name);
    assert(in.is_open());
    std::string line, word;

    // 加载相机参数
    {
   
        int n_cams = 0;
        getline(in, line);
        std::stringstream stream(line);
        stream >> word >> n_cams;
        cams.resize(n_cams);
        for (int i = 0; i < cams.size(); i++) {
   
            getline(in, line);
            std::stringstream stream(line);
            stream >> cams[i].focal_length;
            stream >> cams[i].distortion[0] >> cams[i].distortion[1];
            for (int j = 0; j < 3; j++)stream >> cams[i].translation[j];
            for (int j = 0; j < 9; j++)stream >> cams[i].rotation[j];
        }
    }

    // 加载三维点
    {
   
        int n_points  = 0;
        getline(in, line);
        std::stringstream stream(line);
        stream>>word>>n_points;
        pts3D.resize(n_points);
        for(int i=0; i<n_points; i++) {
   
            getline(in, line);
            std::stringstream stream(line);
            stream>>pts3D[i].pos[0]>>pts3D[i].pos[1]>>pts3D[i].pos[2];
        }
    }

    //加载观察点
    {
   
        int n_observations = 0;
        getline(in, line);
        std::stringstream stream(line);
        stream>>word>>n_observations;
        observations.resize(n_observations);
        for(int i=0; i<observations.size(); i++){
   
            getline(in, line);
            std::stringstream stream(line);
            stream>>observations[i].camera_id
                  >>observations[i].point_id
                  >>observations[i].pos[0]
                  >>observations[i].pos[1];
        }
    }

}


/*
 * Computes for a given matrix A the square matrix A^T * A for the
 * case that block columns of A only need to be multiplied with itself.
 * Becase the resulting matrix is symmetric, only about half the work
 * needs to be performed.
 */
void matrix_block_column_multiply (sfm::ba::SparseMatrix<double> const& A,
std::size_t block_size, sfm::ba::SparseMatrix<double>* B)
{
   
    sfm::ba::SparseMatrix<double>::Triplets triplets;
    triplets.reserve(A.num_cols() * block_size);
    for (std::size_t block = 0; block < A.num_cols(); block += block_size) {
   
        std::vector<sfm::ba::DenseVector<double>> columns(block_size);
        for (std::size_t col = 0; col < block_size; ++col)
            A.column_nonzeros(block + col, &columns[col]);
            for (std::size_t col = 0; col < block_size; ++col) {
   
                double dot = columns[col].dot(columns[col]);
                triplets.emplace_back(block + col, block + col, dot);
                for (std::size_t row = col + 1; row < block_size; ++row) {
   
                    dot = columns[col].dot(columns[row]);
                    triplets.emplace_back(block + row, block + col, dot);
                    triplets.emplace_back(block + col, block + row, dot);
                }
            }
        }
    B->allocate(A.num_cols(), A.num_cols());
    B->set_from_triplets(triplets);
}


/*
 * Inverts a matrix with 3x3 bocks on its diagonal. All other entries
 * must be zero. Reading blocks is thus very efficient.
 */
void
invert_block_matrix_3x3_inplace (sfm::ba::SparseMatrix<double>* A) {
   
    if (A->num_rows() != A->num_cols())
        throw std::invalid_argument("Block matrix must be square");
    if (A->num_non_zero() != A->num_rows() * 3)
        throw std::invalid_argument("Invalid number of non-zeros");

    for (double* iter = A->begin(); iter != A->end(); )
    {
   
        double* iter_backup = iter;
        math::Matrix<double, 3, 3> rot;
        for (int i = 0; i < 9; ++i)
            rot[i] = *(iter++);

        double det = math::matrix_determinant(rot);
        if (MATH_DOUBLE_EQ(det, 0.0))
            continue;

        rot = math::matrix_inverse(rot, det);
        iter = iter_backup;
        for (int i = 0; i < 9; ++i)
            *(iter++) = rot[i];
    }
}


/*
     * Inverts a symmetric, positive definite matrix with NxN bocks on its
     * diagonal using Cholesky decomposition. All other entries must be zero.
     */
void
invert_block_matrix_NxN_inplace (sfm::ba::SparseMatrix<double>* A, int blocksize)
{
   
    if (A->num_rows() != A->num_cols())
        throw std::invalid_argument("Block matrix must be square");
    if (A->num_non_zero() != A->num_rows() * blocksize)
        throw std::invalid_argument("Invalid number of non-zeros");

    int const bs2 = blocksize * blocksize;
    std::vector<double> matrix_block(b