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

Merged develop into feature/fixExpressionFactorKeys

release/4.3a0
Frank Dellaert 2016-04-16 10:36:48 -07:00
parent 8149bffcb7 bcf34f0fea
commit 2c05664731
162 changed files with 1120 additions and 1320 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
gtsam.h
View File

@ -1798,9 +1798,6 @@ class Values {
void insert(size_t j, Vector t);
void insert(size_t j, Matrix t);
// Fixed size version
void insertFixed(size_t j, Vector t, size_t n);
void update(size_t j, const gtsam::Point2& t);
void update(size_t j, const gtsam::Point3& t);
void update(size_t j, const gtsam::Rot2& t);
@ -1818,12 +1815,6 @@ class Values {
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, Matrix}>
T at(size_t j);
/// Fixed size versions, for n in 1..9
void insertFixed(size_t j, Vector t, size_t n);
/// Fixed size versions, for n in 1..9
Vector atFixed(size_t j, size_t n);
/// version for double
void insertDouble(size_t j, double c);
double atDouble(size_t j) const;
@ -2489,10 +2480,30 @@ class NavState {
gtsam::Pose3 pose() const;
};
#include <gtsam/navigation/PreintegratedRotation.h>
virtual class PreintegratedRotationParams {
PreintegratedRotationParams();
void setGyroscopeCovariance(Matrix cov);
void setOmegaCoriolis(Vector omega);
void setBodyPSensor(const gtsam::Pose3& pose);
Matrix getGyroscopeCovariance() const;
// TODO(frank): allow optional
// boost::optional<Vector3> getOmegaCoriolis() const;
// boost::optional<Pose3> getBodyPSensor() const;
};
#include <gtsam/navigation/PreintegrationParams.h>
class PreintegrationParams {
virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
PreintegrationParams(Vector n_gravity);
// TODO(frank): add setters/getters or make this MATLAB wrapper feature
void setAccelerometerCovariance(Matrix cov);
void setIntegrationCovariance(Matrix cov);
void setUse2ndOrderCoriolis(bool flag);
Matrix getAccelerometerCovariance() const;
Matrix getIntegrationCovariance() const;
bool getUse2ndOrderCoriolis() const;
};
#include <gtsam/navigation/PreintegrationBase.h>

82
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
@ -339,7 +273,7 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
list<boost::tuple<Vector, double, double> > results;
Vector pseudo(m); // allocate storage for pseudo-inverse
Vector weights = reciprocal(emul(sigmas,sigmas)); // calculate weights once
Vector weights = sigmas.array().square().inverse(); // calculate weights once
// We loop over all columns, because the columns that can be eliminated
// are not necessarily contiguous. For each one, estimate the corresponding
@ -356,7 +290,7 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
if (precision < 1e-8) continue;
// create solution and copy into r
Vector r(basis(n, j));
Vector r(Vector::Unit(n,j));
for (size_t j2=j+1; j2<n; ++j2)
r(j2) = pseudo.dot(A.col(j2));
@ -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;

118
gtsam/base/Matrix.h
View File

@ -16,6 +16,7 @@
* @author Kai Ni
* @author Frank Dellaert
* @author Alex Cunningham
* @author Alex Hagiopol
*/
// \callgraph
@ -23,17 +24,17 @@
#pragma once
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/Vector.h>
#include <gtsam/config.h> // Configuration from CMake
#include <gtsam/config.h>
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <Eigen/LU>
#endif
#include <boost/format.hpp>
#include <boost/function.hpp>
#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,40 +75,8 @@ 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
* equals with a tolerance
*/
template <class MATRIX>
bool equal_with_abs_tol(const Eigen::DenseBase<MATRIX>& A, const Eigen::DenseBase<MATRIX>& B, double tol = 1e-9) {
@ -166,37 +135,12 @@ GTSAM_EXPORT bool linear_independent(const Matrix& A, const Matrix& B, double to
*/
GTSAM_EXPORT bool linear_dependent(const Matrix& A, const Matrix& B, double tol = 1e-9);
/**
* BLAS Level-2 style e <- e + alpha*A*x
*/
GTSAM_EXPORT void multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e);
/**
* BLAS Level-2 style e <- e + A*x
*/
GTSAM_EXPORT void multiplyAdd(const Matrix& A, const Vector& x, Vector& e);
/**
* overload ^ for trans(A)*v
* We transpose the vectors for speed.
*/
GTSAM_EXPORT Vector operator^(const Matrix& A, const Vector & v);
/**
* BLAS Level-2 style x <- x + alpha*A'*e
*/
GTSAM_EXPORT void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector& x);
/**
* BLAS Level-2 style x <- x + A'*e
*/
GTSAM_EXPORT void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x);
/**
* BLAS Level-2 style x <- x + alpha*A'*e
*/
GTSAM_EXPORT void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x);
/** products using old-style format to improve compatibility */
template<class MATRIX>
inline MATRIX prod(const MATRIX& A, const MATRIX&B) {
@ -281,19 +225,6 @@ const typename MATRIX::ConstRowXpr row(const MATRIX& A, size_t j) {
return A.row(j);
}
/**
* inserts a column into a matrix IN PLACE
* NOTE: there is no size checking
* Alternate form allows for vectors smaller than the whole column to be inserted
* @param A matrix to be modified in place
* @param col is the vector to be inserted
* @param j is the index to insert the column
*/
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
@ -355,17 +286,6 @@ inline typename Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::ReshapedTy
return Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::reshape(m);
}
/**
* solve AX=B via in-place Lu factorization and backsubstitution
* After calling, A contains LU, B the solved RHS vectors
*/
GTSAM_EXPORT void solve(Matrix& A, Matrix& B);
/**
* invert A
*/
GTSAM_EXPORT Matrix inverse(const Matrix& A);
/**
* QR factorization, inefficient, best use imperative householder below
* m*n matrix -> m*m Q, m*n R
@ -492,12 +412,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 +517,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>

75
gtsam/base/Vector.cpp
View File

@ -33,25 +33,6 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
bool zero(const Vector& v) {
bool result = true;
size_t n = v.size();
for( size_t j = 0 ; j < n ; j++)
result = result && (v(j) == 0.0);
return result;
}
/* ************************************************************************* */
Vector repeat(size_t n, double value) {
return Vector::Constant(n, value);
}
/* ************************************************************************* */
Vector delta(size_t n, size_t i, double value) {
return Vector::Unit(n, i) * value;
}
/* ************************************************************************* */
//3 argument call
void print(const Vector& v, const string& s, ostream& stream) {
@ -176,28 +157,6 @@ bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol) {
return flag;
}
/* ************************************************************************* */
ConstSubVector sub(const Vector &v, size_t i1, size_t i2) {
return v.segment(i1,i2-i1);
}
/* ************************************************************************* */
void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {
fullVector.segment(i, subVector.size()) = subVector;
}
/* ************************************************************************* */
Vector emul(const Vector &a, const Vector &b) {
assert (b.size()==a.size());
return a.cwiseProduct(b);
}
/* ************************************************************************* */
Vector ediv(const Vector &a, const Vector &b) {
assert (b.size()==a.size());
return a.cwiseQuotient(b);
}
/* ************************************************************************* */
Vector ediv_(const Vector &a, const Vector &b) {
size_t n = a.size();
@ -210,36 +169,6 @@ Vector ediv_(const Vector &a, const Vector &b) {
return c;
}
/* ************************************************************************* */
double sum(const Vector &a) {
return a.sum();
}
/* ************************************************************************* */
double norm_2(const Vector& v) {
return v.norm();
}
/* ************************************************************************* */
Vector reciprocal(const Vector &a) {
return a.array().inverse();
}
/* ************************************************************************* */
Vector esqrt(const Vector& v) {
return v.cwiseSqrt();
}
/* ************************************************************************* */
Vector abs(const Vector& v) {
return v.cwiseAbs();
}
/* ************************************************************************* */
double max(const Vector &a) {
return a.maxCoeff();
}
/* ************************************************************************* */
// imperative version, pass in x
double houseInPlace(Vector &v) {
@ -292,7 +221,7 @@ double weightedPseudoinverse(const Vector& a, const Vector& weights,
// Basically, instead of doing a normal QR step with the weighted
// pseudoinverse, we enforce the constraint by turning
// ax + AS = b into x + (A/a)S = b/a, for the first row where a!=0
pseudo = delta(m, i, 1.0 / a[i]);
pseudo = Vector::Unit(m,i)*(1.0/a[i]);
return inf;
}
}
@ -363,6 +292,4 @@ Vector concatVectors(size_t nrVectors, ...)
return concatVectors(vs);
}
/* ************************************************************************* */
} // namespace gtsam

146
gtsam/base/Vector.h
View File

@ -14,13 +14,12 @@
* @brief typedef and functions to augment Eigen's VectorXd
* @author Kai Ni
* @author Frank Dellaert
* @author Alex Hagiopol
*/
// \callgraph
#pragma once
#ifndef MKL_BLAS
#define MKL_BLAS MKL_DOMAIN_BLAS
#endif
@ -64,54 +63,6 @@ GTSAM_MAKE_VECTOR_DEFS(12);
typedef Eigen::VectorBlock<Vector> SubVector;
typedef Eigen::VectorBlock<const Vector> ConstSubVector;
/**
* Create vector initialized to a constant value
* @param n is the size of the vector
* @param value is a constant value to insert into the vector
*/
GTSAM_EXPORT Vector repeat(size_t n, double value);
/**
* Create basis vector of dimension n,
* with a constant in spot i
* @param n is the size of the vector
* @param i index of the one
* @param value is the value to insert into the vector
* @return delta vector
*/
GTSAM_EXPORT Vector delta(size_t n, size_t i, double value);
/**
* Create basis vector of dimension n,
* with one in spot i
* @param n is the size of the vector
* @param i index of the one
* @return basis vector
*/
inline Vector basis(size_t n, size_t i) { return delta(n, i, 1.0); }
/**
* Create zero vector
* @param n size
*/
inline Vector zero(size_t n) { return Vector::Zero(n);}
/**
* Create vector initialized to ones
* @param n size
*/
inline Vector ones(size_t n) { return Vector::Ones(n); }
/**
* check if all zero
*/
GTSAM_EXPORT bool zero(const Vector& v);
/**
* dimensionality == size
*/
inline size_t dim(const Vector& v) { return v.size(); }
/**
* print without optional string, must specify cout yourself
*/
@ -196,39 +147,6 @@ GTSAM_EXPORT bool assert_equal(const ConstSubVector& vec1, const ConstSubVector&
*/
GTSAM_EXPORT bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol=1e-9);
/**
* extract subvector, slice semantics, i.e. range = [i1,i2[ excluding i2
* @param v Vector
* @param i1 first row index
* @param i2 last row index + 1
* @return subvector v(i1:i2)
*/
GTSAM_EXPORT ConstSubVector sub(const Vector &v, size_t i1, size_t i2);
/**
* Inserts a subvector into a vector IN PLACE
* @param fullVector is the vector to be changed
* @param subVector is the vector to insert
* @param i is the index where the subvector should be inserted
*/
GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t i);
/**
* elementwise multiplication
* @param a first vector
* @param b second vector
* @return vector [a(i)*b(i)]
*/
GTSAM_EXPORT Vector emul(const Vector &a, const Vector &b);
/**
* elementwise division
* @param a first vector
* @param b second vector
* @return vector [a(i)/b(i)]
*/
GTSAM_EXPORT Vector ediv(const Vector &a, const Vector &b);
/**
* elementwise division, but 0/0 = 0, not inf
* @param a first vector
@ -237,49 +155,6 @@ GTSAM_EXPORT Vector ediv(const Vector &a, const Vector &b);
*/
GTSAM_EXPORT Vector ediv_(const Vector &a, const Vector &b);
/**
* sum vector elements
* @param a vector
* @return sum_i a(i)
*/
GTSAM_EXPORT double sum(const Vector &a);
/**
* Calculates L2 norm for a vector
* modeled after boost.ublas for compatibility
* @param v vector
* @return the L2 norm
*/
GTSAM_EXPORT double norm_2(const Vector& v);
/**
* Elementwise reciprocal of vector elements
* @param a vector
* @return [1/a(i)]
*/
GTSAM_EXPORT Vector reciprocal(const Vector &a);
/**
* Elementwise sqrt of vector elements
* @param v is a vector
* @return [sqrt(a(i))]
*/
GTSAM_EXPORT Vector esqrt(const Vector& v);
/**
* Absolute values of vector elements
* @param v is a vector
* @return [abs(a(i))]
*/
GTSAM_EXPORT Vector abs(const Vector& v);
/**
* Return the max element of a vector
* @param a is a vector
* @return max(a)
*/
GTSAM_EXPORT double max(const Vector &a);
/**
* Dot product
*/
@ -356,6 +231,25 @@ GTSAM_EXPORT Vector concatVectors(const std::list<Vector>& vs);
*/
GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
inline Vector abs(const Vector& v){return v.cwiseAbs();}
inline Vector basis(size_t n, size_t i) { return Vector::Unit(n,i); }
inline Vector delta(size_t n, size_t i, double value){ return Vector::Unit(n, i) * value;}
inline size_t dim(const Vector& v) { return v.size(); }
inline Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);}
inline Vector esqrt(const Vector& v) { return v.cwiseSqrt();}
inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);}
inline double max(const Vector &a){return a.maxCoeff();}
inline double norm_2(const Vector& v) {return v.norm();}
inline Vector ones(size_t n) { return Vector::Ones(n); }
inline Vector reciprocal(const Vector &a) {return a.array().inverse();}
inline Vector repeat(size_t n, double value) {return Vector::Constant(n, value);}
inline const Vector sub(const Vector &v, size_t i1, size_t i2) {return v.segment(i1,i2-i1);}
inline void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;}
inline double sum(const Vector &a){return a.sum();}
inline bool zero(const Vector& v){ return v.isZero(); }
inline Vector zero(size_t n) { return Vector::Zero(n); }
#endif
} // namespace gtsam
#include <boost/serialization/nvp.hpp>

4
gtsam/base/numericalDerivative.h
View File

@ -88,7 +88,7 @@ typename internal::FixedSizeMatrix<X>::type numericalGradient(boost::function<do
TangentX d;
d.setZero();
Vector g = zero(N); // Can be fixed size
Eigen::Matrix<double,N,1> g; g.setZero(); // Can be fixed size
for (int j = 0; j < N; j++) {
d(j) = delta;
double hxplus = h(traits<X>::Retract(x, d));
@ -142,7 +142,7 @@ typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(boost::funct
dx.setZero();
// Fill in Jacobian H
Matrix H = zeros(m, N);
Matrix H = Matrix::Zero(m, N);
const double factor = 1.0 / (2.0 * delta);
for (int j = 0; j < N; j++) {
dx(j) = delta;

114
gtsam/base/tests/testMatrix.cpp
View File

@ -156,8 +156,8 @@ TEST(Matrix, collect2 )
TEST(Matrix, collect3 )
{
Matrix A, B;
A = eye(2, 3);
B = eye(2, 3);
A = Matrix::Identity(2,3);
B = Matrix::Identity(2,3);
vector<const Matrix*> matrices;
matrices.push_back(&A);
matrices.push_back(&B);
@ -211,48 +211,6 @@ TEST(Matrix, column )
EXPECT(assert_equal(a3, exp3));
}
/* ************************************************************************* */
TEST(Matrix, insert_column )
{
Matrix big = zeros(5, 6);
Vector col = ones(5);
size_t j = 3;
insertColumn(big, col, j);
Matrix expected = (Matrix(5, 6) <<
0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0).finished();
EXPECT(assert_equal(expected, big));
}
/* ************************************************************************* */
TEST(Matrix, insert_subcolumn )
{
Matrix big = zeros(5, 6);
Vector col1 = ones(2);
size_t i = 1;
size_t j = 3;
insertColumn(big, col1, i, j); // check 1
Vector col2 = ones(1);
insertColumn(big, col2, 4, 5); // check 2
Matrix expected = (Matrix(5, 6) <<
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 1.0).finished();
EXPECT(assert_equal(expected, big));
}
/* ************************************************************************* */
TEST(Matrix, row )
{
@ -272,26 +230,10 @@ TEST(Matrix, row )
EXPECT(assert_equal(a3, exp3));
}
/* ************************************************************************* */
TEST(Matrix, zeros )
{
Matrix A(2, 3);
A(0, 0) = 0;
A(0, 1) = 0;
A(0, 2) = 0;
A(1, 0) = 0;
A(1, 1) = 0;
A(1, 2) = 0;
Matrix zero = zeros(2, 3);
EQUALITY(A , zero);
}
/* ************************************************************************* */
TEST(Matrix, insert_sub )
{
Matrix big = zeros(5, 6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0,
Matrix big = Matrix::Zero(5,6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0,
1.0).finished();
insertSub(big, small, 1, 2);
@ -307,9 +249,9 @@ TEST(Matrix, insert_sub )
TEST(Matrix, diagMatrices )
{
std::vector<Matrix> Hs;
Hs.push_back(ones(3,3));
Hs.push_back(ones(4,4)*2);
Hs.push_back(ones(2,2)*3);
Hs.push_back(Matrix::Ones(3,3));
Hs.push_back(Matrix::Ones(4,4)*2);
Hs.push_back(Matrix::Ones(2,2)*3);
Matrix actual = diag(Hs);
@ -723,9 +665,9 @@ TEST(Matrix, inverse )
A(2, 1) = 0;
A(2, 2) = 6;
Matrix Ainv = inverse(A);
EXPECT(assert_equal(eye(3), A*Ainv));
EXPECT(assert_equal(eye(3), Ainv*A));
Matrix Ainv = A.inverse();
EXPECT(assert_equal((Matrix) I_3x3, A*Ainv));
EXPECT(assert_equal((Matrix) I_3x3, Ainv*A));
Matrix expected(3, 3);
expected(0, 0) = 1.0909;
@ -746,13 +688,13 @@ TEST(Matrix, inverse )
0.0, -1.0, 1.0,
1.0, 0.0, 2.0,
0.0, 0.0, 1.0).finished(),
inverse(lMg)));
lMg.inverse()));
Matrix gMl((Matrix(3, 3) << 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0).finished());
EXPECT(assert_equal((Matrix(3, 3) <<
0.0, 1.0,-2.0,
-1.0, 0.0, 1.0,
0.0, 0.0, 1.0).finished(),
inverse(gMl)));
gMl.inverse()));
}
/* ************************************************************************* */
@ -769,7 +711,7 @@ TEST(Matrix, inverse2 )
A(2, 1) = 0;
A(2, 2) = 1;
Matrix Ainv = inverse(A);
Matrix Ainv = A.inverse();
Matrix expected(3, 3);
expected(0, 0) = 0;
@ -996,7 +938,7 @@ TEST(Matrix, inverse_square_root )
10.0).finished();
EQUALITY(expected,actual);
EQUALITY(measurement_covariance,inverse(actual*actual));
EQUALITY(measurement_covariance,(actual*actual).inverse());
// Randomly generated test. This test really requires inverse to
// be working well; if it's not, there's the possibility of a
@ -1052,28 +994,6 @@ TEST(Matrix, cholesky_inverse )
EQUALITY(cholesky::M.inverse(), cholesky_inverse(cholesky::M));
}
/* ************************************************************************* */
TEST(Matrix, multiplyAdd )
{
Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished();
Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.),
expected = e + A * x;
multiplyAdd(1, A, x, e);
EXPECT(assert_equal(expected, e));
}
/* ************************************************************************* */
TEST(Matrix, transposeMultiplyAdd )
{
Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished();
Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.),
expected = x + trans(A) * e;
transposeMultiplyAdd(1, A, e, x);
EXPECT(assert_equal(expected, x));
}
/* ************************************************************************* */
TEST(Matrix, linear_dependent )
{
@ -1102,12 +1022,12 @@ TEST(Matrix, linear_dependent3 )
TEST(Matrix, svd1 )
{
Vector v = Vector3(2., 1., 0.);
Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1)
Matrix U1 = Matrix::Identity(4, 3), S1 = v.asDiagonal(), V1 = I_3x3, A = (U1 * S1)
* Matrix(trans(V1));
Matrix U, V;
Vector s;
svd(A, U, s, V);
Matrix S = diag(s);
Matrix S = s.asDiagonal();
EXPECT(assert_equal(U*S*Matrix(trans(V)),A));
EXPECT(assert_equal(S,S1));
}
@ -1158,7 +1078,7 @@ TEST(Matrix, svd3 )
V = -V;
}
Matrix S = diag(s);
Matrix S = s.asDiagonal();
Matrix t = U * S;
Matrix Vt = trans(V);
@ -1202,7 +1122,7 @@ TEST(Matrix, svd4 )
V.col(1) = -V.col(1);
}
Matrix reconstructed = U * diag(s) * trans(V);
Matrix reconstructed = U * s.asDiagonal() * trans(V);
EXPECT(assert_equal(A, reconstructed, 1e-4));
EXPECT(assert_equal(expectedU,U, 1e-3));

