change: a little clean up
zhaoyang
parent
3e1f336ede
commit
400a17d9ab
|
|
@ -80,6 +80,7 @@ Point3 Similarity3::transform_from(const Point3& p, //
|
|||
*H2 = s_ * R_.matrix(); // just 3*3 sub-block of matrix()
|
||||
return R_ * (s_ * p) + t_;
|
||||
// TODO: Effect of scale change is this, right?
|
||||
// No, this is incorrect. Zhaoyang Lv
|
||||
// sR t * (1+v)I 0 * p = s(1+v)R t * p = s(1+v)Rp + t = sRp + vRp + t
|
||||
// 0001 000 1 1 000 1 1
|
||||
}
|
||||
|
|
@ -186,38 +187,15 @@ Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) {
|
|||
// www.ethaneade.org/latex2html/lie/node29.html
|
||||
Vector3 w = Rot3::Logmap(s.R_);
|
||||
double lambda = log(s.s_);
|
||||
Matrix33 V = GetV(w, lambda);
|
||||
Vector3 u = V.inverse() * s.t_.vector();
|
||||
|
||||
Vector7 result;
|
||||
result << w, u, lambda;
|
||||
result << w, GetV(w, lambda).inverse() * s.t_.vector(), lambda;
|
||||
return result;
|
||||
}
|
||||
|
||||
Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) {
|
||||
Matrix31 w(v.head<3>());
|
||||
Vector3 w(v.head<3>());
|
||||
double lambda = v[6];
|
||||
Matrix31 u(v.segment<3>(3));
|
||||
|
||||
Matrix33 V = GetV(w, lambda);
|
||||
// Matrix33 wx = skewSymmetric(w[0], w[1], w[2]);
|
||||
// double lambdasquared = lambda * lambda;
|
||||
// double thetasquared = w.transpose() * w;
|
||||
// double theta = sqrt(thetasquared);
|
||||
// double X = sin(theta)/theta;
|
||||
// double Y = (1-cos(theta))/thetasquared;
|
||||
// double Z = (1-X)/thetasquared;
|
||||
// double W = (0.5-Y)/thetasquared;
|
||||
// double alpha = lambdasquared / (lambdasquared + thetasquared);
|
||||
// double beta = (exp(-lambda)-1+lambda)/lambdasquared;
|
||||
// double gama = Y - (lambda * Z);
|
||||
// double mu = (1-lambda+(0.5*lambdasquared)-exp(-lambda))/(lambdasquared*lambda);
|
||||
// double upsilon = Z-(lambda*W);
|
||||
// double A = (1-exp(-lambda))/lambda;
|
||||
// double B = alpha*(beta-gama)+gama;
|
||||
// double C = alpha*(mu-upsilon)+upsilon;
|
||||
// Matrix33 V = A*Matrix33::Identity() + B*wx + C*wx*wx;
|
||||
return Similarity3(Rot3::Expmap(w), Point3(V*u), 1.0/exp(-lambda));
|
||||
return Similarity3(Rot3::Expmap(w), Point3(GetV(w, lambda)*v.segment<3>(3)), 1.0/exp(-lambda));
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -227,33 +205,6 @@ std::ostream &operator<<(std::ostream &os, const Similarity3& p) {
|
|||
return os;
|
||||
}
|
||||
|
||||
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]
|
||||
|
||||
// Use the Expmap
|
||||
return Similarity3::Expmap(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;
|
||||
|
||||
// Use the Logmap
|
||||
return Similarity3::Logmap(other);
|
||||
}
|
||||
|
||||
const Matrix4 Similarity3::matrix() const {
|
||||
Matrix4 T;
|
||||
T.topRows<3>() << s_ * R_.matrix(), t_.vector();
|
||||
|
|
|
|||
|
|
@ -103,21 +103,25 @@ public:
|
|||
/// @name Lie Group
|
||||
/// @{
|
||||
|
||||
/// Not currently implemented, required because this is a lie group
|
||||
/** Log map at the identity
|
||||
* \f$ [R_x,R_y,R_z, t_x, t_y, t_z, \lambda] \f$
|
||||
*/
|
||||
static Vector7 Logmap(const Similarity3& s, //
|
||||
OptionalJacobian<7, 7> Hm = boost::none);
|
||||
|
||||
/// Not currently implemented, required because this is a lie group
|
||||
/** Exponential map at the identity
|
||||
*/
|
||||
static Similarity3 Expmap(const Vector7& v, //
|
||||
OptionalJacobian<7, 7> Hm = boost::none);
|
||||
|
||||
/// Update Similarity transform via 7-dim vector in tangent space
|
||||
/// Chart at the origin
|
||||
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);
|
||||
static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none) {
|
||||
return Similarity3::Expmap(v);
|
||||
}
|
||||
static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none) {
|
||||
return Similarity3::Logmap(other);
|
||||
}
|
||||
};
|
||||
|
||||
/// Project from one tangent space to another
|
||||
|
|
|
|||
|
|
@ -13,6 +13,7 @@
|
|||
* @file testSimilarity3.cpp
|
||||
* @brief Unit tests for Similarity3 class
|
||||
* @author Paul Drews
|
||||
* @author Zhaoyang Lv
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/geometry/Similarity3.h>
|
||||
|
|
@ -73,18 +74,30 @@ TEST(Similarity3, Getters2) {
|
|||
TEST(Similarity3, AdjointMap) {
|
||||
Similarity3 test(Rot3::ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7);
|
||||
Matrix7 result;
|
||||
result << -1.5739, -2.4512, -6.3651, -50.7671, -11.2503, 16.8859, -28.0000, 6.3167, -2.9884, -0.4111, 0.8502, 8.6373, -49.7260, -35.0000, -2.5734, -5.8362, 2.8839, 33.1363, 0.3024, 30.1811, -42.0000, 0, 0, 0, -0.2248, -0.3502, -0.9093, 0, 0, 0, 0, 0.9024, -0.4269, -0.0587, 0, 0, 0, 0, -0.3676, -0.8337, 0.4120, 0, 0, 0, 0, 0, 0, 0, 1.0000;
|
||||
result << -1.5739, -2.4512, -6.3651, -50.7671, -11.2503, 16.8859, -28.0000,
|
||||
6.3167, -2.9884, -0.4111, 0.8502, 8.6373, -49.7260, -35.0000,
|
||||
-2.5734, -5.8362, 2.8839, 33.1363, 0.3024, 30.1811, -42.0000,
|
||||
0, 0, 0, -0.2248, -0.3502, -0.9093, 0,
|
||||
0, 0, 0, 0.9024, -0.4269, -0.0587, 0,
|
||||
0, 0, 0, -0.3676, -0.8337, 0.4120, 0,
|
||||
0, 0, 0, 0, 0, 0, 1.0000;
|
||||
EXPECT(assert_equal(result, test.AdjointMap(), 1e-3));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Similarity3, inverse) {
|
||||
Similarity3 test(Rot3::ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7);
|
||||
Matrix3 Re;
|
||||
Similarity3 sim3(Rot3::ypr(1, 2, 3).inverse(), Point3(4, 5, 6), 7);
|
||||
Matrix3 Re; // some values from matlab
|
||||
Re << -0.2248, 0.9024, -0.3676, -0.3502, -0.4269, -0.8337, -0.9093, -0.0587, 0.4120;
|
||||
Vector3 te(-9.8472, 59.7640, 10.2125);
|
||||
Similarity3 expected(Re, te, 1.0 / 7.0);
|
||||
EXPECT(assert_equal(expected, test.inverse(), 1e-3));
|
||||
EXPECT(assert_equal(expected, sim3.inverse(), 1e-4));
|
||||
EXPECT(assert_equal(sim3, sim3.inverse().inverse(), 1e-8));
|
||||
|
||||
// test lie group inverse
|
||||
Matrix H1, H2;
|
||||
EXPECT(assert_equal(expected, sim3.inverse(H1), 1e-4));
|
||||
EXPECT(assert_equal(sim3, sim3.inverse().inverse(H2), 1e-8));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
@ -169,7 +182,10 @@ TEST(Similarity3, manifold_first_order) {
|
|||
// Return as a 4*4 Matrix
|
||||
TEST(Similarity3, Matrix) {
|
||||
Matrix4 expected;
|
||||
expected << 2, 0, 0, 1, 0, 2, 0, 1, 0, 0, 2, 0, 0, 0, 0, 1;
|
||||
expected << 2, 0, 0, 1,
|
||||
0, 2, 0, 1,
|
||||
0, 0, 2, 0,
|
||||
0, 0, 0, 1;
|
||||
Matrix4 actual = T4.matrix();
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
|
@ -200,6 +216,7 @@ TEST(Similarity3, ExpLogMap) {
|
|||
Similarity3 Scalc = Similarity3::Expmap(expected);
|
||||
EXPECT(assert_equal(Sexpm, Scalc, 1e-3));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
// Group action on Point3 (with simpler transform)
|
||||
TEST(Similarity3, GroupAction) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue