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

Deprecated Vector zero(size_t n). All unit tests pass.

release/4.3a0
Alex Hagiopol 2016-04-15 16:54:46 -04:00
parent 03e662062f
commit 76308a5d46
63 changed files with 181 additions and 191 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/base/Vector.h
View File

@ -249,14 +249,13 @@ inline const Vector sub(const Vector &v, size_t i1, size_t i2) {return v.segment
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 Vector zero(size_t n) { return Vector::Zero(n);}
#endif
inline Vector delta(size_t n, size_t i, double value){ return Vector::Unit(n, i) * value;}
inline Vector basis(size_t n, size_t i) { return delta(n, i, 1.0); }
inline Vector zero(size_t n) { return Vector::Zero(n);}
inline Vector ones(size_t n) { return Vector::Ones(n); }
inline size_t dim(const Vector& v) { return v.size(); }
#endif
} // namespace gtsam
#include <boost/serialization/nvp.hpp>

2
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
Vector g = Vector::Zero(N); // Can be fixed size
for (int j = 0; j < N; j++) {
d(j) = delta;
double hxplus = h(traits<X>::Retract(x, d));

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

@ -86,15 +86,6 @@ TEST(Vector, zero1 )
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 )
{
@ -256,7 +247,7 @@ 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
}

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

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

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

@ -97,7 +97,7 @@ inline static Vector randomVector(const Vector& minLimits,
// Get the number of dimensions and create the return vector
size_t numDims = dim(minLimits);
Vector vector = zero(numDims);
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

8
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;

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

@ -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));
}

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

@ -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)

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

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

4
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;
@ -594,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]'

2
gtsam/linear/NoiseModel.h
View File

@ -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

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/tests/testKalmanFilter.cpp
View File

@ -198,7 +198,7 @@ TEST( KalmanFilter, QRvsCholesky ) {
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 = Matrix::Zero(9, 1);
Vector u = zero(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,

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

2
gtsam/nonlinear/NonlinearEquality.h
View File

@ -147,7 +147,7 @@ public:
} else if (compare_(feasible_, xj)) {
if (H)
*H = Matrix::Identity(nj,nj);
return zero(nj); // set error to zero if equal
return Vector::Zero(nj); // set error to zero if equal
} else {
if (H)
throw std::invalid_argument(

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());
}
}

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, I_2x2, l5, 2.0 * I_2x2, 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;

4
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;
}
}
@ -272,7 +272,7 @@ public:
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
<< " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
}
return zero(2);
return Z_2x1;
}
/** return the measured */

4
gtsam/slam/SmartProjectionFactor.h
View File

@ -293,7 +293,7 @@ public:
BOOST_FOREACH(Matrix& m, Gs)
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);
}
@ -477,7 +477,7 @@ public:
if (nonDegenerate)
return Base::unwhitenedError(cameras, *result_);
else
return zero(cameras.size() * 2);
return Vector::Zero(cameras.size() * 2);
}
/**

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;

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

@ -89,7 +89,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
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));

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

2
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));
}

2
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;

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

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

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

@ -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,7 +89,7 @@ Vector newtonEuler(const Vector& Vb, const Vector& Fb, const Matrix& Inertia) {
TEST( DiscreteEulerPoincareHelicopter, evaluateError) {
Vector Fu = computeFu(gamma2, u2);
Vector fGravity_g1 = zero(6);
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;
@ -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&)>(

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

@ -25,8 +25,8 @@ 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(Z_3x1, constraint.evaluateError(origin, pose1), tol));
EXPECT(assert_equal(Z_3x1, 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));
}
@ -38,8 +38,8 @@ TEST( testEulerVelocityConstraint, euler_start ) {
// 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(Z_3x1, constraint.evaluateError(origin, origin), tol));
EXPECT(assert_equal(Z_3x1, constraint.evaluateError(pose1, pose2), tol));
EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol));
}
@ -50,8 +50,8 @@ TEST( testEulerVelocityConstraint, euler_end ) {
// 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(Z_3x1, constraint.evaluateError(origin, origin), tol));
EXPECT(assert_equal(Z_3x1, constraint.evaluateError(pose1, pose2), tol));
EXPECT(assert_equal(delta(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));
}
/* ************************************************************************* */

8
gtsam_unstable/geometry/tests/testBearingS2.cpp
View File

