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

Work in progress: deprecating inline functions.

release/4.3a0
alexhagiopol 2016-04-11 11:34:57 -04:00
parent f967a54c70
commit 1feed7c20e
11 changed files with 41 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
gtsam/base/Matrix.cpp
View File

@ -38,26 +38,6 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
Matrix zeros( size_t m, size_t n ) {
return Matrix::Zero(m,n);
}
/* ************************************************************************* */
Matrix ones( size_t m, size_t n ) {
return Matrix::Ones(m,n);
}
/* ************************************************************************* */
Matrix eye( size_t m, size_t n) {
return Matrix::Identity(m, n);
}
/* ************************************************************************* */
Matrix diag(const Vector& v) {
return v.asDiagonal();
}
/* ************************************************************************* */
bool assert_equal(const Matrix& expected, const Matrix& actual, double tol) {
@ -146,16 +126,6 @@ bool linear_dependent(const Matrix& A, const Matrix& B, double tol) {
}
}
/* ************************************************************************* */
void multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e) {
e += alpha * A * x;
}
/* ************************************************************************* */
void multiplyAdd(const Matrix& A, const Vector& x, Vector& e) {
e += A * x;
}
/* ************************************************************************* */
Vector operator^(const Matrix& A, const Vector & v) {
if (A.rows()!=v.size()) throw std::invalid_argument(
@ -166,21 +136,6 @@ Vector operator^(const Matrix& A, const Vector & v) {
return A.transpose() * v;
}
/* ************************************************************************* */
void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector& x) {
x += alpha * A.transpose() * e;
}
/* ************************************************************************* */
void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x) {
x += A.transpose() * e;
}
/* ************************************************************************* */
void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x) {
x += alpha * A.transpose() * e;
}
/* ************************************************************************* */
//3 argument call
void print(const Matrix& A, const string &s, ostream& stream) {
@ -250,7 +205,7 @@ Matrix diag(const std::vector<Matrix>& Hs) {
rows+= Hs[i].rows();
cols+= Hs[i].cols();
}
Matrix results = zeros(rows,cols);
Matrix results = Matrix::Zero(rows,cols);
size_t r = 0, c = 0;
for (size_t i = 0; i<Hs.size(); ++i) {
insertSub(results, Hs[i], r, c);
@ -260,16 +215,6 @@ Matrix diag(const std::vector<Matrix>& Hs) {
return results;
}
/* ************************************************************************* */
void insertColumn(Matrix& A, const Vector& col, size_t j) {
A.col(j) = col;
}
/* ************************************************************************* */
void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j) {
A.col(j).segment(i, col.size()) = col;
}
/* ************************************************************************* */
Vector columnNormSquare(const Matrix &A) {
Vector v (A.cols()) ;
@ -279,24 +224,13 @@ Vector columnNormSquare(const Matrix &A) {
return v ;
}
/* ************************************************************************* */
void solve(Matrix& A, Matrix& B) {
// Eigen version - untested
B = A.fullPivLu().solve(B);
}
/* ************************************************************************* */
Matrix inverse(const Matrix& A) {
return A.inverse();
}
/* ************************************************************************* */
/** Householder QR factorization, Golub & Van Loan p 224, explicit version */
/* ************************************************************************* */
pair<Matrix,Matrix> qr(const Matrix& A) {
const size_t m = A.rows(), n = A.cols(), kprime = min(m,n);
Matrix Q=eye(m,m),R(A);
Matrix Q=Matrix::Identity(m,m),R(A);
Vector v(m);
// loop over the kprime first columns
@ -319,7 +253,7 @@ pair<Matrix,Matrix> qr(const Matrix& A) {
v(k) = k<j ? 0.0 : vjm(k-j);
// create Householder reflection matrix Qj = I-beta*v*v'
Matrix Qj = eye(m) - beta * v * v.transpose();
Matrix Qj = Matrix::Identity(m,m) - beta * v * v.transpose();
R = Qj * R; // update R
Q = Q * Qj; // update Q
@ -600,7 +534,7 @@ Matrix RtR(const Matrix &A)
Matrix cholesky_inverse(const Matrix &A)
{
Eigen::LLT<Matrix> llt(A);
Matrix inv = eye(A.rows());
Matrix inv = Matrix::Identity(A.rows(),A.rows());
llt.matrixU().solveInPlace<Eigen::OnTheRight>(inv);
return inv*inv.transpose();
}
@ -612,7 +546,7 @@ Matrix cholesky_inverse(const Matrix &A)
// inv(B' * B) == A
Matrix inverse_square_root(const Matrix& A) {
Eigen::LLT<Matrix> llt(A);
Matrix inv = eye(A.rows());
Matrix inv = Matrix::Identity(A.rows(),A.rows());
llt.matrixU().solveInPlace<Eigen::OnTheRight>(inv);
return inv.transpose();
}
@ -648,7 +582,7 @@ boost::tuple<int, double, Vector> DLT(const Matrix& A, double rank_tol) {
/* ************************************************************************* */
Matrix expm(const Matrix& A, size_t K) {
Matrix E = eye(A.rows()), A_k = eye(A.rows());
Matrix E = Matrix::Identity(A.rows(),A.rows()), A_k = Matrix::Identity(A.rows(),A.rows());
for(size_t k=1;k<=K;k++) {
A_k = A_k*A/double(k);
E = E + A_k;

63
gtsam/base/Matrix.h
View File

@ -33,7 +33,6 @@
#include <boost/tuple/tuple.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
/**
* Matrix is a typedef in the gtsam namespace
* TODO: make a version to work with matlab wrapping
@ -74,38 +73,6 @@ GTSAM_MAKE_MATRIX_DEFS(9);
typedef Eigen::Block<Matrix> SubMatrix;
typedef Eigen::Block<const Matrix> ConstSubMatrix;
// Matlab-like syntax
/**
* Creates an zeros matrix, with matlab-like syntax
*
* Note: if assigning a block (created from an Eigen block() function) of a matrix to zeros,
* don't use this function, instead use ".setZero(m,n)" to avoid an Eigen error.
*/
GTSAM_EXPORT Matrix zeros(size_t m, size_t n);
/**
* Creates an ones matrix, with matlab-like syntax
*/
GTSAM_EXPORT Matrix ones(size_t m, size_t n);
/**
* Creates an identity matrix, with matlab-like syntax
*
* Note: if assigning a block (created from an Eigen block() function) of a matrix to identity,
* don't use this function, instead use ".setIdentity(m,n)" to avoid an Eigen error.
*/
GTSAM_EXPORT Matrix eye(size_t m, size_t n);
/**
* Creates a square identity matrix, with matlab-like syntax
*
* Note: if assigning a block (created from an Eigen block() function) of a matrix to identity,
* don't use this function, instead use ".setIdentity(m)" to avoid an Eigen error.
*/
inline Matrix eye( size_t m ) { return eye(m,m); }
GTSAM_EXPORT Matrix diag(const Vector& v);
/**
* equals with an tolerance
*/
@ -292,8 +259,6 @@ const typename MATRIX::ConstRowXpr row(const MATRIX& A, size_t j) {
GTSAM_EXPORT void insertColumn(Matrix& A, const Vector& col, size_t j);
GTSAM_EXPORT void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j);
GTSAM_EXPORT Vector columnNormSquare(const Matrix &A);
/**
* Zeros all of the elements below the diagonal of a matrix, in place
* @param A is a matrix, to be modified in place
@ -492,12 +457,6 @@ inline Matrix3 skewSymmetric(const Eigen::MatrixBase<Derived>& w) {
/** Use Cholesky to calculate inverse square root of a matrix */
GTSAM_EXPORT Matrix inverse_square_root(const Matrix& A);
/** Calculate the LL^t decomposition of a S.P.D matrix */
GTSAM_EXPORT Matrix LLt(const Matrix& A);
/** Calculate the R^tR decomposition of a S.P.D matrix */
GTSAM_EXPORT Matrix RtR(const Matrix& A);
/** Return the inverse of a S.P.D. matrix. Inversion is done via Cholesky decomposition. */
GTSAM_EXPORT Matrix cholesky_inverse(const Matrix &A);
@ -603,6 +562,28 @@ struct MultiplyWithInverseFunction {
const Operator phi_;
};
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
inline Matrix zeros( size_t m, size_t n ) { return Matrix::Zero(m,n); }
inline Matrix ones( size_t m, size_t n ) { return Matrix::Ones(m,n); }
inline Matrix eye( size_t m, size_t n) { return Matrix::Identity(m, n); }
inline Matrix eye( size_t m ) { return eye(m,m); }
inline Matrix diag(const Vector& v) { return v.asDiagonal(); }
inline void multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e) { e += alpha * A * x; }
inline void multiplyAdd(const Matrix& A, const Vector& x, Vector& e) { e += A * x; }
inline void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector& x) { x += alpha * A.transpose() * e; }
inline void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x) { x += A.transpose() * e; }
inline void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x) { x += alpha * A.transpose() * e; }
inline void insertColumn(Matrix& A, const Vector& col, size_t j) { A.col(j) = col; }
inline void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j) { A.col(j).segment(i, col.size()) = col; }
inline void solve(Matrix& A, Matrix& B) { B = A.fullPivLu().solve(B); }
inline Matrix inverse(const Matrix& A) { return A.inverse(); }
#endif
GTSAM_EXPORT Matrix LLt(const Matrix& A);
GTSAM_EXPORT Matrix RtR(const Matrix& A);
GTSAM_EXPORT Vector columnNormSquare(const Matrix &A);
} // namespace gtsam
#include <boost/serialization/nvp.hpp>

2
gtsam/geometry/OrientedPlane3.cpp
View File

@ -42,7 +42,7 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
double pred_d = n_.unitVector().dot(xr.translation()) + d_;
if (Hr) {
*Hr = zeros(3, 6);
*Hr = Matrix::Zero(3, 6);
Hr->block<2, 3>(0, 0) = D_rotated_plane;
Hr->block<1, 3>(2, 3) = unit_vec;
}

2
gtsam/geometry/Pose2.cpp
View File

@ -132,7 +132,7 @@ Matrix3 Pose2::AdjointMap() const {
/* ************************************************************************* */
Matrix3 Pose2::adjointMap(const Vector3& v) {
// See Chirikjian12book2, vol.2, pg. 36
Matrix3 ad = zeros(3,3);
Matrix3 ad = Matrix::Zero(3,3);
ad(0,1) = -v[2];
ad(1,0) = v[2];
ad(0,2) = v[1];

2
gtsam/geometry/triangulation.cpp
View File

@ -31,7 +31,7 @@ Vector4 triangulateHomogeneousDLT(
size_t m = projection_matrices.size();
// Allocate DLT matrix
Matrix A = zeros(m * 2, 4);
Matrix A = Matrix::Zero(m * 2, 4);
for (size_t i = 0; i < m; i++) {
size_t row = i * 2;

2
gtsam/linear/KalmanFilter.h
View File

@ -69,7 +69,7 @@ public:
// Constructor
KalmanFilter(size_t n, Factorization method =
KALMANFILTER_DEFAULT_FACTORIZATION) :
n_(n), I_(eye(n_, n_)), function_(
n_(n), I_(Matrix::Identity(n_, n_)), function_(
method == QR ? GaussianFactorGraph::Eliminate(EliminateQR) :
GaussianFactorGraph::Eliminate(EliminateCholesky)) {
}

2
gtsam/linear/NoiseModel.h
View File

@ -341,7 +341,7 @@ namespace gtsam {
* Return R itself, but note that Whiten(H) is cheaper than R*H
*/
virtual Matrix R() const {
return diag(invsigmas());
return invsigmas().asDiagonal();
}
private:

2
gtsam/slam/InitializePose3.cpp
View File

@ -32,7 +32,7 @@ namespace gtsam {
namespace InitializePose3 {
//static const Matrix I = eye(1);
static const Matrix I9 = eye(9);
static const Matrix I9 = Matrix::Identity(9,9);
static const Vector zero9 = Vector::Zero(9);
static const Matrix zero33= Matrix::Zero(3,3);

2
gtsam/slam/PriorFactor.h
View File

@ -86,7 +86,7 @@ namespace gtsam {
/** vector of errors */
Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const {
if (H) (*H) = eye(traits<T>::GetDimension(x));
if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x));
// manifold equivalent of z-x -> Local(x,z)
// TODO(ASL) Add Jacobians.
return -traits<T>::Local(x, prior_);

8
gtsam/slam/dataset.cpp
View File

@ -472,7 +472,7 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
<< p.x() << " " << p.y() << " " << p.z() << " " << R.toQuaternion().x()
<< " " << R.toQuaternion().y() << " " << R.toQuaternion().z() << " " << R.toQuaternion().w();
Matrix InfoG2o = eye(6);
Matrix InfoG2o = Matrix::Identity(6,6);
InfoG2o.block(0,0,3,3) = Info.block(3,3,3,3); // cov translation
InfoG2o.block(3,3,3,3) = Info.block(0,0,3,3); // cov rotation
InfoG2o.block(0,3,3,3) = Info.block(0,3,3,3); // off diagonal
@ -539,7 +539,7 @@ GraphAndValues load3D(const string& filename) {
ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;
Rot3 R = Rot3::Ypr(yaw,pitch,roll);
Point3 t = Point3(x, y, z);
Matrix m = eye(6);
Matrix m = Matrix::Identity(6,6);
for (int i = 0; i < 6; i++)
for (int j = i; j < 6; j++)
ls >> m(i, j);
@ -549,7 +549,7 @@ GraphAndValues load3D(const string& filename) {
graph->push_back(factor);
}
if (tag == "EDGE_SE3:QUAT") {
Matrix m = eye(6);
Matrix m = Matrix::Identity(6,6);
Key id1, id2;
double x, y, z, qx, qy, qz, qw;
ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw;
@ -563,7 +563,7 @@ GraphAndValues load3D(const string& filename) {
m(j, i) = mij;
}
}
Matrix mgtsam = eye(6);
Matrix mgtsam = Matrix::Identity(6,6);
mgtsam.block(0,0,3,3) = m.block(3,3,3,3); // cov rotation
mgtsam.block(3,3,3,3) = m.block(0,0,3,3); // cov translation
mgtsam.block(0,3,3,3) = m.block(0,3,3,3); // off diagonal

4
gtsam/slam/lago.cpp
View File

@ -30,8 +30,8 @@ using namespace std;
namespace gtsam {
namespace lago {
static const Matrix I = eye(1);
static const Matrix I3 = eye(3);
static const Matrix I = Matrix::Identity(1,1);
static const Matrix I3 = Matrix::Identity(3,3);
static const Key keyAnchor = symbol('Z', 9999999);
static const noiseModel::Diagonal::shared_ptr priorOrientationNoise =