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

Merged in support/noMoreVector_ (pull request #4) 

Dangerous constructors fixed
release/4.3a0
Richard Roberts 2014-01-27 16:30:52 -05:00
parent a2c3b8e083 ca0a355498
commit 8cc58686a1
66 changed files with 183 additions and 359 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/base/LieMatrix.cpp
View File

@ -19,20 +19,6 @@
namespace gtsam {
/* ************************************************************************* */
LieMatrix::LieMatrix(size_t m, size_t n, ...)
: Matrix(m,n) {
va_list ap;
va_start(ap, n);
for(size_t i = 0; i < m; ++i) {
for(size_t j = 0; j < n; ++j) {
double value = va_arg(ap, double);
(*this)(i,j) = value;
}
}
va_end(ap);
}
/* ************************************************************************* */
void LieMatrix::print(const std::string& name) const {
gtsam::print(matrix(), name);

3
gtsam/base/LieMatrix.h
View File

@ -48,9 +48,6 @@ struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
LieMatrix(size_t m, size_t n, const double* const data) :
Matrix(Eigen::Map<const Matrix>(data, m, n)) {}
/** Specify arguments directly, as in Matrix_() - always force these to be doubles */
GTSAM_EXPORT LieMatrix(size_t m, size_t n, ...);
/// @}
/// @name Testable interface
/// @{

15
gtsam/base/LieVector.cpp
View File

@ -24,21 +24,10 @@ namespace gtsam {
/* ************************************************************************* */
LieVector::LieVector(size_t m, const double* const data)
: Vector(Vector_(m,data))
{
}
/* ************************************************************************* */
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);
for(size_t i = 0; i < m; i++)
(*this)(i) = data[i];
}
/* ************************************************************************* */

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 ! */
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 */
Vector vector() const {
return static_cast<Vector>(*this);

43
gtsam/base/Matrix.cpp
View File

@ -36,38 +36,6 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
Matrix Matrix_( size_t m, size_t n, const double* const data) {
MatrixRowMajor A(m,n);
copy(data, data+m*n, A.data());
return Matrix(A);
}
/* ************************************************************************* */
Matrix Matrix_( size_t m, size_t n, const Vector& v)
{
Matrix A(m,n);
// column-wise copy
for( size_t j = 0, k=0 ; j < n ; j++)
for( size_t i = 0; i < m ; i++,k++)
A(i,j) = v(k);
return A;
}
/* ************************************************************************* */
Matrix Matrix_(size_t m, size_t n, ...) {
Matrix A(m,n);
va_list ap;
va_start(ap, n);
for( size_t i = 0 ; i < m ; i++)
for( size_t j = 0 ; j < n ; j++) {
double value = va_arg(ap, double);
A(i,j) = value;
}
va_end(ap);
return A;
}
/* ************************************************************************* */
Matrix zeros( size_t m, size_t n ) {
return Matrix::Zero(m,n);
@ -211,17 +179,6 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVec
x += alpha * A.transpose() * e;
}
/* ************************************************************************* */
Vector Vector_(const Matrix& A)
{
size_t m = A.rows(), n = A.cols();
Vector v(m*n);
for( size_t j = 0, k=0 ; j < n ; j++)
for( size_t i = 0; i < m ; i++,k++)
v(k) = A(i,j);
return v;
}
/* ************************************************************************* */
void print(const Matrix& A, const string &s, ostream& stream) {
size_t m = A.rows(), n = A.cols();

26
gtsam/base/Matrix.h
View File

@ -45,27 +45,6 @@ typedef Eigen::Matrix<double,6,6> Matrix6;
typedef Eigen::Block<Matrix> SubMatrix;
typedef Eigen::Block<const Matrix> ConstSubMatrix;
/**
* constructor with size and initial data, row order !
*/
GTSAM_EXPORT Matrix Matrix_(size_t m, size_t n, const double* const data);
/**
* constructor with size and vector data, column order !!!
*/
GTSAM_EXPORT Matrix Matrix_(size_t m, size_t n, const Vector& v);
/**
* constructor from Vector yielding v.size()*1 vector
*/
inline Matrix Matrix_(const Vector& v) { return Matrix_(v.size(),1,v);}
/**
* nice constructor, dangerous as number of arguments must be exactly right
* and you have to pass doubles !!! always use 0.0 never 0
*/
GTSAM_EXPORT Matrix Matrix_(size_t m, size_t n, ...);
// Matlab-like syntax
/**
@ -196,11 +175,6 @@ inline MATRIX prod(const MATRIX& A, const MATRIX&B) {
return result;
}
/**
* convert to column vector, column order !!!
*/
GTSAM_EXPORT Vector Vector_(const Matrix& A);
/**
* print a matrix
*/

27
gtsam/base/Vector.cpp
View File

@ -79,33 +79,6 @@ void odprintf(const char *format, ...) {
//#endif
}
/* ************************************************************************* */
Vector Vector_( size_t m, const double* const data) {
Vector A(m);
copy(data, data+m, A.data());
return A;
}
/* ************************************************************************* */
Vector Vector_(size_t m, ...) {
Vector v(m);
va_list ap;
va_start(ap, m);
for( size_t i = 0 ; i < m ; i++) {
double value = va_arg(ap, double);
v(i) = value;
}
va_end(ap);
return v;
}
/* ************************************************************************* */
Vector Vector_(const std::vector<double>& d) {
Vector v(d.size());
copy(d.begin(), d.end(), v.data());
return v;
}
/* ************************************************************************* */
bool zero(const Vector& v) {
bool result = true;

16
gtsam/base/Vector.h
View File

@ -46,22 +46,6 @@ typedef Eigen::VectorBlock<const Vector> ConstSubVector;
*/
GTSAM_EXPORT void odprintf(const char *format, ...);
/**
* constructor with size and initial data, row order !
*/
GTSAM_EXPORT Vector Vector_( size_t m, const double* const data);
/**
* nice constructor, dangerous as number of arguments must be exactly right
* and you have to pass doubles !!! always use 0.0 never 0
*/
GTSAM_EXPORT Vector Vector_(size_t m, ...);
/**
* Create a numeric vector from an STL vector of doubles
*/
GTSAM_EXPORT Vector Vector_(const std::vector<double>& data);
/**
* Create vector initialized to a constant value
* @param n is the size of the vector

2
gtsam/base/numericalDerivative.h
View File

@ -59,7 +59,7 @@ namespace gtsam {
/** global functions for converting to a LieVector for use with numericalDerivative */
inline LieVector makeLieVector(const Vector& v) { return LieVector(v); }
inline LieVector makeLieVectorD(double d) { return LieVector(Vector_(1, d)); }
inline LieVector makeLieVectorD(double d) { return LieVector((Vector) (Vector(1) << d)); }
/**
* Numerically compute gradient of scalar function

11
gtsam/base/tests/testLieMatrix.cpp
View File

@ -39,20 +39,17 @@ TEST( LieMatrix, construction ) {
TEST( LieMatrix, other_constructors ) {
Matrix init = (Matrix(2,2) << 10.0,20.0, 30.0,40.0);
LieMatrix exp(init);
LieMatrix a(2,2,10.0,20.0,30.0,40.0);
double data[] = {10,30,20,40};
LieMatrix b(2,2,data);
EXPECT(assert_equal(exp, a));
EXPECT(assert_equal(exp, b));
EXPECT(assert_equal(b, a));
}
/* ************************************************************************* */
TEST(LieMatrix, retract) {
LieMatrix init(2,2, 1.0,2.0,3.0,4.0);
Vector update = Vector_(4, 3.0, 4.0, 6.0, 7.0);
LieMatrix init((Matrix(2,2) << 1.0,2.0,3.0,4.0));
Vector update = (Vector(4) << 3.0, 4.0, 6.0, 7.0);
LieMatrix expected(2,2, 4.0, 6.0, 9.0, 11.0);
LieMatrix expected((Matrix(2,2) << 4.0, 6.0, 9.0, 11.0));
LieMatrix actual = init.retract(update);
EXPECT(assert_equal(expected, actual));
@ -63,7 +60,7 @@ TEST(LieMatrix, retract) {
EXPECT(assert_equal(expectedUpdate, actualUpdate));
Vector expectedLogmap = (Vector(4) << 1, 2, 3, 4);
Vector actualLogmap = LieMatrix::Logmap(LieMatrix(2,2, 1.0, 2.0, 3.0, 4.0));
Vector actualLogmap = LieMatrix::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0)));
EXPECT(assert_equal(expectedLogmap, actualLogmap));
}

2
gtsam/base/tests/testLieScalar.cpp
View File

@ -42,7 +42,7 @@ TEST( testLieScalar, construction ) {
TEST( testLieScalar, localCoordinates ) {
LieScalar lie1(1.), lie2(3.);
EXPECT(assert_equal(Vector_(1, 2.), lie1.localCoordinates(lie2)));
EXPECT(assert_equal((Vector)(Vector(1) << 2), lie1.localCoordinates(lie2)));
}
/* ************************************************************************* */

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

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

12
gtsam/base/tests/testMatrix.cpp
View File

@ -43,18 +43,6 @@ TEST( matrix, constructor_data )
EQUALITY(A,B);
}
/* ************************************************************************* */
/*
TEST( matrix, constructor_vector )
{
double data[] = { -5, 3, 0, -5 };
Matrix A = Matrix_(2, 2, -5, 3, 0, -5);
Vector v(4);
copy(data, data + 4, v.data());
Matrix B = Matrix_(2, 2, v); // this one is column order !
EQUALITY(A,trans(B));
}
*/
/* ************************************************************************* */
TEST( matrix, Matrix_ )
{

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

@ -48,7 +48,8 @@ double f2(const LieVector& x) {
/* ************************************************************************* */
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) <<
-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) {
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 actual11 = numericalHessian211(f3, center1, center2);
@ -90,7 +93,11 @@ double f4(const LieVector& x, const LieVector& y, const LieVector& z) {
/* ************************************************************************* */
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);
Matrix expected11 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z);
Matrix actual11 = numericalHessian311(f4, center1, center2, center3);

9
gtsam/base/tests/testVector.cpp
View File

@ -23,15 +23,6 @@
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST( TestVector, Vector_variants )
{
Vector a = (Vector(2) << 10.0,20.0);
double data[] = {10,20};
Vector b = Vector_(2, data);
EXPECT(assert_equal(a, b));
}
namespace {
/* ************************************************************************* */
template<typename Derived>

2
gtsam/geometry/Point2.h
View File

@ -172,7 +172,7 @@ public:
static inline Point2 Expmap(const Vector& v) { return Point2(v); }
/// Log map around identity - just return the Point2 as a vector
static inline Vector Logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); }
static inline Vector Logmap(const Point2& dp) { return (Vector(2) << dp.x(), dp.y()); }
/// @}
/// @name Vector Space

2
gtsam/geometry/Pose3.cpp
View File

@ -98,7 +98,7 @@ Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y,
/* ************************************************************************* */
Matrix6 Pose3::dExpInv_exp(const Vector& xi) {
// Bernoulli numbers, from Wikipedia
static const Vector B = Vector_(9, 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0,
static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0,
0.0, 1.0 / 42.0, 0.0, -1.0 / 30);
static const int N = 5; // order of approximation
Matrix res = I6;

2
gtsam/geometry/Rot2.h
View File

@ -170,7 +170,7 @@ namespace gtsam {
///Log map at identity - return the canonical coordinates of this rotation
static inline Vector Logmap(const Rot2& r) {
return Vector_(1, r.theta());
return (Vector(1) << r.theta());
}
/// @}

2
gtsam/geometry/Rot3.h
View File

@ -194,7 +194,7 @@ namespace gtsam {
* @return incremental rotation matrix
*/
static Rot3 rodriguez(double wx, double wy, double wz)
{ return rodriguez(Vector_(3,wx,wy,wz));}
{ return rodriguez((Vector(3) << wx, wy, wz));}
/// @}
/// @name Testable

2
gtsam/geometry/Sphere2.cpp
View File

@ -185,7 +185,7 @@ Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const {
double alpha = p.transpose() * q;
assert(alpha != 0.0);
Matrix coeffs = (B.transpose() * q) / alpha;
Vector result = Vector_(2, coeffs(0, 0), coeffs(1, 0));
Vector result = (Vector(2) << coeffs(0, 0), coeffs(1, 0));
return result;
} else {
assert (false);

2
gtsam/geometry/tests/testCal3DS2.cpp
View File

@ -36,7 +36,7 @@ TEST( Cal3DS2, uncalibrate)
double g = 1+k[0]*r+k[1]*r*r ;
double tx = 2*k[2]*p.x()*p.y() + k[3]*(r+2*p.x()*p.x()) ;
double ty = k[2]*(r+2*p.y()*p.y()) + 2*k[3]*p.x()*p.y() ;
Vector v_hat = Vector_(3, g*p.x() + tx, g*p.y() + ty, 1.0) ;
Vector v_hat = (Vector(3) << g*p.x() + tx, g*p.y() + ty, 1.0) ;
Vector v_i = K.K() * v_hat ;
Point2 p_i(v_i(0)/v_i(2), v_i(1)/v_i(2)) ;
Point2 q = K.uncalibrate(p);

2
gtsam/geometry/tests/testPoint3.cpp
View File

@ -41,7 +41,7 @@ TEST(Point3, Lie) {
EXPECT(assert_equal(eye(3), H2));
EXPECT(assert_equal(Point3(5, 7, 9), p1.retract((Vector(3) << 4., 5., 6.))));
EXPECT(assert_equal(Vector_(3, 3., 3., 3.), p1.localCoordinates(p2)));
EXPECT(assert_equal((Vector)(Vector(3) << 3.,3.,3.), p1.localCoordinates(p2)));
}
/* ************************************************************************* */

2
gtsam/geometry/tests/testRot2.cpp
View File

@ -96,7 +96,7 @@ TEST(Rot2, logmap)
{
Rot2 rot0(Rot2::fromAngle(M_PI/2.0));
Rot2 rot(Rot2::fromAngle(M_PI));
Vector expected = Vector_(1, M_PI/2.0);
Vector expected = (Vector(1) << M_PI/2.0);
Vector actual = rot0.localCoordinates(rot);
CHECK(assert_equal(expected, actual));
}

16
gtsam/geometry/tests/testSphere2.cpp
View File

@ -213,9 +213,9 @@ inline static Vector randomVector(const Vector& minLimits,
TEST(Sphere2, localCoordinates_retract) {
size_t numIterations = 10000;
Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit =
Vector_(3, 1.0, 1.0, 1.0);
Vector minXiLimit = Vector_(2, -1.0, -1.0), maxXiLimit = Vector_(2, 1.0, 1.0);
Vector minSphereLimit = (Vector(3) << -1.0, -1.0, -1.0), maxSphereLimit =
(Vector(3) << 1.0, 1.0, 1.0);
Vector minXiLimit = (Vector(2) << -1.0, -1.0), maxXiLimit = (Vector(2) << 1.0, 1.0);
for (size_t i = 0; i < numIterations; i++) {
// Sleep for the random number generator (TODO?: Better create all of them first).
@ -243,9 +243,9 @@ TEST(Sphere2, localCoordinates_retract) {
TEST(Sphere2, localCoordinates_retract_expmap) {
size_t numIterations = 10000;
Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit =
Vector_(3, 1.0, 1.0, 1.0);
Vector minXiLimit = Vector_(2, -M_PI, -M_PI), maxXiLimit = Vector_(2, M_PI, M_PI);
Vector minSphereLimit = (Vector(3) << -1.0, -1.0, -1.0), maxSphereLimit =
(Vector(3) << 1.0, 1.0, 1.0);
Vector minXiLimit = (Vector(2) << -M_PI, -M_PI), maxXiLimit = (Vector(2) << M_PI, M_PI);
for (size_t i = 0; i < numIterations; i++) {
// Sleep for the random number generator (TODO?: Better create all of them first).
@ -288,7 +288,7 @@ TEST(Sphere2, localCoordinates_retract_expmap) {
// EXPECT(assert_equal(expected,actual1));
// EXPECT(assert_equal(expected,actual2));
//
// Matrix expectedH1 = Matrix_(3,3,
// Matrix expectedH1 = (Matrix(3,3) <<
// 0.0,-1.0,-2.0,
// 1.0, 0.0,-2.0,
// 0.0, 0.0,-1.0
@ -299,7 +299,7 @@ TEST(Sphere2, localCoordinates_retract_expmap) {
// // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx
// EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1));
//
// Matrix expectedH2 = Matrix_(3,3,
// Matrix expectedH2 = (Matrix(3,3) <<
// 1.0, 0.0, 0.0,
// 0.0, 1.0, 0.0,
// 0.0, 0.0, 1.0

2
gtsam/geometry/tests/timePose2.cpp
View File

@ -138,7 +138,7 @@ int main()
// create a random pose:
double x = 4.0, y = 2.0, r = 0.3;
Vector v = Vector_(3,x,y,r);
Vector v = (Vector(3) << x, y, r);
Pose2 X = Pose2(3,2,0.4), X2 = X.retract(v), X3(5,6,0.3);
TEST(Expmap, Pose2::Expmap(v));

2
gtsam/geometry/tests/timeRot2.cpp
View File

@ -95,7 +95,7 @@ int main()
// create a random direction:
double norm=sqrt(16.0+4.0);
double x=4.0/norm, y=2.0/norm;
Vector v = Vector_(2,x,y);
Vector v = (Vector(2) << x, y);
Rot2 R = Rot2(0.4), R2 = R.retract(v), R3(0.6);
TEST(Rot2_Expmap, Rot2::Expmap(v));

2
gtsam/geometry/tests/timeRot3.cpp
View File

@ -39,7 +39,7 @@ int main()
// create a random direction:
double norm=sqrt(1.0+16.0+4.0);
double x=1.0/norm, y=4.0/norm, z=2.0/norm;
Vector v = Vector_(3,x,y,z);
Vector v = (Vector(3) << x, y, z);
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2), R2 = R.retract(v);
TEST("Rodriguez formula given axis angle", Rot3::rodriguez(v,0.001))

8
gtsam/linear/VectorValues.h
View File

@ -49,15 +49,15 @@ namespace gtsam {
* Example:
* \code
VectorValues values;
values.insert(3, Vector_(3, 1.0, 2.0, 3.0));
values.insert(4, Vector_(2, 4.0, 5.0));
values.insert(0, Vector_(4, 6.0, 7.0, 8.0, 9.0));
values.insert(3, (Vector(3) << 1.0, 2.0, 3.0));
values.insert(4, (Vector(2) << 4.0, 5.0));
values.insert(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0));
// Prints [ 3.0 4.0 ]
gtsam::print(values[1]);
// Prints [ 8.0 9.0 ]
values[1] = Vector_(2, 8.0, 9.0);
values[1] = (Vector(2) << 8.0, 9.0);
gtsam::print(values[1]);
\endcode
*

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

@ -24,7 +24,7 @@ const double tol = 1e-5;
/* ************************************************************************* */
TEST(testSampler, basic) {
Vector sigmas = Vector_(3, 1.0, 0.1, 0.0);
Vector sigmas = (Vector(3) << 1.0, 0.1, 0.0);
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(sigmas);
char seed = 'A';
Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1);

2
gtsam/navigation/ImuBias.h
View File

@ -113,7 +113,7 @@ namespace imuBias {
//
// return measurement - biasGyro_ - w_earth_rate_I;
//
//// Vector bias_gyro_temp(Vector_(3, -bias_gyro_(0), bias_gyro_(1), bias_gyro_(2)));
//// Vector bias_gyro_temp((Vector(3) << -bias_gyro_(0), bias_gyro_(1), bias_gyro_(2)));
//// return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G;
// }

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 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));
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));
LieVector v2(3, 0.5, 0.0, 0.0);
LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;

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

@ -167,9 +167,9 @@ TEST( ImuFactor, Error )
// Linearization point
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));
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));
LieVector v2(3, 0.5, 0.0, 0.0);
LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
@ -238,16 +238,16 @@ TEST( ImuFactor, ErrorWithBiases )
// Linearization point
// 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));
// 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));
// 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)
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));
LieVector v2(3, 0.5, 0.0, 0.0);
LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
@ -445,9 +445,9 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
//{
// // 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));
// 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));
// 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));
//
// // 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)
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));
LieVector v2(3, 0.5, 0.0, 0.0);
LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;

4
gtsam/nonlinear/ISAM2.h
View File

@ -122,8 +122,8 @@ struct GTSAM_EXPORT ISAM2Params {
* entries would be added with:
* \code
FastMap<char,Vector> thresholds;
thresholds['x'] = Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold
thresholds['l'] = Vector_(3, 1.0, 1.0, 1.0); // 1.0 m landmark position threshold
thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold
thresholds['l'] = (Vector(3) << 1.0, 1.0, 1.0); // 1.0 m landmark position threshold
params.relinearizeThreshold = thresholds;
\endcode
*/

2
gtsam/nonlinear/tests/testLinearContainerFactor.cpp
View File

@ -19,7 +19,7 @@ using namespace std;
using namespace boost::assign;
using namespace gtsam;
const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0));
const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 1.0, 1.0));
const double tol = 1e-5;
gtsam::Key l1 = 101, l2 = 102, x1 = 1, x2 = 2;

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

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

