opencv：两个 3D 点云之间的刚性转换

Question

我有两个 3D 点云，我想使用 opencv 找到刚性变换矩阵（平移、旋转、所有 3 个轴之间的恒定缩放）。

我找到了一个estimateRigidTransformation 函数，但显然它只适用于二维点

另外，我找到了estimateAffine3D，不过好像不支持刚性变换模式。

我只需要编写自己的刚性转换函数吗？

Answer 1

我没有在 OpenCV 中找到所需的功能，所以我编写了自己的实现。基于OpenSFM 的想法。

cv::Vec3d
CalculateMean(const cv::Mat_<cv::Vec3d> &points)
{
    cv::Mat_<cv::Vec3d> result;
    cv::reduce(points, result, 0, CV_REDUCE_AVG);
    return result(0, 0);
}

cv::Mat_<double>
FindRigidTransform(const cv::Mat_<cv::Vec3d> &points1, const cv::Mat_<cv::Vec3d> points2)
{
    /* Calculate centroids. */
    cv::Vec3d t1 = -CalculateMean(points1);
    cv::Vec3d t2 = -CalculateMean(points2);

    cv::Mat_<double> T1 = cv::Mat_<double>::eye(4, 4);
    T1(0, 3) = t1[0];
    T1(1, 3) = t1[1];
    T1(2, 3) = t1[2];

    cv::Mat_<double> T2 = cv::Mat_<double>::eye(4, 4);
    T2(0, 3) = -t2[0];
    T2(1, 3) = -t2[1];
    T2(2, 3) = -t2[2];

    /* Calculate covariance matrix for input points. Also calculate RMS deviation from centroid
     * which is used for scale calculation.
     */
    cv::Mat_<double> C(3, 3, 0.0);
    double p1Rms = 0, p2Rms = 0;
    for (int ptIdx = 0; ptIdx < points1.rows; ptIdx++) {
        cv::Vec3d p1 = points1(ptIdx, 0) + t1;
        cv::Vec3d p2 = points2(ptIdx, 0) + t2;
        p1Rms += p1.dot(p1);
        p2Rms += p2.dot(p2);
        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 3; j++) {
                C(i, j) += p2[i] * p1[j];
            }
        }
    }

    cv::Mat_<double> u, s, vh;
    cv::SVD::compute(C, s, u, vh);

    cv::Mat_<double> R = u * vh;

    if (cv::determinant(R) < 0) {
        R -= u.col(2) * (vh.row(2) * 2.0);
    }

    double scale = sqrt(p2Rms / p1Rms);
    R *= scale;

    cv::Mat_<double> M = cv::Mat_<double>::eye(4, 4);
    R.copyTo(M.colRange(0, 3).rowRange(0, 3));

    cv::Mat_<double> result = T2 * M * T1;
    result /= result(3, 3);

    return result.rowRange(0, 3);
}

Answer 2

我发现PCL 是 OpenCV 的一个很好的辅助工具。看看他们的Iterative Closest Point (ICP) example。提供的示例注册两个点云，然后显示刚性变换。

Answer 3

这是我的 rmsd 代码：

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <assert.h>

typedef struct
{
  float m[4][4];
} MATRIX;

#define vdiff2(a,b) ( ((a)[0]-(b)[0]) * ((a)[0]-(b)[0]) +   \
  ((a)[1]-(b)[1]) * ((a)[1]-(b)[1]) + \
  ((a)[2]-(b)[2]) * ((a)[2]-(b)[2]) )

static double alignedrmsd(float *v1, float *v2, int N);
static void centroid(float *ret, float *v, int N);
static int getalignmtx(float *v1, float *v2, int N, MATRIX *mtx);
static void crossproduct(float *ans, float *pt1, float *pt2);
static void mtx_root(MATRIX *mtx);
static int almostequal(MATRIX *a, MATRIX *b);
static void mulpt(MATRIX *mtx, float *pt);
static void mtx_mul(MATRIX *ans, MATRIX *x, MATRIX *y);
static void mtx_identity(MATRIX *mtx);
static void mtx_trans(MATRIX *mtx, float x, float y, float z);
static int mtx_invert(float *mtx, int N);
static float absmaxv(float *v, int N);