2
gtsam/base/tests/testSymmetricBlockMatrix.cpp
View File

@ -42,7 +42,7 @@ TEST(SymmetricBlockMatrix, ReadBlocks)
23, 29).finished();
Matrix actual1 = testBlockMatrix(1, 1);
// Test only writing the upper triangle for efficiency
Matrix actual1t = Matrix::Zero(2, 2);
Matrix actual1t = Z_2x2;
actual1t.triangularView<Eigen::Upper>() = testBlockMatrix(1, 1).triangularView();
EXPECT(assert_equal(expected1, actual1));
EXPECT(assert_equal(Matrix(expected1.triangularView<Eigen::Upper>()), actual1t));

73
gtsam/base/tests/testVector.cpp
View File

@ -79,22 +79,6 @@ TEST(Vector, copy )
EXPECT(assert_equal(a, b));
}
/* ************************************************************************* */
TEST(Vector, zero1 )
{
Vector v = Vector::Zero(2);
EXPECT(zero(v));
}
/* ************************************************************************* */
TEST(Vector, zero2 )
{
Vector a = zero(2);
Vector b = Vector::Zero(2);
EXPECT(a==b);
EXPECT(assert_equal(a, b));
}
/* ************************************************************************* */
TEST(Vector, scalar_multiply )
{
@ -119,36 +103,6 @@ TEST(Vector, negate )
EXPECT(assert_equal(b, -a));
}
/* ************************************************************************* */
TEST(Vector, sub )
{
Vector a(6);
a(0) = 10; a(1) = 20; a(2) = 3;
a(3) = 34; a(4) = 11; a(5) = 2;
Vector result(sub(a,2,5));
Vector b(3);
b(0) = 3; b(1) = 34; b(2) =11;
EXPECT(b==result);
EXPECT(assert_equal(b, result));
}
/* ************************************************************************* */
TEST(Vector, subInsert )
{
Vector big = zero(6),
small = ones(3);
size_t i = 2;
subInsert(big, small, i);
Vector expected = (Vector(6) << 0.0, 0.0, 1.0, 1.0, 1.0, 0.0).finished();
EXPECT(assert_equal(expected, big));
}
/* ************************************************************************* */
TEST(Vector, householder )
{
@ -198,7 +152,7 @@ TEST(Vector, weightedPseudoinverse )
// create sigmas
Vector sigmas(2);
sigmas(0) = 0.1; sigmas(1) = 0.2;
Vector weights = reciprocal(emul(sigmas,sigmas));
Vector weights = sigmas.array().square().inverse();
// perform solve
Vector actual; double precision;
@ -224,8 +178,7 @@ TEST(Vector, weightedPseudoinverse_constraint )
// create sigmas
Vector sigmas(2);
sigmas(0) = 0.0; sigmas(1) = 0.2;
Vector weights = reciprocal(emul(sigmas,sigmas));
Vector weights = sigmas.array().square().inverse();
// perform solve
Vector actual; double precision;
boost::tie(actual, precision) = weightedPseudoinverse(x, weights);
@ -244,7 +197,7 @@ TEST(Vector, weightedPseudoinverse_nan )
{
Vector a = (Vector(4) << 1., 0., 0., 0.).finished();
Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.).finished();
Vector weights = reciprocal(emul(sigmas,sigmas));
Vector weights = sigmas.array().square().inverse();
Vector pseudo; double precision;
boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights);
@ -253,17 +206,6 @@ TEST(Vector, weightedPseudoinverse_nan )
DOUBLES_EQUAL(100, precision, 1e-5);
}
/* ************************************************************************* */
TEST(Vector, ediv )
{
Vector a = Vector3(10., 20., 30.);
Vector b = Vector3(2.0, 5.0, 6.0);
Vector actual(ediv(a,b));
Vector c = Vector3(5.0, 4.0, 5.0);
EXPECT(assert_equal(c,actual));
}
/* ************************************************************************* */
TEST(Vector, dot )
{
@ -298,18 +240,11 @@ TEST(Vector, equals )
TEST(Vector, greater_than )
{
Vector v1 = Vector3(1.0, 2.0, 3.0),
v2 = zero(3);
v2 = Z_3x1;
EXPECT(greaterThanOrEqual(v1, v1)); // test basic greater than
EXPECT(greaterThanOrEqual(v1, v2)); // test equals
}
/* ************************************************************************* */
TEST(Vector, reciprocal )
{
Vector v = Vector3(1.0, 2.0, 4.0);
EXPECT(assert_equal(Vector3(1.0, 0.5, 0.25),reciprocal(v)));
}
/* ************************************************************************* */
TEST(Vector, linear_dependent )
{

2
gtsam/discrete/tests/testDiscreteMarginals.cpp
View File

@ -167,7 +167,7 @@ TEST_UNSAFE( DiscreteMarginals, truss2 ) {
// Calculate the marginals by brute force
vector<DiscreteFactor::Values> allPosbValues = cartesianProduct(
key[0] & key[1] & key[2] & key[3] & key[4]);
Vector T = zero(5), F = zero(5);
Vector T = Z_5x1, F = Z_5x1;
for (size_t i = 0; i < allPosbValues.size(); ++i) {
DiscreteFactor::Values x = allPosbValues[i];
double px = graph(x);

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 = Z_3x3;
ad(0,1) = -v[2];
ad(1,0) = v[2];
ad(0,2) = v[1];

2
gtsam/geometry/Rot2.cpp
View File

@ -63,7 +63,7 @@ Rot2& Rot2::normalize() {
Rot2 Rot2::Expmap(const Vector1& v, OptionalJacobian<1, 1> H) {
if (H)
*H = I_1x1;
if (zero(v))
if (v.isZero())
return (Rot2());
else
return Rot2::fromAngle(v(0));

4
gtsam/geometry/Rot2.h
View File

@ -119,12 +119,12 @@ namespace gtsam {
/// Left-trivialized derivative of the exponential map
static Matrix ExpmapDerivative(const Vector& /*v*/) {
return ones(1);
return I_1x1;
}
/// Left-trivialized derivative inverse of the exponential map
static Matrix LogmapDerivative(const Vector& /*v*/) {
return ones(1);
return I_1x1;
}
// Chart at origin simply uses exponential map and its inverse

4
gtsam/geometry/tests/testCal3_S2.cpp
View File

@ -122,8 +122,8 @@ TEST(Cal3_S2, between) {
Matrix H1, H2;
EXPECT(assert_equal(Cal3_S2(0,1,2,3,4), k1.between(k2, H1, H2)));
EXPECT(assert_equal(-eye(5), H1));
EXPECT(assert_equal(eye(5), H2));
EXPECT(assert_equal(-I_5x5, H1));
EXPECT(assert_equal(I_5x5, H2));
}

6
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -62,7 +62,7 @@ TEST (EssentialMatrix, FromPose3) {
//*******************************************************************************
TEST(EssentialMatrix, localCoordinates0) {
EssentialMatrix E;
Vector expected = zero(5);
Vector expected = Z_5x1;
Vector actual = E.localCoordinates(E);
EXPECT(assert_equal(expected, actual, 1e-8));
}
@ -74,7 +74,7 @@ TEST (EssentialMatrix, localCoordinates) {
Pose3 pose(trueRotation, trueTranslation);
EssentialMatrix hx = EssentialMatrix::FromPose3(pose);
Vector actual = hx.localCoordinates(EssentialMatrix::FromPose3(pose));
EXPECT(assert_equal(zero(5), actual, 1e-8));
EXPECT(assert_equal(Z_5x1, actual, 1e-8));
Vector6 d;
d << 0.1, 0.2, 0.3, 0, 0, 0;
@ -85,7 +85,7 @@ TEST (EssentialMatrix, localCoordinates) {
//*************************************************************************
TEST (EssentialMatrix, retract0) {
EssentialMatrix actual = trueE.retract(zero(5));
EssentialMatrix actual = trueE.retract(Z_5x1);
EXPECT(assert_equal(trueE, actual));
}

6
gtsam/geometry/tests/testOrientedPlane3.cpp
View File

@ -96,8 +96,8 @@ inline static Vector randomVector(const Vector& minLimits,
const Vector& maxLimits) {
// Get the number of dimensions and create the return vector
size_t numDims = dim(minLimits);
Vector vector = zero(numDims);
size_t numDims = minLimits.size();
Vector vector = Vector::Zero(numDims);
// Create the random vector
for (size_t i = 0; i < numDims; i++) {
@ -145,7 +145,7 @@ TEST (OrientedPlane3, error2) {
OrientedPlane3 plane2(-1.1, 0.2, 0.3, 5.4);
// Hard-coded regression values, to ensure the result doesn't change.
EXPECT(assert_equal(zero(3), plane1.errorVector(plane1), 1e-8));
EXPECT(assert_equal((Vector) Z_3x1, plane1.errorVector(plane1), 1e-8));
EXPECT(assert_equal(Vector3(-0.0677674148, -0.0760543588, -0.4), plane1.errorVector(plane2), 1e-5));
// Test the jacobians of transform

2
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -116,7 +116,7 @@ TEST( PinholeCamera, lookat)
Matrix R = camera2.pose().rotation().matrix();
Matrix I = trans(R)*R;
EXPECT(assert_equal(I, eye(3)));
EXPECT(assert_equal(I, I_3x3));
}
/* ************************************************************************* */

2
gtsam/geometry/tests/testPinholePose.cpp
View File

@ -87,7 +87,7 @@ TEST( PinholePose, lookat)
Matrix R = camera2.pose().rotation().matrix();
Matrix I = trans(R)*R;
EXPECT(assert_equal(I, eye(3)));
EXPECT(assert_equal(I, I_3x3));
}
/* ************************************************************************* */

8
gtsam/geometry/tests/testPoint2.cpp
View File

@ -73,12 +73,12 @@ TEST(Point2, Lie) {
Matrix H1, H2;
EXPECT(assert_equal(Point2(5,7), traits<Point2>::Compose(p1, p2, H1, H2)));
EXPECT(assert_equal(eye(2), H1));
EXPECT(assert_equal(eye(2), H2));
EXPECT(assert_equal(I_2x2, H1));
EXPECT(assert_equal(I_2x2, H2));
EXPECT(assert_equal(Point2(3,3), traits<Point2>::Between(p1, p2, H1, H2)));
EXPECT(assert_equal(-eye(2), H1));
EXPECT(assert_equal(eye(2), H2));
EXPECT(assert_equal(-I_2x2, H1));
EXPECT(assert_equal(I_2x2, H2));
EXPECT(assert_equal(Point2(5,7), traits<Point2>::Retract(p1, Vector2(4., 5.))));
EXPECT(assert_equal(Vector2(3.,3.), traits<Point2>::Local(p1,p2)));

8
gtsam/geometry/tests/testPoint3.cpp
View File

@ -47,12 +47,12 @@ TEST(Point3, Lie) {
Matrix H1, H2;
EXPECT(assert_equal(Point3(5, 7, 9), traits<Point3>::Compose(p1, p2, H1, H2)));
EXPECT(assert_equal(eye(3), H1));
EXPECT(assert_equal(eye(3), H2));
EXPECT(assert_equal(I_3x3, H1));
EXPECT(assert_equal(I_3x3, H2));
EXPECT(assert_equal(Point3(3, 3, 3), traits<Point3>::Between(p1, p2, H1, H2)));
EXPECT(assert_equal(-eye(3), H1));
EXPECT(assert_equal(eye(3), H2));
EXPECT(assert_equal(-I_3x3, H1));
EXPECT(assert_equal(I_3x3, H2));
EXPECT(assert_equal(Point3(5, 7, 9), traits<Point3>::Retract(p1, Vector3(4,5,6))));
EXPECT(assert_equal(Vector3(3, 3, 3), traits<Point3>::Local(p1,p2)));

4
gtsam/geometry/tests/testPose2.cpp
View File

@ -102,7 +102,7 @@ TEST(Pose2, expmap3) {
0.99, 0.0, -0.015,
0.0, 0.0, 0.0).finished();
Matrix A2 = A*A/2.0, A3 = A2*A/3.0, A4=A3*A/4.0;
Matrix expected = eye(3) + A + A2 + A3 + A4;
Matrix expected = I_3x3 + A + A2 + A3 + A4;
Vector v = Vector3(0.01, -0.015, 0.99);
Pose2 pose = Pose2::Expmap(v);
@ -311,7 +311,7 @@ TEST(Pose2, compose_a)
-1.0, 0.0, 2.0,
0.0, 0.0, 1.0
).finished();
Matrix expectedH2 = eye(3);
Matrix expectedH2 = I_3x3;
Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
EXPECT(assert_equal(expectedH1,actualDcompose1));

23
gtsam/geometry/tests/testPose3.cpp
View File

@ -61,7 +61,7 @@ TEST( Pose3, constructors)
TEST( Pose3, retract_first_order)
{
Pose3 id;
Vector v = zero(6);
Vector v = Z_6x1;
v(0) = 0.3;
EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), id.retract(v),1e-2));
v(3)=0.2;v(4)=0.7;v(5)=-2;
@ -71,7 +71,7 @@ TEST( Pose3, retract_first_order)
/* ************************************************************************* */
TEST( Pose3, retract_expmap)
{
Vector v = zero(6); v(0) = 0.3;
Vector v = Z_6x1; v(0) = 0.3;
Pose3 pose = Pose3::Expmap(v);
EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), pose, 1e-2));
EXPECT(assert_equal(v,Pose3::Logmap(pose),1e-2));
@ -81,7 +81,7 @@ TEST( Pose3, retract_expmap)
TEST( Pose3, expmap_a_full)
{
Pose3 id;
Vector v = zero(6);
Vector v = Z_6x1;
v(0) = 0.3;
EXPECT(assert_equal(expmap_default<Pose3>(id, v), Pose3(R, Point3(0,0,0))));
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
@ -92,7 +92,7 @@ TEST( Pose3, expmap_a_full)
TEST( Pose3, expmap_a_full2)
{
Pose3 id;
Vector v = zero(6);
Vector v = Z_6x1;
v(0) = 0.3;
EXPECT(assert_equal(expmap_default<Pose3>(id, v), Pose3(R, Point3(0,0,0))));
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
@ -148,12 +148,12 @@ TEST(Pose3, Adjoint_full)
Pose3 Agrawal06iros(const Vector& xi) {
Vector w = xi.head(3);
Vector v = xi.tail(3);
double t = norm_2(w);
double t = w.norm();
if (t < 1e-5)
return Pose3(Rot3(), Point3(v));
else {
Matrix W = skewSymmetric(w/t);
Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W);
Matrix A = I_3x3 + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W);
return Pose3(Rot3::Expmap (w), Point3(A * v));
}
}
@ -267,7 +267,7 @@ TEST( Pose3, inverse)
{
Matrix actualDinverse;
Matrix actual = T.inverse(actualDinverse).matrix();
Matrix expected = inverse(T.matrix());
Matrix expected = T.matrix().inverse();
EXPECT(assert_equal(actual,expected,1e-8));
Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, T);
@ -293,7 +293,7 @@ TEST( Pose3, inverseDerivatives2)
TEST( Pose3, compose_inverse)
{
Matrix actual = (T*T.inverse()).matrix();
Matrix expected = eye(4,4);
Matrix expected = I_4x4;
EXPECT(assert_equal(actual,expected,1e-8));
}
@ -538,7 +538,7 @@ TEST(Pose3, retract_localCoordinates)
/* ************************************************************************* */
TEST(Pose3, expmap_logmap)
{
Vector d12 = repeat(6,0.1);
Vector d12 = Vector6::Constant(0.1);
Pose3 t1 = T, t2 = t1.expmap(d12);
EXPECT(assert_equal(d12, t1.logmap(t2)));
}
@ -712,7 +712,7 @@ TEST(Pose3, Bearing2) {
TEST( Pose3, unicycle )
{
// velocity in X should be X in inertial frame, rather than global frame
Vector x_step = delta(6,3,1.0);
Vector x_step = Vector::Unit(6,3)*1.0;
EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), l1), expmap_default<Pose3>(x1, x_step), tol));
EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), Point3(2,1,0)), expmap_default<Pose3>(x2, x_step), tol));
EXPECT(assert_equal(Pose3(Rot3::Ypr(M_PI/4.0,0,0), Point3(2,2,0)), expmap_default<Pose3>(x3, sqrt(2.0) * x_step), tol));
@ -723,9 +723,8 @@ TEST( Pose3, adjointMap) {
Matrix res = Pose3::adjointMap(screwPose3::xi);
Matrix wh = skewSymmetric(screwPose3::xi(0), screwPose3::xi(1), screwPose3::xi(2));
Matrix vh = skewSymmetric(screwPose3::xi(3), screwPose3::xi(4), screwPose3::xi(5));
Matrix Z3 = zeros(3,3);
Matrix6 expected;
expected << wh, Z3, vh, wh;
expected << wh, Z_3x3, vh, wh;
EXPECT(assert_equal(expected,res,1e-5));
}

10
gtsam/geometry/tests/testRot2.cpp
View File

@ -62,8 +62,8 @@ TEST( Rot2, compose)
Matrix H1, H2;
(void) Rot2::fromAngle(1.0).compose(Rot2::fromAngle(2.0), H1, H2);
EXPECT(assert_equal(eye(1), H1));
EXPECT(assert_equal(eye(1), H2));
EXPECT(assert_equal(I_1x1, H1));
EXPECT(assert_equal(I_1x1, H2));
}
/* ************************************************************************* */
@ -74,8 +74,8 @@ TEST( Rot2, between)
Matrix H1, H2;
(void) Rot2::fromAngle(1.0).between(Rot2::fromAngle(2.0), H1, H2);
EXPECT(assert_equal(-eye(1), H1));
EXPECT(assert_equal(eye(1), H2));
EXPECT(assert_equal(-I_1x1, H1));
EXPECT(assert_equal(I_1x1, H2));
}
/* ************************************************************************* */
@ -89,7 +89,7 @@ TEST( Rot2, equals)
/* ************************************************************************* */
TEST( Rot2, expmap)
{
Vector v = zero(1);
Vector v = Z_1x1;
CHECK(assert_equal(R.retract(v), R));
}