2
gtsam/slam/BearingRangeFactor.h
View File

@ -96,7 +96,7 @@ namespace gtsam {
Vector e1 = Rot::Logmap(measuredBearing_.between(y1));
double y2 = pose.range(point, H21_, H22_);
Vector e2 = Vector_(1, y2 - measuredRange_);
Vector e2 = (Vector(1) << y2 - measuredRange_);
if (H1) *H1 = gtsam::stack(2, &H11, &H21);
if (H2) *H2 = gtsam::stack(2, &H12, &H22);

2
gtsam/slam/RangeFactor.h
View File

@ -73,7 +73,7 @@ namespace gtsam {
} else {
hx = pose.range(point, H1, H2);
}
return Vector_(1, hx - measured_);
return (Vector(1) << hx - measured_);
}
/** return the measured */

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

@ -110,7 +110,7 @@ TEST( GeneralSFMFactor, error ) {
Pose3 x1(R,t1);
values.insert(X(1), GeneralCamera(x1));
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 ;

17
gtsam_unstable/base/FixedVector.h
View File

@ -45,23 +45,6 @@ public:
std::copy(values, values+N, this->data());
}
/**
* nice constructor, dangerous as number of arguments must be exactly right
* and you have to pass doubles !!! always use 0.0 never 0
*
* NOTE: this will throw warnings/explode if there is no argument
* before the variadic section, so there is a meaningless size argument.
*/
FixedVector(size_t n, ...) {
va_list ap;
va_start(ap, n);
for(size_t i = 0 ; i < N ; i++) {
double value = va_arg(ap, double);
(*this)(i) = value;
}
va_end(ap);
}
/**
* Create vector initialized to a constant value
* @param value constant value

19
gtsam_unstable/base/tests/testFixedVector.cpp
View File

@ -28,7 +28,7 @@ static const double tol = 1e-9;
/* ************************************************************************* */
TEST( testFixedVector, conversions ) {
double data1[] = {1.0, 2.0, 3.0};
Vector v1 = Vector_(3, data1);
Vector v1 = (Vector(3) << data1[0], data1[1], data1[2]);
TestVector3 fv1(v1), fv2(data1);
Vector actFv2(fv2);
@ -37,7 +37,7 @@ TEST( testFixedVector, conversions ) {
/* ************************************************************************* */
TEST( testFixedVector, variable_constructor ) {
TestVector3 act(3, 1.0, 2.0, 3.0);
TestVector3 act((Vector(3) << 1.0, 2.0, 3.0));
EXPECT_DOUBLES_EQUAL(1.0, act(0), tol);
EXPECT_DOUBLES_EQUAL(2.0, act(1), tol);
EXPECT_DOUBLES_EQUAL(3.0, act(2), tol);
@ -45,8 +45,9 @@ TEST( testFixedVector, variable_constructor ) {
/* ************************************************************************* */
TEST( testFixedVector, equals ) {
TestVector3 vec1(3, 1.0, 2.0, 3.0), vec2(3, 1.0, 2.0, 3.0), vec3(3, 2.0, 3.0, 4.0);
TestVector5 vec4(5, 1.0, 2.0, 3.0, 4.0, 5.0);
TestVector3 vec1((Vector(3) << 1.0, 2.0, 3.0)), vec2((Vector(3) << 1.0, 2.0, 3.0)),
vec3((Vector(3) << 2.0, 3.0, 4.0));
TestVector5 vec4((Vector(5) << 1.0, 2.0, 3.0, 4.0, 5.0));
EXPECT(assert_equal(vec1, vec1, tol));
EXPECT(assert_equal(vec1, vec2, tol));
@ -60,23 +61,23 @@ TEST( testFixedVector, equals ) {
/* ************************************************************************* */
TEST( testFixedVector, static_constructors ) {
TestVector3 actZero = TestVector3::zero();
TestVector3 expZero(3, 0.0, 0.0, 0.0);
TestVector3 expZero((Vector(3) << 0.0, 0.0, 0.0));
EXPECT(assert_equal(expZero, actZero, tol));
TestVector3 actOnes = TestVector3::ones();
TestVector3 expOnes(3, 1.0, 1.0, 1.0);
TestVector3 expOnes((Vector(3) << 1.0, 1.0, 1.0));
EXPECT(assert_equal(expOnes, actOnes, tol));
TestVector3 actRepeat = TestVector3::repeat(2.3);
TestVector3 expRepeat(3, 2.3, 2.3, 2.3);
TestVector3 expRepeat((Vector(3) << 2.3, 2.3, 2.3));
EXPECT(assert_equal(expRepeat, actRepeat, tol));
TestVector3 actBasis = TestVector3::basis(1);
TestVector3 expBasis(3, 0.0, 1.0, 0.0);
TestVector3 expBasis((Vector(3) << 0.0, 1.0, 0.0));
EXPECT(assert_equal(expBasis, actBasis, tol));
TestVector3 actDelta = TestVector3::delta(1, 2.3);
TestVector3 expDelta(3, 0.0, 2.3, 0.0);
TestVector3 expDelta((Vector(3) << 0.0, 2.3, 0.0));
EXPECT(assert_equal(expDelta, actDelta, tol));
}

2
gtsam_unstable/geometry/BearingS2.cpp
View File

@ -69,7 +69,7 @@ BearingS2 BearingS2::retract(const Vector& v) const {
/* ************************************************************************* */
Vector BearingS2::localCoordinates(const BearingS2& x) const {
return Vector_(2, azimuth_.localCoordinates(x.azimuth_)(0),
return (Vector(2) << azimuth_.localCoordinates(x.azimuth_)(0),
elevation_.localCoordinates(x.elevation_)(0));
}

2
gtsam_unstable/geometry/InvDepthCamera3.h
View File

@ -166,7 +166,7 @@ public:
gtsam::Point3 ray = pw - pt;
double theta = atan2(ray.y(), ray.x()); // longitude
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));
}

2
gtsam_unstable/geometry/SimPolygon2D.cpp
View File

@ -145,7 +145,7 @@ SimPolygon2D SimPolygon2D::randomTriangle(
// extend line by random dist and angle to get BC
double dAB = randomDistance(mean_side_len, sigma_side_len, min_side_len);
double tABC = randomAngle().theta();
Pose2 xB = xA.retract(Vector_(3, dAB, 0.0, tABC));
Pose2 xB = xA.retract((Vector(3) << dAB, 0.0, tABC));
// extend from B to find C
double dBC = randomDistance(mean_side_len, sigma_side_len, min_side_len);

2
gtsam_unstable/geometry/SimWall2D.cpp
View File

@ -128,7 +128,7 @@ std::pair<Pose2, bool> moveWithBounce(const Pose2& cur_pose, double step_size,
// calculate angle to change by
Rot2 dtheta = Rot2::fromAngle(angle_drift.sample()(0) + bias.theta());
Pose2 test_pose = cur_pose.retract(Vector_(3, step_size, 0.0, Rot2::Logmap(dtheta)(0)));
Pose2 test_pose = cur_pose.retract((Vector(3) << step_size, 0.0, Rot2::Logmap(dtheta)(0)));
// create a segment to use for intersection checking
// find the closest intersection

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

@ -29,7 +29,7 @@ TEST( InvDepthFactor, Project1) {
Point2 expected_uv = level_camera.project(landmark);
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);
Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth);
EXPECT(assert_equal(expected_uv, actual_uv));
@ -45,7 +45,7 @@ TEST( InvDepthFactor, Project2) {
Point2 expected = level_camera.project(landmark);
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));
Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual));
@ -60,7 +60,7 @@ TEST( InvDepthFactor, Project3) {
Point2 expected = level_camera.project(landmark);
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));
Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual));
@ -75,7 +75,7 @@ TEST( InvDepthFactor, Project4) {
Point2 expected = level_camera.project(landmark);
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.));
Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual));
@ -88,7 +88,7 @@ Point2 project_(const Pose3& pose, const LieVector& landmark, const LieScalar& i
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);
Matrix expected = numericalDerivative31<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
@ -100,7 +100,7 @@ TEST( InvDepthFactor, Dproject_pose)
/* ************************************************************************* */
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);
Matrix expected = numericalDerivative32<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
@ -112,7 +112,7 @@ TEST( InvDepthFactor, Dproject_landmark)
/* ************************************************************************* */
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);
Matrix expected = numericalDerivative33<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
@ -124,7 +124,7 @@ TEST( InvDepthFactor, Dproject_inv_depth)
/* ************************************************************************* */
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);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Point2 z = inv_camera.project(expected, inv_depth);
@ -140,7 +140,7 @@ TEST(InvDepthFactor, backproject)
TEST(InvDepthFactor, backproject2)
{
// 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);
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);

2
gtsam_unstable/geometry/tests/testPose3Upright.cpp
View File

@ -72,7 +72,7 @@ TEST( testPose3Upright, manifold ) {
EXPECT(assert_equal(x1, x1.retract(zero(4)), tol));
EXPECT(assert_equal(x2, x2.retract(zero(4)), tol));
Vector delta12 = Vector_(4, 3.0, 0.0, 4.0, 0.0), delta21 = -delta12;
Vector delta12 = (Vector(4) << 3.0, 0.0, 4.0, 0.0), delta21 = -delta12;
EXPECT(assert_equal(x2, x1.retract(delta12), tol));
EXPECT(assert_equal(x1, x2.retract(delta21), tol));

2
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -266,7 +266,7 @@ namespace gtsam {
}
}
return Vector_(2, p_inlier, p_outlier);
return (Vector(2) << p_inlier, p_outlier);
}
/* ************************************************************************* */

6
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
View File

@ -537,7 +537,7 @@ public:
Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 );
Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5));
Vector omega_earth_ECEF((Vector(3) << 0.0, 0.0, 7.292115e-5));
omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF;
// Calculating g
@ -551,7 +551,7 @@ public:
double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature
double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) );
double g_calc( g0/( pow(1 + height/Ro, 2) ) );
g_ENU = Vector_(3, 0.0, 0.0, -g_calc);
g_ENU = (Vector(3) << 0.0, 0.0, -g_calc);
// Calculate rho
@ -560,7 +560,7 @@ public:
double rho_E = -Vn/(Rm + height);
double rho_N = Ve/(Rp + height);
double rho_U = Ve*tan(lat_new)/(Rp + height);
rho_ENU = Vector_(3, rho_E, rho_N, rho_U);
rho_ENU = (Vector(3) << rho_E, rho_N, rho_U);
}
static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){

2
gtsam_unstable/slam/InvDepthFactor3.h
View File

@ -96,7 +96,7 @@ public:
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
return gtsam::ones(2) * 2.0 * K_->fx();
}
return gtsam::Vector_(1, 0.0);
return (gtsam::Vector(1) << 0.0);
}
/** return the measurement */

