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

Fix wrapping

release/4.3a0
Frank Dellaert 2022-01-28 11:52:18 -05:00
parent 01f3fe50e4
commit 55d839989d
5 changed files with 41 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/geometry/Rot3.h
View File

@ -49,15 +49,13 @@
namespace gtsam {
/**
* @brief A 3D rotation represented as a rotation matrix if the preprocessor
* symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion if it
* is defined.
/**
* @brief Rot3 is a 3D rotation represented as a rotation matrix if the
* preprocessor symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion
* if it is defined.
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Rot3 : public LieGroup<Rot3,3> {
class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
private:
#ifdef GTSAM_USE_QUATERNIONS
@ -68,7 +66,6 @@ namespace gtsam {
#endif
public:
/// @name Constructors and named constructors
/// @{
@ -83,7 +80,7 @@ namespace gtsam {
*/
Rot3(const Point3& col1, const Point3& col2, const Point3& col3);
/** constructor from a rotation matrix, as doubles in *row-major* order !!! */
/// Construct from a rotation matrix, as doubles in *row-major* order !!!
Rot3(double R11, double R12, double R13,
double R21, double R22, double R23,
double R31, double R32, double R33);
@ -567,6 +564,9 @@ namespace gtsam {
#endif
};
/// std::vector of Rot3s, mainly for wrapper
using Rot3Vector = std::vector<Rot3, Eigen::aligned_allocator<Rot3> >;
/**
* [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R
* and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx'
@ -585,5 +585,6 @@ namespace gtsam {
template<>
struct traits<const Rot3> : public internal::LieGroup<Rot3> {};
}
} // namespace gtsam

14
gtsam/slam/slam.i
View File

@ -316,24 +316,14 @@ class InitializePose3 {
static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph);
};
#include <gtsam/slam/KarcherMeanFactor.h>
#include <gtsam/slam/KarcherMeanFactor-inl.h>
template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose3}>
virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
KarcherMeanFactor(const gtsam::KeyVector& keys);
};
class Rot3Vector {
Rot3Vector();
size_t size() const;
// structure specific methods
gtsam::Rot3 at(size_t i) const;
void push_back(const gtsam::Rot3& R);
};
gtsam::Rot3 FindKarcherMean<gtsam::Rot3>(const Rot3Vector &rotations);
typedef FindKarcherMean<gtsam::Rot3> FindKarcherMeanRot3;
gtsam::Rot3 FindKarcherMeanRot3(const gtsam::Rot3Vector& rotations);
gtsam::Rot3 FindKarcherMean(const gtsam::Rot3Vector& rotations);
#include <gtsam/slam/FrobeniusFactor.h>
gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model,

2
python/gtsam/preamble/slam.h
View File

@ -15,4 +15,4 @@ PYBIND11_MAKE_OPAQUE(
std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > >);
PYBIND11_MAKE_OPAQUE(
std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > >);
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Rot3, Eigen::aligned_allocator<gtsam::Rot3> >);
PYBIND11_MAKE_OPAQUE(gtsam::Rot3Vector);

6
python/gtsam/specializations/slam.h
View File

@ -12,9 +12,9 @@
*/
py::bind_vector<
std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(
std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3>>>>(
m_, "BetweenFactorPose3s");
py::bind_vector<
std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > > >(
std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2>>>>(
m_, "BetweenFactorPose2s");
py::bind_vector<std::vector<gtsam::Rot3, Eigen::aligned_allocator<gtsam::Rot3>>(m_, "Rot3Vector");
py::bind_vector<gtsam::Rot3Vector>(m_, "Rot3Vector");

40
python/gtsam/tests/test_KarcherMeanFactor.py
View File

@ -15,27 +15,15 @@ import unittest
import gtsam
import numpy as np
from gtsam import Rot3
from gtsam.utils.test_case import GtsamTestCase
KEY = 0
MODEL = gtsam.noiseModel.Unit.Create(3)
def find_Karcher_mean_Rot3(rotations):
"""Find the Karcher mean of given values."""
# Cost function C(R) = \sum PriorFactor(R_i)::error(R)
# No closed form solution.
graph = gtsam.NonlinearFactorGraph()
for R in rotations:
graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL))
initial = gtsam.Values()
initial.insert(KEY, gtsam.Rot3())
result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
return result.atRot3(KEY)
# Rot3 version
R = gtsam.Rot3.Expmap(np.array([0.1, 0, 0]))
R = Rot3.Expmap(np.array([0.1, 0, 0]))
class TestKarcherMean(GtsamTestCase):
@ -43,11 +31,23 @@ class TestKarcherMean(GtsamTestCase):
def test_find(self):
# Check that optimizing for Karcher mean (which minimizes Between distance)
# gets correct result.
rotations = {R, R.inverse()}
expected = gtsam.Rot3()
actual = find_Karcher_mean_Rot3(rotations)
rotations = gtsam.Rot3Vector([R, R.inverse()])
expected = Rot3()
actual = gtsam.FindKarcherMean(rotations)
self.gtsamAssertEquals(expected, actual)
def test_find_karcher_mean_identity(self):
"""Averaging 3 identity rotations should yield the identity."""
a1Rb1 = Rot3()
a2Rb2 = Rot3()
a3Rb3 = Rot3()
aRb_list = gtsam.Rot3Vector([a1Rb1, a2Rb2, a3Rb3])
aRb_expected = Rot3()
aRb = gtsam.FindKarcherMean(aRb_list)
self.gtsamAssertEquals(aRb, aRb_expected)
def test_factor(self):
"""Check that the InnerConstraint factor leaves the mean unchanged."""
# Make a graph with two variables, one between, and one InnerConstraint
@ -66,11 +66,11 @@ class TestKarcherMean(GtsamTestCase):
initial = gtsam.Values()
initial.insert(1, R.inverse())
initial.insert(2, R)
expected = find_Karcher_mean_Rot3([R, R.inverse()])
expected = Rot3()
result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
actual = find_Karcher_mean_Rot3(
[result.atRot3(1), result.atRot3(2)])
actual = gtsam.FindKarcherMean(
gtsam.Rot3Vector([result.atRot3(1), result.atRot3(2)]))
self.gtsamAssertEquals(expected, actual)
self.gtsamAssertEquals(
R12, result.atRot3(1).between(result.atRot3(2)))