18
gtsam/geometry/tests/testRot3.cpp
View File

@ -96,7 +96,7 @@ TEST( Rot3, equals)
/* ************************************************************************* */
// Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + ....
Rot3 slow_but_correct_Rodrigues(const Vector& w) {
double t = norm_2(w);
double t = w.norm();
Matrix3 J = skewSymmetric(w / t);
if (t < 1e-5) return Rot3();
Matrix3 R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J);
@ -130,7 +130,7 @@ TEST( Rot3, Rodrigues2)
TEST( Rot3, Rodrigues3)
{
Vector w = Vector3(0.1, 0.2, 0.3);
Rot3 R1 = Rot3::AxisAngle(w / norm_2(w), norm_2(w));
Rot3 R1 = Rot3::AxisAngle(w / w.norm(), w.norm());
Rot3 R2 = slow_but_correct_Rodrigues(w);
CHECK(assert_equal(R2,R1));
}
@ -152,7 +152,7 @@ TEST( Rot3, Rodrigues4)
/* ************************************************************************* */
TEST( Rot3, retract)
{
Vector v = zero(3);
Vector v = Z_3x1;
CHECK(assert_equal(R, R.retract(v)));
// // test Canonical coordinates
@ -213,7 +213,7 @@ TEST(Rot3, log)
#define CHECK_OMEGA_ZERO(X,Y,Z) \
w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \
R = Rot3::Rodrigues(w); \
EXPECT(assert_equal(zero(3), Rot3::Logmap(R)));
EXPECT(assert_equal((Vector) Z_3x1, Rot3::Logmap(R)));
CHECK_OMEGA_ZERO( 2.0*PI, 0, 0)
CHECK_OMEGA_ZERO( 0, 2.0*PI, 0)
@ -224,16 +224,16 @@ TEST(Rot3, log)
/* ************************************************************************* */
TEST(Rot3, retract_localCoordinates)
{
Vector3 d12 = repeat(3,0.1);
Vector3 d12 = Vector3::Constant(0.1);
Rot3 R2 = R.retract(d12);
EXPECT(assert_equal(d12, R.localCoordinates(R2)));
}
/* ************************************************************************* */
TEST(Rot3, expmap_logmap)
{
Vector3 d12 = repeat(3,0.1);
Vector3 d12 = Vector3::Constant(0.1);
Rot3 R2 = R.expmap(d12);
EXPECT(assert_equal(d12, R.logmap(R2)));
EXPECT(assert_equal(d12, (Vector) R.logmap(R2)));
}
/* ************************************************************************* */
@ -555,8 +555,8 @@ TEST(Rot3, quaternion) {
/* ************************************************************************* */
Matrix Cayley(const Matrix& A) {
Matrix::Index n = A.cols();
const Matrix I = eye(n);
return (I-A)*inverse(I+A);
const Matrix I = Matrix::Identity(n,n);
return (I-A)*(I+A).inverse();
}
TEST( Rot3, Cayley ) {

2
gtsam/geometry/tests/testSimpleCamera.cpp
View File

@ -76,7 +76,7 @@ TEST( SimpleCamera, lookat)
Matrix R = camera2.pose().rotation().matrix();
Matrix I = trans(R)*R;
CHECK(assert_equal(I, eye(3)));
CHECK(assert_equal(I, I_3x3));
}
/* ************************************************************************* */

6
gtsam/geometry/tests/testUnit3.cpp
View File

@ -237,7 +237,7 @@ TEST(Unit3, distance) {
TEST(Unit3, localCoordinates0) {
Unit3 p;
Vector actual = p.localCoordinates(p);
EXPECT(assert_equal(zero(2), actual, 1e-8));
EXPECT(assert_equal(Z_2x1, actual, 1e-8));
}
TEST(Unit3, localCoordinates) {
@ -245,14 +245,14 @@ TEST(Unit3, localCoordinates) {
Unit3 p, q;
Vector2 expected = Vector2::Zero();
Vector2 actual = p.localCoordinates(q);
EXPECT(assert_equal(zero(2), actual, 1e-8));
EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-8));
EXPECT(assert_equal(q, p.retract(expected), 1e-8));
}
{
Unit3 p, q(1, 6.12385e-21, 0);
Vector2 expected = Vector2::Zero();
Vector2 actual = p.localCoordinates(q);
EXPECT(assert_equal(zero(2), actual, 1e-8));
EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-8));
EXPECT(assert_equal(q, p.retract(expected), 1e-8));
}
{

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;

21
gtsam/geometry/triangulation.h
View File

@ -363,6 +363,18 @@ struct TriangulationParameters {
<< p.dynamicOutlierRejectionThreshold << std::endl;
return os;
}
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(rankTolerance);
ar & BOOST_SERIALIZATION_NVP(enableEPI);
ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold);
ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold);
}
};
/**
@ -411,6 +423,15 @@ public:
os << "no point, status = " << result.status_ << std::endl;
return os;
}
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(status_);
}
};
/// triangulateSafe: extensive checking of the outcome

2
gtsam/linear/GaussianConditional.cpp
View File

@ -183,7 +183,7 @@ namespace gtsam {
if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front());
for (const_iterator it = beginParents(); it!= endParents(); it++)
gtsam::transposeMultiplyAdd(-1.0, Matrix(getA(it)), frontalVec, gy[*it]);
gy[*it] += -1.0 * Matrix(getA(it)).transpose() * frontalVec;
// Scale by sigmas
if(model_)

4
gtsam/linear/HessianFactor.cpp
View File

@ -377,7 +377,7 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
vector<Vector> y;
y.reserve(size());
for (const_iterator it = begin(); it != end(); it++)
y.push_back(zero(getDim(it)));
y.push_back(Vector::Zero(getDim(it)));
// Accessing the VectorValues one by one is expensive
// So we will loop over columns to access x only once per column
@ -427,7 +427,7 @@ void HessianFactor::gradientAtZero(double* d) const {
Vector HessianFactor::gradient(Key key, const VectorValues& x) const {
Factor::const_iterator i = find(key);
// Sum over G_ij*xj for all xj connecting to xi
Vector b = zero(x.at(key).size());
Vector b = Vector::Zero(x.at(key).size());
for (Factor::const_iterator j = begin(); j != end(); ++j) {
// Obtain Gij from the Hessian factor
// Hessian factor only stores an upper triangular matrix, so be careful when i>j

7
gtsam/linear/JacobianFactor.cpp
View File

@ -543,7 +543,7 @@ void JacobianFactor::updateHessian(const FastVector<Key>& infoKeys,
/* ************************************************************************* */
Vector JacobianFactor::operator*(const VectorValues& x) const {
Vector Ax = zero(Ab_.rows());
Vector Ax = Vector::Zero(Ab_.rows());
if (empty())
return Ax;
@ -565,8 +565,7 @@ void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e,
pair<VectorValues::iterator, bool> xi = x.tryInsert(j, Vector());
if (xi.second)
xi.first->second = Vector::Zero(getDim(begin() + pos));
gtsam::transposeMultiplyAdd(Ab_(pos), E, xi.first->second);
xi.first->second += Ab_(pos).transpose()*E;
}
}
@ -595,7 +594,7 @@ void JacobianFactor::multiplyHessianAdd(double alpha, const double* x, double* y
if (empty())
return;
Vector Ax = zero(Ab_.rows());
Vector Ax = Vector::Zero(Ab_.rows());
/// Just iterate over all A matrices and multiply in correct config part (looping over keys)
/// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]'

4
gtsam/linear/KalmanFilter.cpp
View File

@ -115,7 +115,7 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F,
// f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T
// See documentation in HessianFactor, we have A1 = -F, A2 = I_, b = B*u:
// TODO: starts to seem more elaborate than straight-up KF equations?
Matrix M = inverse(Q), Ft = trans(F);
Matrix M = Q.inverse(), Ft = trans(F);
Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M;
Vector b = B * u, g2 = M * b, g1 = -Ft * g2;
double f = dot(b, g2);
@ -147,7 +147,7 @@ KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H,
KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H,
const Vector& z, const Matrix& Q) const {
Key k = step(p);
Matrix M = inverse(Q), Ht = trans(H);
Matrix M = Q.inverse(), Ht = trans(H);
Matrix G = Ht * M * H;
Vector g = Ht * M * z;
double f = dot(z, M * z);

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)) {
}

6
gtsam/linear/NoiseModel.cpp
View File