/*
  calculate rmsd between two structures
  Params: v1 - first set of points
          v2 - second set of points
          N - number of points
          mtx - return for transfrom matrix used to align structures
  Returns: rmsd score
  Notes: mtx can be null. Transform will be rigid. Inputs must
         be previously aligned for sequence alignment
 */
double rmsd(float *v1, float *v2, int N, float *mtx)
{
  float cent1[3];
  float cent2[3];
  MATRIX tmtx;
  MATRIX tempmtx;
  MATRIX move1;
  MATRIX move2;
  int i;
  double answer;
  float *temp1 = 0;
  float *temp2 = 0;
  int err;

  assert(N > 3);

  temp1 = malloc(N * 3 * sizeof(float));
  temp2 = malloc(N * 3 * sizeof(float));
  if(!temp1 || !temp2)
    goto error_exit;

  centroid(cent1, v1, N);
  centroid(cent2, v2, N);
  for(i=0;i<N;i++)
  {
    temp1[i*3+0] = v1[i*3+0] - cent1[0];
    temp1[i*3+1] = v1[i*3+1] - cent1[1];
    temp1[i*3+2] = v1[i*3+2] - cent1[2];

    temp2[i*3+0] = v2[i*3+0] - cent2[0];
    temp2[i*3+1] = v2[i*3+1] - cent2[1];
    temp2[i*3+2] = v2[i*3+2] - cent2[2];
  }

  err = getalignmtx(temp1, temp2, N, &tmtx);
  if(err == -1)
    goto error_exit;

  mtx_trans(&move1, -cent2[0], -cent2[1], -cent2[2]);
  mtx_mul(&tempmtx, &move1, &tmtx);
  mtx_trans(&move2, cent1[0], cent1[1], cent1[2]);
  mtx_mul(&tmtx, &tempmtx, &move2);
  memcpy(temp2, v2, N * sizeof(float) * 3);
  for(i=0;i<N;i++)
    mulpt(&tmtx, temp2 + i * 3);
  answer = alignedrmsd(v1, temp2, N);
  free(temp1);
  free(temp2);
  if(mtx)
    memcpy(mtx, &tmtx.m, 16 * sizeof(float));

  return answer;
 error_exit:
  free(temp1);
  free(temp2);
  if(mtx)
  {
    for(i=0;i<16;i++)
      mtx[i] = 0;
  }
  return sqrt(-1.0);
}

/*
  calculate rmsd between two aligned structures (trivial)
  Params: v1 - first structure
          v2 - second structure
          N - number of points
  Returns: rmsd
 */
static double alignedrmsd(float *v1, float *v2, int N)
{
  double answer =0;
  int i;

  for(i=0;i<N;i++)
    answer += vdiff2(v1 + i *3, v2 + i * 3);
  return sqrt(answer/N);
}

/*
  compute the centroid
 */
static void centroid(float *ret, float *v, int N)
{
  int i;

  ret[0] = 0;
  ret[1] = 0;
  ret[2] = 0;
  for(i=0;i<N;i++)
  {
    ret[0] += v[i*3+0];
    ret[1] += v[i*3+1];
    ret[2] += v[i*3+2];
  }
  ret[0] /= N;
  ret[1] /= N;
  ret[2] /= N;
}

/*
  get the matrix needed to align two structures
  Params: v1 - reference structure
          v2 - structure to align
          N - number of points
          mtx - return for rigid body alignment matrix
  Notes: only calculates rotation part of matrix.
         assumes input has been aligned to centroids 
 */
