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 {
 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 };
 
  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 = boost::none,
                OptionalJacobian<4, 4> Dv = boost::none) const;
 
  Vector4 localCoordinates(const Line3 &q,
                           OptionalJacobian<4, 4> Dp = boost::none,
                           OptionalJacobian<4, 4> Dq = boost::none) const;
 
  void print(const std::string &s = "") const;
 
  bool equals(const Line3 &l2, double tol = 10e-9) const;
 
  Unit3 project(OptionalJacobian<2, 4> Dline = boost::none) 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_;
  }
 
  friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
                           OptionalJacobian<4, 6> Dpose,
                           OptionalJacobian<4, 4> Dline);
};
 
Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
                  OptionalJacobian<4, 6> Dpose = boost::none,
                  OptionalJacobian<4, 4> Dline = boost::none);
 
template<>
struct traits<Line3> : public internal::Manifold<Line3> {};
 
template<>
struct traits<const Line3> : public internal::Manifold<Line3> {};
}