@ -48,11 +48,11 @@ TEST( testBearingS2, manifold ) {
BearingS2 origin, b1(0.2, 0.3);
EXPECT_LONGS_EQUAL(2, origin.dim());
EXPECT(assert_equal(zero(2), origin.localCoordinates(origin), tol));
EXPECT(assert_equal(origin, origin.retract(zero(2)), tol));
EXPECT(assert_equal(Z_2x1, origin.localCoordinates(origin), tol));
EXPECT(assert_equal(origin, origin.retract(Z_2x1), tol));
EXPECT(assert_equal(zero(2), b1.localCoordinates(b1), tol));
EXPECT(assert_equal(b1, b1.retract(zero(2)), tol));
EXPECT(assert_equal(Z_2x1, b1.localCoordinates(b1), tol));
EXPECT(assert_equal(b1, b1.retract(Z_2x1), tol));
}
/* ************************************************************************* */

10
gtsam_unstable/geometry/tests/testPose3Upright.cpp
View File

@ -68,9 +68,9 @@ TEST( testPose3Upright, manifold ) {
Pose3Upright origin, x1(1.0, 2.0, 3.0, 0.0), x2(4.0, 2.0, 7.0, 0.0);
EXPECT_LONGS_EQUAL(4, origin.dim());
EXPECT(assert_equal(origin, origin.retract(zero(4)), tol));
EXPECT(assert_equal(x1, x1.retract(zero(4)), tol));
EXPECT(assert_equal(x2, x2.retract(zero(4)), tol));
EXPECT(assert_equal(origin, origin.retract(Z_4x1), tol));
EXPECT(assert_equal(x1, x1.retract(Z_4x1), tol));
EXPECT(assert_equal(x2, x2.retract(Z_4x1), tol));
Vector delta12 = (Vector(4) << 3.0, 0.0, 4.0, 0.0).finished(), delta21 = -delta12;
EXPECT(assert_equal(x2, x1.retract(delta12), tol));
@ -83,8 +83,8 @@ TEST( testPose3Upright, manifold ) {
/* ************************************************************************* */
TEST( testPose3Upright, lie ) {
Pose3Upright origin, x1(1.0, 2.0, 3.0, 0.1);
EXPECT(assert_equal(zero(4), Pose3Upright::Logmap(origin), tol));
EXPECT(assert_equal(origin, Pose3Upright::Expmap(zero(4)), tol));
EXPECT(assert_equal(Z_4x1, Pose3Upright::Logmap(origin), tol));
EXPECT(assert_equal(origin, Pose3Upright::Expmap(Z_4x1), tol));
EXPECT(assert_equal(x1, Pose3Upright::Expmap(Pose3Upright::Logmap(x1)), tol));
}

2
gtsam_unstable/geometry/tests/testSimilarity3.cpp
View File

@ -156,7 +156,7 @@ TEST(Similarity3, Manifold) {
//******************************************************************************
TEST( Similarity3, retract_first_order) {
Similarity3 id;
Vector v = zero(7);
Vector v = Z_7x1;
v(0) = 0.3;
EXPECT(assert_equal(Similarity3(R, Point3(0,0,0), 1), id.retract(v), 1e-2));
// v(3) = 0.2;

2
gtsam_unstable/linear/QPSolver.cpp
View File

@ -51,7 +51,7 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key,
// Collect the gradients of unconstrained cost factors to the b vector
if (Aterms.size() > 0) {
Vector b = zero(delta.at(key).size());
Vector b = Vector::Zero(delta.at(key).size());
if (costVariableIndex_.find(key) != costVariableIndex_.end()) {
BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) {
GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx);

28
gtsam_unstable/linear/tests/testQPSolver.cpp
View File

@ -39,7 +39,7 @@ QP createTestCase() {
// Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10
qp.cost.push_back(
HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), -Matrix::Ones(1, 1), 3.0 * ones(1),
2.0 * Matrix::Ones(1, 1), zero(1), 10.0));
2.0 * Matrix::Ones(1, 1), Z_1x1, 10.0));
// Inequality constraints
qp.inequalities.push_back(LinearInequality(X(1), Matrix::Ones(1,1), X(2), Matrix::Ones(1,1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
@ -92,8 +92,8 @@ QP createEqualityConstrainedTest() {
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
// Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0
qp.cost.push_back(
HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), Z_1x1, zero(1),
2.0 * Matrix::Ones(1, 1), zero(1), 0.0));
HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), Z_1x1, Z_1x1,
2.0 * Matrix::Ones(1, 1), Z_1x1, 0.0));
// Equality constraints
// x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector
@ -129,8 +129,8 @@ TEST(QPSolver, indentifyActiveConstraints) {
QPSolver solver(qp);
VectorValues currentSolution;
currentSolution.insert(X(1), zero(1));
currentSolution.insert(X(2), zero(1));
currentSolution.insert(X(1), Z_1x1);
currentSolution.insert(X(2), Z_1x1);
LinearInequalityFactorGraph workingSet =
solver.identifyActiveConstraints(qp.inequalities, currentSolution);
@ -154,8 +154,8 @@ TEST(QPSolver, iterate) {
QPSolver solver(qp);
VectorValues currentSolution;
currentSolution.insert(X(1), zero(1));
currentSolution.insert(X(2), zero(1));
currentSolution.insert(X(1), Z_1x1);
currentSolution.insert(X(2), Z_1x1);
std::vector<VectorValues> expectedSolutions(4), expectedDuals(4);
expectedSolutions[0].insert(X(1), (Vector(1) << 0.0).finished());
@ -199,8 +199,8 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) {
QP qp = createTestCase();
QPSolver solver(qp);
VectorValues initialValues;
initialValues.insert(X(1), zero(1));
initialValues.insert(X(2), zero(1));
initialValues.insert(X(1), Z_1x1);
initialValues.insert(X(2), Z_1x1);
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
VectorValues expectedSolution;
@ -236,8 +236,8 @@ TEST(QPSolver, optimizeMatlabEx) {
QP qp = createTestMatlabQPEx();
QPSolver solver(qp);
VectorValues initialValues;
initialValues.insert(X(1), zero(1));
initialValues.insert(X(2), zero(1));
initialValues.insert(X(1), Z_1x1);
initialValues.insert(X(2), Z_1x1);
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
VectorValues expectedSolution;
@ -269,7 +269,7 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) {
QPSolver solver(qp);
VectorValues initialValues;
initialValues.insert(X(1), (Vector(1) << 2.0).finished());
initialValues.insert(X(2), zero(1));
initialValues.insert(X(2), Z_1x1);
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
@ -283,8 +283,8 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) {
TEST(QPSolver, failedSubproblem) {
QP qp;
qp.cost.push_back(JacobianFactor(X(1), I_2x2, zero(2)));
qp.cost.push_back(HessianFactor(X(1), Z_2x2, zero(2), 100.0));
qp.cost.push_back(JacobianFactor(X(1), I_2x2, Z_2x1));
qp.cost.push_back(HessianFactor(X(1), Z_2x2, Z_2x1, 100.0));
qp.inequalities.push_back(
LinearInequality(X(1), (Matrix(1,2) << -1.0, 0.0).finished(), -1.0, 0));

4
gtsam_unstable/nonlinear/LinearizedFactor.cpp
View File

@ -194,7 +194,7 @@ bool LinearizedHessianFactor::equals(const NonlinearFactor& expected, double tol
double LinearizedHessianFactor::error(const Values& c) const {
// Construct an error vector in key-order from the Values
Vector dx = zero(dim());
Vector dx = Vector::Zero(dim());
size_t index = 0;
for(unsigned int i = 0; i < this->size(); ++i){
Key key = this->keys()[i];
@ -217,7 +217,7 @@ boost::shared_ptr<GaussianFactor>
LinearizedHessianFactor::linearize(const Values& c) const {
// Construct an error vector in key-order from the Values
Vector dx = zero(dim());
Vector dx = Vector::Zero(dim());
size_t index = 0;
for(unsigned int i = 0; i < this->size(); ++i){
Key key = this->keys()[i];

2
gtsam_unstable/slam/AHRS.cpp
View File

@ -101,7 +101,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::initialize(double g_e)
P_plus_k2.block<3,3>(6, 3) = Z_3x3;
P_plus_k2.block<3,3>(6, 6) = Pa;
Vector dx = zero(9);
Vector dx = Z_9x1;
KalmanFilter::State state = KF_.init(dx, P_plus_k2);
return std::make_pair(mech0_, state);
}

2
gtsam_unstable/slam/DummyFactor.cpp
View File

@ -55,7 +55,7 @@ DummyFactor::linearize(const Values& c) const {
noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(rowDim_);
return GaussianFactor::shared_ptr(
new JacobianFactor(terms, zero(rowDim_), model));
new JacobianFactor(terms, Vector::Zero(rowDim_), model));
}
/* ************************************************************************* */

2
gtsam_unstable/slam/Mechanization_bRn2.h
View File

@ -30,7 +30,7 @@ public:
* @param r3 Z-axis of rotated frame
*/
Mechanization_bRn2(const Rot3& initial_bRn = Rot3(),
const Vector3& initial_x_g = zero(3), const Vector3& initial_x_a = zero(3)) :
const Vector3& initial_x_g = Z_3x1, const Vector3& initial_x_a = Z_3x1) :
bRn_(initial_bRn), x_g_(initial_x_g), x_a_(initial_x_a) {
}

2
gtsam_unstable/slam/PartialPriorFactor.h
View File

@ -113,7 +113,7 @@ namespace gtsam {
// FIXME: this was originally the generic retraction - may not produce same results
Vector full_logmap = T::Logmap(p);
// Vector full_logmap = T::identity().localCoordinates(p); // Alternate implementation
Vector masked_logmap = zero(this->dim());
Vector masked_logmap = Vector::Zero(this->dim());
for (size_t i=0; i<mask_.size(); ++i)
masked_logmap(i) = full_logmap(mask_[i]);
return masked_logmap - prior_;

4
gtsam_unstable/slam/SmartRangeFactor.h
View File

@ -142,9 +142,9 @@ public:
// set Jacobians to zero for n<3
for (size_t j = 0; j < n; j++)
(*H)[j] = Matrix::Zero(3, 1);
return zero(1);
return Z_1x1;
} else {
Vector error = zero(1);
Vector error = Z_1x1;
// triangulate to get the optimized point
// TODO: Should we have a (better?) variant that does this in relative coordinates ?

4
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -342,7 +342,7 @@ public:
BOOST_FOREACH(Matrix& m, Gs)
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);
}
@ -528,7 +528,7 @@ public:
if (nonDegenerate)
return Base::unwhitenedError(cameras, *result_);
else
return zero(cameras.size() * Base::ZDim);
return Vector::Zero(cameras.size() * Base::ZDim);
}
/**

2
gtsam_unstable/slam/tests/testDummyFactor.cpp
View File

@ -47,7 +47,7 @@ TEST( testDummyFactor, basics ) {
CHECK(actLinearization);
noiseModel::Diagonal::shared_ptr model3 = noiseModel::Unit::Create(3);
GaussianFactor::shared_ptr expLinearization(new JacobianFactor(
key1, Matrix::Zero(3,3), key2, Matrix::Zero(3,3), zero(3), model3));
key1, Matrix::Zero(3,3), key2, Matrix::Zero(3,3), Z_3x1, model3));
EXPECT(assert_equal(*expLinearization, *actLinearization, tol));
}

12
gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp
View File

@ -152,7 +152,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
Vector ExpectedErr(zero(9));
Vector ExpectedErr(Z_9x1);
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
}
@ -185,7 +185,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
Vector ExpectedErr(zero(9));
Vector ExpectedErr(Z_9x1);
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
}
@ -225,7 +225,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
Vector ExpectedErr(zero(9));
Vector ExpectedErr(Z_9x1);
// TODO: Expected values need to be updated for global velocity version
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
@ -488,7 +488,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
Vector ExpectedErr(zero(9));
Vector ExpectedErr(Z_9x1);
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
}
@ -529,7 +529,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
Vector ExpectedErr(zero(9));
Vector ExpectedErr(Z_9x1);
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
}
@ -579,7 +579,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
Vector ExpectedErr(zero(9));
Vector ExpectedErr(Z_9x1);
// TODO: Expected values need to be updated for global velocity version
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));

4
gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp
View File

@ -34,7 +34,7 @@ TEST( testRelativeElevationFactor, level_zero_error ) {
double measured = 2.0;
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2;
EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, point1, actH1, actH2)));
EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose1, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
@ -79,7 +79,7 @@ TEST( testRelativeElevationFactor, rotated_zero_error ) {
double measured = 2.0;
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2;
EXPECT(assert_equal(zero(1), factor.evaluateError(pose2, point1, actH1, actH2)));
EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose2, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(

4
gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp
View File

@ -97,7 +97,7 @@ TEST( TransformBtwRobotsUnaryFactor, unwhitenedError)
Vector err = g.unwhitenedError(values);
// Equals
CHECK(assert_equal(err, zero(3), 1e-5));
CHECK(assert_equal(err, (Vector) Z_3x1, 1e-5));
}
/* ************************************************************************* */
@ -131,7 +131,7 @@ TEST( TransformBtwRobotsUnaryFactor, unwhitenedError2)
Vector err = g.unwhitenedError(values);
// Equals
CHECK(assert_equal(err, zero(3), 1e-5));
CHECK(assert_equal(err, Z_3x1, 1e-5));
}
/* ************************************************************************* */

4
gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp
View File

@ -108,7 +108,7 @@ TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError)
Vector err = g.unwhitenedError(values);
// Equals
CHECK(assert_equal(err, zero(3), 1e-5));
CHECK(assert_equal(err, Z_3x1, 1e-5));
}
/* ************************************************************************* */
@ -147,7 +147,7 @@ TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError2)
Vector err = g.unwhitenedError(values);
// Equals
CHECK(assert_equal(err, zero(3), 1e-5));
CHECK(assert_equal(err, Z_3x1, 1e-5));
}
/* ************************************************************************* */