2
gtsam_unstable/slam/InvDepthFactorVariant1.h
View File

@ -96,7 +96,7 @@ public:
<< std::endl;
return gtsam::ones(2) * 2.0 * K_->fx();
}
return gtsam::Vector_(1, 0.0);
return (gtsam::Vector(1) << 0.0);
}
/// Evaluate error h(x)-z and optionally derivatives

2
gtsam_unstable/slam/InvDepthFactorVariant2.h
View File

@ -99,7 +99,7 @@ public:
<< std::endl;
return gtsam::ones(2) * 2.0 * K_->fx();
}
return gtsam::Vector_(1, 0.0);
return (gtsam::Vector(1) << 0.0);
}
/// Evaluate error h(x)-z and optionally derivatives

2
gtsam_unstable/slam/Mechanization_bRn2.h
View File

@ -35,7 +35,7 @@ public:
}
Vector b_g(double g_e) {
Vector n_g = Vector_(3, 0.0, 0.0, g_e);
Vector n_g = (Vector(3) << 0.0, 0.0, g_e);
return (bRn_ * n_g).vector();
}

2
gtsam_unstable/slam/PartialPriorFactor.h
View File

@ -70,7 +70,7 @@ namespace gtsam {
/** Single Element Constructor: acts on a single parameter specified by idx */
PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) :
Base(model, key), prior_(Vector_(1, prior)), mask_(1, idx), H_(zeros(1, T::Dim())) {
Base(model, key), prior_((Vector(1) << prior)), mask_(1, idx), H_(zeros(1, T::Dim())) {
assert(model->dim() == 1);
this->fillH();
}

2
gtsam_unstable/slam/RelativeElevationFactor.cpp
View File

@ -32,7 +32,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p
*H2 = zeros(1, 3);
(*H2)(0, 2) = -1.0;
}
return Vector_(1, hx - measured_);
return (Vector(1) << hx - measured_);
}
/* ************************************************************************* */