@ -258,7 +258,7 @@ Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) {
if (variances(j) != variances(0)) goto full;
return Isotropic::Variance(n, variances(0), true);
}
full: return shared_ptr(new Diagonal(esqrt(variances)));
full: return shared_ptr(new Diagonal(variances.cwiseSqrt()));
}
/* ************************************************************************* */
@ -326,7 +326,7 @@ static void fix(const Vector& sigmas, Vector& precisions, Vector& invsigmas) {
/* ************************************************************************* */
Constrained::Constrained(const Vector& sigmas)
: Diagonal(sigmas), mu_(repeat(sigmas.size(), 1000.0)) {
: Diagonal(sigmas), mu_(Vector::Constant(sigmas.size(), 1000.0)) {
internal::fix(sigmas, precisions_, invsigmas_);
}
@ -405,7 +405,7 @@ void Constrained::WhitenInPlace(Eigen::Block<Matrix> H) const {
/* ************************************************************************* */
Constrained::shared_ptr Constrained::unit() const {
Vector sigmas = ones(dim());
Vector sigmas = Vector::Ones(dim());
for (size_t i=0; i<dim(); ++i)
if (constrained(i))
sigmas(i) = 0.0;

28
gtsam/linear/NoiseModel.h
View File

@ -309,7 +309,7 @@ namespace gtsam {
* i.e. the diagonal of the information matrix, i.e., weights
*/
static shared_ptr Precisions(const Vector& precisions, bool smart = true) {
return Variances(reciprocal(precisions), smart);
return Variances(precisions.array().inverse(), smart);
}
virtual void print(const std::string& name) const;
@ -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:
@ -381,7 +381,7 @@ namespace gtsam {
* from appearing in invsigmas or precisions.
* mu set to large default value (1000.0)
*/
Constrained(const Vector& sigmas = zero(1));
Constrained(const Vector& sigmas = Z_1x1);
/**
* Constructor that prevents any inf values
@ -416,7 +416,7 @@ namespace gtsam {
* standard devations, some of which might be zero
*/
static shared_ptr MixedSigmas(const Vector& sigmas) {
return MixedSigmas(repeat(sigmas.size(), 1000.0), sigmas);
return MixedSigmas(Vector::Constant(sigmas.size(), 1000.0), sigmas);
}
/**
@ -424,7 +424,7 @@ namespace gtsam {
* standard devations, some of which might be zero
*/
static shared_ptr MixedSigmas(double m, const Vector& sigmas) {
return MixedSigmas(repeat(sigmas.size(), m), sigmas);
return MixedSigmas(Vector::Constant(sigmas.size(), m), sigmas);
}
/**
@ -432,10 +432,10 @@ namespace gtsam {
* standard devations, some of which might be zero
*/
static shared_ptr MixedVariances(const Vector& mu, const Vector& variances) {
return shared_ptr(new Constrained(mu, esqrt(variances)));
return shared_ptr(new Constrained(mu, variances.cwiseSqrt()));
}
static shared_ptr MixedVariances(const Vector& variances) {
return shared_ptr(new Constrained(esqrt(variances)));
return shared_ptr(new Constrained(variances.cwiseSqrt()));
}
/**
@ -443,10 +443,10 @@ namespace gtsam {
* precisions, some of which might be inf
*/
static shared_ptr MixedPrecisions(const Vector& mu, const Vector& precisions) {
return MixedVariances(mu, reciprocal(precisions));
return MixedVariances(mu, precisions.array().inverse());
}
static shared_ptr MixedPrecisions(const Vector& precisions) {
return MixedVariances(reciprocal(precisions));
return MixedVariances(precisions.array().inverse());
}
/**
@ -458,17 +458,17 @@ namespace gtsam {
/** Fully constrained variations */
static shared_ptr All(size_t dim) {
return shared_ptr(new Constrained(repeat(dim, 1000.0), repeat(dim,0)));
return shared_ptr(new Constrained(Vector::Constant(dim, 1000.0), Vector::Constant(dim,0)));
}
/** Fully constrained variations */
static shared_ptr All(size_t dim, const Vector& mu) {
return shared_ptr(new Constrained(mu, repeat(dim,0)));
return shared_ptr(new Constrained(mu, Vector::Constant(dim,0)));
}
/** Fully constrained variations with a mu parameter */
static shared_ptr All(size_t dim, double mu) {
return shared_ptr(new Constrained(repeat(dim, mu), repeat(dim,0)));
return shared_ptr(new Constrained(Vector::Constant(dim, mu), Vector::Constant(dim,0)));
}
virtual void print(const std::string& name) const;
@ -522,10 +522,10 @@ namespace gtsam {
/** protected constructor takes sigma */
Isotropic(size_t dim, double sigma) :
Diagonal(repeat(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}
Diagonal(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}
/* dummy constructor to allow for serialization */
Isotropic() : Diagonal(repeat(1, 1.0)),sigma_(1.0),invsigma_(1.0) {}
Isotropic() : Diagonal(Vector1::Constant(1.0)),sigma_(1.0),invsigma_(1.0) {}
public:

4
gtsam/linear/RegularJacobianFactor.h
View File

@ -83,7 +83,7 @@ public:
void multiplyHessianAdd(double alpha, const double* x, double* y) const {
if (empty())
return;
Vector Ax = zero(Ab_.rows());
Vector Ax = Vector::Zero(Ab_.rows());
// Just iterate over all A matrices and multiply in correct config part
for (size_t pos = 0; pos < size(); ++pos)
@ -173,7 +173,7 @@ public:
* RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way
*/
Vector operator*(const double* x) const {
Vector Ax = zero(Ab_.rows());
Vector Ax = Vector::Zero(Ab_.rows());
if (empty())
return Ax;

2
gtsam/linear/VectorValues.cpp
View File

@ -52,7 +52,7 @@ namespace gtsam {
Key key;
size_t n;
boost::tie(key, n) = v;
values_.insert(make_pair(key, sub(x, j, j + n)));
values_.insert(make_pair(key, x.segment(j,n)));
j += n;
}
}

2
gtsam/linear/iterative.h
View File

@ -86,7 +86,7 @@ namespace gtsam {
/** x += alpha* A'*e */
void transposeMultiplyAdd(double alpha, const Vector& e, Vector& x) const {
gtsam::transposeMultiplyAdd(alpha, A(), e, x);
x += alpha * A().transpose() * e;
}
};

4
gtsam/linear/tests/testGaussianBayesTree.cpp
View File

@ -170,7 +170,7 @@ TEST(GaussianBayesTree, complicatedMarginal) {
LONGS_EQUAL(1, (long)actualJacobianQR.size());
LONGS_EQUAL(5, (long)actualJacobianQR.keys()[0]);
Matrix actualA = actualJacobianQR.getA(actualJacobianQR.begin());
Matrix actualCov = inverse(actualA.transpose() * actualA);
Matrix actualCov = (actualA.transpose() * actualA).inverse();
EXPECT(assert_equal(expectedCov, actualCov, 1e-1));
// Marginal on 6
@ -187,7 +187,7 @@ TEST(GaussianBayesTree, complicatedMarginal) {
LONGS_EQUAL(1, (long)actualJacobianQR.size());
LONGS_EQUAL(6, (long)actualJacobianQR.keys()[0]);
actualA = actualJacobianQR.getA(actualJacobianQR.begin());
actualCov = inverse(actualA.transpose() * actualA);
actualCov = (actualA.transpose() * actualA).inverse();
EXPECT(assert_equal(expectedCov, actualCov, 1e1));
}

20
gtsam/linear/tests/testGaussianFactorGraph.cpp
View File

@ -47,10 +47,10 @@ TEST(GaussianFactorGraph, initialization) {
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
fg +=
JacobianFactor(0, 10*eye(2), -1.0*ones(2), unit2),
JacobianFactor(0, -10*eye(2),1, 10*eye(2), Vector2(2.0, -1.0), unit2),
JacobianFactor(0, -5*eye(2), 2, 5*eye(2), Vector2(0.0, 1.0), unit2),
JacobianFactor(1, -5*eye(2), 2, 5*eye(2), Vector2(-1.0, 1.5), unit2);
JacobianFactor(0, 10*I_2x2, -1.0*Vector::Ones(2), unit2),
JacobianFactor(0, -10*I_2x2,1, 10*I_2x2, Vector2(2.0, -1.0), unit2),
JacobianFactor(0, -5*I_2x2, 2, 5*I_2x2, Vector2(0.0, 1.0), unit2),
JacobianFactor(1, -5*I_2x2, 2, 5*I_2x2, Vector2(-1.0, 1.5), unit2);
EXPECT_LONGS_EQUAL(4, (long)fg.size());
@ -166,13 +166,13 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() {
GaussianFactorGraph fg;
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2);
fg += JacobianFactor(2, 10*I_2x2, -1.0*Vector::Ones(2), unit2);
// odometry between x1 and x2: x2-x1=[0.2;-0.1]
fg += JacobianFactor(0, 10*eye(2), 2, -10*eye(2), Vector2(2.0, -1.0), unit2);
fg += JacobianFactor(0, 10*I_2x2, 2, -10*I_2x2, Vector2(2.0, -1.0), unit2);
// measurement between x1 and l1: l1-x1=[0.0;0.2]
fg += JacobianFactor(1, 5*eye(2), 2, -5*eye(2), Vector2(0.0, 1.0), unit2);
fg += JacobianFactor(1, 5*I_2x2, 2, -5*I_2x2, Vector2(0.0, 1.0), unit2);
// measurement between x2 and l1: l1-x2=[-0.2;0.3]
fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), Vector2(-1.0, 1.5), unit2);
fg += JacobianFactor(0, -5*I_2x2, 1, 5*I_2x2, Vector2(-1.0, 1.5), unit2);
return fg;
}
@ -280,8 +280,8 @@ TEST( GaussianFactorGraph, multiplyHessianAdd )
/* ************************************************************************* */
static GaussianFactorGraph createGaussianFactorGraphWithHessianFactor() {
GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
gfg += HessianFactor(1, 2, 100*eye(2,2), zeros(2,2), Vector2(0.0, 1.0),
400*eye(2,2), Vector2(1.0, 1.0), 3.0);
gfg += HessianFactor(1, 2, 100*I_2x2, Z_2x2, Vector2(0.0, 1.0),
400*I_2x2, Vector2(1.0, 1.0), 3.0);
return gfg;
}

8
gtsam/linear/tests/testHessianFactor.cpp
View File

@ -54,7 +54,7 @@ TEST(HessianFactor, emptyConstructor)
HessianFactor f;
DOUBLES_EQUAL(0.0, f.constantTerm(), 1e-9); // Constant term should be zero
EXPECT(assert_equal(Vector(), f.linearTerm())); // Linear term should be empty
EXPECT(assert_equal(zeros(1,1), f.info())); // Full matrix should be 1-by-1 zero matrix
EXPECT(assert_equal((Matrix) Z_1x1, f.info())); // Full matrix should be 1-by-1 zero matrix
DOUBLES_EQUAL(0.0, f.error(VectorValues()), 1e-9); // Should have zero error
}
@ -123,11 +123,11 @@ TEST(HessianFactor, Constructor1)
TEST(HessianFactor, Constructor1b)
{
Vector mu = Vector2(1.0,2.0);
Matrix Sigma = eye(2,2);
Matrix Sigma = I_2x2;
HessianFactor factor(0, mu, Sigma);
Matrix G = eye(2,2);
Matrix G = I_2x2;
Vector g = G*mu;
double f = dot(g,mu);
@ -484,7 +484,7 @@ TEST(HessianFactor, combine) {
-8.94427191, 0.0,
0.0, -8.94427191).finished();
Vector b = Vector2(2.23606798,-1.56524758);
SharedDiagonal model = noiseModel::Diagonal::Sigmas(ones(2));
SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector::Ones(2));
GaussianFactor::shared_ptr f(new JacobianFactor(0, A0, 1, A1, 2, A2, b, model));
GaussianFactorGraph factors = list_of(f);

40
gtsam/linear/tests/testJacobianFactor.cpp
View File

@ -168,19 +168,19 @@ namespace simple_graph {
Key keyX(10), keyY(8), keyZ(12);
double sigma1 = 0.1;
Matrix A11 = Matrix::Identity(2, 2);
Matrix A11 = I_2x2;
Vector2 b1(2, -1);
JacobianFactor factor1(keyX, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1));
double sigma2 = 0.5;
Matrix A21 = -2 * Matrix::Identity(2, 2);
Matrix A22 = 3 * Matrix::Identity(2, 2);
Matrix A21 = -2 * I_2x2;
Matrix A22 = 3 * I_2x2;
Vector2 b2(4, -5);
JacobianFactor factor2(keyX, A21, keyY, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2));
double sigma3 = 1.0;
Matrix A32 = -4 * Matrix::Identity(2, 2);
Matrix A33 = 5 * Matrix::Identity(2, 2);
Matrix A32 = -4 * I_2x2;
Matrix A33 = 5 * I_2x2;
Vector2 b3(3, -6);
JacobianFactor factor3(keyY, A32, keyZ, A33, b3, noiseModel::Isotropic::Sigma(2, sigma3));
@ -193,8 +193,8 @@ TEST( JacobianFactor, construct_from_graph)
{
using namespace simple_graph;
Matrix A1(6,2); A1 << A11, A21, Matrix::Zero(2,2);
Matrix A2(6,2); A2 << Matrix::Zero(2,2), A22, A32;
Matrix A1(6,2); A1 << A11, A21, Z_2x2;
Matrix A2(6,2); A2 << Z_2x2, A22, A32;
Matrix A3(6,2); A3 << Matrix::Zero(4,2), A33;
Vector b(6); b << b1, b2, b3;
Vector sigmas(6); sigmas << sigma1, sigma1, sigma2, sigma2, sigma3, sigma3;
@ -260,17 +260,17 @@ TEST(JacobianFactor, matrices_NULL)
// hessianDiagonal
VectorValues expectDiagonal;
expectDiagonal.insert(5, ones(3));
expectDiagonal.insert(10, 4*ones(3));
expectDiagonal.insert(15, 9*ones(3));
expectDiagonal.insert(5, Vector::Ones(3));
expectDiagonal.insert(10, 4*Vector::Ones(3));
expectDiagonal.insert(15, 9*Vector::Ones(3));
EXPECT(assert_equal(expectDiagonal, factor.hessianDiagonal()));
// hessianBlockDiagonal
map<Key,Matrix> actualBD = factor.hessianBlockDiagonal();
LONGS_EQUAL(3,actualBD.size());
EXPECT(assert_equal(1*eye(3),actualBD[5]));
EXPECT(assert_equal(4*eye(3),actualBD[10]));
EXPECT(assert_equal(9*eye(3),actualBD[15]));
EXPECT(assert_equal(1*I_3x3,actualBD[5]));
EXPECT(assert_equal(4*I_3x3,actualBD[10]));
EXPECT(assert_equal(9*I_3x3,actualBD[15]));
}
/* ************************************************************************* */
@ -314,9 +314,9 @@ TEST(JacobianFactor, matrices)
// hessianBlockDiagonal
map<Key,Matrix> actualBD = factor.hessianBlockDiagonal();
LONGS_EQUAL(3,actualBD.size());
EXPECT(assert_equal(4*eye(3),actualBD[5]));
EXPECT(assert_equal(16*eye(3),actualBD[10]));
EXPECT(assert_equal(36*eye(3),actualBD[15]));
EXPECT(assert_equal(4*I_3x3,actualBD[5]));
EXPECT(assert_equal(16*I_3x3,actualBD[10]));
EXPECT(assert_equal(36*I_3x3,actualBD[15]));
}
/* ************************************************************************* */
@ -324,7 +324,7 @@ TEST(JacobianFactor, operators )
{
SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
Matrix I = eye(2);
Matrix I = I_2x2;
Vector b = Vector2(0.2,-0.1);
JacobianFactor lf(1, -I, 2, I, b, sigma0_1);
@ -405,7 +405,7 @@ TEST(JacobianFactor, eliminate)
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
Matrix zero3x3 = zeros(3,3);
Matrix zero3x3 = Matrix::Zero(3,3);
Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
Vector9 b; b << b1, b0, b2;
@ -561,7 +561,7 @@ TEST ( JacobianFactor, constraint_eliminate1 )
{
// construct a linear constraint
Vector v(2); v(0)=1.2; v(1)=3.4;
JacobianFactor lc(1, eye(2), v, noiseModel::Constrained::All(2));
JacobianFactor lc(1, I_2x2, v, noiseModel::Constrained::All(2));
// eliminate it
pair<GaussianConditional::shared_ptr, JacobianFactor::shared_ptr>
@ -572,7 +572,7 @@ TEST ( JacobianFactor, constraint_eliminate1 )
// verify conditional Gaussian
Vector sigmas = Vector2(0.0, 0.0);
GaussianConditional expCG(1, v, eye(2), noiseModel::Diagonal::Sigmas(sigmas));
GaussianConditional expCG(1, v, I_2x2, noiseModel::Diagonal::Sigmas(sigmas));
EXPECT(assert_equal(expCG, *actual.first));
}

34
gtsam/linear/tests/testKalmanFilter.cpp
View File

@ -51,7 +51,7 @@ TEST( KalmanFilter, constructor ) {
EXPECT(assert_equal(x_initial, p1->mean()));
Matrix Sigma = (Matrix(2, 2) << 0.01, 0.0, 0.0, 0.01).finished();
EXPECT(assert_equal(Sigma, p1->covariance()));
EXPECT(assert_equal(inverse(Sigma), p1->information()));
EXPECT(assert_equal(Sigma.inverse(), p1->information()));
// Create one with a sharedGaussian
KalmanFilter::State p2 = kf1.init(x_initial, Sigma);
@ -65,33 +65,33 @@ TEST( KalmanFilter, constructor ) {
TEST( KalmanFilter, linear1 ) {
// Create the controls and measurement properties for our example
Matrix F = eye(2, 2);
Matrix B = eye(2, 2);
Matrix F = I_2x2;
Matrix B = I_2x2;
Vector u = Vector2(1.0, 0.0);
SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1);
Matrix Q = 0.01*eye(2, 2);
Matrix H = eye(2, 2);
Matrix Q = 0.01*I_2x2;
Matrix H = I_2x2;
State z1(1.0, 0.0);
State z2(2.0, 0.0);
State z3(3.0, 0.0);
SharedDiagonal modelR = noiseModel::Isotropic::Sigma(2, 0.1);
Matrix R = 0.01*eye(2, 2);
Matrix R = 0.01*I_2x2;
// Create the set of expected output TestValues
State expected0(0.0, 0.0);
Matrix P00 = 0.01*eye(2, 2);
Matrix P00 = 0.01*I_2x2;
State expected1(1.0, 0.0);
Matrix P01 = P00 + Q;
Matrix I11 = inverse(P01) + inverse(R);
Matrix I11 = P01.inverse() + R.inverse();
State expected2(2.0, 0.0);
Matrix P12 = inverse(I11) + Q;
Matrix I22 = inverse(P12) + inverse(R);
Matrix P12 = I11.inverse() + Q;
Matrix I22 = P12.inverse() + R.inverse();
State expected3(3.0, 0.0);
Matrix P23 = inverse(I22) + Q;
Matrix I33 = inverse(P23) + inverse(R);
Matrix P23 = I22.inverse() + Q;
Matrix I33 = P23.inverse() + R.inverse();
// Create a Kalman filter of dimension 2
KalmanFilter kf(2);
@ -140,7 +140,7 @@ TEST( KalmanFilter, predict ) {
Vector u = Vector3(1.0, 0.0, 2.0);
Matrix R = (Matrix(2, 2) << 1.0, 0.5, 0.0, 3.0).finished();
Matrix M = trans(R)*R;
Matrix Q = inverse(M);
Matrix Q = M.inverse();
// Create a Kalman filter of dimension 2
KalmanFilter kf(2);
@ -167,7 +167,7 @@ TEST( KalmanFilter, predict ) {
// Test both QR and Cholesky versions in case of a realistic (AHRS) dynamics update
TEST( KalmanFilter, QRvsCholesky ) {
Vector mean = ones(9);
Vector mean = Vector::Ones(9);
Matrix covariance = 1e-6 * (Matrix(9, 9) <<
15.0, -6.2, 0.0, 0.0, 0.0, 0.0, 0.0, 63.8, -0.6,
-6.2, 21.9, -0.0, 0.0, 0.0, 0.0, -63.8, -0.0, -0.1,
@ -197,8 +197,8 @@ TEST( KalmanFilter, QRvsCholesky ) {
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0).finished();
Matrix B = zeros(9, 1);
Vector u = zero(1);
Matrix B = Matrix::Zero(9, 1);
Vector u = Z_1x1;
Matrix dt_Q_k = 1e-6 * (Matrix(9, 9) <<
33.7, 3.1, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
3.1, 126.4, -0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
@ -270,7 +270,7 @@ TEST( KalmanFilter, QRvsCholesky ) {
EXPECT(assert_equal(expected2, pb2->covariance(), 1e-7));
// do the above update again, this time with a full Matrix Q
Matrix modelQ = diag(emul(sigmas,sigmas));
Matrix modelQ = ((Matrix) sigmas.array().square()).asDiagonal();
KalmanFilter::State pa3 = kfa.updateQ(pa, H, z, modelQ);
KalmanFilter::State pb3 = kfb.updateQ(pb, H, z, modelQ);

28
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -112,7 +112,7 @@ TEST(NoiseModel, Unit)
TEST(NoiseModel, equals)
{
Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R),
g2 = Gaussian::SqrtInformation(eye(3,3));
g2 = Gaussian::SqrtInformation(I_3x3);
Diagonal::shared_ptr d1 = Diagonal::Sigmas(Vector3(kSigma, kSigma, kSigma)),
d2 = Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3));
Isotropic::shared_ptr i1 = Isotropic::Sigma(3, kSigma),
@ -155,13 +155,13 @@ TEST(NoiseModel, ConstrainedConstructors )
Vector3 mu(200.0, 300.0, 400.0);
actual = Constrained::All(d);
// TODO: why should this be a thousand ??? Dummy variable?
EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu()));
EXPECT(assert_equal(gtsam::repeat(d, 0), actual->sigmas()));
EXPECT(assert_equal(gtsam::repeat(d, 0), actual->invsigmas())); // Actually zero as dummy value
EXPECT(assert_equal(gtsam::repeat(d, 0), actual->precisions())); // Actually zero as dummy value
EXPECT(assert_equal(Vector::Constant(d, 1000.0), actual->mu()));
EXPECT(assert_equal(Vector::Constant(d, 0), actual->sigmas()));
EXPECT(assert_equal(Vector::Constant(d, 0), actual->invsigmas())); // Actually zero as dummy value
EXPECT(assert_equal(Vector::Constant(d, 0), actual->precisions())); // Actually zero as dummy value
actual = Constrained::All(d, m);
EXPECT(assert_equal(gtsam::repeat(d, m), actual->mu()));
EXPECT(assert_equal(Vector::Constant(d, m), actual->mu()));
actual = Constrained::All(d, mu);
EXPECT(assert_equal(mu, actual->mu()));
@ -170,7 +170,7 @@ TEST(NoiseModel, ConstrainedConstructors )
EXPECT(assert_equal(mu, actual->mu()));
actual = Constrained::MixedSigmas(m, sigmas);
EXPECT(assert_equal( gtsam::repeat(d, m), actual->mu()));
EXPECT(assert_equal(Vector::Constant(d, m), actual->mu()));
}
/* ************************************************************************* */
@ -395,7 +395,7 @@ TEST(NoiseModel, SmartSqrtInformation )
{
bool smart = true;
gtsam::SharedGaussian expected = Unit::Create(3);
gtsam::SharedGaussian actual = Gaussian::SqrtInformation(eye(3), smart);
gtsam::SharedGaussian actual = Gaussian::SqrtInformation(I_3x3, smart);
EXPECT(assert_equal(*expected,*actual));
}
@ -404,7 +404,7 @@ TEST(NoiseModel, SmartSqrtInformation2 )
{
bool smart = true;
gtsam::SharedGaussian expected = Unit::Isotropic::Sigma(3,2);
gtsam::SharedGaussian actual = Gaussian::SqrtInformation(0.5*eye(3), smart);
gtsam::SharedGaussian actual = Gaussian::SqrtInformation(0.5*I_3x3, smart);
EXPECT(assert_equal(*expected,*actual));
}
@ -413,7 +413,7 @@ TEST(NoiseModel, SmartInformation )
{
bool smart = true;
gtsam::SharedGaussian expected = Unit::Isotropic::Variance(3,2);
Matrix M = 0.5*eye(3);
Matrix M = 0.5*I_3x3;
EXPECT(checkIfDiagonal(M));
gtsam::SharedGaussian actual = Gaussian::Information(M, smart);
EXPECT(assert_equal(*expected,*actual));
@ -424,7 +424,7 @@ TEST(NoiseModel, SmartCovariance )
{
bool smart = true;
gtsam::SharedGaussian expected = Unit::Create(3);
gtsam::SharedGaussian actual = Gaussian::Covariance(eye(3), smart);
gtsam::SharedGaussian actual = Gaussian::Covariance(I_3x3, smart);
EXPECT(assert_equal(*expected,*actual));
}
@ -433,7 +433,7 @@ TEST(NoiseModel, ScalarOrVector )
{
bool smart = true;
SharedGaussian expected = Unit::Create(3);
SharedGaussian actual = Gaussian::Covariance(eye(3), smart);
SharedGaussian actual = Gaussian::Covariance(I_3x3, smart);
EXPECT(assert_equal(*expected,*actual));
}
@ -442,9 +442,9 @@ TEST(NoiseModel, WhitenInPlace)
{
Vector sigmas = Vector3(0.1, 0.1, 0.1);
SharedDiagonal model = Diagonal::Sigmas(sigmas);
Matrix A = eye(3);
Matrix A = I_3x3;
model->WhitenInPlace(A);
Matrix expected = eye(3) * 10;
Matrix expected = I_3x3 * 10;
EXPECT(assert_equal(expected, A));
}

10
gtsam/linear/tests/testSerializationLinear.cpp
View File

@ -51,7 +51,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
/* ************************************************************************* */
// example noise models
static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3));
static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3));
static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * I_3x3);
static noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2);
static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector3(0.0, 0.0, 0.1));
static noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3);
@ -144,8 +144,8 @@ TEST (Serialization, linear_factors) {
EXPECT(equalsBinary<VectorValues>(values));
Key i1 = 4, i2 = 7;
Matrix A1 = eye(3), A2 = -1.0 * eye(3);
Vector b = ones(3);
Matrix A1 = I_3x3, A2 = -1.0 * I_3x3;
Vector b = Vector::Ones(3);
SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(1.0, 2.0, 3.0));
JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model);
EXPECT(equalsObj(jacobianfactor));
@ -185,8 +185,8 @@ TEST (Serialization, gaussian_factor_graph) {
{
Key i1 = 4, i2 = 7;
Matrix A1 = eye(3), A2 = -1.0 * eye(3);
Vector b = ones(3);
Matrix A1 = I_3x3, A2 = -1.0 * I_3x3;
Vector b = Vector::Ones(3);
SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(1.0, 2.0, 3.0));
JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model);
HessianFactor hessianfactor(jacobianfactor);

2
gtsam/navigation/AttitudeFactor.h
View File

@ -189,7 +189,7 @@ public:
Vector e = attitudeError(nTb.rotation(), H);
if (H) {
Matrix H23 = *H;
*H = Matrix::Zero(2, 6);
*H = Matrix::Zero(2,6);
H->block<2,3>(0,0) = H23;
}
return e;

4
gtsam/navigation/MagFactor.h
View File

@ -154,7 +154,7 @@ public:
// measured bM = nRb<52> * nM + b, where b is unknown bias
Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias;
if (H2)
*H2 = eye(3);
*H2 = I_3x3;
return (hx - measured_);
}
};
@ -205,7 +205,7 @@ public:
*H2 = scale * H * (*H2);
}
if (H3)
*H3 = eye(3);
*H3 = I_3x3;
return (hx - measured_);
}
};

