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

Merged in feature/OptimizeSimilarity (pull request #119) 

A novel use of Sim3
release/4.3a0
Frank Dellaert 2016-02-07 19:35:55 -08:00
parent ed7cdbbfd7 39024aab16
commit 5a94b71c5f
3 changed files with 517 additions and 207 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

249
gtsam_unstable/geometry/Similarity3.cpp
View File

@ -17,79 +17,38 @@
#include <gtsam_unstable/geometry/Similarity3.h> #include <gtsam_unstable/geometry/Similarity3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Manifold.h> #include <gtsam/base/Manifold.h>
namespace gtsam { namespace gtsam {
Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) :
R_(R), t_(t), s_(s) {
}
Similarity3::Similarity3() : Similarity3::Similarity3() :
R_(), t_(), s_(1){ R_(), t_(), s_(1) {
} }
Similarity3::Similarity3(double s) : Similarity3::Similarity3(double s) :
s_ (s) { s_(s) {
} }
Similarity3::Similarity3(const Rotation& R, const Translation& t, double s) : Similarity3::Similarity3(const Rot3& R, const Point3& t, double s) :
R_ (R), t_ (t), s_ (s) { R_(R), t_(t), s_(s) {
} }
Similarity3::operator Pose3() const { Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) :
return Pose3(R_, s_*t_); R_(R), t_(t), s_(s) {
} }
Similarity3 Similarity3::identity() { Similarity3::Similarity3(const Matrix4& T) :
return Similarity3(); } R_(T.topLeftCorner<3, 3>()), t_(T.topRightCorner<3, 1>()), s_(1.0 / T(3, 3)) {
}
//Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { bool Similarity3::equals(const Similarity3& other, double tol) const {
// return Vector7(); return R_.equals(other.R_, tol) && t_.equals(other.t_, tol)
//} && s_ < (other.s_ + tol) && s_ > (other.s_ - tol);
// }
//Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) {
// return Similarity3();
//}
bool Similarity3::operator==(const Similarity3& other) const { bool Similarity3::operator==(const Similarity3& other) const {
return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_); return R_.matrix() == other.R_.matrix() && t_ == other.t_ && s_ == other.s_;
}
bool Similarity3::equals(const Similarity3& sim, double tol) const {
return R_.equals(sim.R_, tol) && t_.equals(sim.t_, tol)
&& s_ < (sim.s_+tol) && s_ > (sim.s_-tol);
}
Similarity3::Translation Similarity3::transform_from(const Translation& p) const {
return R_ * (s_ * p) + t_;
}
Matrix7 Similarity3::AdjointMap() const{
const Matrix3 R = R_.matrix();
const Vector3 t = t_.vector();
Matrix3 A = s_ * skewSymmetric(t) * R;
Matrix7 adj;
adj << s_*R, A, -s_*t, Z_3x3, R, Eigen::Matrix<double, 3, 1>::Zero(), Eigen::Matrix<double, 1, 6>::Zero(), 1;
return adj;
}
inline Similarity3::Translation Similarity3::operator*(const Translation& p) const {
return transform_from(p);
}
Similarity3 Similarity3::inverse() const {
Rotation Rt = R_.inverse();
Translation sRt = R_.inverse() * (-s_ * t_);
return Similarity3(Rt, sRt, 1.0/s_);
}
Similarity3 Similarity3::operator*(const Similarity3& T) const {
return Similarity3(R_ * T.R_, ((1.0/T.s_)*t_) + R_ * T.t_, s_*T.s_);
} }
void Similarity3::print(const std::string& s) const { void Similarity3::print(const std::string& s) const {
@ -100,31 +59,171 @@ void Similarity3::print(const std::string& s) const {
std::cout << "s: " << scale() << std::endl; std::cout << "s: " << scale() << std::endl;
} }
Similarity3 Similarity3::identity() {
return Similarity3();
}
Similarity3 Similarity3::operator*(const Similarity3& T) const {
return Similarity3(R_ * T.R_, ((1.0 / T.s_) * t_) + R_ * T.t_, s_ * T.s_);
}
Similarity3 Similarity3::inverse() const {
Rot3 Rt = R_.inverse();
Point3 sRt = R_.inverse() * (-s_ * t_);
return Similarity3(Rt, sRt, 1.0 / s_);
}
Point3 Similarity3::transform_from(const Point3& p, //
OptionalJacobian<3, 7> H1, OptionalJacobian<3, 3> H2) const {
Point3 q = R_ * p + t_;
if (H1) {
const Matrix3 R = R_.matrix();
Matrix3 DR = s_ * R * skewSymmetric(-p.x(), -p.y(), -p.z());
// TODO(frank): explain the derivative in lambda
*H1 << DR, s_ * R, s_ * p.vector();
}
if (H2)
*H2 = s_ * R_.matrix(); // just 3*3 sub-block of matrix()
return s_ * q;
}
Point3 Similarity3::operator*(const Point3& p) const {
return transform_from(p);
}
Matrix4 Similarity3::wedge(const Vector7& xi) {
// http://www.ethaneade.org/latex2html/lie/node29.html
const auto w = xi.head<3>();
const auto u = xi.segment<3>(3);
double lambda = xi[6];
Matrix4 W;
W << skewSymmetric(w), u, 0, 0, 0, -lambda;
return W;
}
Matrix7 Similarity3::AdjointMap() const {
// http://www.ethaneade.org/latex2html/lie/node30.html
const Matrix3 R = R_.matrix();
const Vector3 t = t_.vector();
const Matrix3 A = s_ * skewSymmetric(t) * R;
Matrix7 adj;
adj <<
R, Z_3x3, Matrix31::Zero(), // 3*7
A, s_ * R, -s_ * t, // 3*7
Matrix16::Zero(), 1; // 1*7
return adj;
}
Matrix3 Similarity3::GetV(Vector3 w, double lambda) {
// http://www.ethaneade.org/latex2html/lie/node29.html
double lambda2 = lambda * lambda;
double theta2 = w.transpose() * w;
double theta = sqrt(theta2);
double A, B, C;
// TODO(frank): eliminate copy/paste
if (theta2 > 1e-9 && lambda2 > 1e-9) {
const double X = sin(theta) / theta;
const double Y = (1 - cos(theta)) / theta2;
const double Z = (1 - X) / theta2;
const double W = (0.5 - Y) / theta2;
const double alpha = lambda2 / (lambda2 + theta2);
const double beta = (exp(-lambda) - 1 + lambda) / lambda2;
const double gamma = Y - (lambda * Z);
const double mu = (1 - lambda + (0.5 * lambda2) - exp(-lambda))
/ (lambda2 * lambda);
const double upsilon = Z - (lambda * W);
A = (1 - exp(-lambda)) / lambda;
B = alpha * (beta - gamma) + gamma;
C = alpha * (mu - upsilon) + upsilon;
} else if (theta2 <= 1e-9 && lambda2 > 1e-9) {
//Taylor series expansions
const double Y = 0.5 - theta2 / 24.0;
const double Z = 1.0 / 6.0 - theta2 / 120.0;
const double W = 1.0 / 24.0 - theta2 / 720.0;
const double alpha = lambda2 / (lambda2 + theta2);
const double beta = (exp(-lambda) - 1 + lambda) / lambda2;
const double gamma = Y - (lambda * Z);
const double mu = (1 - lambda + (0.5 * lambda2) - exp(-lambda))
/ (lambda2 * lambda);
const double upsilon = Z - (lambda * W);
A = (1 - exp(-lambda)) / lambda;
B = alpha * (beta - gamma) + gamma;
C = alpha * (mu - upsilon) + upsilon;
} else if (theta2 > 1e-9 && lambda2 <= 1e-9) {
const double X = sin(theta) / theta;
const double Y = (1 - cos(theta)) / theta2;
const double Z = (1 - X) / theta2;
const double W = (0.5 - Y) / theta2;
const double alpha = lambda2 / (lambda2 + theta2);
const double beta = 0.5 - lambda / 6.0 + lambda2 / 24.0
- (lambda * lambda2) / 120;
const double gamma = Y - (lambda * Z);
const double mu = 1.0 / 6.0 - lambda / 24 + lambda2 / 120
- (lambda * lambda2) / 720;
const double upsilon = Z - (lambda * W);
if (lambda < 1e-9) {
A = 1 - lambda / 2.0 + lambda2 / 6.0;
} else {
A = (1 - exp(-lambda)) / lambda;
}
B = alpha * (beta - gamma) + gamma;
C = alpha * (mu - upsilon) + upsilon;
} else {
const double Y = 0.5 - theta2 / 24.0;
const double Z = 1.0 / 6.0 - theta2 / 120.0;
const double W = 1.0 / 24.0 - theta2 / 720.0;
const double gamma = Y - (lambda * Z);
const double upsilon = Z - (lambda * W);
if (lambda < 1e-9) {
A = 1 - lambda / 2.0 + lambda2 / 6.0;
} else {
A = (1 - exp(-lambda)) / lambda;
}
B = gamma;
C = upsilon;
}
const Matrix3 Wx = skewSymmetric(w[0], w[1], w[2]);
return A * I_3x3 + B * Wx + C * Wx * Wx;
}
Vector7 Similarity3::Logmap(const Similarity3& T, OptionalJacobian<7, 7> Hm) {
// To get the logmap, calculate w and lambda, then solve for u as shown by Ethan at
// www.ethaneade.org/latex2html/lie/node29.html
const Vector3 w = Rot3::Logmap(T.R_);
const double lambda = log(T.s_);
Vector7 result;
result << w, GetV(w, lambda).inverse() * T.t_.vector(), lambda;
if (Hm) {
throw std::runtime_error("Similarity3::Logmap: derivative not implemented");
}
return result;
}
Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) {
const auto w = v.head<3>();
const auto u = v.segment<3>(3);
const double lambda = v[6];
if (Hm) {
throw std::runtime_error("Similarity3::Expmap: derivative not implemented");
}
const Matrix3 V = GetV(w, lambda);
return Similarity3(Rot3::Expmap(w), Point3(V * u), exp(lambda));
}
std::ostream &operator<<(std::ostream &os, const Similarity3& p) { std::ostream &operator<<(std::ostream &os, const Similarity3& p) {
os << "[" << p.rotation().xyz().transpose() << " " << p.translation().vector().transpose() << " " << os << "[" << p.rotation().xyz().transpose() << " "
p.scale() << "]\';"; << p.translation().vector().transpose() << " " << p.scale() << "]\';";
return os; return os;
} }
Similarity3 Similarity3::ChartAtOrigin::Retract(const Vector7& v, ChartJacobian H) { const Matrix4 Similarity3::matrix() const {
// Will retracting or localCoordinating R work if R is not a unit rotation? Matrix4 T;
// Also, how do we actually get s out? Seems like we need to store it somewhere. T.topRows<3>() << R_.matrix(), t_.vector();
Rotation r; //Create a zero rotation to do our retraction. T.bottomRows<1>() << 0, 0, 0, 1.0/s_;
return Similarity3( // return T;
r.retract(v.head<3>()), // retract rotation using v[0,1,2]
Translation(v.segment<3>(3)), // Retract the translation
1.0 + v[6]); //finally, update scale using v[6]
} }
Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other, ChartJacobian H) { Similarity3::operator Pose3() const {
Rotation r; //Create a zero rotation to do the retraction return Pose3(R_, s_ * t_);
Vector7 v;
v.head<3>() = r.localCoordinates(other.R_);
v.segment<3>(3) = other.t_.vector();
//v.segment<3>(3) = translation().localCoordinates(other.translation());
v[6] = other.s_ - 1.0;
return v;
}
} }
}

