本文整理汇总了C++中eigen::Matrix::cross3方法的典型用法代码示例。如果您正苦于以下问题：C++ Matrix::cross3方法的具体用法？C++ Matrix::cross3怎么用？C++ Matrix::cross3使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类eigen::Matrix的用法示例。
在下文中一共展示了Matrix::cross3方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: plane_a_norm
template <typename Scalar> bool
pcl::planeWithPlaneIntersection (const Eigen::Matrix<Scalar, 4, 1> &plane_a,
const Eigen::Matrix<Scalar, 4, 1> &plane_b,
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line,
double angular_tolerance)
{
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
typedef Eigen::Matrix<Scalar, 5, 1> Vector5;
typedef Eigen::Matrix<Scalar, 5, 5> Matrix5;
// Normalize plane normals
Vector3 plane_a_norm (plane_a.template head<3> ());
Vector3 plane_b_norm (plane_b.template head<3> ());
plane_a_norm.normalize ();
plane_b_norm.normalize ();
// Test if planes are parallel
double test_cos = plane_a_norm.dot (plane_b_norm);
double tolerance_cos = 1 - sin (fabs (angular_tolerance));
if (fabs (test_cos) > tolerance_cos)
{
PCL_DEBUG ("Plane A and Plane B are parallel.\n");
return (false);
}
Vector4 line_direction = plane_a.cross3 (plane_b);
line_direction.normalized();
// Construct system of equations using lagrange multipliers with one objective function and two constraints
Matrix5 langrange_coefs;
langrange_coefs << 2,0,0, plane_a[0], plane_b[0],
0,2,0, plane_a[1], plane_b[1],
0,0,2, plane_a[2], plane_b[2],
plane_a[0], plane_a[1], plane_a[2], 0, 0,
plane_b[0], plane_b[1], plane_b[2], 0, 0;
Vector5 b;
b << 0, 0, 0, -plane_a[3], -plane_b[3];
line.resize(6);
// Solve for the lagrange multipliers
line.template head<3>() = langrange_coefs.colPivHouseholderQr().solve(b).template head<3> ();
line.template tail<3>() = line_direction.template head<3>();
return (true);
}