Refactored into class and unit test
Paul Drews
parent
42d8e1fcb2
commit
f9ccd23d4a
|
|
@ -0,0 +1,154 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Similarity3.cpp
|
||||
* @brief Implementation of Similarity3 transform
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/base/Manifold.h>
|
||||
#include <gtsam_unstable/geometry/Similarity3.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) {
|
||||
R_ = R;
|
||||
t_ = t;
|
||||
s_ = s;
|
||||
}
|
||||
|
||||
/// Return the translation
|
||||
const Vector3 Similarity3::t() const {
|
||||
return t_.vector();
|
||||
}
|
||||
|
||||
/// Return the rotation matrix
|
||||
const Matrix3 Similarity3::R() const {
|
||||
return R_.matrix();
|
||||
}
|
||||
|
||||
Similarity3::Similarity3() :
|
||||
R_(), t_(), s_(1){
|
||||
}
|
||||
|
||||
/// Construct pure scaling
|
||||
Similarity3::Similarity3(double s) {
|
||||
s_ = s;
|
||||
}
|
||||
|
||||
/// Construct from GTSAM types
|
||||
Similarity3::Similarity3(const Rot3& R, const Point3& t, double s) {
|
||||
R_ = R;
|
||||
t_ = t;
|
||||
s_ = s;
|
||||
}
|
||||
|
||||
Similarity3 Similarity3::identity() {
|
||||
std::cout << "Identity!" << std::endl;
|
||||
return Similarity3(); }
|
||||
|
||||
Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) {
|
||||
std::cout << "Logmap!" << std::endl;
|
||||
return Vector7();
|
||||
}
|
||||
|
||||
Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) {
|
||||
std::cout << "Expmap!" << std::endl;
|
||||
return Similarity3();
|
||||
}
|
||||
|
||||
bool Similarity3::operator==(const Similarity3& other) const {
|
||||
return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_);
|
||||
}
|
||||
|
||||
/// Compare with tolerance
|
||||
bool Similarity3::equals(const Similarity3& sim, double tol) const {
|
||||
return rotation().equals(sim.rotation(), tol) && translation().equals(sim.translation(), tol)
|
||||
&& scale() < (sim.scale()+tol) && scale() > (sim.scale()-tol);
|
||||
}
|
||||
|
||||
Point3 Similarity3::transform_from(const Point3& 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;
|
||||
}
|
||||
|
||||
/** syntactic sugar for transform_from */
|
||||
inline Point3 Similarity3::operator*(const Point3& p) const {
|
||||
return transform_from(p);
|
||||
}
|
||||
|
||||
Similarity3 Similarity3::inverse() const {
|
||||
Rot3 Rt = R_.inverse();
|
||||
Point3 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 {
|
||||
std::cout << std::endl;
|
||||
std::cout << s;
|
||||
rotation().print("R:\n");
|
||||
translation().print("t: ");
|
||||
std::cout << "s: " << scale() << std::endl;
|
||||
}
|
||||
|
||||
/// Return the rotation matrix
|
||||
Rot3 Similarity3::rotation() const {
|
||||
return R_;
|
||||
}
|
||||
|
||||
/// Return the translation
|
||||
Point3 Similarity3::translation() const {
|
||||
return t_;
|
||||
}
|
||||
|
||||
/// Return the scale
|
||||
double Similarity3::scale() const {
|
||||
return s_;
|
||||
}
|
||||
|
||||
/// Update Similarity transform via 7-dim vector in tangent space
|
||||
Similarity3 Similarity3::ChartAtOrigin::Retract(const Vector7& v, ChartJacobian H) {
|
||||
// Will retracting or localCoordinating R work if R is not a unit rotation?
|
||||
// Also, how do we actually get s out? Seems like we need to store it somewhere.
|
||||
Rot3 r; //Create a zero rotation to do our retraction.
|
||||
return Similarity3( //
|
||||
r.retract(v.head<3>()), // retract rotation using v[0,1,2]
|
||||
Point3(v.segment<3>(3)), // Retract the translation
|
||||
1.0 + v[6]); //finally, update scale using v[6]
|
||||
}
|
||||
|
||||
/// 7-dimensional vector v in tangent space that makes other = this->retract(v)
|
||||
Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other, ChartJacobian H) {
|
||||
Rot3 r; //Create a zero rotation to do the retraction
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,118 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Similarity3.h
|
||||
* @brief Implementation of Similarity3 transform
|
||||
*/
|
||||
|
||||
#ifndef GTSAM_UNSTABLE_GEOMETRY_SIMILARITY3_H_
|
||||
#define GTSAM_UNSTABLE_GEOMETRY_SIMILARITY3_H_
|
||||
|
||||
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/base/Manifold.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* 3D similarity transform
|
||||
*/
|
||||
class Similarity3: public LieGroup<Similarity3, 7> {
|
||||
|
||||
/** Pose Concept requirements */
|
||||
typedef Rot3 Rotation;
|
||||
typedef Point3 Translation;
|
||||
|
||||
private:
|
||||
Rot3 R_;
|
||||
Point3 t_;
|
||||
double s_;
|
||||
|
||||
public:
|
||||
|
||||
Similarity3();
|
||||
|
||||
/// Construct pure scaling
|
||||
Similarity3(double s);
|
||||
|
||||
/// Construct from GTSAM types
|
||||
Similarity3(const Rot3& R, const Point3& t, double s);
|
||||
|
||||
/// Construct from Eigen types
|
||||
Similarity3(const Matrix3& R, const Vector3& t, double s);
|
||||
|
||||
/// Return the translation
|
||||
const Vector3 t() const;
|
||||
|
||||
/// Return the rotation matrix
|
||||
const Matrix3 R() const;
|
||||
|
||||
static Similarity3 identity();
|
||||
|
||||
static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm = boost::none);
|
||||
|
||||
static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm = boost::none);
|
||||
|
||||
bool operator==(const Similarity3& other) const;
|
||||
|
||||
/// Compare with tolerance
|
||||
bool equals(const Similarity3& sim, double tol) const;
|
||||
|
||||
Point3 transform_from(const Point3& p) const;
|
||||
|
||||
Matrix7 AdjointMap() const;
|
||||
|
||||
/** syntactic sugar for transform_from */
|
||||
inline Point3 operator*(const Point3& p) const;
|
||||
|
||||
Similarity3 inverse() const;
|
||||
|
||||
Similarity3 operator*(const Similarity3& T) const;
|
||||
|
||||
void print(const std::string& s) 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;
|
||||
};
|
||||
|
||||
/// Return the rotation matrix
|
||||
Rot3 rotation() const;
|
||||
|
||||
/// Return the translation
|
||||
Point3 translation() const;
|
||||
|
||||
/// Return the scale
|
||||
double scale() const;
|
||||
|
||||
/// Update Similarity transform via 7-dim vector in tangent space
|
||||
struct ChartAtOrigin {
|
||||
static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none);
|
||||
|
||||
/// 7-dimensional vector v in tangent space that makes other = this->retract(v)
|
||||
static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none);
|
||||
};
|
||||
|
||||
using LieGroup<Similarity3, 7>::inverse; // version with derivative
|
||||
};
|
||||
|
||||
template<>
|
||||
struct traits<Similarity3> : public internal::LieGroupTraits<Similarity3> {};
|
||||
}
|
||||
|
||||
#endif /* GTSAM_UNSTABLE_GEOMETRY_SIMILARITY3_H_ */
|
||||
|
|
@ -14,202 +14,7 @@
|
|||
* @brief Unit tests for Similarity3 class
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/base/Manifold.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* 3D similarity transform
|
||||
*/
|
||||
class Similarity3: public LieGroup<Similarity3, 7> {
|
||||
|
||||
/** Pose Concept requirements */
|
||||
typedef Rot3 Rotation;
|
||||
typedef Point3 Translation;
|
||||
|
||||
private:
|
||||
Rot3 R_;
|
||||
Point3 t_;
|
||||
double s_;
|
||||
|
||||
public:
|
||||
/// Construct from Eigen types
|
||||
Similarity3(const Matrix3& R, const Vector3& t, double s) {
|
||||
R_ = R;
|
||||
t_ = t;
|
||||
s_ = s;
|
||||
}
|
||||
|
||||
/// Return the translation
|
||||
const Vector3 t() const {
|
||||
return t_.vector();
|
||||
}
|
||||
|
||||
/// Return the rotation matrix
|
||||
const Matrix3 R() const {
|
||||
return R_.matrix();
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
Similarity3() :
|
||||
R_(), t_(), s_(1){
|
||||
}
|
||||
|
||||
/// Construct pure scaling
|
||||
Similarity3(double s) {
|
||||
s_ = s;
|
||||
}
|
||||
|
||||
/// Construct from GTSAM types
|
||||
Similarity3(const Rot3& R, const Point3& t, double s) {
|
||||
R_ = R;
|
||||
t_ = t;
|
||||
s_ = s;
|
||||
}
|
||||
|
||||
static Similarity3 identity() {
|
||||
std::cout << "Identity!" << std::endl;
|
||||
return Similarity3(); }
|
||||
|
||||
static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm = boost::none) {
|
||||
std::cout << "Logmap!" << std::endl;
|
||||
return Vector7();
|
||||
}
|
||||
|
||||
static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm = boost::none) {
|
||||
std::cout << "Expmap!" << std::endl;
|
||||
return Similarity3();
|
||||
}
|
||||
|
||||
bool operator==(const Similarity3& other) const {
|
||||
return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_);
|
||||
}
|
||||
|
||||
/// Compare with tolerance
|
||||
bool equals(const Similarity3& sim, double tol) const {
|
||||
return rotation().equals(sim.rotation(), tol) && translation().equals(sim.translation(), tol)
|
||||
&& scale() < (sim.scale()+tol) && scale() > (sim.scale()-tol);
|
||||
}
|
||||
|
||||
Point3 transform_from(const Point3& p) const {
|
||||
/*if (Dpose) {
|
||||
const Matrix3 R = R_.matrix();
|
||||
Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
(*Dpose) << DR, R;
|
||||
}
|
||||
if (Dpoint)
|
||||
*Dpoint = R_.matrix();*/
|
||||
return R_ * (s_ * p) + t_;
|
||||
}
|
||||
|
||||
Matrix7 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;
|
||||
}
|
||||
|
||||
/** syntactic sugar for transform_from */
|
||||
inline Point3 operator*(const Point3& p) const {
|
||||
return transform_from(p);
|
||||
}
|
||||
|
||||
Similarity3 inverse() const {
|
||||
Rot3 Rt = R_.inverse();
|
||||
Point3 sRt = R_.inverse() * (-s_ * t_);
|
||||
return Similarity3(Rt, sRt, 1.0/s_);
|
||||
}
|
||||
|
||||
Similarity3 operator*(const Similarity3& T) const {
|
||||
return Similarity3(R_ * T.R_, ((1.0/T.s_)*t_) + R_ * T.t_, s_*T.s_);
|
||||
}
|
||||
|
||||
void print(const std::string& s) const {
|
||||
std::cout << std::endl;
|
||||
std::cout << s;
|
||||
rotation().print("R:\n");
|
||||
translation().print("t: ");
|
||||
std::cout << "s: " << scale() << std::endl;
|
||||
}
|
||||
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/// 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;
|
||||
}
|
||||
|
||||
/// Return the rotation matrix
|
||||
Rot3 rotation() const {
|
||||
return R_;
|
||||
}
|
||||
|
||||
/// Return the translation
|
||||
Point3 translation() const {
|
||||
return t_;
|
||||
}
|
||||
|
||||
/// Return the scale
|
||||
double scale() const {
|
||||
return s_;
|
||||
}
|
||||
|
||||
/// Update Similarity transform via 7-dim vector in tangent space
|
||||
struct ChartAtOrigin {
|
||||
static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none) {
|
||||
|
||||
// Will retracting or localCoordinating R work if R is not a unit rotation?
|
||||
// Also, how do we actually get s out? Seems like we need to store it somewhere.
|
||||
Rot3 r; //Create a zero rotation to do our retraction.
|
||||
return Similarity3( //
|
||||
r.retract(v.head<3>()), // retract rotation using v[0,1,2]
|
||||
Point3(v.segment<3>(3)), // Retract the translation
|
||||
1.0 + v[6]); //finally, update scale using v[6]
|
||||
}
|
||||
|
||||
/// 7-dimensional vector v in tangent space that makes other = this->retract(v)
|
||||
static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none) {
|
||||
Rot3 r; //Create a zero rotation to do the retraction
|
||||
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;
|
||||
}
|
||||
};
|
||||
|
||||
using LieGroup<Similarity3, 7>::inverse; // version with derivative
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Lie Group
|
||||
/// @{
|
||||
|
||||
// compose T1*T2
|
||||
// between T2*inverse(T1)
|
||||
// identity I4
|
||||
// inverse inverse(T)
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
template<>
|
||||
struct traits<Similarity3> : public internal::LieGroupTraits<Similarity3> {};
|
||||
}
|
||||
|
||||
#include <gtsam_unstable/geometry/Similarity3.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
|
|
@ -359,24 +164,20 @@ TEST(Similarity3, manifold_first_order)
|
|||
|
||||
TEST(Similarity3, Optimization) {
|
||||
Similarity3 prior = Similarity3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4);
|
||||
//prior.print("Goal Transform");
|
||||
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1);
|
||||
Symbol key('x',1);
|
||||
PriorFactor<Similarity3> factor(key, prior, model);
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(factor);
|
||||
//graph.print("Full Graph");
|
||||
|
||||
Values initial;
|
||||
initial.insert<Similarity3>(key, Similarity3());
|
||||
//initial.print("Initial Estimate");
|
||||
|
||||
Values result;
|
||||
LevenbergMarquardtParams params;
|
||||
params.setVerbosityLM("TRYCONFIG");
|
||||
result = LevenbergMarquardtOptimizer(graph, initial).optimize();
|
||||
//result.print("Optimized Estimate");
|
||||
EXPECT(assert_equal(prior, result.at<Similarity3>(key), 1e-4));
|
||||
}
|
||||
|
||||
|
|
@ -390,12 +191,14 @@ TEST(Similarity3, Optimization2) {
|
|||
//prior.print("Goal Transform");
|
||||
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1);
|
||||
SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas(
|
||||
(Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.010).finished());
|
||||
(Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1).finished());
|
||||
SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas(
|
||||
(Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.01).finished());
|
||||
PriorFactor<Similarity3> factor(X(1), prior, model);
|
||||
BetweenFactor<Similarity3> b1(X(1), X(2), m1, betweenNoise);
|
||||
BetweenFactor<Similarity3> b2(X(2), X(3), m2, betweenNoise);
|
||||
BetweenFactor<Similarity3> b3(X(3), X(4), m3, betweenNoise);
|
||||
BetweenFactor<Similarity3> b4(X(4), X(1), m4, betweenNoise);
|
||||
BetweenFactor<Similarity3> b4(X(4), X(1), m4, betweenNoise2);
|
||||
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
|
|
@ -409,9 +212,9 @@ TEST(Similarity3, Optimization2) {
|
|||
|
||||
Values initial;
|
||||
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.0));
|
||||
initial.insert<Similarity3>(X(3), Similarity3(Rot3::ypr(2.0*M_PI/2.0, 0, 0), Point3(1, 1.5, 0), 1.0));
|
||||
initial.insert<Similarity3>(X(4), Similarity3(Rot3::ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.0));
|
||||
initial.insert<Similarity3>(X(2), Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.1));
|
||||
initial.insert<Similarity3>(X(3), 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.print("Initial Estimate");
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue