C++ MatrixXf::colPivHouseholderQr方法代码示例

template <typename PointSource, typename PointTarget> inline void
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
                             const std::vector<int> &indices_src,
                             const pcl::PointCloud<PointTarget> &cloud_tgt,
                             const std::vector<int> &indices_tgt,
                             Eigen::Matrix4f &transformation_matrix)
{
  size_t nr_points = indices_src.size ();
  if (indices_tgt.size () != nr_points)
  {
    PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", (unsigned long)indices_src.size (), (unsigned long)indices_tgt.size ());
    return;
  }

  // Approximate as a linear least squares problem
  Eigen::MatrixXf A (nr_points, 6);
  Eigen::MatrixXf b (nr_points, 1);
  for (size_t i = 0; i < nr_points; ++i)
  {
    const float & sx = cloud_src.points[indices_src[i]].x;
    const float & sy = cloud_src.points[indices_src[i]].y;
    const float & sz = cloud_src.points[indices_src[i]].z;
    const float & dx = cloud_tgt.points[indices_tgt[i]].x;
    const float & dy = cloud_tgt.points[indices_tgt[i]].y;
    const float & dz = cloud_tgt.points[indices_tgt[i]].z;
    const float & nx = cloud_tgt.points[indices_tgt[i]].normal[0];
    const float & ny = cloud_tgt.points[indices_tgt[i]].normal[1];
    const float & nz = cloud_tgt.points[indices_tgt[i]].normal[2];
    A (i, 0) = nz*sy - ny*sz;
    A (i, 1) = nx*sz - nz*sx; 
    A (i, 2) = ny*sx - nx*sy;
    A (i, 3) = nx;
    A (i, 4) = ny;
    A (i, 5) = nz;
    b (i, 0) = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
  }

  // Solve A*x = b
  Eigen::VectorXf x = A.colPivHouseholderQr ().solve (b);
  
  // Construct the transformation matrix from x
  constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
}

template<typename PointT> void
pcl::registration::LUM<PointT>::compute ()
{
  int n = static_cast<int> (getNumVertices ());
  if (n < 2)
  {
    PCL_ERROR("[pcl::registration::LUM::compute] The slam graph needs at least 2 vertices.\n");
    return;
  }
  for (int i = 0; i < max_iterations_; ++i)
  {
    // Linearized computation of C^-1 and C^-1*D and convergence checking for all edges in the graph (results stored in slam_graph_)
    typename SLAMGraph::edge_iterator e, e_end;
    for (boost::tuples::tie (e, e_end) = edges (*slam_graph_); e != e_end; ++e)
      computeEdge (*e);

    // Declare matrices G and B
    Eigen::MatrixXf G = Eigen::MatrixXf::Zero (6 * (n - 1), 6 * (n - 1));
    Eigen::VectorXf B = Eigen::VectorXf::Zero (6 * (n - 1));

    // Start at 1 because 0 is the reference pose
    for (int vi = 1; vi != n; ++vi)
    {
      for (int vj = 0; vj != n; ++vj)
      {
        // Attempt to use the forward edge, otherwise use backward edge, otherwise there was no edge
        Edge e;
        bool present1, present2;
        boost::tuples::tie (e, present1) = edge (vi, vj, *slam_graph_);
        if (!present1)
        {
          boost::tuples::tie (e, present2) = edge (vj, vi, *slam_graph_);
          if (!present2)
            continue;
        }

        // Fill in elements of G and B
        if (vj > 0)
          G.block (6 * (vi - 1), 6 * (vj - 1), 6, 6) = -(*slam_graph_)[e].cinv_;
        G.block (6 * (vi - 1), 6 * (vi - 1), 6, 6) += (*slam_graph_)[e].cinv_;
        B.segment (6 * (vi - 1), 6) += (present1 ? 1 : -1) * (*slam_graph_)[e].cinvd_;
      }
    }

    // Computation of the linear equation system: GX = B
    // TODO investigate accuracy vs. speed tradeoff and find the best solving method for our type of linear equation (sparse)
    Eigen::VectorXf X = G.colPivHouseholderQr ().solve (B);

    // Update the poses
    float sum = 0.0;
    for (int vi = 1; vi != n; ++vi)
    {
      Eigen::Vector6f difference_pose = static_cast<Eigen::Vector6f> (-incidenceCorrection (getPose (vi)).inverse () * X.segment (6 * (vi - 1), 6));
      sum += difference_pose.norm ();
      setPose (vi, getPose (vi) + difference_pose);
    }

    // Convergence check
    if (sum <= convergence_threshold_ * static_cast<float> (n - 1))
      return;
  }
}