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

void mesh_core::Plane::leastSquaresFast(
      const EigenSTL::vector_Vector3d& points,
      Eigen::Vector3d* average)
{
  Eigen::Matrix3d m;
  Eigen::Vector3d b;
  Eigen::Vector3d c;

  m.setZero();
  b.setZero();
  c.setZero();

  EigenSTL::vector_Vector3d::const_iterator p = points.begin();
  EigenSTL::vector_Vector3d::const_iterator end = points.end();
  for ( ; p != end ; ++p)
  {
    m(0,0) += p->x() * p->x();
    m(1,0) += p->x() * p->y();
    m(2,0) += p->x();
    m(1,1) += p->y() * p->y();
    m(2,1) += p->y();
    b(0) += p->x() * p->z();
    b(1) += p->y() * p->z();
    b(2) += p->z();
    c += *p;
  }
  m(0,1) = m(1,0);
  m(0,2) = m(2,0);
  m(1,2) = m(2,1);
  m(2,2) = double(points.size());
  c *= 1.0/double(points.size());

  normal_ = m.colPivHouseholderQr().solve(b);
  if (normal_.squaredNorm() > std::numeric_limits<double>::epsilon())
    normal_.normalize();

  d_ = -c.dot(normal_);

  if (average)
    *average = c;
}

std::vector<double>
jerkMinimizingTrajectory(const motion& state0, const motion& state1, double dt) {
  double dt2 = dt*dt;
  double dt3 = dt2*dt;
  double dt4 = dt3*dt;
  double dt5 = dt4*dt;

  Eigen::Matrix3d a;
  a <<  dt3,    dt4,    dt5,
    3*dt2,  4*dt3,  5*dt4,
    6*dt, 12*dt2, 20*dt3;

  Eigen::Vector3d b;
  b << state1[0] - (state0[0] + dt*state0[1] + 0.5*dt2*state0[2]),
    state1[1] - (state0[1] + dt*state0[2]),
    state1[2] -  state0[2];

  Eigen::VectorXd p = a.colPivHouseholderQr().solve(b);

  return {state0[0], state0[1], state0[2]/2.0, p[0], p[1], p[2]};
}

//.........这里部分代码省略.........
        cv::circle(debug_image, point, 3,
                   cv::Scalar(0, 0, 255 * std::min(1.0, z.f / 20.0)), -1);
      }
#endif
      double distance = sqrt(pow(point.x - req.point.x ,2) + pow(point.y - req.point.y,2) ); //Distance (2D) from request point to projected lidar point
      if (distance < req.tolerance) {
#ifdef DO_ROS_DEBUG
        if (int(point.x - 20) > 0 && int(point.x + 20) < camera_info.width &&
            int(point.y - 20) > 0 && int(point.y + 20) < camera_info.height) {
          cv::circle(debug_image, point, 3, cv::Scalar(0, 255, 0), -1);
        }
#endif
        matrixFindPlaneA(0, 0) += x.f * x.f;
        matrixFindPlaneA(1, 0) += y.f * x.f;
        matrixFindPlaneA(2, 0) += x.f;
        matrixFindPlaneA(0, 1) += x.f * y.f;
        matrixFindPlaneA(1, 1) += y.f * y.f;
        matrixFindPlaneA(2, 1) += y.f;
        matrixFindPlaneA(0, 2) += x.f;
        matrixFindPlaneA(1, 2) += y.f;
        matrixFindPlaneA(2, 2) += 1;
        matrixFindPlaneB(0, 0) -= x.f * z.f;
        matrixFindPlaneB(1, 0) -= y.f * z.f;
        matrixFindPlaneB(2, 0) -= z.f;
        
        geometry_msgs::Point geo_point;
        geo_point.x = x.f;
        geo_point.y = y.f;
        geo_point.z = z.f;
        if (distance < minDistance)
        {
          res.closest = geo_point;
          minDistance = distance;
        }
        res.transformed.push_back(geo_point);
      }
    }
  }
  if (res.transformed.size() < 1) {
    res.success = false;
    res.error = navigator_msgs::CameraToLidarTransform::Response::NO_POINTS_FOUND;
    return true;
  }
  
  //Filter out lidar points behind the plane (I don't think this will work, no good way to do this)
  // ~double min_z = (*std::min_element(res.transformed.begin(),res.transformed.end(), [=] (const geometry_msgs::Point& a,const geometry_msgs::Point& b) {
    // ~return a.z < b.z && a.z > MIN_Z;
  // ~})).z;
  // ~res.transformed.erase(std::remove_if(res.transformed.begin(),res.transformed.end(),[min_z] (const geometry_msgs::Point& p) {
    // ~return (p.z - min_z) < MAX_Z_ERROR; 
  // ~}),res.transformed.end());

  res.distance = res.closest.z;
  Eigen::Vector3d normalvec = matrixFindPlaneA.colPivHouseholderQr()
                                  .solve(matrixFindPlaneB)
                                  .normalized();
  geometry_msgs::Vector3 normalvec_ros;
  normalvec_ros.x = normalvec(0, 0);
  normalvec_ros.y = normalvec(1, 0);
  normalvec_ros.z = normalvec(2, 0);
  res.normal = normalvec_ros;
  std::cout << "Plane solution: " << normalvec << std::endl;

