From a1b73b3776d5121f4765a3d44680b629b42f72f2 Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Tue, 8 Sep 2020 22:42:09 -0400 Subject: [PATCH] document and use std::tie --- gtsam/geometry/Point3.h | 2 ++ gtsam_unstable/geometry/Similarity3.cpp | 12 +++++------- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 7ffdd2d03..7f58497e9 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -66,6 +66,8 @@ GTSAM_EXPORT Point3 mean(const CONTAINER& points) { sum = std::accumulate(points.begin(), points.end(), sum); return sum / points.size(); } + +/// mean of Point3 pair GTSAM_EXPORT Point3Pair mean(const std::vector& abPointPairs); template diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index a1220cc80..1d127f8cf 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -99,9 +99,8 @@ Point3 Similarity3::operator*(const Point3& p) const { // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 Similarity3 Similarity3::AlignGivenR(const std::vector& abPointPairs, const Rot3& aRb) { // calculate centroids - const Point3Pair centroids = mean(abPointPairs); - const Point3 aCentroid = centroids.first; - const Point3 bCentroid = centroids.second; + Point3 aCentroid, bCentroid; + std::tie(aCentroid, bCentroid) = mean(abPointPairs); // calculate scale double x = 0; @@ -128,9 +127,8 @@ Similarity3 Similarity3::Align(const std::vector& abPointPairs) { if (n < 3) throw std::runtime_error("input should have at least 3 pairs of points"); // we need at least three pairs // calculate centroids - const Point3Pair centroids = mean(abPointPairs); - const Point3 aCentroid = centroids.first; - const Point3 bCentroid = centroids.second; + Point3 aCentroid, bCentroid; + std::tie(aCentroid, bCentroid) = mean(abPointPairs); // Add to form H matrix Matrix3 H = Z_3x3; @@ -152,7 +150,7 @@ Similarity3 Similarity3::Align(const std::vector& abPosePairs) { const size_t n = abPosePairs.size(); if (n < 2) throw std::runtime_error("input should have at least 2 pairs of poses"); // we need at least two pairs - // calculate rotation and centroids + // calculate rotation vector rotationList; vector abPointPairs; abPointPairs.reserve(n);