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

Merge pull request #702 from borglab/sim3-alignment 

add Sim(3)-based alignment to the wrapper
release/4.3a0
John Lambert 2021-03-11 18:52:59 -05:00 committed by GitHub
parent b83f06a8af 35ea79b2cb
commit 0eb20b6291
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
10 changed files with 247 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/Point3.h
View File

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

1
gtsam/geometry/Pose3.h
View File

@ -389,6 +389,7 @@ inline Matrix wedge<Pose3>(const Vector& xi) {
// Convenience typedef
using Pose3Pair = std::pair<Pose3, Pose3>;
using Pose3Pairs = std::vector<std::pair<Pose3, Pose3> >;
// For MATLAB wrapper
typedef std::vector<Pose3> Pose3Vector;

39
gtsam_unstable/geometry/Similarity3.cpp → gtsam/geometry/Similarity3.cpp
View File

@ -13,9 +13,10 @@
* @file Similarity3.cpp
* @brief Implementation of Similarity3 transform
* @author Paul Drews
* @author John Lambert
*/
#include <gtsam_unstable/geometry/Similarity3.h>
#include <gtsam/geometry/Similarity3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Manifold.h>
@ -24,13 +25,12 @@
namespace gtsam {
using std::vector;
using PointPairs = vector<Point3Pair>;
namespace {
/// Subtract centroids from point pairs.
static PointPairs subtractCentroids(const PointPairs &abPointPairs,
static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs,
const Point3Pair &centroids) {
PointPairs d_abPointPairs;
Point3Pairs d_abPointPairs;
for (const Point3Pair& abPair : abPointPairs) {
Point3 da = abPair.first - centroids.first;
Point3 db = abPair.second - centroids.second;
@ -40,7 +40,7 @@ static PointPairs subtractCentroids(const PointPairs &abPointPairs,
}
/// 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) {
double x = 0, y = 0;
Point3 da, db;
@ -55,7 +55,7 @@ static const double calculateScale(const PointPairs &d_abPointPairs,
}
/// Form outer product H.
static Matrix3 calculateH(const PointPairs &d_abPointPairs) {
static Matrix3 calculateH(const Point3Pairs &d_abPointPairs) {
Matrix3 H = Z_3x3;
for (const Point3Pair& d_abPair : d_abPointPairs) {
H += d_abPair.first * d_abPair.second.transpose();
@ -63,17 +63,20 @@ static Matrix3 calculateH(const PointPairs &d_abPointPairs) {
return H;
}
/// 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,
/// This method estimates the similarity transform from differences point pairs,
// given a known or estimated rotation and point centroids.
static Similarity3 align(const Point3Pairs &d_abPointPairs, const Rot3 &aRb,
const Point3Pair &centroids) {
const double s = calculateScale(d_abPointPairs, aRb);
// dividing aTb by s is required because the registration cost function
// minimizes ||a - sRb - t||, whereas Sim(3) computes s(Rb + t)
const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s;
return Similarity3(aRb, aTb, s);
}
/// 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
static Similarity3 alignGivenR(const PointPairs &abPointPairs,
static Similarity3 alignGivenR(const Point3Pairs &abPointPairs,
const Rot3 &aRb) {
auto centroids = means(abPointPairs);
auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
@ -154,7 +157,7 @@ Point3 Similarity3::operator*(const Point3& p) const {
return transformFrom(p);
}
Similarity3 Similarity3::Align(const PointPairs &abPointPairs) {
Similarity3 Similarity3::Align(const Point3Pairs &abPointPairs) {
// Refer to Chapter 3 of
// http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
if (abPointPairs.size() < 3)
@ -174,18 +177,20 @@ Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
// calculate rotation
vector<Rot3> rotations;
PointPairs abPointPairs;
Point3Pairs abPointPairs;
rotations.reserve(n);
abPointPairs.reserve(n);
Pose3 wTa, wTb;
// Below denotes the pose of the i'th object/camera/etc in frame "a" or frame "b"
Pose3 aTi, bTi;
for (const Pose3Pair &abPair : abPosePairs) {
std::tie(wTa, wTb) = abPair;
rotations.emplace_back(wTa.rotation().compose(wTb.rotation().inverse()));
abPointPairs.emplace_back(wTa.translation(), wTb.translation());
std::tie(aTi, bTi) = abPair;
const Rot3 aRb = aTi.rotation().compose(bTi.rotation().inverse());
rotations.emplace_back(aRb);
abPointPairs.emplace_back(aTi.translation(), bTi.translation());
}
const Rot3 aRb = FindKarcherMean<Rot3>(rotations);
const Rot3 aRb_estimate = FindKarcherMean<Rot3>(rotations);
return alignGivenR(abPointPairs, aRb);
return alignGivenR(abPointPairs, aRb_estimate);
}
Matrix4 Similarity3::wedge(const Vector7 &xi) {

10
gtsam_unstable/geometry/Similarity3.h → gtsam/geometry/Similarity3.h
View File

@ -13,6 +13,7 @@
* @file Similarity3.h
* @brief Implementation of Similarity3 transform
* @author Paul Drews
* @author John Lambert
*/
#pragma once
@ -125,7 +126,14 @@ public:
GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
/**
* Create Similarity3 by aligning at least two pose pairs
* Create the Similarity3 object that aligns at least two pose pairs.
* Each pair is of the form (aTi, bTi).
* Given a list of pairs in frame a, and a list of pairs in frame b, Align()
* will compute the best-fit Similarity3 aSb transformation to align them.
* First, the rotation aRb will be computed as the average (Karcher mean) of
* many estimates aRb (from each pair). Afterwards, the scale factor will be computed
* using the algorithm described here:
* http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
*/
GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);

54
gtsam_unstable/geometry/tests/testSimilarity3.cpp → gtsam/geometry/tests/testSimilarity3.cpp
View File

@ -16,7 +16,7 @@
* @author Zhaoyang Lv
*/
#include <gtsam_unstable/geometry/Similarity3.h>
#include <gtsam/geometry/Similarity3.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
@ -259,19 +259,27 @@ TEST(Similarity3, GroupAction) {
//******************************************************************************
// Group action on Pose3
// Estimate Sim(3) object "aSb" from pose pairs {(aTi, bTi)}
// In the example below, let the "a" frame be the "world" frame below,
// and let the "b" frame be the "egovehicle" frame.
// Suppose within the egovehicle frame, we know the poses of two objects "o1" and "o2"
TEST(Similarity3, GroupActionPose3) {
Similarity3 bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
// Suppose we know the pose of the egovehicle in the world frame
Similarity3 wSe(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
// Create source poses
Pose3 Ta1(Rot3(), Point3(0, 0, 0));
Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
// Create source poses (two objects o1 and o2 living in the egovehicle "e" frame)
// Suppose they are 3d cuboids detected by onboard sensor, in the egovehicle frame
Pose3 eTo1(Rot3(), Point3(0, 0, 0));
Pose3 eTo2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
// Create destination poses
Pose3 expected_Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
Pose3 expected_Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
// Create destination poses (two objects now living in the world/city "w" frame)
// Desired to place the objects into the world frame for tracking
Pose3 expected_wTo1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
Pose3 expected_wTo2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
EXPECT(assert_equal(expected_Tb1, bSa.transformFrom(Ta1)));
EXPECT(assert_equal(expected_Tb2, bSa.transformFrom(Ta2)));
// objects now live in the world frame, instead of in the egovehicle frame
EXPECT(assert_equal(expected_wTo1, wSe.transformFrom(eTo1)));
EXPECT(assert_equal(expected_wTo2, wSe.transformFrom(eTo2)));
}
// Test left group action compatibility.
@ -346,26 +354,28 @@ TEST(Similarity3, AlignPoint3_3) {
//******************************************************************************
// Align with Pose3 Pairs
TEST(Similarity3, AlignPose3) {
Similarity3 expected_aSb(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
Similarity3 expected_wSe(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
// Create source poses
Pose3 Ta1(Rot3(), Point3(0, 0, 0));
Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
// Create source poses (two objects o1 and o2 living in the egovehicle "e" frame)
// Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
Pose3 eTo1(Rot3(), Point3(0, 0, 0));
Pose3 eTo2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
// Create destination poses
Pose3 Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
Pose3 Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
// Create destination poses (same two objects, but instead living in the world/city "w" frame)
Pose3 wTo1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
Pose3 wTo2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
Pose3Pair bTa1(make_pair(Tb1, Ta1));
Pose3Pair bTa2(make_pair(Tb2, Ta2));
Pose3Pair wTe1(make_pair(wTo1, eTo1));
Pose3Pair wTe2(make_pair(wTo2, eTo2));
vector<Pose3Pair> correspondences{bTa1, bTa2};
vector<Pose3Pair> correspondences{wTe1, wTe2};
// Cayley transform cannot accommodate 180 degree rotations,
// hence we only test for Expmap
#ifdef GTSAM_ROT3_EXPMAP
Similarity3 actual_aSb = Similarity3::Align(correspondences);
EXPECT(assert_equal(expected_aSb, actual_aSb));
// Recover the transformation wSe (i.e. world_S_egovehicle)
Similarity3 actual_wSe = Similarity3::Align(correspondences);
EXPECT(assert_equal(expected_wSe, actual_wSe));
#endif
}

41
gtsam/gtsam.i
View File

@ -469,6 +469,15 @@ class Point3 {
void pickle() const;
};
#include <gtsam/geometry/Point3.h>
class Point3Pairs {
Point3Pairs();
size_t size() const;
bool empty() const;
gtsam::Point3Pair at(size_t n) const;
void push_back(const gtsam::Point3Pair& point_pair);
};
#include <gtsam/geometry/Rot2.h>
class Rot2 {
// Standard Constructors and Named Constructors
@ -799,6 +808,15 @@ class Pose3 {
void pickle() const;
};
#include <gtsam/geometry/Pose3.h>
class Pose3Pairs {
Pose3Pairs();
size_t size() const;
bool empty() const;
gtsam::Pose3Pair at(size_t n) const;
void push_back(const gtsam::Pose3Pair& pose_pair);
};
// std::vector<gtsam::Pose3>
#include <gtsam/geometry/Pose3.h>
class Pose3Vector
@ -1121,6 +1139,29 @@ class PinholeCamera {
void pickle() const;
};
#include <gtsam/geometry/Similarity3.h>
class Similarity3 {
// Standard Constructors
Similarity3();
Similarity3(double s);
Similarity3(const gtsam::Rot3& R, const gtsam::Point3& t, double s);
Similarity3(const Matrix& R, const Vector& t, double s);
Similarity3(const Matrix& T);
gtsam::Pose3 transformFrom(const gtsam::Pose3& T);
static Similarity3 Align(const gtsam::Point3Pairs & abPointPairs);
static Similarity3 Align(const gtsam::Pose3Pairs & abPosePairs);
// Standard Interface
const Matrix matrix() const;
const gtsam::Rot3& rotation();
const gtsam::Point3& translation();
double scale() const;
};
// Forward declaration of PinholeCameraCalX is defined here.
#include <gtsam/geometry/SimpleCamera.h>
// Some typedefs for common camera types

1
gtsam_unstable/gtsam_unstable.i
View File

@ -163,6 +163,7 @@ class BearingS2 {
void serializable() const; // enabling serialization functionality
};
#include <gtsam_unstable/geometry/SimWall2D.h>
class SimWall2D {
SimWall2D();

2
python/CMakeLists.txt
View File

@ -41,6 +41,8 @@ set(ignore
gtsam::BetweenFactorPose2s
gtsam::BetweenFactorPose3s
gtsam::Point2Vector
gtsam::Point3Pairs
gtsam::Pose3Pairs
gtsam::Pose3Vector
gtsam::KeyVector
gtsam::BinaryMeasurementsUnit3

2
python/gtsam/specializations.h
View File

@ -6,6 +6,8 @@ py::bind_vector<std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key> > >(m_, "
py::bind_vector<std::vector<gtsam::Key> >(m_, "KeyVector");
#endif
py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> > >(m_, "Point2Vector");
py::bind_vector<std::vector<gtsam::Point3Pair> >(m_, "Point3Pairs");
py::bind_vector<std::vector<gtsam::Pose3Pair> >(m_, "Pose3Pairs");
py::bind_vector<std::vector<gtsam::Pose3> >(m_, "Pose3Vector");
py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(m_, "BetweenFactorPose3s");
py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > > >(m_, "BetweenFactorPose2s");

134
python/gtsam/tests/test_Sim3.py Normal file
View File

@ -0,0 +1,134 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Sim3 unit tests.
Author: John Lambert
"""
# pylint: disable=no-name-in-module
import unittest
import numpy as np
import gtsam
from gtsam import Point3, Pose3, Pose3Pairs, Rot3, Similarity3
from gtsam.utils.test_case import GtsamTestCase
class TestSim3(GtsamTestCase):
"""Test selected Sim3 methods."""
def test_align_poses_along_straight_line(self):
"""Test Align Pose3Pairs method.
Scenario:
3 object poses
same scale (no gauge ambiguity)
world frame has poses rotated about x-axis (90 degree roll)
world and egovehicle frame translated by 15 meters w.r.t. each other
"""
Rx90 = Rot3.Rx(np.deg2rad(90))
# Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
# Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
eTo0 = Pose3(Rot3(), np.array([5, 0, 0]))
eTo1 = Pose3(Rot3(), np.array([10, 0, 0]))
eTo2 = Pose3(Rot3(), np.array([15, 0, 0]))
eToi_list = [eTo0, eTo1, eTo2]
# Create destination poses
# (same three objects, but instead living in the world/city "w" frame)
wTo0 = Pose3(Rx90, np.array([-10, 0, 0]))
wTo1 = Pose3(Rx90, np.array([-5, 0, 0]))
wTo2 = Pose3(Rx90, np.array([0, 0, 0]))
wToi_list = [wTo0, wTo1, wTo2]
we_pairs = Pose3Pairs(list(zip(wToi_list, eToi_list)))
# Recover the transformation wSe (i.e. world_S_egovehicle)
wSe = Similarity3.Align(we_pairs)
for wToi, eToi in zip(wToi_list, eToi_list):
self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
def test_align_poses_along_straight_line_gauge(self):
"""Test if Align Pose3Pairs method can account for gauge ambiguity.
Scenario:
3 object poses
with gauge ambiguity (2x scale)
world frame has poses rotated about z-axis (90 degree yaw)
world and egovehicle frame translated by 11 meters w.r.t. each other
"""
Rz90 = Rot3.Rz(np.deg2rad(90))
# Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
# Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
eTo0 = Pose3(Rot3(), np.array([1, 0, 0]))
eTo1 = Pose3(Rot3(), np.array([2, 0, 0]))
eTo2 = Pose3(Rot3(), np.array([4, 0, 0]))
eToi_list = [eTo0, eTo1, eTo2]
# Create destination poses
# (same three objects, but instead living in the world/city "w" frame)
wTo0 = Pose3(Rz90, np.array([0, 12, 0]))
wTo1 = Pose3(Rz90, np.array([0, 14, 0]))
wTo2 = Pose3(Rz90, np.array([0, 18, 0]))
wToi_list = [wTo0, wTo1, wTo2]
we_pairs = Pose3Pairs(list(zip(wToi_list, eToi_list)))
# Recover the transformation wSe (i.e. world_S_egovehicle)
wSe = Similarity3.Align(we_pairs)
for wToi, eToi in zip(wToi_list, eToi_list):
self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
def test_align_poses_scaled_squares(self):
"""Test if Align Pose3Pairs method can account for gauge ambiguity.
Make sure a big and small square can be aligned.
The u's represent a big square (10x10), and v's represents a small square (4x4).
Scenario:
4 object poses
with gauge ambiguity (2.5x scale)
"""
# 0, 90, 180, and 270 degrees yaw
R0 = Rot3.Rz(np.deg2rad(0))
R90 = Rot3.Rz(np.deg2rad(90))
R180 = Rot3.Rz(np.deg2rad(180))
R270 = Rot3.Rz(np.deg2rad(270))
aTi0 = Pose3(R0, np.array([2, 3, 0]))
aTi1 = Pose3(R90, np.array([12, 3, 0]))
aTi2 = Pose3(R180, np.array([12, 13, 0]))
aTi3 = Pose3(R270, np.array([2, 13, 0]))
aTi_list = [aTi0, aTi1, aTi2, aTi3]
bTi0 = Pose3(R0, np.array([4, 3, 0]))
bTi1 = Pose3(R90, np.array([8, 3, 0]))
bTi2 = Pose3(R180, np.array([8, 7, 0]))
bTi3 = Pose3(R270, np.array([4, 7, 0]))
bTi_list = [bTi0, bTi1, bTi2, bTi3]
ab_pairs = Pose3Pairs(list(zip(aTi_list, bTi_list)))
# Recover the transformation wSe (i.e. world_S_egovehicle)
aSb = Similarity3.Align(ab_pairs)
for aTi, bTi in zip(aTi_list, bTi_list):
self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi))
if __name__ == "__main__":
unittest.main()