diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index ce4ceee895..9ff220e63b 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -77,6 +77,7 @@ double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1, Point3Pair mean(const std::vector &abPointPairs) { const size_t n = abPointPairs.size(); + if (n == 0) throw std::invalid_argument("Point3::mean input Point3Pair vector is empty"); Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0); for (const Point3Pair &abPair : abPointPairs) { aCentroid += abPair.first; diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 7f58497e90..510a5fa803 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -62,6 +62,7 @@ GTSAM_EXPORT double dot(const Point3& p, const Point3& q, /// mean template GTSAM_EXPORT 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();