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

Quaternion now also uppercase

release/4.3a0
Frank Dellaert 2016-01-26 23:57:34 -08:00
parent 4d93a33f61
commit 6eece9cc60
9 changed files with 47 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/Rot3.cpp
View File

@ -123,7 +123,7 @@ Vector3 Rot3::rpy() const {
/* ************************************************************************* */
Vector Rot3::quaternion() const {
Quaternion q = toQuaternion();
gtsam::Quaternion q = toQuaternion();
Vector v(4);
v(0) = q.w();
v(1) = q.x();

21
gtsam/geometry/Rot3.h
View File

@ -59,7 +59,7 @@ namespace gtsam {
#ifdef GTSAM_USE_QUATERNIONS
/** Internal Eigen Quaternion */
Quaternion quaternion_;
gtsam::Quaternion quaternion_;
#else
Matrix3 rot_;
#endif
@ -166,8 +166,8 @@ namespace gtsam {
static Rot3 Ypr(double y, double p, double r) { return RzRyRx(r,p,y);}
/** Create from Quaternion coefficients */
static Rot3 quaternion(double w, double x, double y, double z) {
Quaternion q(w, x, y, z);
static Rot3 Quaternion(double w, double x, double y, double z) {
gtsam::Quaternion q(w, x, y, z);
return Rot3(q);
}
@ -179,7 +179,7 @@ namespace gtsam {
*/
static Rot3 AxisAngle(const Vector3& axis, double angle) {
#ifdef GTSAM_USE_QUATERNIONS
return Quaternion(Eigen::AngleAxis<double>(angle, axis));
return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, axis));
#else
return SO3::AxisAngle(axis,angle);
#endif
@ -313,7 +313,7 @@ namespace gtsam {
static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = boost::none) {
if(H) *H = Rot3::ExpmapDerivative(v);
#ifdef GTSAM_USE_QUATERNIONS
return traits<Quaternion>::Expmap(v);
return traits<gtsam::Quaternion>::Expmap(v);
#else
return traits<SO3>::Expmap(v);
#endif
@ -460,7 +460,7 @@ namespace gtsam {
/** Compute the quaternion representation of this rotation.
* @return The quaternion
*/
Quaternion toQuaternion() const;
gtsam::Quaternion toQuaternion() const;
/**
* Converts to a generic matrix to allow for use with matlab
@ -479,6 +479,8 @@ namespace gtsam {
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
/// @}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
static Rot3 rodriguez(const Vector3& axis, double angle) { return AxisAngle(axis, angle); }
@ -486,13 +488,14 @@ namespace gtsam {
static Rot3 rodriguez(const Unit3& axis, double angle) { return AxisAngle(axis, angle); }
static Rot3 rodriguez(const Vector3& w) { return Rodrigues(w); }
static Rot3 rodriguez(double wx, double wy, double wz) { return Rodrigues(wx, wy, wz); }
/// @}
#ifdef ALLOW_DEPRECATED_IN_GTSAM4
static Rot3 yaw (double t) { return Yaw(t); }
static Rot3 pitch(double t) { return Pitch(t); }
static Rot3 roll (double t) { return Roll(t); }
static Rot3 ypr(double y, double p, double r) { return Ypr(r,p,y);}
static Rot3 quaternion(double w, double x, double y, double z) {
return Rot3::Quaternion q(w, x, y, z);
}
/// @}
#endif
private:

6
gtsam/geometry/Rot3M.cpp
View File

@ -51,7 +51,7 @@ Rot3::Rot3(double R11, double R12, double R13,
}
/* ************************************************************************* */
Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {
Rot3::Rot3(const gtsam::Quaternion& q) : rot_(q.toRotationMatrix()) {
}
/* ************************************************************************* */
@ -191,8 +191,8 @@ Point3 Rot3::r2() const { return Point3(rot_.col(1)); }
Point3 Rot3::r3() const { return Point3(rot_.col(2)); }
/* ************************************************************************* */
Quaternion Rot3::toQuaternion() const {
return Quaternion(rot_);
gtsam::Quaternion Rot3::toQuaternion() const {
return gtsam::Quaternion(rot_);
}
/* ************************************************************************* */

18
gtsam/geometry/Rot3Q.cpp
View File

@ -47,30 +47,30 @@ namespace gtsam {
R31, R32, R33).finished()) {}
/* ************************************************************************* */
Rot3::Rot3(const Quaternion& q) :
Rot3::Rot3(const gtsam::Quaternion& q) :
quaternion_(q) {
}
/* ************************************************************************* */
Rot3 Rot3::Rx(double t) {
return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX()));
return gtsam::Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX()));
}
/* ************************************************************************* */
Rot3 Rot3::Ry(double t) {
return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY()));
return gtsam::Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY()));
}
/* ************************************************************************* */
Rot3 Rot3::Rz(double t) {
return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ()));
return gtsam::Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ()));
}
/* ************************************************************************* */
Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3(
Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) *
Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) *
Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX())));
gtsam::Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) *
gtsam::Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) *
gtsam::Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX())));
}
/* ************************************************************************* */
@ -98,7 +98,7 @@ namespace gtsam {
/* ************************************************************************* */
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) {
return traits<Quaternion>::Logmap(R.quaternion_, H);
return traits<gtsam::Quaternion>::Logmap(R.quaternion_, H);
}
/* ************************************************************************* */
@ -128,7 +128,7 @@ namespace gtsam {
Point3 Rot3::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); }
/* ************************************************************************* */
Quaternion Rot3::toQuaternion() const { return quaternion_; }
gtsam::Quaternion Rot3::toQuaternion() const { return quaternion_; }
/* ************************************************************************* */

4
gtsam/geometry/tests/testRot3.cpp
View File

@ -594,9 +594,9 @@ TEST(Rot3, quaternion) {
// Check creating Rot3 from quaternion
EXPECT(assert_equal(R1, Rot3(q1)));
EXPECT(assert_equal(R1, Rot3::quaternion(q1.w(), q1.x(), q1.y(), q1.z())));
EXPECT(assert_equal(R1, Rot3::Quaternion(q1.w(), q1.x(), q1.y(), q1.z())));
EXPECT(assert_equal(R2, Rot3(q2)));
EXPECT(assert_equal(R2, Rot3::quaternion(q2.w(), q2.x(), q2.y(), q2.z())));
EXPECT(assert_equal(R2, Rot3::Quaternion(q2.w(), q2.x(), q2.y(), q2.z())));
// Check converting Rot3 to quaterion
EXPECT(assert_equal(Vector(R1.toQuaternion().coeffs()), Vector(q1.coeffs())));

2
gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
View File

@ -62,7 +62,7 @@ using namespace gtsam;
// Check that ceres rotation convention is the same
TEST(AdaptAutoDiff, Rotation) {
Vector3 axisAngle(0.1, 0.2, 0.3);
Matrix3 expected = Rot3::rodriguez(axisAngle).matrix();
Matrix3 expected = Rot3::Rodrigues(axisAngle).matrix();
Matrix3 actual;
ceres::AngleAxisToRotationMatrix(axisAngle.data(), actual.data());
EXPECT(assert_equal(expected, actual));

4
gtsam/slam/dataset.cpp
View File

@ -518,7 +518,7 @@ GraphAndValues load3D(const string& filename) {
Key id;
double x, y, z, qx, qy, qz, qw;
ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw;
Rot3 R = Rot3::quaternion(qw, qx, qy, qz);
Rot3 R = Rot3::Quaternion(qw, qx, qy, qz);
Point3 t = Point3(x, y, z);
initial->insert(id, Pose3(R,t));
}
@ -553,7 +553,7 @@ GraphAndValues load3D(const string& filename) {
Key id1, id2;
double x, y, z, qx, qy, qz, qw;
ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw;
Rot3 R = Rot3::quaternion(qw, qx, qy, qz);
Rot3 R = Rot3::Quaternion(qw, qx, qy, qz);
Point3 t = Point3(x, y, z);
for (int i = 0; i < 6; i++){
for (int j = i; j < 6; j++){

28
gtsam/slam/tests/testDataset.cpp
View File

@ -137,23 +137,23 @@ TEST( dataSet, readG2o3D)
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
Values expectedValues;
Rot3 R0 = Rot3::quaternion(1.000000, 0.000000, 0.000000, 0.000000 );
Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 );
Point3 p0 = Point3(0.000000, 0.000000, 0.000000);
expectedValues.insert(0, Pose3(R0, p0));
Rot3 R1 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
Point3 p1 = Point3(1.001367, 0.015390, 0.004948);
expectedValues.insert(1, Pose3(R1, p1));
Rot3 R2 = Rot3::quaternion(0.421446, -0.351729, -0.597838, 0.584174 );
Rot3 R2 = Rot3::Quaternion(0.421446, -0.351729, -0.597838, 0.584174 );
Point3 p2 = Point3(1.993500, 0.023275, 0.003793);
expectedValues.insert(2, Pose3(R2, p2));
Rot3 R3 = Rot3::quaternion(0.067024, 0.331798, -0.200659, 0.919323);
Rot3 R3 = Rot3::Quaternion(0.067024, 0.331798, -0.200659, 0.919323);
Point3 p3 = Point3(2.004291, 1.024305, 0.018047);
expectedValues.insert(3, Pose3(R3, p3));
Rot3 R4 = Rot3::quaternion(0.765488, -0.035697, -0.462490, 0.445933);
Rot3 R4 = Rot3::Quaternion(0.765488, -0.035697, -0.462490, 0.445933);
Point3 p4 = Point3(0.999908, 1.055073, 0.020212);
expectedValues.insert(4, Pose3(R4, p4));
@ -163,27 +163,27 @@ TEST( dataSet, readG2o3D)
NonlinearFactorGraph expectedGraph;
Point3 p01 = Point3(1.001367, 0.015390, 0.004948);
Rot3 R01 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
expectedGraph.add(BetweenFactor<Pose3>(0, 1, Pose3(R01,p01), model));
Point3 p12 = Point3(0.523923, 0.776654, 0.326659);
Rot3 R12 = Rot3::quaternion(0.105373 , 0.311512, 0.656877, -0.678505 );
Rot3 R12 = Rot3::Quaternion(0.105373 , 0.311512, 0.656877, -0.678505 );
expectedGraph.add(BetweenFactor<Pose3>(1, 2, Pose3(R12,p12), model));
Point3 p23 = Point3(0.910927, 0.055169, -0.411761);
Rot3 R23 = Rot3::quaternion(0.568551 , 0.595795, -0.561677, 0.079353 );
Rot3 R23 = Rot3::Quaternion(0.568551 , 0.595795, -0.561677, 0.079353 );
expectedGraph.add(BetweenFactor<Pose3>(2, 3, Pose3(R23,p23), model));
Point3 p34 = Point3(0.775288, 0.228798, -0.596923);
Rot3 R34 = Rot3::quaternion(0.542221 , -0.592077, 0.303380, -0.513226 );
Rot3 R34 = Rot3::Quaternion(0.542221 , -0.592077, 0.303380, -0.513226 );
expectedGraph.add(BetweenFactor<Pose3>(3, 4, Pose3(R34,p34), model));
Point3 p14 = Point3(-0.577841, 0.628016, -0.543592);
Rot3 R14 = Rot3::quaternion(0.327419 , -0.125250, -0.534379, 0.769122 );
Rot3 R14 = Rot3::Quaternion(0.327419 , -0.125250, -0.534379, 0.769122 );
expectedGraph.add(BetweenFactor<Pose3>(1, 4, Pose3(R14,p14), model));
Point3 p30 = Point3(-0.623267, 0.086928, 0.773222);
Rot3 R30 = Rot3::quaternion(0.083672 , 0.104639, 0.627755, 0.766795 );
Rot3 R30 = Rot3::Quaternion(0.083672 , 0.104639, 0.627755, 0.766795 );
expectedGraph.add(BetweenFactor<Pose3>(3, 0, Pose3(R30,p30), model));
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
@ -199,11 +199,11 @@ TEST( dataSet, readG2o3DNonDiagonalNoise)
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
Values expectedValues;
Rot3 R0 = Rot3::quaternion(1.000000, 0.000000, 0.000000, 0.000000 );
Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 );
Point3 p0 = Point3(0.000000, 0.000000, 0.000000);
expectedValues.insert(0, Pose3(R0, p0));
Rot3 R1 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
Point3 p1 = Point3(1.001367, 0.015390, 0.004948);
expectedValues.insert(1, Pose3(R1, p1));
@ -223,7 +223,7 @@ TEST( dataSet, readG2o3DNonDiagonalNoise)
noiseModel::Gaussian::shared_ptr model = noiseModel::Gaussian::Covariance(Info.inverse());
NonlinearFactorGraph expectedGraph;
Point3 p01 = Point3(1.001367, 0.015390, 0.004948);
Rot3 R01 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
expectedGraph.add(BetweenFactor<Pose3>(0, 1, Pose3(R01,p01), model));
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-2));

6
python/handwritten/geometry/Rot3.cpp
View File

@ -27,12 +27,12 @@ using namespace gtsam;
static Rot3 Quaternion_0(const Vector4& q)
{
return Rot3::quaternion(q[0],q[1],q[2],q[3]);
return Rot3::Quaternion(q[0],q[1],q[2],q[3]);
}
static Rot3 Quaternion_1(double w, double x, double y, double z)
{
return Rot3::quaternion(w,x,y,z);
return Rot3::Quaternion(w,x,y,z);
}
// Prototypes used to perform overloading
@ -108,4 +108,4 @@ void exportRot3(){
.def(repr(self)) // __repr__
;
}
}