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

added unitVector() function to Unit3

release/4.3a0
nsrinivasan7 2015-02-11 14:45:13 -05:00
parent 1b634b5472
commit aefd213cba
2 changed files with 9 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/Unit3.cpp
View File

@ -108,7 +108,7 @@ Matrix3 Unit3::skew() const {
}
/* ************************************************************************* */
Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const {
Vector Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const {
// 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
Matrix23 Bt = basis().transpose();
Vector2 xi = Bt * q.p_.vector();

9
gtsam/geometry/Unit3.h
View File

@ -105,13 +105,20 @@ public:
return p_;
}
/// Return unit-norm Vector
Vector unitVector(boost::optional<Matrix&> H = boost::none) const {
if (H)
*H = basis();
return (p_.vector ());
}
/// Return scaled direction as Point3
friend Point3 operator*(double s, const Unit3& d) {
return s * d.p_;
}
/// Signed, vector-valued error between two directions
Vector2 error(const Unit3& q,
Vector error(const Unit3& q,
OptionalJacobian<2,2> H = boost::none) const;
/// Distance between two directions