static int getalignmtx(float *v1, float *v2, int N, MATRIX *mtx)
{
  MATRIX A = { {{0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,1}} };
  MATRIX At;
  MATRIX Ainv;
  MATRIX temp;
  float tv[3];
  float tw[3];
  float tv2[3];
  float tw2[3];
  int k, i, j;
  int flag = 0;
  float correction;

  correction = absmaxv(v1, N * 3) * absmaxv(v2, N * 3);

  for(k=0;k<N;k++)
    for(i=0;i<3;i++)
      for(j=0;j<3;j++)
    A.m[i][j] += (v1[k*3+i] * v2[k*3+j])/correction;

  while(flag < 3)
  {
    for(i=0;i<4;i++)
      for(j=0;j<4;j++)
        At.m[i][j] = A.m[j][i];

    memcpy(&Ainv, &A, sizeof(MATRIX));
    /* this will happen if all points are in a plane */
    if( mtx_invert((float *) &Ainv, 4) == -1)
    {
      if(flag == 0)
      {
        crossproduct(tv, v1, v1+3);
        crossproduct(tw, v2, v2+3);
      }
      else
      {
        crossproduct(tv2, tv, v1);
        crossproduct(tw2, tw, v2);
        memcpy(tv, tv2, 3 * sizeof(float));
        memcpy(tw, tw2, 3 * sizeof(float));
      }
      for(i=0;i<3;i++)
        for(j=0;j<3;j++)
      A.m[i][j] += tv[i] * tw[j];

      flag++;
    }
    else
      flag = 5;
  }
  if(flag != 5)
    return -1;

  mtx_mul(&temp, &At, &A);
  mtx_root(&temp);
  mtx_mul(mtx, &temp, &Ainv); 
  return 0;
}

/*
  get the crossproduct of two vectors.
  Params: ans - return pinter for answer.
          pt1 - first vector
          pt2 - second vector.
  Notes: crossproduct is at right angles to the two vectors.
*/
static void crossproduct(float *ans, float *pt1, float *pt2)
{
  ans[0] = pt1[1] * pt2[2] - pt1[2] * pt2[1];
  ans[1] = pt1[0] * pt2[2] - pt1[2] * pt2[0];
  ans[2] = pt1[0] * pt2[1] - pt1[1] * pt2[0];
}

/*
  Denman-Beavers square root iteration
 */
static void mtx_root(MATRIX *mtx)
{
  MATRIX Y = *mtx;
  MATRIX Z;
  MATRIX Y1;
  MATRIX Z1;
  MATRIX invY;
  MATRIX invZ;
  MATRIX Y2;
  int iter = 0;
  int i, ii;

  mtx_identity(&Z);

  do
  {
    invY = Y;
    invZ = Z;
    if( mtx_invert((float *) &invY, 4) == -1)
      return;
    if( mtx_invert((float *) &invZ, 4) == -1)
      return;
    for(i=0;i<4;i++)
      for(ii=0;ii<4;ii++)
      {
        Y1.m[i][ii] = 0.5 * (Y.m[i][ii] + invZ.m[i][ii]);
    Z1.m[i][ii] = 0.5 * (Z.m[i][ii] + invY.m[i][ii]);
      }
    Y = Y1;
    Z = Z1;

    mtx_mul(&Y2, &Y, &Y);
  }
  while(!almostequal(&Y2, mtx) && iter++ < 20 );

  *mtx = Y;
}

/*
  Check two matrices for near-enough equality
  Params: a - first matrix
          b - second matrix
  Returns: 1 if almost equal, else 0, epsilon 0.0001f.
 */
static int almostequal(MATRIX *a, MATRIX *b)
{
  int i, ii;
  float epsilon = 0.001f;

  for(i=0;i<4;i++)
    for(ii=0;ii<4;ii++)
      if(fabs(a->m[i][ii] - b->m[i][ii]) > epsilon) 
        return 0;
  return 1;
}  

