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

Inlined skewSymmetric

release/4.3a0
Frank Dellaert 2016-01-17 19:22:14 -08:00
parent 25948677c5
commit 3c1ddd7a3f
2 changed files with 9 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/base/Matrix.cpp
View File

@ -580,15 +580,6 @@ Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask) {
return M;
}
/* ************************************************************************* */
Matrix3 skewSymmetric(double wx, double wy, double wz)
{
return (Matrix3() <<
0.0, -wz, +wy,
+wz, 0.0, -wx,
-wy, +wx, 0.0).finished();
}
/* ************************************************************************* */
Matrix LLt(const Matrix& A)
{

12
gtsam/base/Matrix.h
View File

@ -477,9 +477,15 @@ GTSAM_EXPORT Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask
* @param wz
* @return a 3*3 skew symmetric matrix
*/
GTSAM_EXPORT Matrix3 skewSymmetric(double wx, double wy, double wz);
template<class Derived>
inline Matrix3 skewSymmetric(const Eigen::MatrixBase<Derived>& w) { return skewSymmetric(w(0),w(1),w(2));}
inline Matrix3 skewSymmetric(double wx, double wy, double wz) {
return (Matrix3() << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0).finished();
}
template <class Derived>
inline Matrix3 skewSymmetric(const Eigen::MatrixBase<Derived>& w) {
return skewSymmetric(w(0), w(1), w(2));
}
/** Use Cholesky to calculate inverse square root of a matrix */
GTSAM_EXPORT Matrix inverse_square_root(const Matrix& A);