捆绑调整（码—opencv)
作者：互联网
理论知识参考于基于图像的三维重建—捆绑调整(6)
过程比较复杂，这里也不废话，直接上代码
ba_dense_vector.h
#pragma once
#include <cmath>
#include <stdexcept>
#include <vector>
template <typename T>
class DenseVector
{
public:
DenseVector(void) = default;
DenseVector(std::size_t size, T const& value = T(0));
void resize(std::size_t size, T const& value = T(0));
void clear(void);
void fill(T const& value);
std::size_t size(void) const;
T* data(void);
T const* data(void) const;
T* begin(void);
T const* begin(void) const;
T* end(void);
T const* end(void) const;
DenseVector operator- (void) const;
bool operator== (DenseVector const& rhs) const;
T& operator[] (std::size_t index);
T const& operator[] (std::size_t index) const;
T& at(std::size_t index);
T const& at(std::size_t index) const;
T norm(void) const;
T squared_norm(void) const;
T dot(DenseVector const& rhs) const;
DenseVector add(DenseVector const& rhs) const;
DenseVector subtract(DenseVector const& rhs) const;
DenseVector multiply(T const& factor) const;
void multiply_self(T const& factor);
void negate_self(void);
private:
std::vector<T> values;
};
/* ------------------------ Implementation ------------------------ */
template <typename T>
inline DenseVector<T>::DenseVector(std::size_t size, T const& value)
{
this->resize(size, value);
}
template <typename T>
inline void DenseVector<T>::resize(std::size_t size, T const& value)
{
this->values.clear();
this->values.resize(size, value);
}
template <typename T>
inline void DenseVector<T>::clear(void)
{
this->values.clear();
}
template <typename T>
inline void DenseVector<T>::fill(T const& value)
{
std::fill(this->values.begin(), this->values.end(), value);
}
template <typename T>
std::size_t DenseVector<T>::size(void) const
{
return this->values.size();
}
template <typename T>
T* DenseVector<T>::data(void)
{
return this->values.data();
}
template <typename T>
T const* DenseVector<T>::data(void) const
{
return this->values.data();
}
template <typename T>
T* DenseVector<T>::begin(void)
{
return this->values.data();
}
template <typename T>
T const* DenseVector<T>::begin(void) const
{
return this->values.data();
}
template <typename T>
T* DenseVector<T>::end(void)
{
return this->values.data() + this->values.size();
}
template <typename T>
T const* DenseVector<T>::end(void) const
{
return this->values.data() + this->values.size();
}
template <typename T>
DenseVector<T> DenseVector<T>::operator- (void) const
{
DenseVector ret(this->size());
for (std::size_t i = 0; i < this->size(); ++i)
ret[i] = -this->at(i);
return ret;
}
template <typename T>
bool DenseVector<T>::operator== (DenseVector const& rhs) const
{
if (this->size() != rhs.size())
return false;
for (std::size_t i = 0; i < this->size(); ++i)
if (this->at(i) != rhs.at(i))
return false;
return true;
}
template <typename T>
T& DenseVector<T>::operator[] (std::size_t index)
{
return this->values[index];
}
template <typename T>
T const& DenseVector<T>::operator[] (std::size_t index) const
{
return this->values[index];
}
template <typename T>
T& DenseVector<T>::at(std::size_t index)
{
return this->values[index];
}
template <typename T>
T const& DenseVector<T>::at(std::size_t index) const
{
return this->values[index];
}
template <typename T>
inline T DenseVector<T>::norm(void) const
{
return std::sqrt(this->squared_norm());
}
template <typename T>
T DenseVector<T>::squared_norm(void) const
{
return this->dot(*this);
}
template <typename T>
T DenseVector<T>::dot(DenseVector<T> const& rhs) const
{
if (this->size() != rhs.size())
throw std::invalid_argument("Incompatible vector dimensions");
T ret(0);
for (std::size_t i = 0; i < this->size(); ++i)
ret += this->values[i] * rhs.values[i];
return ret;
}
template <typename T>
DenseVector<T> DenseVector<T>::subtract(DenseVector<T> const& rhs) const
{
if (this->size() != rhs.size())
throw std::invalid_argument("Incompatible vector dimensions");
DenseVector<T> ret(this->size(), T(0));
for (std::size_t i = 0; i < this->size(); ++i)
ret.values[i] = this->values[i] - rhs.values[i];
return ret;
}
template <typename T>
DenseVector<T> DenseVector<T>::add(DenseVector<T> const& rhs) const
{
if (this->size() != rhs.size())
throw std::invalid_argument("Incompatible vector dimensions");
DenseVector<T> ret(this->size(), T(0));
for (std::size_t i = 0; i < this->size(); ++i)
ret.values[i] = this->values[i] + rhs.values[i];
return ret;
}
template <typename T>
DenseVector<T> DenseVector<T>::multiply(T const& factor) const
{
DenseVector<T> ret(this->size(), T(0));
for (std::size_t i = 0; i < this->size(); ++i)
ret[i] = this->at(i) * factor;
return ret;
}
template <typename T>
void DenseVector<T>::multiply_self(T const& factor)
{
for (std::size_t i = 0; i < this->size(); ++i)
this->at(i) *= factor;
}
template <typename T>
void DenseVector<T>::negate_self(void)
{
for (std::size_t i = 0; i < this->size(); ++i)
this->at(i) = -this->at(i);
}
ba_sparse_matrix.h
#pragma once
#include <thread>
#include <stdexcept>
#include <vector>
#include <algorithm>
#include <iostream>
#include "ba_dense_vector.h"
/**
* 耶鲁格式的稀疏矩阵类，用于列主矩阵。
*/
template <typename T>
class SparseMatrix
{
public:
/** 带行/列索引的三元组，以及实际值。 */
struct Triplet
{
Triplet(void) = default;
Triplet(std::size_t row, std::size_t col, T const& value);
std::size_t row;
std::size_t col;
T value;
};
/** 三元组数组 */
typedef std::vector<Triplet> Triplets;
public:
SparseMatrix(void);
SparseMatrix(std::size_t rows, std::size_t cols);
void allocate(std::size_t rows, std::size_t cols);
void reserve(std::size_t num_elements);
void set_from_triplets(Triplets const& triplets);
void mult_diagonal(T const& factor);
void cwise_invert(void);
void column_nonzeros(std::size_t col, DenseVector<T>* vector) const;
SparseMatrix transpose(void) const;
SparseMatrix subtract(SparseMatrix const& rhs) const;
SparseMatrix multiply(SparseMatrix const& rhs) const;
SparseMatrix sequential_multiply(SparseMatrix const& rhs) const;
SparseMatrix parallel_multiply(SparseMatrix const& rhs) const;
DenseVector<T> multiply(DenseVector<T> const& rhs) const;
SparseMatrix diagonal_matrix(void) const;
std::size_t num_non_zero(void) const;
std::size_t num_rows(void) const;
std::size_t num_cols(void) const;
T* begin(void);
T* end(void);
void debug(void) const;
private:
std::size_t rows;
std::size_t cols;
std::vector<T> values;
std::vector<std::size_t> outer; // outer[i+1] - outer[i] = 第i行有多少个非零值
std::vector<std::size_t> inner; // 非零元素的列 id
};
template <typename T>
SparseMatrix<T>::Triplet::Triplet(std::size_t row,
std::size_t col, T const& value)
: row(row), col(col), value(value)
{
}
/* --------------------------------------------------------------- */
template <typename T>
SparseMatrix<T>::SparseMatrix(void)
: rows(0)
, cols(0)
{
}
template <typename T>
SparseMatrix<T>::SparseMatrix(std::size_t rows, std::size_t cols)
{
this->allocate(rows, cols);
}
template <typename T>
void SparseMatrix<T>::allocate(std::size_t rows, std::size_t cols)
{
this->rows = rows;
this->cols = cols;
this->values.clear();
this->outer.clear();
this->inner.clear();
this->outer.resize(cols + 1, 0);
}
template <typename T>
void SparseMatrix<T>::reserve(std::size_t num_elements)
{
this->inner.reserve(num_elements);
this->values.reserve(num_elements);
}
template <typename T>
void SparseMatrix<T>::set_from_triplets(Triplets const& triplets)
{
/* 创建临时转置矩阵 */
SparseMatrix<T> transposed(this->cols, this->rows);
transposed.values.resize(triplets.size());
transposed.inner.resize(triplets.size());
// 统计每一行 Non-zero elements 的个数
/* 使用大量的内部值初始化外部索引。 */
for (std::size_t i = 0; i < triplets.size(); ++i)
transposed.outer[triplets[i].row]++;
/* Convert amounts to indices with prefix sum. */
std::size_t sum = 0;
std::vector<std::size_t> scratch(transposed.outer.size());
for (std::size_t i = 0; i < transposed.outer.size(); ++i)
{
std::size_t const temp = transposed.outer[i];
transposed.outer[i] = sum;
scratch[i] = sum;
sum += temp;
}
/* 加上三元组，内部索引将不排序。 */
for (std::size_t i = 0; i < triplets.size(); ++i)
{
Triplet const& t = triplets[i];
std::size_t pos = scratch[t.row]++;
transposed.values[pos] = t.value;
transposed.inner[pos] = t.col;
}
/* 转置矩阵，内部索引的隐式排序。 */
*this = transposed.transpose();
}
template <typename T>
SparseMatrix<T> SparseMatrix<T>::transpose(void) const
{
SparseMatrix ret(this->cols, this->rows);
ret.values.resize(this->num_non_zero());
ret.inner.resize(this->num_non_zero());
/* 计算转置矩阵的内部大小。 */
for (std::size_t i = 0; i < this->inner.size(); ++i)
ret.outer[this->inner[i]] += 1;
/* Compute outer sizes of transposed matrix with prefix sum. */
std::size_t sum = 0;
std::vector<std::size_t> scratch(ret.outer.size());
for (std::size_t i = 0; i < ret.outer.size(); ++i)
{
std::size_t const temp = ret.outer[i];
ret.outer[i] = sum;
scratch[i] = sum;
sum += temp;
}
/* 写出转置矩阵的内部索引和值。 */
for (std::size_t i = 0; i < this->outer.size() - 1; ++i)
for (std::size_t j = this->outer[i]; j < this->outer[i + 1]; ++j)
{
std::size_t pos = scratch[this->inner[j]]++;
ret.inner[pos] = i;
ret.values[pos] = this->values[j];
}
return ret;
}
template <typename T>
SparseMatrix<T> SparseMatrix<T>::subtract(SparseMatrix const& rhs) const
{
if (this->rows != rhs.rows || this->cols != rhs.cols)
throw std::invalid_argument("Incompatible matrix dimensions");
SparseMatrix ret(this->rows, this->cols);
ret.reserve(this->num_non_zero() + rhs.num_non_zero());
std::size_t num_outer = this->outer.size() - 1;
for (std::size_t outer = 0; outer < num_outer; ++outer)
{
ret.outer[outer] = ret.values.size();
std::size_t i1 = this->outer[outer];
std::size_t i2 = rhs.outer[outer];
std::size_t const i1_end = this->outer[outer + 1];
std::size_t const i2_end = rhs.outer[outer + 1];
while (i1 < i1_end || i2 < i2_end)
{
if (i1 >= i1_end)
{
ret.values.push_back(-rhs.values[i2]);
ret.inner.push_back(rhs.inner[i2]);
i2 += 1;
continue;
}
if (i2 >= i2_end)
{
ret.values.push_back(this->values[i1]);
ret.inner.push_back(this->inner[i1]);
i1 += 1;
continue;
}
std::size_t id1 = this->inner[i1];
std::size_t id2 = rhs.inner[i2];
if (id1 < id2)
ret.values.push_back(this->values[i1]);
else if (id2 < id1)
ret.values.push_back(-rhs.values[i2]);
else
ret.values.push_back(this->values[i1] - rhs.values[i2]);
i1 += static_cast<std::size_t>(id1 <= id2);
i2 += static_cast<std::size_t>(id2 <= id1);
ret.inner.push_back(std::min(id1, id2));
}
}
ret.outer.back() = ret.values.size();
return ret;
}
template <typename T>
SparseMatrix<T> SparseMatrix<T>::multiply(SparseMatrix const& rhs) const
{
#ifdef _OPENMP
return this->parallel_multiply(rhs);
#else
return this->sequential_multiply(rhs);
#endif
}
template <typename T>
SparseMatrix<T> SparseMatrix<T>::sequential_multiply(SparseMatrix const& rhs) const
{
if (this->cols != rhs.rows)
throw std::invalid_argument("Incompatible matrix dimensions");
SparseMatrix ret(this->rows, rhs.cols);
ret.reserve(this->num_non_zero() + rhs.num_non_zero());
/* 矩阵乘法 */
std::vector<T> ret_col(ret.rows, T(0));
std::vector<bool> ret_nonzero(ret.rows, false);
for (std::size_t col = 0; col < ret.cols; ++col)
{
ret.outer[col] = ret.values.size();
std::fill(ret_col.begin(), ret_col.end(), T(0));
std::fill(ret_nonzero.begin(), ret_nonzero.end(), false);
std::size_t rhs_col_begin = rhs.outer[col];
std::size_t rhs_col_end = rhs.outer[col + 1];
for (std::size_t i = rhs_col_begin; i < rhs_col_end; ++i)
{
T const& rhs_col_value = rhs.values[i];
std::size_t const lhs_col = rhs.inner[i];
std::size_t const lhs_col_begin = this->outer[lhs_col];
std::size_t const lhs_col_end = this->outer[lhs_col + 1];
for (std::size_t j = lhs_col_begin; j < lhs_col_end; ++j)
{
std::size_t const id = this->inner[j];
ret_col[id] += this->values[j] * rhs_col_value;
ret_nonzero[id] = true;
}
}
for (std::size_t i = 0; i < ret.rows; ++i)
if (ret_nonzero[i])
{
ret.inner.push_back(i);
ret.values.push_back(ret_col[i]);
}
}
ret.outer[ret.cols] = ret.values.size();
return ret;
}
/* 多线程矩阵乘法 */
template <typename T>
SparseMatrix<T> SparseMatrix<T>::parallel_multiply(SparseMatrix const& rhs) const
{
if (this->cols != rhs.rows)
throw std::invalid_argument("Incompatible matrix dimensions");
std::size_t nnz = this->num_non_zero() + rhs.num_non_zero();
SparseMatrix ret(this->rows, rhs.cols);
ret.reserve(nnz);
std::fill(ret.outer.begin(), ret.outer.end(), 0);
std::size_t const chunk_size = 64;
std::size_t const num_chunks = ret.cols / chunk_size
+ (ret.cols % chunk_size != 0);
std::size_t const max_threads = std::max(1u,
std::thread::hardware_concurrency());
std::size_t const num_threads = std::min(num_chunks, max_threads);
#pragma omp parallel num_threads(num_threads)
{
/* 矩阵相乘 */
std::vector<T> ret_col(ret.rows, T(0));
std::vector<bool> ret_nonzero(ret.rows, false);
std::vector<T> thread_values;
thread_values.reserve(nnz / num_chunks);
std::vector<std::size_t> thread_inner;
thread_inner.reserve(nnz / num_chunks);
#pragma omp for ordered schedule(static, 1)
for (std::size_t chunk = 0; chunk < num_chunks; ++chunk)
{
thread_inner.clear();
thread_values.clear();
std::size_t const begin = chunk * chunk_size;
std::size_t const end = std::min(begin + chunk_size, ret.cols);
for (std::size_t col = begin; col < end; ++col)
{
std::fill(ret_col.begin(), ret_col.end(), T(0));
std::fill(ret_nonzero.begin(), ret_nonzero.end(), false);
std::size_t const rhs_col_begin = rhs.outer[col];
std::size_t const rhs_col_end = rhs.outer[col + 1];
for (std::size_t i = rhs_col_begin; i < rhs_col_end; ++i)
{
T const& rhs_col_value = rhs.values[i];
std::size_t const lhs_col = rhs.inner[i];
std::size_t const lhs_col_begin = this->outer[lhs_col];
std::size_t const lhs_col_end = this->outer[lhs_col + 1];
for (std::size_t j = lhs_col_begin; j < lhs_col_end; ++j)
{
std::size_t const id = this->inner[j];
ret_col[id] += this->values[j] * rhs_col_value;
ret_nonzero[id] = true;
}
}
for (std::size_t i = 0; i < ret.rows; ++i)
if (ret_nonzero[i])
{
ret.outer[col + 1] += 1;
thread_inner.push_back(i);
thread_values.push_back(ret_col[i]);
}
}
#pragma omp ordered
{
ret.inner.insert(ret.inner.end(),
thread_inner.begin(), thread_inner.end());
ret.values.insert(ret.values.end(),
thread_values.begin(), thread_values.end());
}
}
}
for (std::size_t col = 0; col < ret.cols; ++col)
ret.outer[col + 1] += ret.outer[col];
return ret;
}
template<typename T>
DenseVector<T> SparseMatrix<T>::multiply(DenseVector<T> const& rhs) const
{
if (rhs.size() != this->cols)
throw std::invalid_argument("Incompatible dimensions");
DenseVector<T> ret(this->rows, T(0));
for (std::size_t i = 0; i < this->cols; ++i)
for (std::size_t id = this->outer[i]; id < this->outer[i + 1]; ++id)
ret[this->inner[id]] += this->values[id] * rhs[i];
return ret;
}
template<typename T>
SparseMatrix<T> SparseMatrix<T>::diagonal_matrix(void) const
{
std::size_t const diag_size = std::min(this->rows, this->cols);
SparseMatrix ret(diag_size, diag_size);
ret.reserve(diag_size);
for (std::size_t i = 0; i < diag_size; ++i)
{
ret.outer[i] = ret.values.size();
for (std::size_t j = this->outer[i]; j < this->outer[i + 1]; ++j)
if (this->inner[j] == i)
{
ret.inner.push_back(i);
ret.values.push_back(this->values[j]);
}
else if (this->inner[j] > i)
break;
}
ret.outer[diag_size] = ret.values.size();
return ret;
}
template<typename T>
void SparseMatrix<T>::mult_diagonal(T const& factor)
{
for (std::size_t i = 0; i < this->outer.size() - 1; ++i)
for (std::size_t j = this->outer[i]; j < this->outer[i + 1]; ++j)
{
if (this->inner[j] == i)
this->values[j] *= factor;
if (this->inner[j] >= i)
break;
}
}
template<typename T>
void SparseMatrix<T>::cwise_invert(void)
{
for (std::size_t i = 0; i < this->values.size(); ++i)
this->values[i] = T(1) / this->values[i];
}
template<typename T>
void SparseMatrix<T>::column_nonzeros(std::size_t col, DenseVector<T>* vector) const
{
std::size_t const start = this->outer[col];
std::size_t const end = this->outer[col + 1];
vector->resize(end - start);
for (std::size_t row = start, i = 0; row < end; ++row, ++i)
vector->at(i) = this->values[row];
}
template<typename T>
inline std::size_t SparseMatrix<T>::num_non_zero(void) const
{
return this->values.size();
}
template<typename T>
inline std::size_t SparseMatrix<T>::num_rows(void) const
{
return this->rows;
}
template<typename T>
inline std::size_t SparseMatrix<T>::num_cols(void) const
{
return this->cols;
}
template<typename T>
inline T* SparseMatrix<T>::begin(void)
{
return this->values.data();
}
template<typename T>
inline T* SparseMatrix<T>::end(void)
{
return this->values.data() + this->values.size();
}
template<typename T>
void SparseMatrix<T>::debug(void) const
{
std::cout << "SparseMatrix ("
<< this->rows << " rows, " << this->cols << " cols, "
<< this->num_non_zero() << " values)" << std::endl;
std::cout << " Values:";
for (std::size_t i = 0; i < this->values.size(); ++i)
std::cout << " " << this->values[i];
std::cout << std::endl << " Inner:";
for (std::size_t i = 0; i < this->inner.size(); ++i)
std::cout << " " << this->inner[i];
std::cout << std::endl << " Outer:";
for (std::size_t i = 0; i < this->outer.size(); ++i)
std::cout << " " << this->outer[i];
std::cout << std::endl;
}
camera_pose.h
#pragma once
#include<opencv2/opencv.hpp>
using namespace cv;
Mat hstack(Mat const & K, Mat const & t)
{
int rows = K.rows;
int cols = K.cols + t.cols;
Mat P(rows, cols, CV_64FC1);
for (int i = 0; i < K.rows; i++)
for (int j = 0; j < K.cols; j++)
P.at<double>(i, j) = K.at<double>(i, j);
for (int i = 0; i < t.rows; i++)
for (int j = 0; j < t.cols; j++)
P.at<double>(i, K.cols + j) = t.at<double>(i, j);
return P;
}
struct CameraPose
{
public:
CameraPose(void);
/** Initializes R with identity and t with zeros. */
void init_canonical_form(void);
/** Returns the P matrix as the product K [R | t]. */
void fill_p_matrix(Mat* result) const;
/** Initializes the K matrix from focal length and principal point. */
void set_k_matrix(double flen, double px, double py);
/** Returns the focal length as average of x and y focal length. */
double get_focal_length(void) const;
/** Returns the camera position (requires valid camera). */
void fill_camera_pos(Mat* camera_pos) const;
//void fill_camera_pos(math::Vector<double, 3>* camera_pos) const;
/** Returns true if K matrix is valid (non-zero focal length). */
bool is_valid(void) const;
public:
Mat K;
Mat R;
Mat t;
/*math::Matrix<double, 3, 3> K;
math::Matrix<double, 3, 3> R;
math::Vector<double, 3> t;*/
};
/* ------------------------ Implementation ------------------------ */
inline
CameraPose::CameraPose(void)
{
K = Mat::zeros(3, 3, CV_64FC1);
R = Mat::zeros(3, 3, CV_64FC1);
t = Mat::zeros(3, 1, CV_64FC1);
}
inline void
CameraPose::init_canonical_form(void)
{
for (int i = 0; i < 3 * 3; i++)
R.at<double>(i) = 1;
t = Mat::zeros(3, 1, CV_64FC1);
}
inline void
CameraPose::fill_p_matrix(Mat* P) const
{
Mat KR = this->K * this->R;
Mat Kt = this->K*this->t;
*P = hstack(KR,Kt);
}
inline void
CameraPose::set_k_matrix(double flen, double px, double py)
{
K = Mat::zeros(3, 3, CV_64FC1);
this->K.at<double>(0) = flen; this->K.at<double>(2) = px;
this->K.at<double>(4) = flen; this->K.at<double>(5) = py;
this->K.at<double>(8) = 1.0;
}
inline double
CameraPose::get_focal_length(void) const
{
return (this->K.at<double>(0) + this->K.at<double>(4)) / 2.0;
}
inline void
CameraPose::fill_camera_pos(Mat* camera_pos) const
{
*camera_pos = -this->R.t().mul(this->t);
}
inline bool
CameraPose::is_valid(void) const
{
return this->K.at<double>(0) != 0.0;
}
ba_linear_solver.h
#pragma once
#include <vector>
#include "ba_sparse_matrix.h"
#include "ba_dense_vector.h"
class LinearSolver
{
public:
struct Options
{
Options(void);
double trust_region_radius;
int cg_max_iterations;
int camera_block_dim;
};
struct Status
{
Status(void);
double predicted_error_decrease;
int num_cg_iterations;
bool success;
};
// 稀疏矩阵设置为double类型
typedef SparseMatrix<double> SparseMatrixType;
// 向量设置为double类型
typedef DenseVector<double> DenseVectorType;
public:
LinearSolver(Options const& options);
// 解正规方程J^TJ delta_x = -J^T f
/**
* Solve the system J^T J x = -J^T f based on the bundle adjustment mode.
* If the Jacobian for cameras is empty, only points are optimized.
* If the Jacobian for points is empty, only cameras are optimized.
* If both, Jacobian for cams and points is given, the Schur complement
* trick is used to solve the linear system.
*/
Status solve(SparseMatrixType const& jac_cams,
SparseMatrixType const& jac_points,
DenseVectorType const& vector_f,
DenseVectorType* delta_x);
private:
// 舒尔补
/**
* Conjugate Gradient on Schur-complement by exploiting the block
* structure of H = J^T * J.
*/
Status solve_schur(SparseMatrixType const& jac_cams,
SparseMatrixType const& jac_points,
DenseVectorType const& values,
DenseVectorType* delta_x);
// block size = 0的时候
/**
* J is the Jacobian of the problem. If H = J^T * J has a block diagonal
* structure (e.g. 'motion only' or 'structure only' problems in BA),
* block_size can be used to directly invert H. If block_size is 0
* the diagonal of H is used as a preconditioner and the linear system
* is solved via conjugate gradient.
*/
Status solve(SparseMatrixType const& J,
DenseVectorType const& vector_f,
DenseVectorType* delta_x,
std::size_t block_size = 0);
private:
Options opts;
};
/* ------------------------ Implementation ------------------------ */
inline LinearSolver::Options::Options(void)
: trust_region_radius(1.0)
, cg_max_iterations(1000)
{
}
inline LinearSolver::Status::Status(void)
: predicted_error_decrease(0.0)
, num_cg_iterations(0)
, success(false)
{
}
inline LinearSolver::LinearSolver(Options const& options)
: opts(options)
{
}
ba_conjugate_gradient.h
#pragma once
#include "ba_dense_vector.h"
#include "ba_sparse_matrix.h"
template <typename T>
class ConjugateGradient
{
public:
typedef SparseMatrix<T> Matrix;
typedef DenseVector<T> Vector;
enum ReturnInfo
{
CG_CONVERGENCE,
CG_MAX_ITERATIONS,
CG_INVALID_INPUT
};
struct Options
{
Options(void);
int max_iterations;
T tolerance;
};
struct Status
{
Status(void);
int num_iterations;
ReturnInfo info;
};
class Functor
{
public:
virtual Vector multiply(Vector const& x) const = 0;
virtual std::size_t input_size(void) const = 0;
virtual std::size_t output_size(void) const = 0;
};
public:
ConjugateGradient(Options const& opts);
Status solve(Matrix const& A, Vector const& b, Vector* x,
Matrix const* P = nullptr);
Status solve(Functor const& A, Vector const& b, Vector* x,
Functor const* P = nullptr);
private:
Options opts;
Status status;
};
template <typename T>
class CGBasicMatrixFunctor : public ConjugateGradient<T>::Functor
{
public:
CGBasicMatrixFunctor(SparseMatrix<T> const& A);
DenseVector<T> multiply(DenseVector<T> const& x) const;
std::size_t input_size(void) const;
std::size_t output_size(void) const;
private:
SparseMatrix<T> const* A;
};
/* ------------------------ Implementation ------------------------ */
template <typename T>
inline
ConjugateGradient<T>::Options::Options(void)
: max_iterations(100)
, tolerance(1e-20)
{
}
template <typename T>
inline
ConjugateGradient<T>::Status::Status(void)
: num_iterations(0)
{
}
template <typename T>
inline
ConjugateGradient<T>::ConjugateGradient
(Options const& options)
: opts(options)
{
}
template <typename T>
inline typename ConjugateGradient<T>::Status
ConjugateGradient<T>::solve(Matrix const& A, Vector const& b, Vector* x,
Matrix const* P)
{
CGBasicMatrixFunctor<T> A_functor(A);
CGBasicMatrixFunctor<T> P_functor(*P);
return this->solve(A_functor, b, x, P == nullptr ? nullptr : &P_functor);
}
template <typename T>
typename ConjugateGradient<T>::Status
ConjugateGradient<T>::solve(Functor const& A, Vector const& b, Vector* x,
Functor const* P)
{
if (x == nullptr)
throw std::invalid_argument("RHS must not be null");
if (A.output_size() != b.size())
{
this->status.info = CG_INVALID_INPUT;
return this->status;
}
/* Set intial x = 0. */
if (x->size() != A.input_size())
{
x->clear();
x->resize(A.input_size(), T(0));
}
else
{
x->fill(T(0));
}
/* Initial residual is r = b - Ax with x = 0. */
Vector r = b;
/* Regular search direction. */
Vector d;
/* Preconditioned search direction. */
Vector z;
/* Norm of residual. */
T r_dot_r;
/* Compute initial search direction. */
if (P == nullptr)
{
r_dot_r = r.dot(r);
d = r;
}
else
{
z = (*P).multiply(r);
r_dot_r = z.dot(r);
d = z;
}
for (this->status.num_iterations = 0;
this->status.num_iterations < this->opts.max_iterations;
this->status.num_iterations += 1)
{
/* Compute step size in search direction. */
Vector Ad = A.multiply(d);
T alpha = r_dot_r / d.dot(Ad);
/* Update parameter vector. */
*x = (*x).add(d.multiply(alpha));
/* Compute new residual and its norm. */
r = r.subtract(Ad.multiply(alpha));
T new_r_dot_r = r.dot(r);
/* Check tolerance condition. */
if (new_r_dot_r < this->opts.tolerance)
{
this->status.info = CG_CONVERGENCE;
return this->status;
}
/* Precondition residual if necessary. */
if (P != nullptr)
{
z = P->multiply(r);
new_r_dot_r = z.dot(r);
}
/*
* Update search direction.
* The next residual will be orthogonal to new Krylov space.
*/
T beta = new_r_dot_r / r_dot_r;
if (P != nullptr)
d = z.add(d.multiply(beta));
else
d = r.add(d.multiply(beta));
/* Update residual norm. */
r_dot_r = new_r_dot_r;
}
this->status.info = CG_MAX_ITERATIONS;
return this->status;
}
/* ---------------------------------------------------------------- */
template <typename T>
inline
CGBasicMatrixFunctor<T>::CGBasicMatrixFunctor(SparseMatrix<T> const& A)
: A(&A)
{
}
template <typename T>
inline DenseVector<T>
CGBasicMatrixFunctor<T>::multiply(DenseVector<T> const& x) const
{
return this->A->multiply(x);
}
template <typename T>
inline std::size_t
CGBasicMatrixFunctor<T>::input_size(void) const
{
return A->num_cols();
}
template <typename T>
inline std::size_t
CGBasicMatrixFunctor<T>::output_size(void) const
{
return A->num_rows();
}
lm.cpp（源文件）
/*
* 实现Levenberg-Marquardt算法，该算法又称为 damped least squares 阻尼最小二乘，用来求解非线性最小二乘
* 问题。LM找到的是局部最小值，该算法介于高斯牛顿法和梯度下降法之间，并通过控制信赖域尺寸的大小，在高斯牛顿法
* 和梯度下降法之间进行调整。LM 算法比高斯牛顿法速度慢但是更为鲁棒，
*
* LM算法的原理是用模型函数f对待估向量p在邻域内做线性估计(泰勒展开），忽略掉二阶以上的导数项，从而转化为线性最小
* 二乘问题。本质上就是用二次曲面对目标函数进行局部近似。LM算法属于一种信赖域法，即：从初始值开始，先假设一个可以
* 信赖的最大的位移s, 然后以当前点为中心，以s为半径的区域内，通过寻找目标函数的一个近似二次函数的最优点来求得真正
* 的位移。在得到位移之后，再计算目标函数值，如果其使得目标函数值得下降满足了一定条件，那么说明这个位移是可靠的
* 则继续按照此规则迭代计算下去；如果其不能使目标函数的下降满足一定的条件，则应该减少信赖域的范围，重新求解。
*
* LM算法的一般流程是：
* 1） 初始化
* 2） 计算雅阁比矩阵J，构造正规方程(JTJ + lambdaI) = JTf
* 3） 求解正规方程（共轭梯度或者预定共轭梯度法）
* 4） 判断若求解成功
* 增加信赖域(1/lambda)，使得求解算法接近于高斯牛顿法,加快收敛速度
* 判断终止条件
* 判断若求解失败
* 减少信赖域(1/lambda), 使得求解算法解决域梯度下降法
* 5) 重复1), 2), 3)，4)
*
* （注意，信赖域的大小为正规方程中lambda的倒数)
*/
#include <iostream>
#include<fstream>
#include<sstream>
#include<cassert>
#include"ba_dense_vector.h"
#include"ba_sparse_matrix.h"
#include"ba_linear_solver.h"
#include"ba_conjugate_gradient.h"
#include"camera_pose.h"
struct Camera
{
Camera(void);
double focal_length = 0.0;
double distortion[2];
double translation[3];
double rotation[9];
bool is_constant = false;
};
struct Point3D
{
double pos[3];
bool is_constant = false;
};
struct Observation
{
double pos[2];
int camera_id;
int point_id;
};
inline Camera::Camera(void)
{
std::fill(this->distortion, this->distortion + 2, 0.0);
std::fill(this->translation, this->translation + 3, 0.0);
std::fill(this->rotation, this->rotation + 9, 0.0);
}
std::vector<Camera> cameras;
std::vector<Point3D> points;
std::vector<Observation> observations;
typedef SparseMatrix<double> SparseMatrixType;
typedef DenseVector<double> DenseVectorType;
// lm 算法最多迭代次数
const int lm_max_iterations = 100;
// 信赖域大小
double trust_region_radius = 1000;
int cg_max_iterations = 1000;
//相机参数个数
const int num_cam_params = 9;
#define TRUST_REGION_RADIUS_INIT (1000)
#define TRUST_REGION_RADIUS_DECREMENT (1.0 / 10.0)
#define TRUST_REGION_RADIUS_GAIN (10.0)
// lm 算法终止条件
double lm_mse_threshold = 1e-16;
double lm_delta_threshold = 1e-8;
// 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;
//相机参数个数
int camera_block_dim = 9;
void load_data(const std::string& file_name, std::vector<Camera>&cams, std::vector<Point3D>&pts3D, std::vector<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];
}
}
}
/**
* /descrition 将角轴法转化成旋转矩阵
* @param r 角轴向量
* @param m 旋转矩阵
*/
void rodrigues_to_matrix(double const* r, double* m)
{
/* Obtain angle from vector length. */
double a = std::sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]);
/* Precompute sine and cosine terms. */
double ct = (a == 0.0) ? 0.5f : (1.0f - std::cos(a)) / (2.0 * a);
double st = (a == 0.0) ? 1.0 : std::sin(a) / a;
/* R = I + st * K + ct * K^2 (with cross product matrix K). */
m[0] = 1.0 - (r[1] * r[1] + r[2] * r[2]) * ct;
m[1] = r[0] * r[1] * ct - r[2] * st;
m[2] = r[2] * r[0] * ct + r[1] * st;
m[3] = r[0] * r[1] * ct + r[2] * st;
m[4] = 1.0f - (r[2] * r[2] + r[0] * r[0]) * ct;
m[5] = r[1] * r[2] * ct - r[0] * st;
m[6] = r[2] * r[0] * ct - r[1] * st;
m[7] = r[1] * r[2] * ct + r[0] * st;
m[8] = 1.0 - (r[0] * r[0] + r[1] * r[1]) * ct;
}
template <typename T>
void matrix_multiply(T const* mat_a, int rows_a, int cols_a,
T const* mat_b, int cols_b, T* mat_res)
{
std::fill(mat_res, mat_res + rows_a * cols_b, T(0));
for (int j = 0; j < cols_b; ++j)
{
for (int i = 0; i < rows_a; ++i)
{
int const ica = i * cols_a;
int const icb = i * cols_b;
for (int k = 0; k < cols_a; ++k)
mat_res[icb + j] += mat_a[ica + k] * mat_b[k * cols_b + j];
}
}
}
/**
* \description 根据求解得到的增量，对相机参数进行更新
* @param cam
* @param update
* @param out
*/
void update_camera(Camera const& cam, double const* update, Camera* out)
{
out->focal_length = cam.focal_length + update[0];
out->distortion[0] = cam.distortion[0] + update[1];
out->distortion[1] = cam.distortion[1] + update[2];
out->translation[0] = cam.translation[0] + update[3];
out->translation[1] = cam.translation[1] + update[4];
out->translation[2] = cam.translation[2] + update[5];
double rot_orig[9];
std::copy(cam.rotation, cam.rotation + 9, rot_orig);
double rot_update[9];
rodrigues_to_matrix(update + 6, rot_update);
matrix_multiply(rot_update, 3, 3, rot_orig, 3, out->rotation);
}
/**
* \description 根据求解的增量，对三维点坐标进行更新
* @param pt
* @param update
* @param out
*/
void update_point(Point3D const& pt, double const* update, Point3D* out)
{
out->pos[0] = pt.pos[0] + update[0];
out->pos[1] = pt.pos[1] + update[1];
out->pos[2] = pt.pos[2] + update[2];
}
/**
* \description 对像素进行径向畸变
* @param x
* @param y
* @param dist
*/
void radial_distort(double* x, double* y, double const* dist)
{
double const radius2 = *x * *x + *y * *y;
double const factor = 1.0 + radius2 * (dist[0] + dist[1] * radius2);
*x *= factor;
*y *= factor;
}
void compute_reprojection_errors(DenseVectorType* vector_f, DenseVectorType const* delta_x, std::vector<Camera>* cameras,
std::vector<Point3D> *points, std::vector<Observation> *observations)
{
if (vector_f->size() != observations->size() * 2)
vector_f->resize(observations->size() * 2);
#pragma omp parallel for
for (std::size_t i = 0; i < observations->size(); ++i)
{
Observation const& obs = observations->at(i);
Point3D const& p3d = points->at(obs.point_id);
Camera const& cam = cameras->at(obs.camera_id);
double const* flen = &cam.focal_length; // 相机焦距
double const* dist = cam.distortion; // 径向畸变系数
double const* rot = cam.rotation; // 相机旋转矩阵
double const* trans = cam.translation; // 相机平移向量
double const* point = p3d.pos; // 三维点坐标
Point3D new_point;
Camera new_camera;
// 如果delta_x 不为空，则先利用delta_x对相机和结构进行更新，然后再计算重投影误差
if (delta_x != nullptr)
{
std::size_t cam_id = obs.camera_id * num_cam_params;
std::size_t pt_id = obs.point_id * 3;
update_camera(cam, delta_x->data() + cam_id, &new_camera);
flen = &new_camera.focal_length;
dist = new_camera.distortion;
rot = new_camera.rotation;
trans = new_camera.translation;
pt_id += cameras->size() * num_cam_params;
update_point(p3d, delta_x->data() + pt_id, &new_point);
point = new_point.pos;
}
/* Project point onto image plane. */
double rp[] = { 0.0, 0.0, 0.0 };
for (int d = 0; d < 3; ++d)
{
rp[0] += rot[0 + d] * point[d];
rp[1] += rot[3 + d] * point[d];
rp[2] += rot[6 + d] * point[d];
}
rp[2] = (rp[2] + trans[2]);
rp[0] = (rp[0] + trans[0]) / rp[2];
rp[1] = (rp[1] + trans[1]) / rp[2];
/* Distort reprojections. */
radial_distort(rp + 0, rp + 1, dist);
/* Compute reprojection error. */
vector_f->at(i * 2 + 0) = rp[0] * (*flen) - obs.pos[0];
vector_f->at(i * 2 + 1) = rp[1] * (*flen) - obs.pos[1];
//std::cout << rp[0] * (*flen) - obs.pos[0] << " " << rp[1] * (*flen) - obs.pos[1] << std::endl;
}
}
/*
* 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(SparseMatrix<double> const& A,
std::size_t block_size, SparseMatrix<double>* B)
{
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<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);
}
/** 浮点和双限制以及 epsilon 值 */
#ifndef __FLT_MIN__
# define __FLT_MIN__ 1.17549435e-38f
#endif
#ifndef __DBL_MIN__
# define __DBL_MIN__ 2.2250738585072014e-308
#endif
#define MATH_FLT_MIN __FLT_MIN__
#define MATH_DBL_MIN __DBL_MIN__
#define MATH_FLT_EPS (MATH_FLT_MIN * 1e8f) // approx. 1.17e-30
#define MATH_DBL_EPS (MATH_DBL_MIN * 1e58) // approx. 2.22e-250
#define MATH_EPSILON_EQ(x,v,eps) (((v - eps) <= x) && (x <= (v + eps)))
#define MATH_DOUBLE_EQ(x,v) MATH_EPSILON_EQ(x,v,MATH_DBL_EPS)
/*
* 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(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;
Mat rot(3, 3, CV_64FC1);
for (int i = 0; i < 9; ++i)
rot.at<double>(i) = *(iter++);
double det = determinant(rot);
if (MATH_DOUBLE_EQ(det, 0.0))
continue;
rot = rot.inv();
iter = iter_backup;
for (int i = 0; i < 9; ++i)
*(iter++) = rot.at<double>(i);
}
}
#define MATH_POW2(x) ((x) * (x))
void cholesky_decomposition(Mat const* A, int cols, Mat* L)
{
/*Mat LL(9, 9, CV_64FC1);
Mat* out_ptr = &LL;*/
Mat LL = *A;
int k = 0;
for (int r = 0; r < cols; ++r)
{
/* Compute left-of-diagonal entries. */
for (int c = 0; c < r; ++c)
{
double result = 0.0;
for (int ci = 0; ci < c; ++ci)
result += LL.at<double>(r * cols + ci) * LL.at<double>(c * cols + ci);
result = A->at<double>(r * cols + c) - result;
LL.at<double>(k++)= result / LL.at<double>(c * cols + c);
//(*LL++) = result / L[c * cols + c];
}
/* Compute diagonal entry. */
{
Mat* L_row_ptr = &LL;
//Mat* L_row_ptr = &LL + r * cols;
double result = 0.0;
for (int c = 0; c < r; ++c)
result += MATH_POW2(L_row_ptr->at<double>(c + r * cols));
result = std::max(0.0, A->at<double>(r * cols + r) - result);
LL.at<double>(k++) = std::sqrt(result);
}
/* Set right-of-diagonal entries zero. */
for (int c = r + 1; c < cols; ++c)
LL.at<double>(k++) = 0;
}
*L = LL;
}
void invert_lower_diagonal(Mat const* A, int const cols, Mat* A_inv)
{
if (A == A_inv)
throw std::invalid_argument("In-place inversion not possible");
for (int r = 0; r < cols; ++r)
{
//Mat const* A_row_ptr = A + r * cols;
Mat const* A_row_ptr = A ;
//Mat* A_inv_row_ptr = A_inv + r * cols;
Mat* A_inv_row_ptr = A_inv ;
/* Compute left-of-diagonal entries. */
for (int c = 0; c < r; ++c)
{
double result = 0.0;
for (int ci = 0; ci < r; ++ci)
result -= A_row_ptr->at<double>(ci + r * cols) * A_inv->at<double>(ci * cols + c);
A_inv_row_ptr->at<double>(c + r * cols) = result / A_row_ptr->at<double>(r + r * cols);
}
/* Compute diagonal entry. */
A_inv_row_ptr->at<double>(r + r * cols) = 1 / A_row_ptr->at<double>(r + r * cols);
/* Set right-of-diagonal entries zero. */
for (int c = r + 1; c < cols; ++c)
A_inv_row_ptr->at<double>(c + r * cols) = 0;
}
}
/**
*用Cholesky变换对称正定矩阵。
*/
void cholesky_invert_inplace(Mat* A, int const cols)
{
cholesky_decomposition(A, cols, A);
Mat* tmp = new Mat(cols, cols, CV_64FC1);
invert_lower_diagonal(A, cols, tmp);
*A = (*tmp).t()* (*tmp);
//math::matrix_transpose_multiply(tmp, cols, cols, A);
delete tmp;
}
/*
* 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(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;
Mat matrix_block(blocksize, blocksize, CV_64FC1);
//std::vector<double> matrix_block(bs2);
for (double* iter = A->begin(); iter != A->end(); )
{
double* iter_backup = iter;
for (int i = 0; i < bs2; ++i)
matrix_block.at<double>(i) = *(iter++);
cholesky_invert_inplace(&matrix_block, blocksize);
iter = iter_backup;
for (int i = 0; i < bs2; ++i)
if (std::isfinite(matrix_block.at<double>(i)))
*(iter++) = matrix_block.at<double>(i);
else
*(iter++) = 0.0;
}
}
LinearSolver::Status my_solve_schur(
SparseMatrixType const& jac_cams,
SparseMatrixType const& jac_points,
DenseVectorType const& values,
DenseVectorType* delta_x) {
/*
* 雅阁比矩阵：
* J = [Jc Jp]
* Jc是与相机相关的模块，Jp是与三维点相关的模块。
* 正规方程
* (J^TJ + lambda*I)delta_x = J^T(x - F)
* 进一步写为
* [ Jcc+ lambda*Icc Jcp ][delta_c]= [v]
* [ Jxp Jpp+lambda*Ipp ][delta_p] [w]
*
* B = Jcc, E = Jcp, C = Jpp
* 其中 Jcc = Jc^T* Jc, Jcx = Jc^T*Jx, Jxc = Jx^TJc, Jxx = Jx^T*Jx
* v = Jc^T(F-x), w = Jx^T(F-x), deta_x = [delta_c; delta_p]
*/
// 误差向量
DenseVectorType const& F = values;
// 关于相机的雅阁比矩阵
SparseMatrixType const& Jc = jac_cams;
// 关于三维点的雅阁比矩阵
SparseMatrixType const& Jp = jac_points;
SparseMatrixType JcT = Jc.transpose();
SparseMatrixType JpT = Jp.transpose();
// 构造正规方程
SparseMatrixType B, C;
// B = Jc^T* Jc
matrix_block_column_multiply(Jc, camera_block_dim, &B);
// C = Jp^T*Jp
matrix_block_column_multiply(Jp, 3, &C);
// E = Jc^T*Jp
SparseMatrixType E = JcT.multiply(Jp);
/* Assemble two values vectors. */
DenseVectorType v = JcT.multiply(F);
DenseVectorType w = JpT.multiply(F);
v.negate_self();
w.negate_self();
/* 以矩阵B和C的对角元素重新构建对角阵*/
// SparseMatrixType B_diag = B.diagonal_matrix();
// SparseMatrixType C_diag = C.diagonal_matrix();
/* 添加信赖域 */
C.mult_diagonal(1.0 + 1.0 / trust_region_radius);
B.mult_diagonal(1.0 + 1.0 / trust_region_radius);
/* 求解C矩阵的逆C = inv(Jx^T+Jx + lambda*Ixx)*/
invert_block_matrix_3x3_inplace(&C);
/* 计算S矩阵的Schur补用于高斯消元. */
SparseMatrixType ET = E.transpose();
// S = (Jcc+lambda*Icc) - Jc^T*Jx*inv(Jxx+ lambda*Ixx)*Jx^T*Jc
SparseMatrixType S = B.subtract(E.multiply(C).multiply(ET));
// rhs = v - Jc^T*Jx*inv(Jxx+ lambda*Ixx)*w
DenseVectorType rhs = v.subtract(E.multiply(C.multiply(w)));
/* Compute pre-conditioner for linear system. */
//SparseMatrixType precond = S.diagonal_matrix();
//precond.cwise_invert();
SparseMatrixType precond = B;
invert_block_matrix_NxN_inplace(&precond, camera_block_dim);
/* 用共轭梯度法求解相机参数. */
DenseVectorType delta_y(Jc.num_cols());
typedef ConjugateGradient<double> CGSolver;
CGSolver::Options cg_opts;
cg_opts.max_iterations = cg_max_iterations;
cg_opts.tolerance = 1e-20;
CGSolver solver(cg_opts);
CGSolver::Status cg_status;
cg_status = solver.solve(S, rhs, &delta_y, &precond);
LinearSolver::Status status;
status.num_cg_iterations = cg_status.num_iterations;
switch (cg_status.info) {
case CGSolver::CG_CONVERGENCE:
status.success = true;
break;
case CGSolver::CG_MAX_ITERATIONS:
status.success = true;
break;
case CGSolver::CG_INVALID_INPUT:
std::cout << "BA: CG failed (invalid input)" << std::endl;
status.success = false;
return status;
default:
break;
}
/* 将相机参数带入到第二个方程中，求解三维点的参数. */
/*E= inv(Jp^T Jp) (JpT.multiply(F)-Jc^T * Jp * delta_y)*/
DenseVectorType delta_z = C.multiply(w.subtract(ET.multiply(delta_y)));
/* Fill output vector. */
std::size_t const jac_cam_cols = Jc.num_cols();
std::size_t const jac_point_cols = Jp.num_cols();
std::size_t const jac_cols = jac_cam_cols + jac_point_cols;
if (delta_x->size() != jac_cols)
delta_x->resize(jac_cols, 0.0);
for (std::size_t i = 0; i < jac_cam_cols; ++i)
delta_x->at(i) = delta_y[i];
for (std::size_t i = 0; i < jac_point_cols; ++i)
delta_x->at(jac_cam_cols + i) = delta_z[i];
return status;
}
/**
* /description 计算观察点坐标(x,y)，相遇对相机参数和三维点坐标的雅阁比矩阵
* @param cam
* @param point
* @param cam_x_ptr
* @param cam_y_ptr
* @param point_x_ptr
* @param point_y_ptr
*/
void my_jacobian(Camera const& cam,Point3D const& point,double* cam_x_ptr, double* cam_y_ptr,
double* point_x_ptr, double* point_y_ptr)
{
const double f = cam.focal_length;
const double *R = cam.rotation;
const double *t = cam.translation;
const double *X = point.pos;
const double k0 = cam.distortion[0];
const double k1 = cam.distortion[1];
const double xc = R[0] * X[0] + R[1] * X[1] + R[2] * X[2] + t[0];
const double yc = R[3] * X[0] + R[4] * X[1] + R[5] * X[2] + t[1];
const double zc = R[6] * X[0] + R[7] * X[1] + R[8] * X[2] + t[2];
const double x = xc / zc;
const double y = yc / zc;
const double r2 = x * x + y * y;
const double distort = 1.0 + (k0 + k1 * r2)*r2;
const double u = f * distort*x;
const double v = f * distort*y;
/*关于焦距的偏导数*/
cam_x_ptr[0] = distort * x;
cam_y_ptr[0] = distort * y;
/*计算关于径向畸变函数k0, k1的偏导数*/
// 计算中间变量
const double u_deriv_distort = f * x;
const double v_deriv_distort = f * y;
const double distort_deriv_k0 = r2;
const double distort_deriv_k1 = r2 * r2;
cam_x_ptr[1] = u_deriv_distort * distort_deriv_k0;
cam_x_ptr[2] = u_deriv_distort * distort_deriv_k1;
cam_y_ptr[1] = v_deriv_distort * distort_deriv_k0;
cam_y_ptr[2] = v_deriv_distort * distort_deriv_k1;
// 计算中间变量 (x,y)关于(xc, yc, zc)的偏导数
const double x_deriv_xc = 1 / zc; const double x_deriv_yc = 0; const double x_deriv_zc = -x / zc;
const double y_deriv_xc = 0; const double y_deriv_yc = 1 / zc; const double y_deriv_zc = -y / zc;
// 计算u, v关于x, y的偏导数
const double u_deriv_x = f * distort;
const double v_deriv_y = f * distort;
// 计算中间变量distort关于r2的偏导数
const double distort_deriv_r2 = k0 + 2 * k1*r2;
// 计算中间变量r2关于xc, yc, zc的偏导数
const double r2_deriv_xc = 2 * x / zc;
const double r2_deriv_yc = 2 * y / zc;
const double r2_deriv_zc = -2 * r2 / zc;
// 计算中间变量distort关于xc, yc, zc的偏导数
const double distort_deriv_xc = distort_deriv_r2 * r2_deriv_xc;
const double distort_deriv_yc = distort_deriv_r2 * r2_deriv_yc;
const double distort_deriv_zc = distort_deriv_r2 * r2_deriv_zc;
// 计算(u,v)关于xc, yc, zc的偏导数
const double u_deriv_xc = u_deriv_distort * distort_deriv_xc + u_deriv_x * x_deriv_xc;
const double u_deriv_yc = u_deriv_distort * distort_deriv_yc + u_deriv_x * x_deriv_yc;
const double u_deriv_zc = u_deriv_distort * distort_deriv_zc + u_deriv_x * x_deriv_zc;
const double v_deriv_xc = v_deriv_distort * distort_deriv_xc + v_deriv_y * y_deriv_xc;
const double v_deriv_yc = v_deriv_distort * distort_deriv_yc + v_deriv_y * y_deriv_yc;
const double v_deriv_zc = v_deriv_distort * distort_deriv_zc + v_deriv_y * y_deriv_zc;
/* 计算关于平移向量的t0, t1, t2的偏导数*/
const double xc_deriv_t0 = 1;
const double yc_deriv_t1 = 1;
const double zc_deriv_t2 = 1;
cam_x_ptr[3] = u_deriv_xc * xc_deriv_t0;
cam_x_ptr[4] = u_deriv_yc * yc_deriv_t1;
cam_x_ptr[5] = u_deriv_zc * zc_deriv_t2;
cam_y_ptr[3] = v_deriv_xc * xc_deriv_t0;
cam_y_ptr[4] = v_deriv_yc * yc_deriv_t1;
cam_y_ptr[5] = v_deriv_zc * zc_deriv_t2;
/* 计算关于旋转矩阵(表示为角轴向量w0, w1, w2)的偏导数 */
const double rx = R[0] * X[0] + R[1] * X[1] + R[2] * X[2];
const double ry = R[3] * X[0] + R[4] * X[1] + R[5] * X[2];
const double rz = R[6] * X[0] + R[7] * X[1] + R[8] * X[2];
const double xc_deriv_w0 = 0; const double xc_deriv_w1 = rz; const double xc_deriv_w2 = -ry;
const double yc_deriv_w0 = -rz; const double yc_deriv_w1 = 0; const double yc_deriv_w2 = rx;
const double zc_deriv_w0 = ry; const double zc_deriv_w1 = -rx; const double zc_deriv_w2 = 0;
cam_x_ptr[6] = u_deriv_yc * yc_deriv_w0 + u_deriv_zc * zc_deriv_w0;
cam_x_ptr[7] = u_deriv_xc * xc_deriv_w1 + u_deriv_zc * zc_deriv_w1;
cam_x_ptr[8] = u_deriv_xc * xc_deriv_w2 + u_deriv_yc * yc_deriv_w2;
cam_y_ptr[6] = v_deriv_yc * yc_deriv_w0 + v_deriv_zc * zc_deriv_w0;
cam_y_ptr[7] = v_deriv_xc * xc_deriv_w1 + v_deriv_zc * zc_deriv_w1;
cam_y_ptr[8] = v_deriv_xc * xc_deriv_w2 + v_deriv_yc * yc_deriv_w2;
/* 计算关于三维点坐标X,Y,X的偏导数*/
const double xc_deriv_X = R[0]; const double xc_deriv_Y = R[1]; const double xc_deriv_Z = R[2];
const double yc_deriv_X = R[3]; const double yc_deriv_Y = R[4]; const double yc_deriv_Z = R[5];
const double zc_deriv_X = R[6]; const double zc_deriv_Y = R[7]; const double zc_deriv_Z = R[8];
point_x_ptr[0] = u_deriv_xc * xc_deriv_X + u_deriv_yc * yc_deriv_X + u_deriv_zc * zc_deriv_X;
point_x_ptr[1] = u_deriv_xc * xc_deriv_Y + u_deriv_yc * yc_deriv_Y + u_deriv_zc * zc_deriv_Y;
point_x_ptr[2] = u_deriv_xc * xc_deriv_Z + u_deriv_yc * yc_deriv_Z + u_deriv_zc * zc_deriv_Z;
point_y_ptr[0] = v_deriv_xc * xc_deriv_X + v_deriv_yc * yc_deriv_X + v_deriv_zc * zc_deriv_X;
point_y_ptr[1] = v_deriv_xc * xc_deriv_Y + v_deriv_yc * yc_deriv_Y + v_deriv_zc * zc_deriv_Y;
point_y_ptr[2] = v_deriv_xc * xc_deriv_Z + v_deriv_yc * yc_deriv_Z + v_deriv_zc * zc_deriv_Z;
}
/**
* \description 计算均方误差
* @param vector_f
* @return
*/
double compute_mse(DenseVectorType const& vector_f) {
double mse = 0.0;
for (std::size_t i = 0; i < vector_f.size(); ++i)
mse += vector_f[i] * vector_f[i];
return mse / static_cast<double>(vector_f.size() / 2);
}
/**
* /descripition 根据求得的delta_x, 更新相机参数和三维点
* @param delta_x
* @param cameras
* @param points
*/
void
update_parameters(DenseVectorType const& delta_x
, std::vector<Camera>*cameras
, std::vector<Point3D>*points)
{
/* Update cameras. */
std::size_t total_camera_params = 0;
for (std::size_t i = 0; i < cameras->size(); ++i) {
update_camera(cameras->at(i),
delta_x.data() + num_cam_params * i,
&cameras->at(i));
total_camera_params = cameras->size() * num_cam_params;
}
/* Update points. */
for (std::size_t i = 0; i < points->size(); ++i) {
update_point(points->at(i),
delta_x.data() + total_camera_params + i * 3,
&points->at(i));
}
}
/**
* \description 构造雅阁比矩阵，采用稀疏矩阵形式，
* 关于相机参数的雅阁比矩阵大小为：(2*observations.size()) x (num_cameras*9)
* 关于三维点坐标的雅阁比矩阵大小为：(2*observation.size()) x (num_points*3)
* @param jac_cam-- 观察点相对于相机参数的雅阁比矩阵
* @param jac_points--观察点相对于三维点的雅阁比矩阵
*/
void analytic_jacobian(SparseMatrixType* jac_cam, SparseMatrixType* jac_points)
{
assert(jac_cam);
assert(jac_points);
// 相机和三维点jacobian矩阵的行数都是n_observations*2
// 相机jacobian矩阵jac_cam的列数是n_cameras* n_cam_params
// 三维点jacobian矩阵jac_points的列数是n_points*3
std::size_t const camera_cols = cameras.size() * num_cam_params;
std::size_t const point_cols = points.size() * 3;
std::size_t const jacobi_rows = observations.size() * 2;
// 定义稀疏矩阵的基本元素
SparseMatrixType::Triplets cam_triplets, point_triplets;
cam_triplets.reserve(observations.size() * 2 * num_cam_params);
point_triplets.reserve(observations.size() * 2 * 3);
double cam_x_ptr[9], cam_y_ptr[9], point_x_ptr[3], point_y_ptr[3];
// 对于每一个观察到的二维点
for (std::size_t i = 0; i < observations.size(); ++i) {
// 获取二维点，obs.point_id 三维点的索引，obs.camera_id 相机的索引
Observation const &obs = observations[i];
// 三维点坐标
Point3D const &p3d = points[obs.point_id];
// 相机参数
Camera const &cam = cameras[obs.camera_id];
/*对一个三维点和相机求解偏导数*/
my_jacobian(cam, p3d, cam_x_ptr, cam_y_ptr, point_x_ptr, point_y_ptr);
/*观察点对应雅各比矩阵的行，第i个观察点在雅各比矩阵的位置是2*i, 2*i+1*/
std::size_t row_x = i * 2 + 0;
std::size_t row_y = i * 2 + 1;
/*jac_cam中相机对应的列数为camera_id* n_cam_params*/
std::size_t cam_col = obs.camera_id * num_cam_params;
/*jac_points中三维点对应的列数为point_id* 3*/
std::size_t point_col = obs.point_id * 3;
for (int j = 0; j < num_cam_params; ++j) {
cam_triplets.push_back(SparseMatrixType::Triplet(row_x, cam_col + j, cam_x_ptr[j]));
cam_triplets.push_back(SparseMatrixType::Triplet(row_y, cam_col + j, cam_y_ptr[j]));
}
for (int j = 0; j < 3; ++j) {
point_triplets.push_back(SparseMatrixType::Triplet(row_x, point_col + j, point_x_ptr[j]));
point_triplets.push_back(SparseMatrixType::Triplet(row_y, point_col + j, point_y_ptr[j]));
}
}
if (jac_cam != nullptr) {
jac_cam->allocate(jacobi_rows, camera_cols);
jac_cam->set_from_triplets(cam_triplets);
}
if (jac_points != nullptr) {
jac_points->allocate(jacobi_rows, point_cols);
jac_points->set_from_triplets(point_triplets);
}
}
void lm_optimization(std::vector<Camera>* cameras, std::vector<Point3D>* points, std::vector<Observation>* observations)
{
/* 1.0 初始化 */
DenseVectorType F, F_new;
// 计算重投影误差
compute_reprojection_errors(&F, nullptr, cameras, points, observations);// todo F 是误差向量
// 计算初始的均方误差
double current_mse = compute_mse(F);
initial_mse = current_mse;
final_mse = current_mse;
std::cout << current_mse;
// 设置共轭梯度法的相关参数
trust_region_radius = TRUST_REGION_RADIUS_INIT;
/* Levenberg-Marquard 算法. */
for (int lm_iter = 0; ; ++lm_iter)
{
// 判断终止条件，均方误差小于一定阈值
if (current_mse < lm_mse_threshold) {
std::cout << "BA: Satisfied MSE threshold." << std::endl;
break;
}
//1.0 计算雅阁比矩阵
SparseMatrixType Jc, Jp;
analytic_jacobian(&Jc, &Jp);
//Jc.debug();
//Jp.debug();
//2.0 预置共轭梯梯度法对正规方程进行求解*/
DenseVectorType delta_x;
LinearSolver::Status cg_status = my_solve_schur(Jc, Jp, F, &delta_x);
//3.0 根据计算得到的偏移量，重新计算冲投影误差和均方误差，用于判断终止条件和更新条件.
double new_mse, delta_mse, delta_mse_ratio = 1.0;
// 正规方程求解成功的情况下
if (cg_status.success) {
/*重新计算相机和三维点，计算重投影误差，注意原始的相机参数没有被更新*/
compute_reprojection_errors(&F_new, &delta_x, cameras, points, observations);
/* 计算新的残差值 */
new_mse = compute_mse(F_new);
/* 均方误差的绝对变化值和相对变化率*/
delta_mse = current_mse - new_mse;
delta_mse_ratio = 1.0 - new_mse / current_mse;
}
// 正规方程求解失败的情况下
else {
new_mse = current_mse;
delta_mse = 0.0;
}
// new_mse < current_mse表示残差值减少
bool successful_iteration = delta_mse > 0.0;
/*
* 如果正规方程求解成功，则更新相机参数和三维点坐标，并且增大信赖域的尺寸，使得求解方式
* 趋近于高斯牛顿法
*/
if (successful_iteration) {
std::cout << "BA: #" << std::setw(2) << std::left << lm_iter
<< " success" << std::right
<< ", MSE " << std::setw(11) << current_mse
<< " -> " << std::setw(11) << new_mse
<< ", CG " << std::setw(3) << cg_status.num_cg_iterations
<< ", TRR " << trust_region_radius
<< ", MSE Ratio: " << delta_mse_ratio
<< std::endl;
num_lm_iterations += 1;
num_lm_successful_iterations += 1;
/* 对相机参数和三点坐标进行更新 */
update_parameters(delta_x, cameras, points);
std::swap(F, F_new);
current_mse = new_mse;
if (delta_mse_ratio < lm_delta_threshold) {
std::cout << "BA: Satisfied delta mse ratio threshold of "
<< lm_delta_threshold << std::endl;
break;
}
// 增大信赖域大小
trust_region_radius *= TRUST_REGION_RADIUS_GAIN;
}
else {
std::cout << "BA: #" << std::setw(2) << std::left << lm_iter
<< " failure" << std::right
<< ", MSE " << std::setw(11) << current_mse
<< ", " << std::setw(11) << " "
<< " CG " << std::setw(3) << cg_status.num_cg_iterations
<< ", TRR " << trust_region_radius
<< std::endl;
num_lm_iterations += 1;
num_lm_unsuccessful_iterations += 1;
// 求解失败的减小信赖域尺寸
trust_region_radius *= TRUST_REGION_RADIUS_DECREMENT;
}
/* 判断是否超过最大的迭代次数. */
if (lm_iter + 1 >= lm_max_iterations) {
std::cout << "BA: Reached maximum LM iterations of "
<< lm_max_iterations << std::endl;
break;
}
}
final_mse = current_mse;
}
int main()
{
/* 加载数据 */
load_data("../data/test_ba.txt", cameras, points, observations);
lm_optimization(&cameras, &points, &observations);
// 将优化后的结果重新赋值
std::vector<CameraPose> new_cam_poses(2);
std::vector<Vec2f> radial_distortion(2);
std::vector<Vec3f> new_pts_3d(points.size());
for (int i = 0; i < cameras.size(); i++) {
for (int j = 0; j < 3; j++)
{
new_cam_poses[i].t.at<double>(j) = cameras[i].translation[j];
}
for (int j = 0; j < 9; j++)
{
new_cam_poses[i].R.at<double>(j) = cameras[i].rotation[j];
}
//std::copy(cameras[i].translation, cameras[i].translation + 3, new_cam_poses[i].t.begin());
//std::copy(cameras[i].rotation, cameras[i].rotation + 9, new_cam_poses[i].R.begin());
radial_distortion[i] = Vec2f(cameras[i].distortion[0], cameras[i].distortion[1]);
new_cam_poses[i].set_k_matrix(cameras[i].focal_length, 0.0, 0.0);
}
for (int i = 0; i < new_pts_3d.size(); i++) {
for (int j = 0; j < 3; j++)
{
new_pts_3d[i] = points[i].pos[j];
}
}
// 输出优化信息
std::cout << "Params after BA: " << std::endl;
std::cout << " f: " << new_cam_poses[0].get_focal_length() << std::endl;
std::cout << " distortion: " << radial_distortion[0][0] << ", " << radial_distortion[0][1] << std::endl;
std::cout << " R: " << new_cam_poses[0].R << std::endl;
std::cout << " t: " << new_cam_poses[0].t << std::endl;
// 输出优化信息
std::cout << "Params after BA: " << std::endl;
std::cout << " f: " << new_cam_poses[1].get_focal_length() << std::endl;
std::cout << " distortion: " << radial_distortion[1][0] << ", " << radial_distortion[1][1] << std::endl;
std::cout << " R: " << new_cam_poses[1].R << std::endl;
std::cout << " t: " << new_cam_poses[1].t << std::endl;
std::cout << "points 3d: " << std::endl;
for (int i = 0; i < points.size(); i++) {
std::cout << points[i].pos[0] << ", " << points[i].pos[1] << ", " << points[i].pos[2] << std::endl;
}
// Params after BA:
// f: 0.919446
// distortion: -0.108421, 0.103782
// R: 0.999999 -0.00068734 -0.00135363
// 0.000675175 0.999952 -0.0104268
// 0.0013597 0.0104261 0.999952
// t: 0.00276221 0.0588868 -0.128463
// Params after BA:
// f: 0.920023
// distortion: -0.106701, 0.104344
// R: 0.999796 -0.0127484 0.0156791
// 0.0128673 0.999897 -0.00735337
// -0.0155827 0.00755345 0.999857
// t: 0.0814124 0.93742 -0.0895658
// points 3d:
// 1.36957, -1.17132, 7.04854
// 0.0225931, 0.978747, 7.48085
return 0;
}
标签：std,捆绑,const,deriv,double,void,opencv,调整,size 来源： https://blog.csdn.net/AAAA202012/article/details/118571323