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:
  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 = {},
                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_;
  }

  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> {};
}