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

remove all LieVector(size_t m, ...), which doesn't check parameter's type and it's also dangerous

release/4.3a0
jing 2014-01-23 18:35:29 -05:00
parent ad0662a860
commit 96296333ae
16 changed files with 114 additions and 123 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/base/LieVector.cpp
View File

@ -30,19 +30,6 @@ LieVector::LieVector(size_t m, const double* const data)
(*this)(i) = data[i]; (*this)(i) = data[i];
} }
/* ************************************************************************* */
LieVector::LieVector(size_t m, ...)
: Vector(m)
{
va_list ap;
va_start(ap, m);
for( size_t i = 0 ; i < m ; i++) {
double value = va_arg(ap, double);
(*this)(i) = value;
}
va_end(ap);
}
/* ************************************************************************* */ /* ************************************************************************* */
void LieVector::print(const std::string& name) const { void LieVector::print(const std::string& name) const {
gtsam::print(vector(), name); gtsam::print(vector(), name);

3
gtsam/base/LieVector.h
View File

@ -44,9 +44,6 @@ struct LieVector : public Vector, public DerivedValue<LieVector> {
/** constructor with size and initial data, row order ! */ /** constructor with size and initial data, row order ! */
GTSAM_EXPORT LieVector(size_t m, const double* const data); GTSAM_EXPORT LieVector(size_t m, const double* const data);
/** Specify arguments directly, as in Vector_() - always force these to be doubles */
GTSAM_EXPORT LieVector(size_t m, ...);
/** get the underlying vector */ /** get the underlying vector */
Vector vector() const { Vector vector() const {
return static_cast<Vector>(*this); return static_cast<Vector>(*this);

5
gtsam/base/tests/testLieVector.cpp
View File

@ -39,14 +39,9 @@ TEST( testLieVector, construction ) {
TEST( testLieVector, other_constructors ) { TEST( testLieVector, other_constructors ) {
Vector init = (Vector(2) << 10.0, 20.0); Vector init = (Vector(2) << 10.0, 20.0);
LieVector exp(init); LieVector exp(init);
LieVector a(2,10.0,20.0);
double data[] = {10,20}; double data[] = {10,20};
LieVector b(2,data); LieVector b(2,data);
LieVector c(2.3), c_exp(LieVector(1, 2.3));
EXPECT(assert_equal(exp, a));
EXPECT(assert_equal(exp, b)); EXPECT(assert_equal(exp, b));
EXPECT(assert_equal(b, a));
EXPECT(assert_equal(c_exp, c));
} }
/* ************************************************************************* */ /* ************************************************************************* */

13
gtsam/base/tests/testNumericalDerivative.cpp
View File

@ -48,7 +48,8 @@ double f2(const LieVector& x) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(testNumericalDerivative, numericalHessian2) { TEST(testNumericalDerivative, numericalHessian2) {
LieVector center(2, 0.5, 1.0); Vector v_center = (Vector(2) << 0.5, 1.0);
LieVector center(v_center);
Matrix expected = (Matrix(2,2) << Matrix expected = (Matrix(2,2) <<
-cos(center(1))*sin(center(0)), -sin(center(1))*cos(center(0)), -cos(center(1))*sin(center(0)), -sin(center(1))*cos(center(0)),
@ -67,7 +68,9 @@ double f3(const LieVector& x1, const LieVector& x2) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(testNumericalDerivative, numericalHessian211) { TEST(testNumericalDerivative, numericalHessian211) {
LieVector center1(1, 1.0), center2(1, 5.0); Vector v_center1 = (Vector(1) << 1.0);
Vector v_center2 = (Vector(1) << 5.0);
LieVector center1(v_center1), center2(v_center2);
Matrix expected11 = (Matrix(1, 1) << -sin(center1(0))*cos(center2(0))); Matrix expected11 = (Matrix(1, 1) << -sin(center1(0))*cos(center2(0)));
Matrix actual11 = numericalHessian211(f3, center1, center2); Matrix actual11 = numericalHessian211(f3, center1, center2);
@ -90,7 +93,11 @@ double f4(const LieVector& x, const LieVector& y, const LieVector& z) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(testNumericalDerivative, numericalHessian311) { TEST(testNumericalDerivative, numericalHessian311) {
LieVector center1(1, 1.0), center2(1, 2.0), center3(1, 3.0); Vector v_center1 = (Vector(1) << 1.0);
Vector v_center2 = (Vector(1) << 2.0);
Vector v_center3 = (Vector(1) << 3.0);
LieVector center1(v_center1), center2(v_center2), center3(v_center3);
double x = center1(0), y = center2(0), z = center3(0); double x = center1(0), y = center2(0), z = center3(0);
Matrix expected11 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z); Matrix expected11 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z);
Matrix actual11 = numericalHessian311(f4, center1, center2, center3); Matrix actual11 = numericalHessian311(f4, center1, center2, center3);

4
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -168,9 +168,9 @@ TEST( CombinedImuFactor, ErrorWithBiases )
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
LieVector v1(3, 0.5, 0.0, 0.0); LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
LieVector v2(3, 0.5, 0.0, 0.0); LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
// Measurements // Measurements
Vector3 gravity; gravity << 0, 0, 9.81; Vector3 gravity; gravity << 0, 0, 9.81;

20
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -167,9 +167,9 @@ TEST( ImuFactor, Error )
// Linearization point // Linearization point
imuBias::ConstantBias bias; // Bias imuBias::ConstantBias bias; // Bias
Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
LieVector v1(3, 0.5, 0.0, 0.0); LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
LieVector v2(3, 0.5, 0.0, 0.0); LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
// Measurements // Measurements
Vector3 gravity; gravity << 0, 0, 9.81; Vector3 gravity; gravity << 0, 0, 9.81;
@ -238,16 +238,16 @@ TEST( ImuFactor, ErrorWithBiases )
// Linearization point // Linearization point
// Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot) // Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot)
// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); // Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
// LieVector v1(3, 0.5, 0.0, 0.0); // LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
// LieVector v2(3, 0.5, 0.0, 0.0); // LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
LieVector v1(3, 0.5, 0.0, 0.0); LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
LieVector v2(3, 0.5, 0.0, 0.0); LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
// Measurements // Measurements
Vector3 gravity; gravity << 0, 0, 9.81; Vector3 gravity; gravity << 0, 0, 9.81;
@ -445,9 +445,9 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
//{ //{
// // Linearization point // // Linearization point
// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); // Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
// LieVector v1(3, 0.5, 0.0, 0.0); // LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
// LieVector v2(3, 0.5, 0.0, 0.0); // LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
// imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012)); // imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012));
// //
// // Pre-integrator // // Pre-integrator
@ -501,9 +501,9 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
LieVector v1(3, 0.5, 0.0, 0.0); LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
LieVector v2(3, 0.5, 0.0, 0.0); LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
// Measurements // Measurements
Vector3 gravity; gravity << 0, 0, 9.81; Vector3 gravity; gravity << 0, 0, 9.81;

83
gtsam/nonlinear/tests/testValues.cpp
View File

@ -65,7 +65,8 @@ public:
TEST( Values, equals1 ) TEST( Values, equals1 )
{ {
Values expected; Values expected;
LieVector v(3, 5.0, 6.0, 7.0); LieVector v((Vector(3) << 5.0, 6.0, 7.0));
expected.insert(key1,v); expected.insert(key1,v);
Values actual; Values actual;
actual.insert(key1,v); actual.insert(key1,v);
@ -76,8 +77,9 @@ TEST( Values, equals1 )
TEST( Values, equals2 ) TEST( Values, equals2 )
{ {
Values cfg1, cfg2; Values cfg1, cfg2;
LieVector v1(3, 5.0, 6.0, 7.0); LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
LieVector v2(3, 5.0, 6.0, 8.0); LieVector v2((Vector(3) << 5.0, 6.0, 8.0));
cfg1.insert(key1, v1); cfg1.insert(key1, v1);
cfg2.insert(key1, v2); cfg2.insert(key1, v2);
CHECK(!cfg1.equals(cfg2)); CHECK(!cfg1.equals(cfg2));
@ -88,8 +90,9 @@ TEST( Values, equals2 )
TEST( Values, equals_nan ) TEST( Values, equals_nan )
{ {
Values cfg1, cfg2; Values cfg1, cfg2;
LieVector v1(3, 5.0, 6.0, 7.0); LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
LieVector v2(3, inf, inf, inf); LieVector v2((Vector(3) << inf, inf, inf));
cfg1.insert(key1, v1); cfg1.insert(key1, v1);
cfg2.insert(key1, v2); cfg2.insert(key1, v2);
CHECK(!cfg1.equals(cfg2)); CHECK(!cfg1.equals(cfg2));
@ -100,10 +103,11 @@ TEST( Values, equals_nan )
TEST( Values, insert_good ) TEST( Values, insert_good )
{ {
Values cfg1, cfg2, expected; Values cfg1, cfg2, expected;
LieVector v1(3, 5.0, 6.0, 7.0); LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
LieVector v2(3, 8.0, 9.0, 1.0); LieVector v2((Vector(3) << 8.0, 9.0, 1.0));
LieVector v3(3, 2.0, 4.0, 3.0); LieVector v3((Vector(3) << 2.0, 4.0, 3.0));
LieVector v4(3, 8.0, 3.0, 7.0); LieVector v4((Vector(3) << 8.0, 3.0, 7.0));
cfg1.insert(key1, v1); cfg1.insert(key1, v1);
cfg1.insert(key2, v2); cfg1.insert(key2, v2);
cfg2.insert(key3, v4); cfg2.insert(key3, v4);
@ -121,10 +125,11 @@ TEST( Values, insert_good )
TEST( Values, insert_bad ) TEST( Values, insert_bad )
{ {
Values cfg1, cfg2; Values cfg1, cfg2;
LieVector v1(3, 5.0, 6.0, 7.0); LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
LieVector v2(3, 8.0, 9.0, 1.0); LieVector v2((Vector(3) << 8.0, 9.0, 1.0));
LieVector v3(3, 2.0, 4.0, 3.0); LieVector v3((Vector(3) << 2.0, 4.0, 3.0));
LieVector v4(3, 8.0, 3.0, 7.0); LieVector v4((Vector(3) << 8.0, 3.0, 7.0));
cfg1.insert(key1, v1); cfg1.insert(key1, v1);
cfg1.insert(key2, v2); cfg1.insert(key2, v2);
cfg2.insert(key2, v3); cfg2.insert(key2, v3);
@ -137,8 +142,8 @@ TEST( Values, insert_bad )
TEST( Values, update_element ) TEST( Values, update_element )
{ {
Values cfg; Values cfg;
LieVector v1(3, 5.0, 6.0, 7.0); LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
LieVector v2(3, 8.0, 9.0, 1.0); LieVector v2((Vector(3) << 8.0, 9.0, 1.0));
cfg.insert(key1, v1); cfg.insert(key1, v1);
CHECK(cfg.size() == 1); CHECK(cfg.size() == 1);
@ -181,8 +186,8 @@ TEST(Values, basic_functions)
//TEST(Values, dim_zero) //TEST(Values, dim_zero)
//{ //{
// Values config0; // Values config0;
// config0.insert(key1, LieVector(2, 2.0, 3.0)); // config0.insert(key1, LieVector((Vector(2) << 2.0, 3.0));
// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); // config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0));
// LONGS_EQUAL(5, config0.dim()); // LONGS_EQUAL(5, config0.dim());
// //
// VectorValues expected; // VectorValues expected;
@ -195,16 +200,16 @@ TEST(Values, basic_functions)
TEST(Values, expmap_a) TEST(Values, expmap_a)
{ {
Values config0; Values config0;
config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
VectorValues increment = pair_list_of<Key, Vector> VectorValues increment = pair_list_of<Key, Vector>
(key1, (Vector(3) << 1.0, 1.1, 1.2)) (key1, (Vector(3) << 1.0, 1.1, 1.2))
(key2, (Vector(3) << 1.3, 1.4, 1.5)); (key2, (Vector(3) << 1.3, 1.4, 1.5));
Values expected; Values expected;
expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2)); expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2)));
expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5)));
CHECK(assert_equal(expected, config0.retract(increment))); CHECK(assert_equal(expected, config0.retract(increment)));
} }
@ -213,15 +218,15 @@ TEST(Values, expmap_a)
TEST(Values, expmap_b) TEST(Values, expmap_b)
{ {
Values config0; Values config0;
config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
VectorValues increment = pair_list_of<Key, Vector> VectorValues increment = pair_list_of<Key, Vector>
(key2, (Vector(3) << 1.3, 1.4, 1.5)); (key2, (Vector(3) << 1.3, 1.4, 1.5));
Values expected; Values expected;
expected.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); expected.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5)));
CHECK(assert_equal(expected, config0.retract(increment))); CHECK(assert_equal(expected, config0.retract(increment)));
} }
@ -230,16 +235,16 @@ TEST(Values, expmap_b)
//TEST(Values, expmap_c) //TEST(Values, expmap_c)
//{ //{
// Values config0; // Values config0;
// config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); // config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); // config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
// //
// Vector increment = LieVector(6, // Vector increment = LieVector(6,
// 1.0, 1.1, 1.2, // 1.0, 1.1, 1.2,
// 1.3, 1.4, 1.5); // 1.3, 1.4, 1.5);
// //
// Values expected; // Values expected;
// expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2)); // expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2)));
// expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); // expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5)));
// //
// CHECK(assert_equal(expected, config0.retract(increment))); // CHECK(assert_equal(expected, config0.retract(increment)));
//} //}
@ -248,8 +253,8 @@ TEST(Values, expmap_b)
TEST(Values, expmap_d) TEST(Values, expmap_d)
{ {
Values config0; Values config0;
config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
//config0.print("config0"); //config0.print("config0");
CHECK(equal(config0, config0)); CHECK(equal(config0, config0));
CHECK(config0.equals(config0)); CHECK(config0.equals(config0));
@ -266,8 +271,8 @@ TEST(Values, expmap_d)
TEST(Values, localCoordinates) TEST(Values, localCoordinates)
{ {
Values valuesA; Values valuesA;
valuesA.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); valuesA.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
valuesA.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); valuesA.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
VectorValues expDelta = pair_list_of<Key, Vector> VectorValues expDelta = pair_list_of<Key, Vector>
(key1, (Vector(3) << 0.1, 0.2, 0.3)) (key1, (Vector(3) << 0.1, 0.2, 0.3))
@ -314,17 +319,17 @@ TEST(Values, exists_)
TEST(Values, update) TEST(Values, update)
{ {
Values config0; Values config0;
config0.insert(key1, LieVector(1, 1.)); config0.insert(key1, LieVector((Vector(1) << 1.)));
config0.insert(key2, LieVector(1, 2.)); config0.insert(key2, LieVector((Vector(1) << 2.)));
Values superset; Values superset;
superset.insert(key1, LieVector(1, -1.)); superset.insert(key1, LieVector((Vector(1) << -1.)));
superset.insert(key2, LieVector(1, -2.)); superset.insert(key2, LieVector((Vector(1) << -2.)));
config0.update(superset); config0.update(superset);
Values expected; Values expected;
expected.insert(key1, LieVector(1, -1.)); expected.insert(key1, LieVector((Vector(1) << -1.)));
expected.insert(key2, LieVector(1, -2.)); expected.insert(key2, LieVector((Vector(1) << -2.)));
CHECK(assert_equal(expected,config0)); CHECK(assert_equal(expected,config0));
} }

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

@ -110,7 +110,7 @@ TEST( GeneralSFMFactor, error ) {
Pose3 x1(R,t1); Pose3 x1(R,t1);
values.insert(X(1), GeneralCamera(x1)); values.insert(X(1), GeneralCamera(x1));
Point3 l1; values.insert(L(1), l1); Point3 l1; values.insert(L(1), l1);
EXPECT(assert_equal((Vector(2) << -3.0, 0.0), factor->unwhitenedError(values))); EXPECT(assert_equal(((Vector) (Vector(2) << -3.0, 0.0)), factor->unwhitenedError(values)));
} }
static const double baseline = 5.0 ; static const double baseline = 5.0 ;

2
gtsam_unstable/geometry/InvDepthCamera3.h
View File

@ -166,7 +166,7 @@ public:
gtsam::Point3 ray = pw - pt; gtsam::Point3 ray = pw - pt;
double theta = atan2(ray.y(), ray.x()); // longitude double theta = atan2(ray.y(), ray.x()); // longitude
double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
return std::make_pair(gtsam::LieVector(5, pt.x(),pt.y(),pt.z(), theta, phi), return std::make_pair(gtsam::LieVector((Vector(5) << pt.x(),pt.y(),pt.z(), theta, phi)),
gtsam::LieScalar(1./depth)); gtsam::LieScalar(1./depth));
} }

18
gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp
View File

@ -29,7 +29,7 @@ TEST( InvDepthFactor, Project1) {
Point2 expected_uv = level_camera.project(landmark); Point2 expected_uv = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
LieVector inv_landmark(5, 1., 0., 1., 0., 0.); LieVector inv_landmark((Vector(5) << 1., 0., 1., 0., 0.));
LieScalar inv_depth(1./4); LieScalar inv_depth(1./4);
Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth); Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth);
EXPECT(assert_equal(expected_uv, actual_uv)); EXPECT(assert_equal(expected_uv, actual_uv));
@ -45,7 +45,7 @@ TEST( InvDepthFactor, Project2) {
Point2 expected = level_camera.project(landmark); Point2 expected = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
LieVector diag_landmark(5, 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0))); LieVector diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0))));
LieScalar inv_depth(1/sqrt(3.0)); LieScalar inv_depth(1/sqrt(3.0));
Point2 actual = inv_camera.project(diag_landmark, inv_depth); Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
@ -60,7 +60,7 @@ TEST( InvDepthFactor, Project3) {
Point2 expected = level_camera.project(landmark); Point2 expected = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
LieVector diag_landmark(5, 0., 0., 0., M_PI/4., atan(2./sqrt(2.0))); LieVector diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(2./sqrt(2.0))));
LieScalar inv_depth( 1./sqrt(1.0+1+4)); LieScalar inv_depth( 1./sqrt(1.0+1+4));
Point2 actual = inv_camera.project(diag_landmark, inv_depth); Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
@ -75,7 +75,7 @@ TEST( InvDepthFactor, Project4) {
Point2 expected = level_camera.project(landmark); Point2 expected = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
LieVector diag_landmark(5, 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.))); LieVector diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.))));
LieScalar inv_depth(1./sqrt(1.+16.+4.)); LieScalar inv_depth(1./sqrt(1.+16.+4.));
Point2 actual = inv_camera.project(diag_landmark, inv_depth); Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
@ -88,7 +88,7 @@ Point2 project_(const Pose3& pose, const LieVector& landmark, const LieScalar& i
TEST( InvDepthFactor, Dproject_pose) TEST( InvDepthFactor, Dproject_pose)
{ {
LieVector landmark(6,0.1,0.2,0.3, 0.1,0.2); LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
LieScalar inv_depth(1./4); LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative31<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth); Matrix expected = numericalDerivative31<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
@ -100,7 +100,7 @@ TEST( InvDepthFactor, Dproject_pose)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( InvDepthFactor, Dproject_landmark) TEST( InvDepthFactor, Dproject_landmark)
{ {
LieVector landmark(5,0.1,0.2,0.3, 0.1,0.2); LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
LieScalar inv_depth(1./4); LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative32<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth); Matrix expected = numericalDerivative32<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
@ -112,7 +112,7 @@ TEST( InvDepthFactor, Dproject_landmark)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( InvDepthFactor, Dproject_inv_depth) TEST( InvDepthFactor, Dproject_inv_depth)
{ {
LieVector landmark(5,0.1,0.2,0.3, 0.1,0.2); LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
LieScalar inv_depth(1./4); LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative33<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth); Matrix expected = numericalDerivative33<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
@ -124,7 +124,7 @@ TEST( InvDepthFactor, Dproject_inv_depth)
/* ************************************************************************* */ /* ************************************************************************* */
TEST(InvDepthFactor, backproject) TEST(InvDepthFactor, backproject)
{ {
LieVector expected(5,0.,0.,1., 0.1,0.2); LieVector expected((Vector(5) << 0.,0.,1., 0.1,0.2));
LieScalar inv_depth(1./4); LieScalar inv_depth(1./4);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Point2 z = inv_camera.project(expected, inv_depth); Point2 z = inv_camera.project(expected, inv_depth);
@ -140,7 +140,7 @@ TEST(InvDepthFactor, backproject)
TEST(InvDepthFactor, backproject2) TEST(InvDepthFactor, backproject2)
{ {
// backwards facing camera // backwards facing camera
LieVector expected(5,-5.,-5.,2., 3., -0.1); LieVector expected((Vector(5) << -5.,-5.,2., 3., -0.1));
LieScalar inv_depth(1./10); LieScalar inv_depth(1./10);
InvDepthCamera3<Cal3_S2> inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K); InvDepthCamera3<Cal3_S2> inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K);
Point2 z = inv_camera.project(expected, inv_depth); Point2 z = inv_camera.project(expected, inv_depth);

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

@ -119,10 +119,10 @@ TEST( InertialNavFactor_GlobalVelocity, Predict)
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
LieVector Vel1(3, 0.50, -0.50, 0.40); LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40));
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04));
LieVector expectedVel2(3, 0.51, -0.48, 0.43); LieVector expectedVel2((Vector(3) << 0.51, -0.48, 0.43));
Pose3 actualPose2; Pose3 actualPose2;
LieVector actualVel2; LieVector actualVel2;
f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2); f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2);
@ -157,8 +157,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel)
Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04));
LieVector Vel1(3, 0.50, -0.50, 0.40); LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40));
LieVector Vel2(3, 0.51, -0.48, 0.43); LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43));
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@ -192,8 +192,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRot)
Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0)); Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0));
Pose3 Pose2(Rot3::Expmap(measurement_gyro*measurement_dt), Point3(2.0,1.0,3.0)); Pose3 Pose2(Rot3::Expmap(measurement_gyro*measurement_dt), Point3(2.0,1.0,3.0));
LieVector Vel1(3,0.0,0.0,0.0); LieVector Vel1((Vector(3) << 0.0, 0.0, 0.0));
LieVector Vel2(3,0.0,0.0,0.0); LieVector Vel2((Vector(3) << 0.0, 0.0, 0.0));
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@ -230,7 +230,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
-0.652537293, 0.709880342, 0.265075427); -0.652537293, 0.709880342, 0.265075427);
Point3 t1(2.0,1.0,3.0); Point3 t1(2.0,1.0,3.0);
Pose3 Pose1(R1, t1); Pose3 Pose1(R1, t1);
LieVector Vel1(3,0.5,-0.5,0.4); LieVector Vel1((Vector(3) << 0.5, -0.5, 0.4));
Rot3 R2(0.473618898, 0.119523052, 0.872582019, Rot3 R2(0.473618898, 0.119523052, 0.872582019,
0.609241153, 0.67099888, -0.422594037, 0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388); -0.636011287, 0.731761397, 0.244979388);
@ -302,13 +302,13 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
-0.652537293, 0.709880342, 0.265075427); -0.652537293, 0.709880342, 0.265075427);
Point3 t1(2.0,1.0,3.0); Point3 t1(2.0,1.0,3.0);
Pose3 Pose1(R1, t1); Pose3 Pose1(R1, t1);
LieVector Vel1(3,0.5,-0.5,0.4); LieVector Vel1((Vector(3) << 0.5, -0.5, 0.4));
Rot3 R2(0.473618898, 0.119523052, 0.872582019, Rot3 R2(0.473618898, 0.119523052, 0.872582019,
0.609241153, 0.67099888, -0.422594037, 0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388); -0.636011287, 0.731761397, 0.244979388);
Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800);
Pose3 Pose2(R2, t2); Pose3 Pose2(R2, t2);
LieVector Vel2(3,0.510000000000000, -0.480000000000000, 0.430000000000000); LieVector Vel2((Vector(3) << 0.510000000000000, -0.480000000000000, 0.430000000000000));
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual; Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual;
@ -447,10 +447,10 @@ TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform)
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);
Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
LieVector Vel1(3, 0.50, -0.50, 0.40); LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40));
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04));
LieVector expectedVel2(3, 0.51, -0.48, 0.43); LieVector expectedVel2((Vector(3) << 0.51, -0.48, 0.43));
Pose3 actualPose2; Pose3 actualPose2;
LieVector actualVel2; LieVector actualVel2;
f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2); f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2);
@ -488,8 +488,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform)
Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04));
LieVector Vel1(3, 0.50, -0.50, 0.40); LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40));
LieVector Vel2(3, 0.51, -0.48, 0.43); LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43));
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@ -527,8 +527,8 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform)
Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0)); Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0));
Pose3 Pose2(Rot3::Expmap(body_P_sensor.rotation().matrix()*measurement_gyro*measurement_dt), Point3(2.0, 1.0, 3.0)); Pose3 Pose2(Rot3::Expmap(body_P_sensor.rotation().matrix()*measurement_gyro*measurement_dt), Point3(2.0, 1.0, 3.0));
LieVector Vel1(3,0.0,0.0,0.0); LieVector Vel1((Vector(3) << 0.0,0.0,0.0));
LieVector Vel2(3,0.0,0.0,0.0); LieVector Vel2((Vector(3) << 0.0,0.0,0.0));
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@ -569,7 +569,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform)
-0.652537293, 0.709880342, 0.265075427); -0.652537293, 0.709880342, 0.265075427);
Point3 t1(2.0,1.0,3.0); Point3 t1(2.0,1.0,3.0);
Pose3 Pose1(R1, t1); Pose3 Pose1(R1, t1);
LieVector Vel1(3,0.5,-0.5,0.4); LieVector Vel1((Vector(3) << 0.5,-0.5,0.4));
Rot3 R2(0.473618898, 0.119523052, 0.872582019, Rot3 R2(0.473618898, 0.119523052, 0.872582019,
0.609241153, 0.67099888, -0.422594037, 0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388); -0.636011287, 0.731761397, 0.244979388);
@ -618,13 +618,13 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
-0.652537293, 0.709880342, 0.265075427); -0.652537293, 0.709880342, 0.265075427);
Point3 t1(2.0,1.0,3.0); Point3 t1(2.0,1.0,3.0);
Pose3 Pose1(R1, t1); Pose3 Pose1(R1, t1);
LieVector Vel1(3,0.5,-0.5,0.4); LieVector Vel1((Vector(3) << 0.5,-0.5,0.4));
Rot3 R2(0.473618898, 0.119523052, 0.872582019, Rot3 R2(0.473618898, 0.119523052, 0.872582019,
0.609241153, 0.67099888, -0.422594037, 0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388); -0.636011287, 0.731761397, 0.244979388);
Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800);
Pose3 Pose2(R2, t2); Pose3 Pose2(R2, t2);
LieVector Vel2(3,0.510000000000000, -0.480000000000000, 0.430000000000000); LieVector Vel2((Vector(3) << 0.510000000000000, -0.480000000000000, 0.430000000000000));
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual; Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual;

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