56
gtsam/navigation/PreintegratedRotation.h
View File

@ -26,6 +26,38 @@
namespace gtsam {
/// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct PreintegratedRotationParams {
Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements
boost::optional<Vector3> omegaCoriolis; ///< Coriolis constant
boost::optional<Pose3> body_P_sensor; ///< The pose of the sensor in the body frame
PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {}
virtual void print(const std::string& s) const;
virtual bool equals(const PreintegratedRotationParams& other, double tol=1e-9) const;
void setGyroscopeCovariance(const Matrix3& cov) { gyroscopeCovariance = cov; }
void setOmegaCoriolis(const Vector3& omega) { omegaCoriolis.reset(omega); }
void setBodyPSensor(const Pose3& pose) { body_P_sensor.reset(pose); }
const Matrix3& getGyroscopeCovariance() const { return gyroscopeCovariance; }
boost::optional<Vector3> getOmegaCoriolis() const { return omegaCoriolis; }
boost::optional<Pose3> getBodyPSensor() const { return body_P_sensor; }
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size()));
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
}
};
/**
* PreintegratedRotation is the base class for all PreintegratedMeasurements
* classes (in AHRSFactor, ImuFactor, and CombinedImuFactor).
@ -33,29 +65,7 @@ namespace gtsam {
*/
class PreintegratedRotation {
public:
/// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct Params {
Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements
boost::optional<Vector3> omegaCoriolis; ///< Coriolis constant
boost::optional<Pose3> body_P_sensor; ///< The pose of the sensor in the body frame
Params() : gyroscopeCovariance(I_3x3) {}
virtual void print(const std::string& s) const;
virtual bool equals(const Params& other, double tol=1e-9) const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size()));
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
}
};
typedef PreintegratedRotationParams Params;
protected:
/// Parameters

2
gtsam/navigation/PreintegrationBase.cpp
View File

@ -105,7 +105,7 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
// Update derivative: centrifugal causes the correlation between acc and omega!!!
if (correctedAcc_H_unbiasedOmega) {
double wdp = correctedOmega.dot(b_arm);
*correctedAcc_H_unbiasedOmega = -(diag(Vector3::Constant(wdp))
*correctedAcc_H_unbiasedOmega = -( (Matrix) Vector3::Constant(wdp).asDiagonal()
+ correctedOmega * b_arm.transpose()) * bRs.matrix()
+ 2 * b_arm * unbiasedOmega.transpose();
}

12
gtsam/navigation/PreintegrationParams.h
View File

@ -23,7 +23,7 @@ namespace gtsam {
/// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct PreintegrationParams: PreintegratedRotation::Params {
struct PreintegrationParams: PreintegratedRotationParams {
Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
@ -50,6 +50,14 @@ struct PreintegrationParams: PreintegratedRotation::Params {
void print(const std::string& s) const;
bool equals(const PreintegratedRotation::Params& other, double tol) const;
void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; }
void setIntegrationCovariance(const Matrix3& cov) { integrationCovariance = cov; }
void setUse2ndOrderCoriolis(bool flag) { use2ndOrderCoriolis = flag; }
const Matrix3& getAccelerometerCovariance() const { return accelerometerCovariance; }
const Matrix3& getIntegrationCovariance() const { return integrationCovariance; }
bool getUse2ndOrderCoriolis() const { return use2ndOrderCoriolis; }
protected:
/// Default constructor for serialization only: uninitialized!
PreintegrationParams() {}
@ -60,7 +68,7 @@ protected:
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & boost::serialization::make_nvp("PreintegratedRotation_Params",
boost::serialization::base_object<PreintegratedRotation::Params>(*this));
boost::serialization::base_object<PreintegratedRotationParams>(*this));
ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size()));
ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size()));
ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);

2
gtsam/navigation/tests/testAHRSFactor.cpp
View File

@ -268,7 +268,7 @@ TEST( AHRSFactor, PartialDerivativeLogmap ) {
const Vector3 x = thetahat; // parametrization of so(3)
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
double normx = norm_2(x);
double normx = x.norm();
const Matrix3 actualDelFdeltheta = I_3x3 + 0.5 * X
+ (1 / (normx * normx) - (1 + cos(normx)) / (2 * normx * sin(normx))) * X
* X;

4
gtsam/navigation/tests/testAttitudeFactor.cpp
View File

@ -42,7 +42,7 @@ TEST( Rot3AttitudeFactor, Constructor ) {
// Create a linearization point at the zero-error point
Rot3 nRb;
EXPECT(assert_equal(zero(2),factor.evaluateError(nRb),1e-5));
EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(nRb),1e-5));
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector,Rot3>(
@ -75,7 +75,7 @@ TEST( Pose3AttitudeFactor, Constructor ) {
// Create a linearization point at the zero-error point
Pose3 T(Rot3(), Point3(-5.0, 8.0, -11.0));
EXPECT(assert_equal(zero(2),factor.evaluateError(T),1e-5));
EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(T),1e-5));
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector,Pose3>(

4
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -58,7 +58,7 @@ TEST( GPSFactor, Constructor ) {
// Create a linearization point at zero error
Pose3 T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U));
EXPECT(assert_equal(zero(3),factor.evaluateError(T),1e-5));
EXPECT(assert_equal(Z_3x3,factor.evaluateError(T),1e-5));
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector,Pose3>(
@ -87,7 +87,7 @@ TEST( GPSFactor2, Constructor ) {
// Create a linearization point at zero error
NavState T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U), Vector3::Zero());
EXPECT(assert_equal(zero(3),factor.evaluateError(T),1e-5));
EXPECT(assert_equal(Z_3x3,factor.evaluateError(T),1e-5));
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector,NavState>(

8
gtsam/navigation/tests/testMagFactor.cpp
View File

@ -71,19 +71,19 @@ TEST( MagFactor, Factors ) {
// MagFactor
MagFactor f(1, measured, s, dir, bias, model);
EXPECT( assert_equal(zero(3),f.evaluateError(theta,H1),1e-5));
EXPECT( assert_equal(Z_3x3,f.evaluateError(theta,H1),1e-5));
EXPECT( assert_equal((Matrix)numericalDerivative11<Vector,Rot2> //
(boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7));
// MagFactor1
MagFactor1 f1(1, measured, s, dir, bias, model);
EXPECT( assert_equal(zero(3),f1.evaluateError(nRb,H1),1e-5));
EXPECT( assert_equal(Z_3x3,f1.evaluateError(nRb,H1),1e-5));
EXPECT( assert_equal(numericalDerivative11<Vector,Rot3> //
(boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7));
// MagFactor2
MagFactor2 f2(1, 2, measured, nRb, model);
EXPECT( assert_equal(zero(3),f2.evaluateError(scaled,bias,H1,H2),1e-5));
EXPECT( assert_equal(Z_3x3,f2.evaluateError(scaled,bias,H1,H2),1e-5));
EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
(boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),//
H1, 1e-7));
@ -93,7 +93,7 @@ TEST( MagFactor, Factors ) {
// MagFactor2
MagFactor3 f3(1, 2, 3, measured, nRb, model);
EXPECT(assert_equal(zero(3),f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
EXPECT(assert_equal(Z_3x3,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> //
(boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),//
H1, 1e-7));

14
gtsam/nonlinear/NonlinearEquality.h
View File

@ -142,18 +142,18 @@ public:
const size_t nj = traits<T>::GetDimension(feasible_);
if (allow_error_) {
if (H)
*H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare
*H = Matrix::Identity(nj,nj); // FIXME: this is not the right linearization for nonlinear compare
return traits<T>::Local(xj,feasible_);
} else if (compare_(feasible_, xj)) {
if (H)
*H = eye(nj);
return zero(nj); // set error to zero if equal
*H = Matrix::Identity(nj,nj);
return Vector::Zero(nj); // set error to zero if equal
} else {
if (H)
throw std::invalid_argument(
"Linearization point not feasible for "
+ DefaultKeyFormatter(this->key()) + "!");
return repeat(nj, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
return Vector::Constant(nj, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
}
}
@ -249,7 +249,7 @@ public:
Vector evaluateError(const X& x1,
boost::optional<Matrix&> H = boost::none) const {
if (H)
(*H) = eye(traits<X>::GetDimension(x1));
(*H) = Matrix::Identity(traits<X>::GetDimension(x1),traits<X>::GetDimension(x1));
// manifold equivalent of h(x)-z -> log(z,h(x))
return traits<X>::Local(value_,x1);
}
@ -322,8 +322,8 @@ public:
Vector evaluateError(const X& x1, const X& x2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
static const size_t p = traits<X>::dimension;
if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p);
if (H1) *H1 = -Matrix::Identity(p,p);
if (H2) *H2 = Matrix::Identity(p,p);
return traits<X>::Local(x1,x2);
}

12
gtsam/nonlinear/NonlinearFactor.h
View File

@ -313,7 +313,7 @@ public:
return evaluateError(x1);
}
} else {
return zero(this->dim());
return Vector::Zero(this->dim());
}
}
@ -388,7 +388,7 @@ public:
return evaluateError(x1, x2);
}
} else {
return zero(this->dim());
return Vector::Zero(this->dim());
}
}
@ -463,7 +463,7 @@ public:
else
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]));
} else {
return zero(this->dim());
return Vector::Zero(this->dim());
}
}
@ -543,7 +543,7 @@ public:
else
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]));
} else {
return zero(this->dim());
return Vector::Zero(this->dim());
}
}
@ -627,7 +627,7 @@ public:
else
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]));
} else {
return zero(this->dim());
return Vector::Zero(this->dim());
}
}
@ -715,7 +715,7 @@ public:
else
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]));
} else {
return zero(this->dim());
return Vector::Zero(this->dim());
}
}

161
gtsam/nonlinear/Values-inl.h
View File

