diff --git a/modules/rgbd/src/FramePose/FramePose.h b/modules/rgbd/src/FramePose/FramePose.h
index 89b73f2eb6b..e164c6bd2f4 100644
--- a/modules/rgbd/src/FramePose/FramePose.h
+++ b/modules/rgbd/src/FramePose/FramePose.h
@@ -1,20 +1,64 @@
+/*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 
+#ifdef HAVE_EIGEN
 #include <Eigen/Dense>
 #include <opencv2/core.hpp>
 #include <opencv2/core/eigen.hpp>
 
-class FramePose {
+class FramePose
+{
 
 public:
   FramePose() {}
 
-  FramePose(Eigen::Matrix3d &r_mat, Eigen::Vector3d &t_vec) {
+  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) {
+  void set_values(Eigen::Matrix3d &r_mat, Eigen::Vector3d &t_vec)
+  {
     this->rmat = r_mat;
     this->tvec = t_vec;
 
@@ -30,7 +74,8 @@ class FramePose {
   cv::Mat cam_pose_mat;
 
 private:
-  void calculate_values() {
+  void calculate_values()
+  {
     eigen2cv(rmat, R);
     eigen2cv(tvec, t);
     R.convertTo(R, CV_32F);
diff --git a/modules/rgbd/src/NPnP/NPnpInput.h b/modules/rgbd/src/NPnP/NPnpInput.h
index be27b3ca9dc..d3b247afca7 100644
--- a/modules/rgbd/src/NPnP/NPnpInput.h
+++ b/modules/rgbd/src/NPnP/NPnpInput.h
@@ -66,7 +66,7 @@ namespace NPnP
                                           std::vector<Eigen::Vector3d> lines,
                                           std::vector<double> weights,
                                           std::vector<int> indices);
-                                          
+
     static std::shared_ptr<PnpInput> parse_input(int argc, char **pString);
   };
 } // namespace NPnP
diff --git a/modules/rgbd/src/NPnP/NPnpObjective.cpp b/modules/rgbd/src/NPnP/NPnpObjective.cpp
index 5bbbdfc8a0e..c4527ae2241 100644
--- a/modules/rgbd/src/NPnP/NPnpObjective.cpp
+++ b/modules/rgbd/src/NPnP/NPnpObjective.cpp
@@ -47,190 +47,190 @@
 
 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();
-    }
+        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>
-    PnpObjective::init(std::shared_ptr<PnpInput> input)
-    {
-        PnpObjective objective;
-        objective.M.setZero();
-        objective.T.setZero();
-        objective.b.setZero();
-        objective.sum_weights = 0.0;
+        std::shared_ptr<PnpObjective>
+        PnpObjective::init(std::shared_ptr<PnpInput> 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<RowMatrix<3, 3>> Q;
+                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<RowMatrix<3, 3>> 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;
+                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;
+                        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;
+                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<PnpObjective>(objective);
         }
-        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<PnpObjective>(objective);
-    }
 } // namespace NPnP
 #endif
diff --git a/modules/rgbd/src/Utils_Npnp/Definitions.h b/modules/rgbd/src/Utils_Npnp/Definitions.h
index ca7fd4b20d2..55e7401cd15 100644
--- a/modules/rgbd/src/Utils_Npnp/Definitions.h
+++ b/modules/rgbd/src/Utils_Npnp/Definitions.h
@@ -6,19 +6,20 @@
 #include <Eigen/Dense>
 #include <Eigen/Sparse>
 
-namespace NPnP {
-template <int ROWS>
-using ColVector = Eigen::Matrix<double, ROWS, 1, Eigen::ColMajor>;
-template <int COLS>
-using RowVector = Eigen::Matrix<double, 1, COLS, Eigen::RowMajor>;
+namespace NPnP
+{
+    template <int ROWS>
+    using ColVector = Eigen::Matrix<double, ROWS, 1, Eigen::ColMajor>;
+    template <int COLS>
+    using RowVector = Eigen::Matrix<double, 1, COLS, Eigen::RowMajor>;
 
-template <int ROWS, int COLS>
-using ColMatrix = Eigen::Matrix<double, ROWS, COLS, Eigen::ColMajor>;
-template <int ROWS, int COLS>
-using RowMatrix = Eigen::Matrix<double, ROWS, COLS, Eigen::RowMajor>;
+    template <int ROWS, int COLS>
+    using ColMatrix = Eigen::Matrix<double, ROWS, COLS, Eigen::ColMajor>;
+    template <int ROWS, int COLS>
+    using RowMatrix = Eigen::Matrix<double, ROWS, COLS, Eigen::RowMajor>;
 
-typedef Eigen::SparseMatrix<double, Eigen::RowMajor> SparseRowMatrix;
-typedef Eigen::SparseMatrix<double, Eigen::ColMajor> SparseColMatrix;
+    typedef Eigen::SparseMatrix<double, Eigen::RowMajor> SparseRowMatrix;
+    typedef Eigen::SparseMatrix<double, Eigen::ColMajor> SparseColMatrix;
 
 #define Y_SIZE 69
 #define NUM_CONSTRAINTS 15
diff --git a/modules/rgbd/src/Utils_Npnp/GeneralUtils.cpp b/modules/rgbd/src/Utils_Npnp/GeneralUtils.cpp
index 223a2492e7f..e2aa004521d 100644
--- a/modules/rgbd/src/Utils_Npnp/GeneralUtils.cpp
+++ b/modules/rgbd/src/Utils_Npnp/GeneralUtils.cpp
@@ -1,20 +1,23 @@
 #ifdef HAVE_EIGEN
 #include "GeneralUtils.h"
 
-namespace NPnP {
-double find_zero_bin_search(const std::function<double(double)> &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
+{
+  double find_zero_bin_search(const std::function<double(double)> &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
index 20e41a5a689..8d59e30d343 100644
--- a/modules/rgbd/src/Utils_Npnp/GeneralUtils.h
+++ b/modules/rgbd/src/Utils_Npnp/GeneralUtils.h
@@ -4,13 +4,16 @@
 #include "Definitions.h"
 #include "functional"
 
-namespace NPnP {
-double find_zero_bin_search(const std::function<double(double)> &func,
-                            double min, double max, int depth);
+namespace NPnP
+{
+  double find_zero_bin_search(const std::function<double(double)> &func,
+                              double min, double max, int depth);
 
-template <typename T> inline T min2(T one, T two) {
-  return one < two ? one : two;
-}
+  template <typename T>
+  inline T min2(T one, T two)
+  {
+    return one < two ? one : two;
+  }
 } // namespace NPnP
 
 #endif
diff --git a/modules/rgbd/src/Utils_Npnp/MatrixUtils.h b/modules/rgbd/src/Utils_Npnp/MatrixUtils.h
index 74bf55fb54a..3154196ad1c 100644
--- a/modules/rgbd/src/Utils_Npnp/MatrixUtils.h
+++ b/modules/rgbd/src/Utils_Npnp/MatrixUtils.h
@@ -3,10 +3,13 @@
 #ifdef HAVE_EIGEN
 #include "Definitions.h"
 
-namespace NPnP {
-template <int Size> void symmetrize(RowMatrix<Size, Size> &matrix) {
-  matrix = 0.5 * (matrix + matrix.transpose().eval());
-}
+namespace NPnP
+{
+  template <int Size>
+  void symmetrize(RowMatrix<Size, Size> &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
index f9bbdd2a2ea..991064f9cc0 100644
--- a/modules/rgbd/src/Utils_Npnp/Parsing.cpp
+++ b/modules/rgbd/src/Utils_Npnp/Parsing.cpp
@@ -6,45 +6,52 @@
 #include <string>
 #include <vector>
 
-namespace NPnP {
-std::vector<Eigen::Vector3d> parse_csv_vector_3d(const std::string &csv_path) {
-  std::vector<Eigen::Vector3d> data;
-  std::ifstream input_file(csv_path);
-  std::string line;
-  while (input_file >> line) {
-    std::stringstream lineStream(line);
-    std::vector<double> current_vec;
-    std::string cell;
-    while (std::getline(lineStream, cell, ',')) {
-      current_vec.push_back(std::stod(cell));
+namespace NPnP
+{
+  std::vector<Eigen::Vector3d> parse_csv_vector_3d(const std::string &csv_path)
+  {
+    std::vector<Eigen::Vector3d> data;
+    std::ifstream input_file(csv_path);
+    std::string line;
+    while (input_file >> line)
+    {
+      std::stringstream lineStream(line);
+      std::vector<double> 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]);
     }
-    data.emplace_back(current_vec[0], current_vec[1], current_vec[2]);
-  }
 
-  return std::move(data);
-}
+    return std::move(data);
+  }
 
-Eigen::MatrixXd parse_csv_matrix(const std::string &csv_path) {
-  std::vector<std::vector<double>> data;
-  std::ifstream input_file(csv_path);
-  std::string line;
-  while (input_file >> line) {
-    std::stringstream lineStream(line);
-    std::vector<double> current_vec;
-    std::string cell;
-    while (std::getline(lineStream, cell, ',')) {
-      current_vec.push_back(std::stod(cell));
+  Eigen::MatrixXd parse_csv_matrix(const std::string &csv_path)
+  {
+    std::vector<std::vector<double>> data;
+    std::ifstream input_file(csv_path);
+    std::string line;
+    while (input_file >> line)
+    {
+      std::stringstream lineStream(line);
+      std::vector<double> current_vec;
+      std::string cell;
+      while (std::getline(lineStream, cell, ','))
+      {
+        current_vec.push_back(std::stod(cell));
+      }
+      Eigen::VectorXd vector = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
+          current_vec.data(), current_vec.size());
+      data.emplace_back(std::move(current_vec));
     }
-    Eigen::VectorXd vector = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
-        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];
+    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;
-}
+    return matrix;
+  }
 } // namespace NPnP
 #endif
diff --git a/modules/rgbd/src/Utils_Npnp/Parsing.h b/modules/rgbd/src/Utils_Npnp/Parsing.h
index 4f5c7b9f147..4f44b9c62ea 100644
--- a/modules/rgbd/src/Utils_Npnp/Parsing.h
+++ b/modules/rgbd/src/Utils_Npnp/Parsing.h
@@ -7,21 +7,23 @@
 #include <string>
 #include <vector>
 
-namespace NPnP {
-template <typename T>
-std::vector<T> parse_csv_array(const std::string &csv_path) {
-  std::vector<T> data;
-  std::ifstream input_file(csv_path);
-  T value;
-  while (input_file >> value)
-    data.push_back(value);
+namespace NPnP
+{
+  template <typename T>
+  std::vector<T> parse_csv_array(const std::string &csv_path)
+  {
+    std::vector<T> data;
+    std::ifstream input_file(csv_path);
+    T value;
+    while (input_file >> value)
+      data.push_back(value);
 
-  return std::move(data);
-}
+    return std::move(data);
+  }
 
-std::vector<Eigen::Vector3d> parse_csv_vector_3d(const std::string &csv_path);
+  std::vector<Eigen::Vector3d> parse_csv_vector_3d(const std::string &csv_path);
 
-Eigen::MatrixXd parse_csv_matrix(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