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

Formatting, use Point3/Rot3, resolved link error of operator*(Point3)

release/4.3a0
dellaert 2015-03-02 20:09:44 -08:00
parent 1e58c0b0a2
commit fcd00450d3
2 changed files with 36 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
gtsam_unstable/geometry/Similarity3.cpp
View File

@ -17,36 +17,34 @@
#include <gtsam_unstable/geometry/Similarity3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Manifold.h>
namespace gtsam {
Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) :
R_(R), t_(t), s_(s) {
R_(R), t_(t), s_(s) {
}
Similarity3::Similarity3() :
R_(), t_(), s_(1){
R_(), t_(), s_(1) {
}
Similarity3::Similarity3(double s) :
s_ (s) {
s_(s) {
}
Similarity3::Similarity3(const Rotation& R, const Translation& t, double s) :
R_ (R), t_ (t), s_ (s) {
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_);
return Pose3(R_, s_ * t_);
}
Similarity3 Similarity3::identity() {
return Similarity3(); }
return Similarity3();
}
//Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) {
// return Vector7();
@ -61,35 +59,36 @@ bool Similarity3::operator==(const Similarity3& other) const {
}
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);
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 {
Point3 Similarity3::transform_from(const Point3& p) const {
return R_ * (s_ * p) + t_;
}
Matrix7 Similarity3::AdjointMap() const{
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;
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 {
Point3 Similarity3::operator*(const Point3& 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_);
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_);
return Similarity3(R_ * T.R_, ((1.0 / T.s_) * t_) + R_ * T.t_, s_ * T.s_);
}
void Similarity3::print(const std::string& s) const {
@ -100,18 +99,20 @@ void Similarity3::print(const std::string& s) const {
std::cout << "s: " << scale() << std::endl;
}
Similarity3 Similarity3::ChartAtOrigin::Retract(const Vector7& v, ChartJacobian H) {
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.
Rotation r; //Create a zero rotation to do our retraction.
Rot3 r; //Create a zero rotation to do our retraction.
return Similarity3( //
r.retract(v.head<3>()), // retract rotation using v[0,1,2]
Translation(v.segment<3>(3)), // Retract the translation
Point3(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) {
Rotation r; //Create a zero rotation to do the retraction
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();
@ -121,4 +122,3 @@ Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other, ChartJacobi
}
}

18
gtsam_unstable/geometry/Similarity3.h
View File

@ -32,13 +32,15 @@ class Pose3;
*/
class Similarity3: public LieGroup<Similarity3, 7> {
/** Pose Concept requirements */
/// @name Pose Concept
/// @{
typedef Rot3 Rotation;
typedef Point3 Translation;
/// @}
private:
Rotation R_;
Translation t_;
Rot3 R_;
Point3 t_;
double s_;
public:
@ -52,7 +54,7 @@ public:
Similarity3(double s);
/// 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
Similarity3(const Matrix3& R, const Vector3& t, double s);
@ -80,10 +82,10 @@ public:
/// Return the inverse
Similarity3 inverse() const;
Translation transform_from(const Translation& p) const;
Point3 transform_from(const Point3& p) const;
/** syntactic sugar for transform_from */
inline Translation operator*(const Translation& p) const;
Point3 operator*(const Point3& p) const;
Similarity3 operator*(const Similarity3& T) const;
@ -92,12 +94,12 @@ public:
/// @{
/// Return a GTSAM rotation
const Rotation& rotation() const {
const Rot3& rotation() const {
return R_;
};
/// Return a GTSAM translation
const Translation& translation() const {
const Point3& translation() const {
return t_;
};