2
gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
View File

@ -276,7 +276,7 @@ namespace gtsam {
}
}
return Vector_(2, p_inlier, p_outlier);
return (Vector(2) << p_inlier, p_outlier);
}
/* ************************************************************************* */

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

@ -75,7 +75,7 @@ TEST (AHRS, Mechanization_integrate) {
Mechanization_bRn2 mech;
KalmanFilter::State state;
// boost::tie(mech,state) = ahrs.initialize(g_e);
// Vector u = Vector_(3,0.05,0.0,0.0);
// Vector u = (Vector(3) << 0.05,0.0,0.0);
// double dt = 2;
// Rot3 expected;
// Mechanization_bRn2 mech2 = mech.integrate(u,dt);

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);
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;
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;
LieVector 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 Pose2(Rot3(), Point3(2.05, 0.95, 3.04));
LieVector Vel1(3, 0.50, -0.50, 0.40);
LieVector Vel2(3, 0.51, -0.48, 0.43);
LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40));
LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43));
imuBias::ConstantBias Bias1;
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 Pose2(Rot3::Expmap(measurement_gyro*measurement_dt), Point3(2.0,1.0,3.0));
LieVector Vel1(3,0.0,0.0,0.0);
LieVector Vel2(3,0.0,0.0,0.0);
LieVector Vel1((Vector(3) << 0.0, 0.0, 0.0));
LieVector Vel2((Vector(3) << 0.0, 0.0, 0.0));
imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@ -230,7 +230,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
-0.652537293, 0.709880342, 0.265075427);
Point3 t1(2.0,1.0,3.0);
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,
0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388);
@ -302,13 +302,13 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
-0.652537293, 0.709880342, 0.265075427);
Point3 t1(2.0,1.0,3.0);
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,
0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388);
Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800);
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;
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);
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;
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;
LieVector 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 Pose2(Rot3(), Point3(2.05, 0.95, 3.04));
LieVector Vel1(3, 0.50, -0.50, 0.40);
LieVector Vel2(3, 0.51, -0.48, 0.43);
LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40));
LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43));
imuBias::ConstantBias Bias1;
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 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 Vel2(3,0.0,0.0,0.0);
LieVector Vel1((Vector(3) << 0.0,0.0,0.0));
LieVector Vel2((Vector(3) << 0.0,0.0,0.0));
imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@ -569,7 +569,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform)
-0.652537293, 0.709880342, 0.265075427);
Point3 t1(2.0,1.0,3.0);
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,
0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388);
@ -618,13 +618,13 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
-0.652537293, 0.709880342, 0.265075427);
Point3 t1(2.0,1.0,3.0);
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,
0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388);
Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800);
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;
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);
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
// in reality this is 1/5, but initial depth is guessed
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 phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
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 phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
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 phi = atan2(landmark_p1.y(), sqrt(landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z()));
double rho = 1./landmark_p1.norm();
LieVector expected(3, theta, phi, rho);
LieVector expected((Vector(3) << theta, phi, rho));

