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

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

release/4.3a0
Alex Hagiopol 2016-04-15 17:30:54 -04:00
parent 76308a5d46
commit b6728ef620
27 changed files with 67 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/base/Vector.h
View File

@ -250,10 +250,10 @@ inline void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fu
inline double sum(const Vector &a){return a.sum();}
inline Vector zero(size_t n) { return Vector::Zero(n);}
inline Vector ones(size_t n) { return Vector::Ones(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 ones(size_t n) { return Vector::Ones(n); }
inline size_t dim(const Vector& v) { return v.size(); }
} // namespace gtsam

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 Vector::Ones(1);
}
/// Left-trivialized derivative inverse of the exponential map
static Matrix LogmapDerivative(const Vector& /*v*/) {
return ones(1);
return Vector::Ones(1);
}
// Chart at origin simply uses exponential map and its inverse

2
gtsam/linear/NoiseModel.cpp
View File

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

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

@ -47,7 +47,7 @@ TEST(GaussianFactorGraph, initialization) {
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
fg +=
JacobianFactor(0, 10*I_2x2, -1.0*ones(2), 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);
@ -166,7 +166,7 @@ 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*I_2x2, -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*I_2x2, 2, -10*I_2x2, Vector2(2.0, -1.0), unit2);
// measurement between x1 and l1: l1-x1=[0.0;0.2]

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

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

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

@ -260,9 +260,9 @@ 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

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

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

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

@ -145,7 +145,7 @@ TEST (Serialization, linear_factors) {
Key i1 = 4, i2 = 7;
Matrix A1 = I_3x3, A2 = -1.0 * I_3x3;
Vector b = ones(3);
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));
@ -186,7 +186,7 @@ TEST (Serialization, gaussian_factor_graph) {
{
Key i1 = 4, i2 = 7;
Matrix A1 = I_3x3, A2 = -1.0 * I_3x3;
Vector b = ones(3);
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/slam/ProjectionFactor.h
View File

@ -154,7 +154,7 @@ namespace gtsam {
if (throwCheirality_)
throw e;
}
return ones(2) * 2.0 * K_->fx();
return Vector::Ones(2) * 2.0 * K_->fx();
}
/** return the measurement */

2
gtsam/slam/StereoFactor.h
View File

@ -146,7 +146,7 @@ public:
if (throwCheirality_)
throw e;
}
return ones(3) * 2.0 * K_->fx();
return Vector::Ones(3) * 2.0 * K_->fx();
}
/** return the measured */

2
gtsam/slam/TriangulationFactor.h
View File

@ -131,7 +131,7 @@ public:
<< std::endl;
if (throwCheirality_)
throw e;
return ones(Measurement::dimension) * 2.0 * camera_.calibration().fx();
return Vector::Ones(Measurement::dimension) * 2.0 * camera_.calibration().fx();
}
}

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

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

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

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

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