/*
  multiply a point by a matrix.
  Params: mtx - matrix
          pt - the point (transformed)
*/
static void mulpt(MATRIX *mtx, float *pt)
{
  float ans[4] = {0};
  int i;
  int ii;

  for(i=0;i<4;i++)
  {
    for(ii=0;ii<3;ii++)
    {
      ans[i] += pt[ii] * mtx->m[ii][i];
    }
    ans[i] += mtx->m[3][i];
  }
  pt[0] = ans[0];
  pt[1] = ans[1];
  pt[2] = ans[2];
} 

/*
  multiply two matrices.
  Params: ans - return pointer for answer.
          x - first matrix
          y - second matrix.
  Notes: ans may not be equal to x or y.
*/
static void mtx_mul(MATRIX *ans, MATRIX *x, MATRIX *y)
{
  int i;
  int ii;
  int iii;

  for(i=0;i<4;i++)
    for(ii=0;ii<4;ii++)
    {
      ans->m[i][ii] = 0;
      for(iii=0;iii<4;iii++)
        ans->m[i][ii] += x->m[i][iii] * y->m[iii][ii];
    }
}


/*
  create an identity matrix.
  Params: mtx - return pointer.
*/
static void mtx_identity(MATRIX *mtx)
{
  int i;
  int ii;

  for(i=0;i<4;i++)
    for(ii=0;ii<4;ii++)
    {
      if(i==ii)
        mtx->m[i][ii] = 1.0f;
      else
        mtx->m[i][ii] = 0;
    }
}

/*
  create a translation matrix.
  Params: mtx - return pointer for matrix.
          x - x translation.
          y - y translation.
          z - z translation
*/
static void mtx_trans(MATRIX *mtx, float x, float y, float z)
{
  mtx->m[0][0] = 1;
  mtx->m[0][1] = 0;
  mtx->m[0][2] = 0;
  mtx->m[0][3] = 0;

  mtx->m[1][0] = 0;
  mtx->m[1][1] = 1;
  mtx->m[1][2] = 0;
  mtx->m[1][3] = 0;

  mtx->m[2][0] = 0;
  mtx->m[2][1] = 0;
  mtx->m[2][2] = 1;
  mtx->m[2][3] = 0;

  mtx->m[3][0] = x;
  mtx->m[3][1] = y;
  mtx->m[3][2] = z;
  mtx->m[3][3] = 1; 
}

/*
   matrix invert routine
  Params: mtx - the matrix in raw format, in/out
          N - width and height
  Returns: 0 on success, -1 on fail
 */
static int mtx_invert(float *mtx, int N)
{
  int indxc[100]; /* these 100s are the only restriction on matrix size */
  int indxr[100];
  int ipiv[100];
  int i, j, k;
  int irow, icol;
  double big;
  double pinv;
  int l, ll;
  double dum;
  double temp;

  assert(N <= 100);

  for(i=0;i<N;i++)
    ipiv[i] = 0;

  for(i=0;i<N;i++)
  {
    big = 0.0;

    /* find biggest element */
    for(j=0;j<N;j++)
      if(ipiv[j] != 1)
        for(k=0;k<N;k++)
          if(ipiv[k] == 0)
            if(fabs(mtx[j*N+k]) >= big)
        {
           big = fabs(mtx[j*N+k]);
               irow = j;
               icol = k;
        }       

    ipiv[icol]=1;

    if(irow != icol)
      for(l=0;l<N;l++)
      {
        temp = mtx[irow * N + l];
        mtx[irow * N + l] = mtx[icol * N + l];
    mtx[icol * N + l] = temp;
      }

    indxr[i] = irow;
    indxc[i] = icol;


    /* if biggest element is zero matrix is singular, bail */
    if(mtx[icol* N + icol] == 0)
       goto error_exit;

    pinv = 1.0/mtx[icol * N + icol];

    mtx[icol * N + icol] = 1.0;

    for(l=0;l<N;l++)
      mtx[icol * N + l] *= pinv;

    for(ll=0;ll<N;ll++)
      if(ll != icol)
      {
        dum = mtx[ll * N + icol];
        mtx[ll * N + icol] = 0.0;
        for(l=0;l<N;l++) 
          mtx[ll * N + l] -= mtx[icol * N + l]*dum;
      }
  }                


  /* unscramble matrix */
  for (l=N-1;l>=0;l--) 
  {
    if (indxr[l] != indxc[l])
    for (k=0;k<N;k++)
    {
      temp = mtx[k * N + indxr[l]];
      mtx[k * N + indxr[l]] = mtx[k * N + indxc[l]];
      mtx[k * N + indxc[l]] = temp;
    }
  } 

  return 0;

 error_exit:
  return -1;
}