2
tests/testBoundingConstraint.cpp
View File

@ -187,7 +187,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) {
EXPECT(!rangeBound.isGreaterThan());
EXPECT(rangeBound.dim() == 1);
EXPECT(assert_equal(Vector_(1, 2.0), rangeBound.evaluateError(pt1, pt1)));
EXPECT(assert_equal(((Vector)Vector(1) << 2.0), rangeBound.evaluateError(pt1, pt1)));
EXPECT(assert_equal(ones(1), rangeBound.evaluateError(pt1, pt2)));
EXPECT(assert_equal(zero(1), rangeBound.evaluateError(pt1, pt3)));
EXPECT(assert_equal(-1.0*ones(1), rangeBound.evaluateError(pt1, pt4)));

2
tests/testGaussianJunctionTreeB.cpp
View File

@ -199,7 +199,7 @@ TEST(GaussianJunctionTreeB, simpleMarginal) {
// Create a simple graph
NonlinearFactorGraph fg;
fg.add(PriorFactor<Pose2>(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0)));
fg.add(BetweenFactor<Pose2>(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector_(3, 10.0, 1.0, 1.0))));
fg.add(BetweenFactor<Pose2>(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas((Vector(3) << 10.0, 1.0, 1.0))));
Values init;
init.insert(X(0), Pose2());

30
tests/testNonlinearFactor.cpp
View File

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

4
tests/testSerializationSLAM.cpp
View File

@ -287,8 +287,8 @@ TEST (testSerializationSLAM, smallExample_nonlinear) {
/* ************************************************************************* */
TEST (testSerializationSLAM, factors) {
LieVector lieVector(4, 1.0, 2.0, 3.0, 4.0);
LieMatrix lieMatrix(2, 3, 1.0, 2.0, 3.0, 4.0, 5.0 ,6.0);
LieVector lieVector((Vector(4) << 1.0, 2.0, 3.0, 4.0));
LieMatrix lieMatrix((Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0 ,6.0));
Point2 point2(1.0, 2.0);
StereoPoint2 stereoPoint2(1.0, 2.0, 3.0);
Point3 point3(1.0, 2.0, 3.0);

2
tests/testSimulated2DOriented.cpp
View File

@ -58,7 +58,7 @@ TEST( simulated2DOriented, Dprior )
TEST( simulated2DOriented, constructor )
{
Pose2 measurement(0.2, 0.3, 0.1);
SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1., 1., 1.));
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 1., 1., 1.));
simulated2DOriented::Odometry factor(measurement, model, X(1), X(2));
simulated2DOriented::Values config;