@ -38,7 +38,7 @@ TEST( InvDepthFactor, optimize) {
Point2 expected_uv = level_camera.project(landmark); Point2 expected_uv = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
LieVector inv_landmark(5, 0., 0., 1., 0., 0.); LieVector inv_landmark((Vector(5) << 0., 0., 1., 0., 0.));
// initialize inverse depth with "incorrect" depth of 1/4 // initialize inverse depth with "incorrect" depth of 1/4
// in reality this is 1/5, but initial depth is guessed // in reality this is 1/5, but initial depth is guessed
LieScalar inv_depth(1./4); LieScalar inv_depth(1./4);

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

@ -45,7 +45,7 @@ TEST( InvDepthFactorVariant1, optimize) {
double theta = atan2(ray.y(), ray.x()); double theta = atan2(ray.y(), ray.x());
double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
double rho = 1./ray.norm(); double rho = 1./ray.norm();
LieVector expected(6, x, y, z, theta, phi, rho); LieVector expected((Vector(6) << x, y, z, theta, phi, rho));

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

@ -43,7 +43,7 @@ TEST( InvDepthFactorVariant2, optimize) {
double theta = atan2(ray.y(), ray.x()); double theta = atan2(ray.y(), ray.x());
double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
double rho = 1./ray.norm(); double rho = 1./ray.norm();
LieVector expected(3, theta, phi, rho); LieVector expected((Vector(3) << theta, phi, rho));

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

@ -43,7 +43,7 @@ TEST( InvDepthFactorVariant3, optimize) {
double theta = atan2(landmark_p1.x(), landmark_p1.z()); double theta = atan2(landmark_p1.x(), landmark_p1.z());
double phi = atan2(landmark_p1.y(), sqrt(landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z())); double phi = atan2(landmark_p1.y(), sqrt(landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z()));
double rho = 1./landmark_p1.norm(); double rho = 1./landmark_p1.norm();
LieVector expected(3, theta, phi, rho); LieVector expected((Vector(3) << theta, phi, rho));

30
tests/testNonlinearFactor.cpp
View File

@ -266,10 +266,10 @@ public:
TEST(NonlinearFactor, NoiseModelFactor4) { TEST(NonlinearFactor, NoiseModelFactor4) {
TestFactor4 tf; TestFactor4 tf;
Values tv; Values tv;
tv.insert(X(1), LieVector(1, 1.0)); tv.insert(X(1), LieVector((Vector(1) << 1.0)));
tv.insert(X(2), LieVector(1, 2.0)); tv.insert(X(2), LieVector((Vector(1) << 2.0)));
tv.insert(X(3), LieVector(1, 3.0)); tv.insert(X(3), LieVector((Vector(1) << 3.0)));
tv.insert(X(4), LieVector(1, 4.0)); tv.insert(X(4), LieVector((Vector(1) << 4.0)));
EXPECT(assert_equal((Vector(1) << 10.0), tf.unwhitenedError(tv))); EXPECT(assert_equal((Vector(1) << 10.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9); DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv))); JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));
@ -312,11 +312,11 @@ public:
TEST(NonlinearFactor, NoiseModelFactor5) { TEST(NonlinearFactor, NoiseModelFactor5) {
TestFactor5 tf; TestFactor5 tf;
Values tv; Values tv;
tv.insert(X(1), LieVector(1, 1.0)); tv.insert(X(1), LieVector((Vector(1) << 1.0)));
tv.insert(X(2), LieVector(1, 2.0)); tv.insert(X(2), LieVector((Vector(1) << 2.0)));
tv.insert(X(3), LieVector(1, 3.0)); tv.insert(X(3), LieVector((Vector(1) << 3.0)));
tv.insert(X(4), LieVector(1, 4.0)); tv.insert(X(4), LieVector((Vector(1) << 4.0)));
tv.insert(X(5), LieVector(1, 5.0)); tv.insert(X(5), LieVector((Vector(1) << 5.0)));
EXPECT(assert_equal((Vector(1) << 15.0), tf.unwhitenedError(tv))); EXPECT(assert_equal((Vector(1) << 15.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9); DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv))); JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));
@ -364,12 +364,12 @@ public:
TEST(NonlinearFactor, NoiseModelFactor6) { TEST(NonlinearFactor, NoiseModelFactor6) {
TestFactor6 tf; TestFactor6 tf;
Values tv; Values tv;
tv.insert(X(1), LieVector(1, 1.0)); tv.insert(X(1), LieVector((Vector(1) << 1.0)));
tv.insert(X(2), LieVector(1, 2.0)); tv.insert(X(2), LieVector((Vector(1) << 2.0)));
tv.insert(X(3), LieVector(1, 3.0)); tv.insert(X(3), LieVector((Vector(1) << 3.0)));
tv.insert(X(4), LieVector(1, 4.0)); tv.insert(X(4), LieVector((Vector(1) << 4.0)));
tv.insert(X(5), LieVector(1, 5.0)); tv.insert(X(5), LieVector((Vector(1) << 5.0)));
tv.insert(X(6), LieVector(1, 6.0)); tv.insert(X(6), LieVector((Vector(1) << 6.0)));
EXPECT(assert_equal((Vector(1) << 21.0), tf.unwhitenedError(tv))); EXPECT(assert_equal((Vector(1) << 21.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9); DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv))); JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));