diff --git a/gtsam/geometry/Line3.cpp b/gtsam/geometry/Line3.cpp index af06e5a9b..5f1487153 100644 --- a/gtsam/geometry/Line3.cpp +++ b/gtsam/geometry/Line3.cpp @@ -1,136 +1,97 @@ #include +#include namespace gtsam { - -Vector6 Line3::points() -{ - Vector3 mid; - mid << a_, b_, 0; - Vector3 mid_rot = R_*mid; - Vector3 start = mid_rot + R_.r3(); - Vector3 end = mid_rot - R_.r3(); - Vector6 start_end; - start_end << start(0), start(1), start(2), - end(0), end(1), end(2); - return start_end; +Line3 Line3::retract(const Vector4 &v, OptionalJacobian<4, 4> H) const { + Vector3 w; + w << v[0], v[1], 0; + Rot3 eps; + if (H) { + OptionalJacobian<3, 3> Dw; + eps = Rot3::Expmap(w, Dw); + H->block<2, 2>(0, 0) = Dw->block<2, 2>(0, 0); + (*H)(2, 2) = 1; + (*H)(3, 3) = 1; + } else { + eps = Rot3::Expmap(w); + } + Rot3 Rt = R_ * eps; + return Line3(Rt, a_ + v[2], b_ + v[3]); } - -Line3 Line3::retract(const Vector4 &v, OptionalJacobian<4,4> H) const { - Vector3 w; - w << v[0], v[1], 0; - Rot3 eps; - if(H) - { - OptionalJacobian<3,3> Dw; - eps = Rot3::Expmap(w, Dw); - H->block<2,2>(0,0) = Dw->block<2,2>(0,0); - (*H)(2,2) = 1; - (*H)(3,3) = 1; - } - else - { - eps = Rot3::Expmap(w); - } - Rot3 Rt = R_*eps; - return Line3(Rt, a_+v[2], b_+v[3]); +Vector4 Line3::localCoordinates(const Line3 &q, OptionalJacobian<4, 4> H) const { + Vector3 local_rot; + Vector4 local; + Vector2 ab_q(q.V()); + if (H) { + OptionalJacobian<3, 3> Dw; + local_rot = Rot3::Logmap(R_.inverse() * q.R(), Dw); + H->block<2, 2>(0, 0) = Dw->block<2, 2>(0, 0); + (*H)(2, 2) = 1; + (*H)(3, 3) = 1; + } else { + local_rot = Rot3::Logmap(R_.inverse() * q.R()); + } + local << local_rot[0], local_rot[1], ab_q[0] - a_, ab_q[1] - b_; + return local; } - -Vector4 Line3::localCoordinates(const Line3 &q, OptionalJacobian<4,4> H) const { - Vector3 local_rot; - Vector4 local; - Vector2 ab_q(q.V()); - if(H) - { - OptionalJacobian<3,3> Dw; - local_rot = Rot3::Logmap(R_.inverse()*q.R(), Dw); - H->block<2,2>(0, 0) = Dw->block<2,2>(0,0); - (*H)(2,2) = 1; - (*H)(3,3) = 1; - } - else - { - local_rot = Rot3::Logmap(R_.inverse()*q.R()); - } - local << local_rot[0], local_rot[1], ab_q[0] - a_, ab_q[1] - b_; - return local; +void Line3::print(const std::string &s) const { + std::cout << s << std::endl; + R_.print("R:\n"); + std::cout << "a: " << a_ << ", b: " << b_ << std::endl; } - -void Line3::print(const std::string &s) const -{ - std::cout << s << std::endl; - R_.print("R:\n"); - std::cout << "a: " << a_ << ", b: " << b_ << std::endl; +bool Line3::equals(const Line3 &l2, double tol) const { + Vector4 diff = localCoordinates(l2); + return fabs(diff[0]) < tol && fabs(diff[1]) < tol + && fabs(diff[2]) < tol && fabs(diff[3]) < tol; } +Point3 Line3::project(OptionalJacobian<3, 4> Dline) const { + Vector3 V_0; + V_0 << -b_, a_, 0; -bool Line3::equals(const Line3 &l2, double tol) const -{ - Vector4 diff = localCoordinates(l2); - return fabs(diff[0]) < tol && fabs(diff[1]) < tol - && fabs(diff[2]) < tol && fabs(diff[3]) < tol; + Unit3 l = R_ * V_0; + if (Dline) { + Dline->setZero(); + Dline->col(0) = a_ * R_.r3(); + Dline->col(1) = b_ * R_.r3(); + Dline->col(2) = R_.r2(); + Dline->col(3) = -R_.r1(); + } + return l; } +Line3 transformTo(const Pose3 &wTc, const Line3 &wL, + OptionalJacobian<4, 6> Dpose, OptionalJacobian<4, 4> Dline) { + Rot3 wRl = wL.R(); + Rot3 wRc = wTc.rotation(); + Rot3 cRw = wRc.inverse(); + Rot3 cRl = cRw * wRl; -Point3 Line3::project(OptionalJacobian<3, 4> Dline) const -{ - Vector3 V_0; - V_0 << -b_, a_, 0; + Vector2 w_ab = wL.V(); + Vector3 t = (wRl.transpose() * wTc.translation()); + Vector2 c_ab(w_ab[0] - t[0], w_ab[1] - t[1]); - Point3 l = R_*V_0; - if(Dline) - { - Dline->setZero(); - Dline->col(0) = a_*R_.r3(); - Dline->col(1) = b_*R_.r3(); - Dline->col(2) = R_.r2(); - Dline->col(3) = -R_.r1(); - } - return l; -} + if (Dpose) { + // translation due to translation + Matrix3 cRl_mat = cRl.matrix(); + Matrix3 lRc = cRl_mat.transpose(); + Dpose->block<1, 3>(2, 3) = -lRc.row(0); + Dpose->block<1, 3>(3, 3) = -lRc.row(1); - -Point3 projectLine(const Line3 &L, - OptionalJacobian<3, 4> Dline) -{ - return L.project(Dline); -} - - -Line3 transformTo(const Pose3 &p, const Line3 &l, - OptionalJacobian<4, 6> Dpose, OptionalJacobian<4, 4> Dline) -{ - Rot3 w_R_l = l.R(); - Rot3 w_R_c = p.rotation(); - Rot3 c_R_w = w_R_c.inverse(); - Rot3 c_R_l = c_R_w*w_R_l; - - Vector2 ab_w = l.V(); - Vector3 t = (w_R_l.transpose()*p.translation()); - Vector2 ab_c(ab_w[0] - t[0], ab_w[1] - t[1]); - - if(Dpose) - { - // translation due to translation - Matrix3 c_R_l_mat = c_R_l.matrix(); - Matrix3 l_R_c = c_R_l_mat.transpose(); - Dpose->block<1,3>(2, 3) = -l_R_c.row(0); - Dpose->block<1,3>(3, 3) = -l_R_c.row(1); - - Dpose->block<1,3>(0, 0) = -l_R_c.row(0); - Dpose->block<1,3>(1, 0) = -l_R_c.row(1); - } - if(Dline) - { - Dline->col(0) << 1.0, 0.0, 0.0, -t[2]; - Dline->col(1) << 0.0, 1.0, t[2], 0.0; - Dline->col(2) << 0.0, 0.0, 1.0, 0.0; - Dline->col(3) << 0.0, 0.0, 0.0, 1.0; - } - return Line3(c_R_l, ab_c[0], ab_c[1]); + Dpose->block<1, 3>(0, 0) = -lRc.row(0); + Dpose->block<1, 3>(1, 0) = -lRc.row(1); + } + if (Dline) { + Dline->col(0) << 1.0, 0.0, 0.0, -t[2]; + Dline->col(1) << 0.0, 1.0, t[2], 0.0; + Dline->col(2) << 0.0, 0.0, 1.0, 0.0; + Dline->col(3) << 0.0, 0.0, 0.0, 1.0; + } + return Line3(cRl, c_ab[0], c_ab[1]); } } diff --git a/gtsam/geometry/Line3.h b/gtsam/geometry/Line3.h index 2ea22b1e0..5b2fb057d 100644 --- a/gtsam/geometry/Line3.h +++ b/gtsam/geometry/Line3.h @@ -18,15 +18,9 @@ #include #include -#include -#include -#include +#include -#include -#include -#include - -namespace gtsam{ +namespace gtsam { /** * A 3D line (R,a,b) : (Rot3,Scalar,Scalar) @@ -34,145 +28,99 @@ namespace gtsam{ * \nosubgrouping */ class Line3 { -private: - Rot3 R_; // Rotation of line about x and y in world frame - double a_, b_; // Intersection of line with x-y world plane + private: + Rot3 R_; // Rotation of line about x and y in world frame + double a_, b_; // Intersection of line with the world x-y plane rotated by R_ + // Also the closest point on line to origin + public: + enum { dimension = 4 }; -public: - enum {dimension = 4}; + /** Default constructor is the Z axis **/ + Line3() : + a_(0), b_(0) {} - /** Default constructor is the Z axis **/ - Line3() : - a_(0), b_(0) - {} + /** Constructor: + * Parallel to z axis, intersecting x-y plane at (a,b) **/ + Line3(const double a, const double b) : + a_(a), b_(b) {} - /** Constructor: - * Parallel to z axis, intersecting x-y plane at (a,b) **/ - Line3(const double a, const double b): - a_(a), b_(b) - {} + /** Constructor for general line from (R, a, b) **/ + Line3(const Rot3 &R, const double a, const double b) : + R_(R), a_(a), b_(b) {} - /** Constructor for general line from (R, a, b) **/ - Line3(const Rot3 &R, const double a, const double b) : - R_(R), a_(a), b_(b) - {} + /** + * The retract method maps from the tangent space back to the manifold. + * The tangent space for the rotation of a line is only two dimensional - + * rotation about x and y + * @param v: increment in tangent space + * @param H: Jacobian of retraction with respect to the increment + * @return: resulting line after adding the increment and mapping to the manifold + */ + Line3 retract(const Vector4 &v, OptionalJacobian<4, 4> H = boost::none) const; - /** Sample two points lying on the line - * @return Vector6 where first 3 elements are one point, and - * next 3 elements are another point **/ - Vector6 points(); + /** + * The localCoordinates method is the inverse of retract and finds the difference + * between two lines in the tangent space. + * @param q Line3 on manifold + * @param H OptionalJacobian of localCoordinates with respect to line + * @return difference in the tangent space + */ + Vector4 localCoordinates(const Line3 &q, OptionalJacobian<4, 4> H = boost::none) const; - /** - * The retract method maps from the tangent space back to the manifold. - * The tangent space for the rotation of a line is only two dimensional - - * rotation about x and y - * @param v: increment in tangent space - * @param H: Jacobian of retraction with respect to the increment - * @return: resulting line after adding the increment and mapping to the manifold - */ - Line3 retract(const Vector4 &v, OptionalJacobian<4,4> H = boost::none) const; + /** + * Rotation of line accessor + * @return Rot3 + */ + Rot3 R() const { + return R_; + } - /** - * The localCoordinates method is the inverse of retract and finds the difference - * between two lines in the tangent space. - * @param q Line3 on manifold - * @param H OptionalJacobian of localCoordinates with respect to line - * @return Vector4 difference in the tangent space - */ - Vector4 localCoordinates(const Line3 &q, OptionalJacobian<4,4> H = boost::none) const; + /** + * Accessor for a, b + * @return Vector2(a, b) + */ + Vector2 V() const { + return Vector2(a_, b_); + } - /** - * Rotation of line accessor - * @return Rot3 - */ - Rot3 R() const - { - return R_; - } + /** + * Print R, a, b + * @param s: optional starting string + */ + void print(const std::string &s = "") const; - /** - * Accessor for a, b - * @return Vector2(a, b) - */ - Vector2 V() const - { - return Vector2(a_, b_); - } + /** + * Check if two lines are equal + * @param l2 - line to be compared + * @param tol : optional tolerance + * @return boolean - true if lines are equal + */ + bool equals(const Line3 &l2, double tol = 10e-9) const; - /** - * Print R, a, b - * @param s: optional starting string - */ - void print(const std::string &s = "") const; - - /** - * Check if two lines are equal - * @param l2 - line to be compared - * @param tol : optional tolerance - * @return boolean - true if lines are equal - */ - bool equals(const Line3 &l2, double tol = 10e-9) const; - - /** - * Projecting a line to an image plane - * @param Dline: OptionalJacobian of projected line with respect to this line - * @return Point3 - projected line in image plane - */ - Point3 project(OptionalJacobian<3, 4> Dline = boost::none) const; + /** + * Projecting a line to the image plane. Assumes this line is in camera frame. + * @param Dline: OptionalJacobian of projected line with respect to this line + * @return Unit3 - projected line in image plane, in homogenous coordinates. + * We use Unit3 since it is a manifold with the right dimension. + */ + Point3 project(OptionalJacobian<3, 4> Dline = boost::none) const; }; -template <> +template<> struct traits : public internal::Manifold {}; -template <> +template<> struct traits : public internal::Manifold {}; -// For expression factor graphs -typedef Expression Line3_; - -/** - * Function projects input line to image plane - * @param L input line - * @param Dline OptionalJacbian of projected line with respect to input line - * @return Point3 - projected line - */ -Point3 projectLine(const Line3 &L, - OptionalJacobian<3, 4> Dline = boost::none); - /** * Transform a line from world to camera frame - * @param p - Pose3 of camera in world frame - * @param l - Line3 in world frame + * @param wTc - Pose3 of camera in world frame + * @param wL - Line3 in world frame * @param Dpose - OptionalJacobian of transformed line with respect to p * @param Dline - OptionalJacobian of transformed line with respect to l * @return Transformed line in camera frame */ -Line3 transformTo(const Pose3 &p, const Line3 &l, - OptionalJacobian<4, 6> Dpose = boost::none, - OptionalJacobian<4, 4> Dline = boost::none); - -/** - * Expression method for projection - * @param line Line3_ expression - * @return Point3_ projected line - */ -inline Point3_ projectLineExpression(const Line3_& line) -{ - Point3 (*f)(const Line3&, OptionalJacobian<3, 4>) = &projectLine; - return Point3_(f, line); -} - -/** - * Expression method for transforming line - * @param p - Pose3_ expression - * @param line - Line3 expression - * @return transformed Line3_ expression - */ -inline Line3_ transformToExpression(const Pose3_& p, const Line3_& line) -{ - Line3 (*f)(const Pose3&, const Line3&, - OptionalJacobian<4,6>, OptionalJacobian<4,4>) = &transformTo; - return Line3_(f, p, line); -} - +Line3 transformTo(const Pose3 &wTc, const Line3 &wL, + OptionalJacobian<4, 6> Dpose = boost::none, + OptionalJacobian<4, 4> Dline = boost::none); } \ No newline at end of file diff --git a/gtsam/geometry/tests/testLine3.cpp b/gtsam/geometry/tests/testLine3.cpp index e46fed479..ba4cb4670 100644 --- a/gtsam/geometry/tests/testLine3.cpp +++ b/gtsam/geometry/tests/testLine3.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include #include @@ -117,7 +118,7 @@ TEST(Line3, transformToExpressionJacobians) val.insert(2, p); SharedNoiseModel model = noiseModel::Isotropic::Sigma(4, 0.1); - ExpressionFactor f(model, transformTo(p, l_w), transformToExpression(p_, l_)); + ExpressionFactor f(model, transformTo(p, l_w), transformTo(p_, l_)); EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7); } @@ -127,17 +128,18 @@ TEST(Line3, projection) { Rot3 r; r = r.Expmap(Vector3(0, 0, 0)); - Line3 l_w(r, 1, 1); + Line3 wL(r, 1, 1); Point3 expected(-1, 1, 0); - EXPECT(expected.equals(projectLine(l_w))); + EXPECT(expected.equals(wL.project())); Values val; - val.insert(1, l_w); - Line3_ l_w_(1); + val.insert(1, wL); + Line3_ wL_(1); SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1); - ExpressionFactor f(model, expected, projectLineExpression(l_w_)); + Point3_ projected_(wL_, &Line3::project); + ExpressionFactor f(model, expected, projected_); EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7); } diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index 4294d17d1..d60923d8e 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -11,6 +11,7 @@ #include #include #include +#include #include namespace gtsam { @@ -31,6 +32,7 @@ typedef Expression Point3_; typedef Expression Unit3_; typedef Expression Rot3_; typedef Expression Pose3_; +typedef Expression Line3_; inline Point3_ transformTo(const Pose3_& x, const Point3_& p) { return Point3_(x, &Pose3::transformTo, p); @@ -40,6 +42,12 @@ inline Point3_ transformFrom(const Pose3_& x, const Point3_& p) { return Point3_(x, &Pose3::transformFrom, p); } +inline Line3_ transformTo(const Pose3_ &wTc, const Line3_ &wL) { + Line3 (*f)(const Pose3 &, const Line3 &, + OptionalJacobian<4, 6>, OptionalJacobian<4, 4>) = &transformTo; + return Line3_(f, wTc, wL); +} + namespace internal { // define getter that returns value rather than reference inline Rot3 rotation(const Pose3& pose, OptionalJacobian<3, 6> H) {