diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index d210be789..af6954341 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -167,7 +167,7 @@ Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)), Cal3Bundler0(1, 0, 0)); Point3 point(10, 0, -5); // negative Z-axis convention of Snavely! Vector9 P = Camera().localCoordinates(camera); -Vector3 X = point.vector(); +Vector3 X = point; Vector2 expectedMeasurement(1.2431567, 1.2525694); Matrix E1 = numericalDerivative21(adapted, P, X); Matrix E2 = numericalDerivative22(adapted, P, X); diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 75af5f634..6c9588d38 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -81,7 +81,7 @@ double f2(const Point3& p, OptionalJacobian<1, 3> H) { return 0.0; } Vector f3(const Point3& p, OptionalJacobian H) { - return p.vector(); + return p; } Expression p(1); set expected = list_of(1); @@ -108,7 +108,7 @@ TEST(Expression, NullaryMethod) { // Create expression Expression p(67); - Expression norm(p, &Point3::norm); + Expression norm(>sam::norm, p); // Create Values Values values; diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp index 706c20e78..73ff34d2a 100644 --- a/gtsam/sam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -361,10 +361,10 @@ TEST( RangeFactor, Camera) { namespace gtsam{ template <> -struct Range { +struct Range { typedef double result_type; - double operator()(const Vector3& v1, const Vector3& v2, - OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) { + double operator()(const Vector4& v1, const Vector4& v2, + OptionalJacobian<1, 4> H1, OptionalJacobian<1, 4> H2) { return (v2 - v1).norm(); // derivatives not implemented } @@ -376,11 +376,11 @@ TEST(RangeFactor, NonGTSAM) { Key poseKey(1); Key pointKey(2); double measurement(10.0); - RangeFactor factor(poseKey, pointKey, measurement, model); + RangeFactor factor(poseKey, pointKey, measurement, model); // Set the linearization point - Vector3 pose(1.0, 2.0, 00); - Vector3 point(-4.0, 11.0, 0); + Vector4 pose(1.0, 2.0, 00, 0); + Vector4 point(-4.0, 11.0, 0, 0); // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance Vector expectedError = (Vector(1) << 0.295630141).finished(); diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index e9a117745..e668d27d3 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -139,7 +139,7 @@ public: const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s); std::cout << " EssentialMatrixFactor2 with measurements\n (" - << dP1_.vector().transpose() << ")' and (" << pn_.vector().transpose() + << dP1_.transpose() << ")' and (" << pn_.vector().transpose() << ")'" << std::endl; } @@ -191,7 +191,7 @@ public: if (Dd) // efficient backwards computation: // (2*3) * (3*3) * (3*1) - *Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2.vector())); + *Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2)); } Point2 reprojectionError = pn - pn_; diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index e9914955c..a24421b34 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -70,19 +70,19 @@ public: (*H).middleCols(transInterval.first, tDim) = R.matrix(); } - return newTrans.vector() - measured_.vector(); + return traits::Local(measured_, newTrans); } /** equals specialized to this factor */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && measured_.equals(e->measured_, tol); + return e != NULL && Base::equals(*e, tol) && traits::Equals(measured_, e->measured_, tol); } /** print contents */ void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s + "PoseTranslationPrior", keyFormatter); - measured_.print("Measured Translation"); + traits::Print(measured_, "Measured Translation"); } private: diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h index ea8811d17..2ac217ac1 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -44,9 +44,9 @@ public: virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s); - std::cout << "RotateFactor:" << std::endl; - p_.print("p"); - z_.print("z"); + std::cout << "RotateFactor:]\n"; + std::cout << "p: " << p_.transpose() << std::endl; + std::cout << "z: " << z_.transpose() << std::endl; } /// vector of errors returns 2D vector diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 66538e802..1aa85b6fe 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -87,7 +87,7 @@ public: Vector9 z; z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang - z.tail(3).operator=(x2.t().vector()); // Strange syntax to work around ambiguous operator error with clang + z.tail(3).operator=(x2.t()); // Strange syntax to work around ambiguous operator error with clang if (H1) *H1 = numericalDerivative21( boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5); if (H2) *H2 = numericalDerivative22( @@ -109,7 +109,7 @@ private: static Vector9 predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) { Vector9 hx; hx.head(6).operator=(x1.imuPrediction(x2, dt)); // Strange syntax to work around ambiguous operator error with clang - hx.tail(3).operator=(x1.translationIntegration(x2, dt).vector()); // Strange syntax to work around ambiguous operator error with clang + hx.tail(3).operator=(x1.translationIntegration(x2, dt)); // Strange syntax to work around ambiguous operator error with clang return hx; } }; diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index d9674b415..ed3797015 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -115,7 +115,7 @@ private: case dynamics::EULER_END : hx = p1 + Point3(v2 * dt); break; default: assert(false); break; } - return p2.vector() - hx.vector(); + return p2 - hx; } }; diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index 1fb2cf39e..8b224f510 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -89,7 +89,8 @@ Vector newtonEuler(const Vector& Vb, const Vector& Fb, const Matrix& Inertia) { TEST( DiscreteEulerPoincareHelicopter, evaluateError) { Vector Fu = computeFu(gamma2, u2); - Vector fGravity_g1 = zero(6); subInsert(fGravity_g1, g1.rotation().unrotate(Point3(0.0, 0.0, -mass*9.81)).vector(), 3); // gravity force in g1 frame + Vector fGravity_g1 = zero(6); + subInsert(fGravity_g1, g1.rotation().unrotate(Point3(0.0, 0.0, -mass*9.81)), 3); // gravity force in g1 frame Vector Fb = Fu+fGravity_g1; Vector dV = newtonEuler(V1_g1, Fb, Inertia); diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 713ea3054..e2f2a2a86 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -64,7 +64,7 @@ TEST(Similarity3, Constructors) { Similarity3 sim3_Construct1; Similarity3 sim3_Construct2(s); Similarity3 sim3_Construct3(R, P, s); - Similarity3 sim4_Construct4(R.matrix(), P.vector(), s); + Similarity3 sim4_Construct4(R.matrix(), P, s); } //****************************************************************************** diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h index 6f39ce1b6..df1873765 100644 --- a/gtsam_unstable/slam/BiasedGPSFactor.h +++ b/gtsam_unstable/slam/BiasedGPSFactor.h @@ -58,15 +58,15 @@ namespace gtsam { virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "BiasedGPSFactor(" << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << ")\n"; - measured_.print(" measured: "); + << keyFormatter(this->key2()) << ")\n" + << " measured: " << measured_.transpose() << std::endl; this->noiseModel_->print(" noise model: "); } /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol); + return e != NULL && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); } /** implement functions needed to derive from Factor */ @@ -82,7 +82,7 @@ namespace gtsam { H2->resize(3,3); // jacobian wrt bias (*H2) << Matrix3::Identity(); } - return pose.translation().vector() + bias.vector() - measured_.vector(); + return pose.translation() + bias - measured_; } /** return the measured */ diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index d8fceeb68..30d3a216d 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -187,7 +187,7 @@ public: Vector GyroCorrected(Bias1.correctGyroscope(measurement_gyro_)); body_omega_body = body_R_sensor * GyroCorrected; Matrix body_omega_body__cross = skewSymmetric(body_omega_body); - body_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor_->translation().vector(); + body_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor_->translation(); } else { body_a_body = AccCorrected; } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 879e1e1dd..fdba78445 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -79,7 +79,7 @@ public: && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) && this->K_->equals(*e->K_, tol) - && this->referencePoint_.equals(e->referencePoint_, tol); + && traits::Equals(this->referencePoint_, e->referencePoint_, tol); } Vector inverseDepthError(const Pose3& pose, const Vector3& landmark) const { diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index efe1df640..75535eebc 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -233,7 +233,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, ///* VADIM - START ************************************************************************* */ //Vector3 predictionRq(const Vector3 angles, const Vector3 q) { -// return (Rot3().RzRyRx(angles) * q).vector(); +// return (Rot3().RzRyRx(angles) * q); //} // //TEST (InertialNavFactor_GlobalVelocity, Rotation_Deriv ) { @@ -435,7 +435,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector measurement_acc = Vector3(0.2, 0.1, -0.3 + 9.81) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() - * body_P_sensor.translation().vector(); // Measured in ENU orientation + * body_P_sensor.translation(); // Measured in ENU orientation InertialNavFactor_GlobalVelocity f( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, @@ -474,7 +474,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector measurement_acc = Vector3(0.2, 0.1, -0.3 + 9.81) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() - * body_P_sensor.translation().vector(); // Measured in ENU orientation + * body_P_sensor.translation(); // Measured in ENU orientation InertialNavFactor_GlobalVelocity f( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, @@ -512,7 +512,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector measurement_acc = Vector3(0.0, 0.0, 0.0 + 9.81) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() - * body_P_sensor.translation().vector(); // Measured in ENU orientation + * body_P_sensor.translation(); // Measured in ENU orientation InertialNavFactor_GlobalVelocity f( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, @@ -554,7 +554,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector3(-6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() - * body_P_sensor.translation().vector(); // Measured in ENU orientation + * body_P_sensor.translation(); // Measured in ENU orientation InertialNavFactor_GlobalVelocity f( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, @@ -605,7 +605,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector3(-6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() - * body_P_sensor.translation().vector(); // Measured in ENU orientation + * body_P_sensor.translation(); // Measured in ENU orientation InertialNavFactor_GlobalVelocity factor( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, diff --git a/tests/simulated3D.h b/tests/simulated3D.h index cf69a8fa3..84d9ec8cd 100644 --- a/tests/simulated3D.h +++ b/tests/simulated3D.h @@ -91,7 +91,7 @@ struct PointPrior3D: public NoiseModelFactor1 { */ Vector evaluateError(const Point3& x, boost::optional H = boost::none) const { - return prior(x, H).vector() - measured_.vector(); + return prior(x, H) - measured_; } }; @@ -122,7 +122,7 @@ struct Simulated3DMeasurement: public NoiseModelFactor2 { */ Vector evaluateError(const Point3& x1, const Point3& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - return mea(x1, x2, H1, H2).vector() - measured_.vector(); + return mea(x1, x2, H1, H2) - measured_; } }; diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 57f03cca6..bda23b3f6 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -139,7 +139,7 @@ TEST(ExpressionFactor, Unary) { typedef Eigen::Matrix Matrix93; Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) { Vector9 v; - v << p.vector(), p.vector(), p.vector(); + v << p, p, p; if (H) *H << eye(3), eye(3), eye(3); return v; }