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

Fix Matrix_(...) to Mat() <<... in gtsam/base & gtsam/geometry

release/4.3a0
Jing Dong 2013-11-13 04:50:42 +00:00
parent b806db8b20
commit 763083d2db
14 changed files with 73 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -31,7 +31,7 @@ double f(const LieVector& x) {
TEST(testNumericalDerivative, numericalHessian) {
LieVector center = ones(2);
Matrix expected = Matrix_(2,2,
Matrix expected = (Mat(2,2) <<
-sin(center(0)), 0.0,
0.0, -cos(center(1)));
@ -50,7 +50,7 @@ double f2(const LieVector& x) {
TEST(testNumericalDerivative, numericalHessian2) {
LieVector center(2, 0.5, 1.0);
Matrix expected = Matrix_(2,2,
Matrix expected = (Mat(2,2) <<
-cos(center(1))*sin(center(0)), -sin(center(1))*cos(center(0)),
-cos(center(0))*sin(center(1)), -sin(center(0))*cos(center(1)));
@ -69,15 +69,15 @@ double f3(const LieVector& x1, const LieVector& x2) {
TEST(testNumericalDerivative, numericalHessian211) {
LieVector center1(1, 1.0), center2(1, 5.0);
Matrix expected11 = Matrix_(1,1,-sin(center1(0))*cos(center2(0)));
Matrix expected11 = (Mat(1, 1) << -sin(center1(0))*cos(center2(0)));
Matrix actual11 = numericalHessian211(f3, center1, center2);
EXPECT(assert_equal(expected11, actual11, 1e-5));
Matrix expected12 = Matrix_(1,1,-cos(center1(0))*sin(center2(0)));
Matrix expected12 = (Mat(1, 1) <<-cos(center1(0))*sin(center2(0)));
Matrix actual12 = numericalHessian212(f3, center1, center2);
EXPECT(assert_equal(expected12, actual12, 1e-5));
Matrix expected22 = Matrix_(1,1,-sin(center1(0))*cos(center2(0)));
Matrix expected22 = (Mat(1, 1) <<-sin(center1(0))*cos(center2(0)));
Matrix actual22 = numericalHessian222(f3, center1, center2);
EXPECT(assert_equal(expected22, actual22, 1e-5));
}
@ -92,27 +92,27 @@ 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);
double x = center1(0), y = center2(0), z = center3(0);
Matrix expected11 = Matrix_(1,1,-sin(x)*cos(y)*z*z);
Matrix expected11 = (Mat(1, 1) << -sin(x)*cos(y)*z*z);
Matrix actual11 = numericalHessian311(f4, center1, center2, center3);
EXPECT(assert_equal(expected11, actual11, 1e-5));
Matrix expected12 = Matrix_(1,1,-cos(x)*sin(y)*z*z);
Matrix expected12 = (Mat(1, 1) << -cos(x)*sin(y)*z*z);
Matrix actual12 = numericalHessian312(f4, center1, center2, center3);
EXPECT(assert_equal(expected12, actual12, 1e-5));
Matrix expected13 = Matrix_(1,1,cos(x)*cos(y)*2*z);
Matrix expected13 = (Mat(1, 1) << cos(x)*cos(y)*2*z);
Matrix actual13 = numericalHessian313(f4, center1, center2, center3);
EXPECT(assert_equal(expected13, actual13, 1e-5));
Matrix expected22 = Matrix_(1,1,-sin(x)*cos(y)*z*z);
Matrix expected22 = (Mat(1, 1) << -sin(x)*cos(y)*z*z);
Matrix actual22 = numericalHessian322(f4, center1, center2, center3);
EXPECT(assert_equal(expected22, actual22, 1e-5));
Matrix expected23 = Matrix_(1,1,-sin(x)*sin(y)*2*z);
Matrix expected23 = (Mat(1, 1) << -sin(x)*sin(y)*2*z);
Matrix actual23 = numericalHessian323(f4, center1, center2, center3);
EXPECT(assert_equal(expected23, actual23, 1e-5));
Matrix expected33 = Matrix_(1,1,sin(x)*cos(y)*2);
Matrix expected33 = (Mat(1, 1) << sin(x)*cos(y)*2);
Matrix actual33 = numericalHessian333(f4, center1, center2, center3);
EXPECT(assert_equal(expected33, actual33, 1e-5));
}

6
gtsam/base/tests/testSerializationBase.cpp
View File

@ -88,19 +88,19 @@ TEST (Serialization, matrix_vector) {
EXPECT(equality<Vector2>(Vector2(1.0, 2.0)));
EXPECT(equality<Vector3>(Vector3(1.0, 2.0, 3.0)));
EXPECT(equality<Vector6>((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished()));
EXPECT(equality<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
EXPECT(equality<Matrix>((Mat(2, 2) << 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityXML<Vector>((Vec(4) << 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityXML<Vector2>(Vector2(1.0, 2.0)));
EXPECT(equalityXML<Vector3>(Vector3(1.0, 2.0, 3.0)));
EXPECT(equalityXML<Vector6>((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished()));
EXPECT(equalityXML<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityXML<Matrix>((Mat(2, 2) << 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityBinary<Vector>((Vec(4) << 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityBinary<Vector2>(Vector2(1.0, 2.0)));
EXPECT(equalityBinary<Vector3>(Vector3(1.0, 2.0, 3.0)));
EXPECT(equalityBinary<Vector6>((Vector6() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished()));
EXPECT(equalityBinary<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityBinary<Matrix>((Mat(2, 2) << 1.0, 2.0, 3.0, 4.0)));
}
/* ************************************************************************* */

16
gtsam/geometry/Cal3DS2.cpp
View File

@ -28,10 +28,10 @@ Cal3DS2::Cal3DS2(const Vector &v):
fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), k3_(v[7]), k4_(v[8]){}
/* ************************************************************************* */
Matrix Cal3DS2::K() const { return Matrix_(3,3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); }
Matrix Cal3DS2::K() const { return (Mat(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); }
/* ************************************************************************* */
Vector Cal3DS2::vector() const { return Vector_(9, fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); }
Vector Cal3DS2::vector() const { return (Vec(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); }
/* ************************************************************************* */
void Cal3DS2::print(const std::string& s) const { gtsam::print(K(), s + ".K"); gtsam::print(Vector(k()), s + ".k"); }
@ -70,7 +70,7 @@ Point2 Cal3DS2::uncalibrate(const Point2& p,
// Inlined derivative for calibration
if (H1) {
*H1 = Matrix_(2, 9, pnx, 0.0, pny, 1.0, 0.0, fx * x * rr + s * y * rr,
*H1 = (Mat(2, 9) << pnx, 0.0, pny, 1.0, 0.0, fx * x * rr + s * y * rr,
fx * x * r4 + s * y * r4, fx * 2. * xy + s * (rr + 2. * yy),
fx * (rr + 2. * xx) + s * (2. * xy), 0.0, pny, 0.0, 0.0, 1.0,
fy * y * rr, fy * y * r4, fy * (rr + 2. * yy), fy * (2. * xy));
@ -87,8 +87,8 @@ Point2 Cal3DS2::uncalibrate(const Point2& p,
const double dDy_dx = 2. * k4 * y + k3 * dr_dx;
const double dDy_dy = 2. * k4 * x + k3 * (dr_dy + 4. * y);
Matrix DK = Matrix_(2, 2, fx, s_, 0.0, fy);
Matrix DR = Matrix_(2, 2, g + x * dg_dx + dDx_dx, x * dg_dy + dDx_dy,
Matrix DK = (Mat(2, 2) << fx, s_, 0.0, fy);
Matrix DR = (Mat(2, 2) << g + x * dg_dx + dDx_dx, x * dg_dy + dDx_dy,
y * dg_dx + dDy_dx, g + y * dg_dy + dDy_dy);
*H2 = DK * DR;
@ -148,8 +148,8 @@ Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
const double dDy_dx = 2*k4*y + k3*dr_dx;
const double dDy_dy = 2*k4*x + k3*(dr_dy + 4*y);
Matrix DK = Matrix_(2, 2, fx_, s_, 0.0, fy_);
Matrix DR = Matrix_(2, 2, g + x*dg_dx + dDx_dx, x*dg_dy + dDx_dy,
Matrix DK = (Mat(2, 2) << fx_, s_, 0.0, fy_);
Matrix DR = (Mat(2, 2) << g + x*dg_dx + dDx_dx, x*dg_dy + dDx_dy,
y*dg_dx + dDy_dx, g + y*dg_dy + dDy_dy);
return DK * DR;
@ -167,7 +167,7 @@ Matrix Cal3DS2::D2d_calibration(const Point2& p) const {
const double pnx = g*x + dx;
const double pny = g*y + dy;
return Matrix_(2, 9,
return (Mat(2, 9) <<
pnx, 0.0, pny, 1.0, 0.0, fx*x*rr + s*y*rr, fx*x*r4 + s*y*r4, fx*2*xy + s*(rr+2*yy), fx*(rr+2*xx) + s*(2*xy),
0.0, pny, 0.0, 0.0, 1.0, fy*y*rr , fy*y*r4 , fy*(rr+2*yy) , fy*(2*xy) );
}

4
gtsam/geometry/Cal3_S2.cpp
View File

@ -76,9 +76,9 @@ Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
boost::optional<Matrix&> Dp) const {
const double x = p.x(), y = p.y();
if (Dcal)
*Dcal = Matrix_(2, 5, x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0);
*Dcal = (Mat(2, 5) << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0);
if (Dp)
*Dp = Matrix_(2, 2, fx_, s_, 0.000, fy_);
*Dp = (Mat(2, 2) << fx_, s_, 0.000, fy_);
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
}

4
gtsam/geometry/Cal3_S2.h
View File

@ -126,7 +126,7 @@ public:
/// return calibration matrix K
Matrix K() const {
return Matrix_(3, 3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
return (Mat(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
}
/** @deprecated The following function has been deprecated, use K above */
@ -137,7 +137,7 @@ public:
/// return inverted calibration matrix inv(K)
Matrix matrix_inverse() const {
const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
return Matrix_(3, 3, 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
return (Mat(3, 3) << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0);
}

6
gtsam/geometry/CalibratedCamera.cpp
View File

@ -36,7 +36,7 @@ Point2 CalibratedCamera::project_to_camera(const Point3& P,
boost::optional<Matrix&> H1) {
if (H1) {
double d = 1.0 / P.z(), d2 = d * d;
*H1 = Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
*H1 = (Mat(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
}
return Point2(P.x() / P.z(), P.y() / P.z());
}
@ -84,12 +84,12 @@ Point2 CalibratedCamera::project(const Point3& point,
const double z = q.z(), d = 1.0 / z;
const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v;
if (Dpose)
*Dpose = Matrix_(2, 6, uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
*Dpose = (Mat(2, 6) << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
-uv, -u, 0., -d, d * v);
if (Dpoint) {
const Matrix R(pose_.rotation().matrix());
*Dpoint = d
* Matrix_(2, 3, R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2),
* (Mat(2, 3) << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2),
R(2, 0) - u * R(2, 2), R(0, 1) - v * R(0, 2),
R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2));
}

4
gtsam/geometry/Point2.cpp
View File

@ -27,7 +27,7 @@ namespace gtsam {
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Point2);
static const Matrix oneOne = Matrix_(1, 2, 1.0, 1.0);
static const Matrix oneOne = (Mat(1, 2) << 1.0, 1.0);
/* ************************************************************************* */
void Point2::print(const string& s) const {
@ -45,7 +45,7 @@ double Point2::norm(boost::optional<Matrix&> H) const {
if (H) {
Matrix D_result_d;
if (fabs(r) > 1e-10)
D_result_d = Matrix_(1, 2, x_ / r, y_ / r);
D_result_d = (Mat(1, 2) << x_ / r, y_ / r);
else
D_result_d = oneOne; // TODO: really infinity, why 1 here??
*H = D_result_d;

16
gtsam/geometry/Pose2.cpp
View File

@ -40,7 +40,7 @@ static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
Matrix Pose2::matrix() const {
Matrix R = r_.matrix();
R = stack(2, &R, &Z12);
Matrix T = Matrix_(3,1, t_.x(), t_.y(), 1.0);
Matrix T = (Mat(3, 1) << t_.x(), t_.y(), 1.0);
return collect(2, &R, &T);
}
@ -110,7 +110,7 @@ Vector Pose2::localCoordinates(const Pose2& p2) const {
// Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
Matrix Pose2::AdjointMap() const {
double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y();
return Matrix_(3,3,
return (Mat(3, 3) <<
c, -s, y,
s, c, -x,
0.0, 0.0, 1.0
@ -130,7 +130,7 @@ Point2 Pose2::transform_to(const Point2& point,
Point2 d = point - t_;
Point2 q = r_.unrotate(d);
if (!H1 && !H2) return q;
if (H1) *H1 = Matrix_(2, 3,
if (H1) *H1 = (Mat(2, 3) <<
-1.0, 0.0, q.y(),
0.0, -1.0, -q.x());
if (H2) *H2 = r_.transpose();
@ -154,7 +154,7 @@ Point2 Pose2::transform_from(const Point2& p,
const Point2 q = r_ * p;
if (H1 || H2) {
const Matrix R = r_.matrix();
const Matrix Drotate1 = Matrix_(2, 1, -q.y(), q.x());
const Matrix Drotate1 = (Mat(2, 1) << -q.y(), q.x());
if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q]
if (H2) *H2 = R; // R
}
@ -184,7 +184,7 @@ Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix&> H1,
if (H1) {
double dt1 = -s2 * x + c2 * y;
double dt2 = -c2 * x - s2 * y;
*H1 = Matrix_(3,3,
*H1 = (Mat(3, 3) <<
-c, -s, dt1,
s, -c, dt2,
0.0, 0.0,-1.0);
@ -225,7 +225,7 @@ double Pose2::range(const Point2& point,
if (!H1 && !H2) return d.norm();
Matrix H;
double r = d.norm(H);
if (H1) *H1 = H * Matrix_(2, 3,
if (H1) *H1 = H * (Mat(2, 3) <<
-r_.c(), r_.s(), 0.0,
-r_.s(), -r_.c(), 0.0);
if (H2) *H2 = H;
@ -240,10 +240,10 @@ double Pose2::range(const Pose2& pose2,
if (!H1 && !H2) return d.norm();
Matrix H;
double r = d.norm(H);
if (H1) *H1 = H * Matrix_(2, 3,
if (H1) *H1 = H * (Mat(2, 3) <<
-r_.c(), r_.s(), 0.0,
-r_.s(), -r_.c(), 0.0);
if (H2) *H2 = H * Matrix_(2, 3,
if (H2) *H2 = H * (Mat(2, 3) <<
pose2.r_.c(), -pose2.r_.s(), 0.0,
pose2.r_.s(), pose2.r_.c(), 0.0);
return r;

12
gtsam/geometry/Rot2.cpp
View File

@ -65,12 +65,12 @@ Rot2& Rot2::normalize() {
/* ************************************************************************* */
Matrix Rot2::matrix() const {
return Matrix_(2, 2, c_, -s_, s_, c_);
return (Mat(2, 2) << c_, -s_, s_, c_);
}
/* ************************************************************************* */
Matrix Rot2::transpose() const {
return Matrix_(2, 2, c_, s_, -s_, c_);
return (Mat(2, 2) << c_, s_, -s_, c_);
}
/* ************************************************************************* */
@ -78,7 +78,7 @@ Matrix Rot2::transpose() const {
Point2 Rot2::rotate(const Point2& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
const Point2 q = Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y());
if (H1) *H1 = Matrix_(2, 1, -q.y(), q.x());
if (H1) *H1 = (Mat(2, 1) << -q.y(), q.x());
if (H2) *H2 = matrix();
return q;
}
@ -88,7 +88,7 @@ Point2 Rot2::rotate(const Point2& p, boost::optional<Matrix&> H1,
Point2 Rot2::unrotate(const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Point2 q = Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y());
if (H1) *H1 = Matrix_(2, 1, q.y(), -q.x()); // R_{pi/2}q
if (H1) *H1 = (Mat(2, 1) << q.y(), -q.x()); // R_{pi/2}q
if (H2) *H2 = transpose();
return q;
}
@ -97,10 +97,10 @@ Point2 Rot2::unrotate(const Point2& p,
Rot2 Rot2::relativeBearing(const Point2& d, boost::optional<Matrix&> H) {
double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
if(fabs(n) > 1e-5) {
if (H) *H = Matrix_(1, 2, -y / d2, x / d2);
if (H) *H = (Mat(1, 2) << -y / d2, x / d2);
return Rot2::fromCosSin(x / n, y / n);
} else {
if (H) *H = Matrix_(1,2, 0.0, 0.0);
if (H) *H = (Mat(1, 2) << 0.0, 0.0);
return Rot2();
}
}

6
gtsam/geometry/StereoCamera.cpp
View File

@ -64,7 +64,7 @@ namespace gtsam {
// optimized version, see StereoCamera.nb
if (H1) {
const double v1 = v/fy, v2 = fx*v1, dx=d*x;
*H1 = Matrix_(3, 6,
*H1 = (Mat(3, 6) <<
uL*v1, -fx-dx*uL, v2, -dfx, 0.0, d*uL,
uR*v1, -fx-dx*uR, v2, -dfx, 0.0, d*uR,
fy + v*v1, -dx*v , -x*dfy, 0.0, -dfy, d*v
@ -72,7 +72,7 @@ namespace gtsam {
}
if (H2) {
const Matrix R(leftCamPose_.rotation().matrix());
*H2 = d * Matrix_(3, 3,
*H2 = d * (Mat(3, 3) <<
fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL,
fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR,
fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v
@ -90,7 +90,7 @@ namespace gtsam {
double d = 1.0 / P.z(), d2 = d*d;
const Cal3_S2Stereo& K = *K_;
double f_x = K.fx(), f_y = K.fy(), b = K.baseline();
return Matrix_(3, 3,
return (Mat(3, 3) <<
f_x*d, 0.0, -d2*f_x* P.x(),
f_x*d, 0.0, -d2*f_x*(P.x() - b),
0.0, f_y*d, -d2*f_y* P.y()

24
gtsam/geometry/tests/testPose2.cpp
View File

@ -92,7 +92,7 @@ TEST(Pose2, expmap2) {
TEST(Pose2, expmap3) {
// do an actual series exponential map
// see e.g. http://www.cis.upenn.edu/~cis610/cis610lie1.ps
Matrix A = Matrix_(3,3,
Matrix A = (Mat(3,3) <<
0.0, -0.99, 0.01,
0.99, 0.0, -0.015,
0.0, 0.0, 0.0);
@ -204,8 +204,8 @@ TEST( Pose2, transform_to )
// expected
Point2 expected(2,2);
Matrix expectedH1 = Matrix_(2,3, -1.0, 0.0, 2.0, 0.0, -1.0, -2.0);
Matrix expectedH2 = Matrix_(2,2, 0.0, 1.0, -1.0, 0.0);
Matrix expectedH1 = (Mat(2,3) << -1.0, 0.0, 2.0, 0.0, -1.0, -2.0);
Matrix expectedH2 = (Mat(2,2) << 0.0, 1.0, -1.0, 0.0);
// actual
Matrix actualH1, actualH2;
@ -236,8 +236,8 @@ TEST (Pose2, transform_from)
Point2 expected(0., 2.);
EXPECT(assert_equal(expected, actual));
Matrix H1_expected = Matrix_(2, 3, 0., -1., -2., 1., 0., -1.);
Matrix H2_expected = Matrix_(2, 2, 0., -1., 1., 0.);
Matrix H1_expected = (Mat(2, 3) << 0., -1., -2., 1., 0., -1.);
Matrix H2_expected = (Mat(2, 2) << 0., -1., 1., 0.);
Matrix numericalH1 = numericalDerivative21(transform_from_proxy, pose, pt);
EXPECT(assert_equal(H1_expected, H1));
@ -261,7 +261,7 @@ TEST(Pose2, compose_a)
Pose2 expected(3.0*M_PI/4.0, Point2(-sqrt(0.5), 3.0*sqrt(0.5)));
EXPECT(assert_equal(expected, actual));
Matrix expectedH1 = Matrix_(3,3,
Matrix expectedH1 = (Mat(3,3) <<
0.0, 1.0, 0.0,
-1.0, 0.0, 2.0,
0.0, 0.0, 1.0
@ -355,7 +355,7 @@ namespace {
Matrix matrix(const Pose2& gTl) {
Matrix gRl = gTl.r().matrix();
Point2 gt = gTl.t();
return Matrix_(3, 3,
return (Mat(3, 3) <<
gRl(0, 0), gRl(0, 1), gt.x(),
gRl(1, 0), gRl(1, 1), gt.y(),
0.0, 0.0, 1.0);
@ -368,7 +368,7 @@ TEST( Pose2, matrix )
Point2 origin, t(1,2);
Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y
Matrix gMl = matrix(gTl);
EXPECT(assert_equal(Matrix_(3,3,
EXPECT(assert_equal((Mat(3,3) <<
0.0, -1.0, 1.0,
1.0, 0.0, 2.0,
0.0, 0.0, 1.0),
@ -376,7 +376,7 @@ TEST( Pose2, matrix )
Rot2 gR1 = gTl.r();
EXPECT(assert_equal(homogeneous(t),gMl*homogeneous(origin)));
Point2 x_axis(1,0), y_axis(0,1);
EXPECT(assert_equal(Matrix_(2,2,
EXPECT(assert_equal((Mat(2,2) <<
0.0, -1.0,
1.0, 0.0),
gR1.matrix()));
@ -387,7 +387,7 @@ TEST( Pose2, matrix )
// check inverse pose
Matrix lMg = matrix(gTl.inverse());
EXPECT(assert_equal(Matrix_(3,3,
EXPECT(assert_equal((Mat(3,3) <<
0.0, 1.0,-2.0,
-1.0, 0.0, 1.0,
0.0, 0.0, 1.0),
@ -421,7 +421,7 @@ TEST( Pose2, between )
EXPECT(assert_equal(expected,actual1));
EXPECT(assert_equal(expected,actual2));
Matrix expectedH1 = Matrix_(3,3,
Matrix expectedH1 = (Mat(3,3) <<
0.0,-1.0,-2.0,
1.0, 0.0,-2.0,
0.0, 0.0,-1.0
@ -432,7 +432,7 @@ TEST( Pose2, between )
// 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 = (Mat(3,3) <<
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0

10
gtsam/geometry/tests/testRot3M.cpp
View File

@ -57,7 +57,7 @@ TEST( Rot3, constructor)
/* ************************************************************************* */
TEST( Rot3, constructor2)
{
Matrix R = Matrix_(3, 3, 11., 12., 13., 21., 22., 23., 31., 32., 33.);
Matrix R = (Mat(3, 3) << 11., 12., 13., 21., 22., 23., 31., 32., 33.);
Rot3 actual(R);
Rot3 expected(11, 12, 13, 21, 22, 23, 31, 32, 33);
CHECK(assert_equal(actual,expected));
@ -501,7 +501,7 @@ TEST( Rot3, RQ)
CHECK(assert_equal((Vector)(Vec(3) << 0.0,0.0,0.1),Rot3::roll (0.1).ypr()));
// Try RQ to recover calibration from 3*3 sub-block of projection matrix
Matrix K = Matrix_(3, 3, 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0);
Matrix K = (Mat(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0);
Matrix A = K * R.matrix();
boost::tie(actualK, actual) = RQ(A);
CHECK(assert_equal(K,actualK));
@ -514,7 +514,7 @@ TEST( Rot3, expmapStability ) {
double theta = w.norm();
double theta2 = theta*theta;
Rot3 actualR = Rot3::Expmap(w);
Matrix W = Matrix_(3,3, 0.0, -w(2), w(1),
Matrix W = (Mat(3, 3) << 0.0, -w(2), w(1),
w(2), 0.0, -w(0),
-w(1), w(0), 0.0 );
Matrix W2 = W*W;
@ -541,13 +541,13 @@ TEST( Rot3, logmapStability ) {
TEST(Rot3, quaternion) {
// NOTE: This is also verifying the ability to convert Vector to Quaternion
Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782);
Rot3 R1 = Rot3(Matrix_(3,3,
Rot3 R1 = Rot3((Matrix)(Mat(3, 3) <<
0.271018623057411, 0.278786459830371, 0.921318086098018,
0.578529366719085, 0.717799701969298, -0.387385285854279,
-0.769319620053772, 0.637998195662053, 0.033250932803219));
Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053);
Rot3 R2 = Rot3(Matrix_(3,3,
Rot3 R2 = Rot3((Matrix)(Mat(3, 3) <<
-0.207341903877828, 0.250149415542075, 0.945745528564780,
0.881304914479026, -0.371869043667957, 0.291573424846290,
0.424630407073532, 0.893945571198514, -0.143353873763946));

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

@ -52,7 +52,7 @@ TEST( Rot3, constructor)
/* ************************************************************************* */
TEST( Rot3, constructor2)
{
Matrix R = Matrix_(3, 3, 11., 12., 13., 21., 22., 23., 31., 32., 33.);
Matrix R = (Mat(3, 3) << 11., 12., 13., 21., 22., 23., 31., 32., 33.);
Rot3 actual(R);
Rot3 expected(11, 12, 13, 21, 22, 23, 31, 32, 33);
CHECK(assert_equal(actual,expected));
@ -298,7 +298,7 @@ TEST( Rot3, between )
Rot3 r1 = Rot3::Rz(M_PI/3.0);
Rot3 r2 = Rot3::Rz(2.0*M_PI/3.0);
Matrix expectedr1 = Matrix_(3,3,
Matrix expectedr1 = (Mat(3, 3) <<
0.5, -sqrt(3.0)/2.0, 0.0,
sqrt(3.0)/2.0, 0.5, 0.0,
0.0, 0.0, 1.0);
@ -399,7 +399,7 @@ TEST( Rot3, RQ)
CHECK(assert_equal((Vector)(Vec(3) <<0.0,0.0,0.1),Rot3::roll (0.1).ypr()));
// Try RQ to recover calibration from 3*3 sub-block of projection matrix
Matrix K = Matrix_(3, 3, 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0);
Matrix K = (Mat(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0);
Matrix A = K * R.matrix();
boost::tie(actualK, actual) = RQ(A);
CHECK(assert_equal(K,actualK));
@ -412,7 +412,7 @@ TEST( Rot3, expmapStability ) {
double theta = w.norm();
double theta2 = theta*theta;
Rot3 actualR = Rot3::Expmap(w);
Matrix W = Matrix_(3,3, 0.0, -w(2), w(1),
Matrix W = (Mat(3, 3) << 0.0, -w(2), w(1),
w(2), 0.0, -w(0),
-w(1), w(0), 0.0 );
Matrix W2 = W*W;
@ -440,13 +440,13 @@ TEST( Rot3, expmapStability ) {
TEST(Rot3, quaternion) {
// NOTE: This is also verifying the ability to convert Vector to Quaternion
Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782);
Rot3 R1 = Rot3(Matrix_(3,3,
Rot3 R1 = Rot3((Mat(3, 3) <<
0.271018623057411, 0.278786459830371, 0.921318086098018,
0.578529366719085, 0.717799701969298, -0.387385285854279,
-0.769319620053772, 0.637998195662053, 0.033250932803219));
Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053);
Rot3 R2 = Rot3(Matrix_(3,3,
Rot3 R2 = Rot3((Mat(3, 3) <<
-0.207341903877828, 0.250149415542075, 0.945745528564780,
0.881304914479026, -0.371869043667957, 0.291573424846290,
0.424630407073532, 0.893945571198514, -0.143353873763946));

4
gtsam/geometry/tests/testSimpleCamera.cpp
View File

@ -28,7 +28,7 @@ using namespace gtsam;
static const Cal3_S2 K(625, 625, 0, 0, 0);
static const Pose3 pose1(Matrix_(3,3,
static const Pose3 pose1((Matrix)(Mat(3,3) <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
@ -144,7 +144,7 @@ TEST( SimpleCamera, simpleCamera)
Point3 T(1000,2000,1500);
SimpleCamera expected(Pose3(R.inverse(),T),K);
// H&Z example, 2nd edition, page 163
Matrix P = Matrix_(3,4,
Matrix P = (Mat(3,4) <<
3.53553e2, 3.39645e2, 2.77744e2, -1.44946e6,
-1.03528e2, 2.33212e1, 4.59607e2, -6.32525e5,
7.07107e-1, -3.53553e-1,6.12372e-1, -9.18559e2);