Line3.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */
 
// \callgraph
 
#pragma once
 
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
 
namespace gtsam {
 
class Line3;
 
GTSAM_EXPORT Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
                  OptionalJacobian<4, 6> Dpose = {},
                  OptionalJacobian<4, 4> Dline = {});
 
 
class GTSAM_EXPORT 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:
  inline constexpr static auto dimension = 4;
 
  Line3() :
      a_(0), b_(0) {}
 
  Line3(const Rot3 &R, const double a, const double b) :
      R_(R), a_(a), b_(b) {}
 
  Line3 retract(const Vector4 &v,
                OptionalJacobian<4, 4> Dp = {},
                OptionalJacobian<4, 4> Dv = {}) const;
 
  Vector4 localCoordinates(const Line3 &q,
                           OptionalJacobian<4, 4> Dp = {},
                           OptionalJacobian<4, 4> Dq = {}) const;
 
  void print(const std::string &s = "") const;
 
  bool equals(const Line3 &l2, double tol = 10e-9) const;
 
  Unit3 project(OptionalJacobian<2, 4> Dline = {}) const;
 
  Point3 point(double distance = 0) const;
 
  inline Rot3 R() const {
    return R_;
  }
 
  inline double a() const {
    return a_;
  }
 
  inline double b() const {
    return b_;
  }
 
  GTSAM_EXPORT friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
                           OptionalJacobian<4, 6> Dpose,
                           OptionalJacobian<4, 4> Dline);
};
 
template<>
struct traits<Line3> : public internal::Manifold<Line3> {};
 
template<>
struct traits<const Line3> : public internal::Manifold<Line3> {};
}