#ifdef DO_ROS_DEBUG
  //Create marker for normal
  visualization_msgs::Marker marker_normal;
  marker_normal.header = req.header;
  marker_normal.header.seq = 0;
  marker_normal.header.frame_id =  req.header.frame_id;
  marker_normal.id = 3000;
  marker_normal.type = visualization_msgs::Marker::ARROW;

  geometry_msgs::Point sdp_normalvec_ros;
  sdp_normalvec_ros.x = normalvec_ros.x + res.transformed[0].x;
  sdp_normalvec_ros.y = normalvec_ros.y + res.transformed[0].y;
  sdp_normalvec_ros.z = normalvec_ros.z + res.transformed[0].z;
  marker_normal.points.push_back(res.closest);
  marker_normal.points.push_back(sdp_normalvec_ros);

  marker_normal.scale.x = 0.1;
  marker_normal.scale.y = 0.5;
  marker_normal.scale.z = 0.5;
  marker_normal.color.a = 1.0;
  marker_normal.color.r = 1.0;
  marker_normal.color.g = 0.0;
  marker_normal.color.b = 0.0;
  markers.markers.push_back(marker_normal);
  
  //Publish 3D debug market
  pubMarkers.publish(markers);
  
  //Publish debug image
  cv_bridge::CvImage ros_debug_image;
  ros_debug_image.encoding = "bgr8";
  ros_debug_image.image = debug_image.clone();
  points_debug_publisher.publish(ros_debug_image.toImageMsg());
#endif
  res.success = true;
  return true;
}

//.........这里部分代码省略.........

  // Gaussian reduction
  for (int k = 0; k < 3; ++k)
    for (int i = k + 1; i < size; ++i)
      system.row(i) = system.row(i) - system.row(k) * system.row(i)[10 + k] / system.row(k)[10 + k];

  // Quaternion q
  Eigen::Vector4d q;

  // Transform to inhomogeneous (Eq. 13)
  bool P_is_ok(false);
  while (not P_is_ok)
  {
    Eigen::Matrix4d P(Eigen::Matrix4d::Random());
    while (P.determinant() < 1e-5)
      P = Eigen::Matrix4d::Random();

    Eigen::MatrixXd reduced_system(size - 3, 10);
    for (int i = 3; i < size; ++i)
    {
      const Eigen::VectorXd & row = system.row(i);
      Eigen::Matrix4d Mi_tilde;

      Mi_tilde << row[0]    , row[1] / 2, row[2] / 2, row[3] / 2,
                  row[1] / 2, row[4]    , row[5] / 2, row[6] / 2,
                  row[2] / 2, row[5] / 2, row[7]    , row[8] / 2,
                  row[3] / 2, row[6] / 2, row[8] / 2, row[9]    ;

      Eigen::Matrix4d Mi_bar(P.transpose() * Mi_tilde * P);

      reduced_system.row(i - 3) << Mi_bar(0, 0),
                                   Mi_bar(0, 1) + Mi_bar(1, 0),
                                   Mi_bar(0, 2) + Mi_bar(2, 0),
                                   Mi_bar(0, 3) + Mi_bar(3, 0),
                                   Mi_bar(1, 1),
                                   Mi_bar(1, 2) + Mi_bar(2, 1),
                                   Mi_bar(1, 3) + Mi_bar(3, 1),
                                   Mi_bar(2, 2),
                                   Mi_bar(2, 3) + Mi_bar(3, 2),
                                   Mi_bar(3, 3);
    }

    // Solve  A m* = b
    Eigen::MatrixXd A = reduced_system.rightCols<9>();
    Eigen::VectorXd b = - reduced_system.leftCols<1>();
    Eigen::VectorXd m_star = A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(b);

    Eigen::Vector4d q_bar(1, m_star[0], m_star[1], m_star[2]);

    Eigen::VectorXd err(6);
    err << q_bar[1] * q_bar[1],
           q_bar[1] * q_bar[2],
           q_bar[1] * q_bar[3],
           q_bar[2] * q_bar[2],
           q_bar[2] * q_bar[3],
           q_bar[3] * q_bar[3];
    err -= m_star.tail<6>();

    //if (err.norm() < 0.1) // P is not ok?
      P_is_ok = true;

    //std::cout << m_star.transpose() << std::endl;
    //std::cout << q_bar[1] * q_bar[1] << " " << q_bar[1] * q_bar[2] << " " << q_bar[1] * q_bar[3] << " "
    //<< q_bar[2] * q_bar[2] << " " << q_bar[2] * q_bar[3] << " " << q_bar[3] * q_bar[3] << std::endl;

    q = P * q_bar;
  }

  // We want q.w > 0 (Why?)
  //if (q[0] < 0)
  //  q = -q;

  Eigen::Vector4d tmp_q(q.normalized());
  Eigen::Quaterniond rotation(tmp_q[0], tmp_q[1], tmp_q[2], tmp_q[3]);

  Eigen::VectorXd m(10);
  m << q[0] * q[0],
       q[0] * q[1],
       q[0] * q[2],
       q[0] * q[3],
       q[1] * q[1],
       q[1] * q[2],
       q[1] * q[3],
       q[2] * q[2],
       q[2] * q[3],
       q[3] * q[3];

  // Solve A (q^T q t) = b

  Eigen::Matrix3d A = system.topRightCorner<3, 3>();
  Eigen::Vector3d b = - system.topLeftCorner<3, 10>() * m;
  Eigen::Translation3d translation(A.colPivHouseholderQr().solve(b) / q.squaredNorm());

//  Eigen::Quaterniond tmp(pose.linear());
//  Eigen::Translation3d tmp2(pose.translation());
//  std::cout << "Prev: " << tmp.normalized().coeffs().transpose() << " " << tmp2.vector().transpose() << std::endl;
  //std::cout << "PoP : " << rotation.coeffs().transpose() << " " << translation.vector().transpose() << std::endl;

  return translation * rotation;
}