@ -269,25 +269,114 @@ namespace gtsam {
return filter(key_value.key);
}
/* ************************************************************************* */
template<typename ValueType>
const ValueType& Values::at(Key j) const {
// Find the item
KeyValueMap::const_iterator item = values_.find(j);
/* ************************************************************************* */
// Throw exception if it does not exist
if(item == values_.end())
throw ValuesKeyDoesNotExist("retrieve", j);
namespace internal {
// Check the type and throw exception if incorrect
const Value& value = *item->second;
try {
return dynamic_cast<const GenericValue<ValueType>&>(value).value();
} catch (std::bad_cast &) {
// NOTE(abe): clang warns about potential side effects if done in typeid
const Value* value = item->second;
throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType));
}
// Generic version, partially specialized below for various Eigen Matrix types
template<typename ValueType>
struct handle {
ValueType operator()(Key j, const gtsam::Value * const pointer) {
try {
// value returns a const ValueType&, and the return makes a copy !!!!!
return dynamic_cast<const GenericValue<ValueType>&>(*pointer).value();
} catch (std::bad_cast &) {
throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType));
}
}
};
// Handle dynamic vectors
template<>
struct handle<Eigen::Matrix<double, -1, 1> > {
Eigen::Matrix<double, -1, 1> operator()(Key j,
const gtsam::Value * const pointer) {
try {
// value returns a const Vector&, and the return makes a copy !!!!!
return dynamic_cast<const GenericValue<Eigen::Matrix<double, -1, 1> >&>(*pointer).value();
} catch (std::bad_cast &) {
// If a fixed vector was stored, we end up here as well.
throw ValuesIncorrectType(j, typeid(*pointer),
typeid(Eigen::Matrix<double, -1, 1>));
}
}
};
// Handle dynamic matrices
template<int N>
struct handle<Eigen::Matrix<double, -1, N> > {
Eigen::Matrix<double, -1, N> operator()(Key j,
const gtsam::Value * const pointer) {
try {
// value returns a const Matrix&, and the return makes a copy !!!!!
return dynamic_cast<const GenericValue<Eigen::Matrix<double, -1, N> >&>(*pointer).value();
} catch (std::bad_cast &) {
// If a fixed matrix was stored, we end up here as well.
throw ValuesIncorrectType(j, typeid(*pointer),
typeid(Eigen::Matrix<double, -1, N>));
}
}
};
// Request for a fixed vector
// TODO(jing): is this piece of code really needed ???
template<int M>
struct handle<Eigen::Matrix<double, M, 1> > {
Eigen::Matrix<double, M, 1> operator()(Key j,
const gtsam::Value * const pointer) {
try {
// value returns a const VectorM&, and the return makes a copy !!!!!
return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, 1> >&>(*pointer).value();
} catch (std::bad_cast &) {
// Check if a dynamic vector was stored
Matrix A = handle<Eigen::VectorXd>()(j, pointer); // will throw if not....
// Yes: check size, and throw if not a match
if (A.rows() != M || A.cols() != 1)
throw NoMatchFoundForFixed(M, 1, A.rows(), A.cols());
else
// This is not a copy because of RVO
return A;
}
}
};
// Request for a fixed matrix
template<int M, int N>
struct handle<Eigen::Matrix<double, M, N> > {
Eigen::Matrix<double, M, N> operator()(Key j,
const gtsam::Value * const pointer) {
try {
// value returns a const MatrixMN&, and the return makes a copy !!!!!
return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N> >&>(*pointer).value();
} catch (std::bad_cast &) {
// Check if a dynamic matrix was stored
Matrix A = handle<Eigen::MatrixXd>()(j, pointer); // will throw if not....
// Yes: check size, and throw if not a match
if (A.rows() != M || A.cols() != N)
throw NoMatchFoundForFixed(M, N, A.rows(), A.cols());
else
// This is not a copy because of RVO
return A;
}
}
};
} // internal
/* ************************************************************************* */
template<typename ValueType>
ValueType Values::at(Key j) const {
// Find the item
KeyValueMap::const_iterator item = values_.find(j);
// Throw exception if it does not exist
if(item == values_.end())
throw ValuesKeyDoesNotExist("at", j);
// Check the type and throw exception if incorrect
return internal::handle<ValueType>()(j,item->second);
}
/* ************************************************************************* */
@ -312,16 +401,48 @@ namespace gtsam {
}
/* ************************************************************************* */
// wrap all fixed in dynamics when insert and update
namespace internal {
// general type
template<typename ValueType>
struct handle_wrap {
inline gtsam::GenericValue<ValueType> operator()(Key j, const ValueType& val) {
return gtsam::GenericValue<ValueType>(val);
}
};
// when insert/update a fixed size vector: convert to dynamic size
template<int M>
struct handle_wrap<Eigen::Matrix<double, M, 1> > {
inline gtsam::GenericValue<Eigen::Matrix<double, -1, 1> > operator()(
Key j, const Eigen::Matrix<double, M, 1>& val) {
return gtsam::GenericValue<Eigen::Matrix<double, -1, 1> >(val);
}
};
// when insert/update a fixed size matrix: convert to dynamic size
template<int M, int N>
struct handle_wrap<Eigen::Matrix<double, M, N> > {
inline gtsam::GenericValue<Eigen::Matrix<double, -1, -1> > operator()(
Key j, const Eigen::Matrix<double, M, N>& val) {
return gtsam::GenericValue<Eigen::Matrix<double, -1, -1> >(val);
}
};
}
// insert a templated value
template<typename ValueType>
void Values::insert(Key j, const ValueType& val) {
insert(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
}
void Values::insert(Key j, const ValueType& val) {
insert(j, static_cast<const Value&>(internal::handle_wrap<ValueType>()(j, val)));
}
// update with templated value
template <typename ValueType>
void Values::update(Key j, const ValueType& val) {
update(j, static_cast<const Value&>(GenericValue<ValueType >(val)));
update(j, static_cast<const Value&>(internal::handle_wrap<ValueType>()(j, val)));
}
}

55
gtsam/nonlinear/Values.cpp
View File

@ -25,8 +25,6 @@
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/VectorValues.h>
#include <list>
#include <boost/foreach.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
@ -38,6 +36,9 @@
#endif
#include <boost/iterator/transform_iterator.hpp>
#include <list>
#include <sstream>
using namespace std;
namespace gtsam {
@ -112,24 +113,6 @@ namespace gtsam {
return result;
}
/* ************************************************************************* */
Vector Values::atFixed(Key j, size_t n) {
switch (n) {
case 1: return at<Vector1>(j);
case 2: return at<Vector2>(j);
case 3: return at<Vector3>(j);
case 4: return at<Vector4>(j);
case 5: return at<Vector5>(j);
case 6: return at<Vector6>(j);
case 7: return at<Vector7>(j);
case 8: return at<Vector8>(j);
case 9: return at<Vector9>(j);
default:
throw runtime_error(
"Values::at fixed size can only handle n in 1..9");
}
}
/* ************************************************************************* */
const Value& Values::at(Key j) const {
// Find the item
@ -148,24 +131,6 @@ namespace gtsam {
throw ValuesKeyAlreadyExists(j);
}
/* ************************************************************************* */
void Values::insertFixed(Key j, const Vector& v, size_t n) {
switch (n) {
case 1: insert<Vector1>(j,v); break;
case 2: insert<Vector2>(j,v); break;
case 3: insert<Vector3>(j,v); break;
case 4: insert<Vector4>(j,v); break;
case 5: insert<Vector5>(j,v); break;
case 6: insert<Vector6>(j,v); break;
case 7: insert<Vector7>(j,v); break;
case 8: insert<Vector8>(j,v); break;
case 9: insert<Vector9>(j,v); break;
default:
throw runtime_error(
"Values::insert fixed size can only handle n in 1..9");
}
}
/* ************************************************************************* */
void Values::insert(const Values& values) {
for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
@ -269,4 +234,18 @@ namespace gtsam {
return message_.c_str();
}
/* ************************************************************************* */
const char* NoMatchFoundForFixed::what() const throw() {
if(message_.empty()) {
ostringstream oss;
oss
<< "Attempting to retrieve fixed-size matrix with dimensions " //
<< M1_ << "x" << N1_
<< ", but found dynamic Matrix with mismatched dimensions " //
<< M2_ << "x" << N2_;
message_ = oss.str();
}
return message_.c_str();
}
}

39
gtsam/nonlinear/Values.h
View File

@ -168,16 +168,13 @@ namespace gtsam {
/** Retrieve a variable by key \c j. The type of the value associated with
* this key is supplied as a template argument to this function.
* @param j Retrieve the value associated with this key
* @tparam Value The type of the value stored with this key, this method
* throws DynamicValuesIncorrectType if this requested type is not correct.
* @return A const reference to the stored value
* @tparam ValueType The type of the value stored with this key, this method
* Throws DynamicValuesIncorrectType if this requested type is not correct.
* Dynamic matrices/vectors can be retrieved as fixed-size, but not vice-versa.
* @return The stored value
*/
template<typename ValueType>
const ValueType& at(Key j) const;
/// Special version for small fixed size vectors, for matlab/python
/// throws truntime error if n<1 || n>9
Vector atFixed(Key j, size_t n);
ValueType at(Key j) const;
/// version for double
double atDouble(size_t key) const { return at<double>(key);}
@ -259,10 +256,6 @@ namespace gtsam {
template <typename ValueType>
void insert(Key j, const ValueType& val);
/// Special version for small fixed size vectors, for matlab/python
/// throws truntime error if n<1 || n>9
void insertFixed(Key j, const Vector& v, size_t n);
/// version for double
void insertDouble(Key j, double c) { insert<double>(j,c); }
@ -500,6 +493,28 @@ namespace gtsam {
}
};
/* ************************************************************************* */
class GTSAM_EXPORT NoMatchFoundForFixed: public std::exception {
protected:
const size_t M1_, N1_;
const size_t M2_, N2_;
private:
mutable std::string message_;
public:
NoMatchFoundForFixed(size_t M1, size_t N1, size_t M2, size_t N2) throw () :
M1_(M1), N1_(N1), M2_(M2), N2_(N2) {
}
virtual ~NoMatchFoundForFixed() throw () {
}
virtual const char* what() const throw ();
};
/* ************************************************************************* */
/// traits
template<>
struct traits<Values> : public Testable<Values> {

2
gtsam/nonlinear/expressions.h
View File

@ -25,7 +25,7 @@ Expression<T> compose(const Expression<T>& t1, const Expression<T>& t2) {
}
// Some typedefs
typedef Expression<double> double_;
typedef Expression<double> Double_;
typedef Expression<Vector1> Vector1_;
typedef Expression<Vector2> Vector2_;
typedef Expression<Vector3> Vector3_;

20
gtsam/nonlinear/tests/testExpression.cpp
View File

@ -17,7 +17,7 @@
* @brief unit tests for Block Automatic Differentiation
*/
#include <gtsam/nonlinear/Expression.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Point3.h>
@ -32,9 +32,7 @@ using boost::assign::map_list_of;
using namespace std;
using namespace gtsam;
typedef Expression<double> double_;
typedef Expression<Point3> Point3_;
typedef Expression<Vector3> Vector3_;
typedef Expression<Pose3> Pose3_;
typedef Expression<Rot3> Rot3_;
@ -101,7 +99,7 @@ TEST(Expression, Unary1) {
}
TEST(Expression, Unary2) {
using namespace unary;
double_ e(f2, p);
Double_ e(f2, p);
EXPECT(expected == e.keys());
}
@ -156,7 +154,7 @@ Point3_ p_cam(x, &Pose3::transform_to, p);
// Check that creating an expression to double compiles
TEST(Expression, BinaryToDouble) {
using namespace binary;
double_ p_cam(doubleF, x, p);
Double_ p_cam(doubleF, x, p);
}
/* ************************************************************************* */
@ -269,11 +267,11 @@ Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobi
OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) {
// return dummy derivatives (not correct, but that's ok for testing here)
if (H1)
*H1 = eye(3);
*H1 = I_3x3;
if (H2)
*H2 = eye(3);
*H2 = I_3x3;
if (H3)
*H3 = eye(3);
*H3 = I_3x3;
return R1 * (R2 * R3);
}
@ -372,8 +370,8 @@ TEST(Expression, TripleSum) {
/* ************************************************************************* */
TEST(Expression, SumOfUnaries) {
const Key key(67);
const double_ norm_(&gtsam::norm, Point3_(key));
const double_ sum_ = norm_ + norm_;
const Double_ norm_(&gtsam::norm, Point3_(key));
const Double_ sum_ = norm_ + norm_;
Values values;
values.insert<Point3>(key, Point3(6, 0, 0));
@ -391,7 +389,7 @@ TEST(Expression, SumOfUnaries) {
TEST(Expression, UnaryOfSum) {
const Key key1(42), key2(67);
const Point3_ sum_ = Point3_(key1) + Point3_(key2);
const double_ norm_(&gtsam::norm, sum_);
const Double_ norm_(&gtsam::norm, sum_);
map<Key, int> actual_dims, expected_dims = map_list_of<Key, int>(key1, 3)(key2, 3);
norm_.dims(actual_dims);

2
gtsam/nonlinear/tests/testLinearContainerFactor.cpp
View File

@ -234,7 +234,7 @@ TEST( testLinearContainerFactor, creation ) {
// create a linear factor
SharedDiagonal model = noiseModel::Unit::Create(2);
JacobianFactor::shared_ptr linear_factor(new JacobianFactor(
l3, eye(2,2), l5, 2.0 * eye(2,2), zero(2), model));
l3, I_2x2, l5, 2.0 * I_2x2, Z_2x1, model));
// create a set of values - build with full set of values
gtsam::Values full_values, exp_values;

54
gtsam/nonlinear/tests/testValues.cpp
View File

@ -477,13 +477,59 @@ TEST(Values, Destructors) {
}
/* ************************************************************************* */
TEST(Values, FixedSize) {
TEST(Values, VectorDynamicInsertFixedRead) {
Values values;
Vector v(3); v << 5.0, 6.0, 7.0;
values.insertFixed(key1, v, 3);
values.insert(key1, v);
Vector3 expected(5.0, 6.0, 7.0);
CHECK(assert_equal((Vector)expected, values.at<Vector3>(key1)));
CHECK_EXCEPTION(values.insertFixed(key1, v, 12),runtime_error);
Vector3 actual = values.at<Vector3>(key1);
CHECK(assert_equal(expected, actual));
CHECK_EXCEPTION(values.at<Vector7>(key1), exception);
}
/* ************************************************************************* */
TEST(Values, VectorDynamicInsertDynamicRead) {
Values values;
Vector v(3); v << 5.0, 6.0, 7.0;
values.insert(key1, v);
Vector expected(3); expected << 5.0, 6.0, 7.0;
Vector actual = values.at<Vector>(key1);
LONGS_EQUAL(3, actual.rows());
LONGS_EQUAL(1, actual.cols());
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(Values, VectorFixedInsertFixedRead) {
Values values;
Vector3 v; v << 5.0, 6.0, 7.0;
values.insert(key1, v);
Vector3 expected; expected << 5.0, 6.0, 7.0;
Vector3 actual = values.at<Vector3>(key1);
CHECK(assert_equal(expected, actual));
CHECK_EXCEPTION(values.at<Vector7>(key1), exception);
}
/* ************************************************************************* */
TEST(Values, VectorFixedInsertDynamicRead) {
Values values;
Vector3 v; v << 5.0, 6.0, 7.0;
values.insert(key1, v);
Vector expected(3); expected << 5.0, 6.0, 7.0;
Vector actual = values.at<Vector>(key1);
LONGS_EQUAL(3, actual.rows());
LONGS_EQUAL(1, actual.cols());
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(Values, MatrixDynamicInsertFixedRead) {
Values values;
Matrix v(1,3); v << 5.0, 6.0, 7.0;
values.insert(key1, v);
Vector3 expected(5.0, 6.0, 7.0);
CHECK(assert_equal((Vector)expected, values.at<Matrix13>(key1)));
CHECK_EXCEPTION(values.at<Matrix23>(key1), exception);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }

10
gtsam/slam/GeneralSFMFactor.h
View File

@ -129,7 +129,7 @@ public:
if (H1) *H1 = JacobianC::Zero();
if (H2) *H2 = JacobianL::Zero();
// TODO warn if verbose output asked for
return zero(2);
return Z_2x1;
}
}
@ -266,13 +266,13 @@ public:
return reprojError.vector();
}
catch( CheiralityException& e) {
if (H1) *H1 = zeros(2, 6);
if (H2) *H2 = zeros(2, 3);
if (H3) *H3 = zeros(2, DimK);
if (H1) *H1 = Matrix::Zero(2, 6);
if (H2) *H2 = Matrix::Zero(2, 3);
if (H3) *H3 = Matrix::Zero(2, DimK);
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
<< " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
}
return zero(2);
return Z_2x1;
}
/** return the measured */

11
gtsam/slam/InitializePose3.cpp
View File

@ -31,10 +31,9 @@ using namespace std;
namespace gtsam {
namespace InitializePose3 {
//static const Matrix I = eye(1);
static const Matrix I9 = eye(9);
static const Matrix I9 = I_9x9;
static const Vector zero9 = Vector::Zero(9);
static const Matrix zero33= Matrix::Zero(3,3);
static const Matrix zero33 = Z_3x3;
static const Key keyAnchor = symbol('Z', 9999999);
@ -54,11 +53,9 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
else
std::cout << "Error in buildLinearOrientationGraph" << std::endl;
// std::cout << "Rij \n" << Rij << std::endl;
const FastVector<Key>& keys = factor->keys();
Key key1 = keys[0], key2 = keys[1];
Matrix M9 = Matrix::Zero(9,9);
Matrix M9 = Z_9x9;
M9.block(0,0,3,3) = Rij;
M9.block(3,3,3,3) = Rij;
M9.block(6,6,3,3) = Rij;
@ -74,7 +71,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
gttic(InitializePose3_computeOrientationsChordal);
Matrix ppm = Matrix::Zero(3,3); // plus plus minus
Matrix ppm = Z_3x3; // plus plus minus
ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1;
Values validRot3;

2
gtsam/slam/JacobianFactorQ.h
View File

@ -56,7 +56,7 @@ public:
Base() {
size_t j = 0, m2 = E.rows(), m = m2 / ZDim;
// Calculate projector Q
Matrix Q = eye(m2) - E * P * E.transpose();
Matrix Q = Matrix::Identity(m2,m2) - E * P * E.transpose();
// Calculate pre-computed Jacobian matrices
// TODO: can we do better ?
std::vector<KeyMatrix> QF;

2
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -47,7 +47,7 @@ Vector OrientedPlane3DirectionPrior::evaluateError(const OrientedPlane3& plane,
Vector e = n_hat_p.error(n_hat_q, H_p);
H->resize(2, 3);
H->block<2, 2>(0, 0) << H_p;
H->block<2, 1>(0, 2) << Matrix::Zero(2, 1);
H->block<2, 1>(0, 2) << Z_2x1;
return e;
} else {
Unit3 n_hat_p = measured_p_.normal();

2
gtsam/slam/PoseRotationPrior.h
View File

@ -75,7 +75,7 @@ public:
Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const {
const Rotation& newR = pose.rotation();
if (H) {
*H = gtsam::zeros(rDim, xDim);
*H = Matrix::Zero(rDim, xDim);
std::pair<size_t, size_t> rotInterval = POSE::rotationInterval();
(*H).middleCols(rotInterval.first, rDim).setIdentity(rDim, rDim);
}

2
gtsam/slam/PoseTranslationPrior.h
View File

@ -65,7 +65,7 @@ public:
const int tDim = traits<Translation>::GetDimension(newTrans);
const int xDim = traits<Pose>::GetDimension(pose);
if (H) {
*H = gtsam::zeros(tDim, xDim);
*H = Matrix::Zero(tDim, xDim);
std::pair<size_t, size_t> transInterval = POSE::translationInterval();
(*H).middleCols(transInterval.first, tDim) = R.matrix();
}

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_);

6
gtsam/slam/ProjectionFactor.h
View File

@ -146,15 +146,15 @@ namespace gtsam {
return reprojectionError.vector();
}
} catch( CheiralityException& e) {
if (H1) *H1 = zeros(2,6);
if (H2) *H2 = zeros(2,3);
if (H1) *H1 = Matrix::Zero(2,6);
if (H2) *H2 = Matrix::Zero(2,3);
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
if (throwCheirality_)
throw e;
}
return ones(2) * 2.0 * K_->fx();
return Vector2::Constant(2.0 * K_->fx());
}
/** return the measurement */

2
gtsam/slam/ReferenceFrameFactor.h
View File

@ -100,7 +100,7 @@ public:
boost::optional<Matrix&> Dlocal = boost::none) const {
Point newlocal = transform_point<Transform,Point>(trans, global, Dtrans, Dforeign);
if (Dlocal)
*Dlocal = -1* gtsam::eye(Point::dimension);
*Dlocal = -1* Matrix::Identity(Point::dimension,Point::dimension);
return traits<Point>::Local(local,newlocal);
}

5
gtsam/slam/SmartFactorBase.h
View File

@ -29,6 +29,7 @@
#include <gtsam/geometry/CameraSet.h>
#include <boost/optional.hpp>
#include <boost/serialization/optional.hpp>
#include <boost/make_shared.hpp>
#include <vector>
@ -73,7 +74,7 @@ protected:
std::vector<Z> measured_;
/// @name Pose of the camera in the body frame
const boost::optional<Pose3> body_P_sensor_; ///< Pose of the camera in the body frame
boost::optional<Pose3> body_P_sensor_; ///< Pose of the camera in the body frame
/// @}
// Cache for Fblocks, to avoid a malloc ever time we re-linearize
@ -385,7 +386,9 @@ private:
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(noiseModel_);
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
};
// end class SmartFactorBase

25
gtsam/slam/SmartProjectionFactor.h
View File

@ -112,6 +112,20 @@ struct GTSAM_EXPORT SmartProjectionParams {
void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) {
triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold;
}
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(linearizationMode);
ar & BOOST_SERIALIZATION_NVP(degeneracyMode);
ar & BOOST_SERIALIZATION_NVP(triangulation);
ar & BOOST_SERIALIZATION_NVP(retriangulationThreshold);
ar & BOOST_SERIALIZATION_NVP(throwCheirality);
ar & BOOST_SERIALIZATION_NVP(verboseCheirality);
}
};
/**
@ -277,9 +291,9 @@ public:
if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
// failed: return"empty" Hessian
BOOST_FOREACH(Matrix& m, Gs)
m = zeros(Base::Dim, Base::Dim);
m = Matrix::Zero(Base::Dim, Base::Dim);
BOOST_FOREACH(Vector& v, gs)
v = zero(Base::Dim);
v = Vector::Zero(Base::Dim);
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
Gs, gs, 0.0);
}
@ -463,7 +477,7 @@ public:
if (nonDegenerate)
return Base::unwhitenedError(cameras, *result_);
else
return zero(cameras.size() * 2);
return Vector::Zero(cameras.size() * 2);
}
/**
@ -535,8 +549,9 @@ private:
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(params_.throwCheirality);
ar & BOOST_SERIALIZATION_NVP(params_.verboseCheirality);
ar & BOOST_SERIALIZATION_NVP(params_);
ar & BOOST_SERIALIZATION_NVP(result_);
ar & BOOST_SERIALIZATION_NVP(cameraPosesTriangulation_);
}
}
;

6
gtsam/slam/StereoFactor.h
View File

@ -138,15 +138,15 @@ public:
return (stereoCam.project(point, H1, H2) - measured_).vector();
}
} catch(StereoCheiralityException& e) {
if (H1) *H1 = zeros(3,6);
if (H2) *H2 = zeros(3,3);
if (H1) *H1 = Matrix::Zero(3,6);
if (H2) *H2 = Z_3x3;
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
if (throwCheirality_)
throw e;
}
return ones(3) * 2.0 * K_->fx();
return Vector3::Constant(2.0 * K_->fx());
}
/** return the measured */

4
gtsam/slam/TriangulationFactor.h
View File

@ -124,14 +124,14 @@ public:
return error.vector();
} catch (CheiralityException& e) {
if (H2)
*H2 = zeros(Measurement::dimension, 3);
*H2 = Matrix::Zero(Measurement::dimension, 3);
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "
<< DefaultKeyFormatter(this->key()) << " moved behind camera"
<< std::endl;
if (throwCheirality_)
throw e;
return ones(Measurement::dimension) * 2.0 * camera_.calibration().fx();
return Eigen::Matrix<double,Measurement::dimension,1>::Constant(2.0 * camera_.calibration().fx());
}
}

18
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 = I_6x6;
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 = I_6x6;
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 = I_6x6;
Key id1, id2;
double x, y, z, qx, qy, qz, qw;
ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw;
@ -563,11 +563,13 @@ GraphAndValues load3D(const string& filename) {
m(j, i) = mij;
}
}
Matrix mgtsam = eye(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
mgtsam.block(3,0,3,3) = m.block(3,0,3,3); // off diagonal
Matrix mgtsam = I_6x6;
mgtsam.block<3,3>(0,0) = m.block<3,3>(3,3); // cov rotation
mgtsam.block<3,3>(3,3) = m.block<3,3>(0,0); // cov translation
mgtsam.block<3,3>(0,3) = m.block<3,3>(0,3); // off diagonal
mgtsam.block<3,3>(3,0) = m.block<3,3>(3,0); // off diagonal
SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose3>(id1, id2, Pose3(R,t), model));
graph->push_back(factor);

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 = I_1x1;
static const Matrix I3 = I_3x3;
static const Key keyAnchor = symbol('Z', 9999999);
static const noiseModel::Diagonal::shared_ptr priorOrientationNoise =

