diff --git a/modules/rgbd/src/FramePose/FramePose.h b/modules/rgbd/src/FramePose/FramePose.h new file mode 100644 index 00000000000..e164c6bd2f4 --- /dev/null +++ b/modules/rgbd/src/FramePose/FramePose.h @@ -0,0 +1,95 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ +#ifndef _H_FRAME_POSE +#define _H_FRAME_POSE +#ifdef HAVE_EIGEN +#include +#include +#include + +class FramePose +{ + +public: + FramePose() {} + + FramePose(Eigen::Matrix3d &r_mat, Eigen::Vector3d &t_vec) + { + set_values(r_mat, t_vec); + } + + void set_values(Eigen::Matrix3d &r_mat, Eigen::Vector3d &t_vec) + { + this->rmat = r_mat; + this->tvec = t_vec; + + calculate_values(); + } + + cv::Mat R; + cv::Mat t; + cv::Mat proj_mat; + Eigen::Matrix3d rmat; + Eigen::Vector3d tvec; + cv::Point3d cam_pose; + cv::Mat cam_pose_mat; + +private: + void calculate_values() + { + eigen2cv(rmat, R); + eigen2cv(tvec, t); + R.convertTo(R, CV_32F); + t.convertTo(t, CV_32F); + auto pos = -(rmat.transpose()) * tvec; + cam_pose = cv::Point3d(pos[0], pos[1], pos[2]); + + proj_mat = cv::Mat(3, 4, CV_32F); + R.copyTo(proj_mat.colRange(0, 3)); + t.copyTo(proj_mat.col(3)); + + cam_pose_mat = -R.t() * t; + } +}; + +#endif +#endif diff --git a/modules/rgbd/src/NPnP/BarrierMethodSettings.h b/modules/rgbd/src/NPnP/BarrierMethodSettings.h new file mode 100644 index 00000000000..6140cf33dd6 --- /dev/null +++ b/modules/rgbd/src/NPnP/BarrierMethodSettings.h @@ -0,0 +1,78 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ +#ifndef PNP_USING_EIGEN_LIBRARY_BARRIERMETHODSETTINGS_H +#define PNP_USING_EIGEN_LIBRARY_BARRIERMETHODSETTINGS_H +#ifdef HAVE_EIGEN +#include + +namespace NPnP +{ + class BarrierMethodSettings + { + public: + double epsilon; + int binary_search_depth; + bool verbose; + int max_inner_iterations; + double miu; + double valid_result_threshold; + + BarrierMethodSettings(double a_epsilon, int a_binary_search_depth, bool a_verbose, + int a_max_inner_iterations, double a_miu, + double a_valid_result_threshold) + : epsilon(a_epsilon), binary_search_depth(a_binary_search_depth), + verbose(a_verbose), max_inner_iterations(a_max_inner_iterations), miu(a_miu), + valid_result_threshold(a_valid_result_threshold) {} + + static std::shared_ptr + init(double epsilon = 4E-8, int binary_search_depth = 20, bool verbose = true, + int max_inner_iterations = 20, double miu = 50, + double valid_result_threshold = 0.001) + { + return std::make_shared( + epsilon, binary_search_depth, verbose, max_inner_iterations, miu, + valid_result_threshold); + } + }; +} // namespace NPnP +#endif +#endif // PNP_USING_EIGEN_LIBRARY_BARRIERMETHODSETTINGS_H diff --git a/modules/rgbd/src/NPnP/DualVar.cpp b/modules/rgbd/src/NPnP/DualVar.cpp new file mode 100644 index 00000000000..d099fa2a80a --- /dev/null +++ b/modules/rgbd/src/NPnP/DualVar.cpp @@ -0,0 +1,215 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ +#ifdef HAVE_EIGEN +#include "DualVar.h" +#include "../Utils_Npnp/Definitions.h" +#include "NPnpProblemSolver.h" +#include +#include + +namespace NPnP +{ + + Eigen::Vector4d DualVar::extract_quaternions() + { + const double eps = 1E-3; + auto q11 = y_vec[4]; + auto q12 = y_vec[5]; + auto q13 = y_vec[6]; + auto q14 = y_vec[7]; + auto q22 = y_vec[8]; + auto q23 = y_vec[9]; + auto q24 = y_vec[10]; + auto q33 = y_vec[11]; + auto q34 = y_vec[12]; + auto q44 = y_vec[13]; + double q1; + double q2; + double q3; + double q4; + + if (q11 > eps) + { + q1 = std::sqrt(q11); + q2 = q12 / q1; + q3 = q13 / q1; + q4 = q14 / q1; + } + else if (q22 > eps) + { + q1 = 0.0; + q2 = std::sqrt(q22); + q3 = q23 / q2; + q4 = q24 / q2; + } + else if (q33 > eps) + { + q1 = 0.0; + q2 = 0.0; + q3 = std::sqrt(q33); + q4 = q34 / q3; + } + else if (q44 > eps) + { + q1 = 0.0; + q2 = 0.0; + q3 = 0.0; + q4 = 1.0; + } + Eigen::Vector4d quats(q1, q2, q3, q4); + quats.normalize(); + return std::move(quats); + } + + DualVar::DualVar(ColVector a_y_vec, ColVector a_zero_vars, + ColVector a_slack, + ColMatrix a_matrix_15_15, + ColMatrix a_matrix_15_15_inv, + ColVector a_matrix_15_15_inv_vec, + Eigen::Array a_eigenvals, + const RowMatrix &A_sub_rows) + : y_vec(std::move(a_y_vec)), zero_vars(std::move(a_zero_vars)), + slack(std::move(a_slack)), matrix_15_15(std::move(a_matrix_15_15)), + matrix_15_15_inv(std::move(a_matrix_15_15_inv)), + matrix_15_15_inv_vec(std::move(a_matrix_15_15_inv_vec)), + eigenvals(std::move(a_eigenvals)) + { + this->gradient_helper_vec = Eigen::VectorXd(A_ROWS); + this->gradient_vec = Eigen::VectorXd(Y_SIZE); + this->hessian_helper_mat = Eigen::MatrixXd::Zero(A_ROWS, A_ROWS); + this->hessian_mat = Eigen::MatrixXd::Zero(Y_SIZE, Y_SIZE); + this->hessian_multiplication_mat = + Eigen::Matrix( + Y_SIZE, A_ROWS); + this->equation_result = Eigen::VectorXd(Y_SIZE + NUM_CONSTRAINTS); + this->res_vec_84 = Eigen::VectorXd::Zero(Y_SIZE + NUM_CONSTRAINTS); + this->temp_vec_69 = Eigen::VectorXd::Zero(Y_SIZE); + + this->equation_mat = + Eigen::MatrixXd::Zero(Y_SIZE + NUM_CONSTRAINTS, Y_SIZE + NUM_CONSTRAINTS); + this->equation_mat.block(0, Y_SIZE, Y_SIZE, NUM_CONSTRAINTS) = + A_sub_rows.transpose(); + this->equation_mat.block(Y_SIZE, 0, NUM_CONSTRAINTS, Y_SIZE) = A_sub_rows; + } + + Eigen::VectorXd *DualVar::cone_barrier_gradient(const PnpProblemSolver &pnp) + { + gradient_helper_vec.block(0, 0).setZero(); + gradient_helper_vec.block( + NUM_CONSTRAINTS, 0) = this->matrix_15_15_inv_vec; + gradient_vec.noalias() = pnp.A_cols.transpose() * gradient_helper_vec; + return &gradient_vec; + } + + Eigen::MatrixXd *DualVar::cone_barrier_hessian(const PnpProblemSolver &pnp) + { + + hessian_helper_mat + .block(NUM_CONSTRAINTS, NUM_CONSTRAINTS, A_ROWS - NUM_CONSTRAINTS, + A_ROWS - NUM_CONSTRAINTS) + .noalias() = matrix_15_15_inv_vec * matrix_15_15_inv_vec.transpose(); + + auto cop = hessian_helper_mat.eval(); + + for (int i = 0; i < M_MATRIX_DIM; i++) + for (int j = 0; j < M_MATRIX_DIM; j++) + if (i != j) + hessian_helper_mat + .block(M_MATRIX_DIM + i * M_MATRIX_DIM, + M_MATRIX_DIM + j * M_MATRIX_DIM, M_MATRIX_DIM, M_MATRIX_DIM) + .transposeInPlace(); + + hessian_multiplication_mat.noalias() = + pnp.A_cols.transpose() * hessian_helper_mat; + hessian_mat.noalias() = hessian_multiplication_mat * pnp.A_cols; + + return &hessian_mat; + } + + std::shared_ptr + DualVar::init(ColVector y_vec, const SparseRowMatrix &A_mat_rows, + const RowMatrix &A_sub_rows, + const SparseColMatrix &c_vec) + { + ColVector slack = (c_vec - A_mat_rows * y_vec).toDense(); + Eigen::Map> zero_part(slack.data(), + NUM_CONSTRAINTS); + Eigen::Map> matrix_15_15( + slack.data() + NUM_CONSTRAINTS, M_MATRIX_DIM, M_MATRIX_DIM); + for (int i = 0; i < M_MATRIX_DIM; i++) + matrix_15_15(i, i) += 0.00000001; + auto matrix_15_15_inv = matrix_15_15.inverse().eval(); + Eigen::Map> matrix_15_15_inv_vec( + matrix_15_15_inv.data(), M_MATRIX_DIM * M_MATRIX_DIM); + auto eigenvals = matrix_15_15.selfadjointView().eigenvalues(); + + return std::make_shared(std::move(y_vec), zero_part, slack, + matrix_15_15, matrix_15_15_inv, + matrix_15_15_inv_vec, eigenvals, A_sub_rows); + } + + void DualVar::set_y_vec(ColVector new_y_vec, + const SparseRowMatrix &A_mat_rows, + const SparseColMatrix &c_vec) + { + this->y_vec = std::move(new_y_vec); + this->slack = (c_vec - A_mat_rows * y_vec).toDense(); + + Eigen::Map> new_zero_vars(slack.data(), + NUM_CONSTRAINTS); + Eigen::Map> new_matrix_15_15( + slack.data() + NUM_CONSTRAINTS, M_MATRIX_DIM, M_MATRIX_DIM); + for (int i = 0; i < M_MATRIX_DIM; i++) + { + new_matrix_15_15(i, i) += 0.00000001; + } + this->matrix_15_15 = new_matrix_15_15; + this->zero_vars = new_zero_vars; + this->matrix_15_15_inv = new_matrix_15_15.inverse(); + + Eigen::Map> new_matrix_15_15_inv_vec( + matrix_15_15_inv.data(), M_MATRIX_DIM * M_MATRIX_DIM); + this->matrix_15_15_inv_vec = new_matrix_15_15_inv_vec; + this->eigenvals = matrix_15_15.selfadjointView().eigenvalues(); + } +} // namespace NPnP +#endif \ No newline at end of file diff --git a/modules/rgbd/src/NPnP/DualVar.h b/modules/rgbd/src/NPnP/DualVar.h new file mode 100644 index 00000000000..889681c0306 --- /dev/null +++ b/modules/rgbd/src/NPnP/DualVar.h @@ -0,0 +1,112 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ +#ifndef PNP_USING_EIGEN_LIBRARY_DUALVAR_H +#define PNP_USING_EIGEN_LIBRARY_DUALVAR_H +#ifdef HAVE_EIGEN +#include + +#include + +#include "BarrierMethodSettings.h" +#include "../Utils_Npnp/Definitions.h" +#include "NPnpObjective.h" + +namespace NPnP +{ + class PnpProblemSolver; + + class DualVar + { + public: + ColVector y_vec; + ColVector zero_vars; + ColVector slack; + ColMatrix matrix_15_15; + ColMatrix matrix_15_15_inv; + ColVector matrix_15_15_inv_vec; + Eigen::Array eigenvals; + + Eigen::VectorXd gradient_helper_vec; + Eigen::VectorXd gradient_vec; + Eigen::MatrixXd hessian_helper_mat; + Eigen::MatrixXd hessian_mat; + Eigen::MatrixXd equation_mat; + Eigen::VectorXd equation_result; + + Eigen::VectorXd res_vec_84; + Eigen::VectorXd temp_vec_69; + + ColMatrix temp_col_1; + ColMatrix temp_col_2; + RowMatrix temp_row_1; + RowMatrix temp_row_2; + ColVector temp_vec; + ColVector B_vec; + Eigen::Matrix + hessian_multiplication_mat; + + DualVar(ColVector y_vec, ColVector zero_vars, + ColVector slack, + ColMatrix matrix_15_15, + ColMatrix matrix_15_15_inv, + ColVector matrix_15_15_inv_vec, + Eigen::Array eigenvals, + const RowMatrix &A_sub_rows); + + static std::shared_ptr + init(ColVector y_vec, const SparseRowMatrix &A_mat_rows, + const RowMatrix &A_sub_rows, + const SparseColMatrix &c_vec); + + void set_y_vec(ColVector y_vec, const SparseRowMatrix &A_mat_rows, + const SparseColMatrix &c_vec); + + Eigen::VectorXd *cone_barrier_gradient(const PnpProblemSolver &pnp); + + Eigen::MatrixXd *cone_barrier_hessian(const PnpProblemSolver &pnp); + + Eigen::Vector4d extract_quaternions(); + }; +} // namespace PnP + +#endif +#endif // PNP_USING_EIGEN_LIBRARY_DUALVAR_H diff --git a/modules/rgbd/src/NPnP/LogDetHessianMatrix.cpp b/modules/rgbd/src/NPnP/LogDetHessianMatrix.cpp new file mode 100644 index 00000000000..35ddad1eee3 --- /dev/null +++ b/modules/rgbd/src/NPnP/LogDetHessianMatrix.cpp @@ -0,0 +1,54 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ +#ifdef HAVE_EIGEN +#include "LogDetHessianMatrix.h" + +namespace NPnP +{ + Eigen::CwiseNullaryOp + make_log_det_hessian_matrix(const inv_mat_15_15_type &inv_mat) + { + return Eigen::MatrixXd::NullaryExpr(A_ROWS, A_ROWS, + log_det_hessian_matrix_functor(inv_mat)); + } +} // namespace NPnP +#endif \ No newline at end of file diff --git a/modules/rgbd/src/NPnP/LogDetHessianMatrix.h b/modules/rgbd/src/NPnP/LogDetHessianMatrix.h new file mode 100644 index 00000000000..0620388643a --- /dev/null +++ b/modules/rgbd/src/NPnP/LogDetHessianMatrix.h @@ -0,0 +1,77 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ +#ifndef PNP_USING_EIGEN_LIBRARY_LOGDETHESSIANMATRIX_H +#define PNP_USING_EIGEN_LIBRARY_LOGDETHESSIANMATRIX_H +#ifdef HAVE_EIGEN +#include "../Utils_Npnp/Definitions.h" + +namespace NPnP +{ + + typedef ColMatrix inv_mat_15_15_type; + + class log_det_hessian_matrix_functor + { + const inv_mat_15_15_type &inv_mat; + + public: + explicit log_det_hessian_matrix_functor(const inv_mat_15_15_type &invmat) + : inv_mat(invmat) {} + + double operator()(int row, int col) const + { + if (row <= M_MATRIX_DIM || col <= M_MATRIX_DIM) + return 0; + row -= M_MATRIX_DIM; + col -= M_MATRIX_DIM; + int row1 = row / M_MATRIX_DIM; + int col1 = row % M_MATRIX_DIM; + int row2 = col / M_MATRIX_DIM; + int col2 = col % M_MATRIX_DIM; + return inv_mat(row1, col2) * inv_mat(col1, row2); + } + }; + Eigen::CwiseNullaryOp + make_log_det_hessian_matrix(const inv_mat_15_15_type &inv_mat); +} // namespace NPnP +#endif +#endif // PNP_USING_EIGEN_LIBRARY_LOGDETHESSIANMATRIX_H diff --git a/modules/rgbd/src/NPnP/NPnPRansac.cpp b/modules/rgbd/src/NPnP/NPnPRansac.cpp new file mode 100644 index 00000000000..69b9eb6a5ea --- /dev/null +++ b/modules/rgbd/src/NPnP/NPnPRansac.cpp @@ -0,0 +1,104 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ +#ifdef HAVE_EIGEN +#include "../precomp.hpp" + +using namespace cv; + +class OptimalPnPRansacCallback : public PointSetRegistrator::Callback +{ + +public: + OptimalPnPRansacCallback(Mat _cameraMatrix = Mat(3, 3, CV_64F)) + : cameraMatrix(_cameraMatrix) {} + + /* Pre: True */ + /* Post: compute _model with given points and return number of found models */ + int runKernel(InputArray scene, InputArray obj, + OutputArray _model) const CV_OVERRIDE + { + Mat opoints = scene.getMat(), ipoints = obj.getMat(); + + bool correspondence = solvePnP(scene, obj, cameraMatrix, distCoeffs, rvec, + tvec, useExtrinsicGuess, flags); + + Mat _local_model; + hconcat(rvec, tvec, _local_model); + _local_model.copyTo(_model); + + return correspondence; + } + + /* Pre: True */ + /* Post: fill _err with projection errors */ + void computeError(InputArray _m1, InputArray _m2, InputArray _model, + OutputArray _err) const CV_OVERRIDE + { + + Mat opoints = _m1.getMat(), ipoints = _m2.getMat(), model = _model.getMat(); + + int i, count = opoints.checkVector(3); + Mat _rvec = model.col(0); + Mat _tvec = model.col(1); + + Mat projpoints(count, 2, CV_32FC1); + projectPoints(opoints, _rvec, _tvec, cameraMatrix, distCoeffs, projpoints); + + const Point2f *ipoints_ptr = ipoints.ptr(); + const Point2f *projpoints_ptr = projpoints.ptr(); + + _err.create(count, 1, CV_32FC1); + float *err = _err.getMat().ptr(); + + for (i = 0; i < count; ++i) + err[i] = + (float)norm(Matx21f(ipoints_ptr[i] - projpoints_ptr[i]), NORM_L2SQR); + } + + Mat cameraMatrix; + Mat distCoeffs; + int flags; + bool useExtrinsicGuess; + Mat rvec; + Mat tvec; +}; +#endif \ No newline at end of file diff --git a/modules/rgbd/src/NPnP/NPnpBarrierMethodSolver.cpp b/modules/rgbd/src/NPnP/NPnpBarrierMethodSolver.cpp new file mode 100644 index 00000000000..36329a5a233 --- /dev/null +++ b/modules/rgbd/src/NPnP/NPnpBarrierMethodSolver.cpp @@ -0,0 +1,231 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ +#ifdef HAVE_EIGEN +#include "../Utils_Npnp/Definitions.h" +#include "DualVar.h" +#include "../Utils_Npnp/GeneralUtils.h" +#include "NPnpProblemSolver.h" +#include "NPnpResult.h" +#include +#include +#include +#include +#include +#include +#include + +namespace NPnP +{ + + double calc_cost(Eigen::Vector4d quats, + std::shared_ptr objective) + { + auto q1 = quats.x(); + auto q2 = quats.y(); + auto q3 = quats.z(); + auto q4 = quats.w(); + auto q11 = q1 * q1; + auto q12 = q1 * q2; + auto q13 = q1 * q3; + auto q14 = q1 * q4; + auto q22 = q2 * q2; + auto q23 = q2 * q3; + auto q24 = q2 * q4; + auto q33 = q3 * q3; + auto q34 = q3 * q4; + auto q44 = q4 * q4; + ColVector<9> rotation; + rotation << q11 + q22 - q33 - q44, 2 * q23 + 2 * q14, 2 * q24 - 2 * q13, + 2 * q23 - 2 * q14, q11 + q33 - q22 - q44, 2 * q34 + 2 * q12, + 2 * q24 + 2 * q13, 2 * q34 - 2 * q12, q11 + q44 - q22 - q33; + auto sum_cost = (rotation.transpose() * objective->M * rotation).eval()(0, 0); + return sum_cost / objective->sum_weights; + } + + void print_vec(Eigen::Vector4d vec) + { + std::cout << vec.x() << " "; + std::cout << vec.y() << " "; + std::cout << vec.z() << " "; + std::cout << vec.w() << " "; + } + + void PnpProblemSolver::line_search( + const ColVector &delta_y, + const std::function &)> &obj_func, + std::shared_ptr barrier_settings) + { + dual_var->B_vec.noalias() = -A_rows * delta_y; + Eigen::Map> B_15_15( + dual_var->B_vec.data() + NUM_CONSTRAINTS, M_MATRIX_DIM, M_MATRIX_DIM); + + double eigs_pow_minus1[M_MATRIX_DIM]; + Eigen::LLT>> + llt_of_matrix_15_15(dual_var->matrix_15_15); + dual_var->temp_col_1.noalias() = + llt_of_matrix_15_15.matrixL().toDenseMatrix(); + dual_var->temp_row_2.noalias() = + dual_var->temp_col_1.inverse(); // temp_row_2 = L^-1 + dual_var->temp_row_1.noalias() = dual_var->temp_row_2 * B_15_15; + dual_var->temp_col_2.noalias() = + dual_var->temp_row_1 * dual_var->temp_row_2.transpose(); + dual_var->temp_vec.noalias() = + dual_var->temp_col_2.selfadjointView().eigenvalues(); + auto &eigs = dual_var->temp_vec; + for (int i = 0; i < M_MATRIX_DIM; i++) + eigs_pow_minus1[i] = 1.0 / eigs(i, 0); + auto const_val = obj_func(delta_y); + std::function line_search_gradient = + [const_val, &eigs_pow_minus1](double t) + { + double res = const_val; + for (double eig : eigs_pow_minus1) + res -= 1.0 / (eig + t); + return res; + }; + auto min_val = 1.0; + for (int i = 0; i < M_MATRIX_DIM; i++) + { + auto eig = eigs(i, 0); + if (eig < 0.0) + min_val = min2(min_val, 0.9999 * (-1.0 / eig)); + } + auto result = find_zero_bin_search(line_search_gradient, 0.0, min_val, + barrier_settings->binary_search_depth); + dual_var->set_y_vec(dual_var->y_vec + result * delta_y, A_rows, c_vec); + } + + void PnpProblemSolver::centering_step( + const std::function &)> &obj_func, + const std::function &gen_grad, + const std::function &gen_hess, + int outer_iteration, bool &is_last_iter, PnpResult &pnp_result, + std::shared_ptr pnp_objective, + std::shared_ptr barrier_method_settings) + { + int inner_iteration = 1; + + while (true) + { + auto &y = *(this->dual_var); + auto &mat = y.equation_mat; + auto &const_vec = y.equation_result; + auto &res = y.res_vec_84; + auto &temp = y.temp_vec_69; + + auto &hessian = gen_hess(y); + auto &gradient = gen_grad(y); + + mat.block(0, 0, Y_SIZE, Y_SIZE) = hessian; + const_vec.block(0, 0, Y_SIZE, 1) = -gradient; + const_vec.block(Y_SIZE, 0, NUM_CONSTRAINTS, 1) = y.zero_vars; + res.noalias() = mat.partialPivLu().solve(const_vec).eval(); + auto delta_y = res.head(); + temp.noalias() = delta_y.transpose() * hessian; + double distance_to_optimal = temp.dot(delta_y); + this->line_search(delta_y, obj_func, barrier_method_settings); + + if ((inner_iteration >= barrier_method_settings->max_inner_iterations) || + (distance_to_optimal < 1.0)) + { + + auto current_quats = dual_var->extract_quaternions(); + PnpResult current_result(current_quats, pnp_objective); + if (current_result.cost() < pnp_result.cost()) + pnp_result = current_result; + current_result = + PnpResult(this->perform_local_search(pnp_objective, current_quats), + pnp_objective); + if (current_result.cost() < pnp_result.cost()) + pnp_result = current_result; + break; + } + inner_iteration++; + } + } + + PnpResult PnpProblemSolver::solve_pnp( + std::shared_ptr pnp_objective, + std::shared_ptr barrier_method_settings) + { + double t = 1000; // 1/t is the barrier weight + bool is_last_iter = false; + Eigen::VectorXd gradient = Eigen::VectorXd::Zero(Y_SIZE); + + std::function &)> obj_func = + [&pnp_objective, &t](const ColVector &y_vec) -> double + { + return -t * pnp_objective->b.tail<35>().dot( + y_vec.tail<35>()); // first 34 values of b are zeros + }; + std::function calc_grad = + [this, &gradient, &pnp_objective, + &t](DualVar &y) -> const Eigen::VectorXd & + { + gradient.noalias() = + -t * pnp_objective->b.transpose() + (*(y.cone_barrier_gradient(*this))); + return gradient; + }; + std::function calc_hess = + [this, &pnp_objective, &t](DualVar &y) -> const Eigen::MatrixXd & + { + return *(y.cone_barrier_hessian(*this)); + }; + + dual_var = DualVar::init(init_y, A_rows, zero_sub_A_rows, c_vec); + + PnpResult pnp_result(dual_var->extract_quaternions(), pnp_objective); + + for (int outer_iteration = 1; !is_last_iter; outer_iteration++) + { + is_last_iter = t * barrier_method_settings->miu > + 1.0 / barrier_method_settings->epsilon; + this->centering_step(obj_func, calc_grad, calc_hess, outer_iteration, + is_last_iter, pnp_result, pnp_objective, + barrier_method_settings); + t *= barrier_method_settings->miu; + } + + return pnp_result; + } +} // namespace NPnP +#endif diff --git a/modules/rgbd/src/NPnP/NPnpInput.cpp b/modules/rgbd/src/NPnP/NPnpInput.cpp new file mode 100644 index 00000000000..0f1ad88ddf9 --- /dev/null +++ b/modules/rgbd/src/NPnP/NPnpInput.cpp @@ -0,0 +1,104 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ +#ifdef HAVE_EIGEN +#include "NPnpInput.h" +#include "../Utils_Npnp/Parsing.h" +#include +namespace NPnP +{ + std::shared_ptr PnpInput::parse_input(int argc, char **argv) + { + if (argc != 5) + { + std::cout << "Expected 4 command line arguements:" << std::endl; + std::cout << "\t1) points.csv path" << std::endl; + std::cout << "\t2) lines.csv path" << std::endl; + std::cout << "\t3) weights.csv path" << std::endl; + std::cout << "\t4) indices.csv path" << std::endl; + exit(1); + } + auto points_path = argv[1]; + auto lines_path = argv[2]; + auto weights_path = argv[3]; + auto indices_path = argv[4]; + + auto points = parse_csv_vector_3d(points_path); + auto lines = parse_csv_vector_3d(lines_path); + auto weights = parse_csv_array(weights_path); + auto indices = parse_csv_array(indices_path); + + if (points.size() != lines.size()) + { + std::cout << "points amount = " << points.size() + << " whereas lines amount = " << lines.size() << std::endl; + exit(1); + } + + if (indices.size() != weights.size()) + { + std::cout << "weights amount = " << weights.size() + << " whereas indices amount = " << indices.size() << std::endl; + exit(1); + } + + return PnpInput::init(points, lines, weights, indices); + } + + PnpInput::PnpInput(std::vector a_points, + std::vector a_lines, + std::vector a_weights, std::vector a_indices, + int a_indices_amount, int a_points_amount) + : points(std::move(a_points)), lines(std::move(a_lines)), + weights(std::move(a_weights)), indices(std::move(a_indices)), + indices_amount(a_indices_amount), points_amount(a_points_amount) {} + + std::shared_ptr PnpInput::init(std::vector a_points, + std::vector a_lines, + std::vector a_weights, + std::vector a_indices) + { + return std::make_shared(std::move(a_points), std::move(a_lines), + std::move(a_weights), std::move(a_indices), + a_indices.size(), a_points.size()); + } +} // namespace NPnP +#endif \ No newline at end of file diff --git a/modules/rgbd/src/NPnP/NPnpInput.h b/modules/rgbd/src/NPnP/NPnpInput.h new file mode 100644 index 00000000000..d3b247afca7 --- /dev/null +++ b/modules/rgbd/src/NPnP/NPnpInput.h @@ -0,0 +1,74 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ +#ifndef PNP_USING_EIGEN_LIBRARY_PNPINPUT_H +#define PNP_USING_EIGEN_LIBRARY_PNPINPUT_H +#ifdef HAVE_EIGEN +#include "../Utils_Npnp/Definitions.h" +#include + +namespace NPnP +{ + class PnpInput + { + + public: + std::vector points; + std::vector lines; + std::vector weights; + std::vector indices; + int indices_amount; + int points_amount; + + PnpInput(std::vector a_points, + std::vector a_lines, std::vector a_weights, + std::vector a_indices, int a_indices_amount, int a_points_amount); + + static std::shared_ptr init(std::vector points, + std::vector lines, + std::vector weights, + std::vector indices); + + static std::shared_ptr parse_input(int argc, char **pString); + }; +} // namespace NPnP +#endif +#endif // PNP_USING_EIGEN_LIBRARY_PNPINPUT_H diff --git a/modules/rgbd/src/NPnP/NPnpLocalSearch.cpp b/modules/rgbd/src/NPnP/NPnpLocalSearch.cpp new file mode 100644 index 00000000000..ad020770eac --- /dev/null +++ b/modules/rgbd/src/NPnP/NPnpLocalSearch.cpp @@ -0,0 +1,98 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ +#ifdef HAVE_EIGEN +#include "../Utils_Npnp/Definitions.h" +#include "NPnpProblemSolver.h" +#include "QuaternionVector.h" +#include +#include +#include + +namespace NPnP +{ + + Eigen::Vector4d PnpProblemSolver::perform_local_search( + std::shared_ptr pnp_objective, Eigen::Vector4d quats) + { + const auto eps = 0.0000001; + auto t = 10.0; + auto mu = 1.5; + + std::function func = + [&t, pnp_objective](const QuaternionVector &q) -> double + { + double barrier_func = (1 - q.q_norm_squared) * (1 - q.q_norm_squared); + return q.obj_func(pnp_objective->b) + t * barrier_func; + }; + std::function grad = + [&t, pnp_objective](const QuaternionVector &q) -> Eigen::Vector4d + { + Eigen::Vector4d barrier_grad = -2 * (1 - q.q_norm_squared) * q.quaternions; + return q.obj_grad(pnp_objective->b) + t * barrier_grad; + }; + std::function hess = + [&t, pnp_objective](const QuaternionVector &q) -> Eigen::Matrix4d + { + Eigen::Matrix4d barrier_hess = q.quaternions * q.quaternions.transpose(); + barrier_hess *= 4; + barrier_hess -= 2 * (1 - q.q_norm_squared) * Eigen::Matrix4d::Identity(); + barrier_hess *= t; + barrier_hess += q.obj_hess(pnp_objective->b); + return barrier_hess; + }; + QuaternionVector q(quats); + while (t < 1.0 / eps) + { + Eigen::Matrix4d h = hess(q); + Eigen::Vector4d g = grad(q); + Eigen::Matrix4d h_inv = h.inverse(); + auto newton_step = -h_inv * g; + + Eigen::Vector4d new_q = q.quaternions + newton_step; + new_q.normalize(); + q = QuaternionVector(new_q); + t *= mu; + } + return q.quaternions; + } +} // namespace NPnP +#endif diff --git a/modules/rgbd/src/NPnP/NPnpObjective.cpp b/modules/rgbd/src/NPnP/NPnpObjective.cpp new file mode 100644 index 00000000000..c4527ae2241 --- /dev/null +++ b/modules/rgbd/src/NPnP/NPnpObjective.cpp @@ -0,0 +1,236 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ +#ifdef HAVE_EIGEN +#include "NPnpObjective.h" +#include "../Utils_Npnp/Definitions.h" +#include "../Utils_Npnp/MatrixUtils.h" +#include + +namespace NPnP +{ + void PnpObjective::set_C(ColMatrix<3, 9> &C, Eigen::Vector3d point) + { + C(0, 0) = C(1, 3) = C(2, 6) = point.x(); + C(0, 1) = C(1, 4) = C(2, 7) = point.y(); + C(0, 2) = C(1, 5) = C(2, 8) = point.z(); + } + + std::shared_ptr + PnpObjective::init(std::shared_ptr input) + { + PnpObjective objective; + objective.M.setZero(); + objective.T.setZero(); + objective.b.setZero(); + objective.sum_weights = 0.0; + + ColMatrix<3, 9> C = ColMatrix<3, 9>::Zero(); + ColMatrix<3, 9> temp_sum = ColMatrix<3, 9>::Zero(); + RowMatrix<3, 3> eye3 = RowMatrix<3, 3>::Identity(); + RowMatrix<3, 3> Q_sum = RowMatrix<3, 3>::Zero(); + RowMatrix<3, 3> q; + std::vector> Q; + + for (int i = 0; i < input->indices_amount; i++) + { + auto index = input->indices[i]; + auto weight = input->weights[i]; + auto point = input->points[index]; + auto line = input->lines[index]; + objective.sum_weights += weight; + + PnpObjective::set_C(C, point); + q.noalias() = line * line.transpose() / line.squaredNorm(); + q -= eye3; + auto temp = q * q; + q = weight * temp; + Q_sum += q; + temp_sum.noalias() += q * C; + Q.emplace_back(q); + } + symmetrize(Q_sum); + RowMatrix<3, 3> Q_sum_inv = Q_sum.inverse(); + objective.T.noalias() = -Q_sum_inv * temp_sum; + + ColMatrix<3, 9> current; + for (int i = 0; i < input->indices_amount; i++) + { + auto index = input->indices[i]; + auto point = input->points[index]; + PnpObjective::set_C(C, point); + current = C + objective.T; + auto temp = current.transpose() * Q[i]; + objective.M.noalias() += temp * current; + } + symmetrize(objective.M); + auto &M = objective.M; + auto &b = objective.b; + b(34) = M(0, 0) + M(0, 4) + M(0, 8) + M(4, 0) + M(4, 4) + M(4, 8) + M(8, 0) + + M(8, 4) + M(8, 8); // q_1 q_1 q_1 q_1 + b(35) = 2 * M(0, 5) - 2 * M(0, 7) + 2 * M(4, 5) - 2 * M(4, 7) + 2 * M(5, 0) + + 2 * M(5, 4) + 2 * M(5, 8) - 2 * M(7, 0) - 2 * M(7, 4) - 2 * M(7, 8) + + 2 * M(8, 5) - 2 * M(8, 7); // q_1 q_1 q_1 q_2 + b(36) = -2 * M(0, 2) + 2 * M(0, 6) - 2 * M(2, 0) - 2 * M(2, 4) - 2 * M(2, 8) - + 2 * M(4, 2) + 2 * M(4, 6) + 2 * M(6, 0) + 2 * M(6, 4) + 2 * M(6, 8) - + 2 * M(8, 2) + 2 * M(8, 6); // q_1 q_1 q_1 q_3 + b(37) = 2 * M(0, 1) - 2 * M(0, 3) + 2 * M(1, 0) + 2 * M(1, 4) + 2 * M(1, 8) - + 2 * M(3, 0) - 2 * M(3, 4) - 2 * M(3, 8) + 2 * M(4, 1) - 2 * M(4, 3) + + 2 * M(8, 1) - 2 * M(8, 3); // q_1 q_1 q_1 q_4 + b(38) = 2 * M(0, 0) - 2 * M(4, 4) - 2 * M(4, 8) + 4 * M(5, 5) - 4 * M(5, 7) - + 4 * M(7, 5) + 4 * M(7, 7) - 2 * M(8, 4) - + 2 * M(8, 8); // q_1 q_1 q_2 q_2 + b(39) = 2 * M(0, 1) + 2 * M(0, 3) + 2 * M(1, 0) + 2 * M(1, 4) + 2 * M(1, 8) - + 4 * M(2, 5) + 4 * M(2, 7) + 2 * M(3, 0) + 2 * M(3, 4) + 2 * M(3, 8) + + 2 * M(4, 1) + 2 * M(4, 3) - 4 * M(5, 2) + 4 * M(5, 6) + 4 * M(6, 5) - + 4 * M(6, 7) + 4 * M(7, 2) - 4 * M(7, 6) + 2 * M(8, 1) + + 2 * M(8, 3); // q_1 q_1 q_2 q_3 + b(40) = +2 * M(0, 2) + 2 * M(0, 6) + 4 * M(1, 5) - 4 * M(1, 7) + 2 * M(2, 0) + + 2 * M(2, 4) + 2 * M(2, 8) - 4 * M(3, 5) + 4 * M(3, 7) + 2 * M(4, 2) + + 2 * M(4, 6) + 4 * M(5, 1) - 4 * M(5, 3) + 2 * M(6, 0) + 2 * M(6, 4) + + 2 * M(6, 8) - 4 * M(7, 1) + 4 * M(7, 3) + 2 * M(8, 2) + + 2 * M(8, 6); // q_1 q_1 q_2 q_4 + b(41) = -2 * M(0, 0) - 2 * M(0, 8) + 4 * M(2, 2) - 4 * M(2, 6) + 2 * M(4, 4) - + 4 * M(6, 2) + 4 * M(6, 6) - 2 * M(8, 0) - + 2 * M(8, 8); // q_1 q_1 q_3 q_3 + b(42) = 2 * M(0, 5) + 2 * M(0, 7) - 4 * M(1, 2) + 4 * M(1, 6) - 4 * M(2, 1) + + 4 * M(2, 3) + 4 * M(3, 2) - 4 * M(3, 6) + 2 * M(4, 5) + 2 * M(4, 7) + + 2 * M(5, 0) + 2 * M(5, 4) + 2 * M(5, 8) + 4 * M(6, 1) - 4 * M(6, 3) + + 2 * M(7, 0) + 2 * M(7, 4) + 2 * M(7, 8) + 2 * M(8, 5) + + 2 * M(8, 7); // q_1 q_1 q_3 q_4 + b(43) = -2 * M(0, 0) - 2 * M(0, 4) + 4 * M(1, 1) - 4 * M(1, 3) - 4 * M(3, 1) + + 4 * M(3, 3) - 2 * M(4, 0) - 2 * M(4, 4) + + 2 * M(8, 8); // q_1 q_1 q_4 q_4 + b(44) = 2 * M(0, 5) - 2 * M(0, 7) - 2 * M(4, 5) + 2 * M(4, 7) + 2 * M(5, 0) - + 2 * M(5, 4) - 2 * M(5, 8) - 2 * M(7, 0) + 2 * M(7, 4) + 2 * M(7, 8) - + 2 * M(8, 5) + 2 * M(8, 7); // q_1 q_2 q_2 q_2 + b(45) = -2 * M(0, 2) + 2 * M(0, 6) + 4 * M(1, 5) - 4 * M(1, 7) - 2 * M(2, 0) + + 2 * M(2, 4) + 2 * M(2, 8) + 4 * M(3, 5) - 4 * M(3, 7) + 2 * M(4, 2) - + 2 * M(4, 6) + 4 * M(5, 1) + 4 * M(5, 3) + 2 * M(6, 0) - 2 * M(6, 4) - + 2 * M(6, 8) - 4 * M(7, 1) - 4 * M(7, 3) + 2 * M(8, 2) - + 2 * M(8, 6); // q_1 q_2 q_2 q_3 + b(46) = 2 * M(0, 1) - 2 * M(0, 3) + 2 * M(1, 0) - 2 * M(1, 4) - 2 * M(1, 8) + + 4 * M(2, 5) - 4 * M(2, 7) - 2 * M(3, 0) + 2 * M(3, 4) + 2 * M(3, 8) - + 2 * M(4, 1) + 2 * M(4, 3) + 4 * M(5, 2) + 4 * M(5, 6) + 4 * M(6, 5) - + 4 * M(6, 7) - 4 * M(7, 2) - 4 * M(7, 6) - 2 * M(8, 1) + + 2 * M(8, 3); // q_1 q_2 q_2 q_4 + b(47) = -2 * M(0, 5) + 2 * M(0, 7) - 4 * M(1, 2) + 4 * M(1, 6) - 4 * M(2, 1) - + 4 * M(2, 3) - 4 * M(3, 2) + 4 * M(3, 6) + 2 * M(4, 5) - 2 * M(4, 7) - + 2 * M(5, 0) + 2 * M(5, 4) - 2 * M(5, 8) + 4 * M(6, 1) + 4 * M(6, 3) + + 2 * M(7, 0) - 2 * M(7, 4) + 2 * M(7, 8) - 2 * M(8, 5) + + 2 * M(8, 7); // q_1 q_2 q_3 q_3 + b(48) = 8 * M(1, 1) - 8 * M(2, 2) - 8 * M(3, 3) + 8 * M(5, 5) + 8 * M(6, 6) - + 8 * M(7, 7); // q_1 q_2 q_3 q_4 + b(49) = -2 * M(0, 5) + 2 * M(0, 7) + 4 * M(1, 2) + 4 * M(1, 6) + 4 * M(2, 1) - + 4 * M(2, 3) - 4 * M(3, 2) - 4 * M(3, 6) - 2 * M(4, 5) + 2 * M(4, 7) - + 2 * M(5, 0) - 2 * M(5, 4) + 2 * M(5, 8) + 4 * M(6, 1) - 4 * M(6, 3) + + 2 * M(7, 0) + 2 * M(7, 4) - 2 * M(7, 8) + 2 * M(8, 5) - + 2 * M(8, 7); // q_1 q_2 q_4 q_4 + b(50) = 2 * M(0, 2) - 2 * M(0, 6) + 2 * M(2, 0) - 2 * M(2, 4) + 2 * M(2, 8) - + 2 * M(4, 2) + 2 * M(4, 6) - 2 * M(6, 0) + 2 * M(6, 4) - 2 * M(6, 8) + + 2 * M(8, 2) - 2 * M(8, 6); // q_1 q_3 q_3 q_3 + b(51) = -2 * M(0, 1) + 2 * M(0, 3) - 2 * M(1, 0) + 2 * M(1, 4) - 2 * M(1, 8) - + 4 * M(2, 5) - 4 * M(2, 7) + 2 * M(3, 0) - 2 * M(3, 4) + 2 * M(3, 8) + + 2 * M(4, 1) - 2 * M(4, 3) - 4 * M(5, 2) + 4 * M(5, 6) + 4 * M(6, 5) + + 4 * M(6, 7) - 4 * M(7, 2) + 4 * M(7, 6) - 2 * M(8, 1) + + 2 * M(8, 3); // q_1 q_3 q_3 q_4 + b(52) = 2 * M(0, 2) - 2 * M(0, 6) + 4 * M(1, 5) + 4 * M(1, 7) + 2 * M(2, 0) + + 2 * M(2, 4) - 2 * M(2, 8) - 4 * M(3, 5) - 4 * M(3, 7) + 2 * M(4, 2) - + 2 * M(4, 6) + 4 * M(5, 1) - 4 * M(5, 3) - 2 * M(6, 0) - 2 * M(6, 4) + + 2 * M(6, 8) + 4 * M(7, 1) - 4 * M(7, 3) - 2 * M(8, 2) + + 2 * M(8, 6); // q_1 q_3 q_4 q_4 + b(53) = -2 * M(0, 1) + 2 * M(0, 3) - 2 * M(1, 0) - 2 * M(1, 4) + 2 * M(1, 8) + + 2 * M(3, 0) + 2 * M(3, 4) - 2 * M(3, 8) - 2 * M(4, 1) + 2 * M(4, 3) + + 2 * M(8, 1) - 2 * M(8, 3); // q_1 q_4 q_4 q_4 + b(54) = M(0, 0) - M(0, 4) - M(0, 8) - M(4, 0) + M(4, 4) + M(4, 8) - M(8, 0) + + M(8, 4) + M(8, 8); // q_2 q_2 q_2 q_2 + b(55) = 2 * M(0, 1) + 2 * M(0, 3) + 2 * M(1, 0) - 2 * M(1, 4) - 2 * M(1, 8) + + 2 * M(3, 0) - 2 * M(3, 4) - 2 * M(3, 8) - 2 * M(4, 1) - 2 * M(4, 3) - + 2 * M(8, 1) - 2 * M(8, 3); // q_2 q_2 q_2 q_3 + b(56) = 2 * M(0, 2) + 2 * M(0, 6) + 2 * M(2, 0) - 2 * M(2, 4) - 2 * M(2, 8) - + 2 * M(4, 2) - 2 * M(4, 6) + 2 * M(6, 0) - 2 * M(6, 4) - 2 * M(6, 8) - + 2 * M(8, 2) - 2 * M(8, 6); // q_2 q_2 q_2 q_4 + b(57) = -2 * M(0, 0) + 2 * M(0, 4) + 4 * M(1, 1) + 4 * M(1, 3) + 4 * M(3, 1) + + 4 * M(3, 3) + 2 * M(4, 0) - 2 * M(4, 4) + + 2 * M(8, 8); // q_2 q_2 q_3 q_3 + b(58) = 2 * M(0, 5) + 2 * M(0, 7) + 4 * M(1, 2) + 4 * M(1, 6) + 4 * M(2, 1) + + 4 * M(2, 3) + 4 * M(3, 2) + 4 * M(3, 6) - 2 * M(4, 5) - 2 * M(4, 7) + + 2 * M(5, 0) - 2 * M(5, 4) - 2 * M(5, 8) + 4 * M(6, 1) + 4 * M(6, 3) + + 2 * M(7, 0) - 2 * M(7, 4) - 2 * M(7, 8) - 2 * M(8, 5) - + 2 * M(8, 7); // q_2 q_2 q_3 q_4 + b(59) = -2 * M(0, 0) + 2 * M(0, 8) + 4 * M(2, 2) + 4 * M(2, 6) + 2 * M(4, 4) + + 4 * M(6, 2) + 4 * M(6, 6) + 2 * M(8, 0) - + 2 * M(8, 8); // q_2 q_2 q_4 q_4 + b(60) = -2 * M(0, 1) - 2 * M(0, 3) - 2 * M(1, 0) + 2 * M(1, 4) - 2 * M(1, 8) - + 2 * M(3, 0) + 2 * M(3, 4) - 2 * M(3, 8) + 2 * M(4, 1) + 2 * M(4, 3) - + 2 * M(8, 1) - 2 * M(8, 3); // q_2 q_3 q_3 q_3 + b(61) = -2 * M(0, 2) - 2 * M(0, 6) + 4 * M(1, 5) + 4 * M(1, 7) - 2 * M(2, 0) + + 2 * M(2, 4) - 2 * M(2, 8) + 4 * M(3, 5) + 4 * M(3, 7) + 2 * M(4, 2) + + 2 * M(4, 6) + 4 * M(5, 1) + 4 * M(5, 3) - 2 * M(6, 0) + 2 * M(6, 4) - + 2 * M(6, 8) + 4 * M(7, 1) + 4 * M(7, 3) - 2 * M(8, 2) - + 2 * M(8, 6); // q_2 q_3 q_3 q_4 + b(62) = -2 * M(0, 1) - 2 * M(0, 3) - 2 * M(1, 0) - 2 * M(1, 4) + 2 * M(1, 8) + + 4 * M(2, 5) + 4 * M(2, 7) - 2 * M(3, 0) - 2 * M(3, 4) + 2 * M(3, 8) - + 2 * M(4, 1) - 2 * M(4, 3) + 4 * M(5, 2) + 4 * M(5, 6) + 4 * M(6, 5) + + 4 * M(6, 7) + 4 * M(7, 2) + 4 * M(7, 6) + 2 * M(8, 1) + + 2 * M(8, 3); // q_2 q_3 q_4 q_4 + b(63) = -2 * M(0, 2) - 2 * M(0, 6) - 2 * M(2, 0) - 2 * M(2, 4) + 2 * M(2, 8) - + 2 * M(4, 2) - 2 * M(4, 6) - 2 * M(6, 0) - 2 * M(6, 4) + 2 * M(6, 8) + + 2 * M(8, 2) + 2 * M(8, 6); // q_2 q_4 q_4 q_4 + b(64) = M(0, 0) - M(0, 4) + M(0, 8) - M(4, 0) + M(4, 4) - M(4, 8) + M(8, 0) - + M(8, 4) + M(8, 8); // q_3 q_3 q_3 q_3 + b(65) = -2 * M(0, 5) - 2 * M(0, 7) + 2 * M(4, 5) + 2 * M(4, 7) - 2 * M(5, 0) + + 2 * M(5, 4) - 2 * M(5, 8) - 2 * M(7, 0) + 2 * M(7, 4) - 2 * M(7, 8) - + 2 * M(8, 5) - 2 * M(8, 7); // q_3 q_3 q_3 q_4 + b(66) = 2 * M(0, 0) - 2 * M(4, 4) + 2 * M(4, 8) + 4 * M(5, 5) + 4 * M(5, 7) + + 4 * M(7, 5) + 4 * M(7, 7) + 2 * M(8, 4) - + 2 * M(8, 8); // q_3 q_3 q_4 q_4 + b(67) = -2 * M(0, 5) - 2 * M(0, 7) - 2 * M(4, 5) - 2 * M(4, 7) - 2 * M(5, 0) - + 2 * M(5, 4) + 2 * M(5, 8) - 2 * M(7, 0) - 2 * M(7, 4) + 2 * M(7, 8) + + 2 * M(8, 5) + 2 * M(8, 7); // q_3 q_4 q_4 q_4 + b(68) = M(0, 0) + M(0, 4) - M(0, 8) + M(4, 0) + M(4, 4) - M(4, 8) - M(8, 0) - + M(8, 4) + M(8, 8); // q_4 q_4 q_4 q_4 + b *= -1; + b /= b.norm(); + return std::make_shared(objective); + } +} // namespace NPnP +#endif diff --git a/modules/rgbd/src/NPnP/NPnpObjective.h b/modules/rgbd/src/NPnP/NPnpObjective.h new file mode 100644 index 00000000000..07566a2d9f6 --- /dev/null +++ b/modules/rgbd/src/NPnP/NPnpObjective.h @@ -0,0 +1,67 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ +#ifndef PNP_USING_EIGEN_LIBRARY_PNPOBJECTIVE_H +#define PNP_USING_EIGEN_LIBRARY_PNPOBJECTIVE_H +#ifdef HAVE_EIGEN +#include "../Utils_Npnp/Definitions.h" +#include "NPnpInput.h" +#include +#include + +namespace NPnP +{ + class PnpObjective + { + public: + RowMatrix<9, 9> M; + RowMatrix<3, 9> T; + RowVector b; + double sum_weights; + + static std::shared_ptr init(std::shared_ptr input); + + static void set_C(ColMatrix<3, 9> &C, Eigen::Vector3d point); + }; +} // namespace NPnP + +#endif +#endif // PNP_USING_EIGEN_LIBRARY_PNPOBJECTIVE_H diff --git a/modules/rgbd/src/NPnP/NPnpProblemSolver.cpp b/modules/rgbd/src/NPnP/NPnpProblemSolver.cpp new file mode 100644 index 00000000000..b8c3d1a45b9 --- /dev/null +++ b/modules/rgbd/src/NPnP/NPnpProblemSolver.cpp @@ -0,0 +1,428 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ +#ifdef HAVE_EIGEN +#include "NPnpProblemSolver.h" +#include "DualVar.h" +#include "../Utils_Npnp/Parsing.h" +#include +#include + +namespace NPnP +{ + PnpProblemSolver::PnpProblemSolver( + const SparseRowMatrix &A_rows, const SparseColMatrix &A_cols, + const SparseColMatrix &c_vec, + RowMatrix zero_sub_A_rows, + ColMatrix zero_sub_A_cols, + ColMatrix zero_mat_15_15, + ColVector init_y, std::shared_ptr dual_var) + : A_rows(A_rows), A_cols(A_cols), c_vec(c_vec), + zero_sub_A_rows(std::move(zero_sub_A_rows)), + zero_sub_A_cols(std::move(zero_sub_A_cols)), + zero_mat_15_15(std::move(zero_mat_15_15)), init_y(std::move(init_y)), + dual_var(std::move(dual_var)) {} + + std::shared_ptr PnpProblemSolver::init() + { + // init c vector + std::vector> c_vec_triplet_list; + c_vec_triplet_list.reserve(2); + c_vec_triplet_list.emplace_back(0, 0, -1); + c_vec_triplet_list.emplace_back(15, 0, 1); + SparseColMatrix c_vec(A_ROWS, 1); + c_vec.setFromTriplets(c_vec_triplet_list.begin(), c_vec_triplet_list.end()); + + // init y vector + ColVector init_y_vec; + init_y_vec << 1.81708374227065E-07, -2.71468522392971E-10, + -1.49378319259558E-10, -2.77091740805445E-11, 0.249631001517346, + 4.96940105201532E-05, -0.000129984428061, 5.97892049448515E-05, + 0.249893826536401, -0.000282031359397, -0.000299838600691, + 0.250010662193677, -0.000119648361021, 0.250464509752576, + 1.81036335999704E-07, -2.7244555135617E-10, -1.44912520866555E-10, + -3.09767807222993E-11, 2.26443899887141E-10, -3.1024811831443E-12, + 3.18281863003413E-13, 2.14461800733467E-10, -1.33638362717957E-12, + 2.31132526688904E-10, 7.61682308171569E-13, -8.51485831453974E-13, + 7.23456344454997E-13, 5.18662287311713E-13, -9.181167075483E-14, + -3.03315632232526E-13, -3.06071262491456E-12, 8.84104200363805E-13, + -5.53599936640479E-13, 1.66004609693395E-12, 0.12485918729074, + 1.24357475655128E-05, -1.06930867828931E-06, -3.41617427196685E-05, + 0.04161036487279, 1.32703449912371E-05, -7.18816799089538E-06, + 0.041597999991592, -3.58128226124226E-06, 0.041563449362224, + 5.72613189555295E-05, -1.80142323379959E-05, 3.18277376965713E-05, + 6.54613708082515E-05, -1.75831016069252E-05, -8.54644268091407E-05, + -0.000152813856463, 6.69489404481575E-05, 4.19129694179984E-05, + -4.82573048020879E-06, 0.124762538478383, -3.70010098743364E-05, + 2.6077418953879E-05, 0.041753219462236, -0.000129437659595, + 0.041767703722993, -0.000320047212861, 0.000129331848335, + 6.17465183469158E-05, -0.000448059699989, 0.125204467737999, + -0.000108969941535, 0.04145497500185, 0.00012234052237, 0.125678381665509; + + // init A matrix + + std::vector> A_mat_triplet_list; + A_mat_triplet_list.reserve(300); + A_mat_triplet_list.emplace_back(0, 4, -1.0); + A_mat_triplet_list.emplace_back(0, 8, -1.0); + A_mat_triplet_list.emplace_back(0, 11, -1.0); + A_mat_triplet_list.emplace_back(0, 13, -1.0); + A_mat_triplet_list.emplace_back(1, 0, 1.0); + A_mat_triplet_list.emplace_back(1, 14, -1.0); + A_mat_triplet_list.emplace_back(1, 18, -1.0); + A_mat_triplet_list.emplace_back(1, 21, -1.0); + A_mat_triplet_list.emplace_back(1, 23, -1.0); + A_mat_triplet_list.emplace_back(2, 1, 1.0); + A_mat_triplet_list.emplace_back(2, 15, -1.0); + A_mat_triplet_list.emplace_back(2, 24, -1.0); + A_mat_triplet_list.emplace_back(2, 27, -1.0); + A_mat_triplet_list.emplace_back(2, 29, -1.0); + A_mat_triplet_list.emplace_back(3, 2, 1.0); + A_mat_triplet_list.emplace_back(3, 16, -1.0); + A_mat_triplet_list.emplace_back(3, 25, -1.0); + A_mat_triplet_list.emplace_back(3, 30, -1.0); + A_mat_triplet_list.emplace_back(3, 32, -1.0); + A_mat_triplet_list.emplace_back(4, 3, 1.0); + A_mat_triplet_list.emplace_back(4, 17, -1.0); + A_mat_triplet_list.emplace_back(4, 26, -1.0); + A_mat_triplet_list.emplace_back(4, 31, -1.0); + A_mat_triplet_list.emplace_back(4, 33, -1.0); + A_mat_triplet_list.emplace_back(5, 4, 1.0); + A_mat_triplet_list.emplace_back(5, 34, -1.0); + A_mat_triplet_list.emplace_back(5, 38, -1.0); + A_mat_triplet_list.emplace_back(5, 41, -1.0); + A_mat_triplet_list.emplace_back(5, 43, -1.0); + A_mat_triplet_list.emplace_back(6, 5, 1.0); + A_mat_triplet_list.emplace_back(6, 35, -1.0); + A_mat_triplet_list.emplace_back(6, 44, -1.0); + A_mat_triplet_list.emplace_back(6, 47, -1.0); + A_mat_triplet_list.emplace_back(6, 49, -1.0); + A_mat_triplet_list.emplace_back(7, 6, 1.0); + A_mat_triplet_list.emplace_back(7, 36, -1.0); + A_mat_triplet_list.emplace_back(7, 45, -1.0); + A_mat_triplet_list.emplace_back(7, 50, -1.0); + A_mat_triplet_list.emplace_back(7, 52, -1.0); + A_mat_triplet_list.emplace_back(8, 7, 1.0); + A_mat_triplet_list.emplace_back(8, 37, -1.0); + A_mat_triplet_list.emplace_back(8, 46, -1.0); + A_mat_triplet_list.emplace_back(8, 51, -1.0); + A_mat_triplet_list.emplace_back(8, 53, -1.0); + A_mat_triplet_list.emplace_back(9, 8, 1.0); + A_mat_triplet_list.emplace_back(9, 38, -1.0); + A_mat_triplet_list.emplace_back(9, 54, -1.0); + A_mat_triplet_list.emplace_back(9, 57, -1.0); + A_mat_triplet_list.emplace_back(9, 59, -1.0); + A_mat_triplet_list.emplace_back(10, 9, 1.0); + A_mat_triplet_list.emplace_back(10, 39, -1.0); + A_mat_triplet_list.emplace_back(10, 55, -1.0); + A_mat_triplet_list.emplace_back(10, 60, -1.0); + A_mat_triplet_list.emplace_back(10, 62, -1.0); + A_mat_triplet_list.emplace_back(11, 10, 1.0); + A_mat_triplet_list.emplace_back(11, 40, -1.0); + A_mat_triplet_list.emplace_back(11, 56, -1.0); + A_mat_triplet_list.emplace_back(11, 61, -1.0); + A_mat_triplet_list.emplace_back(11, 63, -1.0); + A_mat_triplet_list.emplace_back(12, 11, 1.0); + A_mat_triplet_list.emplace_back(12, 41, -1.0); + A_mat_triplet_list.emplace_back(12, 57, -1.0); + A_mat_triplet_list.emplace_back(12, 64, -1.0); + A_mat_triplet_list.emplace_back(12, 66, -1.0); + A_mat_triplet_list.emplace_back(13, 12, 1.0); + A_mat_triplet_list.emplace_back(13, 42, -1.0); + A_mat_triplet_list.emplace_back(13, 58, -1.0); + A_mat_triplet_list.emplace_back(13, 65, -1.0); + A_mat_triplet_list.emplace_back(13, 67, -1.0); + A_mat_triplet_list.emplace_back(14, 13, 1.0); + A_mat_triplet_list.emplace_back(14, 43, -1.0); + A_mat_triplet_list.emplace_back(14, 59, -1.0); + A_mat_triplet_list.emplace_back(14, 66, -1.0); + A_mat_triplet_list.emplace_back(14, 68, -1.0); + A_mat_triplet_list.emplace_back(16, 0, -1.0); + A_mat_triplet_list.emplace_back(17, 1, -1.0); + A_mat_triplet_list.emplace_back(18, 2, -1.0); + A_mat_triplet_list.emplace_back(19, 3, -1.0); + A_mat_triplet_list.emplace_back(20, 4, -1.0); + A_mat_triplet_list.emplace_back(21, 5, -1.0); + A_mat_triplet_list.emplace_back(22, 6, -1.0); + A_mat_triplet_list.emplace_back(23, 7, -1.0); + A_mat_triplet_list.emplace_back(24, 8, -1.0); + A_mat_triplet_list.emplace_back(25, 9, -1.0); + A_mat_triplet_list.emplace_back(26, 10, -1.0); + A_mat_triplet_list.emplace_back(27, 11, -1.0); + A_mat_triplet_list.emplace_back(28, 12, -1.0); + A_mat_triplet_list.emplace_back(29, 13, -1.0); + A_mat_triplet_list.emplace_back(30, 0, -1.0); + A_mat_triplet_list.emplace_back(31, 4, -1.0); + A_mat_triplet_list.emplace_back(32, 5, -1.0); + A_mat_triplet_list.emplace_back(33, 6, -1.0); + A_mat_triplet_list.emplace_back(34, 7, -1.0); + A_mat_triplet_list.emplace_back(35, 14, -1.0); + A_mat_triplet_list.emplace_back(36, 15, -1.0); + A_mat_triplet_list.emplace_back(37, 16, -1.0); + A_mat_triplet_list.emplace_back(38, 17, -1.0); + A_mat_triplet_list.emplace_back(39, 18, -1.0); + A_mat_triplet_list.emplace_back(40, 19, -1.0); + A_mat_triplet_list.emplace_back(41, 20, -1.0); + A_mat_triplet_list.emplace_back(42, 21, -1.0); + A_mat_triplet_list.emplace_back(43, 22, -1.0); + A_mat_triplet_list.emplace_back(44, 23, -1.0); + A_mat_triplet_list.emplace_back(45, 1, -1.0); + A_mat_triplet_list.emplace_back(46, 5, -1.0); + A_mat_triplet_list.emplace_back(47, 8, -1.0); + A_mat_triplet_list.emplace_back(48, 9, -1.0); + A_mat_triplet_list.emplace_back(49, 10, -1.0); + A_mat_triplet_list.emplace_back(50, 15, -1.0); + A_mat_triplet_list.emplace_back(51, 18, -1.0); + A_mat_triplet_list.emplace_back(52, 19, -1.0); + A_mat_triplet_list.emplace_back(53, 20, -1.0); + A_mat_triplet_list.emplace_back(54, 24, -1.0); + A_mat_triplet_list.emplace_back(55, 25, -1.0); + A_mat_triplet_list.emplace_back(56, 26, -1.0); + A_mat_triplet_list.emplace_back(57, 27, -1.0); + A_mat_triplet_list.emplace_back(58, 28, -1.0); + A_mat_triplet_list.emplace_back(59, 29, -1.0); + A_mat_triplet_list.emplace_back(60, 2, -1.0); + A_mat_triplet_list.emplace_back(61, 6, -1.0); + A_mat_triplet_list.emplace_back(62, 9, -1.0); + A_mat_triplet_list.emplace_back(63, 11, -1.0); + A_mat_triplet_list.emplace_back(64, 12, -1.0); + A_mat_triplet_list.emplace_back(65, 16, -1.0); + A_mat_triplet_list.emplace_back(66, 19, -1.0); + A_mat_triplet_list.emplace_back(67, 21, -1.0); + A_mat_triplet_list.emplace_back(68, 22, -1.0); + A_mat_triplet_list.emplace_back(69, 25, -1.0); + A_mat_triplet_list.emplace_back(70, 27, -1.0); + A_mat_triplet_list.emplace_back(71, 28, -1.0); + A_mat_triplet_list.emplace_back(72, 30, -1.0); + A_mat_triplet_list.emplace_back(73, 31, -1.0); + A_mat_triplet_list.emplace_back(74, 32, -1.0); + A_mat_triplet_list.emplace_back(75, 3, -1.0); + A_mat_triplet_list.emplace_back(76, 7, -1.0); + A_mat_triplet_list.emplace_back(77, 10, -1.0); + A_mat_triplet_list.emplace_back(78, 12, -1.0); + A_mat_triplet_list.emplace_back(79, 13, -1.0); + A_mat_triplet_list.emplace_back(80, 17, -1.0); + A_mat_triplet_list.emplace_back(81, 20, -1.0); + A_mat_triplet_list.emplace_back(82, 22, -1.0); + A_mat_triplet_list.emplace_back(83, 23, -1.0); + A_mat_triplet_list.emplace_back(84, 26, -1.0); + A_mat_triplet_list.emplace_back(85, 28, -1.0); + A_mat_triplet_list.emplace_back(86, 29, -1.0); + A_mat_triplet_list.emplace_back(87, 31, -1.0); + A_mat_triplet_list.emplace_back(88, 32, -1.0); + A_mat_triplet_list.emplace_back(89, 33, -1.0); + A_mat_triplet_list.emplace_back(90, 4, -1.0); + A_mat_triplet_list.emplace_back(91, 14, -1.0); + A_mat_triplet_list.emplace_back(92, 15, -1.0); + A_mat_triplet_list.emplace_back(93, 16, -1.0); + A_mat_triplet_list.emplace_back(94, 17, -1.0); + A_mat_triplet_list.emplace_back(95, 34, -1.0); + A_mat_triplet_list.emplace_back(96, 35, -1.0); + A_mat_triplet_list.emplace_back(97, 36, -1.0); + A_mat_triplet_list.emplace_back(98, 37, -1.0); + A_mat_triplet_list.emplace_back(99, 38, -1.0); + A_mat_triplet_list.emplace_back(100, 39, -1.0); + A_mat_triplet_list.emplace_back(101, 40, -1.0); + A_mat_triplet_list.emplace_back(102, 41, -1.0); + A_mat_triplet_list.emplace_back(103, 42, -1.0); + A_mat_triplet_list.emplace_back(104, 43, -1.0); + A_mat_triplet_list.emplace_back(105, 5, -1.0); + A_mat_triplet_list.emplace_back(106, 15, -1.0); + A_mat_triplet_list.emplace_back(107, 18, -1.0); + A_mat_triplet_list.emplace_back(108, 19, -1.0); + A_mat_triplet_list.emplace_back(109, 20, -1.0); + A_mat_triplet_list.emplace_back(110, 35, -1.0); + A_mat_triplet_list.emplace_back(111, 38, -1.0); + A_mat_triplet_list.emplace_back(112, 39, -1.0); + A_mat_triplet_list.emplace_back(113, 40, -1.0); + A_mat_triplet_list.emplace_back(114, 44, -1.0); + A_mat_triplet_list.emplace_back(115, 45, -1.0); + A_mat_triplet_list.emplace_back(116, 46, -1.0); + A_mat_triplet_list.emplace_back(117, 47, -1.0); + A_mat_triplet_list.emplace_back(118, 48, -1.0); + A_mat_triplet_list.emplace_back(119, 49, -1.0); + A_mat_triplet_list.emplace_back(120, 6, -1.0); + A_mat_triplet_list.emplace_back(121, 16, -1.0); + A_mat_triplet_list.emplace_back(122, 19, -1.0); + A_mat_triplet_list.emplace_back(123, 21, -1.0); + A_mat_triplet_list.emplace_back(124, 22, -1.0); + A_mat_triplet_list.emplace_back(125, 36, -1.0); + A_mat_triplet_list.emplace_back(126, 39, -1.0); + A_mat_triplet_list.emplace_back(127, 41, -1.0); + A_mat_triplet_list.emplace_back(128, 42, -1.0); + A_mat_triplet_list.emplace_back(129, 45, -1.0); + A_mat_triplet_list.emplace_back(130, 47, -1.0); + A_mat_triplet_list.emplace_back(131, 48, -1.0); + A_mat_triplet_list.emplace_back(132, 50, -1.0); + A_mat_triplet_list.emplace_back(133, 51, -1.0); + A_mat_triplet_list.emplace_back(134, 52, -1.0); + A_mat_triplet_list.emplace_back(135, 7, -1.0); + A_mat_triplet_list.emplace_back(136, 17, -1.0); + A_mat_triplet_list.emplace_back(137, 20, -1.0); + A_mat_triplet_list.emplace_back(138, 22, -1.0); + A_mat_triplet_list.emplace_back(139, 23, -1.0); + A_mat_triplet_list.emplace_back(140, 37, -1.0); + A_mat_triplet_list.emplace_back(141, 40, -1.0); + A_mat_triplet_list.emplace_back(142, 42, -1.0); + A_mat_triplet_list.emplace_back(143, 43, -1.0); + A_mat_triplet_list.emplace_back(144, 46, -1.0); + A_mat_triplet_list.emplace_back(145, 48, -1.0); + A_mat_triplet_list.emplace_back(146, 49, -1.0); + A_mat_triplet_list.emplace_back(147, 51, -1.0); + A_mat_triplet_list.emplace_back(148, 52, -1.0); + A_mat_triplet_list.emplace_back(149, 53, -1.0); + A_mat_triplet_list.emplace_back(150, 8, -1.0); + A_mat_triplet_list.emplace_back(151, 18, -1.0); + A_mat_triplet_list.emplace_back(152, 24, -1.0); + A_mat_triplet_list.emplace_back(153, 25, -1.0); + A_mat_triplet_list.emplace_back(154, 26, -1.0); + A_mat_triplet_list.emplace_back(155, 38, -1.0); + A_mat_triplet_list.emplace_back(156, 44, -1.0); + A_mat_triplet_list.emplace_back(157, 45, -1.0); + A_mat_triplet_list.emplace_back(158, 46, -1.0); + A_mat_triplet_list.emplace_back(159, 54, -1.0); + A_mat_triplet_list.emplace_back(160, 55, -1.0); + A_mat_triplet_list.emplace_back(161, 56, -1.0); + A_mat_triplet_list.emplace_back(162, 57, -1.0); + A_mat_triplet_list.emplace_back(163, 58, -1.0); + A_mat_triplet_list.emplace_back(164, 59, -1.0); + A_mat_triplet_list.emplace_back(165, 9, -1.0); + A_mat_triplet_list.emplace_back(166, 19, -1.0); + A_mat_triplet_list.emplace_back(167, 25, -1.0); + A_mat_triplet_list.emplace_back(168, 27, -1.0); + A_mat_triplet_list.emplace_back(169, 28, -1.0); + A_mat_triplet_list.emplace_back(170, 39, -1.0); + A_mat_triplet_list.emplace_back(171, 45, -1.0); + A_mat_triplet_list.emplace_back(172, 47, -1.0); + A_mat_triplet_list.emplace_back(173, 48, -1.0); + A_mat_triplet_list.emplace_back(174, 55, -1.0); + A_mat_triplet_list.emplace_back(175, 57, -1.0); + A_mat_triplet_list.emplace_back(176, 58, -1.0); + A_mat_triplet_list.emplace_back(177, 60, -1.0); + A_mat_triplet_list.emplace_back(178, 61, -1.0); + A_mat_triplet_list.emplace_back(179, 62, -1.0); + A_mat_triplet_list.emplace_back(180, 10, -1.0); + A_mat_triplet_list.emplace_back(181, 20, -1.0); + A_mat_triplet_list.emplace_back(182, 26, -1.0); + A_mat_triplet_list.emplace_back(183, 28, -1.0); + A_mat_triplet_list.emplace_back(184, 29, -1.0); + A_mat_triplet_list.emplace_back(185, 40, -1.0); + A_mat_triplet_list.emplace_back(186, 46, -1.0); + A_mat_triplet_list.emplace_back(187, 48, -1.0); + A_mat_triplet_list.emplace_back(188, 49, -1.0); + A_mat_triplet_list.emplace_back(189, 56, -1.0); + A_mat_triplet_list.emplace_back(190, 58, -1.0); + A_mat_triplet_list.emplace_back(191, 59, -1.0); + A_mat_triplet_list.emplace_back(192, 61, -1.0); + A_mat_triplet_list.emplace_back(193, 62, -1.0); + A_mat_triplet_list.emplace_back(194, 63, -1.0); + A_mat_triplet_list.emplace_back(195, 11, -1.0); + A_mat_triplet_list.emplace_back(196, 21, -1.0); + A_mat_triplet_list.emplace_back(197, 27, -1.0); + A_mat_triplet_list.emplace_back(198, 30, -1.0); + A_mat_triplet_list.emplace_back(199, 31, -1.0); + A_mat_triplet_list.emplace_back(200, 41, -1.0); + A_mat_triplet_list.emplace_back(201, 47, -1.0); + A_mat_triplet_list.emplace_back(202, 50, -1.0); + A_mat_triplet_list.emplace_back(203, 51, -1.0); + A_mat_triplet_list.emplace_back(204, 57, -1.0); + A_mat_triplet_list.emplace_back(205, 60, -1.0); + A_mat_triplet_list.emplace_back(206, 61, -1.0); + A_mat_triplet_list.emplace_back(207, 64, -1.0); + A_mat_triplet_list.emplace_back(208, 65, -1.0); + A_mat_triplet_list.emplace_back(209, 66, -1.0); + A_mat_triplet_list.emplace_back(210, 12, -1.0); + A_mat_triplet_list.emplace_back(211, 22, -1.0); + A_mat_triplet_list.emplace_back(212, 28, -1.0); + A_mat_triplet_list.emplace_back(213, 31, -1.0); + A_mat_triplet_list.emplace_back(214, 32, -1.0); + A_mat_triplet_list.emplace_back(215, 42, -1.0); + A_mat_triplet_list.emplace_back(216, 48, -1.0); + A_mat_triplet_list.emplace_back(217, 51, -1.0); + A_mat_triplet_list.emplace_back(218, 52, -1.0); + A_mat_triplet_list.emplace_back(219, 58, -1.0); + A_mat_triplet_list.emplace_back(220, 61, -1.0); + A_mat_triplet_list.emplace_back(221, 62, -1.0); + A_mat_triplet_list.emplace_back(222, 65, -1.0); + A_mat_triplet_list.emplace_back(223, 66, -1.0); + A_mat_triplet_list.emplace_back(224, 67, -1.0); + A_mat_triplet_list.emplace_back(225, 13, -1.0); + A_mat_triplet_list.emplace_back(226, 23, -1.0); + A_mat_triplet_list.emplace_back(227, 29, -1.0); + A_mat_triplet_list.emplace_back(228, 32, -1.0); + A_mat_triplet_list.emplace_back(229, 33, -1.0); + A_mat_triplet_list.emplace_back(230, 43, -1.0); + A_mat_triplet_list.emplace_back(231, 49, -1.0); + A_mat_triplet_list.emplace_back(232, 52, -1.0); + A_mat_triplet_list.emplace_back(233, 53, -1.0); + A_mat_triplet_list.emplace_back(234, 59, -1.0); + A_mat_triplet_list.emplace_back(235, 62, -1.0); + A_mat_triplet_list.emplace_back(236, 63, -1.0); + A_mat_triplet_list.emplace_back(237, 66, -1.0); + A_mat_triplet_list.emplace_back(238, 67, -1.0); + A_mat_triplet_list.emplace_back(239, 68, -1.0); + + SparseColMatrix A_mat_cols(A_ROWS, Y_SIZE); + A_mat_cols.setFromTriplets(A_mat_triplet_list.begin(), + A_mat_triplet_list.end()); + + SparseRowMatrix A_mat_rows(A_ROWS, Y_SIZE); + A_mat_rows.setFromTriplets(A_mat_triplet_list.begin(), + A_mat_triplet_list.end()); + + auto zero_sub_A_cols = A_mat_cols.block(0, 0, NUM_CONSTRAINTS, Y_SIZE).eval(); + auto zero_sub_A_rows = A_mat_rows.block(0, 0, NUM_CONSTRAINTS, Y_SIZE).eval(); + SparseColMatrix zero_mat_15_15(NUM_CONSTRAINTS, NUM_CONSTRAINTS); + + A_mat_rows.makeCompressed(); + A_mat_cols.makeCompressed(); + c_vec.makeCompressed(); + zero_sub_A_cols.makeCompressed(); + zero_sub_A_rows.makeCompressed(); + zero_mat_15_15.makeCompressed(); + auto dual_var = std::shared_ptr(nullptr); + + return std::make_shared( + A_mat_rows, A_mat_cols, c_vec, zero_sub_A_rows, zero_sub_A_cols, + zero_mat_15_15, init_y_vec, dual_var); + } +} // namespace NPnP +#endif diff --git a/modules/rgbd/src/NPnP/NPnpProblemSolver.h b/modules/rgbd/src/NPnP/NPnpProblemSolver.h new file mode 100644 index 00000000000..af205f4c807 --- /dev/null +++ b/modules/rgbd/src/NPnP/NPnpProblemSolver.h @@ -0,0 +1,99 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ +#ifndef PNP_USING_EIGEN_LIBRARY_PNPPROBLEMSOLVER_H +#define PNP_USING_EIGEN_LIBRARY_PNPPROBLEMSOLVER_H +#ifdef HAVE_EIGEN +#include "BarrierMethodSettings.h" +#include "../Utils_Npnp/Definitions.h" +#include "DualVar.h" +#include "NPnpInput.h" +#include "NPnpObjective.h" +#include "NPnpResult.h" + +namespace NPnP +{ + + class PnpProblemSolver + { + public: + SparseRowMatrix A_rows; + SparseColMatrix A_cols; + SparseColMatrix c_vec; + RowMatrix zero_sub_A_rows; + ColMatrix zero_sub_A_cols; + ColMatrix zero_mat_15_15; + ColVector init_y; + std::shared_ptr dual_var; + + PnpProblemSolver(const SparseRowMatrix &A_rows, const SparseColMatrix &A_cols, + const SparseColMatrix &c_vec, + RowMatrix zero_sub_A_rows, + ColMatrix zero_sub_A_cols, + ColMatrix zero_mat_15_15, + ColVector init_y, std::shared_ptr dual_var); + + static std::shared_ptr init(); + + PnpResult + solve_pnp(std::shared_ptr pnp_objective, + std::shared_ptr barrier_method_settings); + + void + line_search(const ColVector &delta_y, + const std::function &)> &obj_func, + std::shared_ptr barrier_settings); + + void centering_step( + const std::function &)> &obj_func, + const std::function &gen_grad, + const std::function &gen_hess, + int outer_iteration, bool &is_last_iter, PnpResult &pnp_result, + std::shared_ptr pnp_objective, + std::shared_ptr barrier_method_settings); + + Eigen::Vector4d + perform_local_search(std::shared_ptr pnp_objective, + Eigen::Vector4d quats); + }; +} // namespace NPnP +#endif +#endif // PNP_USING_EIGEN_LIBRARY_PNPPROBLEMSOLVER_H diff --git a/modules/rgbd/src/NPnP/NPnpResult.cpp b/modules/rgbd/src/NPnP/NPnpResult.cpp new file mode 100644 index 00000000000..b3cefa26213 --- /dev/null +++ b/modules/rgbd/src/NPnP/NPnpResult.cpp @@ -0,0 +1,88 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ +#ifdef HAVE_EIGEN +#include "NPnpResult.h" +#include "../Utils_Npnp/Definitions.h" + +namespace NPnP +{ + double PnpResult::cost() + { + auto rotation_matrix = this->rotation_matrix(); + Eigen::Map> rotation_vector(rotation_matrix.data(), 9); + auto sum_error = + (rotation_vector.transpose() * pnp_objective->M * rotation_vector)(0, 0); + return sum_error / pnp_objective->sum_weights; + } + + Eigen::Matrix3d PnpResult::rotation_matrix() + { + auto q1 = this->quaternions.x(); + auto q2 = this->quaternions.y(); + auto q3 = this->quaternions.z(); + auto q4 = this->quaternions.w(); + auto q11 = q1 * q1; + auto q12 = q1 * q2; + auto q13 = q1 * q3; + auto q14 = q1 * q4; + auto q22 = q2 * q2; + auto q23 = q2 * q3; + auto q24 = q2 * q4; + auto q33 = q3 * q3; + auto q34 = q3 * q4; + auto q44 = q4 * q4; + Eigen::Matrix3d rotation_matrix; + rotation_matrix << q11 + q22 - q33 - q44, 2 * q23 - 2 * q14, + 2 * q24 + 2 * q13, 2 * q23 + 2 * q14, q11 + q33 - q22 - q44, + 2 * q34 - 2 * q12, 2 * q24 - 2 * q13, 2 * q34 + 2 * q12, + q11 + q44 - q22 - q33; + return rotation_matrix; + } + + Eigen::Vector3d PnpResult::translation_vector() + { + auto rotation_matrix = this->rotation_matrix(); + Eigen::Map> rotation_vector(rotation_matrix.data(), 9); + return pnp_objective->T * rotation_vector; + } +} // namespace NPnP +#endif \ No newline at end of file diff --git a/modules/rgbd/src/NPnP/NPnpResult.h b/modules/rgbd/src/NPnP/NPnpResult.h new file mode 100644 index 00000000000..7dfa1171803 --- /dev/null +++ b/modules/rgbd/src/NPnP/NPnpResult.h @@ -0,0 +1,67 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ +#ifndef PNP_SOLVER_PNPRESULT_H +#define PNP_SOLVER_PNPRESULT_H +#ifdef HAVE_EIGEN +#include "../Utils_Npnp/Definitions.h" +#include "NPnpObjective.h" +#include + +namespace NPnP +{ + class PnpResult + { + public: + Eigen::Vector4d quaternions; + std::shared_ptr pnp_objective; + + PnpResult(Eigen::Vector4d quats, std::shared_ptr objective) + : quaternions(std::move(quats)), pnp_objective(std::move(objective)) {} + + double cost(); + Eigen::Matrix3d rotation_matrix(); + Eigen::Vector3d translation_vector(); + }; +} // namespace NPnP + +#endif +#endif // PNP_SOLVER_PNPRESULT_H diff --git a/modules/rgbd/src/NPnP/QuaternionVector.cpp b/modules/rgbd/src/NPnP/QuaternionVector.cpp new file mode 100644 index 00000000000..5591fd88523 --- /dev/null +++ b/modules/rgbd/src/NPnP/QuaternionVector.cpp @@ -0,0 +1,383 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ +#ifdef HAVE_EIGEN +#include "QuaternionVector.h" + +namespace NPnP +{ + QuaternionVector::QuaternionVector(const Eigen::Vector4d &quaternions) + : quaternions(quaternions) + { + q1 = quaternions.x(); + q2 = quaternions.y(); + q3 = quaternions.z(); + q4 = quaternions.w(); + q1q1 = q1 * q1; + q1q2 = q1 * q2; + q1q3 = q1 * q3; + q1q4 = q1 * q4; + q2q2 = q2 * q2; + q2q3 = q2 * q3; + q2q4 = q2 * q4; + q3q3 = q3 * q3; + q3q4 = q3 * q4; + q4q4 = q4 * q4; + q1q1q1 = q1q1 * q1; + q1q1q2 = q1q1 * q2; + q1q1q3 = q1q1 * q3; + q1q1q4 = q1q1 * q4; + q1q2q2 = q1q2 * q2; + q1q2q3 = q1q2 * q3; + q1q2q4 = q1q2 * q4; + q1q3q3 = q1q3 * q3; + q1q3q4 = q1q3 * q4; + q1q4q4 = q1q4 * q4; + q2q2q2 = q2q2 * q2; + q2q2q3 = q2q2 * q3; + q2q2q4 = q2q2 * q4; + q2q3q3 = q2q3 * q3; + q2q3q4 = q2q3 * q4; + q2q4q4 = q2q4 * q4; + q3q3q3 = q3q3 * q3; + q3q3q4 = q3q3 * q4; + q3q4q4 = q3q4 * q4; + q4q4q4 = q4q4 * q4; + q1q1q1q1 = q1q1 * q1q1; + q1q1q1q2 = q1q1 * q1q2; + q1q1q1q3 = q1q1 * q1q3; + q1q1q1q4 = q1q1 * q1q4; + q1q1q2q2 = q1q1 * q2q2; + q1q1q2q3 = q1q1 * q2q3; + q1q1q2q4 = q1q1 * q2q4; + q1q1q3q3 = q1q1 * q3q3; + q1q1q3q4 = q1q1 * q3q4; + q1q1q4q4 = q1q1 * q4q4; + q1q2q2q2 = q1q2 * q2q2; + q1q2q2q3 = q1q2 * q2q3; + q1q2q2q4 = q1q2 * q2q4; + q1q2q3q3 = q1q2 * q3q3; + q1q2q3q4 = q1q2 * q3q4; + q1q2q4q4 = q1q2 * q4q4; + q1q3q3q3 = q1q3 * q3q3; + q1q3q3q4 = q1q3 * q3q4; + q1q3q4q4 = q1q3 * q4q4; + q1q4q4q4 = q1q4 * q4q4; + q2q2q2q2 = q2q2 * q2q2; + q2q2q2q3 = q2q2 * q2q3; + q2q2q2q4 = q2q2 * q2q4; + q2q2q3q3 = q2q2 * q3q3; + q2q2q3q4 = q2q2 * q3q4; + q2q2q4q4 = q2q2 * q4q4; + q2q3q3q3 = q2q3 * q3q3; + q2q3q3q4 = q2q3 * q3q4; + q2q3q4q4 = q2q3 * q4q4; + q2q4q4q4 = q2q4 * q4q4; + q3q3q3q3 = q3q3 * q3q3; + q3q3q3q4 = q3q3 * q3q4; + q3q3q4q4 = q3q3 * q4q4; + q3q4q4q4 = q3q4 * q4q4; + q4q4q4q4 = q4q4 * q4q4; + q_norm_squared = q1q1 + q2q2 + q3q3 + q4q4; + } + + double QuaternionVector::obj_func(const ColVector &b) const + { + double sum = 0.0; + sum += b[34] * q1q1q1q1; + sum += b[35] * q1q1q1q2; + sum += b[36] * q1q1q1q3; + sum += b[37] * q1q1q1q4; + sum += b[38] * q1q1q2q2; + sum += b[39] * q1q1q2q3; + sum += b[40] * q1q1q2q4; + sum += b[41] * q1q1q3q3; + sum += b[42] * q1q1q3q4; + sum += b[43] * q1q1q4q4; + sum += b[44] * q1q2q2q2; + sum += b[45] * q1q2q2q3; + sum += b[46] * q1q2q2q4; + sum += b[47] * q1q2q3q3; + sum += b[48] * q1q2q3q4; + sum += b[49] * q1q2q4q4; + sum += b[50] * q1q3q3q3; + sum += b[51] * q1q3q3q4; + sum += b[52] * q1q3q4q4; + sum += b[53] * q1q4q4q4; + sum += b[54] * q2q2q2q2; + sum += b[55] * q2q2q2q3; + sum += b[56] * q2q2q2q4; + sum += b[57] * q2q2q3q3; + sum += b[58] * q2q2q3q4; + sum += b[59] * q2q2q4q4; + sum += b[60] * q2q3q3q3; + sum += b[61] * q2q3q3q4; + sum += b[62] * q2q3q4q4; + sum += b[63] * q2q4q4q4; + sum += b[64] * q3q3q3q3; + sum += b[65] * q3q3q3q4; + sum += b[66] * q3q3q4q4; + sum += b[67] * q3q4q4q4; + sum += b[68] * q4q4q4q4; + return sum; + } + Eigen::Vector4d QuaternionVector::obj_grad(const ColVector &b) const + { + double q1_grad = 0; + double q2_grad = 0; + double q3_grad = 0; + double q4_grad = 0; + q1_grad += b[34] * 4 * q1q1q1; + q1_grad += b[35] * 3 * q1q1q2; + q1_grad += b[36] * 3 * q1q1q3; + q1_grad += b[37] * 3 * q1q1q4; + q1_grad += b[38] * 2 * q1q2q2; + q1_grad += b[39] * 2 * q1q2q3; + q1_grad += b[40] * 2 * q1q2q4; + q1_grad += b[41] * 2 * q1q3q3; + q1_grad += b[42] * 2 * q1q3q4; + q1_grad += b[43] * 2 * q1q4q4; + q1_grad += b[44] * 1 * q2q2q2; + q1_grad += b[45] * 1 * q2q2q3; + q1_grad += b[46] * 1 * q2q2q4; + q1_grad += b[47] * 1 * q2q3q3; + q1_grad += b[48] * 1 * q2q3q4; + q1_grad += b[49] * 1 * q2q4q4; + q1_grad += b[50] * 1 * q3q3q3; + q1_grad += b[51] * 1 * q3q3q4; + q1_grad += b[52] * 1 * q3q4q4; + q1_grad += b[53] * 1 * q4q4q4; + + q2_grad += b[35] * 1 * q1q1q1; + q2_grad += b[38] * 2 * q1q1q2; + q2_grad += b[39] * 1 * q1q1q3; + q2_grad += b[40] * 1 * q1q1q4; + q2_grad += b[44] * 3 * q1q2q2; + q2_grad += b[45] * 2 * q1q2q3; + q2_grad += b[46] * 2 * q1q2q4; + q2_grad += b[47] * 1 * q1q3q3; + q2_grad += b[48] * 1 * q1q3q4; + q2_grad += b[49] * 1 * q1q4q4; + q2_grad += b[54] * 4 * q2q2q2; + q2_grad += b[55] * 3 * q2q2q3; + q2_grad += b[56] * 3 * q2q2q4; + q2_grad += b[57] * 2 * q2q3q3; + q2_grad += b[58] * 2 * q2q3q4; + q2_grad += b[59] * 2 * q2q4q4; + q2_grad += b[60] * 1 * q3q3q3; + q2_grad += b[61] * 1 * q3q3q4; + q2_grad += b[62] * 1 * q3q4q4; + q2_grad += b[63] * 1 * q4q4q4; + + q3_grad += b[36] * 1 * q1q1q1; + q3_grad += b[39] * 1 * q1q1q2; + q3_grad += b[41] * 2 * q1q1q3; + q3_grad += b[42] * 1 * q1q1q4; + q3_grad += b[45] * 1 * q1q2q2; + q3_grad += b[47] * 2 * q1q2q3; + q3_grad += b[48] * 1 * q1q2q4; + q3_grad += b[50] * 3 * q1q3q3; + q3_grad += b[51] * 2 * q1q3q4; + q3_grad += b[52] * 1 * q1q4q4; + q3_grad += b[55] * 1 * q2q2q2; + q3_grad += b[57] * 2 * q2q2q3; + q3_grad += b[58] * 1 * q2q2q4; + q3_grad += b[60] * 3 * q2q3q3; + q3_grad += b[61] * 2 * q2q3q4; + q3_grad += b[62] * 1 * q2q4q4; + q3_grad += b[64] * 4 * q3q3q3; + q3_grad += b[65] * 3 * q3q3q4; + q3_grad += b[66] * 2 * q3q4q4; + q3_grad += b[67] * 1 * q4q4q4; + + q4_grad += b[68] * q4q4q4 * 4; + q4_grad += b[67] * q3q4q4 * 3; + q4_grad += b[53] * q1q4q4 * 3; + q4_grad += b[63] * q2q4q4 * 3; + q4_grad += b[66] * q3q3q4 * 2; + q4_grad += b[43] * q1q1q4 * 2; + q4_grad += b[49] * q1q2q4 * 2; + q4_grad += b[52] * q1q3q4 * 2; + q4_grad += b[59] * q2q2q4 * 2; + q4_grad += b[62] * q2q3q4 * 2; + q4_grad += b[46] * q1q2q2 * 1; + q4_grad += b[48] * q1q2q3 * 1; + q4_grad += b[51] * q1q3q3 * 1; + q4_grad += b[56] * q2q2q2 * 1; + q4_grad += b[58] * q2q2q3 * 1; + q4_grad += b[61] * q2q3q3 * 1; + q4_grad += b[65] * q3q3q3 * 1; + q4_grad += b[37] * q1q1q1 * 1; + q4_grad += b[40] * q1q1q2 * 1; + q4_grad += b[42] * q1q1q3 * 1; + + return std::move(Eigen::Vector4d(q1_grad, q2_grad, q3_grad, q4_grad)); + } + Eigen::Matrix4d QuaternionVector::obj_hess(const ColVector &b) const + { + double q1q1_hess = 0; + double q1q2_hess = 0; + double q1q3_hess = 0; + double q1q4_hess = 0; + double q2q2_hess = 0; + double q2q3_hess = 0; + double q2q4_hess = 0; + double q3q3_hess = 0; + double q3q4_hess = 0; + double q4q4_hess = 0; + q1q1_hess += b[34] * 4 * 3 * q1q1; + q1q1_hess += b[35] * 3 * 2 * q1q2; + q1q1_hess += b[36] * 3 * 2 * q1q3; + q1q1_hess += b[37] * 3 * 2 * q1q4; + q1q1_hess += b[38] * 2 * 1 * q2q2; + q1q1_hess += b[39] * 2 * 1 * q2q3; + q1q1_hess += b[40] * 2 * 1 * q2q4; + q1q1_hess += b[41] * 2 * 1 * q3q3; + q1q1_hess += b[42] * 2 * 1 * q3q4; + q1q1_hess += b[43] * 2 * 1 * q4q4; + + q1q2_hess += b[35] * 3 * 1 * q1q1; + q1q2_hess += b[38] * 2 * 2 * q1q2; + q1q2_hess += b[39] * 2 * 1 * q1q3; + q1q2_hess += b[40] * 2 * 1 * q1q4; + q1q2_hess += b[44] * 1 * 3 * q2q2; + q1q2_hess += b[45] * 1 * 2 * q2q3; + q1q2_hess += b[46] * 1 * 2 * q2q4; + q1q2_hess += b[47] * 1 * 1 * q3q3; + q1q2_hess += b[48] * 1 * 1 * q3q4; + q1q2_hess += b[49] * 1 * 1 * q4q4; + + q1q3_hess += b[36] * 3 * 1 * q1q1; + q1q3_hess += b[39] * 2 * 1 * q1q2; + q1q3_hess += b[41] * 2 * 2 * q1q3; + q1q3_hess += b[42] * 2 * 1 * q1q4; + q1q3_hess += b[45] * 1 * 1 * q2q2; + q1q3_hess += b[47] * 1 * 2 * q2q3; + q1q3_hess += b[48] * 1 * 1 * q2q4; + q1q3_hess += b[50] * 1 * 3 * q3q3; + q1q3_hess += b[51] * 1 * 2 * q3q4; + q1q3_hess += b[52] * 1 * 1 * q4q4; + + q1q4_hess += b[37] * 3 * 1 * q1q1; + q1q4_hess += b[40] * 2 * 1 * q1q2; + q1q4_hess += b[42] * 2 * 1 * q1q3; + q1q4_hess += b[43] * 2 * 2 * q1q4; + q1q4_hess += b[46] * 1 * 1 * q2q2; + q1q4_hess += b[48] * 1 * 1 * q2q3; + q1q4_hess += b[49] * 1 * 2 * q2q4; + q1q4_hess += b[51] * 1 * 1 * q3q3; + q1q4_hess += b[52] * 1 * 2 * q3q4; + q1q4_hess += b[53] * 1 * 3 * q4q4; + + q2q2_hess += b[38] * 2 * 1 * q1q1; + q2q2_hess += b[44] * 3 * 2 * q1q2; + q2q2_hess += b[45] * 1 * 2 * q1q3; + q2q2_hess += b[46] * 2 * 1 * q1q4; + q2q2_hess += b[54] * 4 * 3 * q2q2; + q2q2_hess += b[55] * 3 * 2 * q2q3; + q2q2_hess += b[56] * 3 * 2 * q2q4; + q2q2_hess += b[57] * 1 * 2 * q3q3; + q2q2_hess += b[58] * 1 * 2 * q3q4; + q2q2_hess += b[59] * 1 * 2 * q4q4; + + q2q3_hess += b[39] * 1 * 1 * q1q1; + q2q3_hess += b[45] * 2 * 1 * q1q2; + q2q3_hess += b[47] * 1 * 2 * q1q3; + q2q3_hess += b[48] * 1 * 1 * q1q4; + q2q3_hess += b[55] * 3 * 1 * q2q2; + q2q3_hess += b[57] * 2 * 2 * q2q3; + q2q3_hess += b[58] * 2 * 1 * q2q4; + q2q3_hess += b[60] * 1 * 3 * q3q3; + q2q3_hess += b[61] * 1 * 2 * q3q4; + q2q3_hess += b[62] * 1 * 1 * q4q4; + + q2q4_hess += b[63] * 1 * 3 * q4q4; + q2q4_hess += b[49] * 1 * 2 * q1q4; + q2q4_hess += b[62] * 1 * 2 * q3q4; + q2q4_hess += b[59] * 2 * 2 * q2q4; + q2q4_hess += b[58] * 2 * 1 * q2q3; + q2q4_hess += b[46] * 2 * 1 * q1q2; + q2q4_hess += b[40] * 1 * 1 * q1q1; + q2q4_hess += b[48] * 1 * 1 * q1q3; + q2q4_hess += b[61] * 1 * 1 * q3q3; + q2q4_hess += b[56] * 3 * 1 * q2q2; + + q3q3_hess += b[41] * 1 * 2 * q1q1; + q3q3_hess += b[47] * 1 * 2 * q1q2; + q3q3_hess += b[50] * 2 * 3 * q1q3; + q3q3_hess += b[51] * 1 * 2 * q1q4; + q3q3_hess += b[57] * 1 * 2 * q2q2; + q3q3_hess += b[60] * 2 * 3 * q2q3; + q3q3_hess += b[61] * 1 * 2 * q2q4; + q3q3_hess += b[64] * 4 * 3 * q3q3; + q3q3_hess += b[65] * 3 * 2 * q3q4; + q3q3_hess += b[66] * 2 * 1 * q4q4; + + q3q4_hess += b[42] * 1 * 1 * q1q1; + q3q4_hess += b[48] * 1 * 1 * q1q2; + q3q4_hess += b[51] * 2 * 1 * q1q3; + q3q4_hess += b[52] * 1 * 2 * q1q4; + q3q4_hess += b[58] * 1 * 1 * q2q2; + q3q4_hess += b[61] * 2 * 1 * q2q3; + q3q4_hess += b[62] * 1 * 2 * q2q4; + q3q4_hess += b[65] * 3 * 1 * q3q3; + q3q4_hess += b[66] * 2 * 2 * q3q4; + q3q4_hess += b[67] * 1 * 3 * q4q4; + + q4q4_hess += b[43] * 1 * 2 * q1q1; + q4q4_hess += b[49] * 1 * 2 * q1q2; + q4q4_hess += b[52] * 1 * 2 * q1q3; + q4q4_hess += b[53] * 2 * 3 * q1q4; + q4q4_hess += b[59] * 1 * 2 * q2q2; + q4q4_hess += b[62] * 1 * 2 * q2q3; + q4q4_hess += b[63] * 2 * 3 * q2q4; + q4q4_hess += b[66] * 1 * 2 * q3q3; + q4q4_hess += b[67] * 2 * 3 * q3q4; + q4q4_hess += b[68] * 3 * 4 * q4q4; + + Eigen::Matrix4d result_hess; + result_hess << q1q1_hess, q1q2_hess, q1q3_hess, q1q4_hess, q1q2_hess, + q2q2_hess, q2q3_hess, q2q4_hess, q1q3_hess, q2q3_hess, q3q3_hess, + q3q4_hess, q1q4_hess, q2q4_hess, q3q4_hess, q4q4_hess; + return std::move(result_hess); + } +} // namespace NPnP +#endif diff --git a/modules/rgbd/src/NPnP/QuaternionVector.h b/modules/rgbd/src/NPnP/QuaternionVector.h new file mode 100644 index 00000000000..eb2ca79a98a --- /dev/null +++ b/modules/rgbd/src/NPnP/QuaternionVector.h @@ -0,0 +1,134 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ +#ifndef PNP_SOLVER_QUATERNIONVECTOR_H +#define PNP_SOLVER_QUATERNIONVECTOR_H +#ifdef HAVE_EIGEN +#include "../Utils_Npnp/Definitions.h" + +namespace NPnP +{ + class QuaternionVector + { + public: + Eigen::Vector4d quaternions; + double q1; + double q2; + double q3; + double q4; + double q1q1; + double q1q2; + double q1q3; + double q1q4; + double q2q2; + double q2q3; + double q2q4; + double q3q3; + double q3q4; + double q4q4; + double q1q1q1; + double q1q1q2; + double q1q1q3; + double q1q1q4; + double q1q2q2; + double q1q2q3; + double q1q2q4; + double q1q3q3; + double q1q3q4; + double q1q4q4; + double q2q2q2; + double q2q2q3; + double q2q2q4; + double q2q3q3; + double q2q3q4; + double q2q4q4; + double q3q3q3; + double q3q3q4; + double q3q4q4; + double q4q4q4; + double q1q1q1q1; + double q1q1q1q2; + double q1q1q1q3; + double q1q1q1q4; + double q1q1q2q2; + double q1q1q2q3; + double q1q1q2q4; + double q1q1q3q3; + double q1q1q3q4; + double q1q1q4q4; + double q1q2q2q2; + double q1q2q2q3; + double q1q2q2q4; + double q1q2q3q3; + double q1q2q3q4; + double q1q2q4q4; + double q1q3q3q3; + double q1q3q3q4; + double q1q3q4q4; + double q1q4q4q4; + double q2q2q2q2; + double q2q2q2q3; + double q2q2q2q4; + double q2q2q3q3; + double q2q2q3q4; + double q2q2q4q4; + double q2q3q3q3; + double q2q3q3q4; + double q2q3q4q4; + double q2q4q4q4; + double q3q3q3q3; + double q3q3q3q4; + double q3q3q4q4; + double q3q4q4q4; + double q4q4q4q4; + + double q_norm_squared; + + QuaternionVector(const Eigen::Vector4d &quaternions); + + double obj_func(const ColVector &b) const; + Eigen::Vector4d obj_grad(const ColVector &b) const; + Eigen::Matrix4d obj_hess(const ColVector &b) const; + }; +} // namespace NPnP + +#endif +#endif // PNP_SOLVER_QUATERNIONVECTOR_H diff --git a/modules/rgbd/src/NewtonPnP.cpp b/modules/rgbd/src/NewtonPnP.cpp new file mode 100644 index 00000000000..221da0ba26f --- /dev/null +++ b/modules/rgbd/src/NewtonPnP.cpp @@ -0,0 +1,158 @@ +#ifdef HAVE_EIGEN +#include "NewtonPnP.h" +#include +#include +#include + +// ----------------------------------------- +// Find camera pose from rotation matrix and translation vector +// ----------------------------------------- +bool NewtonPnP::calculate_camera_pose(const Eigen::Matrix3d &rmat, + const Eigen::Vector3d &tvec, + Point3d &camera_pose) +{ + auto pos = -(rmat.transpose()) * tvec; + + if (isnan(pos[0])) + { + return false; + } + camera_pose = Point3d(pos[0], pos[1], pos[2]); + return true; +} + +// ----------------------------------------- +// Optimal PnP Solver +// ----------------------------------------- +void NewtonPnP::optimal_pnp_solver( + const std::vector &points, + const std::vector &lines, + const std::vector &weights, const std::vector &indices, + Eigen::Matrix3d &rmat, Eigen::Vector3d &tvec) +{ + // pnp fucntion call and output + auto pnp_input = PnpInput::init(points, lines, weights, indices); + auto pnp_objective = PnpObjective::init(pnp_input); + auto pnp_res = + this->pnp->solve_pnp(pnp_objective, this->barrier_method_settings); + rmat = pnp_res.rotation_matrix().transpose(); + tvec = pnp_res.translation_vector(); +} + +// ----------------------------------------- +// Reproject points +// ----------------------------------------- +void NewtonPnP::reproject_points(const vector &obj_points, + const Eigen::Matrix3d &rmat, + const Eigen::Vector3d &tvec, + vector &image_points) +{ + auto R = rmat; + auto t = tvec; + + uint nPoints = obj_points.size(); + + image_points.reserve(nPoints); + for (uint i = 0; i < nPoints; i++) + { + const cv::Point3d &p3d = obj_points[i]; + double Xc = R.coeff(0, 0) * p3d.x + R.coeff(0, 1) * p3d.y + + R.coeff(0, 2) * p3d.z + t.coeff(0); + double Yc = R.coeff(1, 0) * p3d.x + R.coeff(1, 1) * p3d.y + + R.coeff(1, 2) * p3d.z + t.coeff(1); + double Zc = R.coeff(2, 0) * p3d.x + R.coeff(2, 1) * p3d.y + + R.coeff(2, 2) * p3d.z + t.coeff(2); + double invZc = 1 / Zc; + + double ue = uc + fu * Xc * invZc; + double ve = vc + fv * Yc * invZc; + image_points.emplace_back(ue, ve); + } +} + +// ----------------------------------------- +// PnP caller: +// ----------------------------------------- +bool NewtonPnP::newton_pnp(const vector &scene, + const vector &obj, + FramePose &frame_pose) +{ + // Prepare inliers + // ----------------------------------------- + vector inliers_scene; + vector inliers_obj; + + for (uint i = 0; i < scene.size(); i++) + { + + inliers_scene.push_back(scene[i]); + inliers_obj.push_back(obj[i]); + } + + uint nPnts = inliers_scene.size(); + + std::vector weights(nPnts, 1); + std::vector indexes(nPnts); + + //--- fill indexes with 0,1,...,nPnts-1 + std::iota(begin(indexes), end(indexes), 0); + // ----------------------------------------- + + // Convert matched pixels to lines + // ----------------------------------------- + std::vector lines_double; + lines_double.reserve(nPnts); + + for (uint i = 0; i < nPnts; i++) + { + // 2D point from the scene (from keypoint) + const cv::Point2f &point2d = inliers_scene[i]; + cv::Mat pt = (cv::Mat_(3, 1) << point2d.x, point2d.y, 1); // + cv::Mat templine = this->invK * pt; + templine = templine / norm(templine); + cv::Point3f v(templine); + lines_double.push_back(v); + } + // ----------------------------------------- + + // convert to Eigen: + // ----------------------------------------- + std::vector points_vector; + std::vector lines_vector; + points_vector.reserve(nPnts); + lines_vector.reserve(nPnts); + // + for (uint i = 0; i < nPnts; i++) + { + lines_vector.emplace_back(lines_double[i].x, lines_double[i].y, + lines_double[i].z); + points_vector.emplace_back(inliers_obj[i].x, inliers_obj[i].y, + inliers_obj[i].z); + } // + // ----------------------------------------- + + // Solver + // ----------------------------------------- + Eigen::Matrix3d rmat; + Eigen::Vector3d tvec; + this->optimal_pnp_solver(points_vector, lines_vector, weights, indexes, rmat, tvec); + + frame_pose.set_values(rmat, tvec); + if (!calculate_camera_pose(frame_pose.rmat, frame_pose.tvec, frame_pose.cam_pose)) + { + std::cout << "BAD POSE" << std::endl; + return false; + } // + + vector scene_reproject; + + cv::Mat cv_R, cv_t; + frame_pose.R.convertTo(cv_R, CV_64F); + frame_pose.t.convertTo(cv_t, CV_64F); + projectPoints(obj, cv_R, cv_t, this->camera_matrix, cv::Mat(), scene_reproject); + + CV_LOG_INFO(NULL, "using Newton PnP"); + + return true; +} +#endif \ No newline at end of file diff --git a/modules/rgbd/src/NewtonPnP.h b/modules/rgbd/src/NewtonPnP.h new file mode 100644 index 00000000000..e6bb024ee2c --- /dev/null +++ b/modules/rgbd/src/NewtonPnP.h @@ -0,0 +1,96 @@ +#ifndef _RANSAC_OPTIMAL_PNP_H +#define _RANSAC_OPTIMAL_PNP_H +#ifdef HAVE_EIGEN +#include "NPnP/BarrierMethodSettings.h" +#include "NPnP/NPnpInput.h" +#include "NPnP/NPnpObjective.h" +#include "NPnP/NPnpProblemSolver.h" +#include "Utils_Npnp/Definitions.h" +#include +#include +#include +#include +#include +#include + +#include "FramePose/FramePose.h" + +using namespace std; +using namespace cv; +using namespace NPnP; +using Eigen::MatrixXd; + +class NewtonPnP +{ +public: + // Ransac optimal PnP constructor, initialize options + NewtonPnP( cv::Mat &cameramatrix, + cv::Mat &distcoeffs) + : camera_matrix(cameramatrix), dist_coeffs(distcoeffs) + { + + this->pnp = PnpProblemSolver::init(); + this->barrier_method_settings = BarrierMethodSettings::init(1E-14, 20, false, 1000, 50, 0.001); + camera_matrix.convertTo(K, CV_32F); + this->invK = K.inv(); + + dist_coeffs.convertTo(dist_coeffs_float, CV_32F); + + this->rng = mt19937(chrono::steady_clock::now().time_since_epoch().count()); + + this->uc = camera_matrix.at(0, 2); + this->vc = camera_matrix.at(1, 2); + this->fu = camera_matrix.at(0, 0); + this->fv = camera_matrix.at(1, 1); + } + + // Ransac optimal PnP solver robust to outliers + bool ransac_solve_pnp(const vector &scene, + const vector &obj, vector &is_inliers, + int &inliers_cnt_result, FramePose &frame_pose); + + // PnP caller + bool optimal_pnp(const vector &scene, const vector &obj, + vector &is_inliers, int &inliers_cnt_result, + FramePose &frame_pose); + + // PnP caller + bool newton_pnp(const vector &scene, const vector &obj, FramePose &frame_pose); + +private: + // Find camera pose from rotation matrix and translation vector + bool calculate_camera_pose(const Eigen::Matrix3d &rmat, + const Eigen::Vector3d &tvec, Point3d &camera_pose); + + // Optimal PnP Solver, calculate rotation matrix and translation vector + void optimal_pnp_solver(const std::vector &points, + const std::vector &lines, + const std::vector &weights, + const std::vector &indices, + Eigen::Matrix3d &rmat, Eigen::Vector3d &tvec); + + // Reproject points + void reproject_points(const vector &obj_points, + const Eigen::Matrix3d &rmat, + const Eigen::Vector3d &tvec, + vector &image_points); + + // Find inliers by distance from reprojection + int check_inliers(const vector &scene, + const vector &scene_reproj, + vector &is_inliers); + +private: + cv::Mat camera_matrix; + cv::Mat dist_coeffs; + cv::Mat dist_coeffs_float; + cv::Mat K; + cv::Mat invK; + std::shared_ptr pnp; + std::shared_ptr barrier_method_settings; + mt19937 rng; + + double uc, vc, fu, fv; +}; +#endif +#endif diff --git a/modules/rgbd/src/RansacOptimalNPnP/RansacOptimalNPnP.cpp b/modules/rgbd/src/RansacOptimalNPnP/RansacOptimalNPnP.cpp new file mode 100644 index 00000000000..79b8667876a --- /dev/null +++ b/modules/rgbd/src/RansacOptimalNPnP/RansacOptimalNPnP.cpp @@ -0,0 +1,293 @@ +#ifdef HAVE_EIGEN +#include "RansacOptimalNPnP.h" +#include +#include + +// ----------------------------------------- +// Find camera pose from rotation matrix and translation vector +// ----------------------------------------- +bool RansacOptimalPnP::calculate_camera_pose(const Eigen::Matrix3d &rmat, + const Eigen::Vector3d &tvec, + Point3d &camera_pose) +{ + auto pos = -(rmat.transpose()) * tvec; + + if (isnan(pos[0])) + { + return false; + } + camera_pose = Point3d(pos[0], pos[1], pos[2]); + return true; +} + +// ----------------------------------------- +// Optimal PnP Solver +// ----------------------------------------- +void RansacOptimalPnP::optimal_pnp_solver( + const std::vector &points, + const std::vector &lines, + const std::vector &weights, const std::vector &indices, + Eigen::Matrix3d &rmat, Eigen::Vector3d &tvec) +{ + // pnp fucntion call and output + auto pnp_input = PnpInput::init(points, lines, weights, indices); + auto pnp_objective = PnpObjective::init(pnp_input); + auto pnp_res = + this->pnp->solve_pnp(pnp_objective, this->barrier_method_settings); + rmat = pnp_res.rotation_matrix().transpose(); + tvec = pnp_res.translation_vector(); +} + +// ----------------------------------------- +// Reproject points +// ----------------------------------------- +void RansacOptimalPnP::reproject_points(const vector &obj_points, + const Eigen::Matrix3d &rmat, + const Eigen::Vector3d &tvec, + vector &image_points) +{ + auto R = rmat; + auto t = tvec; + + uint nPoints = obj_points.size(); + + image_points.reserve(nPoints); + for (uint i = 0; i < nPoints; i++) + { + const cv::Point3d &p3d = obj_points[i]; + double Xc = R.coeff(0, 0) * p3d.x + R.coeff(0, 1) * p3d.y + + R.coeff(0, 2) * p3d.z + t.coeff(0); + double Yc = R.coeff(1, 0) * p3d.x + R.coeff(1, 1) * p3d.y + + R.coeff(1, 2) * p3d.z + t.coeff(1); + double Zc = R.coeff(2, 0) * p3d.x + R.coeff(2, 1) * p3d.y + + R.coeff(2, 2) * p3d.z + t.coeff(2); + double invZc = 1 / Zc; + + double ue = uc + fu * Xc * invZc; + double ve = vc + fv * Yc * invZc; + image_points.emplace_back(ue, ve); + } +} + +// ----------------------------------------- +// Find inliers by distance from reprojection +// ----------------------------------------- +int RansacOptimalPnP::check_inliers(const vector &scene, + const vector &scene_reproj, + vector &is_inliers) +{ + int inliers_cnt = 0; + is_inliers.resize(scene.size()); + + for (uint i = 0; i < scene.size(); i++) + { + const cv::Point2d &p2d = scene[i]; + const cv::Point2d &p2d_reproj = scene_reproj[i]; + double distX = p2d.x - p2d_reproj.x; + double distY = p2d.y - p2d_reproj.y; + + double err = distX * distX + distY * distY; + + if (err <= this->max_error_threshold) + { + inliers_cnt++; + is_inliers[i] = true; + } + else + { + is_inliers[i] = false; + } + } + return inliers_cnt; +} + +// ----------------------------------------- +// PnP caller: +// ----------------------------------------- +bool RansacOptimalPnP::optimal_pnp(const vector &scene, + const vector &obj, + vector &is_inliers, + int &inliers_cnt_result, + FramePose &frame_pose) +{ + // Prepare inliers + // ----------------------------------------- + vector inliers_scene; + vector inliers_obj; + + for (uint i = 0; i < scene.size(); i++) + { + if (is_inliers[i]) + { + inliers_scene.push_back(scene[i]); + inliers_obj.push_back(obj[i]); + } + } + + uint nPnts = inliers_scene.size(); + + std::vector weights(nPnts, 1); + std::vector indexes(nPnts); + + //--- fill indexes with 0,1,...,nPnts-1 + std::iota(begin(indexes), end(indexes), 0); + // ----------------------------------------- + + // Convert matched pixels to lines + // ----------------------------------------- + std::vector lines_double; + lines_double.reserve(nPnts); + + for (uint i = 0; i < nPnts; i++) + { + // 2D point from the scene (from keypoint) + const cv::Point2f &point2d = inliers_scene[i]; + cv::Mat pt = (cv::Mat_(3, 1) << point2d.x, point2d.y, 1); // + cv::Mat templine = this->invK * pt; + templine = templine / norm(templine); + cv::Point3f v(templine); + lines_double.push_back(v); + } + // ----------------------------------------- + + // convert to Eigen: + // ----------------------------------------- + std::vector points_vector; + std::vector lines_vector; + points_vector.reserve(nPnts); + lines_vector.reserve(nPnts); + // + for (uint i = 0; i < nPnts; i++) + { + lines_vector.emplace_back(lines_double[i].x, lines_double[i].y, + lines_double[i].z); + points_vector.emplace_back(inliers_obj[i].x, inliers_obj[i].y, + inliers_obj[i].z); + } // + // ----------------------------------------- + + // Solver + // ----------------------------------------- + Eigen::Matrix3d rmat; + Eigen::Vector3d tvec; + this->optimal_pnp_solver(points_vector, lines_vector, weights, indexes, rmat, tvec); + + frame_pose.set_values(rmat, tvec); + if (!calculate_camera_pose(frame_pose.rmat, frame_pose.tvec, frame_pose.cam_pose)) + { + std::cout << "BAD POSE" << std::endl; + return false; + } // + + vector scene_reproject; + + cv::Mat cv_R, cv_t; + frame_pose.R.convertTo(cv_R, CV_64F); + frame_pose.t.convertTo(cv_t, CV_64F); + projectPoints(obj, cv_R, cv_t, this->camera_matrix, cv::Mat(), scene_reproject); + + inliers_cnt_result = this->check_inliers(scene, scene_reproject, is_inliers); + return true; +} + +// ----------------------------------------- +// Ransac optimal PnP solver robust to outliers +// ----------------------------------------- +bool RansacOptimalPnP::ransac_solve_pnp(const vector &scene, + const vector &obj, + vector &is_inliers, + int &inliers_cnt_result, + FramePose &frame_pose) +{ + bool result = false; + int curr_iter = 0; + int max_inliers_cnt = 0; + vector max_inliers; + + int success_inliers_cnt = this->min_inliers; + int stop_inliers_cnt = success_inliers_cnt * 2; + + uint nPnts = scene.size(); + + // fill available_index with [0,1,...,nPnts-1] + vector available_index(nPnts); + std::iota(begin(available_index), end(available_index), 0); + + + while (curr_iter < this->max_iterations) + { + curr_iter++; + // Solve pnp on 4 random points + vector curr_inliers(nPnts, false); + + for (uint i = 0; i < 4; i++) + { // 4 points per iteration + int rand_ind = uniform_int_distribution(0, nPnts - 1 - i)(this->rng); + int idx = available_index[rand_ind]; + + curr_inliers[idx] = true; + + available_index[rand_ind] = available_index[nPnts - 1 - i]; + available_index[nPnts - 1 - i] = idx; + } + + int curr_inliers_cnt; + + bool res = this->optimal_pnp(scene, obj, curr_inliers, curr_inliers_cnt, frame_pose); + + + if (res) + { + if (max_inliers_cnt < curr_inliers_cnt) + { + max_inliers_cnt = curr_inliers_cnt; + max_inliers = curr_inliers; + } + } + + if (max_inliers_cnt >= stop_inliers_cnt && + curr_iter >= this->min_iterations) + { + break; + } + } + + if (max_inliers_cnt >= success_inliers_cnt) + { + + bool res = this->optimal_pnp(scene, obj, max_inliers, max_inliers_cnt, frame_pose); + + if (res && max_inliers_cnt >= success_inliers_cnt) + { + result = true; + + //--- Refine pose + vector refine_inliers; + int refine_inliers_cnt; + FramePose refined_frame_pose; + + for (int i = 0; i < 2; i++) + { + refine_inliers = max_inliers; + refine_inliers_cnt = max_inliers_cnt; + bool resu = this->optimal_pnp(scene, obj, refine_inliers, refine_inliers_cnt, refined_frame_pose); + + if (resu && refine_inliers_cnt > max_inliers_cnt) + { + max_inliers = refine_inliers; + max_inliers_cnt = refine_inliers_cnt; + frame_pose = refined_frame_pose; + } + else + { + break; + } + } + } + } + + is_inliers = max_inliers; + inliers_cnt_result = max_inliers_cnt; + return result; +} +#endif diff --git a/modules/rgbd/src/RansacOptimalNPnP/RansacOptimalNPnP.h b/modules/rgbd/src/RansacOptimalNPnP/RansacOptimalNPnP.h new file mode 100644 index 00000000000..c8c41181b0b --- /dev/null +++ b/modules/rgbd/src/RansacOptimalNPnP/RansacOptimalNPnP.h @@ -0,0 +1,101 @@ +#ifndef _RANSAC_OPTIMAL_PNP_H +#define _RANSAC_OPTIMAL_PNP_H +#ifdef HAVE_EIGEN +#include "../NPnP/BarrierMethodSettings.h" +#include "../NPnP/NPnpInput.h" +#include "../NPnP/NPnpObjective.h" +#include "../NPnP/NPnpProblemSolver.h" +#include "../Utils_Npnp/Definitions.h" +#include +#include +#include +#include +#include +#include + +#include "../FramePose/FramePose.h" + +using namespace std; +using namespace cv; +using namespace NPnP; +using Eigen::MatrixXd; + +class RansacOptimalPnP +{ +public: + // Ransac optimal PnP constructor, initialize options + RansacOptimalPnP(int a_min_iterations, int a_max_iterations, int a_min_inliers, + int a_max_error_threshold, const cv::Mat &a_camera_matrix, + const cv::Mat &a_dist_coeffs) + : min_iterations(a_min_iterations), max_iterations(a_max_iterations), + min_inliers(a_min_inliers), max_error_threshold(a_max_error_threshold), + camera_matrix(a_camera_matrix), dist_coeffs(a_dist_coeffs) + { + + this->pnp = PnpProblemSolver::init(); + this->barrier_method_settings = BarrierMethodSettings::init(1E-14, 20, false, 1000, 50, 0.001); + camera_matrix.convertTo(K, CV_32F); + this->invK = K.inv(); + + dist_coeffs.convertTo(dist_coeffs_float, CV_32F); + + this->rng = mt19937(chrono::steady_clock::now().time_since_epoch().count()); + + this->uc = camera_matrix.at(0, 2); + this->vc = camera_matrix.at(1, 2); + this->fu = camera_matrix.at(0, 0); + this->fv = camera_matrix.at(1, 1); + } + + // Ransac optimal PnP solver robust to outliers + bool ransac_solve_pnp(const vector &scene, + const vector &obj, vector &is_inliers, + int &inliers_cnt_result, FramePose &frame_pose); + + // PnP caller + bool optimal_pnp(const vector &scene, const vector &obj, + vector &is_inliers, int &inliers_cnt_result, + FramePose &frame_pose); + +private: + // Find camera pose from rotation matrix and translation vector + bool calculate_camera_pose(const Eigen::Matrix3d &rmat, + const Eigen::Vector3d &tvec, Point3d &camera_pose); + + // Optimal PnP Solver, calculate rotation matrix and translation vector + void optimal_pnp_solver(const std::vector &points, + const std::vector &lines, + const std::vector &weights, + const std::vector &indices, + Eigen::Matrix3d &rmat, Eigen::Vector3d &tvec); + + // Reproject points + void reproject_points(const vector &obj_points, + const Eigen::Matrix3d &rmat, + const Eigen::Vector3d &tvec, + vector &image_points); + + // Find inliers by distance from reprojection + int check_inliers(const vector &scene, + const vector &scene_reproj, + vector &is_inliers); + +private: + const int min_iterations, max_iterations; + const int min_inliers; + // const double min_inliers_ratio, max_inliers_ratio; + const double max_error_threshold; + const cv::Mat camera_matrix; + cv::Mat dist_coeffs; + cv::Mat dist_coeffs_float; + cv::Mat K; + cv::Mat invK; + std::shared_ptr pnp; + std::shared_ptr barrier_method_settings; + mt19937 rng; + + double uc, vc, fu, fv; +}; + +#endif +#endif diff --git a/modules/rgbd/src/Utils_Npnp/Definitions.h b/modules/rgbd/src/Utils_Npnp/Definitions.h new file mode 100644 index 00000000000..55e7401cd15 --- /dev/null +++ b/modules/rgbd/src/Utils_Npnp/Definitions.h @@ -0,0 +1,30 @@ +#ifndef PNP_USING_EIGEN_LIBRARY_DEFINITIONS_H +#define PNP_USING_EIGEN_LIBRARY_DEFINITIONS_H +#ifdef HAVE_EIGEN +#include "Definitions.h" +#include +#include +#include + +namespace NPnP +{ + template + using ColVector = Eigen::Matrix; + template + using RowVector = Eigen::Matrix; + + template + using ColMatrix = Eigen::Matrix; + template + using RowMatrix = Eigen::Matrix; + + typedef Eigen::SparseMatrix SparseRowMatrix; + typedef Eigen::SparseMatrix SparseColMatrix; + +#define Y_SIZE 69 +#define NUM_CONSTRAINTS 15 +#define M_MATRIX_DIM 15 +#define A_ROWS 240 +} // namespace NPnP +#endif +#endif // PNP_USING_EIGEN_LIBRARY_DEFINITIONS_H diff --git a/modules/rgbd/src/Utils_Npnp/GeneralUtils.cpp b/modules/rgbd/src/Utils_Npnp/GeneralUtils.cpp new file mode 100644 index 00000000000..e2aa004521d --- /dev/null +++ b/modules/rgbd/src/Utils_Npnp/GeneralUtils.cpp @@ -0,0 +1,23 @@ +#ifdef HAVE_EIGEN +#include "GeneralUtils.h" + +namespace NPnP +{ + double find_zero_bin_search(const std::function &func, + double min, double max, int depth) + { + while (true) + { + double mid = (min + max) / 2.0; + if (depth-- == 0) + return mid; + if (func(max) <= 0) + return max; + if (func(mid) < 0) + min = mid; + else + max = mid; + } + } +} // namespace NPnP +#endif \ No newline at end of file diff --git a/modules/rgbd/src/Utils_Npnp/GeneralUtils.h b/modules/rgbd/src/Utils_Npnp/GeneralUtils.h new file mode 100644 index 00000000000..8d59e30d343 --- /dev/null +++ b/modules/rgbd/src/Utils_Npnp/GeneralUtils.h @@ -0,0 +1,20 @@ +#ifndef PNP_USING_EIGEN_LIBRARY_GENERALUTILS_H +#define PNP_USING_EIGEN_LIBRARY_GENERALUTILS_H +#ifdef HAVE_EIGEN +#include "Definitions.h" +#include "functional" + +namespace NPnP +{ + double find_zero_bin_search(const std::function &func, + double min, double max, int depth); + + template + inline T min2(T one, T two) + { + return one < two ? one : two; + } +} // namespace NPnP + +#endif +#endif // PNP_USING_EIGEN_LIBRARY_GENERALUTILS_H diff --git a/modules/rgbd/src/Utils_Npnp/MatrixUtils.h b/modules/rgbd/src/Utils_Npnp/MatrixUtils.h new file mode 100644 index 00000000000..3154196ad1c --- /dev/null +++ b/modules/rgbd/src/Utils_Npnp/MatrixUtils.h @@ -0,0 +1,15 @@ +#ifndef PNP_USING_EIGEN_LIBRARY_MATRIXUTILS_H +#define PNP_USING_EIGEN_LIBRARY_MATRIXUTILS_H +#ifdef HAVE_EIGEN +#include "Definitions.h" + +namespace NPnP +{ + template + void symmetrize(RowMatrix &matrix) + { + matrix = 0.5 * (matrix + matrix.transpose().eval()); + } +} // namespace NPnP +#endif +#endif // PNP_USING_EIGEN_LIBRARY_MATRIXUTILS_H diff --git a/modules/rgbd/src/Utils_Npnp/Parsing.cpp b/modules/rgbd/src/Utils_Npnp/Parsing.cpp new file mode 100644 index 00000000000..991064f9cc0 --- /dev/null +++ b/modules/rgbd/src/Utils_Npnp/Parsing.cpp @@ -0,0 +1,57 @@ +#ifdef HAVE_EIGEN +#include "Parsing.h" +#include "Definitions.h" +#include +#include +#include +#include + +namespace NPnP +{ + std::vector parse_csv_vector_3d(const std::string &csv_path) + { + std::vector data; + std::ifstream input_file(csv_path); + std::string line; + while (input_file >> line) + { + std::stringstream lineStream(line); + std::vector current_vec; + std::string cell; + while (std::getline(lineStream, cell, ',')) + { + current_vec.push_back(std::stod(cell)); + } + data.emplace_back(current_vec[0], current_vec[1], current_vec[2]); + } + + return std::move(data); + } + + Eigen::MatrixXd parse_csv_matrix(const std::string &csv_path) + { + std::vector> data; + std::ifstream input_file(csv_path); + std::string line; + while (input_file >> line) + { + std::stringstream lineStream(line); + std::vector current_vec; + std::string cell; + while (std::getline(lineStream, cell, ',')) + { + current_vec.push_back(std::stod(cell)); + } + Eigen::VectorXd vector = Eigen::Map( + current_vec.data(), current_vec.size()); + data.emplace_back(std::move(current_vec)); + } + Eigen::MatrixXd matrix(data.size(), data[0].size()); + for (int i = 0; i < data.size(); i++) + for (int j = 0; j < data[i].size(); j++) + matrix(i, j) = data[i][j]; + + return matrix; + } +} // namespace NPnP +#endif diff --git a/modules/rgbd/src/Utils_Npnp/Parsing.h b/modules/rgbd/src/Utils_Npnp/Parsing.h new file mode 100644 index 00000000000..4f44b9c62ea --- /dev/null +++ b/modules/rgbd/src/Utils_Npnp/Parsing.h @@ -0,0 +1,29 @@ +#ifndef PNP_USING_EIGEN_LIBRARY_PARSING_H +#define PNP_USING_EIGEN_LIBRARY_PARSING_H +#ifdef HAVE_EIGEN +#include "Definitions.h" +#include +#include +#include +#include + +namespace NPnP +{ + template + std::vector parse_csv_array(const std::string &csv_path) + { + std::vector data; + std::ifstream input_file(csv_path); + T value; + while (input_file >> value) + data.push_back(value); + + return std::move(data); + } + + std::vector parse_csv_vector_3d(const std::string &csv_path); + + Eigen::MatrixXd parse_csv_matrix(const std::string &csv_path); +} // namespace NPnP +#endif +#endif // PNP_USING_EIGEN_LIBRARY_PARSING_H