@ -38,7 +38,7 @@ QP createTestCase() {
// 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 = -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),
HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), -Matrix::Ones(1, 1), 3.0 * Vector::Ones(1),
2.0 * Matrix::Ones(1, 1), Z_1x1, 10.0));
// Inequality constraints
@ -110,8 +110,8 @@ TEST(QPSolver, dual) {
// Initials values
VectorValues initialValues;
initialValues.insert(X(1), ones(1));
initialValues.insert(X(2), ones(1));
initialValues.insert(X(1), Vector::Ones(1));
initialValues.insert(X(2), Vector::Ones(1));
QPSolver solver(qp);
@ -219,8 +219,8 @@ QP createTestMatlabQPEx() {
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
// Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0
qp.cost.push_back(
HessianFactor(X(1), X(2), 1.0 * Matrix::Ones(1,1), -Matrix::Ones(1, 1), 2.0 * ones(1),
2.0 * Matrix::Ones(1, 1), 6 * ones(1), 1000.0));
HessianFactor(X(1), X(2), 1.0 * Matrix::Ones(1,1), -Matrix::Ones(1, 1), 2.0 * Vector::Ones(1),
2.0 * Matrix::Ones(1, 1), 6 * Vector::Ones(1), 1000.0));
// Inequality constraints
qp.inequalities.push_back(LinearInequality(X(1), One, X(2), One, 2, 0)); // x1 + x2 <= 2
@ -251,8 +251,8 @@ TEST(QPSolver, optimizeMatlabEx) {
QP createTestNocedal06bookEx16_4() {
QP qp;
qp.cost.push_back(JacobianFactor(X(1), Matrix::Ones(1, 1), ones(1)));
qp.cost.push_back(JacobianFactor(X(2), Matrix::Ones(1, 1), 2.5 * ones(1)));
qp.cost.push_back(JacobianFactor(X(1), Matrix::Ones(1, 1), Vector::Ones(1)));
qp.cost.push_back(JacobianFactor(X(2), Matrix::Ones(1, 1), 2.5 * Vector::Ones(1)));
// Inequality constraints
qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2 * One, 2, 0));

6
gtsam_unstable/slam/AHRS.cpp
View File

@ -45,9 +45,9 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
F_g_ = -I_3x3 / tau_g;
F_a_ = -I_3x3 / tau_a;
Vector3 var_omega_w = 0 * ones(3); // TODO
Vector3 var_omega_g = (0.0034 * 0.0034) * ones(3);
Vector3 var_omega_a = (0.034 * 0.034) * ones(3);
Vector3 var_omega_w = 0 * Vector::Ones(3); // TODO
Vector3 var_omega_g = (0.0034 * 0.0034) * Vector::Ones(3);
Vector3 var_omega_a = (0.034 * 0.034) * Vector::Ones(3);
Vector3 sigmas_v_g_sq = sigmas_v_g.array().square();
var_w_ << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a;

2
gtsam_unstable/slam/InvDepthFactor3.h
View File

@ -94,7 +94,7 @@ public:
if (H3) *H2 = Matrix::Zero(2,1);
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
return gtsam::ones(2) * 2.0 * K_->fx();
return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
}
return (gtsam::Vector(1) << 0.0).finished();
}

2
gtsam_unstable/slam/InvDepthFactorVariant1.h
View File

@ -93,7 +93,7 @@ public:
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]"
<< " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]"
<< std::endl;
return gtsam::ones(2) * 2.0 * K_->fx();
return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
}
return (gtsam::Vector(1) << 0.0).finished();
}

2
gtsam_unstable/slam/InvDepthFactorVariant2.h
View File

@ -96,7 +96,7 @@ public:
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]"
<< " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]"
<< std::endl;
return gtsam::ones(2) * 2.0 * K_->fx();
return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
}
return (gtsam::Vector(1) << 0.0).finished();
}

4
gtsam_unstable/slam/InvDepthFactorVariant3.h
View File

@ -96,7 +96,7 @@ public:
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key2()) << "]"
<< " moved behind camera [" << DefaultKeyFormatter(this->key1()) << "]"
<< std::endl;
return gtsam::ones(2) * 2.0 * K_->fx();
return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
}
return (Vector(1) << 0.0).finished();
}
@ -215,7 +215,7 @@ public:
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key3()) << "]"
<< " moved behind camera " << DefaultKeyFormatter(this->key2())
<< std::endl;
return gtsam::ones(2) * 2.0 * K_->fx();
return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
}
return (Vector(1) << 0.0).finished();
}

2
gtsam_unstable/slam/MultiProjectionFactor.h
View File

@ -192,7 +192,7 @@ namespace gtsam {
if (throwCheirality_)
throw e;
}
return ones(2) * 2.0 * K_->fx();
return Vector::Ones(2) * 2.0 * K_->fx();
}
/** return the measurements */

2
gtsam_unstable/slam/ProjectionFactorPPP.h
View File

@ -146,7 +146,7 @@ namespace gtsam {
if (throwCheirality_)
throw e;
}
return ones(2) * 2.0 * K_->fx();
return Vector::Ones(2) * 2.0 * K_->fx();
}
/** return the measurement */

2
gtsam_unstable/slam/ProjectionFactorPPPC.h
View File

@ -142,7 +142,7 @@ namespace gtsam {
if (throwCheirality_)
throw e;
}
return ones(2) * 2.0 * K.fx();
return Vector::Ones(2) * 2.0 * K.fx();
}
/** return the measurement */

6
gtsam_unstable/slam/tests/testSerialization.cpp
View File

@ -48,9 +48,9 @@ Values exampleValues() {
NonlinearFactorGraph exampleGraph() {
NonlinearFactorGraph graph;
graph.add(PriorFactor<Pose2>(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(ones(3))));
graph.add(BetweenFactor<Pose2>(234, 567, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(ones(3))));
graph.add(BearingRangeFactor<Pose2,Point2>(234, 567, Rot2::fromAngle(0.3), 2.0, noiseModel::Diagonal::Sigmas(ones(2))));
graph.add(PriorFactor<Pose2>(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3))));
graph.add(BetweenFactor<Pose2>(234, 567, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3))));
graph.add(BearingRangeFactor<Pose2,Point2>(234, 567, Rot2::fromAngle(0.3), 2.0, noiseModel::Diagonal::Sigmas(Vector::Ones(2))));
return graph;
}

