diff --git a/Triangulation/triangulation_method_backup.cpp b/Triangulation/triangulation_method_backup.cpp new file mode 100644 index 0000000..a90e10f --- /dev/null +++ b/Triangulation/triangulation_method_backup.cpp @@ -0,0 +1,419 @@ +/** + * Copyright (C) 2015 by Liangliang Nan (liangliang.nan@gmail.com) + * https://3d.bk.tudelft.nl/liangliang/ + * + * This file is part of Easy3D. If it is useful in your research/work, + * I would be grateful if you show your appreciation by citing it: + * ------------------------------------------------------------------ + * Liangliang Nan. + * Easy3D: a lightweight, easy-to-use, and efficient C++ + * library for processing and rendering 3D data. 2018. + * ------------------------------------------------------------------ + * Easy3D is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License Version 3 + * as published by the Free Software Foundation. + * + * Easy3D is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include "triangulation.h" +#include "matrix_algo.h" +#include + + +using namespace easy3d; + +namespace GEO1016_debugger { + void PrintMatrix33(const Matrix33& M) + { + std::cout << M(0, 0) << "," << M(0, 1) << "," << M(0, 2) << '\n'; + std::cout << M(1, 0) << "," << M(1, 1) << "," << M(1, 2) << '\n'; + std::cout << M(2, 0) << "," << M(2, 1) << "," << M(2, 2) << '\n'; + } + + void PrintPoints(const std::vector& points) + { + int count = 0; // display the first 10 points + for (const auto& p : points) + { + std::cout << p.x() << "," << p.y() << '\n'; + ++count; + if (count > 10)break; + } + } + + void PrintMatrix(const Matrix& M) + { + auto Nrows = M.rows(); + auto Ncols = M.cols(); + for (auto i = 0; i != Nrows; ++i) + { + for (auto j = 0; j != Ncols; ++j) + { + std::cout << M(i, j) << ","; + } + std::cout << '\n'; + } + } +} + +namespace GEO1016_A2 { + /* + * if the input is valid + * CONSTRAINTS + * (1) correspondences >= 8 + * (2) points_0.size() == points_1.size() + */ + bool isInputValid(const std::vector& points_0, const std::vector& points_1) + { + if (points_0.size() < 8 || points_1.size() < 8) // at least 8 correspondences + { + LOG(ERROR) << "insufficient correspondences\n"; + return false; + } + + if (points_0.size() != points_1.size()) // size doesn't match + { + LOG(ERROR) << "point size MUST match\n"; + return false; + } + + return true; + } + + + /* + * construct transform matrix for normalization + * explanation: + * (tx, ty): denote the center of the image + * pk(k = 1,2,...N): denote the centered pixel point(NOT original pixel point) + * dc: denote the distance of each new calculated point to the origin(0, 0) + * where dc = sqrt(SUM pk^2) + * dc divided by the number of tracked points(N), results in the average distance to the origin: + * avg = dc/N + * in order to make the average distance of a point p from the origin is equal to ¡Ì2: + * avg * s = ¡Ì2, thus s = ¡Ì2/avg, s is the scaling factor + * + * Then the transform matrix for one image is: + * s 0 -stx + * T = 0 s -sty -> T is a 3 by 3 matrix + * 0 0 1 + * + * @return: + * std::pair + * first element is the constructed transform matrix + * second element is a bool variable indicating whether the matrix is susseccfully constructed + */ + std::pair getNormalizeTransformMatrix( + const std::vector& points) + { + // elements will be returned + Matrix33 T; + bool T_valid = false; + + // transform matrix -------------------------------------------------------------- + double sumX = 0; // for calculating image's center + double sumY = 0; + const double N = static_cast(points.size()); // here N is guaranteed to be larger than 0(ifInputValid() gets executed first) + + for (const auto& p : points) + { + sumX += p.x(); + sumY += p.y(); + } + if (sumX < 1e-8 || sumY < 1e-8) // sumX or sumY is considered equal to 0 + { + LOG(ERROR) << "please check the divisor for x and y coordinates\n"; + return std::make_pair(T, T_valid); // T_valid remains false, will not trigger further process + } + + // image center: (tx, ty) + double tx = sumX / N; + double ty = sumY / N; + + // scale factor + double dc_squared = 0; + for (const auto& p : points) + dc_squared += (p.x() - tx) * (p.x() - tx) + (p.y() - ty) * (p.y() - ty); + double dc = sqrt(dc_squared); // dist from the origin(here origin should be the image center) + double avg_dc = dc / N; + if (avg_dc < 1e-8) + { + LOG(ERROR) << "please check the average distance to the origin\n"; + return std::make_pair(T, T_valid); // T_valid remains false, will not trigger further process + } + double s = sqrt(2) / avg_dc; // scale factor + + // construct the transform matrix for image_0, set the T0_flag to true + T.set_row(0, { s, 0, -s * tx }); + T.set_row(1, { 0, s, -s * ty }); + T.set_row(2, { 0, 0, 1 }); + T_valid = true; // T is successfully constructed + // transform matrix -------------------------------------------------------------- + + return std::make_pair(T, T_valid); + } + + + /* + * NormalizePoints + * apply transform matrix to 2D points + * + * @param: + * points need to be normalized, transform matrix M + * + * @return: + * std::pair, bool> + * first element is the normalized points set + * second element is a bool variable indicating whether the normalized points set is valid. + */ + std::vector NormalizePoints( + const std::vector& points, const Matrix33& T) + { + // elements will be returned + std::vector np; + np.reserve(points.size()); + + // normalize points + for (const auto& p : points) + { + Vector3D q = T * p.homogeneous(); // normalize points: q = T*p + np.push_back(q.cartesian()); // add the normalized 2d coordinates to the result vector + } + + return np; + } + + + /* + * getFundaFromSVD + * Wf = 0 + * construct W and solve it using SVD + * use the last column of V to form Fundamental matrix(F needs to be further processed) + * @return: + * std::pair, first is the F matrix, second is its validation status + */ + std::pair getFundaFromSVD( + const std::vector& normal_points_0, + const std::vector& normal_points_1) + { + // elements will be returned: Fundamental Matrix, its status + Matrix33 F; + bool F_valid = false; + // elements will be returned: Fundamental Matrix, its status + + + // initialize W matrix: Wf = 0, W: m by n matrix + int m = static_cast(normal_points_0.size()); // number of rows + int n = 9; // according to notes, number of columns = 9 + Matrix W(m, n); + + // define u, v for normalpoints_0, u_, v_ for normalpoints_1 + double u{}, v{}, u_{}, v_{}; + for (int i = 0; i != normal_points_0.size(); ++i) + { + u = normal_points_0[i].x(); v = normal_points_0[i].y(); + u_ = normal_points_1[i].x(); v_ = normal_points_1[i].y(); + W.set_row(i, { u * u_, v * u_, u_, u * v_, v * v_, v_, u, v, 1 }); + } + + + // Now matrix W (constructed by normalized points) is constructed ---------------------------- + + + // solve W using SVD + Matrix U(m, m, 0.0); // initialized with 0s + Matrix S(m, n, 0.0); // initialized with 0s + Matrix V(n, n, 0.0); // initialized with 0s + svd_decompose(W, U, S, V); + + // Form F using the last column of V + Vector vlc = V.get_column(V.cols() - 1); // get the last column of V + F.set_row(0, { vlc[0], vlc[1], vlc[2] }); + F.set_row(1, { vlc[3], vlc[4], vlc[5] }); + F.set_row(2, { vlc[6], vlc[7], vlc[8] }); + F_valid = true; + + return std::make_pair(F, F_valid); + } + + + /* + * get Fundamental Matrix + * process: + * (1)decompose F using SVD + * (2)rank-2 approximation - make S(2, 2) = 0 + * (3)denormalization - scale F such that F(2, 2) = 1.0 + * @return: + * std::pair, first is the processed F matrix, second is its validation status + */ + + +} + +/** + * TODO: Finish this function for reconstructing 3D geometry from corresponding image points. + * @return True on success, otherwise false. On success, the reconstructed 3D points must be written to 'points_3d' + * and the recovered relative pose must be written to R and t. + */ +bool Triangulation::triangulation( + double fx, double fy, /// input: the focal lengths (same for both cameras) + double cx, double cy, /// input: the principal point (same for both cameras) + const std::vector &points_0, /// input: 2D image points in the 1st image. + const std::vector &points_1, /// input: 2D image points in the 2nd image. + std::vector &points_3d, /// output: reconstructed 3D points + Matrix33 &R, /// output: 3 by 3 matrix, which is the recovered rotation of the 2nd camera + Vector3D &t /// output: 3D vector, which is the recovered translation of the 2nd camera +) const +{ + /// NOTE: there might be multiple workflows for reconstructing 3D geometry from corresponding image points. + /// This assignment uses the commonly used one explained in our lecture. + /// It is advised to define a function for the sub-tasks. This way you have a clean and well-structured + /// implementation, which also makes testing and debugging easier. You can put your other functions above + /// triangulation(), or put them in one or multiple separate files. + + std::cout << "\nTODO: I am going to implement the triangulation() function in the following file:" << std::endl + << "\t - triangulation_method.cpp\n\n"; + + std::cout << "[Liangliang]:\n" + "\tFeel free to use any provided data structures and functions. For your convenience, the\n" + "\tfollowing three files implement basic linear algebra data structures and operations:\n" + "\t - Triangulation/matrix.h Matrices of arbitrary dimensions and related functions.\n" + "\t - Triangulation/vector.h Vectors of arbitrary dimensions and related functions.\n" + "\t - Triangulation/matrix_algo.h Determinant, inverse, SVD, linear least-squares...\n" + "\tPlease refer to the above files for a complete list of useful functions and their usage.\n\n" + "\tIf you choose to implement the non-linear method for triangulation (optional task). Please\n" + "\trefer to 'Tutorial_NonlinearLeastSquares/main.cpp' for an example and some explanations.\n\n" + "\tIn your final submission, please\n" + "\t - delete ALL unrelated test or debug code and avoid unnecessary output.\n" + "\t - include all the source code (and please do NOT modify the structure of the directories).\n" + "\t - do NOT include the 'build' directory (which contains the intermediate files in a build step).\n" + "\t - make sure your code compiles and can reproduce your results without ANY modification.\n\n" << std::flush; + + /// Below are a few examples showing some useful data structures and APIs. + + /// define a 2D vector/point + Vector2D b(1.1, 2.2); + + /// define a 3D vector/point + Vector3D a(1.1, 2.2, 3.3); + + /// get the Cartesian coordinates of a (a is treated as Homogeneous coordinates) + Vector2D p = a.cartesian(); + + /// get the Homogeneous coordinates of p + Vector3D q = p.homogeneous(); + + /// define a 3 by 3 matrix (and all elements initialized to 0.0) + Matrix33 A; + + /// define and initialize a 3 by 3 matrix + Matrix33 T1(1.1, 2.2, 3.3, + 0, 2.2, 3.3, + 0, 0, 1); + + /// define and initialize a 3 by 4 matrix + Matrix34 M(1.1, 2.2, 3.3, 0, + 0, 2.2, 3.3, 1, + 0, 0, 1, 1); + + /// set first row by a vector + M.set_row(0, Vector4D(1.1, 2.2, 3.3, 4.4)); + + /// set second column by a vector + M.set_column(1, Vector3D(5.5, 5.5, 5.5)); + + /// define a 15 by 9 matrix (and all elements initialized to 0.0) + Matrix W(15, 9, 0.0); + /// set the first row by a 9-dimensional vector + W.set_row(0, {0, 1, 2, 3, 4, 5, 6, 7, 8}); // {....} is equivalent to a std::vector + + /// get the number of rows. + int num_rows = W.rows(); + + /// get the number of columns. + int num_cols = W.cols(); + + /// get the the element at row 1 and column 2 + double value = W(1, 2); + + /// get the last column of a matrix + Vector last_column = W.get_column(W.cols() - 1); + + /// define a 3 by 3 identity matrix + Matrix33 I = Matrix::identity(3, 3, 1.0); + + /// matrix-vector product + Vector3D v = M * Vector4D(1, 2, 3, 4); // M is 3 by 4 + + ///For more functions of Matrix and Vector, please refer to 'matrix.h' and 'vector.h' + + // TODO: delete all above example code in your final submission + + //-------------------------------------------------------------------------------------------------------------- + // implementation starts ... + + // TODO: check if the input is valid (always good because you never known how others will call your function). + auto valid = GEO1016_A2::isInputValid(points_0, points_1); + if (!valid)return false; + + // TODO: Estimate relative pose of two views. This can be subdivided into + // - estimate the fundamental matrix F; + // - compute the essential matrix E; + // - recover rotation R and t. + + + // get transform matrix -------------------------------------------------------------------------- + auto trans_0 = GEO1016_A2::getNormalizeTransformMatrix(points_0); + auto trans_1 = GEO1016_A2::getNormalizeTransformMatrix(points_1); + if (trans_0.second == false || trans_1.second == false) + { + LOG(ERROR) << "construct Normalize Transform Matrix fail, please check\n" + << "getNormalizeTransformMatrix() function in triangulation_method.cpp\n"; + return false; + } + Matrix33 T = trans_0.first; Matrix33 T_ = trans_1.first; + // get transform matrix -------------------------------------------------------------------------- + + + // normalize points ------------------------------------------------------------------------------ + auto normal_points_0 = GEO1016_A2::NormalizePoints(points_0, T); + auto normal_points_1 = GEO1016_A2::NormalizePoints(points_1, T_); + // normalize points ------------------------------------------------------------------------------ + + + // get fundamental matrix from Wf = 0, use SVD to solve W ---------------------------------------- + auto Funda = GEO1016_A2::getFundaFromSVD(normal_points_0, normal_points_1); + if (Funda.second == false) + { + LOG(ERROR) << "construct Fundamental Matrix from Wf=0 (solved using SVD) fail, please check\n" + << "getFundaFromSVD() function in triangulation_method.cpp\n"; + return false; + } + Matrix33 F_fromSVD = Funda.first; + // get fundamental matrix from Wf = 0, use SVD to solve W ---------------------------------------- + + GEO1016_debugger::PrintMatrix33(F_fromSVD); + + + // TODO: Reconstruct 3D points. The main task is + // - triangulate a pair of image points (i.e., compute the 3D coordinates for each corresponding point pair) + + // TODO: Don't forget to + // - write your recovered 3D points into 'points_3d' (so the viewer can visualize the 3D points for you); + // - write the recovered relative pose into R and t (the view will be updated as seen from the 2nd camera, + // which can help you check if R and t are correct). + // You must return either 'true' or 'false' to indicate whether the triangulation was successful (so the + // viewer will be notified to visualize the 3D points and update the view). + // There are a few cases you should return 'false' instead, for example: + // - function not implemented yet; + // - input not valid (e.g., not enough points, point numbers don't match); + // - encountered failure in any step. + return points_3d.size() > 0; +} \ No newline at end of file