diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index ce4ceee89..8665f7716 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -75,17 +75,15 @@ double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1, return p.x() * q.x() + p.y() * q.y() + p.z() * q.z(); } -Point3Pair mean(const std::vector &abPointPairs) { +Point3Pair means(const std::vector &abPointPairs) { const size_t n = abPointPairs.size(); - Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0); + Point3 aSum(0, 0, 0), bSum(0, 0, 0); for (const Point3Pair &abPair : abPointPairs) { - aCentroid += abPair.first; - bCentroid += abPair.second; + aSum += abPair.first; + bSum += abPair.second; } const double f = 1.0 / n; - aCentroid *= f; - bCentroid *= f; - return make_pair(aCentroid, bCentroid); + return {aSum * f, bSum * f}; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 7f58497e9..33b5836f8 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -67,8 +67,8 @@ GTSAM_EXPORT Point3 mean(const CONTAINER& points) { return sum / points.size(); } -/// mean of Point3 pair -GTSAM_EXPORT Point3Pair mean(const std::vector& abPointPairs); +/// mean of Point3 pair +GTSAM_EXPORT Point3Pair means(const std::vector &abPointPairs); template struct Range;