三维点云刚体变换-CSDN博客

本文链接：https://blog.csdn.net/sinat_25333079/article/details/101196122

#include<opencv2/opencv.hpp>

#include<vector>
#include<iostream>

cv::Mat_<double> getMC(cv::Mat_<double> &M)
{
   if (3 != M.rows || 3 != M.cols)
   {
       std::cout << "输入数据有误" << std::endl;
       return M;
   }

   double sum_x = 0.0, sum_y = 0.0, sum_z = 0.0;//mat column mean
   for (int row = 0; row < 3; ++row)
   {
       sum_x += (double)M.at<double>(row, 0);
       sum_y += (double)M.at<double>(row, 1);
       sum_z += (double)M.at<double>(row, 2);
   }
   //std::cout << "sum_x: " << sum_x << "\tsum_y: " << sum_y << "\tsum_z: " << sum_z << std::endl;
   cv::Mat MC = cv::Mat::zeros(1, 3, CV_64FC1);
   //std::cout << "MC:\n" << MC << std::endl;
   MC.at<double>(0, 0) = sum_x / 3.0;
   MC.at<double>(0, 1) = sum_y / 3.0;
   MC.at<double>(0, 2) = sum_z / 3.0;
   //std::cout << "MC:\n" << MC << std::endl;
   return MC;
}

cv::Mat_<double> getM_MC(cv::Mat_<double> &M, cv::Mat_<double> &MC)
{
   cv::Mat_<double> M_MC(M.size(), M.type());
   if (M.cols != MC.cols)
   {
       std::cout << "M.cols != MC.cols, return M" << std::endl;
       return M;
   }
   int rows = M.rows, cols = M.cols;
   for (int row = 0; row < rows; ++row)
   {
       for (int col = 0; col < cols; ++col)
       {
           M_MC.at<double>(row, col) = M.at<double>(row, col) - MC.at<double>(0, col);
       }
   }
   std::cout << "\nM:\n" << M << "\nMC:\n" << MC << "\nM_MC:\n" << M_MC << std::endl;//3*3-1*3大小不一致

return M_MC;
}

int rigid_transform_3D
(
std::vector<cv::Mat_<double>> &A,
std::vector<cv::Mat_<double>> &B,
std::vector<cv::Mat_<double>> &R,
std::vector<cv::Mat_<double>> &t
)
{
   size_t sA = A.size(), sB = B.size();
   if (0 == sA)
   {
       std::cout << "第一组点云数据为空" << std::endl;
       return -1;
   }
   if (0 == sB)
   {
       std::cout << "第二组点云数据为空" << std::endl;
       return -1;
   }
   if (sA != sB)
   {
       std::cout << "两组点云数目不匹配" << std::endl;
       return -1;
   }

   R.clear();
   t.clear();
   cv::Mat_<double> Ri(3, 3, CV_64FC1), ti(1, 3, CV_64FC1);
   for (size_t i = 0; i < sA; ++i)
   {
       int sAiRow = A[i].rows, sAiCol = A[i].cols, sBiRow = B[i].rows, sBiCol = B[i].cols;
       if (3 != sAiRow || 3 != sAiCol || 3 != sBiRow || 3 != sBiCol)
       {
           std::cout << "第" << i + 1 << "组点平面点数据不正确" << std::endl;
       }
       else
       {
           cv::Mat_<double> mcAi = getMC(A[i]);
           cv::Mat_<double> mcBi = getMC(B[i]);

cv::Mat_<double> mAi_cAi = getM_MC(A[i], mcAi);
cv::Mat_<double> mBi_cBi = getM_MC(B[i], mcBi);

cv::Mat_<double> H = mAi_cAi.t()*mBi_cBi;
//std::cout << "H:\n" << H << std::endl;

cv::Mat_<double> W, U, Vt;
cv::SVD::compute(H, W, U, Vt);

           std::cout << "W:\n" << W << std::endl;
           std::cout << "U:\n" << U << std::endl;
           std::cout << "Vt:\n" << Vt << std::endl;
           std::cout << "-Vt:\n" << -Vt << std::endl;

           std::cout << "Vt.t():\n" << Vt.t() << std::endl;
           std::cout << "W.t():\n" << W.t() << std::endl;
           return 0;
           Ri = Vt.t()*W.t();

           //Ri.at<double>(0, 0) = 0.99850369;
           //Ri.at<double>(0, 1) = -0.05414827;
           //Ri.at<double>(0, 2) = 0.00763785;
           //Ri.at<double>(1, 0) = -0.05421055;
           //Ri.at<double>(1, 1) = 0.99849588;
           //Ri.at<double>(1, 2) = 0.00819763;
           //Ri.at<double>(2, 0) = 0.00718248;
           //Ri.at<double>(2, 1) = 0.00859942;
           //Ri.at<double>(2, 2) = 0.99993723;

//std::cout << "Ri:\n" << Ri << std::endl;

           if (cv::determinant(Ri) < 0)
           {
               std::cout << "Reflection detected" << std::endl;
               Vt = -Vt;
               Ri = Vt.t()*W.t();
           }

ti = -Ri*mcAi + mcBi;

           R.emplace_back(Ri);
           t.emplace_back(ti);
       }
   }

return 0;
}

int main()
{
std::vector<cv::Mat_<double>> R, t;
std::vector<cv::Mat_<double>> A, B;

   //#手动输入两组点
   //   lA = [[0.78725555, 0.79282773, 0.41933589],
   //       [0.35591897, 0.69076946, 0.59409881],
   //       [0.09733009, 0.41589826, 0.3484507]]
   //       lB = [[1.47324929, 1.45627525, 0.78542786],
   //       [1.03569703, 1.37632084, 0.9562041],
   //       [0.7644875, 1.11789506, 0.70635037]];

   A.resize(1);
   A[0] = cv::Mat_<double>(3, 3, CV_64FC1);
   A[0].at<double>(0, 0) = 0.78725555;
   A[0].at<double>(0, 1) = 0.79282773;
   A[0].at<double>(0, 2) = 0.41933589;
   A[0].at<double>(1, 0) = 0.35591897;
   A[0].at<double>(1, 1) = 0.69076946;
   A[0].at<double>(1, 2) = 0.59409881;
   A[0].at<double>(2, 0) = 0.09733009;
   A[0].at<double>(2, 1) = 0.41589826;
   A[0].at<double>(2, 2) = 0.3484507;
   B.resize(1);
   B[0] = cv::Mat_<double>(3, 3, CV_64FC1);
   B[0].at<double>(0, 0) = 1.47324929;
   B[0].at<double>(0, 1) = 1.45627525;
   B[0].at<double>(0, 2) = 0.78542786;
   B[0].at<double>(1, 0) = 1.03569703;
   B[0].at<double>(1, 1) = 1.37632084;
   B[0].at<double>(1, 2) = 0.9562041;
   B[0].at<double>(2, 0) = 0.7644875;
   B[0].at<double>(2, 1) = 1.11789506;
   B[0].at<double>(2, 2) = 0.70635037;

//std::cout << "A[0]:\n" << A[0] << "\nB[0]:\n" << B[0] << std::endl;
std::cout << "\nrigid_transform_3D result: " << rigid_transform_3D(A, B, R, t) << std::endl;

   rsize_t n = R.size();
   for (size_t i = 0; i < n; ++i)
   {
       std::cout << "R[" << i << "]:" << std::endl;
       std::cout << R[i] << std::endl;
       std::cout << "t[" << i << "]:" << std::endl;
       std::cout << t[i] << std::endl;
       putchar('\n');
   }

   std::cout << "Done" << std::endl;
   system("pause");
   return 0;
}