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

Capitalize Identity trait since it is static

release/4.3a0
Varun Agrawal 2022-07-07 12:31:27 -04:00
parent b91c43e691
commit c31298d367
44 changed files with 135 additions and 135 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/base/Group.h
View File

@ -95,7 +95,7 @@ template<class Class>
struct MultiplicativeGroupTraits { struct MultiplicativeGroupTraits {
typedef group_tag structure_category; typedef group_tag structure_category;
typedef multiplicative_group_tag group_flavor; typedef multiplicative_group_tag group_flavor;
static Class Identity() { return Class::identity(); } static Class Identity() { return Class::Identity(); }
static Class Compose(const Class &g, const Class & h) { return g * h;} static Class Compose(const Class &g, const Class & h) { return g * h;}
static Class Between(const Class &g, const Class & h) { return g.inverse() * h;} static Class Between(const Class &g, const Class & h) { return g.inverse() * h;}
static Class Inverse(const Class &g) { return g.inverse();} static Class Inverse(const Class &g) { return g.inverse();}
@ -111,7 +111,7 @@ template<class Class>
struct AdditiveGroupTraits { struct AdditiveGroupTraits {
typedef group_tag structure_category; typedef group_tag structure_category;
typedef additive_group_tag group_flavor; typedef additive_group_tag group_flavor;
static Class Identity() { return Class::identity(); } static Class Identity() { return Class::Identity(); }
static Class Compose(const Class &g, const Class & h) { return g + h;} static Class Compose(const Class &g, const Class & h) { return g + h;}
static Class Between(const Class &g, const Class & h) { return h - g;} static Class Between(const Class &g, const Class & h) { return h - g;}
static Class Inverse(const Class &g) { return -g;} static Class Inverse(const Class &g) { return -g;}
@ -147,7 +147,7 @@ public:
DirectProduct(const G& g, const H& h):std::pair<G,H>(g,h) {} DirectProduct(const G& g, const H& h):std::pair<G,H>(g,h) {}
// identity // identity
static DirectProduct identity() { return DirectProduct(); } static DirectProduct Identity() { return DirectProduct(); }
DirectProduct operator*(const DirectProduct& other) const { DirectProduct operator*(const DirectProduct& other) const {
return DirectProduct(traits<G>::Compose(this->first, other.first), return DirectProduct(traits<G>::Compose(this->first, other.first),
@ -181,7 +181,7 @@ public:
DirectSum(const G& g, const H& h):std::pair<G,H>(g,h) {} DirectSum(const G& g, const H& h):std::pair<G,H>(g,h) {}
// identity // identity
static DirectSum identity() { return DirectSum(); } static DirectSum Identity() { return DirectSum(); }
DirectSum operator+(const DirectSum& other) const { DirectSum operator+(const DirectSum& other) const {
return DirectSum(g()+other.g(), h()+other.h()); return DirectSum(g()+other.g(), h()+other.h());

2
gtsam/base/Lie.h
View File

@ -177,7 +177,7 @@ struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> {
/// @name Group /// @name Group
/// @{ /// @{
typedef multiplicative_group_tag group_flavor; typedef multiplicative_group_tag group_flavor;
static Class Identity() { return Class::identity();} static Class Identity() { return Class::Identity();}
/// @} /// @}
/// @name Manifold /// @name Manifold

2
gtsam/base/ProductLieGroup.h
View File

@ -48,7 +48,7 @@ public:
/// @name Group /// @name Group
/// @{ /// @{
typedef multiplicative_group_tag group_flavor; typedef multiplicative_group_tag group_flavor;
static ProductLieGroup identity() {return ProductLieGroup();} static ProductLieGroup Identity() {return ProductLieGroup();}
ProductLieGroup operator*(const ProductLieGroup& other) const { ProductLieGroup operator*(const ProductLieGroup& other) const {
return ProductLieGroup(traits<G>::Compose(this->first,other.first), return ProductLieGroup(traits<G>::Compose(this->first,other.first),

4
gtsam/base/VectorSpace.h
View File

@ -169,7 +169,7 @@ struct HasVectorSpacePrereqs {
Vector v; Vector v;
BOOST_CONCEPT_USAGE(HasVectorSpacePrereqs) { BOOST_CONCEPT_USAGE(HasVectorSpacePrereqs) {
p = Class::identity(); // identity p = Class::Identity(); // identity
q = p + p; // addition q = p + p; // addition
q = p - p; // subtraction q = p - p; // subtraction
v = p.vector(); // conversion to vector v = p.vector(); // conversion to vector
@ -192,7 +192,7 @@ struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
/// @name Group /// @name Group
/// @{ /// @{
typedef additive_group_tag group_flavor; typedef additive_group_tag group_flavor;
static Class Identity() { return Class::identity();} static Class Identity() { return Class::Identity();}
/// @} /// @}
/// @name Manifold /// @name Manifold

2
gtsam/base/tests/testGroup.cpp
View File

@ -29,7 +29,7 @@ class Symmetric: private Eigen::PermutationMatrix<N> {
Eigen::PermutationMatrix<N>(P) { Eigen::PermutationMatrix<N>(P) {
} }
public: public:
static Symmetric identity() { return Symmetric(); } static Symmetric Identity() { return Symmetric(); }
Symmetric() { Symmetric() {
Eigen::PermutationMatrix<N>::setIdentity(); Eigen::PermutationMatrix<N>::setIdentity();
} }

4
gtsam/basis/ParameterMatrix.h
View File

@ -189,9 +189,9 @@ class ParameterMatrix {
* NOTE: The size at compile time is unknown so this identity is zero * NOTE: The size at compile time is unknown so this identity is zero
* length and thus not valid. * length and thus not valid.
*/ */
inline static ParameterMatrix identity() { inline static ParameterMatrix Identity() {
// throw std::runtime_error( // throw std::runtime_error(
// "ParameterMatrix::identity(): Don't use this function"); // "ParameterMatrix::Identity(): Don't use this function");
return ParameterMatrix(0); return ParameterMatrix(0);
} }

2
gtsam/basis/tests/testParameterMatrix.cpp
View File

@ -133,7 +133,7 @@ TEST(ParameterMatrix, VectorSpace) {
// vector // vector
EXPECT(assert_equal(Vector::Ones(M * N), params.vector())); EXPECT(assert_equal(Vector::Ones(M * N), params.vector()));
// identity // identity
EXPECT(assert_equal(ParameterMatrix<M>::identity(), EXPECT(assert_equal(ParameterMatrix<M>::Identity(),
ParameterMatrix<M>(Matrix::Zero(M, 0)))); ParameterMatrix<M>(Matrix::Zero(M, 0))));
} }

2
gtsam/geometry/Cyclic.h
View File

@ -38,7 +38,7 @@ public:
/// Default constructor yields identity /// Default constructor yields identity
Cyclic():i_(0) { Cyclic():i_(0) {
} }
static Cyclic identity() { return Cyclic();} static Cyclic Identity() { return Cyclic();}
/// Cast to size_t /// Cast to size_t
operator size_t() const { operator size_t() const {

2
gtsam/geometry/PinholeCamera.h
View File

@ -213,7 +213,7 @@ public:
} }
/// for Canonical /// for Canonical
static PinholeCamera identity() { static PinholeCamera Identity() {
return PinholeCamera(); // assumes that the default constructor is valid return PinholeCamera(); // assumes that the default constructor is valid
} }

2
gtsam/geometry/PinholePose.h
View File

@ -412,7 +412,7 @@ public:
} }
/// for Canonical /// for Canonical
static PinholePose identity() { static PinholePose Identity() {
return PinholePose(); // assumes that the default constructor is valid return PinholePose(); // assumes that the default constructor is valid
} }

2
gtsam/geometry/Pose2.h
View File

@ -119,7 +119,7 @@ public:
/// @{ /// @{
/// identity for group operation /// identity for group operation
inline static Pose2 identity() { return Pose2(); } inline static Pose2 Identity() { return Pose2(); }
/// inverse /// inverse
GTSAM_EXPORT Pose2 inverse() const; GTSAM_EXPORT Pose2 inverse() const;

2
gtsam/geometry/Pose3.h
View File

@ -103,7 +103,7 @@ public:
/// @{ /// @{
/// identity for group operation /// identity for group operation
static Pose3 identity() { static Pose3 Identity() {
return Pose3(); return Pose3();
} }

4
gtsam/geometry/Rot2.h
View File

@ -107,8 +107,8 @@ namespace gtsam {
/// @name Group /// @name Group
/// @{ /// @{
/** identity */ /** Identity */
inline static Rot2 identity() { return Rot2(); } inline static Rot2 Identity() { return Rot2(); }
/** The inverse rotation - negative angle */ /** The inverse rotation - negative angle */
Rot2 inverse() const { return Rot2(c_, -s_);} Rot2 inverse() const { return Rot2(c_, -s_);}

2
gtsam/geometry/Rot3.h
View File

@ -297,7 +297,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
/// @{ /// @{
/// identity rotation for group operation /// identity rotation for group operation
inline static Rot3 identity() { inline static Rot3 Identity() {
return Rot3(); return Rot3();
} }

4
gtsam/geometry/SOn.h
View File

@ -178,13 +178,13 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
/// SO<N> identity for N >= 2 /// SO<N> identity for N >= 2
template <int N_ = N, typename = IsFixed<N_>> template <int N_ = N, typename = IsFixed<N_>>
static SO identity() { static SO Identity() {
return SO(); return SO();
} }
/// SO<N> identity for N == Eigen::Dynamic /// SO<N> identity for N == Eigen::Dynamic
template <int N_ = N, typename = IsDynamic<N_>> template <int N_ = N, typename = IsDynamic<N_>>
static SO identity(size_t n = 0) { static SO Identity(size_t n = 0) {
return SO(n); return SO(n);
} }

2
gtsam/geometry/Similarity2.cpp
View File

@ -134,7 +134,7 @@ void Similarity2::print(const std::string& s) const {
<< std::endl; << std::endl;
} }
Similarity2 Similarity2::identity() { return Similarity2(); } Similarity2 Similarity2::Identity() { return Similarity2(); }
Similarity2 Similarity2::operator*(const Similarity2& S) const { Similarity2 Similarity2::operator*(const Similarity2& S) const {
return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_); return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_);

2
gtsam/geometry/Similarity2.h
View File

@ -83,7 +83,7 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
/// @{ /// @{
/// Return an identity transform /// Return an identity transform
static Similarity2 identity(); static Similarity2 Identity();
/// Composition /// Composition
Similarity2 operator*(const Similarity2& S) const; Similarity2 operator*(const Similarity2& S) const;

2
gtsam/geometry/Similarity3.cpp
View File

@ -122,7 +122,7 @@ void Similarity3::print(const std::string& s) const {
std::cout << "t: " << translation().transpose() << " s: " << scale() << std::endl; std::cout << "t: " << translation().transpose() << " s: " << scale() << std::endl;
} }
Similarity3 Similarity3::identity() { Similarity3 Similarity3::Identity() {
return Similarity3(); return Similarity3();
} }
Similarity3 Similarity3::operator*(const Similarity3& S) const { Similarity3 Similarity3::operator*(const Similarity3& S) const {

2
gtsam/geometry/Similarity3.h
View File

@ -84,7 +84,7 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
/// @{ /// @{
/// Return an identity transform /// Return an identity transform
static Similarity3 identity(); static Similarity3 Identity();
/// Composition /// Composition
Similarity3 operator*(const Similarity3& S) const; Similarity3 operator*(const Similarity3& S) const;

6
gtsam/geometry/SphericalCamera.h
View File

@ -88,7 +88,7 @@ class GTSAM_EXPORT SphericalCamera {
/// Default constructor /// Default constructor
SphericalCamera() SphericalCamera()
: pose_(Pose3::identity()), emptyCal_(boost::make_shared<EmptyCal>()) {} : pose_(Pose3()), emptyCal_(boost::make_shared<EmptyCal>()) {}
/// Constructor with pose /// Constructor with pose
explicit SphericalCamera(const Pose3& pose) explicit SphericalCamera(const Pose3& pose)
@ -198,9 +198,9 @@ class GTSAM_EXPORT SphericalCamera {
} }
/// for Canonical /// for Canonical
static SphericalCamera identity() { static SphericalCamera Identity() {
return SphericalCamera( return SphericalCamera(
Pose3::identity()); // assumes that the default constructor is valid Pose3::Identity()); // assumes that the default constructor is valid
} }
/// for Linear Triangulation /// for Linear Triangulation

2
gtsam/geometry/StereoPoint2.h
View File

@ -71,7 +71,7 @@ public:
/// @{ /// @{
/// identity /// identity
inline static StereoPoint2 identity() { inline static StereoPoint2 Identity() {
return StereoPoint2(); return StereoPoint2();
} }

20
gtsam/geometry/geometry.i
View File

@ -16,7 +16,7 @@ class Point2 {
bool equals(const gtsam::Point2& point, double tol) const; bool equals(const gtsam::Point2& point, double tol) const;
// Group // Group
static gtsam::Point2 identity(); static gtsam::Point2 Identity();
// Standard Interface // Standard Interface
double x() const; double x() const;
@ -73,7 +73,7 @@ class StereoPoint2 {
bool equals(const gtsam::StereoPoint2& point, double tol) const; bool equals(const gtsam::StereoPoint2& point, double tol) const;
// Group // Group
static gtsam::StereoPoint2 identity(); static gtsam::StereoPoint2 Identity();
gtsam::StereoPoint2 inverse() const; gtsam::StereoPoint2 inverse() const;
gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const; gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const;
gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const; gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const;
@ -115,7 +115,7 @@ class Point3 {
bool equals(const gtsam::Point3& p, double tol) const; bool equals(const gtsam::Point3& p, double tol) const;
// Group // Group
static gtsam::Point3 identity(); static gtsam::Point3 Identity();
// Standard Interface // Standard Interface
Vector vector() const; Vector vector() const;
@ -149,7 +149,7 @@ class Rot2 {
bool equals(const gtsam::Rot2& rot, double tol) const; bool equals(const gtsam::Rot2& rot, double tol) const;
// Group // Group
static gtsam::Rot2 identity(); static gtsam::Rot2 Identity();
gtsam::Rot2 inverse(); gtsam::Rot2 inverse();
gtsam::Rot2 compose(const gtsam::Rot2& p2) const; gtsam::Rot2 compose(const gtsam::Rot2& p2) const;
gtsam::Rot2 between(const gtsam::Rot2& p2) const; gtsam::Rot2 between(const gtsam::Rot2& p2) const;
@ -198,7 +198,7 @@ class SO3 {
bool equals(const gtsam::SO3& other, double tol) const; bool equals(const gtsam::SO3& other, double tol) const;
// Group // Group
static gtsam::SO3 identity(); static gtsam::SO3 Identity();
gtsam::SO3 inverse() const; gtsam::SO3 inverse() const;
gtsam::SO3 between(const gtsam::SO3& R) const; gtsam::SO3 between(const gtsam::SO3& R) const;
gtsam::SO3 compose(const gtsam::SO3& R) const; gtsam::SO3 compose(const gtsam::SO3& R) const;
@ -228,7 +228,7 @@ class SO4 {
bool equals(const gtsam::SO4& other, double tol) const; bool equals(const gtsam::SO4& other, double tol) const;
// Group // Group
static gtsam::SO4 identity(); static gtsam::SO4 Identity();
gtsam::SO4 inverse() const; gtsam::SO4 inverse() const;
gtsam::SO4 between(const gtsam::SO4& Q) const; gtsam::SO4 between(const gtsam::SO4& Q) const;
gtsam::SO4 compose(const gtsam::SO4& Q) const; gtsam::SO4 compose(const gtsam::SO4& Q) const;
@ -258,7 +258,7 @@ class SOn {
bool equals(const gtsam::SOn& other, double tol) const; bool equals(const gtsam::SOn& other, double tol) const;
// Group // Group
static gtsam::SOn identity(); static gtsam::SOn Identity();
gtsam::SOn inverse() const; gtsam::SOn inverse() const;
gtsam::SOn between(const gtsam::SOn& Q) const; gtsam::SOn between(const gtsam::SOn& Q) const;
gtsam::SOn compose(const gtsam::SOn& Q) const; gtsam::SOn compose(const gtsam::SOn& Q) const;
@ -322,7 +322,7 @@ class Rot3 {
bool equals(const gtsam::Rot3& rot, double tol) const; bool equals(const gtsam::Rot3& rot, double tol) const;
// Group // Group
static gtsam::Rot3 identity(); static gtsam::Rot3 Identity();
gtsam::Rot3 inverse() const; gtsam::Rot3 inverse() const;
gtsam::Rot3 compose(const gtsam::Rot3& p2) const; gtsam::Rot3 compose(const gtsam::Rot3& p2) const;
gtsam::Rot3 between(const gtsam::Rot3& p2) const; gtsam::Rot3 between(const gtsam::Rot3& p2) const;
@ -380,7 +380,7 @@ class Pose2 {
bool equals(const gtsam::Pose2& pose, double tol) const; bool equals(const gtsam::Pose2& pose, double tol) const;
// Group // Group
static gtsam::Pose2 identity(); static gtsam::Pose2 Identity();
gtsam::Pose2 inverse() const; gtsam::Pose2 inverse() const;
gtsam::Pose2 compose(const gtsam::Pose2& p2) const; gtsam::Pose2 compose(const gtsam::Pose2& p2) const;
gtsam::Pose2 between(const gtsam::Pose2& p2) const; gtsam::Pose2 between(const gtsam::Pose2& p2) const;
@ -444,7 +444,7 @@ class Pose3 {
bool equals(const gtsam::Pose3& pose, double tol) const; bool equals(const gtsam::Pose3& pose, double tol) const;
// Group // Group
static gtsam::Pose3 identity(); static gtsam::Pose3 Identity();
gtsam::Pose3 inverse() const; gtsam::Pose3 inverse() const;
gtsam::Pose3 inverse(Eigen::Ref<Eigen::MatrixXd> H) const; gtsam::Pose3 inverse(Eigen::Ref<Eigen::MatrixXd> H) const;
gtsam::Pose3 compose(const gtsam::Pose3& pose) const; gtsam::Pose3 compose(const gtsam::Pose3& pose) const;

2
gtsam/geometry/tests/testPose2.cpp
View File

@ -902,7 +902,7 @@ TEST(Pose2 , TransformCovariance3) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, Print) { TEST(Pose2, Print) {
Pose2 pose(Rot2::identity(), Point2(1, 2)); Pose2 pose(Rot2::Identity(), Point2(1, 2));
// Generate the expected output // Generate the expected output
string s = "Planar Pose"; string s = "Planar Pose";

12
gtsam/geometry/tests/testPose3.cpp
View File

@ -1133,7 +1133,7 @@ Pose3 testing_interpolate(const Pose3& t1, const Pose3& t2, double gamma) { retu
TEST(Pose3, interpolateJacobians) { TEST(Pose3, interpolateJacobians) {
{ {
Pose3 X = Pose3::identity(); Pose3 X = Pose3::Identity();
Pose3 Y(Rot3::Rz(M_PI_2), Point3(1, 0, 0)); Pose3 Y(Rot3::Rz(M_PI_2), Point3(1, 0, 0));
double t = 0.5; double t = 0.5;
Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0.5, -0.207107, 0)); // note: different from test above: this is full Pose3 interpolation Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0.5, -0.207107, 0)); // note: different from test above: this is full Pose3 interpolation
@ -1147,10 +1147,10 @@ TEST(Pose3, interpolateJacobians) {
EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
} }
{ {
Pose3 X = Pose3::identity(); Pose3 X = Pose3::Identity();
Pose3 Y(Rot3::identity(), Point3(1, 0, 0)); Pose3 Y(Rot3::Identity(), Point3(1, 0, 0));
double t = 0.3; double t = 0.3;
Pose3 expectedPoseInterp(Rot3::identity(), Point3(0.3, 0, 0)); Pose3 expectedPoseInterp(Rot3::Identity(), Point3(0.3, 0, 0));
Matrix actualJacobianX, actualJacobianY; Matrix actualJacobianX, actualJacobianY;
EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5)); EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5));
@ -1161,7 +1161,7 @@ TEST(Pose3, interpolateJacobians) {
EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6));
} }
{ {
Pose3 X = Pose3::identity(); Pose3 X = Pose3::Identity();
Pose3 Y(Rot3::Rz(M_PI_2), Point3(0, 0, 0)); Pose3 Y(Rot3::Rz(M_PI_2), Point3(0, 0, 0));
double t = 0.5; double t = 0.5;
Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0)); Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0));
@ -1204,7 +1204,7 @@ TEST(Pose3, Create) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose3, Print) { TEST(Pose3, Print) {
Pose3 pose(Rot3::identity(), Point3(1, 2, 3)); Pose3 pose(Rot3::Identity(), Point3(1, 2, 3));
// Generate the expected output // Generate the expected output
std::string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: 1 2 3\n"; std::string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: 1 2 3\n";

6
gtsam/geometry/tests/testSimilarity3.cpp
View File

@ -203,11 +203,11 @@ TEST(Similarity3, ExpLogMap) {
Vector7 zeros; Vector7 zeros;
zeros << 0, 0, 0, 0, 0, 0, 0; zeros << 0, 0, 0, 0, 0, 0, 0;
Vector7 logIdentity = Similarity3::Logmap(Similarity3::identity()); Vector7 logIdentity = Similarity3::Logmap(Similarity3::Identity());
EXPECT(assert_equal(zeros, logIdentity)); EXPECT(assert_equal(zeros, logIdentity));
Similarity3 expZero = Similarity3::Expmap(zeros); Similarity3 expZero = Similarity3::Expmap(zeros);
Similarity3 ident = Similarity3::identity(); Similarity3 ident = Similarity3::Identity();
EXPECT(assert_equal(expZero, ident)); EXPECT(assert_equal(expZero, ident));
// Compare to matrix exponential, using expm in Lie.h // Compare to matrix exponential, using expm in Lie.h
@ -391,7 +391,7 @@ TEST(Similarity3, Optimization) {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.addPrior(key, prior, model); graph.addPrior(key, prior, model);
// Create initial estimate with identity transform // Create initial estimate with Identity transform
Values initial; Values initial;
initial.insert<Similarity3>(key, Similarity3()); initial.insert<Similarity3>(key, Similarity3());

2
gtsam/navigation/ImuBias.h
View File

@ -105,7 +105,7 @@ public:
/// @{ /// @{
/** identity for group operation */ /** identity for group operation */
static ConstantBias identity() { static ConstantBias Identity() {
return ConstantBias(); return ConstantBias();
} }

2
gtsam/navigation/navigation.i
View File

@ -17,7 +17,7 @@ class ConstantBias {
bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const; bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const;
// Group // Group
static gtsam::imuBias::ConstantBias identity(); static gtsam::imuBias::ConstantBias Identity();
// Operator Overloads // Operator Overloads
gtsam::imuBias::ConstantBias operator-() const; gtsam::imuBias::ConstantBias operator-() const;

4
gtsam/nonlinear/tests/testExpression.cpp
View File

@ -122,7 +122,7 @@ class Class : public Point3 {
enum {dimension = 3}; enum {dimension = 3};
using Point3::Point3; using Point3::Point3;
const Vector3& vector() const { return *this; } const Vector3& vector() const { return *this; }
inline static Class identity() { return Class(0,0,0); } inline static Class Identity() { return Class(0,0,0); }
double norm(OptionalJacobian<1,3> H = boost::none) const { double norm(OptionalJacobian<1,3> H = boost::none) const {
return norm3(*this, H); return norm3(*this, H);
} }
@ -285,7 +285,7 @@ TEST(Expression, compose2) {
// Test compose with one arguments referring to constant rotation. // Test compose with one arguments referring to constant rotation.
TEST(Expression, compose3) { TEST(Expression, compose3) {
// Create expression // Create expression
Rot3_ R1(Rot3::identity()), R2(3); Rot3_ R1(Rot3::Identity()), R2(3);
Rot3_ R3 = R1 * R2; Rot3_ R3 = R1 * R2;
// Check keys // Check keys

4
gtsam/slam/tests/testOrientedPlane3Factor.cpp
View File

@ -217,7 +217,7 @@ TEST(OrientedPlane3Factor, Issue561Simplified) {
// Setup prior factors // Setup prior factors
// Note: If x0 is too far away from the origin (e.g. x=100) this test can fail. // Note: If x0 is too far away from the origin (e.g. x=100) this test can fail.
Pose3 x0(Rot3::identity(), Vector3(10, -1, 1)); Pose3 x0(Rot3::Identity(), Vector3(10, -1, 1));
auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01); auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01);
graph.addPrior<Pose3>(X(0), x0, x0_noise); graph.addPrior<Pose3>(X(0), x0, x0_noise);
@ -241,7 +241,7 @@ TEST(OrientedPlane3Factor, Issue561Simplified) {
// Initial values // Initial values
// Just offset the initial pose by 1m. This is what we are trying to optimize. // Just offset the initial pose by 1m. This is what we are trying to optimize.
Values initialEstimate; Values initialEstimate;
Pose3 x0_initial = x0.compose(Pose3(Rot3::identity(), Vector3(1,0,0))); Pose3 x0_initial = x0.compose(Pose3(Rot3::Identity(), Vector3(1,0,0)));
initialEstimate.insert(P(1), p1); initialEstimate.insert(P(1), p1);
initialEstimate.insert(P(2), p2); initialEstimate.insert(P(2), p2);
initialEstimate.insert(X(0), x0_initial); initialEstimate.insert(X(0), x0_initial);

54
gtsam/slam/tests/testSmartProjectionRigFactor.cpp
View File

@ -69,7 +69,7 @@ SmartProjectionParams params(
TEST(SmartProjectionRigFactor, Constructor) { TEST(SmartProjectionRigFactor, Constructor) {
using namespace vanillaRig; using namespace vanillaRig;
boost::shared_ptr<Cameras> cameraRig(new Cameras()); boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3(), sharedK));
SmartRigFactor::shared_ptr factor1( SmartRigFactor::shared_ptr factor1(
new SmartRigFactor(model, cameraRig, params)); new SmartRigFactor(model, cameraRig, params));
} }
@ -89,7 +89,7 @@ TEST(SmartProjectionRigFactor, Constructor2) {
TEST(SmartProjectionRigFactor, Constructor3) { TEST(SmartProjectionRigFactor, Constructor3) {
using namespace vanillaRig; using namespace vanillaRig;
boost::shared_ptr<Cameras> cameraRig(new Cameras()); boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3(), sharedK));
SmartRigFactor::shared_ptr factor1( SmartRigFactor::shared_ptr factor1(
new SmartRigFactor(model, cameraRig, params)); new SmartRigFactor(model, cameraRig, params));
factor1->add(measurement1, x1, cameraId1); factor1->add(measurement1, x1, cameraId1);
@ -99,7 +99,7 @@ TEST(SmartProjectionRigFactor, Constructor3) {
TEST(SmartProjectionRigFactor, Constructor4) { TEST(SmartProjectionRigFactor, Constructor4) {
using namespace vanillaRig; using namespace vanillaRig;
boost::shared_ptr<Cameras> cameraRig(new Cameras()); boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3(), sharedK));
SmartProjectionParams params2( SmartProjectionParams params2(
gtsam::HESSIAN, gtsam::HESSIAN,
gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
@ -112,7 +112,7 @@ TEST(SmartProjectionRigFactor, Constructor4) {
TEST(SmartProjectionRigFactor, Equals) { TEST(SmartProjectionRigFactor, Equals) {
using namespace vanillaRig; using namespace vanillaRig;
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3(), sharedK));
SmartRigFactor::shared_ptr factor1( SmartRigFactor::shared_ptr factor1(
new SmartRigFactor(model, cameraRig, params)); new SmartRigFactor(model, cameraRig, params));
@ -140,7 +140,7 @@ TEST(SmartProjectionRigFactor, noiseless) {
Point2 level_uv_right = level_camera_right.project(landmark1); Point2 level_uv_right = level_camera_right.project(landmark1);
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3(), sharedK));
SmartRigFactor factor(model, cameraRig, params); SmartRigFactor factor(model, cameraRig, params);
factor.add(level_uv, x1); // both taken from the same camera factor.add(level_uv, x1); // both taken from the same camera
@ -197,7 +197,7 @@ TEST(SmartProjectionRigFactor, noisy) {
using namespace vanillaRig; using namespace vanillaRig;
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3(), sharedK));
// Project two landmarks into two cameras // Project two landmarks into two cameras
Point2 pixelError(0.2, 0.2); Point2 pixelError(0.2, 0.2);
@ -323,7 +323,7 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) {
Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1)); Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1));
Pose3 body_T_sensor2 = Pose3 body_T_sensor2 =
Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), Point3(0, 0, 1)); Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), Point3(0, 0, 1));
Pose3 body_T_sensor3 = Pose3::identity(); Pose3 body_T_sensor3 = Pose3();
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig->push_back(Camera(body_T_sensor1, sharedK)); cameraRig->push_back(Camera(body_T_sensor1, sharedK));
@ -407,7 +407,7 @@ TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) {
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig->push_back(Camera(Pose3::identity(), sharedK2)); cameraRig->push_back(Camera(Pose3(), sharedK2));
// Project three landmarks into three cameras // Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -495,7 +495,7 @@ TEST(SmartProjectionRigFactor, Factors) {
FastVector<size_t> cameraIds{0, 0}; FastVector<size_t> cameraIds{0, 0};
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3(), sharedK));
SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared<SmartRigFactor>( SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared<SmartRigFactor>(
model, cameraRig, params); model, cameraRig, params);
@ -578,7 +578,7 @@ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) {
// create smart factor // create smart factor
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3(), sharedK));
FastVector<size_t> cameraIds{0, 0, 0}; FastVector<size_t> cameraIds{0, 0, 0};
SmartRigFactor::shared_ptr smartFactor1( SmartRigFactor::shared_ptr smartFactor1(
new SmartRigFactor(model, cameraRig, params)); new SmartRigFactor(model, cameraRig, params));
@ -646,7 +646,7 @@ TEST(SmartProjectionRigFactor, landmarkDistance) {
params.setEnableEPI(false); params.setEnableEPI(false);
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3(), sharedK));
FastVector<size_t> cameraIds{0, 0, 0}; FastVector<size_t> cameraIds{0, 0, 0};
SmartRigFactor::shared_ptr smartFactor1( SmartRigFactor::shared_ptr smartFactor1(
@ -717,7 +717,7 @@ TEST(SmartProjectionRigFactor, dynamicOutlierRejection) {
params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3(), sharedK));
FastVector<size_t> cameraIds{0, 0, 0}; FastVector<size_t> cameraIds{0, 0, 0};
SmartRigFactor::shared_ptr smartFactor1( SmartRigFactor::shared_ptr smartFactor1(
@ -783,7 +783,7 @@ TEST(SmartProjectionRigFactor, CheckHessian) {
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3(), sharedK));
FastVector<size_t> cameraIds{0, 0, 0}; FastVector<size_t> cameraIds{0, 0, 0};
SmartRigFactor::shared_ptr smartFactor1( SmartRigFactor::shared_ptr smartFactor1(
@ -859,7 +859,7 @@ TEST(SmartProjectionRigFactor, Hessian) {
measurements_cam1.push_back(cam2_uv1); measurements_cam1.push_back(cam2_uv1);
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig->push_back(Camera(Pose3::identity(), sharedK2)); cameraRig->push_back(Camera(Pose3(), sharedK2));
FastVector<size_t> cameraIds{0, 0}; FastVector<size_t> cameraIds{0, 0};
SmartProjectionParams params( SmartProjectionParams params(
@ -889,7 +889,7 @@ TEST(SmartProjectionRigFactor, Hessian) {
TEST(SmartProjectionRigFactor, ConstructorWithCal3Bundler) { TEST(SmartProjectionRigFactor, ConstructorWithCal3Bundler) {
using namespace bundlerPose; using namespace bundlerPose;
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig->push_back(Camera(Pose3::identity(), sharedBundlerK)); cameraRig->push_back(Camera(Pose3(), sharedBundlerK));
SmartProjectionParams params; SmartProjectionParams params;
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
@ -917,7 +917,7 @@ TEST(SmartProjectionRigFactor, Cal3Bundler) {
KeyVector views{x1, x2, x3}; KeyVector views{x1, x2, x3};
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig->push_back(Camera(Pose3::identity(), sharedBundlerK)); cameraRig->push_back(Camera(Pose3(), sharedBundlerK));
FastVector<size_t> cameraIds{0, 0, 0}; FastVector<size_t> cameraIds{0, 0, 0};
SmartRigFactor::shared_ptr smartFactor1( SmartRigFactor::shared_ptr smartFactor1(
@ -988,7 +988,7 @@ TEST(SmartProjectionRigFactor,
KeyVector keys{x1, x2, x3, x1}; KeyVector keys{x1, x2, x3, x1};
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3(), sharedK));
FastVector<size_t> cameraIds{0, 0, 0, 0}; FastVector<size_t> cameraIds{0, 0, 0, 0};
SmartRigFactor::shared_ptr smartFactor1( SmartRigFactor::shared_ptr smartFactor1(
@ -1116,7 +1116,7 @@ TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) {
// create inputs // create inputs
KeyVector keys{x1, x2, x3}; KeyVector keys{x1, x2, x3};
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3(), sharedK));
FastVector<size_t> cameraIds{0, 0, 0}; FastVector<size_t> cameraIds{0, 0, 0};
FastVector<size_t> cameraIdsRedundant{0, 0, 0, 0}; FastVector<size_t> cameraIdsRedundant{0, 0, 0, 0};
@ -1195,11 +1195,11 @@ TEST(SmartProjectionRigFactor, timing) {
// Default cameras for simple derivatives // Default cameras for simple derivatives
static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
Rot3 R = Rot3::identity(); Rot3 R = Rot3::Identity();
Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
Pose3 body_P_sensorId = Pose3::identity(); Pose3 body_P_sensorId = Pose3();
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple)); cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple));
@ -1267,7 +1267,7 @@ TEST(SmartProjectionFactorP, optimization_3poses_sphericalCamera) {
keys.push_back(x3); keys.push_back(x3);
boost::shared_ptr<Cameras> cameraRig(new Cameras()); boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), emptyK)); cameraRig->push_back(Camera(Pose3(), emptyK));
SmartProjectionParams params( SmartProjectionParams params(
gtsam::HESSIAN, gtsam::HESSIAN,
@ -1330,10 +1330,10 @@ TEST(SmartProjectionFactorP, optimization_3poses_sphericalCamera) {
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionFactorP, timing_sphericalCamera) { TEST(SmartProjectionFactorP, timing_sphericalCamera) {
// create common data // create common data
Rot3 R = Rot3::identity(); Rot3 R = Rot3::Identity();
Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
Pose3 body_P_sensorId = Pose3::identity(); Pose3 body_P_sensorId = Pose3();
Point3 landmark1(0, 0, 10); Point3 landmark1(0, 0, 10);
// create spherical data // create spherical data
@ -1423,7 +1423,7 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) {
boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig( boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
new CameraSet<PinholePose<Cal3_S2>>()); // single camera in the rig new CameraSet<PinholePose<Cal3_S2>>()); // single camera in the rig
cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), sharedK)); cameraRig->push_back(PinholePose<Cal3_S2>(Pose3(), sharedK));
SmartRigFactor::shared_ptr smartFactor1( SmartRigFactor::shared_ptr smartFactor1(
new SmartRigFactor(model, cameraRig, params)); new SmartRigFactor(model, cameraRig, params));
@ -1461,7 +1461,7 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) {
boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig( boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
new CameraSet<PinholePose<Cal3_S2>>()); // single camera in the rig new CameraSet<PinholePose<Cal3_S2>>()); // single camera in the rig
cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), canonicalK)); cameraRig->push_back(PinholePose<Cal3_S2>(Pose3(), canonicalK));
SmartRigFactor::shared_ptr smartFactor1( SmartRigFactor::shared_ptr smartFactor1(
new SmartRigFactor(model, cameraRig, params)); new SmartRigFactor(model, cameraRig, params));
@ -1498,7 +1498,7 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) {
boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig( boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
new CameraSet<PinholePose<Cal3_S2>>()); // single camera in the rig new CameraSet<PinholePose<Cal3_S2>>()); // single camera in the rig
cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), canonicalK)); cameraRig->push_back(PinholePose<Cal3_S2>(Pose3(), canonicalK));
SmartRigFactor::shared_ptr smartFactor1( SmartRigFactor::shared_ptr smartFactor1(
new SmartRigFactor(model, cameraRig, params)); new SmartRigFactor(model, cameraRig, params));
@ -1537,7 +1537,7 @@ TEST(SmartProjectionFactorP, 2poses_sphericalCamera_rankTol) {
boost::shared_ptr<CameraSet<SphericalCamera>> cameraRig( boost::shared_ptr<CameraSet<SphericalCamera>> cameraRig(
new CameraSet<SphericalCamera>()); // single camera in the rig new CameraSet<SphericalCamera>()); // single camera in the rig
cameraRig->push_back(SphericalCamera(Pose3::identity(), emptyK)); cameraRig->push_back(SphericalCamera(Pose3(), emptyK));
// TRIANGULATION TEST WITH DEFAULT RANK TOL // TRIANGULATION TEST WITH DEFAULT RANK TOL
{ // rankTol = 1 or 0.1 gives a degenerate point, which is undesirable for a { // rankTol = 1 or 0.1 gives a degenerate point, which is undesirable for a

4
gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp
View File

@ -15,9 +15,9 @@ const Key x1 = 1, x2 = 2;
const double dt = 1.0; const double dt = 1.0;
PoseRTV origin, PoseRTV origin,
pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0)), pose1(Point3(0.5, 0.0, 0.0), Rot3(), Velocity3(1.0, 0.0, 0.0)),
pose1a(Point3(0.5, 0.0, 0.0)), pose1a(Point3(0.5, 0.0, 0.0)),
pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0)); pose2(Point3(1.5, 0.0, 0.0), Rot3(), Velocity3(1.0, 0.0, 0.0));
/* ************************************************************************* */ /* ************************************************************************* */
TEST( testVelocityConstraint, trapezoidal ) { TEST( testVelocityConstraint, trapezoidal ) {

2
gtsam_unstable/examples/GncPoseAveragingExample.cpp
View File

@ -53,7 +53,7 @@ int main(int argc, char** argv){
normal_distribution<double> normalInliers(0.0, 0.05); normal_distribution<double> normalInliers(0.0, 0.05);
Values initial; Values initial;
initial.insert(0, Pose3::identity()); // identity pose as initialization initial.insert(0, Pose3()); // identity pose as initialization
// create ground truth pose // create ground truth pose
Vector6 poseGtVector; Vector6 poseGtVector;

6
gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp
View File

@ -129,8 +129,8 @@ int main(int argc, char* argv[]) {
// Pose prior - at identity // Pose prior - at identity
auto priorPoseNoise = noiseModel::Diagonal::Sigmas( auto priorPoseNoise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished()); (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished());
graph.addPrior(X(1), Pose3::identity(), priorPoseNoise); graph.addPrior(X(1), Pose3::Identity(), priorPoseNoise);
initialEstimate.insert(X(0), Pose3::identity()); initialEstimate.insert(X(0), Pose3::Identity());
// Bias prior // Bias prior
graph.addPrior(B(1), imu.priorImuBias, graph.addPrior(B(1), imu.priorImuBias,
@ -157,7 +157,7 @@ int main(int argc, char* argv[]) {
if (frame != lastFrame || in.eof()) { if (frame != lastFrame || in.eof()) {
cout << "Running iSAM for frame: " << lastFrame << "\n"; cout << "Running iSAM for frame: " << lastFrame << "\n";
initialEstimate.insert(X(lastFrame), Pose3::identity()); initialEstimate.insert(X(lastFrame), Pose3::Identity());
initialEstimate.insert(V(lastFrame), Vector3(0, 0, 0)); initialEstimate.insert(V(lastFrame), Vector3(0, 0, 0));
initialEstimate.insert(B(lastFrame), imu.prevBias); initialEstimate.insert(B(lastFrame), imu.prevBias);

2
gtsam_unstable/geometry/Pose3Upright.h
View File

@ -95,7 +95,7 @@ public:
/// @{ /// @{
/// identity for group operation /// identity for group operation
static Pose3Upright identity() { return Pose3Upright(); } static Pose3Upright Identity() { return Pose3Upright(); }
/// inverse transformation with derivatives /// inverse transformation with derivatives
Pose3Upright inverse(boost::optional<Matrix&> H1=boost::none) const; Pose3Upright inverse(boost::optional<Matrix&> H1=boost::none) const;

2
gtsam_unstable/gtsam_unstable.i
View File

@ -130,7 +130,7 @@ class Pose3Upright {
gtsam::Pose3Upright retract(Vector v) const; gtsam::Pose3Upright retract(Vector v) const;
Vector localCoordinates(const gtsam::Pose3Upright& p2) const; Vector localCoordinates(const gtsam::Pose3Upright& p2) const;
static gtsam::Pose3Upright identity(); static gtsam::Pose3Upright Identity();
gtsam::Pose3Upright inverse() const; gtsam::Pose3Upright inverse() const;
gtsam::Pose3Upright compose(const gtsam::Pose3Upright& p2) const; gtsam::Pose3Upright compose(const gtsam::Pose3Upright& p2) const;
gtsam::Pose3Upright between(const gtsam::Pose3Upright& p2) const; gtsam::Pose3Upright between(const gtsam::Pose3Upright& p2) const;

12
gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp
View File

@ -44,8 +44,8 @@ TEST(LocalOrientedPlane3Factor, lm_translation_error) {
// Init pose and prior. Pose Prior is needed since a single plane measurement // Init pose and prior. Pose Prior is needed since a single plane measurement
// does not fully constrain the pose // does not fully constrain the pose
Pose3 init_pose = Pose3::identity(); Pose3 init_pose = Pose3::Identity();
Pose3 anchor_pose = Pose3::identity(); Pose3 anchor_pose = Pose3::Identity();
graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001)); graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001));
graph.addPrior(X(1), anchor_pose, noiseModel::Isotropic::Sigma(6, 0.001)); graph.addPrior(X(1), anchor_pose, noiseModel::Isotropic::Sigma(6, 0.001));
@ -89,7 +89,7 @@ TEST (LocalOrientedPlane3Factor, lm_rotation_error) {
// Init pose and prior. Pose Prior is needed since a single plane measurement // Init pose and prior. Pose Prior is needed since a single plane measurement
// does not fully constrain the pose // does not fully constrain the pose
Pose3 init_pose = Pose3::identity(); Pose3 init_pose = Pose3::Identity();
graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001)); graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001));
// Add two landmark measurements, differing in angle // Add two landmark measurements, differing in angle
@ -180,8 +180,8 @@ TEST(LocalOrientedPlane3Factor, Issue561Simplified) {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
// Setup prior factors // Setup prior factors
Pose3 x0(Rot3::identity(), Vector3(100, 30, 10)); // the "sensor pose" Pose3 x0(Rot3::Identity(), Vector3(100, 30, 10)); // the "sensor pose"
Pose3 x1(Rot3::identity(), Vector3(90, 40, 5) ); // the "anchor pose" Pose3 x1(Rot3::Identity(), Vector3(90, 40, 5) ); // the "anchor pose"
auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01); auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01);
auto x1_noise = noiseModel::Isotropic::Sigma(6, 0.01); auto x1_noise = noiseModel::Isotropic::Sigma(6, 0.01);
@ -213,7 +213,7 @@ TEST(LocalOrientedPlane3Factor, Issue561Simplified) {
// Initial values // Initial values
// Just offset the initial pose by 1m. This is what we are trying to optimize. // Just offset the initial pose by 1m. This is what we are trying to optimize.
Values initialEstimate; Values initialEstimate;
Pose3 x0_initial = x0.compose(Pose3(Rot3::identity(), Vector3(1, 0, 0))); Pose3 x0_initial = x0.compose(Pose3(Rot3::Identity(), Vector3(1, 0, 0)));
initialEstimate.insert(P(1), p1_in_x1); initialEstimate.insert(P(1), p1_in_x1);
initialEstimate.insert(P(2), p2_in_x1); initialEstimate.insert(P(2), p2_in_x1);
initialEstimate.insert(X(0), x0_initial); initialEstimate.insert(X(0), x0_initial);

2
gtsam_unstable/slam/tests/testPartialPriorFactor.cpp
View File

@ -173,7 +173,7 @@ TEST(PartialPriorFactor, Constructors3) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(PartialPriorFactor, JacobianAtIdentity3) { TEST(PartialPriorFactor, JacobianAtIdentity3) {
Key poseKey(1); Key poseKey(1);
Pose3 measurement = Pose3::identity(); Pose3 measurement = Pose3::Identity();
SharedNoiseModel model = NM::Isotropic::Sigma(1, 0.25); SharedNoiseModel model = NM::Isotropic::Sigma(1, 0.25);
TestPartialPriorFactor3 factor(poseKey, kIndexTy, measurement.translation().y(), model); TestPartialPriorFactor3 factor(poseKey, kIndexTy, measurement.translation().y(), model);

8
gtsam_unstable/slam/tests/testPoseToPointFactor.cpp
View File

@ -16,7 +16,7 @@ using namespace gtsam::noiseModel;
/* ************************************************************************* */ /* ************************************************************************* */
// Verify zero error when there is no noise // Verify zero error when there is no noise
TEST(PoseToPointFactor, errorNoiseless_2D) { TEST(PoseToPointFactor, errorNoiseless_2D) {
Pose2 pose = Pose2::identity(); Pose2 pose = Pose2::Identity();
Point2 point(1.0, 2.0); Point2 point(1.0, 2.0);
Point2 noise(0.0, 0.0); Point2 noise(0.0, 0.0);
Point2 measured = point + noise; Point2 measured = point + noise;
@ -33,7 +33,7 @@ TEST(PoseToPointFactor, errorNoiseless_2D) {
/* ************************************************************************* */ /* ************************************************************************* */
// Verify expected error in test scenario // Verify expected error in test scenario
TEST(PoseToPointFactor, errorNoise_2D) { TEST(PoseToPointFactor, errorNoise_2D) {
Pose2 pose = Pose2::identity(); Pose2 pose = Pose2::Identity();
Point2 point(1.0, 2.0); Point2 point(1.0, 2.0);
Point2 noise(-1.0, 0.5); Point2 noise(-1.0, 0.5);
Point2 measured = point + noise; Point2 measured = point + noise;
@ -86,7 +86,7 @@ TEST(PoseToPointFactor, jacobian_2D) {
/* ************************************************************************* */ /* ************************************************************************* */
// Verify zero error when there is no noise // Verify zero error when there is no noise
TEST(PoseToPointFactor, errorNoiseless_3D) { TEST(PoseToPointFactor, errorNoiseless_3D) {
Pose3 pose = Pose3::identity(); Pose3 pose = Pose3::Identity();
Point3 point(1.0, 2.0, 3.0); Point3 point(1.0, 2.0, 3.0);
Point3 noise(0.0, 0.0, 0.0); Point3 noise(0.0, 0.0, 0.0);
Point3 measured = point + noise; Point3 measured = point + noise;
@ -103,7 +103,7 @@ TEST(PoseToPointFactor, errorNoiseless_3D) {
/* ************************************************************************* */ /* ************************************************************************* */
// Verify expected error in test scenario // Verify expected error in test scenario
TEST(PoseToPointFactor, errorNoise_3D) { TEST(PoseToPointFactor, errorNoise_3D) {
Pose3 pose = Pose3::identity(); Pose3 pose = Pose3::Identity();
Point3 point(1.0, 2.0, 3.0); Point3 point(1.0, 2.0, 3.0);
Point3 noise(-1.0, 0.5, 0.3); Point3 noise(-1.0, 0.5, 0.3);
Point3 measured = point + noise; Point3 measured = point + noise;

34
gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp
View File

@ -88,7 +88,7 @@ typedef SmartProjectionPoseFactorRollingShutter<PinholePose<Cal3_S2>>
TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { TEST(SmartProjectionPoseFactorRollingShutter, Constructor) {
using namespace vanillaPoseRS; using namespace vanillaPoseRS;
boost::shared_ptr<Cameras> cameraRig(new Cameras()); boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
SmartFactorRS::shared_ptr factor1( SmartFactorRS::shared_ptr factor1(
new SmartFactorRS(model, cameraRig, params)); new SmartFactorRS(model, cameraRig, params));
} }
@ -97,7 +97,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor) {
TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) {
using namespace vanillaPoseRS; using namespace vanillaPoseRS;
boost::shared_ptr<Cameras> cameraRig(new Cameras()); boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
params.setRankTolerance(rankTol); params.setRankTolerance(rankTol);
SmartFactorRS factor1(model, cameraRig, params); SmartFactorRS factor1(model, cameraRig, params);
} }
@ -106,7 +106,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) {
TEST(SmartProjectionPoseFactorRollingShutter, add) { TEST(SmartProjectionPoseFactorRollingShutter, add) {
using namespace vanillaPoseRS; using namespace vanillaPoseRS;
boost::shared_ptr<Cameras> cameraRig(new Cameras()); boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
SmartFactorRS::shared_ptr factor1( SmartFactorRS::shared_ptr factor1(
new SmartFactorRS(model, cameraRig, params)); new SmartFactorRS(model, cameraRig, params));
factor1->add(measurement1, x1, x2, interp_factor); factor1->add(measurement1, x1, x2, interp_factor);
@ -230,7 +230,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) {
// Project two landmarks into two cameras // Project two landmarks into two cameras
Point2 level_uv = cam1.project(landmark1); Point2 level_uv = cam1.project(landmark1);
Point2 level_uv_right = cam2.project(landmark1); Point2 level_uv_right = cam2.project(landmark1);
Pose3 body_P_sensorId = Pose3::identity(); Pose3 body_P_sensorId = Pose3::Identity();
boost::shared_ptr<Cameras> cameraRig(new Cameras()); boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(body_P_sensorId, sharedK)); cameraRig->push_back(Camera(body_P_sensorId, sharedK));
@ -405,7 +405,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) {
interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
boost::shared_ptr<Cameras> cameraRig(new Cameras()); boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
SmartFactorRS::shared_ptr smartFactor1( SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, cameraRig, params)); new SmartFactorRS(model, cameraRig, params));
@ -480,7 +480,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) {
boost::shared_ptr<Cameras> cameraRig(new Cameras()); boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(body_P_sensor, sharedK)); cameraRig->push_back(Camera(body_P_sensor, sharedK));
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
SmartFactorRS::shared_ptr smartFactor1( SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, cameraRig, params)); new SmartFactorRS(model, cameraRig, params));
@ -633,11 +633,11 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) {
// Default cameras for simple derivatives // Default cameras for simple derivatives
static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
Rot3 R = Rot3::identity(); Rot3 R = Rot3::Identity();
Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
Pose3 body_P_sensorId = Pose3::identity(); Pose3 body_P_sensorId = Pose3::Identity();
// one landmarks 1m in front of camera // one landmarks 1m in front of camera
Point3 landmark1(0, 0, 10); Point3 landmark1(0, 0, 10);
@ -747,7 +747,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) {
params.setEnableEPI(true); params.setEnableEPI(true);
boost::shared_ptr<Cameras> cameraRig(new Cameras()); boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
SmartFactorRS smartFactor1(model, cameraRig, params); SmartFactorRS smartFactor1(model, cameraRig, params);
smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors);
@ -816,7 +816,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
params.setEnableEPI(false); params.setEnableEPI(false);
boost::shared_ptr<Cameras> cameraRig(new Cameras()); boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
SmartFactorRS smartFactor1(model, cameraRig, params); SmartFactorRS smartFactor1(model, cameraRig, params);
smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors);
@ -894,7 +894,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
params.setEnableEPI(false); params.setEnableEPI(false);
boost::shared_ptr<Cameras> cameraRig(new Cameras()); boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
SmartFactorRS::shared_ptr smartFactor1( SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, cameraRig, params)); new SmartFactorRS(model, cameraRig, params));
@ -961,7 +961,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
boost::shared_ptr<Cameras> cameraRig(new Cameras()); boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
SmartFactorRS::shared_ptr smartFactor1( SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, cameraRig, params)); new SmartFactorRS(model, cameraRig, params));
@ -1102,7 +1102,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
interp_factors.push_back(interp_factor1); interp_factors.push_back(interp_factor1);
boost::shared_ptr<Cameras> cameraRig(new Cameras()); boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
SmartFactorRS::shared_ptr smartFactor1( SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, cameraRig, params)); new SmartFactorRS(model, cameraRig, params));
@ -1261,7 +1261,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
interp_factors.at(0)); // we readd the first interp factor interp_factors.at(0)); // we readd the first interp factor
boost::shared_ptr<Cameras> cameraRig(new Cameras()); boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3::Identity(), sharedK));
SmartFactorRS::shared_ptr smartFactor1( SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, cameraRig, params)); new SmartFactorRS(model, cameraRig, params));
@ -1331,11 +1331,11 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) {
gtsam::HESSIAN, gtsam::HESSIAN,
gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors
Rot3 R = Rot3::identity(); Rot3 R = Rot3::Identity();
Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
Pose3 body_P_sensorId = Pose3::identity(); Pose3 body_P_sensorId = Pose3::Identity();
// one landmarks 1m in front of camera // one landmarks 1m in front of camera
Point3 landmark1(0, 0, 10); Point3 landmark1(0, 0, 10);
@ -1431,7 +1431,7 @@ TEST(SmartProjectionPoseFactorRollingShutter,
params.setRankTolerance(0.1); params.setRankTolerance(0.1);
boost::shared_ptr<Cameras> cameraRig(new Cameras()); boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), emptyK)); cameraRig->push_back(Camera(Pose3::Identity(), emptyK));
SmartFactorRS_spherical::shared_ptr smartFactor1( SmartFactorRS_spherical::shared_ptr smartFactor1(
new SmartFactorRS_spherical(model, cameraRig, params)); new SmartFactorRS_spherical(model, cameraRig, params));

8
gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp
View File

@ -198,10 +198,10 @@ TEST(testISAM2SmartFactor, Stereo_Batch) {
// prior, for the first keyframe: // prior, for the first keyframe:
if (kf_id == 0) { if (kf_id == 0) {
batch_graph.addPrior(X(kf_id), Pose3::identity(), priorPoseNoise); batch_graph.addPrior(X(kf_id), Pose3::Identity(), priorPoseNoise);
} }
batch_values.insert(X(kf_id), Pose3::identity()); batch_values.insert(X(kf_id), Pose3::Identity());
} }
LevenbergMarquardtParams parameters; LevenbergMarquardtParams parameters;
@ -267,7 +267,7 @@ TEST(testISAM2SmartFactor, Stereo_iSAM2) {
// Storage of smart factors: // Storage of smart factors:
std::map<lm_id_t, SmartStereoProjectionPoseFactor::shared_ptr> smartFactors; std::map<lm_id_t, SmartStereoProjectionPoseFactor::shared_ptr> smartFactors;
Pose3 lastKeyframePose = Pose3::identity(); Pose3 lastKeyframePose = Pose3::Identity();
// Run one timestep at once: // Run one timestep at once:
for (const auto &entries : dataset) { for (const auto &entries : dataset) {
@ -307,7 +307,7 @@ TEST(testISAM2SmartFactor, Stereo_iSAM2) {
// prior, for the first keyframe: // prior, for the first keyframe:
if (kf_id == 0) { if (kf_id == 0) {
newFactors.addPrior(X(kf_id), Pose3::identity(), priorPoseNoise); newFactors.addPrior(X(kf_id), Pose3::Identity(), priorPoseNoise);
} }
// 2) Run iSAM2: // 2) Run iSAM2:

14
gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp
View File

@ -181,8 +181,8 @@ TEST_UNSAFE( SmartStereoProjectionFactorPP, noiseless_error_identityExtrinsics )
Values values; Values values;
values.insert(x1, w_Pose_cam1); values.insert(x1, w_Pose_cam1);
values.insert(x2, w_Pose_cam2); values.insert(x2, w_Pose_cam2);
values.insert(body_P_cam1_key, Pose3::identity()); values.insert(body_P_cam1_key, Pose3::Identity());
values.insert(body_P_cam2_key, Pose3::identity()); values.insert(body_P_cam2_key, Pose3::Identity());
SmartStereoProjectionFactorPP factor1(model); SmartStereoProjectionFactorPP factor1(model);
factor1.add(cam1_uv, x1, body_P_cam1_key, K2); factor1.add(cam1_uv, x1, body_P_cam1_key, K2);
@ -426,7 +426,7 @@ TEST( SmartStereoProjectionFactorPP, 3poses_optimization_multipleExtrinsics ) {
// Values // Values
Pose3 body_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI, 1., 0.1),Point3(0, 1, 0)); Pose3 body_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI, 1., 0.1),Point3(0, 1, 0));
Pose3 body_Pose_cam2 = Pose3(Rot3::Ypr(-M_PI / 4, 0.1, 1.0),Point3(1, 1, 1)); Pose3 body_Pose_cam2 = Pose3(Rot3::Ypr(-M_PI / 4, 0.1, 1.0),Point3(1, 1, 1));
Pose3 body_Pose_cam3 = Pose3::identity(); Pose3 body_Pose_cam3 = Pose3::Identity();
Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam1.inverse()); Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam1.inverse());
Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam2.inverse()); Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam2.inverse());
Pose3 w_Pose_body3 = w_Pose_cam3.compose(body_Pose_cam3.inverse()); Pose3 w_Pose_body3 = w_Pose_cam3.compose(body_Pose_cam3.inverse());
@ -1147,7 +1147,7 @@ TEST( SmartStereoProjectionFactorPP, landmarkDistance ) {
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
graph.addPrior(x1, pose1, noisePrior); graph.addPrior(x1, pose1, noisePrior);
graph.addPrior(x2, pose2, noisePrior); graph.addPrior(x2, pose2, noisePrior);
graph.addPrior(body_P_cam_key, Pose3::identity(), noisePrior); graph.addPrior(body_P_cam_key, Pose3::Identity(), noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -1156,7 +1156,7 @@ TEST( SmartStereoProjectionFactorPP, landmarkDistance ) {
values.insert(x1, pose1); values.insert(x1, pose1);
values.insert(x2, pose2); values.insert(x2, pose2);
values.insert(x3, pose3 * noise_pose); values.insert(x3, pose3 * noise_pose);
values.insert(body_P_cam_key, Pose3::identity()); values.insert(body_P_cam_key, Pose3::Identity());
// All smart factors are disabled and pose should remain where it is // All smart factors are disabled and pose should remain where it is
Values result; Values result;
@ -1245,7 +1245,7 @@ TEST( SmartStereoProjectionFactorPP, dynamicOutlierRejection ) {
values.insert(x1, pose1); values.insert(x1, pose1);
values.insert(x2, pose2); values.insert(x2, pose2);
values.insert(x3, pose3); values.insert(x3, pose3);
values.insert(body_P_cam_key, Pose3::identity()); values.insert(body_P_cam_key, Pose3::Identity());
EXPECT_DOUBLES_EQUAL(0, smartFactor1->error(values), 1e-9); EXPECT_DOUBLES_EQUAL(0, smartFactor1->error(values), 1e-9);
EXPECT_DOUBLES_EQUAL(0, smartFactor2->error(values), 1e-9); EXPECT_DOUBLES_EQUAL(0, smartFactor2->error(values), 1e-9);
@ -1267,7 +1267,7 @@ TEST( SmartStereoProjectionFactorPP, dynamicOutlierRejection ) {
Values result; Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
result = optimizer.optimize(); result = optimizer.optimize();
EXPECT(assert_equal(Pose3::identity(), result.at<Pose3>(body_P_cam_key))); EXPECT(assert_equal(Pose3::Identity(), result.at<Pose3>(body_P_cam_key)));
} }
/* ************************************************************************* */ /* ************************************************************************* */

2
tests/testExpressionFactor.cpp
View File

@ -382,7 +382,7 @@ TEST(ExpressionFactor, compose2) {
TEST(ExpressionFactor, compose3) { TEST(ExpressionFactor, compose3) {
// Create expression // Create expression
Rot3_ R1(Rot3::identity()), R2(3); Rot3_ R1(Rot3::Identity()), R2(3);
Rot3_ R3 = R1 * R2; Rot3_ R3 = R1 * R2;
// Create factor // Create factor

2
tests/testManifold.cpp
View File

@ -139,7 +139,7 @@ TEST(Manifold, DefaultChart) {
Vector v3(3); Vector v3(3);
v3 << 1, 1, 1; v3 << 1, 1, 1;
Rot3 I = Rot3::identity(); Rot3 I = Rot3::Identity();
Rot3 R = I.retract(v3); Rot3 R = I.retract(v3);
//DefaultChart<Rot3> chart5; //DefaultChart<Rot3> chart5;
EXPECT(assert_equal(v3, traits<Rot3>::Local(I, R))); EXPECT(assert_equal(v3, traits<Rot3>::Local(I, R)));

6
timing/timeShonanFactor.cpp
View File

@ -62,9 +62,9 @@ int main(int argc, char* argv[]) {
// Build graph // Build graph
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
// graph.add(NonlinearEquality<SOn>(0, SOn::identity(4))); // graph.add(NonlinearEquality<SOn>(0, SOn::Identity(4)));
auto priorModel = noiseModel::Isotropic::Sigma(6, 10000); auto priorModel = noiseModel::Isotropic::Sigma(6, 10000);
graph.add(PriorFactor<SOn>(0, SOn::identity(4), priorModel)); graph.add(PriorFactor<SOn>(0, SOn::Identity(4), priorModel));
auto G = boost::make_shared<Matrix>(SOn::VectorizedGenerators(4)); auto G = boost::make_shared<Matrix>(SOn::VectorizedGenerators(4));
for (const auto &m : measurements) { for (const auto &m : measurements) {
const auto &keys = m.keys(); const auto &keys = m.keys();
@ -92,7 +92,7 @@ int main(int argc, char* argv[]) {
for (size_t i = 0; i < 100; i++) { for (size_t i = 0; i < 100; i++) {
gttic_(optimize); gttic_(optimize);
Values initial; Values initial;
initial.insert(0, SOn::identity(4)); initial.insert(0, SOn::Identity(4));
for (size_t j = 1; j < poses.size(); j++) { for (size_t j = 1; j < poses.size(); j++) {
initial.insert(j, SOn::Random(rng, 4)); initial.insert(j, SOn::Random(rng, 4));
} }