Point3.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/config.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/Vector.h>
#include <gtsam/dllexport.h>
#include <gtsam/base/VectorSerialization.h>
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#endif
#include <numeric>
 
namespace gtsam {

using Point3 = Vector3;
using Point3Vector = std::vector<Point3, Eigen::aligned_allocator<Point3>>;
 
// Convenience typedef
using Point3Pair = std::pair<Point3, Point3>;
GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
 
using Point3Pairs = std::vector<Point3Pair>;

GTSAM_EXPORT double distance3(const Point3& p1, const Point3& q,
                                  OptionalJacobian<1, 3> H1 = {},
                              OptionalJacobian<1, 3> H2 = {});

GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = {});

GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = {});

GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q,
                          OptionalJacobian<3, 3> H_p = {},
                          OptionalJacobian<3, 3> H_q = {});

GTSAM_EXPORT Point3 doubleCross(const Point3& p, const Point3& q,
                                OptionalJacobian<3, 3> H1 = {},
                                OptionalJacobian<3, 3> H2 = {});

GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
                        OptionalJacobian<1, 3> H_p = {},
                        OptionalJacobian<1, 3> H_q = {});

template <class CONTAINER>
Point3 mean(const CONTAINER& points) {
  if (points.size() == 0) throw std::invalid_argument("Point3::mean input container is empty");
  Point3 sum(0, 0, 0);
  sum = std::accumulate(points.begin(), points.end(), sum);
  return sum / points.size();
}

GTSAM_EXPORT Point3Pair means(const std::vector<Point3Pair> &abPointPairs);
 
template <typename A1, typename A2>
struct Range;
 
template <>
struct Range<Point3, Point3> {
  typedef double result_type;
  double operator()(const Point3& p, const Point3& q,
                    OptionalJacobian<1, 3> H1 = {},
                    OptionalJacobian<1, 3> H2 = {}) {
    return distance3(p, q, H1, H2);
  }
};
 
}  // namespace gtsam