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;
// Cache the result and jacobian
H_B_.reset(jacobian);
B_.reset(B);
H_B_ = (jacobian);
B_ = (B);
}
// 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);
B.col(0) = normalize(B1);
B.col(1) = gtsam::cross(n, B.col(0));
B_.reset(B);
B_ = (B);
}
return *B_;

26
gtsam/geometry/Unit3.h
View File

@ -45,8 +45,8 @@ class GTSAM_EXPORT Unit3 {
private:
Vector3 p_; ///< The location of the point on the unit sphere
mutable boost::optional<Matrix32> B_; ///< Cached basis
mutable boost::optional<Matrix62> H_B_; ///< Cached basis derivative
mutable std::optional<Matrix32> B_; ///< Cached basis
mutable std::optional<Matrix62> H_B_; ///< Cached basis derivative
#ifdef GTSAM_USE_TBB
mutable std::mutex B_mutex_; ///< Mutex to protect the cached basis.
@ -98,7 +98,7 @@ public:
/// Named constructor from Point3 with optional Jacobian
static Unit3 FromPoint3(const Point3& point, //
OptionalJacobian<2, 3> H = boost::none);
OptionalJacobian<2, 3> H = {});
/**
* Random direction, using boost::uniform_on_sphere
@ -133,16 +133,16 @@ public:
* tangent to the sphere at the current direction.
* 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
Matrix3 skew() const;
/// Return unit-norm Point3
Point3 point3(OptionalJacobian<3, 2> H = boost::none) const;
Point3 point3(OptionalJacobian<3, 2> H = {}) const;
/// 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
friend Point3 operator*(double s, const Unit3& d) {
@ -150,20 +150,20 @@ public:
}
/// Return dot product with q
double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, //
OptionalJacobian<1,2> H2 = boost::none) const;
double dot(const Unit3& q, OptionalJacobian<1,2> H1 = {}, //
OptionalJacobian<1,2> H2 = {}) const;
/// Signed, vector-valued error between two directions
/// @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
/// 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, //
OptionalJacobian<2, 2> H_q = boost::none) const;
Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = {}, //
OptionalJacobian<2, 2> H_q = {}) const;
/// 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
Unit3 cross(const Unit3& q) const {
@ -196,7 +196,7 @@ public:
};
/// 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
Vector2 localCoordinates(const Unit3& s) const;