2
gtsam/slam/tests/testEssentialMatrixConstraint.cpp
View File

@ -45,7 +45,7 @@ TEST( EssentialMatrixConstraint, test ) {
Rot3::RzRyRx(0.179693265735950, 0.002945368776519, 0.102274823253840),
Point3(-3.37493895, 6.14660244, -8.93650986));
Vector expected = zero(5);
Vector expected = Z_5x1;
Vector actual = factor.evaluateError(pose1,pose2);
CHECK(assert_equal(expected, actual, 1e-8));

4
gtsam/slam/tests/testPoseRotationPrior.cpp
View File

@ -52,7 +52,7 @@ TEST( testPoseRotationFactor, level3_zero_error ) {
Pose3 pose1(rot3A, point3A);
Pose3RotationPrior factor(poseKey, rot3A, model3);
Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22<Vector3,Pose3RotationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -78,7 +78,7 @@ TEST( testPoseRotationFactor, level2_zero_error ) {
Pose2 pose1(rot2A, point2A);
Pose2RotationPrior factor(poseKey, rot2A, model1);
Matrix actH1;
EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22<Vector1,Pose2RotationPrior,Pose2>(evalFactorError2, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}

8
gtsam/slam/tests/testPoseTranslationPrior.cpp
View File

@ -48,7 +48,7 @@ TEST( testPoseTranslationFactor, level3_zero_error ) {
Pose3 pose1(rot3A, point3A);
Pose3TranslationPrior factor(poseKey, point3A, model3);
Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22<Vector3,Pose3TranslationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -68,7 +68,7 @@ TEST( testPoseTranslationFactor, pitched3_zero_error ) {
Pose3 pose1(rot3B, point3A);
Pose3TranslationPrior factor(poseKey, point3A, model3);
Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22<Vector3,Pose3TranslationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -88,7 +88,7 @@ TEST( testPoseTranslationFactor, smallrot3_zero_error ) {
Pose3 pose1(rot3C, point3A);
Pose3TranslationPrior factor(poseKey, point3A, model3);
Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22<Vector3,Pose3TranslationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -108,7 +108,7 @@ TEST( testPoseTranslationFactor, level2_zero_error ) {
Pose2 pose1(rot2A, point2A);
Pose2TranslationPrior factor(poseKey, point2A, model2);
Matrix actH1;
EXPECT(assert_equal(zero(2), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal(Z_2x1, factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22<Vector2,Pose2TranslationPrior,Pose2>(evalFactorError2, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}

2
gtsam/slam/tests/testReferenceFrameFactor.cpp
View File

@ -93,7 +93,7 @@ TEST( ReferenceFrameFactor, jacobians_zero ) {
PointReferenceFrameFactor tc(lA1, tA1, lB1);
Vector actCost = tc.evaluateError(global, trans, local),
expCost = zero(2);
expCost = Z_2x1;
EXPECT(assert_equal(expCost, actCost, 1e-5));
Matrix actualDT, actualDL, actualDF;

54
gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
View File

@ -49,9 +49,9 @@ const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished();
//*************************************************************************************
TEST( regularImplicitSchurFactor, creation ) {
// Matrix E = Matrix::Ones(6,3);
Matrix E = zeros(6, 3);
E.block<2,2>(0, 0) = eye(2);
E.block<2,3>(2, 0) = 2 * ones(2, 3);
Matrix E = Matrix::Zero(6, 3);
E.block<2,2>(0, 0) = I_2x2;
E.block<2,3>(2, 0) = 2 * Matrix::Ones(2, 3);
Matrix3 P = (E.transpose() * E).inverse();
RegularImplicitSchurFactor<CalibratedCamera> expected(keys, FBlocks, E, P, b);
Matrix expectedP = expected.getPointCovariance();
@ -61,35 +61,35 @@ TEST( regularImplicitSchurFactor, creation ) {
/* ************************************************************************* */
TEST( regularImplicitSchurFactor, addHessianMultiply ) {
Matrix E = zeros(6, 3);
E.block<2,2>(0, 0) = eye(2);
E.block<2,3>(2, 0) = 2 * ones(2, 3);
E.block<2,2>(4, 1) = eye(2);
Matrix E = Matrix::Zero(6, 3);
E.block<2,2>(0, 0) = I_2x2;
E.block<2,3>(2, 0) = 2 * Matrix::Ones(2, 3);
E.block<2,2>(4, 1) = I_2x2;
Matrix3 P = (E.transpose() * E).inverse();
double alpha = 0.5;
VectorValues xvalues = map_list_of //
(0, gtsam::repeat(6, 2))//
(1, gtsam::repeat(6, 4))//
(2, gtsam::repeat(6, 0))// distractor
(3, gtsam::repeat(6, 8));
(0, Vector::Constant(6, 2))//
(1, Vector::Constant(6, 4))//
(2, Vector::Constant(6, 0))// distractor
(3, Vector::Constant(6, 8));
VectorValues yExpected = map_list_of//
(0, gtsam::repeat(6, 27))//
(1, gtsam::repeat(6, -40))//
(2, gtsam::repeat(6, 0))// distractor
(3, gtsam::repeat(6, 279));
(0, Vector::Constant(6, 27))//
(1, Vector::Constant(6, -40))//
(2, Vector::Constant(6, 0))// distractor
(3, Vector::Constant(6, 279));
// Create full F
size_t M=4, m = 3, d = 6;
Matrix F(2 * m, d * M);
F << F0, zeros(2, d * 3), zeros(2, d), F1, zeros(2, d*2), zeros(2, d * 3), F3;
F << F0, Matrix::Zero(2, d * 3), Matrix::Zero(2, d), F1, Matrix::Zero(2, d*2), Matrix::Zero(2, d * 3), F3;
// Calculate expected result F'*alpha*(I - E*P*E')*F*x
FastVector<Key> keys2;
keys2 += 0,1,2,3;
Vector x = xvalues.vector(keys2);
Vector expected = zero(24);
Vector expected = Vector::Zero(24);
RegularImplicitSchurFactor<CalibratedCamera>::multiplyHessianAdd(F, E, P, alpha, x, expected);
EXPECT(assert_equal(expected, yExpected.vector(keys2), 1e-8));
@ -170,9 +170,9 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
}
VectorValues expectedVV;
expectedVV.insert(0,-3.5*ones(6));
expectedVV.insert(1,10*ones(6)/3);
expectedVV.insert(3,-19.5*ones(6));
expectedVV.insert(0,-3.5*Vector::Ones(6));
expectedVV.insert(1,10*Vector::Ones(6)/3);
expectedVV.insert(3,-19.5*Vector::Ones(6));
{ // Check gradientAtZero
VectorValues actual = implicitFactor.gradientAtZero();
EXPECT(assert_equal(expectedVV, jfQ.gradientAtZero(), 1e-8));
@ -210,9 +210,9 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
TEST(regularImplicitSchurFactor, hessianDiagonal)
{
/* TESTED AGAINST MATLAB
* F = [ones(2,6) zeros(2,6) zeros(2,6)
zeros(2,6) 2*ones(2,6) zeros(2,6)
zeros(2,6) zeros(2,6) 3*ones(2,6)]
* F = [Vector::Ones(2,6) zeros(2,6) zeros(2,6)
zeros(2,6) 2*Vector::Ones(2,6) zeros(2,6)
zeros(2,6) zeros(2,6) 3*Vector::Ones(2,6)]
E = [[1:6] [1:6] [0.5 1:5]];
E = reshape(E',3,6)'
P = inv(E' * E)
@ -228,9 +228,9 @@ TEST(regularImplicitSchurFactor, hessianDiagonal)
// hessianDiagonal
VectorValues expected;
expected.insert(0, 1.195652*ones(6));
expected.insert(1, 4.782608*ones(6));
expected.insert(3, 7.043478*ones(6));
expected.insert(0, 1.195652*Vector::Ones(6));
expected.insert(1, 4.782608*Vector::Ones(6));
expected.insert(3, 7.043478*Vector::Ones(6));
EXPECT(assert_equal(expected, factor.hessianDiagonal(),1e-5));
// hessianBlockDiagonal
@ -246,7 +246,7 @@ TEST(regularImplicitSchurFactor, hessianDiagonal)
EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3]));
// variant two
Matrix I2 = eye(2);
Matrix I2 = I_2x2;
Matrix E0 = E.block<2,3>(0, 0);
Matrix F0t = F0.transpose();
EXPECT(assert_equal(F0t*F0-F0t*E0*P*E0.transpose()*F0,actualBD[0]));

4
gtsam/slam/tests/testRotateFactor.cpp
View File

@ -58,7 +58,7 @@ TEST (RotateFactor, checkMath) {
TEST (RotateFactor, test) {
Model model = noiseModel::Isotropic::Sigma(3, 0.01);
RotateFactor f(1, i1Ri2, c1Zc2, model);
EXPECT(assert_equal(zero(3), f.evaluateError(iRc), 1e-8));
EXPECT(assert_equal(Z_3x1, f.evaluateError(iRc), 1e-8));
Rot3 R = iRc.retract(Vector3(0.1, 0.2, 0.1));
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
@ -127,7 +127,7 @@ TEST (RotateFactor, minimization) {
TEST (RotateDirectionsFactor, test) {
Model model = noiseModel::Isotropic::Sigma(2, 0.01);
RotateDirectionsFactor f(1, p1, z1, model);
EXPECT(assert_equal(zero(2), f.evaluateError(iRc), 1e-8));
EXPECT(assert_equal(Z_2x1, f.evaluateError(iRc), 1e-8));
Rot3 R = iRc.retract(Vector3(0.1, 0.2, 0.1));

4
gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
View File

@ -123,7 +123,7 @@ TEST( SmartProjectionCameraFactor, noiseless ) {
double expectedError = 0.0;
DOUBLES_EQUAL(expectedError, factor1->error(values), 1e-7);
CHECK(
assert_equal(zero(4),
assert_equal(Z_4x1,
factor1->reprojectionErrorAfterTriangulation(values), 1e-7));
}
@ -652,7 +652,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) {
Vector e1 = sfm1.evaluateError(values.at<Camera>(c1), values.at<Point3>(l1));
Vector e2 = sfm2.evaluateError(values.at<Camera>(c2), values.at<Point3>(l1));
double actualError = 0.5
* (norm_2(e1) * norm_2(e1) + norm_2(e2) * norm_2(e2));
* (e1.norm() * e1.norm() + e2.norm() * e2.norm());
double actualErrorGraph = generalGraph.error(values);
DOUBLES_EQUAL(expectedErrorGraph, actualErrorGraph, 1e-7);

22
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -140,7 +140,7 @@ TEST( SmartProjectionPoseFactor, noiseless ) {
Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E);
EXPECT(assert_equal(expectedE, E, 1e-7));
EXPECT(assert_equal(zero(4), actualErrors, 1e-7));
EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7));
// Calculate using computeJacobians
Vector b;
@ -1409,6 +1409,26 @@ TEST(SmartProjectionPoseFactor, serialize) {
EXPECT(equalsBinary(factor));
}
TEST(SmartProjectionPoseFactor, serialize2) {
using namespace vanillaPose;
using namespace gtsam::serializationTestHelpers;
SmartProjectionParams params;
params.setRankTolerance(rankTol);
Pose3 bts;
SmartFactor factor(model, sharedK, bts, params);
// insert some measurments
vector<Key> key_view;
vector<Point2> meas_view;
key_view.push_back(Symbol('x', 1));
meas_view.push_back(Point2(10, 10));
factor.add(meas_view, key_view);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;

8
gtsam_unstable/dynamics/DynamicsPriors.h
View File

@ -57,7 +57,7 @@ struct VelocityPrior : public gtsam::PartialPriorFactor<PoseRTV> {
this->mask_[0] = 6;
this->mask_[1] = 7;
this->mask_[2] = 8;
this->H_ = zeros(3, 9);
this->H_ = Matrix::Zero(3, 9);
this->fillH();
}
};
@ -75,13 +75,13 @@ struct DGroundConstraint : public gtsam::PartialPriorFactor<PoseRTV> {
*/
DGroundConstraint(Key key, double height, const gtsam::SharedNoiseModel& model)
: Base(key, model) {
this->prior_ = delta(4, 0, height); // [z, vz, roll, pitch]
this->prior_ = Vector::Unit(4,0)*height; // [z, vz, roll, pitch]
this->mask_.resize(4);
this->mask_[0] = 5; // z = height
this->mask_[1] = 8; // vz
this->mask_[2] = 0; // roll
this->mask_[3] = 1; // pitch
this->H_ = zeros(3, 9);
this->H_ = Matrix::Zero(3, 9);
this->fillH();
}
@ -97,7 +97,7 @@ struct DGroundConstraint : public gtsam::PartialPriorFactor<PoseRTV> {
this->mask_[1] = 8; // vz
this->mask_[2] = 0; // roll
this->mask_[3] = 1; // pitch
this->H_ = zeros(3, 9);
this->H_ = Matrix::Zero(3, 9);
this->fillH();
}
};

24
gtsam_unstable/dynamics/Pendulum.h
View File

@ -50,9 +50,9 @@ public:
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = 1;
if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p);
if (H3) *H3 = eye(p)*h_;
if (H1) *H1 = -Matrix::Identity(p,p);
if (H2) *H2 = Matrix::Identity(p,p);
if (H3) *H3 = Matrix::Identity(p,p)*h_;
return (Vector(1) << qk+v*h_-qk1).finished();
}
@ -98,9 +98,9 @@ public:
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = 1;
if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p);
if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q);
if (H1) *H1 = -Matrix::Identity(p,p);
if (H2) *H2 = Matrix::Identity(p,p);
if (H3) *H3 = -Matrix::Identity(p,p)*h_*g_/r_*cos(q);
return (Vector(1) << vk - h_ * g_ / r_ * sin(q) - vk1).finished();
}
@ -154,9 +154,9 @@ public:
double mr2_h = 1/h_*m_*r_*r_;
double mgrh = m_*g_*r_*h_;
if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid));
if (H3) *H3 = eye(p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid));
if (H1) *H1 = -Matrix::Identity(p,p);
if (H2) *H2 = Matrix::Identity(p,p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid));
if (H3) *H3 = Matrix::Identity(p,p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid));
return (Vector(1) << mr2_h * (qk1 - qk) + mgrh * (1 - alpha_) * sin(qmid) - pk).finished();
}
@ -210,9 +210,9 @@ public:
double mr2_h = 1/h_*m_*r_*r_;
double mgrh = m_*g_*r_*h_;
if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid));
if (H3) *H3 = eye(p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid));
if (H1) *H1 = -Matrix::Identity(p,p);
if (H2) *H2 = Matrix::Identity(p,p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid));
if (H3) *H3 = Matrix::Identity(p,p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid));
return (Vector(1) << mr2_h * (qk1 - qk) - mgrh * alpha_ * sin(qmid) - pk1).finished();
}