152
gtsam_unstable/geometry/Similarity3.h
View File

@ -32,13 +32,15 @@ class Pose3;
*/ */
class Similarity3: public LieGroup<Similarity3, 7> { class Similarity3: public LieGroup<Similarity3, 7> {
/** Pose Concept requirements */ /// @name Pose Concept
/// @{
typedef Rot3 Rotation; typedef Rot3 Rotation;
typedef Point3 Translation; typedef Point3 Translation;
/// @}
private: private:
Rotation R_; Rot3 R_;
Translation t_; Point3 t_;
double s_; double s_;
public: public:
@ -46,17 +48,21 @@ public:
/// @name Constructors /// @name Constructors
/// @{ /// @{
/// Default constructor
Similarity3(); Similarity3();
/// Construct pure scaling /// Construct pure scaling
Similarity3(double s); Similarity3(double s);
/// Construct from GTSAM types /// Construct from GTSAM types
Similarity3(const Rotation& R, const Translation& t, double s); Similarity3(const Rot3& R, const Point3& t, double s);
/// Construct from Eigen types /// Construct from Eigen types
Similarity3(const Matrix3& R, const Vector3& t, double s); Similarity3(const Matrix3& R, const Vector3& t, double s);
/// Construct from matrix [R t; 0 s^-1]
Similarity3(const Matrix4& T);
/// @} /// @}
/// @name Testable /// @name Testable
/// @{ /// @{
@ -64,7 +70,7 @@ public:
/// Compare with tolerance /// Compare with tolerance
bool equals(const Similarity3& sim, double tol) const; bool equals(const Similarity3& sim, double tol) const;
/// Compare with standard tolerance /// Exact equality
bool operator==(const Similarity3& other) const; bool operator==(const Similarity3& other) const;
/// Print with optional string /// Print with optional string
@ -79,74 +85,118 @@ public:
/// Return an identity transform /// Return an identity transform
static Similarity3 identity(); static Similarity3 identity();
/// Composition
Similarity3 operator*(const Similarity3& T) const;
/// Return the inverse /// Return the inverse
Similarity3 inverse() const; Similarity3 inverse() const;
Translation transform_from(const Translation& p) const; /// @}
/// @name Group action on Point3
/// @{
/// Action on a point p is s*(R*p+t)
Point3 transform_from(const Point3& p, //
OptionalJacobian<3, 7> H1 = boost::none, //
OptionalJacobian<3, 3> H2 = boost::none) const;
/** syntactic sugar for transform_from */ /** syntactic sugar for transform_from */
inline Translation operator*(const Translation& p) const; Point3 operator*(const Point3& p) const;
Similarity3 operator*(const Similarity3& T) const;
/// @} /// @}
/// @name Standard interface /// @name Lie Group
/// @{ /// @{
/// Return a GTSAM rotation /** Log map at the identity
const Rotation& rotation() const { * \f$ [R_x,R_y,R_z, t_x, t_y, t_z, \lambda] \f$
return R_; */
}; static Vector7 Logmap(const Similarity3& s, //
OptionalJacobian<7, 7> Hm = boost::none);
/// Return a GTSAM translation /** Exponential map at the identity
const Translation& translation() const { */
return t_; static Similarity3 Expmap(const Vector7& v, //
}; OptionalJacobian<7, 7> Hm = boost::none);
/// Return the scale /// Chart at the origin
double scale() const {
return s_;
};
/// Convert to a rigid body pose
operator Pose3() const;
/// Dimensionality of tangent space = 7 DOF - used to autodetect sizes
inline static size_t Dim() {
return 7;
};
/// Dimensionality of tangent space = 7 DOF
inline size_t dim() const {
return 7;
};
/// @}
/// @name Chart
/// @{
/// Update Similarity transform via 7-dim vector in tangent space
struct ChartAtOrigin { struct ChartAtOrigin {
static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none); static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none) {
return Similarity3::Expmap(v, H);
/// 7-dimensional vector v in tangent space that makes other = this->retract(v) }
static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none); static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none) {
return Similarity3::Logmap(other, H);
}
}; };
using LieGroup<Similarity3, 7>::inverse;
/**
* wedge for Similarity3:
* @param xi 7-dim twist (w,u,lambda) where
* @return 4*4 element of Lie algebra that can be exponentiated
* TODO(frank): rename to Hat, make part of traits
*/
static Matrix4 wedge(const Vector7& xi);
/// Project from one tangent space to another /// Project from one tangent space to another
Matrix7 AdjointMap() const; Matrix7 AdjointMap() const;
/// @} /// @}
/// @name Stubs /// @name Standard interface
/// @{ /// @{
/// Not currently implemented, required because this is a lie group /// Calculate 4*4 matrix group equivalent
static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm = boost::none); const Matrix4 matrix() const;
static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm = boost::none);
using LieGroup<Similarity3, 7>::inverse; // version with derivative /// Return a GTSAM rotation
const Rot3& rotation() const {
return R_;
}
/// Return a GTSAM translation
const Point3& translation() const {
return t_;
}
/// Return the scale
double scale() const {
return s_;
}
/// Convert to a rigid body pose (R, s*t)
/// TODO(frank): why is this here? Red flag! Definitely don't have it as a cast.
operator Pose3() const;
/// Dimensionality of tangent space = 7 DOF - used to autodetect sizes
inline static size_t Dim() {
return 7;
}
/// Dimensionality of tangent space = 7 DOF
inline size_t dim() const {
return 7;
}
/// @}
/// @name Helper functions
/// @{
/// Calculate expmap and logmap coefficients.
private:
static Matrix3 GetV(Vector3 w, double lambda);
/// @}
}; };
template<> template<>
struct traits<Similarity3> : public internal::LieGroup<Similarity3> {}; inline Matrix wedge<Similarity3>(const Vector& xi) {
return Similarity3::wedge(xi);
} }
template<>
struct traits<Similarity3> : public internal::LieGroup<Similarity3> {};
template<>
struct traits<const Similarity3> : public internal::LieGroup<Similarity3> {};
} // namespace gtsam

