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

Global replace of Vector::Ones(1) and Matrix::Ones(1,1) with I_1x1

release/4.3a0
Alex Hagiopol 2016-04-16 12:32:48 -04:00
parent df2693d2ae
commit b021ccef36
3 changed files with 23 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

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