17 return R_foo_line.
matrix().col(1);
28 normal_foo.transpose());
29 normal_foo.normalize();
30 return SO2<T>(normal_foo.y(), -normal_foo.x());
39 return R_foo_plane.
matrix().col(2);
61 "xDirHint (%) and yDirHint (%) must be perpendicular.",
62 xDirHint_foo.transpose(), yDirHint_foo.transpose());
65 T
const xDirHint_foo_sqr_length = xDirHint_foo.squaredNorm();
66 T
const yDirHint_foo_sqr_length = yDirHint_foo.squaredNorm();
67 T
const normal_foo_sqr_length = normal_foo.squaredNorm();
69 xDirHint_foo.transpose());
71 yDirHint_foo.transpose());
73 normal_foo.transpose());
76 basis_foo.col(2) = normal_foo;
79 xDirHint_foo.normalize();
82 yDirHint_foo.normalize();
85 basis_foo.col(2).normalize();
88 T abs_x_dot_z = abs(basis_foo.col(2).dot(xDirHint_foo));
89 T abs_y_dot_z = abs(basis_foo.col(2).dot(yDirHint_foo));
90 if (abs_x_dot_z < abs_y_dot_z) {
92 basis_foo.col(1) = basis_foo.col(2).cross(xDirHint_foo).normalized();
93 basis_foo.col(0) = basis_foo.col(1).cross(basis_foo.col(2));
96 basis_foo.col(0) = yDirHint_foo.cross(basis_foo.col(2)).normalized();
97 basis_foo.col(1) = basis_foo.col(2).cross(basis_foo.col(0));
99 T
det = basis_foo.determinant();
102 "Determinant of basis is not 1, but %. Basis is \n%\n", det,
133 T
const d = line_foo.offset();
136 return SE2<T>(R_foo_plane, -d * n);
155 T
const d = plane_foo.offset();
158 return SE3<T>(R_foo_plane, -d * n);
164 template <
class T,
int N>
166 const Eigen::Hyperplane<T, N>& plane) {
167 if (plane.offset() >= 0) {
171 return Eigen::Hyperplane<T, N>(-plane.normal(), -plane.offset());
176 #endif // GEOMETRY_HPP
Eigen::Hyperplane< T, 3 > Plane3
EIGEN_STRONG_INLINE Scalar det() const
Eigen::Hyperplane< T, 2 > Line2
#define SOPHUS_ENSURE(expr, description,...)
SOPHUS_FUNC TranslationMember & translation()
Vector2< T > normalFromSO2(SO2< T > const &R_foo_line)
Vector< Scalar, 2, Options > Vector2
static SOPHUS_FUNC Scalar epsilon()
Vector3< T > normalFromSO3(SO3< T > const &R_foo_plane)
SOPHUS_FUNC SO2Member & so2()
Vector< Scalar, 3, Options > Vector3
Eigen::Hyperplane< T, N > makeHyperplaneUnique(const Eigen::Hyperplane< T, N > &plane)
Matrix3< T > rotationFromNormal(Vector3< T > const &normal_foo, Vector3< T > xDirHint_foo=Vector3< T >(T(1), T(0), T(0)), Vector3< T > yDirHint_foo=Vector3< T >(T(0), T(1), T(0)))
SOPHUS_FUNC Transformation matrix() const
Matrix< Scalar, 3, 3 > Matrix3
SO3< T > SO3FromNormal(Vector3< T > const &normal_foo)
SOPHUS_FUNC SO3Member & so3()
SE2< T > SE2FromLine(Line2< T > const &line_foo)
SOPHUS_FUNC Transformation matrix() const
Plane3< T > planeFromSE3(SE3< T > const &T_foo_plane)
SOPHUS_FUNC TranslationMember & translation()
SE3< T > SE3FromPlane(Plane3< T > const &plane_foo)
Line2< T > lineFromSE2(SE2< T > const &T_foo_line)
SO2< T > SO2FromNormal(Vector2< T > normal_foo)