Reformatted with new style file, renamed some derivatives to Dcal, Dpose, Dpoint etc.
Frank Dellaert
parent
ca9caf6a66
commit
752a9877c5
|
|
@ -22,61 +22,72 @@
|
|||
#include <iostream>
|
||||
|
||||
namespace gtsam {
|
||||
using namespace std;
|
||||
using namespace std;
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3_S2::Cal3_S2(double fov, int w, int h) :
|
||||
/* ************************************************************************* */
|
||||
Cal3_S2::Cal3_S2(double fov, int w, int h) :
|
||||
s_(0), u0_((double) w / 2.0), v0_((double) h / 2.0) {
|
||||
double a = fov * M_PI / 360.0; // fov/2 in radians
|
||||
fx_ = (double)w / (2.0*tan(a)); // old formula: fx_ = (double) w * tan(a);
|
||||
fy_ = fx_;
|
||||
}
|
||||
double a = fov * M_PI / 360.0; // fov/2 in radians
|
||||
fx_ = (double) w / (2.0 * tan(a)); // old formula: fx_ = (double) w * tan(a);
|
||||
fy_ = fx_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3_S2::Cal3_S2(const std::string &path) :
|
||||
/* ************************************************************************* */
|
||||
Cal3_S2::Cal3_S2(const std::string &path) :
|
||||
fx_(320), fy_(320), s_(0), u0_(320), v0_(140) {
|
||||
|
||||
char buffer[200];
|
||||
buffer[0] = 0;
|
||||
sprintf(buffer, "%s/calibration_info.txt", path.c_str());
|
||||
std::ifstream infile(buffer, std::ios::in);
|
||||
char buffer[200];
|
||||
buffer[0] = 0;
|
||||
sprintf(buffer, "%s/calibration_info.txt", path.c_str());
|
||||
std::ifstream infile(buffer, std::ios::in);
|
||||
|
||||
if (infile)
|
||||
infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
|
||||
else {
|
||||
printf("Unable to load the calibration\n");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
infile.close();
|
||||
if (infile)
|
||||
infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
|
||||
else {
|
||||
printf("Unable to load the calibration\n");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Cal3_S2::print(const std::string& s) const {
|
||||
gtsam::print(matrix(), s);
|
||||
}
|
||||
infile.close();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
|
||||
if (fabs(fx_ - K.fx_) > tol) return false;
|
||||
if (fabs(fy_ - K.fy_) > tol) return false;
|
||||
if (fabs(s_ - K.s_) > tol) return false;
|
||||
if (fabs(u0_ - K.u0_) > tol) return false;
|
||||
if (fabs(v0_ - K.v0_) > tol) return false;
|
||||
return true;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
void Cal3_S2::print(const std::string& s) const {
|
||||
gtsam::print(matrix(), s);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
const double x = p.x(), y = p.y();
|
||||
if (H1)
|
||||
*H1 = Matrix_(2, 5,
|
||||
x, 0.0, y, 1.0, 0.0,
|
||||
0.0, y, 0.0, 0.0, 1.0);
|
||||
if (H2) *H2 = Matrix_(2, 2, fx_, s_, 0.000, fy_);
|
||||
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
|
||||
if (fabs(fx_ - K.fx_) > tol)
|
||||
return false;
|
||||
if (fabs(fy_ - K.fy_) > tol)
|
||||
return false;
|
||||
if (fabs(s_ - K.s_) > tol)
|
||||
return false;
|
||||
if (fabs(u0_ - K.u0_) > tol)
|
||||
return false;
|
||||
if (fabs(v0_ - K.v0_) > tol)
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
|
||||
boost::optional<Matrix&> Dp) const {
|
||||
const double x = p.x(), y = p.y();
|
||||
if (Dcal)
|
||||
*Dcal = Matrix_(2, 5, x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0);
|
||||
if (Dp)
|
||||
*Dp = Matrix_(2, 2, fx_, s_, 0.000, fy_);
|
||||
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3_S2::calibrate(const Point2& p) const {
|
||||
const double u = p.x(), v = p.y();
|
||||
return Point2((1 / fx_) * (u - u0_ - (s_ / fy_) * (v - v0_)),
|
||||
(1 / fy_) * (v - v0_));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -26,166 +26,179 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* @brief The most common 5DOF 3D->2D calibration
|
||||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT Cal3_S2 : public DerivedValue<Cal3_S2> {
|
||||
private:
|
||||
double fx_, fy_, s_, u0_, v0_;
|
||||
/**
|
||||
* @brief The most common 5DOF 3D->2D calibration
|
||||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT Cal3_S2: public DerivedValue<Cal3_S2> {
|
||||
private:
|
||||
double fx_, fy_, s_, u0_, v0_;
|
||||
|
||||
public:
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object
|
||||
typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/// Create a default calibration that leaves coordinates unchanged
|
||||
Cal3_S2() :
|
||||
/// Create a default calibration that leaves coordinates unchanged
|
||||
Cal3_S2() :
|
||||
fx_(1), fy_(1), s_(0), u0_(0), v0_(0) {
|
||||
}
|
||||
}
|
||||
|
||||
/// constructor from doubles
|
||||
Cal3_S2(double fx, double fy, double s, double u0, double v0) :
|
||||
/// constructor from doubles
|
||||
Cal3_S2(double fx, double fy, double s, double u0, double v0) :
|
||||
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {
|
||||
}
|
||||
}
|
||||
|
||||
/// constructor from vector
|
||||
Cal3_S2(const Vector &d): fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)){}
|
||||
/// constructor from vector
|
||||
Cal3_S2(const Vector &d) :
|
||||
fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {
|
||||
}
|
||||
|
||||
/**
|
||||
* Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect
|
||||
* @param fov field of view in degrees
|
||||
* @param w image width
|
||||
* @param h image height
|
||||
*/
|
||||
Cal3_S2(double fov, int w, int h);
|
||||
|
||||
/**
|
||||
* Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect
|
||||
* @param fov field of view in degrees
|
||||
* @param w image width
|
||||
* @param h image height
|
||||
*/
|
||||
Cal3_S2(double fov, int w, int h);
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
/// load calibration from location (default name is calibration_info.txt)
|
||||
Cal3_S2(const std::string &path);
|
||||
|
||||
/// load calibration from location (default name is calibration_info.txt)
|
||||
Cal3_S2(const std::string &path);
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
/// print with optional string
|
||||
void print(const std::string& s = "Cal3_S2") const;
|
||||
|
||||
/// print with optional string
|
||||
void print(const std::string& s = "Cal3_S2") const;
|
||||
/// Check if equal up to specified tolerance
|
||||
bool equals(const Cal3_S2& K, double tol = 10e-9) const;
|
||||
|
||||
/// Check if equal up to specified tolerance
|
||||
bool equals(const Cal3_S2& K, double tol = 10e-9) const;
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
/// focal length x
|
||||
inline double fx() const {
|
||||
return fx_;
|
||||
}
|
||||
|
||||
/// focal length x
|
||||
inline double fx() const { return fx_;}
|
||||
/// focal length y
|
||||
inline double fy() const {
|
||||
return fy_;
|
||||
}
|
||||
|
||||
/// focal length y
|
||||
inline double fy() const { return fy_;}
|
||||
/// skew
|
||||
inline double skew() const {
|
||||
return s_;
|
||||
}
|
||||
|
||||
/// skew
|
||||
inline double skew() const { return s_;}
|
||||
/// image center in x
|
||||
inline double px() const {
|
||||
return u0_;
|
||||
}
|
||||
|
||||
/// image center in x
|
||||
inline double px() const { return u0_;}
|
||||
/// image center in y
|
||||
inline double py() const {
|
||||
return v0_;
|
||||
}
|
||||
|
||||
/// image center in y
|
||||
inline double py() const { return v0_;}
|
||||
/// return the principal point
|
||||
Point2 principalPoint() const {
|
||||
return Point2(u0_, v0_);
|
||||
}
|
||||
|
||||
/// return the principal point
|
||||
Point2 principalPoint() const {
|
||||
return Point2(u0_, v0_);
|
||||
}
|
||||
/// vectorized form (column-wise)
|
||||
Vector vector() const {
|
||||
double r[] = { fx_, fy_, s_, u0_, v0_ };
|
||||
Vector v(5);
|
||||
std::copy(r, r + 5, v.data());
|
||||
return v;
|
||||
}
|
||||
|
||||
/// vectorized form (column-wise)
|
||||
Vector vector() const {
|
||||
double r[] = { fx_, fy_, s_, u0_, v0_ };
|
||||
Vector v(5);
|
||||
std::copy(r, r + 5, v.data());
|
||||
return v;
|
||||
}
|
||||
/// return calibration matrix K
|
||||
Matrix matrix() const {
|
||||
return Matrix_(3, 3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
|
||||
}
|
||||
|
||||
/// return calibration matrix K
|
||||
Matrix matrix() const {
|
||||
return Matrix_(3, 3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
|
||||
}
|
||||
/// return inverted calibration matrix inv(K)
|
||||
Matrix matrix_inverse() const {
|
||||
const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
|
||||
return Matrix_(3, 3, 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
|
||||
1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0);
|
||||
}
|
||||
|
||||
/// return inverted calibration matrix inv(K)
|
||||
Matrix matrix_inverse() const {
|
||||
const double fxy = fx_*fy_, sv0 = s_*v0_, fyu0 = fy_*u0_;
|
||||
return Matrix_(3, 3,
|
||||
1.0/fx_, -s_/fxy, (sv0-fyu0)/fxy,
|
||||
0.0, 1.0/fy_, -v0_/fy_,
|
||||
0.0, 0.0, 1.0);
|
||||
}
|
||||
/**
|
||||
* convert intrinsic coordinates xy to image coordinates uv
|
||||
* @param p point in intrinsic coordinates
|
||||
* @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
|
||||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
|
||||
* @return point in image coordinates
|
||||
*/
|
||||
Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal =
|
||||
boost::none, boost::optional<Matrix&> Dp = boost::none) const;
|
||||
|
||||
/**
|
||||
* convert intrinsic coordinates xy to image coordinates uv
|
||||
* with optional derivatives
|
||||
*/
|
||||
Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> H1 =
|
||||
boost::none, boost::optional<Matrix&> H2 = boost::none) const;
|
||||
/**
|
||||
* convert image coordinates uv to intrinsic coordinates xy
|
||||
* @param p point in image coordinates
|
||||
* @return point in intrinsic coordinates
|
||||
*/
|
||||
Point2 calibrate(const Point2& p) const;
|
||||
|
||||
/// convert image coordinates uv to intrinsic coordinates xy
|
||||
Point2 calibrate(const Point2& p) const {
|
||||
const double u = p.x(), v = p.y();
|
||||
return Point2((1 / fx_) * (u - u0_ - (s_ / fy_) * (v - v0_)),
|
||||
(1 / fy_) * (v - v0_));
|
||||
}
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
/// return DOF, dimensionality of tangent space
|
||||
inline size_t dim() const {
|
||||
return 5;
|
||||
}
|
||||
|
||||
/// return DOF, dimensionality of tangent space
|
||||
inline size_t dim() const {
|
||||
return 5;
|
||||
}
|
||||
/// return DOF, dimensionality of tangent space
|
||||
static size_t Dim() {
|
||||
return 5;
|
||||
}
|
||||
|
||||
/// return DOF, dimensionality of tangent space
|
||||
static size_t Dim() {
|
||||
return 5;
|
||||
}
|
||||
/// Given 5-dim tangent vector, create new calibration
|
||||
inline Cal3_S2 retract(const Vector& d) const {
|
||||
return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4));
|
||||
}
|
||||
|
||||
/// Given 5-dim tangent vector, create new calibration
|
||||
inline Cal3_S2 retract(const Vector& d) const {
|
||||
return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4));
|
||||
}
|
||||
/// Unretraction for the calibration
|
||||
Vector localCoordinates(const Cal3_S2& T2) const {
|
||||
return vector() - T2.vector();
|
||||
}
|
||||
|
||||
/// Unretraction for the calibration
|
||||
Vector localCoordinates(const Cal3_S2& T2) const {
|
||||
return vector() - T2.vector();
|
||||
}
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
private:
|
||||
|
||||
private:
|
||||
/// Serialization function
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar
|
||||
& boost::serialization::make_nvp("Cal3_S2",
|
||||
boost::serialization::base_object<Value>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(fx_);
|
||||
ar & BOOST_SERIALIZATION_NVP(fy_);
|
||||
ar & BOOST_SERIALIZATION_NVP(s_);
|
||||
ar & BOOST_SERIALIZATION_NVP(u0_);
|
||||
ar & BOOST_SERIALIZATION_NVP(v0_);
|
||||
}
|
||||
|
||||
/// Serialization function
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar & boost::serialization::make_nvp("Cal3_S2",
|
||||
boost::serialization::base_object<Value>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(fx_);
|
||||
ar & BOOST_SERIALIZATION_NVP(fy_);
|
||||
ar & BOOST_SERIALIZATION_NVP(s_);
|
||||
ar & BOOST_SERIALIZATION_NVP(u0_);
|
||||
ar & BOOST_SERIALIZATION_NVP(v0_);
|
||||
}
|
||||
/// @}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
};
|
||||
|
||||
} // \ namespace gtsam
|
||||
|
|
|
|||
|
|
@ -26,333 +26,348 @@ using namespace std;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/** Explicit instantiation of base class to export members */
|
||||
INSTANTIATE_LIE(Pose3);
|
||||
/** Explicit instantiation of base class to export members */
|
||||
INSTANTIATE_LIE(Pose3);
|
||||
|
||||
/** instantiate concept checks */
|
||||
GTSAM_CONCEPT_POSE_INST(Pose3);
|
||||
/** instantiate concept checks */
|
||||
GTSAM_CONCEPT_POSE_INST(Pose3);
|
||||
|
||||
static const Matrix3 I3 = eye(3), Z3 = zeros(3, 3), _I3=-I3;
|
||||
static const Matrix6 I6 = eye(6);
|
||||
static const Matrix3 I3 = eye(3), Z3 = zeros(3, 3), _I3 = -I3;
|
||||
static const Matrix6 I6 = eye(6);
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3::Pose3(const Pose2& pose2) :
|
||||
R_(Rot3::rodriguez(0, 0, pose2.theta())),
|
||||
t_(Point3(pose2.x(), pose2.y(), 0)) {
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Pose3::Pose3(const Pose2& pose2) :
|
||||
R_(Rot3::rodriguez(0, 0, pose2.theta())), t_(
|
||||
Point3(pose2.x(), pose2.y(), 0)) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Calculate Adjoint map
|
||||
// Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
|
||||
// Experimental - unit tests of derivatives based on it do not check out yet
|
||||
Matrix6 Pose3::AdjointMap() const {
|
||||
const Matrix3 R = R_.matrix();
|
||||
const Vector3 t = t_.vector();
|
||||
Matrix3 A = skewSymmetric(t)*R;
|
||||
Matrix6 adj;
|
||||
adj << R, Z3, A, R;
|
||||
return adj;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
// Calculate Adjoint map
|
||||
// Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
|
||||
// Experimental - unit tests of derivatives based on it do not check out yet
|
||||
Matrix6 Pose3::AdjointMap() const {
|
||||
const Matrix3 R = R_.matrix();
|
||||
const Vector3 t = t_.vector();
|
||||
Matrix3 A = skewSymmetric(t) * R;
|
||||
Matrix6 adj;
|
||||
adj << R, Z3, A, R;
|
||||
return adj;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix6 Pose3::adjointMap(const Vector& xi) {
|
||||
Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
|
||||
Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5));
|
||||
Matrix6 adj;
|
||||
adj << w_hat, Z3, v_hat, w_hat;
|
||||
/* ************************************************************************* */
|
||||
Matrix6 Pose3::adjointMap(const Vector& xi) {
|
||||
Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
|
||||
Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5));
|
||||
Matrix6 adj;
|
||||
adj << w_hat, Z3, v_hat, w_hat;
|
||||
|
||||
return adj;
|
||||
}
|
||||
return adj;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Pose3::adjoint(const Vector& xi, const Vector& y, boost::optional<Matrix&> H) {
|
||||
if (H) {
|
||||
*H = zeros(6,6);
|
||||
for (int i = 0; i<6; ++i) {
|
||||
Vector dxi = zero(6); dxi(i) = 1.0;
|
||||
Matrix Gi = adjointMap(dxi);
|
||||
(*H).col(i) = Gi*y;
|
||||
}
|
||||
}
|
||||
return adjointMap(xi)*y;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y, boost::optional<Matrix&> H) {
|
||||
if (H) {
|
||||
*H = zeros(6,6);
|
||||
for (int i = 0; i<6; ++i) {
|
||||
Vector dxi = zero(6); dxi(i) = 1.0;
|
||||
Matrix GTi = adjointMap(dxi).transpose();
|
||||
(*H).col(i) = GTi*y;
|
||||
}
|
||||
}
|
||||
Matrix adjT = adjointMap(xi).transpose();
|
||||
return adjointMap(xi).transpose() * y;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix6 Pose3::dExpInv_exp(const Vector& xi) {
|
||||
// Bernoulli numbers, from Wikipedia
|
||||
static const Vector B = Vector_(9, 1.0, -1.0/2.0, 1./6., 0.0, -1.0/30.0, 0.0, 1.0/42.0, 0.0, -1.0/30);
|
||||
static const int N = 5; // order of approximation
|
||||
Matrix res = I6;
|
||||
Matrix6 ad_i = I6;
|
||||
Matrix6 ad_xi = adjointMap(xi);
|
||||
double fac = 1.0;
|
||||
for (int i = 1 ; i<N; ++i) {
|
||||
ad_i = ad_xi * ad_i;
|
||||
fac = fac*i;
|
||||
res = res + B(i)/fac*ad_i;
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Pose3::print(const string& s) const {
|
||||
cout << s;
|
||||
R_.print("R:\n");
|
||||
t_.print("t: ");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Pose3::equals(const Pose3& pose, double tol) const {
|
||||
return R_.equals(pose.R_,tol) && t_.equals(pose.t_,tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/** Modified from Murray94book version (which assumes w and v normalized?) */
|
||||
Pose3 Pose3::Expmap(const Vector& xi) {
|
||||
|
||||
// get angular velocity omega and translational velocity v from twist xi
|
||||
Point3 w(xi(0),xi(1),xi(2)), v(xi(3),xi(4),xi(5));
|
||||
|
||||
double theta = w.norm();
|
||||
if (theta < 1e-10) {
|
||||
static const Rot3 I;
|
||||
return Pose3(I, v);
|
||||
}
|
||||
else {
|
||||
Point3 n(w/theta); // axis unit vector
|
||||
Rot3 R = Rot3::rodriguez(n.vector(),theta);
|
||||
double vn = n.dot(v); // translation parallel to n
|
||||
Point3 n_cross_v = n.cross(v); // points towards axis
|
||||
Point3 t = (n_cross_v - R*n_cross_v)/theta + vn*n;
|
||||
return Pose3(R, t);
|
||||
/* ************************************************************************* */
|
||||
Vector Pose3::adjoint(const Vector& xi, const Vector& y,
|
||||
boost::optional<Matrix&> H) {
|
||||
if (H) {
|
||||
*H = zeros(6, 6);
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
Vector dxi = zero(6);
|
||||
dxi(i) = 1.0;
|
||||
Matrix Gi = adjointMap(dxi);
|
||||
(*H).col(i) = Gi * y;
|
||||
}
|
||||
}
|
||||
return adjointMap(xi) * y;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector6 Pose3::Logmap(const Pose3& p) {
|
||||
Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector();
|
||||
double t = w.norm();
|
||||
if (t < 1e-10) {
|
||||
Vector6 log;
|
||||
log << w, T;
|
||||
return log;
|
||||
}
|
||||
else {
|
||||
Matrix3 W = skewSymmetric(w/t);
|
||||
// Formula from Agrawal06iros, equation (14)
|
||||
// simplified with Mathematica, and multiplying in T to avoid matrix math
|
||||
double Tan = tan(0.5*t);
|
||||
Vector3 WT = W*T;
|
||||
Vector3 u = T - (0.5*t)*WT + (1 - t/(2.*Tan)) * (W * WT);
|
||||
Vector6 log;
|
||||
log << w, u;
|
||||
return log;
|
||||
/* ************************************************************************* */
|
||||
Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y,
|
||||
boost::optional<Matrix&> H) {
|
||||
if (H) {
|
||||
*H = zeros(6, 6);
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
Vector dxi = zero(6);
|
||||
dxi(i) = 1.0;
|
||||
Matrix GTi = adjointMap(dxi).transpose();
|
||||
(*H).col(i) = GTi * y;
|
||||
}
|
||||
}
|
||||
Matrix adjT = adjointMap(xi).transpose();
|
||||
return adjointMap(xi).transpose() * y;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::retractFirstOrder(const Vector& xi) const {
|
||||
Vector3 omega(sub(xi, 0, 3));
|
||||
Point3 v(sub(xi, 3, 6));
|
||||
Rot3 R = R_.retract(omega); // R is done exactly
|
||||
Point3 t = t_ + R_ * v; // First order t approximation
|
||||
return Pose3(R, t);
|
||||
/* ************************************************************************* */
|
||||
Matrix6 Pose3::dExpInv_exp(const Vector& xi) {
|
||||
// Bernoulli numbers, from Wikipedia
|
||||
static const Vector B = Vector_(9, 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0,
|
||||
0.0, 1.0 / 42.0, 0.0, -1.0 / 30);
|
||||
static const int N = 5; // order of approximation
|
||||
Matrix res = I6;
|
||||
Matrix6 ad_i = I6;
|
||||
Matrix6 ad_xi = adjointMap(xi);
|
||||
double fac = 1.0;
|
||||
for (int i = 1; i < N; ++i) {
|
||||
ad_i = ad_xi * ad_i;
|
||||
fac = fac * i;
|
||||
res = res + B(i) / fac * ad_i;
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Different versions of retract
|
||||
Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const {
|
||||
if(mode == Pose3::EXPMAP) {
|
||||
// Lie group exponential map, traces out geodesic
|
||||
return compose(Expmap(xi));
|
||||
} else if(mode == Pose3::FIRST_ORDER) {
|
||||
// First order
|
||||
return retractFirstOrder(xi);
|
||||
} else {
|
||||
// Point3 t = t_.retract(v.vector()); // Incorrect version retracts t independently
|
||||
// Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); // Second order t approximation
|
||||
assert(false);
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
void Pose3::print(const string& s) const {
|
||||
cout << s;
|
||||
R_.print("R:\n");
|
||||
t_.print("t: ");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// different versions of localCoordinates
|
||||
Vector6 Pose3::localCoordinates(const Pose3& T, Pose3::CoordinatesMode mode) const {
|
||||
if(mode == Pose3::EXPMAP) {
|
||||
// Lie group logarithm map, exact inverse of exponential map
|
||||
return Logmap(between(T));
|
||||
} else if(mode == Pose3::FIRST_ORDER) {
|
||||
// R is always done exactly in all three retract versions below
|
||||
Vector3 omega = R_.localCoordinates(T.rotation());
|
||||
/* ************************************************************************* */
|
||||
bool Pose3::equals(const Pose3& pose, double tol) const {
|
||||
return R_.equals(pose.R_, tol) && t_.equals(pose.t_, tol);
|
||||
}
|
||||
|
||||
// Incorrect version
|
||||
// Independently computes the logmap of the translation and rotation
|
||||
// Vector v = t_.localCoordinates(T.translation());
|
||||
/* ************************************************************************* */
|
||||
/** Modified from Murray94book version (which assumes w and v normalized?) */
|
||||
Pose3 Pose3::Expmap(const Vector& xi) {
|
||||
|
||||
// Correct first order t inverse
|
||||
Point3 d = R_.unrotate(T.translation() - t_);
|
||||
// get angular velocity omega and translational velocity v from twist xi
|
||||
Point3 w(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
|
||||
|
||||
// TODO: correct second order t inverse
|
||||
Vector6 local;
|
||||
local << omega(0),omega(1),omega(2),d.x(),d.y(),d.z();
|
||||
return local;
|
||||
} else {
|
||||
assert(false);
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix4 Pose3::matrix() const {
|
||||
const Matrix3 R = R_.matrix();
|
||||
const Vector3 T = t_.vector();
|
||||
Eigen::Matrix<double,1,4> A14;
|
||||
A14 << 0.0, 0.0, 0.0, 1.0;
|
||||
Matrix4 mat;
|
||||
mat << R, T, A14;
|
||||
return mat;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::transform_to(const Pose3& pose) const {
|
||||
Rot3 cRv = R_ * Rot3(pose.R_.inverse());
|
||||
Point3 t = pose.transform_to(t_);
|
||||
return Pose3(cRv, t);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Pose3::transform_from(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) {
|
||||
const Matrix R = R_.matrix();
|
||||
Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
H1->resize(3,6);
|
||||
(*H1) << DR, R;
|
||||
}
|
||||
if (H2) *H2 = R_.matrix();
|
||||
return R_ * p + t_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Pose3::transform_to(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
const Point3 result = R_.unrotate(p - t_);
|
||||
if (H1) {
|
||||
const Point3& q = result;
|
||||
Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
|
||||
H1->resize(3,6);
|
||||
(*H1) << DR, _I3;
|
||||
}
|
||||
if (H2) *H2 = R_.transpose();
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::compose(const Pose3& p2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = p2.inverse().AdjointMap();
|
||||
if (H2) *H2 = I6;
|
||||
return (*this) * p2;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
|
||||
if (H1) *H1 = -AdjointMap();
|
||||
Rot3 Rt = R_.inverse();
|
||||
return Pose3(Rt, Rt*(-t_));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// between = compose(p2,inverse(p1));
|
||||
Pose3 Pose3::between(const Pose3& p2, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
Pose3 result = inverse()*p2;
|
||||
if (H1) *H1 = -result.inverse().AdjointMap();
|
||||
if (H2) *H2 = I6;
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Pose3::range(const Point3& point,
|
||||
boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
if (!H1 && !H2) return transform_to(point).norm();
|
||||
Point3 d = transform_to(point, H1, H2);
|
||||
double x = d.x(), y = d.y(), z = d.z(),
|
||||
d2 = x * x + y * y + z * z, n = sqrt(d2);
|
||||
Matrix D_result_d = Matrix_(1, 3, x / n, y / n, z / n);
|
||||
if (H1) *H1 = D_result_d * (*H1);
|
||||
if (H2) *H2 = D_result_d * (*H2);
|
||||
return n;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Pose3::range(const Pose3& point,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
double r = range(point.translation(), H1, H2);
|
||||
if (H2) {
|
||||
Matrix H2_ = *H2 * point.rotation().matrix();
|
||||
*H2 = zeros(1, 6);
|
||||
insertSub(*H2, H2_, 0, 3);
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
|
||||
const size_t n = pairs.size();
|
||||
if (n<3) return boost::none; // we need at least three pairs
|
||||
|
||||
// calculate centroids
|
||||
Vector cp = zero(3),cq = zero(3);
|
||||
BOOST_FOREACH(const Point3Pair& pair, pairs) {
|
||||
cp += pair.first.vector();
|
||||
cq += pair.second.vector();
|
||||
}
|
||||
double f = 1.0/n;
|
||||
cp *= f; cq *= f;
|
||||
|
||||
// Add to form H matrix
|
||||
Matrix H = zeros(3,3);
|
||||
BOOST_FOREACH(const Point3Pair& pair, pairs) {
|
||||
Vector dp = pair.first.vector() - cp;
|
||||
Vector dq = pair.second.vector() - cq;
|
||||
H += dp * dq.transpose();
|
||||
}
|
||||
|
||||
// Compute SVD
|
||||
Matrix U,V;
|
||||
Vector S;
|
||||
svd(H,U,S,V);
|
||||
|
||||
// Recover transform with correction from Eggert97machinevisionandapplications
|
||||
Matrix UVtranspose = U * V.transpose();
|
||||
Matrix detWeighting = eye(3,3);
|
||||
detWeighting(2,2) = UVtranspose.determinant();
|
||||
Rot3 R(Matrix(V * detWeighting * U.transpose()));
|
||||
Point3 t = Point3(cq) - R * Point3(cp);
|
||||
double theta = w.norm();
|
||||
if (theta < 1e-10) {
|
||||
static const Rot3 I;
|
||||
return Pose3(I, v);
|
||||
} else {
|
||||
Point3 n(w / theta); // axis unit vector
|
||||
Rot3 R = Rot3::rodriguez(n.vector(), theta);
|
||||
double vn = n.dot(v); // translation parallel to n
|
||||
Point3 n_cross_v = n.cross(v); // points towards axis
|
||||
Point3 t = (n_cross_v - R * n_cross_v) / theta + vn * n;
|
||||
return Pose3(R, t);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
|
||||
os << pose.rotation() << "\n" << pose.translation() << endl;
|
||||
return os;
|
||||
/* ************************************************************************* */
|
||||
Vector6 Pose3::Logmap(const Pose3& p) {
|
||||
Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector();
|
||||
double t = w.norm();
|
||||
if (t < 1e-10) {
|
||||
Vector6 log;
|
||||
log << w, T;
|
||||
return log;
|
||||
} else {
|
||||
Matrix3 W = skewSymmetric(w / t);
|
||||
// Formula from Agrawal06iros, equation (14)
|
||||
// simplified with Mathematica, and multiplying in T to avoid matrix math
|
||||
double Tan = tan(0.5 * t);
|
||||
Vector3 WT = W * T;
|
||||
Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT);
|
||||
Vector6 log;
|
||||
log << w, u;
|
||||
return log;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::retractFirstOrder(const Vector& xi) const {
|
||||
Vector3 omega(sub(xi, 0, 3));
|
||||
Point3 v(sub(xi, 3, 6));
|
||||
Rot3 R = R_.retract(omega); // R is done exactly
|
||||
Point3 t = t_ + R_ * v; // First order t approximation
|
||||
return Pose3(R, t);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Different versions of retract
|
||||
Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const {
|
||||
if (mode == Pose3::EXPMAP) {
|
||||
// Lie group exponential map, traces out geodesic
|
||||
return compose(Expmap(xi));
|
||||
} else if (mode == Pose3::FIRST_ORDER) {
|
||||
// First order
|
||||
return retractFirstOrder(xi);
|
||||
} else {
|
||||
// Point3 t = t_.retract(v.vector()); // Incorrect version retracts t independently
|
||||
// Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); // Second order t approximation
|
||||
assert(false);
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// different versions of localCoordinates
|
||||
Vector6 Pose3::localCoordinates(const Pose3& T,
|
||||
Pose3::CoordinatesMode mode) const {
|
||||
if (mode == Pose3::EXPMAP) {
|
||||
// Lie group logarithm map, exact inverse of exponential map
|
||||
return Logmap(between(T));
|
||||
} else if (mode == Pose3::FIRST_ORDER) {
|
||||
// R is always done exactly in all three retract versions below
|
||||
Vector3 omega = R_.localCoordinates(T.rotation());
|
||||
|
||||
// Incorrect version
|
||||
// Independently computes the logmap of the translation and rotation
|
||||
// Vector v = t_.localCoordinates(T.translation());
|
||||
|
||||
// Correct first order t inverse
|
||||
Point3 d = R_.unrotate(T.translation() - t_);
|
||||
|
||||
// TODO: correct second order t inverse
|
||||
Vector6 local;
|
||||
local << omega(0), omega(1), omega(2), d.x(), d.y(), d.z();
|
||||
return local;
|
||||
} else {
|
||||
assert(false);
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix4 Pose3::matrix() const {
|
||||
const Matrix3 R = R_.matrix();
|
||||
const Vector3 T = t_.vector();
|
||||
Eigen::Matrix<double, 1, 4> A14;
|
||||
A14 << 0.0, 0.0, 0.0, 1.0;
|
||||
Matrix4 mat;
|
||||
mat << R, T, A14;
|
||||
return mat;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::transform_to(const Pose3& pose) const {
|
||||
Rot3 cRv = R_ * Rot3(pose.R_.inverse());
|
||||
Point3 t = pose.transform_to(t_);
|
||||
return Pose3(cRv, t);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
|
||||
boost::optional<Matrix&> Dpoint) const {
|
||||
if (Dpose) {
|
||||
const Matrix R = R_.matrix();
|
||||
Matrix DR = R * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
Dpose->resize(3, 6);
|
||||
(*Dpose) << DR, R;
|
||||
}
|
||||
if (Dpoint)
|
||||
*Dpoint = R_.matrix();
|
||||
return R_ * p + t_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose,
|
||||
boost::optional<Matrix&> Dpoint) const {
|
||||
const Point3 result = R_.unrotate(p - t_);
|
||||
if (Dpose) {
|
||||
const Point3& q = result;
|
||||
Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
|
||||
Dpose->resize(3, 6);
|
||||
(*Dpose) << DR, _I3;
|
||||
}
|
||||
if (Dpoint)
|
||||
*Dpoint = R_.transpose();
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::compose(const Pose3& p2, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
if (H1)
|
||||
*H1 = p2.inverse().AdjointMap();
|
||||
if (H2)
|
||||
*H2 = I6;
|
||||
return (*this) * p2;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
|
||||
if (H1)
|
||||
*H1 = -AdjointMap();
|
||||
Rot3 Rt = R_.inverse();
|
||||
return Pose3(Rt, Rt * (-t_));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// between = compose(p2,inverse(p1));
|
||||
Pose3 Pose3::between(const Pose3& p2, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
Pose3 result = inverse() * p2;
|
||||
if (H1)
|
||||
*H1 = -result.inverse().AdjointMap();
|
||||
if (H2)
|
||||
*H2 = I6;
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Pose3::range(const Point3& point, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
if (!H1 && !H2)
|
||||
return transform_to(point).norm();
|
||||
Point3 d = transform_to(point, H1, H2);
|
||||
double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, n = sqrt(
|
||||
d2);
|
||||
Matrix D_result_d = Matrix_(1, 3, x / n, y / n, z / n);
|
||||
if (H1)
|
||||
*H1 = D_result_d * (*H1);
|
||||
if (H2)
|
||||
*H2 = D_result_d * (*H2);
|
||||
return n;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Pose3::range(const Pose3& point, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
double r = range(point.translation(), H1, H2);
|
||||
if (H2) {
|
||||
Matrix H2_ = *H2 * point.rotation().matrix();
|
||||
*H2 = zeros(1, 6);
|
||||
insertSub(*H2, H2_, 0, 3);
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
|
||||
const size_t n = pairs.size();
|
||||
if (n < 3)
|
||||
return boost::none; // we need at least three pairs
|
||||
|
||||
// calculate centroids
|
||||
Vector cp = zero(3), cq = zero(3);
|
||||
BOOST_FOREACH(const Point3Pair& pair, pairs){
|
||||
cp += pair.first.vector();
|
||||
cq += pair.second.vector();
|
||||
}
|
||||
double f = 1.0 / n;
|
||||
cp *= f;
|
||||
cq *= f;
|
||||
|
||||
// Add to form H matrix
|
||||
Matrix H = zeros(3, 3);
|
||||
BOOST_FOREACH(const Point3Pair& pair, pairs){
|
||||
Vector dp = pair.first.vector() - cp;
|
||||
Vector dq = pair.second.vector() - cq;
|
||||
H += dp * dq.transpose();
|
||||
}
|
||||
|
||||
// Compute SVD
|
||||
Matrix U, V;
|
||||
Vector S;
|
||||
svd(H, U, S, V);
|
||||
|
||||
// Recover transform with correction from Eggert97machinevisionandapplications
|
||||
Matrix UVtranspose = U * V.transpose();
|
||||
Matrix detWeighting = eye(3, 3);
|
||||
detWeighting(2, 2) = UVtranspose.determinant();
|
||||
Rot3 R(Matrix(V * detWeighting * U.transpose()));
|
||||
Point3 t = Point3(cq) - R * Point3(cp);
|
||||
return Pose3(R, t);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
|
||||
os << pose.rotation() << "\n" << pose.translation() << endl;
|
||||
return os;
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -15,7 +15,6 @@
|
|||
*/
|
||||
|
||||
// \callgraph
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/config.h>
|
||||
|
|
@ -32,111 +31,123 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
class Pose2; // forward declare
|
||||
class Pose2;
|
||||
// forward declare
|
||||
|
||||
/**
|
||||
* A 3D pose (R,t) : (Rot3,Point3)
|
||||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT Pose3: public DerivedValue<Pose3> {
|
||||
public:
|
||||
static const size_t dimension = 6;
|
||||
|
||||
/** Pose Concept requirements */
|
||||
typedef Rot3 Rotation;
|
||||
typedef Point3 Translation;
|
||||
|
||||
private:
|
||||
Rot3 R_;
|
||||
Point3 t_;
|
||||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Default constructor is origin */
|
||||
Pose3() {
|
||||
}
|
||||
|
||||
/** Copy constructor */
|
||||
Pose3(const Pose3& pose) :
|
||||
R_(pose.R_), t_(pose.t_) {
|
||||
}
|
||||
|
||||
/** Construct from R,t */
|
||||
Pose3(const Rot3& R, const Point3& t) :
|
||||
R_(R), t_(t) {
|
||||
}
|
||||
|
||||
/** Construct from Pose2 */
|
||||
explicit Pose3(const Pose2& pose2);
|
||||
|
||||
/** Constructor from 4*4 matrix */
|
||||
Pose3(const Matrix &T) :
|
||||
R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), T(2, 1),
|
||||
T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// print with optional string
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals(const Pose3& pose, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
/// @name Group
|
||||
/// @{
|
||||
|
||||
/// identity for group operation
|
||||
static Pose3 identity() {
|
||||
return Pose3();
|
||||
}
|
||||
|
||||
/// inverse transformation with derivatives
|
||||
Pose3 inverse(boost::optional<Matrix&> H1 = boost::none) const;
|
||||
|
||||
///compose this transformation onto another (first *this and then p2)
|
||||
Pose3 compose(const Pose3& p2, boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const;
|
||||
|
||||
/// compose syntactic sugar
|
||||
Pose3 operator*(const Pose3& T) const {
|
||||
return Pose3(R_ * T.R_, t_ + R_ * T.t_);
|
||||
}
|
||||
|
||||
/**
|
||||
* A 3D pose (R,t) : (Rot3,Point3)
|
||||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
* Return relative pose between p1 and p2, in p1 coordinate frame
|
||||
* as well as optionally the derivatives
|
||||
*/
|
||||
class GTSAM_EXPORT Pose3 : public DerivedValue<Pose3> {
|
||||
public:
|
||||
static const size_t dimension = 6;
|
||||
Pose3 between(const Pose3& p2, boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const;
|
||||
|
||||
/** Pose Concept requirements */
|
||||
typedef Rot3 Rotation;
|
||||
typedef Point3 Translation;
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
private:
|
||||
Rot3 R_;
|
||||
Point3 t_;
|
||||
/** Enum to indicate which method should be used in Pose3::retract() and
|
||||
* Pose3::localCoordinates()
|
||||
*/
|
||||
enum CoordinatesMode {
|
||||
EXPMAP, ///< The correct exponential map, computationally expensive.
|
||||
FIRST_ORDER ///< A fast first-order approximation to the exponential map.
|
||||
};
|
||||
|
||||
public:
|
||||
/// Dimensionality of tangent space = 6 DOF - used to autodetect sizes
|
||||
static size_t Dim() {
|
||||
return dimension;
|
||||
}
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
/// Dimensionality of the tangent space = 6 DOF
|
||||
size_t dim() const {
|
||||
return dimension;
|
||||
}
|
||||
|
||||
/** Default constructor is origin */
|
||||
Pose3() {}
|
||||
/// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ from R^ with fast first-order approximation to the exponential map
|
||||
Pose3 retractFirstOrder(const Vector& d) const;
|
||||
|
||||
/** Copy constructor */
|
||||
Pose3(const Pose3& pose) : R_(pose.R_), t_(pose.t_) {}
|
||||
/// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ to Pose3 manifold neighborhood around current pose
|
||||
Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode =
|
||||
POSE3_DEFAULT_COORDINATES_MODE) const;
|
||||
|
||||
/** Construct from R,t */
|
||||
Pose3(const Rot3& R, const Point3& t) : R_(R), t_(t) {}
|
||||
|
||||
/** Construct from Pose2 */
|
||||
explicit Pose3(const Pose2& pose2);
|
||||
|
||||
/** Constructor from 4*4 matrix */
|
||||
Pose3(const Matrix &T) :
|
||||
R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0),
|
||||
T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// print with optional string
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals(const Pose3& pose, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
/// @name Group
|
||||
/// @{
|
||||
|
||||
/// identity for group operation
|
||||
static Pose3 identity() { return Pose3(); }
|
||||
|
||||
/// inverse transformation with derivatives
|
||||
Pose3 inverse(boost::optional<Matrix&> H1=boost::none) const;
|
||||
|
||||
///compose this transformation onto another (first *this and then p2)
|
||||
Pose3 compose(const Pose3& p2,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const;
|
||||
|
||||
/// compose syntactic sugar
|
||||
Pose3 operator*(const Pose3& T) const {
|
||||
return Pose3(R_*T.R_, t_ + R_*T.t_);
|
||||
}
|
||||
|
||||
/**
|
||||
* Return relative pose between p1 and p2, in p1 coordinate frame
|
||||
* as well as optionally the derivatives
|
||||
*/
|
||||
Pose3 between(const Pose3& p2,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/** Enum to indicate which method should be used in Pose3::retract() and
|
||||
* Pose3::localCoordinates()
|
||||
*/
|
||||
enum CoordinatesMode {
|
||||
EXPMAP, ///< The correct exponential map, computationally expensive.
|
||||
FIRST_ORDER ///< A fast first-order approximation to the exponential map.
|
||||
};
|
||||
|
||||
/// Dimensionality of tangent space = 6 DOF - used to autodetect sizes
|
||||
static size_t Dim() { return dimension; }
|
||||
|
||||
/// Dimensionality of the tangent space = 6 DOF
|
||||
size_t dim() const { return dimension; }
|
||||
|
||||
/// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ from R^ with fast first-order approximation to the exponential map
|
||||
Pose3 retractFirstOrder(const Vector& d) const;
|
||||
|
||||
/// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ to Pose3 manifold neighborhood around current pose
|
||||
Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const;
|
||||
|
||||
/// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose
|
||||
Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const;
|
||||
/// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose
|
||||
Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode =POSE3_DEFAULT_COORDINATES_MODE) const;
|
||||
|
||||
/// @}
|
||||
/// @name Lie Group
|
||||
|
|
@ -218,16 +229,28 @@ namespace gtsam {
|
|||
/// @name Group Action on Point3
|
||||
/// @{
|
||||
|
||||
/** receives the point in Pose coordinates and transforms it to world coordinates */
|
||||
/**
|
||||
* @brief takes point in Pose coordinates and transforms it to world coordinates
|
||||
* @param p point in Pose coordinates
|
||||
* @param Dpose optional 3*6 Jacobian wrpt this pose
|
||||
* @param Dpoint optional 3*3 Jacobian wrpt point
|
||||
* @return point in world coordinates
|
||||
*/
|
||||
Point3 transform_from(const Point3& p,
|
||||
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
|
||||
boost::optional<Matrix&> Dpose=boost::none, boost::optional<Matrix&> Dpoint=boost::none) const;
|
||||
|
||||
/** syntactic sugar for transform_from */
|
||||
inline Point3 operator*(const Point3& p) const { return transform_from(p); }
|
||||
|
||||
/** receives the point in world coordinates and transforms it to Pose coordinates */
|
||||
/**
|
||||
* @brief takes point in world coordinates and transforms it to Pose coordinates
|
||||
* @param p point in world coordinates
|
||||
* @param Dpose optional 3*6 Jacobian wrpt this pose
|
||||
* @param Dpoint optional 3*3 Jacobian wrpt point
|
||||
* @return point in Pose coordinates
|
||||
*/
|
||||
Point3 transform_to(const Point3& p,
|
||||
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
|
||||
boost::optional<Matrix&> Dpose=boost::none, boost::optional<Matrix&> Dpoint=boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
|
|
@ -305,7 +328,7 @@ namespace gtsam {
|
|||
}
|
||||
/// @}
|
||||
|
||||
}; // Pose3 class
|
||||
};// Pose3 class
|
||||
|
||||
/**
|
||||
* wedge for Pose3:
|
||||
|
|
@ -314,16 +337,16 @@ namespace gtsam {
|
|||
* v = 3D velocity
|
||||
* @return xihat, 4*4 element of Lie algebra that can be exponentiated
|
||||
*/
|
||||
template <>
|
||||
inline Matrix wedge<Pose3>(const Vector& xi) {
|
||||
return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5));
|
||||
}
|
||||
template<>
|
||||
inline Matrix wedge<Pose3>(const Vector& xi) {
|
||||
return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5));
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate pose between a vector of 3D point correspondences (p,q)
|
||||
* where q = Pose3::transform_from(p) = t + R*p
|
||||
*/
|
||||
typedef std::pair<Point3,Point3> Point3Pair;
|
||||
GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
|
||||
/**
|
||||
* Calculate pose between a vector of 3D point correspondences (p,q)
|
||||
* where q = Pose3::transform_from(p) = t + R*p
|
||||
*/
|
||||
typedef std::pair<Point3, Point3> Point3Pair;
|
||||
GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
Loading…
Reference in New Issue