Question

我从类似kinect的仪器中取出了许多点云，这些仪器安装在三脚架上然后旋转。如何准确确定旋转轴？我正在使用c ++ PCL和Eigen。

我可以将点云与ICP匹配并运行全局注册（SLAM或ELCH）以获得组合点云，但出于多种原因，我希望能够准确地确定轴并强制注册遵守这个轮换。

与此问题相关的一个问题是我的乐器的起源。我可以非常准确地测量距设备外部尺寸到旋转轴的距离，但我不确切知道设备末端的原点在哪里。解决这个问题可以帮助我找到原点。

我正在考虑两种方法。

一个是获取已注册点云的变换矩阵并提取表示变换将在当前位置投影内部原点的位置的平移向量。对于以这种方式获得的点集，我可以尝试拟合圆，并且中心点将表示从原点到旋转轴的矢量，并且圆的法线方向将是轴的方向。

另一个选项是直接从任何单个旋转矩阵确定旋转轴，但旋转轴的矢量似乎是不稳定的。

有关此问题的更好的解决方案或见解吗？

Answer 1

您需要计算惯性张量的主轴。 https://en.m.wikipedia.org/wiki/Moment_of_inertia

所有点都可以被认为具有相同的质量。然后你可以使用Steiner方法。

Answer 2

一个选项是将棋盘放置在~1 [m]。使用kinect相机制作不同旋转的图像，孔棋盘仍然可见。使用OpenCV调整棋盘。

目标是找到不同方向的棋盘的xyz坐标。使用相机api功能确定棋盘的xyz坐标或执行以下操作：

确定相机1和2的相机内在因素。（使用彩色和红外图像作为kinect）。
确定相机外部设备（相机2 [R，t]相机1）
使用这些值计算投影矩阵
使用棋盘的triangulate the points投影矩阵来获取[X，Y，Z]和相机1坐标系中的坐标。

每组棋盘点称为[x_i]。现在我们可以写出等式：

<强>更新这个等式可以用非线性求解器求解，我使用ceres-solver。

#include "ceres/ceres.h"
#include "ceres/rotation.h"
#include "glog/logging.h"
#include "opencv2/opencv.hpp"
#include "csv.h"
#include "Eigen/Eigen"

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;

struct AxisRotationError {
    AxisRotationError(double observed_x0, double observed_y0, double observed_z0, double observed_x1, double observed_y1, double observed_z1)
        : observed_x0(observed_x0), observed_y0(observed_y0), observed_z0(observed_z0), observed_x1(observed_x1), observed_y1(observed_y1), observed_z1(observed_z1) {}

    template <typename T>
    bool operator()(const T* const axis, const T* const angle, const T* const trans, T* residuals) const {
    //bool operator()(const T* const axis, const T* const trans, T* residuals) const {
        // Normalize axis
        T a[3];
        T k = axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2];
        a[0] = axis[0] / sqrt(k);
        a[1] = axis[1] / sqrt(k);
        a[2] = axis[2] / sqrt(k);

        // Define quaternion from axis and angle. Convert angle to radians
        T pi = T(3.14159265359);
        //T angle[1] = {T(10.0)};
        T quaternion[4] = { cos((angle[0]*pi / 180.0) / 2.0), 
            a[0] * sin((angle[0] * pi / 180.0) / 2.0),
            a[1] * sin((angle[0] * pi / 180.0) / 2.0),
            a[2] * sin((angle[0] * pi / 180.0) / 2.0) };

        // Define transformation
        T t[3] = { trans[0], trans[1], trans[2] };

        // Calculate predicted positions
        T observedPoint0[3] = { T(observed_x0), T(observed_y0), T(observed_z0)};
        T point[3]; point[0] = observedPoint0[0] - t[0]; point[1] = observedPoint0[1] - t[1]; point[2] = observedPoint0[2] - t[2];
        T rotatedPoint[3];
        ceres::QuaternionRotatePoint(quaternion, point, rotatedPoint);
        T predicted_x = rotatedPoint[0] + t[0];
        T predicted_y = rotatedPoint[1] + t[1];
        T predicted_z = rotatedPoint[2] + t[2];

        // The error is the difference between the predicted and observed position.
        residuals[0] = predicted_x - T(observed_x1);
        residuals[1] = predicted_y - T(observed_y1);
        residuals[2] = predicted_z - T(observed_z1);
        return true;
    }

    // Factory to hide the construction of the CostFunction object from
    // the client code.
    static ceres::CostFunction* Create(const double observed_x0, const double observed_y0, const double observed_z0, 
                                       const double observed_x1, const double observed_y1, const double observed_z1) {
        // Define AutoDiffCostFunction. <AxisRotationError, #residuals, #dim axis, #dim angle, #dim trans
        return (new ceres::AutoDiffCostFunction<AxisRotationError, 3, 3, 1,3>(
            new AxisRotationError(observed_x0, observed_y0, observed_z0, observed_x1, observed_y1, observed_z1)));
    }

    double observed_x0;
    double observed_y0;
    double observed_z0;
    double observed_x1;
    double observed_y1;
    double observed_z1;
};

