diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c27dd43 --- /dev/null +++ b/.gitignore @@ -0,0 +1,41 @@ +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +# Folders +build/* +/cmake-build-debug/* +.DS_Store +*/.DS_Store +tmp/* +*.vscode +examples/data/* diff --git a/examples/task1/class1_test_features.cc b/examples/task1/class1_test_features.cc index d31bb5f..8e7ff71 100644 --- a/examples/task1/class1_test_features.cc +++ b/examples/task1/class1_test_features.cc @@ -28,9 +28,9 @@ sift_compare (features::Sift::Descriptor const& d1, features::Sift::Descriptor c int main (int argc, char** argv) { - if (argc < 2) + if (argc < 3) { - std::cerr << "Syntax: " << argv[0] << " " << std::endl; + std::cerr << "Syntax: " << argv[0] << " " << "out put file name path without .png format"< #include #include +#include #include #include "math/functions.h" #include "sfm/fundamental.h" diff --git a/examples/task2/class2_test_math_basic.cc b/examples/task2/class2_test_math_basic.cc index 8a9c9ee..9720a37 100644 --- a/examples/task2/class2_test_math_basic.cc +++ b/examples/task2/class2_test_math_basic.cc @@ -1 +1,56 @@ -// // Created by caoqi on 2018/8/30. // #include #include #include "math/matrix.h" #include "math/vector.h" int main(int argc, char *argv[]) { /*构建一个维度为4x5的矩阵，数据类型为double的矩阵*/ math::Matrix A; /*矩阵元素的设置和访问*/ int id=0; for(int i=0; i< A.rows; i++){ for(int j=0; j< A.cols; j++){ A(i,j) = ++id; std::cout< col4 = A.col(4); // 取第5列元素 std::cout<<"col4: "< row2 = A.row(2); // 取第3行元素 std::cout<<"row2: "< v1; for(int i=0; iU; math::Matrix S, V; math::matrix_svd (A,&U, &S, &V); std::cout<<"U: "< +#include +#include "math/matrix.h" +#include "math/vector.h" + +int main(int argc, char *argv[]) +{ + + /*构建一个维度为4x5的矩阵，数据类型为double的矩阵*/ + math::Matrix A; + + /*矩阵元素的设置和访问*/ + int id=0; + for(int i=0; i< A.rows; i++){ + for(int j=0; j< A.cols; j++){ + A(i,j) = ++id; + std::cout< col4 = A.col(4); // 取第5列元素 + std::cout<<"col4: "< row2 = A.row(2); // 取第3行元素 + std::cout<<"row2: "< v1; + for(int i=0; i U; + math::Matrix S, V; + math::matrix_svd (A,&U, &S, &V); + std::cout<<"U: "< #include #include +#include #include "math/matrix.h" #include "math/vector.h" diff --git a/examples/task3/class3_test_lm_optimize.cc b/examples/task3/class3_test_lm_optimize.cc index f0924f9..3c6ee7a 100644 --- a/examples/task3/class3_test_lm_optimize.cc +++ b/examples/task3/class3_test_lm_optimize.cc @@ -1 +1,940 @@ -// // Created by caoqi on 2018/9/2. /* * 实现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 #include #include #include #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 SparseMatrixType; typedef sfm::ba::DenseVector DenseVectorType; //global variables std::vector cameras; std::vector points; std::vector 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&cams ,std::vector&pts3D ,std::vector &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>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[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 const& A, std::size_t block_size, sfm::ba::SparseMatrix* B) { sfm::ba::SparseMatrix::Triplets triplets; triplets.reserve(A.num_cols() * block_size); for (std::size_t block = 0; block < A.num_cols(); block += block_size) { std::vector> 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* 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 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* 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 matrix_block(bs2); for (double* iter = A->begin(); iter != A->end(); ) { double* iter_backup = iter; for (int i = 0; i < bs2; ++i) matrix_block[i] = *(iter++); sfm::ba::cholesky_invert_inplace(matrix_block.data(), blocksize); iter = iter_backup; for (int i = 0; i < bs2; ++i) if (std::isfinite(matrix_block[i])) *(iter++) = matrix_block[i]; else *(iter++) = 0.0; } } /** * /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; } /** * \description 根据求解得到的增量，对相机参数进行更新 * @param cam * @param update * @param out */ void update_camera (sfm::ba::Camera const& cam, double const* update, sfm::ba::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); math::matrix_multiply(rot_update, 3, 3, rot_orig, 3, out->rotation); } /** * \description 根据求解的增量，对三维点坐标进行更新 * @param pt * @param update * @param out */ void update_point (sfm::ba::Point3D const& pt, double const* update, sfm::ba::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]; } /** * /descripition 根据求得的delta_x, 更新相机参数和三维点 * @param delta_x * @param cameras * @param points */ void update_parameters (DenseVectorType const& delta_x , std::vector*cameras , std::vector*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 对像素进行径向畸变 * @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; } /** * \description 计算重投影误差 * @param vector_f * @param delta_x * @param cameras * @param points * @param observations */ void compute_reprojection_errors (DenseVectorType* vector_f , DenseVectorType const* delta_x , std::vector* cameras , std::vector *points ,std::vector *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) { sfm::ba::Observation const& obs = observations->at(i); sfm::ba::Point3D const& p3d = points->at(obs.point_id); sfm::ba::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; // 三维点坐标 sfm::ba::Point3D new_point; sfm::ba::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]; } } /** * \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(vector_f.size() / 2); } /** * /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(sfm::ba::Camera const& cam, sfm::ba::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 构造雅阁比矩阵，采用稀疏矩阵形式， * 关于相机参数的雅阁比矩阵大小为：(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 相机的索引 sfm::ba::Observation const &obs = observations[i]; // 三维点坐标 sfm::ba::Point3D const &p3d = points[obs.point_id]; // 相机参数 sfm::ba::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); } } sfm::ba::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 sfm::ba::ConjugateGradient 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); sfm::ba::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 LM 算法流程 * @param cameras * @param points * @param observations */ void lm_optimization(std::vector*cameras ,std::vector*points ,std::vector* 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; // 设置共轭梯度法的相关参数 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); //2.0 预置共轭梯梯度法对正规方程进行求解*/ DenseVectorType delta_x; sfm::ba::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: "<= lm_max_iterations) { std::cout << "BA: Reached maximum LM iterations of " << lm_max_iterations << std::endl; break; } } final_mse = current_mse; } int main(int argc, char* argv[]) { /* 加载数据 */ load_data("./examples/task2/test_ba.txt",cameras, points, observations); lm_optimization(&cameras, &points, &observations); // ba优化 // sfm::ba::BundleAdjustment::Options ba_opts; // ba_opts.verbose_output = true; // ba_opts.lm_mse_threshold = 1e-16; // ba_opts.lm_delta_threshold = 1e-8; // sfm::ba::BundleAdjustment ba(ba_opts); // ba.set_cameras(&cameras); // ba.set_points(&points); // ba.set_observations(&observations); // ba.optimize(); // ba.print_status(); // 将优化后的结果重新赋值 std::vector new_cam_poses(2); std::vector radial_distortion(2); std::vector new_pts_3d(points.size()); for(int i=0; i +#include +#include +#include +#include +#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 SparseMatrixType; +typedef sfm::ba::DenseVector DenseVectorType; + +//global variables +std::vector cameras; +std::vector points; +std::vector 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&cams + ,std::vector&pts3D + ,std::vector &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>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[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 const& A, +std::size_t block_size, sfm::ba::SparseMatrix* B) +{ + sfm::ba::SparseMatrix::Triplets triplets; + triplets.reserve(A.num_cols() * block_size); + for (std::size_t block = 0; block < A.num_cols(); block += block_size) { + std::vector> 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* 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 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* 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 matrix_block(bs2); + for (double* iter = A->begin(); iter != A->end(); ) + { + double* iter_backup = iter; + for (int i = 0; i < bs2; ++i) + matrix_block[i] = *(iter++); + + sfm::ba::cholesky_invert_inplace(matrix_block.data(), blocksize); + + iter = iter_backup; + for (int i = 0; i < bs2; ++i) + if (std::isfinite(matrix_block[i])) + *(iter++) = matrix_block[i]; + else + *(iter++) = 0.0; + } +} + +/** + * /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; +} + +/** + * \description 根据求解得到的增量，对相机参数进行更新 + * @param cam + * @param update + * @param out + */ +void update_camera (sfm::ba::Camera const& cam, + double const* update, sfm::ba::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); + math::matrix_multiply(rot_update, 3, 3, rot_orig, 3, out->rotation); +} + +/** + * \description 根据求解的增量，对三维点坐标进行更新 + * @param pt + * @param update + * @param out + */ +void update_point (sfm::ba::Point3D const& pt, + double const* update, sfm::ba::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]; +} + +/** + * /descripition 根据求得的delta_x, 更新相机参数和三维点 + * @param delta_x + * @param cameras + * @param points + */ +void +update_parameters (DenseVectorType const& delta_x + , std::vector*cameras +, std::vector*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 对像素进行径向畸变 + * @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; +} + +/** + * \description 计算重投影误差 + * @param vector_f + * @param delta_x + * @param cameras + * @param points + * @param observations + */ +void compute_reprojection_errors (DenseVectorType* vector_f + , DenseVectorType const* delta_x + , std::vector* cameras + , std::vector *points + ,std::vector *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) + { + sfm::ba::Observation const& obs = observations->at(i); + sfm::ba::Point3D const& p3d = points->at(obs.point_id); + sfm::ba::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; // 三维点坐标 + + sfm::ba::Point3D new_point; + sfm::ba::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]; + } +} + +/** + * \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(vector_f.size() / 2); +} + +/** + * /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(sfm::ba::Camera const& cam, + sfm::ba::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 构造雅阁比矩阵，采用稀疏矩阵形式， + * 关于相机参数的雅阁比矩阵大小为：(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 相机的索引 + sfm::ba::Observation const &obs = observations[i]; + // 三维点坐标 + sfm::ba::Point3D const &p3d = points[obs.point_id]; + // 相机参数 + sfm::ba::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); + } +} + + + +sfm::ba::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 sfm::ba::ConjugateGradient 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); + + sfm::ba::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 LM 算法流程 + * @param cameras + * @param points + * @param observations + */ +void lm_optimization(std::vector*cameras + ,std::vector*points + ,std::vector* 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; + + + // 设置共轭梯度法的相关参数 + 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); + + //2.0 预置共轭梯梯度法对正规方程进行求解*/ + DenseVectorType delta_x; + sfm::ba::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: "<= lm_max_iterations) { + std::cout << "BA: Reached maximum LM iterations of " + << lm_max_iterations << std::endl; + break; + } + } + + final_mse = current_mse; +} + +int main(int argc, char* argv[]) +{ + + /* 加载数据 */ + load_data("./examples/task2/test_ba.txt",cameras, points, observations); + + lm_optimization(&cameras, &points, &observations); + + // ba优化 +// sfm::ba::BundleAdjustment::Options ba_opts; +// ba_opts.verbose_output = true; +// ba_opts.lm_mse_threshold = 1e-16; +// ba_opts.lm_delta_threshold = 1e-8; +// sfm::ba::BundleAdjustment ba(ba_opts); +// ba.set_cameras(&cameras); +// ba.set_points(&points); +// ba.set_observations(&observations); +// ba.optimize(); +// ba.print_status(); + + // 将优化后的结果重新赋值 + std::vector new_cam_poses(2); + std::vector radial_distortion(2); + std::vector new_pts_3d(points.size()); + for(int i=0; i #include #include #include #include /** *\description 创建一个场景 * @param image_folder_path * @param scene_path * @return */ core::Scene::Ptr make_scene(const std::string & image_folder_path, const std::string & scene_path){ util::WallTimer timer; /*** 创建文件夹 ***/ const std::string views_path = util::fs::join_path(scene_path, "views/"); util::fs::mkdir(scene_path.c_str()); util::fs::mkdir(views_path.c_str()); /***扫描文件夹，获取所有的图像文件路径***/ util::fs::Directory dir; try {dir.scan(image_folder_path); } catch (std::exception&e){ std::cerr << "Error scanning input dir: " << e.what() << std::endl; std::exit(EXIT_FAILURE); } std::cout << "Found " << dir.size() << " directory entries." << std::endl; core::Scene::Ptr scene= core::Scene::create(""); /**** 开始加载图像 ****/ std::sort(dir.begin(), dir.end()); int num_imported = 0; for(std::size_t i=0; i< dir.size(); i++){ // 是一个文件夹 if(dir[i].is_dir){ std::cout<<"Skipping directory "<set_id(num_imported); view->set_name(remove_file_extension(fname)); // 限制图像尺寸 int orig_width = image->width(); image = limit_image_size(image, MAX_PIXELS); if (orig_width == image->width() && has_jpeg_extension(fname)) view->set_image_ref(afname, "original"); else view->set_image(image, "original"); add_exif_to_view(view, exif); scene->get_views().push_back(view); /***保存视角信息到本地****/ std::string mve_fname = make_image_name(num_imported); std::cout << "Importing image: " << fname << ", writing MVE view: " << mve_fname << "..." << std::endl; view->save_view_as(util::fs::join_path(views_path, mve_fname)); num_imported+=1; } std::cout << "Imported " << num_imported << " input images, " << "took " << timer.get_elapsed() << " ms." << std::endl; return scene; } /** * * @param scene * @param viewports * @param pairwise_matching */ void features_and_matching (core::Scene::Ptr scene, sfm::bundler::ViewportList* viewports, sfm::bundler::PairwiseMatching* pairwise_matching){ /* Feature computation for the scene. */ sfm::bundler::Features::Options feature_opts; feature_opts.image_embedding = "original"; feature_opts.max_image_size = MAX_PIXELS; feature_opts.feature_options.feature_types = sfm::FeatureSet::FEATURE_SIFT; std::cout << "Computing image features..." << std::endl; { util::WallTimer timer; sfm::bundler::Features bundler_features(feature_opts); bundler_features.compute(scene, viewports); std::cout << "Computing features took " << timer.get_elapsed() << " ms." << std::endl; std::cout<<"Feature detection took " + util::string::get(timer.get_elapsed()) + "ms."<empty()) { std::cerr << "Error: No matching image pairs. Exiting." << std::endl; std::exit(EXIT_FAILURE); } } int main(int argc, char *argv[]) { if(argc < 3){ std::cout<<"Usage: [input]image_dir [output]scen_dir"<get_views().size()<<" views. "<cache_cleanup(); for (std::size_t i = 0; i < viewports.size(); ++i) viewports[i].features.clear_descriptors(); /* Check if there are some matching images. */ if (pairwise_matching.empty()) { std::cerr << "No matching image pairs. Exiting." << std::endl; std::exit(EXIT_FAILURE); } // 计算相机内参数，从Exif中读取 { sfm::bundler::Intrinsics::Options intrinsics_opts; std::cout << "Initializing camera intrinsics..." << std::endl; sfm::bundler::Intrinsics intrinsics(intrinsics_opts); intrinsics.compute(scene, &viewports); } /****** 开始增量的捆绑调整*****/ util::WallTimer timer; /* Compute connected feature components, i.e. feature tracks. */ sfm::bundler::TrackList tracks; { sfm::bundler::Tracks::Options tracks_options; tracks_options.verbose_output = true; sfm::bundler::Tracks bundler_tracks(tracks_options); std::cout << "Computing feature tracks..." << std::endl; bundler_tracks.compute(pairwise_matching, &viewports, &tracks); std::cout << "Created a total of " << tracks.size() << " tracks." << std::endl; } /* Remove color data and pairwise matching to save memory. */ for (std::size_t i = 0; i < viewports.size(); ++i) viewports[i].features.colors.clear(); pairwise_matching.clear(); // 计算初始的匹配对 sfm::bundler::InitialPair::Result init_pair_result; sfm::bundler::InitialPair::Options init_pair_opts; //init_pair_opts.homography_opts.max_iterations = 1000; //init_pair_opts.homography_opts.threshold = 0.005f; init_pair_opts.homography_opts.verbose_output = false; init_pair_opts.max_homography_inliers = 0.8f; init_pair_opts.verbose_output = true; // 开始计算初始的匹配对 sfm::bundler::InitialPair init_pair(init_pair_opts); init_pair.initialize(viewports, tracks); init_pair.compute_pair(&init_pair_result); if (init_pair_result.view_1_id < 0 || init_pair_result.view_2_id < 0 || init_pair_result.view_1_id >= static_cast(viewports.size()) || init_pair_result.view_2_id >= static_cast(viewports.size())) { std::cerr << "Error finding initial pair, exiting!" << std::endl; std::cerr << "Try manually specifying an initial pair." << std::endl; std::exit(EXIT_FAILURE); } std::cout << "Using views " << init_pair_result.view_1_id << " and " << init_pair_result.view_2_id << " as initial pair." << std::endl; /* Incrementally compute full bundle. */ sfm::bundler::Incremental::Options incremental_opts; incremental_opts.pose_p3p_opts.max_iterations = 1000; incremental_opts.pose_p3p_opts.threshold = 0.005f; incremental_opts.pose_p3p_opts.verbose_output = false; incremental_opts.track_error_threshold_factor = TRACK_ERROR_THRES_FACTOR; incremental_opts.new_track_error_threshold = NEW_TRACK_ERROR_THRES; incremental_opts.min_triangulation_angle = MATH_DEG2RAD(1.0); incremental_opts.ba_fixed_intrinsics = false; //incremental_opts.ba_shared_intrinsics = conf.shared_intrinsics; incremental_opts.verbose_output = true; incremental_opts.verbose_ba = true; /* Initialize viewports with initial pair. */ viewports[init_pair_result.view_1_id].pose = init_pair_result.view_1_pose; viewports[init_pair_result.view_2_id].pose = init_pair_result.view_2_pose; /* Initialize the incremental bundler and reconstruct first tracks. */ sfm::bundler::Incremental incremental(incremental_opts); incremental.initialize(&viewports, &tracks); // 对当前两个视角进行track重建，并且如果track存在外点，则将每个track的外点剥离成新的track incremental.triangulate_new_tracks(2); // 根据重投影误差进行筛选 incremental.invalidate_large_error_tracks(); /* Run bundle adjustment. */ std::cout << "Running full bundle adjustment..." << std::endl; incremental.bundle_adjustment_full(); /* Reconstruct remaining views. */ int num_cameras_reconstructed = 2; int full_ba_num_skipped = 0; while (true) { /* Find suitable next views for reconstruction. */ std::vector next_views; incremental.find_next_views(&next_views); /* Reconstruct the next view. */ int next_view_id = -1; for (std::size_t i = 0; i < next_views.size(); ++i) { std::cout << std::endl; std::cout << "Adding next view ID " << next_views[i] << " (" << (num_cameras_reconstructed + 1) << " of " << viewports.size() << ")..." << std::endl; if (incremental.reconstruct_next_view(next_views[i])) { next_view_id = next_views[i]; break; } } if (next_view_id < 0) { if (full_ba_num_skipped == 0) { std::cout << "No valid next view." << std::endl; std::cout << "SfM reconstruction finished." << std::endl; break; } else { incremental.triangulate_new_tracks(MIN_VIEWS_PER_TRACK); std::cout << "Running full bundle adjustment..." << std::endl; incremental.invalidate_large_error_tracks(); incremental.bundle_adjustment_full(); full_ba_num_skipped = 0; continue; } } /* Run single-camera bundle adjustment. */ std::cout << "Running single camera bundle adjustment..." << std::endl; incremental.bundle_adjustment_single_cam(next_view_id); num_cameras_reconstructed += 1; /* Run full bundle adjustment only after a couple of views. */ int const full_ba_skip_views = std::min(100, num_cameras_reconstructed / 10); if (full_ba_num_skipped < full_ba_skip_views) { std::cout << "Skipping full bundle adjustment (skipping " << full_ba_skip_views << " views)." << std::endl; full_ba_num_skipped += 1; } else { incremental.triangulate_new_tracks(MIN_VIEWS_PER_TRACK); std::cout << "Running full bundle adjustment..." << std::endl; incremental.invalidate_large_error_tracks(); incremental.bundle_adjustment_full(); full_ba_num_skipped = 0; } } sfm::bundler::TrackList valid_tracks; for(int i=0; iget_cameras(); core::Scene::ViewList const& views = scene->get_views(); if (bundle_cams.size() != views.size()) { std::cerr << "Error: Invalid number of cameras!" << std::endl; std::exit(EXIT_FAILURE); } #pragma omp parallel for schedule(dynamic,1) for (std::size_t i = 0; i < bundle_cams.size(); ++i) { core::View::Ptr view = views[i]; core::CameraInfo const& cam = bundle_cams[i]; if (view == nullptr) continue; if (view->get_camera().flen == 0.0f && cam.flen == 0.0f) continue; view->set_camera(cam); /* Undistort image. */ if (!undistorted_name.empty()) { core::ByteImage::Ptr original = view->get_byte_image(original_name); if (original == nullptr) continue; core::ByteImage::Ptr undist = core::image::image_undistort_k2k4 (original, cam.flen, cam.dist[0], cam.dist[1]); view->set_image(undist, undistorted_name); } #pragma omp critical std::cout << "Saving view " << view->get_directory() << std::endl; view->save_view(); view->cache_cleanup(); } // log_message(conf, "SfM reconstruction done.\n"); return 0; } \ No newline at end of file +// +// Created by caoqi on 2018/8/28. +// +#include "defines.h" +#include "functions.h" +#include "sfm/bundler_common.h" +#include "sfm/bundler_features.h" +#include "sfm/bundler_matching.h" +#include "sfm/bundler_intrinsics.h" +#include "sfm/bundler_init_pair.h" +#include "sfm/bundler_tracks.h" +#include "sfm/bundler_incremental.h" +#include "core/scene.h" +#include "util/timer.h" +#include + +#include +#include +#include +#include +#include + +/** + *\description 创建一个场景 + * @param image_folder_path + * @param scene_path + * @return + */ +core::Scene::Ptr +make_scene(const std::string & image_folder_path, const std::string & scene_path){ + + util::WallTimer timer; + + /*** 创建文件夹 ***/ + const std::string views_path = util::fs::join_path(scene_path, "views/"); + util::fs::mkdir(scene_path.c_str()); + util::fs::mkdir(views_path.c_str()); + + /***扫描文件夹，获取所有的图像文件路径***/ + util::fs::Directory dir; + try {dir.scan(image_folder_path); + } + catch (std::exception&e){ + std::cerr << "Error scanning input dir: " << e.what() << std::endl; + std::exit(EXIT_FAILURE); + } + std::cout << "Found " << dir.size() << " directory entries." << std::endl; + + core::Scene::Ptr scene= core::Scene::create(""); + + /**** 开始加载图像 ****/ + std::sort(dir.begin(), dir.end()); + int num_imported = 0; + for(std::size_t i=0; i< dir.size(); i++){ + // 是一个文件夹 + if(dir[i].is_dir){ + std::cout<<"Skipping directory "<set_id(num_imported); + view->set_name(remove_file_extension(fname)); + + // 限制图像尺寸 + int orig_width = image->width(); + image = limit_image_size(image, MAX_PIXELS); + if (orig_width == image->width() && has_jpeg_extension(fname)) + view->set_image_ref(afname, "original"); + else + view->set_image(image, "original"); + + add_exif_to_view(view, exif); + + scene->get_views().push_back(view); + + /***保存视角信息到本地****/ + std::string mve_fname = make_image_name(num_imported); + std::cout << "Importing image: " << fname + << ", writing MVE view: " << mve_fname << "..." << std::endl; + view->save_view_as(util::fs::join_path(views_path, mve_fname)); + + num_imported+=1; + } + + std::cout << "Imported " << num_imported << " input images, " + << "took " << timer.get_elapsed() << " ms." << std::endl; + + return scene; +} + +/** + * + * @param scene + * @param viewports + * @param pairwise_matching + */ +void +features_and_matching (core::Scene::Ptr scene, + sfm::bundler::ViewportList* viewports, + sfm::bundler::PairwiseMatching* pairwise_matching){ + + /* Feature computation for the scene. */ + sfm::bundler::Features::Options feature_opts; + feature_opts.image_embedding = "original"; + feature_opts.max_image_size = MAX_PIXELS; + feature_opts.feature_options.feature_types = sfm::FeatureSet::FEATURE_SIFT; + + std::cout << "Computing image features..." << std::endl; + { + util::WallTimer timer; + sfm::bundler::Features bundler_features(feature_opts); + bundler_features.compute(scene, viewports); + + std::cout << "Computing features took " << timer.get_elapsed() + << " ms." << std::endl; + std::cout<<"Feature detection took " + util::string::get(timer.get_elapsed()) + "ms."<empty()) { + std::cerr << "Error: No matching image pairs. Exiting." << std::endl; + std::exit(EXIT_FAILURE); + } +} + + +int main(int argc, char *argv[]) +{ + + if(argc < 4){ + std::cout<<"Usage: [input]image_dir [output]scen_dir [output]PLYFILE.ply"<get_views().size()<<" views. "<cache_cleanup(); + for (std::size_t i = 0; i < viewports.size(); ++i) + viewports[i].features.clear_descriptors(); + + /* Check if there are some matching images. */ + if (pairwise_matching.empty()) { + std::cerr << "No matching image pairs. Exiting." << std::endl; + std::exit(EXIT_FAILURE); + } + + // 计算相机内参数，从Exif中读取 + { + sfm::bundler::Intrinsics::Options intrinsics_opts; + std::cout << "Initializing camera intrinsics..." << std::endl; + sfm::bundler::Intrinsics intrinsics(intrinsics_opts); + intrinsics.compute(scene, &viewports); + } + + /****** 开始增量的捆绑调整*****/ + util::WallTimer timer; + /* Compute connected feature components, i.e. feature tracks. */ + sfm::bundler::TrackList tracks; + { + sfm::bundler::Tracks::Options tracks_options; + tracks_options.verbose_output = true; + + sfm::bundler::Tracks bundler_tracks(tracks_options); + std::cout << "Computing feature tracks..." << std::endl; + bundler_tracks.compute(pairwise_matching, &viewports, &tracks); + std::cout << "Created a total of " << tracks.size() + << " tracks." << std::endl; + } + + /* Remove color data and pairwise matching to save memory. */ + for (std::size_t i = 0; i < viewports.size(); ++i) + viewports[i].features.colors.clear(); + pairwise_matching.clear(); + + + // 计算初始的匹配对 + sfm::bundler::InitialPair::Result init_pair_result; + sfm::bundler::InitialPair::Options init_pair_opts; + //init_pair_opts.homography_opts.max_iterations = 1000; + //init_pair_opts.homography_opts.threshold = 0.005f; + init_pair_opts.homography_opts.verbose_output = false; + init_pair_opts.max_homography_inliers = 0.8f; + init_pair_opts.verbose_output = true; + + // 开始计算初始的匹配对 + sfm::bundler::InitialPair init_pair(init_pair_opts); + init_pair.initialize(viewports, tracks); + init_pair.compute_pair(&init_pair_result); + if (init_pair_result.view_1_id < 0 || init_pair_result.view_2_id < 0 + || init_pair_result.view_1_id >= static_cast(viewports.size()) + || init_pair_result.view_2_id >= static_cast(viewports.size())) + { + std::cerr << "Error finding initial pair, exiting!" << std::endl; + std::cerr << "Try manually specifying an initial pair." << std::endl; + std::exit(EXIT_FAILURE); + } + + std::cout << "Using views " << init_pair_result.view_1_id + << " and " << init_pair_result.view_2_id + << " as initial pair." << std::endl; + + + /* Incrementally compute full bundle. */ + sfm::bundler::Incremental::Options incremental_opts; + incremental_opts.pose_p3p_opts.max_iterations = 1000; + incremental_opts.pose_p3p_opts.threshold = 0.005f; + incremental_opts.pose_p3p_opts.verbose_output = false; + incremental_opts.track_error_threshold_factor = TRACK_ERROR_THRES_FACTOR; + incremental_opts.new_track_error_threshold = NEW_TRACK_ERROR_THRES; + incremental_opts.min_triangulation_angle = MATH_DEG2RAD(1.0); + incremental_opts.ba_fixed_intrinsics = false; + //incremental_opts.ba_shared_intrinsics = conf.shared_intrinsics; + incremental_opts.verbose_output = true; + incremental_opts.verbose_ba = true; + + /* Initialize viewports with initial pair. */ + viewports[init_pair_result.view_1_id].pose = init_pair_result.view_1_pose; + viewports[init_pair_result.view_2_id].pose = init_pair_result.view_2_pose; + + /* Initialize the incremental bundler and reconstruct first tracks. */ + sfm::bundler::Incremental incremental(incremental_opts); + incremental.initialize(&viewports, &tracks); + + // 对当前两个视角进行track重建，并且如果track存在外点，则将每个track的外点剥离成新的track + incremental.triangulate_new_tracks(2); + + // 根据重投影误差进行筛选 + incremental.invalidate_large_error_tracks(); + + /* Run bundle adjustment. */ + std::cout << "Running full bundle adjustment..." << std::endl; + incremental.bundle_adjustment_full(); + + /* Reconstruct remaining views. */ + int num_cameras_reconstructed = 2; + int full_ba_num_skipped = 0; + while (true) + { + /* Find suitable next views for reconstruction. */ + std::vector next_views; + incremental.find_next_views(&next_views); + + /* Reconstruct the next view. */ + int next_view_id = -1; + for (std::size_t i = 0; i < next_views.size(); ++i) + { + std::cout << std::endl; + std::cout << "Adding next view ID " << next_views[i] + << " (" << (num_cameras_reconstructed + 1) << " of " + << viewports.size() << ")..." << std::endl; + if (incremental.reconstruct_next_view(next_views[i])) + { + next_view_id = next_views[i]; + break; + } + } + + if (next_view_id < 0) { + if (full_ba_num_skipped == 0) { + std::cout << "No valid next view." << std::endl; + std::cout << "SfM reconstruction finished." << std::endl; + break; + } + else + { + incremental.triangulate_new_tracks(MIN_VIEWS_PER_TRACK); + std::cout << "Running full bundle adjustment..." << std::endl; + incremental.invalidate_large_error_tracks(); + incremental.bundle_adjustment_full(); + full_ba_num_skipped = 0; + continue; + } + } + + /* Run single-camera bundle adjustment. */ + std::cout << "Running single camera bundle adjustment..." << std::endl; + incremental.bundle_adjustment_single_cam(next_view_id); + num_cameras_reconstructed += 1; + + /* Run full bundle adjustment only after a couple of views. */ + int const full_ba_skip_views = std::min(100, num_cameras_reconstructed / 10); + if (full_ba_num_skipped < full_ba_skip_views) + { + std::cout << "Skipping full bundle adjustment (skipping " + << full_ba_skip_views << " views)." << std::endl; + full_ba_num_skipped += 1; + } + else + { + incremental.triangulate_new_tracks(MIN_VIEWS_PER_TRACK); + std::cout << "Running full bundle adjustment..." << std::endl; + incremental.invalidate_large_error_tracks(); + incremental.bundle_adjustment_full(); + full_ba_num_skipped = 0; + } + } + + sfm::bundler::TrackList valid_tracks; + for(int i=0; iget_cameras(); + core::Scene::ViewList const& views = scene->get_views(); + if (bundle_cams.size() != views.size()) + { + std::cerr << "Error: Invalid number of cameras!" << std::endl; + std::exit(EXIT_FAILURE); + } + +#pragma omp parallel for schedule(dynamic,1) + for (std::size_t i = 0; i < bundle_cams.size(); ++i) + { + core::View::Ptr view = views[i]; + core::CameraInfo const& cam = bundle_cams[i]; + if (view == nullptr) + continue; + if (view->get_camera().flen == 0.0f && cam.flen == 0.0f) + continue; + + view->set_camera(cam); + + /* Undistort image. */ + if (!undistorted_name.empty()) + { + core::ByteImage::Ptr original + = view->get_byte_image(original_name); + if (original == nullptr) + continue; + core::ByteImage::Ptr undist + = core::image::image_undistort_k2k4 + (original, cam.flen, cam.dist[0], cam.dist[1]); + view->set_image(undist, undistorted_name); + } + +#pragma omp critical + std::cout << "Saving view " << view->get_directory() << std::endl; + view->save_view(); + view->cache_cleanup(); + } + + // log_message(conf, "SfM reconstruction done.\n"); + + return 0; +} + diff --git a/examples/task5/class5_scene2pset_multi_views.cc b/examples/task5/class5_scene2pset_multi_views.cc index f78f757..f170a95 100644 --- a/examples/task5/class5_scene2pset_multi_views.cc +++ b/examples/task5/class5_scene2pset_multi_views.cc @@ -15,6 +15,7 @@ #include #include #include +#include #include "core/depthmap.h" #include "core/mesh_info.h" @@ -169,7 +170,7 @@ main (int argc, char** argv) /* Write mesh to disc. */ std::cout << "Writing final point set (" << verts.size() << " points)..." << std::endl; - assert(util::string::right(arg.outmesh, 4) == ".ply"); + assert(util::string::right(conf.outmesh, 4) == ".ply"); { core::geom::SavePLYOptions opts; opts.write_vertex_normals = conf.with_normals; diff --git a/examples/task5/class5_scene2pset_single_view.cc b/examples/task5/class5_scene2pset_single_view.cc index 7efdc61..51b6802 100644 --- a/examples/task5/class5_scene2pset_single_view.cc +++ b/examples/task5/class5_scene2pset_single_view.cc @@ -1 +1,404 @@ -// // Created by caoqi on 2018/10/01. // #include #include #include #include #include #include #include #include #include "core/depthmap.h" #include "core/mesh_info.h" #include "core/mesh_io.h" #include "core/mesh_io_ply.h" #include "core/mesh_tools.h" #include "core/scene.h" #include "core/view.h" #include "core/depthmap.h" struct AppSettings { std::string scenedir; std::string outmesh; std::string dmname = "depth-L0"; std::string imagename = "undistorted"; std::string mask; std::string aabb; bool with_normals = true; bool with_scale = true; bool with_conf = true; bool poisson_normals = false; float min_valid_fraction = 0.0f; float scale_factor = 2.5f; /* "Radius" of MVS patch (usually 5x5). */ std::vector ids; int view_id = -1; float dd_factor = 5.0f; int scale = 0; }; /** * \description 给定像素坐标，深度值以及内参矩阵的逆，求对应三维点的坐标 * @param x -- 像素坐标x * @param y -- 像素坐标y * @param depth --深度值即，三维点到相机中心的距离 * @param invproj -- 内参矩阵的逆 * @return 三维点坐标 */ math::Vec3f pixel_3dpos (std::size_t x, std::size_t y, float depth, math::Matrix3f const& invproj) { // 图像上的每个像素对应三维中的一条射线(向量） math::Vec3f ray = invproj * math::Vec3f ((float)x + 0.5f, (float)y + 0.5f, 1.0f); // 将射线所在的向量进行归一化，并乘以深度值，得到三维点的坐标 return ray.normalized() * depth; } /** * \description 判断深度值是否具有一致性 * @param widths -- 4x1 4个图像像素对应在三维空间中的宽度，可以理解成空间中长度为widths[i1]的物体 * 投影到图像中i1位置的大小刚好为一个像素 * @param depths -- 深度值 4个图像像素的深度值 * @param dd_factor -- 判断深度一致性的阈值 * 4个像素构成一个grid, 序号分别为 * [0, 1] * [2, 3] * @param i1 -- i1 \in [0,3] * @param i2 -- i2 \in [0,3] * @return */ bool dm_is_depthdisc (float* widths, float* depths , float dd_factor, int i1, int i2) { /* Find index that corresponds to smaller depth. */ int i_min = i1; int i_max = i2; if (depths[i2] < depths[i1]) std::swap(i_min, i_max); /* Check if indices are a diagonal. */ if (i1 + i2 == 3) dd_factor *= MATH_SQRT2; /* Check for depth discontinuity. */ if (depths[i_max] - depths[i_min] > widths[i_min] * dd_factor) return true; return false; } /** * \description 计算1个图像像素在三维空间点p处的长度l，可以理解成空间点p处长度为l的物体 * 投影到图像大小刚好为一个像素 * @param x -- 图像像素坐标 * @param y -- 图像像素坐标 * @param depth -- 图像深度值 * @param invproj -- 相机内参矩阵的逆 * @return */ float pixel_footprint (std::size_t x, std::size_t y, float depth, math::Matrix3f const& invproj) { math::Vec3f v = invproj * math::Vec3f ((float)x + 0.5f, (float)y + 0.5f, 1.0f); return invproj[0] * depth / v.norm(); } /** * \description 创建一个三角面片 * @param mesh -- 需要创建的三维网格 * @param vidx -- 图像像素对应三维顶点索引的映射图 * @param dm -- 深度图像 * @param invproj -- 相机内参矩阵的逆矩阵 * @param i -- 像素在图像中的全局索引[0, w*h-1] * @param tverts -- 三角面片的顶点索引，用的是在局部grid中的索引，即[0, 1] * [2, 3] * */ void dm_make_triangle (core::TriangleMesh* mesh, core::Image& vidx, core::FloatImage const* dm, math::Matrix3f const& invproj, std::size_t i, int* tverts) { int const width = vidx.width(); //int const height = vidx.height(); core::TriangleMesh::VertexList& verts(mesh->get_vertices()); core::TriangleMesh::FaceList& faces(mesh->get_faces()); for (int j = 0; j < 3; ++j) { int iidx = i + (tverts[j] % 2) + width * (tverts[j] / 2); int x = iidx % width; int y = iidx / width; // 如果当前像素尚没有对应的三维点 if (vidx.at(iidx) == MATH_MAX_UINT) { /* Add vertex for depth pixel. */ vidx.at(iidx) = verts.size(); float depth = dm->at(iidx, 0); verts.push_back(pixel_3dpos(x, y, depth, invproj)); } faces.push_back(vidx.at(iidx)); } } /** * \description 给定深度图，彩色图像以及相机参数，恢复带有颜色和法向量的三维点云 * @param dm -- 深度图像 * @param ci -- 彩色图像 * @param invproj -- 相机逆投影矩阵 * @param dd_factor -- 用于判断深度一致性的阈值 * @return */ core::TriangleMesh::Ptr my_depthmap_triangulate (core::FloatImage::ConstPtr dm, core::ByteImage::ConstPtr ci, math::Matrix3f const& invproj, float dd_factor) { // 深度图像不为空 if (dm == nullptr) throw std::invalid_argument("Null depthmap given"); // 图像的宽和高 int const width = dm->width(); int const height = dm->height(); // 创建三角网格接结构体 /* Prepare triangle mesh. */ core::TriangleMesh::Ptr mesh(core::TriangleMesh::create()); /* Generate image that maps image pixels to vertex IDs. */ // 创建映射图，将图像像素映射到三维点的索引 core::Image vidx(width, height, 1); vidx.fill(MATH_MAX_UINT); // 在深度图中遍历2x2 blocks，并且创建三角面片 /* Iterate over 2x2-blocks in the depthmap and create triangles. */ int i = 0; for (int y = 0; y < height - 1; ++y, ++i) { for (int x = 0; x < width - 1; ++x, ++i) { /* Cache the four depth values. */ /* * 0, 1 * 2, 3 */ float depths[4] = { dm->at(i, 0), dm->at(i + 1, 0), dm->at(i + width, 0), dm->at(i + width + 1, 0) }; /* Create a mask representation of the available depth values. */ /* 创建mask记录深度有效的像素个数 * mask=0000, 0001, 0010, 0011, 0100, 0101, 0110, 0111, 1000, 1001, * 1010, 1011, 1100, 1101, 1110, 1111 */ int mask = 0; int pixels = 0; for (int j = 0; j < 4; ++j){ if (depths[j] > 0.0f) { mask |= 1 << j; pixels += 1; } } // 至少保证3个深度值是可靠的 /* At least three valid depth values are required. */ if (pixels < 3) continue; /* Possible triangles, vertex indices relative to 2x2 block. */ /* 可能出现的三角面片对,4个点有2个面片 */ int tris[4][3] = { { 0, 2, 1 }, { 0, 3, 1 }, { 0, 2, 3 }, { 1, 2, 3 } }; /* Decide which triangles to issue. */ /* 决定用哪对面片 */ int tri[2] = { 0, 0 }; switch (mask) { case 7: tri[0] = 1; break; // 0111- 0，1，2 case 11: tri[0] = 2; break; // 1011- 0，1，3 case 13: tri[0] = 3; break; // 1101- 0，2，3 case 14: tri[0] = 4; break; // 1110- 1，2，3 case 15: // 1111- 0，1，2，3 { // 空圆特性 /* Choose the triangulation with smaller diagonal. */ float ddiff1 = std::abs(depths[0] - depths[3]); float ddiff2 = std::abs(depths[1] - depths[2]); if (ddiff1 < ddiff2) { tri[0] = 2; tri[1] = 3; } else { tri[0] = 1; tri[1] = 4; } break; } default: continue; } /* Omit depth discontinuity detection if dd_factor is zero. */ if (dd_factor > 0.0f) { /* Cache pixel footprints. */ float widths[4]; for (int j = 0; j < 4; ++j) { if (depths[j] == 0.0f) continue; widths[j] = pixel_footprint(x + (j % 2), y + (j / 2), depths[j], invproj);// w, h, focal_len); } // 检查深度不一致性，相邻像素的深度差值不要超过像素宽度(三维空间中）的dd_factor倍 /* Check for depth discontinuities. */ for (int j = 0; j < 2 && tri[j] != 0; ++j) { int* tv = tris[tri[j] - 1]; #define DM_DD_ARGS widths, depths, dd_factor if (dm_is_depthdisc(DM_DD_ARGS, tv[0], tv[1])) tri[j] = 0; if (dm_is_depthdisc(DM_DD_ARGS, tv[1], tv[2])) tri[j] = 0; if (dm_is_depthdisc(DM_DD_ARGS, tv[2], tv[0])) tri[j] = 0; } } /* Build triangles. */ for (int j = 0; j < 2; ++j) { if (tri[j] == 0) continue; #define DM_MAKE_TRI_ARGS mesh.get(), vidx, dm.get(), invproj, i dm_make_triangle(DM_MAKE_TRI_ARGS, tris[tri[j] - 1]); } } } // 获取颜色 // 彩色图像不为空，且和深度图像宽和高一致 if (ci != nullptr && (ci->width() != width || ci->height() != height)){ std::cout<<"Color image dimension mismatch"<get_vertex_colors()); core::TriangleMesh::VertexList const& verts(mesh->get_vertices()); colors.resize(verts.size()); // 像素个数 int num_pixel = vidx.get_pixel_amount(); for (int i = 0; i < num_pixel; ++i) { // 像素没有对应的顶点 if (vidx[i] == MATH_MAX_UINT) continue; math::Vec4f color(ci->at(i, 0), 0.0f, 0.0f, 255.0f); if (ci->channels() >= 3) { color[1] = ci->at(i, 1); color[2] = ci->at(i, 2); } else { color[1] = color[2] = color[0]; } colors[vidx[i]] = color / 255.0f; } return mesh; } int main(int argc, char *argv[]){ if(argc<5){ std::cout<<"usage: scendir outmeshdir(.ply) scale view_id"<>conf.scale; // 获取要重建的视角 std::stringstream stream2(argv[4]); stream2>>conf.view_id; conf.dmname = std::string("depth-L") + argv[3]; conf.imagename = (conf.scale == 0) ? "undistorted" : std::string("undist-L") + argv[3]; std::cout << "Using depthmap \"" << conf.dmname << "\" and color image \"" << conf.imagename << "\"" << std::endl; /* Load scene. */ core::Scene::Ptr scene = core::Scene::create(conf.scenedir); /* Iterate over views and get points. */ core::Scene::ViewList& views(scene->get_views()); core::View::Ptr view = views[conf.view_id]; assert(view!= nullptr); // 获取存在相机参数 core::CameraInfo const& cam = view->get_camera(); assert(cam.flen); // 加载深度图 core::FloatImage::Ptr dm = view->get_float_image(conf.dmname); assert(dm != nullptr); // 加载彩色图像 core::ByteImage::Ptr ci; if (!conf.imagename.empty()) ci = view->get_byte_image(conf.imagename); std::cout << "Processing view \"" << view->get_name() << "\"" << (ci != nullptr ? " (with colors)" : "") << "..." << std::endl; /********************Triangulate depth map***********************/ core::TriangleMesh::Ptr mesh; // 计算投影矩阵的逆矩阵 math::Matrix3f invproj; cam.fill_inverse_calibration(*invproj, dm->width(), dm->height()); // 对深度图进行三角化，注意此时的mesh顶点坐标位于相机坐标系中 mesh = my_depthmap_triangulate(dm, ci, invproj, conf.dd_factor); /* Transform mesh to world coordinates. */ // 将网格从相机坐标系转化到世界坐标系中 math::Matrix4f ctw; cam.fill_cam_to_world(*ctw); core::geom::mesh_transform(mesh, ctw); if (conf.with_normals) mesh->ensure_normals(); dm.reset(); ci.reset(); view->cache_cleanup(); /***************************************************************/ /* Write mesh to disc. */ std::cout << "Writing final point set (" << mesh->get_vertices().size() << " points)..." << std::endl; assert(util::string::right(conf.outmesh, 4) == ".ply"); { core::geom::SavePLYOptions opts; opts.write_vertex_normals = conf.with_normals; core::geom::save_ply_mesh(mesh, conf.outmesh, opts); } return EXIT_SUCCESS; } \ No newline at end of file +// +// Created by caoqi on 2018/10/01. +// + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "core/depthmap.h" +#include "core/mesh_info.h" +#include "core/mesh_io.h" +#include "core/mesh_io_ply.h" +#include "core/mesh_tools.h" +#include "core/scene.h" +#include "core/view.h" +#include "core/depthmap.h" + + +struct AppSettings +{ + std::string scenedir; + std::string outmesh; + std::string dmname = "depth-L0"; + std::string imagename = "undistorted"; + std::string mask; + std::string aabb; + bool with_normals = true; + bool with_scale = true; + bool with_conf = true; + bool poisson_normals = false; + float min_valid_fraction = 0.0f; + float scale_factor = 2.5f; /* "Radius" of MVS patch (usually 5x5). */ + std::vector ids; + int view_id = -1; + float dd_factor = 5.0f; + int scale = 0; +}; + + +/** + * \description 给定像素坐标，深度值以及内参矩阵的逆，求对应三维点的坐标 + * @param x -- 像素坐标x + * @param y -- 像素坐标y + * @param depth --深度值即，三维点到相机中心的距离 + * @param invproj -- 内参矩阵的逆 + * @return 三维点坐标 + */ +math::Vec3f +pixel_3dpos (std::size_t x, std::size_t y, float depth, + math::Matrix3f const& invproj) { + + // 图像上的每个像素对应三维中的一条射线(向量） + math::Vec3f ray = invproj * math::Vec3f + ((float)x + 0.5f, (float)y + 0.5f, 1.0f); + + // 将射线所在的向量进行归一化，并乘以深度值，得到三维点的坐标 + return ray.normalized() * depth; +} + +/** + * \description 判断深度值是否具有一致性 + * @param widths -- 4x1 4个图像像素对应在三维空间中的宽度，可以理解成空间中长度为widths[i1]的物体 + * 投影到图像中i1位置的大小刚好为一个像素 + * @param depths -- 深度值 4个图像像素的深度值 + * @param dd_factor -- 判断深度一致性的阈值 + * 4个像素构成一个grid, 序号分别为 + * [0, 1] + * [2, 3] + * @param i1 -- i1 \in [0,3] + * @param i2 -- i2 \in [0,3] + * @return + */ +bool dm_is_depthdisc (float* widths, float* depths + , float dd_factor, int i1, int i2) { + + /* Find index that corresponds to smaller depth. */ + int i_min = i1; + int i_max = i2; + if (depths[i2] < depths[i1]) + std::swap(i_min, i_max); + + /* Check if indices are a diagonal. */ + if (i1 + i2 == 3) + dd_factor *= MATH_SQRT2; + + /* Check for depth discontinuity. */ + if (depths[i_max] - depths[i_min] > widths[i_min] * dd_factor) + return true; + + return false; +} + + +/** + * \description 计算1个图像像素在三维空间点p处的长度l，可以理解成空间点p处长度为l的物体 + * 投影到图像大小刚好为一个像素 + * @param x -- 图像像素坐标 + * @param y -- 图像像素坐标 + * @param depth -- 图像深度值 + * @param invproj -- 相机内参矩阵的逆 + * @return + */ +float +pixel_footprint (std::size_t x, std::size_t y, float depth, + math::Matrix3f const& invproj) { + math::Vec3f v = invproj * math::Vec3f + ((float)x + 0.5f, (float)y + 0.5f, 1.0f); + return invproj[0] * depth / v.norm(); +} + +/** + * \description 创建一个三角面片 + * @param mesh -- 需要创建的三维网格 + * @param vidx -- 图像像素对应三维顶点索引的映射图 + * @param dm -- 深度图像 + * @param invproj -- 相机内参矩阵的逆矩阵 + * @param i -- 像素在图像中的全局索引[0, w*h-1] + * @param tverts -- 三角面片的顶点索引，用的是在局部grid中的索引，即[0, 1] + * [2, 3] + * + */ +void +dm_make_triangle (core::TriangleMesh* mesh, core::Image& vidx, + core::FloatImage const* dm, math::Matrix3f const& invproj, + std::size_t i, int* tverts) { + + int const width = vidx.width(); + //int const height = vidx.height(); + core::TriangleMesh::VertexList& verts(mesh->get_vertices()); + core::TriangleMesh::FaceList& faces(mesh->get_faces()); + + for (int j = 0; j < 3; ++j) { + int iidx = i + (tverts[j] % 2) + width * (tverts[j] / 2); + int x = iidx % width; + int y = iidx / width; + + // 如果当前像素尚没有对应的三维点 + if (vidx.at(iidx) == MATH_MAX_UINT) { + /* Add vertex for depth pixel. */ + vidx.at(iidx) = verts.size(); + float depth = dm->at(iidx, 0); + verts.push_back(pixel_3dpos(x, y, depth, invproj)); + } + faces.push_back(vidx.at(iidx)); + } +} + +/** + * \description 给定深度图，彩色图像以及相机参数，恢复带有颜色和法向量的三维点云 + * @param dm -- 深度图像 + * @param ci -- 彩色图像 + * @param invproj -- 相机逆投影矩阵 + * @param dd_factor -- 用于判断深度一致性的阈值 + * @return + */ +core::TriangleMesh::Ptr +my_depthmap_triangulate (core::FloatImage::ConstPtr dm, core::ByteImage::ConstPtr ci, + math::Matrix3f const& invproj, float dd_factor) +{ + // 深度图像不为空 + if (dm == nullptr) + throw std::invalid_argument("Null depthmap given"); + + // 图像的宽和高 + int const width = dm->width(); + int const height = dm->height(); + + // 创建三角网格接结构体 + /* Prepare triangle mesh. */ + core::TriangleMesh::Ptr mesh(core::TriangleMesh::create()); + + /* Generate image that maps image pixels to vertex IDs. */ + // 创建映射图，将图像像素映射到三维点的索引 + core::Image vidx(width, height, 1); + vidx.fill(MATH_MAX_UINT); + + // 在深度图中遍历2x2 blocks，并且创建三角面片 + /* Iterate over 2x2-blocks in the depthmap and create triangles. */ + int i = 0; + for (int y = 0; y < height - 1; ++y, ++i) { + for (int x = 0; x < width - 1; ++x, ++i) { + /* Cache the four depth values. */ + /* + * 0, 1 + * 2, 3 + */ + float depths[4] = { dm->at(i, 0), dm->at(i + 1, 0), + dm->at(i + width, 0), dm->at(i + width + 1, 0) + }; + + /* Create a mask representation of the available depth values. */ + /* 创建mask记录深度有效的像素个数 + * mask=0000, 0001, 0010, 0011, 0100, 0101, 0110, 0111, 1000, 1001, + * 1010, 1011, 1100, 1101, 1110, 1111 + */ + int mask = 0; + int pixels = 0; + for (int j = 0; j < 4; ++j){ + if (depths[j] > 0.0f) { + mask |= 1 << j; + pixels += 1; + } + } + + // 至少保证3个深度值是可靠的 + /* At least three valid depth values are required. */ + if (pixels < 3) + continue; + + + /* Possible triangles, vertex indices relative to 2x2 block. */ + /* 可能出现的三角面片对,4个点有2个面片 + */ + int tris[4][3] = { + { 0, 2, 1 }, { 0, 3, 1 }, { 0, 2, 3 }, { 1, 2, 3 } + }; + + /* Decide which triangles to issue. */ + /* 决定用哪对面片 + */ + int tri[2] = { 0, 0 }; + + switch (mask) { + + case 7: tri[0] = 1; break; // 0111- 0，1，2 + case 11: tri[0] = 2; break; // 1011- 0，1，3 + case 13: tri[0] = 3; break; // 1101- 0，2，3 + case 14: tri[0] = 4; break; // 1110- 1，2，3 + case 15: // 1111- 0，1，2，3 + { + // 空圆特性 + /* Choose the triangulation with smaller diagonal. */ + float ddiff1 = std::abs(depths[0] - depths[3]); + float ddiff2 = std::abs(depths[1] - depths[2]); + if (ddiff1 < ddiff2) + { tri[0] = 2; tri[1] = 3; } + else + { tri[0] = 1; tri[1] = 4; } + break; + } + default: continue; + } + + /* Omit depth discontinuity detection if dd_factor is zero. */ + if (dd_factor > 0.0f) { + /* Cache pixel footprints. */ + float widths[4]; + for (int j = 0; j < 4; ++j) { + if (depths[j] == 0.0f) + continue; + widths[j] = pixel_footprint(x + (j % 2), y + (j / 2), depths[j], invproj);// w, h, focal_len); + } + + // 检查深度不一致性，相邻像素的深度差值不要超过像素宽度(三维空间中）的dd_factor倍 + /* Check for depth discontinuities. */ + for (int j = 0; j < 2 && tri[j] != 0; ++j) { + int* tv = tris[tri[j] - 1]; + #define DM_DD_ARGS widths, depths, dd_factor + if (dm_is_depthdisc(DM_DD_ARGS, tv[0], tv[1])) tri[j] = 0; + if (dm_is_depthdisc(DM_DD_ARGS, tv[1], tv[2])) tri[j] = 0; + if (dm_is_depthdisc(DM_DD_ARGS, tv[2], tv[0])) tri[j] = 0; + } + } + + /* Build triangles. */ + for (int j = 0; j < 2; ++j) { + if (tri[j] == 0) continue; + #define DM_MAKE_TRI_ARGS mesh.get(), vidx, dm.get(), invproj, i + dm_make_triangle(DM_MAKE_TRI_ARGS, tris[tri[j] - 1]); + } + } + } + + // 获取颜色 + // 彩色图像不为空，且和深度图像宽和高一致 + if (ci != nullptr && (ci->width() != width || ci->height() != height)){ + std::cout<<"Color image dimension mismatch"<get_vertex_colors()); + core::TriangleMesh::VertexList const& verts(mesh->get_vertices()); + colors.resize(verts.size()); + + // 像素个数 + int num_pixel = vidx.get_pixel_amount(); + for (int i = 0; i < num_pixel; ++i) { + // 像素没有对应的顶点 + if (vidx[i] == MATH_MAX_UINT) + continue; + + math::Vec4f color(ci->at(i, 0), 0.0f, 0.0f, 255.0f); + if (ci->channels() >= 3) { + color[1] = ci->at(i, 1); + color[2] = ci->at(i, 2); + } + else { + color[1] = color[2] = color[0]; + } + colors[vidx[i]] = color / 255.0f; + } + + return mesh; +} + + +int main(int argc, char *argv[]){ + + if(argc<5){ + std::cout<<"usage: scendir outmeshdir(.ply) scale view_id"<>conf.scale; + // 获取要重建的视角 + std::stringstream stream2(argv[4]); + stream2>>conf.view_id; + + conf.dmname = std::string("depth-L") + argv[3]; + conf.imagename = (conf.scale == 0) + ? "undistorted" + : std::string("undist-L") + argv[3]; + + std::cout << "Using depthmap \"" << conf.dmname + << "\" and color image \"" << conf.imagename << "\"" << std::endl; + + /* Load scene. */ + core::Scene::Ptr scene = core::Scene::create(conf.scenedir); + + /* Iterate over views and get points. */ + core::Scene::ViewList& views(scene->get_views()); + + core::View::Ptr view = views[conf.view_id]; + assert(view!= nullptr); + + // 获取存在相机参数 + core::CameraInfo const& cam = view->get_camera(); + assert(cam.flen); + + // 加载深度图 + core::FloatImage::Ptr dm = view->get_float_image(conf.dmname); + assert(dm != nullptr); + + // 加载彩色图像 + core::ByteImage::Ptr ci; + if (!conf.imagename.empty()) + ci = view->get_byte_image(conf.imagename); + + std::cout << "Processing view \"" << view->get_name() + << "\"" << (ci != nullptr ? " (with colors)" : "") + << "..." << std::endl; + + /********************Triangulate depth map***********************/ + + core::TriangleMesh::Ptr mesh; + // 计算投影矩阵的逆矩阵 + math::Matrix3f invproj; + cam.fill_inverse_calibration(*invproj, dm->width(), dm->height()); + + // 对深度图进行三角化，注意此时的mesh顶点坐标位于相机坐标系中 + mesh = my_depthmap_triangulate(dm, ci, invproj, conf.dd_factor); + + /* Transform mesh to world coordinates. */ + // 将网格从相机坐标系转化到世界坐标系中 + math::Matrix4f ctw; + cam.fill_cam_to_world(*ctw); + core::geom::mesh_transform(mesh, ctw); + + if (conf.with_normals) + mesh->ensure_normals(); + + dm.reset(); + ci.reset(); + view->cache_cleanup(); + /***************************************************************/ + + /* Write mesh to disc. */ + std::cout << "Writing final point set (" << mesh->get_vertices().size() << " points)..." << std::endl; + assert(util::string::right(conf.outmesh, 4) == ".ply"); + { + core::geom::SavePLYOptions opts; + opts.write_vertex_normals = conf.with_normals; + core::geom::save_ply_mesh(mesh, conf.outmesh, opts); + } + + return EXIT_SUCCESS; +} diff --git a/examples/task7/CMakeLists.txt b/examples/task7/CMakeLists.txt old mode 100644 new mode 100755 index fec33cd..8123d5f --- a/examples/task7/CMakeLists.txt +++ b/examples/task7/CMakeLists.txt @@ -24,4 +24,4 @@ set(TEXTURING_SOURCES arguments.cpp class7_texrecon.cpp) add_executable(task7_texturing ${TEXTURING_SOURCES}) -target_link_libraries(task7_texturing mvs util core texturing coldet mrf gco) +target_link_libraries(task7_texturing mvs util texturing core coldet mrf gco) diff --git a/examples/task7/class7_texrecon.cpp b/examples/task7/class7_texrecon.cpp index dfad2ec..b9e2756 100755 --- a/examples/task7/class7_texrecon.cpp +++ b/examples/task7/class7_texrecon.cpp @@ -171,7 +171,7 @@ int main(int argc, char **argv) { colors[i][2] = rand()&255; } - std::ofstream out("./examples/task7/view_selection_result.ply"); + std::ofstream out("./home/xsun/ImageBasedModellingEduV1.0/examples/data/sequence_scene/view_selection_result.ply"); assert(out.is_open()); out<<"ply"<(nn_result.dist_1st_best); -// float square_dist_2st_best = static_cast(nn_result.dist_2nd_best); -// float const square_lowe_thres = MATH_POW2(options.lowe_ratio_threshold); + float square_dist_1st_best = static_cast(nn_result.dist_1st_best); + float square_dist_2st_best = static_cast(nn_result.dist_2nd_best); + float const square_lowe_thres = MATH_POW2(options.lowe_ratio_threshold); /* */ /* 此处添加代码 */ diff --git a/texturing/global_seam_leveling.cpp b/texturing/global_seam_leveling.cpp index 8db071f..b27ab22 100755 --- a/texturing/global_seam_leveling.cpp +++ b/texturing/global_seam_leveling.cpp @@ -135,7 +135,7 @@ calculate_difference(VertexProjectionInfos const & vertex_projection_infos, /* The order is essential. */ math::Vec3f difference = color2 - color1; - assert(!isnan(difference[0]) && !isnan(difference[1]) && !isnan(difference[2])); + assert(!std::isnan(difference[0]) && !std::isnan(difference[1]) && !std::isnan(difference[2])); return difference; } diff --git a/tmp/matching_featureset.png b/tmp/matching_featureset.png index 928f5ac..bdd4171 100644 Binary files a/tmp/matching_featureset.png and b/tmp/matching_featureset.png differ