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

unit3

release/4.3a0
kartik arcot 2023-01-12 08:40:47 -08:00
parent 4495efe233
commit 0bf269f854
2 changed files with 16 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/geometry/Unit3.cpp
View File

@ -109,8 +109,8 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
jacobian.block<3, 2>(3, 0) = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; jacobian.block<3, 2>(3, 0) = H_b2_n * H_n_p + H_b2_b1 * H_b1_p;
// Cache the result and jacobian // Cache the result and jacobian
H_B_.reset(jacobian); H_B_ = (jacobian);
B_.reset(B); B_ = (B);
} }
// Return cached jacobian, possibly computed just above // Return cached jacobian, possibly computed just above
@ -126,7 +126,7 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
const Point3 B1 = gtsam::cross(n, axis); const Point3 B1 = gtsam::cross(n, axis);
B.col(0) = normalize(B1); B.col(0) = normalize(B1);
B.col(1) = gtsam::cross(n, B.col(0)); B.col(1) = gtsam::cross(n, B.col(0));
B_.reset(B); B_ = (B);
} }
return *B_; return *B_;

26
gtsam/geometry/Unit3.h
View File

@ -45,8 +45,8 @@ class GTSAM_EXPORT Unit3 {
private: private:
Vector3 p_; ///< The location of the point on the unit sphere Vector3 p_; ///< The location of the point on the unit sphere
mutable boost::optional<Matrix32> B_; ///< Cached basis mutable std::optional<Matrix32> B_; ///< Cached basis
mutable boost::optional<Matrix62> H_B_; ///< Cached basis derivative mutable std::optional<Matrix62> H_B_; ///< Cached basis derivative
#ifdef GTSAM_USE_TBB #ifdef GTSAM_USE_TBB
mutable std::mutex B_mutex_; ///< Mutex to protect the cached basis. mutable std::mutex B_mutex_; ///< Mutex to protect the cached basis.
@ -98,7 +98,7 @@ public:
/// Named constructor from Point3 with optional Jacobian /// Named constructor from Point3 with optional Jacobian
static Unit3 FromPoint3(const Point3& point, // static Unit3 FromPoint3(const Point3& point, //
OptionalJacobian<2, 3> H = boost::none); OptionalJacobian<2, 3> H = {});
/** /**
* Random direction, using boost::uniform_on_sphere * Random direction, using boost::uniform_on_sphere
@ -133,16 +133,16 @@ public:
* tangent to the sphere at the current direction. * tangent to the sphere at the current direction.
* Provides derivatives of the basis with the two basis vectors stacked up as a 6x1. * Provides derivatives of the basis with the two basis vectors stacked up as a 6x1.
*/ */
const Matrix32& basis(OptionalJacobian<6, 2> H = boost::none) const; const Matrix32& basis(OptionalJacobian<6, 2> H = {}) const;
/// Return skew-symmetric associated with 3D point on unit sphere /// Return skew-symmetric associated with 3D point on unit sphere
Matrix3 skew() const; Matrix3 skew() const;
/// Return unit-norm Point3 /// Return unit-norm Point3
Point3 point3(OptionalJacobian<3, 2> H = boost::none) const; Point3 point3(OptionalJacobian<3, 2> H = {}) const;
/// Return unit-norm Vector /// Return unit-norm Vector
Vector3 unitVector(OptionalJacobian<3, 2> H = boost::none) const; Vector3 unitVector(OptionalJacobian<3, 2> H = {}) const;
/// Return scaled direction as Point3 /// Return scaled direction as Point3
friend Point3 operator*(double s, const Unit3& d) { friend Point3 operator*(double s, const Unit3& d) {
@ -150,20 +150,20 @@ public:
} }
/// Return dot product with q /// Return dot product with q
double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, // double dot(const Unit3& q, OptionalJacobian<1,2> H1 = {}, //
OptionalJacobian<1,2> H2 = boost::none) const; OptionalJacobian<1,2> H2 = {}) const;
/// Signed, vector-valued error between two directions /// Signed, vector-valued error between two directions
/// @deprecated, errorVector has the proper derivatives, this confusingly has only the second. /// @deprecated, errorVector has the proper derivatives, this confusingly has only the second.
Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const; Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = {}) const;
/// Signed, vector-valued error between two directions /// Signed, vector-valued error between two directions
/// NOTE(hayk): This method has zero derivatives if this (p) and q are orthogonal. /// NOTE(hayk): This method has zero derivatives if this (p) and q are orthogonal.
Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, // Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = {}, //
OptionalJacobian<2, 2> H_q = boost::none) const; OptionalJacobian<2, 2> H_q = {}) const;
/// Distance between two directions /// Distance between two directions
double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const; double distance(const Unit3& q, OptionalJacobian<1, 2> H = {}) const;
/// Cross-product between two Unit3s /// Cross-product between two Unit3s
Unit3 cross(const Unit3& q) const { Unit3 cross(const Unit3& q) const {
@ -196,7 +196,7 @@ public:
}; };
/// The retract function /// The retract function
Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = boost::none) const; Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = {}) const;
/// The local coordinates function /// The local coordinates function
Vector2 localCoordinates(const Unit3& s) const; Vector2 localCoordinates(const Unit3& s) const;