int main(int argc, char** argv) {
  google::InitGoogleLogging(argv[0]);

  // Load points.csv into cv::Mat's
  // 216 rows with (x0, y0, z0, x1, y1, z1)
  // [x1,y1,z1] = R* [x0-tx,y0-ty,z0-tz] + [tx,ty,tz]
  // The xyz coordinates are points on a chessboard, where the chessboard 
  // is rotated for 4x. Each chessboard has 54 xyz points. So 4x 54,
  // gives the 216 rows of observations.
  // The chessboard is located at [0,0,1], as the camera_0 is located
  // at [-0.1,0,0], the t should become [0.1,0,1.0].
  // The chessboard is rotated around axis [0.0,1.0,0.0]
  io::CSVReader<6> in("points.csv");
  float x0, y0, z0, x1, y1, z1;

  // The observations
  cv::Mat x_0(216, 3, CV_32F);
  cv::Mat x_1(216, 3, CV_32F);
  for (int rowNr = 0; rowNr < 216; rowNr++){
      if (in.read_row(x0, y0, z0, x1, y1, z1))
      {
          x_0.at<float>(rowNr, 0) = x0;
          x_0.at<float>(rowNr, 1) = y0;
          x_0.at<float>(rowNr, 2) = z0;
          x_1.at<float>(rowNr, 0) = x1;
          x_1.at<float>(rowNr, 1) = y1;
          x_1.at<float>(rowNr, 2) = z1;
      }
  }

  std::cout << x_0(cv::Rect(0, 0, 2, 5)) << std::endl;

  // The variable to solve for with its initial value. It will be
  // mutated in place by the solver.
  int numObservations = 216;
  double axis[3] = { 0.0, 1.0, 0.0 };
  double* pAxis; pAxis = axis;
  double angles[4] = { 10.0, 10.0, 10.0, 10.0 };
  double* pAngles; pAngles = angles;
  double t[3] = { 0.0, 0.0, 1.0,};
  double* pT; pT = t;
  bool FLAGS_robustify = true;

  // Build the problem.
  Problem problem;

  // Set up the only cost function (also known as residual). This uses
  // auto-differentiation to obtain the derivative (jacobian).
  for (int i = 0; i < numObservations; ++i) {
      ceres::CostFunction* cost_function =
          AxisRotationError::Create(
          x_0.at<float>(i, 0), x_0.at<float>(i, 1), x_0.at<float>(i, 2),
          x_1.at<float>(i, 0), x_1.at<float>(i, 1), x_1.at<float>(i, 2));
      //std::cout << "pAngles: " << pAngles[i / 54] << ", " << i / 54 << std::endl;
      ceres::LossFunction* loss_function = FLAGS_robustify ? new ceres::HuberLoss(0.001) : NULL;
      //ceres::LossFunction* loss_function = FLAGS_robustify ? new ceres::CauchyLoss(0.002) : NULL;
      problem.AddResidualBlock(cost_function, loss_function, pAxis, &pAngles[i/54], pT);
      //problem.AddResidualBlock(cost_function, loss_function, pAxis, pT);
  }

  // Run the solver!
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  //options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = true;
  options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
  options.num_threads = 4;
  options.use_nonmonotonic_steps = false;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);
  //std::cout << summary.FullReport() << "\n";
  std::cout << summary.BriefReport() << "\n";

  // Normalize axis
  double k = axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2];
  axis[0] = axis[0] / sqrt(k);
  axis[1] = axis[1] / sqrt(k);
  axis[2] = axis[2] / sqrt(k);

  // Plot results
  std::cout << "axis: [ " << axis[0] << "," << axis[1] << "," << axis[2] << " ]" << std::endl;
  std::cout << "t: [ " << t[0] << "," << t[1] << "," << t[2] << " ]" << std::endl;
  std::cout << "angles: [ " << angles[0] << "," << angles[1] << "," << angles[2] << "," << angles[3] << " ]" << std::endl;

  return 0;
}

我得到的结果：

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  3.632073e-003    0.00e+000    3.76e-002   0.00e+000   0.00e+000  1.00e+004        0    4.30e-004    7.57e-004
   1  3.787837e-005    3.59e-003    2.10e-003   1.17e-001   1.92e+000  3.00e+004        1    7.43e-004    8.55e-003
   2  3.756202e-005    3.16e-007    1.73e-003   5.49e-001   1.61e-001  2.29e+004        1    5.35e-004    1.13e-002
   3  3.589147e-005    1.67e-006    2.90e-004   9.19e-002   9.77e-001  6.87e+004        1    5.96e-004    1.46e-002
   4  3.584281e-005    4.87e-008    1.38e-005   2.70e-002   1.00e+000  2.06e+005        1    4.99e-004    1.73e-002
   5  3.584268e-005    1.35e-010    1.02e-007   1.63e-003   1.01e+000  6.18e+005        1    6.32e-004    2.01e-002
