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

Make the code prettier.

release/4.3a0
Paul Drews 2015-02-26 15:10:59 -05:00
parent 5c40d62b56
commit 6faa881de6
3 changed files with 99 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
gtsam_unstable/geometry/Similarity3.cpp
View File

@ -12,45 +12,33 @@
/**
* @file Similarity3.cpp
* @brief Implementation of Similarity3 transform
* @author Paul Drews
*/
#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>
#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(const Matrix3& R, const Vector3& t, double s) :
R_(R), t_(t), s_(s) {
}
Similarity3::Similarity3() :
R_(), t_(), s_(1){
R_(), t_(), s_(1){
}
/// Construct pure scaling
Similarity3::Similarity3(double s) {
s_ = s;
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(const Rotation& R, const Translation& t, double s) :
R_ (R), t_ (t), s_ (s) {
}
Similarity3::operator Pose3() const {
@ -58,30 +46,26 @@ Similarity3::operator Pose3() const {
}
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();
}
//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_);
}
/// 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);
return R_.equals(sim.R_, tol) && t_.equals(sim.t_, tol)
&& s_ < (sim.s_+tol) && s_ > (sim.s_-tol);
}
Point3 Similarity3::transform_from(const Point3& p) const {
Similarity3::Translation Similarity3::transform_from(const Translation& p) const {
return R_ * (s_ * p) + t_;
}
@ -94,14 +78,13 @@ Matrix7 Similarity3::AdjointMap() const{
return adj;
}
/** syntactic sugar for transform_from */
inline Point3 Similarity3::operator*(const Point3& p) const {
inline Similarity3::Translation Similarity3::operator*(const Translation& p) const {
return transform_from(p);
}
Similarity3 Similarity3::inverse() const {
Rot3 Rt = R_.inverse();
Point3 sRt = R_.inverse() * (-s_ * t_);
Rotation Rt = R_.inverse();
Translation sRt = R_.inverse() * (-s_ * t_);
return Similarity3(Rt, sRt, 1.0/s_);
}
@ -117,35 +100,18 @@ void Similarity3::print(const std::string& s) const {
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.
Rotation 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
Translation(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
Rotation 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();

106
gtsam_unstable/geometry/Similarity3.h
View File

@ -12,19 +12,21 @@
/**
* @file Similarity3.h
* @brief Implementation of Similarity3 transform
* @author Paul Drews
*/
#ifndef GTSAM_UNSTABLE_GEOMETRY_SIMILARITY3_H_
#define GTSAM_UNSTABLE_GEOMETRY_SIMILARITY3_H_
#pragma once
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/Manifold.h>
namespace gtsam {
// Forward declarations
class Pose3;
/**
* 3D similarity transform
*/
@ -35,55 +37,77 @@ class Similarity3: public LieGroup<Similarity3, 7> {
typedef Point3 Translation;
private:
Rot3 R_;
Point3 t_;
Rotation R_;
Translation t_;
double s_;
public:
/// @name Constructors
/// @{
Similarity3();
/// Construct pure scaling
Similarity3(double s);
/// Construct from GTSAM types
Similarity3(const Rot3& R, const Point3& t, double s);
Similarity3(const Rotation& R, const Translation& t, double s);
/// Construct from Eigen types
Similarity3(const Matrix3& R, const Vector3& t, double s);
/// Convert to a rigid body pose
operator Pose3() const;
/// 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;
/// @}
/// @name Testable
/// @{
/// Compare with tolerance
bool equals(const Similarity3& sim, double tol) const;
Point3 transform_from(const Point3& p) const;
/// Compare with standard tolerance
bool operator==(const Similarity3& other) const;
Matrix7 AdjointMap() const;
/// Print with optional string
void print(const std::string& s) const;
/// @}
/// @name Group
/// @{
/// Return an identity transform
static Similarity3 identity();
/// Return the inverse
Similarity3 inverse() const;
Translation transform_from(const Translation& p) const;
/** syntactic sugar for transform_from */
inline Point3 operator*(const Point3& p) const;
Similarity3 inverse() const;
inline Translation operator*(const Translation& p) const;
Similarity3 operator*(const Similarity3& T) const;
void print(const std::string& s) const;
/// @}
/// @name Standard interface
/// @{
/// Return a GTSAM rotation
const Rotation& rotation() const {
return R_;
};
/// Return a GTSAM translation
const Translation& translation() const {
return t_;
};
/// Return the scale
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() {
@ -95,14 +119,9 @@ public:
return 7;
};
/// Return the rotation matrix
Rot3 rotation() const;
/// Return the translation
Point3 translation() const;
/// Return the scale
double scale() const;
/// @}
/// @name Chart
/// @{
/// Update Similarity transform via 7-dim vector in tangent space
struct ChartAtOrigin {
@ -112,11 +131,20 @@ public:
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<>
struct traits<Similarity3> : public internal::LieGroupTraits<Similarity3> {};
}
#endif /* GTSAM_UNSTABLE_GEOMETRY_SIMILARITY3_H_ */

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

@ -12,9 +12,13 @@
/**
* @file testSimilarity3.cpp
* @brief Unit tests for Similarity3 class
* @author Paul Drews
*/
#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/BetweenFactor.h>