323
gtsam_unstable/geometry/tests/testSimilarity3.cpp
View File

@ -13,89 +13,108 @@
* @file testSimilarity3.cpp * @file testSimilarity3.cpp
* @brief Unit tests for Similarity3 class * @brief Unit tests for Similarity3 class
* @author Paul Drews * @author Paul Drews
* @author Zhaoyang Lv
*/ */
#include <gtsam_unstable/geometry/Similarity3.h> #include <gtsam_unstable/geometry/Similarity3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/testLie.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/function.hpp>
#include <boost/bind.hpp>
using namespace gtsam; using namespace gtsam;
using namespace std; using namespace std;
using symbol_shorthand::X; using symbol_shorthand::X;
GTSAM_CONCEPT_TESTABLE_INST(Similarity3) GTSAM_CONCEPT_TESTABLE_INST(Similarity3)
static Point3 P(0.2,0.7,-2); static const Point3 P(0.2, 0.7, -2);
static Rot3 R = Rot3::Rodrigues(0.3,0,0); static const Rot3 R = Rot3::Rodrigues(0.3, 0, 0);
static Similarity3 T(R,Point3(3.5,-8.2,4.2),1); static const double s = 4;
static Similarity3 T2(Rot3::Rodrigues(0.3,0.2,0.1),Point3(3.5,-8.2,4.2),1); static const Similarity3 id;
static Similarity3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3), 1); static const Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1);
static const Similarity3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2), 1);
static const Similarity3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3), 1);
static const Similarity3 T4(R, P, s);
static const Similarity3 T5(R, P, 10);
static const Similarity3 T6(Rot3(), Point3(1, 1, 0), 2); // Simpler transform
//******************************************************************************
TEST(Similarity3, Concepts) {
BOOST_CONCEPT_ASSERT((IsGroup<Similarity3 >));
BOOST_CONCEPT_ASSERT((IsManifold<Similarity3 >));
BOOST_CONCEPT_ASSERT((IsLieGroup<Similarity3 >));
}
//****************************************************************************** //******************************************************************************
TEST(Similarity3, Constructors) { TEST(Similarity3, Constructors) {
Similarity3 test; Similarity3 sim3_Construct1;
Similarity3 sim3_Construct2(s);
Similarity3 sim3_Construct3(R, P, s);
Similarity3 sim4_Construct4(R.matrix(), P.vector(), s);
} }
//****************************************************************************** //******************************************************************************
TEST(Similarity3, Getters) { TEST(Similarity3, Getters) {
Similarity3 test; Similarity3 sim3_default;
EXPECT(assert_equal(Rot3(), test.rotation())); EXPECT(assert_equal(Rot3(), sim3_default.rotation()));
EXPECT(assert_equal(Point3(), test.translation())); EXPECT(assert_equal(Point3(), sim3_default.translation()));
EXPECT_DOUBLES_EQUAL(1.0, test.scale(), 1e-9); EXPECT_DOUBLES_EQUAL(1.0, sim3_default.scale(), 1e-9);
Similarity3 sim3(Rot3::Ypr(1, 2, 3), Point3(4, 5, 6), 7);
EXPECT(assert_equal(Rot3::Ypr(1, 2, 3), sim3.rotation()));
EXPECT(assert_equal(Point3(4, 5, 6), sim3.translation()));
EXPECT_DOUBLES_EQUAL(7.0, sim3.scale(), 1e-9);
} }
//****************************************************************************** //******************************************************************************
TEST(Similarity3, Getters2) {
Similarity3 test(Rot3::Ypr(1, 2, 3), Point3(4, 5, 6), 7);
EXPECT(assert_equal(Rot3::Ypr(1, 2, 3), test.rotation()));
EXPECT(assert_equal(Point3(4, 5, 6), test.translation()));
EXPECT_DOUBLES_EQUAL(7.0, test.scale(), 1e-9);
}
TEST(Similarity3, AdjointMap) { TEST(Similarity3, AdjointMap) {
Similarity3 test(Rot3::Ypr(1,2,3).inverse(), Point3(4,5,6), 7); const Matrix4 T = T2.matrix();
Matrix7 result; // Check Ad with actual definition
result << -1.5739, -2.4512, -6.3651, -50.7671, -11.2503, 16.8859, -28.0000, Vector7 delta;
6.3167, -2.9884, -0.4111, 0.8502, 8.6373, -49.7260, -35.0000, delta << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7;
-2.5734, -5.8362, 2.8839, 33.1363, 0.3024, 30.1811, -42.0000, Matrix4 W = Similarity3::wedge(delta);
0, 0, 0, -0.2248, -0.3502, -0.9093, 0, Matrix4 TW = Similarity3::wedge(T2.AdjointMap() * delta);
0, 0, 0, 0.9024, -0.4269, -0.0587, 0, EXPECT(assert_equal(TW, Matrix4(T * W * T.inverse()), 1e-9));
0, 0, 0, -0.3676, -0.8337, 0.4120, 0,
0, 0, 0, 0, 0, 0, 1.0000;
EXPECT(assert_equal(result, test.AdjointMap(), 1e-3));
} }
//******************************************************************************
TEST(Similarity3, inverse) { TEST(Similarity3, inverse) {
Similarity3 test(Rot3::Ypr(1,2,3).inverse(), Point3(4,5,6), 7); Similarity3 sim3(Rot3::Ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7);
Matrix3 Re; Matrix3 Re; // some values from matlab
Re << -0.2248, 0.9024, -0.3676, Re << -0.2248, 0.9024, -0.3676, -0.3502, -0.4269, -0.8337, -0.9093, -0.0587, 0.4120;
-0.3502, -0.4269, -0.8337,
-0.9093, -0.0587, 0.4120;
Vector3 te(-9.8472, 59.7640, 10.2125); Vector3 te(-9.8472, 59.7640, 10.2125);
Similarity3 expected(Re, te, 1.0/7.0); Similarity3 expected(Re, te, 1.0 / 7.0);
EXPECT(assert_equal(expected, test.inverse(), 1e-3)); EXPECT(assert_equal(expected, sim3.inverse(), 1e-4));
EXPECT(assert_equal(sim3, sim3.inverse().inverse(), 1e-8));
// test lie group inverse
Matrix H1, H2;
EXPECT(assert_equal(expected, sim3.inverse(H1), 1e-4));
EXPECT(assert_equal(sim3, sim3.inverse().inverse(H2), 1e-8));
} }
TEST(Similarity3, multiplication) { //******************************************************************************
Similarity3 test1(Rot3::Ypr(1,2,3).inverse(), Point3(4,5,6), 7); TEST(Similarity3, Multiplication) {
Similarity3 test2(Rot3::Ypr(1,2,3).inverse(), Point3(8,9,10), 11); Similarity3 test1(Rot3::Ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7);
Similarity3 test2(Rot3::Ypr(1, 2, 3).inverse(), Point3(8, 9, 10), 11);
Matrix3 re; Matrix3 re;
re << 0.0688, 0.9863, -0.1496, re << 0.0688, 0.9863, -0.1496, -0.5665, -0.0848, -0.8197, -0.8211, 0.1412, 0.5530;
-0.5665, -0.0848, -0.8197,
-0.8211, 0.1412, 0.5530;
Vector3 te(-13.6797, 3.2441, -5.7794); Vector3 te(-13.6797, 3.2441, -5.7794);
Similarity3 expected(re, te, 77); Similarity3 expected(re, te, 77);
EXPECT(assert_equal(expected, test1*test2, 1e-2)); EXPECT(assert_equal(expected, test1 * test2, 1e-2));
} }
//****************************************************************************** //******************************************************************************
@ -117,15 +136,14 @@ TEST(Similarity3, Manifold) {
v3 << 0, 0, 0, 1, 2, 3, 0; v3 << 0, 0, 0, 1, 2, 3, 0;
EXPECT(assert_equal(v3, sim2.localCoordinates(sim3))); EXPECT(assert_equal(v3, sim2.localCoordinates(sim3)));
// Similarity3 other = Similarity3(Rot3::Ypr(0.01, 0.02, 0.03), Point3(0.4, 0.5, 0.6), 1); Similarity3 other = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(4, 5, 6), 1);
Similarity3 other = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3),Point3(4,5,6),1);
Vector vlocal = sim.localCoordinates(other); Vector vlocal = sim.localCoordinates(other);
EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2)); EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2));
Similarity3 other2 = Similarity3(Rot3::Ypr(0.3, 0, 0),Point3(4,5,6),1); Similarity3 other2 = Similarity3(Rot3::Ypr(0.3, 0, 0), Point3(4, 5, 6), 1);
Rot3 R = Rot3::Rodrigues(0.3,0,0); Rot3 R = Rot3::Rodrigues(0.3, 0, 0);
Vector vlocal2 = sim.localCoordinates(other2); Vector vlocal2 = sim.localCoordinates(other2);
@ -134,30 +152,29 @@ TEST(Similarity3, Manifold) {
// TODO add unit tests for retract and localCoordinates // TODO add unit tests for retract and localCoordinates
} }
/* ************************************************************************* */ //******************************************************************************
TEST( Similarity3, retract_first_order) TEST( Similarity3, retract_first_order) {
{
Similarity3 id; Similarity3 id;
Vector v = zero(7); Vector v = zero(7);
v(0) = 0.3; v(0) = 0.3;
EXPECT(assert_equal(Similarity3(R, Point3(), 1), id.retract(v),1e-2)); EXPECT(assert_equal(Similarity3(R, Point3(), 1), id.retract(v), 1e-2));
v(3)=0.2;v(4)=0.7;v(5)=-2; // v(3) = 0.2;
EXPECT(assert_equal(Similarity3(R, P, 1),id.retract(v),1e-2)); // v(4) = 0.7;
// v(5) = -2;
// EXPECT(assert_equal(Similarity3(R, P, 1), id.retract(v), 1e-2));
} }
/* ************************************************************************* */ //******************************************************************************
TEST(Similarity3, localCoordinates_first_order) TEST(Similarity3, localCoordinates_first_order) {
{ Vector d12 = repeat(7, 0.1);
Vector d12 = repeat(7,0.1);
d12(6) = 1.0; d12(6) = 1.0;
Similarity3 t1 = T, t2 = t1.retract(d12); Similarity3 t1 = T1, t2 = t1.retract(d12);
EXPECT(assert_equal(d12, t1.localCoordinates(t2))); EXPECT(assert_equal(d12, t1.localCoordinates(t2)));
} }
/* ************************************************************************* */ //******************************************************************************
TEST(Similarity3, manifold_first_order) TEST(Similarity3, manifold_first_order) {
{ Similarity3 t1 = T1;
Similarity3 t1 = T;
Similarity3 t2 = T3; Similarity3 t2 = T3;
Similarity3 origin; Similarity3 origin;
Vector d12 = t1.localCoordinates(t2); Vector d12 = t1.localCoordinates(t2);
@ -166,48 +183,137 @@ TEST(Similarity3, manifold_first_order)
EXPECT(assert_equal(t1, t2.retract(d21))); EXPECT(assert_equal(t1, t2.retract(d21)));
} }
//******************************************************************************
// Return as a 4*4 Matrix
TEST(Similarity3, Matrix) {
Matrix4 expected;
expected <<
1, 0, 0, 1,
0, 1, 0, 1,
0, 0, 1, 0,
0, 0, 0, 0.5;
Matrix4 actual = T6.matrix();
EXPECT(assert_equal(expected, actual));
}
//*****************************************************************************
// Exponential and log maps
TEST(Similarity3, ExpLogMap) {
Vector7 delta;
delta << 0.1,0.2,0.3,0.4,0.5,0.6,0.7;
Vector7 actual = Similarity3::Logmap(Similarity3::Expmap(delta));
EXPECT(assert_equal(delta, actual));
Vector7 zeros;
zeros << 0,0,0,0,0,0,0;
Vector7 logIdentity = Similarity3::Logmap(Similarity3::identity());
EXPECT(assert_equal(zeros, logIdentity));
Similarity3 expZero = Similarity3::Expmap(zeros);
Similarity3 ident = Similarity3::identity();
EXPECT(assert_equal(expZero, ident));
// Compare to matrix exponential, using expm in Lie.h
EXPECT(assert_equal(expm<Similarity3>(delta), Similarity3::Expmap(delta), 1e-3));
}
//******************************************************************************
// Group action on Point3 (with simpler transform)
TEST(Similarity3, GroupAction) {
EXPECT(assert_equal(Point3(2, 2, 0), T6 * Point3(0, 0, 0)));
EXPECT(assert_equal(Point3(4, 2, 0), T6 * Point3(1, 0, 0)));
// Test group action on R^4 via matrix representation
Vector4 qh;
qh << 1, 0, 0, 1;
Vector4 ph;
ph << 2, 1, 0, 0.5; // equivalent to Point3(4, 2, 0)
EXPECT(assert_equal((Vector )ph, T6.matrix() * qh));
// Test some more...
Point3 pa = Point3(1, 0, 0);
Similarity3 Ta(Rot3(), Point3(1, 2, 3), 1.0);
Similarity3 Tb(Rot3(), Point3(1, 2, 3), 2.0);
EXPECT(assert_equal(Point3(2, 2, 3), Ta.transform_from(pa)));
EXPECT(assert_equal(Point3(4, 4, 6), Tb.transform_from(pa)));
Similarity3 Tc(Rot3::Rz(M_PI/2.0), Point3(1, 2, 3), 1.0);
Similarity3 Td(Rot3::Rz(M_PI/2.0), Point3(1, 2, 3), 2.0);
EXPECT(assert_equal(Point3(1, 3, 3), Tc.transform_from(pa)));
EXPECT(assert_equal(Point3(2, 6, 6), Td.transform_from(pa)));
// Test derivative
boost::function<Point3(Similarity3, Point3)> f = boost::bind(
&Similarity3::transform_from, _1, _2, boost::none, boost::none);
Point3 q(1, 2, 3);
for (const auto T : { T1, T2, T3, T4, T5, T6 }) {
Point3 q(1, 0, 0);
Matrix H1 = numericalDerivative21<Point3, Similarity3, Point3>(f, T1, q);
Matrix H2 = numericalDerivative22<Point3, Similarity3, Point3>(f, T1, q);
Matrix actualH1, actualH2;
T1.transform_from(q, actualH1, actualH2);
EXPECT(assert_equal(H1, actualH1));
EXPECT(assert_equal(H2, actualH2));
}
}
//******************************************************************************
// Test very simple prior optimization example
TEST(Similarity3, Optimization) { TEST(Similarity3, Optimization) {
// Create a PriorFactor with a Sim3 prior
Similarity3 prior = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4); Similarity3 prior = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4);
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1); noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1);
Symbol key('x',1); Symbol key('x', 1);
PriorFactor<Similarity3> factor(key, prior, model); PriorFactor<Similarity3> factor(key, prior, model);
// Create graph
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.push_back(factor); graph.push_back(factor);
// Create initial estimate with identity transform
Values initial; Values initial;
initial.insert<Similarity3>(key, Similarity3()); initial.insert<Similarity3>(key, Similarity3());
// Optimize
Values result; Values result;
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
params.setVerbosityLM("TRYCONFIG"); params.setVerbosityLM("TRYCONFIG");
result = LevenbergMarquardtOptimizer(graph, initial).optimize(); result = LevenbergMarquardtOptimizer(graph, initial).optimize();
// After optimization, result should be prior
EXPECT(assert_equal(prior, result.at<Similarity3>(key), 1e-4)); EXPECT(assert_equal(prior, result.at<Similarity3>(key), 1e-4));
} }
//******************************************************************************
// Test optimization with both Prior and BetweenFactors
TEST(Similarity3, Optimization2) { TEST(Similarity3, Optimization2) {
Similarity3 prior = Similarity3(); Similarity3 prior = Similarity3();
Similarity3 m1 = Similarity3(Rot3::Ypr(M_PI/4.0, 0, 0), Point3(2.0, 0, 0), 1.0); Similarity3 m1 = Similarity3(Rot3::Ypr(M_PI / 4.0, 0, 0), Point3(2.0, 0, 0),
Similarity3 m2 = Similarity3(Rot3::Ypr(M_PI/2.0, 0, 0), Point3(sqrt(8)*0.9, 0, 0), 1.0); 1.0);
Similarity3 m3 = Similarity3(Rot3::Ypr(3*M_PI/4.0, 0, 0), Point3(sqrt(32)*0.8, 0, 0), 1.0); Similarity3 m2 = Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0),
Similarity3 m4 = Similarity3(Rot3::Ypr(M_PI/2.0, 0, 0), Point3(6*0.7, 0, 0), 1.0); Point3(sqrt(8) * 0.9, 0, 0), 1.0);
Similarity3 m3 = Similarity3(Rot3::Ypr(3 * M_PI / 4.0, 0, 0),
Point3(sqrt(32) * 0.8, 0, 0), 1.0);
Similarity3 m4 = Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0),
Point3(6 * 0.7, 0, 0), 1.0);
Similarity3 loop = Similarity3(1.42); Similarity3 loop = Similarity3(1.42);
//prior.print("Goal Transform"); //prior.print("Goal Transform");
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 0.01); noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7,
0.01);
SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas( SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas(
(Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished()); (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished());
SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas( SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas(
(Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished()); (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished());
PriorFactor<Similarity3> factor(X(1), prior, model); PriorFactor<Similarity3> factor(X(1), prior, model); // Prior !
BetweenFactor<Similarity3> b1(X(1), X(2), m1, betweenNoise); BetweenFactor<Similarity3> b1(X(1), X(2), m1, betweenNoise);
BetweenFactor<Similarity3> b2(X(2), X(3), m2, betweenNoise); BetweenFactor<Similarity3> b2(X(2), X(3), m2, betweenNoise);
BetweenFactor<Similarity3> b3(X(3), X(4), m3, betweenNoise); BetweenFactor<Similarity3> b3(X(3), X(4), m3, betweenNoise);
BetweenFactor<Similarity3> b4(X(4), X(5), m4, betweenNoise); BetweenFactor<Similarity3> b4(X(4), X(5), m4, betweenNoise);
BetweenFactor<Similarity3> lc(X(5), X(1), loop, betweenNoise2); BetweenFactor<Similarity3> lc(X(5), X(1), loop, betweenNoise2);
// Create graph
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.push_back(factor); graph.push_back(factor);
graph.push_back(b1); graph.push_back(b1);
@ -217,13 +323,16 @@ TEST(Similarity3, Optimization2) {
graph.push_back(lc); graph.push_back(lc);
//graph.print("Full Graph\n"); //graph.print("Full Graph\n");
Values initial; Values initial;
initial.insert<Similarity3>(X(1), Similarity3()); initial.insert<Similarity3>(X(1), Similarity3());
initial.insert<Similarity3>(X(2), Similarity3(Rot3::Ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.1)); initial.insert<Similarity3>(X(2),
initial.insert<Similarity3>(X(3), Similarity3(Rot3::Ypr(2.0*M_PI/2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2)); Similarity3(Rot3::Ypr(M_PI / 2.0, 0, 0), Point3(1, 0, 0), 1.1));
initial.insert<Similarity3>(X(4), Similarity3(Rot3::Ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.3)); initial.insert<Similarity3>(X(3),
initial.insert<Similarity3>(X(5), Similarity3(Rot3::Ypr(4.0*M_PI/2.0, 0, 0), Point3(0, 0, 0), 1.0)); Similarity3(Rot3::Ypr(2.0 * M_PI / 2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2));
initial.insert<Similarity3>(X(4),
Similarity3(Rot3::Ypr(3.0 * M_PI / 2.0, 0, 0), Point3(0, 1, 0), 1.3));
initial.insert<Similarity3>(X(5),
Similarity3(Rot3::Ypr(4.0 * M_PI / 2.0, 0, 0), Point3(0, 0, 0), 1.0));
//initial.print("Initial Estimate\n"); //initial.print("Initial Estimate\n");
@ -247,6 +356,58 @@ TEST(Similarity3, Optimization2) {
EXPECT(assert_equal(expected, result.at<Similarity3>(X(5)), 0.4)); EXPECT(assert_equal(expected, result.at<Similarity3>(X(5)), 0.4));
} }
//******************************************************************************
// Align points (p,q) assuming that p = T*q + noise
TEST(Similarity3, AlignScaledPointClouds) {
// Create ground truth
Point3 q1(0, 0, 0), q2(1, 0, 0), q3(0, 1, 0);
// Create transformed cloud (noiseless)
// Point3 p1 = T4 * q1, p2 = T4 * q2, p3 = T4 * q3;
// Create an unknown expression
Expression<Similarity3> unknownT(0); // use key 0
// Create constant expressions for the ground truth points
Expression<Point3> q1_(q1), q2_(q2), q3_(q3);
// Create prediction expressions
Expression<Point3> predict1(unknownT, &Similarity3::transform_from, q1_);
Expression<Point3> predict2(unknownT, &Similarity3::transform_from, q2_);
Expression<Point3> predict3(unknownT, &Similarity3::transform_from, q3_);
//// Create Expression factor graph
// ExpressionFactorGraph graph;
// graph.addExpressionFactor(predict1, p1, R); // |T*q1 - p1|
// graph.addExpressionFactor(predict2, p2, R); // |T*q2 - p2|
// graph.addExpressionFactor(predict3, p3, R); // |T*q3 - p3|
}
//******************************************************************************
TEST(Similarity3 , Invariants) {
Similarity3 id;
EXPECT(check_group_invariants(id,id));
EXPECT(check_group_invariants(id,T3));
EXPECT(check_group_invariants(T2,id));
EXPECT(check_group_invariants(T2,T3));
EXPECT(check_manifold_invariants(id,id));
EXPECT(check_manifold_invariants(id,T3));
EXPECT(check_manifold_invariants(T2,id));
EXPECT(check_manifold_invariants(T2,T3));
}
//******************************************************************************
TEST(Similarity3 , LieGroupDerivatives) {
Similarity3 id;
CHECK_LIE_GROUP_DERIVATIVES(id,id);
CHECK_LIE_GROUP_DERIVATIVES(id,T2);
CHECK_LIE_GROUP_DERIVATIVES(T2,id);
CHECK_LIE_GROUP_DERIVATIVES(T2,T3);
}
//****************************************************************************** //******************************************************************************
int main() { int main() {
TestResult tr; TestResult tr;