Ceres Solver Report: Iterations: 6, Initial cost: 3.632073e-003, Final cost: 3.584268e-005, Termination: CONVERGENCE
axis: [ 0.00119037,0.999908,-0.0134817 ]
t: [ 0.0993185,-0.0080394,1.00236 ]
angles: [ 9.90614,9.94415,9.93216,10.1119 ]

角度结果非常好10度。这些甚至可以修复我的情况，因为我从旋转阶段非常准确地知道旋转。 t和轴有一个小的差异。这是由于我的虚拟立体声相机模拟中的不准确造成的。我的棋盘方块不完全是方形的，尺寸也有点偏离......

我的模拟脚本，图片和结果：blender_simulation.zip

Answer 3

这不是对原始问题的回答，但对于在两个位置之间定义旋转轴的讨论进行澄清

速冻。这是一个Octave脚本，用于演示我们在聊天中讨论的内容。我知道在没有提出答案时发表答案可能是不好的做法，但我希望这能让你深入了解我试图解释翻译矢量和旋转轴上的点之间的关系（t_i在你的符号）。

rotation_axis_unit = [0,0,1]' % parallel to z axis
angle = 1 /180 *pi() % one degree

% a rotaion matrix of one degree rotation
R = [cos(angle) -sin(angle) 0 ; sin(angle) cos(angle) 0 ; 0 0 1 ]; 

% the point around wich to rotate
axis_point = [-10,0,0]' % a point

% just a point used to demonstrate that all points form a circular path
test_point = [10,5,0]';


%containers for plotting
path_origin = zeros(360,3);
path_test_point = zeros(360,3);
path_v = zeros(360,3);

origin = [0,0,0]';

% updating rotation matrix
R_i = R;
M1 = [R,R*-axis_point.+axis_point;[0,0,0,1]];
% go around a full circle
for i=1:360

  % v = the last column of M. Created from axis_point.
  % -axis_point is the vector from axis_point to origin which is being rotated
  % then a correction is applied to center it around the axis point
  v = R_i * -axis_point .+ axis_point;

  % building 4x4 transformation matrix
  M = [R_i, v;[0,0,0,1]];
  % M could also be built M_i = M1 * M_i, rotating the previous M by one degree around axis_point

  % rotatin testing point and saving it
  test_point_i = M * [test_point;1];
  path_test_point(i,:) = test_point_i(1:3,1)';

  % saving the translation part of M
  path_v(i,:) = v';

  % rotating origin point and saving it
  origin_i = test_point_i = M * [origin;1];
  path_origin(i,:) = origin_i(1:3,1)';

  R_i = R * R_i ;

end
figure(1)
% plot test point path, just to show it forms a circular path, center and axis_point
scatter3(path_test_point(:,1), path_test_point(:,2), path_test_point(:,3), 5,'r')
hold on
% plotting origin path, circular, center at axis_point
scatter3(path_origin(:,1), path_origin(:,2), path_origin(:,3), 7,'r')
hold on
% plotting translation vectors, identical to origin path, (if invisible rotate so that you are watching it from z axis direction)
scatter3(path_v(:,1), path_v(:,2), path_v(:,3), 1, 'black');
hold on 

% plots for visual analysis
scatter3(0,0,0,5,'b') % origin
hold on
scatter3(axis_point(1), axis_point(2), axis_point(3), 5, 'g') % axis point, center of the circles
hold on 
scatter3(test_point(1), test_point(2), test_point(3), 5, 'black') % test point origin
hold off

% what does this demonstrate?
% it shows that that the ralationship between a 4x4 
% transformation matrix and axis_angle notation plus point on the rotation axis
% M = [ R, v,; [0,0,0,1]] =  [ R_i , R_i * -c + c; [ 0, 0, 0, 1] ]
% 
% where c equals axis_point ( = perpendicular vector from origin to the axis of rotation )

% pay attention to path_v and path_origin
% they are identical
% path_v was extracted from the 4x4 transformation matrix M
% and path_origin was created by rotating the origin point by M
%--> v = R_i * -.c +.c

% Notice that since M describes a rotation of alpha angles around an 
% axis that goes through c 
% and its translation vector lies on a circle whose center
% is at the rotation_axis and radius is the distance from that
% point to origin ->
%
% M * M will describe a rotation of 2 x alpha angles around the same axis
% Therefore you can easily create more points that lay on that circle  
% by multiplying M by itself and extracting the translation vector
%
% c can then be solved by normal circle fit algorithms.
%------------------------------------------------------------

% CAUTION !!!


% this applies perfectly when the transformation matrices have been created so 
% that the translation is perfectly orthogonal to the rotation axis
% in real world matrices the translation will not be orthogonal

% therefore the points will not travel on a circular path but on a helix and this needs to be
% dealt with when solving the center of rotation.

如何从一组点云确定旋转轴

3 个答案: