Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix / Move Rot3::quaternion to deprecated block #1219

Merged
merged 3 commits into from
Jun 10, 2022
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions gtsam/geometry/Rot3.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -228,6 +228,7 @@ double Rot3::yaw(OptionalJacobian<1, 3> H) const {
}

/* ************************************************************************* */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
Vector Rot3::quaternion() const {
gtsam::Quaternion q = toQuaternion();
Vector v(4);
Expand All @@ -237,6 +238,7 @@ Vector Rot3::quaternion() const {
v(3) = q.z();
return v;
}
#endif

/* ************************************************************************* */
pair<Unit3, double> Rot3::axisAngle() const {
Expand Down
7 changes: 6 additions & 1 deletion gtsam/geometry/Rot3.h
Original file line number Diff line number Diff line change
Expand Up @@ -515,11 +515,16 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
*/
gtsam::Quaternion toQuaternion() const;

#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/**
* Converts to a generic matrix to allow for use with matlab
* In format: w x y z
* @deprecated: use Rot3::toQuaternion() instead.
* If still using this API, remind that in the returned Vector `V`,
* `V.x()` denotes the actual `qw`, `V.y()` denotes 'qx', `V.z()` denotes `qy`, and `V.w()` denotes 'qz'.
*/
Vector quaternion() const;
Vector GTSAM_DEPRECATED quaternion() const;
#endif

/**
* @brief Spherical Linear intERPolation between *this and other
Expand Down
2 changes: 1 addition & 1 deletion gtsam/geometry/geometry.i
Original file line number Diff line number Diff line change
Expand Up @@ -355,7 +355,7 @@ class Rot3 {
double yaw() const;
pair<gtsam::Unit3, double> axisAngle() const;
gtsam::Quaternion toQuaternion() const;
Vector quaternion() const;
// Vector quaternion() const; // @deprecated, see https://github.com/borglab/gtsam/pull/1219
gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const;

// enabling serialization functionality
Expand Down
6 changes: 3 additions & 3 deletions gtsam_unstable/timing/timeShonanAveraging.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -79,9 +79,9 @@ void saveG2oResult(string name, const Values& values, std::map<Key, Pose3> poses
myfile << "VERTEX_SE3:QUAT" << " ";
myfile << i << " ";
myfile << poses[i].x() << " " << poses[i].y() << " " << poses[i].z() << " ";
Vector quaternion = Rot3(R).quaternion();
myfile << quaternion(3) << " " << quaternion(2) << " " << quaternion(1)
<< " " << quaternion(0);
Quaternion quaternion = Rot3(R).toQuaternion();
myfile << quaternion.x() << " " << quaternion.y() << " " << quaternion.z()
HViktorTsoi marked this conversation as resolved.
Show resolved Hide resolved
<< " " << quaternion.w();
myfile << "\n";
}
myfile.close();
Expand Down