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

PointPairs to Point3Pairs, and move to Point3.h

release/4.3a0
John Lambert 2021-03-03 00:16:08 -05:00
parent 7caaf69ae5
commit 89dfdf082f
2 changed files with 10 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/Point3.h
View File

@ -38,6 +38,8 @@ typedef Vector3 Point3;
typedef std::pair<Point3, Point3> Point3Pair; typedef std::pair<Point3, Point3> Point3Pair;
GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
using Point3Pairs = std::vector<Point3Pair>;
/// distance between two points /// distance between two points
GTSAM_EXPORT double distance3(const Point3& p1, const Point3& q, GTSAM_EXPORT double distance3(const Point3& p1, const Point3& q,
OptionalJacobian<1, 3> H1 = boost::none, OptionalJacobian<1, 3> H1 = boost::none,

16
gtsam/geometry/Similarity3.cpp
View File

@ -27,9 +27,9 @@ using std::vector;
namespace { namespace {
/// Subtract centroids from point pairs. /// Subtract centroids from point pairs.
static PointPairs subtractCentroids(const PointPairs &abPointPairs, static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs,
const Point3Pair &centroids) { const Point3Pair &centroids) {
PointPairs d_abPointPairs; Point3Pairs d_abPointPairs;
for (const Point3Pair& abPair : abPointPairs) { for (const Point3Pair& abPair : abPointPairs) {
Point3 da = abPair.first - centroids.first; Point3 da = abPair.first - centroids.first;
Point3 db = abPair.second - centroids.second; Point3 db = abPair.second - centroids.second;
@ -39,7 +39,7 @@ static PointPairs subtractCentroids(const PointPairs &abPointPairs,
} }
/// Form inner products x and y and calculate scale. /// Form inner products x and y and calculate scale.
static const double calculateScale(const PointPairs &d_abPointPairs, static const double calculateScale(const Point3Pairs &d_abPointPairs,
const Rot3 &aRb) { const Rot3 &aRb) {
double x = 0, y = 0; double x = 0, y = 0;
Point3 da, db; Point3 da, db;
@ -54,7 +54,7 @@ static const double calculateScale(const PointPairs &d_abPointPairs,
} }
/// Form outer product H. /// Form outer product H.
static Matrix3 calculateH(const PointPairs &d_abPointPairs) { static Matrix3 calculateH(const Point3Pairs &d_abPointPairs) {
Matrix3 H = Z_3x3; Matrix3 H = Z_3x3;
for (const Point3Pair& d_abPair : d_abPointPairs) { for (const Point3Pair& d_abPair : d_abPointPairs) {
H += d_abPair.first * d_abPair.second.transpose(); H += d_abPair.first * d_abPair.second.transpose();
@ -63,7 +63,7 @@ static Matrix3 calculateH(const PointPairs &d_abPointPairs) {
} }
/// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids. /// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids.
static Similarity3 align(const PointPairs &d_abPointPairs, const Rot3 &aRb, static Similarity3 align(const Point3Pairs &d_abPointPairs, const Rot3 &aRb,
const Point3Pair &centroids) { const Point3Pair &centroids) {
const double s = calculateScale(d_abPointPairs, aRb); const double s = calculateScale(d_abPointPairs, aRb);
const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s; const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s;
@ -72,7 +72,7 @@ static Similarity3 align(const PointPairs &d_abPointPairs, const Rot3 &aRb,
/// This method estimates the similarity transform from point pairs, given a known or estimated rotation. /// This method estimates the similarity transform from point pairs, given a known or estimated rotation.
// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3
static Similarity3 alignGivenR(const PointPairs &abPointPairs, static Similarity3 alignGivenR(const Point3Pairs &abPointPairs,
const Rot3 &aRb) { const Rot3 &aRb) {
auto centroids = means(abPointPairs); auto centroids = means(abPointPairs);
auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
@ -153,7 +153,7 @@ Point3 Similarity3::operator*(const Point3& p) const {
return transformFrom(p); return transformFrom(p);
} }
Similarity3 Similarity3::Align(const PointPairs &abPointPairs) { Similarity3 Similarity3::Align(const Point3Pairs &abPointPairs) {
// Refer to Chapter 3 of // Refer to Chapter 3 of
// http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf // http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
if (abPointPairs.size() < 3) if (abPointPairs.size() < 3)
@ -173,7 +173,7 @@ Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
// calculate rotation // calculate rotation
vector<Rot3> rotations; vector<Rot3> rotations;
PointPairs abPointPairs; Point3Pairs abPointPairs;
rotations.reserve(n); rotations.reserve(n);
abPointPairs.reserve(n); abPointPairs.reserve(n);
Pose3 wTa, wTb; Pose3 wTa, wTb;