6
tests/smallExample.h
View File

@ -260,9 +260,9 @@ inline VectorValues createZeroDelta() {
using symbol_shorthand::X;
using symbol_shorthand::L;
VectorValues c;
c.insert(L(1), zero(2));
c.insert(X(1), zero(2));
c.insert(X(2), zero(2));
c.insert(L(1), Z_2x1);
c.insert(X(1), Z_2x1);
c.insert(X(2), Z_2x1);
return c;
}

16
tests/testBoundingConstraint.cpp
View File

@ -56,8 +56,8 @@ TEST( testBoundingConstraint, unary_basics_inactive1 ) {
EXPECT(constraint2.isGreaterThan());
EXPECT(assert_equal(ones(1), constraint1.evaluateError(pt1), tol));
EXPECT(assert_equal(ones(1), constraint2.evaluateError(pt1), tol));
EXPECT(assert_equal(zero(1), constraint1.unwhitenedError(config), tol));
EXPECT(assert_equal(zero(1), constraint2.unwhitenedError(config), tol));
EXPECT(assert_equal(Z_1x1, constraint1.unwhitenedError(config), tol));
EXPECT(assert_equal(Z_1x1, constraint2.unwhitenedError(config), tol));
EXPECT_DOUBLES_EQUAL(0.0, constraint1.error(config), tol);
EXPECT_DOUBLES_EQUAL(0.0, constraint2.error(config), tol);
}
@ -75,8 +75,8 @@ TEST( testBoundingConstraint, unary_basics_inactive2 ) {
EXPECT(!constraint4.isGreaterThan());
EXPECT(assert_equal(Vector::Constant(1, 3.0), constraint3.evaluateError(pt2), tol));
EXPECT(assert_equal(Vector::Constant(1, 5.0), constraint4.evaluateError(pt2), tol));
EXPECT(assert_equal(zero(1), constraint3.unwhitenedError(config), tol));
EXPECT(assert_equal(zero(1), constraint4.unwhitenedError(config), tol));
EXPECT(assert_equal(Z_1x1, constraint3.unwhitenedError(config), tol));
EXPECT(assert_equal(Z_1x1, constraint4.unwhitenedError(config), tol));
EXPECT_DOUBLES_EQUAL(0.0, constraint3.error(config), tol);
EXPECT_DOUBLES_EQUAL(0.0, constraint4.error(config), tol);
}
@ -189,26 +189,26 @@ TEST( testBoundingConstraint, MaxDistance_basics) {
EXPECT(assert_equal((Vector(1) << 2.0).finished(), rangeBound.evaluateError(pt1, pt1)));
EXPECT(assert_equal(ones(1), rangeBound.evaluateError(pt1, pt2)));
EXPECT(assert_equal(zero(1), rangeBound.evaluateError(pt1, pt3)));
EXPECT(assert_equal(Z_1x1, rangeBound.evaluateError(pt1, pt3)));
EXPECT(assert_equal(-1.0*ones(1), rangeBound.evaluateError(pt1, pt4)));
Values config1;
config1.insert(key1, pt1);
config1.insert(key2, pt1);
EXPECT(!rangeBound.active(config1));
EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1)));
EXPECT(assert_equal(Z_1x1, rangeBound.unwhitenedError(config1)));
EXPECT(!rangeBound.linearize(config1));
EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol);
config1.update(key2, pt2);
EXPECT(!rangeBound.active(config1));
EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1)));
EXPECT(assert_equal(Z_1x1, rangeBound.unwhitenedError(config1)));
EXPECT(!rangeBound.linearize(config1));
EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol);
config1.update(key2, pt3);
EXPECT(rangeBound.active(config1));
EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1)));
EXPECT(assert_equal(Z_1x1, rangeBound.unwhitenedError(config1)));
EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol);
config1.update(key2, pt4);

