diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index fb54156df..65dd5f609 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 Vector::Ones(1); + return I_1x1; } /// Left-trivialized derivative inverse of the exponential map static Matrix LogmapDerivative(const Vector& /*v*/) { - return Vector::Ones(1); + return I_1x1; } // Chart at origin simply uses exponential map and its inverse diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 9a67cbb53..7442540f5 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -26,7 +26,7 @@ using namespace std; using namespace gtsam; using namespace gtsam::symbol_shorthand; -const Matrix One = Matrix::Ones(1,1); +const Matrix One = I_1x1; /* ************************************************************************* */ // Create test graph according to Forst10book_pg171Ex5 @@ -38,14 +38,14 @@ 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 * Vector::Ones(1), + HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), -Matrix::Ones(1, 1), 3.0 * I_1x1, 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 - qp.inequalities.push_back(LinearInequality(X(1), -Matrix::Ones(1,1), 0, 1)); // -x1 <= 0 - qp.inequalities.push_back(LinearInequality(X(2), -Matrix::Ones(1,1), 0, 2)); // -x2 <= 0 - qp.inequalities.push_back(LinearInequality(X(1), Matrix::Ones(1,1), 1.5, 3)); // x1 <= 3/2 + qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 + qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 1)); // -x1 <= 0 + qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 2)); // -x2 <= 0 + qp.inequalities.push_back(LinearInequality(X(1), I_1x1, 1.5, 3)); // x1 <= 3/2 return qp; } @@ -110,8 +110,8 @@ TEST(QPSolver, dual) { // Initials values VectorValues initialValues; - initialValues.insert(X(1), Vector::Ones(1)); - initialValues.insert(X(2), Vector::Ones(1)); + initialValues.insert(X(1), I_1x1); + initialValues.insert(X(2), I_1x1); 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 * Vector::Ones(1), - 2.0 * Matrix::Ones(1, 1), 6 * Vector::Ones(1), 1000.0)); + HessianFactor(X(1), X(2), 1.0 * I_1x1, -Matrix::Ones(1, 1), 2.0 * I_1x1, + 2.0 * Matrix::Ones(1, 1), 6 * I_1x1, 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), Vector::Ones(1))); - qp.cost.push_back(JacobianFactor(X(2), Matrix::Ones(1, 1), 2.5 * Vector::Ones(1))); + qp.cost.push_back(JacobianFactor(X(1), Matrix::Ones(1, 1), I_1x1)); + qp.cost.push_back(JacobianFactor(X(2), Matrix::Ones(1, 1), 2.5 * I_1x1)); // Inequality constraints qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2 * One, 2, 0)); diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index 064deca5b..b0b748d95 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(Vector::Ones(1), constraint1.evaluateError(pt1), tol)); - EXPECT(assert_equal(Vector::Ones(1), constraint2.evaluateError(pt1), tol)); + EXPECT(assert_equal(I_1x1, constraint1.evaluateError(pt1), tol)); + EXPECT(assert_equal(I_1x1, 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 * 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(assert_equal(-1.0 * I_1x1, constraint3.evaluateError(pt1), tol)); + EXPECT(assert_equal(-1.0 * I_1x1, constraint4.evaluateError(pt1), tol)); + EXPECT(assert_equal(-1.0 * I_1x1, constraint3.unwhitenedError(config), tol)); + EXPECT(assert_equal(-1.0 * I_1x1, 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(Vector::Ones(1), rangeBound.evaluateError(pt1, pt2))); + EXPECT(assert_equal(I_1x1, rangeBound.evaluateError(pt1, pt2))); EXPECT(assert_equal(Z_1x1, rangeBound.evaluateError(pt1, pt3))); - EXPECT(assert_equal(-1.0*Vector::Ones(1), rangeBound.evaluateError(pt1, pt4))); + EXPECT(assert_equal(-1.0*I_1x1, 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*Vector::Ones(1), rangeBound.unwhitenedError(config1))); + EXPECT(assert_equal(-1.0*I_1x1, rangeBound.unwhitenedError(config1))); EXPECT_DOUBLES_EQUAL(0.5*mu, rangeBound.error(config1), tol); }