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

Re-ordered methods in .h and .cpp to match them

release/4.3a0
dellaert 2015-03-03 07:42:31 -08:00
parent d8822e5b57
commit 74605df641
2 changed files with 77 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

89
gtsam_unstable/geometry/Similarity3.cpp
View File

@ -22,10 +22,6 @@
namespace gtsam {
Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) :
R_(R), t_(t), s_(s) {
}
Similarity3::Similarity3() :
R_(), t_(), s_(1) {
}
@ -38,24 +34,8 @@ Similarity3::Similarity3(const Rot3& R, const Point3& t, double s) :
R_(R), t_(t), s_(s) {
}
Similarity3::operator Pose3() const {
return Pose3(R_, s_ * t_);
}
Similarity3 Similarity3::identity() {
return Similarity3();
}
//Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) {
// return Vector7();
//}
//
//Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) {
// return Similarity3();
//}
bool Similarity3::operator==(const Similarity3& other) const {
return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_);
Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) :
R_(R), t_(t), s_(s) {
}
bool Similarity3::equals(const Similarity3& sim, double tol) const {
@ -63,6 +43,31 @@ bool Similarity3::equals(const Similarity3& sim, double tol) const {
&& s_ > (sim.s_ - tol);
}
bool Similarity3::operator==(const Similarity3& other) const {
return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.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;
}
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 {
if (H1) {
@ -78,11 +83,8 @@ Point3 Similarity3::transform_from(const Point3& p, //
// 0001 000 1 1 000 1 1
}
const Matrix4 Similarity3::matrix() const {
Matrix4 T;
T.topRows<3>() << s_ * R_.matrix(), t_.vector();
T.bottomRows<1>() << 0, 0, 0, 1;
return T;
Point3 Similarity3::operator*(const Point3& p) const {
return transform_from(p);
}
Matrix7 Similarity3::AdjointMap() const {
@ -96,26 +98,12 @@ Matrix7 Similarity3::AdjointMap() const {
return adj;
}
Point3 Similarity3::operator*(const Point3& p) const {
return transform_from(p);
Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) {
return Vector7();
}
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;
Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) {
return Similarity3();
}
Similarity3 Similarity3::ChartAtOrigin::Retract(const Vector7& v,
@ -139,5 +127,16 @@ Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other,
v[6] = other.s_ - 1.0;
return v;
}
const Matrix4 Similarity3::matrix() const {
Matrix4 T;
T.topRows<3>() << s_ * R_.matrix(), t_.vector();
T.bottomRows<1>() << 0, 0, 0, 1;
return T;
}
Similarity3::operator Pose3() const {
return Pose3(R_, s_ * t_);
}
}

61
gtsam_unstable/geometry/Similarity3.h
View File

@ -48,6 +48,7 @@ public:
/// @name Constructors
/// @{
/// Default constructor
Similarity3();
/// Construct pure scaling
@ -79,9 +80,16 @@ public:
/// Return an identity transform
static Similarity3 identity();
/// Composition
Similarity3 operator*(const Similarity3& T) const;
/// Return the inverse
Similarity3 inverse() const;
/// @}
/// @name Group action on Point3
/// @{
Point3 transform_from(const Point3& p, //
OptionalJacobian<3, 7> H1 = boost::none, //
OptionalJacobian<3, 3> H2 = boost::none) const;
@ -89,7 +97,31 @@ public:
/** syntactic sugar for transform_from */
Point3 operator*(const Point3& p) const;
Similarity3 operator*(const Similarity3& T) const;
/// @}
/// @name Lie Group
/// @{
/// Not currently implemented, required because this is a lie group
static Vector7 Logmap(const Similarity3& s, //
OptionalJacobian<7, 7> Hm = boost::none);
/// Not currently implemented, required because this is a lie group
static Similarity3 Expmap(const Vector7& v, //
OptionalJacobian<7, 7> Hm = boost::none);
/// 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);
};
/// Project from one tangent space to another
Matrix7 AdjointMap() const;
using LieGroup<Similarity3, 7>::inverse;
/// @}
/// @name Standard interface
@ -127,33 +159,6 @@ public:
}
/// @}
/// @name Chart
/// @{
/// 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);
};
/// Project from one tangent space to another
Matrix7 AdjointMap() const;
/// @}
/// @name Stubs
/// @{
/// Not currently implemented, required because this is a lie group
static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm =
boost::none);
static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm =
boost::none);
using LieGroup<Similarity3, 7>::inverse;
// version with derivative
};
template<>