8
tests/testExpressionFactor.cpp
View File

@ -338,7 +338,7 @@ TEST(ExpressionFactor, Compose1) {
EXPECT( assert_equal(I_3x3, H[1],1e-9));
// Check linearization
JacobianFactor expected(1, I_3x3, 2, I_3x3, zero(3));
JacobianFactor expected(1, I_3x3, 2, I_3x3, Z_3x1);
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
@ -367,7 +367,7 @@ TEST(ExpressionFactor, compose2) {
EXPECT( assert_equal(2*I_3x3, H[0],1e-9));
// Check linearization
JacobianFactor expected(1, 2 * I_3x3, zero(3));
JacobianFactor expected(1, 2 * I_3x3, Z_3x1);
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
@ -396,7 +396,7 @@ TEST(ExpressionFactor, compose3) {
EXPECT( assert_equal(I_3x3, H[0],1e-9));
// Check linearization
JacobianFactor expected(3, I_3x3, zero(3));
JacobianFactor expected(3, I_3x3, Z_3x1);
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
@ -441,7 +441,7 @@ TEST(ExpressionFactor, composeTernary) {
EXPECT( assert_equal(I_3x3, H[2],1e-9));
// Check linearization
JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, zero(3));
JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, Z_3x1);
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);

34
tests/testGaussianBayesTreeB.cpp
View File

@ -79,7 +79,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts )
double sigma3 = 0.61808;
Matrix A56 = (Matrix(2,2) << -0.382022,0.,0.,-0.382022).finished();
GaussianBayesNet expected3;
expected3 += GaussianConditional(X(5), zero(2), I_2x2/sigma3, X(6), A56/sigma3);
expected3 += GaussianConditional(X(5), Z_2x1, I_2x2/sigma3, X(6), A56/sigma3);
GaussianBayesTree::sharedClique C3 = bayesTree[X(4)];
GaussianBayesNet actual3 = C3->shortcut(R);
EXPECT(assert_equal(expected3,actual3,tol));
@ -88,7 +88,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts )
double sigma4 = 0.661968;
Matrix A46 = (Matrix(2,2) << -0.146067,0.,0.,-0.146067).finished();
GaussianBayesNet expected4;
expected4 += GaussianConditional(X(4), zero(2), I_2x2/sigma4, X(6), A46/sigma4);
expected4 += GaussianConditional(X(4), Z_2x1, I_2x2/sigma4, X(6), A46/sigma4);
GaussianBayesTree::sharedClique C4 = bayesTree[X(3)];
GaussianBayesNet actual4 = C4->shortcut(R);
EXPECT(assert_equal(expected4,actual4,tol));
@ -132,7 +132,7 @@ TEST( GaussianBayesTree, balanced_smoother_marginals )
double tol=1e-5;
// Check marginal on x1
JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), zero(2), sigmax1);
JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), Z_2x1, sigmax1);
JacobianFactor actual1 = *bayesTree.marginalFactor(X(1));
Matrix expectedCovarianceX1 = I_2x2 * (sigmax1 * sigmax1);
Matrix actualCovarianceX1;
@ -143,22 +143,22 @@ TEST( GaussianBayesTree, balanced_smoother_marginals )
// Check marginal on x2
double sigx2 = 0.68712938; // FIXME: this should be corrected analytically
JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), zero(2), sigx2);
JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), Z_2x1, sigx2);
JacobianFactor actual2 = *bayesTree.marginalFactor(X(2));
EXPECT(assert_equal(expected2,actual2,tol));
// Check marginal on x3
JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), zero(2), sigmax3);
JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), Z_2x1, sigmax3);
JacobianFactor actual3 = *bayesTree.marginalFactor(X(3));
EXPECT(assert_equal(expected3,actual3,tol));
// Check marginal on x4
JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), zero(2), sigmax4);
JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), Z_2x1, sigmax4);
JacobianFactor actual4 = *bayesTree.marginalFactor(X(4));
EXPECT(assert_equal(expected4,actual4,tol));
// Check marginal on x7 (should be equal to x1)
JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), zero(2), sigmax7);
JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), Z_2x1, sigmax7);
JacobianFactor actual7 = *bayesTree.marginalFactor(X(7));
EXPECT(assert_equal(expected7,actual7,tol));
}
@ -212,8 +212,8 @@ TEST( GaussianBayesTree, balanced_smoother_shortcuts )
//
// // Check the clique marginal P(C3)
// double sigmax2_alt = 1/1.45533; // THIS NEEDS TO BE CHECKED!
// GaussianBayesNet expected = simpleGaussian(ordering[X(2)],zero(2),sigmax2_alt);
// push_front(expected,ordering[X(1)], zero(2), eye(2)*sqrt(2), ordering[X(2)], -eye(2)*sqrt(2)/2, ones(2));
// GaussianBayesNet expected = simpleGaussian(ordering[X(2)],Z_2x1,sigmax2_alt);
// push_front(expected,ordering[X(1)], Z_2x1, eye(2)*sqrt(2), ordering[X(2)], -eye(2)*sqrt(2)/2, ones(2));
// GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering[X(1)]];
// GaussianFactorGraph marginal = C3->marginal(R);
// GaussianVariableIndex varIndex(marginal);
@ -248,17 +248,17 @@ TEST( GaussianBayesTree, balanced_smoother_joint )
// Check the joint density P(x1,x7) factored as P(x1|x7)P(x7)
GaussianBayesNet expected1 = list_of
// Why does the sign get flipped on the prior?
(GaussianConditional(X(1), zero(2), I/sigmax7, X(7), A/sigmax7))
(GaussianConditional(X(7), zero(2), -1*I/sigmax7));
(GaussianConditional(X(1), Z_2x1, I/sigmax7, X(7), A/sigmax7))
(GaussianConditional(X(7), Z_2x1, -1*I/sigmax7));
GaussianBayesNet actual1 = *bayesTree.jointBayesNet(X(1),X(7));
EXPECT(assert_equal(expected1, actual1, tol));
// // Check the joint density P(x7,x1) factored as P(x7|x1)P(x1)
// GaussianBayesNet expected2;
// GaussianConditional::shared_ptr
// parent2(new GaussianConditional(X(1), zero(2), -1*I/sigmax1, ones(2)));
// parent2(new GaussianConditional(X(1), Z_2x1, -1*I/sigmax1, ones(2)));
// expected2.push_front(parent2);
// push_front(expected2,X(7), zero(2), I/sigmax1, X(1), A/sigmax1, sigma);
// push_front(expected2,X(7), Z_2x1, I/sigmax1, X(1), A/sigmax1, sigma);
// GaussianBayesNet actual2 = *bayesTree.jointBayesNet(X(7),X(1));
// EXPECT(assert_equal(expected2,actual2,tol));
@ -266,19 +266,19 @@ TEST( GaussianBayesTree, balanced_smoother_joint )
double sig14 = 0.784465;
Matrix A14 = -0.0769231*I;
GaussianBayesNet expected3 = list_of
(GaussianConditional(X(1), zero(2), I/sig14, X(4), A14/sig14))
(GaussianConditional(X(4), zero(2), I/sigmax4));
(GaussianConditional(X(1), Z_2x1, I/sig14, X(4), A14/sig14))
(GaussianConditional(X(4), Z_2x1, I/sigmax4));
GaussianBayesNet actual3 = *bayesTree.jointBayesNet(X(1),X(4));
EXPECT(assert_equal(expected3,actual3,tol));
// // Check the joint density P(x4,x1), i.e. with a root variable, factored the other way
// GaussianBayesNet expected4;
// GaussianConditional::shared_ptr
// parent4(new GaussianConditional(X(1), zero(2), -1.0*I/sigmax1, ones(2)));
// parent4(new GaussianConditional(X(1), Z_2x1, -1.0*I/sigmax1, ones(2)));
// expected4.push_front(parent4);
// double sig41 = 0.668096;
// Matrix A41 = -0.055794*I;
// push_front(expected4,X(4), zero(2), I/sig41, X(1), A41/sig41, sigma);
// push_front(expected4,X(4), Z_2x1, I/sig41, X(1), A41/sig41, sigma);
// GaussianBayesNet actual4 = *bayesTree.jointBayesNet(X(4),X(1));
// EXPECT(assert_equal(expected4,actual4,tol));
}

