Loading...
Searching...
No Matches
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>
#include <boost/serialization/nvp.hpp>
#include <numeric>
namespace gtsam {
typedef Vector3 Point3;
typedef std::vector<Point3, Eigen::aligned_allocator<Point3> > Point3Vector;
// 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 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none);
GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = boost::none);
GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none);
GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q,
OptionalJacobian<3, 3> H_p = boost::none,
OptionalJacobian<3, 3> H_q = boost::none);
GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
OptionalJacobian<1, 3> H_p = boost::none,
OptionalJacobian<1, 3> H_q = boost::none);
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 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) {
return distance3(p, q, H1, H2);
}
};
} // namespace gtsam
Vector.h
typedef and functions to augment Eigen's VectorXd
VectorSerialization.h
serialization for Vectors
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::mean
Point3 mean(const CONTAINER &points)
mean
Definition Point3.h:68
gtsam::cross
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
cross product
Definition Point3.cpp:64
gtsam::means
Point2Pair means(const std::vector< Point2Pair > &abPointPairs)
Calculate the two means of a set of Point2 pairs.
Definition Point2.cpp:116
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:27
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:36
gtsam::norm3
double norm3(const Point3 &p, OptionalJacobian< 1, 3 > H)
Distance of the point from the origin, with Jacobian.
Definition Point3.cpp:41
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
gtsam::Range
Definition BearingRange.h:40