Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

add Pose2.align() to wrapper #866

Merged
merged 14 commits into from
Sep 2, 2021
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 6 additions & 4 deletions gtsam/geometry/Point2.h
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,12 @@ namespace gtsam {
/// As of GTSAM 4, in order to make GTSAM more lean,
/// it is now possible to just typedef Point2 to Vector2
typedef Vector2 Point2;

// Convenience typedef
using Point2Pair = std::pair<Point2, Point2>;
GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);

using Point2Pairs = std::vector<Point2Pair>;

/// Distance of the point from the origin, with Jacobian
GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none);
Expand All @@ -34,10 +40,6 @@ GTSAM_EXPORT double distance2(const Point2& p1, const Point2& q,
OptionalJacobian<1, 2> H1 = boost::none,
OptionalJacobian<1, 2> H2 = boost::none);

// Convenience typedef
typedef std::pair<Point2, Point2> Point2Pair;
GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);

// For MATLAB wrapper
typedef std::vector<Point2, Eigen::aligned_allocator<Point2> > Point2Vector;

Expand Down
10 changes: 10 additions & 0 deletions gtsam/geometry/geometry.i
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,14 @@ class Point2 {
// enable pickling in python
void pickle() const;
};

class Point2Pairs {
Point2Pairs();
size_t size() const;
bool empty() const;
gtsam::Point2Pair at(size_t n) const;
void push_back(const gtsam::Point2Pair& point_pair);
};

// std::vector<gtsam::Point2>
class Point2Vector {
Expand Down Expand Up @@ -429,6 +437,8 @@ class Pose2 {
// enable pickling in python
void pickle() const;
};

boost::optional<gtsam::Pose2> align(const gtsam::Point2Pairs& pairs);

#include <gtsam/geometry/Pose3.h>
class Pose3 {
Expand Down
1 change: 1 addition & 0 deletions python/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@ set(ignore
gtsam::BetweenFactorPose2s
gtsam::BetweenFactorPose3s
gtsam::Point2Vector
gtsam::Point2Pairs
gtsam::Point3Pairs
gtsam::Pose3Pairs
gtsam::Pose3Vector
Expand Down
9 changes: 9 additions & 0 deletions python/gtsam/preamble/geometry.h
Original file line number Diff line number Diff line change
Expand Up @@ -10,9 +10,18 @@
* Without this they will be automatically converted to a Python object, and all
* mutations on Python side will not be reflected on C++.
*/
#include <pybind11/stl.h>

// Support for binding boost::optional types in C++11.
// https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
namespace pybind11 { namespace detail {
template <typename T>
struct type_caster<boost::optional<T>> : optional_caster<boost::optional<T>> {};
}}

PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2>>);
PYBIND11_MAKE_OPAQUE(gtsam::Point2Pairs);
PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs);
PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs);
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
Expand Down
1 change: 1 addition & 0 deletions python/gtsam/specializations/geometry.h
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
py::bind_vector<
std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2>>>(
m_, "Point2Vector");
py::bind_vector<std::vector<gtsam::Point2Pair>>(m_, "Point2Pairs");
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");
Expand Down
45 changes: 41 additions & 4 deletions python/gtsam/tests/test_Pose2.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,27 +6,64 @@
See LICENSE for the license information

Pose2 unit tests.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
Author: Frank Dellaert & Duy Nguyen Ta & John Lambert
"""
import unittest

import numpy as np

import gtsam
from gtsam import Pose2
from gtsam import Point2, Point2Pairs, Pose2
from gtsam.utils.test_case import GtsamTestCase


class TestPose2(GtsamTestCase):
"""Test selected Pose2 methods."""

def test_adjoint(self):
def test_adjoint(self) -> None:
"""Test adjoint method."""
xi = np.array([1, 2, 3])
expected = np.dot(Pose2.adjointMap_(xi), xi)
actual = Pose2.adjoint_(xi, xi)
np.testing.assert_array_equal(actual, expected)

def test_align(self) -> None:
"""Ensure estimation of the Pose2 element to align two 2d point clouds succeeds.

Two point clouds represent horseshoe-shapes of the same size, just rotated and translated:

| X---X
| |
| X---X
------------------
|
|
O | O
| | |
O---O
"""
pts_a = [
Point2(3, 1),
Point2(1, 1),
Point2(1, 3),
Point2(3, 3),
]
pts_b = [
Point2(1, -3),
Point2(1, -5),
Point2(-1, -5),
Point2(-1, -3),
]

# fmt: on
ab_pairs = Point2Pairs(list(zip(pts_a, pts_b)))
bTa = gtsam.align(ab_pairs)
aTb = bTa.inverse()
assert aTb is not None

for pt_a, pt_b in zip(pts_a, pts_b):
pt_a_ = aTb.transformFrom(pt_b)
assert np.allclose(pt_a, pt_a_)


if __name__ == "__main__":
unittest.main()