8
tests/testGaussianFactorGraphB.cpp
View File

@ -473,13 +473,13 @@ TEST(GaussianFactorGraph, replace)
SharedDiagonal noise(noiseModel::Isotropic::Sigma(3, 1.0));
GaussianFactorGraph::sharedFactor f1(new JacobianFactor(
ord[X(1)], I_3x3, ord[X(2)], I_3x3, zero(3), noise));
ord[X(1)], I_3x3, ord[X(2)], I_3x3, Z_3x1, noise));
GaussianFactorGraph::sharedFactor f2(new JacobianFactor(
ord[X(2)], I_3x3, ord[X(3)], I_3x3, zero(3), noise));
ord[X(2)], I_3x3, ord[X(3)], I_3x3, Z_3x1, noise));
GaussianFactorGraph::sharedFactor f3(new JacobianFactor(
ord[X(3)], I_3x3, ord[X(4)], I_3x3, zero(3), noise));
ord[X(3)], I_3x3, ord[X(4)], I_3x3, Z_3x1, noise));
GaussianFactorGraph::sharedFactor f4(new JacobianFactor(
ord[X(5)], I_3x3, ord[X(6)], I_3x3, zero(3), noise));
ord[X(5)], I_3x3, ord[X(6)], I_3x3, Z_3x1, noise));
GaussianFactorGraph actual;
actual.push_back(f1);