/*
  get the asolute maximum of an array
 */
static float absmaxv(float *v, int N)
{
  float answer;
  int i;

  for(i=0;i<N;i++)
    if(answer < fabs(v[i]))
      answer = fabs(v[i]);
  return answer;
}

#include <stdio.h>

/*
  debug utlitiy
 */
static void printmtx(FILE *fp, MATRIX *mtx)
{
  int i, ii;

  for(i=0;i<4;i++)
  {
    for(ii=0;ii<4;ii++)
      fprintf(fp, "%f, ", mtx->m[i][ii]);
    fprintf(fp, "\n");
  }
}

int rmsdmain(void)
{
  float one[4*3] = {0,0,0, 1,0,0, 2,1,0, 0,3,1};
  float two[4*3] = {0,0,0, 0,1,0, 1,2,0, 3,0,1};
  MATRIX mtx;
  double diff;
  int i;

  diff = rmsd(one, two, 4, (float *) &mtx.m);
  printf("%f\n", diff);
  printmtx(stdout, &mtx);
  for(i=0;i<4;i++)
  {
    mulpt(&mtx, two + i * 3);
    printf("%f %f %f\n", two[i*3], two[i*3+1], two[i*3+2]);
  }
  return 0;
}

Answer 4

我采用了@vagran 的实现并在其上添加了 RANSAC，因为 estimateRigidTransform2d 做到了，而且它对我很有帮助，因为我的数据很嘈杂。 （注意：此代码没有沿所有 3 个轴进行恒定缩放；您可以通过与 vargran 的比较轻松地将其重新添加）。

cv::Vec3f CalculateMean(const cv::Mat_<cv::Vec3f> &points)
{
    if(points.size().height == 0){
      return 0;
    }
    assert(points.size().width == 1);
    double mx = 0.0;
    double my = 0.0;
    double mz = 0.0;
    int n_points = points.size().height;
    for(int i = 0; i < n_points; i++){
      double x = double(points(i)[0]);
      double y = double(points(i)[1]);
      double z = double(points(i)[2]);
      mx += x;
      my += y;
      mz += z;
    }
    return cv::Vec3f(mx/n_points, my/n_points, mz/n_points);
}