4
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -11,7 +11,7 @@ namespace gtsam {
using namespace std;
static const Vector kGravity = delta(3, 2, 9.81);
static const Vector kGravity = Vector::Unit(3,2)*9.81;
/* ************************************************************************* */
double bound(double a, double min, double max) {
@ -105,7 +105,7 @@ PoseRTV PoseRTV::flyingDynamics(
Vector Acc_n =
yaw_correction_bn.rotate(forward) // applies locally forward force in the global frame
- drag * (Vector(3) << v1.x(), v1.y(), 0.0).finished() // drag term dependent on v1
+ delta(3, 2, loss_lift - lift_control); // falling due to lift lost from pitch
+ Vector::Unit(3,2)*(loss_lift - lift_control); // falling due to lift lost from pitch
// Update Vehicle Position and Velocity
Velocity3 v2 = v1 + Velocity3(Acc_n * dt);

2
gtsam_unstable/dynamics/SimpleHelicopter.h
View File

@ -140,7 +140,7 @@ public:
}
if (H3) {
*H3 = zeros(6,6);
*H3 = Z_6x6;
insertSub(*H3, -h_*D_gravityBody_gk, 3, 0);
}

6
gtsam_unstable/dynamics/VelocityConstraint3.h
View File

@ -41,9 +41,9 @@ public:
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = 1;
if (H1) *H1 = eye(p);
if (H2) *H2 = -eye(p);
if (H3) *H3 = eye(p)*dt_;
if (H1) *H1 = Matrix::Identity(p,p);
if (H2) *H2 = -Matrix::Identity(p,p);
if (H3) *H3 = Matrix::Identity(p,p)*dt_;
return (Vector(1) << x1+v*dt_-x2).finished();
}

10
gtsam_unstable/dynamics/tests/testIMUSystem.cpp
View File

@ -26,7 +26,7 @@ using namespace gtsam;
const double tol=1e-5;
static const Key x0 = 0, x1 = 1, x2 = 2, x3 = 3, x4 = 4;
static const Vector g = delta(3, 2, -9.81);
static const Vector g = Vector::Unit(3,2)*(-9.81);
/* ************************************************************************* */
TEST(testIMUSystem, instantiations) {
@ -38,7 +38,7 @@ TEST(testIMUSystem, instantiations) {
gtsam::SharedNoiseModel model6 = gtsam::noiseModel::Unit::Create(6);
gtsam::SharedNoiseModel model9 = gtsam::noiseModel::Unit::Create(9);
Vector accel = ones(3), gyro = ones(3);
Vector accel = Vector::Ones(3), gyro = Vector::Ones(3);
IMUFactor<PoseRTV> imu(accel, gyro, 0.01, x1, x2, model6);
FullIMUFactor<PoseRTV> full_imu(accel, gyro, 0.01, x1, x2, model9);
@ -48,7 +48,7 @@ TEST(testIMUSystem, instantiations) {
VelocityConstraint constraint(x1, x2, 0.1, 10000);
PriorFactor<gtsam::PoseRTV> posePrior(x1, x1_v, model9);
DHeightPrior heightPrior(x1, 0.1, model1);
VelocityPrior velPrior(x1, ones(3), model3);
VelocityPrior velPrior(x1, Vector::Ones(3), model3);
}
/* ************************************************************************* */
@ -149,8 +149,8 @@ TEST( testIMUSystem, linear_trajectory) {
const double dt = 1.0;
PoseRTV start;
Vector accel = delta(3, 0, 0.5); // forward force
Vector gyro = delta(3, 0, 0.1); // constant rotation
Vector accel = Vector::Unit(3,0)*0.5; // forward force
Vector gyro = Vector::Unit(3,0)*0.1; // constant rotation
SharedDiagonal model = noiseModel::Unit::Create(9);
Values true_traj, init_traj;

8
gtsam_unstable/dynamics/tests/testPendulumFactors.cpp
View File

@ -29,7 +29,7 @@ TEST( testPendulumFactor1, evaluateError) {
PendulumFactor1 constraint(Q(2), Q(1), V(1), h);
// verify error function
EXPECT(assert_equal(zero(1), constraint.evaluateError(q2, q1, v1), tol));
EXPECT(assert_equal(Z_1x1, constraint.evaluateError(q2, q1, v1), tol));
}
/* ************************************************************************* */
@ -38,7 +38,7 @@ TEST( testPendulumFactor2, evaluateError) {
PendulumFactor2 constraint(V(2), V(1), Q(1), h);
// verify error function
EXPECT(assert_equal(zero(1), constraint.evaluateError(v2, v1, q1), tol));
EXPECT(assert_equal(Z_1x1, constraint.evaluateError(v2, v1, q1), tol));
}
/* ************************************************************************* */
@ -49,7 +49,7 @@ TEST( testPendulumFactorPk, evaluateError) {
double pk( 1/h * (q2-q1) + h*g*sin(q1) );
// verify error function
EXPECT(assert_equal(zero(1), constraint.evaluateError(pk, q1, q2), tol));
EXPECT(assert_equal(Z_1x1, constraint.evaluateError(pk, q1, q2), tol));
}
/* ************************************************************************* */
@ -60,7 +60,7 @@ TEST( testPendulumFactorPk1, evaluateError) {
double pk1( 1/h * (q2-q1) );
// verify error function
EXPECT(assert_equal(zero(1), constraint.evaluateError(pk1, q1, q2), tol));
EXPECT(assert_equal(Z_1x1, constraint.evaluateError(pk1, q1, q2), tol));
}

18
gtsam_unstable/dynamics/tests/testPoseRTV.cpp
View File

@ -76,12 +76,12 @@ TEST( testPoseRTV, equals ) {
TEST( testPoseRTV, Lie ) {
// origin and zero deltas
PoseRTV identity;
EXPECT(assert_equal(identity, (PoseRTV)identity.retract(zero(9))));
EXPECT(assert_equal(zero(9), identity.localCoordinates(identity)));
EXPECT(assert_equal(identity, (PoseRTV)identity.retract(Z_9x1)));
EXPECT(assert_equal((Vector) Z_9x1, identity.localCoordinates(identity)));
PoseRTV state1(pt, rot, vel);
EXPECT(assert_equal(state1, (PoseRTV)state1.retract(zero(9))));
EXPECT(assert_equal(zero(9), state1.localCoordinates(state1)));
EXPECT(assert_equal(state1, (PoseRTV)state1.retract(Z_9x1)));
EXPECT(assert_equal((Vector) Z_9x1, state1.localCoordinates(state1)));
Vector delta(9);
delta << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3;
@ -111,7 +111,7 @@ TEST( testPoseRTV, dynamics_identities ) {
const double dt = 0.1;
Vector accel = Vector3(0.2, 0.0, 0.0), gyro = Vector3(0.0, 0.0, 0.2);
Vector imu01 = zero(6), imu12 = zero(6), imu23 = zero(6), imu34 = zero(6);
Vector imu01 = Z_6x1, imu12 = Z_6x1, imu23 = Z_6x1, imu34 = Z_6x1;
x1 = x0.generalDynamics(accel, gyro, dt);
x2 = x1.generalDynamics(accel, gyro, dt);
@ -227,15 +227,15 @@ TEST( testPoseRTV, transformed_from_2 ) {
/* ************************************************************************* */
TEST(testPoseRTV, RRTMbn) {
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(kZero3)));
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3())));
EXPECT(assert_equal(I_3x3, PoseRTV::RRTMbn(kZero3)));
EXPECT(assert_equal(I_3x3, PoseRTV::RRTMbn(Rot3())));
EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::Ypr(0.1, 0.2, 0.3))));
}
/* ************************************************************************* */
TEST(testPoseRTV, RRTMnb) {
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(kZero3)));
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3())));
EXPECT(assert_equal(I_3x3, PoseRTV::RRTMnb(kZero3)));
EXPECT(assert_equal(I_3x3, PoseRTV::RRTMnb(Rot3())));
EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::Ypr(0.1, 0.2, 0.3))));
}

12
gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
View File

@ -29,8 +29,8 @@ Vector gamma2 = Vector2(0.0, 0.0); // no shape
Vector u2 = Vector2(0.0, 0.0); // no control at time 2
double distT = 1.0; // distance from the body-centered x axis to the big top motor
double distR = 5.0; // distance from the body-centered z axis to the small motor
Matrix Mass = diag((Vector(3) << mass, mass, mass).finished());
Matrix Inertia = diag((Vector(6) << 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass).finished());
Matrix Mass = ((Vector(3) << mass, mass, mass).finished()).asDiagonal();
Matrix Inertia = (Vector(6) << 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass).finished().asDiagonal();
Vector computeFu(const Vector& gamma, const Vector& control) {
double gamma_r = gamma(0), gamma_p = gamma(1);
@ -53,7 +53,7 @@ TEST( Reconstruction, evaluateError) {
// verify error function
Matrix H1, H2, H3;
EXPECT(
assert_equal(zero(6), constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol));
assert_equal(Z_6x1, constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol));
Matrix numericalH1 = numericalDerivative31(
boost::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
@ -89,8 +89,8 @@ Vector newtonEuler(const Vector& Vb, const Vector& Fb, const Matrix& Inertia) {
TEST( DiscreteEulerPoincareHelicopter, evaluateError) {
Vector Fu = computeFu(gamma2, u2);
Vector fGravity_g1 = zero(6);
subInsert(fGravity_g1, g1.rotation().unrotate(Point3(0.0, 0.0, -mass*9.81)), 3); // gravity force in g1 frame
Vector fGravity_g1 = Z_6x1;
fGravity_g1.segment<3>(3) = g1.rotation().unrotate(Vector3(0, 0, -mass*9.81)); // gravity force in g1 frame
Vector Fb = Fu+fGravity_g1;
Vector dV = newtonEuler(V1_g1, Fb, Inertia);
@ -106,7 +106,7 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) {
// verify error function
Matrix H1, H2, H3;
EXPECT(assert_equal(zero(6), constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0));
EXPECT(assert_equal(Z_6x1, constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0));
Matrix numericalH1 = numericalDerivative31(
boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(

24
gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp
View File

@ -25,10 +25,10 @@ TEST( testVelocityConstraint, trapezoidal ) {
VelocityConstraint constraint(x1, x2, dynamics::TRAPEZOIDAL, dt);
// verify error function
EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, pose1), tol));
EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, origin), tol));
EXPECT(assert_equal(delta(3, 0,-1.0), constraint.evaluateError(pose1, pose1), tol));
EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol));
EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, pose1), tol));
EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol));
EXPECT(assert_equal(Vector::Unit(3,0)*(-1.0), constraint.evaluateError(pose1, pose1), tol));
EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1a), tol));
}
/* ************************************************************************* */
@ -37,10 +37,10 @@ TEST( testEulerVelocityConstraint, euler_start ) {
VelocityConstraint constraint(x1, x2, dynamics::EULER_START, dt);
// verify error function
EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1), tol));
EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, origin), tol));
EXPECT(assert_equal(zero(3), constraint.evaluateError(pose1, pose2), tol));
EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol));
EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1), tol));
EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol));
EXPECT(assert_equal(Z_3x1, constraint.evaluateError(pose1, pose2), tol));
EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1a), tol));
}
/* ************************************************************************* */
@ -49,10 +49,10 @@ TEST( testEulerVelocityConstraint, euler_end ) {
VelocityConstraint constraint(x1, x2, dynamics::EULER_END, dt);
// verify error function
EXPECT(assert_equal(delta(3, 0,-0.5), constraint.evaluateError(origin, pose1), tol));
EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, origin), tol));
EXPECT(assert_equal(zero(3), constraint.evaluateError(pose1, pose2), tol));
EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol));
EXPECT(assert_equal(Vector::Unit(3,0)*(-0.5), constraint.evaluateError(origin, pose1), tol));
EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol));
EXPECT(assert_equal(Z_3x1, constraint.evaluateError(pose1, pose2), tol));
EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1a), tol));
}
/* ************************************************************************* */

2
gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp
View File

@ -23,7 +23,7 @@ TEST( testVelocityConstraint3, evaluateError) {
VelocityConstraint3 constraint(X(1), X(2), V(1), dt);
// verify error function
EXPECT(assert_equal(zero(1), constraint.evaluateError(x1, x2, v), tol));
EXPECT(assert_equal(Z_1x1, constraint.evaluateError(x1, x2, v), tol));
}
/* ************************************************************************* */

2
gtsam_unstable/geometry/InvDepthCamera3.h
View File

@ -102,7 +102,7 @@ public:
gtsam::Matrix J2;
gtsam::Point2 uv= camera.project(landmark,H1, J2, boost::none);
if (H1) {
*H1 = (*H1) * gtsam::eye(6);
*H1 = (*H1) * I_6x6;
}
double cos_theta = cos(theta);

10
gtsam_unstable/geometry/Pose3Upright.cpp
View File

@ -81,7 +81,7 @@ Pose3 Pose3Upright::pose() const {
Pose3Upright Pose3Upright::inverse(boost::optional<Matrix&> H1) const {
Pose3Upright result(T_.inverse(H1), -z_);
if (H1) {
Matrix H1_ = -eye(4,4);
Matrix H1_ = -I_4x4;
H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2);
H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1);
*H1 = H1_;
@ -96,12 +96,12 @@ Pose3Upright Pose3Upright::compose(const Pose3Upright& p2,
return Pose3Upright(T_.compose(p2.T_), z_ + p2.z_);
Pose3Upright result(T_.compose(p2.T_, H1), z_ + p2.z_);
if (H1) {
Matrix H1_ = eye(4,4);
Matrix H1_ = I_4x4;
H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2);
H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1);
*H1 = H1_;
}
if (H2) *H2 = eye(4,4);
if (H2) *H2 = I_4x4;
return result;
}
@ -112,12 +112,12 @@ Pose3Upright Pose3Upright::between(const Pose3Upright& p2,
return Pose3Upright(T_.between(p2.T_), p2.z_ - z_);
Pose3Upright result(T_.between(p2.T_, H1, H2), p2.z_ - z_);
if (H1) {
Matrix H1_ = -eye(4,4);
Matrix H1_ = -I_4x4;
H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2);
H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1);
*H1 = H1_;
}
if (H2) *H2 = eye(4,4);
if (H2) *H2 = I_4x4;
return result;
}

Some files were not shown because too many files have changed in this diff Show More