6
tests/testIterative.cpp
View File

@ -62,7 +62,7 @@ TEST( Iterative, conjugateGradientDescent )
// get matrices
Matrix A;
Vector b;
Vector x0 = gtsam::zero(6);
Vector x0 = Z_6x1;
boost::tie(A, b) = fg.jacobian();
Vector expectedX = (Vector(6) << -0.1, 0.1, -0.1, -0.1, 0.1, -0.2).finished();
@ -104,7 +104,7 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint )
VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters);
VectorValues expected;
expected.insert(X(1), zero(3));
expected.insert(X(1), Z_3x1);
expected.insert(X(2), Vector3(-0.5,0.,0.));
CHECK(assert_equal(expected, actual));
}
@ -131,7 +131,7 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint )
VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters);
VectorValues expected;
expected.insert(X(1), zero(3));
expected.insert(X(1), Z_3x1);
expected.insert(X(2), Vector3(-0.5,0.,0.));
CHECK(assert_equal(expected, actual));
}

4
tests/testManifold.cpp
View File

@ -131,7 +131,7 @@ TEST(Manifold, DefaultChart) {
EXPECT_DOUBLES_EQUAL(traits<double>::Retract(0, v1), 1, 1e-9);
// Dynamic does not work yet !
Vector z = zero(2), v(2);
Vector z = Z_2x1, v(2);
v << 1, 0;
//DefaultChart<Vector> chart4;
// EXPECT(assert_equal(traits<Vector>::Local(z, v), v));
@ -146,7 +146,7 @@ TEST(Manifold, DefaultChart) {
EXPECT(assert_equal(traits<Rot3>::Retract(I, v3), R));
// Check zero vector
//DefaultChart<Rot3> chart6;
EXPECT(assert_equal(zero(3), traits<Rot3>::Local(R, R)));
EXPECT(assert_equal((Vector) Z_3x1, traits<Rot3>::Local(R, R)));
}
//******************************************************************************

16
tests/testNonlinearEquality.cpp
View File

@ -56,7 +56,7 @@ TEST ( NonlinearEquality, linearization ) {
// check linearize
SharedDiagonal constraintModel = noiseModel::Constrained::All(3);
JacobianFactor expLF(key, I_3x3, zero(3), constraintModel);
JacobianFactor expLF(key, I_3x3, Z_3x1, constraintModel);
GaussianFactor::shared_ptr actualLF = nle->linearize(linearize);
EXPECT(assert_equal((const GaussianFactor&)expLF, *actualLF));
}
@ -133,7 +133,7 @@ TEST ( NonlinearEquality, error ) {
// check error function outputs
Vector actual = nle->unwhitenedError(feasible);
EXPECT(assert_equal(actual, zero(3)));
EXPECT(assert_equal(actual, Z_3x1));
actual = nle->unwhitenedError(bad_linearize);
EXPECT(
@ -263,8 +263,8 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) {
Values config1;
config1.insert(key, pt);
EXPECT(constraint.active(config1));
EXPECT(assert_equal(zero(2), constraint.evaluateError(pt), tol));
EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol));
EXPECT(assert_equal(Z_2x1, constraint.evaluateError(pt), tol));
EXPECT(assert_equal(Z_2x1, constraint.unwhitenedError(config1), tol));
EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol);
Values config2;
@ -289,7 +289,7 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) {
config1.insert(key, pt);
GaussianFactor::shared_ptr actual1 = constraint.linearize(config1);
GaussianFactor::shared_ptr expected1(
new JacobianFactor(key, I_2x2, zero(2), hard_model));
new JacobianFactor(key, I_2x2, Z_2x1, hard_model));
EXPECT(assert_equal(*expected1, *actual1, tol));
Values config2;
@ -345,8 +345,8 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) {
config1.insert(key1, x1);
config1.insert(key2, x2);
EXPECT(constraint.active(config1));
EXPECT(assert_equal(zero(2), constraint.evaluateError(x1, x2), tol));
EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol));
EXPECT(assert_equal(Z_2x1, constraint.evaluateError(x1, x2), tol));
EXPECT(assert_equal(Z_2x1, constraint.unwhitenedError(config1), tol));
EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol);
Values config2;
@ -374,7 +374,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) {
config1.insert(key2, x2);
GaussianFactor::shared_ptr actual1 = constraint.linearize(config1);
GaussianFactor::shared_ptr expected1(
new JacobianFactor(key1, -I_2x2, key2, I_2x2, zero(2),
new JacobianFactor(key1, -I_2x2, key2, I_2x2, Z_2x1,
hard_model));
EXPECT(assert_equal(*expected1, *actual1, tol));

6
tests/testNonlinearOptimizer.cpp
View File

@ -257,9 +257,9 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) {
expected.insert(2, Pose2(1, 1, M_PI));
VectorValues expectedGradient;
expectedGradient.insert(0,zero(3));
expectedGradient.insert(1,zero(3));
expectedGradient.insert(2,zero(3));
expectedGradient.insert(0,Z_3x1);
expectedGradient.insert(1,Z_3x1);
expectedGradient.insert(2,Z_3x1);
// Try LM and Dogleg
LevenbergMarquardtParams params = LevenbergMarquardtParams::LegacyDefaults();

2
timing/timeSchurFactors.cpp
View File

@ -109,7 +109,7 @@ void timeAll(size_t m, size_t N) {
double* xdata = x.data();
// create a y
Vector y = zero(m * D);
Vector y = Vector::Zero(m * D);
TIME(RawImplicit, implicitFactor, xdata, y.data())
TIME(RawJacobianQ, jf, xdata, y.data())
TIME(RawJacobianQR, jqr, xdata, y.data())