cv::Mat_<double>
FindRigidTransform(const cv::Mat_<cv::Vec3f> &points1, const cv::Mat_<cv::Vec3f> points2)
{
    /* Calculate centroids. */
    cv::Vec3f t1 = CalculateMean(points1);
    cv::Vec3f t2 = CalculateMean(points2);

    cv::Mat_<double> T1 = cv::Mat_<double>::eye(4, 4);
    T1(0, 3) = double(-t1[0]);
    T1(1, 3) = double(-t1[1]);
    T1(2, 3) = double(-t1[2]);

    cv::Mat_<double> T2 = cv::Mat_<double>::eye(4, 4);
    T2(0, 3) = double(t2[0]);
    T2(1, 3) = double(t2[1]);
    T2(2, 3) = double(t2[2]);

    /* Calculate covariance matrix for input points. Also calculate RMS deviation from centroid
     * which is used for scale calculation.
     */
    cv::Mat_<double> C(3, 3, 0.0);
    for (int ptIdx = 0; ptIdx < points1.rows; ptIdx++) {
        cv::Vec3f p1 = points1(ptIdx) - t1;
        cv::Vec3f p2 = points2(ptIdx) - t2;
        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 3; j++) {
                C(i, j) += double(p2[i] * p1[j]);
            }
        }
    }

    cv::Mat_<double> u, s, vt;
    cv::SVD::compute(C, s, u, vt);

    cv::Mat_<double> R = u * vt;

    if (cv::determinant(R) < 0) {
        R -= u.col(2) * (vt.row(2) * 2.0);
    }

    cv::Mat_<double> M = cv::Mat_<double>::eye(4, 4);
    R.copyTo(M.colRange(0, 3).rowRange(0, 3));

    cv::Mat_<double> result = T2 * M * T1;
    result /= result(3, 3);
    return result;
}

cv::Mat_<double> RANSACFindRigidTransform(const cv::Mat_<cv::Vec3f> &points1, const cv::Mat_<cv::Vec3f> &points2)
{
  cv::Mat points1Homo;
  cv::convertPointsToHomogeneous(points1, points1Homo);
  int iterations = 100;
  int min_n_points = 3;
  int n_points = points1.size().height;
  std::vector<int> range(n_points);
  cv::Mat_<double> best;
  int best_inliers = -1;
  // inlier points should be projected within this many units
  float threshold = .02;
  std::iota(range.begin(), range.end(), 0);
  auto gen = std::mt19937{std::random_device{}()};
  for(int i = 0; i < iterations; i++) {
    std::shuffle(range.begin(), range.end(), gen);
    cv::Mat_<cv::Vec3f> points1subset(min_n_points, 1, cv::Vec3f(0,0,0));
    cv::Mat_<cv::Vec3f> points2subset(min_n_points, 1, cv::Vec3f(0,0,0));
    for(int j = 0; j < min_n_points; j++) {
      points1subset(j) = points1(range[j]);
      points2subset(j) = points2(range[j]);
    }
    cv::Mat_<float> rigidT = FindRigidTransform(points1subset, points2subset);
    cv::Mat_<float> rigidT_float = cv::Mat::eye(4, 4, CV_32F);
    rigidT.convertTo(rigidT_float, CV_32F);
    std::vector<int> inliers;
    for(int j = 0; j < n_points; j++) {
        cv::Mat_<float> t1_3d = rigidT_float * cv::Mat_<float>(points1Homo.at<cv::Vec4f>(j));
        if(t1_3d(3) == 0) {
          continue; // Avoid 0 division
        }
        float dx = (t1_3d(0)/t1_3d(3) - points2(j)[0]);
        float dy = (t1_3d(1)/t1_3d(3) - points2(j)[1]);
        float dz = (t1_3d(2)/t1_3d(3) - points2(j)[2]);
        float square_dist = dx * dx + dy * dy + dz * dz;
        if(square_dist < threshold * threshold){
          inliers.push_back(j);
        }
    }
    int n_inliers = inliers.size();
    if(n_inliers > best_inliers) {
      best_inliers = n_inliers;
      best = rigidT;
    }
  }
  return best;
}

Answer 5

@vagran 感谢您的代码！看起来效果很好。

不过，我确实有一些术语建议。由于您在转换期间估计和应用比例，因此它是 7 参数转换，或 Helmert / 相似度转换。并且在刚性变换中，不应用缩放，因为需要保留所有欧几里得距离。

我会将此添加为评论，但没有足够的积分.. D：抱歉。

• 刚性变换：https://en.wikipedia.org/wiki/Rigid_transformation
• 赫尔默特变换：https://www.researchgate.net/publication/322841143_Parameter_estimation_in_3D_affine_and_similarity_transformation_implementation_of_variance_component_estimation