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

modified Vector_() in gtsam/base and gtsam/geometry folders

release/4.3a0
jing 2014-01-23 01:17:07 -05:00
parent 8aac62ec1e
commit 5e9540470a
15 changed files with 20 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

@ -50,7 +50,7 @@ TEST( LieMatrix, other_constructors ) {
/* ************************************************************************* */
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);
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 actual = init.retract(update);

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)));
}
/* ************************************************************************* */

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

@ -170,7 +170,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

@ -40,7 +40,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));
}

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

@ -212,9 +212,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).
@ -242,9 +242,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).

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