diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 2d09dcc90..3d854916c 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -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 diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 198cd5862..fb54156df 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -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 diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 31b059989..5bcf3d635 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -405,7 +405,7 @@ void Constrained::WhitenInPlace(Eigen::Block H) const { /* ************************************************************************* */ Constrained::shared_ptr Constrained::unit() const { - Vector sigmas = ones(dim()); + Vector sigmas = Vector::Ones(dim()); for (size_t i=0; ifx(); + return Vector::Ones(2) * 2.0 * K_->fx(); } /** return the measurement */ diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 528479f80..c330276d3 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -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 */ diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index 2748599c4..4f86023ca 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -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(); } } diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 4df3ed1e6..4184d6769 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -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 diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index 3fc6a0197..5fe29a57f 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -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 imu(accel, gyro, 0.01, x1, x2, model6); FullIMUFactor full_imu(accel, gyro, 0.01, x1, x2, model9); @@ -48,7 +48,7 @@ TEST(testIMUSystem, instantiations) { VelocityConstraint constraint(x1, x2, 0.1, 10000); PriorFactor posePrior(x1, x1_v, model9); DHeightPrior heightPrior(x1, 0.1, model1); - VelocityPrior velPrior(x1, ones(3), model3); + VelocityPrior velPrior(x1, Vector::Ones(3), model3); } /* ************************************************************************* */ diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index ebe927738..9a67cbb53 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -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)); diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index a0beef07f..0a3fa0283 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -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; diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index 3de6a1824..7509fe3b2 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -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(); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index 2fd964a35..e9f894faf 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -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(); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index fdba78445..ec2615ed6 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -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(); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index feff0b62c..cc5878d85 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -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(); } diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index 84d4ae0db..dc250fd9d 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -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 */ diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index 3b331d66b..adfc1d108 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -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 */ diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index 1bd352a33..2fd622ea1 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -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 */ diff --git a/gtsam_unstable/slam/tests/testSerialization.cpp b/gtsam_unstable/slam/tests/testSerialization.cpp index 1d8b30850..dc05711e3 100644 --- a/gtsam_unstable/slam/tests/testSerialization.cpp +++ b/gtsam_unstable/slam/tests/testSerialization.cpp @@ -48,9 +48,9 @@ Values exampleValues() { NonlinearFactorGraph exampleGraph() { NonlinearFactorGraph graph; - graph.add(PriorFactor(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(ones(3)))); - graph.add(BetweenFactor(234, 567, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(ones(3)))); - graph.add(BearingRangeFactor(234, 567, Rot2::fromAngle(0.3), 2.0, noiseModel::Diagonal::Sigmas(ones(2)))); + graph.add(PriorFactor(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3)))); + graph.add(BetweenFactor(234, 567, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3)))); + graph.add(BearingRangeFactor(234, 567, Rot2::fromAngle(0.3), 2.0, noiseModel::Diagonal::Sigmas(Vector::Ones(2)))); return graph; } diff --git a/tests/smallExample.h b/tests/smallExample.h index d46cc34e1..215655593 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -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)); diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index 18dabf18b..064deca5b 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -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); } diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index becd407aa..10de57435 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -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()); } } diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 05cb9c4ad..fb85c3742 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -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(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;