gtsam  4.1.0
gtsam
Point3.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
20 // \callgraph
21 
22 #pragma once
23 
24 #include <gtsam/config.h>
25 #include <gtsam/base/VectorSpace.h>
26 #include <gtsam/base/Vector.h>
27 #include <gtsam/dllexport.h>
28 #include <boost/serialization/nvp.hpp>
29 #include <numeric>
30 
31 namespace gtsam {
32 
35 typedef Vector3 Point3;
36 
37 // Convenience typedef
38 typedef std::pair<Point3, Point3> Point3Pair;
39 GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
40 
42 GTSAM_EXPORT double distance3(const Point3& p1, const Point3& q,
43  OptionalJacobian<1, 3> H1 = boost::none,
44  OptionalJacobian<1, 3> H2 = boost::none);
45 
47 GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = boost::none);
48 
50 GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none);
51 
53 GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q,
54  OptionalJacobian<3, 3> H_p = boost::none,
55  OptionalJacobian<3, 3> H_q = boost::none);
56 
58 GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
59  OptionalJacobian<1, 3> H_p = boost::none,
60  OptionalJacobian<1, 3> H_q = boost::none);
61 
63 template <class CONTAINER>
64 GTSAM_EXPORT Point3 mean(const CONTAINER& points) {
65  if (points.size() == 0) throw std::invalid_argument("Point3::mean input container is empty");
66  Point3 sum(0, 0, 0);
67  sum = std::accumulate(points.begin(), points.end(), sum);
68  return sum / points.size();
69 }
70 
72 GTSAM_EXPORT Point3Pair means(const std::vector<Point3Pair> &abPointPairs);
73 
74 template <typename A1, typename A2>
75 struct Range;
76 
77 template <>
78 struct Range<Point3, Point3> {
79  typedef double result_type;
80  double operator()(const Point3& p, const Point3& q,
81  OptionalJacobian<1, 3> H1 = boost::none,
82  OptionalJacobian<1, 3> H2 = boost::none) {
83  return distance3(p, q, H1, H2);
84  }
85 };
86 
87 } // namespace gtsam
88 
gtsam::normalize
Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H)
normalize, with optional Jacobian
Definition: Point3.cpp:51
gtsam::norm3
double norm3(const Point3 &p, OptionalJacobian< 1, 3 > H)
Distance of the point from the origin, with Jacobian.
Definition: Point3.cpp:40
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
gtsam::mean
GTSAM_EXPORT Point3 mean(const CONTAINER &points)
mean
Definition: Point3.h:64
gtsam::means
Point3Pair means(const std::vector< Point3Pair > &abPointPairs)
Calculate the two means of a set of Point3 pairs.
Definition: Point3.cpp:78
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
Vector.h
typedef and functions to augment Eigen's VectorXd
gtsam::Range
Definition: BearingRange.h:39
gtsam::distance3
double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1, OptionalJacobian< 1, 3 > H2)
distance between two points
Definition: Point3.cpp:26
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:35
gtsam::cross
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
cross product
Definition: Point3.cpp:63
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition: Vector.h:194