From 9fd5c66a24ed58ab5551b353fc63f4d1fc3d068d Mon Sep 17 00:00:00 2001
From: alexma3312 <alexma3312@gmail.com>
Date: Sun, 23 Aug 2020 20:32:16 -0400
Subject: [PATCH] Add mean function into Point3 class.

---
 gtsam/geometry/Point3.cpp           | 13 +++++++++++++
 gtsam/geometry/Point3.h             | 10 ++++++++++
 gtsam/geometry/tests/testPoint3.cpp | 26 ++++++++++++++++++++++++++
 3 files changed, 49 insertions(+)

diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp
index 7a46f5988..ce4ceee89 100644
--- a/gtsam/geometry/Point3.cpp
+++ b/gtsam/geometry/Point3.cpp
@@ -75,6 +75,19 @@ 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<Point3Pair> &abPointPairs) {
+  const size_t n = abPointPairs.size();
+  Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0);
+  for (const Point3Pair &abPair : abPointPairs) {
+    aCentroid += abPair.first;
+    bCentroid += abPair.second;
+  }
+  const double f = 1.0 / n;
+  aCentroid *= f;
+  bCentroid *= f;
+  return make_pair(aCentroid, bCentroid);
+}
+
 /* ************************************************************************* */
 ostream &operator<<(ostream &os, const gtsam::Point3Pair &p) {
   os << p.first << " <-> " << p.second;
diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index 19e328022..7ffdd2d03 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -26,6 +26,7 @@
 #include <gtsam/base/Vector.h>
 #include <gtsam/dllexport.h>
 #include <boost/serialization/nvp.hpp>
+#include <numeric>
 
 namespace gtsam {
 
@@ -58,6 +59,15 @@ GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
                         OptionalJacobian<1, 3> H_p = boost::none,
                         OptionalJacobian<1, 3> H_q = boost::none);
 
+/// mean
+template <class CONTAINER>
+GTSAM_EXPORT Point3 mean(const CONTAINER& points) {
+  Point3 sum(0, 0, 0);
+  sum = std::accumulate(points.begin(), points.end(), sum);
+  return sum / points.size();
+}
+GTSAM_EXPORT Point3Pair mean(const std::vector<Point3Pair>& abPointPairs);
+
 template <typename A1, typename A2>
 struct Range;
 
diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp
index a7c2ac50c..68518b1ff 100644
--- a/gtsam/geometry/tests/testPoint3.cpp
+++ b/gtsam/geometry/tests/testPoint3.cpp
@@ -164,6 +164,32 @@ TEST (Point3, normalize) {
   EXPECT(assert_equal(expectedH, actualH, 1e-8));
 }
 
+//*************************************************************************
+TEST(Point3, mean) {
+  Point3 expected_a_mean(2, 2, 2), expected_b_mean(-1, 1, 0);
+  Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3);
+  Point3 b1(-1, 0, 0), b2(-2, 4, 0), b3(0, -1, 0);
+  std::vector<Point3> a_points{a1, a2, a3}, b_points{b1, b2, b3};
+  Point3 actual_a_mean = mean(a_points);
+  Point3 actual_b_mean = mean(b_points);
+  EXPECT(assert_equal(expected_a_mean, actual_a_mean));
+  EXPECT(assert_equal(expected_b_mean, actual_b_mean));
+}
+
+TEST(Point3, mean_pair) {
+  Point3 a_mean(2, 2, 2), b_mean(-1, 1, 0);
+  Point3Pair expected_mean = std::make_pair(a_mean, b_mean);
+  Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3);
+  Point3 b1(-1, 0, 0), b2(-2, 4, 0), b3(0, -1, 0);
+  Point3Pair ab1(std::make_pair(a1, b1));
+  Point3Pair ab2(std::make_pair(a2, b2));
+  Point3Pair ab3(std::make_pair(a3, b3));
+  std::vector<Point3Pair> point_pairs{ab1, ab2, ab3};
+  Point3Pair actual_mean = mean(point_pairs);
+  EXPECT(assert_equal(expected_mean.first, actual_mean.first));
+  EXPECT(assert_equal(expected_mean.second, actual_mean.second));
+}
+
 //*************************************************************************
 double norm_proxy(const Point3& point) {
   return double(point.norm());