12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add missing `means()` function for Point2

release/4.3a0
John Lambert 2021-11-06 21:40:42 -04:00 committed by GitHub
parent 4372ed82f2
commit 1dd20a39fc
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 12 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/geometry/Point2.cpp
View File

@ -113,6 +113,18 @@ list<Point2> circleCircleIntersection(Point2 c1, double r1, Point2 c2,
return circleCircleIntersection(c1, c2, fh); return circleCircleIntersection(c1, c2, fh);
} }
Point2Pair means(const std::vector<Point2Pair> &abPointPairs) {
const size_t n = abPointPairs.size();
if (n == 0) throw std::invalid_argument("Point2::mean input Point2Pair vector is empty");
Point2 aSum(0, 0, 0), bSum(0, 0, 0);
for (const Point2Pair &abPair : abPointPairs) {
aSum += abPair.first;
bSum += abPair.second;
}
const double f = 1.0 / n;
return {aSum * f, bSum * f};
}
/* ************************************************************************* */ /* ************************************************************************* */
ostream &operator<<(ostream &os, const gtsam::Point2Pair &p) { ostream &operator<<(ostream &os, const gtsam::Point2Pair &p) {
os << p.first << " <-> " << p.second; os << p.first << " <-> " << p.second;