2
tests/smallExample.h
View File

@ -274,7 +274,7 @@ inline GaussianFactorGraph createGaussianFactorGraph() {
GaussianFactorGraph fg;
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
fg += JacobianFactor(X(1), 10*I_2x2, -1.0*ones(2));
fg += JacobianFactor(X(1), 10*I_2x2, -1.0*Vector::Ones(2));
// odometry between x1 and x2: x2-x1=[0.2;-0.1]
fg += JacobianFactor(X(1), -10*I_2x2, X(2), 10*I_2x2, Vector2(2.0, -1.0));

18
tests/testBoundingConstraint.cpp
View File

@ -54,8 +54,8 @@ TEST( testBoundingConstraint, unary_basics_inactive1 ) {
EXPECT_DOUBLES_EQUAL(2.0, constraint2.threshold(), tol);
EXPECT(constraint1.isGreaterThan());
EXPECT(constraint2.isGreaterThan());
EXPECT(assert_equal(ones(1), constraint1.evaluateError(pt1), tol));
EXPECT(assert_equal(ones(1), constraint2.evaluateError(pt1), tol));
EXPECT(assert_equal(Vector::Ones(1), constraint1.evaluateError(pt1), tol));
EXPECT(assert_equal(Vector::Ones(1), constraint2.evaluateError(pt1), 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);
@ -103,10 +103,10 @@ TEST( testBoundingConstraint, unary_basics_active2 ) {
config.insert(key, pt1);
EXPECT(constraint3.active(config));
EXPECT(constraint4.active(config));
EXPECT(assert_equal(-1.0 * ones(1), constraint3.evaluateError(pt1), tol));
EXPECT(assert_equal(-1.0 * ones(1), constraint4.evaluateError(pt1), tol));
EXPECT(assert_equal(-1.0 * ones(1), constraint3.unwhitenedError(config), tol));
EXPECT(assert_equal(-1.0 * ones(1), constraint4.unwhitenedError(config), tol));
EXPECT(assert_equal(-1.0 * Vector::Ones(1), constraint3.evaluateError(pt1), tol));
EXPECT(assert_equal(-1.0 * Vector::Ones(1), constraint4.evaluateError(pt1), tol));
EXPECT(assert_equal(-1.0 * Vector::Ones(1), constraint3.unwhitenedError(config), tol));
EXPECT(assert_equal(-1.0 * Vector::Ones(1), constraint4.unwhitenedError(config), tol));
EXPECT_DOUBLES_EQUAL(5.0, constraint3.error(config), tol);
EXPECT_DOUBLES_EQUAL(5.0, constraint4.error(config), tol);
}
@ -188,9 +188,9 @@ TEST( testBoundingConstraint, MaxDistance_basics) {
EXPECT(rangeBound.dim() == 1);
EXPECT(assert_equal((Vector(1) << 2.0).finished(), rangeBound.evaluateError(pt1, pt1)));
EXPECT(assert_equal(ones(1), rangeBound.evaluateError(pt1, pt2)));
EXPECT(assert_equal(Vector::Ones(1), rangeBound.evaluateError(pt1, pt2)));
EXPECT(assert_equal(Z_1x1, rangeBound.evaluateError(pt1, pt3)));
EXPECT(assert_equal(-1.0*ones(1), rangeBound.evaluateError(pt1, pt4)));
EXPECT(assert_equal(-1.0*Vector::Ones(1), rangeBound.evaluateError(pt1, pt4)));
Values config1;
config1.insert(key1, pt1);
@ -213,7 +213,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) {
config1.update(key2, pt4);
EXPECT(rangeBound.active(config1));
EXPECT(assert_equal(-1.0*ones(1), rangeBound.unwhitenedError(config1)));
EXPECT(assert_equal(-1.0*Vector::Ones(1), rangeBound.unwhitenedError(config1)));
EXPECT_DOUBLES_EQUAL(0.5*mu, rangeBound.error(config1), tol);
}

16
tests/testGaussianFactorGraphB.cpp
View File

@ -97,7 +97,7 @@ TEST( GaussianFactorGraph, eliminateOne_x2 )
// create expected Conditional Gaussian
double sig = 0.0894427;
Matrix I = I_2x2/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
Vector d = Vector2(0.2, -0.14)/sig, sigma = ones(2);
Vector d = Vector2(0.2, -0.14)/sig, sigma = Vector::Ones(2);
GaussianConditional expected(ordering[X(2)],d,R11,ordering[L(1)],S12,ordering[X(1)],S13,sigma);
EXPECT(assert_equal(expected,*actual,tol));
@ -113,7 +113,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1 )
// create expected Conditional Gaussian
double sig = sqrt(2.0)/10.;
Matrix I = I_2x2/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
Vector d = Vector2(-0.1, 0.25)/sig, sigma = ones(2);
Vector d = Vector2(-0.1, 0.25)/sig, sigma = Vector::Ones(2);
GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma);
EXPECT(assert_equal(expected,*actual,tol));
@ -130,7 +130,7 @@ TEST( GaussianFactorGraph, eliminateOne_x1_fast )
// create expected Conditional Gaussian
Matrix I = 15*I_2x2, R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
Vector d = Vector2(-0.133333, -0.0222222), sigma = ones(2);
Vector d = Vector2(-0.133333, -0.0222222), sigma = Vector::Ones(2);
GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma);
// Create expected remaining new factor
@ -160,7 +160,7 @@ TEST( GaussianFactorGraph, eliminateOne_x2_fast )
// create expected Conditional Gaussian
double sig = 0.0894427;
Matrix I = I_2x2/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
Vector d = Vector2(0.2, -0.14)/sig, sigma = ones(2);
Vector d = Vector2(0.2, -0.14)/sig, sigma = Vector::Ones(2);
GaussianConditional expected(ordering[X(2)],d,R11,ordering[X(1)],S13,ordering[L(1)],S12,sigma);
EXPECT(assert_equal(expected,*actual,tol));
@ -176,7 +176,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1_fast )
// create expected Conditional Gaussian
double sig = sqrt(2.0)/10.;
Matrix I = I_2x2/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
Vector d = Vector2(-0.1, 0.25)/sig, sigma = ones(2);
Vector d = Vector2(-0.1, 0.25)/sig, sigma = Vector::Ones(2);
GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma);
EXPECT(assert_equal(expected,*actual,tol));
@ -195,11 +195,11 @@ TEST( GaussianFactorGraph, eliminateAll )
GaussianBayesNet expected = simpleGaussian(ordering[X(1)],d1,0.1);
double sig1 = 0.149071;
Vector d2 = Vector2(0.0, 0.2)/sig1, sigma2 = ones(2);
Vector d2 = Vector2(0.0, 0.2)/sig1, sigma2 = Vector::Ones(2);
push_front(expected,ordering[L(1)],d2, I/sig1,ordering[X(1)], (-1)*I/sig1,sigma2);
double sig2 = 0.0894427;
Vector d3 = Vector2(0.2, -0.14)/sig2, sigma3 = ones(2);
Vector d3 = Vector2(0.2, -0.14)/sig2, sigma3 = Vector::Ones(2);
push_front(expected,ordering[X(2)],d3, I/sig2,ordering[L(1)], (-0.2)*I/sig2, ordering[X(1)], (-0.8)*I/sig2, sigma3);
// Check one ordering
@ -588,7 +588,7 @@ TEST( GaussianFactorGraph, conditional_sigma_failure) {
BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, actBT.nodes() | br::map_values) {
GaussianConditional::shared_ptr conditional = clique->conditional();
//size_t dim = conditional->rows();
//EXPECT(assert_equal(gtsam::ones(dim), conditional->get_model()->sigmas(), tol));
//EXPECT(assert_equal(gtsam::Vector::Ones(dim), conditional->get_model()->sigmas(), tol));
EXPECT(!conditional->get_model());
}
}

2
tests/testNonlinearFactor.cpp
View File

@ -91,7 +91,7 @@ TEST( NonlinearFactor, NonlinearFactor )
// calculate the error_vector from the factor "f1"
// error_vector = [0.1 0.1]
Vector actual_e = boost::dynamic_pointer_cast<NoiseModelFactor>(factor)->unwhitenedError(cfg);
CHECK(assert_equal(0.1*ones(2),actual_e));
CHECK(assert_equal(0.1*Vector::Ones(2),actual_e));
// error = 0.5 * [1 1] * [1;1] = 1
double expected = 1.0;