diff --git a/gtsam/geometry/Line3.cpp b/gtsam/geometry/Line3.cpp index 4982df7b1..c8e133636 100644 --- a/gtsam/geometry/Line3.cpp +++ b/gtsam/geometry/Line3.cpp @@ -3,116 +3,116 @@ namespace gtsam { Line3 Line3::retract(const Vector4 &v, OptionalJacobian<4, 4> H) const { - Vector3 w; - w << v[0], v[1], 0; - Rot3 eps; - if (H) { - Eigen::Matrix3d Dw_mat = Eigen::Matrix3d::Zero(); - OptionalJacobian<3, 3> Dw(Dw_mat); - Dw->setZero(); - 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]); + Vector3 w; + w << v[0], v[1], 0; + Rot3 eps; + if (H) { + Eigen::Matrix3d Dw_mat = Eigen::Matrix3d::Zero(); + OptionalJacobian<3, 3> Dw(Dw_mat); + Dw->setZero(); + 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; - if (H) { - Eigen::Matrix3d Dw_mat = Eigen::Matrix3d::Zero(); - OptionalJacobian<3, 3> Dw(Dw_mat); - Dw->setZero(); - 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], q.a_ - a_, q.b_ - b_; - return local; + Vector3 local_rot; + Vector4 local; + if (H) { + Eigen::Matrix3d Dw_mat = Eigen::Matrix3d::Zero(); + OptionalJacobian<3, 3> Dw(Dw_mat); + Dw->setZero(); + 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], q.a_ - a_, q.b_ - 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; + 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; + Vector4 diff = localCoordinates(l2); + return fabs(diff[0]) < tol && fabs(diff[1]) < tol + && fabs(diff[2]) < tol && fabs(diff[3]) < tol; } Unit3 Line3::project(OptionalJacobian<2, 4> Dline) const { - Vector3 V_0; - V_0 << -b_, a_, 0.0; + Vector3 V_0; + V_0 << -b_, a_, 0.0; - Unit3 l; - if (Dline) { - // Jacobian of the normalized Unit3 projected line with respect to - // un-normalized Vector3 projected line in homogeneous coordinates. - Eigen::Matrix D_mat = Eigen::Matrix::Zero(); - OptionalJacobian<2, 3> D_unit_line(D_mat); - l = Unit3::FromPoint3(Point3(R_ * V_0), D_unit_line); - // Jacobian of the un-normalized Vector3 line with respect to - // input 3D line - Eigen::Matrix D_vec_line = Eigen::Matrix::Zero(); - D_vec_line.col(0) = a_ * R_.r3(); - D_vec_line.col(1) = b_ * R_.r3(); - D_vec_line.col(2) = R_.r2(); - D_vec_line.col(3) = -R_.r1(); - // Jacobian of output wrt input is the product of the two. - Eigen::Matrix Dline_mat = (*D_unit_line) * D_vec_line; - Dline = OptionalJacobian<2, 4>(Dline_mat); - } else { - l = Unit3::FromPoint3(Point3(R_ * V_0)); - } - return l; + Unit3 l; + if (Dline) { + // Jacobian of the normalized Unit3 projected line with respect to + // un-normalized Vector3 projected line in homogeneous coordinates. + Eigen::Matrix D_mat = Eigen::Matrix::Zero(); + OptionalJacobian<2, 3> D_unit_line(D_mat); + l = Unit3::FromPoint3(Point3(R_ * V_0), D_unit_line); + // Jacobian of the un-normalized Vector3 line with respect to + // input 3D line + Eigen::Matrix D_vec_line = Eigen::Matrix::Zero(); + D_vec_line.col(0) = a_ * R_.r3(); + D_vec_line.col(1) = b_ * R_.r3(); + D_vec_line.col(2) = R_.r2(); + D_vec_line.col(3) = -R_.r1(); + // Jacobian of output wrt input is the product of the two. + Eigen::Matrix Dline_mat = (*D_unit_line) * D_vec_line; + Dline = OptionalJacobian<2, 4>(Dline_mat); + } else { + l = Unit3::FromPoint3(Point3(R_ * V_0)); + } + return l; } Point3 Line3::point(double distance) const { - // defining "center" of the line to be the point where it - // intersects rotated XY axis - Point3 center(a_, b_, 0); - Point3 rotated_center = R_ * center; - return rotated_center + distance * R_.r3(); + // defining "center" of the line to be the point where it + // intersects rotated XY axis + Point3 center(a_, b_, 0); + Point3 rotated_center = R_ * center; + return rotated_center + distance * R_.r3(); } Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian<4, 6> Dpose, OptionalJacobian<4, 4> Dline) { - Rot3 wRc = wTc.rotation(); - Rot3 cRw = wRc.inverse(); - Rot3 cRl = cRw * wL.R_; + Rot3 wRc = wTc.rotation(); + Rot3 cRw = wRc.inverse(); + Rot3 cRl = cRw * wL.R_; - Vector2 w_ab; - Vector3 t = ((wL.R_).transpose() * wTc.translation()); - Vector2 c_ab(wL.a_ - t[0], wL.b_ - t[1]); + Vector2 w_ab; + Vector3 t = ((wL.R_).transpose() * wTc.translation()); + Vector2 c_ab(wL.a_ - t[0], wL.b_ - t[1]); - 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); + 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); - 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]); + 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 a176039d5..fb0c878ea 100644 --- a/gtsam/geometry/Line3.h +++ b/gtsam/geometry/Line3.h @@ -27,83 +27,83 @@ 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 the world x-y plane rotated by R_ - // Also the closest point on line to origin -public: - enum { dimension = 4 }; + 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 }; - /** Default constructor is the Z axis **/ - Line3() : - a_(0), b_(0) {} + /** Default constructor is the Z axis **/ + Line3() : + a_(0), b_(0) {} - /** 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; + /** + * 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; - /** - * 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 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; - /** - * Print R, a, b - * @param s: optional starting string - */ - void print(const std::string &s = "") 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; + /** + * 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 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. - */ - Unit3 project(OptionalJacobian<2, 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. + */ + Unit3 project(OptionalJacobian<2, 4> Dline = boost::none) const; - /** - * Returns point on the line that is at a certain distance starting from the - * point where the rotated XY axis intersects the line. - * @param distance (can be positive or negative) - positive is the positive - * direction of the rotated z axis that forms the line. The default value of zero - * returns the point where the rotated XY axis intersects the line. - * @return Point on the line - */ - Point3 point(double distance = 0) const; + /** + * Returns point on the line that is at a certain distance starting from the + * point where the rotated XY axis intersects the line. + * @param distance (can be positive or negative) - positive is the positive + * direction of the rotated z axis that forms the line. The default value of zero + * returns the point where the rotated XY axis intersects the line. + * @return Point on the line + */ + Point3 point(double distance = 0) const; - /** - * Transform a line from world to camera 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 - */ - friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL, - OptionalJacobian<4, 6> Dpose, - OptionalJacobian<4, 4> Dline); + /** + * Transform a line from world to camera 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 + */ + friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL, + OptionalJacobian<4, 6> Dpose, + OptionalJacobian<4, 4> Dline); }; /** diff --git a/gtsam/geometry/tests/testLine3.cpp b/gtsam/geometry/tests/testLine3.cpp index 7e362df3c..6d20196e9 100644 --- a/gtsam/geometry/tests/testLine3.cpp +++ b/gtsam/geometry/tests/testLine3.cpp @@ -16,119 +16,119 @@ static const Line3 l(Rot3(), 1, 1); // Testing equals function of Line3 TEST(Line3, equals) { - Line3 l_same = l; - EXPECT(l.equals(l_same)); - Line3 l2(Rot3(), 1, 2); - EXPECT(!l.equals(l2)); + Line3 l_same = l; + EXPECT(l.equals(l_same)); + Line3 l2(Rot3(), 1, 2); + EXPECT(!l.equals(l2)); } // testing localCoordinates along 4 dimensions TEST(Line3, localCoordinates) { - // l1 and l differ only in a_ - Line3 l1(Rot3(), 2, 1); - Vector4 v1(0, 0, -1, 0); - CHECK(assert_equal(l1.localCoordinates(l), v1)); + // l1 and l differ only in a_ + Line3 l1(Rot3(), 2, 1); + Vector4 v1(0, 0, -1, 0); + CHECK(assert_equal(l1.localCoordinates(l), v1)); - // l2 and l differ only in b_ - Line3 l2(Rot3(), 1, 2); - Vector4 v2(0, 0, 0, -1); - CHECK(assert_equal(l2.localCoordinates(l), v2)); + // l2 and l differ only in b_ + Line3 l2(Rot3(), 1, 2); + Vector4 v2(0, 0, 0, -1); + CHECK(assert_equal(l2.localCoordinates(l), v2)); - // l3 and l differ in R_x - Rot3 r3 = Rot3::Expmap(Vector3(M_PI / 4, 0, 0)); - Line3 l3(r3, 1, 1); - Vector4 v3(-M_PI / 4, 0, 0, 0); - CHECK(assert_equal(l3.localCoordinates(l), v3)); + // l3 and l differ in R_x + Rot3 r3 = Rot3::Expmap(Vector3(M_PI / 4, 0, 0)); + Line3 l3(r3, 1, 1); + Vector4 v3(-M_PI / 4, 0, 0, 0); + CHECK(assert_equal(l3.localCoordinates(l), v3)); - // l4 and l differ in R_y - Rot3 r4; - r4 = r4.Expmap(Vector3(0, M_PI / 4, 0)); - Line3 l4(r4, 1, 1); - Vector4 v4(0, -M_PI / 4, 0, 0); - CHECK(assert_equal(l4.localCoordinates(l), v4)); + // l4 and l differ in R_y + Rot3 r4; + r4 = r4.Expmap(Vector3(0, M_PI / 4, 0)); + Line3 l4(r4, 1, 1); + Vector4 v4(0, -M_PI / 4, 0, 0); + CHECK(assert_equal(l4.localCoordinates(l), v4)); } // testing retract along 4 dimensions TEST(Line3, retract) { - // l1 and l differ only in a_ - Vector4 v1(0, 0, 0, 1); - Line3 l1(Rot3(), 1, 2); - EXPECT(l1.equals(l.retract(v1))); + // l1 and l differ only in a_ + Vector4 v1(0, 0, 0, 1); + Line3 l1(Rot3(), 1, 2); + EXPECT(l1.equals(l.retract(v1))); - // l2 and l differ only in b_ - Vector4 v2(0, 0, 1, 0); - Line3 l2(Rot3(), 2, 1); - EXPECT(l2.equals(l.retract(v2))); + // l2 and l differ only in b_ + Vector4 v2(0, 0, 1, 0); + Line3 l2(Rot3(), 2, 1); + EXPECT(l2.equals(l.retract(v2))); - // l3 and l differ in R_x - Vector4 v3(M_PI / 4, 0, 0, 0); - Rot3 r3; - r3 = r3.Expmap(Vector3(M_PI / 4, 0, 0)); - Line3 l3(r3, 1, 1); - EXPECT(l3.equals(l.retract(v3))); + // l3 and l differ in R_x + Vector4 v3(M_PI / 4, 0, 0, 0); + Rot3 r3; + r3 = r3.Expmap(Vector3(M_PI / 4, 0, 0)); + Line3 l3(r3, 1, 1); + EXPECT(l3.equals(l.retract(v3))); - // l4 and l differ in R_y - Vector4 v4(0, M_PI / 4, 0, 0); - Rot3 r4 = Rot3::Expmap(Vector3(0, M_PI / 4, 0)); - Line3 l4(r4, 1, 1); - EXPECT(l4.equals(l.retract(v4))); + // l4 and l differ in R_y + Vector4 v4(0, M_PI / 4, 0, 0); + Rot3 r4 = Rot3::Expmap(Vector3(0, M_PI / 4, 0)); + Line3 l4(r4, 1, 1); + EXPECT(l4.equals(l.retract(v4))); } // testing manifold property - Retract(p, Local(p,q)) == q TEST(Line3, retractOfLocalCoordinates) { - Rot3 r2 = Rot3::Expmap(Vector3(M_PI / 4, M_PI / 3, 0)); - Line3 l2(r2, 5, 9); - EXPECT(assert_equal(l.retract(l.localCoordinates(l2)), l2)); + Rot3 r2 = Rot3::Expmap(Vector3(M_PI / 4, M_PI / 3, 0)); + Line3 l2(r2, 5, 9); + EXPECT(assert_equal(l.retract(l.localCoordinates(l2)), l2)); } // testing manifold property - Local(p, Retract(p, v)) == v TEST(Line3, localCoordinatesOfRetract) { - Vector4 r2(2.3, 0.987, -3, 4); - EXPECT(assert_equal(l.localCoordinates(l.retract(r2)), r2)); + Vector4 r2(2.3, 0.987, -3, 4); + EXPECT(assert_equal(l.localCoordinates(l.retract(r2)), r2)); } // transform from world to camera test TEST(Line3, transformToExpressionJacobians) { - Rot3 r = Rot3::Expmap(Vector3(0, M_PI / 3, 0)); - Vector3 t(0, 0, 0); - Pose3 p(r, t); + Rot3 r = Rot3::Expmap(Vector3(0, M_PI / 3, 0)); + Vector3 t(0, 0, 0); + Pose3 p(r, t); - Line3 l_c(r.inverse(), 1, 1); - Line3 l_w(Rot3(), 1, 1); - EXPECT(l_c.equals(transformTo(p, l_w))); + Line3 l_c(r.inverse(), 1, 1); + Line3 l_w(Rot3(), 1, 1); + EXPECT(l_c.equals(transformTo(p, l_w))); - Line3_ l_(1); - Pose3_ p_(2); - Values val; - val.insert(1, l_w); - val.insert(2, p); + Line3_ l_(1); + Pose3_ p_(2); + Values val; + val.insert(1, l_w); + val.insert(2, p); - SharedNoiseModel model = noiseModel::Isotropic::Sigma(4, 0.1); - ExpressionFactor f(model, transformTo(p, l_w), transformTo(p_, l_)); - EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(4, 0.1); + ExpressionFactor f(model, transformTo(p, l_w), transformTo(p_, l_)); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7); } // projection in camera frame test TEST(Line3, projection) { - Rot3 r = Rot3::Expmap(Vector3(0, 0, 0)); - Line3 wL(r, 1, 1); + Rot3 r = Rot3::Expmap(Vector3(0, 0, 0)); + Line3 wL(r, 1, 1); - Unit3 expected = Unit3::FromPoint3(Point3(-1, 1, 0)); - EXPECT(expected.equals(wL.project())); + Unit3 expected = Unit3::FromPoint3(Point3(-1, 1, 0)); + EXPECT(expected.equals(wL.project())); - Values val; - val.insert(1, wL); - Line3_ wL_(1); + Values val; + val.insert(1, wL); + Line3_ wL_(1); - SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.1); - Unit3_ projected_(wL_, &Line3::project); - ExpressionFactor f(model, expected, projected_); - EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.1); + Unit3_ projected_(wL_, &Line3::project); + ExpressionFactor f(model, expected, projected_); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7); } int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); + TestResult tr; + return TestRegistry::runAllTests(tr); }